diff --git a/.cproject b/.cproject
index ed9f7c34a..abc2dfb38 100644
--- a/.cproject
+++ b/.cproject
@@ -57,6 +57,66 @@
+
+
+
+
+ make
+ -j4
+ testSmartStereoProjectionPoseFactor.run
+ true
+ true
+ true
+
+
+ make
+ -j4
+ testTriangulation.run
+ true
+ true
+ true
+
+
+ make
+ -j4
+ testStereoCamera.run
+ true
+ true
+ true
+
+
+ make
+ -j4
+ testSmartProjectionCameraFactor.run
+ true
+ true
+ true
+
+
+ make
+ -j4
+ testSmartProjectionPoseFactor.run
+ true
+ true
+ true
+
+
+ make
+ -j8 check
+ check
+ true
+ false
+ true
+
+
+ make
+ -j4
+ true
+ true
+ true
+
+
+
@@ -113,6 +173,66 @@
+
+
+
+
+ make
+ -j4
+ testSmartStereoProjectionPoseFactor.run
+ true
+ true
+ true
+
+
+ make
+ -j4
+ testTriangulation.run
+ true
+ true
+ true
+
+
+ make
+ -j4
+ testStereoCamera.run
+ true
+ true
+ true
+
+
+ make
+ -j4
+ testSmartProjectionCameraFactor.run
+ true
+ true
+ true
+
+
+ make
+ -j4
+ testSmartProjectionPoseFactor.run
+ true
+ true
+ true
+
+
+ make
+ -j8 check
+ check
+ true
+ false
+ true
+
+
+ make
+ -j4
+ true
+ true
+ true
+
+
+
@@ -173,6 +293,66 @@
+
+
+
+
+ make
+ -j4
+ testSmartStereoProjectionPoseFactor.run
+ true
+ true
+ true
+
+
+ make
+ -j4
+ testTriangulation.run
+ true
+ true
+ true
+
+
+ make
+ -j4
+ testStereoCamera.run
+ true
+ true
+ true
+
+
+ make
+ -j4
+ testSmartProjectionCameraFactor.run
+ true
+ true
+ true
+
+
+ make
+ -j4
+ testSmartProjectionPoseFactor.run
+ true
+ true
+ true
+
+
+ make
+ -j8 check
+ check
+ true
+ false
+ true
+
+
+ make
+ -j4
+ true
+ true
+ true
+
+
+
diff --git a/gtsam/geometry/StereoCamera.cpp b/gtsam/geometry/StereoCamera.cpp
index 5c6646454..c34d4d8f3 100644
--- a/gtsam/geometry/StereoCamera.cpp
+++ b/gtsam/geometry/StereoCamera.cpp
@@ -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_;
diff --git a/gtsam/geometry/tests/testStereoCamera.cpp b/gtsam/geometry/tests/testStereoCamera.cpp
index 497599a6a..bc432cad3 100644
--- a/gtsam/geometry/tests/testStereoCamera.cpp
+++ b/gtsam/geometry/tests/testStereoCamera.cpp
@@ -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)
{
diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h
index 57a53daa3..1453d80a7 100644
--- a/gtsam/slam/SmartFactorBase.h
+++ b/gtsam/slam/SmartFactorBase.h
@@ -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
diff --git a/gtsam/slam/SmartFactorParams.h b/gtsam/slam/SmartFactorParams.h
new file mode 100644
index 000000000..761703f8b
--- /dev/null
+++ b/gtsam/slam/SmartFactorParams.h
@@ -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
+
+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
+ 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
diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h
index b015f1d7d..840ecbc4d 100644
--- a/gtsam/slam/SmartProjectionFactor.h
+++ b/gtsam/slam/SmartProjectionFactor.h
@@ -20,6 +20,7 @@
#pragma once
#include
+#include
#include
#include
@@ -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
- 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
diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
index f0a864fdd..fa1f83a75 100644
--- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
+++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
@@ -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 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(x3).rotation(),
Point3(0,0,1)), result.at(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(x3),1e-3));
+#endif
}
/* *************************************************************************/
diff --git a/gtsam_unstable/linear/QPSParser.cpp b/gtsam_unstable/linear/QPSParser.cpp
index 748c4db38..a6fc072c7 100644
--- a/gtsam_unstable/linear/QPSParser.cpp
+++ b/gtsam_unstable/linear/QPSParser.cpp
@@ -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)];
diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h
index 9b9d3c935..29ae6233b 100644
--- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h
+++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h
@@ -21,6 +21,7 @@
#pragma once
#include
+#include
#include
#include
@@ -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.