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 OptimizerCeres class (Optimizer/Strategy=3)

Browse Source
This commit is contained in:
matlabbe
2019-08-17 03:55:04 -04:00
parent 38044040a6
commit 536f001479
29 changed files with 1982 additions and 284 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
corelib/include/rtabmap/core/Optimizer.h
View File

@@ -62,7 +62,8 @@ public:
kTypeTORO = 0,
kTypeG2O = 1,
kTypeGTSAM = 2,
kTypeCVSBA = 3
kTypeCeres = 3,
kTypeCVSBA = 4
};
static bool isAvailable(Optimizer::Type type);
static Optimizer * create(const ParametersMap & parameters);

22
corelib/include/rtabmap/core/Parameters.h
View File

@@ -374,25 +374,31 @@ class RTABMAP_EXP Parameters
// Graph optimization
#ifdef RTABMAP_GTSAM
RTABMAP_PARAM(Optimizer, Strategy, int, 2, "Graph optimization strategy: 0=TORO, 1=g2o and 2=GTSAM.");
RTABMAP_PARAM(Optimizer, Strategy, int, 2, "Graph optimization strategy: 0=TORO, 1=g2o, 2=GTSAM and 3=Ceres.");
RTABMAP_PARAM(Optimizer, Iterations, int, 20, "Optimization iterations.");
RTABMAP_PARAM(Optimizer, Epsilon, double, 0.00001, "Stop optimizing when the error improvement is less than this value.");
#else
#ifdef RTABMAP_G2O
RTABMAP_PARAM(Optimizer, Strategy, int, 1, "Graph optimization strategy: 0=TORO, 1=g2o and 2=GTSAM.");
RTABMAP_PARAM(Optimizer, Strategy, int, 1, "Graph optimization strategy: 0=TORO, 1=g2o, 2=GTSAM and 3=Ceres.");
RTABMAP_PARAM(Optimizer, Iterations, int, 20, "Optimization iterations.");
RTABMAP_PARAM(Optimizer, Epsilon, double, 0.0, "Stop optimizing when the error improvement is less than this value.");
#else
RTABMAP_PARAM(Optimizer, Strategy, int, 0, "Graph optimization strategy: 0=TORO, 1=g2o and 2=GTSAM.");
#ifdef RTABMAP_CERES
RTABMAP_PARAM(Optimizer, Strategy, int, 3, "Graph optimization strategy: 0=TORO, 1=g2o, 2=GTSAM and 3=Ceres.");
RTABMAP_PARAM(Optimizer, Iterations, int, 20, "Optimization iterations.");
RTABMAP_PARAM(Optimizer, Epsilon, double, 0.000001, "Stop optimizing when the error improvement is less than this value.");
#else
RTABMAP_PARAM(Optimizer, Strategy, int, 0, "Graph optimization strategy: 0=TORO, 1=g2o, 2=GTSAM and 3=Ceres.");
RTABMAP_PARAM(Optimizer, Iterations, int, 100, "Optimization iterations.");
RTABMAP_PARAM(Optimizer, Epsilon, double, 0.00001, "Stop optimizing when the error improvement is less than this value.");
#endif
#endif
#endif
RTABMAP_PARAM(Optimizer, VarianceIgnored, bool, false, "Ignore constraints' variance. If checked, identity information matrix is used for each constraint. Otherwise, an information matrix is generated from the variance saved in the links.");
RTABMAP_PARAM(Optimizer, Robust, bool, false, uFormat("Robust graph optimization using Vertigo (only work for g2o and GTSAM optimization strategies). Not compatible with \"%s\" if enabled.", kRGBDOptimizeMaxError().c_str()));
RTABMAP_PARAM(Optimizer, PriorsIgnored, bool, true, "Ignore prior constraints (global pose or GPS) while optimizing. Currently only g2o and gtsam optimization supports this.");
RTABMAP_PARAM(Optimizer, LandmarksIgnored, bool, false, "Ignore landmark constraints while optimizing. Currently only g2o and gtsam optimization supports this.");
RTABMAP_PARAM(Optimizer, GravitySigma, float, 0.0, uFormat("Gravity sigma value (>=0, typically between 0.1 and 0.3). Optimization is done while preserving gravity orientation of the poses. This should be used only with visual/lidar inertial odometry approaches, for which we assume that all odometry poses are aligned with gravity. Set to 0 to disable gravity constraints. Currently supported only with GTSAM optimization strategy (see %s).", kOptimizerStrategy().c_str()));
RTABMAP_PARAM(Optimizer, GravitySigma, float, 0.0, uFormat("Gravity sigma value (>=0, typically between 0.1 and 0.3). Optimization is done while preserving gravity orientation of the poses. This should be used only with visual/lidar inertial odometry approaches, for which we assume that all odometry poses are aligned with gravity. Set to 0 to disable gravity constraints. Currently supported only with g2o and GTSAM optimization strategies (see %s).", kOptimizerStrategy().c_str()));
#ifdef RTABMAP_ORB_SLAM2
RTABMAP_PARAM(g2o, Solver, int, 3, "0=csparse 1=pcg 2=cholmod 3=Eigen");
@@ -436,9 +442,9 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(OdomF2M, ScanSubtractAngle, float, 45, uFormat("[Geometry] Max angle (degrees) used to filter points of a new added scan to local map (when \"%s\">0). 0 means any angle.", kOdomF2MScanSubtractRadius().c_str()).c_str());
RTABMAP_PARAM(OdomF2M, ValidDepthRatio, float, 0.75, "If a new frame has points without valid depth, they are added to local feature map only if points with valid depth on total points is over this ratio. Setting to 1 means no points without valid depth are added to local feature map.");
#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM2)
RTABMAP_PARAM(OdomF2M, BundleAdjustment, int, 1, "Local bundle adjustment: 0=disabled, 1=g2o, 2=cvsba.");
RTABMAP_PARAM(OdomF2M, BundleAdjustment, int, 1, "Local bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres.");
#else
RTABMAP_PARAM(OdomF2M, BundleAdjustment, int, 0, "Local bundle adjustment: 0=disabled, 1=g2o, 2=cvsba.");
RTABMAP_PARAM(OdomF2M, BundleAdjustment, int, 0, "Local bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres.");
#endif
RTABMAP_PARAM(OdomF2M, BundleAdjustmentMaxFrames, int, 10, "Maximum frames used for bundle adjustment (0=inf or all current frames in the local map).");
@@ -599,9 +605,9 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Vis, CorFlowEps, float, 0.01, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
RTABMAP_PARAM(Vis, CorFlowMaxLevel, int, 3, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM2)
RTABMAP_PARAM(Vis, BundleAdjustment, int, 1, "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba.");
RTABMAP_PARAM(Vis, BundleAdjustment, int, 1, "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres.");
#else
RTABMAP_PARAM(Vis, BundleAdjustment, int, 0, "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba.");
RTABMAP_PARAM(Vis, BundleAdjustment, int, 0, "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres.");
#endif
// ICP registration parameters

75
corelib/include/rtabmap/core/optimizer/OptimizerCeres.h Normal file
View File

@@ -0,0 +1,75 @@
/*
Copyright (c) 2010-2019, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef OPTIMIZERCERES_H_
#define OPTIMIZERCERES_H_
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include <rtabmap/core/Optimizer.h>
namespace rtabmap {
class RTABMAP_EXP OptimizerCeres : public Optimizer
{
public:
static bool available();
public:
OptimizerCeres(
int iterations = Parameters::defaultOptimizerIterations(),
bool slam2d = Parameters::defaultRegForce3DoF(),
bool covarianceIgnored = Parameters::defaultOptimizerVarianceIgnored(),
double epsilon = Parameters::defaultOptimizerEpsilon()) :
Optimizer(iterations, slam2d, covarianceIgnored, epsilon) {}
OptimizerCeres(const ParametersMap & parameters) :
Optimizer(parameters) {}
virtual ~OptimizerCeres() {}
virtual Type type() const {return kTypeCeres;}
virtual std::map<int, Transform> optimize(
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & edgeConstraints,
cv::Mat & outputCovariance,
std::list<std::map<int, Transform> > * intermediateGraphes = 0,
double * finalError = 0,
int * iterationsDone = 0);
virtual std::map<int, Transform> optimizeBA(
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & links,
const std::map<int, CameraModel> & models, // in case of stereo, Tx should be set
std::map<int, cv::Point3f> & points3DMap,
const std::map<int, std::map<int, FeatureBA> > & wordReferences, // <ID words, IDs frames + keypoint(x,y,depth)>
std::set<int> * outliers = 0);
};
} /* namespace rtabmap */
#endif /* OPTIMIZERCERES_H_ */