I have a set of point cloud forming a plane in 3D, which I've obtained from RANSAC plane fitting. For some specific kind of analysis of the data I need to convert this to a 2D problem so that I can have all the z-values almost same. Suppose the equation of the plane is ax+by+cz+1=0. My question is that:
- How can I get the values of a,b,c from raw point cloud data? Will least square be the best approach or there is any way to obtain these values from RANSAC fitting?
- From some tutorials I got an idea about doing the following steps: translate to centre of mass, rotate about x-axis, rotate about y-axis, then un-translate again. Does it make sense? 3. How can I get the rotation angle in terms of a,b,c?
I need some general idea of converting a 3D plane to 2D so that all the z-coordinate values are same.
You need a change of basis.
Let us create the new orthonormal basis formed of three vectors X, Y and Z.
Your new Z will be the normal vector of the plane, (a, b, c). Normalize it by dividing all three components by Sqrt(a^2+b^2+c^2).
Then take an arbitrary vector, not parallel to the first, let U. Compute the dot product U.Z and subtract (U.Z).Z from U. Normalize the resulting vector as X.
Lastly, compute the cross product Y = Z /\ X.
Then, transform every point Pi to 2D by computing the dot products (Pi.X, Pi.Y).
- Build covariation matrix:
- find average of all points
- subtract if from every point
matrix(i,j)=sum(p[i]*p[j])(p[1],p[2],p[3]are coordinates of point p)
- Find eigen vectors and values of the matrix
- the vector corresponding to the smallest eigen value is Z, the rest - X and Y
There are two problems to be solved.
Problem 1: Given a set of 3D points, characterise the plane that they lie within.
Problem 2: Given a set of 3D points, project them onto that plane.
Problem 2 is addressed by other answers; I go for the change of basis approach.
For problem 1, you need to find the normal to the place. To do this, select some random triplets from the point cloud and find the normal to the triangle each forms. Take the average of those normals to estimate the normal to the plane. Use that to form the first element of the new orthonormal basis.
Here goes, along with some code to generate a test set. [ Point3D is not production quality; you probably can re-use something else for that anyway.]
#include <iostream>
#include <random>
#include <vector>
#include <algorithm> // std::random_shuffle
#include <numeric> // std::random_shuffle
#include <math.h>
struct Point3D
{
Point3D(double a = 0, double b = 0, double c = 0)
{
x_[0] = a;
x_[1] = b;
x_[2] = c;
}
int maxIndex() const
{
int maxIndex = 0;
double maxVal = 0;
for (int i = 0; i != 3; ++i)
{
if (fabs(x_[i]) > maxVal)
{
maxIndex = i;
maxVal = fabs(x_[i]);
}
}
return maxIndex;
}
double lengthSquared() const
{
return std::accumulate(std::begin(x_), std::end(x_), 0.0, [](double sum, double x) { return sum + x*x; });
}
double length() const { return sqrt(lengthSquared()); }
double dot(const Point3D& other) const
{
double d = 0;
for (int i = 0; i != 3; ++i)
d += x_[i] * other.x_[i];
return d;
}
const Point3D& add(const Point3D& other)
{
for (int i = 0; i != 3; ++i)
x_[i] += other.x_[i];
return *this;
}
const Point3D& subtract(const Point3D& other)
{
for (int i = 0; i != 3; ++i)
x_[i] -= other.x_[i];
return *this;
}
const Point3D& multiply(double scalar)
{
for (int i = 0; i != 3; ++i)
x_[i] *= scalar;
return *this;
}
const Point3D& divide(double scalar)
{
return multiply(1 / scalar);
}
const Point3D& unitise()
{
return divide(length());
}
// Returns the component of other parallel to this.
Point3D projectionOfOther(const Point3D& other) const
{
double factor = dot(other) / lengthSquared();
Point3D projection(*this);
projection.multiply(factor);
return projection;
}
double x_[3];
};
void print(const Point3D& p)
{
std::cout << p.x_[0] << ", " << p.x_[1] << ", " << p.x_[2] << ", " << std::endl;
}
typedef Point3D Point3D;
Point3D difference(const Point3D& a, const Point3D& b)
{
return Point3D(a).subtract(b);
}
Point3D sum(const Point3D& a, const Point3D& b)
{
return Point3D(a).add(b);
}
Point3D crossProduct(const Point3D& v1 ,const Point3D& v2)
{
return Point3D(v1.x_[1] * v2.x_[2] - v1.x_[2] * v2.x_[1],
v1.x_[2] * v2.x_[0] - v1.x_[0] * v2.x_[2],
v1.x_[0] * v2.x_[1] - v1.x_[1] * v2.x_[0]
);
}
Point3D crossProductFromThreePoints(std::vector<Point3D>::iterator first)
{
std::vector<Point3D>::iterator second = first + 1;
std::vector<Point3D>::iterator third = first + 2;
return crossProduct(difference(*third, *first), difference(*third, *second));
}
std::vector<Point3D> makeCloud(int numPoints);
Point3D determinePlaneFromCloud(const std::vector<Point3D>& cloud)
{
std::vector<Point3D> randomised(cloud);
int extra = 3- randomised.size() % 3;
// Might not need this shuffle; but should reduce problems should neighbouring points in the list tend to be close in 3D space
// giving small triangles that might have normals a long way from the main plane.
random_shuffle(begin(randomised), end(randomised));
randomised.insert(end(randomised), begin(randomised), begin(randomised) + extra);
int numTriangles = randomised.size() / 3;
Point3D normals;
int primaryDir = -1;
for (int tri = 0; tri != numTriangles; ++tri )
{
Point3D cp = crossProductFromThreePoints(begin(randomised) + tri * 3);
cp.unitise();
// The first triangle defines the component we'll look at to determine orientation.
if (primaryDir < 0)
primaryDir = cp.maxIndex();
// Flip the orientation if needed.
if (cp.x_[primaryDir] < 0)
cp.multiply(-1);
normals.add(cp);
}
// Now we have a candidate normal to the plane.
// Scale it so that we have normal.p = 1 for each p in the cloud. This is only an average in the case where the p are not completely planar.
double sum = std::accumulate(std::begin(cloud), std::end(cloud), 0.0, [normals](double sum, const Point3D& p) { return sum + p.dot(normals); });
double meanC = sum / cloud.size();
normals.divide(meanC);
return normals;
}
// Return an orthonormal basis whose first direction is aligned with v1
std::vector<Point3D> createOrthonormalBasis(const Point3D& v1)
{
// Need a complete basis as a starting point.
std::vector<Point3D> basis(3);
basis[0] = v1;
// This gives the direction in the current basis with which v1 is closest aligned.
int mi = v1.maxIndex();
// start with the other vectors being the other principle directions from mi; this ensures that the basis is complete.
basis[1].x_[(mi + 1) % 3 ] = 1;
basis[2].x_[(mi + 2) % 3] = 1;
// Now perform a Gram–Schmidt process to make that an Orthonormal basis.
for (int i = 0; i != 3; ++i)
{
for (int j = 0; j != i; ++j)
{
basis[i].subtract(basis[j].projectionOfOther(basis[i]));
}
basis[i].unitise();
}
return basis;
}
Point3D convertBasis(const Point3D& p, const std::vector<Point3D>& newBasis)
{
Point3D newCoords;
for (int i = 0; i != 3; ++i)
{
newCoords.x_[i] = p.dot(newBasis[i]);
}
return newCoords;
}
int main()
{
std::vector<Point3D> cloud = makeCloud(99);
Point3D normal = determinePlaneFromCloud(cloud);
print(normal);
// Now we want to express each point p in the form:
// p = n + au + bv
// where n.u = n.v = u.v = 0
std::vector<Point3D> basis = createOrthonormalBasis(normal);
// The [1] and [2] components are now the 2D locations in the desired plane.
for each (Point3D p in cloud)
{
Point3D mapped = convertBasis(p, basis);
print(mapped);
}
return 0;
}
std::vector<Point3D> makeCloud(int numPoints)
{
std::default_random_engine generator;
std::uniform_real_distribution<double> distribution(0.0, 1.0);
std::vector<Point3D> cloud(numPoints);
Point3D planeNormal;
for (int i = 0; i != 2; ++i)
planeNormal.x_[i] = distribution(generator);
// Dodgy!
while (planeNormal.x_[2] < 1e-2)
planeNormal.x_[2] = distribution(generator);
for (int i = 0; i != numPoints; ++i)
{
for (int j = 0; j != 2; ++j)
cloud[i].x_[j] = distribution(generator);
cloud[i].x_[2] = (1 - cloud[i].x_[0] * planeNormal.x_[0] - cloud[i].x_[1] * planeNormal.x_[1]) / planeNormal.x_[2];
// Add in some noise.
// for (int j = 0; j != 3; ++j) cloud[i].x_[j] += distribution(generator) * 0.05;
}
planeNormal.unitise();
return cloud;
}
来源：https://stackoverflow.com/questions/23814234/convert-3d-plane-to-2d