diff --git a/include/laser/sensor/sensor_laser_3d.h b/include/laser/sensor/sensor_laser_3d.h
index fbe7d34ae03614e9799902d5f52c7ca30b7aa60f..771b96e061fd5bbe842169c6cac57c1458d3aaf3 100644
--- a/include/laser/sensor/sensor_laser_3d.h
+++ b/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
diff --git a/include/laser/utils/laser3d_tools.h b/include/laser/utils/laser3d_tools.h
index c258ee7c7a1c1a19177d851d6b0681724db8834e..b3a61908b0bb5c52a989fb4dd058df110938f4d8 100644
--- a/include/laser/utils/laser3d_tools.h
+++ b/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
diff --git a/test/gtest_laser3d_tools.cpp b/test/gtest_laser3d_tools.cpp
index 196284fc8b581ca62e4dfa59c0d620e5c375fd47..d0cedb247a70c58e624f1bae5130e21646de5cc3 100644
--- a/test/gtest_laser3d_tools.cpp
+++ b/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();
}