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

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());
bool imuUpdated = false;
if(!data.imu().empty())
{
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 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
{
@@ -217,7 +216,6 @@ Transform OdometryOkvis::computeTransform(
}
}
bool imageUpdated = false;
if(!data.imageRaw().empty())
{
UDEBUG("Image update stamp=%f", data.stamp());
@@ -273,6 +271,7 @@ Transform OdometryOkvis::computeTransform(
}
}
bool imageUpdated = false;
if(images.size())
{
// initialization
@@ -450,48 +449,45 @@ Transform OdometryOkvis::computeTransform(
++imagesProcessed_;
}
}
}
if((imageUpdated || imuUpdated) && 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())
if(imageUpdated && imagesProcessed_ > 10)
{
p = fixPos * p * fixRot;
if(this->getPose().rotation().isIdentity())
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())
{
initGravity_ = true;
this->reset(this->getPose()*p.rotation());
}
p = fixPos * p * fixRot;
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)
if(this->getPose().rotation().isIdentity())
{
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());
}
}