insertion of deskewed points into ivox discards normals

[themightyoarfish]

I am trying to fuse pointclouds with normals with the integrated_ct_icp_factor, and it crashes when trying to access the frame normals for computing the error. I tracked this down to this code in the dynamic integrator

  auto deskewed = std::make_shared<FrameCPU>(factor->deskewed_source_points(values));
  deskewed->add_covs(covariance_estimation.estimate(deskewed->points_storage, neighbors));
  deskewed->add_intensities(points->intensities, points->size());
  target_ivox->insert(*deskewed);

Here, the ivox structure complains about this frame not having normals, whereas the first inserted frame did have normals. So it appears the the ct_icp factor is currently broken. Or am I misunderstanding something?

It prints

error: inconsistent input point attributes (normals)

and crashes here (line numbers may be changed a bit wrt to origin/main):

(lldb) bt
* thread #1, queue = 'com.apple.main-thread', stop reason = EXC_BAD_ACCESS (code=1, address=0x20)
  * frame #0: 0x00000001016558f4 libdirect_visual_lidar_calibration.dylib`Eigen::PlainObjectBase<Eigen::Matrix<double, 4, 1, 0, 4, 1>>::PlainObjectBase(this=0x000000016fdfccc0, other=0x0000000000000020) at PlainObjectBase.h:512:17
    frame #1: 0x00000001016558bc libdirect_visual_lidar_calibration.dylib`Eigen::Matrix<double, 4, 1, 0, 4, 1>::Matrix(this=0x000000016fdfccc0, other=0x0000000000000020) at Matrix.h:414:55
    frame #2: 0x0000000101655888 libdirect_visual_lidar_calibration.dylib`Eigen::Matrix<double, 4, 1, 0, 4, 1>::Matrix(this=0x000000016fdfccc0, other=0x0000000000000020) at Matrix.h:415:5
    frame #3: 0x000000010186801c libdirect_visual_lidar_calibration.dylib`auto vlcal::frame::normal<vlcal::iVox, (void*)0>(t=0x0000600000e1c120, i=1) at frame_traits.hpp:146:10
    frame #4: 0x0000000101861044 libdirect_visual_lidar_calibration.dylib`vlcal::IntegratedCT_ICPFactor_<vlcal::iVox, vlcal::Frame>::error(this=0x0000000121b3f188, values=0x000000016fdfd7a0) const at integrated_ct_icp_factor_impl.hpp:85:33
    frame #5: 0x000000010a575324 libgtsam.4.dylib`gtsam::NonlinearFactorGraph::error(gtsam::Values const&) const + 80
    frame #6: 0x000000010a567d2c libgtsam.4.dylib`gtsam::LevenbergMarquardtOptimizer::LevenbergMarquardtOptimizer(gtsam::NonlinearFactorGraph const&, gtsam::Values const&, gtsam::LevenbergMarquardtParams const&) + 88
    frame #7: 0x0000000101846e20 libdirect_visual_lidar_calibration.dylib`vlcal::DynamicPointCloudIntegrator::insert_points(this=0x000000016fdfe310, raw_points_=std::__1::shared_ptr<const vlcal::Frame>::element_type @ 0x0000000121b45b88 strong=2 weak=1) at dynamic_point_cloud_integrator.cpp:101:14

[koide3]

deskewed has only covariance matrices and lacks normals. You need to add normals by calling add_normals if you want to use CT_ICP. There is also CT_GICP that uses covariances matrices, and it should work with deskewed in your post.

BTW, we released gtsam_points that includes refined implementations of iVox and CT_ICP, which may be more suitable to your purpose.

[themightyoarfish]

How would I go about adding the normals there? My input points to the dynamic integrator already do have normals.

[koide3]

You can let covariance_estimation estimate normals in addition to covs:

std::vector<Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d>> normals;
std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>> covs;
covariance_estimation.estimate(deskewed->points_storage, neighbors, normals, covs);

auto deskewed = std::make_shared<FrameCPU>(factor->deskewed_source_points(values));
deskewed->add_normals(normals);
deskewed->add_covs(covs);
deskewed->add_intensities(points->intensities, points->size());
target_ivox->insert(*deskewed);

It is also possible to transform normals of your input point cloud using the interpolation result of CT_ICP. This way will be more efficient but complicated. If you want to try it, use get_time_table(), get_time_indices(), and get_source_poses() in CT_ICP.

direct_visual_lidar_calibration/include/vlcal/common/integrated_ct_icp_factor.hpp

Line 64 in 105ca48

const std::vector<gtsam::Pose3, Eigen::aligned_allocator<gtsam::Pose3>>& get_source_poses() const { return source_poses; }