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

CameraImages: moved scan filtering stuff to CameraThread

Browse Source 
Preferences->Source: refactored scan filtering parameters
OdometryF2M and OdometryF2F: moved odom orientation init from IMU to OdometryROS instead
This commit is contained in:
matlabbe
2019-01-15 15:41:07 -05:00
parent 42c8ed0c0f
commit 9da2c1918f
12 changed files with 210 additions and 269 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -91,30 +91,6 @@ 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,31 +938,6 @@ 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())
{