Remove typedefs and using aliases for PointCloud

include/laser/sensor/sensor_laser_3d.h

@@ -50,10 +50,10 @@ class SensorLaser3d : public SensorBase
     SensorLaser3d(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr);
     SensorLaser3d(const Eigen::VectorXd& _extrinsics, const std::shared_ptr<ParamsSensorLaser3d> _params);
     ~SensorLaser3d();
+    WOLF_SENSOR_CREATE(SensorLaser3d, ParamsSensorLaser3d, 7);
     ParamsSensorLaser3dPtr params_laser3d_;
 };
 
-WOLF_SENSOR_CREATE(SensorLaser3d, ParamsSensorLaser3d, 7);
 
 }  // namespace laser
 }  // namespace wolf

include/laser/utils/laser3d_tools.h

@@ -20,7 +20,6 @@
 //
 //--------LICENSE_END--------
 
-
 #include <pcl/point_types.h>
 #include <pcl/point_cloud.h>
 #include <pcl/common/transforms.h>
@@ -32,27 +31,28 @@
 #include <pcl/registration/icp_nl.h>
 #include <pcl/registration/transforms.h>
 
-using PointCloud = pcl::PointCloud<pcl::PointXYZ>;
-typedef boost::shared_ptr<PointCloud>       PointCloudBoostPtr;
-typedef boost::shared_ptr<const PointCloud> PointCloudBoostConstPtr;
+// using PointCloud = pcl::PointCloud<pcl::PointXYZ>;
+// typedef boost::shared_ptr<PointCloud>       PointCloudBoostPtr;
+// typedef boost::shared_ptr<const PointCloud> PointCloudBoostConstPtr;
 
 namespace wolf
 {
 namespace laser
 {
 // _cloud_ref: first PointCloud
-// _cloud_otherref: first PointCloud
-inline void pairAlign(const PointCloudBoostConstPtr                                      _cloud_ref,
-                      const PointCloudBoostConstPtr                                      _cloud_other,
+// _cloud_other: second PointCloud
+inline void pairAlign(const pcl::PointCloud<pcl::PointXYZ>::Ptr                          _cloud_ref,
+                      const pcl::PointCloud<pcl::PointXYZ>::Ptr                          _cloud_other,
                       const Eigen::Isometry3d                                           &_transform_guess,
                       Eigen::Isometry3d                                                 &_transform_final,
                       pcl::IterativeClosestPointNonLinear<pcl::PointXYZ, pcl::PointXYZ> &_registration_solver,
                       bool                                                               _downsample = false)
 {
     // Internal PointCloud pointers (boost::shared_ptr)
-    PointCloudBoostPtr cloud_ref = boost::make_shared<PointCloud>();  // std::make_shared<PoinCloud> cloud_ref;
-    PointCloudBoostPtr cloud_other =
-        boost::make_shared<PointCloud>();  // std::make_shared<PoinCloud> cloud_other(new PointCloud);
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ref =
+        pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();  
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_other =
+        pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();  
 
     // Downsample for consistency and speed
     // \note enable this for large datasets

test/gtest_laser3d_tools.cpp

@@ -26,34 +26,32 @@
 
 // //Wolf
 #include "core/common/wolf.h"
-// #include "core/math/rotations.h"
 #include "core/utils/utils_gtest.h"
+#include <core/math/rotations.h>
+
+// pcl
+#include <pcl/registration/gicp.h>
+#include <pcl/registration/gicp6d.h>
 
 // Eigen
 #include <Eigen/Geometry>
 
-// //std
+// std
 #include <iostream>
 #include <string>
 #include <filesystem>
 #include <unistd.h>
-// #include <iostream>
-// #include <fstream>
-// #include <iomanip>
 
 //#define write_results
 
 //////  vsainz: i have to write a test
 
-// using namespace wolf
-
-// using namespace Eigenç
+using namespace wolf;
+using namespace Eigen;
 
 std::string laser_root_dir = _WOLF_LASER_ROOT_DIR;
 
-
-
-void loadData(std::string fname, PointCloud& cloud)
+void loadData(std::string fname, pcl::PointCloud<pcl::PointXYZ>& cloud)
 {
     pcl::io::loadPCDFile(fname, cloud);
     // remove NAN points from the cloud
@@ -61,24 +59,21 @@ void loadData(std::string fname, PointCloud& cloud)
     pcl::removeNaNFromPointCloud(cloud, cloud, indices);
 };
 
-TEST(laser3d_tools, pairAlign_identity)
+TEST(pairAlign, identity)
 {
-    //    typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
-    using PointCloud = pcl::PointCloud<pcl::PointXYZ>;
-
     // Load data
-    PointCloudBoostPtr pcl_ref = boost::make_shared<PointCloud>();
+    pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_ref = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
     loadData(laser_root_dir + "/test/data/1.pcd", *pcl_ref);
 
-    PointCloudBoostPtr pcl_other = pcl_ref;
-    // loadData("data/2.pcd", pcl_other);
+    pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_other = pcl_ref;  // same PCld --> ground truth transform is identity
+    WOLF_INFO("Ground truth transform : \n", Matrix4d::Identity());
 
     // Guess
-    Eigen::Isometry3d transformation_guess = Eigen::Translation3d(1,1,1) * Eigen::AngleAxisd(0.1,Eigen::Vector3d(1,2,3).normalized());
+    Isometry3d transformation_guess = Translation3d(1, 1, 1) * AngleAxisd(0.2, Vector3d(1, 2, 3).normalized());
     WOLF_INFO("Initial guess transform: \n", transformation_guess.matrix());
 
     // final transform
-    Eigen::Isometry3d transformation_final;
+    Isometry3d transformation_final;
 
     // Solver configuration
     pcl::IterativeClosestPointNonLinear<pcl::PointXYZ, pcl::PointXYZ> registration_solver;
@@ -91,60 +86,75 @@ TEST(laser3d_tools, pairAlign_identity)
     registration_solver.setMaximumIterations(200);
 
     // Alignment
-    wolf::laser::pairAlign(pcl_ref, pcl_other, transformation_guess, transformation_final, registration_solver, true); //false);
+    wolf::laser::pairAlign(pcl_ref,
+                           pcl_other,
+                           transformation_guess,
+                           transformation_final,
+                           registration_solver,
+                           true);  // false);
 
     WOLF_INFO("Final transform: \n", transformation_final.matrix());
 
-    ASSERT_MATRIX_APPROX(transformation_final.matrix(), Eigen::Matrix4d::Identity(), 1e-2);
+    ASSERT_MATRIX_APPROX(transformation_final.matrix(), Matrix4d::Identity(), 1e-2);
 }
 
-TEST(laser3d_tools, pairAlign_known_transformation)
+TEST(pairAlign, known_transformation)
 {
-    //    typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
-    using PointCloud = pcl::PointCloud<pcl::PointXYZ>;
-
     // Load data
-    PointCloudBoostPtr pcl_ref = boost::make_shared<PointCloud>();
+    pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_ref = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
     loadData(laser_root_dir + "/test/data/1.pcd", *pcl_ref);
 
     // known transform
-    Eigen::Affine3d transformation_known = Eigen::Affine3d::Identity();
+    Affine3d transformation_known = Affine3d::Identity();
     transformation_known.translation() << 1.0, 0.5, 0.2;
-    transformation_known.rotate (Eigen::AngleAxisd (0.3, Eigen::Vector3d::UnitZ()));
+    transformation_known.rotate(AngleAxisd(0.3, Vector3d::UnitZ()));
 
-    // Move point cloud 
-    PointCloudBoostPtr pcl_other = boost::make_shared<PointCloud>();
-    pcl::transformPointCloud (*pcl_ref, *pcl_other, transformation_known);
+    // Move point cloud
+    pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_other = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
+    pcl::transformPointCloud(*pcl_ref, *pcl_other, transformation_known);
 
     // Guess
-    Eigen::Isometry3d transformation_guess = Eigen::Isometry3d::Identity();
+    Isometry3d transformation_guess = Isometry3d::Identity();
 
     // final transform
-    Eigen::Isometry3d transformation_final;
+    Isometry3d transformation_final;
     WOLF_INFO("Applied transformation: \n", transformation_known.matrix());
-    
+
     // Solver configuration
     pcl::IterativeClosestPointNonLinear<pcl::PointXYZ, pcl::PointXYZ> registration_solver;
-    registration_solver.setTransformationEpsilon(1e-6);
+    // pcl::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> registration_solver;
+
+    double max_trans_epsilon_meters = 0.001;
+    double max_rot_epsilon_degrees  = 0.1;
+    double trf_rotation_epsilon     = cos(toRad(max_rot_epsilon_degrees));
+
+    registration_solver.setTransformationEpsilon(max_trans_epsilon_meters * max_trans_epsilon_meters);
+    registration_solver.setTransformationRotationEpsilon(trf_rotation_epsilon);
     // Set the maximum distance between two correspondences (src<->tgt) to 10cm
     // Note: adjust this based on the size of your datasets
     registration_solver.setMaxCorrespondenceDistance(0.5);  // visaub: changed to 100 cm
-    // Set the point representation
-    // reg.setPointRepresentation(pcl::make_shared<const MyPointRepresentation>(point_representation));
-    registration_solver.setMaximumIterations(200);
+    registration_solver.setMaximumIterations(100);
 
     // Alignment
-    wolf::laser::pairAlign(pcl_ref, pcl_other, transformation_guess, transformation_final, registration_solver, true); //false);
+    wolf::laser::pairAlign(pcl_ref,
+                           pcl_other,
+                           transformation_guess,
+                           transformation_final,
+                           registration_solver,
+                           true);  // false);
 
     WOLF_INFO("Final transform: \n", transformation_final.matrix());
 
-    ASSERT_MATRIX_APPROX(transformation_final.matrix(),  transformation_known.matrix(), 1e-2);
-}
+    auto convcrit = registration_solver.getConvergeCriteria();
+
+    WOLF_INFO("convergence criteria: ", registration_solver.getConvergeCriteria()->getConvergenceState());
 
+    ASSERT_MATRIX_APPROX(transformation_final.matrix(), transformation_known.matrix(), 1e-2);
+}
 
-int main(int argc, char **argv)
- {
+int main(int argc, char** argv)
+{
     testing::InitGoogleTest(&argc, argv);
-    // ::testing::GTEST_FLAG(filter) = "Conversions.R2q_norm_of_q";
+    ::testing::GTEST_FLAG(filter) = "pairAlign.known_transformation";
     return RUN_ALL_TESTS();
 }