i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

Memory: Added parameter Mem/UseOdomGravity, adding kGravity links when creating a node if IMU is present or Mem/UseOdomGravity is set. Fixed OpenCV4 related build errors on stereo fisheye rectification code. Signature: changed links from map to multimap to support having multiple self references (prior, gravity constraints...). DBReader: publish IMU orientation if a gravity link is detected.

Browse Source
This commit is contained in:
matlabbe
2019-05-31 15:36:35 -04:00
parent 71f7515775
commit e887d462ce
28 changed files with 1264 additions and 344 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
corelib/src/optimizer/OptimizerG2O.cpp
View File

@@ -530,7 +530,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
}
}
}
else if(!isSlam2d() && gravitySigma() > 0 && iter->second.type() == Link::kPoseOdom && poses.find(iter->first) != poses.end())
else if(!isSlam2d() && gravitySigma() > 0 && iter->second.type() == Link::kGravity && poses.find(iter->first) != poses.end())
{
Eigen::Matrix<double, 6, 1> m;
// Up vector in robot frame
@@ -1403,7 +1403,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
{
#ifndef RTABMAP_ORB_SLAM2
g2o::HyperGraph::Edge * edge = 0;
if(gravitySigma() > 0 && iter->second.type() == Link::kPoseOdom && poses.find(iter->first) != poses.end())
if(gravitySigma() > 0 && iter->second.type() == Link::kGravity && poses.find(iter->first) != poses.end())
{
Eigen::Matrix<double, 6, 1> m;
// Up vector in robot frame
@@ -1932,7 +1932,11 @@ bool OptimizerG2O::saveGraph(
bool isSE2 = true;
bool isSE3 = true;
if (iter->second.type() == Link::kPosePrior)
if (iter->second.type() == Link::kGravity)
{
continue;
}
else if (iter->second.type() == Link::kPosePrior)
{
if (this->priorsIgnored())
{

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

@@ -282,7 +282,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
}
}
}
else if(!isSlam2d() && gravitySigma() > 0 && iter->second.type() == Link::kPoseOdom && poses.find(iter->first) != poses.end())
else if(!isSlam2d() && gravitySigma() > 0 && iter->second.type() == Link::kGravity && poses.find(iter->first) != poses.end())
{
Vector3 r = gtsam::Pose3(iter->second.transform().toEigen4d()).rotation().xyz();
gtsam::Unit3 nG = gtsam::Rot3::RzRyRx(r.x(), r.y(), 0).rotate(gtsam::Unit3(0,0,-1));
@@ -376,13 +376,12 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
}
}
}
else
else // id1 != id2
{
#ifdef RTABMAP_VERTIGO
if(this->isRobust() &&
iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged &&
iter->second.type() != Link::kPosePrior)
iter->second.type() != Link::kNeighborMerged)
{
// create new switch variable
// Sunderhauf IROS 2012:
@@ -451,8 +450,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
#ifdef RTABMAP_VERTIGO
if(this->isRobust() &&
iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged &&
iter->second.type() != Link::kPosePrior)
iter->second.type() != Link::kNeighborMerged)
{
// create switchable edge factor
graph.add(vertigo::BetweenFactorSwitchableLinear<gtsam::Pose3>(id1, id2, gtsam::Symbol('s', switchCounter++), gtsam::Pose3(iter->second.transform().toEigen4d()), model));

2
corelib/src/optimizer/OptimizerTORO.cpp
View File

@@ -395,7 +395,7 @@ bool OptimizerTORO::saveGraph(
for(std::multimap<int, Link>::const_iterator iter = edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
{
if (iter->second.type() != Link::kPosePrior)
if (iter->second.type() != Link::kPosePrior && iter->second.type() != Link::kGravity)
{
if (isSlam2d())
{