diff --git a/.cproject b/.cproject
index bcf690995..617aa3795 100644
--- a/.cproject
+++ b/.cproject
@@ -854,6 +854,14 @@
true
true
+
+ make
+ -j4
+ testSmartStereoProjectionPoseFactor.run
+ true
+ true
+ true
+
make
-j5
@@ -2953,6 +2961,14 @@
true
true
+
+ make
+ -j4
+ testRangeFactor.run
+ true
+ true
+ true
+
make
-j2
diff --git a/.gitignore b/.gitignore
index 60633adaf..f3f1efd5b 100644
--- a/.gitignore
+++ b/.gitignore
@@ -5,3 +5,5 @@
/examples/Data/dubrovnik-3-7-pre-rewritten.txt
/examples/Data/pose2example-rewritten.txt
/examples/Data/pose3example-rewritten.txt
+*.txt.user
+*.txt.user.6d59f0c
diff --git a/gtsam.h b/gtsam.h
index 96d51117a..67b3f2888 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -758,10 +758,6 @@ class CalibratedCamera {
gtsam::CalibratedCamera retract(const Vector& d) const;
Vector localCoordinates(const gtsam::CalibratedCamera& T2) const;
- // Group
- gtsam::CalibratedCamera compose(const gtsam::CalibratedCamera& c) const;
- gtsam::CalibratedCamera inverse() const;
-
// Action on Point3
gtsam::Point2 project(const gtsam::Point3& point) const;
static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint);
@@ -2198,10 +2194,14 @@ typedef gtsam::RangeFactor RangeFactorPosePoint2;
typedef gtsam::RangeFactor RangeFactorPosePoint3;
typedef gtsam::RangeFactor RangeFactorPose2;
typedef gtsam::RangeFactor RangeFactorPose3;
-typedef gtsam::RangeFactor RangeFactorCalibratedCameraPoint;
-typedef gtsam::RangeFactor RangeFactorSimpleCameraPoint;
-typedef gtsam::RangeFactor RangeFactorCalibratedCamera;
-typedef gtsam::RangeFactor RangeFactorSimpleCamera;
+
+// Commented out by Frank Dec 2014: not poses!
+// If needed, we need a RangeFactor that takes a camera, extracts the pose
+// Should be easy with Expressions
+//typedef gtsam::RangeFactor RangeFactorCalibratedCameraPoint;
+//typedef gtsam::RangeFactor RangeFactorSimpleCameraPoint;
+//typedef gtsam::RangeFactor RangeFactorCalibratedCamera;
+//typedef gtsam::RangeFactor RangeFactorSimpleCamera;
#include
diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h
index 9c273f78c..3ecfcf8fa 100644
--- a/gtsam/base/Matrix.h
+++ b/gtsam/base/Matrix.h
@@ -37,36 +37,36 @@ namespace gtsam {
typedef Eigen::MatrixXd Matrix;
typedef Eigen::Matrix MatrixRowMajor;
-typedef Eigen::Matrix2d Matrix2;
-typedef Eigen::Matrix3d Matrix3;
-typedef Eigen::Matrix4d Matrix4;
-typedef Eigen::Matrix Matrix5;
-typedef Eigen::Matrix Matrix6;
+// Create handy typedefs and constants for square-size matrices
+// MatrixMN, MatrixN = MatrixNN, I_NxN, and Z_NxN, for M,N=1..9
+#define GTSAM_MAKE_TYPEDEFS(SIZE, SUFFIX) \
+typedef Eigen::Matrix Matrix##SUFFIX; \
+typedef Eigen::Matrix Matrix1##SUFFIX; \
+typedef Eigen::Matrix Matrix2##SUFFIX; \
+typedef Eigen::Matrix Matrix3##SUFFIX; \
+typedef Eigen::Matrix Matrix4##SUFFIX; \
+typedef Eigen::Matrix Matrix5##SUFFIX; \
+typedef Eigen::Matrix Matrix6##SUFFIX; \
+typedef Eigen::Matrix Matrix7##SUFFIX; \
+typedef Eigen::Matrix Matrix8##SUFFIX; \
+typedef Eigen::Matrix Matrix9##SUFFIX; \
+static const Eigen::MatrixBase::IdentityReturnType I_##SUFFIX##x##SUFFIX = Matrix##SUFFIX::Identity(); \
+static const Eigen::MatrixBase::ConstantReturnType Z_##SUFFIX##x##SUFFIX = Matrix##SUFFIX::Zero();
-typedef Eigen::Matrix Matrix23;
-typedef Eigen::Matrix Matrix24;
-typedef Eigen::Matrix Matrix25;
-typedef Eigen::Matrix Matrix26;
-typedef Eigen::Matrix Matrix27;
-typedef Eigen::Matrix Matrix28;
-typedef Eigen::Matrix Matrix29;
-
-typedef Eigen::Matrix Matrix32;
-typedef Eigen::Matrix Matrix34;
-typedef Eigen::Matrix Matrix35;
-typedef Eigen::Matrix Matrix36;
-typedef Eigen::Matrix Matrix37;
-typedef Eigen::Matrix Matrix38;
-typedef Eigen::Matrix Matrix39;
+GTSAM_MAKE_TYPEDEFS(1,1);
+GTSAM_MAKE_TYPEDEFS(2,2);
+GTSAM_MAKE_TYPEDEFS(3,3);
+GTSAM_MAKE_TYPEDEFS(4,4);
+GTSAM_MAKE_TYPEDEFS(5,5);
+GTSAM_MAKE_TYPEDEFS(6,6);
+GTSAM_MAKE_TYPEDEFS(7,7);
+GTSAM_MAKE_TYPEDEFS(8,8);
+GTSAM_MAKE_TYPEDEFS(9,9);
// Matrix expressions for accessing parts of matrices
typedef Eigen::Block SubMatrix;
typedef Eigen::Block ConstSubMatrix;
-// Two very commonly used matrices:
-const Matrix3 Z_3x3 = Matrix3::Zero();
-const Matrix3 I_3x3 = Matrix3::Identity();
-
// Matlab-like syntax
/**
diff --git a/gtsam/base/OptionalJacobian.h b/gtsam/base/OptionalJacobian.h
new file mode 100644
index 000000000..5651816ba
--- /dev/null
+++ b/gtsam/base/OptionalJacobian.h
@@ -0,0 +1,115 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 OptionalJacobian.h
+ * @brief Special class for optional Matrix arguments
+ * @author Frank Dellaert
+ * @author Natesh Srinivasan
+ * @date Nov 28, 2014
+ */
+
+#pragma once
+
+#include
+
+#ifndef OPTIONALJACOBIAN_NOBOOST
+#include
+#endif
+
+namespace gtsam {
+
+/**
+ * OptionalJacobian is an Eigen::Ref like class that can take be constructed using
+ * either a fixed size or dynamic Eigen matrix. In the latter case, the dynamic
+ * matrix will be resized. Finally, there is a constructor that takes
+ * boost::none, the default constructor acts like boost::none, and
+ * boost::optional is also supported for backwards compatibility.
+ */
+template
+class OptionalJacobian {
+
+public:
+
+ /// Fixed size type
+ typedef Eigen::Matrix Fixed;
+
+private:
+
+ Eigen::Map map_; /// View on constructor argument, if given
+
+ // Trick from http://eigen.tuxfamily.org/dox/group__TutorialMapClass.html
+ // uses "placement new" to make map_ usurp the memory of the fixed size matrix
+ void usurp(double* data) {
+ new (&map_) Eigen::Map(data);
+ }
+
+public:
+
+ /// Default constructor acts like boost::none
+ OptionalJacobian() :
+ map_(NULL) {
+ }
+
+ /// Constructor that will usurp data of a fixed-size matrix
+ OptionalJacobian(Fixed& fixed) :
+ map_(NULL) {
+ usurp(fixed.data());
+ }
+
+ /// Constructor that will usurp data of a fixed-size matrix, pointer version
+ OptionalJacobian(Fixed* fixedPtr) :
+ map_(NULL) {
+ if (fixedPtr)
+ usurp(fixedPtr->data());
+ }
+
+ /// Constructor that will resize a dynamic matrix (unless already correct)
+ OptionalJacobian(Eigen::MatrixXd& dynamic) :
+ map_(NULL) {
+ dynamic.resize(Rows, Cols); // no malloc if correct size
+ usurp(dynamic.data());
+ }
+
+#ifndef OPTIONALJACOBIAN_NOBOOST
+
+ /// Constructor with boost::none just makes empty
+ OptionalJacobian(boost::none_t none) :
+ map_(NULL) {
+ }
+
+ /// Constructor compatible with old-style derivatives
+ OptionalJacobian(const boost::optional optional) :
+ map_(NULL) {
+ if (optional) {
+ optional->resize(Rows, Cols);
+ usurp(optional->data());
+ }
+ }
+
+#endif
+
+ /// Return true is allocated, false if default constructor was used
+ operator bool() const {
+ return map_.data();
+ }
+
+ /// De-reference, like boost optional
+ Eigen::Map& operator*() {
+ return map_;
+ }
+
+ /// TODO: operator->()
+ Eigen::Map* operator->(){ return &map_; }
+};
+
+} // namespace gtsam
+
diff --git a/gtsam/base/tests/testOptionalJacobian.cpp b/gtsam/base/tests/testOptionalJacobian.cpp
new file mode 100644
index 000000000..6584c82d1
--- /dev/null
+++ b/gtsam/base/tests/testOptionalJacobian.cpp
@@ -0,0 +1,105 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 testOptionalJacobian.cpp
+ * @brief Unit test for OptionalJacobian
+ * @author Frank Dellaert
+ * @date Nov 28, 2014
+ **/
+
+#include
+#include
+#include
+
+using namespace std;
+using namespace gtsam;
+
+//******************************************************************************
+TEST( OptionalJacobian, Constructors ) {
+ Matrix23 fixed;
+
+ OptionalJacobian<2, 3> H1;
+ EXPECT(!H1);
+
+ OptionalJacobian<2, 3> H2(fixed);
+ EXPECT(H2);
+
+ OptionalJacobian<2, 3> H3(&fixed);
+ EXPECT(H3);
+
+ Matrix dynamic;
+ OptionalJacobian<2, 3> H4(dynamic);
+ EXPECT(H4);
+
+ OptionalJacobian<2, 3> H5(boost::none);
+ EXPECT(!H5);
+
+ boost::optional optional(dynamic);
+ OptionalJacobian<2, 3> H6(optional);
+ EXPECT(H6);
+}
+
+//******************************************************************************
+void test(OptionalJacobian<2, 3> H = boost::none) {
+ if (H)
+ *H = Matrix23::Zero();
+}
+
+void testPtr(Matrix23* H = NULL) {
+ if (H)
+ *H = Matrix23::Zero();
+}
+
+TEST( OptionalJacobian, Ref2) {
+
+ Matrix expected;
+ expected = Matrix23::Zero();
+
+ // Default argument does nothing
+ test();
+
+ // Fixed size, no copy
+ Matrix23 fixed1;
+ fixed1.setOnes();
+ test(fixed1);
+ EXPECT(assert_equal(expected,fixed1));
+
+ // Fixed size, no copy, pointer style
+ Matrix23 fixed2;
+ fixed2.setOnes();
+ test(&fixed2);
+ EXPECT(assert_equal(expected,fixed2));
+
+ // Empty is no longer a sign we don't want a matrix, we want it resized
+ Matrix dynamic0;
+ test(dynamic0);
+ EXPECT(assert_equal(expected,dynamic0));
+
+ // Dynamic wrong size
+ Matrix dynamic1(3, 5);
+ dynamic1.setOnes();
+ test(dynamic1);
+ EXPECT(assert_equal(expected,dynamic0));
+
+ // Dynamic right size
+ Matrix dynamic2(2, 5);
+ dynamic2.setOnes();
+ test(dynamic2);
+ EXPECT(assert_equal(dynamic2,dynamic0));
+}
+
+//******************************************************************************
+int main() {
+ TestResult tr;
+ return TestRegistry::runAllTests(tr);
+}
+//******************************************************************************
diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp
index 8ff728d26..ccd061d7c 100644
--- a/gtsam/geometry/Cal3Bundler.cpp
+++ b/gtsam/geometry/Cal3Bundler.cpp
@@ -34,15 +34,17 @@ Cal3Bundler::Cal3Bundler(double f, double k1, double k2, double u0, double v0) :
}
/* ************************************************************************* */
-Matrix Cal3Bundler::K() const {
+Matrix3 Cal3Bundler::K() const {
Matrix3 K;
K << f_, 0, u0_, 0, f_, v0_, 0, 0, 1;
return K;
}
/* ************************************************************************* */
-Vector Cal3Bundler::k() const {
- return (Vector(4) << k1_, k2_, 0, 0).finished();
+Vector4 Cal3Bundler::k() const {
+ Vector4 rvalue_;
+ rvalue_ << k1_, k2_, 0, 0;
+ return rvalue_;
}
/* ************************************************************************* */
@@ -64,21 +66,9 @@ bool Cal3Bundler::equals(const Cal3Bundler& K, double tol) const {
return true;
}
-/* ************************************************************************* */
-Point2 Cal3Bundler::uncalibrate(const Point2& p) const {
- // r = x^2 + y^2;
- // g = (1 + k(1)*r + k(2)*r^2);
- // pi(:,i) = g * pn(:,i)
- const double x = p.x(), y = p.y();
- const double r = x * x + y * y;
- const double g = 1. + (k1_ + k2_ * r) * r;
- const double u = g * x, v = g * y;
- return Point2(u0_ + f_ * u, v0_ + f_ * v);
-}
-
/* ************************************************************************* */
Point2 Cal3Bundler::uncalibrate(const Point2& p, //
- boost::optional Dcal, boost::optional Dp) const {
+ OptionalJacobian<2, 3> Dcal, OptionalJacobian<2, 2> Dp) const {
// r = x^2 + y^2;
// g = (1 + k(1)*r + k(2)*r^2);
// pi(:,i) = g * pn(:,i)
@@ -103,35 +93,6 @@ Point2 Cal3Bundler::uncalibrate(const Point2& p, //
return Point2(u0_ + f_ * u, v0_ + f_ * v);
}
-/* ************************************************************************* */
-Point2 Cal3Bundler::uncalibrate(const Point2& p, //
- boost::optional Dcal, boost::optional Dp) const {
- // r = x^2 + y^2;
- // g = (1 + k(1)*r + k(2)*r^2);
- // pi(:,i) = g * pn(:,i)
- const double x = p.x(), y = p.y();
- const double r = x * x + y * y;
- const double g = 1. + (k1_ + k2_ * r) * r;
- const double u = g * x, v = g * y;
-
- // Derivatives make use of intermediate variables above
- if (Dcal) {
- double rx = r * x, ry = r * y;
- Dcal->resize(2, 3);
- *Dcal << u, f_ * rx, f_ * r * rx, v, f_ * ry, f_ * r * ry;
- }
-
- if (Dp) {
- const double a = 2. * (k1_ + 2. * k2_ * r);
- const double axx = a * x * x, axy = a * x * y, ayy = a * y * y;
- Dp->resize(2,2);
- *Dp << g + axx, axy, axy, g + ayy;
- *Dp *= f_;
- }
-
- return Point2(u0_ + f_ * u, v0_ + f_ * v);
-}
-
/* ************************************************************************* */
Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol) const {
// Copied from Cal3DS2 :-(
@@ -161,24 +122,25 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol) const {
}
/* ************************************************************************* */
-Matrix Cal3Bundler::D2d_intrinsic(const Point2& p) const {
- Matrix Dp;
+Matrix2 Cal3Bundler::D2d_intrinsic(const Point2& p) const {
+ Matrix2 Dp;
uncalibrate(p, boost::none, Dp);
return Dp;
}
/* ************************************************************************* */
-Matrix Cal3Bundler::D2d_calibration(const Point2& p) const {
- Matrix Dcal;
+Matrix23 Cal3Bundler::D2d_calibration(const Point2& p) const {
+ Matrix23 Dcal;
uncalibrate(p, Dcal, boost::none);
return Dcal;
}
/* ************************************************************************* */
-Matrix Cal3Bundler::D2d_intrinsic_calibration(const Point2& p) const {
- Matrix Dcal, Dp;
+Matrix25 Cal3Bundler::D2d_intrinsic_calibration(const Point2& p) const {
+ Matrix23 Dcal;
+ Matrix2 Dp;
uncalibrate(p, Dcal, Dp);
- Matrix H(2, 5);
+ Matrix25 H;
H << Dp, Dcal;
return H;
}
diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h
index 4c77fdf23..fc1d63e10 100644
--- a/gtsam/geometry/Cal3Bundler.h
+++ b/gtsam/geometry/Cal3Bundler.h
@@ -69,8 +69,8 @@ public:
/// @name Standard Interface
/// @{
- Matrix K() const; ///< Standard 3*3 calibration matrix
- Vector k() const; ///< Radial distortion parameters (4 of them, 2 0)
+ Matrix3 K() const; ///< Standard 3*3 calibration matrix
+ Vector4 k() const; ///< Radial distortion parameters (4 of them, 2 0)
Vector3 vector() const;
@@ -106,43 +106,27 @@ public:
/**
- * convert intrinsic coordinates xy to image coordinates uv
- * @param p point in intrinsic coordinates
- * @return point in image coordinates
- */
- Point2 uncalibrate(const Point2& p) const;
-
- /**
- * Version of uncalibrate with fixed derivatives
+ * @brief: convert intrinsic coordinates xy to image coordinates uv
+ * Version of uncalibrate with derivatives
* @param p point in intrinsic coordinates
* @param Dcal optional 2*3 Jacobian wrpt CalBundler parameters
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in image coordinates
*/
- Point2 uncalibrate(const Point2& p, boost::optional Dcal,
- boost::optional Dp) const;
-
- /**
- * Version of uncalibrate with dynamic derivatives
- * @param p point in intrinsic coordinates
- * @param Dcal optional 2*3 Jacobian wrpt CalBundler parameters
- * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
- * @return point in image coordinates
- */
- Point2 uncalibrate(const Point2& p, boost::optional Dcal,
- boost::optional Dp) const;
+ Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 3> Dcal = boost::none,
+ OptionalJacobian<2, 2> Dp = boost::none) const;
/// Conver a pixel coordinate to ideal coordinate
Point2 calibrate(const Point2& pi, const double tol = 1e-5) const;
/// @deprecated might be removed in next release, use uncalibrate
- Matrix D2d_intrinsic(const Point2& p) const;
+ Matrix2 D2d_intrinsic(const Point2& p) const;
/// @deprecated might be removed in next release, use uncalibrate
- Matrix D2d_calibration(const Point2& p) const;
+ Matrix23 D2d_calibration(const Point2& p) const;
/// @deprecated might be removed in next release, use uncalibrate
- Matrix D2d_intrinsic_calibration(const Point2& p) const;
+ Matrix25 D2d_intrinsic_calibration(const Point2& p) const;
/// @}
/// @name Manifold
diff --git a/gtsam/geometry/Cal3DS2_Base.cpp b/gtsam/geometry/Cal3DS2_Base.cpp
index 1105fecfb..cfbce2b28 100644
--- a/gtsam/geometry/Cal3DS2_Base.cpp
+++ b/gtsam/geometry/Cal3DS2_Base.cpp
@@ -28,8 +28,10 @@ Cal3DS2_Base::Cal3DS2_Base(const Vector &v):
fx_(v[0]), fy_(v[1]), s_(v[2]), u0_(v[3]), v0_(v[4]), k1_(v[5]), k2_(v[6]), p1_(v[7]), p2_(v[8]){}
/* ************************************************************************* */
-Matrix Cal3DS2_Base::K() const {
- return (Matrix(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0).finished();
+Matrix3 Cal3DS2_Base::K() const {
+ Matrix3 K;
+ K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0;
+ return K;
}
/* ************************************************************************* */
@@ -39,7 +41,7 @@ Vector Cal3DS2_Base::vector() const {
/* ************************************************************************* */
void Cal3DS2_Base::print(const std::string& s_) const {
- gtsam::print(K(), s_ + ".K");
+ gtsam::print((Matrix)K(), s_ + ".K");
gtsam::print(Vector(k()), s_ + ".k");
}
@@ -91,8 +93,7 @@ static Matrix2 D2dintrinsic(double x, double y, double rr,
/* ************************************************************************* */
Point2 Cal3DS2_Base::uncalibrate(const Point2& p,
- boost::optional H1,
- boost::optional H2) const {
+ OptionalJacobian<2,9> H1, OptionalJacobian<2,2> H2) const {
// rr = x^2 + y^2;
// g = (1 + k(1)*rr + k(2)*rr^2);
@@ -126,26 +127,6 @@ Point2 Cal3DS2_Base::uncalibrate(const Point2& p,
return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_);
}
-/* ************************************************************************* */
-Point2 Cal3DS2_Base::uncalibrate(const Point2& p,
- boost::optional H1,
- boost::optional H2) const {
-
- if (H1 || H2) {
- Matrix29 D1;
- Matrix2 D2;
- Point2 pu = uncalibrate(p, D1, D2);
- if (H1)
- *H1 = D1;
- if (H2)
- *H2 = D2;
- return pu;
-
- } else {
- return uncalibrate(p);
- }
-}
-
/* ************************************************************************* */
Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const {
// Use the following fixed point iteration to invert the radial distortion.
@@ -177,7 +158,7 @@ Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const {
}
/* ************************************************************************* */
-Matrix Cal3DS2_Base::D2d_intrinsic(const Point2& p) const {
+Matrix2 Cal3DS2_Base::D2d_intrinsic(const Point2& p) const {
const double x = p.x(), y = p.y(), xx = x * x, yy = y * y;
const double rr = xx + yy;
const double r4 = rr * rr;
@@ -188,7 +169,7 @@ Matrix Cal3DS2_Base::D2d_intrinsic(const Point2& p) const {
}
/* ************************************************************************* */
-Matrix Cal3DS2_Base::D2d_calibration(const Point2& p) const {
+Matrix29 Cal3DS2_Base::D2d_calibration(const Point2& p) const {
const double x = p.x(), y = p.y(), xx = x * x, yy = y * y, xy = x * y;
const double rr = xx + yy;
const double r4 = rr * rr;
diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h
index 61c01e349..f9292d4f6 100644
--- a/gtsam/geometry/Cal3DS2_Base.h
+++ b/gtsam/geometry/Cal3DS2_Base.h
@@ -45,7 +45,7 @@ protected:
double p1_, p2_ ; // tangential distortion
public:
- Matrix K() const ;
+ Matrix3 K() const ;
Eigen::Vector4d k() const { return Eigen::Vector4d(k1_, k2_, p1_, p2_); }
Vector vector() const ;
@@ -113,23 +113,18 @@ public:
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in (distorted) image coordinates
*/
-
Point2 uncalibrate(const Point2& p,
- boost::optional Dcal = boost::none,
- boost::optional Dp = boost::none) const ;
-
- Point2 uncalibrate(const Point2& p,
- boost::optional Dcal,
- boost::optional Dp) const ;
+ OptionalJacobian<2,9> Dcal = boost::none,
+ OptionalJacobian<2,2> Dp = boost::none) const ;
/// Convert (distorted) image coordinates uv to intrinsic coordinates xy
Point2 calibrate(const Point2& p, const double tol=1e-5) const;
/// Derivative of uncalibrate wrpt intrinsic coordinates
- Matrix D2d_intrinsic(const Point2& p) const ;
+ Matrix2 D2d_intrinsic(const Point2& p) const ;
/// Derivative of uncalibrate wrpt the calibration parameters
- Matrix D2d_calibration(const Point2& p) const ;
+ Matrix29 D2d_calibration(const Point2& p) const ;
private:
diff --git a/gtsam/geometry/Cal3Unified.cpp b/gtsam/geometry/Cal3Unified.cpp
index 6bc4c4bb3..5bdf89856 100644
--- a/gtsam/geometry/Cal3Unified.cpp
+++ b/gtsam/geometry/Cal3Unified.cpp
@@ -52,8 +52,8 @@ bool Cal3Unified::equals(const Cal3Unified& K, double tol) const {
/* ************************************************************************* */
// todo: make a fixed sized jacobian version of this
Point2 Cal3Unified::uncalibrate(const Point2& p,
- boost::optional H1,
- boost::optional H2) const {
+ OptionalJacobian<2,10> H1,
+ OptionalJacobian<2,2> H2) const {
// this part of code is modified from Cal3DS2,
// since the second part of this model (after project to normalized plane)
@@ -81,10 +81,7 @@ Point2 Cal3Unified::uncalibrate(const Point2& p,
Vector2 DU;
DU << -xs * sqrt_nx * xi_sqrt_nx2, //
-ys * sqrt_nx * xi_sqrt_nx2;
-
- H1->resize(2,10);
- H1->block<2,9>(0,0) = H1base;
- H1->block<2,1>(0,9) = H2base * DU;
+ *H1 << H1base, H2base * DU;
}
// Inlined derivative for points
@@ -96,7 +93,7 @@ Point2 Cal3Unified::uncalibrate(const Point2& p,
DU << (sqrt_nx + xi*(ys*ys + 1)) * denom, mid, //
mid, (sqrt_nx + xi*(xs*xs + 1)) * denom;
- *H2 = H2base * DU;
+ *H2 << H2base * DU;
}
return puncalib;
diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h
index f65b27780..133e330ba 100644
--- a/gtsam/geometry/Cal3Unified.h
+++ b/gtsam/geometry/Cal3Unified.h
@@ -96,8 +96,8 @@ public:
* @return point in image coordinates
*/
Point2 uncalibrate(const Point2& p,
- boost::optional Dcal = boost::none,
- boost::optional Dp = boost::none) const ;
+ OptionalJacobian<2,10> Dcal = boost::none,
+ OptionalJacobian<2,2> Dp = boost::none) const ;
/// Conver a pixel coordinate to ideal coordinate
Point2 calibrate(const Point2& p, const double tol=1e-5) const;
diff --git a/gtsam/geometry/Cal3_S2.cpp b/gtsam/geometry/Cal3_S2.cpp
index aece1ded1..3ec29bbd2 100644
--- a/gtsam/geometry/Cal3_S2.cpp
+++ b/gtsam/geometry/Cal3_S2.cpp
@@ -53,7 +53,7 @@ Cal3_S2::Cal3_S2(const std::string &path) :
/* ************************************************************************* */
void Cal3_S2::print(const std::string& s) const {
- gtsam::print(matrix(), s);
+ gtsam::print((Matrix)matrix(), s);
}
/* ************************************************************************* */
@@ -72,32 +72,13 @@ bool Cal3_S2::equals(const Cal3_S2& K, double tol) const {
}
/* ************************************************************************* */
-Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional Dcal,
- boost::optional Dp) const {
+Point2 Cal3_S2::uncalibrate(const Point2& p, OptionalJacobian<2, 5> Dcal,
+ OptionalJacobian<2, 2> Dp) const {
const double x = p.x(), y = p.y();
- if (Dcal) {
- Dcal->resize(2,5);
+ if (Dcal)
*Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0;
- }
- if (Dp) {
- Dp->resize(2,2);
+ if (Dp)
*Dp << fx_, s_, 0.0, fy_;
- }
- return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
-}
-
-/* ************************************************************************* */
-Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional Dcal,
- boost::optional Dp) const {
- const double x = p.x(), y = p.y();
- if (Dcal) *Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0;
- if (Dp) *Dp << fx_, s_, 0.0, fy_;
- return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
-}
-
-/* ************************************************************************* */
-Point2 Cal3_S2::uncalibrate(const Point2& p) const {
- const double x = p.x(), y = p.y();
return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
}
diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h
index 6317d251d..cf358d0b2 100644
--- a/gtsam/geometry/Cal3_S2.h
+++ b/gtsam/geometry/Cal3_S2.h
@@ -125,29 +125,26 @@ public:
}
/// return calibration matrix K
- Matrix K() const {
- return (Matrix(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0).finished();
+ Matrix3 K() const {
+ Matrix3 K;
+ K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0;
+ return K;
}
/** @deprecated The following function has been deprecated, use K above */
- Matrix matrix() const {
+ Matrix3 matrix() const {
return K();
}
/// return inverted calibration matrix inv(K)
- Matrix matrix_inverse() const {
+ Matrix3 matrix_inverse() const {
const double fxy = fx_ * fy_, sv0 = s_ * v0_, fyu0 = fy_ * u0_;
- return (Matrix(3, 3) << 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0,
- 1.0 / fy_, -v0_ / fy_, 0.0, 0.0, 1.0).finished();
+ Matrix3 K_inverse;
+ K_inverse << 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0,
+ 1.0 / fy_, -v0_ / fy_, 0.0, 0.0, 1.0;
+ return K_inverse;
}
- /**
- * convert intrinsic coordinates xy to image coordinates uv
- * @param p point in intrinsic coordinates
- * @return point in image coordinates
- */
- Point2 uncalibrate(const Point2& p) const;
-
/**
* convert intrinsic coordinates xy to image coordinates uv, fixed derivaitves
* @param p point in intrinsic coordinates
@@ -155,18 +152,8 @@ public:
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in image coordinates
*/
- Point2 uncalibrate(const Point2& p, boost::optional Dcal,
- boost::optional Dp) const;
-
- /**
- * convert intrinsic coordinates xy to image coordinates uv, dynamic derivaitves
- * @param p point in intrinsic coordinates
- * @param Dcal optional 2*5 Jacobian wrpt Cal3_S2 parameters
- * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
- * @return point in image coordinates
- */
- Point2 uncalibrate(const Point2& p, boost::optional Dcal,
- boost::optional Dp) const;
+ Point2 uncalibrate(const Point2& p, OptionalJacobian<2,5> Dcal = boost::none,
+ OptionalJacobian<2,2> Dp = boost::none) const;
/**
* convert image coordinates uv to intrinsic coordinates xy
@@ -184,10 +171,10 @@ public:
/// "Between", subtracts calibrations. between(p,q) == compose(inverse(p),q)
inline Cal3_S2 between(const Cal3_S2& q,
- boost::optional H1=boost::none,
- boost::optional H2=boost::none) const {
- if(H1) *H1 = -eye(5);
- if(H2) *H2 = eye(5);
+ OptionalJacobian<5,5> H1=boost::none,
+ OptionalJacobian<5,5> H2=boost::none) const {
+ if(H1) *H1 = -I_5x5;
+ if(H2) *H2 = I_5x5;
return Cal3_S2(q.fx_-fx_, q.fy_-fy_, q.s_-s_, q.u0_-u0_, q.v0_-v0_);
}
diff --git a/gtsam/geometry/CalibratedCamera.cpp b/gtsam/geometry/CalibratedCamera.cpp
index 392a53858..1f5f1f8a5 100644
--- a/gtsam/geometry/CalibratedCamera.cpp
+++ b/gtsam/geometry/CalibratedCamera.cpp
@@ -33,10 +33,10 @@ CalibratedCamera::CalibratedCamera(const Vector &v) :
/* ************************************************************************* */
Point2 CalibratedCamera::project_to_camera(const Point3& P,
- boost::optional H1) {
+ OptionalJacobian<2,3> H1) {
if (H1) {
double d = 1.0 / P.z(), d2 = d * d;
- *H1 = (Matrix(2, 3) << d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2).finished();
+ *H1 << d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2;
}
return Point2(P.x() / P.z(), P.y() / P.z());
}
@@ -59,10 +59,12 @@ CalibratedCamera CalibratedCamera::Level(const Pose2& pose2, double height) {
/* ************************************************************************* */
Point2 CalibratedCamera::project(const Point3& point,
- boost::optional Dpose, boost::optional Dpoint) const {
+ OptionalJacobian<2,6> Dpose, OptionalJacobian<2,3> Dpoint) const {
#ifdef CALIBRATEDCAMERA_CHAIN_RULE
- Point3 q = pose_.transform_to(point, Dpose, Dpoint);
+ Matrix36 Dpose_;
+ Matrix3 Dpoint_;
+ Point3 q = pose_.transform_to(point, Dpose ? Dpose_ : 0, Dpoint ? Dpoint_ : 0);
#else
Point3 q = pose_.transform_to(point);
#endif
@@ -75,23 +77,26 @@ Point2 CalibratedCamera::project(const Point3& point,
if (Dpose || Dpoint) {
#ifdef CALIBRATEDCAMERA_CHAIN_RULE
// just implement chain rule
- Matrix H;
- project_to_camera(q,H);
- if (Dpose) *Dpose = H * (*Dpose);
- if (Dpoint) *Dpoint = H * (*Dpoint);
+ if(Dpose && Dpoint) {
+ Matrix23 H;
+ project_to_camera(q,H);
+ if (Dpose) *Dpose = H * (*Dpose_);
+ if (Dpoint) *Dpoint = H * (*Dpoint_);
+ }
#else
// optimized version, see CalibratedCamera.nb
const double z = q.z(), d = 1.0 / z;
const double u = intrinsic.x(), v = intrinsic.y(), uv = u * v;
if (Dpose)
- *Dpose = (Matrix(2, 6) << uv, -(1. + u * u), v, -d, 0., d * u, (1. + v * v),
- -uv, -u, 0., -d, d * v).finished();
+ *Dpose << uv, -(1. + u * u), v, -d, 0., d * u, (1. + v * v),
+ -uv, -u, 0., -d, d * v;
if (Dpoint) {
- const Matrix R(pose_.rotation().matrix());
- *Dpoint = d
- * (Matrix(2, 3) << R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2),
+ const Matrix3 R(pose_.rotation().matrix());
+ Matrix23 Dpoint_;
+ Dpoint_ << R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2),
R(2, 0) - u * R(2, 2), R(0, 1) - v * R(0, 2),
- R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2)).finished();
+ R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2);
+ *Dpoint = d * Dpoint_;
}
#endif
}
diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h
index c66941091..f5a8b4469 100644
--- a/gtsam/geometry/CalibratedCamera.h
+++ b/gtsam/geometry/CalibratedCamera.h
@@ -18,9 +18,8 @@
#pragma once
-#include
-#include
#include
+#include
namespace gtsam {
@@ -88,26 +87,6 @@ public:
return pose_;
}
- /// compose the two camera poses: TODO Frank says this might not make sense
- inline const CalibratedCamera compose(const CalibratedCamera &c,
- boost::optional H1 = boost::none, boost::optional H2 =
- boost::none) const {
- return CalibratedCamera(pose_.compose(c.pose(), H1, H2));
- }
-
- /// between the two camera poses: TODO Frank says this might not make sense
- inline const CalibratedCamera between(const CalibratedCamera& c,
- boost::optional H1 = boost::none, boost::optional H2 =
- boost::none) const {
- return CalibratedCamera(pose_.between(c.pose(), H1, H2));
- }
-
- /// invert the camera pose: TODO Frank says this might not make sense
- inline const CalibratedCamera inverse(boost::optional H1 =
- boost::none) const {
- return CalibratedCamera(pose_.inverse(H1));
- }
-
/**
* Create a level camera at the given 2D pose and height
* @param pose2 specifies the location and viewing direction
@@ -152,8 +131,8 @@ public:
* @return the intrinsic coordinates of the projected point
*/
Point2 project(const Point3& point,
- boost::optional Dpose = boost::none,
- boost::optional Dpoint = boost::none) const;
+ OptionalJacobian<2, 6> Dpose = boost::none,
+ OptionalJacobian<2, 3> Dpoint = boost::none) const;
/**
* projects a 3-dimensional point in camera coordinates into the
@@ -161,7 +140,7 @@ public:
* With optional 2by3 derivative
*/
static Point2 project_to_camera(const Point3& cameraPoint,
- boost::optional H1 = boost::none);
+ OptionalJacobian<2, 3> H1 = boost::none);
/**
* backproject a 2-dimensional point to a 3-dimension point
@@ -175,8 +154,8 @@ public:
* @param H2 optionally computed Jacobian with respect to the 3D point
* @return range (double)
*/
- double range(const Point3& point, boost::optional H1 = boost::none,
- boost::optional H2 = boost::none) const {
+ double range(const Point3& point, OptionalJacobian<1, 6> H1 = boost::none,
+ OptionalJacobian<1, 3> H2 = boost::none) const {
return pose_.range(point, H1, H2);
}
@@ -187,8 +166,8 @@ public:
* @param H2 optionally computed Jacobian with respect to the 3D point
* @return range (double)
*/
- double range(const Pose3& pose, boost::optional H1 = boost::none,
- boost::optional H2 = boost::none) const {
+ double range(const Pose3& pose, OptionalJacobian<1, 6> H1 = boost::none,
+ OptionalJacobian<1, 6> H2 = boost::none) const {
return pose_.range(pose, H1, H2);
}
@@ -199,8 +178,8 @@ public:
* @param H2 optionally computed Jacobian with respect to the 3D point
* @return range (double)
*/
- double range(const CalibratedCamera& camera, boost::optional H1 =
- boost::none, boost::optional H2 = boost::none) const {
+ double range(const CalibratedCamera& camera, OptionalJacobian<1, 6> H1 =
+ boost::none, OptionalJacobian<1, 6> H2 = boost::none) const {
return pose_.range(camera.pose_, H1, H2);
}
@@ -224,15 +203,16 @@ private:
namespace traits {
template<>
-struct GTSAM_EXPORT is_group : public boost::true_type{
+struct GTSAM_EXPORT is_group : public boost::true_type {
};
template<>
-struct GTSAM_EXPORT is_manifold : public boost::true_type{
+struct GTSAM_EXPORT is_manifold : public boost::true_type {
};
template<>
-struct GTSAM_EXPORT dimension : public boost::integral_constant{
+struct GTSAM_EXPORT dimension : public boost::integral_constant<
+ int, 6> {
};
}
diff --git a/gtsam/geometry/EssentialMatrix.cpp b/gtsam/geometry/EssentialMatrix.cpp
index e65e5d097..c2b690496 100644
--- a/gtsam/geometry/EssentialMatrix.cpp
+++ b/gtsam/geometry/EssentialMatrix.cpp
@@ -14,7 +14,7 @@ namespace gtsam {
/* ************************************************************************* */
EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _1P2_,
- boost::optional H) {
+ OptionalJacobian<5, 6> H) {
const Rot3& _1R2_ = _1P2_.rotation();
const Point3& _1T2_ = _1P2_.translation();
if (!H) {
@@ -25,13 +25,10 @@ EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _1P2_,
// Calculate the 5*6 Jacobian H = D_E_1P2
// D_E_1P2 = [D_E_1R2 D_E_1T2], 5*3 wrpt rotation, 5*3 wrpt translation
// First get 2*3 derivative from Unit3::FromPoint3
- Matrix D_direction_1T2;
+ Matrix23 D_direction_1T2;
Unit3 direction = Unit3::FromPoint3(_1T2_, D_direction_1T2);
- H->resize(5, 6);
- H->block<3, 3>(0, 0) << Matrix::Identity(3, 3); // upper left
- H->block<2, 3>(3, 0) << Matrix::Zero(2, 3); // lower left
- H->block<3, 3>(0, 3) << Matrix::Zero(3, 3); // upper right
- H->block<2, 3>(3, 3) << D_direction_1T2 * _1R2_.matrix(); // lower right
+ *H << I_3x3, Z_3x3, //
+ Matrix23::Zero(), D_direction_1T2 * _1R2_.matrix();
return EssentialMatrix(_1R2_, direction);
}
}
@@ -55,22 +52,23 @@ EssentialMatrix EssentialMatrix::retract(const Vector& xi) const {
/* ************************************************************************* */
Vector EssentialMatrix::localCoordinates(const EssentialMatrix& other) const {
- return (Vector(5) <<
- aRb_.localCoordinates(other.aRb_), aTb_.localCoordinates(other.aTb_)).finished();
+ return (Vector(5) << aRb_.localCoordinates(other.aRb_), aTb_.localCoordinates(
+ other.aTb_)).finished();
}
/* ************************************************************************* */
-Point3 EssentialMatrix::transform_to(const Point3& p,
- boost::optional DE, boost::optional Dpoint) const {
+Point3 EssentialMatrix::transform_to(const Point3& p, OptionalJacobian<3, 5> DE,
+ OptionalJacobian<3, 3> Dpoint) const {
Pose3 pose(aRb_, aTb_.point3());
- Point3 q = pose.transform_to(p, DE, Dpoint);
+ Matrix36 DE_;
+ Point3 q = pose.transform_to(p, DE ? &DE_ : 0, Dpoint);
if (DE) {
// DE returned by pose.transform_to is 3*6, but we need it to be 3*5
// The last 3 columns are derivative with respect to change in translation
// The derivative of translation with respect to a 2D sphere delta is 3*2 aTb_.basis()
// Duy made an educated guess that this needs to be rotated to the local frame
- Matrix H(3, 5);
- H << DE->block<3, 3>(0, 0), -aRb_.transpose() * aTb_.basis();
+ Matrix35 H;
+ H << DE_.block < 3, 3 > (0, 0), -aRb_.transpose() * aTb_.basis();
*DE = H;
}
return q;
@@ -78,7 +76,7 @@ Point3 EssentialMatrix::transform_to(const Point3& p,
/* ************************************************************************* */
EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb,
- boost::optional HE, boost::optional HR) const {
+ OptionalJacobian<5, 5> HE, OptionalJacobian<5, 3> HR) const {
// The rotation must be conjugated to act in the camera frame
Rot3 c1Rc2 = aRb_.conjugate(cRb);
@@ -89,18 +87,16 @@ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb,
return EssentialMatrix(c1Rc2, c1Tc2);
} else {
// Calculate derivatives
- Matrix D_c1Tc2_cRb, D_c1Tc2_aTb; // 2*3 and 2*2
+ Matrix23 D_c1Tc2_cRb; // 2*3
+ Matrix2 D_c1Tc2_aTb; // 2*2
Unit3 c1Tc2 = cRb.rotate(aTb_, D_c1Tc2_cRb, D_c1Tc2_aTb);
- if (HE) {
- *HE = zeros(5, 5);
- HE->block<3, 3>(0, 0) << cRb.matrix(); // a change in aRb_ will yield a rotated change in c1Rc2
- HE->block<2, 2>(3, 3) << D_c1Tc2_aTb; // (2*2)
- }
+ if (HE)
+ *HE << cRb.matrix(), Matrix32::Zero(), //
+ Matrix23::Zero(), D_c1Tc2_aTb;
if (HR) {
throw runtime_error(
"EssentialMatrix::rotate: derivative HR not implemented yet");
/*
- HR->resize(5, 3);
HR->block<3, 3>(0, 0) << zeros(3, 3); // a change in the rotation yields ?
HR->block<2, 3>(3, 0) << zeros(2, 3); // (2*3) * (3*3) ?
*/
@@ -110,13 +106,12 @@ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb,
}
/* ************************************************************************* */
-double EssentialMatrix::error(const Vector& vA, const Vector& vB, //
- boost::optional H) const {
+double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, //
+ OptionalJacobian<1, 5> H) const {
if (H) {
- H->resize(1, 5);
// See math.lyx
- Matrix HR = vA.transpose() * E_ * skewSymmetric(-vB);
- Matrix HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB)
+ Matrix13 HR = vA.transpose() * E_ * skewSymmetric(-vB);
+ Matrix12 HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB)
* aTb_.basis();
*H << HR, HD;
}
diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h
index fd5f45160..b25286412 100644
--- a/gtsam/geometry/EssentialMatrix.h
+++ b/gtsam/geometry/EssentialMatrix.h
@@ -50,7 +50,7 @@ public:
/// Named constructor converting a Pose3 with scale to EssentialMatrix (no scale)
static EssentialMatrix FromPose3(const Pose3& _1P2_,
- boost::optional H = boost::none);
+ OptionalJacobian<5, 6> H = boost::none);
/// Random, using Rot3::Random and Unit3::Random
template
@@ -132,16 +132,16 @@ public:
* @return point in pose coordinates
*/
Point3 transform_to(const Point3& p,
- boost::optional DE = boost::none,
- boost::optional Dpoint = boost::none) const;
+ OptionalJacobian<3,5> DE = boost::none,
+ OptionalJacobian<3,3> Dpoint = boost::none) const;
/**
* Given essential matrix E in camera frame B, convert to body frame C
* @param cRb rotation from body frame to camera frame
* @param E essential matrix E in camera frame C
*/
- EssentialMatrix rotate(const Rot3& cRb, boost::optional HE =
- boost::none, boost::optional HR = boost::none) const;
+ EssentialMatrix rotate(const Rot3& cRb, OptionalJacobian<5, 5> HE =
+ boost::none, OptionalJacobian<5, 3> HR = boost::none) const;
/**
* Given essential matrix E in camera frame B, convert to body frame C
@@ -153,8 +153,8 @@ public:
}
/// epipolar error, algebraic
- double error(const Vector& vA, const Vector& vB, //
- boost::optional H = boost::none) const;
+ double error(const Vector3& vA, const Vector3& vB, //
+ OptionalJacobian<1,5> H = boost::none) const;
/// @}
diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h
index c5174ae65..9b881dc78 100644
--- a/gtsam/geometry/PinholeCamera.h
+++ b/gtsam/geometry/PinholeCamera.h
@@ -18,16 +18,9 @@
#pragma once
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
#include
+#include
+#include
namespace gtsam {
@@ -42,7 +35,9 @@ private:
Pose3 pose_;
Calibration K_;
- static const int DimK = traits::dimension::value;
+ // Get dimensions of calibration type and This at compile time
+ static const int DimK = traits::dimension::value, //
+ DimC = 6 + DimK;
public:
@@ -114,9 +109,9 @@ public:
/// @{
explicit PinholeCamera(const Vector &v) {
- pose_ = Pose3::Expmap(v.head(Pose3::Dim()));
- if (v.size() > Pose3::Dim()) {
- K_ = Calibration(v.tail(Calibration::Dim()));
+ pose_ = Pose3::Expmap(v.head(6));
+ if (v.size() > 6) {
+ K_ = Calibration(v.tail(DimK));
}
}
@@ -167,82 +162,30 @@ public:
return K_;
}
- /// @}
- /// @name Group ?? Frank says this might not make sense
- /// @{
-
- /// compose two cameras: TODO Frank says this might not make sense
- inline const PinholeCamera compose(const PinholeCamera &c,
- boost::optional H1 = boost::none, boost::optional H2 =
- boost::none) const {
- PinholeCamera result(pose_.compose(c.pose(), H1, H2), K_);
- if (H1) {
- H1->conservativeResize(Dim(), Dim());
- H1->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(),
- Calibration::Dim());
- H1->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim());
- }
- if (H2) {
- H2->conservativeResize(Dim(), Dim());
- H2->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(),
- Calibration::Dim());
- H2->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim());
- }
- return result;
- }
-
- /// between two cameras: TODO Frank says this might not make sense
- inline const PinholeCamera between(const PinholeCamera& c,
- boost::optional H1 = boost::none, boost::optional H2 =
- boost::none) const {
- PinholeCamera result(pose_.between(c.pose(), H1, H2), K_);
- if (H1) {
- H1->conservativeResize(Dim(), Dim());
- H1->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(),
- Calibration::Dim());
- H1->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim());
- }
- if (H2) {
- H2->conservativeResize(Dim(), Dim());
- H2->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(),
- Calibration::Dim());
- H2->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim());
- }
- return result;
- }
-
- /// inverse camera: TODO Frank says this might not make sense
- inline const PinholeCamera inverse(
- boost::optional H1 = boost::none) const {
- PinholeCamera result(pose_.inverse(H1), K_);
- if (H1) {
- H1->conservativeResize(Dim(), Dim());
- H1->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(),
- Calibration::Dim());
- H1->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim());
- }
- return result;
- }
-
- /// compose two cameras: TODO Frank says this might not make sense
- inline const PinholeCamera compose(const Pose3 &c) const {
- return PinholeCamera(pose_.compose(c), K_);
- }
-
/// @}
/// @name Manifold
/// @{
- /// move a cameras according to d
- PinholeCamera retract(const Vector& d) const {
- if ((size_t) d.size() == pose_.dim())
- return PinholeCamera(pose().retract(d), calibration());
- else
- return PinholeCamera(pose().retract(d.head(pose().dim())),
- calibration().retract(d.tail(calibration().dim())));
+ /// Manifold dimension
+ inline size_t dim() const {
+ return DimC;
}
- typedef Eigen::Matrix VectorK6;
+ /// Manifold dimension
+ inline static size_t Dim() {
+ return DimC;
+ }
+
+ typedef Eigen::Matrix VectorK6;
+
+ /// move a cameras according to d
+ PinholeCamera retract(const Vector& d) const {
+ if ((size_t) d.size() == 6)
+ return PinholeCamera(pose().retract(d), calibration());
+ else
+ return PinholeCamera(pose().retract(d.head(6)),
+ calibration().retract(d.tail(calibration().dim())));
+ }
/// return canonical coordinate
VectorK6 localCoordinates(const PinholeCamera& T2) const {
@@ -252,16 +195,6 @@ public:
return d;
}
- /// Manifold dimension
- inline size_t dim() const {
- return pose_.dim() + K_.dim();
- }
-
- /// Manifold dimension
- inline static size_t Dim() {
- return Pose3::Dim() + Calibration::Dim();
- }
-
/// @}
/// @name Transformations and measurement functions
/// @{
@@ -272,8 +205,8 @@ public:
* @param P A point in camera coordinates
* @param Dpoint is the 2*3 Jacobian w.r.t. P
*/
- inline static Point2 project_to_camera(const Point3& P,
- boost::optional Dpoint = boost::none) {
+ static Point2 project_to_camera(const Point3& P, //
+ OptionalJacobian<2, 3> Dpoint = boost::none) {
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
if (P.z() <= 0)
throw CheiralityException();
@@ -292,21 +225,7 @@ public:
return std::make_pair(K_.uncalibrate(pn), pc.z() > 0);
}
- /** project a point from world coordinate to the image
- * @param pw is a point in world coordinates
- */
- inline Point2 project(const Point3& pw) const {
-
- // Transform to camera coordinates and check cheirality
- const Point3 pc = pose_.transform_to(pw);
-
- // Project to normalized image coordinates
- const Point2 pn = project_to_camera(pc);
-
- return K_.uncalibrate(pn);
- }
-
- typedef Eigen::Matrix Matrix2K;
+ typedef Eigen::Matrix Matrix2K;
/** project a point from world coordinate to the image
* @param pw is a point in world coordinates
@@ -314,11 +233,9 @@ public:
* @param Dpoint is the Jacobian w.r.t. point3
* @param Dcal is the Jacobian w.r.t. calibration
*/
- inline Point2 project(
- const Point3& pw, //
- boost::optional Dpose,
- boost::optional Dpoint,
- boost::optional Dcal) const {
+ inline Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose =
+ boost::none, OptionalJacobian<2, 3> Dpoint = boost::none,
+ OptionalJacobian<2, DimK> Dcal = boost::none) const {
// Transform to camera coordinates and check cheirality
const Point3 pc = pose_.transform_to(pw);
@@ -340,46 +257,7 @@ public:
calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint);
return pi;
} else
- return K_.uncalibrate(pn, Dcal, boost::none);
- }
-
- /** project a point from world coordinate to the image
- * @param pw is a point in world coordinates
- * @param Dpose is the Jacobian w.r.t. pose3
- * @param Dpoint is the Jacobian w.r.t. point3
- * @param Dcal is the Jacobian w.r.t. calibration
- */
- inline Point2 project(
- const Point3& pw, //
- boost::optional Dpose,
- boost::optional Dpoint,
- boost::optional Dcal) const {
-
- // Transform to camera coordinates and check cheirality
- const Point3 pc = pose_.transform_to(pw);
-
- // Project to normalized image coordinates
- const Point2 pn = project_to_camera(pc);
-
- if (Dpose || Dpoint) {
- const double z = pc.z(), d = 1.0 / z;
-
- // uncalibration
- Matrix Dpi_pn(2, 2);
- const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
-
- // chain the Jacobian matrices
- if (Dpose) {
- Dpose->resize(2, 6);
- calculateDpose(pn, d, Dpi_pn, *Dpose);
- }
- if (Dpoint) {
- Dpoint->resize(2, 3);
- calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint);
- }
- return pi;
- } else
- return K_.uncalibrate(pn, Dcal, boost::none);
+ return K_.uncalibrate(pn, Dcal);
}
/** project a point at infinity from world coordinate to the image
@@ -388,11 +266,10 @@ public:
* @param Dpoint is the Jacobian w.r.t. point3
* @param Dcal is the Jacobian w.r.t. calibration
*/
- inline Point2 projectPointAtInfinity(
- const Point3& pw, //
- boost::optional Dpose = boost::none,
- boost::optional Dpoint = boost::none,
- boost::optional Dcal = boost::none) const {
+ inline Point2 projectPointAtInfinity(const Point3& pw,
+ OptionalJacobian<2, 6> Dpose = boost::none,
+ OptionalJacobian<2, 2> Dpoint = boost::none,
+ OptionalJacobian<2, DimK> Dcal = boost::none) const {
if (!Dpose && !Dpoint && !Dcal) {
const Point3 pc = pose_.rotation().unrotate(pw); // get direction in camera frame (translation does not matter)
@@ -401,40 +278,30 @@ public:
}
// world to camera coordinate
- Matrix Dpc_rot /* 3*3 */, Dpc_point /* 3*3 */;
+ Matrix3 Dpc_rot, Dpc_point;
const Point3 pc = pose_.rotation().unrotate(pw, Dpc_rot, Dpc_point);
- Matrix Dpc_pose = Matrix::Zero(3, 6);
- Dpc_pose.block(0, 0, 3, 3) = Dpc_rot;
+ Matrix36 Dpc_pose;
+ Dpc_pose.setZero();
+ Dpc_pose.leftCols<3>() = Dpc_rot;
// camera to normalized image coordinate
Matrix23 Dpn_pc; // 2*3
const Point2 pn = project_to_camera(pc, Dpn_pc);
// uncalibration
- Matrix Dpi_pn; // 2*2
+ Matrix2 Dpi_pn; // 2*2
const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
// chain the Jacobian matrices
- const Matrix Dpi_pc = Dpi_pn * Dpn_pc;
+ const Matrix23 Dpi_pc = Dpi_pn * Dpn_pc;
if (Dpose)
*Dpose = Dpi_pc * Dpc_pose;
if (Dpoint)
- *Dpoint = (Dpi_pc * Dpc_point).block(0, 0, 2, 2); // only 2dof are important for the point (direction-only)
+ *Dpoint = (Dpi_pc * Dpc_point).leftCols<2>(); // only 2dof are important for the point (direction-only)
return pi;
}
- typedef Eigen::Matrix Matrix2K6;
-
- /** project a point from world coordinate to the image
- * @param pw is a point in the world coordinate
- */
- Point2 project2(const Point3& pw) const {
- const Point3 pc = pose_.transform_to(pw);
- const Point2 pn = project_to_camera(pc);
- return K_.uncalibrate(pn);
- }
-
/** project a point from world coordinate to the image, fixed Jacobians
* @param pw is a point in the world coordinate
* @param Dcamera is the Jacobian w.r.t. [pose3 calibration]
@@ -442,8 +309,8 @@ public:
*/
Point2 project2(
const Point3& pw, //
- boost::optional Dcamera,
- boost::optional Dpoint) const {
+ OptionalJacobian<2, DimC> Dcamera = boost::none,
+ OptionalJacobian<2, 3> Dpoint = boost::none) const {
const Point3 pc = pose_.transform_to(pw);
const Point2 pn = project_to_camera(pc);
@@ -459,8 +326,8 @@ public:
const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
if (Dcamera) { // TODO why does leftCols<6>() not compile ??
- calculateDpose(pn, d, Dpi_pn, Dcamera->leftCols(6));
- Dcamera->rightCols(K_.dim()) = Dcal; // Jacobian wrt calib
+ calculateDpose(pn, d, Dpi_pn, (*Dcamera).leftCols(6));
+ (*Dcamera).rightCols(DimK) = Dcal; // Jacobian wrt calib
}
if (Dpoint) {
calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint);
@@ -469,40 +336,6 @@ public:
}
}
- /** project a point from world coordinate to the image
- * @param pw is a point in the world coordinate
- * @param Dcamera is the Jacobian w.r.t. [pose3 calibration]
- * @param Dpoint is the Jacobian w.r.t. point3
- */
- Point2 project2(const Point3& pw, //
- boost::optional Dcamera, boost::optional Dpoint) const {
-
- const Point3 pc = pose_.transform_to(pw);
- const Point2 pn = project_to_camera(pc);
-
- if (!Dcamera && !Dpoint) {
- return K_.uncalibrate(pn);
- } else {
- const double z = pc.z(), d = 1.0 / z;
-
- // uncalibration
- Matrix2K Dcal;
- Matrix2 Dpi_pn;
- const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
-
- if (Dcamera) {
- Dcamera->resize(2, this->dim());
- calculateDpose(pn, d, Dpi_pn, Dcamera->leftCols<6>());
- Dcamera->rightCols(K_.dim()) = Dcal; // Jacobian wrt calib
- }
- if (Dpoint) {
- Dpoint->resize(2, 3);
- calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint);
- }
- return pi;
- }
- }
-
/// backproject a 2-dimensional point to a 3-dimensional point at given depth
inline Point3 backproject(const Point2& p, double depth) const {
const Point2 pn = K_.calibrate(p);
@@ -520,71 +353,64 @@ public:
/**
* Calculate range to a landmark
* @param point 3D location of landmark
- * @param Dpose the optionally computed Jacobian with respect to pose
+ * @param Dcamera the optionally computed Jacobian with respect to pose
* @param Dpoint the optionally computed Jacobian with respect to the landmark
* @return range (double)
*/
double range(
const Point3& point, //
- boost::optional Dpose = boost::none,
- boost::optional Dpoint = boost::none) const {
- double result = pose_.range(point, Dpose, Dpoint);
- if (Dpose) {
- // Add columns of zeros to Jacobian for calibration
- Matrix& H1r(*Dpose);
- H1r.conservativeResize(Eigen::NoChange, pose_.dim() + K_.dim());
- H1r.block(0, pose_.dim(), 1, K_.dim()) = Matrix::Zero(1, K_.dim());
- }
+ OptionalJacobian<1, DimC> Dcamera = boost::none,
+ OptionalJacobian<1, 3> Dpoint = boost::none) const {
+ Matrix16 Dpose_;
+ double result = pose_.range(point, Dcamera ? &Dpose_ : 0, Dpoint);
+ if (Dcamera)
+ *Dcamera << Dpose_, Eigen::Matrix::Zero();
return result;
}
/**
* Calculate range to another pose
* @param pose Other SO(3) pose
- * @param Dpose the optionally computed Jacobian with respect to pose
+ * @param Dcamera the optionally computed Jacobian with respect to pose
* @param Dpose2 the optionally computed Jacobian with respect to the other pose
* @return range (double)
*/
double range(
const Pose3& pose, //
- boost::optional Dpose = boost::none,
- boost::optional Dpose2 = boost::none) const {
- double result = pose_.range(pose, Dpose, Dpose2);
- if (Dpose) {
- // Add columns of zeros to Jacobian for calibration
- Matrix& H1r(*Dpose);
- H1r.conservativeResize(Eigen::NoChange, pose_.dim() + K_.dim());
- H1r.block(0, pose_.dim(), 1, K_.dim()) = Matrix::Zero(1, K_.dim());
- }
+ OptionalJacobian<1, DimC> Dcamera = boost::none,
+ OptionalJacobian<1, 6> Dpose = boost::none) const {
+ Matrix16 Dpose_;
+ double result = pose_.range(pose, Dcamera ? &Dpose_ : 0, Dpose);
+ if (Dcamera)
+ *Dcamera << Dpose_, Eigen::Matrix::Zero();
return result;
}
/**
* Calculate range to another camera
* @param camera Other camera
- * @param Dpose the optionally computed Jacobian with respect to pose
+ * @param Dcamera the optionally computed Jacobian with respect to pose
* @param Dother the optionally computed Jacobian with respect to the other camera
* @return range (double)
*/
template
double range(
const PinholeCamera& camera, //
- boost::optional Dpose = boost::none,
- boost::optional Dother = boost::none) const {
- double result = pose_.range(camera.pose_, Dpose, Dother);
- if (Dpose) {
- // Add columns of zeros to Jacobian for calibration
- Matrix& H1r(*Dpose);
- H1r.conservativeResize(Eigen::NoChange, pose_.dim() + K_.dim());
- H1r.block(0, pose_.dim(), 1, K_.dim()) = Matrix::Zero(1, K_.dim());
+ OptionalJacobian<1, DimC> Dcamera = boost::none,
+// OptionalJacobian<1, 6 + traits::dimension::value> Dother =
+ boost::optional Dother =
+ boost::none) const {
+ Matrix16 Dcamera_, Dother_;
+ double result = pose_.range(camera.pose(), Dcamera ? &Dcamera_ : 0,
+ Dother ? &Dother_ : 0);
+ if (Dcamera) {
+ Dcamera->resize(1, 6 + DimK);
+ *Dcamera << Dcamera_, Eigen::Matrix::Zero();
}
if (Dother) {
- // Add columns of zeros to Jacobian for calibration
- Matrix& H2r(*Dother);
- H2r.conservativeResize(Eigen::NoChange,
- camera.pose().dim() + camera.calibration().dim());
- H2r.block(0, camera.pose().dim(), 1, camera.calibration().dim()) =
- Matrix::Zero(1, camera.calibration().dim());
+ Dother->resize(1, 6+traits::dimension::value);
+ Dother->setZero();
+ Dother->block(0, 0, 1, 6) = Dother_;
}
return result;
}
@@ -592,15 +418,15 @@ public:
/**
* Calculate range to another camera
* @param camera Other camera
- * @param Dpose the optionally computed Jacobian with respect to pose
+ * @param Dcamera the optionally computed Jacobian with respect to pose
* @param Dother the optionally computed Jacobian with respect to the other camera
* @return range (double)
*/
double range(
const CalibratedCamera& camera, //
- boost::optional Dpose = boost::none,
- boost::optional Dother = boost::none) const {
- return pose_.range(camera.pose_, Dpose, Dother);
+ OptionalJacobian<1, DimC> Dcamera = boost::none,
+ OptionalJacobian<1, 6> Dother = boost::none) const {
+ return range(camera.pose(), Dcamera, Dother);
}
private:
@@ -663,7 +489,7 @@ private:
namespace traits {
template
-struct GTSAM_EXPORT is_manifold > : public boost::true_type{
+struct GTSAM_EXPORT is_manifold > : public boost::true_type {
};
template
diff --git a/gtsam/geometry/Point2.cpp b/gtsam/geometry/Point2.cpp
index 8b90aed63..17e3ef370 100644
--- a/gtsam/geometry/Point2.cpp
+++ b/gtsam/geometry/Point2.cpp
@@ -38,15 +38,9 @@ bool Point2::equals(const Point2& q, double tol) const {
}
/* ************************************************************************* */
-double Point2::norm() const {
- return sqrt(x_ * x_ + y_ * y_);
-}
-
-/* ************************************************************************* */
-double Point2::norm(boost::optional H) const {
- double r = norm();
+double Point2::norm(OptionalJacobian<1,2> H) const {
+ double r = sqrt(x_ * x_ + y_ * y_);
if (H) {
- H->resize(1,2);
if (fabs(r) > 1e-10)
*H << x_ / r, y_ / r;
else
@@ -56,12 +50,11 @@ double Point2::norm(boost::optional H) const {
}
/* ************************************************************************* */
-static const Matrix I2 = eye(2);
-double Point2::distance(const Point2& point, boost::optional H1,
- boost::optional H2) const {
+double Point2::distance(const Point2& point, OptionalJacobian<1,2> H1,
+ OptionalJacobian<1,2> H2) const {
Point2 d = point - *this;
if (H1 || H2) {
- Matrix H;
+ Matrix12 H;
double r = d.norm(H);
if (H1) *H1 = -H;
if (H2) *H2 = H;
diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h
index a4e0cc296..8c8e03db4 100644
--- a/gtsam/geometry/Point2.h
+++ b/gtsam/geometry/Point2.h
@@ -20,7 +20,7 @@
#include
#include
-#include
+#include
#include
namespace gtsam {
@@ -125,10 +125,10 @@ public:
/// "Compose", just adds the coordinates of two points. With optional derivatives
inline Point2 compose(const Point2& q,
- boost::optional H1=boost::none,
- boost::optional H2=boost::none) const {
- if(H1) *H1 = eye(2);
- if(H2) *H2 = eye(2);
+ OptionalJacobian<2,2> H1=boost::none,
+ OptionalJacobian<2,2> H2=boost::none) const {
+ if(H1) *H1 = I_2x2;
+ if(H2) *H2 = I_2x2;
return *this + q;
}
@@ -137,10 +137,10 @@ public:
/// "Between", subtracts point coordinates. between(p,q) == compose(inverse(p),q)
inline Point2 between(const Point2& q,
- boost::optional H1=boost::none,
- boost::optional H2=boost::none) const {
- if(H1) *H1 = -eye(2);
- if(H2) *H2 = eye(2);
+ OptionalJacobian<2,2> H1=boost::none,
+ OptionalJacobian<2,2> H2=boost::none) const {
+ if(H1) *H1 = -I_2x2;
+ if(H2) *H2 = I_2x2;
return q - (*this);
}
@@ -180,15 +180,12 @@ public:
/** creates a unit vector */
Point2 unit() const { return *this/norm(); }
- /** norm of point */
- double norm() const;
-
/** norm of point, with derivative */
- double norm(boost::optional H) const;
+ double norm(OptionalJacobian<1,2> H = boost::none) const;
/** distance between two points */
- double distance(const Point2& p2, boost::optional H1 = boost::none,
- boost::optional H2 = boost::none) const;
+ double distance(const Point2& p2, OptionalJacobian<1,2> H1 = boost::none,
+ OptionalJacobian<1,2> H2 = boost::none) const;
/** @deprecated The following function has been deprecated, use distance above */
inline double dist(const Point2& p2) const {
diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp
index ce56c78c1..330fafb97 100644
--- a/gtsam/geometry/Point3.cpp
+++ b/gtsam/geometry/Point3.cpp
@@ -63,22 +63,18 @@ Point3 Point3::operator/(double s) const {
}
/* ************************************************************************* */
-Point3 Point3::add(const Point3 &q, boost::optional H1,
- boost::optional H2) const {
- if (H1)
- *H1 = eye(3, 3);
- if (H2)
- *H2 = eye(3, 3);
+Point3 Point3::add(const Point3 &q, OptionalJacobian<3,3> H1,
+ OptionalJacobian<3,3> H2) const {
+ if (H1) *H1 = I_3x3;
+ if (H2) *H2 = I_3x3;
return *this + q;
}
/* ************************************************************************* */
-Point3 Point3::sub(const Point3 &q, boost::optional H1,
- boost::optional H2) const {
- if (H1)
- *H1 = eye(3, 3);
- if (H2)
- *H2 = -eye(3, 3);
+Point3 Point3::sub(const Point3 &q, OptionalJacobian<3,3> H1,
+ OptionalJacobian<3,3> H2) const {
+ if (H1) *H1 = I_3x3;
+ if (H2) *H2 = I_3x3;
return *this - q;
}
@@ -94,26 +90,8 @@ double Point3::dot(const Point3 &q) const {
}
/* ************************************************************************* */
-double Point3::norm() const {
- return sqrt(x_ * x_ + y_ * y_ + z_ * z_);
-}
-
-/* ************************************************************************* */
-double Point3::norm(boost::optional H) const {
- double r = norm();
- if (H) {
- H->resize(1,3);
- if (fabs(r) > 1e-10)
- *H << x_ / r, y_ / r, z_ / r;
- else
- *H << 1, 1, 1; // really infinity, why 1 ?
- }
- return r;
-}
-
-/* ************************************************************************* */
-double Point3::norm(boost::optional&> H) const {
- double r = norm();
+double Point3::norm(OptionalJacobian<1,3> H) const {
+ double r = sqrt(x_ * x_ + y_ * y_ + z_ * z_);
if (H) {
if (fabs(r) > 1e-10)
*H << x_ / r, y_ / r, z_ / r;
@@ -124,13 +102,12 @@ double Point3::norm(boost::optional&> H) const {
}
/* ************************************************************************* */
-Point3 Point3::normalize(boost::optional H) const {
+Point3 Point3::normalize(OptionalJacobian<3,3> H) const {
Point3 normalized = *this / norm();
if (H) {
// 3*3 Derivative
double x2 = x_ * x_, y2 = y_ * y_, z2 = z_ * z_;
double xy = x_ * y_, xz = x_ * z_, yz = y_ * z_;
- H->resize(3, 3);
*H << y2 + z2, -xy, -xz, /**/-xy, x2 + z2, -yz, /**/-xz, -yz, x2 + y2;
*H /= pow(x2 + y2 + z2, 1.5);
}
diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h
index b7f7f8ffa..56d9b8bef 100644
--- a/gtsam/geometry/Point3.h
+++ b/gtsam/geometry/Point3.h
@@ -21,9 +21,9 @@
#pragma once
-#include
#include
#include
+#include
#include
@@ -93,10 +93,10 @@ namespace gtsam {
/// "Compose" - just adds coordinates of two points
inline Point3 compose(const Point3& p2,
- boost::optional H1=boost::none,
- boost::optional H2=boost::none) const {
- if (H1) *H1 = eye(3);
- if (H2) *H2 = eye(3);
+ OptionalJacobian<3,3> H1=boost::none,
+ OptionalJacobian<3,3> H2=boost::none) const {
+ if (H1) *H1 << I_3x3;
+ if (H2) *H2 << I_3x3;
return *this + p2;
}
@@ -105,10 +105,10 @@ namespace gtsam {
/** Between using the default implementation */
inline Point3 between(const Point3& p2,
- boost::optional H1=boost::none,
- boost::optional H2=boost::none) const {
- if(H1) *H1 = -eye(3);
- if(H2) *H2 = eye(3);
+ OptionalJacobian<3,3> H1=boost::none,
+ OptionalJacobian<3,3> H2=boost::none) const {
+ if(H1) *H1 = -I_3x3;
+ if(H2) *H2 = I_3x3;
return p2 - *this;
}
@@ -142,13 +142,13 @@ namespace gtsam {
static inline Vector3 Logmap(const Point3& dp) { return Vector3(dp.x(), dp.y(), dp.z()); }
/// Left-trivialized derivative of the exponential map
- static Matrix dexpL(const Vector& v) {
- return eye(3);
+ static Matrix3 dexpL(const Vector& v) {
+ return I_3x3;
}
/// Left-trivialized derivative inverse of the exponential map
- static Matrix dexpInvL(const Vector& v) {
- return eye(3);
+ static Matrix3 dexpInvL(const Vector& v) {
+ return I_3x3;
}
/// @}
@@ -163,14 +163,16 @@ namespace gtsam {
/** distance between two points */
inline double distance(const Point3& p2,
- boost::optional H1 = boost::none, boost::optional H2 = boost::none) const {
+ OptionalJacobian<1,3> H1 = boost::none, OptionalJacobian<1,3> H2 = boost::none) const {
double d = (p2 - *this).norm();
if (H1) {
- *H1 = (Matrix(1, 3) << x_-p2.x(), y_-p2.y(), z_-p2.z()).finished()*(1./d);
+ *H1 << x_-p2.x(), y_-p2.y(), z_-p2.z();
+ *H1 = *H1 *(1./d);
}
if (H2) {
- *H2 = (Matrix(1, 3) << -x_+p2.x(), -y_+p2.y(), -z_+p2.z()).finished()*(1./d);
+ *H2 << -x_+p2.x(), -y_+p2.y(), -z_+p2.z();
+ *H2 << *H2 *(1./d);
}
return d;
}
@@ -180,17 +182,11 @@ namespace gtsam {
return (p2 - *this).norm();
}
- /** Distance of the point from the origin */
- double norm() const;
-
/** Distance of the point from the origin, with Jacobian */
- double norm(boost::optional H) const;
-
- /** Distance of the point from the origin, with Jacobian */
- double norm(boost::optional&> H) const;
+ double norm(OptionalJacobian<1,3> H = boost::none) const;
/** normalize, with optional Jacobian */
- Point3 normalize(boost::optional H = boost::none) const;
+ Point3 normalize(OptionalJacobian<3, 3> H = boost::none) const;
/** cross product @return this x q */
Point3 cross(const Point3 &q) const;
@@ -219,11 +215,11 @@ namespace gtsam {
/** add two points, add(this,q) is same as this + q */
Point3 add (const Point3 &q,
- boost::optional H1=boost::none, boost::optional H2=boost::none) const;
+ OptionalJacobian<3, 3> H1=boost::none, OptionalJacobian<3, 3> H2=boost::none) const;
/** subtract two points, sub(this,q) is same as this - q */
Point3 sub (const Point3 &q,
- boost::optional H1=boost::none, boost::optional H2=boost::none) const;
+ OptionalJacobian<3,3> H1=boost::none, OptionalJacobian<3,3> H2=boost::none) const;
/// @}
diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp
index 7a693ed3d..c9aab4185 100644
--- a/gtsam/geometry/Pose2.cpp
+++ b/gtsam/geometry/Pose2.cpp
@@ -33,15 +33,20 @@ INSTANTIATE_LIE(Pose2);
/** instantiate concept checks */
GTSAM_CONCEPT_POSE_INST(Pose2);
-static const Matrix I3 = eye(3), Z12 = zeros(1,2);
static const Rot2 R_PI_2(Rot2::fromCosSin(0., 1.));
/* ************************************************************************* */
-Matrix Pose2::matrix() const {
- Matrix R = r_.matrix();
- R = stack(2, &R, &Z12);
- Matrix T = (Matrix(3, 1) << t_.x(), t_.y(), 1.0).finished();
- return collect(2, &R, &T);
+Matrix3 Pose2::matrix() const {
+ Matrix2 R = r_.matrix();
+ Matrix32 R0;
+ R0.block<2,2>(0,0) = R;
+ R0.block<1,2>(2,0).setZero();
+ Matrix31 T;
+ T << t_.x(), t_.y(), 1.0;
+ Matrix3 RT_;
+ RT_.block<3,2>(0,0) = R0;
+ RT_.block<3,1>(0,2) = T;
+ return RT_;
}
/* ************************************************************************* */
@@ -108,105 +113,64 @@ Vector Pose2::localCoordinates(const Pose2& p2) const {
/* ************************************************************************* */
// Calculate Adjoint map
// Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi)
-Matrix Pose2::AdjointMap() const {
+Matrix3 Pose2::AdjointMap() const {
double c = r_.c(), s = r_.s(), x = t_.x(), y = t_.y();
- return (Matrix(3, 3) <<
+ Matrix3 rvalue;
+ rvalue <<
c, -s, y,
s, c, -x,
- 0.0, 0.0, 1.0
- ).finished();
+ 0.0, 0.0, 1.0;
+ return rvalue;
}
/* ************************************************************************* */
-Pose2 Pose2::inverse(boost::optional H1) const {
+Pose2 Pose2::inverse(OptionalJacobian<3,3> H1) const {
if (H1) *H1 = -AdjointMap();
return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y())));
}
-/* ************************************************************************* */
-// see doc/math.lyx, SE(2) section
-Point2 Pose2::transform_to(const Point2& point) const {
- Point2 d = point - t_;
- return r_.unrotate(d);
-}
-
/* ************************************************************************* */
// see doc/math.lyx, SE(2) section
Point2 Pose2::transform_to(const Point2& point,
- boost::optional H1, boost::optional H2) const {
+ OptionalJacobian<2, 3> H1, OptionalJacobian<2, 2> H2) const {
Point2 d = point - t_;
Point2 q = r_.unrotate(d);
if (!H1 && !H2) return q;
if (H1) *H1 <<
-1.0, 0.0, q.y(),
0.0, -1.0, -q.x();
- if (H2) *H2 = r_.transpose();
+ if (H2) *H2 << r_.transpose();
return q;
}
/* ************************************************************************* */
// see doc/math.lyx, SE(2) section
-Point2 Pose2::transform_to(const Point2& point,
- boost::optional H1, boost::optional H2) const {
- Point2 d = point - t_;
- Point2 q = r_.unrotate(d);
- if (!H1 && !H2) return q;
- if (H1) *H1 = (Matrix(2, 3) <<
- -1.0, 0.0, q.y(),
- 0.0, -1.0, -q.x()).finished();
- if (H2) *H2 = r_.transpose();
- return q;
-}
-
-/* ************************************************************************* */
-// see doc/math.lyx, SE(2) section
-Pose2 Pose2::compose(const Pose2& p2, boost::optional H1,
- boost::optional H2) const {
+Pose2 Pose2::compose(const Pose2& p2, OptionalJacobian<3,3> H1,
+ OptionalJacobian<3,3> H2) const {
// TODO: inline and reuse?
if(H1) *H1 = p2.inverse().AdjointMap();
- if(H2) *H2 = I3;
+ if(H2) *H2 = I_3x3;
return (*this)*p2;
}
/* ************************************************************************* */
// see doc/math.lyx, SE(2) section
Point2 Pose2::transform_from(const Point2& p,
- boost::optional H1, boost::optional H2) const {
+ OptionalJacobian<2, 3> H1, OptionalJacobian<2, 2> H2) const {
const Point2 q = r_ * p;
if (H1 || H2) {
- const Matrix R = r_.matrix();
- const Matrix Drotate1 = (Matrix(2, 1) << -q.y(), q.x()).finished();
- if (H1) *H1 = collect(2, &R, &Drotate1); // [R R_{pi/2}q]
- if (H2) *H2 = R; // R
+ const Matrix2 R = r_.matrix();
+ Matrix21 Drotate1;
+ Drotate1 << -q.y(), q.x();
+ if (H1) *H1 << R, Drotate1;
+ if (H2) *H2 = R; // R
}
return q + t_;
}
/* ************************************************************************* */
-Pose2 Pose2::between(const Pose2& p2) const {
- // get cosines and sines from rotation matrices
- const Rot2& R1 = r_, R2 = p2.r();
- double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s();
-
- // Assert that R1 and R2 are normalized
- assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5);
-
- // Calculate delta rotation = between(R1,R2)
- double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2;
- Rot2 R(Rot2::atan2(s,c)); // normalizes
-
- // Calculate delta translation = unrotate(R1, dt);
- Point2 dt = p2.t() - t_;
- double x = dt.x(), y = dt.y();
- // t = R1' * (t2-t1)
- Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y);
-
- return Pose2(R,t);
-}
-
-/* ************************************************************************* */
-Pose2 Pose2::between(const Pose2& p2, boost::optional H1,
- boost::optional H2) const {
+Pose2 Pose2::between(const Pose2& p2, OptionalJacobian<3,3> H1,
+ OptionalJacobian<3,3> H2) const {
// get cosines and sines from rotation matrices
const Rot2& R1 = r_, R2 = p2.r();
double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s();
@@ -233,97 +197,75 @@ Pose2 Pose2::between(const Pose2& p2, boost::optional H1,
s, -c, dt2,
0.0, 0.0,-1.0;
}
- if (H2) *H2 = I3;
-
- return Pose2(R,t);
-}
-
-/* ************************************************************************* */
-Pose2 Pose2::between(const Pose2& p2, boost::optional H1,
- boost::optional H2) const {
- // get cosines and sines from rotation matrices
- const Rot2& R1 = r_, R2 = p2.r();
- double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s();
-
- // Assert that R1 and R2 are normalized
- assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5);
-
- // Calculate delta rotation = between(R1,R2)
- double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2;
- Rot2 R(Rot2::atan2(s,c)); // normalizes
-
- // Calculate delta translation = unrotate(R1, dt);
- Point2 dt = p2.t() - t_;
- double x = dt.x(), y = dt.y();
- // t = R1' * (t2-t1)
- Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y);
-
- // FD: This is just -AdjointMap(between(p2,p1)) inlined and re-using above
- if (H1) {
- double dt1 = -s2 * x + c2 * y;
- double dt2 = -c2 * x - s2 * y;
- *H1 = (Matrix(3, 3) <<
- -c, -s, dt1,
- s, -c, dt2,
- 0.0, 0.0,-1.0).finished();
- }
- if (H2) *H2 = I3;
+ if (H2) *H2 = I_3x3;
return Pose2(R,t);
}
/* ************************************************************************* */
Rot2 Pose2::bearing(const Point2& point,
- boost::optional H1, boost::optional H2) const {
- Point2 d = transform_to(point, H1, H2);
+ OptionalJacobian<1, 3> H1, OptionalJacobian<1, 2> H2) const {
+ // make temporary matrices
+ Matrix23 D1; Matrix2 D2;
+ Point2 d = transform_to(point, H1 ? &D1 : 0, H2 ? &D2 : 0); // uses pointer version
if (!H1 && !H2) return Rot2::relativeBearing(d);
- Matrix D_result_d;
+ Matrix12 D_result_d;
Rot2 result = Rot2::relativeBearing(d, D_result_d);
- if (H1) *H1 = D_result_d * (*H1);
- if (H2) *H2 = D_result_d * (*H2);
+ if (H1) *H1 = D_result_d * (D1);
+ if (H2) *H2 = D_result_d * (D2);
return result;
}
/* ************************************************************************* */
-Rot2 Pose2::bearing(const Pose2& point,
- boost::optional H1, boost::optional H2) const {
- Rot2 result = bearing(point.t(), H1, H2);
+Rot2 Pose2::bearing(const Pose2& pose,
+ OptionalJacobian<1, 3> H1, OptionalJacobian<1, 3> H2) const {
+ Matrix12 D2;
+ Rot2 result = bearing(pose.t(), H1, H2 ? &D2 : 0);
if (H2) {
- Matrix H2_ = *H2 * point.r().matrix();
- *H2 = zeros(1, 3);
- insertSub(*H2, H2_, 0, 0);
+ Matrix12 H2_ = D2 * pose.r().matrix();
+ *H2 << H2_, Z_1x1;
}
return result;
}
-
/* ************************************************************************* */
double Pose2::range(const Point2& point,
- boost::optional H1, boost::optional H2) const {
+ OptionalJacobian<1,3> H1, OptionalJacobian<1,2> H2) const {
Point2 d = point - t_;
if (!H1 && !H2) return d.norm();
- Matrix H;
+ Matrix12 H;
double r = d.norm(H);
- if (H1) *H1 = H * (Matrix(2, 3) <<
- -r_.c(), r_.s(), 0.0,
- -r_.s(), -r_.c(), 0.0).finished();
+ if (H1) {
+ Matrix23 H1_;
+ H1_ << -r_.c(), r_.s(), 0.0,
+ -r_.s(), -r_.c(), 0.0;
+ *H1 = H * H1_;
+ }
if (H2) *H2 = H;
return r;
}
/* ************************************************************************* */
-double Pose2::range(const Pose2& pose2,
- boost::optional H1,
- boost::optional H2) const {
- Point2 d = pose2.t() - t_;
+double Pose2::range(const Pose2& pose,
+ OptionalJacobian<1,3> H1,
+ OptionalJacobian<1,3> H2) const {
+ Point2 d = pose.t() - t_;
if (!H1 && !H2) return d.norm();
- Matrix H;
+ Matrix12 H;
double r = d.norm(H);
- if (H1) *H1 = H * (Matrix(2, 3) <<
+ if (H1) {
+ Matrix23 H1_;
+ H1_ <<
-r_.c(), r_.s(), 0.0,
- -r_.s(), -r_.c(), 0.0).finished();
- if (H2) *H2 = H * (Matrix(2, 3) <<
- pose2.r_.c(), -pose2.r_.s(), 0.0,
- pose2.r_.s(), pose2.r_.c(), 0.0).finished();
+ -r_.s(), -r_.c(), 0.0;
+ *H1 = H * H1_;
+ }
+ if (H2) {
+ Matrix23 H2_;
+ H2_ <<
+ pose.r_.c(), -pose.r_.s(), 0.0,
+ pose.r_.s(), pose.r_.c(), 0.0;
+ *H2 = H * H2_;
+ }
return r;
}
diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h
index d43be6afb..49eb444b2 100644
--- a/gtsam/geometry/Pose2.h
+++ b/gtsam/geometry/Pose2.h
@@ -107,12 +107,12 @@ public:
inline static Pose2 identity() { return Pose2(); }
/// inverse transformation with derivatives
- Pose2 inverse(boost::optional H1=boost::none) const;
+ Pose2 inverse(OptionalJacobian<3, 3> H1=boost::none) const;
/// compose this transformation onto another (first *this and then p2)
Pose2 compose(const Pose2& p2,
- boost::optional