CODEWITHSUNDEEP
matrixandroid-ndkpoint-cloud-library
Ronald
Ronald

Reputation: 754

Iterative closest point returns incorrect transformation matrix

I want to create a point cloud by moving and rotating my depth sensor(the structure sensor). What I have so far is the following:

Although I'm moving the sensor slowly new points are added in strange locations so I'm not sure if I'm doing this right. I have a feeling the transformation matrix is wrong because I get values in my translation vector ranging from really low(0,00008) to pretty high (3,00000), these values are both negative and positive.

Some extra information:

Any chance someone can help me out?

Edit, added some code

Filtering

pcl::VoxelGrid<pcl::PointXYZ> grid;

grid.setLeafSize (5.01, 5.01, 5.01);
grid.setInputCloud (cloud_in);
grid.filter (*in);

grid.setInputCloud (cloud_out);
grid.filter (*out);

Point adding

pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
icp.setInputCloud(out);
icp.setInputTarget(in);
pcl::PointCloud<pcl::PointXYZ> Final;
pcl::PointCloud<pcl::PointXYZ> transformedCloud;
icp.setMaxCorrespondenceDistance(0.1);
icp.setRANSACIterations(5);
icp.setMaximumIterations(20);
icp.setTransformationEpsilon(1e-2);
icp.setEuclideanFitnessEpsilon(1e-5);

icp.align(Final);

if(icp.hasConverged()){
    Eigen::Matrix4f transformationMatrix = icp.getFinalTransformation();

    pcl::transformPointCloud(*cloud_out, *cloud_out, transformationMatrix);
    vtkSmartPointer<vtkCellArray> conn = vtkSmartPointer<vtkCellArray>::New();

    vtkPoints* oldPoints = totalPolyData->GetPoints();

    for(pcl::PointCloud<pcl::PointXYZ>::iterator it = cloud_out->begin(); it != cloud_out->end(); it++){
        if(!find((double) it->x, (double) it->y, (double) it->z)){
            oldPoints->InsertNextPoint((double) it->x, (double) it->y, (double) it->z);
        }
    }

    totalPolyData->SetPoints(oldPoints);
    for (int i = 0; i < oldPoints->GetNumberOfPoints(); i++)
        conn->InsertNextCell(1, &i);
    totalPolyData->SetVerts(conn);

Upvotes: 0

Views: 716

Answers (1)

Vtik
Vtik

Reputation: 3140

You may try to concatenate the two pointclouds entirely (not adding only the non existent points) and check if it's good or not. It seems like something wrong is going there and I suspect that it's PCL fault. Can you post the filtering and points adding code you're using ?

Upvotes: 0

Related Questions