Merge branch 'feature/heterogeneousSmartFactorNoise' into feature/improvementsIncrementalFilter
commit
b6c2ab6664
180
.cproject
180
.cproject
|
@ -57,6 +57,66 @@
|
|||
<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
|
||||
<storageModule moduleId="org.eclipse.cdt.core.language.mapping"/>
|
||||
<storageModule moduleId="org.eclipse.cdt.internal.ui.text.commentOwnerProjectMappings"/>
|
||||
<storageModule moduleId="scannerConfiguration"/>
|
||||
<storageModule moduleId="org.eclipse.cdt.make.core.buildtargets">
|
||||
<buildTargets>
|
||||
<target name="testSmartStereoProjectionPoseFactor.run" path="build/gtsam_unstable/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j4</buildArguments>
|
||||
<buildTarget>testSmartStereoProjectionPoseFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testTriangulation.run" path="build/gtsam/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j4</buildArguments>
|
||||
<buildTarget>testTriangulation.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testStereoCamera.run" path="build/gtsam/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j4</buildArguments>
|
||||
<buildTarget>testStereoCamera.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testSmartProjectionCameraFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j4</buildArguments>
|
||||
<buildTarget>testSmartProjectionCameraFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testSmartProjectionPoseFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j4</buildArguments>
|
||||
<buildTarget>testSmartProjectionPoseFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="check" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j8 check</buildArguments>
|
||||
<buildTarget>check</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="make" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j4</buildArguments>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
</buildTargets>
|
||||
</storageModule>
|
||||
</cconfiguration>
|
||||
<cconfiguration id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.1441575890">
|
||||
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.1441575890" moduleId="org.eclipse.cdt.core.settings" name="Timing">
|
||||
|
@ -113,6 +173,66 @@
|
|||
<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
|
||||
<storageModule moduleId="org.eclipse.cdt.core.language.mapping"/>
|
||||
<storageModule moduleId="org.eclipse.cdt.internal.ui.text.commentOwnerProjectMappings"/>
|
||||
<storageModule moduleId="scannerConfiguration"/>
|
||||
<storageModule moduleId="org.eclipse.cdt.make.core.buildtargets">
|
||||
<buildTargets>
|
||||
<target name="testSmartStereoProjectionPoseFactor.run" path="build/gtsam_unstable/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j4</buildArguments>
|
||||
<buildTarget>testSmartStereoProjectionPoseFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testTriangulation.run" path="build/gtsam/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j4</buildArguments>
|
||||
<buildTarget>testTriangulation.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testStereoCamera.run" path="build/gtsam/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j4</buildArguments>
|
||||
<buildTarget>testStereoCamera.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testSmartProjectionCameraFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j4</buildArguments>
|
||||
<buildTarget>testSmartProjectionCameraFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testSmartProjectionPoseFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j4</buildArguments>
|
||||
<buildTarget>testSmartProjectionPoseFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="check" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j8 check</buildArguments>
|
||||
<buildTarget>check</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="make" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j4</buildArguments>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
</buildTargets>
|
||||
</storageModule>
|
||||
</cconfiguration>
|
||||
<cconfiguration id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.127261216">
|
||||
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.127261216" moduleId="org.eclipse.cdt.core.settings" name="fast">
|
||||
|
@ -173,6 +293,66 @@
|
|||
<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
|
||||
<storageModule moduleId="org.eclipse.cdt.core.language.mapping"/>
|
||||
<storageModule moduleId="org.eclipse.cdt.internal.ui.text.commentOwnerProjectMappings"/>
|
||||
<storageModule moduleId="scannerConfiguration"/>
|
||||
<storageModule moduleId="org.eclipse.cdt.make.core.buildtargets">
|
||||
<buildTargets>
|
||||
<target name="testSmartStereoProjectionPoseFactor.run" path="build/gtsam_unstable/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j4</buildArguments>
|
||||
<buildTarget>testSmartStereoProjectionPoseFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testTriangulation.run" path="build/gtsam/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j4</buildArguments>
|
||||
<buildTarget>testTriangulation.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testStereoCamera.run" path="build/gtsam/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j4</buildArguments>
|
||||
<buildTarget>testStereoCamera.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testSmartProjectionCameraFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j4</buildArguments>
|
||||
<buildTarget>testSmartProjectionCameraFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testSmartProjectionPoseFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j4</buildArguments>
|
||||
<buildTarget>testSmartProjectionPoseFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="check" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j8 check</buildArguments>
|
||||
<buildTarget>check</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="make" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j4</buildArguments>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
</buildTargets>
|
||||
</storageModule>
|
||||
</cconfiguration>
|
||||
</storageModule>
|
||||
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
|
||||
|
|
|
@ -39,7 +39,10 @@ namespace gtsam {
|
|||
|
||||
const Point3 q = leftCamPose_.transform_to(point);
|
||||
|
||||
if ( q.z() <= 0 ) throw StereoCheiralityException();
|
||||
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
|
||||
if (q.z() <= 0)
|
||||
throw StereoCheiralityException();
|
||||
#endif
|
||||
|
||||
// get calibration
|
||||
const Cal3_S2Stereo& K = *K_;
|
||||
|
|
|
@ -104,6 +104,23 @@ TEST( StereoCamera, Dproject)
|
|||
CHECK(assert_equal(expected2,actual2,1e-7));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( StereoCamera, projectCheirality)
|
||||
{
|
||||
// create a Stereo camera
|
||||
Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5));
|
||||
StereoCamera stereoCam(Pose3(), K);
|
||||
|
||||
// point behind the camera
|
||||
Point3 p(0, 0, -5);
|
||||
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
|
||||
CHECK_EXCEPTION(stereoCam.project2(p), StereoCheiralityException);
|
||||
#else // otherwise project should not throw the exception
|
||||
StereoPoint2 expected = StereoPoint2(320, 470, 240);
|
||||
CHECK(assert_equal(expected,stereoCam.project2(p),1e-7));
|
||||
#endif
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( StereoCamera, backproject_case1)
|
||||
{
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
|
@ -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
|
||||
|
|
|
@ -1022,6 +1022,7 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac
|
|||
/* *************************************************************************/
|
||||
TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) {
|
||||
|
||||
// this test considers a condition in which the cheirality constraint is triggered
|
||||
using namespace vanillaPose;
|
||||
|
||||
vector<Key> views;
|
||||
|
@ -1096,8 +1097,14 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor )
|
|||
|
||||
// Since we do not do anything on degenerate instances (ZERO_ON_DEGENERACY)
|
||||
// rotation remains the same as the initial guess, but position is fixed by PoseTranslationPrior
|
||||
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
|
||||
EXPECT(assert_equal(Pose3(values.at<Pose3>(x3).rotation(),
|
||||
Point3(0,0,1)), result.at<Pose3>(x3)));
|
||||
#else
|
||||
// if the check is disabled, no cheirality exception if thrown and the pose converges to the right rotation
|
||||
// with modest accuracy since the configuration is essentially degenerate without the translation due to noise (noise_pose)
|
||||
EXPECT(assert_equal(pose3, result.at<Pose3>(x3),1e-3));
|
||||
#endif
|
||||
}
|
||||
|
||||
/* *************************************************************************/
|
||||
|
|
|
@ -85,8 +85,8 @@ struct QPSParser::MPSGrammar: base_grammar {
|
|||
>> +blank >> double_)[colDouble] >> *blank];
|
||||
quad_l = lexeme[*blank >> word >> +blank >> word >> +blank >> double_
|
||||
>> *blank][addQuadTerm];
|
||||
bound = lexeme[*blank >> word >> +blank >> word >> +blank >> word >> +blank
|
||||
>> double_ >> *blank][addBound];
|
||||
bound = lexeme[(*blank >> word >> +blank >> word >> +blank >> word >> +blank
|
||||
>> double_)[addBound] >> *blank];
|
||||
bound_fr = lexeme[*blank >> word >> +blank >> word >> +blank >> word
|
||||
>> *blank][addBoundFr];
|
||||
rows = lexeme[lit("ROWS") >> *blank >> eol >> +(row >> eol)];
|
||||
|
|
|
@ -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.
|
||||
|
|
Loading…
Reference in New Issue