diff --git a/CMakeLists.txt b/CMakeLists.txt
index c731890330b4df5f4550864e82af4f8f20c9c827..599d7d8773e383fe7855a47e026d73743cf0e1fe 100644
--- a/CMakeLists.txt
+++ b/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
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 4e03c1a2a284f02b2fe26869ad656e5c959a450a..a6305564d0a80a4de10f4eecb74c0ad29ff6249f 100644
--- a/test/CMakeLists.txt
+++ b/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})
diff --git a/test/gtest_factor_velocity_local_direction_3d.cpp b/test/gtest_factor_velocity_local_direction_3d.cpp
index 6e4ba1bbd6035da368af457dfefd80a522875491..caf49d273660722bd9a1a5e531c958af17a3aaf9 100644
--- a/test/gtest_factor_velocity_local_direction_3d.cpp
+++ b/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);