diff --git a/.cproject b/.cproject
index e190d8f65..94e8c3a50 100644
--- a/.cproject
+++ b/.cproject
@@ -1043,6 +1043,14 @@
true
true
+
+ make
+ -j4
+ testPinholeSet.run
+ true
+ true
+ true
+
make
-j2
diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h
index 2192c38b9..179ec9c50 100644
--- a/gtsam/geometry/CameraSet.h
+++ b/gtsam/geometry/CameraSet.h
@@ -78,7 +78,7 @@ public:
}
/// equals
- virtual bool equals(const CameraSet& p, double tol = 1e-9) const {
+ bool equals(const CameraSet& p, double tol = 1e-9) const {
if (this->size() != p.size())
return false;
bool camerasAreEqual = true;
@@ -143,7 +143,7 @@ public:
Vector reprojectionError(const Point3& point, const std::vector& measured,
boost::optional F = boost::none, //
boost::optional E = boost::none) const {
- return ErrorVector(project2(point,F,E), measured);
+ return ErrorVector(project2(point, F, E), measured);
}
/// Calculate vector of re-projection errors, from point at infinity
diff --git a/gtsam/geometry/PinholeSet.h b/gtsam/geometry/PinholeSet.h
new file mode 100644
index 000000000..c950a9150
--- /dev/null
+++ b/gtsam/geometry/PinholeSet.h
@@ -0,0 +1,381 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 PinholeSet.h
+ * @brief A CameraSet of either CalibratedCamera, PinholePose, or PinholeCamera
+ * @author Frank Dellaert
+ * @author Luca Carlone
+ * @author Zsolt Kira
+ */
+
+#pragma once
+
+#include
+#include
+#include
+#include
+
+namespace gtsam {
+
+/**
+ * PinholeSet: triangulates point and keeps an estimate of it around.
+ */
+template
+class PinholeSet: public CameraSet {
+
+private:
+ typedef CameraSet Base;
+ typedef PinholeSet This;
+
+protected:
+
+ // Some triangulation parameters
+ const double rankTolerance_; ///< threshold to decide whether triangulation is degenerate_
+ const double retriangulationThreshold_; ///< threshold to decide whether to re-triangulate
+ mutable std::vector cameraPosesTriangulation_; ///< current triangulation poses
+
+ const bool enableEPI_; ///< if set to true, will refine triangulation using LM
+
+ mutable Point3 point_; ///< Current estimate of the 3D point
+
+ mutable bool degenerate_;
+ mutable bool cheiralityException_;
+
+ // verbosity handling for Cheirality Exceptions
+ const bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false)
+ const bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false)
+
+ double landmarkDistanceThreshold_; // if the landmark is triangulated at a
+ // distance larger than that the factor is considered degenerate
+
+ double dynamicOutlierRejectionThreshold_; // if this is nonnegative the factor will check if the
+ // average reprojection error is smaller than this threshold after triangulation,
+ // and the factor is disregarded if the error is large
+
+public:
+
+ /// shorthand for a smart pointer to a factor
+ typedef boost::shared_ptr shared_ptr;
+
+ /// shorthand for a set of cameras
+ typedef CameraSet Cameras;
+
+ /**
+ * Constructor
+ * @param rankTol tolerance used to check if point triangulation is degenerate
+ * otherwise the factor is simply neglected
+ * @param enableEPI if set to true linear triangulation is refined with embedded LM iterations
+ */
+ PinholeSet(const double rankTol = 1.0, const bool enableEPI = false,
+ double landmarkDistanceThreshold = 1e10,
+ double dynamicOutlierRejectionThreshold = -1) :
+ rankTolerance_(rankTol), retriangulationThreshold_(1e-5), enableEPI_(
+ enableEPI), degenerate_(false), cheiralityException_(false), throwCheirality_(
+ false), verboseCheirality_(false), landmarkDistanceThreshold_(
+ landmarkDistanceThreshold), dynamicOutlierRejectionThreshold_(
+ dynamicOutlierRejectionThreshold) {
+ }
+
+ /** Virtual destructor */
+ virtual ~PinholeSet() {
+ }
+
+ /**
+ * print
+ * @param s optional string naming the factor
+ * @param keyFormatter optional formatter useful for printing Symbols
+ */
+ void print(const std::string& s = "") const {
+ std::cout << s << "PinholeSet, z = \n";
+ std::cout << "rankTolerance_ = " << rankTolerance_ << std::endl;
+ std::cout << "degenerate_ = " << degenerate_ << std::endl;
+ std::cout << "cheiralityException_ = " << cheiralityException_ << std::endl;
+ Base::print("");
+ }
+
+ /// equals
+ bool equals(const PinholeSet& p, double tol = 1e-9) const {
+ return Base::equals(p, tol); // TODO all flags
+ }
+
+ /// Check if the new linearization point_ is the same as the one used for previous triangulation
+ bool decideIfTriangulate(const Cameras& cameras) const {
+ // several calls to linearize will be done from the same linearization point_, hence it is not needed to re-triangulate
+ // Note that this is not yet "selecting linearization", that will come later, and we only check if the
+ // current linearization is the "same" (up to tolerance) w.r.t. the last time we triangulated the point_
+
+ size_t m = cameras.size();
+
+ bool retriangulate = false;
+
+ // if we do not have a previous linearization point_ or the new linearization point_ includes more poses
+ if (cameraPosesTriangulation_.empty()
+ || cameras.size() != cameraPosesTriangulation_.size())
+ retriangulate = true;
+
+ if (!retriangulate) {
+ for (size_t i = 0; i < cameras.size(); i++) {
+ if (!cameras[i].pose().equals(cameraPosesTriangulation_[i],
+ retriangulationThreshold_)) {
+ retriangulate = true; // at least two poses are different, hence we retriangulate
+ break;
+ }
+ }
+ }
+
+ if (retriangulate) { // we store the current poses used for triangulation
+ cameraPosesTriangulation_.clear();
+ cameraPosesTriangulation_.reserve(m);
+ for (size_t i = 0; i < m; i++)
+ // cameraPosesTriangulation_[i] = cameras[i].pose();
+ cameraPosesTriangulation_.push_back(cameras[i].pose());
+ }
+
+ return retriangulate; // if we arrive to this point_ all poses are the same and we don't need re-triangulation
+ }
+
+ /// triangulateSafe
+ size_t triangulateSafe(const Values& values) const {
+ return triangulateSafe(this->cameras(values));
+ }
+
+ /// triangulateSafe
+ size_t triangulateSafe(const Cameras& cameras) const {
+
+ size_t m = cameras.size();
+ if (m < 2) { // if we have a single pose the corresponding factor is uninformative
+ degenerate_ = true;
+ return m;
+ }
+ bool retriangulate = decideIfTriangulate(cameras);
+
+ if (retriangulate) {
+ // We triangulate the 3D position of the landmark
+ try {
+ // std::cout << "triangulatePoint3 i \n" << rankTolerance << std::endl;
+ point_ = triangulatePoint3(cameras, this->measured_,
+ rankTolerance_, enableEPI_);
+ degenerate_ = false;
+ cheiralityException_ = false;
+
+ // Check landmark distance and reprojection errors to avoid outliers
+ double totalReprojError = 0.0;
+ size_t i = 0;
+ BOOST_FOREACH(const CAMERA& camera, cameras) {
+ Point3 cameraTranslation = camera.pose().translation();
+ // we discard smart factors corresponding to points that are far away
+ if (cameraTranslation.distance(point_) > landmarkDistanceThreshold_) {
+ degenerate_ = true;
+ break;
+ }
+ const Point2& zi = this->measured_.at(i);
+ try {
+ Point2 reprojectionError(camera.project(point_) - zi);
+ totalReprojError += reprojectionError.vector().norm();
+ } catch (CheiralityException) {
+ cheiralityException_ = true;
+ }
+ i += 1;
+ }
+ // we discard smart factors that have large reprojection error
+ if (dynamicOutlierRejectionThreshold_ > 0
+ && totalReprojError / m > dynamicOutlierRejectionThreshold_)
+ degenerate_ = true;
+
+ } catch (TriangulationUnderconstrainedException&) {
+ // if TriangulationUnderconstrainedException can be
+ // 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before
+ // 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark)
+ // in the second case we want to use a rotation-only smart factor
+ degenerate_ = true;
+ cheiralityException_ = false;
+ } catch (TriangulationCheiralityException&) {
+ // point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers
+ // we manage this case by either discarding the smart factor, or imposing a rotation-only constraint
+ cheiralityException_ = true;
+ }
+ }
+ return m;
+ }
+
+ /// triangulate
+ bool triangulateForLinearize(const Cameras& cameras) const {
+
+ bool isDebug = false;
+ size_t nrCameras = this->triangulateSafe(cameras);
+
+ if (nrCameras < 2 || (this->cheiralityException_ || this->degenerate_)) {
+ if (isDebug) {
+ std::cout
+ << "createRegularImplicitSchurFactor: degenerate configuration"
+ << std::endl;
+ }
+ return false;
+ } else {
+
+ // instead, if we want to manage the exception..
+ if (this->cheiralityException_ || this->degenerate_) { // if we want to manage the exceptions with rotation-only factors
+ this->degenerate_ = true;
+ }
+ return true;
+ }
+ }
+
+ /// Returns true if nonDegenerate
+ bool computeCamerasAndTriangulate(const Values& values,
+ Cameras& cameras) const {
+ Values valuesFactor;
+
+ // Select only the cameras
+ BOOST_FOREACH(const Key key, this->keys_)
+ valuesFactor.insert(key, values.at(key));
+
+ cameras = this->cameras(valuesFactor);
+ size_t nrCameras = this->triangulateSafe(cameras);
+
+ if (nrCameras < 2 || (this->cheiralityException_ || this->degenerate_))
+ return false;
+
+ // instead, if we want to manage the exception..
+ if (this->cheiralityException_ || this->degenerate_) // if we want to manage the exceptions with rotation-only factors
+ this->degenerate_ = true;
+
+ if (this->degenerate_) {
+ std::cout << "PinholeSet: this is not ready" << std::endl;
+ std::cout << "this->cheiralityException_ " << this->cheiralityException_
+ << std::endl;
+ std::cout << "this->degenerate_ " << this->degenerate_ << std::endl;
+ }
+ return true;
+ }
+
+ /**
+ * Triangulate and compute derivative of error with respect to point
+ * @return whether triangulation worked
+ */
+ bool triangulateAndComputeE(Matrix& E, const Values& values) const {
+ Cameras cameras;
+ bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
+ if (nonDegenerate)
+ cameras.project2(point_, boost::none, E);
+ return nonDegenerate;
+ }
+
+ /// Calculate vector of re-projection errors, before applying noise model
+ Vector reprojectionErrorAfterTriangulation(const Values& values) const {
+ Cameras cameras;
+ bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
+ if (nonDegenerate)
+ return Base::reprojectionError(cameras, point_);
+ else
+ return zero(cameras.size() * 2);
+ }
+
+ /**
+ * Calculate the error of the factor.
+ * This is the log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/\sigma^2 \f$ in case of Gaussian.
+ * In this class, we take the raw prediction error \f$ h(x)-z \f$, ask the noise model
+ * to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5.
+ */
+ double totalReprojectionError(const Cameras& cameras,
+ boost::optional externalPoint = boost::none) const {
+
+ size_t nrCameras;
+ if (externalPoint) {
+ nrCameras = this->keys_.size();
+ point_ = *externalPoint;
+ degenerate_ = false;
+ cheiralityException_ = false;
+ } else {
+ nrCameras = this->triangulateSafe(cameras);
+ }
+
+ if (nrCameras < 2 || (this->cheiralityException_ || this->degenerate_)) {
+ // if we don't want to manage the exceptions we discard the factor
+ // std::cout << "In error evaluation: exception" << std::endl;
+ return 0.0;
+ }
+
+ if (this->cheiralityException_) { // if we want to manage the exceptions with rotation-only factors
+ std::cout
+ << "SmartProjectionHessianFactor: cheirality exception (this should not happen if CheiralityException is disabled)!"
+ << std::endl;
+ this->degenerate_ = true;
+ }
+
+ if (this->degenerate_) {
+ // return 0.0; // TODO: this maybe should be zero?
+ std::cout
+ << "SmartProjectionHessianFactor: trying to manage degeneracy (this should not happen is manageDegeneracy is disabled)!"
+ << std::endl;
+ // 3D parameterization of point at infinity
+ const Point2& zi = this->measured_.at(0);
+ this->point_ = cameras.front().backprojectPointAtInfinity(zi);
+ return Base::totalReprojectionErrorAtInfinity(cameras, this->point_);
+ } else {
+ // Just use version in base class
+ return Base::totalReprojectionError(cameras, point_);
+ }
+ }
+
+ /** return the landmark */
+ boost::optional point() const {
+ return point_;
+ }
+
+ /** COMPUTE the landmark */
+ boost::optional point(const Values& values) const {
+ triangulateSafe(values);
+ return point_;
+ }
+
+ /** return the degenerate state */
+ inline bool isDegenerate() const {
+ return (cheiralityException_ || degenerate_);
+ }
+
+ /** return the cheirality status flag */
+ inline bool isPointBehindCamera() const {
+ return cheiralityException_;
+ }
+
+ /** return cheirality verbosity */
+ inline bool verboseCheirality() const {
+ return verboseCheirality_;
+ }
+
+ /** return flag for throwing cheirality exceptions */
+ inline bool throwCheirality() const {
+ return throwCheirality_;
+ }
+
+private:
+
+ /// Serialization function
+ friend class boost::serialization::access;
+ template
+ void serialize(ARCHIVE & ar, const unsigned int version) {
+ ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
+ ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
+ ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
+ }
+};
+
+template
+struct traits > : public Testable > {
+};
+
+template
+struct traits > : public Testable > {
+};
+
+} // \ namespace gtsam
diff --git a/gtsam/geometry/tests/testPinholeSet.cpp b/gtsam/geometry/tests/testPinholeSet.cpp
new file mode 100644
index 000000000..079833ec4
--- /dev/null
+++ b/gtsam/geometry/tests/testPinholeSet.cpp
@@ -0,0 +1,131 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 testCameraSet.cpp
+ * @brief Unit tests for testCameraSet Class
+ * @author Frank Dellaert
+ * @date Feb 19, 2015
+ */
+
+#include
+#include
+#include
+#include
+#include
+
+using namespace std;
+using namespace gtsam;
+
+/* ************************************************************************* */
+#include
+TEST(PinholeSet, Stereo) {
+ typedef vector ZZ;
+ PinholeSet set;
+ CalibratedCamera camera;
+ set.push_back(camera);
+ set.push_back(camera);
+ Point3 p(0, 0, 1);
+ EXPECT_LONGS_EQUAL(6, traits::dimension);
+
+ // Check measurements
+ Point2 expected(0, 0);
+ ZZ z = set.project2(p);
+ EXPECT(assert_equal(expected, z[0]));
+ EXPECT(assert_equal(expected, z[1]));
+
+ // Calculate expected derivatives using Pinhole
+ Matrix43 actualE;
+ Matrix F1;
+ {
+ Matrix23 E1;
+ camera.project2(p, F1, E1);
+ actualE << E1, E1;
+ }
+
+ // Check computed derivatives
+ PinholeSet::FBlocks F;
+ Matrix E;
+ set.project2(p, F, E);
+ LONGS_EQUAL(2, F.size());
+ EXPECT(assert_equal(F1, F[0]));
+ EXPECT(assert_equal(F1, F[1]));
+ EXPECT(assert_equal(actualE, E));
+}
+
+/* ************************************************************************* */
+// Cal3Bundler test
+#include
+#include
+TEST(PinholeSet, Pinhole) {
+ typedef PinholeCamera Camera;
+ typedef vector ZZ;
+ PinholeSet set;
+ Camera camera;
+ set.push_back(camera);
+ set.push_back(camera);
+ Point3 p(0, 0, 1);
+ EXPECT(assert_equal(set, set));
+ PinholeSet set2 = set;
+ set2.push_back(camera);
+ EXPECT(!set.equals(set2));
+
+ // Check measurements
+ Point2 expected;
+ ZZ z = set.project2(p);
+ EXPECT(assert_equal(expected, z[0]));
+ EXPECT(assert_equal(expected, z[1]));
+
+ // Calculate expected derivatives using Pinhole
+ Matrix43 actualE;
+ Matrix F1;
+ {
+ Matrix23 E1;
+ Matrix23 H1;
+ camera.project2(p, F1, E1);
+ actualE << E1, E1;
+ }
+
+ // Check computed derivatives
+ PinholeSet::FBlocks F;
+ Matrix E, H;
+ set.project2(p, F, E);
+ LONGS_EQUAL(2, F.size());
+ EXPECT(assert_equal(F1, F[0]));
+ EXPECT(assert_equal(F1, F[1]));
+ EXPECT(assert_equal(actualE, E));
+
+ // Check errors
+ ZZ measured;
+ measured.push_back(Point2(1, 2));
+ measured.push_back(Point2(3, 4));
+ Vector4 expectedV;
+
+ // reprojectionError
+ expectedV << -1, -2, -3, -4;
+ Vector actualV = set.reprojectionError(p, measured);
+ EXPECT(assert_equal(expectedV, actualV));
+
+ // reprojectionErrorAtInfinity
+ EXPECT(
+ assert_equal(Point3(0, 0, 1),
+ camera.backprojectPointAtInfinity(Point2())));
+ actualV = set.reprojectionErrorAtInfinity(p, measured);
+ EXPECT(assert_equal(expectedV, actualV));
+}
+
+/* ************************************************************************* */
+int main() {
+ TestResult tr;
+ return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */
+