Skip to content
Snippets Groups Projects
Commit 36c6362e authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Configure ProcessorOdomICP

Also format some code for readability
parent 4c3ea9c5
No related branches found
No related tags found
3 merge requests!30Release after RAL,!29After 2nd RAL submission,!3Resolve "new processor: pc matching for demo"
...@@ -37,32 +37,33 @@ struct ProcessorParamsOdomICP : public ProcessorParamsTracker ...@@ -37,32 +37,33 @@ struct ProcessorParamsOdomICP : public ProcessorParamsTracker
ProcessorParamsOdomICP(std::string _unique_name, const ParamsServer &_server): ProcessorParamsOdomICP(std::string _unique_name, const ParamsServer &_server):
ProcessorParamsTracker(_unique_name, _server) ProcessorParamsTracker(_unique_name, _server)
{ {
use_point_to_line_distance = _server.getParam<int>(_unique_name + "/use_point_to_line_distance"); 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_correspondence_dist = _server.getParam<int> (_unique_name + "/max_correspondence_dist");
max_iterations = _server.getParam<int>(_unique_name + "/max_iterations"); max_iterations = _server.getParam<int> (_unique_name + "/max_iterations");
use_corr_tricks = _server.getParam<int>(_unique_name + "/use_corr_tricks"); use_corr_tricks = _server.getParam<int> (_unique_name + "/use_corr_tricks");
outliers_maxPerc = _server.getParam<double>(_unique_name + "/outliers_maxPerc"); outliers_maxPerc = _server.getParam<double> (_unique_name + "/outliers_maxPerc");
outliers_adaptive_order = _server.getParam<double>(_unique_name + "/outliers_adaptive_order"); outliers_adaptive_order = _server.getParam<double> (_unique_name + "/outliers_adaptive_order");
outliers_adaptive_mult = _server.getParam<double>(_unique_name + "/outliers_adaptive_mult"); outliers_adaptive_mult = _server.getParam<double> (_unique_name + "/outliers_adaptive_mult");
vfk_min_dist = _server.getParam<double>(_unique_name + "/vfk_min_dist"); vfk_min_dist = _server.getParam<double> (_unique_name + "/vfk_min_dist");
vfk_min_angle = _server.getParam<double>(_unique_name + "/vfk_min_angle"); vfk_min_angle = _server.getParam<double> (_unique_name + "/vfk_min_angle");
vfk_min_time = _server.getParam<double>(_unique_name + "/vfk_min_time"); vfk_min_time = _server.getParam<double> (_unique_name + "/vfk_min_time");
vfk_min_error = _server.getParam<double>(_unique_name + "/vfk_min_error"); vfk_min_error = _server.getParam<double> (_unique_name + "/vfk_min_error");
vfk_max_points = _server.getParam<int>(_unique_name + "/vfk_max_points"); vfk_max_points = _server.getParam<int> (_unique_name + "/vfk_max_points");
} }
std::string print() { std::string print() {
return "\n" + ProcessorParamsTracker::print() + "\n" + "use_point_to_line_distance: " + std::to_string(use_point_to_line_distance) + "\n" return "\n" + ProcessorParamsTracker::print() + "\n"
"max_correspondence_dist: " + std::to_string(max_correspondence_dist) + "\n" + "use_point_to_line_distance: "+ std::to_string(use_point_to_line_distance)+ "\n"
"max_iterations: " + std::to_string(max_iterations) + "\n" + "max_correspondence_dist: " + std::to_string(max_correspondence_dist) + "\n"
"use_corr_tricks: " + std::to_string(use_corr_tricks) + "\n" + "max_iterations: " + std::to_string(max_iterations) + "\n"
"outliers_maxPerc: " + std::to_string(outliers_maxPerc) + "\n" + "use_corr_tricks: " + std::to_string(use_corr_tricks) + "\n"
"outliers_adaptive_order: " + std::to_string(outliers_adaptive_order) + "\n" + "outliers_maxPerc: " + std::to_string(outliers_maxPerc) + "\n"
"outliers_adaptive_mult: " + std::to_string(outliers_adaptive_mult) + "\n" + "outliers_adaptive_order: " + std::to_string(outliers_adaptive_order) + "\n"
"vfk_min_dist: " + std::to_string(vfk_min_dist) + "\n" + "outliers_adaptive_mult: " + std::to_string(outliers_adaptive_mult) + "\n"
"vfk_min_angle: " + std::to_string(vfk_min_angle) + "\n" + "vfk_min_dist: " + std::to_string(vfk_min_dist) + "\n"
"vfk_min_time: " + std::to_string(vfk_min_time) + "\n" + "vfk_min_angle: " + std::to_string(vfk_min_angle) + "\n"
"vfk_min_error: " + std::to_string(vfk_min_error) + "\n" + "vfk_min_time: " + std::to_string(vfk_min_time) + "\n"
"vfk_max_points: " + std::to_string(vfk_max_points) + "\n"; + "vfk_min_error: " + std::to_string(vfk_min_error) + "\n"
+ "vfk_max_points: " + std::to_string(vfk_max_points) + "\n";
} }
}; };
......
...@@ -8,19 +8,22 @@ ProcessorOdomICP::ProcessorOdomICP(ProcessorParamsOdomICPPtr _params): ...@@ -8,19 +8,22 @@ ProcessorOdomICP::ProcessorOdomICP(ProcessorParamsOdomICPPtr _params):
{ {
proc_params_ = _params; proc_params_ = _params;
origin_last_.res_covar = Eigen::Matrix3s::Identity();
origin_incoming_.res_covar = Eigen::Matrix3s::Identity(); icp_params_.use_point_to_line_distance = proc_params_->use_point_to_line_distance;
last_incoming_.res_covar = Eigen::Matrix3s::Identity(); 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_.use_point_to_line_distance = proc_params_->use_point_to_line_distance; icp_params_.outliers_maxPerc = proc_params_->outliers_maxPerc;
icp_params_.max_correspondence_dist = proc_params_->max_correspondence_dist; icp_params_.outliers_adaptive_order = proc_params_->outliers_adaptive_order;
icp_params_.max_iterations = proc_params_->max_iterations; icp_params_.outliers_adaptive_mult = proc_params_->outliers_adaptive_mult;
icp_params_.use_corr_tricks = proc_params_->use_corr_tricks;
icp_params_.outliers_maxPerc = proc_params_->outliers_maxPerc; // ICP algorithm
icp_params_.outliers_adaptive_order = proc_params_->outliers_adaptive_order;
icp_params_.outliers_adaptive_mult = proc_params_->outliers_adaptive_mult;
icp_tools_ptr_ = std::make_shared<ICP>(); 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() ProcessorOdomICP::~ProcessorOdomICP()
...@@ -165,7 +168,7 @@ void ProcessorOdomICP::resetDerived() ...@@ -165,7 +168,7 @@ void ProcessorOdomICP::resetDerived()
void ProcessorOdomICP::configure(SensorBasePtr _sensor) void ProcessorOdomICP::configure(SensorBasePtr _sensor)
{ {
icp_params_.sigma = std::static_pointer_cast<SensorLaser2D>(getSensor())->getScanParams().range_std_dev_;
} }
void ProcessorOdomICP::preProcess() void ProcessorOdomICP::preProcess()
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment