working on gtest and changed names

CMakeLists.txt

@@ -229,7 +229,7 @@ SET(HDRS_FACTOR
   include/core/factor/factor_relative_pose_2d_with_extrinsics.h
   include/core/factor/factor_relative_pose_3d.h
   include/core/factor/factor_pose_3d_with_extrinsics.h
-  include/core/factor/factor_velocity_direction_3d.h
+  include/core/factor/factor_velocity_local_direction_3d.h
   )
 SET(HDRS_FEATURE
   include/core/feature/feature_base.h
@@ -246,7 +246,6 @@ SET(HDRS_PROCESSOR
   include/core/processor/is_motion.h
   include/core/processor/motion_buffer.h
   include/core/processor/processor_base.h
-  include/core/processor/processor_constant_velocity.h
   include/core/processor/processor_diff_drive.h
   include/core/processor/processor_fix_wing_model.h
   include/core/processor/factory_processor.h
@@ -347,7 +346,6 @@ SET(SRCS_PROCESSOR
   src/processor/is_motion.cpp
   src/processor/motion_buffer.cpp
   src/processor/processor_base.cpp
-  src/processor/processor_constant_velocity.cpp
   src/processor/processor_diff_drive.cpp
   src/processor/processor_fix_wing_model.cpp
   src/processor/processor_loop_closure.cpp

test/CMakeLists.txt

@@ -217,6 +217,10 @@ target_link_libraries(gtest_factor_relative_pose_2d_with_extrinsics ${PLUGIN_NAM
 wolf_add_gtest(gtest_factor_relative_pose_3d gtest_factor_relative_pose_3d.cpp)
 target_link_libraries(gtest_factor_relative_pose_3d ${PLUGIN_NAME})
 
+# FactorVelocityLocalDirection3d class test
+wolf_add_gtest(gtest_factor_velocity_local_direction_3d gtest_factor_velocity_local_direction_3d.cpp)
+target_link_libraries(gtest_factor_velocity_local_direction_3d ${PLUGIN_NAME})
+
 # MakePosDef function test
 wolf_add_gtest(gtest_make_posdef gtest_make_posdef.cpp)
 target_link_libraries(gtest_make_posdef ${PLUGIN_NAME})

test/gtest_factor_velocity_local_direction_3d.cpp

@@ -9,121 +9,192 @@
 using namespace Eigen;
 using namespace wolf;
 
+class FactorVelocityLocalDirection3dTest : public testing::Test
+{
+    protected:
+        ProblemPtr problem;
+        SolverManagerPtr solver;
+        FrameBasePtr frm;
+        StateBlockPtr sb_p;
+        StateBlockPtr sb_o;
+        StateBlockPtr sb_v;
+        CaptureBasePtr cap;
+
+        virtual void SetUp()
+        {
+            // create problem
+            problem = Problem::create("POV", 3);
+
+            // create solver
+            solver = std::make_shared<SolverCeres>(problem);
+
+            // Frame
+            frm = problem->emplaceFrame(0.0, (Vector10d() << 0,0,0,0,0,0,1,1,0,0).finished());
+            sb_p = frm->getP();
+            sb_o = frm->getO();
+            sb_v = frm->getV();
+
+            // Capture
+            cap = CaptureBase::emplace<CaptureBase>(frm, "CaptureBase",
+                                                    frm->getTimeStamp(), nullptr);
+        }
+
+        FactorBasePtr emplaceFeatureAndFactor(const Vector3d& v_local, const double& angle_stdev )
+        {
+            // emplace feature
+            FeatureBasePtr fea = FeatureBase::emplace<FeatureBase>(cap, "FeatureBase",
+                                                                   v_local,
+                                                                   Matrix1d(angle_stdev * angle_stdev));
+
+            // emplace factor
+            return FactorBase::emplace<FactorVelocityLocalDirection3d>(fea,
+                                                                       fea,
+                                                                       0,
+                                                                       nullptr,
+                                                                       false);
+        }
+};
+
 // Input odometry data and covariance
-Matrix3d data_cov = 0.1*Matrix3d::Identity();
 Vector3d v_local(1.0, 0.0, 0.0);
 double angle_stdev = 0.1;
 
-// Problem and solver
-ProblemPtr problem_ptr = Problem::create("POV", 3);
-SolverCeres solver(problem_ptr);
+TEST_F(FactorVelocityLocalDirection3dTest, check_tree)
+{
+    ASSERT_TRUE(problem->check(0));
 
-// Frame
-FrameBasePtr frm = problem_ptr->emplaceFrame(0.0, (Vector10d() << 0,0,0,0,0,0,1,1,0,0).finished());
+    emplaceFeatureAndFactor(Vector3d(1.0, 0.0, 0.0), 0.1);
 
-// Capture
-CaptureBase cap = CaptureBase::emplace<CaptureBase>(frm, "CaptureBase",
-                                                    frm->getTimeStamp(), nullptr);
+    ASSERT_TRUE(problem->check(0));
+    ASSERT_DEATH({emplaceFeatureAndFactor(Vector3d(10.0, 0.0, 0.0), 0.1);},""); // Not normalized
+}
 
-// emplace feature
-FeatureBase fea = FeatureBase::emplace<FeatureBase>(cap, "FeatureBase",
-                                                    v_local,
-                                                    Eigen::Matrix1d(angle_stdev * angle_stdev));
+TEST_F(FactorVelocityLocalDirection3dTest, origin_x_fix_PO_solve)
+{
+    // set state
+    sb_p->setState(Vector3d::Zero());
+    sb_o->setState(Quaterniond::Identity().coeffs());
+    sb_v->setState(Vector3d::Random());
 
-// emplace factor
-FactorBase fac = FactorBase::emplace<FactorVelocityLocalDirection3d>(fea,
-                                                                     fea,
-                                                                     0,
-                                                                     nullptr,
-                                                                     false);
+    // fix
+    sb_p->fix();
+    sb_o->fix();
+    sb_v->unfix();
 
-TEST(FactorVelocityLocalDirection3dTest, check_tree)
-{
-    ASSERT_TRUE(problem_ptr->check(0));
-}
+    // emplace feature & factor
+    Vector3d v_local(1.0, 0.0, 0.0);
+    emplaceFeatureAndFactor(v_local, 0.1);
 
-TEST(FactorVelocityLocalDirection3dTest, fix_PO_solve)
-{
-    for (int i = 0; i < 1e3; i++)
-    {
-        // Random delta
-        Vector3d delta = Vector3d::Random() * 10;
-        pi2pi(delta(2));
-
-        // Random initial pose
-        Vector3d x0 = Vector3d::Random() * 10;
-        pi2pi(x0(2));
-
-        // x1 groundtruth
-        Vector3d x1;
-        x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * delta.head<2>();
-        x1(2) = pi2pi(x0(2) + delta(2));
-
-        // Set states and measurement
-        frm0->setState(x0);
-        frm1->setState(x1);
-        cap1->setData(delta);
-
-        // feature & factor with delta measurement
-        auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
-        FactorBase::emplace<FactorVelocityLocalDirection3d>(fea1, fea1, frm0, nullptr, false, TOP_MOTION);
-
-        // Fix frm0, perturb frm1
-        frm0->fix();
-        frm1->unfix();
-        frm1->perturb(5);
-
-        // solve for frm1
-        std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-
-        ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
-
-        // remove feature (and factor) for the next loop
-        fea1->remove();
-    }
+    std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+
+    ASSERT_LT(abs(sb_v->getState()(1)), 1e-9);
+    ASSERT_LT(abs(sb_v->getState()(2)), 1e-9);
 }
 
-TEST(FactorVelocityLocalDirection3dTest, fix_1_solve)
+TEST_F(FactorVelocityLocalDirection3dTest, origin_y_fix_PO_solve)
 {
-    for (int i = 0; i < 1e3; i++)
-    {
-        // Random delta
-        Vector3d delta = Vector3d::Random() * 10;
-        pi2pi(delta(2));
-
-        // Random initial pose
-        Vector3d x0 = Vector3d::Random() * 10;
-        pi2pi(x0(2));
-
-        // x1 groundtruth
-        Vector3d x1;
-        x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * delta.head<2>();
-        x1(2) = pi2pi(x0(2) + delta(2));
-
-        // Set states and measurement
-        frm0->setState(x0);
-        frm1->setState(x1);
-        cap1->setData(delta);
-
-        // feature & factor with delta measurement
-        auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
-        FactorBase::emplace<FactorVelocityLocalDirection3d>(fea1, fea1, frm0, nullptr, false, TOP_MOTION);
-
-        // Fix frm1, perturb frm0
-        frm1->fix();
-        frm0->unfix();
-        frm0->perturb(5);
-
-        // solve for frm0
-        std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-
-        ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
-
-        // remove feature (and factor) for the next loop
-        fea1->remove();
-    }
+    // set state
+    sb_p->setState(Vector3d::Zero());
+    sb_o->setState(Quaterniond::Identity().coeffs());
+    sb_v->setState(Vector3d::Random());
+
+    // fix
+    sb_p->fix();
+    sb_o->fix();
+    sb_v->unfix();
+
+    // emplace feature & factor
+    Vector3d v_local(0.0, 1.0, 0.0);
+    emplaceFeatureAndFactor(v_local, 0.1);
+
+    std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+
+    ASSERT_LT(abs(sb_v->getState()(0)), 1e-9);
+    ASSERT_LT(abs(sb_v->getState()(2)), 1e-9);
 }
 
+//TEST_F(FactorVelocityLocalDirection3dTest, fix_PO_solve)
+//{
+//    for (int i = 0; i < 1e3; i++)
+//    {
+//        // Random delta
+//        Vector3d delta = Vector3d::Random() * 10;
+//        pi2pi(delta(2));
+//
+//        // Random initial pose
+//        Vector3d x0 = Vector3d::Random() * 10;
+//        pi2pi(x0(2));
+//
+//        // x1 groundtruth
+//        Vector3d x1;
+//        x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * delta.head<2>();
+//        x1(2) = pi2pi(x0(2) + delta(2));
+//
+//        // Set states and measurement
+//        frm0->setState(x0);
+//        frm1->setState(x1);
+//        cap1->setData(delta);
+//
+//        // feature & factor with delta measurement
+//        auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
+//        FactorBase::emplace<FactorVelocityLocalDirection3d>(fea1, fea1, frm0, nullptr, false, TOP_MOTION);
+//
+//        // Fix frm0, perturb frm1
+//        frm0->fix();
+//        frm1->unfix();
+//        frm1->perturb(5);
+//
+//        // solve for frm1
+//        std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+//
+//        ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
+//
+//        // remove feature (and factor) for the next loop
+//        fea1->remove();
+//    }
+//}
+//
+//TEST_F(FactorVelocityLocalDirection3dTest, fix_1_solve)
+//{
+//    for (int i = 0; i < 1e3; i++)
+//    {
+//        // Random delta
+//        Vector3d delta = Vector3d::Random() * 10;
+//        pi2pi(delta(2));
+//
+//        // Random initial pose
+//        Vector3d x0 = Vector3d::Random() * 10;
+//        pi2pi(x0(2));
+//
+//        // x1 groundtruth
+//        Vector3d x1;
+//        x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * delta.head<2>();
+//        x1(2) = pi2pi(x0(2) + delta(2));
+//
+//        // Set states and measurement
+//        frm0->setState(x0);
+//        frm1->setState(x1);
+//        cap1->setData(delta);
+//
+//        // feature & factor with delta measurement
+//        auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
+//        FactorBase::emplace<FactorVelocityLocalDirection3d>(fea1, fea1, frm0, nullptr, false, TOP_MOTION);
+//
+//        // Fix frm1, perturb frm0
+//        frm1->fix();
+//        frm0->unfix();
+//        frm0->perturb(5);
+//
+//        // solve for frm0
+//        std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+//
+//        ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
+//
+//        // remove feature (and factor) for the next loop
+//        fea1->remove();
+//    }
+//}
+
 int main(int argc, char **argv)
 {
   testing::InitGoogleTest(&argc, argv);