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

Add test with solver

parent 8e9ba929
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"
Hide whitespace changes
Inline Side-by-side
src/processor/processor_odom_icp.cpp
+ 6
− 3
View file @ 155a05b8
...@@ -50,7 +50,7 @@ unsigned int ProcessorOdomICP::processKnown() ...@@ -50,7 +50,7 @@ unsigned int ProcessorOdomICP::processKnown()
{ {
// Match the incoming with the origin, passing the transform from origin to last as initialization // Match the incoming with the origin, passing the transform from origin to last as initialization
if (processing_step_ != FIRST_TIME_WITH_PACK && processing_step_ != FIRST_TIME_WITHOUT_PACK) // It's not FIRST_TIME if (origin_ptr_) // Make sure it's not the FIRST_TIME
{ {
CaptureLaser2DPtr origin_ptr = std::static_pointer_cast<CaptureLaser2D>(origin_ptr_); CaptureLaser2DPtr origin_ptr = std::static_pointer_cast<CaptureLaser2D>(origin_ptr_);
CaptureLaser2DPtr incoming_ptr = std::static_pointer_cast<CaptureLaser2D>(incoming_ptr_); CaptureLaser2DPtr incoming_ptr = std::static_pointer_cast<CaptureLaser2D>(incoming_ptr_);
...@@ -200,8 +200,11 @@ void ProcessorOdomICP::resetDerived() ...@@ -200,8 +200,11 @@ void ProcessorOdomICP::resetDerived()
void ProcessorOdomICP::establishFactors() void ProcessorOdomICP::establishFactors()
{ {
auto ftr_ptr = emplaceFeature(last_ptr_); if (last_ptr_ != origin_ptr_)
emplaceFactor(ftr_ptr); {
auto ftr_ptr = emplaceFeature(last_ptr_);
emplaceFactor(ftr_ptr);
}
} }
FeatureBasePtr ProcessorOdomICP::emplaceFeature(CaptureBasePtr _capture_laser) FeatureBasePtr ProcessorOdomICP::emplaceFeature(CaptureBasePtr _capture_laser)
......
test/gtest_processor_odom_icp.cpp
+ 54
− 19
View file @ 155a05b8
...@@ -5,9 +5,13 @@ ...@@ -5,9 +5,13 @@
* \author: jsola * \author: jsola
*/ */
#include "laser/processor/processor_odom_icp.h"
#include "laser/internal/config.h" #include "laser/internal/config.h"
#include "core/utils/utils_gtest.h"
#include <core/ceres_wrapper/ceres_manager.h>
#include <core/utils/utils_gtest.h>
#include "laser/processor/processor_odom_icp.h" // THIS AT THE END OTHERWISE IT FAILS COMPILING
using namespace wolf; using namespace wolf;
using namespace Eigen; using namespace Eigen;
...@@ -19,9 +23,9 @@ class ProcessorOdomICPPublic : public ProcessorOdomICP ...@@ -19,9 +23,9 @@ class ProcessorOdomICPPublic : public ProcessorOdomICP
public: public:
ProcessorOdomICPPublic(const ProcessorParamsOdomICPPtr& _params) : ProcessorOdomICPPublic(const ProcessorParamsOdomICPPtr& _params) :
ProcessorOdomICP(_params) ProcessorOdomICP(_params)
{ {
} }
virtual ~ProcessorOdomICPPublic(){} virtual ~ProcessorOdomICPPublic(){}
// we can add public methods here to access protected stuff in ProcessorOdomICP // we can add public methods here to access protected stuff in ProcessorOdomICP
...@@ -33,9 +37,12 @@ class ProcessorOdomICP_Test : public testing::Test ...@@ -33,9 +37,12 @@ class ProcessorOdomICP_Test : public testing::Test
{ {
public: public:
ProblemPtr problem; ProblemPtr problem;
CeresManagerPtr solver;
SensorLaser2DPtr lidar; SensorLaser2DPtr lidar;
ProcessorOdomICPPublicPtr processor; ProcessorOdomICPPublicPtr processor;
std::vector<float> ranges;
TimeStamp t0; TimeStamp t0;
Vector3s x0; Vector3s x0;
Matrix3s P0; Matrix3s P0;
...@@ -43,15 +50,17 @@ class ProcessorOdomICP_Test : public testing::Test ...@@ -43,15 +50,17 @@ class ProcessorOdomICP_Test : public testing::Test
virtual void SetUp() virtual void SetUp()
{ {
problem = Problem::create("PO", 2); problem = Problem::create("PO", 2);
solver = std::make_shared<CeresManager>(problem);
auto sen = problem->installSensor("LASER 2D", "lidar", Eigen::Vector3s(0,0,0), laser_root_dir + "/test/yaml/sensor_laser_2D.yaml"); auto sen = problem->installSensor("LASER 2D", "lidar", Eigen::Vector3s(0,0,0), laser_root_dir + "/test/yaml/sensor_laser_2D.yaml");
lidar = std::static_pointer_cast<SensorLaser2D>(sen); lidar = std::static_pointer_cast<SensorLaser2D>(sen);
auto prc = problem->installProcessor("ODOM ICP", "prc icp", "lidar", laser_root_dir + "/test/yaml/processor_odom_ICP.yaml"); auto prc = problem->installProcessor("ODOM ICP", "prc icp", "lidar", laser_root_dir + "/test/yaml/processor_odom_ICP.yaml");
processor = std::static_pointer_cast<ProcessorOdomICPPublic>(prc); processor = std::static_pointer_cast<ProcessorOdomICPPublic>(prc);
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});
WOLF_TRACE("\n\n========== Prior ==========")
t0 = 0.0; t0 = 0.0;
x0 = Vector3s(0,0,0); x0 = Vector3s(0,0,0);
P0 . setIdentity(); P0 . setIdentity();
...@@ -106,24 +115,21 @@ TEST(ProcessorOdomIcp, creator_yaml) ...@@ -106,24 +115,21 @@ TEST(ProcessorOdomIcp, creator_yaml)
TEST_F(ProcessorOdomICP_Test, full) TEST_F(ProcessorOdomICP_Test, full)
{ {
std::vector<float> ranges({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});
problem->print(4,1,1,0);
TimeStamp t(t0); TimeStamp t(t0);
// ASSERT_MATRIX_APPROX(x, x0, 1e-6);
// ASSERT_MATRIX_APPROX(F1->getState(), x0, 1e-6);
for (int i = 0; i < 7; i++) for (int i = 0; i < 7; i++)
{ {
WOLF_TRACE("\n\n========== Process ", t.getSeconds(), " =========="); std::cout << "\n========== Process " << t.getSeconds() << " sec. ==========" << std::endl;
CaptureLaser2DPtr scan = std::make_shared<CaptureLaser2D>(t, lidar, ranges); CaptureLaser2DPtr scan = std::make_shared<CaptureLaser2D>(t, lidar, ranges);
scan->process(); scan->process();
F = problem->getLastFrame(); F = problem->getLastFrame();
ASSERT_MATRIX_APPROX(F->getState(), x0, 1e-6);
WOLF_TRACE("F", F->id() , " ", F->getState().transpose()); WOLF_TRACE("F", F->id() , " ", F->getState().transpose());
if (i >= 2 && i <= 4) if (i >= 2 && i <= 4)
...@@ -133,20 +139,49 @@ TEST_F(ProcessorOdomICP_Test, full) ...@@ -133,20 +139,49 @@ TEST_F(ProcessorOdomICP_Test, full)
WOLF_TRACE("F", F->id() , " ", F->getState().transpose(), " perturbed!"); WOLF_TRACE("F", F->id() , " ", F->getState().transpose(), " perturbed!");
} }
problem->print(4,1,1,0);
t += 1.0; t += 1.0;
} }
ASSERT_MATRIX_APPROX(F->getState(), x0, 1e-6); ASSERT_MATRIX_APPROX(F->getState(), x0, 1e-6);
}
TEST_F(ProcessorOdomICP_Test, solve)
{
TimeStamp t(t0);
for (int i = 0; i < 4; i++)
{
std::cout << "\n========== Process " << t.getSeconds() << " sec. ==========" << std::endl;
CaptureLaser2DPtr scan = std::make_shared<CaptureLaser2D>(t, lidar, ranges);
scan->process();
t += 1.0;
}
for (auto F : problem->getTrajectory()->getFrameList())
{
if (F->isKey())
F->setState(0.5 * Vector3s::Random());
}
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
WOLF_TRACE(report);
for (auto F : problem->getTrajectory()->getFrameList())
{
if (F->isKey())
{
ASSERT_MATRIX_APPROX(F->getState() , x0 , 1e-6);
}
}
} }
int main(int argc, char **argv) int main(int argc, char **argv)
{ {
testing::InitGoogleTest(&argc, argv); testing::InitGoogleTest(&argc, argv);
::testing::GTEST_FLAG(filter) = "ProcessorOdomICP_Test.full"; ::testing::GTEST_FLAG(filter) = "ProcessorOdomICP_Test.solve";
return RUN_ALL_TESTS(); return RUN_ALL_TESTS();
} }
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment