Transform point cloud's coordinates to another coordinates in Point Cloud Library, which makes the ground plane as the X-O-Y plane?

Question

I have a point cloud from kinect fusion and use Point Cloud Library to segment the ground plane(a x+b y+c*z+d=0) successfully(I got the a,b,c,d in pcl::ModelCoefficients of the ground plane). Now I need to transform the Cartesian coordinates to new Cartesian coordinates that makes the ground plane became the XOY plane(0*x+0*y+z=0). I guess I can do it by this API(but I don't know how): http://docs.pointclouds.org/trunk/group__common.html#transformPointCloud

My Answer : Look at this PCL api: http://docs.pointclouds.org/1.7.2/a02405.html#ga4375e99ec2ae368eec9379f506568611

I successfully solved this problem!

Answer 1

I can't open the link to your API but guess you can transform you plane using simple transformations:

1. you should add to all your points vector {a * d, b * d, c * d} - that moves your points to plane ax + by + cz = 0
2. then you should find rotation matrix around axis [{a, b, c} cross {0, 0, 1}] on angle [{a, b, c} dot {0, 0, 1}] and transform your points by this matrix http://www.euclideanspace.com/maths/geometry/rotations/conversions/angleToMatrix- here you can see how to find the rotation matrix from axis and angle

Answer 2

This function requires camera pose, which is a 4x4 matrix, of the form

| R   t |
| 0   1 |

Here, R is 3x3 rotation matrix, t is a 3x1 translation vector, 0 - is a 1x3 vector of zeros, and 1 is a unity (scalar).

You should design this matrix in such a way, that Z axis in a new coordinate system will be collinear to the normal vector of your plane. New X and Y axes are arbitrary, the only restriction is that they must form orthogonal basis.

This link explains how to derive matrix R.

Answer 3

Now, I meet this question. I want to project point cloud to XY plane,YZ plane, and XZ plane. Finally, the answer be found in this page: https://pcl.readthedocs.io/projects/tutorials/en/latest/project_inliers.html?highlight=ModelCoefficients :

In this tutorial we will learn how to project points onto a parametric model (eg, plane, sphere, etc). The parametric model is given through a set of coefficients – in the case of a plane, through its equation: ax + by + cz + d = 0. Avoid the page missing, copy the code as follow:

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/filters/project_inliers.h>

int
 main (int argc, char** argv)
{
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected (new pcl::PointCloud<pcl::PointXYZ>);

  // Fill in the cloud data
  //We then create the point cloud structure, fill in the respective values,        and display the content on screen.
  cloud->width  = 5;
  cloud->height = 1;
  cloud->points.resize (cloud->width * cloud->height);

  for (auto& point: *cloud)
  {
    point.x = 1024 * rand () / (RAND_MAX + 1.0f);
    point.y = 1024 * rand () / (RAND_MAX + 1.0f);
    point.z = 1024 * rand () / (RAND_MAX + 1.0f);
  }

  std::cerr << "Cloud before projection: " << std::endl;
  for (const auto& point: *cloud)
    std::cerr << "    " << point.x << " "
                        << point.y << " "
                        << point.z << std::endl;

  // Create a set of planar coefficients with X=Y=0,Z=1
  //We fill in the ModelCoefficients values. In this case, we use a plane model, with ax+by+cz+d=0, where a=b=d=0, and c=1, or said differently, the X-Y plane.
  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
  coefficients->values.resize (4);
  coefficients->values[0] = coefficients->values[1] = 0;
  coefficients->values[2] = 1.0;
  coefficients->values[3] = 0;

  // Create the filtering object
  //We create the ProjectInliers object and use the ModelCoefficients defined above as the model to project onto.
  pcl::ProjectInliers<pcl::PointXYZ> proj;
  proj.setModelType (pcl::SACMODEL_PLANE);
  proj.setInputCloud (cloud);
  proj.setModelCoefficients (coefficients);
  proj.filter (*cloud_projected);

  std::cerr << "Cloud after projection: " << std::endl;
  for (const auto& point: *cloud_projected)
    std::cerr << "    " << point.x << " "
                        << point.y << " "
                        << point.z << std::endl;

  return (0);
}

The code above from PCL website.