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

Deprecated getClosestTransform(), use getTransform() instead. OdometryF2M: removed patch about forcing roll/pitch from IMU (BA is already doing the job).

Browse Source
This commit is contained in:
matlabbe
2019-09-15 17:31:58 -04:00
parent 350d3cd85f
commit b2b1977f8b
3 changed files with 50 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -333,22 +333,6 @@ Transform OdometryF2M::computeTransform(
UDEBUG("Registration time = %fs", regInfo.totalTime);
if(!transform.isNull())
{
cv::Mat var = regInfo.covariance.clone();
Transform imuT;
if(!imus_.empty())
{
imuT = Transform::getClosestTransform(imus_, lastFrame_->getStamp());
if(!imuT.isNull())
{
float roll, pitch, yaw;
imuT.getEulerAngles(roll, pitch, yaw);
transform = Transform(transform.x(),transform.y(),transform.z(),roll,pitch,transform.theta());
var.at<double>(3,3)/=100.0; // roll
var.at<double>(4,4)/=100.0; // pitch
}
}
// local bundle adjustment
if(bundleAdjustment_>0 && sba_ &&
regPipeline_->isImageRequired() &&
@@ -379,12 +363,17 @@ Transform OdometryF2M::computeTransform(
UASSERT_MSG(bundlePoses.find(lastFrame_->id()) == bundlePoses.end(),
uFormat("Frame %d already added! Make sure the input frames have unique IDs!", lastFrame_->id()).c_str());
bundleLinks.insert(std::make_pair(bundlePoses_.rbegin()->first, Link(bundlePoses_.rbegin()->first, lastFrame_->id(), Link::kNeighbor, bundlePoses_.rbegin()->second.inverse()*transform, var.inv())));
bundleLinks.insert(std::make_pair(bundlePoses_.rbegin()->first, Link(bundlePoses_.rbegin()->first, lastFrame_->id(), Link::kNeighbor, bundlePoses_.rbegin()->second.inverse()*transform, regInfo.covariance.inv())));
bundlePoses.insert(std::make_pair(lastFrame_->id(), transform));
if(!imuT.isNull())
Transform imuT;
if(!imus_.empty())
{
bundleLinks.insert(std::make_pair(lastFrame_->id(), Link(lastFrame_->id(), lastFrame_->id(), Link::kGravity, imuT)));
imuT = Transform::getTransform(imus_, lastFrame_->getStamp());
if(!imuT.isNull())
{
bundleLinks.insert(std::make_pair(lastFrame_->id(), Link(lastFrame_->id(), lastFrame_->id(), Link::kGravity, imuT)));
}
}
CameraModel model;