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

74
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,48 +449,45 @@ 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 fixRot(0,0,1,0, 0,-1,0,0, 1,0,0,0);
Transform p = okvisCallbackHandler_->getLastTransform();
if(!p.isNull())
{ {
p = fixPos * p * fixRot; 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);
if(this->getPose().rotation().isIdentity()) Transform p = okvisCallbackHandler_->getLastTransform();
if(!p.isNull())
{ {
initGravity_ = true; p = fixPos * p * fixRot;
this->reset(this->getPose()*p.rotation());
}
if(previousPose_.isIdentity()) if(this->getPose().rotation().isIdentity())
{
previousPose_ = p;
}
// make it incremental
t = previousPose_.inverse()*p;
previousPose_ = p;
if(info)
{
info->reg.covariance = cv::Mat::eye(6,6, CV_64FC1);
info->reg.covariance *= this->framesProcessed() == 0?9999:0.0001;
// FIXME: the scale of landmarks doesn't seem to fit well the environment...
/*info->localMap = okvisCallbackHandler_->getLastLandmarks();
info->localMapSize = info->localMap.size();
for(std::map<int, cv::Point3f>::iterator iter=info->localMap.begin(); iter!=info->localMap.end(); ++iter)
{ {
iter->second = util3d::transformPoint(iter->second, fixPos); initGravity_ = true;
}*/ this->reset(this->getPose()*p.rotation());
}
if(previousPose_.isIdentity())
{
previousPose_ = p;
}
// make it incremental
t = previousPose_.inverse()*p;
previousPose_ = p;
if(info)
{
info->reg.covariance = cv::Mat::eye(6,6, CV_64FC1);
info->reg.covariance *= this->framesProcessed() == 0?9999:0.0001;
// FIXME: the scale of landmarks doesn't seem to fit well the environment...
/*info->localMap = okvisCallbackHandler_->getLastLandmarks();
info->localMapSize = info->localMap.size();
for(std::map<int, cv::Point3f>::iterator iter=info->localMap.begin(); iter!=info->localMap.end(); ++iter)
{
iter->second = util3d::transformPoint(iter->second, fixPos);
}*/
}
} }
}
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());
} }
} }