diff --git a/test/gtest_processor_tracker_feature_landmark_external.cpp b/test/gtest_processor_tracker_feature_landmark_external.cpp
index a21ebf37efaf63c2edf4ac707db58fa6c2879738..a82e385f35739d3065749cdfc67b43b07c71589d 100644
--- a/test/gtest_processor_tracker_feature_landmark_external.cpp
+++ b/test/gtest_processor_tracker_feature_landmark_external.cpp
@@ -57,8 +57,9 @@ class ProcessorTrackerFeatureLandmarkExternalTest : public testing::Test
ProcessorMotionPtr processor_motion;
std::vector<LandmarkBasePtr> landmarks;
- // Groundtruth poses
- VectorXd p_robot, o_robot, p_sensor, o_sensor;
+ // Groundtruth states
+ VectorComposite state_robot, state_sensor;
+ std::vector<VectorComposite> state_landmarks;
virtual void SetUp() {};
void initProblem(int _dim,
@@ -77,6 +78,7 @@ class ProcessorTrackerFeatureLandmarkExternalTest : public testing::Test
int track_length,
double time_span,
bool add_landmarks);
+ void assertVectorComposite(const VectorComposite& v1, const VectorComposite& v2) const;
};
void ProcessorTrackerFeatureLandmarkExternalTest::initProblem(int _dim,
@@ -179,13 +181,12 @@ void ProcessorTrackerFeatureLandmarkExternalTest::initProblem(int _dim,
nullptr));
lmk->setId(i);
landmarks.push_back(lmk);
+ state_landmarks.push_back(lmk->getState());
}
// Store groundtruth poses
- p_robot = F0->getP()->getState();
- o_robot = F0->getO()->getState();
- p_sensor = sensor->getP()->getState();
- o_sensor = sensor->getO()->getState();
+ state_robot = F0->getState("PO");
+ state_sensor = sensor->getState("PO");
}
void ProcessorTrackerFeatureLandmarkExternalTest::randomStep()
@@ -210,38 +211,31 @@ void ProcessorTrackerFeatureLandmarkExternalTest::randomStep()
cap_odom->process();
// update groundtruth pose with random movement
- auto state = processor_motion->getState("PO");
- p_robot = state.vector("P");
- o_robot = state.vector("O");
+ state_robot = processor_motion->getState("PO");
}
CaptureLandmarksExternalPtr ProcessorTrackerFeatureLandmarkExternalTest::computeCaptureLandmarks() const
{
// Detections
auto cap = std::make_shared<CaptureLandmarksExternal>(t, sensor);
- VectorXd p_lmk, o_lmk;
for (auto i = 0; i < landmarks.size(); i++)
{
- p_lmk = landmarks.at(i)->getP()->getState();
- if (orientation)
- o_lmk = landmarks.at(i)->getO()->getState();
-
// compute detection
VectorXd meas(orientation?(dim==2?3:7):(dim==2?2:3));
if (dim == 2)
{
- meas.head(2) = Rotation2Dd(-o_sensor(0))*(Rotation2Dd(-o_robot(0))*(p_lmk - p_robot) - p_sensor);
+ meas.head(2) = Rotation2Dd(-state_sensor.at('O')(0))*(Rotation2Dd(-state_robot.at('O')(0))*(state_landmarks.at(i).at('P') - state_robot.at('P')) - state_sensor.at('P'));
if (orientation)
- meas(2) = o_lmk(0) - o_robot(0) - o_sensor(0);
+ meas(2) = state_landmarks.at(i).at('O')(0) - state_robot.at('O')(0) - state_sensor.at('O')(0);
}
else
{
- auto q_sensor = Quaterniond(Vector4d(o_sensor));
- auto q_robot = Quaterniond(Vector4d(o_robot));
+ auto q_sensor = Quaterniond(Vector4d(state_sensor.at('O')));
+ auto q_robot = Quaterniond(Vector4d(state_robot.at('O')));
- meas.head(3) = q_sensor.conjugate() * (q_robot.conjugate() * (p_lmk - p_robot) - p_sensor);
+ meas.head(3) = q_sensor.conjugate() * (q_robot.conjugate() * (state_landmarks.at(i).at('P') - state_robot.at('P')) - state_sensor.at('P'));
if (orientation)
- meas.tail(4) = (q_sensor.conjugate() * q_robot.conjugate() * Quaterniond(Vector4d(o_lmk))).coeffs();
+ meas.tail(4) = (q_sensor.conjugate() * q_robot.conjugate() * Quaterniond(Vector4d(state_landmarks.at(i).at('O')))).coeffs();
}
// cov
MatrixXd cov = MatrixXd::Identity(meas.size(), meas.size());
@@ -323,20 +317,8 @@ void ProcessorTrackerFeatureLandmarkExternalTest::testConfiguration(int dim,
for (auto lmk_map : landmarks_map)
{
ASSERT_TRUE(lmk_map->id() < landmarks.size());
- auto lmk_gt = landmarks.at(lmk_map->id());
- ASSERT_EQ(lmk_map->id(), lmk_gt->id());
- if (not orientation)
- {
- ASSERT_MATRIX_APPROX(lmk_map->getState().vector("P"), lmk_gt->getState().vector("P"), Constants::EPS);
- }
- else if (dim == 2)
- {
- ASSERT_POSE2d_APPROX(lmk_map->getState().vector("PO"), lmk_gt->getState().vector("PO"), Constants::EPS);
- }
- else if (dim == 3)
- {
- ASSERT_POSE3d_APPROX(lmk_map->getState().vector("PO"), lmk_gt->getState().vector("PO"), Constants::EPS);
- }
+ ASSERT_EQ(lmk_map->id(), landmarks.at(lmk_map->id())->id());
+ assertVectorComposite(lmk_map->getState(), state_landmarks.at(lmk_map->id()));
}
}
}
@@ -350,24 +332,55 @@ void ProcessorTrackerFeatureLandmarkExternalTest::testConfiguration(int dim,
ASSERT_EQ(problem->getMap()->getLandmarkList().empty(), not add_landmarks);
}
+ // step with random movement
+ t+=dt;
+ randomStep();
+
// solve
- if (not problem->getMap()->getLandmarkList().empty())
+ if (should_emplace_KF)
{
- // TODO store gt values
+ // perturb landmarks
+ for (auto lmk : problem->getMap()->getLandmarkList())
+ lmk->perturb();
- // TODO perturb
+ // fix frame and extrinsics
+ sensor->fix();
+ problem->getLastFrame()->fix();
// solve
- // auto report = solver->solve(SolverManager::ReportVerbosity::FULL);
- // WOLF_INFO(report);
+ auto report = solver->solve(SolverManager::ReportVerbosity::FULL);
+ WOLF_INFO(report);
- // TODO check values
+ // check values
+ //assertVectorComposite(sensor->getState("PO"), state_sensor);
+ //assertVectorComposite(problem->getState("PO"), state_robot);
+ for (auto lmk_map : problem->getMap()->getLandmarkList())
+ {
+ assertVectorComposite(lmk_map->getState(), state_landmarks.at(lmk_map->id()));
+ }
}
+ }
+}
- // step with random movement
- t+=dt;
- randomStep();
+void ProcessorTrackerFeatureLandmarkExternalTest::assertVectorComposite(const VectorComposite& v1, const VectorComposite& v2) const
+{
+ if (v1.includesStructure("PO") and v2.includesStructure("PO"))
+ {
+ if (dim == 2)
+ {
+ ASSERT_POSE2d_APPROX(v1.vector("PO"), v2.vector("PO"), Constants::EPS);
+ }
+ else
+ {
+ ASSERT_POSE3d_APPROX(v1.vector("PO"), v2.vector("PO"), Constants::EPS);
+ }
}
+ else if (v1.includesStructure("P") and v2.includesStructure("P"))
+ {
+ ASSERT_MATRIX_APPROX(v1.vector("P"), v2.vector("P"), Constants::EPS);
+ }
+ else
+ throw std::runtime_error("wrong vector composite");
}
// / TESTS //////////////////////////////////////////
@@ -397,10 +410,10 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, P_2d_no_lmks_quality)
{
testConfiguration(2, //int dim
false, //bool orientation
- 0.3, //double quality_th
+ 0.3, //double quality_th
1e6, //double dist_th
6, //int track_length
- 4.5*dt, //double time_span
+ 4.5*dt, //double time_span
false); //bool add_landmarks
}
@@ -411,7 +424,7 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, PO_2d_existing_lmks)
0, //double quality_th
1e6, //double dist_th
3, //int track_length
- 4.5*dt, //double time_span
+ 4.5*dt, //double time_span
true); //bool add_landmarks
}
@@ -422,7 +435,7 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, PO_2d_no_lmks)
0, //double quality_th
1e6, //double dist_th
1, //int track_length
- 4.5*dt, //double time_span
+ 4.5*dt, //double time_span
false); //bool add_landmarks
}
@@ -430,10 +443,10 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, PO_2d_no_lmks_quality)
{
testConfiguration(2, //int dim
true, //bool orientation
- 0.3, //double quality_th
+ 0.3, //double quality_th
1e6, //double dist_th
0, //int track_length
- 4.5*dt, //double time_span
+ 4.5*dt, //double time_span
false); //bool add_landmarks
}
@@ -444,7 +457,7 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, P_3d_existing_lmks)
0, //double quality_th
1e6, //double dist_th
7, //int track_length
- 4.5*dt, //double time_span
+ 4.5*dt, //double time_span
true); //bool add_landmarks
}
@@ -454,8 +467,8 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, P_3d_no_lmks)
false, //bool orientation
0, //double quality_th
1e6, //double dist_th
- 53, //int track_length
- 4.5*dt, //double time_span
+ 53, //int track_length
+ 4.5*dt, //double time_span
false); //bool add_landmarks
}
@@ -463,10 +476,10 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, P_3d_no_lmks_quality)
{
testConfiguration(3, //int dim
false, //bool orientation
- 0.3, //double quality_th
+ 0.3, //double quality_th
1e6, //double dist_th
2, //int track_length
- 4.5*dt, //double time_span
+ 4.5*dt, //double time_span
false); //bool add_landmarks
}
@@ -477,7 +490,7 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, PO_3d_existing_lmks)
0, //double quality_th
1e6, //double dist_th
1, //int track_length
- 4.5*dt, //double time_span
+ 4.5*dt, //double time_span
true); //bool add_landmarks
}
@@ -488,7 +501,7 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, PO_3d_no_lmks)
0, //double quality_th
1e6, //double dist_th
4, //int track_length
- 4.5*dt, //double time_span
+ 4.5*dt, //double time_span
false); //bool add_landmarks
}
@@ -496,10 +509,10 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, PO_3d_no_lmks_quality)
{
testConfiguration(3, //int dim
true, //bool orientation
- 0.2, //double quality_th
+ 0.2, //double quality_th
1e6, //double dist_th
5, //int track_length
- 4.5*dt, //double time_span
+ 4.5*dt, //double time_span
false); //bool add_landmarks
}