Pcl: Assertion error when using PCL PointToPlane objects

[msy22]

Your Environment

• Operating System and version: Ubuntu 14.04
• Compiler: Cmake/catkin (I'm using PCL with ROS)
• PCL Version: 1.7.2

Context

I'm trying to get PCL's TransformationEstimationPointToPlane (and it's variants) working. I've started with the PCL Interactive ICP demo and added the code snippet described in this thread. However, while everything compiles I get an assertion error when trying to run the code (see below). The assertion error tells me that either my point representation is wrong or my cloud contains inf/NaN. However I'm using the PointNormal type as specified in the sample code, and I've confirmed that my clouds don't contain inf or NaN.

I'm not entirely sure where to log this, so I've also posted the same issue on the mailing list here.

Expected Behavior

I expect the code to work the same way the regular interactive demo works, but using Point-to-Plane ICP instead of the default Point-to-Point.

Current Behavior

I get the following error:

point_to_plane_icp_test: /build/buildd/pcl-1.7-1.7.2/kdtree/include/pcl/kdtree/impl/
kdtree_flann.hpp:136: int pcl::KdTreeFLANN<PointT, Dist>::nearestKSearch(const 
PointT&, int, std::vector<int>&, std::vector<float>&) const [with PointT = pcl::PointNormal; 
Dist = flann::L2_Simple<float>]: Assertion `point_representation_->isValid (point) 
&& "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"' failed.
Aborted (core dumped)

Specifically, if I use either of:

• TransformationEstimationPointToPlaneLLS
• TransformationEstimationPointToPlaneLLSWeighted

Then I get this error. If I use either of:

• TransformationEstimationPointToPlane
• TransformationEstimationPointToPlaneWeighted

Then I get no error, the code runs through fine. But the transformation matrix generated is just the identity matrix, and the two objects are not aligned at all, or even moved closer to each other:

Code to Reproduce

My code additions are:

#include <pcl/features/normal_3d.h>  
#include <pcl/registration/transformation_estimation_point_to_plane_lls.h>
... 
typedef pcl::registration::TransformationEstimationPointToPlaneLLS<pcl::PointNormal, pcl::PointNormal> PointToPlane; 
... 
pcl::NormalEstimation<pcl::PointNormal, pcl::PointNormal> norm_est; 
norm_est.setSearchMethod (pcl::search::KdTree<pcl::PointNormal>::Ptr (new pcl::search::KdTree<pcl::PointNormal>)); 
norm_est.setKSearch (10); 
norm_est.setInputCloud (cloud_icp); 
norm_est.compute (*cloud_icp); 
boost::shared_ptr<PointToPlane> point_to_plane(new PointToPlane); 
... 
// Normal ICP object setup 
... 
icp.setTransformationEstimation(point_to_plane); 
... 
// rest of the code continues as normal.

In addition, I've re-written the demo code to use the PointNormal type instead of the PointXYZ type. I've also added the following code to demonstrate that both clouds don't contain inf or NaN:

printf("Does the input cloud (cloud_in) contain inf/NaN?: ");
if (cloud_in->is_dense) {printf("No\n");} else {printf("Yes\n");}
printf("Does the output cloud (cloud_icp) contain inf/NaN?: ");
if (cloud_icp->is_dense) {printf("No\n");} else {printf("Yes\n");}

(In both cases I get "No")

Possible Solution

Not sure because I don't know enough about the underlying code. I'm aware of the same error message being reported in a different thread here. But in my case I've already confirmed that my transformation matrix doesn't contain any inf or NaN.

[taketwo]

1. PCL 1.7.2 was released more than 3 years ago, we have 1395 commits in the master branch since then. There is a high chance that the issue you are facing has been resolved.
2. Where are your point clouds coming from? is_dense is only a flag and it can be that the producer of the point cloud did not set it properly. To be absolutely sure that there are no infinite values in the point cloud, loop through the points and test each one with pcl::isFinite().

[msy22]

Thanks for the prompt response Taketwo! My point clouds are loaded from a .pcd file using the following code:

pcl::PCLPointCloud2 blob;
pcl::io::loadPCDFile (input_filename, blob);
pcl::PointCloud<PointNormal>::Ptr cloud_tmp (new pcl::PointCloud<PointNormal>);
pcl::fromPCLPointCloud2 (blob, *cloud_tmp);

Or:

pcl::PCLPointCloud2 blob;
pcl::io::loadPCDFile (input_filename, blob);
pcl::PointCloud<PointXYZ>::Ptr cloud_tmp (new pcl::PointCloud<PointXYZ>);
pcl::fromPCLPointCloud2 (blob, *cloud_tmp);
pcl::PointCloud<pcl::PointNormal>::Ptr cloud (new pcl::PointCloud<pcl::PointNormal>);
pcl::copyPointCloud(*cloud_tmp, *cloud);

I find the first block of code gives me the following warnings:

Failed to find match for field 'normal_x'.
Failed to find match for field 'normal_y'.
Failed to find match for field 'normal_z'.
Failed to find match for field 'curvature'.

Which I assume is just because the loadPCDFile can't find normal information in the .pcd file. So I've tended to use the second block of code.

I'll try PCL 1.8 and the pcl::isFinite() function and then report back.

[msy22]

I've had a setback with this issue as I've encountered an error that is preventing me from installing PCL 1.8. I've filed a new issue here

[msy22]

I've also done as you suggested and used pcl::isFinite() to test each point individually, using the following code:

pcl::PointCloud<pcl::PointNormal>::iterator itr;
for (itr = cloud_icp->begin(); itr != cloud_icp->end(); itr++)
{
    if (!pcl::isFinite(*itr))
    {
        printf("Found a point with inf!");
        cout << itr->x << "," << itr->y << "," << itr->z << endl;
    }
}

However, like when I used is_dense and removeNaNFromPointCloud, it appears that the clouds are fine and don't contain any NaN's or inf's.

[taketwo]

Hm, maybe also check if normals are finite? pcl::isFinite only tests XYZ coordinates. Try to add pcl_isfinite(itr->normal_x).

[msy22]

Ran the following code, but nothing turned up. Still seems like the clouds themselves are fine

pcl::PointCloud<pcl::PointNormal>::iterator itr;
for (itr = input_cloud->begin(); itr != input_cloud->end(); itr++)
{
    if (!pcl::isFinite(*itr))
    {
        printf("Found a point with inf!");
        cout << itr->x << "," << itr->y << "," << itr->z << endl;
    }
    if ((!pcl_isfinite(itr->normal_x)) ||
        (!pcl_isfinite(itr->normal_y)) ||
        (!pcl_isfinite(itr->normal_z)))
    {
        printf("Found a point normal with inf!");
        cout << itr->normal_x << "," << itr->normal_y << ","
             << itr->normal_z << endl;
    }
}

[msy22]

I've also gone through and printed each point, e.g.

Point: -0.0475816,0.70065,0.443133 Normal: 0.982233,0.0111219,-0.187335

And then searched through the terminal output for any instance of "NaN" or "inf" but again, nothing turns up. So I really do think that the clouds themselves are fine. My inclination is that the error is related to the point representation, and that I'm doing something unusual somewhere along the way to mess with the assertion. Either that or it is a genuine bug.

[msy22]

So, after spending literally the entire last two days trying to install PCL 1.8, I finally got it working (dealing with Cmake is an absolute nightmare)...

...and...

...I still get the same assertion error, this time with PCL 1.8:

point_to_plane_icp_test: /usr/include/pcl-1.8/pcl/kdtree/include/pcl/kdtree/impl/
kdtree_flann.hpp:136: int pcl::KdTreeFLANN<PointT, Dist>::nearestKSearch(const PointT&, 
int, std::vector<int>&, std::vector<float>&) const [with PointT = pcl::PointNormal; 
Dist = flann::L2_Simple<float>]: Assertion `point_representation_->isValid (point) && 
"Invalid (NaN, Inf) point coordinates given to nearestKSearch!"' failed.
Aborted (core dumped)

[taketwo]

I checked the relevant code and this assertion triggers only if any of the x, y, z fields of a point is infinite. Since your input is confirmed to be finite, it must be the transformations that are applied during alignment that cause the trouble. Unfortunately I don't have time to dive deeper into this. If you really need this to work, I'd suggest to add some debug prints in "icp.hpp" to check the transformation matrices.

[msy22]

No worries, I really appreciate the time and effort you've spent so far to help me!

Your suggestion is a good idea, so I'll spend a few more days digging around the source code and see if I can nail down the exact issue.

[msy22]

So, I added some code to each function in icp.hpp, e.g.

std::cout << "Running IterativeClosestPoint::computeTransform" << std::endl;

to narrow down the error to a particular function. But I never see the print out, so the code clearly doesn't even reach these functions before hitting the assertion block.

The assertion failure comes from this line in kdtree_flann.hpp. I would like to print out the XYZ values of point just before this assertion happens, but what is the correct syntax? I can't for the life of my figure out how to work with the template format.

[SergioRAgostinho]

The assertion failure comes from this line in kdtree_flann.hpp. I would like to print out the XYZ values of point just before this assertion happens, but what is the correct syntax? I can't for the life of my figure out how to work with the template format.

std::cout << point << std::endl;

This should do the trick. Invoke it before the assert.

[taketwo]

So, I added some code to each function in icp.hpp ... But I never see the print out

Sorry, but I have to double-check. Where is the file you are editing located? Are you sure it is the file that is eventually included in your program? Try to add debug output somewhere it will definitely be executed, e.g. in class constructor.

[msy22]

@SergioRAgostinho I just tried that and got this error:

/usr/include/c++/4.8/bits/basic_string.h:2753:5: note:   template argument deduction/substitution failed:
In file included from /usr/include/pcl-1.8/pcl/registration/include/pcl/registration/gicp6d.h:45:0,
                 from /usr/include/pcl-1.8/pcl/registration/src/gicp6d.cpp:39:
/usr/include/pcl-1.8/pcl/kdtree/include/pcl/kdtree/impl/kdtree_flann.hpp:136:52: note:   ‘const 
pcl::PointXYZLAB’ is not derived from ‘const std::basic_string<_CharT, _Traits, _Alloc>’
   std::cout << "Attempting assertion with point: " << point << std::endl;
                                                    ^
make[2]: *** [registration/CMakeFiles/pcl_registration.dir/src/gicp6d.cpp.o] Error 1
make[1]: *** [registration/CMakeFiles/pcl_registration.dir/all] Error 2
make: *** [all] Error 2

Which is a little odd, because the point type shouldn't be pcl::PointXYZLAB, if anything it should be pcl::PointNormal because that's what I set it to. Does this error message mean anything to either of you?

@taketwo No worries. I'm editing two files:

• /usr/include/pcl-1.8/pcl/registration/include/pcl/registration/impl/icp.hpp
• /usr/include/pcl-1.8/pcl/kdtree/include/pcl/kdtree/impl/kdtree_flann.hpp

To both, I've added code that should print whenever that assertion runs on line 136 of kdtree_flann.hpp or when any of the functions in "icp.hpp" run. What I see printed out to terminal is this:

...
Attempting assertion
Attempting assertion
Attempting assertion
Attempting assertion
Attempting assertion
point_to_plane_icp_test: /usr/include/pcl-1.8/pcl/kdtree/include/pcl/kdtree/impl/kdtree_flann.hpp:138: 
int pcl::KdTreeFLANN<PointT, Dist>::nearestKSearch(const PointT&, int, std::vector<int>&, 
std::vector<float>&) const [with PointT = pcl::PointNormal; Dist = flann::L2_Simple<float>]: Assertion 
`point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to 
nearestKSearch!"' failed.
Aborted (core dumped)

Where that "attempting assertion" prints out _exactly_ 50,001 times before hitting the error. The test point cloud I'm using is exactly 50,000 points. So depending on what's happening under the hood, either the second of the two clouds (reference or reading) is failing the assertion, or the reading cloud is fine the first time, but after being transformed once, contains NaN or inf and fails immediately.

For reference, this is the relevant code block:

printf("Before ICP\n");
pcl::IterativeClosestPoint<pcl::PointNormal, pcl::PointNormal> icp;
icp.setTransformationEstimation(point_to_plane);
icp.setMaximumIterations (iterations);
icp.setInputSource (cloud_icp);
icp.setInputTarget (cloud_in);
icp.align (*cloud_icp);
icp.setMaximumIterations (1);  // We set this variable to 1 for the next time we will call .align () function
printf("After ICP\n");

The 50,001 "Attempting assertion" messages appear after "Before ICP" but the error message appears and crashes the program before "After ICP" can be printed.

[SergioRAgostinho]

Which is a little odd, because the point type shouldn't be pcl::PointXYZLAB, if anything it should be pcl::PointNormal because that's what I set it to. Does this error message mean anything to either of you?

You just found a minor bug. Nobody defined the "output stream operator" for PointXYZLAB as it is for all other point types in this file. This just means you'll have to print the internal members of the point individually.

https://github.com/PointCloudLibrary/pcl/blob/637ba7e2a08089b476c537ee4a58e8b0898fa218/registration/include/pcl/registration/gicp6d.h#L50-L64

I can't really say if it's strange or not, because I haven't seen the code which invokes gicp6d anywhere on this issue. But the error you're having does explicitly say the file is being included from gicp6d.cpp

Where that "attempting assertion" prints out _exactly_ 50,001 times before hitting the error. The test point cloud I'm using is exactly 50,000 points. So depending on what's happening under the hood, either the second of the two clouds (reference or reading) is failing the assertion, or the reading cloud is fine the first time, but after being transformed once, contains NaN or inf and fails immediately.

This feels like some iteration is going out of range.

[msy22]

I'll try defining the output stream operator for PointXYZLAB and see how that works, because at the moment I can't figure out the correct syntax to print the points individually. Trying to do it in kdtree_flann.hpp like so:

std::cout << "Attempting assertion with point: " << point.x << ", " << point.y << ", " << point.z << std::endl;

just generates a whole bunch of errors along the lines of:

/usr/include/pcl-1.8/pcl/kdtree/include/pcl/kdtree/impl/kdtree_flann.hpp:136:52: error: ‘const struct pcl::ReferenceFrame’ has no member named ‘x’
   std::cout << "Attempting assertion with point: " << point.x << ", " << point.y << ", " << point.z << std::endl;
                                                    ^

This feels like some iteration is going out of range.

Any advice on how I might confirm/deny this? I'm not at all familiar with the underlying PCL source code unfortunately. So it will take some time to dig through.

[msy22]

I tried defining the output stream operator for PointXYZLAB but I didn't get anywhere, mostly because I don't fully understand the source code and only code what I've seen. I'd need a detailed walk-through to do that myself.

[SergioRAgostinho]

just generates a whole bunch of errors along the lines of:

/usr/include/pcl-1.8/pcl/kdtree/include/pcl/kdtree/impl/kdtree_flann.hpp:136:52: error: ‘const struct pcl::ReferenceFrame’ has no member named ‘x’
   std::cout << "Attempting assertion with point: " << point.x << ", " << point.y << ", " << point.z << std::endl;

That's because KdTree gets invoked with types which don't have x y z fields like `pcl::ReferenceFrame'. You really need to implement the output stream operator. You have to follow the same pattern of

https://github.com/PointCloudLibrary/pcl/blob/26123cf3197677e569bf0a9b6db8475476b78e69/common/include/pcl/impl/point_types.hpp#L281-L302

Define the operator as a friend and then implement it in the cpp file

https://github.com/PointCloudLibrary/pcl/blob/26123cf3197677e569bf0a9b6db8475476b78e69/common/src/point_types.cpp#L41-L46

Make sure you're encapsulating things in the right name spaces.

I tried defining the output stream operator for PointXYZLAB but I didn't get anywhere, mostly because I don't fully understand the source code and only code what I've seen. I'd need a detailed walk-through to do that myself.

What happened to googling for things before giving up right away? https://stackoverflow.com/questions/476272/how-to-properly-overload-the-operator-for-an-ostream

Any advice on how I might confirm/deny this? I'm not at all familiar with the underlying PCL source code unfortunately. So it will take some time to dig through.

Compile PCL in debug mode. Run your code in gdb. Let the assertion trigger. Jump into the right frame and start printing variables' content. The backtrace will tell you exactly where you are and the contents of your variables.

Important

Start coming up with a code snippet which isolates the code that is failing. Does it rely on specific data? Save it to a PCD file and then just load it before invoking the failing method. I still have no idea what code is failing. Just that it is based of a tutorial.

[msy22]

What happened to googling for things before giving up right away?

I'm sorry, I am googling stuff and trying to learn as much as I can, as fast as I can. But since we're in different hemispheres I figured that it might save you guys time if I ask for detailed help _before_ I need it, rather than after I've already spent a few days trying and possibly failing to do it all myself.

I still have no idea what code is failing. Just that it is based of a tutorial.

Sorry again, let me restated everything as clearly as I can. I am using the PCL interactive ICP demo as a basis, but that really has nothing to do with the error. Specifically, I am running this block of code:

// Set up the PointToPlane object
typedef pcl::registration::TransformationEstimationPointToPlaneLLS<pcl::PointNormal, pcl::PointNormal> PointToPlane; 
boost::shared_ptr<PointToPlane> point_to_plane(new PointToPlane);

// The Iterative Closest Point algorithm
printf("Before ICP\n");
pcl::IterativeClosestPoint<pcl::PointNormal, pcl::PointNormal> icp;
icp.setTransformationEstimation(point_to_plane);
icp.setMaximumIterations (iterations);
icp.setInputSource (cloud_icp);
icp.setInputTarget (cloud_in);
icp.align (*cloud_icp);        // <<< Code fails within this function
printf("After ICP\n");

Where the I am loaded this PCD file: demo_cloud.pcd.zip as both the reference (cloud_in) and reading (cloud_in) file. This cloud contains exactly 50,000 points. The reading cloud is transformed by a pre-determined frame transform before the block of code above executes.

The code fails with this error message:

point_to_plane_icp_test: /build/buildd/pcl-1.7-1.7.2/kdtree/include/pcl/kdtree/impl/
kdtree_flann.hpp:136: int pcl::KdTreeFLANN<PointT, Dist>::nearestKSearch(const 
PointT&, int, std::vector<int>&, std::vector<float>&) const [with PointT = pcl::PointNormal; 
Dist = flann::L2_Simple<float>]: Assertion `point_representation_->isValid (point) 
&& "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"' failed.
Aborted (core dumped)

Which indicates that the assertion error on line 136 in kdtree_flann.hpp is failing, within the function nearestKSearch. Taketwo says that this assertion triggers _only_ if any of the x,y,z fields of a point are infinite. However, I have used multiple different techniques, including inspecting the clouds with (gdb) print *cloud_icp and (gdb) print *cloud_in to confirm that right before this line executes, the clouds do not contain any NaNs or infs.

The full backtrace after the crash is here: backtrace.txt which probably means a lot more to you than it does to me.

Now, if I use GDB to display the inputs to nearestKSearch I can see their contents just before and after the assertion fails. I've copied the relevant GDB output here: variable_display.txt.

The key thing is that (as Taketwo hypothesized) the points leading up to the failure are fine (up to the 50,000th point):

8: point = (const pcl::PointNormal &) @0x7fffc75b4ee0: {<pcl::_PointNormal> = {{data = {-0.0378986895, 0.41527912, 0.595601678, 1}, {x = -0.0378986895, 
        y = 0.41527912, z = 0.595601678}}, {data_n = {0.943280816, -0.210456997, -0.256766558, 0}, normal = {0.943280816, -0.210456997, -0.256766558}, {
        normal_x = 0.943280816, normal_y = -0.210456997, normal_z = -0.256766558}}, {{curvature = 0.00671842694}, data_c = {0.00671842694, 0, 1.21507823e-38, 
        0}}}, <No data fields>}

But the point on which it fails (the 50,001th point) is not :

8: point = (const pcl::PointNormal &) @0x7fffc736b010: {<pcl::_PointNormal> = {{data = {-nan(0x400000), -nan(0x400000), nan(0x400000), 1}, {x = -nan(0x400000), 
        y = -nan(0x400000), z = nan(0x400000)}}, {data_n = {-nan(0x400000), -nan(0x400000), -nan(0x400000), 0}, normal = {-nan(0x400000), -nan(0x400000), 
        -nan(0x400000)}, {normal_x = -nan(0x400000), normal_y = -nan(0x400000), normal_z = -nan(0x400000)}}, {{curvature = 0.00349996984}, data_c = {
        0.00349996984, 0, 1.21507823e-38, 0}}}, <No data fields>}

Now I don't understand the source code well enough yet to understand exactly what stage of the ICP alignment process this is at when everything breaks down, but I'll hopefully be able to post more once I've done more digging.

I'll also start implementing the output stream operator and report back when I've made progress.

[SergioRAgostinho]

I can't load the normals from your pcd file. The header says is has XYZ, rgb and some other scalar field value which I have no idea what it is

$ head -n 11 SX10_corner_cloud_50k.pcd 
# .PCD v0.7 - Point Cloud Data file format
VERSION 0.7
FIELDS Scalar_field rgb x y z _
SIZE 4 4 4 4 4 1
TYPE F F F F F U
COUNT 1 1 1 1 1 4
WIDTH 50000
HEIGHT 1
VIEWPOINT 0 0 0 1 0 0 0
POINTS 50000
DATA binary

Was this the PCD file of cloud_in after the normals were already computed? If it is (I hope not), which parameters did you use to run normal estimation?

[taketwo]

He estimates normals within his program, check the code snippet from the very first post.

[msy22]

@SergioRAgostinho The PCD file doesn't contain normals, they are calculated separately as per the only PointToPlane code snippet I've found, which is in this PCL forum thread. The scalar field is the LiDAR return intensity.

Here's a copy of the whole file (unfortunately I can't share the whole repo): PointToPlane_test.txt but it should show you the commands I'm using and in which order. Regarding normal estimation, I am using this block of code _after_ I load the PCD file:

pcl::NormalEstimation<pcl::PointNormal, pcl::PointNormal> norm_est;
norm_est.setSearchMethod (pcl::search::KdTree<pcl::PointNormal>::Ptr (new pcl::search::KdTree<pcl::PointNormal>));
norm_est.setKSearch (10);
norm_est.setInputCloud (cloud_icp);
norm_est.compute (*cloud_icp);

[msy22]

I've narrowed down the problem to line 222 of icp.hpp. At this line:

transformation_estimation_->estimateRigidTransformation (*input_transformed, *target_, *correspondences_, transformation_);

The variable transformation_ goes in as the identity matrix:

array = {1, 0, 0, 0, 
         0, 1, 0, 0, 
         0, 0, 1, 0, 
         0, 0, 0, 1}

But comes out filled with NaNs:

array = {-nan(0x400000), -nan(0x400000),  nan(0x400000), 0,  
         -nan(0x400000), -nan(0x400000), -nan(0x400000), 0, 
         -nan(0x400000), -nan(0x400000), -nan(0x400000), 0, 
         -nan(0x400000), -nan(0x400000), -nan(0x400000), 1}

This variable is then used to transform the point cloud and everything falls apart after that.

[msy22]

These are the values of the inputs right before the transformation_ matrix gets filled with NaNs:

input_transformed_before.txt
transformation_before.txt
correspondences_before.txt
target_before.txt

@taketwo @SergioRAgostinho I'll keep digging, but in the meantime, do any of these inputs look wrong or odd to either of you? Nothing contains any NaNs or infs.

I haven't been able to get the output stream operator working, and I've found that whenever I use std::cout in either icp.hpp or registration.hpp, it never actually prints anything to terminal. So at this point the only way I'm able to inspect these variables is with GDB

[SergioRAgostinho]

You're not computing the normals to of cloud_in they're all set to 0. So one cloud has normals all set to 0 and the other doesn't and because of that, the transformation estimation is ill-defined.

Modify things like this

// Perform Normal estimation
  pcl::NormalEstimation<pcl::PointNormal, pcl::PointNormal> norm_est;
  norm_est.setSearchMethod (pcl::search::KdTree<pcl::PointNormal>::Ptr (new pcl::search::KdTree<pcl::PointNormal>));
  norm_est.setKSearch (10);
  norm_est.setInputCloud (cloud_in);
  norm_est.compute (*cloud_in);

  // Create, apply and print the transformation matrix
  Eigen::Matrix4d transformation_matrix = transformFromEulerAngles(g_default_rotation,
                                                   g_default_translation);
  std::cout << "Applying this rigid transformation to: cloud_in -> cloud_icp" << std::endl;
  print4x4Matrix (transformation_matrix);
  pcl::transformPointCloudWithNormals (*cloud_in, *cloud_icp, transformation_matrix);

Notice the use of pcl::transformPointCloudWithNormals, instead of pcl::transformPointCloud.

[taketwo]

Notice the use of pcl::transformPointCloudWithNormals, instead of pcl::transformPointCloud.

Side-note: I have a PR in the queue to fix this counter-intuitive interface. pcl::transformPointCloud will become capable of transforming normals (when they are present).

[msy22]

That worked! The program executes successfully:

So essentially the problem then is that both input clouds (target and source) need to have their normals calculated? I really should have posted my full code to begin with, that probably would have saved a lot of time, so my bad - that's on me.

However, I think that in future it would be good to have a short tutorial on using the PointToPlane object, or modify the existing Interactive ICP tutorial to use either.

[SergioRAgostinho]

So essentially the problem then is that both input clouds (target and source) need to have their normals calculated? I really should have posted my full code to begin with, that probably would have saved a lot of time, so my bad - that's on me.

The point type you're using has normals so the algorithm tries to use them. I actually assumed you ran normal estimation exactly with that idea in mind, to use normals as an additional check in ICP. You should always post the code but I only found it out after reproducing the problem and then slowly going after symptoms. So having it could have saved time, but I was looking elsewhere completely.

[msy22]

Ah, I assumed that only the reference cloud would need normals, and that the reading cloud wouldn't.

It also doesn't help that the assertion/error message in this case wasn't particularly indicative of the true problem, I feel like that kind of misled all of us. It would probably be a good idea to have an assertion that actually checks both clouds for normals and would therefore catch this edge case. Or better yet, a tutorial explaining what's happening with a working example.

[msy22]

@SergioRAgostinho So does this then mean that the normals have to be calculated for both the target and source cloud before inputting them to PointToPlane ICP? Or is is possible to input the target as a PointNormal cloud (with it's normals calculated) and the source cloud as a PointXYZ cloud without any normals?

Oh, and I forgot to say, thank you both very much for your help resolving this! I really appreciate it.

[edwincasallas22]

¡Eso funciono! El programa se ejecuta con éxito:

Entonces, el problema es entonces que ambas nubes de entrada (objetivo y fuente) necesitan tener sus normales calculadas. Para empezar, realmente debería haber publicado mi código completo, que probablemente habría ahorrado mucho tiempo, así que mi error, eso es mi culpa.

Sin embargo, creo que en el futuro sería bueno tener un breve tutorial sobre el uso del objeto PointToPlane, o modificar el tutorial de ICP interactivo existente para usar cualquiera de los dos.

Hi, I have the same problem. How did you solve it?

[msy22]

Hi @edwincasallas22.

If you're having the same problem I had, it's because you are calculating point normals for only ONE of your clouds - either the target or source. The PCL point-to-plane objects require BOTH clouds to have their normals calculated, otherwise they give you that incredibly vague error.

As an aside, if you are interested in point cloud registration, I would highly recommend checking out Generalized ICP, as it is faster and more accurate than point-to-plane ICP, and doesn't require you to explicitly calculate normals first.