[QUESTION] Scan-to-scan odometry with TEASER-plusplus (Quatro version)

[InguChoi]

Have you read the documentation?

• [x] Yes
• [ ] No - then this issue will be closed.

I saw these isseus.

1. https://github.com/MIT-SPARK/TEASER-plusplus/issues/49
2. https://github.com/MIT-SPARK/TEASER-plusplus/issues/110

Post your theoretical questions / usage questions here.

Hello, I want to calculate an odometry of our mobile robot by using the TEASER-plusplus package (Especially, Quatro version)

The example of quatro in TEASER-plusplus package is excuted well like below screen capture.

So, I make a ros2 package including TEASER-plusplus cmake. The progress is below

1. Prepare the dataset (Point Cloud Data{t=1}, Point Cloud Data{t=2}) from pointcloud callback function
2. Convert the sensor_msgs::msg::PointCloud2::SharedPtr to pcl::PointCloud<pcl::PointXYZ>::Ptr
3. Convert the pcl::PointCloud<pcl::PointXYZ>::Ptr to teaser::PointCloud
4. So, I have the data called teaser::PointCloud tgt_cloud (=Point Cloud Data{t=1}) and teaser::PointCloud src_cloud (=Point Cloud Data{t=2})
5. Excute the example code (https://github.com/MIT-SPARK/TEASER-plusplus/blob/master/examples/teaser_cpp_fpfh/quatro_cpp_fpfh.cc)

Total code like below.

    case LO_ALGORITHM_MODE_QUATRO:{

      teaser::PointCloud tgt_cloud;
      teaser::PointCloud src_cloud;

      Eigen::Matrix4d T = Eigen::Matrix4d::Identity();

      size_t min_size = std::min(pcl_point_cloud_filter->size(), pcl_point_cloud_pre_filter->size());

      pclToTeaser(pcl_point_cloud_filter, src_cloud, min_size);
      pclToTeaser(pcl_point_cloud_pre_filter, tgt_cloud, min_size);

      std::cout << "src_cloud.size() = " << src_cloud.size() << std::endl;
      std::cout << "tgt_cloud.size() = " << tgt_cloud.size() << std::endl;

      // Compute FPFH
      teaser::FPFHEstimation fpfh;
      auto obj_descriptors = fpfh.computeFPFHFeatures(src_cloud, 0.02, 0.04);
      auto scene_descriptors = fpfh.computeFPFHFeatures(tgt_cloud, 0.02, 0.04);

      teaser::Matcher matcher;
      auto correspondences = matcher.calculateCorrespondences(
          src_cloud, tgt_cloud, *obj_descriptors, *scene_descriptors, false, true, false, 0.95);

      // Prepare solver parameters
      teaser::RobustRegistrationSolver::Params quatro_param, teaser_param;
      getParams(NOISE_BOUND / 2, "Quatro", quatro_param);
      std::chrono::steady_clock::time_point begin_q = std::chrono::steady_clock::now();
      teaser::RobustRegistrationSolver Quatro(quatro_param);
      Quatro.solve(src_cloud, tgt_cloud, correspondences);
      std::chrono::steady_clock::time_point end_q = std::chrono::steady_clock::now();
      auto solution_by_quatro = Quatro.getSolution();

      std::cout << "=====================================" << std::endl;
      std::cout << "           Quatro Results            " << std::endl;
      std::cout << "=====================================" << std::endl;
      double rot_error_quatro, ts_error_quatro;
      calcErrors(T, solution_by_quatro.rotation, solution_by_quatro.translation,
                rot_error_quatro, ts_error_quatro);
      // Compare results
      std::cout << "Expected rotation: " << std::endl;
      std::cout << T.topLeftCorner(3, 3) << std::endl;
      std::cout << "Estimated rotation: " << std::endl;
      std::cout << solution_by_quatro.rotation << std::endl;
      std::cout << "Error (rad): " << rot_error_quatro << std::endl;
      std::cout << "Expected translation: " << std::endl;
      std::cout << T.topRightCorner(3, 1) << std::endl;
      std::cout << "Estimated translation: " << std::endl;
      std::cout << solution_by_quatro.translation << std::endl;
      std::cout << "Error (m): " << ts_error_quatro << std::endl;
      std::cout << "Time taken (s): "
                << std::chrono::duration_cast<std::chrono::microseconds>(end_q - begin_q).count() /
                      1000000.0 << std::endl;
      std::cout << "=====================================" << std::endl;

      break;
    }

The rviz with two point cloud is like this.

The terminal is like this.

1. Is it possible to calculate the odometry (scan-to-scan) by using TEASER++?
2. Why the Max core number is 0 in the terminal?
3. Do point cloud data have to be the same size? (For your information, I forced myself to fit the smaller of the two point cloud data.)

Thank you!

[LimHyungTae]

1-1. Is voxelization applied? 1-2. In outdoor cases, radiuses should be larger (see how we set the parameter: https://arxiv.org/abs/2409.15615)

2. I guess it's because of the radii settings of 0.02 and 0.04, respectively. It's too small in outdoor cases.
3. How can the source and target point clouds be of the same size? could you elaborate on that more?