CloudCamera::calculateNormalsOrganized() failed when processing PointCloudXYZRGBA

[sharronliu]

When input point clouds are XYZRGBA, cloud camera failed at normal calculation.

The assertion failure message:
/usr/local/include/eigen3/Eigen/src/Core/PlainObjectBase.h:241: void Eigen::PlainObjectBase<Derived>::resize(Eigen::PlainObjectBase<Derived>::Index, Eigen::PlainObjectBase<Derived>::Index) [with Derived = Eigen::Matrix<double, 3, -1>; Eigen::PlainObjectBase<Derived>::Index = long int]: Assertion (!(RowsAtCompileTime!=Dynamic) || (nbRows==RowsAtCompileTime)) && (!(ColsAtCompileTime!=Dynamic) || (nbCols==ColsAtCompileTime)) && (!(RowsAtCompileTime==Dynamic && MaxRowsAtCompileTime!=Dynamic) || (nbRows<=MaxRowsAtCompileTime)) && (!(ColsAtCompileTime==Dynamic && MaxColsAtCompileTime!=Dynamic) || (nbCols<=MaxColsAtCompileTime)) && nbRows>=0 && nbCols>=0 && "Invalid sizes when resizing a matrix or array."' failed.

GDB back trace when the assertion happened:
Thread 1 "grasp_library" received signal SIGABRT, Aborted.
__GI_raise (sig=sig@entry=6) at ../sysdeps/unix/sysv/linux/raise.c:51
51 ../sysdeps/unix/sysv/linux/raise.c: No such file or directory.
(gdb) bt
0 0x00007ffff46b5e97 in __GI_raise (sig=sig@entry=6) at ../sysdeps/unix/sysv/linux/raise.c:51
1 0x00007ffff46b7801 in __GI_abort () at abort.c:79
2 0x00007ffff46a739a in __assert_fail_base (fmt=0x7ffff482e7d8 "%s%s%s:%u: %s%sAssertion %s' failed.\n%n", assertion=assertion@entry=0x5555555b6600 "(!(RowsAtCompileTime!=Dynamic) || (nbRows==RowsAtCompileTime)) && (!(ColsAtCompileTime!=Dynamic) || (nbCols==ColsAtCompileTime)) && (!(RowsAtCompileTime==Dynamic && MaxRowsAtCompileTime!=Dynamic) || ("..., file=file@entry=0x5555555b65c0 "/usr/local/include/eigen3/Eigen/src/Core/PlainObjectBase.h", line=line@entry=241, function=function@entry=0x5555555b77e0 <Eigen::PlainObjectBase<Eigen::Matrix<double, 3, -1, 0, 3, -1> >::resize(long, long)::PRETTY_FUNCTION> "void Eigen::PlainObjectBase::resize(Eigen::PlainObjectBase::Index, Eigen::PlainObjectBase::Index) [with Derived = Eigen::Matrix<double, 3, -1>; Eigen::PlainObjectBase<Derive"...) at assert.c:92
3 0x00007ffff46a7412 in __GI___assert_fail (assertion=0x5555555b6600 "(!(RowsAtCompileTime!=Dynamic) || (nbRows==RowsAtCompileTime)) && (!(ColsAtCompileTime!=Dynamic) || (nbCols==ColsAtCompileTime)) && (!(RowsAtCompileTime==Dynamic && MaxRowsAtCompileTime!=Dynamic) || ("..., file=0x5555555b65c0 "/usr/local/include/eigen3/Eigen/src/Core/PlainObjectBase.h", line=241, function=0x5555555b77e0 <Eigen::PlainObjectBase<Eigen::Matrix<double, 3, -1, 0, 3, -1> >::resize(long, long)::PRETTY_FUNCTION> "void Eigen::PlainObjectBase::resize(Eigen::PlainObjectBase::Index, Eigen::PlainObjectBase::Index) [with Derived = Eigen::Matrix<double, 3, -1>; Eigen::PlainObjectBase<Derive"...) at assert.c:101
4 0x00005555555800fa in Eigen::PlainObjectBase<Eigen::Matrix<double, 3, -1, 0, 3, -1> >::resize(long, long) ()
5 0x00007ffff6e221d8 in CloudCamera::calculateNormalsOrganized() () at /usr/local/lib/libgrasp_candidates_generator.so
6 0x00007ffff6e24596 in CloudCamera::calculateNormals(int) () at /usr/local/lib/libgrasp_candidates_generator.so
7 0x00007ffff6e03f1a in CandidatesGenerator::preprocessPointCloud(CloudCamera&) () at /usr/local/lib/libgrasp_candidates_generator.so
8 0x000055555557083d in GraspLibraryNode::detectGraspPosesInTopic() ()
9 0x00005555555705a1 in GraspLibraryNode::onInit() ()
10 0x0000555555571196 in main ()

[sharronliu]

To my analysis, when processing XYZRGBA pointclouds, cloud_normals->getMatrixXfMap() returns a matrix of 8 rows, which introduces failure when being resized to the normals_, type of Matrix3Xd (3 rows).

A simple way to fix this issue, and to make the codes more safe, is to invoke getMatrixXfMap with specified (dim, stride), as explained in the PCL docs.

cloud_normals->getMatrixXfMap(3, 4, 0)

This will ensure the normals_ only contains 3 rows, XYZ which is as expected. I will make a patch for this.

[sharronliu]

@atenpas I added a patch for this issue. Could you review?