diff --git a/include/laser/processor/processor_odom_icp.h b/include/laser/processor/processor_odom_icp.h
index 5b80854e4d716889e2c54c2f7da983da5d1e259d..de4d764cd387c659d19261c2da9a059c4566d7c4 100644
--- a/include/laser/processor/processor_odom_icp.h
+++ b/include/laser/processor/processor_odom_icp.h
@@ -37,32 +37,33 @@ struct ProcessorParamsOdomICP : public ProcessorParamsTracker
ProcessorParamsOdomICP(std::string _unique_name, const ParamsServer &_server):
ProcessorParamsTracker(_unique_name, _server)
{
- use_point_to_line_distance = _server.getParam<int>(_unique_name + "/use_point_to_line_distance");
- max_correspondence_dist = _server.getParam<int>(_unique_name + "/max_correspondence_dist");
- max_iterations = _server.getParam<int>(_unique_name + "/max_iterations");
- use_corr_tricks = _server.getParam<int>(_unique_name + "/use_corr_tricks");
- outliers_maxPerc = _server.getParam<double>(_unique_name + "/outliers_maxPerc");
- outliers_adaptive_order = _server.getParam<double>(_unique_name + "/outliers_adaptive_order");
- outliers_adaptive_mult = _server.getParam<double>(_unique_name + "/outliers_adaptive_mult");
- vfk_min_dist = _server.getParam<double>(_unique_name + "/vfk_min_dist");
- vfk_min_angle = _server.getParam<double>(_unique_name + "/vfk_min_angle");
- vfk_min_time = _server.getParam<double>(_unique_name + "/vfk_min_time");
- vfk_min_error = _server.getParam<double>(_unique_name + "/vfk_min_error");
- vfk_max_points = _server.getParam<int>(_unique_name + "/vfk_max_points");
+ use_point_to_line_distance = _server.getParam<int> (_unique_name + "/use_point_to_line_distance");
+ max_correspondence_dist = _server.getParam<int> (_unique_name + "/max_correspondence_dist");
+ max_iterations = _server.getParam<int> (_unique_name + "/max_iterations");
+ use_corr_tricks = _server.getParam<int> (_unique_name + "/use_corr_tricks");
+ outliers_maxPerc = _server.getParam<double> (_unique_name + "/outliers_maxPerc");
+ outliers_adaptive_order = _server.getParam<double> (_unique_name + "/outliers_adaptive_order");
+ outliers_adaptive_mult = _server.getParam<double> (_unique_name + "/outliers_adaptive_mult");
+ vfk_min_dist = _server.getParam<double> (_unique_name + "/vfk_min_dist");
+ vfk_min_angle = _server.getParam<double> (_unique_name + "/vfk_min_angle");
+ vfk_min_time = _server.getParam<double> (_unique_name + "/vfk_min_time");
+ vfk_min_error = _server.getParam<double> (_unique_name + "/vfk_min_error");
+ vfk_max_points = _server.getParam<int> (_unique_name + "/vfk_max_points");
}
std::string print() {
- return "\n" + ProcessorParamsTracker::print() + "\n" + "use_point_to_line_distance: " + std::to_string(use_point_to_line_distance) + "\n"
- "max_correspondence_dist: " + std::to_string(max_correspondence_dist) + "\n"
- "max_iterations: " + std::to_string(max_iterations) + "\n"
- "use_corr_tricks: " + std::to_string(use_corr_tricks) + "\n"
- "outliers_maxPerc: " + std::to_string(outliers_maxPerc) + "\n"
- "outliers_adaptive_order: " + std::to_string(outliers_adaptive_order) + "\n"
- "outliers_adaptive_mult: " + std::to_string(outliers_adaptive_mult) + "\n"
- "vfk_min_dist: " + std::to_string(vfk_min_dist) + "\n"
- "vfk_min_angle: " + std::to_string(vfk_min_angle) + "\n"
- "vfk_min_time: " + std::to_string(vfk_min_time) + "\n"
- "vfk_min_error: " + std::to_string(vfk_min_error) + "\n"
- "vfk_max_points: " + std::to_string(vfk_max_points) + "\n";
+ return "\n" + ProcessorParamsTracker::print() + "\n"
+ + "use_point_to_line_distance: "+ std::to_string(use_point_to_line_distance)+ "\n"
+ + "max_correspondence_dist: " + std::to_string(max_correspondence_dist) + "\n"
+ + "max_iterations: " + std::to_string(max_iterations) + "\n"
+ + "use_corr_tricks: " + std::to_string(use_corr_tricks) + "\n"
+ + "outliers_maxPerc: " + std::to_string(outliers_maxPerc) + "\n"
+ + "outliers_adaptive_order: " + std::to_string(outliers_adaptive_order) + "\n"
+ + "outliers_adaptive_mult: " + std::to_string(outliers_adaptive_mult) + "\n"
+ + "vfk_min_dist: " + std::to_string(vfk_min_dist) + "\n"
+ + "vfk_min_angle: " + std::to_string(vfk_min_angle) + "\n"
+ + "vfk_min_time: " + std::to_string(vfk_min_time) + "\n"
+ + "vfk_min_error: " + std::to_string(vfk_min_error) + "\n"
+ + "vfk_max_points: " + std::to_string(vfk_max_points) + "\n";
}
};
diff --git a/src/processor/processor_odom_icp.cpp b/src/processor/processor_odom_icp.cpp
index aa7be21466c868189ab38f2df91718ec16f6b57f..d0f038a085ad3ce4ed84afc7bc2e79bb5f4d53de 100644
--- a/src/processor/processor_odom_icp.cpp
+++ b/src/processor/processor_odom_icp.cpp
@@ -8,19 +8,22 @@ ProcessorOdomICP::ProcessorOdomICP(ProcessorParamsOdomICPPtr _params):
{
proc_params_ = _params;
- origin_last_.res_covar = Eigen::Matrix3s::Identity();
- origin_incoming_.res_covar = Eigen::Matrix3s::Identity();
- last_incoming_.res_covar = Eigen::Matrix3s::Identity();
-
-
- icp_params_.use_point_to_line_distance = proc_params_->use_point_to_line_distance;
- icp_params_.max_correspondence_dist = proc_params_->max_correspondence_dist;
- icp_params_.max_iterations = proc_params_->max_iterations;
- icp_params_.use_corr_tricks = proc_params_->use_corr_tricks;
- icp_params_.outliers_maxPerc = proc_params_->outliers_maxPerc;
- icp_params_.outliers_adaptive_order = proc_params_->outliers_adaptive_order;
- icp_params_.outliers_adaptive_mult = proc_params_->outliers_adaptive_mult;
+
+ icp_params_.use_point_to_line_distance = proc_params_->use_point_to_line_distance;
+ icp_params_.max_correspondence_dist = proc_params_->max_correspondence_dist;
+ icp_params_.max_iterations = proc_params_->max_iterations;
+ icp_params_.use_corr_tricks = proc_params_->use_corr_tricks;
+ icp_params_.outliers_maxPerc = proc_params_->outliers_maxPerc;
+ icp_params_.outliers_adaptive_order = proc_params_->outliers_adaptive_order;
+ icp_params_.outliers_adaptive_mult = proc_params_->outliers_adaptive_mult;
+
+ // ICP algorithm
icp_tools_ptr_ = std::make_shared<ICP>();
+
+ // Frame transforms
+ origin_last_.res_covar = Eigen::Matrix3s::Identity();
+ origin_incoming_.res_covar = Eigen::Matrix3s::Identity();
+ last_incoming_.res_covar = Eigen::Matrix3s::Identity();
}
ProcessorOdomICP::~ProcessorOdomICP()
@@ -165,7 +168,7 @@ void ProcessorOdomICP::resetDerived()
void ProcessorOdomICP::configure(SensorBasePtr _sensor)
{
-
+ icp_params_.sigma = std::static_pointer_cast<SensorLaser2D>(getSensor())->getScanParams().range_std_dev_;
}
void ProcessorOdomICP::preProcess()