diff --git a/test/gtest_odom_2D.cpp b/test/gtest_odom_2D.cpp
index 5d296342191c30f1632daa923677057e5b26daa3..b4647a0a83947463c8c5f8e9a975823c2f4f9fdb 100644
--- a/test/gtest_odom_2D.cpp
+++ b/test/gtest_odom_2D.cpp
@@ -8,14 +8,16 @@
 #include "core/utils/utils_gtest.h"
 
 // Classes under test
+#include "core/sensor/sensor_odom_2D.h"
 #include "core/processor/processor_odom_2D.h"
 #include "core/factor/factor_odom_2D.h"
+#include "core/capture/capture_odom_2D.h"
 
 // Wolf includes
-#include "core/sensor/sensor_odom_2D.h"
 #include "core/state_block/state_block.h"
 #include "core/common/wolf.h"
 #include "core/ceres_wrapper/ceres_manager.h"
+#include "core/capture/capture_pose.h"
 
 // STL includes
 #include <map>
@@ -26,7 +28,6 @@
 // General includes
 #include <iostream>
 #include <iomanip>      // std::setprecision
-#include "core/capture/capture_pose.h"
 
 using namespace wolf;
 using namespace Eigen;
@@ -234,6 +235,9 @@ TEST(Odom2D, VoteForKfAndSolve)
     std::vector<Eigen::VectorXs> integrated_pose_vector;
     std::vector<Eigen::MatrixXs> integrated_cov_vector;
 
+    integrated_pose_vector.push_back(integrated_pose);
+    integrated_cov_vector.push_back(integrated_cov);
+
 //    std::cout << "\nIntegrating data..." << std::endl;
 
     t += dt;
@@ -249,10 +253,7 @@ TEST(Odom2D, VoteForKfAndSolve)
         // Processor
         capture->process();
         ASSERT_TRUE(problem->check(0));
-//        Matrix3s odom2d_delta_cov = processor_odom2d->integrateBufferCovariance(processor_odom2d->getBuffer());
         Matrix3s odom2d_delta_cov = processor_odom2d->getMotion().delta_integr_cov_;
-        //        std::cout << "State(" << (t - t0) << ") : " << processor_odom2d->getCurrentState().transpose() << std::endl;
-        //        std::cout << "PRC  cov: \n" << odom2d_delta_cov << std::endl;
 
         // Integrate Delta
         if (n==3 || n==6) // keyframes
@@ -291,12 +292,21 @@ TEST(Odom2D, VoteForKfAndSolve)
 
     // Solve
     std::string report = ceres_manager.solve(SolverManager::ReportVerbosity::BRIEF);
-//    std::cout << report << std::endl;
     ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
 
+
+    // Check the 3 KFs' states and covariances
     MatrixXs computed_cov;
-    ASSERT_TRUE(problem->getLastKeyFrameCovariance(computed_cov));
-    ASSERT_MATRIX_APPROX(computed_cov , integrated_cov_vector[5], 1e-6);
+    int n = 0;
+    for (auto F : problem->getTrajectory()->getFrameList())
+    {
+        if (!F->isKey()) break;
+
+        ASSERT_POSE2D_APPROX(F->getState(), integrated_pose_vector[n]   , 1e-6);
+        ASSERT_TRUE         (F->getCovariance(computed_cov));
+        ASSERT_MATRIX_APPROX(computed_cov , integrated_cov_vector[n]    , 1e-6);
+        n += 3;
+    }
 }
 
 TEST(Odom2D, KF_callback)
@@ -314,7 +324,7 @@ TEST(Odom2D, KF_callback)
     Eigen::Matrix3s x0_cov = Eigen::Matrix3s::Identity() * 0.1;
     VectorXs data(Vector2s(1, M_PI/4) ); // advance 1m
     Eigen::MatrixXs data_cov = Eigen::MatrixXs::Identity(2, 2) * 0.01;
-    int N = 16; // number of process() steps
+    int N = 8; // number of process() steps
 
     // NOTE: We integrate and create KFs as follows:
     //  n =   0    1    2    3    4    5    6    7    8
@@ -361,7 +371,7 @@ TEST(Odom2D, KF_callback)
 //    std::cout << "\nIntegrating data..." << std::endl;
 
     // Capture to use as container for all incoming data
-    CaptureMotionPtr capture = std::make_shared<CaptureMotion>("ODOM 2D", t, sensor_odom2d, data, data_cov, 3, 3, nullptr);
+    auto capture = std::make_shared<CaptureOdom2D>(t, sensor_odom2d, data, data_cov, nullptr);
 
     std::cout << "t: " << t << std::endl;
     for (int n=1; n<=N; n++)
@@ -405,11 +415,16 @@ TEST(Odom2D, KF_callback)
     Vector3s x_split = processor_odom2d->getState(t_split);
     FrameBasePtr keyframe_2 = problem->emplaceFrame(KEY, x_split, t_split);
 
+    // there must be 2KF and one F
+    ASSERT_EQ(problem->getTrajectory()->getFrameList().size(), 3);
+    // The last KF must have TS = 0.08
+    ASSERT_EQ(problem->getLastKeyFrame()->getTimeStamp().getNanoSeconds(), 80000000);
+
     ASSERT_TRUE(problem->check(0));
     processor_odom2d->keyFrameCallback(keyframe_2, dt/2);
     ASSERT_TRUE(problem->check(0));
     t += dt;
-    capture = std::make_shared<CaptureMotion>("ODOM 2D", t, sensor_odom2d, data, data_cov, 3, 3, nullptr);
+    capture = std::make_shared<CaptureOdom2D>(t, sensor_odom2d, data, data_cov, nullptr);
     capture->process();
     ASSERT_TRUE(problem->check(0));
 
@@ -441,7 +456,7 @@ TEST(Odom2D, KF_callback)
     processor_odom2d->keyFrameCallback(keyframe_1, dt/2);
     ASSERT_TRUE(problem->check(0));
     t += dt;
-    capture = std::make_shared<CaptureMotion>("ODOM 2D", t, sensor_odom2d, data, data_cov, 3, 3, nullptr);
+    capture = std::make_shared<CaptureOdom2D>(t, sensor_odom2d, data, data_cov, nullptr);
     capture->process();
     ASSERT_TRUE(problem->check(0));
 
@@ -485,7 +500,7 @@ TEST(Odom2D, KF_callback)
 int main(int argc, char **argv)
 {
   testing::InitGoogleTest(&argc, argv);
-//  testing::GTEST_FLAG(filter) = "Odom2D.KF_callback";
+//  testing::GTEST_FLAG(filter) = "Odom2D.VoteForKfAndSolve";
   return RUN_ALL_TESTS();
 }