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

Refactoring: Split all graph optimization approaches in multiple files

Browse Source
This commit is contained in:
matlabbe
2015-11-27 11:09:39 -05:00
parent 93326ab1fe
commit 7d35906398
23 changed files with 2373 additions and 1958 deletions
Download Patch File Download Diff File Expand all files Collapse all files

206
corelib/include/rtabmap/core/Graph.h
View File

@@ -33,8 +33,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <map>
#include <list>
#include <rtabmap/core/Link.h>
#include <rtabmap/core/Parameters.h>
#include <rtabmap/core/Signature.h>
namespace rtabmap {
class Memory;
@@ -42,213 +40,17 @@ class Memory;
namespace graph {
////////////////////////////////////////////
// Graph optimizers
// Graph utilities
////////////////////////////////////////////
class RTABMAP_EXP Optimizer
{
public:
enum Type {
kTypeUndef = -1,
kTypeTORO = 0,
kTypeG2O = 1,
kTypeGTSAM = 2,
kTypeCVSBA = 3
};
static Optimizer * create(const ParametersMap & parameters);
static Optimizer * create(Optimizer::Type & type, const ParametersMap & parameters = ParametersMap());
// Get connected poses and constraints from a set of links
static void getConnectedGraph(
int fromId,
const std::map<int, Transform> & posesIn,
const std::multimap<int, Link> & linksIn, // only one link between two poses
std::map<int, Transform> & posesOut,
std::multimap<int, Link> & linksOut,
int depth = 0);
public:
virtual ~Optimizer() {}
virtual Type type() const = 0;
int iterations() const {return iterations_;}
bool isSlam2d() const {return slam2d_;}
bool isCovarianceIgnored() const {return covarianceIgnored_;}
double epsilon() const {return epsilon_;}
bool isRobust() const {return robust_;}
// inherited classes should implement one of these methods
virtual std::map<int, Transform> optimize(
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & constraints,
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, Signature> & signatures);
virtual void parseParameters(const ParametersMap & parameters);
protected:
Optimizer(
int iterations = Parameters::defaultRGBDOptimizeIterations(),
bool slam2d = Parameters::defaultRGBDOptimizeSlam2D(),
bool covarianceIgnored = Parameters::defaultRGBDOptimizeVarianceIgnored(),
double epsilon = Parameters::defaultRGBDOptimizeEpsilon(),
bool robust = Parameters::defaultRGBDOptimizeRobust());
Optimizer(const ParametersMap & parameters);
private:
int iterations_;
bool slam2d_;
bool covarianceIgnored_;
double epsilon_;
bool robust_;
};
class RTABMAP_EXP TOROOptimizer : public Optimizer
{
public:
static bool saveGraph(
const std::string & fileName,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & edgeConstraints);
static bool loadGraph(
const std::string & fileName,
std::map<int, Transform> & poses,
std::multimap<int, Link> & edgeConstraints);
public:
TOROOptimizer(
int iterations = Parameters::defaultRGBDOptimizeIterations(),
bool slam2d = Parameters::defaultRGBDOptimizeSlam2D(),
bool covarianceIgnored = Parameters::defaultRGBDOptimizeVarianceIgnored(),
double epsilon = Parameters::defaultRGBDOptimizeEpsilon()) :
Optimizer(iterations, slam2d, covarianceIgnored, epsilon) {}
TOROOptimizer(const ParametersMap & parameters) :
Optimizer(parameters) {}
virtual ~TOROOptimizer() {}
virtual Type type() const {return kTypeTORO;}
virtual std::map<int, Transform> optimize(
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & edgeConstraints,
std::list<std::map<int, Transform> > * intermediateGraphes = 0,
double * finalError = 0,
int * iterationsDone = 0);
};
class RTABMAP_EXP G2OOptimizer : public Optimizer
{
public:
static bool available();
static bool saveGraph(
const std::string & fileName,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & edgeConstraints,
bool useRobustConstraints = false);
public:
G2OOptimizer(
int iterations = Parameters::defaultRGBDOptimizeIterations(),
bool slam2d = Parameters::defaultRGBDOptimizeSlam2D(),
bool covarianceIgnored = Parameters::defaultRGBDOptimizeVarianceIgnored(),
double epsilon = Parameters::defaultRGBDOptimizeEpsilon(),
bool robust = Parameters::defaultRGBDOptimizeRobust()) :
Optimizer(iterations, slam2d, covarianceIgnored, epsilon, robust) {}
G2OOptimizer(const ParametersMap & parameters) :
Optimizer(parameters) {}
virtual ~G2OOptimizer() {}
virtual Type type() const {return kTypeG2O;}
virtual std::map<int, Transform> optimize(
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & edgeConstraints,
std::list<std::map<int, Transform> > * intermediateGraphes = 0,
double * finalError = 0,
int * iterationsDone = 0);
};
class RTABMAP_EXP GTSAMOptimizer : public Optimizer
{
public:
static bool available();
public:
GTSAMOptimizer(
int iterations = Parameters::defaultRGBDOptimizeIterations(),
bool slam2d = Parameters::defaultRGBDOptimizeSlam2D(),
bool covarianceIgnored = Parameters::defaultRGBDOptimizeVarianceIgnored(),
double epsilon = Parameters::defaultRGBDOptimizeEpsilon(),
bool robust = Parameters::defaultRGBDOptimizeRobust()) :
Optimizer(iterations, slam2d, covarianceIgnored, epsilon, robust) {}
GTSAMOptimizer(const ParametersMap & parameters) :
Optimizer(parameters) {}
virtual ~GTSAMOptimizer() {}
virtual Type type() const {return kTypeGTSAM;}
virtual std::map<int, Transform> optimize(
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & edgeConstraints,
std::list<std::map<int, Transform> > * intermediateGraphes = 0,
double * finalError = 0,
int * iterationsDone = 0);
};
class RTABMAP_EXP CVSBAOptimizer : public Optimizer
{
public:
static bool available();
public:
CVSBAOptimizer(int iterations = 100, bool slam2d = false, bool covarianceIgnored = false) :
Optimizer(iterations, slam2d, covarianceIgnored),
inlierDistance_(0.02),
minInliers_(10){}
CVSBAOptimizer(const ParametersMap & parameters) :
Optimizer(parameters),
inlierDistance_(0.02),
minInliers_(10){}
virtual ~CVSBAOptimizer() {}
virtual Type type() const {return kTypeCVSBA;}
void setInlierDistance(float inlierDistance) {inlierDistance_ = inlierDistance;}
void setMinInliers(int minInliers) {minInliers_ = minInliers;}
virtual std::map<int, Transform> optimizeBA(
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & links,
const std::map<int, Signature> & signatures);
private:
float inlierDistance_;
float minInliers_;
};
bool RTABMAP_EXP exportPoses(
const std::string & filePath,
int format, // 0=Raw (*.txt), 1=RGBD-SLAM (*.txt), 2=KITTI (*.txt), 3=TORO (*.graph), 4=g2o (*.g2o)
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & constraints, // required for formats 3 and 4
const std::map<int, double> & stamps); // required for format 1
const std::multimap<int, Link> & constraints = std::multimap<int, Link>(), // required for formats 3 and 4
const std::map<int, double> & stamps = std::map<int, double>(), // required for format 1
bool g2oRobust = false); // optional for format 4
////////////////////////////////////////////
// Graph utilities
////////////////////////////////////////////
std::multimap<int, Link>::iterator RTABMAP_EXP findLink(
std::multimap<int, Link> & links,
int from,

112
corelib/include/rtabmap/core/Optimizer.h Normal file
View File

@@ -0,0 +1,112 @@
/*
Copyright (c) 2010-2014, 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 OPTIMIZER_H_
#define OPTIMIZER_H_
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include <map>
#include <list>
#include <rtabmap/core/Link.h>
#include <rtabmap/core/Parameters.h>
#include <rtabmap/core/Signature.h>
namespace rtabmap {
////////////////////////////////////////////
// Graph optimizers
////////////////////////////////////////////
class RTABMAP_EXP Optimizer
{
public:
enum Type {
kTypeUndef = -1,
kTypeTORO = 0,
kTypeG2O = 1,
kTypeGTSAM = 2,
kTypeCVSBA = 3
};
static bool isAvailable(Optimizer::Type type);
static Optimizer * create(const ParametersMap & parameters);
static Optimizer * create(Optimizer::Type & type, const ParametersMap & parameters = ParametersMap());
// Get connected poses and constraints from a set of links
static void getConnectedGraph(
int fromId,
const std::map<int, Transform> & posesIn,
const std::multimap<int, Link> & linksIn, // only one link between two poses
std::map<int, Transform> & posesOut,
std::multimap<int, Link> & linksOut,
int depth = 0);
public:
virtual ~Optimizer() {}
virtual Type type() const = 0;
int iterations() const {return iterations_;}
bool isSlam2d() const {return slam2d_;}
bool isCovarianceIgnored() const {return covarianceIgnored_;}
double epsilon() const {return epsilon_;}
bool isRobust() const {return robust_;}
// inherited classes should implement one of these methods
virtual std::map<int, Transform> optimize(
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & constraints,
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, Signature> & signatures);
virtual void parseParameters(const ParametersMap & parameters);
protected:
Optimizer(
int iterations = Parameters::defaultOptimizerIterations(),
bool slam2d = Parameters::defaultOptimizerSlam2D(),
bool covarianceIgnored = Parameters::defaultOptimizerVarianceIgnored(),
double epsilon = Parameters::defaultOptimizerEpsilon(),
bool robust = Parameters::defaultOptimizerRobust());
Optimizer(const ParametersMap & parameters);
private:
int iterations_;
bool slam2d_;
bool covarianceIgnored_;
double epsilon_;
bool robust_;
};
} /* namespace rtabmap */
#endif /* OPTIMIZER_H_ */

73
corelib/include/rtabmap/core/OptimizerCVSBA.h Normal file
View File

@@ -0,0 +1,73 @@
/*
Copyright (c) 2010-2014, 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 OPTIMIZERCVSBA_H_
#define OPTIMIZERCVSBA_H_
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include <rtabmap/core/Optimizer.h>
namespace rtabmap {
class RTABMAP_EXP OptimizerCVSBA : public Optimizer
{
public:
static bool available();
public:
OptimizerCVSBA(
int iterations = Parameters::defaultOptimizerIterations(),
bool slam2d = Parameters::defaultOptimizerSlam2D(),
bool covarianceIgnored = Parameters::defaultOptimizerVarianceIgnored()) :
Optimizer(iterations, slam2d, covarianceIgnored),
inlierDistance_(0.02),
minInliers_(10){}
OptimizerCVSBA(const ParametersMap & parameters) :
Optimizer(parameters),
inlierDistance_(0.02),
minInliers_(10){}
virtual ~OptimizerCVSBA() {}
virtual Type type() const {return kTypeCVSBA;}
void setInlierDistance(float inlierDistance) {inlierDistance_ = inlierDistance;}
void setMinInliers(int minInliers) {minInliers_ = minInliers;}
virtual std::map<int, Transform> optimizeBA(
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & links,
const std::map<int, Signature> & signatures);
private:
float inlierDistance_;
float minInliers_;
};
} /* namespace rtabmap */
#endif /* OPTIMIZERCVSBA_H_ */

72
corelib/include/rtabmap/core/OptimizerG2O.h Normal file
View File

@@ -0,0 +1,72 @@
/*
Copyright (c) 2010-2014, 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 OPTIMIZERG2O_H_
#define OPTIMIZERG2O_H_
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include <rtabmap/core/Optimizer.h>
namespace rtabmap {
class RTABMAP_EXP OptimizerG2O : public Optimizer
{
public:
static bool available();
static bool saveGraph(
const std::string & fileName,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & edgeConstraints,
bool useRobustConstraints = false);
public:
OptimizerG2O(
int iterations = Parameters::defaultOptimizerIterations(),
bool slam2d = Parameters::defaultOptimizerSlam2D(),
bool covarianceIgnored = Parameters::defaultOptimizerVarianceIgnored(),
double epsilon = Parameters::defaultOptimizerEpsilon(),
bool robust = Parameters::defaultOptimizerRobust()) :
Optimizer(iterations, slam2d, covarianceIgnored, epsilon, robust) {}
OptimizerG2O(const ParametersMap & parameters) :
Optimizer(parameters) {}
virtual ~OptimizerG2O() {}
virtual Type type() const {return kTypeG2O;}
virtual std::map<int, Transform> optimize(
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & edgeConstraints,
std::list<std::map<int, Transform> > * intermediateGraphes = 0,
double * finalError = 0,
int * iterationsDone = 0);
};
} /* namespace rtabmap */
#endif /* OPTIMIZERG2O_H_ */

67
corelib/include/rtabmap/core/OptimizerGTSAM.h Normal file
View File

@@ -0,0 +1,67 @@
/*
Copyright (c) 2010-2014, 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 OPTIMIZERGTSAM_H_
#define OPTIMIZERGTSAM_H_
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include <rtabmap/core/Optimizer.h>
namespace rtabmap {
class RTABMAP_EXP OptimizerGTSAM : public Optimizer
{
public:
static bool available();
public:
OptimizerGTSAM(
int iterations = Parameters::defaultOptimizerIterations(),
bool slam2d = Parameters::defaultOptimizerSlam2D(),
bool covarianceIgnored = Parameters::defaultOptimizerVarianceIgnored(),
double epsilon = Parameters::defaultOptimizerEpsilon(),
bool robust = Parameters::defaultOptimizerRobust()) :
Optimizer(iterations, slam2d, covarianceIgnored, epsilon, robust) {}
OptimizerGTSAM(const ParametersMap & parameters) :
Optimizer(parameters) {}
virtual ~OptimizerGTSAM() {}
virtual Type type() const {return kTypeGTSAM;}
virtual std::map<int, Transform> optimize(
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & edgeConstraints,
std::list<std::map<int, Transform> > * intermediateGraphes = 0,
double * finalError = 0,
int * iterationsDone = 0);
};
} /* namespace rtabmap */
#endif /* OPTIMIZERGTSAM_H_ */

72
corelib/include/rtabmap/core/OptimizerTORO.h Normal file
View File

@@ -0,0 +1,72 @@
/*
Copyright (c) 2010-2014, 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 OPTIMIZERTORO_H_
#define OPTIMIZERTORO_H_
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include <rtabmap/core/Optimizer.h>
namespace rtabmap {
class RTABMAP_EXP OptimizerTORO : public Optimizer
{
public:
static bool saveGraph(
const std::string & fileName,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & edgeConstraints);
static bool loadGraph(
const std::string & fileName,
std::map<int, Transform> & poses,
std::multimap<int, Link> & edgeConstraints);
public:
OptimizerTORO(
int iterations = Parameters::defaultOptimizerIterations(),
bool slam2d = Parameters::defaultOptimizerSlam2D(),
bool covarianceIgnored = Parameters::defaultOptimizerVarianceIgnored(),
double epsilon = Parameters::defaultOptimizerEpsilon()) :
Optimizer(iterations, slam2d, covarianceIgnored, epsilon) {}
OptimizerTORO(const ParametersMap & parameters) :
Optimizer(parameters) {}
virtual ~OptimizerTORO() {}
virtual Type type() const {return kTypeTORO;}
virtual std::map<int, Transform> optimize(
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & edgeConstraints,
std::list<std::map<int, Transform> > * intermediateGraphes = 0,
double * finalError = 0,
int * iterationsDone = 0);
};
} /* namespace rtabmap */
#endif /* GRAPH_H_ */

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

@@ -316,12 +316,12 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(RGBD, ProximityPathRawPosesUsed, bool, true, "When comparing to a local path, merge the scan using the odometry poses (with neighbor link optimizations) instead of the ones in the optimized local graph.");
// Graph optimization
RTABMAP_PARAM(RGBD, OptimizeStrategy, int, 0, "Graph optimization strategy: 0=TORO, 1=g2o and 2=GTSAM.");
RTABMAP_PARAM(RGBD, OptimizeIterations, int, 100, "Optimization iterations.");
RTABMAP_PARAM(RGBD, OptimizeSlam2D, bool, false, "If optimization is done only on x,y and theta (3DoF). Otherwise, it is done on full 6DoF poses.");
RTABMAP_PARAM(RGBD, OptimizeVarianceIgnored, 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(RGBD, OptimizeEpsilon, double, 0.0001, "Stop optimizing when the error improvement is less than this value.");
RTABMAP_PARAM(RGBD, OptimizeRobust, bool, true, "Robust graph optimization using Vertigo (only work for g2o and GTSAM optimization strategies).");
RTABMAP_PARAM(Optimizer, Strategy, int, 0, "Graph optimization strategy: 0=TORO, 1=g2o and 2=GTSAM.")
RTABMAP_PARAM(Optimizer, Iterations, int, 100, "Optimization iterations.");
RTABMAP_PARAM(Optimizer, Slam2D, bool, false, "If optimization is done only on x,y and theta (3DoF). Otherwise, it is done on full 6DoF poses.");
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, Epsilon, double, 0.0001, "Stop optimizing when the error improvement is less than this value.");
RTABMAP_PARAM(Optimizer, Robust, bool, true, "Robust graph optimization using Vertigo (only work for g2o and GTSAM optimization strategies).");
// Odometry
RTABMAP_PARAM(Odom, Strategy, int, 0, "0=Bag-of-words 1=Optical Flow");

4
corelib/include/rtabmap/core/Rtabmap.h
View File

@@ -47,9 +47,7 @@ class EpipolarGeometry;
class Memory;
class BayesFilter;
class Signature;
namespace graph {
class Optimizer;
}
class RTABMAP_EXP Rtabmap
{
@@ -216,7 +214,7 @@ private:
// strategies for a type of signature or configuration.
EpipolarGeometry * _epipolarGeometry;
BayesFilter * _bayesFilter;
graph::Optimizer * _graphOptimizer;
Optimizer * _graphOptimizer;
ParametersMap _modifiedParameters;
Memory * _memory;

6
corelib/src/CMakeLists.txt
View File

@@ -44,6 +44,12 @@ SET(SRC_FILES
Compression.cpp
Link.cpp
Optimizer.cpp
OptimizerTORO.cpp
OptimizerG2O.cpp
OptimizerGTSAM.cpp
OptimizerCVSBA.cpp
RegistrationIcp.cpp
RegistrationVis.cpp

1647
corelib/src/Graph.cpp
View File

File diff suppressed because it is too large Load Diff

7
corelib/src/OdometryBOW.cpp
View File

@@ -33,7 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/util3d_registration.h"
#include "rtabmap/core/util3d_correspondences.h"
#include "rtabmap/core/util3d_motion_estimation.h"
#include "rtabmap/core/Graph.h"
#include "rtabmap/core/Optimizer.h"
#include "rtabmap/core/VWDictionary.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UTimer.h"
@@ -137,8 +137,9 @@ OdometryBOW::OdometryBOW(const ParametersMap & parameters) :
if(poses.size())
{
//optimize the graph
graph::TOROOptimizer optimizer;
std::map<int, Transform> optimizedPoses = optimizer.optimize(poses.begin()->first, poses, links);
Optimizer * optimizer = Optimizer::create(parameters);
std::map<int, Transform> optimizedPoses = optimizer->optimize(poses.begin()->first, poses, links);
delete optimizer;
// fill the local map
for(std::map<int, Transform>::iterator posesIter=optimizedPoses.begin();

255
corelib/src/Optimizer.cpp Normal file
View File

@@ -0,0 +1,255 @@
/*
Copyright (c) 2010-2014, 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.
*/
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UMath.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/core/Optimizer.h>
#include <rtabmap/core/Graph.h>
#include <set>
#include <queue>
#include <rtabmap/core/OptimizerTORO.h>
#include <rtabmap/core/OptimizerG2O.h>
#include <rtabmap/core/OptimizerGTSAM.h>
#include <rtabmap/core/OptimizerCVSBA.h>
namespace rtabmap {
bool Optimizer::isAvailable(Optimizer::Type type)
{
if(type == Optimizer::kTypeG2O)
{
return OptimizerG2O::available();
}
else if(type == Optimizer::kTypeGTSAM)
{
return OptimizerGTSAM::available();
}
else if(type == Optimizer::kTypeCVSBA)
{
return OptimizerCVSBA::available();
}
else if(type == Optimizer::kTypeTORO)
{
return true;
}
return false;
}
Optimizer * Optimizer::create(const ParametersMap & parameters)
{
int optimizerTypeInt = Parameters::defaultOptimizerStrategy();
Parameters::parse(parameters, Parameters::kOptimizerStrategy(), optimizerTypeInt);
Optimizer::Type type = (Optimizer::Type)optimizerTypeInt;
if(!OptimizerG2O::available() && type == Optimizer::kTypeG2O)
{
UWARN("g2o optimizer not available. TORO will be used instead.");
type = Optimizer::kTypeTORO;
}
if(!OptimizerGTSAM::available() && type == Optimizer::kTypeGTSAM)
{
UWARN("GTSAM optimizer not available. TORO will be used instead.");
type = Optimizer::kTypeTORO;
}
if(!OptimizerCVSBA::available() && type == Optimizer::kTypeCVSBA)
{
UWARN("CVSBA optimizer not available. TORO will be used instead.");
type = Optimizer::kTypeTORO;
}
Optimizer * optimizer = 0;
switch(type)
{
case Optimizer::kTypeGTSAM:
optimizer = new OptimizerGTSAM(parameters);
break;
case Optimizer::kTypeG2O:
optimizer = new OptimizerG2O(parameters);
break;
case Optimizer::kTypeCVSBA:
optimizer = new OptimizerCVSBA(parameters);
break;
case Optimizer::kTypeTORO:
default:
optimizer = new OptimizerTORO(parameters);
break;
}
return optimizer;
}
Optimizer * Optimizer::create(Optimizer::Type & type, const ParametersMap & parameters)
{
if(!OptimizerG2O::available() && type == Optimizer::kTypeG2O)
{
UWARN("g2o optimizer not available. TORO will be used instead.");
type = Optimizer::kTypeTORO;
}
if(!OptimizerGTSAM::available() && type == Optimizer::kTypeGTSAM)
{
UWARN("GTSAM optimizer not available. TORO will be used instead.");
type = Optimizer::kTypeTORO;
}
Optimizer * optimizer = 0;
switch(type)
{
case Optimizer::kTypeGTSAM:
optimizer = new OptimizerGTSAM(parameters);
break;
case Optimizer::kTypeG2O:
optimizer = new OptimizerG2O(parameters);
break;
case Optimizer::kTypeTORO:
default:
optimizer = new OptimizerTORO(parameters);
type = Optimizer::kTypeTORO;
break;
}
return optimizer;
}
Optimizer::Optimizer(int iterations, bool slam2d, bool covarianceIgnored, double epsilon, bool robust) :
iterations_(iterations),
slam2d_(slam2d),
covarianceIgnored_(covarianceIgnored),
epsilon_(epsilon),
robust_(robust)
{
}
Optimizer::Optimizer(const ParametersMap & parameters) :
iterations_(Parameters::defaultOptimizerIterations()),
slam2d_(Parameters::defaultOptimizerSlam2D()),
covarianceIgnored_(Parameters::defaultOptimizerVarianceIgnored()),
epsilon_(Parameters::defaultOptimizerEpsilon()),
robust_(Parameters::defaultOptimizerRobust())
{
parseParameters(parameters);
}
void Optimizer::parseParameters(const ParametersMap & parameters)
{
Parameters::parse(parameters, Parameters::kOptimizerIterations(), iterations_);
Parameters::parse(parameters, Parameters::kOptimizerVarianceIgnored(), covarianceIgnored_);
Parameters::parse(parameters, Parameters::kOptimizerSlam2D(), slam2d_);
Parameters::parse(parameters, Parameters::kOptimizerEpsilon(), epsilon_);
Parameters::parse(parameters, Parameters::kOptimizerRobust(), robust_);
}
std::map<int, Transform> Optimizer::optimize(
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & constraints,
std::list<std::map<int, Transform> > * intermediateGraphes,
double * finalError,
int * iterationsDone)
{
UERROR("Optimizer %d doesn't implement optimize() method.", (int)this->type());
return std::map<int, Transform>();
}
std::map<int, Transform> Optimizer::optimizeBA(
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & links,
const std::map<int, Signature> & signatures)
{
UERROR("Optimizer %d doesn't implement optimizeBA() method.", (int)this->type());
return std::map<int, Transform>();
}
void Optimizer::getConnectedGraph(
int fromId,
const std::map<int, Transform> & posesIn,
const std::multimap<int, Link> & linksIn,
std::map<int, Transform> & posesOut,
std::multimap<int, Link> & linksOut,
int depth)
{
UASSERT(depth >= 0);
UASSERT(fromId>0);
UASSERT(uContains(posesIn, fromId));
posesOut.clear();
linksOut.clear();
std::set<int> ids;
std::set<int> curentDepth;
std::set<int> nextDepth;
nextDepth.insert(fromId);
int d = 0;
std::multimap<int, int> biLinks;
for(std::multimap<int, Link>::const_iterator iter=linksIn.begin(); iter!=linksIn.end(); ++iter)
{
UASSERT_MSG(graph::findLink(biLinks, iter->second.from(), iter->second.to()) == biLinks.end(),
uFormat("Input links should be unique between two poses (%d->%d).",
iter->second.from(), iter->second.to()).c_str());
biLinks.insert(std::make_pair(iter->second.from(), iter->second.to()));
biLinks.insert(std::make_pair(iter->second.to(), iter->second.from()));
}
while((depth == 0 || d < depth) && nextDepth.size())
{
curentDepth = nextDepth;
nextDepth.clear();
for(std::set<int>::iterator jter = curentDepth.begin(); jter!=curentDepth.end(); ++jter)
{
if(ids.find(*jter) == ids.end())
{
ids.insert(*jter);
posesOut.insert(*posesIn.find(*jter));
for(std::multimap<int, int>::const_iterator iter=biLinks.find(*jter); iter!=biLinks.end() && iter->first==*jter; ++iter)
{
int nextId = iter->second;
if(ids.find(nextId) == ids.end() && uContains(posesIn, nextId))
{
nextDepth.insert(nextId);
std::multimap<int, Link>::const_iterator kter = graph::findLink(linksIn, *jter, nextId);
if(depth == 0 || d < depth-1)
{
linksOut.insert(*kter);
}
else if(curentDepth.find(nextId) != curentDepth.end() ||
ids.find(nextId) != ids.end())
{
linksOut.insert(*kter);
}
}
}
}
}
++d;
}
}
} /* namespace rtabmap */

268
corelib/src/OptimizerCVSBA.cpp Normal file
View File

@@ -0,0 +1,268 @@
/*
Copyright (c) 2010-2014, 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.
*/
#include "rtabmap/core/Graph.h"
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UMath.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UFile.h>
#include <rtabmap/core/Memory.h>
#include <pcl/search/kdtree.h>
#include <pcl/common/eigen.h>
#include <pcl/common/common.h>
#include <set>
#include <rtabmap/core/OptimizerCVSBA.h>
#ifdef WITH_CVSBA
#include <cvsba/cvsba.h>
#include "rtabmap/core/util3d_motion_estimation.h"
#include "rtabmap/core/util3d_transforms.h"
#include "rtabmap/core/util3d_correspondences.h"
#endif
namespace rtabmap {
bool OptimizerCVSBA::available()
{
#ifdef WITH_CVSBA
return true;
#else
return false;
#endif
}
std::map<int, Transform> OptimizerCVSBA::optimizeBA(
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & links,
const std::map<int, Signature> & signatures)
{
#ifdef WITH_CVSBA
// run sba optimization
cvsba::Sba sba;
// change params if desired
cvsba::Sba::Params params ;
params.type = cvsba::Sba::MOTIONSTRUCTURE;
params.iterations = this->iterations();
params.minError = this->epsilon();
params.fixedIntrinsics = 5;
params.fixedDistortion = 5;
params.verbose=ULogger::level() <= ULogger::kInfo;
sba.setParams(params);
std::map<int, Transform> frames = poses;
std::vector<cv::Mat> cameraMatrix(frames.size()); //nframes
std::vector<cv::Mat> R(frames.size()); //nframes
std::vector<cv::Mat> T(frames.size()); //nframes
std::vector<cv::Mat> distCoeffs(frames.size()); //nframes
std::map<int, int> frameIdToIndex;
std::map<int, CameraModel> models;
int oi=0;
for(std::map<int, Transform>::iterator iter=frames.begin(); iter!=frames.end(); )
{
CameraModel model;
if(uContains(signatures, iter->first))
{
if(signatures.at(iter->first).sensorData().cameraModels().size() == 1 && signatures.at(iter->first).sensorData().cameraModels().at(0).isValid())
{
model = signatures.at(iter->first).sensorData().cameraModels()[0];
}
else if(signatures.at(iter->first).sensorData().stereoCameraModel().isValid())
{
model = signatures.at(iter->first).sensorData().stereoCameraModel().left();
}
else
{
UERROR("Missing calibration for node %d", iter->first);
}
}
else
{
UERROR("Did not find node %d in cache", iter->first);
}
if(model.isValid())
{
frameIdToIndex.insert(std::make_pair(iter->first, oi));
cameraMatrix[oi] = model.K();
distCoeffs[oi] = model.D();
Transform t = (iter->second * model.localTransform()).inverse();
R[oi] = (cv::Mat_<double>(3,3) <<
(double)t.r11(), (double)t.r12(), (double)t.r13(),
(double)t.r21(), (double)t.r22(), (double)t.r23(),
(double)t.r31(), (double)t.r32(), (double)t.r33());
T[oi] = (cv::Mat_<double>(1,3) << (double)t.x(), (double)t.y(), (double)t.z());
++oi;
models.insert(std::make_pair(iter->first, model));
UDEBUG("Pose %d = %s", iter->first, t.prettyPrint().c_str());
++iter;
}
else
{
frames.erase(iter++);
}
}
cameraMatrix.resize(oi);
R.resize(oi);
T.resize(oi);
distCoeffs.resize(oi);
std::map<int, pcl::PointXYZ> points3DMap;
std::multimap<int, std::pair<int, cv::Point2f> > wordReferences; // <ID words, IDs frames + keypoint>
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
Link link = iter->second;
if(link.to() < link.from())
{
link = link.inverse();
}
if(uContains(signatures, link.from()) &&
uContains(signatures, link.to()) &&
uContains(frames, link.from()))
{
const Signature & sFrom = signatures.at(link.from());
const Signature & sTo = signatures.at(link.to());
std::vector<int> inliers;
Transform t = util3d::estimateMotion3DTo3D(
uMultimapToMapUnique(sFrom.getWords3()),
uMultimapToMapUnique(sTo.getWords3()),
minInliers_,
inlierDistance_,
100,
10,
0,
0,
&inliers);
if(!t.isNull())
{
Transform pose = frames.at(sFrom.id());
for(unsigned int i=0; i<inliers.size(); ++i)
{
pcl::PointXYZ p = util3d::transformPoint(sFrom.getWords3().lower_bound(inliers[i])->second, pose);
std::map<int, pcl::PointXYZ>::iterator jter = points3DMap.find(inliers[i]);
if(jter == points3DMap.end())
{
points3DMap.insert(std::make_pair(inliers[i], p));
wordReferences.insert(std::make_pair(inliers[i], std::make_pair(sFrom.id(), sFrom.getWords().lower_bound(inliers[i])->second.pt)));
wordReferences.insert(std::make_pair(inliers[i], std::make_pair(sTo.id(), sTo.getWords().lower_bound(inliers[i])->second.pt)));
}
else
{
float dist = uNorm(p.x - jter->second.x, p.y - jter->second.y, p.z - jter->second.z);
if(dist <= inlierDistance_)
{
// in case of loop closure links
wordReferences.insert(std::make_pair(inliers[i], std::make_pair(sFrom.id(), sFrom.getWords().lower_bound(inliers[i])->second.pt)));
wordReferences.insert(std::make_pair(inliers[i], std::make_pair(sTo.id(), sTo.getWords().lower_bound(inliers[i])->second.pt)));
}
}
}
}
else
{
UWARN("Not enough inliers (%d) between %d and %d", inliers.size(), sFrom.id(), sTo.id());
}
}
}
std::list<int> wordReferencesKeys = uUniqueKeys(wordReferences);
UDEBUG("points=%d frames=%d", (int)wordReferencesKeys.size(), (int)frames.size());
std::vector<cv::Point3f> points(wordReferencesKeys.size()); //npoints
std::vector<std::vector<cv::Point2f> > imagePoints(frames.size()); //nframes -> npoints
std::vector<std::vector<int> > visibility(frames.size()); //nframes -> npoints
for(unsigned int i=0; i<frames.size(); ++i)
{
imagePoints[i].resize(wordReferencesKeys.size(), cv::Point2f(std::numeric_limits<float>::quiet_NaN(), std::numeric_limits<float>::quiet_NaN()));
visibility[i].resize(wordReferencesKeys.size(), 0);
}
int i=0;
for(std::list<int>::iterator iter = wordReferencesKeys.begin(); iter!=wordReferencesKeys.end(); ++iter)
{
pcl::PointXYZ & p = points3DMap.at(*iter);
points[i].x = p.x;
points[i].y = p.y;
points[i].z = p.z;
std::multimap<int, std::pair<int, cv::Point2f> >::iterator jter = wordReferences.lower_bound(*iter);
while(jter->first == *iter && jter != wordReferences.end())
{
imagePoints[frameIdToIndex.at(jter->second.first)][i] = jter->second.second;
visibility[frameIdToIndex.at(jter->second.first)][i] = 1;
++jter;
}
++i;
}
// SBA
try
{
sba.run( points, imagePoints, visibility, cameraMatrix, R, T, distCoeffs);
}
catch(cv::Exception & e)
{
UERROR("Running SBA... error! %s", e.what());
return std::map<int, Transform>();
}
//update poses
i=0;
for(std::map<int, Transform>::iterator iter=frames.begin(); iter!=frames.end(); ++iter)
{
Transform t(R[i].at<double>(0,0), R[i].at<double>(0,1), R[i].at<double>(0,2), T[i].at<double>(0),
R[i].at<double>(1,0), R[i].at<double>(1,1), R[i].at<double>(1,2), T[i].at<double>(1),
R[i].at<double>(2,0), R[i].at<double>(2,1), R[i].at<double>(2,2), T[i].at<double>(2));
UDEBUG("New pose %d = %s", iter->first, t.prettyPrint().c_str());
iter->second = (models.at(iter->first).localTransform() * t).inverse();
++i;
}
return frames;
#else
UERROR("RTAB-Map is not built with cvsba!");
return std::map<int, Transform>();
#endif
}
} /* namespace rtabmap */

551
corelib/src/OptimizerG2O.cpp Normal file
View File

@@ -0,0 +1,551 @@
/*
Copyright (c) 2010-2014, 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.
*/
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UMath.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UFile.h>
#include <rtabmap/core/Memory.h>
#include <pcl/search/kdtree.h>
#include <pcl/common/eigen.h>
#include <pcl/common/common.h>
#include <set>
#include <rtabmap/core/OptimizerG2O.h>
#ifdef WITH_G2O
#include "g2o/core/sparse_optimizer.h"
#include "g2o/core/block_solver.h"
#include "g2o/core/factory.h"
#include "g2o/core/optimization_algorithm_factory.h"
#include "g2o/core/optimization_algorithm_gauss_newton.h"
#include "g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/solvers/csparse/linear_solver_csparse.h"
#include "g2o/solvers/cholmod/linear_solver_cholmod.h"
#include "g2o/solvers/pcg/linear_solver_pcg.h"
#include "g2o/types/slam3d/vertex_se3.h"
#include "g2o/types/slam3d/edge_se3.h"
#include "g2o/types/slam2d/vertex_se2.h"
#include "g2o/types/slam2d/edge_se2.h"
typedef g2o::BlockSolver< g2o::BlockSolverTraits<-1, -1> > SlamBlockSolver;
typedef g2o::LinearSolverCSparse<SlamBlockSolver::PoseMatrixType> SlamLinearCSparseSolver;
typedef g2o::LinearSolverCholmod<SlamBlockSolver::PoseMatrixType> SlamLinearCholmodSolver;
typedef g2o::LinearSolverPCG<SlamBlockSolver::PoseMatrixType> SlamLinearPCGSolver;
#include "vertigo/g2o/edge_switchPrior.h"
#include "vertigo/g2o/edge_se2Switchable.h"
#include "vertigo/g2o/edge_se3Switchable.h"
#include "vertigo/g2o/vertex_switchLinear.h"
#endif // end WITH_G2O
namespace rtabmap {
bool OptimizerG2O::available()
{
#ifdef WITH_G2O
return true;
#else
return false;
#endif
}
std::map<int, Transform> OptimizerG2O::optimize(
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & edgeConstraints,
std::list<std::map<int, Transform> > * intermediateGraphes,
double * finalError,
int * iterationsDone)
{
std::map<int, Transform> optimizedPoses;
#ifdef WITH_G2O
UDEBUG("Optimizing graph...");
optimizedPoses.clear();
if(edgeConstraints.size()>=1 && poses.size()>=2 && iterations() > 0)
{
// Apply g2o optimization
g2o::SparseOptimizer optimizer;
optimizer.setVerbose(ULogger::level()==ULogger::kDebug);
int solverApproach = 0;
int optimizationApproach = 1;
SlamBlockSolver * blockSolver;
if(solverApproach == 1)
{
//pcg
SlamLinearPCGSolver * linearSolver = new SlamLinearPCGSolver();
blockSolver = new SlamBlockSolver(linearSolver);
}
else if(solverApproach == 2)
{
//csparse
SlamLinearCSparseSolver* linearSolver = new SlamLinearCSparseSolver();
linearSolver->setBlockOrdering(false);
blockSolver = new SlamBlockSolver(linearSolver);
}
else
{
//chmold
SlamLinearCholmodSolver * linearSolver = new SlamLinearCholmodSolver();
linearSolver->setBlockOrdering(false);
blockSolver = new SlamBlockSolver(linearSolver);
}
if(optimizationApproach == 1)
{
optimizer.setAlgorithm(new g2o::OptimizationAlgorithmGaussNewton(blockSolver));
}
else
{
optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(blockSolver));
}
UDEBUG("fill poses to g2o...");
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
UASSERT(!iter->second.isNull());
g2o::HyperGraph::Vertex * vertex = 0;
if(isSlam2d())
{
g2o::VertexSE2 * v2 = new g2o::VertexSE2();
v2->setEstimate(g2o::SE2(iter->second.x(), iter->second.y(), iter->second.theta()));
if(iter->first == rootId)
{
v2->setFixed(true);
}
vertex = v2;
}
else
{
g2o::VertexSE3 * v3 = new g2o::VertexSE3();
Eigen::Affine3d a = iter->second.toEigen3d();
Eigen::Isometry3d pose;
pose = a.rotation();
pose.translation() = a.translation();
v3->setEstimate(pose);
if(iter->first == rootId)
{
v3->setFixed(true);
}
vertex = v3;
}
vertex->setId(iter->first);
UASSERT_MSG(optimizer.addVertex(vertex), uFormat("cannot insert vertex %d!?", iter->first).c_str());
}
UDEBUG("fill edges to g2o...");
int vertigoVertexId = poses.rbegin()->first+1;
for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
{
int id1 = iter->first;
int id2 = iter->second.to();
UASSERT(!iter->second.transform().isNull());
g2o::HyperGraph::Edge * edge = 0;
VertexSwitchLinear * v = 0;
if(this->isRobust() &&
iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged)
{
// For loop closure links, add switchable edges
// create new switch variable
// Sunderhauf IROS 2012:
// "Since it is reasonable to initially accept all loop closure constraints,
// a proper and convenient initial value for all switch variables would be
// sij = 1 when using the linear switch function"
v = new VertexSwitchLinear();
v->setEstimate(1.0);
v->setId(vertigoVertexId++);
UASSERT_MSG(optimizer.addVertex(v), uFormat("cannot insert switchable vertex %d!?", v->id()).c_str());
// create switch prior factor
// "If the front-end is not able to assign sound individual values
// for Ξij , it is save to set all Ξij = 1, since this value is close
// to the individual optimal choice of Ξij for a large range of
// outliers."
EdgeSwitchPrior * prior = new EdgeSwitchPrior();
prior->setMeasurement(1.0);
prior->setVertex(0, v);
UASSERT_MSG(optimizer.addEdge(prior), uFormat("cannot insert switchable prior edge %d!?", v->id()).c_str());
}
if(isSlam2d())
{
Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
if(!isCovarianceIgnored())
{
information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
information(0,2) = iter->second.infMatrix().at<double>(0,5); // x-theta
information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
information(1,2) = iter->second.infMatrix().at<double>(1,5); // y-theta
information(2,0) = iter->second.infMatrix().at<double>(5,0); // theta-x
information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y
information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
}
if(this->isRobust() &&
iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged)
{
EdgeSE2Switchable * e = new EdgeSE2Switchable();
g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
g2o::VertexSE2* v2 = (g2o::VertexSE2*)optimizer.vertex(id2);
UASSERT(v1 != 0);
UASSERT(v2 != 0);
e->setVertex(0, v1);
e->setVertex(1, v2);
e->setVertex(2, v);
e->setMeasurement(g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()));
e->setInformation(information);
edge = e;
}
else
{
g2o::EdgeSE2 * e = new g2o::EdgeSE2();
g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
g2o::VertexSE2* v2 = (g2o::VertexSE2*)optimizer.vertex(id2);
UASSERT(v1 != 0);
UASSERT(v2 != 0);
e->setVertex(0, v1);
e->setVertex(1, v2);
e->setMeasurement(g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()));
e->setInformation(information);
edge = e;
}
}
else
{
Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
if(!isCovarianceIgnored())
{
memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
}
Eigen::Affine3d a = iter->second.transform().toEigen3d();
Eigen::Isometry3d constraint;
constraint = a.rotation();
constraint.translation() = a.translation();
if(this->isRobust() &&
iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged)
{
EdgeSE3Switchable * e = new EdgeSE3Switchable();
g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
g2o::VertexSE3* v2 = (g2o::VertexSE3*)optimizer.vertex(id2);
UASSERT(v1 != 0);
UASSERT(v2 != 0);
e->setVertex(0, v1);
e->setVertex(1, v2);
e->setVertex(2, v);
e->setMeasurement(constraint);
e->setInformation(information);
edge = e;
}
else
{
g2o::EdgeSE3 * e = new g2o::EdgeSE3();
g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
g2o::VertexSE3* v2 = (g2o::VertexSE3*)optimizer.vertex(id2);
UASSERT(v1 != 0);
UASSERT(v2 != 0);
e->setVertex(0, v1);
e->setVertex(1, v2);
e->setMeasurement(constraint);
e->setInformation(information);
edge = e;
}
}
if (!optimizer.addEdge(edge))
{
delete edge;
UERROR("Map: Failed adding constraint between %d and %d, skipping", id1, id2);
}
}
UDEBUG("Initial optimization...");
optimizer.initializeOptimization();
UINFO("g2o optimizing begin (max iterations=%d, robust=%d)", iterations(), isRobust()?1:0);
int it = 0;
UTimer timer;
double lastError = 0.0;
if(intermediateGraphes || this->epsilon() > 0.0)
{
for(int i=0; i<iterations(); ++i)
{
if(intermediateGraphes)
{
if(i > 0)
{
std::map<int, Transform> tmpPoses;
if(isSlam2d())
{
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
const g2o::VertexSE2* v = (const g2o::VertexSE2*)optimizer.vertex(iter->first);
if(v)
{
float roll, pitch, yaw;
iter->second.getEulerAngles(roll, pitch, yaw);
Transform t(v->estimate().translation()[0], v->estimate().translation()[1], iter->second.z(), roll, pitch, v->estimate().rotation().angle());
tmpPoses.insert(std::pair<int, Transform>(iter->first, t));
UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
}
else
{
UERROR("Vertex %d not found!?", iter->first);
}
}
}
else
{
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
const g2o::VertexSE3* v = (const g2o::VertexSE3*)optimizer.vertex(iter->first);
if(v)
{
Transform t = Transform::fromEigen3d(v->estimate());
tmpPoses.insert(std::pair<int, Transform>(iter->first, t));
UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
}
else
{
UERROR("Vertex %d not found!?", iter->first);
}
}
}
intermediateGraphes->push_back(tmpPoses);
}
}
it += optimizer.optimize(1);
// early stop condition
optimizer.computeActiveErrors();
double chi2 = optimizer.activeRobustChi2();
UDEBUG("iteration %d: %d nodes, %d edges, chi2: %f", i, (int)optimizer.vertices().size(), (int)optimizer.edges().size(), chi2);
double errorDelta = lastError - chi2;
if(i>0 && errorDelta < this->epsilon())
{
if(errorDelta < 0)
{
UDEBUG("Negative improvement?! Ignore and continue optimizing... (%f < %f)", errorDelta, this->epsilon());
}
else
{
UINFO("Stop optimizing, not enough improvement (%f < %f)", errorDelta, this->epsilon());
break;
}
}
else if(i==0 && chi2 < this->epsilon())
{
UINFO("Stop optimizing, error is already under epsilon (%f < %f)", chi2, this->epsilon());
break;
}
lastError = chi2;
}
}
else
{
it = optimizer.optimize(iterations());
optimizer.computeActiveErrors();
UDEBUG("%d nodes, %d edges, chi2: %f", (int)optimizer.vertices().size(), (int)optimizer.edges().size(), optimizer.activeRobustChi2());
}
if(finalError)
{
*finalError = lastError;
}
if(iterationsDone)
{
*iterationsDone = it;
}
UINFO("g2o optimizing end (%d iterations done, error=%f, time = %f s)", it, optimizer.activeRobustChi2(), timer.ticks());
if(isSlam2d())
{
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
const g2o::VertexSE2* v = (const g2o::VertexSE2*)optimizer.vertex(iter->first);
if(v)
{
float roll, pitch, yaw;
iter->second.getEulerAngles(roll, pitch, yaw);
Transform t(v->estimate().translation()[0], v->estimate().translation()[1], iter->second.z(), roll, pitch, v->estimate().rotation().angle());
optimizedPoses.insert(std::pair<int, Transform>(iter->first, t));
UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
}
else
{
UERROR("Vertex %d not found!?", iter->first);
}
}
}
else
{
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
const g2o::VertexSE3* v = (const g2o::VertexSE3*)optimizer.vertex(iter->first);
if(v)
{
Transform t = Transform::fromEigen3d(v->estimate());
optimizedPoses.insert(std::pair<int, Transform>(iter->first, t));
UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
}
else
{
UERROR("Vertex %d not found!?", iter->first);
}
}
}
optimizer.clear();
g2o::Factory::destroy();
g2o::OptimizationAlgorithmFactory::destroy();
g2o::HyperGraphActionLibrary::destroy();
}
else if(poses.size() == 1 || iterations() <= 0)
{
optimizedPoses = poses;
}
else
{
UWARN("This method should be called at least with 1 pose!");
}
UDEBUG("Optimizing graph...end!");
#else
UERROR("Not built with G2O support!");
#endif
return optimizedPoses;
}
bool OptimizerG2O::saveGraph(
const std::string & fileName,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & edgeConstraints,
bool useRobustConstraints)
{
FILE * file = 0;
#ifdef _MSC_VER
fopen_s(&file, fileName.c_str(), "w");
#else
file = fopen(fileName.c_str(), "w");
#endif
if(file)
{
// VERTEX_SE3 id x y z qw qx qy qz
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
Eigen::Quaternionf q = iter->second.getQuaternionf();
fprintf(file, "VERTEX_SE3:QUAT %d %f %f %f %f %f %f %f\n",
iter->first,
iter->second.x(),
iter->second.y(),
iter->second.z(),
q.x(),
q.y(),
q.z(),
q.w());
}
//EDGE_SE3 observed_vertex_id observing_vertex_id x y z qx qy qz qw inf_11 inf_12 .. inf_16 inf_22 .. inf_66
int virtualVertexId = poses.size()?poses.rbegin()->first+1:0;
for(std::multimap<int, Link>::const_iterator iter = edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
{
std::string prefix = "EDGE_SE3:QUAT";
std::string suffix = "";
if(useRobustConstraints &&
iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged)
{
prefix = "EDGE_SE3_SWITCHABLE";
fprintf(file, "VERTEX_SWITCH %d 1\n", virtualVertexId);
fprintf(file, "EDGE_SWITCH_PRIOR %d 1 1.0\n", virtualVertexId);
suffix = uFormat(" %d", virtualVertexId++);
}
Eigen::Quaternionf q = iter->second.transform().getQuaternionf();
fprintf(file, "%s %d %d%s %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n",
prefix.c_str(),
iter->first,
iter->second.to(),
suffix.c_str(),
iter->second.transform().x(),
iter->second.transform().y(),
iter->second.transform().z(),
q.x(),
q.y(),
q.z(),
q.w(),
iter->second.infMatrix().at<double>(0,0),
iter->second.infMatrix().at<double>(0,1),
iter->second.infMatrix().at<double>(0,2),
iter->second.infMatrix().at<double>(0,3),
iter->second.infMatrix().at<double>(0,4),
iter->second.infMatrix().at<double>(0,5),
iter->second.infMatrix().at<double>(1,1),
iter->second.infMatrix().at<double>(1,2),
iter->second.infMatrix().at<double>(1,3),
iter->second.infMatrix().at<double>(1,4),
iter->second.infMatrix().at<double>(1,5),
iter->second.infMatrix().at<double>(2,2),
iter->second.infMatrix().at<double>(2,3),
iter->second.infMatrix().at<double>(2,4),
iter->second.infMatrix().at<double>(2,5),
iter->second.infMatrix().at<double>(3,3),
iter->second.infMatrix().at<double>(3,4),
iter->second.infMatrix().at<double>(3,5),
iter->second.infMatrix().at<double>(4,4),
iter->second.infMatrix().at<double>(4,5),
iter->second.infMatrix().at<double>(5,5));
}
UINFO("Graph saved to %s", fileName.c_str());
fclose(file);
}
else
{
UERROR("Cannot save to file %s", fileName.c_str());
return false;
}
return true;
}
} /* namespace rtabmap */

322
corelib/src/OptimizerGTSAM.cpp Normal file
View File

@@ -0,0 +1,322 @@
/*
Copyright (c) 2010-2014, 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.
*/
#include "rtabmap/core/Graph.h"
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UMath.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UFile.h>
#include <rtabmap/core/Memory.h>
#include <pcl/search/kdtree.h>
#include <pcl/common/eigen.h>
#include <pcl/common/common.h>
#include <set>
#include <rtabmap/core/OptimizerGTSAM.h>
#ifdef WITH_GTSAM
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/inference/Key.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/DoglegOptimizer.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/Values.h>
#include "vertigo/gtsam/betweenFactorMaxMix.h"
#include "vertigo/gtsam/betweenFactorSwitchable.h"
#include "vertigo/gtsam/switchVariableLinear.h"
#include "vertigo/gtsam/switchVariableSigmoid.h"
#endif // end WITH_GTSAM
namespace rtabmap {
bool OptimizerGTSAM::available()
{
#ifdef WITH_GTSAM
return true;
#else
return false;
#endif
}
std::map<int, Transform> OptimizerGTSAM::optimize(
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & edgeConstraints,
std::list<std::map<int, Transform> > * intermediateGraphes,
double * finalError,
int * iterationsDone)
{
std::map<int, Transform> optimizedPoses;
#ifdef WITH_GTSAM
UDEBUG("Optimizing graph...");
if(edgeConstraints.size()>=1 && poses.size()>=2 && iterations() > 0)
{
gtsam::NonlinearFactorGraph graph;
//prior first pose
UASSERT(uContains(poses, rootId));
const Transform & initialPose = poses.at(rootId);
if(isSlam2d())
{
gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector3(0.01, 0.01, 0.01));
graph.add(gtsam::PriorFactor<gtsam::Pose2>(rootId, gtsam::Pose2(initialPose.x(), initialPose.y(), initialPose.theta()), priorNoise));
}
else
{
gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
graph.add(gtsam::PriorFactor<gtsam::Pose3>(rootId, gtsam::Pose3(initialPose.toEigen4d()), priorNoise));
}
UDEBUG("fill poses to gtsam...");
gtsam::Values initialEstimate;
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
UASSERT(!iter->second.isNull());
if(isSlam2d())
{
initialEstimate.insert(iter->first, gtsam::Pose2(iter->second.x(), iter->second.y(), iter->second.theta()));
}
else
{
initialEstimate.insert(iter->first, gtsam::Pose3(iter->second.toEigen4d()));
}
}
UDEBUG("fill edges to gtsam...");
int switchCounter = poses.rbegin()->first+1;
for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
{
int id1 = iter->first;
int id2 = iter->second.to();
UASSERT(!iter->second.transform().isNull());
if(this->isRobust() &&
iter->second.type()!=Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged)
{
// create new switch variable
// Sunderhauf IROS 2012:
// "Since it is reasonable to initially accept all loop closure constraints,
// 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));
// create switch prior factor
// "If the front-end is not able to assign sound individual values
// for Ξij , it is save to set all Ξij = 1, since this value is close
// 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<vertigo::SwitchVariableLinear> (gtsam::Symbol('s',switchCounter), vertigo::SwitchVariableLinear(prior), switchPriorModel));
}
if(isSlam2d())
{
Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
if(!isCovarianceIgnored())
{
// For some reasons, dividing by 1000 avoids some exceptions (maybe too large numbers on optimization)
information(0,0) = iter->second.infMatrix().at<double>(0,0)/1000.0; // x-x
information(0,1) = iter->second.infMatrix().at<double>(0,1)/1000.0; // x-y
information(0,2) = iter->second.infMatrix().at<double>(0,5)/1000.0; // x-theta
information(1,0) = iter->second.infMatrix().at<double>(1,0)/1000.0; // y-x
information(1,1) = iter->second.infMatrix().at<double>(1,1)/1000.0; // y-y
information(1,2) = iter->second.infMatrix().at<double>(1,5)/1000.0; // y-theta
information(2,0) = iter->second.infMatrix().at<double>(5,0)/1000.0; // theta-x
information(2,1) = iter->second.infMatrix().at<double>(5,1)/1000.0; // theta-y
information(2,2) = iter->second.infMatrix().at<double>(5,5)/1000.0; // theta-theta
}
gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information);
if(this->isRobust() &&
iter->second.type()!=Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged)
{
// create switchable edge factor
graph.add(vertigo::BetweenFactorSwitchableLinear<gtsam::Pose2>(id1, id2, gtsam::Symbol('s', switchCounter++), gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model));
}
else
{
graph.add(gtsam::BetweenFactor<gtsam::Pose2>(id1, id2, gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model));
}
}
else
{
Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
if(!isCovarianceIgnored())
{
memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
// For some reasons, dividing by 1000 avoids some exceptions (maybe too large numbers on optimization)
information = information / 1000.0;
}
gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information);
if(this->isRobust() &&
iter->second.type()!=Link::kNeighbor &&
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));
}
else
{
graph.add(gtsam::BetweenFactor<gtsam::Pose3>(id1, id2, gtsam::Pose3(iter->second.transform().toEigen4d()), model));
}
}
}
UDEBUG("create optimizer");
gtsam::GaussNewtonParams parameters;
parameters.relativeErrorTol = epsilon();
parameters.maxIterations = iterations();
gtsam::GaussNewtonOptimizer optimizer(graph, initialEstimate, parameters);
//gtsam::LevenbergMarquardtParams parametersLev;
//parametersLev.relativeErrorTol = epsilon();
//parametersLev.maxIterations = iterations();
//gtsam::LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parametersLev);
//gtsam::DoglegParams parametersDogleg;
//parametersDogleg.relativeErrorTol = epsilon();
//parametersDogleg.maxIterations = iterations();
//gtsam::DoglegOptimizer optimizer(graph, initialEstimate, parametersDogleg);
UINFO("GTSAM optimizing begin (max iterations=%d, robust=%d)", iterations(), isRobust()?1:0);
UTimer timer;
int it = 0;
double lastError = 0.0;
for(int i=0; i<iterations(); ++i)
{
if(intermediateGraphes && i > 0)
{
std::map<int, Transform> tmpPoses;
for(gtsam::Values::const_iterator iter=optimizer.values().begin(); iter!=optimizer.values().end(); ++iter)
{
if(iter->value.dim() > 1)
{
if(isSlam2d())
{
gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>();
tmpPoses.insert(std::make_pair((int)iter->key, Transform(p.x(), p.y(), p.theta())));
}
else
{
gtsam::Pose3 p = iter->value.cast<gtsam::Pose3>();
tmpPoses.insert(std::make_pair((int)iter->key, Transform::fromEigen4d(p.matrix())));
}
}
}
intermediateGraphes->push_back(tmpPoses);
}
try
{
optimizer.iterate();
++it;
}
catch(gtsam::IndeterminantLinearSystemException & e)
{
UERROR("GTSAM exception catched: %s", e.what());
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(errorDelta < 0)
{
UDEBUG("Negative improvement?! Ignore and continue optimizing... (%f < %f)", errorDelta, this->epsilon());
}
else
{
UINFO("Stop optimizing, not enough improvement (%f < %f)", errorDelta, this->epsilon());
break;
}
}
else if(i==0 && error < this->epsilon())
{
UINFO("Stop optimizing, error is already under epsilon (%f < %f)", error, this->epsilon());
break;
}
lastError = error;
}
if(finalError)
{
*finalError = lastError;
}
if(iterationsDone)
{
*iterationsDone = it;
}
UINFO("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());
for(gtsam::Values::const_iterator iter=optimizer.values().begin(); iter!=optimizer.values().end(); ++iter)
{
if(iter->value.dim() > 1)
{
if(isSlam2d())
{
gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>();
optimizedPoses.insert(std::make_pair((int)iter->key, Transform(p.x(), p.y(), p.theta())));
}
else
{
gtsam::Pose3 p = iter->value.cast<gtsam::Pose3>();
optimizedPoses.insert(std::make_pair((int)iter->key, Transform::fromEigen4d(p.matrix())));
}
}
}
}
else if(poses.size() == 1 || iterations() <= 0)
{
optimizedPoses = poses;
}
else
{
UWARN("This method should be called at least with 1 pose!");
}
UDEBUG("Optimizing graph...end!");
#else
UERROR("Not built with GTSAM support!");
#endif
return optimizedPoses;
}
} /* namespace rtabmap */

479
corelib/src/OptimizerTORO.cpp Normal file
View File

@@ -0,0 +1,479 @@
/*
Copyright (c) 2010-2014, 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.
*/
#include "rtabmap/core/Graph.h"
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UMath.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UFile.h>
#include <rtabmap/core/Memory.h>
#include <pcl/search/kdtree.h>
#include <pcl/common/eigen.h>
#include <pcl/common/common.h>
#include <set>
#include <rtabmap/core/OptimizerTORO.h>
#include "toro3d/treeoptimizer3.hh"
#include "toro3d/treeoptimizer2.hh"
namespace rtabmap {
std::map<int, Transform> OptimizerTORO::optimize(
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & edgeConstraints,
std::list<std::map<int, Transform> > * intermediateGraphes, // contains poses after tree init to last one before the end
double * finalError,
int * iterationsDone)
{
std::map<int, Transform> optimizedPoses;
UDEBUG("Optimizing graph (pose=%d constraints=%d)...", (int)poses.size(), (int)edgeConstraints.size());
if(edgeConstraints.size()>=1 && poses.size()>=2 && iterations() > 0)
{
// Apply TORO optimization
AISNavigation::TreeOptimizer2 pg2;
AISNavigation::TreeOptimizer3 pg3;
pg2.verboseLevel = 0;
pg3.verboseLevel = 0;
UDEBUG("fill poses to TORO...");
if(isSlam2d())
{
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
UASSERT(!iter->second.isNull());
AISNavigation::TreePoseGraph2::Pose p(iter->second.x(), iter->second.y(), iter->second.theta());
AISNavigation::TreePoseGraph2::Vertex* v = pg2.addVertex(iter->first, p);
UASSERT_MSG(v != 0, uFormat("cannot insert vertex %d!?", iter->first).c_str());
}
}
else
{
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
UASSERT(!iter->second.isNull());
float x,y,z, roll,pitch,yaw;
iter->second.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
AISNavigation::TreePoseGraph3::Pose p(x, y, z, roll, pitch, yaw);
AISNavigation::TreePoseGraph3::Vertex* v = pg3.addVertex(iter->first, p);
UASSERT_MSG(v != 0, uFormat("cannot insert vertex %d!?", iter->first).c_str());
v->transformation=AISNavigation::TreePoseGraph3::Transformation(p);
}
}
UDEBUG("fill edges to TORO...");
if(isSlam2d())
{
for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
{
UASSERT(!iter->second.transform().isNull());
AISNavigation::TreePoseGraph2::Pose p(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta());
AISNavigation::TreePoseGraph2::InformationMatrix inf;
//Identity:
if(isCovarianceIgnored())
{
inf.values[0][0] = 1.0; inf.values[0][1] = 0.0; inf.values[0][2] = 0.0; // x
inf.values[1][0] = 0.0; inf.values[1][1] = 1.0; inf.values[1][2] = 0.0; // y
inf.values[2][0] = 0.0; inf.values[2][1] = 0.0; inf.values[2][2] = 1.0; // theta/yaw
}
else
{
inf.values[0][0] = iter->second.infMatrix().at<double>(0,0); // x-x
inf.values[0][1] = iter->second.infMatrix().at<double>(0,1); // x-y
inf.values[0][2] = iter->second.infMatrix().at<double>(0,5); // x-theta
inf.values[1][0] = iter->second.infMatrix().at<double>(1,0); // y-x
inf.values[1][1] = iter->second.infMatrix().at<double>(1,1); // y-y
inf.values[1][2] = iter->second.infMatrix().at<double>(1,5); // y-theta
inf.values[2][0] = iter->second.infMatrix().at<double>(5,0); // theta-x
inf.values[2][1] = iter->second.infMatrix().at<double>(5,1); // theta-y
inf.values[2][2] = iter->second.infMatrix().at<double>(5,5); // theta-theta
}
int id1 = iter->first;
int id2 = iter->second.to();
AISNavigation::TreePoseGraph2::Vertex* v1=pg2.vertex(id1);
AISNavigation::TreePoseGraph2::Vertex* v2=pg2.vertex(id2);
UASSERT(v1 != 0);
UASSERT(v2 != 0);
AISNavigation::TreePoseGraph2::Transformation t(p);
if (!pg2.addEdge(v1, v2, t, inf))
{
UERROR("Map: Edge already exits between nodes %d and %d, skipping", id1, id2);
}
}
}
else
{
for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
{
UASSERT(!iter->second.transform().isNull());
float x,y,z, roll,pitch,yaw;
iter->second.transform().getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
AISNavigation::TreePoseGraph3::Pose p(x, y, z, roll, pitch, yaw);
AISNavigation::TreePoseGraph3::InformationMatrix inf = DMatrix<double>::I(6);
if(!isCovarianceIgnored())
{
memcpy(inf[0], iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
}
int id1 = iter->first;
int id2 = iter->second.to();
AISNavigation::TreePoseGraph3::Vertex* v1=pg3.vertex(id1);
AISNavigation::TreePoseGraph3::Vertex* v2=pg3.vertex(id2);
UASSERT(v1 != 0);
UASSERT(v2 != 0);
AISNavigation::TreePoseGraph3::Transformation t(p);
if (!pg3.addEdge(v1, v2, t, inf))
{
UERROR("Map: Edge already exits between nodes %d and %d, skipping", id1, id2);
}
}
}
UDEBUG("buildMST... root=%d", rootId);
UASSERT(uContains(poses, rootId));
if(isSlam2d())
{
pg2.buildMST(rootId); // pg.buildSimpleTree();
//UDEBUG("initializeOnTree()");
//pg2.initializeOnTree();
UDEBUG("initializeTreeParameters()");
pg2.initializeTreeParameters();
UDEBUG("Building TORO tree... (if a crash happens just after this msg, "
"TORO is not able to find the root of the graph!)");
pg2.initializeOptimization();
}
else
{
pg3.buildMST(rootId); // pg.buildSimpleTree();
//UDEBUG("initializeOnTree()");
//pg3.initializeOnTree();
UDEBUG("initializeTreeParameters()");
pg3.initializeTreeParameters();
UDEBUG("Building TORO tree... (if a crash happens just after this msg, "
"TORO is not able to find the root of the graph!)");
pg3.initializeOptimization();
}
UINFO("Initial error = %f", pg2.error());
UINFO("TORO optimizing begin (iterations=%d)", iterations());
double lastError = 0;
int i=0;
UTimer timer;
for (; i<iterations(); i++)
{
if(intermediateGraphes && i>0)
{
std::map<int, Transform> tmpPoses;
if(isSlam2d())
{
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
AISNavigation::TreePoseGraph2::Vertex* v=pg2.vertex(iter->first);
float roll, pitch, yaw;
iter->second.getEulerAngles(roll, pitch, yaw);
Transform newPose(v->pose.x(), v->pose.y(), iter->second.z(), roll, pitch, v->pose.theta());
UASSERT_MSG(!newPose.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
tmpPoses.insert(std::pair<int, Transform>(iter->first, newPose));
}
}
else
{
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
AISNavigation::TreePoseGraph3::Vertex* v=pg3.vertex(iter->first);
AISNavigation::TreePoseGraph3::Pose pose=v->transformation.toPoseType();
Transform newPose(pose.x(), pose.y(), pose.z(), pose.roll(), pose.pitch(), pose.yaw());
UASSERT_MSG(!newPose.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
tmpPoses.insert(std::pair<int, Transform>(iter->first, newPose));
}
}
intermediateGraphes->push_back(tmpPoses);
}
double error = 0;
if(isSlam2d())
{
pg2.iterate();
// compute the error and dump it
error=pg2.error();
UDEBUG("iteration %d global error=%f error/constraint=%f", i, error, error/pg2.edges.size());
}
else
{
pg3.iterate();
// compute the error and dump it
double mte, mre, are, ate;
error=pg3.error(&mre, &mte, &are, &ate);
UDEBUG("i %d RotGain=%f global error=%f error/constraint=%f",
i, pg3.getRotGain(), error, error/pg3.edges.size());
}
// early stop condition
double errorDelta = lastError - error;
if(i>0 && errorDelta < this->epsilon())
{
if(errorDelta < 0)
{
UDEBUG("Negative improvement?! Ignore and continue optimizing... (%f < %f)", errorDelta, this->epsilon());
}
else
{
UINFO("Stop optimizing, not enough improvement (%f < %f)", errorDelta, this->epsilon());
break;
}
}
else if(i==0 && error < this->epsilon())
{
UINFO("Stop optimizing, error is already under epsilon (%f < %f)", error, this->epsilon());
break;
}
lastError = error;
}
if(finalError)
{
*finalError = lastError;
}
if(iterationsDone)
{
*iterationsDone = i;
}
UINFO("TORO optimizing end (%d iterations done, error=%f, time = %f s)", i, lastError, timer.ticks());
if(isSlam2d())
{
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
AISNavigation::TreePoseGraph2::Vertex* v=pg2.vertex(iter->first);
float roll, pitch, yaw;
iter->second.getEulerAngles(roll, pitch, yaw);
Transform newPose(v->pose.x(), v->pose.y(), iter->second.z(), roll, pitch, v->pose.theta());
UASSERT_MSG(!newPose.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
optimizedPoses.insert(std::pair<int, Transform>(iter->first, newPose));
}
}
else
{
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
AISNavigation::TreePoseGraph3::Vertex* v=pg3.vertex(iter->first);
AISNavigation::TreePoseGraph3::Pose pose=v->transformation.toPoseType();
Transform newPose(pose.x(), pose.y(), pose.z(), pose.roll(), pose.pitch(), pose.yaw());
UASSERT_MSG(!newPose.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
optimizedPoses.insert(std::pair<int, Transform>(iter->first, newPose));
}
}
}
else if(poses.size() == 1 || iterations() <= 0)
{
optimizedPoses = poses;
}
else
{
UWARN("This method should be called at least with 1 pose!");
}
UDEBUG("Optimizing graph...end!");
return optimizedPoses;
}
bool OptimizerTORO::saveGraph(
const std::string & fileName,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & edgeConstraints)
{
FILE * file = 0;
#ifdef _MSC_VER
fopen_s(&file, fileName.c_str(), "w");
#else
file = fopen(fileName.c_str(), "w");
#endif
if(file)
{
// VERTEX3 id x y z phi theta psi
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
float x,y,z, yaw,pitch,roll;
pcl::getTranslationAndEulerAngles(iter->second.toEigen3f(), x,y,z, roll, pitch, yaw);
fprintf(file, "VERTEX3 %d %f %f %f %f %f %f\n",
iter->first,
x,
y,
z,
roll,
pitch,
yaw);
}
//EDGE3 observed_vertex_id observing_vertex_id x y z roll pitch yaw inf_11 inf_12 .. inf_16 inf_22 .. inf_66
for(std::multimap<int, Link>::const_iterator iter = edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
{
float x,y,z, yaw,pitch,roll;
pcl::getTranslationAndEulerAngles(iter->second.transform().toEigen3f(), x,y,z, roll, pitch, yaw);
fprintf(file, "EDGE3 %d %d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n",
iter->first,
iter->second.to(),
x,
y,
z,
roll,
pitch,
yaw,
iter->second.infMatrix().at<double>(0,0),
iter->second.infMatrix().at<double>(0,1),
iter->second.infMatrix().at<double>(0,2),
iter->second.infMatrix().at<double>(0,3),
iter->second.infMatrix().at<double>(0,4),
iter->second.infMatrix().at<double>(0,5),
iter->second.infMatrix().at<double>(1,1),
iter->second.infMatrix().at<double>(1,2),
iter->second.infMatrix().at<double>(1,3),
iter->second.infMatrix().at<double>(1,4),
iter->second.infMatrix().at<double>(1,5),
iter->second.infMatrix().at<double>(2,2),
iter->second.infMatrix().at<double>(2,3),
iter->second.infMatrix().at<double>(2,4),
iter->second.infMatrix().at<double>(2,5),
iter->second.infMatrix().at<double>(3,3),
iter->second.infMatrix().at<double>(3,4),
iter->second.infMatrix().at<double>(3,5),
iter->second.infMatrix().at<double>(4,4),
iter->second.infMatrix().at<double>(4,5),
iter->second.infMatrix().at<double>(5,5));
}
UINFO("Graph saved to %s", fileName.c_str());
fclose(file);
}
else
{
UERROR("Cannot save to file %s", fileName.c_str());
return false;
}
return true;
}
bool OptimizerTORO::loadGraph(
const std::string & fileName,
std::map<int, Transform> & poses,
std::multimap<int, Link> & edgeConstraints)
{
FILE * file = 0;
#ifdef _MSC_VER
fopen_s(&file, fileName.c_str(), "r");
#else
file = fopen(fileName.c_str(), "r");
#endif
if(file)
{
char line[400];
while ( fgets (line , 400 , file) != NULL )
{
std::vector<std::string> strList = uListToVector(uSplit(uReplaceChar(line, '\n', ' '), ' '));
if(strList.size() == 8)
{
//VERTEX3
int id = atoi(strList[1].c_str());
float x = uStr2Float(strList[2]);
float y = uStr2Float(strList[3]);
float z = uStr2Float(strList[4]);
float roll = uStr2Float(strList[5]);
float pitch = uStr2Float(strList[6]);
float yaw = uStr2Float(strList[7]);
Transform pose = Transform::fromEigen3f(pcl::getTransformation(x, y, z, roll, pitch, yaw));
if(poses.find(id) == poses.end())
{
poses.insert(std::make_pair(id, pose));
}
else
{
UFATAL("Pose %d already added", id);
}
}
else if(strList.size() == 30)
{
//EDGE3
int idFrom = atoi(strList[1].c_str());
int idTo = atoi(strList[2].c_str());
float x = uStr2Float(strList[3]);
float y = uStr2Float(strList[4]);
float z = uStr2Float(strList[5]);
float roll = uStr2Float(strList[6]);
float pitch = uStr2Float(strList[7]);
float yaw = uStr2Float(strList[8]);
float infR = uStr2Float(strList[9]);
float infP = uStr2Float(strList[15]);
float infW = uStr2Float(strList[20]);
UASSERT_MSG(infR > 0 && infP > 0 && infW > 0, uFormat("Information matrix should not be null! line=\"%s\"", line).c_str());
float rotVariance = infR<=infP && infR<=infW?infR:infP<=infW?infP:infW; // maximum variance
float infX = uStr2Float(strList[24]);
float infY = uStr2Float(strList[27]);
float infZ = uStr2Float(strList[29]);
UASSERT_MSG(infX > 0 && infY > 0 && infZ > 0, uFormat("Information matrix should not be null! line=\"%s\"", line).c_str());
float transVariance = 1.0f/(infX<=infY && infX<=infZ?infX:infY<=infW?infY:infZ); // maximum variance
UINFO("id=%d rotV=%f transV=%f", idFrom, rotVariance, transVariance);
Transform transform = Transform::fromEigen3f(pcl::getTransformation(x, y, z, roll, pitch, yaw));
if(poses.find(idFrom) != poses.end() && poses.find(idTo) != poses.end())
{
//Link type is unknown
Link link(idFrom, idTo, Link::kUndef, transform, rotVariance, transVariance);
edgeConstraints.insert(std::pair<int, Link>(idFrom, link));
}
else
{
UFATAL("Referred poses from the link not exist!");
}
}
else if(strList.size())
{
UFATAL("Error parsing graph file %s on line \"%s\" (strList.size()=%d)", fileName.c_str(), line, (int)strList.size());
}
}
UINFO("Graph loaded from %s", fileName.c_str());
fclose(file);
}
else
{
UERROR("Cannot open file %s", fileName.c_str());
return false;
}
return true;
}
} /* namespace rtabmap */

9
corelib/src/Parameters.cpp
View File

@@ -194,6 +194,13 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
removedParameters_.insert(std::make_pair("RGBD/LocalLoopDetectionPathFilteringRadius", std::make_pair(true, Parameters::kRGBDProximityPathFilteringRadius())));
removedParameters_.insert(std::make_pair("RGBD/LocalLoopDetectionPathRawPosesUsed", std::make_pair(true, Parameters::kRGBDProximityPathRawPosesUsed())));
removedParameters_.insert(std::make_pair("RGBD/OptimizeStrategy", std::make_pair(true, Parameters::kOptimizerStrategy())));
removedParameters_.insert(std::make_pair("RGBD/OptimizeEpsilon", std::make_pair(true, Parameters::kOptimizerEpsilon())));
removedParameters_.insert(std::make_pair("RGBD/OptimizeIterations", std::make_pair(true, Parameters::kOptimizerIterations())));
removedParameters_.insert(std::make_pair("RGBD/OptimizeRobust", std::make_pair(true, Parameters::kOptimizerRobust())));
removedParameters_.insert(std::make_pair("RGBD/OptimizeSlam2D", std::make_pair(true, Parameters::kOptimizerSlam2D())));
removedParameters_.insert(std::make_pair("RGBD/OptimizeVarianceIgnored", std::make_pair(true, Parameters::kOptimizerVarianceIgnored())));
// before 0.11.0
removedParameters_.insert(std::make_pair("GFTT/MaxCorners", std::make_pair(true, Parameters::kVisMaxFeatures())));
removedParameters_.insert(std::make_pair("LccBow/MaxDepth", std::make_pair(true, Parameters::kVisMaxDepth())));
@@ -201,7 +208,7 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
removedParameters_.insert(std::make_pair("Rtabmap/DetectorStrategy", std::make_pair(true, Parameters::kKpDetectorStrategy())));
removedParameters_.insert(std::make_pair("RGBD/ScanMatchingSize", std::make_pair(true, Parameters::kRGBDNeighborLinkRefining())));
removedParameters_.insert(std::make_pair("RGBD/LocalLoopDetectionRadius", std::make_pair(true, Parameters::kRGBDLocalRadius())));
removedParameters_.insert(std::make_pair("RGBD/ToroIterations", std::make_pair(true, Parameters::kRGBDOptimizeIterations())));
removedParameters_.insert(std::make_pair("RGBD/ToroIterations", std::make_pair(true, Parameters::kOptimizerIterations())));
removedParameters_.insert(std::make_pair("Mem/RehearsedNodesKept", std::make_pair(true, Parameters::kMemNotLinkedNodesKept())));
removedParameters_.insert(std::make_pair("RGBD/LocalLoopDetectionMaxDiffID", std::make_pair(true, Parameters::kRGBDProximityMaxGraphDepth())));
removedParameters_.insert(std::make_pair("RGBD/PlanVirtualLinksMaxDiffID", std::make_pair(false, "")));

15
corelib/src/Rtabmap.cpp
View File

@@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/Rtabmap.h"
#include "rtabmap/core/Version.h"
#include "rtabmap/core/Features2d.h"
#include "rtabmap/core/Optimizer.h"
#include "rtabmap/core/Graph.h"
#include "rtabmap/core/Signature.h"
@@ -416,12 +417,12 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
// If they already exists, we check the parameters if a change is requested
// Graph optimizer
graph::Optimizer::Type optimizerType = graph::Optimizer::kTypeUndef;
if((iter=parameters.find(Parameters::kRGBDOptimizeStrategy())) != parameters.end())
Optimizer::Type optimizerType = Optimizer::kTypeUndef;
if((iter=parameters.find(Parameters::kOptimizerStrategy())) != parameters.end())
{
optimizerType = (graph::Optimizer::Type)std::atoi((*iter).second.c_str());
optimizerType = (Optimizer::Type)std::atoi((*iter).second.c_str());
}
if(optimizerType!=graph::Optimizer::kTypeUndef)
if(optimizerType!=Optimizer::kTypeUndef)
{
UDEBUG("new detector strategy %d", int(optimizerType));
if(_graphOptimizer)
@@ -430,7 +431,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
_graphOptimizer = 0;
}
_graphOptimizer = graph::Optimizer::create(optimizerType, parameters);
_graphOptimizer = Optimizer::create(optimizerType, parameters);
}
else if(_graphOptimizer)
{
@@ -438,8 +439,8 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
}
else
{
optimizerType = (graph::Optimizer::Type)Parameters::defaultRGBDOptimizeStrategy();
_graphOptimizer = graph::Optimizer::create(optimizerType, parameters);
optimizerType = (Optimizer::Type)Parameters::defaultOptimizerStrategy();
_graphOptimizer = Optimizer::create(optimizerType, parameters);
}
if(_memory)