Added IMU tests

corelib/include/rtabmap/core/IMU.h

@@ -1,9 +1,29 @@
 /*
- * IMU.h
+Copyright (c) 2010-2018, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
- *
+All rights reserved.
- *  Created on: 2018-03-05
+
- *      Author: mathieu
+Redistribution and use in source and binary forms, with or without
- */
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
 
 #ifndef IMU_H_
 #define IMU_H_
@@ -14,12 +34,40 @@
 
 namespace rtabmap {
 
-
+/**
-// Correspondence class to sensor_msgs/IMU
+ * @class IMU
+ * @brief Inertial measurement sample (ROS @c sensor_msgs/Imu-like fields).
+ *
+ * Holds orientation (quaternion), angular velocity, linear acceleration, optional
+ * 3×3 row-major covariance matrices (double), and an optional @ref Transform
+ * expressing the IMU frame relative to the robot base.
+ *
+ * @ref empty() is true when @ref localTransform() is null (default constructor).
+ * A sample constructed with @ref Transform::getIdentity() is not empty.
+ *
+ * @ref convertToBaseFrame() rotates linear/angular velocity (and orientation when
+ * quaternion x/y/z are non-zero) into the base frame, then clears rotation in
+ * @ref localTransform() while keeping translation.
+ *
+ * @see SensorData::imu()
+ * @see IMUEvent
+ */
 class IMU
 {
 public:
+	/** @brief Default-constructs an empty sample (null @ref localTransform()). */
 	IMU() {}
+
+	/**
+	 * @brief Constructs a sample with orientation and motion data.
+	 * @param orientation Unit quaternion (qx, qy, qz, qw).
+	 * @param orientationCovariance 3×3 row-major covariance about x, y, z (empty if unused).
+	 * @param angularVelocity Rad/s about x, y, z.
+	 * @param angularVelocityCovariance 3×3 row-major covariance (empty if unused).
+	 * @param linearAcceleration m/s² about x, y, z.
+	 * @param linearAccelerationCovariance 3×3 row-major covariance (empty if unused).
+	 * @param localTransform IMU frame in base coordinates (default identity).
+	 */
 	IMU(const cv::Vec4d & orientation, // qx qy qz qw
 		const cv::Mat & orientationCovariance,
 		const cv::Vec3d & angularVelocity,
@@ -36,6 +84,15 @@ public:
 			localTransform_(localTransform)
 	{
 	}
+
+	/**
+	 * @brief Constructs a sample without orientation (e.g. no magnetometer / no attitude).
+	 * @param angularVelocity Rad/s about x, y, z.
+	 * @param angularVelocityCovariance 3×3 row-major covariance (empty if unused).
+	 * @param linearAcceleration m/s² about x, y, z.
+	 * @param linearAccelerationCovariance 3×3 row-major covariance (empty if unused).
+	 * @param localTransform IMU frame in base coordinates (default identity).
+	 */
 	IMU(const cv::Vec3d & angularVelocity,
 		const cv::Mat & angularVelocityCovariance,
 		const cv::Vec3d & linearAcceleration,
@@ -49,21 +106,37 @@ public:
 	{
 	}
 
-	// qx qy qz qw
+	/** @return Orientation quaternion (qx, qy, qz, qw). */
 	const cv::Vec4d & orientation() const {return orientation_;}
-	const cv::Mat & orientationCovariance() const {return orientationCovariance_;} // 3x3 double Row major about x, y, z axes, empty if orientation is not set
+	/** @return 3×3 orientation covariance (row-major, empty if orientation unset). */
+	const cv::Mat & orientationCovariance() const {return orientationCovariance_;}
 
+	/** @return Angular velocity (rad/s). */
 	const cv::Vec3d & angularVelocity() const {return angularVelocity_;}
-	const cv::Mat & angularVelocityCovariance() const {return angularVelocityCovariance_;} // 3x3 double Row major about x, y, z axes, empty if angularVelocity is not set
+	/** @return 3×3 angular velocity covariance (row-major, empty if unused). */
+	const cv::Mat & angularVelocityCovariance() const {return angularVelocityCovariance_;}
 
-	const cv::Vec3d linearAcceleration() const {return linearAcceleration_;}
+	/** @return Linear acceleration (m/s²). */
-	const cv::Mat & linearAccelerationCovariance() const {return linearAccelerationCovariance_;} // 3x3 double Row major x, y z, empty if linearAcceleration is not set
+	const cv::Vec3d & linearAcceleration() const {return linearAcceleration_;}
+	/** @return 3×3 linear acceleration covariance (row-major, empty if unused). */
+	const cv::Mat & linearAccelerationCovariance() const {return linearAccelerationCovariance_;}
 
+	/** @return Transform from IMU frame to base frame. */
 	const Transform & localTransform() const {return localTransform_;}
 
-	// apply local transform rotation to data, and set Identity rotation for local transform
+	/**
+	 * @brief Rotate motion (and optionally orientation) into the base frame.
+	 *
+	 * Applies @ref localTransform() rotation to vectors and covariances, then sets
+	 * rotational part of @ref localTransform() to identity (translation unchanged).
+	 * No-op if @ref localTransform() is null or rotation is identity.
+	 * Orientation is updated only when quaternion x, y, z are not all zero.
+	 */
 	void convertToBaseFrame();
 
+	/**
+	 * @brief True when @ref localTransform() is null (placeholder / unset sample).
+	 */
 	bool empty() const
 	{
 		return localTransform_.isNull();
@@ -71,30 +144,43 @@ public:
 
 private:
 	cv::Vec4d orientation_;
-	cv::Mat orientationCovariance_; // 3x3 double Row major about x, y, z axes, empty if orientation is not set
+	cv::Mat orientationCovariance_;
 
 	cv::Vec3d angularVelocity_;
-	cv::Mat angularVelocityCovariance_; // 3x3 double Row major about x, y, z axes, empty if angularVelocity is not set
+	cv::Mat angularVelocityCovariance_;
 
 	cv::Vec3d linearAcceleration_;
-	cv::Mat linearAccelerationCovariance_; // 3x3 double Row major x, y z, empty if linearAcceleration is not set
+	cv::Mat linearAccelerationCovariance_;
 
 	Transform localTransform_;
 };
 
+/**
+ * @class IMUEvent
+ * @brief @ref UEvent carrying an @ref IMU sample and timestamp.
+ */
 class IMUEvent : public UEvent
 {
 public:
+	/** @brief Default-constructs an event with zero stamp. */
 	IMUEvent() :
 		stamp_(0.0)
 	{}
+	/**
+	 * @brief Constructs an event with IMU data and stamp.
+	 * @param data IMU sample.
+	 * @param stamp Timestamp in seconds.
+	 */
 	IMUEvent(const IMU & data, double stamp) :
 		data_(data),
 		stamp_(stamp)
 	{
 	}
+	/** @return Event type name for the utilite event system. */
 	virtual std::string getClassName() const {return "IMUEvent";}
+	/** @return IMU payload. */
 	const IMU & getData() const {return data_;}
+	/** @return Timestamp in seconds. */
 	double getStamp() const {return stamp_;}
 
 private:
@@ -104,5 +190,4 @@ private:
 
 }
 
-
 #endif /* IMU_H_ */

corelib/src/IMU.cpp

@@ -38,15 +38,17 @@ void IMU::convertToBaseFrame()
 		localTransform_.rotationMatrix().convertTo(rotationMatrix, CV_64FC1);
 		cv::transpose(rotationMatrix, rotationMatrixT);
 
-		cv::Mat_<double> v = rotationMatrix * cv::Mat(linearAcceleration_);
+		cv::Mat_<double> linearIn = (cv::Mat_<double>(3,1) << linearAcceleration_[0], linearAcceleration_[1], linearAcceleration_[2]);
-		linearAcceleration_ = cv::Vec3d(v(0,0), v(0,1), v(0,2));
+		cv::Mat_<double> linearOut = rotationMatrix * linearIn;
+		linearAcceleration_ = cv::Vec3d(linearOut(0,0), linearOut(1,0), linearOut(2,0));
 		if(!linearAccelerationCovariance_.empty())
 		{
 			linearAccelerationCovariance_ = rotationMatrix * linearAccelerationCovariance_ * rotationMatrixT;
 		}
 
-		v = rotationMatrix * cv::Mat(angularVelocity_);
+		cv::Mat_<double> angularIn = (cv::Mat_<double>(3,1) << angularVelocity_[0], angularVelocity_[1], angularVelocity_[2]);
-		angularVelocity_ = cv::Vec3d(v(0,0), v(0,1), v(0,2));
+		cv::Mat_<double> angularOut = rotationMatrix * angularIn;
+		angularVelocity_ = cv::Vec3d(angularOut(0,0), angularOut(1,0), angularOut(2,0));
 		if(!angularVelocityCovariance_.empty())
 		{
 			angularVelocityCovariance_ = rotationMatrix * angularVelocityCovariance_ * rotationMatrixT;

corelib/test/CMakeLists.txt

@@ -106,6 +106,11 @@ add_executable(test_gps test_gps.cpp)
 target_link_libraries(test_gps gtest_main rtabmap_core)
 add_test(NAME test_gps COMMAND test_gps)
 
+#IMU.h
+add_executable(test_imu test_imu.cpp)
+target_link_libraries(test_imu gtest_main rtabmap_core)
+add_test(NAME test_imu COMMAND test_imu)
+
 #GeodeticCoords.h
 add_executable(test_geodeticcoords test_geodeticcoords.cpp)
 target_link_libraries(test_geodeticcoords gtest_main rtabmap_core)

corelib/test/test_imu.cpp

@@ -0,0 +1,156 @@
+#include <gtest/gtest.h>
+#include <rtabmap/core/IMU.h>
+#include <opencv2/core.hpp>
+#include <cmath>
+
+using namespace rtabmap;
+
+namespace {
+
+static cv::Mat covariance3x3Diagonal(double d0, double d1, double d2)
+{
+	cv::Mat cov = cv::Mat::zeros(3, 3, CV_64FC1);
+	cov.at<double>(0, 0) = d0;
+	cov.at<double>(1, 1) = d1;
+	cov.at<double>(2, 2) = d2;
+	return cov;
+}
+
+static void expectVec3Near(const cv::Vec3d & a, const cv::Vec3d & b, double tol = 1e-5)
+{
+	EXPECT_NEAR(a[0], b[0], tol);
+	EXPECT_NEAR(a[1], b[1], tol);
+	EXPECT_NEAR(a[2], b[2], tol);
+}
+
+} // namespace
+
+TEST(IMUTest, DefaultConstructorIsEmpty)
+{
+	const IMU imu;
+	EXPECT_TRUE(imu.empty());
+	EXPECT_TRUE(imu.localTransform().isNull());
+	EXPECT_TRUE(imu.orientationCovariance().empty());
+	EXPECT_TRUE(imu.angularVelocityCovariance().empty());
+	EXPECT_TRUE(imu.linearAccelerationCovariance().empty());
+}
+
+TEST(IMUTest, IdentityLocalTransformIsNotEmpty)
+{
+	const IMU imu(
+			cv::Vec3d(0.1, 0.2, 0.3),
+			cv::Mat(),
+			cv::Vec3d(1.0, 0.0, 0.0),
+			cv::Mat(),
+			Transform::getIdentity());
+	EXPECT_FALSE(imu.empty());
+	EXPECT_TRUE(imu.localTransform().isIdentity());
+}
+
+TEST(IMUTest, FullConstructorStoresFields)
+{
+	const cv::Vec4d orientation(0.1, 0.2, 0.3, 0.9);
+	const cv::Vec3d angularVelocity(0.01, 0.02, 0.03);
+	const cv::Vec3d linearAcceleration(9.8, 0.1, 0.2);
+	const cv::Mat orientationCov = covariance3x3Diagonal(1.0, 1.0, 1.0);
+	const cv::Mat angularCov = covariance3x3Diagonal(2.0, 2.0, 2.0);
+	const cv::Mat linearCov = covariance3x3Diagonal(3.0, 3.0, 3.0);
+	const Transform local(0.1f, 0.2f, 0.3f, 0.f, 0.f, 0.5f);
+
+	const IMU imu(orientation, orientationCov, angularVelocity, angularCov, linearAcceleration, linearCov, local);
+
+	EXPECT_FALSE(imu.empty());
+	EXPECT_EQ(imu.orientation(), orientation);
+	EXPECT_EQ(imu.angularVelocity(), angularVelocity);
+	EXPECT_EQ(imu.linearAcceleration(), linearAcceleration);
+	EXPECT_EQ(cv::norm(imu.orientationCovariance(), orientationCov, cv::NORM_INF), 0);
+	EXPECT_EQ(cv::norm(imu.angularVelocityCovariance(), angularCov, cv::NORM_INF), 0);
+	EXPECT_EQ(cv::norm(imu.linearAccelerationCovariance(), linearCov, cv::NORM_INF), 0);
+	EXPECT_FLOAT_EQ(imu.localTransform().x(), 0.1f);
+	EXPECT_FLOAT_EQ(imu.localTransform().theta(), 0.5f);
+}
+
+TEST(IMUTest, VelocityOnlyConstructorLeavesOrientationUnset)
+{
+	const IMU imu(cv::Vec3d(1, 2, 3), cv::Mat(), cv::Vec3d(4, 5, 6), cv::Mat(), Transform::getIdentity());
+
+	EXPECT_EQ(imu.orientation(), cv::Vec4d());
+	EXPECT_TRUE(imu.orientationCovariance().empty());
+	EXPECT_EQ(imu.angularVelocity(), cv::Vec3d(1, 2, 3));
+	EXPECT_EQ(imu.linearAcceleration(), cv::Vec3d(4, 5, 6));
+}
+
+TEST(IMUTest, ConvertToBaseFrameNoOpWhenLocalTransformNull)
+{
+	IMU imu;
+	imu.convertToBaseFrame();
+	EXPECT_TRUE(imu.empty());
+}
+
+TEST(IMUTest, ConvertToBaseFrameNoOpWhenRotationIdentity)
+{
+	IMU imu(cv::Vec3d(1, 0, 0), cv::Mat(), cv::Vec3d(0, 0, 9.8), cv::Mat(), Transform::getIdentity());
+	const cv::Vec3d accelBefore = imu.linearAcceleration();
+	const cv::Vec3d gyroBefore = imu.angularVelocity();
+
+	imu.convertToBaseFrame();
+
+	expectVec3Near(imu.linearAcceleration(), accelBefore);
+	expectVec3Near(imu.angularVelocity(), gyroBefore);
+	EXPECT_TRUE(imu.localTransform().isIdentity());
+}
+
+TEST(IMUTest, ConvertToBaseFrameRotatesLinearAcceleration)
+{
+	const double halfPi = CV_PI / 2.0;
+	const Transform local(0.f, 0.f, 0.f, 0.f, 0.f, static_cast<float>(halfPi));
+	IMU imu(cv::Vec3d(), cv::Mat(), cv::Vec3d(1.0, 0.0, 0.0), cv::Mat(), local);
+
+	imu.convertToBaseFrame();
+
+	expectVec3Near(imu.linearAcceleration(), cv::Vec3d(0.0, 1.0, 0.0), 1e-4);
+	EXPECT_NEAR(imu.localTransform().theta(), 0.0f, 1e-5f);
+	EXPECT_FLOAT_EQ(imu.localTransform().x(), 0.f);
+}
+
+TEST(IMUTest, ConvertToBaseFrameRotatesCovariance)
+{
+	const double halfPi = CV_PI / 2.0;
+	const Transform local(0.f, 0.f, 0.f, 0.f, 0.f, static_cast<float>(halfPi));
+	// Distinct diagonal: a 90° yaw swaps x/y variance (1 and 2), z (4) unchanged.
+	const cv::Mat cov = covariance3x3Diagonal(1.0, 2.0, 4.0);
+	IMU imu(cv::Vec3d(1, 0, 0), cov, cv::Vec3d(), cv::Mat(), local);
+
+	imu.convertToBaseFrame();
+
+	const cv::Mat & rotated = imu.angularVelocityCovariance();
+	ASSERT_EQ(rotated.rows, 3);
+	EXPECT_NE(rotated.at<double>(0, 0), 1.0);
+	EXPECT_NE(rotated.at<double>(1, 1), 2.0);
+	EXPECT_NEAR(rotated.at<double>(0, 0), 2.0, 1e-5);
+	EXPECT_NEAR(rotated.at<double>(1, 1), 1.0, 1e-5);
+	EXPECT_NEAR(rotated.at<double>(2, 2), 4.0, 1e-5);
+	EXPECT_NEAR(rotated.at<double>(0, 1), 0.0, 1e-5);
+	EXPECT_NEAR(rotated.at<double>(1, 0), 0.0, 1e-5);
+}
+
+TEST(IMUEventTest, DefaultConstructor)
+{
+	const IMUEvent event;
+	EXPECT_EQ(event.getClassName(), "IMUEvent");
+	EXPECT_DOUBLE_EQ(event.getStamp(), 0.0);
+	EXPECT_TRUE(event.getData().empty());
+}
+
+TEST(IMUEventTest, StoresDataAndStamp)
+{
+	const IMU imu(cv::Vec3d(0.1, 0.2, 0.3), cv::Mat(), cv::Vec3d(1, 0, 0), cv::Mat(), Transform::getIdentity());
+	const double stamp = 123.456;
+	const IMUEvent event(imu, stamp);
+
+	EXPECT_EQ(event.getClassName(), "IMUEvent");
+	EXPECT_DOUBLE_EQ(event.getStamp(), stamp);
+	EXPECT_FALSE(event.getData().empty());
+	EXPECT_EQ(event.getData().angularVelocity(), imu.angularVelocity());
+	EXPECT_EQ(event.getData().linearAcceleration(), imu.linearAcceleration());
+}