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

OdometryOkvis: return pose only after processing measurements (images)

Browse Source
This commit is contained in:
matlabbe
2019-03-20 22:10:15 -04:00
parent ae3d651e83
commit 6f4dce52fc
1 changed files with 35 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
corelib/src/odometry/OdometryOkvis.cpp
View File

@@ -194,7 +194,6 @@ Transform OdometryOkvis::computeTransform(
okvis::Time timeOkvis = okvis::Time(data.stamp()); okvis::Time timeOkvis = okvis::Time(data.stamp());
bool imuUpdated = false;
if(!data.imu().empty()) if(!data.imu().empty())
{ {
UDEBUG("IMU update stamp=%f acc=%f %f %f gyr=%f %f %f", data.stamp(), UDEBUG("IMU update stamp=%f acc=%f %f %f gyr=%f %f %f", data.stamp(),
@@ -208,7 +207,7 @@ Transform OdometryOkvis::computeTransform(
{ {
Eigen::Vector3d acc(data.imu().linearAcceleration()[0], data.imu().linearAcceleration()[1], data.imu().linearAcceleration()[2]); 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]); Eigen::Vector3d ang(data.imu().angularVelocity()[0], data.imu().angularVelocity()[1], data.imu().angularVelocity()[2]);
imuUpdated = okvisEstimator_->addImuMeasurement(timeOkvis, acc, ang); okvisEstimator_->addImuMeasurement(timeOkvis, acc, ang);
} }
else else
{ {
@@ -217,7 +216,6 @@ Transform OdometryOkvis::computeTransform(
} }
} }
bool imageUpdated = false;
if(!data.imageRaw().empty()) if(!data.imageRaw().empty())
{ {
UDEBUG("Image update stamp=%f", data.stamp()); UDEBUG("Image update stamp=%f", data.stamp());
@@ -273,6 +271,7 @@ Transform OdometryOkvis::computeTransform(
} }
} }
bool imageUpdated = false;
if(images.size()) if(images.size())
{ {
// initialization // initialization
@@ -450,9 +449,8 @@ Transform OdometryOkvis::computeTransform(
++imagesProcessed_; ++imagesProcessed_;
} }
} }
}
if((imageUpdated || imuUpdated) && imagesProcessed_ > 10) if(imageUpdated && imagesProcessed_ > 10)
{ {
Transform fixPos(-1,0,0,0, 0,-1,0,0, 0,0,1,0); Transform fixPos(-1,0,0,0, 0,-1,0,0, 0,0,1,0);
Transform fixRot(0,0,1,0, 0,-1,0,0, 1,0,0,0); Transform fixRot(0,0,1,0, 0,-1,0,0, 1,0,0,0);
@@ -490,8 +488,6 @@ Transform OdometryOkvis::computeTransform(
}*/ }*/
} }
} }
if(imageUpdated)
{
UINFO("Odom update time = %fs p=%s", timer.elapsed(), p.prettyPrint().c_str()); UINFO("Odom update time = %fs p=%s", timer.elapsed(), p.prettyPrint().c_str());
} }
} }