removed constant velocity and working on gtests

include/core/factor/factor_velocity_direction_3d.h → include/core/factor/factor_velocity_local_direction_3d.h

 
-#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);

include/core/processor/processor_constant_velocity.h

-/*
- * 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_ */

src/processor/processor_constant_velocity.cpp

-/*
- * 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
-

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 */

test/gtest_factor_velocity_local_direction_3d.cpp

+#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();
+}

test/gtest_processor_fix_wing_model.cpp

+
+#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();
+}