[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.

<Questions>

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!