i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

IMU: changed members from Eigen to cv to avoid seg faults about Eigen memory alignment when copying IMU object created dynamically (#270)

Browse Source
This commit is contained in:
matlabbe
2018-03-21 21:31:41 -04:00
parent 4188da2ef2
commit 00412749e7
5 changed files with 60 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
corelib/include/rtabmap/core/IMU.h
View File

@@ -19,11 +19,11 @@ class IMU
{
public:
IMU() {}
IMU(const Eigen::Quaterniond & orientation,
IMU(const cv::Vec4d & orientation,
const cv::Mat & orientationCovariance,
const Eigen::Vector3d & angularVelocity,
const cv::Vec3d & angularVelocity,
const cv::Mat & angularVelocityCovariance,
const Eigen::Vector3d & linearAcceleration,
const cv::Vec3d & linearAcceleration,
const cv::Mat & linearAccelerationCovariance,
const Transform & localTransform = Transform::getIdentity()) :
orientation_(orientation),
@@ -38,9 +38,9 @@ public:
UASSERT(!angularVelocityCovariance.empty() && angularVelocityCovariance.cols == 3 && angularVelocityCovariance.rows == 3 && angularVelocityCovariance.type() == CV_64FC1);
UASSERT(!linearAccelerationCovariance.empty() && linearAccelerationCovariance.cols == 3 && linearAccelerationCovariance.rows == 3 && linearAccelerationCovariance.type() == CV_64FC1);
}
IMU(const Eigen::Vector3d & angularVelocity,
IMU(const cv::Vec3d & angularVelocity,
const cv::Mat & angularVelocityCovariance,
const Eigen::Vector3d & linearAcceleration,
const cv::Vec3d & linearAcceleration,
const cv::Mat & linearAccelerationCovariance,
const Transform & localTransform = Transform::getIdentity()) :
angularVelocity_(angularVelocity),
@@ -53,13 +53,13 @@ public:
UASSERT(!linearAccelerationCovariance.empty() && linearAccelerationCovariance.cols == 3 && linearAccelerationCovariance.rows == 3 && linearAccelerationCovariance.type() == CV_64FC1);
}
const Eigen::Quaterniond & orientation() const {return orientation_;}
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
const Eigen::Vector3d & angularVelocity() const {return angularVelocity_;}
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
const Eigen::Vector3d linearAcceleration() const {return linearAcceleration_;}
const cv::Vec3d linearAcceleration() const {return linearAcceleration_;}
const cv::Mat & linearAccelerationCovariance() const {return linearAccelerationCovariance_;} // 3x3 double Row major x, y z, empty if linearAcceleration is not set
const Transform & localTransform() const {return localTransform_;}
@@ -71,13 +71,13 @@ public:
private:
Eigen::Quaterniond orientation_;
cv::Vec4d orientation_;
cv::Mat orientationCovariance_; // 3x3 double Row major about x, y, z axes, empty if orientation is not set
Eigen::Vector3d angularVelocity_;
cv::Vec3d angularVelocity_;
cv::Mat angularVelocityCovariance_; // 3x3 double Row major about x, y, z axes, empty if angularVelocity is not set
Eigen::Vector3d linearAcceleration_;
cv::Vec3d linearAcceleration_;
cv::Mat linearAccelerationCovariance_; // 3x3 double Row major x, y z, empty if linearAcceleration is not set
Transform localTransform_;

4
corelib/src/IMUThread.cpp
View File

@@ -121,13 +121,13 @@ void IMUThread::mainLoop()
std::string nanoseconds = s.substr(s.size() - 9, 9);
std::string seconds = s.substr(0, s.size() - 9);
Eigen::Vector3d gyr;
cv::Vec3d gyr;
for (int j = 0; j < 3; ++j) {
std::getline(stream, s, ',');
gyr[j] = uStr2Double(s);
}
Eigen::Vector3d acc;
cv::Vec3d acc;
for (int j = 0; j < 3; ++j) {
std::getline(stream, s, ',');
acc[j] = uStr2Double(s);

4
corelib/src/OdometryOkvis.cpp
View File

@@ -200,7 +200,9 @@ Transform OdometryOkvis::computeTransform(
data.imu().angularVelocity()[2]);
if(okvisEstimator_ != 0)
{
imuUpdated = okvisEstimator_->addImuMeasurement(timeOkvis, data.imu().linearAcceleration(), data.imu().angularVelocity());
Eigen::Vector3d acc(data.imu().linearAcceleration()[0], data.imu().linearAcceleration()[1], data.imu().linearAcceleration()[2]);
Eigen::Vector3d ang(data.imu().angularVelocity()[0], data.imu().angularVelocity()[1], data.imu().angularVelocity()[2]);
imuUpdated = okvisEstimator_->addImuMeasurement(timeOkvis, acc, ang);
}
else
{