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

CameraFreenect: set accelerometer values in IMU member of SensorData. OdomF2M and OdomF2F: intialize orientation with gravity for the first frame if accelerometer value is valid in SensorData.

Browse Source
This commit is contained in:
matlabbe
2018-12-01 00:25:48 -05:00
parent 8a8f46c325
commit abbcc4f8a9
3 changed files with 67 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
corelib/src/odometry/OdometryF2F.cpp
View File

@@ -91,6 +91,30 @@ Transform OdometryF2F::computeTransform(
UASSERT(!this->getPose().isNull());
if(lastKeyFramePose_.isNull())
{
if(this->getPose().rotation().isIdentity() &&
data.imu().linearAcceleration()[0]!=0.0 &&
data.imu().linearAcceleration()[1]!=0.0 &&
data.imu().linearAcceleration()[2]!=0.0 &&
!data.imu().localTransform().isNull())
{
// align with gravity
Eigen::Vector3f n(data.imu().linearAcceleration()[0], data.imu().linearAcceleration()[1], data.imu().linearAcceleration()[2]);
n = data.imu().localTransform().toEigen3f() * n;
n.normalize();
n[0]*=-1;
n[1]*=-1;
n[2]*=-1;
Eigen::Vector3f z(0,0,1);
//get rotation from z to n;
Eigen::Matrix3f R;
R = Eigen::Quaternionf().setFromTwoVectors(n,z);
Transform rotation(
R(0,0), R(0,1), R(0,2), 0,
R(1,0), R(1,1), R(1,2), 0,
R(2,0), R(2,1), R(2,2), 0);
this->reset(rotation);
}
lastKeyFramePose_ = this->getPose(); // reset to current pose
}
Transform motionSinceLastKeyFrame = lastKeyFramePose_.inverse()*this->getPose();

25
corelib/src/odometry/OdometryF2M.cpp
View File

@@ -938,6 +938,31 @@ Transform OdometryF2M::computeTransform(
regInfo.covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0;
bool frameValid = false;
if(this->getPose().rotation().isIdentity() &&
data.imu().linearAcceleration()[0]!=0.0 &&
data.imu().linearAcceleration()[1]!=0.0 &&
data.imu().linearAcceleration()[2]!=0.0 &&
!data.imu().localTransform().isNull())
{
// align with gravity
Eigen::Vector3f n(data.imu().linearAcceleration()[0], data.imu().linearAcceleration()[1], data.imu().linearAcceleration()[2]);
n = data.imu().localTransform().toEigen3f() * n;
n.normalize();
n[0]*=-1;
n[1]*=-1;
n[2]*=-1;
Eigen::Vector3f z(0,0,1);
//get rotation from z to n;
Eigen::Matrix3f R;
R = Eigen::Quaternionf().setFromTwoVectors(n,z);
Transform rotation(
R(0,0), R(0,1), R(0,2), 0,
R(1,0), R(1,1), R(1,2), 0,
R(2,0), R(2,1), R(2,2), 0);
this->reset(rotation);
}
Transform newFramePose = this->getPose(); // initial pose may be not identity...
if(regPipeline_->isImageRequired())
{