diff --git a/test/gtest_processor_fix_wing_model.cpp b/test/gtest_processor_fix_wing_model.cpp index a403302f28c19c9a7f6e0e4086e84a15cc931b41..e817a9c400c53cdd569c72de90c672ac703fdaf4 100644 --- a/test/gtest_processor_fix_wing_model.cpp +++ b/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);