i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-03 01:50:24 +08:00
Code Issues Packages Projects Releases Wiki Activity

CameraRealSense2: fixed wrong IR image index. DBReader: fixed imu not published. OdometryF2M: use IMU roll/pitch values when IMU is used (reducing variance on those angles before BA).

Browse Source
This commit is contained in:
matlabbe
2019-09-13 21:45:13 -04:00
parent bdc8d839ff
commit 350d3cd85f
13 changed files with 182 additions and 99 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -333,10 +333,20 @@ 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
@@ -369,9 +379,6 @@ 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());
cv::Mat var = regInfo.covariance;//cv::Mat::eye(6,6,CV_64FC1); //regInfo.covariance.inv()
//var(cv::Range(0,3), cv::Range(0,3)) *= 0.001;
//var(cv::Range(3,6), cv::Range(3,6)) *= 0.001;
bundleLinks.insert(std::make_pair(bundlePoses_.rbegin()->first, Link(bundlePoses_.rbegin()->first, lastFrame_->id(), Link::kNeighbor, bundlePoses_.rbegin()->second.inverse()*transform, var.inv())));
bundlePoses.insert(std::make_pair(lastFrame_->id(), transform));