diff --git a/include/laser/processor/params_icp.h b/include/laser/processor/params_icp.h
index f305379252a2ab957a5a57f374c29ef717f09757..b5dc29f32706dc2e61b1739ba4287deb374ff6ce 100644
--- a/include/laser/processor/params_icp.h
+++ b/include/laser/processor/params_icp.h
@@ -80,6 +80,11 @@ struct ParamsIcp
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");
icp_params.cov_max_eigv_factor = _server.getParam<double> (_prefix_and_unique_name + "/icp/cov_max_eigv_factor");
+
+ icp_params.attempts = _server.getParam<unsigned int> (_prefix_and_unique_name + "/icp/attempts");
+ icp_params.perturbation_new_attempts = _server.getParam<double> (_prefix_and_unique_name + "/icp/perturbation_new_attempts");
+ icp_params.max_mean_error = _server.getParam<double> (_prefix_and_unique_name + "/icp/max_mean_error");
+ icp_params.min_points_ratio = _server.getParam<double> (_prefix_and_unique_name + "/icp/min_points_ratio");
}
std::string print() const
@@ -114,7 +119,11 @@ struct ParamsIcp
+ "/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"
- + "/icp/cov_max_eigv_factor" + std::to_string(icp_params.cov_max_eigv_factor) + "\n";
+ + "/icp/cov_max_eigv_factor" + std::to_string(icp_params.cov_max_eigv_factor) + "\n"
+ + "/icp/attempts" + std::to_string(icp_params.attempts) + "\n"
+ + "/icp/perturbation_new_attempts" + std::to_string(icp_params.perturbation_new_attempts) + "\n"
+ + "/icp/max_mean_error" + std::to_string(icp_params.max_mean_error) + "\n"
+ + "/icp/min_points_ratio" + std::to_string(icp_params.min_points_ratio) + "\n";
}
};
diff --git a/src/yaml/processor_odom_icp_yaml.cpp b/src/yaml/processor_odom_icp_yaml.cpp
index 52c5be60d5c066b714d5c6fe9b2fdd134498a707..0156e8da0de80e3686edb3f4de5728484d0bcc60 100644
--- a/src/yaml/processor_odom_icp_yaml.cpp
+++ b/src/yaml/processor_odom_icp_yaml.cpp
@@ -76,6 +76,11 @@ ParamsProcessorBasePtr createParamsProcessorOdomIcp(const std::string& _filename
params->vfk_max_points = config["vfk_max_points"] .as<int>();
params->initial_guess = config["initial_guess"] .as<std::string>();
+
+ params->icp_params.attempts = config["attempts"].as<unsigned int>();
+ params->icp_params.perturbation_new_attempts = config["perturbation_new_attempts"].as<double>();
+ params->icp_params.max_mean_error = config["max_mean_error"].as<double>();
+ params->icp_params.min_points_ratio = config["min_points_ratio"].as<double>();
return params;
}
diff --git a/test/gtest_processor_loop_closure_icp.cpp b/test/gtest_processor_loop_closure_icp.cpp
index 137f4028c179a54e7da7f37494a9b9ba4f58166d..c60b42724b0669364f3ec8bc1e109baf5debe8d3 100644
--- a/test/gtest_processor_loop_closure_icp.cpp
+++ b/test/gtest_processor_loop_closure_icp.cpp
@@ -88,6 +88,11 @@ class ProcessorLoopClosureIcp_Test : public testing::Test
params->icp_params.use_ml_weights = false;
params->icp_params.use_sigma_weights = false;
+ params->icp_params.attempts = 5;
+ params->icp_params.perturbation_new_attempts = 1e-1;
+ params->icp_params.max_mean_error = 1e-2;
+ params->icp_params.min_points_ratio = 0.5;
+
processor = std::static_pointer_cast<ProcessorLoopClosureIcp>(problem->installProcessor("ProcessorLoopClosureIcp",
"prc icp",
lidar,
diff --git a/test/gtest_processor_odom_icp.cpp b/test/gtest_processor_odom_icp.cpp
index f0317a0c491f525cfa085587546d0a579d1764e7..5b7e27bf3a9b82caf32e71df582708da683c3d34 100644
--- a/test/gtest_processor_odom_icp.cpp
+++ b/test/gtest_processor_odom_icp.cpp
@@ -43,28 +43,13 @@ using namespace Eigen;
std::string laser_root_dir = _WOLF_LASER_ROOT_DIR;
-class ProcessorOdomIcpPublic : public ProcessorOdomIcp
-{
- public:
- ProcessorOdomIcpPublic(const ParamsProcessorOdomIcpPtr& _params) :
- ProcessorOdomIcp(_params)
- {
-
- }
- ~ProcessorOdomIcpPublic() override{}
-
- // we can add public methods here to access protected stuff in ProcessorOdomIcp
-};
-
-WOLF_PTR_TYPEDEFS(ProcessorOdomIcpPublic);
-
class ProcessorOdomIcp_Test : public testing::Test
{
public:
ProblemPtr problem;
SolverCeresPtr solver;
SensorLaser2dPtr lidar;
- ProcessorOdomIcpPublicPtr processor;
+ ProcessorOdomIcpPtr processor;
std::vector<float> ranges;
@@ -83,7 +68,9 @@ class ProcessorOdomIcp_Test : public testing::Test
lidar = std::static_pointer_cast<SensorLaser2d>(sen);
auto prc = problem->installProcessor("ProcessorOdomIcp", "prc icp", "lidar", laser_root_dir + "/test/yaml/processor_odom_icp.yaml");
- processor = std::static_pointer_cast<ProcessorOdomIcpPublic>(prc);
+ processor = std::static_pointer_cast<ProcessorOdomIcp>(prc);
+
+ WOLF_INFO(processor->params_odom_icp_->print());
ranges = std::vector<float>({49.97591781616211, 49.996429443359375, 49.999759674072266, 50.0, 50.0, 50.0, 49.998634338378906, 50.0, 50.0, 49.99236297607422, 49.99384307861328, 50.0, 49.9869270324707, 50.0, 49.99005889892578, 49.99773406982422, 50.0, 49.98741912841797, 50.0, 49.99842071533203, 50.0, 49.99243927001953, 49.997291564941406, 50.0, 50.0, 49.98580551147461, 49.991844177246094, 49.98896026611328, 50.0, 50.0, 49.9897346496582, 49.998111724853516, 49.99882125854492, 50.0, 50.0, 50.0, 50.0, 49.999698638916016, 50.0, 50.0, 50.0, 50.0, 49.991397857666016, 49.99360275268555, 49.999027252197266, 49.99750900268555, 49.99100112915039, 49.998714447021484, 49.98794174194336, 50.0, 50.0, 50.0, 50.0, 50.0, 49.98186492919922, 50.0, 50.0, 50.0, 49.99155807495117, 49.997196197509766, 49.98872375488281, 49.99138259887695, 50.0, 49.99021530151367, 49.99164581298828, 50.0, 49.991390228271484, 50.0, 50.0, 50.0, 49.997249603271484, 50.0, 49.991851806640625, 50.0, 50.0, 50.0, 50.0, 50.0, 50.0, 49.97983169555664, 49.98630142211914, 50.0, 20.6888370513916, 49.9797477722168, 49.98846435546875, 49.99418640136719, 50.0, 50.0, 50.0, 49.99342346191406, 50.0, 49.9906005859375, 50.0, 49.99853515625, 49.989444732666016, 38.552852630615234, 38.28703689575195, 38.04865264892578, 37.78112030029297, 37.54747772216797, 37.28171157836914, 37.206565856933594, 36.835411071777344, 36.61864471435547, 36.39655685424805, 36.1579475402832, 35.95964431762695, 35.75605773925781, 35.552188873291016, 35.75926208496094, 36.27781677246094, 35.16993713378906, 34.99699401855469, 34.82910919189453, 34.6483039855957, 34.48637390136719, 34.32539749145508, 34.16202163696289, 34.232051849365234, 33.86043167114258, 33.71691131591797, 33.566650390625, 33.42384338378906, 33.2882080078125, 33.16693115234375, 33.041419982910156, 32.906009674072266, 33.008323669433594, 33.706356048583984, 34.43825149536133, 35.25088119506836, 36.05652618408203, 36.930118560791016, 37.83384704589844, 33.12321472167969, 33.02351760864258, 32.9192008972168, 32.82925796508789, 32.74382781982422, 32.64959716796875, 32.580204010009766, 32.49120330810547, 33.05714797973633, 32.343536376953125, 32.26381301879883, 32.21063232421875, 32.12488555908203, 32.06965255737305, 32.0222282409668, 31.954957962036133, 31.903532028198242, 31.83578872680664, 32.51456832885742, 34.189456939697266, 31.12668228149414, 31.076339721679688, 31.047151565551758, 30.967018127441406, 30.956220626831055, 30.924589157104492, 30.893285751342773, 30.869199752807617, 30.843050003051758, 32.791847229003906, 30.809431076049805, 30.79128074645996, 30.779237747192383, 30.776460647583008, 30.74305534362793, 30.74994468688965, 30.7137393951416, 30.734609603881836, 30.719928741455078, 30.71673011779785, 49.99970626831055, 50.0, 49.987911224365234, 33.68583679199219, 31.76846694946289, 31.8026123046875, 31.802202224731445, 31.818490982055664, 31.85223960876465, 31.86141014099121, 31.906801223754883, 31.93423843383789, 31.964210510253906, 33.567230224609375, 32.055015563964844, 32.07001876831055, 32.13076400756836, 32.16000747680664, 32.22781753540039, 32.26890563964844, 32.323944091796875, 32.36326217651367, 32.430908203125, 49.980655670166016, 34.32135772705078, 33.09465789794922, 32.27247619628906, 32.33710479736328, 32.41763687133789, 32.498661041259766, 32.57213592529297, 32.67158126831055, 32.74591827392578, 32.814476013183594, 32.93477249145508, 33.04751968383789, 33.136863708496094, 33.23999786376953, 33.34675979614258, 33.42970657348633, 33.53573226928711, 33.66716003417969, 33.78378677368164, 33.905670166015625, 34.02836608886719, 34.151817321777344, 34.2794189453125, 34.41516876220703, 34.551273345947266, 34.702728271484375, 34.84151840209961, 34.986881256103516, 35.162757873535156, 35.332794189453125, 35.47941970825195, 35.65633010864258, 35.82624435424805, 36.00060272216797, 36.17729187011719, 36.36515808105469, 36.55763626098633, 36.744773864746094, 38.46407699584961, 37.869808197021484, 37.767921447753906, 37.958900451660156, 38.20857620239258, 38.38622283935547, 38.68323516845703, 38.871334075927734, 39.151519775390625, 39.377750396728516, 39.68268966674805, 39.89873123168945, 40.197330474853516, 40.47549819946289, 40.73743438720703, 41.04566955566406, 42.33650207519531, 41.92591094970703, 41.43912124633789, 41.045528411865234, 41.32114028930664, 41.581878662109375, 41.944580078125, 42.318912506103516, 42.6725959777832, 43.07264709472656, 43.443477630615234, 43.83216094970703, 44.19996643066406, 44.63225555419922, 45.06049346923828, 45.468536376953125, 45.89896774291992, 46.330604553222656, 46.778343200683594, 47.31666946411133, 47.789310455322266, 48.26376724243164, 48.826602935791016, 49.33188247680664, 49.990909576416016, 50.0, 50.0, 50.0, 50.0, 49.995697021484375, 49.99568176269531, 49.98163986206055, 50.0, 50.0, 49.9764289855957, 50.0, 50.0, 49.98639678955078, 49.99431228637695, 50.0, 50.0, 50.0, 50.0, 49.9874267578125, 50.0, 49.98714828491211, 50.0, 49.99470901489258, 49.99113464355469, 50.0, 50.0, 50.0, 49.985504150390625, 49.99067306518555, 50.0, 49.997161865234375, 50.0, 50.0, 50.0, 49.995513916015625, 49.993038177490234, 50.0, 49.99763107299805, 50.0, 49.98752975463867, 50.0, 49.988037109375, 50.0, 50.0, 50.0, 49.9975700378418, 50.0, 49.998756408691406, 49.97819519042969, 49.99104690551758, 49.99087905883789, 49.94268798828125, 49.85968017578125, 49.786617279052734, 49.70594787597656, 49.62562561035156, 49.56686782836914, 49.50475311279297, 49.416934967041016, 49.35699462890625, 49.308589935302734, 49.990482330322266, 50.0, 49.998836517333984, 50.0, 50.0, 50.0, 50.0, 50.0, 50.0, 49.980472564697266, 49.99903869628906, 50.0, 50.0, 49.989845275878906, 49.98395919799805, 50.0, 49.99302673339844, 49.99530792236328, 49.99745559692383, 50.0, 49.99560546875, 21.569303512573242});
diff --git a/test/yaml/params_processor_loop_closure_falko.yaml b/test/yaml/params_processor_loop_closure_falko.yaml
index 27f8284dccabb10938ae35ce92beb0253c950f7d..b0b45fe39cb2e41bc741e00ed49286b37d883fcf 100644
--- a/test/yaml/params_processor_loop_closure_falko.yaml
+++ b/test/yaml/params_processor_loop_closure_falko.yaml
@@ -79,4 +79,8 @@
min_reading: 0.023
max_reading: 60
use_ml_weights: false
- use_sigma_weights: false
\ No newline at end of file
+ use_sigma_weights: false
+ attempts: 5
+ perturbation_new_attempts: 1e-1
+ max_mean_error: 1
+ min_points_ratio: 0.1
\ 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 c5125820901c97e44dc0066bdbe662bbec732eb7..6acd0a01a574ca42eb0bd7b5159956a75dabc514 100644
--- a/test/yaml/processor_loop_closure_icp.yaml
+++ b/test/yaml/processor_loop_closure_icp.yaml
@@ -20,4 +20,8 @@ outliers_adaptive_order : 6
outliers_adaptive_mult : 7
max_reading : 100
min_reading : 0
-do_compute_covariance : true
\ No newline at end of file
+do_compute_covariance : true
+attempts : 5
+perturbation_new_attempts : 1e-1
+max_mean_error : 1e-2
+min_points_ratio : 0.75
\ No newline at end of file
diff --git a/test/yaml/processor_odom_icp.yaml b/test/yaml/processor_odom_icp.yaml
index 601de1ad83f470e1a9999aeedd7521909c247f6f..b8ed2bec25500721a325f1115876e509c23a32d1 100644
--- a/test/yaml/processor_odom_icp.yaml
+++ b/test/yaml/processor_odom_icp.yaml
@@ -26,3 +26,7 @@ vfk_min_time : 0
vfk_min_error : 0
vfk_max_points : 0
initial_guess : "zero"
+attempts : 5
+perturbation_new_attempts : 1e-1
+max_mean_error : 1e-2
+min_points_ratio : 0.75