diff --git a/include/laser/processor/processor_closeloop_icp.h b/include/laser/processor/processor_closeloop_icp.h
index a78d0a41f8bac5aa0bc02e6317125cf563070e6f..8ef8240641f344f94dbcf2afd6e94cf78cd3cff5 100644
--- a/include/laser/processor/processor_closeloop_icp.h
+++ b/include/laser/processor/processor_closeloop_icp.h
@@ -189,20 +189,28 @@ class ProcessorCloseloopIcp : public ProcessorBase
protected:
virtual void processCapture(CaptureBasePtr) override;
- virtual void processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) override;
+ virtual void processKeyFrame(FrameBasePtr _keyframe_ptr,
+ const double& _time_tolerance) override;
virtual bool triggerInCapture(CaptureBasePtr) const override;
- virtual bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) const override;
+ virtual bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr,
+ const double& _time_tolerance) const override;
virtual bool storeKeyFrame(FrameBasePtr) override;
virtual bool storeCapture(CaptureBasePtr) override;
virtual bool voteForKeyFrame() const override;
- FrameBasePtrList selectCandidates(FrameBasePtr _keyframe_ptr, const double &_time_tolerance);
- std::map<double, CapturesAligned> evaluateCandidates(FrameBasePtr _keyframe_own, FrameBasePtrList& _keyframe_candidates);
+
+ FrameBasePtrList selectCandidates(FrameBasePtr _keyframe_ptr,
+ const double &_time_tolerance);
+
+ std::map<double, CapturesAligned> evaluateCandidates(FrameBasePtr _keyframe_own,
+ FrameBasePtrList& _keyframe_candidates);
+
CapturesAligned bestCandidate(std::map<double, CapturesAligned>& _capture_candidates);
+
FactorBasePtr emplaceFeatureAndFactor(CapturesAligned& _captures_aligned);
};
diff --git a/include/laser/processor/processor_odom_icp.h b/include/laser/processor/processor_odom_icp.h
index 27afd3e8d4105930d8b2f8c024f6f3f283551651..479eab02b6be08141f389c28ec81e07935d05fa8 100644
--- a/include/laser/processor/processor_odom_icp.h
+++ b/include/laser/processor/processor_odom_icp.h
@@ -169,9 +169,9 @@ class ProcessorOdomIcp : public ProcessorTracker, public IsMotion
virtual ~ProcessorOdomIcp();
virtual void configure(SensorBasePtr _sensor) override;
- virtual void getCurrentState(Eigen::VectorXd& _x) const override;
- virtual void getCurrentTimeStamp(TimeStamp& _ts) const override;
- virtual bool getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const override;
+ virtual VectorComposite getState() const override;
+ virtual TimeStamp getTimeStamp( ) const override;
+ virtual VectorComposite getState(const TimeStamp& _ts) const override;
void setProblem(ProblemPtr _problem_ptr) override;
protected:
diff --git a/src/processor/processor_closeloop_icp.cpp b/src/processor/processor_closeloop_icp.cpp
index 4e191c925246158ee46f7486b9c53cc768e90931..5f586c0804dd6cd10d3902a61c4d6478458cf8c8 100644
--- a/src/processor/processor_closeloop_icp.cpp
+++ b/src/processor/processor_closeloop_icp.cpp
@@ -217,7 +217,7 @@ std::map<double, CapturesAligned> ProcessorCloseloopIcp::evaluateCandidates(Fram
WOLF_DEBUG("DBG ------------------------------");
WOLF_DEBUG("DBG valid? ", trf.valid, " m_er ", mean_error, " ", points_coeff_used * 100, "% own_id: ", _keyframe_own->id(),
" other_id: ", _keyframe_other->id());
- WOLF_DEBUG("DBG own_POSE: ", _keyframe_own->getState().transpose(), " other_POSE: ", _keyframe_other->getState().transpose(),
+ WOLF_DEBUG("DBG own_POSE: ", _keyframe_own->getState().vector("PO").transpose(), " other_POSE: ", _keyframe_other->getState().vector("PO").transpose(),
" Icp_guess: ", transform_guess.transpose(), " Icp_trf: ", trf.res_transf.transpose());
// WOLF_DEBUG("Covariance \n", trf.res_covar);
if (trf.valid == 1 and mean_error < max_error_threshold and points_coeff_used * 100 > min_points_percent) {
diff --git a/src/processor/processor_odom_icp.cpp b/src/processor/processor_odom_icp.cpp
index 2270785068fefa1ac8a8d415fbd553bfd02d10f8..8ea5e822a278b3f7d4018d06b267652bccdf497b 100644
--- a/src/processor/processor_odom_icp.cpp
+++ b/src/processor/processor_odom_icp.cpp
@@ -5,7 +5,7 @@ using namespace wolf;
using namespace laserscanutils;
ProcessorOdomIcp::ProcessorOdomIcp(ParamsProcessorOdomIcpPtr _params):
- ProcessorTracker("ProcessorOdomIcp", 2, _params)
+ ProcessorTracker("ProcessorOdomIcp", "PO", 2, _params)
{
proc_params_ = _params;
@@ -236,7 +236,7 @@ void ProcessorOdomIcp::advanceDerived()
// overwrite last frame's state
- Isometry2d w_T_ro = Translation2d(origin_ptr_->getFrame()->getState().head(2)) * Rotation2Dd(origin_ptr_->getFrame()->getState()(2));
+ Isometry2d w_T_ro = Translation2d(origin_ptr_->getFrame()->getState().at("P")) * Rotation2Dd(origin_ptr_->getFrame()->getState().at("O")(0));
Isometry2d ro_T_so = Translation2d(origin_ptr_->getSensorP()->getState()) * Rotation2Dd(origin_ptr_->getSensorO()->getState()(0));
Isometry2d& ri_T_si = ro_T_so; // A reference just to have nice names without computing overhead
@@ -247,7 +247,7 @@ void ProcessorOdomIcp::advanceDerived()
Vector3d x_inc; x_inc << w_T_ri.translation() , Rotation2Dd(w_T_ri.rotation()).angle();
// Put the state of incoming in last
- last_ptr_->getFrame()->setState(x_inc);
+ last_ptr_->getFrame()->setState(x_inc, "PO", {2,1});
trf_origin_last_ = trf_origin_incoming_;
}
@@ -273,7 +273,7 @@ void ProcessorOdomIcp::resetDerived()
// x1_p_y1_y2: translation vector from y1 to y2 expressed in frame x1
- Isometry2d w_T_ro = Translation2d(origin_ptr_->getFrame()->getState().head(2)) * Rotation2Dd(origin_ptr_->getFrame()->getState()(2));
+ Isometry2d w_T_ro = Translation2d(origin_ptr_->getFrame()->getP()->getState()) * Rotation2Dd(origin_ptr_->getFrame()->getO()->getState()(0));
Isometry2d ro_T_so = Translation2d(origin_ptr_->getSensorP()->getState()) * Rotation2Dd(origin_ptr_->getSensorO()->getState()(0));
Isometry2d &ri_T_si = ro_T_so; // A reference just to have nice names without computing overhead
@@ -286,7 +286,7 @@ void ProcessorOdomIcp::resetDerived()
// last
- last_ptr_->getFrame()->setState(x_current);
+ last_ptr_->getFrame()->setState(x_current, "PO", {2,1});
// incoming_ptr_->getFrame()->setState(x_current);
trf_origin_last_ = trf_last_incoming_;
@@ -319,9 +319,9 @@ FactorBasePtr ProcessorOdomIcp::emplaceFactor(FeatureBasePtr _feature)
params_->apply_loss_function);
}
-void ProcessorOdomIcp::getCurrentState(Eigen::VectorXd& _x) const
+VectorComposite ProcessorOdomIcp::getState( ) const
{
- Isometry2d w_T_ro = Translation2d(origin_ptr_->getFrame()->getState().head(2)) * Rotation2Dd(origin_ptr_->getFrame()->getState()(2));
+ Isometry2d w_T_ro = Translation2d(origin_ptr_->getFrame()->getP()->getState()) * Rotation2Dd(origin_ptr_->getFrame()->getO()->getState()(0));
Isometry2d ro_T_so = Translation2d(origin_ptr_->getSensorP()->getState()) * Rotation2Dd(origin_ptr_->getSensorO()->getState()(0));
Isometry2d &ri_T_si = ro_T_so; // A reference just to have nice names without computing overhead
@@ -331,39 +331,50 @@ void ProcessorOdomIcp::getCurrentState(Eigen::VectorXd& _x) const
Isometry2d w_T_ri = w_T_ro * ro_T_so * so_T_si * ri_T_si.inverse();
Vector3d x_current;
- Eigen::Vector3d state = Vector3d(0, 0, 0);
- state.head(2) << w_T_ri.translation();
- state(2) = Rotation2Dd(w_T_ri.rotation()).angle();
- _x = state;
+ VectorComposite state("PO", {w_T_ri.translation(), Vector1d(Rotation2Dd(w_T_ri.rotation()).angle())});
- // _x = Vector3d(1,1,1);
+// Eigen::Vector3d state = Vector3d(0, 0, 0);
+// state.head(2) << w_T_ri.translation();
+// state(2) = Rotation2Dd(w_T_ri.rotation()).angle();
+
+ return state;
}
-void ProcessorOdomIcp::getCurrentTimeStamp(TimeStamp& _ts) const
+TimeStamp ProcessorOdomIcp::getTimeStamp() const
{
- // if( last_ptr_ == nullptr ) _ts = 1490285227;
- if( last_ptr_ == nullptr ) _ts = 0;
- else _ts = last_ptr_->getTimeStamp();
+ if( last_ptr_ == nullptr )
+ return 0;
+ else
+ return last_ptr_->getTimeStamp();
}
-bool ProcessorOdomIcp::getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const
+VectorComposite ProcessorOdomIcp::getState(const TimeStamp& _ts) const
{
- // getCurrentTimeStamp(_ts);
- if(last_ptr_ == nullptr) {
- _x = Vector3d(0, 0, 0);
- return false;
- }
- double d = std::abs(_ts - last_ptr_->getTimeStamp());
- if( d < 0.01 )
+ // todo fix this code to get any state in the whole trajectory!
+
+
+ if (last_ptr_ != nullptr)
{
- getCurrentState(_x);
- return true;
+ double d = fabs(_ts - last_ptr_->getTimeStamp());
+ if( d < params_->time_tolerance )
+ {
+ return getState( );
+ }
+ else
+ {
+ WOLF_WARN("Requested state with time stamp out of tolerance. Returning zero state");
+ return getProblem()->stateZero("PO"); // return zero
+ }
}
- else
- {
- _x = Vector3d(0, 0, 0);
- return false;
+
+ else { // return state at origin if possible
+ if (origin_ptr_ == nullptr || fabs(_ts - last_ptr_->getTimeStamp()) > params_->time_tolerance)
+ return VectorComposite("PO", {Vector2d(0,0), Vector1d(0)});
+ else
+ return origin_ptr_->getFrame()->getState("PO");
}
+
+
}
void ProcessorOdomIcp::setProblem(ProblemPtr _problem_ptr)
diff --git a/src/processor/processor_tracker_feature_polyline_2d.cpp b/src/processor/processor_tracker_feature_polyline_2d.cpp
index dd221805c0b1ee1c61baa59ef594903c62e1fc3c..fefda45e831f4b454a63960d7b4f8736cc5dad26 100644
--- a/src/processor/processor_tracker_feature_polyline_2d.cpp
+++ b/src/processor/processor_tracker_feature_polyline_2d.cpp
@@ -11,7 +11,7 @@ namespace wolf
{
ProcessorTrackerFeaturePolyline2d::ProcessorTrackerFeaturePolyline2d(ParamsProcessorTrackerFeaturePolyline2dPtr _params) :
- ProcessorTrackerFeature("TRACKER FEATURE POLYLINE", 3, _params),
+ ProcessorTrackerFeature("TRACKER FEATURE POLYLINE", "PO", 3, _params),
params_tracker_feature_polyline_(_params),
line_finder_(_params->line_finder_params),
extrinsics_transformation_computed_(false)
@@ -1302,11 +1302,8 @@ bool ProcessorTrackerFeaturePolyline2d::tryUpdateMatchTransformation(LandmarkMat
void ProcessorTrackerFeaturePolyline2d::computeTransformations()
{
//WOLF_DEBUG("ProcessorTrackerFeaturePolyline::computeTransformations: ");
- Eigen::Vector3d vehicle_pose_incoming = getProblem()->getState(incoming_ptr_->getTimeStamp());
- Eigen::Vector3d vehicle_pose_last = getProblem()->getState(last_ptr_->getTimeStamp());
- //WOLF_DEBUG("ProcessorTrackerFeaturePolyline::computeTransformations:",
- // "\n\tvehicle_pose_incoming: ", vehicle_pose_incoming.transpose(),
- // "\n\tvehicle_pose_last: ", vehicle_pose_last.transpose());
+ const auto& vehicle_pose_incoming = getProblem()->getState(incoming_ptr_->getTimeStamp()).vector("PO");
+ const auto& vehicle_pose_last = getProblem()->getState(last_ptr_->getTimeStamp()).vector("PO");
// world_robot
Eigen::Matrix2d R_world_robot_incoming = Eigen::Rotation2Dd(vehicle_pose_incoming(2)).matrix();
diff --git a/test/gtest_landmark_polyline.cpp b/test/gtest_landmark_polyline.cpp
index 971f48ea61355e50a5ca5e3de7bba92f0f4a61b5..2fb6c777c6b4f7f916dedaea57c10fc27fe7688b 100644
--- a/test/gtest_landmark_polyline.cpp
+++ b/test/gtest_landmark_polyline.cpp
@@ -114,7 +114,7 @@ TEST_F(LandmarkPolylineTest, Classify)
ASSERT_EQ(lmk_ptr->getClassification().D, classification_groundtruth[i].D);
// test center pose
- ASSERT_MATRIX_APPROX(lmk_ptr->getState(), lmks_groundtruth[i], 1e-9);
+ ASSERT_MATRIX_APPROX(lmk_ptr->getState().vector("PO"), lmks_groundtruth[i], 1e-9);
// test closed
ASSERT_TRUE(lmk_ptr->isClosed());
diff --git a/test/gtest_processor_odom_icp.cpp b/test/gtest_processor_odom_icp.cpp
index 81d367d49bac2caa6f4a17752fa050cf280024f2..e836f357608c4f01b3dc5e894714d743f76ed314 100644
--- a/test/gtest_processor_odom_icp.cpp
+++ b/test/gtest_processor_odom_icp.cpp
@@ -44,20 +44,16 @@ class ProcessorOdomIcp_Test : public testing::Test
std::vector<float> ranges;
TimeStamp t0;
- Vector3d x0;
- Matrix3d P0;
+ VectorComposite x0; // prior state
+ VectorComposite s0; // prior sigmas
FrameBasePtr F0, F; // keyframes
virtual void SetUp()
{
problem = Problem::create("PO", 2);
solver = std::make_shared<CeresManager>(problem);
-//<<<<<<< HEAD
auto sen = problem->installSensor("SensorLaser2d", "lidar", Eigen::Vector3d(0,0,0), laser_root_dir + "/test/yaml/sensor_laser_2d.yaml");
-//=======
-//
-// auto sen = problem->installSensor("LASER 2d", "lidar", Eigen::Vector3d(0,0,0), laser_root_dir + "/test/yaml/sensor_laser_2d.yaml");
-//>>>>>>> 8-replace-scalar-to-double
+
lidar = std::static_pointer_cast<SensorLaser2d>(sen);
auto prc = problem->installProcessor("ProcessorOdomIcp", "prc icp", "lidar", laser_root_dir + "/test/yaml/processor_odom_icp.yaml");
@@ -66,9 +62,9 @@ class ProcessorOdomIcp_Test : public testing::Test
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});
t0 = 0.0;
- x0 = Vector3d(0,0,0);
- P0 . setIdentity();
- F0 = problem->setPriorFactor(x0, P0, t0, 0.1);
+ x0 = VectorComposite("PO", {Vector2d(0,0), Vector1d(0)});
+ s0 = VectorComposite("PO", {Vector2d(1,1), Vector1d(1)});
+ F0 = problem->setPriorFactor(x0, s0, t0, 0.1);
}
virtual void TearDown(){}
};
@@ -131,21 +127,21 @@ TEST_F(ProcessorOdomIcp_Test, full)
F = problem->getLastFrame();
- ASSERT_MATRIX_APPROX(F->getState(), x0, 1e-6);
+ ASSERT_MATRIX_APPROX(F->getState().vector("PO"), x0.vector("PO"), 1e-6);
- WOLF_TRACE("F", F->id() , " ", F->getState().transpose());
+ WOLF_TRACE("F", F->id() , " ", F->getState().vector("PO").transpose());
if (i >= 2 && i <= 4)
{
// perturb F1
- F->setState(0.1 * Vector3d::Random());
- WOLF_TRACE("F", F->id() , " ", F->getState().transpose(), " perturbed!");
+ F->perturb(0.1);
+ WOLF_TRACE("F", F->id() , " ", F->getState().vector("PO").transpose(), " perturbed!");
}
t += 1.0;
}
- ASSERT_MATRIX_APPROX(F->getState(), x0, 1e-6);
+ ASSERT_MATRIX_APPROX(F->getState().vector("PO"), x0.vector("PO"), 1e-6);
}
TEST_F(ProcessorOdomIcp_Test, solve)
@@ -165,7 +161,8 @@ TEST_F(ProcessorOdomIcp_Test, solve)
for (auto F : problem->getTrajectory()->getFrameList())
{
if (F->isKey())
- F->setState(0.5 * Vector3d::Random());
+// F->setState(0.5 * Vector3d::Random());
+ F->perturb(0.5);
}
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
@@ -175,7 +172,7 @@ TEST_F(ProcessorOdomIcp_Test, solve)
{
if (F->isKey())
{
- ASSERT_MATRIX_APPROX(F->getState() , x0 , 1e-6);
+ ASSERT_MATRIX_APPROX(F->getState().vector("PO") , x0.vector("PO") , 1e-6);
}
}
}