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

Fixed GTSAM reference frame yaw drift over time when gravity links are used

Browse Source
This commit is contained in:
matlabbe
2020-11-22 16:40:30 -05:00
parent f88845e138
commit bce7ae6acd
2 changed files with 5 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
corelib/src/camera/CameraRealSense2.cpp
View File

@@ -860,10 +860,6 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
video_profile.stream_name().c_str(), video_profile.stream_name().c_str(),
video_profile.stream_type()); video_profile.stream_type());
} }
if(isL500_)
{
UERROR("L500 sensor is detected, note that only 640x480:30FPS configuration is currently supported.");
}
return false; return false;
} }
} }

8
corelib/src/optimizer/OptimizerGTSAM.cpp
View File

@@ -107,12 +107,14 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
// detect if there is a global pose prior set, if so remove rootId // detect if there is a global pose prior set, if so remove rootId
bool gpsPriorOnly = false; bool gpsPriorOnly = false;
bool hasPriorPoses = false;
if(!priorsIgnored()) if(!priorsIgnored())
{ {
for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter) for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
{ {
if(iter->second.from() == iter->second.to() && iter->second.type() == Link::kPosePrior) if(iter->second.from() == iter->second.to() && iter->second.type() == Link::kPosePrior)
{ {
hasPriorPoses = true;
if ((isSlam2d() && 1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) < 9999) || if ((isSlam2d() && 1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) < 9999) ||
(1 / static_cast<double>(iter->second.infMatrix().at<double>(3,3)) < 9999.0 && (1 / static_cast<double>(iter->second.infMatrix().at<double>(3,3)) < 9999.0 &&
1 / static_cast<double>(iter->second.infMatrix().at<double>(4,4)) < 9999.0 && 1 / static_cast<double>(iter->second.infMatrix().at<double>(4,4)) < 9999.0 &&
@@ -138,15 +140,15 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
const Transform & initialPose = poses.at(rootId); const Transform & initialPose = poses.at(rootId);
if(isSlam2d()) if(isSlam2d())
{ {
gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances(gtsam::Vector3(0.01, 0.01, 0.01)); gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances(gtsam::Vector3(0.01, 0.01, hasPriorPoses?1e-2:std::numeric_limits<double>::min()));
graph.add(gtsam::PriorFactor<gtsam::Pose2>(rootId, gtsam::Pose2(initialPose.x(), initialPose.y(), initialPose.theta()), priorNoise)); graph.add(gtsam::PriorFactor<gtsam::Pose2>(rootId, gtsam::Pose2(initialPose.x(), initialPose.y(), initialPose.theta()), priorNoise));
} }
else else
{ {
gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances( gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances(
(gtsam::Vector(6) << (gtsam::Vector(6) <<
(gpsPriorOnly?2:1e-2), gpsPriorOnly?2:1e-2, gpsPriorOnly?2:1e-2, 1e-2, 1e-2, hasPriorPoses?1e-2:std::numeric_limits<double>::min(), // roll, pitch, fixed yaw if there are no priors
1e-2, 1e-2, 1e-2 (gpsPriorOnly?2:1e-2), gpsPriorOnly?2:1e-2, gpsPriorOnly?2:1e-2 // xyz
).finished()); ).finished());
graph.add(gtsam::PriorFactor<gtsam::Pose3>(rootId, gtsam::Pose3(initialPose.toEigen4d()), priorNoise)); graph.add(gtsam::PriorFactor<gtsam::Pose3>(rootId, gtsam::Pose3(initialPose.toEigen4d()), priorNoise));
} }