diff --git a/CMakeLists.txt b/CMakeLists.txt
index b16e4964e99fd5092e512860d3edca8cfa5bc4ef..e8337442a6d16430145fa8068288dcc616684d40 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -207,6 +207,7 @@ endif()
if(csm_FOUND)
message("Found CSM. Compiling some extra classes.")
SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
+ include/laser/processor/params_icp.h
include/laser/processor/processor_loop_closure_icp.h
include/laser/processor/processor_odom_icp.h
)
diff --git a/include/laser/processor/params_icp.h b/include/laser/processor/params_icp.h
new file mode 100644
index 0000000000000000000000000000000000000000..b93bfb469f45db6e98b419395d55200db82fc6fd
--- /dev/null
+++ b/include/laser/processor/params_icp.h
@@ -0,0 +1,99 @@
+#ifndef INCLUDE_LASER_PROCESSOR_PARAMS_ICP_H_
+#define INCLUDE_LASER_PROCESSOR_PARAMS_ICP_H_
+
+#include "core/common/wolf.h"
+#include "laser_scan_utils/icp.h"
+#undef max
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(ProcessorOdomIcp);
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorOdomIcp);
+
+struct ParamsIcp
+{
+ // ICP Params
+ laserscanutils::icpParams icp_params;
+
+ ParamsIcp() = default;
+ ParamsIcp(std::string _prefix_and_unique_name, const ParamsServer &_server)
+ {
+ icp_params.verbose = _server.getParam<bool> (_prefix_and_unique_name + "/icp/verbose");
+
+ icp_params.use_point_to_line_distance = _server.getParam<bool> (_prefix_and_unique_name + "/icp/use_point_to_line_distance");
+ icp_params.max_angular_correction_deg = _server.getParam<double> (_prefix_and_unique_name + "/icp/max_angular_correction_deg");
+ icp_params.max_linear_correction = _server.getParam<double> (_prefix_and_unique_name + "/icp/max_linear_correction");
+ icp_params.max_iterations = _server.getParam<int> (_prefix_and_unique_name + "/icp/max_iterations");
+ icp_params.epsilon_xy = _server.getParam<double> (_prefix_and_unique_name + "/icp/epsilon_xy");
+ icp_params.epsilon_theta = _server.getParam<double> (_prefix_and_unique_name + "/icp/epsilon_theta");
+
+ icp_params.max_correspondence_dist = _server.getParam<double> (_prefix_and_unique_name + "/icp/max_correspondence_dist");
+ icp_params.use_corr_tricks = _server.getParam<bool> (_prefix_and_unique_name + "/icp/use_corr_tricks");
+ icp_params.debug_verify_tricks = _server.getParam<bool> (_prefix_and_unique_name + "/icp/debug_verify_tricks");
+
+ icp_params.restart = _server.getParam<bool> (_prefix_and_unique_name + "/icp/restart");
+ icp_params.restart_threshold_mean_error = _server.getParam<double> (_prefix_and_unique_name + "/icp/restart_threshold_mean_error");
+ icp_params.restart_dt = _server.getParam<double> (_prefix_and_unique_name + "/icp/restart_dt");
+ icp_params.restart_dtheta = _server.getParam<double> (_prefix_and_unique_name + "/icp/restart_dtheta");
+
+ icp_params.min_reading = _server.getParam<double> (_prefix_and_unique_name + "/icp/min_reading");
+ icp_params.max_reading = _server.getParam<double> (_prefix_and_unique_name + "/icp/max_reading");
+ icp_params.outliers_maxPerc = _server.getParam<double> (_prefix_and_unique_name + "/icp/outliers_maxPerc");
+ icp_params.outliers_adaptive_order = _server.getParam<double> (_prefix_and_unique_name + "/icp/outliers_adaptive_order");
+ icp_params.outliers_adaptive_mult = _server.getParam<double> (_prefix_and_unique_name + "/icp/outliers_adaptive_mult");
+ icp_params.outliers_remove_doubles = _server.getParam<bool> (_prefix_and_unique_name + "/icp/outliers_remove_doubles");
+
+ icp_params.do_visibility_test = _server.getParam<bool> (_prefix_and_unique_name + "/icp/do_visibility_test");
+
+ icp_params.do_alpha_test = _server.getParam<bool> (_prefix_and_unique_name + "/icp/do_alpha_test");
+ icp_params.do_alpha_test_thresholdDeg = _server.getParam<double> (_prefix_and_unique_name + "/icp/do_alpha_test_thresholdDeg");
+
+ icp_params.clustering_threshold = _server.getParam<double> (_prefix_and_unique_name + "/icp/clustering_threshold");
+ icp_params.orientation_neighbourhood = _server.getParam<int> (_prefix_and_unique_name + "/icp/orientation_neighbourhood");
+
+ icp_params.use_ml_weights = _server.getParam<bool> (_prefix_and_unique_name + "/icp/use_ml_weights");
+ icp_params.use_sigma_weights = _server.getParam<bool> (_prefix_and_unique_name + "/icp/use_sigma_weights");
+ icp_params.sigma = _server.getParam<double> (_prefix_and_unique_name + "/icp/sigma");
+
+ icp_params.do_compute_covariance = _server.getParam<bool> (_prefix_and_unique_name + "/icp/do_compute_covariance");
+ icp_params.cov_factor = _server.getParam<double> (_prefix_and_unique_name + "/icp/cov_factor");
+ }
+
+ std::string print() const
+ {
+ return "/icp/verbose" + std::to_string(icp_params.verbose) + "\n"
+ + "/icp/use_point_to_line_distance" + std::to_string(icp_params.use_point_to_line_distance) + "\n"
+ + "/icp/max_angular_correction_deg" + std::to_string(icp_params.max_angular_correction_deg) + "\n"
+ + "/icp/max_linear_correction" + std::to_string(icp_params.max_linear_correction) + "\n"
+ + "/icp/max_iterations" + std::to_string(icp_params.max_iterations) + "\n"
+ + "/icp/epsilon_xy" + std::to_string(icp_params.epsilon_xy) + "\n"
+ + "/icp/epsilon_theta" + std::to_string(icp_params.epsilon_theta) + "\n"
+ + "/icp/max_correspondence_dist" + std::to_string(icp_params.max_correspondence_dist) + "\n"
+ + "/icp/use_corr_tricks" + std::to_string(icp_params.use_corr_tricks) + "\n"
+ + "/icp/debug_verify_tricks" + std::to_string(icp_params.debug_verify_tricks) + "\n"
+ + "/icp/restart" + std::to_string(icp_params.restart) + "\n"
+ + "/icp/restart_threshold_mean_error" + std::to_string(icp_params.restart_threshold_mean_error) + "\n"
+ + "/icp/restart_dt" + std::to_string(icp_params.restart_dt) + "\n"
+ + "/icp/restart_dtheta" + std::to_string(icp_params.restart_dtheta) + "\n"
+ + "/icp/min_reading" + std::to_string(icp_params.min_reading) + "\n"
+ + "/icp/max_reading" + std::to_string(icp_params.max_reading) + "\n"
+ + "/icp/outliers_maxPerc" + std::to_string(icp_params.outliers_maxPerc) + "\n"
+ + "/icp/outliers_adaptive_order" + std::to_string(icp_params.outliers_adaptive_order) + "\n"
+ + "/icp/outliers_adaptive_mult" + std::to_string(icp_params.outliers_adaptive_mult) + "\n"
+ + "/icp/outliers_remove_doubles" + std::to_string(icp_params.outliers_remove_doubles) + "\n"
+ + "/icp/do_visibility_test" + std::to_string(icp_params.do_visibility_test) + "\n"
+ + "/icp/do_alpha_test" + std::to_string(icp_params.do_alpha_test) + "\n"
+ + "/icp/do_alpha_test_thresholdDeg" + std::to_string(icp_params.do_alpha_test_thresholdDeg) + "\n"
+ + "/icp/clustering_threshold" + std::to_string(icp_params.clustering_threshold) + "\n"
+ + "/icp/orientation_neighbourhood" + std::to_string(icp_params.orientation_neighbourhood) + "\n"
+ + "/icp/use_ml_weights" + std::to_string(icp_params.use_ml_weights) + "\n"
+ + "/icp/use_sigma_weights" + std::to_string(icp_params.use_sigma_weights) + "\n"
+ + "/icp/sigma" + std::to_string(icp_params.sigma) + "\n"
+ + "/icp/do_compute_covariance" + std::to_string(icp_params.do_compute_covariance) + "\n"
+ + "/icp/cov_factor" + std::to_string(icp_params.cov_factor) + "\n";
+ }
+};
+
+}
+
+#endif /* INCLUDE_LASER_PROCESSOR_PARAMS_ICP_H_ */
diff --git a/include/laser/processor/processor_loop_closure_falko_icp.h b/include/laser/processor/processor_loop_closure_falko_icp.h
index 5ed8e6bb2de33f330f43642da1d0cacadaee2158..556ace850edcc6ac07be307dd958ea200e9f513c 100644
--- a/include/laser/processor/processor_loop_closure_falko_icp.h
+++ b/include/laser/processor/processor_loop_closure_falko_icp.h
@@ -26,12 +26,12 @@
* WOLF includes *
**************************/
#include <laser/processor/processor_loop_closure_falko.h>
+#include <laser/processor/params_icp.h>
#include <laser/feature/feature_icp_align.h>
/**************************
* laseScanUtils - ICP includes *
**************************/
-
#include <laser_scan_utils/icp.h>
using namespace laserscanutils;
@@ -40,69 +40,28 @@ namespace wolf {
WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorLoopClosureFalkoIcp);
-struct ParamsProcessorLoopClosureFalkoIcp : public ParamsProcessorLoopClosureFalko
+struct ParamsProcessorLoopClosureFalkoIcp : public ParamsProcessorLoopClosureFalko, public ParamsIcp
{
-
// ParameterLoopClosureFalko param;
double max_error_threshold;
double min_points_percent;
- laserscanutils::icpParams icp_params;
-
ParamsProcessorLoopClosureFalkoIcp() = default;
- ParamsProcessorLoopClosureFalkoIcp(std::string _unique_name, const ParamsServer &_server)
- : ParamsProcessorLoopClosureFalko(_unique_name, _server)
+ ParamsProcessorLoopClosureFalkoIcp(std::string _unique_name, const ParamsServer &_server) :
+ ParamsProcessorLoopClosureFalko(_unique_name, _server),
+ ParamsIcp(prefix + _unique_name, _server)
{
- // param.matcher_ddesc_th_ = _server.getParam<double>(prefix + _unique_name + "/matcher_ddesc_th");
- // param.keypoints_number_th_ = _server.getParam<int>(prefix + _unique_name + "/keypoints_number_th");
max_error_threshold = _server.getParam<double>(prefix + _unique_name + "/max_error_threshold");
min_points_percent = _server.getParam<double>(prefix + _unique_name + "/min_points_percent");
-
- icp_params.use_point_to_line_distance =
- _server.getParam<int>(prefix + _unique_name + "/icp/use_point_to_line_distance");
- icp_params.max_correspondence_dist =
- _server.getParam<double>(prefix + _unique_name + "/icp/max_correspondence_dist");
- icp_params.max_iterations = _server.getParam<int>(prefix + _unique_name + "/icp/max_iterations");
- icp_params.use_corr_tricks = _server.getParam<int>(prefix + _unique_name + "/icp/use_corr_tricks");
- icp_params.outliers_maxPerc = _server.getParam<double>(prefix + _unique_name + "/icp/outliers_maxPerc");
- icp_params.outliers_adaptive_order =
- _server.getParam<double>(prefix + _unique_name + "/icp/outliers_adaptive_order");
- icp_params.outliers_adaptive_mult =
- _server.getParam<double>(prefix + _unique_name + "/icp/outliers_adaptive_mult");
- icp_params.do_compute_covariance = _server.getParam<int>(prefix + _unique_name + "/icp/do_compute_covariance");
- icp_params.max_angular_correction_deg =
- _server.getParam<double>(prefix + _unique_name + "/icp/max_angular_correction_deg");
- icp_params.max_linear_correction =
- _server.getParam<double>(prefix + _unique_name + "/icp/max_linear_correction");
- icp_params.epsilon_xy = _server.getParam<double>(prefix + _unique_name + "/icp/epsilon_xy");
- icp_params.epsilon_theta = _server.getParam<double>(prefix + _unique_name + "/icp/epsilon_theta");
- icp_params.sigma = _server.getParam<double>(prefix + _unique_name + "/icp/sigma");
- icp_params.restart = _server.getParam<int>(prefix + _unique_name + "/icp/restart");
- icp_params.restart_threshold_mean_error =
- _server.getParam<double>(prefix + _unique_name + "/icp/restart_threshold_mean_error");
- icp_params.restart_dt = _server.getParam<double>(prefix + _unique_name + "/icp/restart_dt");
- icp_params.restart_dtheta = _server.getParam<double>(prefix + _unique_name + "/icp/restart_dtheta");
- icp_params.clustering_threshold = _server.getParam<double>(prefix + _unique_name + "/icp/clustering_threshold");
- icp_params.orientation_neighbourhood =
- _server.getParam<int>(prefix + _unique_name + "/icp/orientation_neighbourhood");
- icp_params.do_alpha_test = _server.getParam<int>(prefix + _unique_name + "/icp/do_alpha_test");
- icp_params.do_alpha_test_thresholdDeg =
- _server.getParam<double>(prefix + _unique_name + "/icp/do_alpha_test_thresholdDeg");
- icp_params.do_visibility_test = _server.getParam<int>(prefix + _unique_name + "/icp/do_visibility_test");
- icp_params.outliers_remove_doubles =
- _server.getParam<int>(prefix + _unique_name + "/icp/outliers_remove_doubles");
- icp_params.debug_verify_tricks = _server.getParam<int>(prefix + _unique_name + "/icp/debug_verify_tricks");
- icp_params.gpm_theta_bin_size_deg =
- _server.getParam<double>(prefix + _unique_name + "/icp/gpm_theta_bin_size_deg");
- icp_params.gpm_extend_range_deg = _server.getParam<double>(prefix + _unique_name + "/icp/gpm_extend_range_deg");
- icp_params.gpm_interval = _server.getParam<double>(prefix + _unique_name + "/icp/gpm_interval");
- icp_params.min_reading = _server.getParam<double>(prefix + _unique_name + "/icp/min_reading");
- icp_params.max_reading = _server.getParam<double>(prefix + _unique_name + "/icp/max_reading");
- icp_params.use_ml_weights = _server.getParam<int>(prefix + _unique_name + "/icp/use_ml_weights");
- icp_params.use_sigma_weights = _server.getParam<int>(prefix + _unique_name + "/icp/use_sigma_weights");
};
- virtual ~ParamsProcessorLoopClosureFalkoIcp() = default;
+ std::string print() const override
+ {
+ return "\n" + ParamsProcessorLoopClosureFalko::print() + "\n"
+ + ParamsIcp::print() + "\n"
+ + "max_error_threshold: " + std::to_string(max_error_threshold) + "\n"
+ + "min_points_percent: " + std::to_string(min_points_percent) + "\n";
+ }
};
/** \brief A class to implement a loop closure processor using loop closure detection from falko library
@@ -123,40 +82,40 @@ template <typename D, typename Extr, template <typename, typename> typename L>
class ProcessorLoopClosureFalkoIcp : public ProcessorLoopClosureFalko<D, Extr, L>
{
- public:
- ProcessorLoopClosureFalkoIcp(ParamsProcessorLoopClosureFalkoIcpPtr _params_falko)
- : ProcessorLoopClosureFalko<D, Extr, L>(_params_falko)
- , params_falko_icp_(_params_falko){
+ public:
+ ProcessorLoopClosureFalkoIcp(ParamsProcessorLoopClosureFalkoIcpPtr _params_falko)
+ : ProcessorLoopClosureFalko<D, Extr, L>(_params_falko)
+ , params_falko_icp_(_params_falko){
- };
+ };
- ~ProcessorLoopClosureFalkoIcp() = default;
+ ~ProcessorLoopClosureFalkoIcp() = default;
- WOLF_PROCESSOR_CREATE(ProcessorLoopClosureFalkoIcp, ParamsProcessorLoopClosureFalkoIcp);
+ WOLF_PROCESSOR_CREATE(ProcessorLoopClosureFalkoIcp, ParamsProcessorLoopClosureFalkoIcp);
- protected:
- ParamsProcessorLoopClosureFalkoIcpPtr params_falko_icp_;
- // ICP algorithm
- std::shared_ptr<laserscanutils::ICP> icp_tools_ptr_;
+ protected:
+ ParamsProcessorLoopClosureFalkoIcpPtr params_falko_icp_;
+ // ICP algorithm
+ std::shared_ptr<laserscanutils::ICP> icp_tools_ptr_;
- /** \brief Function that validates the falko loop closures with an ICP aligment.
- *
- * For the ICP alignment it uses the transform provided by the falko algorithm as an initial guess.
- *
- * \param match structure that contains scenes and capture pointers that are a match
- **/
- bool validateLoopClosure(MatchLoopClosurePtr match) override
- {
- auto start_validation = std::chrono::high_resolution_clock::now();
- auto capture_own = match->capture_reference; //< Capture reference
- auto capture_other = match->capture_target; // Capture target
- auto frame_own = capture_own->getFrame();
- auto frame_other = capture_other->getFrame();
- bool match_valid = false;
+ /** \brief Function that validates the falko loop closures with an ICP aligment.
+ *
+ * For the ICP alignment it uses the transform provided by the falko algorithm as an initial guess.
+ *
+ * \param match structure that contains scenes and capture pointers that are a match
+ **/
+ bool validateLoopClosure(MatchLoopClosurePtr match) override
+ {
+ auto start_validation = std::chrono::high_resolution_clock::now();
+ auto capture_own = match->capture_reference; //< Capture reference
+ auto capture_other = match->capture_target; // Capture target
+ auto frame_own = capture_own->getFrame();
+ auto frame_other = capture_other->getFrame();
+ bool match_valid = false;
- auto match_falko = std::static_pointer_cast<MatchLoopClosureFalko>(match);
+ auto match_falko = std::static_pointer_cast<MatchLoopClosureFalko>(match);
- if ((capture_own->getType() == "CaptureLaser2d") & (capture_other->getType() == "CaptureLaser2d"))
+ if ((capture_own->getType() == "CaptureLaser2d") & (capture_other->getType() == "CaptureLaser2d"))
{
CaptureLaser2dPtr capture_laser_other = std::static_pointer_cast<CaptureLaser2d>(capture_other);
CaptureLaser2dPtr capture_laser_own = std::static_pointer_cast<CaptureLaser2d>(capture_own);
@@ -167,66 +126,66 @@ class ProcessorLoopClosureFalkoIcp : public ProcessorLoopClosureFalko<D, Extr, L
WOLF_INFO("DBG Attempting to close loop with ", frame_own->id(), " and ", frame_other->id());
try
- {
- laserscanutils::icpOutput trf = icp_tools_ptr_->align(
+ {
+ laserscanutils::icpOutput trf = icp_tools_ptr_->align(
capture_laser_other->getScan(), capture_laser_own->getScan(), sensor_own->getScanParams(),
sensor_other->getScanParams(), params_falko_icp_->icp_params,
match_falko->match_falko_->transform_vector);
- double points_coeff_used =
+ double points_coeff_used =
((double)trf.nvalid / (double)fmin(capture_laser_own->getScan().ranges_raw_.size(),
capture_laser_other->getScan().ranges_raw_.size()));
- double mean_error = trf.error / trf.nvalid;
- Eigen::Vector3d displacement_other_own;
- displacement_other_own.head(2) =
+ double mean_error = trf.error / trf.nvalid;
+ Eigen::Vector3d displacement_other_own;
+ displacement_other_own.head(2) =
Eigen::Rotation2Dd(-frame_other->getState().vector("O")(0)) *
(frame_own->getState().vector("P") - frame_other->getState().vector("P"));
- displacement_other_own(2) =
+ displacement_other_own(2) =
frame_own->getState().vector("O")(0) - frame_other->getState().vector("O")(0);
- Eigen::Vector3d displacement_own_other;
- displacement_own_other.head(2) =
+ Eigen::Vector3d displacement_own_other;
+ displacement_own_other.head(2) =
Eigen::Rotation2Dd(-frame_own->getState().vector("O")(0)) *
(frame_other->getState().vector("P") - frame_own->getState().vector("P"));
- displacement_own_other(2) =
+ displacement_own_other(2) =
frame_other->getState().vector("O")(0) - frame_own->getState().vector("O")(0);
- WOLF_INFO("DBG ------------------------------");
- WOLF_INFO("DBG valid? ", trf.valid, "\n m_er ", mean_error,
- " \n % points used :", points_coeff_used * 100);
- // WOLF_INFO("DBG own_POSE: ", frame_own->getState().vector("PO").transpose(),
- // "\nother_POSE: ", frame_other->getState().vector("PO").transpose(),
- // WOLF_INFO("\ndisplacement_other_own : ", displacement_other_own.transpose(),
- WOLF_INFO("\ndisplacement_own_other : ", displacement_own_other.transpose(),
- "\nIcp initial guess: ", match_falko->match_falko_->transform_vector.transpose(),
- "\nIcp final transfr: ", trf.res_transf.transpose());
- // WOLF_INFO("Covariance \n", trf.res_covar);
- if (trf.valid == 1 and mean_error < params_falko_icp_->max_error_threshold and
+ WOLF_INFO("DBG ------------------------------");
+ WOLF_INFO("DBG valid? ", trf.valid, "\n m_er ", mean_error,
+ " \n % points used :", points_coeff_used * 100);
+ // WOLF_INFO("DBG own_POSE: ", frame_own->getState().vector("PO").transpose(),
+ // "\nother_POSE: ", frame_other->getState().vector("PO").transpose(),
+ // WOLF_INFO("\ndisplacement_other_own : ", displacement_other_own.transpose(),
+ WOLF_INFO("\ndisplacement_own_other : ", displacement_own_other.transpose(),
+ "\nIcp initial guess: ", match_falko->match_falko_->transform_vector.transpose(),
+ "\nIcp final transfr: ", trf.res_transf.transpose());
+ // WOLF_INFO("Covariance \n", trf.res_covar);
+ if (trf.valid == 1 and mean_error < params_falko_icp_->max_error_threshold and
points_coeff_used * 100 > params_falko_icp_->min_points_percent)
- {
- WOLF_INFO("Match validated ", frame_own->id(), " and ", frame_other->id());
- match_valid = true;
- match_falko->match_falko_->transform_vector = trf.res_transf;
- this->num_ok_ += 1;
- this->duration_validation_ok += std::chrono::duration_cast<std::chrono::microseconds>(
- std::chrono::high_resolution_clock::now() - start_validation);
- }
- else
- {
- WOLF_INFO("Match DISCARDED ", frame_own->id(), " and ", frame_other->id());
- this->num_nok_ += 1;
- this->duration_validation_nok += std::chrono::duration_cast<std::chrono::microseconds>(
- std::chrono::high_resolution_clock::now() - start_validation);
- }
+ {
+ WOLF_INFO("Match validated ", frame_own->id(), " and ", frame_other->id());
+ match_valid = true;
+ match_falko->match_falko_->transform_vector = trf.res_transf;
+ this->num_ok_ += 1;
+ this->duration_validation_ok += std::chrono::duration_cast<std::chrono::microseconds>(
+ std::chrono::high_resolution_clock::now() - start_validation);
}
- catch (std::exception &e)
+ else
{
+ WOLF_INFO("Match DISCARDED ", frame_own->id(), " and ", frame_other->id());
this->num_nok_ += 1;
- WOLF_WARN(e.what())
+ this->duration_validation_nok += std::chrono::duration_cast<std::chrono::microseconds>(
+ std::chrono::high_resolution_clock::now() - start_validation);
}
+ }
+ catch (std::exception &e)
+ {
+ this->num_nok_ += 1;
+ WOLF_WARN(e.what())
+ }
}
- return match_valid;
- }
+ return match_valid;
+ }
};
WOLF_STRUCT_PTR_TYPEDEFS(ProcessorLoopClosureFalkoIcpNnBsc);
@@ -234,15 +193,15 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorLoopClosureFalkoIcpNnBsc);
/** \brief A class that implements the loop closure with BSC descriptors and NN matcher
**/
class ProcessorLoopClosureFalkoIcpNnBsc
- : public ProcessorLoopClosureFalkoIcp<bsc, bscExtractor, laserscanutils::LoopClosureFalkoNn>
+ : public ProcessorLoopClosureFalkoIcp<bsc, bscExtractor, laserscanutils::LoopClosureFalkoNn>
{
- public:
- ProcessorLoopClosureFalkoIcpNnBsc(ParamsProcessorLoopClosureFalkoIcpPtr _params_falko)
- : ProcessorLoopClosureFalkoIcp(_params_falko){};
+ public:
+ ProcessorLoopClosureFalkoIcpNnBsc(ParamsProcessorLoopClosureFalkoIcpPtr _params_falko)
+ : ProcessorLoopClosureFalkoIcp(_params_falko){};
- WOLF_PROCESSOR_CREATE(ProcessorLoopClosureFalkoIcpNnBsc, ParamsProcessorLoopClosureFalkoIcp);
+ WOLF_PROCESSOR_CREATE(ProcessorLoopClosureFalkoIcpNnBsc, ParamsProcessorLoopClosureFalkoIcp);
- ~ProcessorLoopClosureFalkoIcpNnBsc() = default;
+ ~ProcessorLoopClosureFalkoIcpNnBsc() = default;
};
WOLF_STRUCT_PTR_TYPEDEFS(ProcessorLoopClosureFalkoIcpAhtBsc);
@@ -250,15 +209,15 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorLoopClosureFalkoIcpAhtBsc);
/** \brief A class that implements the loop closure with BSC descriptors and NN matcher
**/
class ProcessorLoopClosureFalkoIcpAhtBsc
- : public ProcessorLoopClosureFalkoIcp<bsc, bscExtractor, laserscanutils::LoopClosureFalkoAht>
+ : public ProcessorLoopClosureFalkoIcp<bsc, bscExtractor, laserscanutils::LoopClosureFalkoAht>
{
- public:
- ProcessorLoopClosureFalkoIcpAhtBsc(ParamsProcessorLoopClosureFalkoIcpPtr _params_falko)
- : ProcessorLoopClosureFalkoIcp(_params_falko){};
+ public:
+ ProcessorLoopClosureFalkoIcpAhtBsc(ParamsProcessorLoopClosureFalkoIcpPtr _params_falko)
+ : ProcessorLoopClosureFalkoIcp(_params_falko){};
- WOLF_PROCESSOR_CREATE(ProcessorLoopClosureFalkoIcpAhtBsc, ParamsProcessorLoopClosureFalkoIcp);
+ WOLF_PROCESSOR_CREATE(ProcessorLoopClosureFalkoIcpAhtBsc, ParamsProcessorLoopClosureFalkoIcp);
- ~ProcessorLoopClosureFalkoIcpAhtBsc() = default;
+ ~ProcessorLoopClosureFalkoIcpAhtBsc() = default;
};
WOLF_STRUCT_PTR_TYPEDEFS(ProcessorLoopClosureFalkoIcpNnCgh);
@@ -266,15 +225,15 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorLoopClosureFalkoIcpNnCgh);
/** \brief A class that implements the loop closure with Cgh descriptors and NN matcher
**/
class ProcessorLoopClosureFalkoIcpNnCgh
- : public ProcessorLoopClosureFalkoIcp<cgh, cghExtractor, laserscanutils::LoopClosureFalkoNn>
+ : public ProcessorLoopClosureFalkoIcp<cgh, cghExtractor, laserscanutils::LoopClosureFalkoNn>
{
- public:
- ProcessorLoopClosureFalkoIcpNnCgh(ParamsProcessorLoopClosureFalkoIcpPtr _params_falko)
- : ProcessorLoopClosureFalkoIcp(_params_falko){};
+ public:
+ ProcessorLoopClosureFalkoIcpNnCgh(ParamsProcessorLoopClosureFalkoIcpPtr _params_falko)
+ : ProcessorLoopClosureFalkoIcp(_params_falko){};
- WOLF_PROCESSOR_CREATE(ProcessorLoopClosureFalkoIcpNnCgh, ParamsProcessorLoopClosureFalkoIcp);
+ WOLF_PROCESSOR_CREATE(ProcessorLoopClosureFalkoIcpNnCgh, ParamsProcessorLoopClosureFalkoIcp);
- ~ProcessorLoopClosureFalkoIcpNnCgh() = default;
+ ~ProcessorLoopClosureFalkoIcpNnCgh() = default;
};
WOLF_STRUCT_PTR_TYPEDEFS(ProcessorLoopClosureFalkoIcpAhtCgh);
@@ -282,15 +241,15 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorLoopClosureFalkoIcpAhtCgh);
/** \brief A class that implements the loop closure with Cgh descriptors and NN matcher
**/
class ProcessorLoopClosureFalkoIcpAhtCgh
- : public ProcessorLoopClosureFalkoIcp<cgh, cghExtractor, laserscanutils::LoopClosureFalkoAht>
+ : public ProcessorLoopClosureFalkoIcp<cgh, cghExtractor, laserscanutils::LoopClosureFalkoAht>
{
- public:
- ProcessorLoopClosureFalkoIcpAhtCgh(ParamsProcessorLoopClosureFalkoIcpPtr _params_falko)
- : ProcessorLoopClosureFalkoIcp(_params_falko){};
+ public:
+ ProcessorLoopClosureFalkoIcpAhtCgh(ParamsProcessorLoopClosureFalkoIcpPtr _params_falko)
+ : ProcessorLoopClosureFalkoIcp(_params_falko){};
- WOLF_PROCESSOR_CREATE(ProcessorLoopClosureFalkoIcpAhtCgh, ParamsProcessorLoopClosureFalkoIcp);
+ WOLF_PROCESSOR_CREATE(ProcessorLoopClosureFalkoIcpAhtCgh, ParamsProcessorLoopClosureFalkoIcp);
- ~ProcessorLoopClosureFalkoIcpAhtCgh() = default;
+ ~ProcessorLoopClosureFalkoIcpAhtCgh() = default;
};
} // namespace wolf
diff --git a/include/laser/processor/processor_loop_closure_icp.h b/include/laser/processor/processor_loop_closure_icp.h
index 079ab82e3a184eebce74b466efb859b15bb31d6a..b1ac1868cfe669ede330bb20b7c2579e7a106f23 100644
--- a/include/laser/processor/processor_loop_closure_icp.h
+++ b/include/laser/processor/processor_loop_closure_icp.h
@@ -29,6 +29,7 @@
#include "core/processor/processor_loop_closure.h"
#include "core/factor/factor_relative_pose_2d_with_extrinsics.h"
+#include "laser/processor/params_icp.h"
#include "laser/capture/capture_laser_2d.h"
#include "laser/feature/feature_icp_align.h"
@@ -43,60 +44,27 @@ namespace wolf
WOLF_PTR_TYPEDEFS(ProcessorLoopClosureIcp);
WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorLoopClosureIcp);
-struct ParamsProcessorLoopClosureIcp : public ParamsProcessorLoopClosure
+struct ParamsProcessorLoopClosureIcp : public ParamsProcessorLoopClosure, public ParamsIcp
{
int recent_frames_ignored, frames_ignored_after_loop;
double max_error_threshold;
double min_points_percent;
- // ICP Params
- laserscanutils::icpParams icp_params;
-
ParamsProcessorLoopClosureIcp() = default;
- ParamsProcessorLoopClosureIcp(std::string _unique_name, const ParamsServer& _server):
- ParamsProcessorLoopClosure(_unique_name, _server)
+ ParamsProcessorLoopClosureIcp(std::string _unique_name, const ParamsServer& _server) :
+ ParamsProcessorLoopClosure(_unique_name, _server),
+ ParamsIcp(prefix + _unique_name, _server)
{
recent_frames_ignored = _server.getParam<int> (prefix + _unique_name + "/recent_frames_ignored");
frames_ignored_after_loop = _server.getParam<int> (prefix + _unique_name + "/frames_ignored_after_loop");
max_error_threshold = _server.getParam<double> (prefix + _unique_name + "/max_error_threshold");
min_points_percent = _server.getParam<double> (prefix + _unique_name + "/min_points_percent");
-
- icp_params.use_point_to_line_distance = _server.getParam<int> (prefix + _unique_name + "/use_point_to_line_distance");
- icp_params.max_correspondence_dist = _server.getParam<double> (prefix + _unique_name + "/max_correspondence_dist");
- icp_params.max_iterations = _server.getParam<int> (prefix + _unique_name + "/max_iterations");
- icp_params.use_corr_tricks = _server.getParam<int> (prefix + _unique_name + "/use_corr_tricks");
- icp_params.outliers_maxPerc = _server.getParam<double> (prefix + _unique_name + "/outliers_maxPerc");
- icp_params.outliers_adaptive_order = _server.getParam<double> (prefix + _unique_name + "/outliers_adaptive_order");
- icp_params.outliers_adaptive_mult = _server.getParam<double> (prefix + _unique_name + "/outliers_adaptive_mult");
- icp_params.cov_factor = _server.getParam<double> (prefix + _unique_name + "/cov_factor");
- icp_params.do_compute_covariance = _server.getParam<int> (prefix + _unique_name + "/do_compute_covariance");
- icp_params.max_angular_correction_deg = _server.getParam<double> (prefix + _unique_name + "/max_angular_correction_deg");
- icp_params.max_linear_correction = _server.getParam<double> (prefix + _unique_name + "/max_linear_correction");
- icp_params.epsilon_xy = _server.getParam<double> (prefix + _unique_name + "/epsilon_xy");
- icp_params.epsilon_theta = _server.getParam<double> (prefix + _unique_name + "/epsilon_theta");
- icp_params.sigma = _server.getParam<double> (prefix + _unique_name + "/sigma");
- icp_params.restart = _server.getParam<int> (prefix + _unique_name + "/restart");
- icp_params.restart_threshold_mean_error = _server.getParam<double> (prefix + _unique_name + "/restart_threshold_mean_error");
- icp_params.restart_dt = _server.getParam<double> (prefix + _unique_name + "/restart_dt");
- icp_params.restart_dtheta = _server.getParam<double> (prefix + _unique_name + "/restart_dtheta");
- icp_params.clustering_threshold = _server.getParam<double> (prefix + _unique_name + "/clustering_threshold");
- icp_params.orientation_neighbourhood = _server.getParam<int> (prefix + _unique_name + "/orientation_neighbourhood");
- icp_params.do_alpha_test = _server.getParam<int> (prefix + _unique_name + "/do_alpha_test");
- icp_params.do_alpha_test_thresholdDeg = _server.getParam<double> (prefix + _unique_name + "/do_alpha_test_thresholdDeg");
- icp_params.do_visibility_test = _server.getParam<int> (prefix + _unique_name + "/do_visibility_test");
- icp_params.outliers_remove_doubles = _server.getParam<int> (prefix + _unique_name + "/outliers_remove_doubles");
- icp_params.debug_verify_tricks = _server.getParam<int> (prefix + _unique_name + "/debug_verify_tricks");
- icp_params.gpm_theta_bin_size_deg = _server.getParam<double> (prefix + _unique_name + "/gpm_theta_bin_size_deg");
- icp_params.gpm_extend_range_deg = _server.getParam<double> (prefix + _unique_name + "/gpm_extend_range_deg");
- icp_params.gpm_interval = _server.getParam<double> (prefix + _unique_name + "/gpm_interval");
- icp_params.min_reading = _server.getParam<double> (prefix + _unique_name + "/min_reading");
- icp_params.max_reading = _server.getParam<double> (prefix + _unique_name + "/max_reading");
- icp_params.use_ml_weights = _server.getParam<int> (prefix + _unique_name + "/use_ml_weights");
- icp_params.use_sigma_weights = _server.getParam<int> (prefix + _unique_name + "/use_sigma_weights");
}
+
std::string print() const override
{
- return "\n" + ParamsProcessorBase::print()
+ return "\n" + ParamsProcessorLoopClosure::print()
+ + ParamsIcp::print() + "\n"
+ "recent_frames_ignored: " + std::to_string(recent_frames_ignored) + "\n"
+ "frames_ignored_after_loop: " + std::to_string(frames_ignored_after_loop) + "\n"
+ "max_error_threshold: " + std::to_string(max_error_threshold) + "\n"
diff --git a/include/laser/processor/processor_odom_icp.h b/include/laser/processor/processor_odom_icp.h
index 12051e701336ec0850533ee98f9de6405d56f6ff..711bd767e9d5fd937e1f926065b1fbebe1372a8e 100644
--- a/include/laser/processor/processor_odom_icp.h
+++ b/include/laser/processor/processor_odom_icp.h
@@ -27,6 +27,8 @@
**************************/
#include "core/common/wolf.h"
#include "core/processor/processor_tracker.h"
+#include "core/processor/motion_provider.h"
+#include "laser/processor/params_icp.h"
#include "laser/capture/capture_laser_2d.h"
#include "core/factor/factor_relative_pose_2d_with_extrinsics.h"
#include "laser/feature/feature_icp_align.h"
@@ -42,12 +44,8 @@ namespace wolf {
WOLF_PTR_TYPEDEFS(ProcessorOdomIcp);
WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorOdomIcp);
-struct ParamsProcessorOdomIcp : public ParamsProcessorTracker, public ParamsMotionProvider
+struct ParamsProcessorOdomIcp : public ParamsProcessorTracker, public ParamsMotionProvider, public ParamsIcp
{
- // ICP Params
- laserscanutils::icpParams icp_params;
-
- // Other processor params
double vfk_min_dist;
double vfk_min_angle;
double vfk_min_time;
@@ -60,64 +58,24 @@ struct ParamsProcessorOdomIcp : public ParamsProcessorTracker, public ParamsMoti
ParamsProcessorOdomIcp() = default;
ParamsProcessorOdomIcp(std::string _unique_name, const ParamsServer &_server):
ParamsProcessorTracker(_unique_name, _server),
- ParamsMotionProvider(_unique_name, _server)
+ ParamsMotionProvider(_unique_name, _server),
+ ParamsIcp(prefix + _unique_name, _server)
{
// keyframe voting
- vfk_min_dist = _server.getParam<double> (prefix + _unique_name + "/keyframe_vote/min_dist");
- vfk_min_angle = _server.getParam<double> (prefix + _unique_name + "/keyframe_vote/min_angle");
- vfk_min_time = _server.getParam<double> (prefix + _unique_name + "/keyframe_vote/min_time");
- vfk_min_error = _server.getParam<double> (prefix + _unique_name + "/keyframe_vote/min_error");
- vfk_max_points = _server.getParam<int> (prefix + _unique_name + "/keyframe_vote/max_points");
-
- // ICP
- icp_params.cov_factor = _server.getParam<double> (prefix + _unique_name + "/cov_factor");
-
- icp_params.use_point_to_line_distance = _server.getParam<int> (prefix + _unique_name + "/use_point_to_line_distance");
- icp_params.max_correspondence_dist = _server.getParam<double> (prefix + _unique_name + "/max_correspondence_dist");
- icp_params.max_iterations = _server.getParam<int> (prefix + _unique_name + "/max_iterations");
- icp_params.use_corr_tricks = _server.getParam<int> (prefix + _unique_name + "/use_corr_tricks");
- icp_params.outliers_maxPerc = _server.getParam<double> (prefix + _unique_name + "/outliers_maxPerc");
- icp_params.outliers_adaptive_order = _server.getParam<double> (prefix + _unique_name + "/outliers_adaptive_order");
- icp_params.outliers_adaptive_mult = _server.getParam<double> (prefix + _unique_name + "/outliers_adaptive_mult");
-
- icp_params.do_compute_covariance = _server.getParam<int> (prefix + _unique_name + "/do_compute_covariance");
- icp_params.max_angular_correction_deg = _server.getParam<double> (prefix + _unique_name + "/max_angular_correction_deg");
- icp_params.max_linear_correction = _server.getParam<double> (prefix + _unique_name + "/max_linear_correction");
- icp_params.epsilon_xy = _server.getParam<double> (prefix + _unique_name + "/epsilon_xy");
- icp_params.epsilon_theta = _server.getParam<double> (prefix + _unique_name + "/epsilon_theta");
- icp_params.sigma = _server.getParam<double> (prefix + _unique_name + "/sigma");
- icp_params.restart = _server.getParam<int> (prefix + _unique_name + "/restart");
- icp_params.restart_threshold_mean_error = _server.getParam<double> (prefix + _unique_name + "/restart_threshold_mean_error");
- icp_params.restart_dt = _server.getParam<double> (prefix + _unique_name + "/restart_dt");
- icp_params.restart_dtheta = _server.getParam<double> (prefix + _unique_name + "/restart_dtheta");
- icp_params.clustering_threshold = _server.getParam<double> (prefix + _unique_name + "/clustering_threshold");
- icp_params.orientation_neighbourhood = _server.getParam<int> (prefix + _unique_name + "/orientation_neighbourhood");
- icp_params.do_alpha_test = _server.getParam<int> (prefix + _unique_name + "/do_alpha_test");
- icp_params.do_alpha_test_thresholdDeg = _server.getParam<double> (prefix + _unique_name + "/do_alpha_test_thresholdDeg");
- icp_params.do_visibility_test = _server.getParam<int> (prefix + _unique_name + "/do_visibility_test");
- icp_params.outliers_remove_doubles = _server.getParam<int> (prefix + _unique_name + "/outliers_remove_doubles");
- icp_params.debug_verify_tricks = _server.getParam<int> (prefix + _unique_name + "/debug_verify_tricks");
- icp_params.gpm_theta_bin_size_deg = _server.getParam<double> (prefix + _unique_name + "/gpm_theta_bin_size_deg");
- icp_params.gpm_extend_range_deg = _server.getParam<double> (prefix + _unique_name + "/gpm_extend_range_deg");
- icp_params.gpm_interval = _server.getParam<double> (prefix + _unique_name + "/gpm_interval");
-
- icp_params.min_reading = _server.getParam<double> (prefix + _unique_name + "/min_reading");
- icp_params.max_reading = _server.getParam<double> (prefix + _unique_name + "/max_reading");
- icp_params.use_ml_weights = _server.getParam<int> (prefix + _unique_name + "/use_ml_weights");
- icp_params.use_sigma_weights = _server.getParam<int> (prefix + _unique_name + "/use_sigma_weights");
-
- initial_guess = _server.getParam<std::string>(prefix + _unique_name + "/initial_guess");
+ vfk_min_dist = _server.getParam<double> (prefix + _unique_name + "/keyframe_vote/min_dist");
+ vfk_min_angle = _server.getParam<double> (prefix + _unique_name + "/keyframe_vote/min_angle");
+ vfk_min_time = _server.getParam<double> (prefix + _unique_name + "/keyframe_vote/min_time");
+ vfk_min_error = _server.getParam<double> (prefix + _unique_name + "/keyframe_vote/min_error");
+ vfk_max_points = _server.getParam<int> (prefix + _unique_name + "/keyframe_vote/max_points");
+
+ initial_guess = _server.getParam<std::string>(prefix + _unique_name + "/initial_guess");
}
- std::string print() const override {
+
+ std::string print() const override
+ {
return "\n" + ParamsProcessorTracker::print() + "\n"
- + "cov_factor: " + std::to_string(icp_params.cov_factor) + "\n"
- + "use_point_to_line_distance: "+ std::to_string(icp_params.use_point_to_line_distance) + "\n"
- + "max_correspondence_dist: " + std::to_string(icp_params.max_correspondence_dist) + "\n"
- + "max_iterations: " + std::to_string(icp_params.max_iterations) + "\n"
- + "use_corr_tricks: " + std::to_string(icp_params.use_corr_tricks) + "\n"
- + "outliers_maxPerc: " + std::to_string(icp_params.outliers_maxPerc) + "\n"
- + "outliers_adaptive_order: " + std::to_string(icp_params.outliers_adaptive_order) + "\n"
- + "outliers_adaptive_mult: " + std::to_string(icp_params.outliers_adaptive_mult) + "\n"
+ + ParamsMotionProvider::print() + "\n"
+ + ParamsIcp::print() + "\n"
+ "initial_guess: " + initial_guess + "\n"
+ "keyframe_vote/min_dist: " + std::to_string(vfk_min_dist) + "\n"
+ "keyframe_vote/min_angle: " + std::to_string(vfk_min_angle) + "\n"
diff --git a/src/yaml/processor_odom_icp_yaml.cpp b/src/yaml/processor_odom_icp_yaml.cpp
index ac45306cf57f971c5bf02f393738a29dbc341f48..a21e86bd2cbbd83aa3192870ca738609c52f661f 100644
--- a/src/yaml/processor_odom_icp_yaml.cpp
+++ b/src/yaml/processor_odom_icp_yaml.cpp
@@ -58,16 +58,16 @@ ParamsProcessorBasePtr createParamsProcessorOdomIcp(const std::string& _filename
params->max_new_features = config["max_new_features"] .as<SizeEigen>();
params->icp_params.cov_factor = config["cov_factor"] .as<double>();
- params->icp_params.use_point_to_line_distance = config["use_point_to_line_distance"] .as<int>();
+ params->icp_params.use_point_to_line_distance = config["use_point_to_line_distance"] .as<bool>();
params->icp_params.max_correspondence_dist = config["max_correspondence_dist"] .as<int>();
params->icp_params.max_iterations = config["max_iterations"] .as<int>();
- params->icp_params.use_corr_tricks = config["use_corr_tricks"] .as<int>();
+ params->icp_params.use_corr_tricks = config["use_corr_tricks"] .as<bool>();
params->icp_params.outliers_maxPerc = config["outliers_maxPerc"] .as<double>();
params->icp_params.outliers_adaptive_order = config["outliers_adaptive_order"] .as<double>();
params->icp_params.outliers_adaptive_mult = config["outliers_adaptive_mult"] .as<double>();
params->icp_params.min_reading = config["min_reading"] .as<double>();
params->icp_params.max_reading = config["max_reading"] .as<double>();
- params->icp_params.do_compute_covariance = config["do_compute_covariance"] .as<int>();
+ params->icp_params.do_compute_covariance = config["do_compute_covariance"] .as<bool>();
params->vfk_min_dist = config["vfk_min_dist"] .as<double>();
params->vfk_min_angle = config["vfk_min_angle"] .as<double>();
diff --git a/test/gtest_processor_loop_closure_icp.cpp b/test/gtest_processor_loop_closure_icp.cpp
index c5941b063f42865d97b2d5d70eb64e512fb58acf..998bc2e77021e116347ac20be0f34577082a47d8 100644
--- a/test/gtest_processor_loop_closure_icp.cpp
+++ b/test/gtest_processor_loop_closure_icp.cpp
@@ -78,9 +78,6 @@ class ProcessorLoopClosureIcp_Test : public testing::Test
params->icp_params.do_visibility_test = 0;
params->icp_params.outliers_remove_doubles = 0;
params->icp_params.debug_verify_tricks = 0;
- params->icp_params.gpm_theta_bin_size_deg = 0;
- params->icp_params.gpm_extend_range_deg = 0;
- params->icp_params.gpm_interval = 0;
params->icp_params.min_reading = 0;
params->icp_params.max_reading = 100;
params->icp_params.use_ml_weights = 0;
diff --git a/test/gtest_processor_odom_icp.cpp b/test/gtest_processor_odom_icp.cpp
index db934f54a34c0d3516a8d64819e2211c4f85298c..64ee03ad44502ba5e5752a5cb13ff89a17d1fa12 100644
--- a/test/gtest_processor_odom_icp.cpp
+++ b/test/gtest_processor_odom_icp.cpp
@@ -96,9 +96,9 @@ TEST(ParamsProcessorOdomIcp, default_construct_and_print)
ASSERT_TRUE(params); // not nullptr
- params->icp_params.use_corr_tricks = 15;
+ params->icp_params.use_corr_tricks = false;
- ASSERT_EQ(params->icp_params.use_corr_tricks, 15);
+ ASSERT_EQ(params->icp_params.use_corr_tricks, false);
WOLF_INFO("params: ", params->print());
}
diff --git a/test/yaml/params_processor_loop_closure_falko.yaml b/test/yaml/params_processor_loop_closure_falko.yaml
index c41ca1198cfa4c7e5cc6cb2e8089b830ce6ec3a0..a936e3029a9681df069492f873091f53385c097b 100644
--- a/test/yaml/params_processor_loop_closure_falko.yaml
+++ b/test/yaml/params_processor_loop_closure_falko.yaml
@@ -48,46 +48,34 @@
min_points_percent : 0.5
# from CSM
icp:
+ verbose: false
max_iterations: 50
max_correspondence_dist: 1
- use_corr_tricks: 1
+ use_corr_tricks: true
outliers_maxPerc: 0.9
- use_point_to_line_distance: 1
+ use_point_to_line_distance: true
outliers_adaptive_order: 0.7
outliers_adaptive_mult: 1.5
- do_compute_covariance: 1
+ do_compute_covariance: true
+ cov_factor: 1
- max_angular_correction_deg: 0
- max_linear_correction: 0
- epsilon_xy: 0
- epsilon_theta: 0
+ max_angular_correction_deg: 4
+ max_linear_correction: 10
+ epsilon_xy: 1e-3
+ epsilon_theta: 1e-2
sigma: 0.2
- restart: 0
+ restart: false
restart_threshold_mean_error: 0
restart_dt: 0
restart_dtheta: 0
clustering_threshold: 0
orientation_neighbourhood: 0
- do_alpha_test: 0
+ do_alpha_test: false
do_alpha_test_thresholdDeg: 0
- do_visibility_test: 0
- outliers_remove_doubles: 0
- debug_verify_tricks: 0
- gpm_theta_bin_size_deg: 0
- gpm_extend_range_deg: 0
- gpm_interval: 0
- laser_x: 0
- laser_y: 0
- laser_theta: 0
+ do_visibility_test: false
+ outliers_remove_doubles: false
+ debug_verify_tricks: false
min_reading: 0.023
max_reading: 60
- use_ml_weights: 0
- use_sigma_weights: 0
- hsm_linear_cell_size: 0.03
- hsm_angular_cell_size_deg: 1
- hsm_num_angular_hypotheses: 8
- hsm_xc_directions_min_distance_deg: 10
- hsm_xc_ndirections: 3
- hsm_angular_hyp_min_distance_deg: 10
- hsm_linear_xc_max_npeaks: 5
- hsm_linear_xc_peaks_min_distance: 5
\ No newline at end of file
+ use_ml_weights: false
+ use_sigma_weights: false
\ No newline at end of file
diff --git a/test/yaml/processor_loop_closure_icp.yaml b/test/yaml/processor_loop_closure_icp.yaml
index b0fb57d38e7e27e45fc8fc87fd72a0627c87f543..a553959b916989ffb66b1a70f9afe900f3a21c77 100644
--- a/test/yaml/processor_loop_closure_icp.yaml
+++ b/test/yaml/processor_loop_closure_icp.yaml
@@ -8,16 +8,16 @@ max_new_features : 0
# from processor odom ICP
cov_factor : 1
-use_point_to_line_distance : 1
+use_point_to_line_distance : true
max_correspondence_dist : 2
max_iterations : 3
-use_corr_tricks : 1
+use_corr_tricks : true
outliers_maxPerc : 5
outliers_adaptive_order : 6
outliers_adaptive_mult : 7
max_reading : 100
min_reading : 0
-do_compute_covariance : 1
+do_compute_covariance : true
vfk_min_dist : 0
vfk_min_angle : 0
vfk_min_time : 0
diff --git a/test/yaml/processor_odom_icp.yaml b/test/yaml/processor_odom_icp.yaml
index c07305f9ec7e36e27973ca72bcd2cbf8356371bd..601de1ad83f470e1a9999aeedd7521909c247f6f 100644
--- a/test/yaml/processor_odom_icp.yaml
+++ b/test/yaml/processor_odom_icp.yaml
@@ -10,16 +10,16 @@ max_new_features : 0
# from processor odom ICP
cov_factor : 1
-use_point_to_line_distance : 1
+use_point_to_line_distance : true
max_correspondence_dist : 2
max_iterations : 3
-use_corr_tricks : 1
+use_corr_tricks : true
outliers_maxPerc : 5
outliers_adaptive_order : 6
outliers_adaptive_mult : 7
max_reading : 100
min_reading : 0
-do_compute_covariance : 1
+do_compute_covariance : true
vfk_min_dist : 0
vfk_min_angle : 0
vfk_min_time : 0