diff --git a/include/core/factor/factor_velocity_direction_3d.h b/include/core/factor/factor_velocity_local_direction_3d.h
similarity index 84%
rename from include/core/factor/factor_velocity_direction_3d.h
rename to include/core/factor/factor_velocity_local_direction_3d.h
index b903113f2e8790808bea4b04749f5ca74ca6c687..b66f57af4f18e43bf820af7b2894d98a743246a8 100644
--- a/include/core/factor/factor_velocity_direction_3d.h
+++ b/include/core/factor/factor_velocity_local_direction_3d.h
@@ -1,6 +1,6 @@
-#ifndef FACTOR_VELOCITY_DIRECTION_3D_H_
-#define FACTOR_VELOCITY_DIRECTION_3D_H_
+#ifndef FACTOR_VELOCITY_LOCAL_DIRECTION_3D_H_
+#define FACTOR_VELOCITY_LOCAL_DIRECTION_3D_H_
//Wolf includes
#include "core/factor/factor_autodiff.h"
@@ -11,21 +11,21 @@
namespace wolf {
-WOLF_PTR_TYPEDEFS(FactorVelocityDirection3d);
+WOLF_PTR_TYPEDEFS(FactorVelocityLocalDirection3d);
//class
-class FactorVelocityDirection3d: public FactorAutodiff<FactorVelocityDirection3d,1,3,4>
+class FactorVelocityLocalDirection3d: public FactorAutodiff<FactorVelocityLocalDirection3d,1,3,4>
{
protected:
double min_vel_sq_norm_; // stored in squared norm for efficiency
public:
- FactorVelocityDirection3d(FeatureBasePtr _ftr_ptr,
+ FactorVelocityLocalDirection3d(FeatureBasePtr _ftr_ptr,
const double& _min_vel_norm,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) :
- FactorAutodiff<FactorVelocityDirection3d,1,3,4>("FactorVelocityDirection3d",
+ FactorAutodiff<FactorVelocityLocalDirection3d,1,3,4>("FactorVelocityDirection3d",
TOP_ABS,
_ftr_ptr,
nullptr, nullptr, nullptr, nullptr,
@@ -41,14 +41,14 @@ class FactorVelocityDirection3d: public FactorAutodiff<FactorVelocityDirection3d
assert(abs(_ftr_ptr->getMeasurement().norm() - 1.0) < wolf::Constants::EPS && "velocity direction measurement must be normalized");
}
- ~FactorVelocityDirection3d() override = default;
+ ~FactorVelocityLocalDirection3d() override = default;
template<typename T>
bool operator ()(const T* const _v, const T* const _o, T* _residuals) const;
};
template<typename T>
-inline bool FactorVelocityDirection3d::operator ()(const T* const _v, const T* const _q, T* _residuals) const
+inline bool FactorVelocityLocalDirection3d::operator ()(const T* const _v, const T* const _q, T* _residuals) const
{
Eigen::Map<const Eigen::Matrix<T,3,1> > v(_v);
Eigen::Map<const Eigen::Quaternion<T> > q(_q);
diff --git a/include/core/processor/processor_constant_velocity.h b/include/core/processor/processor_constant_velocity.h
deleted file mode 100644
index 156e9be878df5386f493e8699d12f1ea2efe1a07..0000000000000000000000000000000000000000
--- a/include/core/processor/processor_constant_velocity.h
+++ /dev/null
@@ -1,85 +0,0 @@
-/*
- * processor_constant_velocity.h
- *
- * Created on: Sep 6, 2021
- * Author: joanvallve
- */
-
-#ifndef INCLUDE_CORE_PROCESSOR_PROCESSOR_CONSTANT_VELOCITY_H_
-#define INCLUDE_CORE_PROCESSOR_PROCESSOR_CONSTANT_VELOCITY_H_
-
-#include "core/processor/processor_base.h"
-
-namespace wolf
-{
-
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorConstantVelocity);
-
-struct ParamsProcessorConstantVelocity : public ParamsProcessorBase
-{
- Eigen::MatrixXd cov_v_rate;
-
- ParamsProcessorConstantVelocity() = default;
- ParamsProcessorConstantVelocity(std::string _unique_name, const wolf::ParamsServer & _server) :
- ParamsProcessorBase(_unique_name, _server)
- {
- cov_v_rate = _server.getParam<Eigen::VectorXd> (prefix + _unique_name + "/cov_rate_diagonal").asDiagonal();
- }
- std::string print() const override
- {
- return ParamsProcessorBase::print() + "\n"
- + "cov_v_rate: print not implemented\n";
- }
-};
-
-WOLF_PTR_TYPEDEFS(ProcessorConstantVelocity);
-
-class ProcessorConstantVelocity : public ProcessorBase
-{
- protected:
- ParamsProcessorConstantVelocityPtr params_processor_;
- FrameBasePtr last_keyframe_;
-
- public:
- ProcessorConstantVelocity(ParamsProcessorConstantVelocityPtr _params);
-
- // Factory method for high level API
- WOLF_PROCESSOR_CREATE(ProcessorConstantVelocity, ParamsProcessorConstantVelocity);
-
- virtual ~ProcessorConstantVelocity() override;
- void configure(SensorBasePtr _sensor) override;
-
- protected:
-
- /** \brief process an incoming capture NEVER CALLED
- */
- virtual void processCapture(CaptureBasePtr) override {};
-
- /** \brief process an incoming key-frame: applies the motion model between consecutive keyframes
- */
- virtual void processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) override;
-
- /** \brief trigger in capture
- */
- virtual bool triggerInCapture(CaptureBasePtr) const override {return false;};
-
- /** \brief trigger in key-frame
- */
- virtual bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) const override {return true;};
-
- /** \brief store key frame
- */
- virtual bool storeKeyFrame(FrameBasePtr) override {return false;};
-
- /** \brief store capture
- */
- virtual bool storeCapture(CaptureBasePtr) override {return false;};
-
- /** \brief Vote for KeyFrame generation
- */
- virtual bool voteForKeyFrame() const override {return false;};
-};
-
-} /* namespace wolf */
-
-#endif /* INCLUDE_CORE_PROCESSOR_PROCESSOR_CONSTANT_VELOCITY_H_ */
diff --git a/src/processor/processor_constant_velocity.cpp b/src/processor/processor_constant_velocity.cpp
deleted file mode 100644
index dd98e0a93894b70a5966ded73a7fe5ddebb0fb47..0000000000000000000000000000000000000000
--- a/src/processor/processor_constant_velocity.cpp
+++ /dev/null
@@ -1,87 +0,0 @@
-/*
- * processor_constant_velocity.cpp
- *
- * Created on: Sep 6, 2021
- * Author: joanvallve
- */
-
-#include "core/processor/processor_constant_velocity.h"
-
-#include "core/capture/capture_base.h"
-#include "core/feature/feature_base.h"
-#include "core/factor/factor_block_difference.h"
-
-namespace wolf
-{
-
-ProcessorConstantVelocity::ProcessorConstantVelocity(ParamsProcessorConstantVelocityPtr _params) :
- ProcessorBase("ProcessorConstantVelocity", 0, _params),
- params_processor_(_params),
- last_keyframe_(nullptr)
-{
-}
-
-ProcessorConstantVelocity::~ProcessorConstantVelocity()
-{
-}
-
-void ProcessorConstantVelocity::configure(SensorBasePtr _sensor)
-{
- assert(params_processor_->cov_v_rate.rows() == _sensor->getProblem()->getDim() && "cov_v_rate size is wrong");
- assert(_sensor->getProblem()->getFrameStructure().find('V') != std::string::npos && "Processor only works with problems with V");
-}
-
-void ProcessorConstantVelocity::processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance)
-{
- // TODO: handle if _keyframe_ptr is not the newest one
-
- // only if not null previous keyframe
- if (last_keyframe_ and
- _keyframe_ptr->getTimeStamp() > last_keyframe_->getTimeStamp())
- {
- // emplace capture
- auto cap = CaptureBase::emplace<CaptureBase>(_keyframe_ptr, "CaptureBase",
- _keyframe_ptr->getTimeStamp(), getSensor());
-
- double dt = _keyframe_ptr->getTimeStamp() - last_keyframe_->getTimeStamp();
-
- // STATE BLOCK V
- // emplace feature
- auto fea_v = FeatureBase::emplace<FeatureBase>(cap,
- "FeatureBase",
- Eigen::VectorXd::Zero(getProblem()->getDim()),
- params_processor_->cov_v_rate * dt);
-
- // emplace factor
- auto fac_v = FactorBase::emplace<FactorBlockDifference>(fea_v,
- fea_v,
- last_keyframe_->getV(),
- _keyframe_ptr->getV(),
- last_keyframe_, nullptr, nullptr, nullptr,
- 0, -1, 0, -1,
- shared_from_this());
-
- /*// STATE BLOCK P
- // emplace feature
- auto fea_p = FeatureBase::emplace<FeatureBase>(cap,
- "FeatureBase",
- Eigen::VectorXd::Zero(getProblem()->getDim()),
- params_processor_->cov_p_rate * dt);
-
- // emplace factor ct vel TODO*/
- }
-
- // store keyframes
- last_keyframe_ = _keyframe_ptr;
-}
-
-} /* namespace wolf */
-
-
-// Register in the FactoryProcessor
-#include "core/processor/factory_processor.h"
-namespace wolf {
-WOLF_REGISTER_PROCESSOR(ProcessorConstantVelocity);
-WOLF_REGISTER_PROCESSOR_AUTO(ProcessorConstantVelocity);
-} // namespace wolf
-
diff --git a/src/processor/processor_fix_wing_model.cpp b/src/processor/processor_fix_wing_model.cpp
index 2fab50466750b0b6944bffc34aaca6b5eb6647e5..869bbc087dce968c50c9e49b58b8297bf6f35b4a 100644
--- a/src/processor/processor_fix_wing_model.cpp
+++ b/src/processor/processor_fix_wing_model.cpp
@@ -9,7 +9,7 @@
#include "core/capture/capture_base.h"
#include "core/feature/feature_base.h"
-#include "core/factor/factor_velocity_direction_3d.h"
+#include "core/factor/factor_velocity_local_direction_3d.h"
namespace wolf
{
@@ -44,11 +44,11 @@ void ProcessorFixWingModel::processKeyFrame(FrameBasePtr _keyframe_ptr, const do
Eigen::Matrix1d(params_processor_->angle_stdev * params_processor_->angle_stdev));
// emplace factor
- auto fac = FactorBase::emplace<FactorVelocityDirection3d>(fea,
- fea,
- params_processor_->min_vel_norm,
- shared_from_this(),
- false);
+ auto fac = FactorBase::emplace<FactorVelocityLocalDirection3d>(fea,
+ fea,
+ params_processor_->min_vel_norm,
+ shared_from_this(),
+ false);
}
} /* namespace wolf */
diff --git a/test/gtest_factor_velocity_local_direction_3d.cpp b/test/gtest_factor_velocity_local_direction_3d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6e4ba1bbd6035da368af457dfefd80a522875491
--- /dev/null
+++ b/test/gtest_factor_velocity_local_direction_3d.cpp
@@ -0,0 +1,131 @@
+#include "core/utils/utils_gtest.h"
+
+#include "core/ceres_wrapper/solver_ceres.h"
+#include "core/factor/factor_velocity_local_direction_3d.h"
+#include "core/capture/capture_odom_2d.h"
+#include "core/math/rotations.h"
+
+
+using namespace Eigen;
+using namespace wolf;
+
+// 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);
+
+// Frame
+FrameBasePtr frm = problem_ptr->emplaceFrame(0.0, (Vector10d() << 0,0,0,0,0,0,1,1,0,0).finished());
+
+// Capture
+CaptureBase cap = CaptureBase::emplace<CaptureBase>(frm, "CaptureBase",
+ frm->getTimeStamp(), nullptr);
+
+// emplace feature
+FeatureBase fea = FeatureBase::emplace<FeatureBase>(cap, "FeatureBase",
+ v_local,
+ Eigen::Matrix1d(angle_stdev * angle_stdev));
+
+// emplace factor
+FactorBase fac = FactorBase::emplace<FactorVelocityLocalDirection3d>(fea,
+ fea,
+ 0,
+ nullptr,
+ false);
+
+TEST(FactorVelocityLocalDirection3dTest, check_tree)
+{
+ ASSERT_TRUE(problem_ptr->check(0));
+}
+
+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();
+ }
+}
+
+TEST(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);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_processor_fix_wing_model.cpp b/test/gtest_processor_fix_wing_model.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1dd0fba63f5ade3e1d5f2ad5c46d081d2f2c66b0
--- /dev/null
+++ b/test/gtest_processor_fix_wing_model.cpp
@@ -0,0 +1,314 @@
+
+#include "core/utils/utils_gtest.h"
+#include "core/problem/problem.h"
+#include "core/ceres_wrapper/solver_ceres.h"
+#include "core/processor/processor_fix_wing_model.h"
+
+// STL
+#include <iterator>
+#include <iostream>
+
+using namespace wolf;
+using namespace Eigen;
+
+class ProcessorFixWingModelTest : public testing::Test
+{
+ protected:
+ ProblemPtr problem;
+ SolverManagerPtr solver;
+ SensorBasePtr sensor;
+ ProcessorBasePtr processor;
+
+ virtual void SetUp()
+ {
+ // create problem
+ problem = Problem::create("POV", 3);
+
+ // create solver
+ solver = std::make_shared<SolverCeres>(problem);
+
+ // Emplace sensor
+ sensor = SensorBase::emplace<SensorBase>(problem->getHardware(),
+ "SensorBase",
+ std::make_shared<StateBlock>(Vector3d::Zero()),
+ std::make_shared<StateQuaternion>((Vector4d() << 0,0,0,1).finished()),
+ nullptr,
+ 2);
+
+ // Emplace processor
+ ParamsProcessorFixWingModelPtr params = std::make_shared<ParamsProcessorFixWingModel>();
+ params->velocity_local = (Vector3d() << 1, 0, 0).finished();
+ params->angle_stdev = 0.1;
+ params->min_vel_norm = 1;
+ processor = ProcessorBase::emplace<ProcessorFixWingModel>(sensor, params);
+ }
+
+ FrameBasePtr emplaceFrame(TimeStamp ts, const Vector10d& x)
+ {
+ // new frame
+ return problem->emplaceFrame(ts, x);
+ }
+};
+
+TEST_F(ProcessorFixWingModelTest, setup)
+{
+ EXPECT_TRUE(problem->check());
+}
+
+TEST_F(ProcessorFixWingModelTest, frame_stored)
+{
+ // new frame
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr, 0.5);
+
+ EXPECT_EQ(processor->getNStoredFrames(), 1);
+ EXPECT_EQ(processor->getNStoredCaptures(), 0);
+}
+
+TEST_F(ProcessorFixWingModelTest, capture_stored)
+{
+ // new capture
+ auto cap1 = createCapture(1);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+
+ EXPECT_EQ(processor->getNStoredFrames(), 0);
+ EXPECT_EQ(processor->getNStoredCaptures(), 1);
+}
+
+TEST_F(ProcessorFixWingModelTest, captureCallbackCase1)
+{
+ // emplace frame and capture
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+ auto cap1 = emplaceCapture(frm1);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+
+ EXPECT_EQ(cap1->getFeatureList().size(), 1); // capture processed by the processor
+ EXPECT_EQ(processor->getNStoredFrames(), 0);
+ EXPECT_EQ(processor->getNStoredCaptures(), 0);
+}
+
+TEST_F(ProcessorFixWingModelTest, captureCallbackCase2)
+{
+ // new frame
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+
+ // new capture
+ auto cap1 = createCapture(1);
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr, 0.5);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+
+ EXPECT_EQ(cap1->getFrame(), frm1); // capture processed by the processor
+ EXPECT_EQ(cap1->getFeatureList().size(), 1); // capture processed by the processor
+ EXPECT_EQ(processor->getNStoredFrames(), 0);
+ EXPECT_EQ(processor->getNStoredCaptures(), 0);
+}
+
+TEST_F(ProcessorFixWingModelTest, captureCallbackCase3)
+{
+ // new frame
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+ // new capture
+ auto cap1 = createCapture(2);
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr, 0.5);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+
+ EXPECT_TRUE(cap1->getFrame() == nullptr);
+ EXPECT_EQ(cap1->getFeatureList().size(), 0);
+ EXPECT_EQ(processor->getNStoredFrames(), 1);
+ EXPECT_EQ(processor->getNStoredCaptures(), 1);
+}
+
+TEST_F(ProcessorFixWingModelTest, keyFrameCallbackCase1)
+{
+ // emplace frame and capture
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+ auto cap1 = emplaceCapture(frm1);
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr, 0.5);
+
+ EXPECT_EQ(cap1->getFeatureList().size(), 1); // capture processed by the processor
+ EXPECT_EQ(processor->getNStoredFrames(), 0);
+ EXPECT_EQ(processor->getNStoredCaptures(), 0);
+}
+
+TEST_F(ProcessorFixWingModelTest, keyFrameCallbackCase2)
+{
+ // new frame
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+ // new capture
+ auto cap1 = createCapture(1);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr, 0.5);
+
+ EXPECT_EQ(cap1->getFrame(), frm1); // capture processed by the processor
+ EXPECT_EQ(cap1->getFeatureList().size(), 1); // capture processed by the processor
+ EXPECT_EQ(processor->getNStoredFrames(), 0);
+ EXPECT_EQ(processor->getNStoredCaptures(), 0);
+}
+
+TEST_F(ProcessorFixWingModelTest, keyFrameCallbackCase3)
+{
+ // new frame
+ auto frm1 = emplaceFrame(2, Vector3d::Zero());
+ // new capture
+ auto cap1 = createCapture(1);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr, 0.5);
+
+ EXPECT_TRUE(cap1->getFrame() == nullptr);
+ EXPECT_EQ(cap1->getFeatureList().size(), 0);
+ EXPECT_EQ(processor->getNStoredFrames(), 1);
+ EXPECT_EQ(processor->getNStoredCaptures(), 1);
+}
+
+TEST_F(ProcessorFixWingModelTest, keyFrameCallbackCase4)
+{
+ // new frame
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+ // new capture
+ auto cap1 = createCapture(2);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr, 0.5);
+
+ EXPECT_TRUE(cap1->getFrame() == nullptr);
+ EXPECT_EQ(cap1->getFeatureList().size(), 0);
+ EXPECT_EQ(processor->getNStoredFrames(), 0);
+ EXPECT_EQ(processor->getNStoredCaptures(), 1);
+}
+
+TEST_F(ProcessorFixWingModelTest, captureCallbackMatch)
+{
+ // 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);
+
+ // 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);
+
+ 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);
+
+ 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);
+}
+
+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);
+}
+
+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);
+ return RUN_ALL_TESTS();
+}