i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-06 01:57:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added IMU tests

Browse Source
This commit is contained in:
matlabbe
2026-05-17 10:03:13 -07:00
parent c102c687d9
commit c498a71bc1
4 changed files with 269 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/IMU.cpp
+6 -4
View File
@@ -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_);
linearAcceleration_ = cv::Vec3d(v(0,0), v(0,1), v(0,2));
cv::Mat_<double> linearIn = (cv::Mat_<double>(3,1) << linearAcceleration_[0], linearAcceleration_[1], linearAcceleration_[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_);
angularVelocity_ = cv::Vec3d(v(0,0), v(0,1), v(0,2));
cv::Mat_<double> angularIn = (cv::Mat_<double>(3,1) << angularVelocity_[0], angularVelocity_[1], angularVelocity_[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;