New tests for linear advancement and pure rotation

test/gtest_processor_diff_drive.cpp

@@ -224,7 +224,6 @@ TEST_F(ProcessorDiffDriveTest, computeCurrentDelta)
 
 TEST_F(ProcessorDiffDriveTest, deltaPlusDelta)
 {
-
     Vector2s data;
     Matrix2s data_cov; data_cov . setIdentity();
     Vector3s calib(1,1,1);
@@ -265,7 +264,6 @@ TEST_F(ProcessorDiffDriveTest, deltaPlusDelta)
 
 TEST_F(ProcessorDiffDriveTest, statePlusDelta)
 {
-
     Vector2s data;
     Matrix2s data_cov; data_cov . setIdentity();
     Vector3s calib(1,1,1);
@@ -306,7 +304,6 @@ TEST_F(ProcessorDiffDriveTest, statePlusDelta)
 
 TEST_F(ProcessorDiffDriveTest, process)
 {
-
     Vector2s data;
     Matrix2s data_cov; data_cov . setIdentity();
     TimeStamp t = 0.0;
@@ -336,6 +333,61 @@ TEST_F(ProcessorDiffDriveTest, process)
     problem->print(4,1,1,1);
 }
 
+TEST_F(ProcessorDiffDriveTest, linear)
+{
+    Vector2s data;
+    Matrix2s data_cov; data_cov . setIdentity();
+    TimeStamp t = 0.0;
+    Vector3s x(0,0,0);
+    Matrix3s P; P.setIdentity();
+
+    auto F0 = problem->setPrior(x, P, t, 0.1);
+
+    // Straight one turn of the wheels, in one go
+    data(0) = 100.0 ; // one turn of the wheels
+    data(1) = 100.0 ;
+
+    auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0);
+
+    C->process();
+    WOLF_TRACE("t = ", t, "; x = ", processor->getCurrentState().transpose());
+
+    // radius is 1.0m, 100 ticks per revolution, so advanced distance is
+    Scalar distance = 2 * M_PI * 1.0;
+
+    ASSERT_MATRIX_APPROX(processor->getCurrentState(), Vector3s(distance,0,0), 1e-6)
+}
+
+TEST_F(ProcessorDiffDriveTest, angular)
+{
+    Vector2s data;
+    Matrix2s data_cov; data_cov . setIdentity();
+    TimeStamp t = 0.0;
+    Vector3s x(0,0,0);
+    Matrix3s P; P.setIdentity();
+
+    auto F0 = problem->setPrior(x, P, t, 0.1);
+
+    // Straight one turn of the wheels, in one go
+    data(0) = -20.0 ; // one fifth of a turn of the left wheel, in reverse
+    data(1) =  20.0 ; // one fifth of a turn of the right wheel, forward --> we'll turn left --> positive angle
+
+    auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0);
+
+    C->process();
+    WOLF_TRACE("t = ", t, "; x = ", processor->getCurrentState().transpose());
+
+    // this is a turn in place, so distance = 0;
+    Scalar distance = 0.0;
+
+    // radius is 1.0m, 100 ticks per revolution, and wheel separation is 1m, so turn angle is
+    Scalar angle = pi2pi(2 * M_PI * 1.0 / 0.5 / 5);
+
+    ASSERT_MATRIX_APPROX(processor->getCurrentState(), Vector3s(distance,0,angle), 1e-6)
+}
+
+
+
 int main(int argc, char **argv)
 {
     testing::InitGoogleTest(&argc, argv);