added test for odom3D

src/test/gtest_odom_3D.cpp

@@ -14,6 +14,7 @@
 #include "logging.h"
 
 #include "processor_odom_3D.h"
+#include "ceres_wrapper/ceres_manager.h"
 
 #include <iostream>
 
@@ -60,6 +61,59 @@ class ProcessorOdom3DTest : public ProcessorOdom3D
         Scalar& dvar_min() {return min_disp_var_;}
         Scalar& rvar_min() {return min_rot_var_;}
 };
+
+class ProcessorOdomBase : public testing::Test
+{
+
+    public: //These can be accessed in fixtures
+        wolf::ProblemPtr problem;
+        wolf::SensorBasePtr sensor_ptr;
+        wolf::TimeStamp t;
+        CaptureMotionPtr cap_mot_ptr;
+        wolf::ProcessorOdom3DPtr processor_ptr_odom3D;
+        CeresManager* ceres_manager;
+        Eigen::Vector7s problem_origin;
+
+    //a new of this will be created for each new test
+    virtual void SetUp()
+    {
+        using namespace wolf;
+        using namespace Eigen;
+        using std::shared_ptr;
+        using std::make_shared;
+        using std::static_pointer_cast;
+
+        std::string wolf_root = _WOLF_ROOT_DIR;
+
+        // Wolf problem
+        problem = Problem::create("PO 3D");
+        t.set(0);
+        problem_origin << 0,0,0, 0,0,0,1;
+        TimeStamp t(0);
+        Eigen::Vector6s data((Eigen::Vector6s()<<0,0,0,0,0,0).finished());
+
+        // CERES WRAPPER
+        ceres_manager = new CeresManager(problem);
+
+        // SENSOR + PROCESSOR ODOM 3D
+        sensor_ptr = problem->installSensor("ODOM 3D", "odom", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_odom_3D.yaml");
+        ProcessorOdom3DParamsPtr prc_odom3D_params = std::make_shared<ProcessorOdom3DParams>();
+        prc_odom3D_params->max_time_span = 0.1999;
+        prc_odom3D_params->max_buff_length = 100;
+        prc_odom3D_params->dist_traveled = 100000;
+        prc_odom3D_params->angle_turned = 100000;
+
+        ProcessorBasePtr processor_ptr_odom = problem->installProcessor("ODOM 3D", "odom", sensor_ptr, prc_odom3D_params);
+        processor_ptr_odom3D = std::static_pointer_cast<ProcessorOdom3D>(processor_ptr_odom);
+
+        FrameBasePtr KF1 = std::static_pointer_cast<FrameBase>(processor_ptr_odom3D->setOrigin(problem_origin, t));
+
+        cap_mot_ptr = make_shared<CaptureMotion>(t, sensor_ptr, data, 6, 7, 6, 0);
+    }
+
+    virtual void TearDown(){}
+};
+
 ProcessorOdom3DTest::ProcessorOdom3DTest() : ProcessorOdom3D()
 {
     dvar_min() = 0.5;
@@ -504,7 +558,42 @@ TEST(ProcessorOdom3D, Interpolate2) // timestamp out of bounds test
 
 }
 
-
+TEST_F(ProcessorOdomBase, check_KF_state_pureTranslation)
+{   
+    FrameBaseList frm_list = problem->getTrajectoryPtr()->getFrameList();
+    FrameBasePtr KF1 = problem->getTrajectoryPtr()->closestKeyFrameToTimeStamp(t.get());
+    //We have only 1 KF at this time, but 2 frames
+    ASSERT_EQ(frm_list.size(),2);
+    ASSERT_MATRIX_APPROX(processor_ptr_odom3D->getCurrentState(), KF1->getState(), Constants::EPS_SMALL);
+
+    //starting at 0
+    Eigen::Vector6s data;
+    data << 0.5,2,1.2, 0,0,0;
+    t.set(1);
+    cap_mot_ptr->setData(data);
+    cap_mot_ptr->setTimeStamp(t);
+    sensor_ptr->process(cap_mot_ptr);
+
+    //Keyframe has been created
+    frm_list = problem->getTrajectoryPtr()->getFrameList();
+    ASSERT_EQ(frm_list.size(),3);
+    FrameBasePtr KF2 = problem->getTrajectoryPtr()->closestKeyFrameToTimeStamp(t.get());
+    ASSERT_MATRIX_APPROX(processor_ptr_odom3D->getCurrentState().transpose(), KF2->getState().transpose(), Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(KF2->getState().head(3), KF1->getState().head(3) + data.head(3), Constants::EPS_SMALL);
+
+    data << 1.6,0.4,0.244, 0,0,0;
+    t.set(2);
+    cap_mot_ptr->setData(data);
+    cap_mot_ptr->setTimeStamp(t);
+    sensor_ptr->process(cap_mot_ptr);
+
+    //Keyframe has been created
+    frm_list = problem->getTrajectoryPtr()->getFrameList();
+    ASSERT_EQ(frm_list.size(),4);
+    FrameBasePtr KF3 = problem->getTrajectoryPtr()->closestKeyFrameToTimeStamp(t.get());
+    ASSERT_MATRIX_APPROX(processor_ptr_odom3D->getCurrentState(), KF3->getState(), Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(KF3->getState().head(3), KF2->getState().head(3) + data.head(3), Constants::EPS_SMALL);
+}
 
 
 int main(int argc, char **argv)