Started ROS Imu unit-test

* Gazebo simulation
* RosInputProviderSuite.testImuToPoseSensor

oomact_ros/test/RosInputProviderTest.cpp

@@ -8,7 +8,9 @@
 #include <aslam/calibration/calibrator/CalibratorI.hpp>
 #include <aslam/calibration/model/FrameGraphModel.h>
 #include <aslam/calibration/model/PoseTrajectory.h>
+#include <aslam/calibration/model/sensors/Imu.h>
 #include <aslam/calibration/model/sensors/PoseSensor.hpp>
+#include <aslam/calibration/test/TestData.h>
 #include <aslam/calibration/tools/SmartPointerTools.h>
 
 #include "bag_tools.h"
@@ -18,7 +20,7 @@ using namespace aslam::calibration;
 using namespace aslam::calibration::ros;
 
 
-TEST(RosInputProviderSuite, testEasy) {
+TEST(RosInputProviderSuite, testPoseSensors) {
   // Create configuration // TODO D support compiler validation for configuration a bit
   auto vs = ValueStoreRef::fromString(
       "model{"
@@ -58,3 +60,31 @@ TEST(RosInputProviderSuite, testEasy) {
 
   EXPECT_NEAR(0.0, psA.getTranslationToParent()[1], 0.0001); // it should be zero now because it is receiving the same data as b, which has zero transformation and doesn't get calibrated.
 }
+
+TEST(RosInputProviderSuite, testImuToPoseSensor) {
+  const std::string testBagPath = "oomact_ros/imu-pose.bag";
+  OOMACT_SKIP_IF_TESTDATA_UNAVAILABLE(testBagPath);
+
+  auto vs = ValueStoreRef::fromFile("imu-pose.info");
+
+  // Construct objects
+  FrameGraphModel m(vs.getChild("model"));
+  PoseSensor psA(m, "pose");
+  Imu imu(m, "imu");
+  PoseTrajectory traj(m, "traj");
+
+  // init model with all sensors :
+  m.addModulesAndInit(psA, imu, traj);
+
+  // Create local pseudo shared pointer to model and create a batchCalibrator for it
+  auto spModel = to_local_shared_ptr(m);
+  auto c = createBatchCalibrator(vs.getChild("calibrator"), spModel);
+
+  RosInputProvider(spModel).feedBag(test::getTestDataPath(testBagPath), *c); // Feed the test.bag
+
+  EXPECT_NEAR(0.0, imu.getTranslationToParent()[1], 0.0001); // check the y position of the sensor a BEFORE calibration
+
+  c->calibrate();
+
+//  EXPECT_NEAR(0.0, psA.getTranslationToParent()[1], 0.0001); // it should be zero now because it is receiving the same data as b, which has zero transformation and doesn't get calibrated.
+}

oomact_ros/test/imu-pose.info

@@ -0,0 +1,113 @@
+model {
+	Gravity {
+		used true
+		magnitude 9.81
+	}
+	frames "body,world"
+
+	pose {
+		frame body
+		targetFrame world
+		invertInput false
+
+		topic "/firefly/vi_sensor/ground_truth/transform"
+//		topic "/firefly/odometry_sensor1/transform"
+
+		covPosition { sigma 0.001 }
+		covOrientation { sigma 0.001 }
+
+		absoluteMeasurements true
+
+		rotation { used false }
+		translation { used false }
+		delay { used false }
+
+		mestimator
+		{
+			name "Cauchy"
+			cauchySigma2 "1"
+		}
+	}
+	imu {
+		used true
+
+		frame body
+		inertiaFrame world
+		topic "/firefly/vi_sensor/imu"
+//		topic "/firefly/imu"
+
+		acc {
+			used true
+			hasBias true
+			biasVector {
+				x 0
+				y 0
+				z 0
+			}
+			noise {
+				cov { sigma 0.01 }
+				biasRandomWalk 4e-3
+			}
+			enforceCovariance true
+		}
+		gyro {
+			used true
+			hasBias true
+			biasVector {
+				x 0
+				y 0
+				z 0
+			}
+			noise {
+				cov { sigma 0.001 }
+				biasRandomWalk 4e-3
+			}
+
+			enforceCovariance true
+		}
+
+		rotation {
+			yaw 0
+			pitch 0
+			roll 0
+		}
+		translation {
+			x 0
+			y 0
+			z 0
+		}
+		delay 0.005
+		{
+			used true
+			relativeBounds true
+			upperBound 0.1
+			lowerBound -0.1
+		}
+	}
+
+	traj {
+		frame body
+		referenceFrame world
+		initWithPoseMeasurements true
+		McSensor pose
+		splines {
+			knotsPerSecond 100
+			rotSplineOrder 4
+			rotFittingLambda 0.0000001
+			transSplineOrder 4
+			transFittingLambda 0.000000001
+		}
+	}
+}
+
+calibrator {
+	verbose true
+	timeBaseSensor pose
+	estimator {
+		optimizer {
+			maxIterations 150
+			convergenceDeltaError 1e-6
+			convergenceDeltaX 1e-6
+		}
+	}
+}