less workarounds in tests for joining prior KF

include/core/processor/processor_motion.h

@@ -204,11 +204,6 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
          */
         CaptureMotionPtr findCaptureContainingTimeStamp(const TimeStamp& _ts) const;
 
-        MotionBuffer& getBuffer();
-        const MotionBuffer& getBuffer() const;
-
-        // Helper functions:
-    protected:
         /** Set the origin of all motion for this processor
          * \param _origin_frame the keyframe to be the origin
          */
@@ -220,6 +215,12 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
          */
         FrameBasePtr setOrigin(const Eigen::VectorXd& _x_origin, const TimeStamp& _ts_origin);
 
+        MotionBuffer& getBuffer();
+        const MotionBuffer& getBuffer() const;
+
+        // Helper functions:
+    protected:
+
         /** \brief process an incoming capture
          *
          * Each derived processor should implement this function. It will be called if:

src/processor/processor_motion.cpp

@@ -102,14 +102,14 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
     {
         case FIRST_TIME_WITHOUT_KF :
         {
-            // There is no KF: create own origin (before this capture: 2 x time_tolerance before)
-            setOrigin(getProblem()->zeroState(), _incoming_ptr->getTimeStamp()-2*params_->time_tolerance);
+            // There is no KF: create own origin
+            setOrigin(getProblem()->zeroState(), _incoming_ptr->getTimeStamp());
             break;
         }
         case FIRST_TIME_WITH_KF_BEFORE_INCOMING :
         {
-            // cannot joint to the KF: create own origin (before this capture: 2 x time_tolerance before)
-            setOrigin(getProblem()->zeroState(), _incoming_ptr->getTimeStamp()-2*params_->time_tolerance);
+            // cannot joint to the KF: create own origin
+            setOrigin(getProblem()->zeroState(), _incoming_ptr->getTimeStamp());
             break;
         }
         case FIRST_TIME_WITH_KF_ON_INCOMING :
@@ -120,8 +120,8 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
         }
         case FIRST_TIME_WITH_KF_AFTER_INCOMING :
         {
-            // cannot joint to the KF: create own origin (before this capture: 2 x time_tolerance before)
-            setOrigin(getProblem()->zeroState(), _incoming_ptr->getTimeStamp()-2*params_->time_tolerance);
+            // cannot joint to the KF: create own origin
+            setOrigin(getProblem()->zeroState(), _incoming_ptr->getTimeStamp());
             break;
         }
         case RUNNING_WITHOUT_KF :
@@ -296,9 +296,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
     // NOW on with the received data
 
     // integrate data
-    WOLF_INFO("before integrating, getCurrentState = ", getCurrentState().transpose());
     integrateOneStep();
-    WOLF_INFO("after integrating, getCurrentState  = ", getCurrentState().transpose());
 
     // Update state and time stamps
     last_ptr_->setTimeStamp(getCurrentTimeStamp());

test/gtest_factor_diff_drive.cpp

@@ -449,8 +449,9 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
     Vector3d x2(3.0, -3.0, 0.0);
     Matrix3d P0; P0.setIdentity();
 
-    //auto F0 = problem->setPriorFactor(x0, P0, t+dt, 0.1);
-    problem->setPriorOptions("factor", 0.1, x0, P0);
+    auto F0 = problem->setPriorFactor(x0, P0, t, 0.1); // set prior at t=0
+    // process a dummy capture to join F0 at t=t0
+    std::make_shared<CaptureDiffDrive>(t, sensor, Vector3d::Zero(), Matrix2d::Identity(), nullptr)->process();
 
     // right turn 90 deg in N steps --> ends up in (1.5, -1.5, -pi/2)
     int N = 6;
@@ -460,8 +461,6 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
     data(0) = 0.50*intr->ticks_per_wheel_revolution / N;
     data(1) = 0.25*intr->ticks_per_wheel_revolution / N;
 
-    // initial dummy capture process for seting prior at t=0
-    std::make_shared<CaptureDiffDrive>(t, sensor, Eigen::Vector2d::Zero(), Eigen::Matrix2d::Identity()*0.01, nullptr)->process();
 
     auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, nullptr);
 
@@ -484,7 +483,6 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
     }
 
     auto F2 = problem->getLastKeyFrame();
-    auto F0 = problem->getTrajectory()->getFrameList().front();
 
     // Fix boundaries
     F0->fix();
@@ -584,7 +582,9 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
     Vector3d x2(3.0, -3.0, 0.0);
     Matrix3d P0; P0.setIdentity();
 
-    problem->setPriorOptions("factor", 0.1, x0, P0);
+    auto F0 = problem->setPriorFactor(x0, P0, t, 0.1);
+    // process a dummy capture to join F0 at t=t0
+    std::make_shared<CaptureDiffDrive>(t, sensor, Vector3d::Zero(), Matrix2d::Identity(), nullptr)->process();
 
     // right turn 90 deg in N steps --> ends up in (1.5, -1.5, -pi/2)
     int N = 6;
@@ -594,9 +594,6 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
     data(0) = 0.50*intr->ticks_per_wheel_revolution / N;
     data(1) = 0.25*intr->ticks_per_wheel_revolution / N;
 
-    // initial dummy capture process for seting prior at t=0
-    std::make_shared<CaptureDiffDrive>(t, sensor, Eigen::Vector2d::Zero(), Eigen::Matrix2d::Identity()*0.01, nullptr)->process();
-
     auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, nullptr);
 
     for (int n = 0; n < N; n++)
@@ -619,7 +616,6 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
         C->process();
     }
 
-    auto F0 = problem->getTrajectory()->getFrameList().front();
     auto F2 = problem->getLastKeyFrame();
 
     F2->setState(x2); // Impose known final state regardless of integrated value.

test/gtest_odom_2d.cpp

@@ -201,7 +201,7 @@ TEST(Odom2d, VoteForKfAndSolve)
     params->voting_active   = true;
     params->dist_traveled   = 100;
     params->angle_turned    = data(1)*2.5; // force KF at every third process()
-    params->max_time_span   = 1e4; // since prior is automatically set at first capture, first KF is not in t=0
+    params->max_time_span   = 100;
     params->cov_det         = 100;
     params->time_tolerance  = dt/2;
     params->unmeasured_perturbation_std = 0.00;
@@ -217,8 +217,8 @@ TEST(Odom2d, VoteForKfAndSolve)
     // Ceres wrapper
     CeresManager ceres_manager(problem);
 
-    // Origin Key Frame
-    problem->setPriorOptions("factor", dt/2, x0, P0);
+    // Origin Key Frame (in t1 to let processor motion to join the KF)
+    problem->setPriorFactor(x0, P0, t+dt, dt/2);
     ceres_manager.solve(SolverManager::ReportVerbosity::BRIEF);
     ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
 
@@ -340,9 +340,10 @@ TEST(Odom2d, KF_callback)
     SensorBasePtr sensor_odom2d = problem->installSensor("SensorOdom2d", "odom", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2d.yaml");
     ParamsProcessorOdom2dPtr params(std::make_shared<ParamsProcessorOdom2d>());
     params->dist_traveled   = 100;
-    params->angle_turned    = 6.28;
+    params->angle_turned    = data(1)*2.5; // force KF at every third process()
     params->max_time_span   = 100;
     params->cov_det         = 100;
+    params->time_tolerance  = dt/2;
     params->unmeasured_perturbation_std = 0.000001;
     Matrix3d unmeasured_cov = params->unmeasured_perturbation_std*params->unmeasured_perturbation_std*Matrix3d::Identity();
     ProcessorBasePtr prc_base = problem->installProcessor("ProcessorOdom2d", "odom", sensor_odom2d, params);
@@ -352,10 +353,8 @@ TEST(Odom2d, KF_callback)
     // Ceres wrapper
     CeresManager ceres_manager(problem);
 
-    // Origin Key Frame
-    //FrameBasePtr keyframe_0 = problem->setPriorFactor(x0, x0_cov, t0, dt/2);
-    problem->setPriorOptions("factor", dt/2, x0, x0_cov);
-
+    // Origin Key Frame (in t1 to let processor motion to join the KF)
+    FrameBasePtr keyframe_0 = problem->setPriorFactor(x0, x0_cov, t0+dt, dt/2);
 
     // Check covariance values
     Eigen::Vector3d integrated_pose = x0;
@@ -468,7 +467,6 @@ TEST(Odom2d, KF_callback)
 
     // solve
 //    cout << "===== full ====" << endl;
-    FrameBasePtr keyframe_0 = problem->getTrajectory()->getFrameList().front();
     keyframe_0->setState(Vector3d(1,2,3));
     keyframe_1->setState(Vector3d(2,3,1));
     keyframe_2->setState(Vector3d(3,1,2));

test/gtest_processor_diff_drive.cpp

@@ -325,7 +325,6 @@ TEST_F(ProcessorDiffDriveTest, process)
         C->setTimeStamp(t);
         C->process();
         WOLF_TRACE("t = ", t, "; x = ", processor->getCurrentState().transpose());
-
         t += dt;
     }
 

test/gtest_processor_motion.cpp

@@ -52,7 +52,7 @@ class ProcessorMotion_test : public testing::Test{
         Matrix2d                    data_cov;
 
 //        ProcessorMotion_test() :
-//            ProcessorOdom2d(std::make_shared<ParamsProcessorOdom2d>()),
+//            ProcessorOdom2d(std::make_shared<ProcessorParamsOdom2d>()),
 //            dt(0)
 //        { }