diff --git a/include/laser/processor/processor_odom_icp_3d.h b/include/laser/processor/processor_odom_icp_3d.h index b9b77f457de2aa0188943218b66e5b35aeb7606e..9cd130a8dd2151fcd0fc6dbb0f942c5482b7f0bb 100644 --- a/include/laser/processor/processor_odom_icp_3d.h +++ b/include/laser/processor/processor_odom_icp_3d.h @@ -40,7 +40,6 @@ **************************/ #include <pcl/point_types.h> #include <pcl/point_cloud.h> -#include <pcl/registration/icp_nl.h> #include <pcl/registration/registration.h> namespace wolf @@ -50,10 +49,12 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorOdomIcp3d); struct ParamsProcessorOdomIcp3d : public ParamsProcessorTracker, public ParamsMotionProvider { - bool pcl_downsample; - double max_time_span; + bool pcl_downsample; + double pcl_downsample_voxel_size; + + string icp_algorithm; // "icp", "icpnl", "gicp" int icp_max_iterations; double icp_transformation_translation_epsilon; // squared value of translation epsilon double icp_transformation_rotation_epsilon; // cosinus of rotation angle epsilon @@ -65,10 +66,12 @@ struct ParamsProcessorOdomIcp3d : public ParamsProcessorTracker, public ParamsMo ParamsProcessorOdomIcp3d(std::string _unique_name, const ParamsServer& _server) : ParamsProcessorTracker(_unique_name, _server), ParamsMotionProvider(_unique_name, _server) { - pcl_downsample = _server.getParam<bool>(prefix + _unique_name + "/pcl_downsample"); - max_time_span = _server.getParam<double>(prefix + _unique_name + "/keyframe_vote/max_time_span"); + pcl_downsample = _server.getParam<bool>(prefix + _unique_name + "/pcl_downsample"); + pcl_downsample_voxel_size = _server.getParam<double>(prefix + _unique_name + "/pcl_downsample_voxel_size"); + + icp_algorithm = _server.getParam<string>(prefix + _unique_name + "/icp_algorithm"); icp_max_iterations = _server.getParam<int>(prefix + _unique_name + "/icp_max_iterations"); icp_transformation_translation_epsilon = _server.getParam<double>(prefix + _unique_name + "/icp_transformation_translation_epsilon"); @@ -79,8 +82,8 @@ struct ParamsProcessorOdomIcp3d : public ParamsProcessorTracker, public ParamsMo } std::string print() const override - { - // TODO + { + // TODO return "\n" + ParamsProcessorTracker::print() + "\n" + ParamsMotionProvider::print() + "\n"; // + "initial_guess: " + initial_guess + "\n" // + "keyframe_vote/min_dist: " + std::to_string(vfk_min_dist) + "\n" @@ -93,21 +96,16 @@ struct ParamsProcessorOdomIcp3d : public ParamsProcessorTracker, public ParamsMo WOLF_PTR_TYPEDEFS(ProcessorOdomIcp3d); + class ProcessorOdomIcp3d : public ProcessorTracker, public MotionProvider { protected: // Useful sensor stuff - SensorLaser3dPtr sensor_laser_; // casted pointer to parent - + SensorLaser3dPtr sensor_laser_; // casted pointer to parent CaptureLaser3dPtr origin_laser_, last_laser_, incoming_laser_; - Eigen::Isometry3d T_origin_last_, T_last_incoming_, T_origin_incoming_, T_robot_sensor_, T_sensor_robot_; - - RegistrationPtr registration_solver_; - - // std::shared_ptr<pcl::IterativeClosestPointNonLinear<pcl::PointXYZ, pcl::PointXYZ>> registration_solver_; - - Eigen::Matrix6d transform_cov_; + RegistrationPtr registration_solver_; + Eigen::Matrix6d transform_cov_; public: ParamsProcessorOdomIcp3dPtr params_odom_icp_; diff --git a/include/laser/utils/laser3d_tools.h b/include/laser/utils/laser3d_tools.h index 47f2cec9be81ed440abff579904bb677d2040bd0..74a6222199d333ad63ed6c103a2ec5322ba68619 100644 --- a/include/laser/utils/laser3d_tools.h +++ b/include/laser/utils/laser3d_tools.h @@ -64,7 +64,7 @@ inline void pairAlign(const pcl::PointCloud<pcl::PointXYZ>::Ptr _cloud_ref, pcl::VoxelGrid<pcl::PointXYZ> grid; // downsample for performance if (_downsample) { - grid.setLeafSize(0.05, 0.05, 0.05); + grid.setLeafSize(_voxel_size, _voxel_size, _voxel_size); // grid.setInputCloud(boost::static_pointer_cast<PointCloud>(_cloud_ref)); grid.setInputCloud(_cloud_ref); grid.filter(*cloud_ref); diff --git a/src/processor/processor_odom_icp_3d.cpp b/src/processor/processor_odom_icp_3d.cpp index b9b7982597622547c255a15a88f93a10376b2dba..a2cc33ee0a3c734cad602e4b93e6d64c324cc332 100644 --- a/src/processor/processor_odom_icp_3d.cpp +++ b/src/processor/processor_odom_icp_3d.cpp @@ -123,7 +123,9 @@ unsigned int ProcessorOdomIcp3d::processKnown() incoming_laser_->getPointCloud(), T_origin_last_, T_origin_incoming_, - registration_solver_); + registration_solver_, + params_odom_icp_->pcl_downsample, + params_odom_icp_->pcl_downsample_voxel_size); } return 0; }; diff --git a/test/yaml/problem_odom_icp_3d.yaml b/test/yaml/problem_odom_icp_3d.yaml index f21e65fb131bf736dd070e7962d0883b9f215985..0c2fdcc42eaf39aecdf5deac03eaafc30fcb7f7d 100644 --- a/test/yaml/problem_odom_icp_3d.yaml +++ b/test/yaml/problem_odom_icp_3d.yaml @@ -31,19 +31,21 @@ config: sensor_name: "Lidar" plugin: "laser" time_tolerance: 0.05 + max_new_features: 0 + apply_loss_function: false keyframe_vote: voting_active: true min_features_for_keyframe : 0 max_time_span: 0.99 angle_turned: 1.0 - max_new_features: 0 - apply_loss_function: false state_getter: true state_priority: 1 + pcl_downsample: true + pcl_downsample_voxel_size: 0.2 + icp_algorithm: "icp_nl" # "icp", "icp_nl", "gicp" icp_max_iterations: 20 - icp_transformation_rotation_epsilon: 0.99 - icp_transformation_translation_epsilon: 1e-6 + icp_transformation_rotation_epsilon: 0.99 # this is cos(alpha) + icp_transformation_translation_epsilon: 1e-6 # this is translation squared icp_max_correspondence_distance: 0.5 - pcl_downsample: true