Draft: Resolve "Processor motion intrinsics update behaviour gtest"

Closes #377

test/gtest_processor_motion_intrinsics_update.cpp

+#include "core/utils/utils_gtest.h"
+
+#include "core/common/wolf.h"
+#include "core/problem/problem.h"
+#include <core/ceres_wrapper/solver_ceres.h>
+#include "core/capture/capture_void.h"
+#include "core/feature/feature_base.h"
+#include "core/factor/factor_block_absolute.h"
+#include "core/processor/processor_diff_drive.h"
+#include "core/sensor/sensor_diff_drive.h"
+#include "core/capture/capture_diff_drive.h"
+
+using namespace wolf;
+using namespace Eigen;
+
+
+
+/**
+Simplified test rolling along x axis only:
+Delta_x = 2*pi*R*(ticks/ticks_per_wheel_revolution)
+
+If we take ticks_per_wheel_revolution = 2*pi, even simpler formulat:
+Delta_x = R*ticks
+
+**/
+
+class ProcessorDiffDriveTest : public testing::Test
+{
+    public:
+        ParamsSensorDiffDrivePtr      intr_;
+        SensorDiffDrivePtr          sensor_;
+        ParamsProcessorDiffDrivePtr params_;
+        ProcessorDiffDrivePtr       processor_;
+        ProblemPtr                  problem_;
+        SolverCeresPtr solver_;
+        FrameBasePtr KF0_;
+        double radius;
+
+        void SetUp() override
+        {
+            problem_ = Problem::create("PO", 2);
+            solver_ = std::make_shared<SolverCeres>(problem_);
+
+            VectorComposite x_origin("PO", {Vector2d::Zero(), Vector1d::Zero()});
+            VectorComposite std_origin("PO", {0.001*Vector2d::Ones(), 0.001*Vector1d::Ones()});
+
+            // ground truth of wheels radius
+            radius = 1;
+            // make a sensor first
+            intr_ = std::make_shared<ParamsSensorDiffDrive>();
+            intr_->radius_left                   = 1.042;
+            intr_->radius_right                  = 1.042;
+            intr_->wheel_separation              = 1.0;
+            intr_->ticks_per_wheel_revolution    = 2*M_PI;
+            Vector3d extr(0,0,0);
+            sensor_ = std::static_pointer_cast<SensorDiffDrive>(problem_->installSensor("SensorDiffDrive", "sensor diff drive", extr, intr_));
+
+            // params and processor
+            params_ = std::make_shared<ParamsProcessorDiffDrive>();
+            params_->voting_active   = true;
+            params_->angle_turned    = 1000000;
+            params_->dist_traveled   = 200000000;
+            params_->max_buff_length = 1000000000;   
+            params_->max_time_span   = 2.999; // controls KF creation
+            params_->cov_det         = 10000000;
+            
+            params_->unmeasured_perturbation_std = 1e-4;
+            processor_ = std::static_pointer_cast<ProcessorDiffDrive>(problem_->installProcessor("ProcessorDiffDrive", "processor diff drive", sensor_, params_));
+
+            KF0_ = problem_->setPriorFactor(x_origin, std_origin, 0, 0.01);
+
+            processor_->setOrigin(KF0_);
+        }
+
+        void TearDown() override{}
+
+};
+
+TEST_F(ProcessorDiffDriveTest, test1)
+{
+    Matrix2d data_cov; data_cov . setIdentity();
+    // Advances at constant speed on both wheels
+    double ticks = 1;
+
+    Vector2d data; data << ticks, ticks;
+    auto C1 = std::make_shared<CaptureDiffDrive>(1, sensor_, data, data_cov, KF0_->getCaptureList().front()); C1->process();
+    auto C2 = std::make_shared<CaptureDiffDrive>(2, sensor_, data, data_cov, KF0_->getCaptureList().front()); C2->process();
+    auto C3 = std::make_shared<CaptureDiffDrive>(3, sensor_, data, data_cov, KF0_->getCaptureList().front()); C3->process();
+    FrameBasePtr KF1 = problem_->getTrajectory()->getLastFrame();
+    Vector2d state_pred1; state_pred1 << 3*ticks, 0; 
+    auto CA1 = CaptureBase::emplace<CaptureVoid>(KF1, KF1->getTimeStamp(), nullptr);
+    auto FA1 = FeatureBase::emplace<FeatureBase>(CA1, "POSITION", state_pred1, pow(0.001, 2)*Matrix2d::Identity());
+    FactorBase::emplace<FactorBlockAbsolute>(FA1, FA1, FA1->getFrame()->getP(), nullptr, false);
+
+    problem_->print(4,true,true,true);
+    solver_->solve();
+    problem_->print(4,true,true,true);
+
+    // now, intrinsics are updated in KF0 and KF1 captures
+    auto C4 = std::make_shared<CaptureDiffDrive>(4, sensor_, data, data_cov, C3); C4->process();
+    auto C5 = std::make_shared<CaptureDiffDrive>(5, sensor_, data, data_cov, C3); C5->process();
+    auto C6 = std::make_shared<CaptureDiffDrive>(6, sensor_, data, data_cov, C3); C6->process();
+
+    FrameBasePtr KF2 = problem_->getTrajectory()->getLastFrame();
+    Vector2d state_pred2; state_pred2 << 6*ticks, 0; 
+    auto CA2 = CaptureBase::emplace<CaptureVoid>(KF2, KF2->getTimeStamp(), nullptr);
+    auto FA2 = FeatureBase::emplace<FeatureBase>(CA2, "POSITION", state_pred2, pow(0.001, 2)*Matrix2d::Identity());
+    FactorBase::emplace<FactorBlockAbsolute>(FA2, FA2, FA2->getFrame()->getP(), nullptr, false);
+
+    problem_->print(4,true,true,true);
+    solver_->solve();
+    problem_->print(4,true,true,true);
+
+
+    // now, intrinsics are updated in KF0 and KF1 captures
+    auto C7 = std::make_shared<CaptureDiffDrive>(7, sensor_, data, data_cov, C6); C7->process();
+    auto C8 = std::make_shared<CaptureDiffDrive>(8, sensor_, data, data_cov, C6); C8->process();
+    auto C9 = std::make_shared<CaptureDiffDrive>(9, sensor_, data, data_cov, C6); C9->process();
+
+    FrameBasePtr KF3 = problem_->getTrajectory()->getLastFrame();
+    Vector2d state_pred3; state_pred3 << 9*ticks, 0; 
+    auto CA3 = CaptureBase::emplace<CaptureVoid>(KF3, KF3->getTimeStamp(), nullptr);
+    auto FA3 = FeatureBase::emplace<FeatureBase>(CA3, "POSITION", state_pred3, pow(0.001, 2)*Matrix2d::Identity());
+    FactorBase::emplace<FactorBlockAbsolute>(FA3, FA3, FA3->getFrame()->getP(), nullptr, false);
+
+    problem_->print(4,true,true,true);
+    solver_->solve();
+    problem_->print(4,true,true,true);
+
+
+
+    // problem_->getTrajectory()->getLastFrame()
+    // problem_->print(4,true,true,true);
+    // problem_->perturb();
+    // problem_->print(4,true,true,true);
+
+
+
+}
+
+
+
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}