Resolve "Processor motion model"

Closes #407 (closed)

test/gtest_processor_fix_wing_model.cpp

@@ -40,7 +40,7 @@ class ProcessorFixWingModelTest : public testing::Test
             ParamsProcessorFixWingModelPtr params = std::make_shared<ParamsProcessorFixWingModel>();
             params->velocity_local = (Vector3d() << 1, 0, 0).finished();
             params->angle_stdev = 0.1;
-            params->min_vel_norm = 1;
+            params->min_vel_norm = 0;
             processor = ProcessorBase::emplace<ProcessorFixWingModel>(sensor, params);
         }
 
@@ -73,12 +73,8 @@ TEST_F(ProcessorFixWingModelTest, keyFrameCallback)
     ASSERT_EQ(cap->getFeatureList().size(), 1);
 
     // check one factor
-    //auto fac = frm1->getFactorOf(processor, "FactorVelocityLocalDirection3d");
-    auto fac = frm1->getFactorOf(processor);
-    WOLF_INFO("processor ", processor);
-    WOLF_INFO("factor processor ", cap->getFeatureList().front()->getFactorList().front()->getProcessor());
+    auto fac = frm1->getFactorOf(processor, "FactorVelocityLocalDirection3d");
     ASSERT_TRUE(fac != nullptr);
-    WOLF_INFO("fac type", fac->getType());
     ASSERT_TRUE(fac->getFeature() != nullptr);
     ASSERT_TRUE(fac->getCapture() == cap);
 }
@@ -106,113 +102,33 @@ TEST_F(ProcessorFixWingModelTest, keyFrameCallbackRepeated)
     auto fac = frm1->getFactorOf(processor, "FactorVelocityLocalDirection3d");
     ASSERT_TRUE(fac != nullptr);
     ASSERT_TRUE(fac->getFeature() != nullptr);
-    ASSERT_TRUE(fac->getCapture() == frm1->getCaptureOf(sensor));
+    ASSERT_TRUE(fac->getCapture() == cap);
 }
 
-/*TEST_F(ProcessorFixWingModelTest, captureCallbackMatch)
+TEST_F(ProcessorFixWingModelTest, solve_origin)
 {
     // new frame
-    auto frm1 = emplaceFrame(1, Vector3d::Zero());
-    auto frm2 = emplaceFrame(2, Vector3d::Zero());
-    auto frm3 = emplaceFrame(3, Vector3d::Zero());
-    auto frm4 = emplaceFrame(4, Vector3d::Zero());
-    auto frm5 = emplaceFrame(5, Vector3d::Zero());
-    // new captures
-    auto cap4 = createCapture(4);
+    auto frm1 = emplaceFrame(1, (Vector10d() << 0,0,0,0,0,0,1,1,0,0).finished());
 
     // keyframecallback
     problem->keyFrameCallback(frm1, nullptr, 0.5);
-    problem->keyFrameCallback(frm2, nullptr, 0.5);
-    problem->keyFrameCallback(frm3, nullptr, 0.5);
-    problem->keyFrameCallback(frm4, nullptr, 0.5);
-    problem->keyFrameCallback(frm5, nullptr, 0.5);
 
-    // captureCallback
-    processor->captureCallback(cap4);
+    // perturb
+    frm1->getP()->fix();
+    frm1->getO()->fix();
+    frm1->getV()->unfix();
+    frm1->getV()->setState(Vector3d::Random());
 
-    EXPECT_EQ(frm1->getCaptureList().size(), 0);
-    EXPECT_EQ(frm2->getCaptureList().size(), 0);
-    EXPECT_EQ(frm3->getCaptureList().size(), 0);
-    EXPECT_EQ(frm4->getCaptureList().size(), 1);
-    EXPECT_EQ(frm5->getCaptureList().size(), 0);
+    // solve
+    std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
 
-    EXPECT_TRUE(cap4->getFrame() == frm4);
-    EXPECT_EQ(cap4->getFeatureList().size(), 1);
-
-    EXPECT_EQ(processor->getNStoredFrames(), 1); // all oldest frames are removed from buffer
-    EXPECT_EQ(processor->getNStoredCaptures(), 0);
-}
+    WOLF_INFO(report);
 
-TEST_F(ProcessorFixWingModelTest, keyFrameCallbackMatch)
-{
-    // new frame
-    auto frm2 = emplaceFrame(2, Vector3d::Zero());
-    // new captures
-    auto cap1 = createCapture(1);
-    auto cap2 = createCapture(2);
-    auto cap3 = createCapture(3);
-    auto cap4 = createCapture(4);
-    auto cap5 = createCapture(5);
-
-    // captureCallback
-    processor->captureCallback(cap1);
-    processor->captureCallback(cap2);
-    processor->captureCallback(cap3);
-    processor->captureCallback(cap4);
-    processor->captureCallback(cap5);
-
-    // keyframecallback
-    problem->keyFrameCallback(frm2, nullptr, 0.5);
-
-    EXPECT_TRUE(cap1->getFrame() == nullptr);
-    EXPECT_TRUE(cap2->getFrame() == frm2);
-    EXPECT_TRUE(cap3->getFrame() == nullptr);
-    EXPECT_TRUE(cap4->getFrame() == nullptr);
-    EXPECT_TRUE(cap5->getFrame() == nullptr);
-
-    EXPECT_EQ(cap1->getFeatureList().size(), 0);
-    EXPECT_EQ(cap2->getFeatureList().size(), 1);
-    EXPECT_EQ(cap3->getFeatureList().size(), 0);
-    EXPECT_EQ(cap4->getFeatureList().size(), 0);
-    EXPECT_EQ(cap5->getFeatureList().size(), 0);
-
-    EXPECT_EQ(processor->getNStoredFrames(), 0);
-    EXPECT_EQ(processor->getNStoredCaptures(), 4);
+    ASSERT_GT(frm1->getV()->getState()(0), 0);
+    ASSERT_NEAR(frm1->getV()->getState()(1), 0, Constants::EPS);
+    ASSERT_NEAR(frm1->getV()->getState()(2), 0, Constants::EPS);
 }
 
-TEST_F(ProcessorFixWingModelTest, emplaceFactors)
-{
-    // emplace frame and capture
-    auto cap1 = emplaceCapture(emplaceFrame(1, Vector3d::Zero()));
-    processor->captureCallback(cap1);
-
-    auto cap2 = emplaceCapture(emplaceFrame(2, Vector3d::Ones()));
-    processor->captureCallback(cap2);
-
-    auto cap3 = emplaceCapture(emplaceFrame(3, 2*Vector3d::Ones()));
-    processor->captureCallback(cap3);
-
-    auto cap4 = emplaceCapture(emplaceFrame(4, Vector3d::Zero()));
-    processor->captureCallback(cap4);
-
-    EXPECT_EQ(cap1->getFrame()->getConstrainedByList().size(), 1);
-    EXPECT_EQ(cap2->getFrame()->getConstrainedByList().size(), 0);
-    EXPECT_EQ(cap3->getFrame()->getConstrainedByList().size(), 0);
-    EXPECT_EQ(cap4->getFrame()->getConstrainedByList().size(), 0);
-
-    EXPECT_EQ(cap1->getFeatureList().size(), 1);
-    EXPECT_EQ(cap2->getFeatureList().size(), 1);
-    EXPECT_EQ(cap3->getFeatureList().size(), 1);
-    EXPECT_EQ(cap4->getFeatureList().size(), 1);
-
-    EXPECT_EQ(cap1->getFeatureList().front()->getFactorList().size(), 0);
-    EXPECT_EQ(cap2->getFeatureList().front()->getFactorList().size(), 0);
-    EXPECT_EQ(cap3->getFeatureList().front()->getFactorList().size(), 0);
-    EXPECT_EQ(cap4->getFeatureList().front()->getFactorList().size(), 1);
-
-    EXPECT_EQ(cap1->getFrame()->getConstrainedByList().front(), cap4->getFeatureList().front()->getFactorList().front());
-}*/
-
 int main(int argc, char **argv)
 {
   testing::InitGoogleTest(&argc, argv);