From cc17ebe92b8276d36d001b27d3b0d67cc845edfc Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 27 Mar 2024 17:18:02 -0700 Subject: [PATCH] iSAM2 integration (#1249) * isam2 integration * fonctional iSAM2, added related parameters * updated default params after testing large-scale enviroment * proximity search optimization * boost<1.68 fix * boost version * Fixed map not showing in graphview * iSAM2: supporting MM/localization/multi-session modes --- corelib/include/rtabmap/core/Parameters.h | 3 + corelib/include/rtabmap/core/Statistics.h | 1 + .../rtabmap/core/optimizer/OptimizerGTSAM.h | 33 +- corelib/src/CameraThread.cpp | 5 + corelib/src/GlobalMap.cpp | 2 +- corelib/src/Rtabmap.cpp | 49 ++- corelib/src/optimizer/OptimizerGTSAM.cpp | 361 +++++++++++++++--- guilib/src/MainWindow.cpp | 8 +- guilib/src/PreferencesDialog.cpp | 3 + guilib/src/ui/preferencesDialog.ui | 69 ++++ 10 files changed, 450 insertions(+), 84 deletions(-) diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index b5ee0072e4..790ab0aebb 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -436,6 +436,9 @@ class RTABMAP_CORE_EXPORT Parameters RTABMAP_PARAM(g2o, Baseline, double, 0.075, "When doing bundle adjustment with RGB-D data, we can set a fake baseline (m) to do stereo bundle adjustment (if 0, mono bundle adjustment is done). For stereo data, the baseline in the calibration is used directly."); RTABMAP_PARAM(GTSAM, Optimizer, int, 1, "0=Levenberg 1=GaussNewton 2=Dogleg"); + RTABMAP_PARAM(GTSAM, Incremental, bool, false, uFormat("Do graph optimization incrementally (iSAM2) to increase optimization speed on loop closures. Note that only GaussNewton and Dogleg optimization algorithms are supported (%s) in this mode.", kGTSAMOptimizer().c_str())); + RTABMAP_PARAM(GTSAM, IncRelinearizeThreshold, double, 0.01, "Only relinearize variables whose linear delta magnitude is greater than this threshold. See GTSAM::ISAM2 doc for more info."); + RTABMAP_PARAM(GTSAM, IncRelinearizeSkip, int, 1, "Only relinearize any variables every X calls to ISAM2::update(). See GTSAM::ISAM2 doc for more info."); // Odometry RTABMAP_PARAM(Odom, Strategy, int, 0, "0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM2 6=OKVIS 7=LOAM 8=MSCKF_VIO 9=VINS-Fusion 10=OpenVINS 11=FLOAM 12=Open3D"); diff --git a/corelib/include/rtabmap/core/Statistics.h b/corelib/include/rtabmap/core/Statistics.h index 743b46bf04..66d0f358c0 100644 --- a/corelib/include/rtabmap/core/Statistics.h +++ b/corelib/include/rtabmap/core/Statistics.h @@ -157,6 +157,7 @@ class RTABMAP_CORE_EXPORT Statistics RTABMAP_STATS(Timing, Memory_update, ms); RTABMAP_STATS(Timing, Neighbor_link_refining, ms); RTABMAP_STATS(Timing, Proximity_by_time, ms); + RTABMAP_STATS(Timing, Proximity_by_space_search, ms); RTABMAP_STATS(Timing, Proximity_by_space_visual, ms); RTABMAP_STATS(Timing, Proximity_by_space, ms); RTABMAP_STATS(Timing, Cleaning_neighbors, ms); diff --git a/corelib/include/rtabmap/core/optimizer/OptimizerGTSAM.h b/corelib/include/rtabmap/core/optimizer/OptimizerGTSAM.h index 5cec5c2415..2d3eb6ba01 100644 --- a/corelib/include/rtabmap/core/optimizer/OptimizerGTSAM.h +++ b/corelib/include/rtabmap/core/optimizer/OptimizerGTSAM.h @@ -30,6 +30,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include +namespace gtsam { + class ISAM2; +} + namespace rtabmap { class RTABMAP_CORE_EXPORT OptimizerGTSAM : public Optimizer @@ -38,13 +42,8 @@ class RTABMAP_CORE_EXPORT OptimizerGTSAM : public Optimizer static bool available(); public: - OptimizerGTSAM(const ParametersMap & parameters = ParametersMap()) : - Optimizer(parameters), - optimizer_(Parameters::defaultGTSAMOptimizer()) - { - parseParameters(parameters); - } - virtual ~OptimizerGTSAM() {} + OptimizerGTSAM(const ParametersMap & parameters = ParametersMap()); + virtual ~OptimizerGTSAM(); virtual Type type() const {return kTypeGTSAM;} @@ -60,7 +59,25 @@ class RTABMAP_CORE_EXPORT OptimizerGTSAM : public Optimizer int * iterationsDone = 0); private: - int optimizer_; + int internalOptimizerType_; + + gtsam::ISAM2 * isam2_; + struct ConstraintToFactor { + ConstraintToFactor(int _from, int _to, std::uint64_t _factorIndice) + { + from = _from; + to = _to; + factorIndice = _factorIndice; + } + int from; + int to; + std::uint64_t factorIndice; + }; + + std::vector lastAddedConstraints_; + int lastSwitchId_; + std::set addedPoses_; + std::pair lastRootFactorIndex_; }; } /* namespace rtabmap */ diff --git a/corelib/src/CameraThread.cpp b/corelib/src/CameraThread.cpp index a18fc2c18f..46997ad2b3 100644 --- a/corelib/src/CameraThread.cpp +++ b/corelib/src/CameraThread.cpp @@ -292,6 +292,11 @@ bool CameraThread::odomProvided() const void CameraThread::mainLoopBegin() { ULogger::registerCurrentThread("Camera"); + if(_imuFilter) + { + // In case we paused the camera and moved somewhere else, restart filtering. + _imuFilter->reset(); + } _camera->resetTimer(); } diff --git a/corelib/src/GlobalMap.cpp b/corelib/src/GlobalMap.cpp index 0c57f7ae7d..256aaeda13 100644 --- a/corelib/src/GlobalMap.cpp +++ b/corelib/src/GlobalMap.cpp @@ -121,7 +121,7 @@ bool GlobalMap::update(const std::map & poses) } else { - UDEBUG("Updated pose for node %d is not found, some points may not be copied. Use negative ids to just update cell values without adding new ones.", jter->first); + UDEBUG("Updated pose for node %d is not found, some points may not be copied. Use negative ids to just update cell values without adding new ones.", iter->first); } } diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index 0c19ded6b4..fc2c8cd57c 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -1094,6 +1094,13 @@ void Rtabmap::resetMemory() { UERROR("RTAB-Map is not initialized. No memory to reset..."); } + + if(_graphOptimizer) + { + delete _graphOptimizer; + _graphOptimizer = Optimizer::create(_parameters); + } + this->setupLogFiles(true); } @@ -1177,6 +1184,7 @@ bool Rtabmap::process( double timeMemoryUpdate = 0; double timeNeighborLinkRefining = 0; double timeProximityByTimeDetection = 0; + double timeProximityBySpaceSearch = 0; double timeProximityBySpaceVisualDetection = 0; double timeProximityBySpaceDetection = 0; double timeCleaningNeighbors = 0; @@ -2598,22 +2606,39 @@ bool Rtabmap::process( // 1) compare visually with nearest locations // UDEBUG("Proximity detection (local loop closure in SPACE using matching images, local radius=%fm)", _localRadius); - std::map nearestIds; - if(_memory->isIncremental() && _proximityMaxGraphDepth > 0) - { - nearestIds = _memory->getNeighborsIdRadius(signature->id(), _localRadius, _optimizedPoses, _proximityMaxGraphDepth); - } - else - { - nearestIds = graph::findNearestNodes(signature->id(), _optimizedPoses, _localRadius); - } + std::map nearestIds = graph::findNearestNodes(signature->id(), _optimizedPoses, _localRadius); UDEBUG("nearestIds=%d/%d", (int)nearestIds.size(), (int)_optimizedPoses.size()); std::map nearestPoses; + std::multimap links; + if(_memory->isIncremental() && _proximityMaxGraphDepth>0) + { + // get bidirectional links + for(std::multimap::iterator iter=_constraints.begin(); iter!=_constraints.end(); ++iter) + { + if(uContains(_optimizedPoses, iter->second.from()) && uContains(_optimizedPoses, iter->second.to())) + { + links.insert(std::make_pair(iter->second.from(), iter->second.to())); + links.insert(std::make_pair(iter->second.to(), iter->second.from())); // <-> + } + } + } for(std::map::iterator iter=nearestIds.lower_bound(1); iter!=nearestIds.end(); ++iter) { if(_memory->getStMem().find(iter->first) == _memory->getStMem().end()) { - nearestPoses.insert(std::make_pair(iter->first, _optimizedPoses.at(iter->first))); + if(_memory->isIncremental() && _proximityMaxGraphDepth > 0) + { + std::list > path = graph::computePath(_optimizedPoses, links, signature->id(), iter->first); + UDEBUG("Graph depth to %d = %ld", iter->first, path.size()); + if(!path.empty() && (int)path.size() <= _proximityMaxGraphDepth) + { + nearestPoses.insert(std::make_pair(iter->first, _optimizedPoses.at(iter->first))); + } + } + else + { + nearestPoses.insert(std::make_pair(iter->first, _optimizedPoses.at(iter->first))); + } } } UDEBUG("nearestPoses=%d", (int)nearestPoses.size()); @@ -2645,6 +2670,9 @@ bool Rtabmap::process( } UDEBUG("nearestPaths=%d proximityMaxPaths=%d", (int)nearestPaths.size(), _proximityMaxPaths); + timeProximityBySpaceSearch = timer.ticks(); + ULOGGER_INFO("timeProximityBySpaceSearch=%fs", timeProximityBySpaceSearch); + float proximityFilteringRadius = _proximityFilteringRadius; if(_maxLoopClosureDistance>0.0f && (proximityFilteringRadius <= 0.0f || _maxLoopClosureDistance #include +#include #ifdef RTABMAP_VERTIGO #include "vertigo/gtsam/betweenFactorSwitchable.h" @@ -62,6 +63,23 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap { +OptimizerGTSAM::OptimizerGTSAM(const ParametersMap & parameters) : + Optimizer(parameters), + internalOptimizerType_(Parameters::defaultGTSAMOptimizer()), + isam2_(0), + lastSwitchId_(1000000000) +{ + lastRootFactorIndex_.first = 0; + parseParameters(parameters); +} + +OptimizerGTSAM::~OptimizerGTSAM() +{ +#ifdef RTABMAP_GTSAM + delete isam2_; +#endif +} + bool OptimizerGTSAM::available() { #ifdef RTABMAP_GTSAM @@ -74,7 +92,47 @@ bool OptimizerGTSAM::available() void OptimizerGTSAM::parseParameters(const ParametersMap & parameters) { Optimizer::parseParameters(parameters); - Parameters::parse(parameters, Parameters::kGTSAMOptimizer(), optimizer_); +#ifdef RTABMAP_GTSAM + Parameters::parse(parameters, Parameters::kGTSAMOptimizer(), internalOptimizerType_); + + bool incremental = isam2_; + double threshold = Parameters::defaultGTSAMIncRelinearizeThreshold(); + int skip = Parameters::defaultGTSAMIncRelinearizeSkip(); + Parameters::parse(parameters, Parameters::kGTSAMIncremental(), incremental); + Parameters::parse(parameters, Parameters::kGTSAMIncRelinearizeThreshold(), threshold); + Parameters::parse(parameters, Parameters::kGTSAMIncRelinearizeSkip(), skip); + UDEBUG("GTSAM %s=%d", Parameters::kGTSAMOptimizer().c_str(), internalOptimizerType_); + UDEBUG("GTSAM %s=%d", Parameters::kGTSAMIncremental().c_str(), incremental?1:0); + UDEBUG("GTSAM %s=%f", Parameters::kGTSAMIncRelinearizeThreshold().c_str(), threshold); + UDEBUG("GTSAM %s=%d", Parameters::kGTSAMIncRelinearizeSkip().c_str(), skip); + if(incremental && !isam2_) + { + gtsam::ISAM2Params::OptimizationParams optParams; + if(internalOptimizerType_==2) + { + optParams = gtsam::ISAM2DoglegParams(); + } + else + { + optParams = gtsam::ISAM2GaussNewtonParams(); + } + gtsam::ISAM2Params params(optParams); + params.relinearizeThreshold = threshold; + params.relinearizeSkip = skip; + params.evaluateNonlinearError = true; + isam2_ = new ISAM2(params); + + addedPoses_.clear(); + lastAddedConstraints_.clear(); + lastRootFactorIndex_.first = 0; + lastSwitchId_ = 1000000000; + } + else if(!incremental && isam2_) + { + delete isam2_; + isam2_ = 0; + } +#endif } std::map OptimizerGTSAM::optimize( @@ -138,9 +196,13 @@ std::map OptimizerGTSAM::optimize( } } + std::vector addedPrior; + gtsam::FactorIndices removeFactorIndices; + //prior first pose - if(rootId != 0) + if(rootId != 0 && (!isam2_ || lastRootFactorIndex_.first != rootId)) { + UDEBUG("Setting prior for rootId=%d", rootId); UASSERT(uContains(poses, rootId)); const Transform & initialPose = poses.at(rootId); UDEBUG("hasGPSPrior=%s", hasGPSPrior?"true":"false"); @@ -148,6 +210,7 @@ std::map OptimizerGTSAM::optimize( { gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances(gtsam::Vector3(0.01, 0.01, hasGPSPrior?1e-2:std::numeric_limits::min())); graph.add(gtsam::PriorFactor(rootId, gtsam::Pose2(initialPose.x(), initialPose.y(), initialPose.theta()), priorNoise)); + addedPrior.push_back(ConstraintToFactor(rootId, rootId, -1)); } else { @@ -157,14 +220,95 @@ std::map OptimizerGTSAM::optimize( (hasGPSPrior?2:1e-2), hasGPSPrior?2:1e-2, hasGPSPrior?2:1e-2 // xyz ).finished()); graph.add(gtsam::PriorFactor(rootId, gtsam::Pose3(initialPose.toEigen4d()), priorNoise)); + addedPrior.push_back(ConstraintToFactor(rootId, rootId, -1)); + } + if(isam2_ && lastRootFactorIndex_.first!=0) + { + if(uContains(poses, lastRootFactorIndex_.first)) + { + UDEBUG("isam2: switching rootid from %d to %d", lastRootFactorIndex_.first, rootId); + removeFactorIndices.push_back(lastRootFactorIndex_.second); + } + else + { + UDEBUG("isam2: reset iSAM2, disjoint mapping sessions between previous root %d and new root %d", lastRootFactorIndex_.first, rootId); + // reset iSAM2, disjoint mapping session + gtsam::ISAM2Params params = isam2_->params(); + delete isam2_; + isam2_ = new gtsam::ISAM2(params); + addedPoses_.clear(); + lastAddedConstraints_.clear(); + lastRootFactorIndex_.first = 0; + lastSwitchId_ = 1000000000; + } + lastRootFactorIndex_.first = 0; } } + std::map newPoses; + std::multimap newEdgeConstraints; + + if(isam2_) + { + UDEBUG("Add new poses..."); + // new poses? + for(std::map::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter) + { + if(addedPoses_.find(iter->first) == addedPoses_.end()) + { + newPoses.insert(*iter); + UDEBUG("Adding pose %d to factor graph", iter->first); + } + } + UDEBUG("Add new links..."); + // new links? + for(std::multimap::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter) + { + if(addedPoses_.find(iter->second.from()) == addedPoses_.end() || + addedPoses_.find(iter->second.to()) == addedPoses_.end()) + { + newEdgeConstraints.insert(*iter); + UDEBUG("Adding constraint %d (%d->%d) to factor graph", iter->first, iter->second.from(), iter->second.to()); + } + } + + if(!this->isRobust()) + { + UDEBUG("Remove links..."); + // Remove constraints not there anymore in case the last loop closures were rejected. + // As we don't track "switch" constraints, we don't support this if vertigo is used. + for(size_t i=0; i%d (factor indice=%ld)", + lastAddedConstraints_[i].from, + lastAddedConstraints_[i].to, + lastAddedConstraints_[i].factorIndice); + } + } + } + else if(poses.rbegin()->first >= 1000000000) + { + UERROR("Lastest pose id (%d) is too huge for robust optimization (over switch offset of 1000000000)", poses.rbegin()->first); + return optimizedPoses; + } + + lastAddedConstraints_ = addedPrior; + } + else + { + newPoses = poses; + newEdgeConstraints = edgeConstraints; + } + UDEBUG("fill poses to gtsam... rootId=%d (priorsIgnored=%d landmarksIgnored=%d)", rootId, priorsIgnored()?1:0, landmarksIgnored()?1:0); gtsam::Values initialEstimate; std::map isLandmarkWithRotation; - for(std::map::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) + for(std::map::const_iterator iter = newPoses.begin(); iter!=newPoses.end(); ++iter) { UASSERT(!iter->second.isNull()); if(isSlam2d()) @@ -172,12 +316,13 @@ std::map OptimizerGTSAM::optimize( if(iter->first > 0) { initialEstimate.insert(iter->first, gtsam::Pose2(iter->second.x(), iter->second.y(), iter->second.theta())); + addedPoses_.insert(iter->first); } else if(!landmarksIgnored()) { // check if it is SE2 or only PointXY - std::multimap::const_iterator jter=edgeConstraints.find(iter->first); - UASSERT_MSG(jter != edgeConstraints.end(), uFormat("Not found landmark %d in edges!", iter->first).c_str()); + std::multimap::const_iterator jter=newEdgeConstraints.find(iter->first); + UASSERT_MSG(jter != newEdgeConstraints.end(), uFormat("Not found landmark %d in edges!", iter->first).c_str()); if (1 / static_cast(jter->second.infMatrix().at(5,5)) >= 9999.0) { @@ -189,6 +334,7 @@ std::map OptimizerGTSAM::optimize( initialEstimate.insert(iter->first, gtsam::Pose2(iter->second.x(), iter->second.y(), iter->second.theta())); isLandmarkWithRotation.insert(std::make_pair(iter->first, true)); } + addedPoses_.insert(iter->first); } } @@ -197,12 +343,13 @@ std::map OptimizerGTSAM::optimize( if(iter->first > 0) { initialEstimate.insert(iter->first, gtsam::Pose3(iter->second.toEigen4d())); + addedPoses_.insert(iter->first); } else if(!landmarksIgnored()) { // check if it is SE3 or only PointXYZ - std::multimap::const_iterator jter=edgeConstraints.find(iter->first); - UASSERT_MSG(jter != edgeConstraints.end(), uFormat("Not found landmark %d in edges!", iter->first).c_str()); + std::multimap::const_iterator jter=newEdgeConstraints.find(iter->first); + UASSERT_MSG(jter != newEdgeConstraints.end(), uFormat("Not found landmark %d in edges!", iter->first).c_str()); if (1 / static_cast(jter->second.infMatrix().at(3,3)) >= 9999.0 || 1 / static_cast(jter->second.infMatrix().at(4,4)) >= 9999.0 || @@ -216,19 +363,23 @@ std::map OptimizerGTSAM::optimize( initialEstimate.insert(iter->first, gtsam::Pose3(iter->second.toEigen4d())); isLandmarkWithRotation.insert(std::make_pair(iter->first, true)); } + addedPoses_.insert(iter->first); } } } UDEBUG("fill edges to gtsam..."); - int switchCounter = poses.rbegin()->first+1; - for(std::multimap::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter) + if(!isam2_) + { + lastSwitchId_ = newPoses.rbegin()->first+1; + } + for(std::multimap::const_iterator iter=newEdgeConstraints.begin(); iter!=newEdgeConstraints.end(); ++iter) { int id1 = iter->second.from(); int id2 = iter->second.to(); - UASSERT_MSG(initialEstimate.find(id1)!=initialEstimate.end(), uFormat("id1=%d", id1).c_str()); - UASSERT_MSG(initialEstimate.find(id2)!=initialEstimate.end(), uFormat("id2=%d", id2).c_str()); + UASSERT_MSG(poses.find(id1)!=poses.end(), uFormat("id1=%d", id1).c_str()); + UASSERT_MSG(poses.find(id2)!=poses.end(), uFormat("id2=%d", id2).c_str()); UASSERT(!iter->second.transform().isNull()); if(id1 == id2) @@ -244,6 +395,7 @@ std::map OptimizerGTSAM::optimize( 1/iter->second.infMatrix().at(0,0), 1/iter->second.infMatrix().at(1,1))); graph.add(XYFactor(id1, gtsam::Point2(iter->second.transform().x(), iter->second.transform().y()), model)); + lastAddedConstraints_.push_back(ConstraintToFactor(id1, id1, -1)); } else if (1 / static_cast(iter->second.infMatrix().at(5,5)) >= 9999.0) { @@ -251,6 +403,7 @@ std::map OptimizerGTSAM::optimize( 1/iter->second.infMatrix().at(0,0), 1/iter->second.infMatrix().at(1,1))); graph.add(XYFactor(id1, gtsam::Point2(iter->second.transform().x(), iter->second.transform().y()), model)); + lastAddedConstraints_.push_back(ConstraintToFactor(id1, id1, -1)); } else { @@ -270,6 +423,7 @@ std::map OptimizerGTSAM::optimize( gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information); graph.add(gtsam::PriorFactor(id1, gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model)); + lastAddedConstraints_.push_back(ConstraintToFactor(id1, id1, -1)); } } else @@ -281,6 +435,7 @@ std::map OptimizerGTSAM::optimize( iter->second.infMatrix().at(1,1), iter->second.infMatrix().at(2,2))); graph.add(XYZFactor(id1, gtsam::Point3(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().z()), model)); + lastAddedConstraints_.push_back(ConstraintToFactor(id1, id1, -1)); } else if (1 / static_cast(iter->second.infMatrix().at(3,3)) >= 9999.0 || 1 / static_cast(iter->second.infMatrix().at(4,4)) >= 9999.0 || @@ -291,6 +446,7 @@ std::map OptimizerGTSAM::optimize( iter->second.infMatrix().at(1,1), iter->second.infMatrix().at(2,2))); graph.add(XYZFactor(id1, gtsam::Point3(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().z()), model)); + lastAddedConstraints_.push_back(ConstraintToFactor(id1, id1, -1)); } else { @@ -308,15 +464,17 @@ std::map OptimizerGTSAM::optimize( gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(mgtsam); graph.add(gtsam::PriorFactor(id1, gtsam::Pose3(iter->second.transform().toEigen4d()), model)); + lastAddedConstraints_.push_back(ConstraintToFactor(id1, id1, -1)); } } } - else if(!isSlam2d() && gravitySigma() > 0 && iter->second.type() == Link::kGravity && poses.find(iter->first) != poses.end()) + else if(!isSlam2d() && gravitySigma() > 0 && iter->second.type() == Link::kGravity && newPoses.find(iter->first) != newPoses.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)); gtsam::SharedNoiseModel model = gtsam::noiseModel::Isotropic::Sigmas(gtsam::Vector2(gravitySigma(), 10)); graph.add(Pose3GravityFactor(iter->first, nG, model, Unit3(0,0,1))); + lastAddedConstraints_.push_back(ConstraintToFactor(iter->first, iter->first, -1)); } } else if(id1<0 || id2 < 0) @@ -354,6 +512,7 @@ std::map OptimizerGTSAM::optimize( } gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information); graph.add(gtsam::BetweenFactor(id1, id2, gtsam::Pose2(t.x(), t.y(), t.theta()), model)); + lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1)); } else { @@ -368,6 +527,7 @@ std::map OptimizerGTSAM::optimize( gtsam::Point2 landmark(t.x(), t.y()); gtsam::Pose2 p; graph.add(gtsam::BearingRangeFactor(id1, id2, p.bearing(landmark), p.range(landmark), model)); + lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1)); } } else @@ -387,6 +547,7 @@ std::map OptimizerGTSAM::optimize( mgtsam.block(3,0,3,3) = information.block(0,3,3,3); // off diagonal gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(mgtsam); graph.add(gtsam::BetweenFactor(id1, id2, gtsam::Pose3(t.toEigen4d()), model)); + lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1)); } else { @@ -401,6 +562,7 @@ std::map OptimizerGTSAM::optimize( gtsam::Point3 landmark(t.x(), t.y(), t.z()); gtsam::Pose3 p; graph.add(gtsam::BearingRangeFactor(id1, id2, p.bearing(landmark), p.range(landmark), model)); + lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1)); } } } @@ -418,7 +580,7 @@ std::map OptimizerGTSAM::optimize( // a proper and convenient initial value for all switch variables would be // sij = 1 when using the linear switch function" double prior = 1.0; - initialEstimate.insert(gtsam::Symbol('s',switchCounter), vertigo::SwitchVariableLinear(prior)); + initialEstimate.insert(gtsam::Symbol('s',lastSwitchId_), vertigo::SwitchVariableLinear(prior)); // create switch prior factor // "If the front-end is not able to assign sound individual values @@ -426,7 +588,7 @@ std::map OptimizerGTSAM::optimize( // to the individual optimal choice of Ξij for a large range of // outliers." gtsam::noiseModel::Diagonal::shared_ptr switchPriorModel = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector1(1.0)); - graph.add(gtsam::PriorFactor (gtsam::Symbol('s',switchCounter), vertigo::SwitchVariableLinear(prior), switchPriorModel)); + graph.add(gtsam::PriorFactor (gtsam::Symbol('s',lastSwitchId_), vertigo::SwitchVariableLinear(prior), switchPriorModel)); } #endif if(isSlam2d()) @@ -452,12 +614,13 @@ std::map OptimizerGTSAM::optimize( iter->second.type() != Link::kNeighborMerged) { // create switchable edge factor - graph.add(vertigo::BetweenFactorSwitchableLinear(id1, id2, gtsam::Symbol('s', switchCounter++), gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model)); + graph.add(vertigo::BetweenFactorSwitchableLinear(id1, id2, gtsam::Symbol('s', lastSwitchId_++), gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model)); } else #endif { graph.add(gtsam::BetweenFactor(id1, id2, gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model)); + lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1)); } } else @@ -481,61 +644,68 @@ std::map OptimizerGTSAM::optimize( iter->second.type() != Link::kNeighborMerged) { // create switchable edge factor - graph.add(vertigo::BetweenFactorSwitchableLinear(id1, id2, gtsam::Symbol('s', switchCounter++), gtsam::Pose3(iter->second.transform().toEigen4d()), model)); + graph.add(vertigo::BetweenFactorSwitchableLinear(id1, id2, gtsam::Symbol('s', lastSwitchId_++), gtsam::Pose3(iter->second.transform().toEigen4d()), model)); } else #endif { graph.add(gtsam::BetweenFactor(id1, id2, gtsam::Pose3(iter->second.transform().toEigen4d()), model)); + lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1)); } } } } UDEBUG("create optimizer"); - gtsam::NonlinearOptimizer * optimizer; + gtsam::NonlinearOptimizer * optimizer = 0; - if(optimizer_ == 2) + if(!isam2_) // Batch optimization { - gtsam::DoglegParams parameters; - parameters.relativeErrorTol = epsilon(); - parameters.maxIterations = iterations(); - optimizer = new gtsam::DoglegOptimizer(graph, initialEstimate, parameters); - } - else if(optimizer_ == 1) - { - gtsam::GaussNewtonParams parameters; - parameters.relativeErrorTol = epsilon(); - parameters.maxIterations = iterations(); - optimizer = new gtsam::GaussNewtonOptimizer(graph, initialEstimate, parameters); + UDEBUG("Batch optimization..."); + if(internalOptimizerType_ == 2) + { + gtsam::DoglegParams parameters; + parameters.relativeErrorTol = epsilon(); + parameters.maxIterations = iterations(); + optimizer = new gtsam::DoglegOptimizer(graph, initialEstimate, parameters); + } + else if(internalOptimizerType_ == 1) + { + gtsam::GaussNewtonParams parameters; + parameters.relativeErrorTol = epsilon(); + parameters.maxIterations = iterations(); + optimizer = new gtsam::GaussNewtonOptimizer(graph, initialEstimate, parameters); + } + else + { + gtsam::LevenbergMarquardtParams parameters; + parameters.relativeErrorTol = epsilon(); + parameters.maxIterations = iterations(); + optimizer = new gtsam::LevenbergMarquardtOptimizer(graph, initialEstimate, parameters); + } } else { - gtsam::LevenbergMarquardtParams parameters; - parameters.relativeErrorTol = epsilon(); - parameters.maxIterations = iterations(); - optimizer = new gtsam::LevenbergMarquardtOptimizer(graph, initialEstimate, parameters); + UDEBUG("iSAM2 optimization..."); } UDEBUG("GTSAM optimizing begin (max iterations=%d, robust=%d)", iterations(), isRobust()?1:0); UTimer timer; int it = 0; - double lastError = optimizer->error(); + double initialError = optimizer?graph.error(initialEstimate):0; + double lastError = optimizer?optimizer->error():0; for(int i=0; i 0) { float x,y,z,roll,pitch,yaw; std::map tmpPoses; -#if GTSAM_VERSION_NUMERIC >= 40200 - for(gtsam::Values::deref_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter) -#else - for(gtsam::Values::const_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter) -#endif + Values values = isam2_?isam2_->calculateEstimate():optimizer->values(); + for(gtsam::Values::iterator iter=values.begin(); iter!=values.end(); ++iter) { - if(iter->value.dim() > 1) + int key = (int)iter->key; + if(iter->value.dim() > 1 && uContains(newPoses, key)) { - int key = (int)iter->key; if(isSlam2d()) { if(key > 0) @@ -547,13 +717,13 @@ std::map OptimizerGTSAM::optimize( { if(isLandmarkWithRotation.at(key)) { - poses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); + newPoses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); gtsam::Pose2 p = iter->value.cast(); tmpPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), z, roll, pitch, p.theta()))); } else { - poses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); + newPoses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); gtsam::Point2 p = iter->value.cast(); tmpPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), z, roll,pitch,yaw))); } @@ -575,7 +745,7 @@ std::map OptimizerGTSAM::optimize( } else { - poses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); + newPoses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); gtsam::Point3 p = iter->value.cast(); tmpPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), p.z(), roll,pitch,yaw))); } @@ -585,25 +755,89 @@ std::map OptimizerGTSAM::optimize( } intermediateGraphes->push_back(tmpPoses); } + + gtsam::ISAM2Result result; + double error = 0; try { - optimizer->iterate(); + if(optimizer) // Batch optimization + { + optimizer->iterate(); + error = optimizer->error(); + } + else if(i==0) // iSAM2 (add factors) + { + UDEBUG("Update iSAM with the new factors"); + result = isam2_->update(graph, initialEstimate, removeFactorIndices); +#if BOOST_VERSION >= 106800 + UASSERT(result.errorBefore.has_value()); + UASSERT(result.errorAfter.has_value()); +#else + UASSERT(result.errorBefore.is_initialized()); + UASSERT(result.errorAfter.is_initialized()); +#endif + UDEBUG("error before = %f after=%f", result.errorBefore.value(), result.errorAfter.value()); + initialError = lastError = result.errorBefore.value(); + error = result.errorAfter.value(); + if(!this->isRobust()) + { + UASSERT_MSG(lastAddedConstraints_.size() == result.newFactorsIndices.size(), + uFormat("%ld versus %ld", lastAddedConstraints_.size(), result.newFactorsIndices.size()).c_str()); + for(size_t j=0; j=1); + lastRootFactorIndex_.first = rootId; + lastRootFactorIndex_.second = result.newFactorsIndices[0]; // first one should be always the root prior + } + } + else // iSAM2 (more iterations) + { + result = isam2_->update(); +#if BOOST_VERSION >= 106800 + UASSERT(result.errorBefore.has_value()); + UASSERT(result.errorAfter.has_value()); +#else + UASSERT(result.errorBefore.is_initialized()); + UASSERT(result.errorAfter.is_initialized()); +#endif + UDEBUG("error before = %f after=%f", result.errorBefore.value(), result.errorAfter.value()); + + lastError = result.errorBefore.value(); + error = result.errorAfter.value(); + } ++it; } catch(gtsam::IndeterminantLinearSystemException & e) { UWARN("GTSAM exception caught: %s\n Graph has %d edges and %d vertices", e.what(), - (int)edgeConstraints.size(), - (int)poses.size()); + (int)newEdgeConstraints.size(), + (int)newPoses.size()); delete optimizer; + if(isam2_) + { + // We are in bad state, cleanup + UDEBUG("Reset iSAM2!"); + gtsam::ISAM2Params params = isam2_->params(); + delete isam2_; + isam2_ = new gtsam::ISAM2(params); + addedPoses_.clear(); + lastAddedConstraints_.clear(); + lastRootFactorIndex_.first = 0; + lastSwitchId_ = 1000000000; + } return optimizedPoses; } // early stop condition - double error = optimizer->error(); UDEBUG("iteration %d error =%f", i+1, error); double errorDelta = lastError - error; - if(i>0 && errorDelta < this->epsilon()) + if((isam2_ || i>0) && errorDelta < this->epsilon()) { if(errorDelta < 0) { @@ -630,19 +864,16 @@ std::map OptimizerGTSAM::optimize( { *iterationsDone = it; } - UDEBUG("GTSAM optimizing end (%d iterations done, error=%f (initial=%f final=%f), time=%f s)", - optimizer->iterations(), optimizer->error(), graph.error(initialEstimate), graph.error(optimizer->values()), timer.ticks()); + UDEBUG("GTSAM optimizing end (%d iterations done (error initial=%f final=%f), time=%f s)", + it, initialError, lastError, timer.ticks()); float x,y,z,roll,pitch,yaw; -#if GTSAM_VERSION_NUMERIC >= 40200 - for(gtsam::Values::deref_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter) -#else - for(gtsam::Values::const_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter) -#endif + gtsam::Values values = isam2_?isam2_->calculateEstimate():optimizer->values(); + for(gtsam::Values::iterator iter=values.begin(); iter!=values.end(); ++iter) { - if(iter->value.dim() > 1) + int key = (int)iter->key; + if(iter->value.dim() > 1 && uContains(poses, key)) { - int key = (int)iter->key; if(isSlam2d()) { if(key > 0) @@ -694,10 +925,18 @@ std::map OptimizerGTSAM::optimize( // compute marginals try { - UDEBUG("Computing marginals..."); + UDEBUG("Computing marginals for node %d...", poses.rbegin()->first); UTimer t; - gtsam::Marginals marginals(graph, optimizer->values()); - gtsam::Matrix info = marginals.marginalCovariance(poses.rbegin()->first); + gtsam::Matrix info; + if(optimizer) + { + gtsam::Marginals marginals(graph, optimizer->values()); + info = marginals.marginalCovariance(poses.rbegin()->first); + } + else //iSAM2 + { + info = isam2_->marginalCovariance(poses.rbegin()->first); + } UDEBUG("Computed marginals = %fs (key=%d)", t.ticks(), poses.rbegin()->first); if(isSlam2d() && info.cols() == 3 && info.cols() == 3) { diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index cd6ec0e80c..3aa04a3f7f 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -3429,7 +3429,8 @@ void MainWindow::updateMapCloud( } } cv::Mat map8U; - if((_ui->graphicsView_graphView->isVisible() || _preferencesDialog->getGridMapShown())) + if((_ui->graphicsView_graphView->isVisible() && _ui->graphicsView_graphView->isGridMapVisible()) || + (_cloudViewer->isVisible() && _preferencesDialog->getGridMapShown())) { float xMin, yMin; float resolution = _occupancyGrid->getCellSize(); @@ -3438,7 +3439,6 @@ void MainWindow::updateMapCloud( if(_preferencesDialog->isOctomap2dGrid()) { map8S = _octomap->createProjectionMap(xMin, yMin, resolution, 0, _preferencesDialog->getOctomapTreeDepth()); - } else #endif @@ -3458,12 +3458,12 @@ void MainWindow::updateMapCloud( //convert to gray scaled map map8U = util3d::convertMap2Image8U(map8S); - if(_preferencesDialog->getGridMapShown()) + if(_cloudViewer->isVisible() && _preferencesDialog->getGridMapShown()) { float opacity = _preferencesDialog->getGridMapOpacity(); _cloudViewer->addOccupancyGridMap(map8U, resolution, xMin, yMin, opacity); } - if(_ui->graphicsView_graphView->isVisible()) + if(_ui->graphicsView_graphView->isVisible() && _ui->graphicsView_graphView->isGridMapVisible()) { _ui->graphicsView_graphView->updateMap(map8U, resolution, xMin, yMin); } diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index df7efb2068..6b60789258 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -1137,6 +1137,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->doubleSpinBox_g2o_baseline->setObjectName(Parameters::kg2oBaseline().c_str()); _ui->comboBox_gtsam_optimizer->setObjectName(Parameters::kGTSAMOptimizer().c_str()); + _ui->gtsam_incremental->setObjectName(Parameters::kGTSAMIncremental().c_str()); + _ui->gtsam_incremental_threshold->setObjectName(Parameters::kGTSAMIncRelinearizeThreshold().c_str()); + _ui->gtsam_incremental_skip->setObjectName(Parameters::kGTSAMIncRelinearizeSkip().c_str()); _ui->graphPlan_goalReachedRadius->setObjectName(Parameters::kRGBDGoalReachedRadius().c_str()); _ui->graphPlan_goalsSavedInUserData->setObjectName(Parameters::kRGBDGoalsSavedInUserData().c_str()); diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 8d6c65728d..dc06feee56 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -23285,6 +23285,16 @@ Lower the ratio -> higher the precision. + + + + iSAM2: Do graph optimization incrementally to increase optimization speed on loop closures. Note that only GaussNewton and Dogleg optimization algorithms are supported in this mode. + + + true + + + @@ -23295,6 +23305,65 @@ Lower the ratio -> higher the precision. + + + + 4 + + + 0.000100000000000 + + + 999.000000000000000 + + + 0.100000000000000 + + + 0.100000000000000 + + + + + + + Only relinearize variables whose linear delta magnitude is greater than this threshold. See GTSAM::ISAM2 doc for more info. + + + true + + + + + + + + + + + + + + 1 + + + 10000 + + + 10 + + + + + + + Only relinearize any variables every X calls to ISAM2::update(). See GTSAM::ISAM2 doc for more info. + + + true + + +