12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

created SmartFactorParams class collecting common parameters in smartProjectionFactor and smartStereoProjectFactor classes. This largely reduced copy-paste. SmartStereoProjectionParams is now a typedef

release/4.3a0
Luca 2016-08-14 18:54:00 -04:00
parent edcf94591d
commit c1b00e28b1
4 changed files with 140 additions and 184 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/slam/SmartFactorBase.h
View File

@ -35,16 +35,6 @@
namespace gtsam {
/// Linearization mode: what factor to linearize to
enum LinearizationMode {
HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD
};
/// How to manage degeneracy
enum DegeneracyMode {
IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY
};
/**
* @brief Base class for smart factors
* This base class has no internal point, but it has a measurement, noise model

134
gtsam/slam/SmartFactorParams.h Normal file
View File

@ -0,0 +1,134 @@
/* ----------------------------------------------------------------------------
* 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
* -------------------------------------------------------------------------- */
/**
* @file SmartFactorParams
* @brief Collect common parameters for SmartProjection and SmartStereoProjection factors
* @author Luca Carlone
* @author Zsolt Kira
* @author Frank Dellaert
*/
#pragma once
#include <gtsam/geometry/triangulation.h>
namespace gtsam {
/**
* SmartFactorParams: parameters and (linearization/degeneracy) modes for SmartProjection and SmartStereoProjection factors
*/
/// Linearization mode: what factor to linearize to
enum LinearizationMode {
HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD
};
/// How to manage degeneracy
enum DegeneracyMode {
IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY
};
/*
* Parameters for the smart (stereo) projection factors
*/
struct GTSAM_EXPORT SmartProjectionParams {
LinearizationMode linearizationMode; ///< How to linearize the factor
DegeneracyMode degeneracyMode; ///< How to linearize the factor
/// @name Parameters governing the triangulation
/// @{
TriangulationParameters triangulation;
double retriangulationThreshold; ///< threshold to decide whether to re-triangulate
/// @}
/// @name Parameters governing how triangulation result is treated
/// @{
bool throwCheirality; ///< If true, re-throws Cheirality exceptions (default: false)
bool verboseCheirality; ///< If true, prints text for Cheirality exceptions (default: false)
/// @}
// Constructor
SmartProjectionParams(LinearizationMode linMode = HESSIAN,
DegeneracyMode degMode = IGNORE_DEGENERACY, bool throwCheirality = false,
bool verboseCheirality = false, double retriangulationTh = 1e-5) :
linearizationMode(linMode), degeneracyMode(degMode), retriangulationThreshold(
retriangulationTh), throwCheirality(throwCheirality), verboseCheirality(
verboseCheirality) {
}
virtual ~SmartProjectionParams() {
}
void print(const std::string& str) const {
std::cout << "linearizationMode: " << linearizationMode << "\n";
std::cout << " degeneracyMode: " << degeneracyMode << "\n";
std::cout << triangulation << std::endl;
}
// get class variables
LinearizationMode getLinearizationMode() const {
return linearizationMode;
}
DegeneracyMode getDegeneracyMode() const {
return degeneracyMode;
}
TriangulationParameters getTriangulationParameters() const {
return triangulation;
}
bool getVerboseCheirality() const {
return verboseCheirality;
}
bool getThrowCheirality() const {
return throwCheirality;
}
double getRetriangulationThreshold() const {
return retriangulationThreshold;
}
// set class variables
void setLinearizationMode(LinearizationMode linMode) {
linearizationMode = linMode;
}
void setDegeneracyMode(DegeneracyMode degMode) {
degeneracyMode = degMode;
}
void setRetriangulationThreshold(double retriangulationTh) {
retriangulationThreshold = retriangulationTh;
}
void setRankTolerance(double rankTol) {
triangulation.rankTolerance = rankTol;
}
void setEnableEPI(bool enableEPI) {
triangulation.enableEPI = enableEPI;
}
void setLandmarkDistanceThreshold(double landmarkDistanceThreshold) {
triangulation.landmarkDistanceThreshold = landmarkDistanceThreshold;
}
void setDynamicOutlierRejectionThreshold(double dynOutRejectionThreshold) {
triangulation.dynamicOutlierRejectionThreshold = dynOutRejectionThreshold;
}
private:
/// Serialization function
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(linearizationMode);
ar & BOOST_SERIALIZATION_NVP(degeneracyMode);
ar & BOOST_SERIALIZATION_NVP(triangulation);
ar & BOOST_SERIALIZATION_NVP(retriangulationThreshold);
ar & BOOST_SERIALIZATION_NVP(throwCheirality);
ar & BOOST_SERIALIZATION_NVP(verboseCheirality);
}
};
} // \ namespace gtsam

94
gtsam/slam/SmartProjectionFactor.h
View File

@ -20,6 +20,7 @@
#pragma once
#include <gtsam/slam/SmartFactorBase.h>
#include <gtsam/slam/SmartFactorParams.h>
#include <gtsam/geometry/triangulation.h>
#include <gtsam/inference/Symbol.h>
@ -31,99 +32,6 @@
namespace gtsam {
/*
* Parameters for the smart projection factors
*/
struct GTSAM_EXPORT SmartProjectionParams {
LinearizationMode linearizationMode; ///< How to linearize the factor
DegeneracyMode degeneracyMode; ///< How to linearize the factor
/// @name Parameters governing the triangulation
/// @{
TriangulationParameters triangulation;
double retriangulationThreshold; ///< threshold to decide whether to re-triangulate
/// @}
/// @name Parameters governing how triangulation result is treated
/// @{
bool throwCheirality; ///< If true, re-throws Cheirality exceptions (default: false)
bool verboseCheirality; ///< If true, prints text for Cheirality exceptions (default: false)
/// @}
// Constructor
SmartProjectionParams(LinearizationMode linMode = HESSIAN,
DegeneracyMode degMode = IGNORE_DEGENERACY, bool throwCheirality = false,
bool verboseCheirality = false, double retriangulationTh = 1e-5) :
linearizationMode(linMode), degeneracyMode(degMode), retriangulationThreshold(
retriangulationTh), throwCheirality(throwCheirality), verboseCheirality(
verboseCheirality) {
}
virtual ~SmartProjectionParams() {
}
void print(const std::string& str) const {
std::cout << "linearizationMode: " << linearizationMode << "\n";
std::cout << " degeneracyMode: " << degeneracyMode << "\n";
std::cout << triangulation << std::endl;
}
LinearizationMode getLinearizationMode() const {
return linearizationMode;
}
DegeneracyMode getDegeneracyMode() const {
return degeneracyMode;
}
TriangulationParameters getTriangulationParameters() const {
return triangulation;
}
bool getVerboseCheirality() const {
return verboseCheirality;
}
bool getThrowCheirality() const {
return throwCheirality;
}
double getRetriangulationThreshold() const {
return retriangulationThreshold;
}
void setLinearizationMode(LinearizationMode linMode) {
linearizationMode = linMode;
}
void setRetriangulationThreshold(double retriangulationTh) {
retriangulationThreshold = retriangulationTh;
}
void setDegeneracyMode(DegeneracyMode degMode) {
degeneracyMode = degMode;
}
void setRankTolerance(double rankTol) {
triangulation.rankTolerance = rankTol;
}
void setEnableEPI(bool enableEPI) {
triangulation.enableEPI = enableEPI;
}
void setLandmarkDistanceThreshold(double landmarkDistanceThreshold) {
triangulation.landmarkDistanceThreshold = landmarkDistanceThreshold;
}
void setDynamicOutlierRejectionThreshold(double dynOutRejectionThreshold) {
triangulation.dynamicOutlierRejectionThreshold = dynOutRejectionThreshold;
}
private:
/// Serialization function
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(linearizationMode);
ar & BOOST_SERIALIZATION_NVP(degeneracyMode);
ar & BOOST_SERIALIZATION_NVP(triangulation);
ar & BOOST_SERIALIZATION_NVP(retriangulationThreshold);
ar & BOOST_SERIALIZATION_NVP(throwCheirality);
ar & BOOST_SERIALIZATION_NVP(verboseCheirality);
}
};
/**
* SmartProjectionFactor: triangulates point and keeps an estimate of it around.
* This factor operates with monocular cameras, where a camera is expected to

86
gtsam_unstable/slam/SmartStereoProjectionFactor.h
View File

@ -21,6 +21,7 @@
#pragma once
#include <gtsam/slam/SmartFactorBase.h>
#include <gtsam/slam/SmartFactorParams.h>
#include <gtsam/geometry/triangulation.h>
#include <gtsam/geometry/Pose3.h>
@ -35,87 +36,10 @@
namespace gtsam {
/*
* Parameters for the smart stereo projection factors
*/
struct GTSAM_EXPORT SmartStereoProjectionParams {
LinearizationMode linearizationMode; ///< How to linearize the factor
DegeneracyMode degeneracyMode; ///< How to linearize the factor
/// @name Parameters governing the triangulation
/// @{
TriangulationParameters triangulation;
double retriangulationThreshold; ///< threshold to decide whether to re-triangulate
/// @}
/// @name Parameters governing how triangulation result is treated
/// @{
bool throwCheirality; ///< If true, re-throws Cheirality exceptions (default: false)
bool verboseCheirality; ///< If true, prints text for Cheirality exceptions (default: false)
/// @}
/// Constructor
SmartStereoProjectionParams(LinearizationMode linMode = HESSIAN,
DegeneracyMode degMode = IGNORE_DEGENERACY, bool throwCheirality = false,
bool verboseCheirality = false) :
linearizationMode(linMode), degeneracyMode(degMode), retriangulationThreshold(
1e-5), throwCheirality(throwCheirality), verboseCheirality(
verboseCheirality) {
}
virtual ~SmartStereoProjectionParams() {
}
void print(const std::string& str) const {
std::cout << "linearizationMode: " << linearizationMode << "\n";
std::cout << " degeneracyMode: " << degeneracyMode << "\n";
std::cout << triangulation << std::endl;
}
// get class variables
LinearizationMode getLinearizationMode() const {
return linearizationMode;
}
DegeneracyMode getDegeneracyMode() const {
return degeneracyMode;
}
TriangulationParameters getTriangulationParameters() const {
return triangulation;
}
bool getVerboseCheirality() const {
return verboseCheirality;
}
bool getThrowCheirality() const {
return throwCheirality;
}
double getRetriangulationThreshold() const {
return retriangulationThreshold;
}
// set class variables
void setLinearizationMode(LinearizationMode linMode) {
linearizationMode = linMode;
}
void setDegeneracyMode(DegeneracyMode degMode) {
degeneracyMode = degMode;
}
void setRetriangulationThreshold(double retriangulationTh) {
retriangulationThreshold = retriangulationTh;
}
void setRankTolerance(double rankTol) {
triangulation.rankTolerance = rankTol;
}
void setEnableEPI(bool enableEPI) {
triangulation.enableEPI = enableEPI;
}
void setLandmarkDistanceThreshold(double landmarkDistanceThreshold) {
triangulation.landmarkDistanceThreshold = landmarkDistanceThreshold;
}
void setDynamicOutlierRejectionThreshold(double dynOutRejectionThreshold) {
triangulation.dynamicOutlierRejectionThreshold = dynOutRejectionThreshold;
}
};
/*
* Parameters for the smart stereo projection factors (identical to the SmartProjectionParams)
*/
typedef SmartProjectionParams SmartStereoProjectionParams;
/**
* SmartStereoProjectionFactor: triangulates point and keeps an estimate of it around.