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

Merge branch 'feature/heterogeneousSmartFactorNoise' into feature/improvementsIncrementalFilter

release/4.3a0
Luca 2016-08-16 19:08:31 -04:00
parent ad984807bf 2a6d372b0a
commit b6c2ab6664
9 changed files with 350 additions and 187 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

180
.cproject
View File

@ -57,6 +57,66 @@
<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/> <storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
<storageModule moduleId="org.eclipse.cdt.core.language.mapping"/> <storageModule moduleId="org.eclipse.cdt.core.language.mapping"/>
<storageModule moduleId="org.eclipse.cdt.internal.ui.text.commentOwnerProjectMappings"/> <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>
<cconfiguration id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.1441575890"> <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"> <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.externalSettings"/>
<storageModule moduleId="org.eclipse.cdt.core.language.mapping"/> <storageModule moduleId="org.eclipse.cdt.core.language.mapping"/>
<storageModule moduleId="org.eclipse.cdt.internal.ui.text.commentOwnerProjectMappings"/> <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>
<cconfiguration id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.127261216"> <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"> <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.externalSettings"/>
<storageModule moduleId="org.eclipse.cdt.core.language.mapping"/> <storageModule moduleId="org.eclipse.cdt.core.language.mapping"/>
<storageModule moduleId="org.eclipse.cdt.internal.ui.text.commentOwnerProjectMappings"/> <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>
</storageModule> </storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0"> <storageModule moduleId="cdtBuildSystem" version="4.0.0">

5
gtsam/geometry/StereoCamera.cpp
View File

@ -39,7 +39,10 @@ namespace gtsam {
const Point3 q = leftCamPose_.transform_to(point); 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 // get calibration
const Cal3_S2Stereo& K = *K_; const Cal3_S2Stereo& K = *K_;

17
gtsam/geometry/tests/testStereoCamera.cpp
View File

@ -104,6 +104,23 @@ TEST( StereoCamera, Dproject)
CHECK(assert_equal(expected2,actual2,1e-7)); 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) TEST( StereoCamera, backproject_case1)
{ {

10
gtsam/slam/SmartFactorBase.h
View File

@ -35,16 +35,6 @@
namespace gtsam { 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 * @brief Base class for smart factors
* This base class has no internal point, but it has a measurement, noise model * 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 #pragma once
#include <gtsam/slam/SmartFactorBase.h> #include <gtsam/slam/SmartFactorBase.h>
#include <gtsam/slam/SmartFactorParams.h>
#include <gtsam/geometry/triangulation.h> #include <gtsam/geometry/triangulation.h>
#include <gtsam/inference/Symbol.h> #include <gtsam/inference/Symbol.h>
@ -31,99 +32,6 @@
namespace gtsam { 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. * SmartProjectionFactor: triangulates point and keeps an estimate of it around.
* This factor operates with monocular cameras, where a camera is expected to * This factor operates with monocular cameras, where a camera is expected to

7
gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
View File

@ -1022,6 +1022,7 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac
/* *************************************************************************/ /* *************************************************************************/
TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) { TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) {
// this test considers a condition in which the cheirality constraint is triggered
using namespace vanillaPose; using namespace vanillaPose;
vector<Key> views; 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) // 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 // 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(), EXPECT(assert_equal(Pose3(values.at<Pose3>(x3).rotation(),
Point3(0,0,1)), result.at<Pose3>(x3))); 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
} }
/* *************************************************************************/ /* *************************************************************************/

4
gtsam_unstable/linear/QPSParser.cpp
View File

@ -85,8 +85,8 @@ struct QPSParser::MPSGrammar: base_grammar {
>> +blank >> double_)[colDouble] >> *blank]; >> +blank >> double_)[colDouble] >> *blank];
quad_l = lexeme[*blank >> word >> +blank >> word >> +blank >> double_ quad_l = lexeme[*blank >> word >> +blank >> word >> +blank >> double_
>> *blank][addQuadTerm]; >> *blank][addQuadTerm];
bound = lexeme[*blank >> word >> +blank >> word >> +blank >> word >> +blank bound = lexeme[(*blank >> word >> +blank >> word >> +blank >> word >> +blank
>> double_ >> *blank][addBound]; >> double_)[addBound] >> *blank];
bound_fr = lexeme[*blank >> word >> +blank >> word >> +blank >> word bound_fr = lexeme[*blank >> word >> +blank >> word >> +blank >> word
>> *blank][addBoundFr]; >> *blank][addBoundFr];
rows = lexeme[lit("ROWS") >> *blank >> eol >> +(row >> eol)]; rows = lexeme[lit("ROWS") >> *blank >> eol >> +(row >> eol)];

86
gtsam_unstable/slam/SmartStereoProjectionFactor.h
View File

@ -21,6 +21,7 @@
#pragma once #pragma once
#include <gtsam/slam/SmartFactorBase.h> #include <gtsam/slam/SmartFactorBase.h>
#include <gtsam/slam/SmartFactorParams.h>
#include <gtsam/geometry/triangulation.h> #include <gtsam/geometry/triangulation.h>
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
@ -35,87 +36,10 @@
namespace gtsam { namespace gtsam {
/* /*
* Parameters for the smart stereo projection factors * Parameters for the smart stereo projection factors (identical to the SmartProjectionParams)
*/ */
struct GTSAM_EXPORT SmartStereoProjectionParams { typedef SmartProjectionParams 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;
}
};
/** /**
* SmartStereoProjectionFactor: triangulates point and keeps an estimate of it around. * SmartStereoProjectionFactor: triangulates point and keeps an estimate of it around.