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

CameraRealSense2: fixed wrong IR image index. DBReader: fixed imu not published. OdometryF2M: use IMU roll/pitch values when IMU is used (reducing variance on those angles before BA).

Browse Source
This commit is contained in:
matlabbe
2019-09-13 21:45:13 -04:00
parent bdc8d839ff
commit 350d3cd85f
13 changed files with 182 additions and 99 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/CameraThread.cpp
+32 -20
View File
@@ -405,26 +405,18 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
// IMU filtering
if(_imuFilter && !data.imu().empty())
{
_imuFilter->update(
data.imu().angularVelocity()[0],
data.imu().angularVelocity()[1],
data.imu().angularVelocity()[2],
data.imu().linearAcceleration()[0],
data.imu().linearAcceleration()[1],
data.imu().linearAcceleration()[2],
data.stamp());
double qx,qy,qz,qw;
_imuFilter->getOrientation(qx,qy,qz,qw);
data.setIMU(IMU(
cv::Vec4d(qx,qy,qz,qw), cv::Mat::eye(3,3,CV_64FC1),
data.imu().angularVelocity(), data.imu().angularVelocityCovariance(),
data.imu().linearAcceleration(), data.imu().linearAccelerationCovariance(),
data.imu().localTransform()));
UDEBUG("%f %f %f %f (gyro=%f %f %f, acc=%f %f %f, %fs)",
data.imu().orientation()[0],
data.imu().orientation()[1],
data.imu().orientation()[2],
data.imu().orientation()[3],
if(data.imu().angularVelocity()[0] == 0 &&
data.imu().angularVelocity()[1] == 0 &&
data.imu().angularVelocity()[2] == 0 &&
data.imu().linearAcceleration()[0] == 0 &&
data.imu().linearAcceleration()[1] == 0 &&
data.imu().linearAcceleration()[2] == 0)
{
UWARN("IMU's acc and gyr values are null! Please disable IMU filtering.");
}
else
{
_imuFilter->update(
data.imu().angularVelocity()[0],
data.imu().angularVelocity()[1],
data.imu().angularVelocity()[2],
@@ -432,6 +424,26 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
data.imu().linearAcceleration()[1],
data.imu().linearAcceleration()[2],
data.stamp());
double qx,qy,qz,qw;
_imuFilter->getOrientation(qx,qy,qz,qw);
data.setIMU(IMU(
cv::Vec4d(qx,qy,qz,qw), cv::Mat::eye(3,3,CV_64FC1),
data.imu().angularVelocity(), data.imu().angularVelocityCovariance(),
data.imu().linearAcceleration(), data.imu().linearAccelerationCovariance(),
data.imu().localTransform()));
UDEBUG("%f %f %f %f (gyro=%f %f %f, acc=%f %f %f, %fs)",
data.imu().orientation()[0],
data.imu().orientation()[1],
data.imu().orientation()[2],
data.imu().orientation()[3],
data.imu().angularVelocity()[0],
data.imu().angularVelocity()[1],
data.imu().angularVelocity()[2],
data.imu().linearAcceleration()[0],
data.imu().linearAcceleration()[1],
data.imu().linearAcceleration()[2],
data.stamp());
}
}
}