mirror of
https://github.com/introlab/rtabmap.git
synced 2026-10-04 09:07:47 +08:00
Merge branch 'master' of github.com:introlab/rtabmap into auto_repair_max_graph_optimization
This commit is contained in:
matlabbe
@@ -4,7 +4,7 @@
|
||||
},
|
||||
"customizations": {
|
||||
"vscode": {
|
||||
"extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools"]
|
||||
"extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools", "ms-vscode.cpptools-extension-pack"]
|
||||
}
|
||||
},
|
||||
"workspaceMount": "source=${localWorkspaceFolder},target=/home/vscode/rtabmap,type=bind",
|
||||
@@ -14,5 +14,17 @@
|
||||
"terminal.integrated.defaultProfile.linux": "bash"
|
||||
},
|
||||
"remoteUser": "vscode",
|
||||
"runArgs": ["--privileged", "--network=host"]
|
||||
"hostRequirements": {
|
||||
"gpu": "optional"
|
||||
},
|
||||
"runArgs": ["--privileged",
|
||||
"--network=host",
|
||||
"--gpus=all",
|
||||
//"--runtime=nvidia", // uncommment this if rtabmap doesn't show up in nvidia-smi on the host computer
|
||||
"--env=DISPLAY",
|
||||
"--env=QT_X11_NO_MITSHM=1",
|
||||
"--volume=/tmp/.X11-unix:/tmp/.X11-unix"],
|
||||
"containerEnv": {
|
||||
"NVIDIA_VISIBLE_DEVICES": "all"
|
||||
}
|
||||
}
|
||||
|
||||
+1
-1
@@ -931,7 +931,7 @@ IF(NOT (APPLE OR WIN32) AND BUILD_WITH_RPATH_NOT_RUNPATH)
|
||||
ENDIF()
|
||||
|
||||
IF(NOT MSVC)
|
||||
IF(Qt6_FOUND OR (G2O_FOUND AND G2O_CPP11 EQUAL 1) OR TORCH_FOUND OR MRPT_FOUND)
|
||||
IF(Qt6_FOUND OR (G2O_FOUND AND G2O_CPP11 EQUAL 1) OR (GTSAM_FOUND AND GTSAM_VERSION VERSION_GREATER_EQUAL "4.3.0") OR TORCH_FOUND OR MRPT_FOUND)
|
||||
# Qt6 requires c++17
|
||||
include(CheckCXXCompilerFlag)
|
||||
CHECK_CXX_COMPILER_FLAG("-std=c++17" COMPILER_SUPPORTS_CXX17)
|
||||
|
||||
@@ -804,10 +804,6 @@ IF(CUVSLAM_FOUND)
|
||||
ENDIF(CUVSLAM_FOUND)
|
||||
|
||||
IF(GTSAM_FOUND)
|
||||
SET(SRC_FILES
|
||||
${SRC_FILES}
|
||||
optimizer/gtsam/GravityFactor.cpp
|
||||
)
|
||||
SET(LIBRARIES
|
||||
${LIBRARIES}
|
||||
gtsam
|
||||
|
||||
@@ -749,7 +749,7 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
|
||||
data.setStereoCameraModels(combinedStereoModels);
|
||||
}
|
||||
}
|
||||
data.setId(seq);
|
||||
data.setId(s->getWeight()==-1 ? -1 : seq);
|
||||
data.setStamp(s->getStamp());
|
||||
data.setGroundTruth(s->getGroundTruthPose());
|
||||
if(!globalPose.isNull())
|
||||
|
||||
@@ -51,7 +51,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#include <gtsam/nonlinear/NonlinearOptimizer.h>
|
||||
#include <gtsam/nonlinear/Marginals.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
#include "gtsam/GravityFactor.h"
|
||||
#include <gtsam/navigation/AttitudeFactor.h>
|
||||
#include <optimizer/gtsam/XYFactor.h>
|
||||
#include <optimizer/gtsam/XYZFactor.h>
|
||||
#include <gtsam/nonlinear/ISAM2.h>
|
||||
@@ -121,7 +121,7 @@ void OptimizerGTSAM::parseParameters(const ParametersMap & parameters)
|
||||
params.relinearizeThreshold = threshold;
|
||||
params.relinearizeSkip = skip;
|
||||
params.evaluateNonlinearError = true;
|
||||
isam2_ = new ISAM2(params);
|
||||
isam2_ = new gtsam::ISAM2(params);
|
||||
|
||||
addedPoses_.clear();
|
||||
lastAddedConstraints_.clear();
|
||||
@@ -392,7 +392,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
|
||||
{
|
||||
if(id1 < 0 && !isLandmarkWithRotation.at(id1))
|
||||
{
|
||||
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Variances(Vector2(
|
||||
gtsam::noiseModel::Diagonal::shared_ptr model = gtsam::noiseModel::Diagonal::Variances(gtsam::Vector2(
|
||||
1/iter->second.infMatrix().at<double>(0,0),
|
||||
1/iter->second.infMatrix().at<double>(1,1)));
|
||||
graph.add(XYFactor<gtsam::Point2>(id1, gtsam::Point2(iter->second.transform().x(), iter->second.transform().y()), model));
|
||||
@@ -400,7 +400,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
|
||||
}
|
||||
else if (1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) >= 9999.0)
|
||||
{
|
||||
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Variances(Vector2(
|
||||
gtsam::noiseModel::Diagonal::shared_ptr model = gtsam::noiseModel::Diagonal::Variances(gtsam::Vector2(
|
||||
1/iter->second.infMatrix().at<double>(0,0),
|
||||
1/iter->second.infMatrix().at<double>(1,1)));
|
||||
graph.add(XYFactor<gtsam::Pose2>(id1, gtsam::Point2(iter->second.transform().x(), iter->second.transform().y()), model));
|
||||
@@ -431,7 +431,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
|
||||
{
|
||||
if(id1 < 0 && !isLandmarkWithRotation.at(id1))
|
||||
{
|
||||
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions(Vector3(
|
||||
gtsam::noiseModel::Diagonal::shared_ptr model = gtsam::noiseModel::Diagonal::Precisions(gtsam::Vector3(
|
||||
iter->second.infMatrix().at<double>(0,0),
|
||||
iter->second.infMatrix().at<double>(1,1),
|
||||
iter->second.infMatrix().at<double>(2,2)));
|
||||
@@ -442,7 +442,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
|
||||
1 / static_cast<double>(iter->second.infMatrix().at<double>(4,4)) >= 9999.0 ||
|
||||
1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) >= 9999.0)
|
||||
{
|
||||
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions(Vector3(
|
||||
gtsam::noiseModel::Diagonal::shared_ptr model = gtsam::noiseModel::Diagonal::Precisions(gtsam::Vector3(
|
||||
iter->second.infMatrix().at<double>(0,0),
|
||||
iter->second.infMatrix().at<double>(1,1),
|
||||
iter->second.infMatrix().at<double>(2,2)));
|
||||
@@ -471,10 +471,17 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
|
||||
}
|
||||
else if(!isSlam2d() && gravitySigma() > 0 && iter->second.type() == Link::kGravity && newPoses.find(iter->first) != newPoses.end())
|
||||
{
|
||||
Vector3 r = gtsam::Pose3(iter->second.transform().toEigen4d()).rotation().xyz();
|
||||
gtsam::Unit3 nG = gtsam::Rot3::RzRyRx(r.x(), r.y(), 0).rotate(gtsam::Unit3(0,0,-1));
|
||||
gtsam::SharedNoiseModel model = gtsam::noiseModel::Isotropic::Sigmas(gtsam::Vector2(gravitySigma(), gravitySigma()));
|
||||
graph.add(Pose3GravityFactor(iter->first, nG, model, Unit3(0,0,1)));
|
||||
gtsam::Rot3 nRbMeas = gtsam::Pose3(iter->second.transform().toEigen4d()).rotation();
|
||||
gtsam::Unit3 nZ(0,0,1);
|
||||
gtsam::Unit3 bGMeas = nRbMeas.unrotate(nZ);
|
||||
gtsam::SharedNoiseModel model = gtsam::noiseModel::Isotropic::Sigma(2, gravitySigma());
|
||||
#if GTSAM_VERSION_NUMERIC <= 40300
|
||||
// Note: till 40301 is officially released, version 40300 with "4.3a1" would fail here.
|
||||
// Just replace "<=" above by "<" to use AttitudeFactor<Pose3> below.
|
||||
graph.add(gtsam::Pose3AttitudeFactor(iter->first, nZ, model, bGMeas));
|
||||
#else
|
||||
graph.add(gtsam::AttitudeFactor<gtsam::Pose3>(iter->first, nZ, model, bGMeas));
|
||||
#endif
|
||||
lastAddedConstraints_.push_back(ConstraintToFactor(iter->first, iter->first, -1));
|
||||
}
|
||||
}
|
||||
@@ -783,7 +790,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
|
||||
{
|
||||
float x,y,z,roll,pitch,yaw;
|
||||
std::map<int, Transform> tmpPoses;
|
||||
const Values values = isam2_?isam2_->calculateEstimate():optimizer->values();
|
||||
const gtsam::Values values = isam2_?isam2_->calculateEstimate():optimizer->values();
|
||||
#if GTSAM_VERSION_NUMERIC >= 40200
|
||||
for(gtsam::Values::deref_iterator iter=values.begin(); iter!=values.end(); ++iter)
|
||||
#else
|
||||
|
||||
@@ -1,91 +0,0 @@
|
||||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* Author: Mathieu Labbe
|
||||
* This file is a copy of AttitudeFactor.cpp of gtsam library but
|
||||
* with attitudeError() function overridden to ignore yaw errors.
|
||||
* For the noise model, use Sigmas(Vector2(0.1, 10)) (with second sigma high!)
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file GravityFactor.cpp
|
||||
* @author Frank Dellaert
|
||||
* @brief Implementation file for Attitude factor
|
||||
* @date January 28, 2014
|
||||
**/
|
||||
|
||||
#include "GravityFactor.h"
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace rtabmap {
|
||||
|
||||
//***************************************************************************
|
||||
Vector GravityFactor::attitudeError(const Rot3& nRb,
|
||||
OptionalJacobian<2, 3> H) const {
|
||||
if (H) {
|
||||
Matrix23 D_nRef_R;
|
||||
Matrix22 D_e_nRef;
|
||||
Vector3 r = nRb.xyz();
|
||||
Unit3 nRef = Rot3::RzRyRx(r.x(), r.y(), 0).rotate(bRef_, D_nRef_R);
|
||||
Vector e = nZ_.error(nRef, D_e_nRef);
|
||||
(*H) = D_e_nRef * D_nRef_R;
|
||||
//printf("ref=%f %f %f grav=%f %f %f e= %f %f H=%f %f %f, %f %f %f\n",
|
||||
// nRef.point3().x(), nRef.point3().y(), nRef.point3().z(), nZ_.point3().x(), nZ_.point3().y(), nZ_.point3().z(), e(0), e(1),
|
||||
// (*H)(0,0), (*H)(0,1), (*H)(0,2), (*H)(1,0), (*H)(1,1), (*H)(1,2));
|
||||
return e;
|
||||
} else {
|
||||
Vector3 r = nRb.xyz();
|
||||
Unit3 nRef = Rot3::RzRyRx(r.x(), r.y(), 0) * bRef_;
|
||||
Vector e = nZ_.error(nRef);
|
||||
//printf("ref=%f %f %f grav=%f %f %f e= %f %f\n", nRef.point3().x(), nRef.point3().y(), nRef.point3().z(), nZ_.point3().x(), nZ_.point3().y(), nZ_.point3().z(), e(0), e(1));
|
||||
return e;
|
||||
}
|
||||
}
|
||||
|
||||
//***************************************************************************
|
||||
void Rot3GravityFactor::print(const string& s,
|
||||
const KeyFormatter& keyFormatter) const {
|
||||
cout << s << "Rot3GravityFactor on " << keyFormatter(this->key()) << "\n";
|
||||
nZ_.print(" measured direction in nav frame: ");
|
||||
bRef_.print(" reference direction in body frame: ");
|
||||
this->noiseModel_->print(" noise model: ");
|
||||
}
|
||||
|
||||
//***************************************************************************
|
||||
bool Rot3GravityFactor::equals(const NonlinearFactor& expected,
|
||||
double tol) const {
|
||||
const This* e = dynamic_cast<const This*>(&expected);
|
||||
return e != NULL && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol)
|
||||
&& this->bRef_.equals(e->bRef_, tol);
|
||||
}
|
||||
|
||||
//***************************************************************************
|
||||
void Pose3GravityFactor::print(const string& s,
|
||||
const KeyFormatter& keyFormatter) const {
|
||||
cout << s << "Pose3GravityFactor on " << keyFormatter(this->key()) << "\n";
|
||||
nZ_.print(" measured direction in nav frame: ");
|
||||
bRef_.print(" reference direction in body frame: ");
|
||||
this->noiseModel_->print(" noise model: ");
|
||||
}
|
||||
|
||||
//***************************************************************************
|
||||
bool Pose3GravityFactor::equals(const NonlinearFactor& expected,
|
||||
double tol) const {
|
||||
const This* e = dynamic_cast<const This*>(&expected);
|
||||
return e != NULL && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol)
|
||||
&& this->bRef_.equals(e->bRef_, tol);
|
||||
}
|
||||
|
||||
//***************************************************************************
|
||||
|
||||
}/// namespace gtsam
|
||||
@@ -1,271 +0,0 @@
|
||||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* Author: Mathieu Labbe
|
||||
* This file is a copy of AttitudeFactor.h of gtsam library but
|
||||
* with attitudeError() function overridden to ignore yaw errors.
|
||||
* For the noise model, use Sigmas(Vector2(0.1, 10)) (with second sigma high!)
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file Pose3GravityFactor.h
|
||||
* @author Frank Dellaert
|
||||
* @brief Header file for Attitude factor
|
||||
* @date January 28, 2014
|
||||
**/
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
#if GTSAM_VERSION_NUMERIC >= 40300 && defined(GTSAM_WITH_NOISE_MODEL_FACTOR_N)
|
||||
#include <gtsam/nonlinear/NoiseModelFactorN.h>
|
||||
#endif
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/geometry/Unit3.h>
|
||||
|
||||
using namespace gtsam;
|
||||
|
||||
namespace rtabmap {
|
||||
|
||||
/**
|
||||
* Base class for prior on gravity
|
||||
* Example:
|
||||
* - measurement is direction of gravity in navigation frame nG
|
||||
* - reference is direction of z axis in body frame bF
|
||||
* This factor will give zero error if nG is opposite direction of bF
|
||||
* @addtogroup Navigation
|
||||
*/
|
||||
class GravityFactor {
|
||||
|
||||
protected:
|
||||
|
||||
const Unit3 nZ_, bRef_; ///< Position measurement in
|
||||
|
||||
public:
|
||||
|
||||
/** default constructor - only use for serialization */
|
||||
GravityFactor() {
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Constructor
|
||||
* @param nZ measured direction in navigation frame
|
||||
* @param bRef reference direction in body frame (default Z-axis in NED frame, i.e., [0; 0; 1])
|
||||
*/
|
||||
GravityFactor(const Unit3& nZ, const Unit3& bRef = Unit3(0, 0, 1)) :
|
||||
nZ_(nZ), bRef_(bRef) {
|
||||
}
|
||||
|
||||
/** vector of errors */
|
||||
Vector attitudeError(const Rot3& p,
|
||||
#if GTSAM_VERSION_NUMERIC >= 40300
|
||||
OptionalJacobian<2,3> H = {}) const;
|
||||
#else
|
||||
OptionalJacobian<2,3> H = boost::none) const;
|
||||
#endif
|
||||
|
||||
/** Serialization function */
|
||||
#if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_NUMERIC < 40300
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
|
||||
/*ar & boost::serialization::make_nvp("nZ_", const_cast<Unit3&>(nZ_));
|
||||
ar & boost::serialization::make_nvp("bRef_", const_cast<Unit3&>(bRef_));*/
|
||||
}
|
||||
#endif
|
||||
};
|
||||
|
||||
/**
|
||||
* Version of GravityFactor for Rot3
|
||||
* @addtogroup Navigation
|
||||
*/
|
||||
class Rot3GravityFactor: public NoiseModelFactor1<Rot3>, public GravityFactor {
|
||||
|
||||
typedef NoiseModelFactor1<Rot3> Base;
|
||||
|
||||
public:
|
||||
|
||||
/// shorthand for a smart pointer to a factor
|
||||
#if GTSAM_VERSION_NUMERIC >= 40300
|
||||
typedef std::shared_ptr<Rot3GravityFactor> shared_ptr;
|
||||
#else
|
||||
typedef boost::shared_ptr<Rot3GravityFactor> shared_ptr;
|
||||
#endif
|
||||
|
||||
/// Typedef to this class
|
||||
typedef Rot3GravityFactor This;
|
||||
|
||||
/** default constructor - only use for serialization */
|
||||
Rot3GravityFactor() {
|
||||
}
|
||||
|
||||
virtual ~Rot3GravityFactor() {
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Constructor
|
||||
* @param key of the Rot3 variable that will be constrained
|
||||
* @param nZ measured direction in navigation frame (remove yaw before rotating the gravity vector)
|
||||
* @param model Gaussian noise model
|
||||
* @param bRef reference direction in body frame (default Z-axis)
|
||||
*/
|
||||
Rot3GravityFactor(Key key, const Unit3& nZ, const SharedNoiseModel& model,
|
||||
const Unit3& bRef = Unit3(0, 0, 1)) :
|
||||
Base(model, key), GravityFactor(nZ, bRef) {
|
||||
}
|
||||
|
||||
/// @return a deep copy of this factor
|
||||
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
|
||||
#if GTSAM_VERSION_NUMERIC >= 40300
|
||||
return std::static_pointer_cast<gtsam::NonlinearFactor>(
|
||||
#else
|
||||
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
|
||||
#endif
|
||||
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
|
||||
}
|
||||
|
||||
/** print */
|
||||
virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
|
||||
DefaultKeyFormatter) const;
|
||||
|
||||
/** equals */
|
||||
virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
|
||||
|
||||
/** vector of errors */
|
||||
virtual Vector evaluateError(const Rot3& nRb, //
|
||||
#if GTSAM_VERSION_NUMERIC >= 40300
|
||||
OptionalMatrixType H = OptionalNone) const {
|
||||
#else
|
||||
boost::optional<Matrix&> H = boost::none) const {
|
||||
#endif
|
||||
return attitudeError(nRb, H);
|
||||
}
|
||||
Unit3 nZ() const {
|
||||
return nZ_;
|
||||
}
|
||||
Unit3 bRef() const {
|
||||
return bRef_;
|
||||
}
|
||||
|
||||
private:
|
||||
#if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_NUMERIC < 40300
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
|
||||
/*ar & boost::serialization::make_nvp("NoiseModelFactor1",
|
||||
boost::serialization::base_object<Base>(*this));
|
||||
ar & boost::serialization::make_nvp("GravityFactor",
|
||||
boost::serialization::base_object<GravityFactor>(*this));*/
|
||||
}
|
||||
#endif
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
|
||||
|
||||
/**
|
||||
* Version of GravityFactor for Pose3
|
||||
* @addtogroup Navigation
|
||||
*/
|
||||
class Pose3GravityFactor: public NoiseModelFactor1<Pose3>,
|
||||
public GravityFactor {
|
||||
|
||||
typedef NoiseModelFactor1<Pose3> Base;
|
||||
|
||||
public:
|
||||
|
||||
/// shorthand for a smart pointer to a factor
|
||||
#if GTSAM_VERSION_NUMERIC >= 40300
|
||||
typedef std::shared_ptr<Pose3GravityFactor> shared_ptr;
|
||||
#else
|
||||
typedef boost::shared_ptr<Pose3GravityFactor> shared_ptr;
|
||||
#endif
|
||||
/// Typedef to this class
|
||||
typedef Pose3GravityFactor This;
|
||||
|
||||
/** default constructor - only use for serialization */
|
||||
Pose3GravityFactor() {
|
||||
}
|
||||
|
||||
virtual ~Pose3GravityFactor() {
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Constructor
|
||||
* @param key of the Pose3 variable that will be constrained
|
||||
* @param nZ measured direction in navigation frame (remove yaw before rotating the gravity vector)
|
||||
* @param model Gaussian noise model
|
||||
* @param bRef reference direction in body frame (default Z-axis)
|
||||
*/
|
||||
Pose3GravityFactor(Key key, const Unit3& nZ, const SharedNoiseModel& model,
|
||||
const Unit3& bRef = Unit3(0, 0, 1)) :
|
||||
Base(model, key), GravityFactor(nZ, bRef) {
|
||||
}
|
||||
|
||||
/// @return a deep copy of this factor
|
||||
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
|
||||
#if GTSAM_VERSION_NUMERIC >= 40300
|
||||
return std::static_pointer_cast<gtsam::NonlinearFactor>(
|
||||
#else
|
||||
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
|
||||
#endif
|
||||
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
|
||||
}
|
||||
|
||||
/** print */
|
||||
virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
|
||||
DefaultKeyFormatter) const;
|
||||
|
||||
/** equals */
|
||||
virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
|
||||
|
||||
/** vector of errors */
|
||||
virtual Vector evaluateError(const Pose3& nTb, //
|
||||
#if GTSAM_VERSION_NUMERIC >= 40300
|
||||
OptionalMatrixType H = OptionalNone) const {
|
||||
#else
|
||||
boost::optional<Matrix&> H = boost::none) const {
|
||||
#endif
|
||||
Vector e = attitudeError(nTb.rotation(), H);
|
||||
if (H) {
|
||||
Matrix H23 = *H;
|
||||
*H = Matrix::Zero(2,6);
|
||||
H->block<2,3>(0,0) = H23;
|
||||
}
|
||||
return e;
|
||||
}
|
||||
Unit3 nZ() const {
|
||||
return nZ_;
|
||||
}
|
||||
Unit3 bRef() const {
|
||||
return bRef_;
|
||||
}
|
||||
|
||||
private:
|
||||
#if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_NUMERIC < 40300
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
|
||||
/*ar & boost::serialization::make_nvp("NoiseModelFactor1",
|
||||
boost::serialization::base_object<Base>(*this));
|
||||
ar & boost::serialization::make_nvp("GravityFactor",
|
||||
boost::serialization::base_object<GravityFactor>(*this));*/
|
||||
}
|
||||
#endif
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
|
||||
} /// namespace gtsam
|
||||
|
||||
@@ -43,7 +43,7 @@ public:
|
||||
// @param H the optional Jacobian matrix, which use boost optional and has default null pointer
|
||||
gtsam::Vector evaluateError(const VALUE& p,
|
||||
#if GTSAM_VERSION_NUMERIC >= 40300
|
||||
OptionalMatrixType H = OptionalNone) const {
|
||||
gtsam::OptionalMatrixType H = OptionalNone) const {
|
||||
#else
|
||||
boost::optional<gtsam::Matrix&> H = boost::none) const {
|
||||
#endif
|
||||
@@ -59,5 +59,5 @@ public:
|
||||
|
||||
};
|
||||
|
||||
} // namespace gtsamexamples
|
||||
} // namespace rtabmap
|
||||
|
||||
|
||||
@@ -43,7 +43,7 @@ public:
|
||||
// @param H the optional Jacobian matrix, which use boost optional and has default null pointer
|
||||
gtsam::Vector evaluateError(const gtsam::Pose3& p,
|
||||
#if GTSAM_VERSION_NUMERIC >= 40300
|
||||
OptionalMatrixType H = OptionalNone) const {
|
||||
gtsam::OptionalMatrixType H = OptionalNone) const {
|
||||
#else
|
||||
boost::optional<gtsam::Matrix&> H = boost::none) const {
|
||||
#endif
|
||||
@@ -55,7 +55,7 @@ public:
|
||||
}
|
||||
gtsam::Vector evaluateError(const gtsam::Point3& p,
|
||||
#if GTSAM_VERSION_NUMERIC >= 40300
|
||||
OptionalMatrixType H = OptionalNone) const {
|
||||
gtsam::OptionalMatrixType H = OptionalNone) const {
|
||||
#else
|
||||
boost::optional<gtsam::Matrix&> H = boost::none) const {
|
||||
#endif
|
||||
@@ -63,5 +63,5 @@ public:
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace gtsamexamples
|
||||
} // namespace rtabmap
|
||||
|
||||
|
||||
@@ -31,9 +31,9 @@ namespace vertigo {
|
||||
|
||||
gtsam::Vector evaluateError(const VALUE& p1, const VALUE& p2, const SwitchVariableLinear& s,
|
||||
#if GTSAM_VERSION_NUMERIC >= 40300
|
||||
OptionalMatrixType H1 = OptionalNone,
|
||||
OptionalMatrixType H2 = OptionalNone,
|
||||
OptionalMatrixType H3 = OptionalNone) const
|
||||
gtsam::OptionalMatrixType H1 = OptionalNone,
|
||||
gtsam::OptionalMatrixType H2 = OptionalNone,
|
||||
gtsam::OptionalMatrixType H3 = OptionalNone) const
|
||||
#else
|
||||
boost::optional<gtsam::Matrix&> H1 = boost::none,
|
||||
boost::optional<gtsam::Matrix&> H2 = boost::none,
|
||||
@@ -71,9 +71,9 @@ namespace vertigo {
|
||||
|
||||
gtsam::Vector evaluateError(const VALUE& p1, const VALUE& p2, const SwitchVariableSigmoid& s,
|
||||
#if GTSAM_VERSION_NUMERIC >= 40300
|
||||
OptionalMatrixType H1 = OptionalNone,
|
||||
OptionalMatrixType H2 = OptionalNone,
|
||||
OptionalMatrixType H3 = OptionalNone) const
|
||||
gtsam::OptionalMatrixType H1 = OptionalNone,
|
||||
gtsam::OptionalMatrixType H2 = OptionalNone,
|
||||
gtsam::OptionalMatrixType H3 = OptionalNone) const
|
||||
#else
|
||||
boost::optional<gtsam::Matrix&> H1 = boost::none,
|
||||
boost::optional<gtsam::Matrix&> H2 = boost::none,
|
||||
|
||||
@@ -13,6 +13,7 @@
|
||||
// DerivedValue2.h removed from gtsam repo (Dec 2018): https://github.com/borglab/gtsam/commit/e550f4f2aec423cb3f2791b81cb5858b8826ebac
|
||||
#include "DerivedValue.h"
|
||||
#include <gtsam/base/Lie.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
|
||||
namespace vertigo {
|
||||
|
||||
@@ -77,8 +78,8 @@ namespace vertigo {
|
||||
/** between operation */
|
||||
inline SwitchVariableLinear between(const SwitchVariableLinear& l2,
|
||||
#if GTSAM_VERSION_NUMERIC >= 40300
|
||||
OptionalMatrixType H1=OptionalNone,
|
||||
OptionalMatrixType H2=OptionalNone) const {
|
||||
gtsam::OptionalMatrixType H1=OptionalNone,
|
||||
gtsam::OptionalMatrixType H2=OptionalNone) const {
|
||||
#else
|
||||
boost::optional<gtsam::Matrix&> H1=boost::none,
|
||||
boost::optional<gtsam::Matrix&> H2=boost::none) const {
|
||||
|
||||
@@ -13,6 +13,7 @@
|
||||
// DerivedValue.h removed from gtsam repo (Dec 2018): https://github.com/borglab/gtsam/commit/e550f4f2aec423cb3f2791b81cb5858b8826ebac
|
||||
#include "DerivedValue.h"
|
||||
#include <gtsam/base/Lie.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
|
||||
namespace vertigo {
|
||||
|
||||
@@ -77,8 +78,8 @@ namespace vertigo {
|
||||
/** between operation */
|
||||
inline SwitchVariableSigmoid between(const SwitchVariableSigmoid& l2,
|
||||
#if GTSAM_VERSION_NUMERIC >= 40300
|
||||
OptionalMatrixType H1=OptionalNone,
|
||||
OptionalMatrixType H2=OptionalNone) const {
|
||||
gtsam::OptionalMatrixType H1=OptionalNone,
|
||||
gtsam::OptionalMatrixType H2=OptionalNone) const {
|
||||
#else
|
||||
boost::optional<gtsam::Matrix&> H1=boost::none,
|
||||
boost::optional<gtsam::Matrix&> H2=boost::none) const {
|
||||
|
||||
@@ -27441,7 +27441,7 @@ Lower the ratio -> higher the precision.</string>
|
||||
<number>2</number>
|
||||
</property>
|
||||
<property name="minimum">
|
||||
<double>0.010000000000000</double>
|
||||
<double>0.000000000000000</double>
|
||||
</property>
|
||||
<property name="maximum">
|
||||
<double>100.000000000000000</double>
|
||||
|
||||
@@ -37,6 +37,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#include <rtabmap/utilite/UTimer.h>
|
||||
#include <rtabmap/utilite/UFile.h>
|
||||
#include <rtabmap/utilite/UStl.h>
|
||||
#ifdef RTABMAP_PYTHON
|
||||
#include <rtabmap/core/PythonInterface.h>
|
||||
#endif
|
||||
#include <pcl/filters/filter.h>
|
||||
#include <pcl/io/ply_io.h>
|
||||
#include <pcl/io/obj_io.h>
|
||||
@@ -115,6 +118,10 @@ int main(int argc, char * argv[])
|
||||
printf(" keep_latest = %s\n", keepLatest?"true":"false");
|
||||
printf(" keep_linked = %s\n", keepLinked?"true":"false");
|
||||
printf(" pre_cleanup = %s\n", preCleanup?"true":"false");
|
||||
|
||||
#ifdef RTABMAP_PYTHON
|
||||
rtabmap::PythonInterface pythonInterface;
|
||||
#endif
|
||||
|
||||
// Just parse logging options
|
||||
Parameters::parseArguments(argc, argv);
|
||||
|
||||
Reference in New Issue
Block a user