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

Added Features2d::limitKeypoints() with grid options. Odometry: if imu is provided and no guess is provided, the change of orientation of imu is used for rotation guess (overwrite rotation from Odom/GuessFromMotion). OdometryInfo: added gravity errors when imu is used. Preferences: added a second GravitySigma parameter (overwritting Optimizer/GravitySigma for odometry is not negative) for F2M odometry panel.

Browse Source
This commit is contained in:
matlabbe
2020-05-31 11:22:12 -04:00
parent 6e55525a7b
commit 415a2778f1
15 changed files with 186 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -507,6 +507,17 @@ Transform OdometryF2M::computeTransform(
std::multimap<int, Link>::iterator iter = graph::findLink(bundleLinks, bundlePoses_.rbegin()->first, lastFrame_->id(), false);
UASSERT(iter != bundleLinks.end());
iter->second.setTransform(bundlePoses_.rbegin()->second.inverse()*transform);
iter = graph::findLink(bundleLinks, lastFrame_->id(), lastFrame_->id(), false);
if(info && iter!=bundleLinks.end() && iter->second.type() == Link::kGravity)
{
float rollImu,pitchImu,yaw;
iter->second.transform().getEulerAngles(rollImu, pitchImu, yaw);
float roll,pitch;
transform.getEulerAngles(roll, pitch, yaw);
info->gravityRollError = fabs(rollImu - roll);
info->gravityPitchError = fabs(pitchImu - pitch);
}
}
}
UDEBUG("Local Bundle Adjustment After : %s", transform.prettyPrint().c_str());