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

BIG CHANGE: the OptionalJacobian<M,N> obviates the need for the `optional<Matrix&> Jacobian arguments. They will continue to exist, for backwards compatibility, in the old-style factors, but everywhere else they should disappear. This PR by Natesh has eradicated all but one in gtsam/geometry. Great job !!!! 

Merged in feature/fixedSizeDerivatives (pull request #54)

Proposed way to do Jacobians from now on via Eigen::Ref like type
release/4.3a0
Frank Dellaert 2014-12-05 22:54:36 +01:00
parent a7b61373a9 468aa5d6b6
commit 8cc26c759d
66 changed files with 1432 additions and 1705 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
.cproject
View File

@ -854,6 +854,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSmartStereoProjectionPoseFactor.run" path="build/gtsam_unstable/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testSmartStereoProjectionPoseFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianFactorGraph.run" path="build/gtsam/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -2953,6 +2961,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testRangeFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testRangeFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="SimpleRotation.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>

2
.gitignore vendored
View File

@ -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

16
gtsam.h
View File

@ -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<gtsam::Pose2, gtsam::Point2> RangeFactorPosePoint2;
typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Point3> RangeFactorPosePoint3;
typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Pose2> RangeFactorPose2;
typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Pose3> RangeFactorPose3;
typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::Point3> RangeFactorCalibratedCameraPoint;
typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::Point3> RangeFactorSimpleCameraPoint;
typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::CalibratedCamera> RangeFactorCalibratedCamera;
typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::SimpleCamera> 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<gtsam::CalibratedCamera, gtsam::Point3> RangeFactorCalibratedCameraPoint;
//typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::Point3> RangeFactorSimpleCameraPoint;
//typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::CalibratedCamera> RangeFactorCalibratedCamera;
//typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::SimpleCamera> RangeFactorSimpleCamera;
#include <gtsam/slam/BearingFactor.h>

48
gtsam/base/Matrix.h
View File

@ -37,36 +37,36 @@ namespace gtsam {
typedef Eigen::MatrixXd Matrix;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixRowMajor;
typedef Eigen::Matrix2d Matrix2;
typedef Eigen::Matrix3d Matrix3;
typedef Eigen::Matrix4d Matrix4;
typedef Eigen::Matrix<double,5,5> Matrix5;
typedef Eigen::Matrix<double,6,6> 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<double, SIZE, SIZE> Matrix##SUFFIX; \
typedef Eigen::Matrix<double, 1, SIZE> Matrix1##SUFFIX; \
typedef Eigen::Matrix<double, 2, SIZE> Matrix2##SUFFIX; \
typedef Eigen::Matrix<double, 3, SIZE> Matrix3##SUFFIX; \
typedef Eigen::Matrix<double, 4, SIZE> Matrix4##SUFFIX; \
typedef Eigen::Matrix<double, 5, SIZE> Matrix5##SUFFIX; \
typedef Eigen::Matrix<double, 6, SIZE> Matrix6##SUFFIX; \
typedef Eigen::Matrix<double, 7, SIZE> Matrix7##SUFFIX; \
typedef Eigen::Matrix<double, 8, SIZE> Matrix8##SUFFIX; \
typedef Eigen::Matrix<double, 9, SIZE> Matrix9##SUFFIX; \
static const Eigen::MatrixBase<Matrix##SUFFIX>::IdentityReturnType I_##SUFFIX##x##SUFFIX = Matrix##SUFFIX::Identity(); \
static const Eigen::MatrixBase<Matrix##SUFFIX>::ConstantReturnType Z_##SUFFIX##x##SUFFIX = Matrix##SUFFIX::Zero();
typedef Eigen::Matrix<double,2,3> Matrix23;
typedef Eigen::Matrix<double,2,4> Matrix24;
typedef Eigen::Matrix<double,2,5> Matrix25;
typedef Eigen::Matrix<double,2,6> Matrix26;
typedef Eigen::Matrix<double,2,7> Matrix27;
typedef Eigen::Matrix<double,2,8> Matrix28;
typedef Eigen::Matrix<double,2,9> Matrix29;
typedef Eigen::Matrix<double,3,2> Matrix32;
typedef Eigen::Matrix<double,3,4> Matrix34;
typedef Eigen::Matrix<double,3,5> Matrix35;
typedef Eigen::Matrix<double,3,6> Matrix36;
typedef Eigen::Matrix<double,3,7> Matrix37;
typedef Eigen::Matrix<double,3,8> Matrix38;
typedef Eigen::Matrix<double,3,9> 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<Matrix> SubMatrix;
typedef Eigen::Block<const Matrix> ConstSubMatrix;
// Two very commonly used matrices:
const Matrix3 Z_3x3 = Matrix3::Zero();
const Matrix3 I_3x3 = Matrix3::Identity();
// Matlab-like syntax
/**

115
gtsam/base/OptionalJacobian.h Normal file
View File

@ -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 <gtsam/3rdparty/Eigen/Eigen/Dense>
#ifndef OPTIONALJACOBIAN_NOBOOST
#include <boost/optional.hpp>
#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<Eigen::MatrixXd&> is also supported for backwards compatibility.
*/
template<int Rows, int Cols>
class OptionalJacobian {
public:
/// Fixed size type
typedef Eigen::Matrix<double, Rows, Cols> Fixed;
private:
Eigen::Map<Fixed> 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<Fixed>(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<Eigen::MatrixXd&> 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<Fixed>& operator*() {
return map_;
}
/// TODO: operator->()
Eigen::Map<Fixed>* operator->(){ return &map_; }
};
} // namespace gtsam

105
gtsam/base/tests/testOptionalJacobian.cpp Normal file
View File

@ -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 <gtsam/base/Matrix.h>
#include <gtsam/base/OptionalJacobian.h>
#include <CppUnitLite/TestHarness.h>
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<Matrix&> 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);
}
//******************************************************************************

66
gtsam/geometry/Cal3Bundler.cpp
View File

@ -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<Matrix23&> Dcal, boost::optional<Matrix2&> 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<Matrix&> Dcal, boost::optional<Matrix&> 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;
}

34
gtsam/geometry/Cal3Bundler.h
View File

@ -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<Matrix23&> Dcal,
boost::optional<Matrix2&> 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<Matrix&> Dcal,
boost::optional<Matrix&> 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

35
gtsam/geometry/Cal3DS2_Base.cpp
View File

@ -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<Matrix29&> H1,
boost::optional<Matrix2&> 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<Matrix&> H1,
boost::optional<Matrix&> 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;

15
gtsam/geometry/Cal3DS2_Base.h
View File

@ -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<Matrix29&> Dcal = boost::none,
boost::optional<Matrix2&> Dp = boost::none) const ;
Point2 uncalibrate(const Point2& p,
boost::optional<Matrix&> Dcal,
boost::optional<Matrix&> 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:

11
gtsam/geometry/Cal3Unified.cpp
View File

@ -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<Matrix&> H1,
boost::optional<Matrix&> 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;

4
gtsam/geometry/Cal3Unified.h
View File

@ -96,8 +96,8 @@ public:
* @return point in image coordinates
*/
Point2 uncalibrate(const Point2& p,
boost::optional<Matrix&> Dcal = boost::none,
boost::optional<Matrix&> 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;

29
gtsam/geometry/Cal3_S2.cpp
View File

@ -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<Matrix&> Dcal,
boost::optional<Matrix&> 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<Matrix25&> Dcal,
boost::optional<Matrix2&> 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_);
}

45
gtsam/geometry/Cal3_S2.h
View File

@ -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<Matrix25&> Dcal,
boost::optional<Matrix2&> 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<Matrix&> Dcal,
boost::optional<Matrix&> 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<Matrix&> H1=boost::none,
boost::optional<Matrix&> 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_);
}

33
gtsam/geometry/CalibratedCamera.cpp
View File

@ -33,10 +33,10 @@ CalibratedCamera::CalibratedCamera(const Vector &v) :
/* ************************************************************************* */
Point2 CalibratedCamera::project_to_camera(const Point3& P,
boost::optional<Matrix&> 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<Matrix&> Dpose, boost::optional<Matrix&> 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
}

48
gtsam/geometry/CalibratedCamera.h
View File

@ -18,9 +18,8 @@
#pragma once
#include <gtsam/base/DerivedValue.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Point2.h>
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<Matrix&> H1 = boost::none, boost::optional<Matrix&> 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<Matrix&> H1 = boost::none, boost::optional<Matrix&> 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<Matrix&> 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<Matrix&> Dpose = boost::none,
boost::optional<Matrix&> 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<Matrix&> 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<Matrix&> H1 = boost::none,
boost::optional<Matrix&> 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<Matrix&> H1 = boost::none,
boost::optional<Matrix&> 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<Matrix&> H1 =
boost::none, boost::optional<Matrix&> 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<CalibratedCamera> : public boost::true_type{
struct GTSAM_EXPORT is_group<CalibratedCamera> : public boost::true_type {
};
template<>
struct GTSAM_EXPORT is_manifold<CalibratedCamera> : public boost::true_type{
struct GTSAM_EXPORT is_manifold<CalibratedCamera> : public boost::true_type {
};
template<>
struct GTSAM_EXPORT dimension<CalibratedCamera> : public boost::integral_constant<int, 6>{
struct GTSAM_EXPORT dimension<CalibratedCamera> : public boost::integral_constant<
int, 6> {
};
}

49
gtsam/geometry/EssentialMatrix.cpp
View File

@ -14,7 +14,7 @@ namespace gtsam {
/* ************************************************************************* */
EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _1P2_,
boost::optional<Matrix&> 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<Matrix&> DE, boost::optional<Matrix&> 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<Matrix&> HE, boost::optional<Matrix&> 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<Matrix&> 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;
}

14
gtsam/geometry/EssentialMatrix.h
View File

@ -50,7 +50,7 @@ public:
/// Named constructor converting a Pose3 with scale to EssentialMatrix (no scale)
static EssentialMatrix FromPose3(const Pose3& _1P2_,
boost::optional<Matrix&> H = boost::none);
OptionalJacobian<5, 6> H = boost::none);
/// Random, using Rot3::Random and Unit3::Random
template<typename Engine>
@ -132,16 +132,16 @@ public:
* @return point in pose coordinates
*/
Point3 transform_to(const Point3& p,
boost::optional<Matrix&> DE = boost::none,
boost::optional<Matrix&> 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<Matrix&> HE =
boost::none, boost::optional<Matrix&> 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<Matrix&> H = boost::none) const;
double error(const Vector3& vA, const Vector3& vB, //
OptionalJacobian<1,5> H = boost::none) const;
/// @}

336
gtsam/geometry/PinholeCamera.h
View File

@ -18,16 +18,9 @@
#pragma once
#include <cmath>
#include <boost/optional.hpp>
#include <boost/serialization/nvp.hpp>
#include <gtsam/base/DerivedValue.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/geometry/Pose2.h>
#include <cmath>
namespace gtsam {
@ -42,7 +35,9 @@ private:
Pose3 pose_;
Calibration K_;
static const int DimK = traits::dimension<Calibration>::value;
// Get dimensions of calibration type and This at compile time
static const int DimK = traits::dimension<Calibration>::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<Matrix&> H1 = boost::none, boost::optional<Matrix&> 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<Matrix&> H1 = boost::none, boost::optional<Matrix&> 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<Matrix&> 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<double,6+DimK,1> VectorK6;
/// Manifold dimension
inline static size_t Dim() {
return DimC;
}
typedef Eigen::Matrix<double, DimC, 1> 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<Matrix23&> 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<double,2,DimK> Matrix2K;
typedef Eigen::Matrix<double, 2, DimK> 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<Matrix26&> Dpose,
boost::optional<Matrix23&> Dpoint,
boost::optional<Matrix2K&> 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<Matrix&> Dpose,
boost::optional<Matrix&> Dpoint,
boost::optional<Matrix&> 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<Matrix&> Dpose = boost::none,
boost::optional<Matrix&> Dpoint = boost::none,
boost::optional<Matrix&> 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<double,2,6+DimK> 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<Matrix2K6&> Dcamera,
boost::optional<Matrix23&> 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<Matrix&> Dcamera, boost::optional<Matrix&> 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<Matrix&> Dpose = boost::none,
boost::optional<Matrix&> 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<double, 1, DimK>::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<Matrix&> Dpose = boost::none,
boost::optional<Matrix&> 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<double, 1, DimK>::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<class CalibrationB>
double range(
const PinholeCamera<CalibrationB>& camera, //
boost::optional<Matrix&> Dpose = boost::none,
boost::optional<Matrix&> 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<CalibrationB>::value> Dother =
boost::optional<Matrix&> 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<double, 1, DimK>::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<CalibrationB>::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<Matrix&> Dpose = boost::none,
boost::optional<Matrix&> 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<typename Calibration>
struct GTSAM_EXPORT is_manifold<PinholeCamera<Calibration> > : public boost::true_type{
struct GTSAM_EXPORT is_manifold<PinholeCamera<Calibration> > : public boost::true_type {
};
template<typename Calibration>

17
gtsam/geometry/Point2.cpp
View File

@ -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<Matrix&> 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<Matrix&> H) const {
}
/* ************************************************************************* */
static const Matrix I2 = eye(2);
double Point2::distance(const Point2& point, boost::optional<Matrix&> H1,
boost::optional<Matrix&> 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;

27
gtsam/geometry/Point2.h
View File

@ -20,7 +20,7 @@
#include <boost/serialization/nvp.hpp>
#include <gtsam/base/DerivedValue.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/base/Lie.h>
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<Matrix&> H1=boost::none,
boost::optional<Matrix&> 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<Matrix&> H1=boost::none,
boost::optional<Matrix&> 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<Matrix&> H) const;
double norm(OptionalJacobian<1,2> H = boost::none) const;
/** distance between two points */
double distance(const Point2& p2, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> 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 {

45
gtsam/geometry/Point3.cpp
View File

@ -63,22 +63,18 @@ Point3 Point3::operator/(double s) const {
}
/* ************************************************************************* */
Point3 Point3::add(const Point3 &q, boost::optional<Matrix&> H1,
boost::optional<Matrix&> 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<Matrix&> H1,
boost::optional<Matrix&> 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<Matrix&> 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<Eigen::Matrix<double,1,3>&> 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<Eigen::Matrix<double,1,3>&> H) const {
}
/* ************************************************************************* */
Point3 Point3::normalize(boost::optional<Matrix&> 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);
}

48
gtsam/geometry/Point3.h
View File

@ -21,9 +21,9 @@
#pragma once
#include <gtsam/base/Matrix.h>
#include <gtsam/base/DerivedValue.h>
#include <gtsam/base/Lie.h>
#include <gtsam/base/OptionalJacobian.h>
#include <boost/serialization/nvp.hpp>
@ -93,10 +93,10 @@ namespace gtsam {
/// "Compose" - just adds coordinates of two points
inline Point3 compose(const Point3& p2,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> 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<Matrix&> H1=boost::none,
boost::optional<Matrix&> 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<Matrix&> H1 = boost::none, boost::optional<Matrix&> 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<Matrix&> H) const;
/** Distance of the point from the origin, with Jacobian */
double norm(boost::optional<Eigen::Matrix<double,1,3>&> H) const;
double norm(OptionalJacobian<1,3> H = boost::none) const;
/** normalize, with optional Jacobian */
Point3 normalize(boost::optional<Matrix&> 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<Matrix&> H1=boost::none, boost::optional<Matrix&> 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<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
OptionalJacobian<3,3> H1=boost::none, OptionalJacobian<3,3> H2=boost::none) const;
/// @}

198
gtsam/geometry/Pose2.cpp
View File

@ -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<Matrix&> 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<Matrix23&> H1, boost::optional<Matrix2&> 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<Matrix&> H1, boost::optional<Matrix&> 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<Matrix&> H1,
boost::optional<Matrix&> 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<Matrix&> H1, boost::optional<Matrix&> 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<Matrix3&> H1,
boost::optional<Matrix3&> 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<Matrix3&> 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<Matrix&> H1,
boost::optional<Matrix&> 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<Matrix&> H1, boost::optional<Matrix&> 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<Matrix&> H1, boost::optional<Matrix&> 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<Matrix&> H1, boost::optional<Matrix&> 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<Matrix&> H1,
boost::optional<Matrix&> 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;
}

61
gtsam/geometry/Pose2.h
View File

@ -107,12 +107,12 @@ public:
inline static Pose2 identity() { return Pose2(); }
/// inverse transformation with derivatives
Pose2 inverse(boost::optional<Matrix&> 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<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const;
OptionalJacobian<3, 3> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) const;
/// compose syntactic sugar
inline Pose2 operator*(const Pose2& p2) const {
@ -122,19 +122,8 @@ public:
/**
* Return relative pose between p1 and p2, in p1 coordinate frame
*/
Pose2 between(const Pose2& p2) const;
/**
* Return relative pose between p1 and p2, in p1 coordinate frame
*/
Pose2 between(const Pose2& p2, boost::optional<Matrix3&> H1,
boost::optional<Matrix3&> H2) const;
/**
* Return relative pose between p1 and p2, in p1 coordinate frame
*/
Pose2 between(const Pose2& p2, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const;
Pose2 between(const Pose2& p2, OptionalJacobian<3,3> H1 = boost::none,
OptionalJacobian<3,3> H = boost::none) const;
/// @}
/// @name Manifold
@ -166,7 +155,7 @@ public:
* Calculate Adjoint map
* Ad_pose is 3*3 matrix that when applied to twist xi \f$ [T_x,T_y,\theta] \f$, returns Ad_pose(xi)
*/
Matrix AdjointMap() const;
Matrix3 AdjointMap() const;
inline Vector Adjoint(const Vector& xi) const {
assert(xi.size() == 3);
return AdjointMap()*xi;
@ -179,8 +168,8 @@ public:
* v (vx,vy) = 2D velocity
* @return xihat, 3*3 element of Lie algebra that can be exponentiated
*/
static inline Matrix wedge(double vx, double vy, double w) {
return (Matrix(3,3) <<
static inline Matrix3 wedge(double vx, double vy, double w) {
return (Matrix(3,3) <<
0.,-w, vx,
w, 0., vy,
0., 0., 0.).finished();
@ -190,23 +179,15 @@ public:
/// @name Group Action on Point2
/// @{
/** Return point coordinates in pose coordinate frame */
Point2 transform_to(const Point2& point) const;
/** Return point coordinates in pose coordinate frame */
Point2 transform_to(const Point2& point,
boost::optional<Matrix23&> H1,
boost::optional<Matrix2&> H2) const;
/** Return point coordinates in pose coordinate frame */
Point2 transform_to(const Point2& point,
boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const;
OptionalJacobian<2, 3> H1 = boost::none,
OptionalJacobian<2, 2> H2 = boost::none) const;
/** Return point coordinates in global frame */
Point2 transform_from(const Point2& point,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
OptionalJacobian<2, 3> H1 = boost::none,
OptionalJacobian<2, 2> H2 = boost::none) const;
/** syntactic sugar for transform_from */
inline Point2 operator*(const Point2& point) const { return transform_from(point);}
@ -237,7 +218,7 @@ public:
inline const Rot2& rotation() const { return r_; }
//// return transformation matrix
Matrix matrix() const;
Matrix3 matrix() const;
/**
* Calculate bearing to a landmark
@ -245,17 +226,15 @@ public:
* @return 2D rotation \f$ \in SO(2) \f$
*/
Rot2 bearing(const Point2& point,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
OptionalJacobian<1, 3> H1=boost::none, OptionalJacobian<1, 2> H2=boost::none) const;
/**
* Calculate bearing to another pose
* @param point SO(2) location of other pose
* @return 2D rotation \f$ \in SO(2) \f$
*/
Rot2 bearing(const Pose2& point,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
Rot2 bearing(const Pose2& pose,
OptionalJacobian<1, 3> H1=boost::none, OptionalJacobian<1, 3> H2=boost::none) const;
/**
* Calculate range to a landmark
@ -263,8 +242,8 @@ public:
* @return range (double)
*/
double range(const Point2& point,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
OptionalJacobian<1, 3> H1=boost::none,
OptionalJacobian<1, 2> H2=boost::none) const;
/**
* Calculate range to another pose
@ -272,8 +251,8 @@ public:
* @return range (double)
*/
double range(const Pose2& point,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
OptionalJacobian<1, 3> H1=boost::none,
OptionalJacobian<1, 3> H2=boost::none) const;
/// @}
/// @name Advanced Interface

148
gtsam/geometry/Pose3.cpp
View File

@ -32,9 +32,6 @@ INSTANTIATE_LIE(Pose3);
/** instantiate concept checks */
GTSAM_CONCEPT_POSE_INST(Pose3);
static const Matrix3 I3 = eye(3), Z3 = zeros(3, 3), _I3 = -I3;
static const Matrix6 I6 = eye(6);
/* ************************************************************************* */
Pose3::Pose3(const Pose2& pose2) :
R_(Rot3::rodriguez(0, 0, pose2.theta())), t_(
@ -50,59 +47,62 @@ Matrix6 Pose3::AdjointMap() const {
const Vector3 t = t_.vector();
Matrix3 A = skewSymmetric(t) * R;
Matrix6 adj;
adj << R, Z3, A, R;
adj << R, Z_3x3, A, R;
return adj;
}
/* ************************************************************************* */
Matrix6 Pose3::adjointMap(const Vector& xi) {
Matrix6 Pose3::adjointMap(const Vector6& xi) {
Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2));
Matrix3 v_hat = skewSymmetric(xi(3), xi(4), xi(5));
Matrix6 adj;
adj << w_hat, Z3, v_hat, w_hat;
adj << w_hat, Z_3x3, v_hat, w_hat;
return adj;
}
/* ************************************************************************* */
Vector Pose3::adjoint(const Vector& xi, const Vector& y,
boost::optional<Matrix&> H) {
Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y,
OptionalJacobian<6,6> H) {
if (H) {
*H = zeros(6, 6);
H->setZero();
for (int i = 0; i < 6; ++i) {
Vector dxi = zero(6);
Vector6 dxi;
dxi.setZero();
dxi(i) = 1.0;
Matrix Gi = adjointMap(dxi);
(*H).col(i) = Gi * y;
Matrix6 Gi = adjointMap(dxi);
H->col(i) = Gi * y;
}
}
return adjointMap(xi) * y;
}
/* ************************************************************************* */
Vector Pose3::adjointTranspose(const Vector& xi, const Vector& y,
boost::optional<Matrix&> H) {
Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y,
OptionalJacobian<6,6> H) {
if (H) {
*H = zeros(6, 6);
H->setZero();
for (int i = 0; i < 6; ++i) {
Vector dxi = zero(6);
Vector6 dxi;
dxi.setZero();
dxi(i) = 1.0;
Matrix GTi = adjointMap(dxi).transpose();
(*H).col(i) = GTi * y;
Matrix6 GTi = adjointMap(dxi).transpose();
H->col(i) = GTi * y;
}
}
Matrix adjT = adjointMap(xi).transpose();
return adjointMap(xi).transpose() * y;
}
/* ************************************************************************* */
Matrix6 Pose3::dExpInv_exp(const Vector& xi) {
Matrix6 Pose3::dExpInv_exp(const Vector6& xi) {
// Bernoulli numbers, from Wikipedia
static const Vector B = (Vector(9) << 1.0, -1.0 / 2.0, 1. / 6., 0.0, -1.0 / 30.0,
0.0, 1.0 / 42.0, 0.0, -1.0 / 30).finished();
static const int N = 5; // order of approximation
Matrix res = I6;
Matrix6 ad_i = I6;
Matrix6 res;
res = I_6x6;
Matrix6 ad_i;
ad_i = I_6x6;
Matrix6 ad_xi = adjointMap(xi);
double fac = 1.0;
for (int i = 1; i < N; ++i) {
@ -225,7 +225,7 @@ Vector6 Pose3::localCoordinates(const Pose3& T,
Matrix4 Pose3::matrix() const {
const Matrix3 R = R_.matrix();
const Vector3 T = t_.vector();
Eigen::Matrix<double, 1, 4> A14;
Matrix14 A14;
A14 << 0.0, 0.0, 0.0, 1.0;
Matrix4 mat;
mat << R, T, A14;
@ -240,12 +240,11 @@ Pose3 Pose3::transform_to(const Pose3& pose) const {
}
/* ************************************************************************* */
Point3 Pose3::transform_from(const Point3& p, boost::optional<Matrix&> Dpose,
boost::optional<Matrix&> Dpoint) const {
Point3 Pose3::transform_from(const Point3& p, OptionalJacobian<3,6> Dpose,
OptionalJacobian<3,3> Dpoint) const {
if (Dpose) {
const Matrix3 R = R_.matrix();
Matrix3 DR = R * skewSymmetric(-p.x(), -p.y(), -p.z());
Dpose->resize(3, 6);
(*Dpose) << DR, R;
}
if (Dpoint)
@ -254,13 +253,8 @@ Point3 Pose3::transform_from(const Point3& p, boost::optional<Matrix&> Dpose,
}
/* ************************************************************************* */
Point3 Pose3::transform_to(const Point3& p) const {
return R_.unrotate(p - t_);
}
/* ************************************************************************* */
Point3 Pose3::transform_to(const Point3& p, boost::optional<Matrix36&> Dpose,
boost::optional<Matrix3&> Dpoint) const {
Point3 Pose3::transform_to(const Point3& p, OptionalJacobian<3,6> Dpose,
OptionalJacobian<3,3> Dpoint) const {
// Only get transpose once, to avoid multiple allocations,
// as well as multiple conversions in the Quaternion case
const Matrix3 Rt = R_.transpose();
@ -278,77 +272,57 @@ Point3 Pose3::transform_to(const Point3& p, boost::optional<Matrix36&> Dpose,
}
/* ************************************************************************* */
Point3 Pose3::transform_to(const Point3& p, boost::optional<Matrix&> Dpose,
boost::optional<Matrix&> Dpoint) const {
const Matrix3 Rt = R_.transpose();
const Point3 q(Rt*(p - t_).vector());
if (Dpose) {
const double wx = q.x(), wy = q.y(), wz = q.z();
Dpose->resize(3, 6);
(*Dpose) <<
0.0, -wz, +wy,-1.0, 0.0, 0.0,
+wz, 0.0, -wx, 0.0,-1.0, 0.0,
-wy, +wx, 0.0, 0.0, 0.0,-1.0;
}
if (Dpoint)
*Dpoint = Rt;
return q;
}
/* ************************************************************************* */
Pose3 Pose3::compose(const Pose3& p2, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
if (H1)
*H1 = p2.inverse().AdjointMap();
if (H2)
*H2 = I6;
Pose3 Pose3::compose(const Pose3& p2, OptionalJacobian<6,6> H1,
OptionalJacobian<6,6> H2) const {
if (H1) *H1 = p2.inverse().AdjointMap();
if (H2) *H2 = I_6x6;
return (*this) * p2;
}
/* ************************************************************************* */
Pose3 Pose3::inverse(boost::optional<Matrix&> H1) const {
if (H1)
*H1 = -AdjointMap();
Pose3 Pose3::inverse(OptionalJacobian<6,6> H1) const {
if (H1) *H1 = -AdjointMap();
Rot3 Rt = R_.inverse();
return Pose3(Rt, Rt * (-t_));
}
/* ************************************************************************* */
// between = compose(p2,inverse(p1));
Pose3 Pose3::between(const Pose3& p2, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
Pose3 Pose3::between(const Pose3& p2, OptionalJacobian<6,6> H1,
OptionalJacobian<6,6> H2) const {
Pose3 result = inverse() * p2;
if (H1)
*H1 = -result.inverse().AdjointMap();
if (H2)
*H2 = I6;
if (H1) *H1 = -result.inverse().AdjointMap();
if (H2) *H2 = I_6x6;
return result;
}
/* ************************************************************************* */
double Pose3::range(const Point3& point, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
double Pose3::range(const Point3& point, OptionalJacobian<1, 6> H1,
OptionalJacobian<1, 3> H2) const {
if (!H1 && !H2)
return transform_to(point).norm();
Point3 d = transform_to(point, H1, H2);
double x = d.x(), y = d.y(), z = d.z(), d2 = x * x + y * y + z * z, n = sqrt(
d2);
Matrix D_result_d = (Matrix(1, 3) << x / n, y / n, z / n).finished();
if (H1)
*H1 = D_result_d * (*H1);
if (H2)
*H2 = D_result_d * (*H2);
return n;
else {
Matrix36 D1;
Matrix3 D2;
Point3 d = transform_to(point, H1 ? &D1 : 0, H2 ? &D2 : 0);
const double x = d.x(), y = d.y(), z = d.z(), d2 = x * x + y * y + z * z,
n = sqrt(d2);
Matrix13 D_result_d;
D_result_d << x / n, y / n, z / n;
if (H1) *H1 = D_result_d * D1;
if (H2) *H2 = D_result_d * D2;
return n;
}
}
/* ************************************************************************* */
double Pose3::range(const Pose3& point, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
double r = range(point.translation(), H1, H2);
double Pose3::range(const Pose3& pose, OptionalJacobian<1,6> H1,
OptionalJacobian<1,6> H2) const {
Matrix13 D2;
double r = range(pose.translation(), H1, H2? &D2 : 0);
if (H2) {
Matrix H2_ = *H2 * point.rotation().matrix();
*H2 = zeros(1, 6);
insertSub(*H2, H2_, 0, 3);
Matrix13 H2_ = D2 * pose.rotation().matrix();
*H2 << Matrix13::Zero(), H2_;
}
return r;
}
@ -370,7 +344,7 @@ boost::optional<Pose3> align(const vector<Point3Pair>& pairs) {
cq *= f;
// Add to form H matrix
Matrix H = zeros(3, 3);
Matrix3 H = Eigen::Matrix3d::Zero();
BOOST_FOREACH(const Point3Pair& pair, pairs){
Vector dp = pair.first.vector() - cp;
Vector dq = pair.second.vector() - cq;
@ -378,13 +352,13 @@ boost::optional<Pose3> align(const vector<Point3Pair>& pairs) {
}
// Compute SVD
Matrix U, V;
Matrix U,V;
Vector S;
svd(H, U, S, V);
// Recover transform with correction from Eggert97machinevisionandapplications
Matrix UVtranspose = U * V.transpose();
Matrix detWeighting = eye(3, 3);
Matrix3 UVtranspose = U * V.transpose();
Matrix3 detWeighting = I_3x3;
detWeighting(2, 2) = UVtranspose.determinant();
Rot3 R(Matrix(V * detWeighting * U.transpose()));
Point3 t = Point3(cq) - R * Point3(cp);

42
gtsam/geometry/Pose3.h
View File

@ -70,6 +70,12 @@ public:
R_(R), t_(t) {
}
/** Construct from R,t, where t \in vector3 */
Pose3(const Rot3& R, const Vector3& t) :
R_(R), t_(t) {
}
/** Construct from Pose2 */
explicit Pose3(const Pose2& pose2);
@ -99,11 +105,11 @@ public:
}
/// inverse transformation with derivatives
Pose3 inverse(boost::optional<Matrix&> H1 = boost::none) const;
Pose3 inverse(OptionalJacobian<6, 6> H1 = boost::none) const;
///compose this transformation onto another (first *this and then p2)
Pose3 compose(const Pose3& p2, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const;
Pose3 compose(const Pose3& p2, OptionalJacobian<6, 6> H1 = boost::none,
OptionalJacobian<6, 6> H2 = boost::none) const;
/// compose syntactic sugar
Pose3 operator*(const Pose3& T) const {
@ -114,8 +120,8 @@ public:
* Return relative pose between p1 and p2, in p1 coordinate frame
* as well as optionally the derivatives
*/
Pose3 between(const Pose3& p2, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const;
Pose3 between(const Pose3& p2, OptionalJacobian<6, 6> H1 = boost::none,
OptionalJacobian<6, 6> H2 = boost::none) const;
/// @}
/// @name Manifold
@ -186,17 +192,17 @@ public:
* and its inverse transpose in the discrete Euler Poincare' (DEP) operator.
*
*/
static Matrix6 adjointMap(const Vector& xi);
static Matrix6 adjointMap(const Vector6 &xi);
/**
* Action of the adjointMap on a Lie-algebra vector y, with optional derivatives
*/
static Vector adjoint(const Vector& xi, const Vector& y, boost::optional<Matrix&> H = boost::none);
static Vector6 adjoint(const Vector6 &xi, const Vector6 &y, OptionalJacobian<6,6> = boost::none);
/**
* The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space.
*/
static Vector adjointTranspose(const Vector& xi, const Vector& y, boost::optional<Matrix&> H = boost::none);
static Vector6 adjointTranspose(const Vector6& xi, const Vector6& y, OptionalJacobian<6, 6> H = boost::none);
/**
* Compute the inverse right-trivialized tangent (derivative) map of the exponential map,
@ -208,7 +214,7 @@ public:
* Ernst Hairer, et al., Geometric Numerical Integration,
* Structure-Preserving Algorithms for Ordinary Differential Equations, 2nd edition, Springer-Verlag, 2006.
*/
static Matrix6 dExpInv_exp(const Vector& xi);
static Matrix6 dExpInv_exp(const Vector6 &xi);
/**
* wedge for Pose3:
@ -237,7 +243,7 @@ public:
* @return point in world coordinates
*/
Point3 transform_from(const Point3& p,
boost::optional<Matrix&> Dpose=boost::none, boost::optional<Matrix&> Dpoint=boost::none) const;
OptionalJacobian<3,6> Dpose=boost::none, OptionalJacobian<3,3> Dpoint=boost::none) const;
/** syntactic sugar for transform_from */
inline Point3 operator*(const Point3& p) const { return transform_from(p); }
@ -249,13 +255,9 @@ public:
* @param Dpoint optional 3*3 Jacobian wrpt point
* @return point in Pose coordinates
*/
Point3 transform_to(const Point3& p) const;
Point3 transform_to(const Point3& p,
boost::optional<Matrix36&> Dpose, boost::optional<Matrix3&> Dpoint) const;
Point3 transform_to(const Point3& p,
boost::optional<Matrix&> Dpose, boost::optional<Matrix&> Dpoint) const;
OptionalJacobian<3,6> Dpose = boost::none,
OptionalJacobian<3,3> Dpoint = boost::none) const;
/// @}
/// @name Standard Interface
@ -288,8 +290,8 @@ public:
* @return range (double)
*/
double range(const Point3& point,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
OptionalJacobian<1,6> H1=boost::none,
OptionalJacobian<1,3> H2=boost::none) const;
/**
* Calculate range to another pose
@ -297,8 +299,8 @@ public:
* @return range (double)
*/
double range(const Pose3& pose,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
OptionalJacobian<1,6> H1=boost::none,
OptionalJacobian<1,6> H2=boost::none) const;
/// @}
/// @name Advanced Interface

30
gtsam/geometry/Rot2.cpp
View File

@ -64,21 +64,25 @@ Rot2& Rot2::normalize() {
}
/* ************************************************************************* */
Matrix Rot2::matrix() const {
return (Matrix(2, 2) << c_, -s_, s_, c_).finished();
Matrix2 Rot2::matrix() const {
Matrix2 rvalue_;
rvalue_ << c_, -s_, s_, c_;
return rvalue_;
}
/* ************************************************************************* */
Matrix Rot2::transpose() const {
return (Matrix(2, 2) << c_, s_, -s_, c_).finished();
Matrix2 Rot2::transpose() const {
Matrix2 rvalue_;
rvalue_ << c_, s_, -s_, c_;
return rvalue_;
}
/* ************************************************************************* */
// see doc/math.lyx, SO(2) section
Point2 Rot2::rotate(const Point2& p, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
Point2 Rot2::rotate(const Point2& p, OptionalJacobian<2, 1> H1,
OptionalJacobian<2, 2> H2) const {
const Point2 q = Point2(c_ * p.x() + -s_ * p.y(), s_ * p.x() + c_ * p.y());
if (H1) *H1 = (Matrix(2, 1) << -q.y(), q.x()).finished();
if (H1) *H1 << -q.y(), q.x();
if (H2) *H2 = matrix();
return q;
}
@ -86,21 +90,23 @@ Point2 Rot2::rotate(const Point2& p, boost::optional<Matrix&> H1,
/* ************************************************************************* */
// see doc/math.lyx, SO(2) section
Point2 Rot2::unrotate(const Point2& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
OptionalJacobian<2, 1> H1, OptionalJacobian<2, 2> H2) const {
const Point2 q = Point2(c_ * p.x() + s_ * p.y(), -s_ * p.x() + c_ * p.y());
if (H1) *H1 = (Matrix(2, 1) << q.y(), -q.x()).finished(); // R_{pi/2}q
if (H1) *H1 << q.y(), -q.x();
if (H2) *H2 = transpose();
return q;
}
/* ************************************************************************* */
Rot2 Rot2::relativeBearing(const Point2& d, boost::optional<Matrix&> H) {
Rot2 Rot2::relativeBearing(const Point2& d, OptionalJacobian<1, 2> H) {
double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2);
if(fabs(n) > 1e-5) {
if (H) *H = (Matrix(1, 2) << -y / d2, x / d2).finished();
if (H) {
*H << -y / d2, x / d2;
}
return Rot2::fromCosSin(x / n, y / n);
} else {
if (H) *H = (Matrix(1, 2) << 0.0, 0.0).finished();
if (H) *H << 0.0, 0.0;
return Rot2();
}
}

30
gtsam/geometry/Rot2.h
View File

@ -87,7 +87,7 @@ namespace gtsam {
* @param H optional reference for Jacobian
* @return 2D rotation \f$ \in SO(2) \f$
*/
static Rot2 relativeBearing(const Point2& d, boost::optional<Matrix&> H =
static Rot2 relativeBearing(const Point2& d, OptionalJacobian<1,2> H =
boost::none);
/** Named constructor that behaves as atan2, i.e., y,x order (!) and normalizes */
@ -116,10 +116,10 @@ namespace gtsam {
}
/** Compose - make a new rotation by adding angles */
inline Rot2 compose(const Rot2& R, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none) const {
if (H1) *H1 = eye(1);
if (H2) *H2 = eye(1);
inline Rot2 compose(const Rot2& R, OptionalJacobian<1,1> H1 =
boost::none, OptionalJacobian<1,1> H2 = boost::none) const {
if (H1) *H1 = I_1x1; // set to 1x1 identity matrix
if (H2) *H2 = I_1x1; // set to 1x1 identity matrix
return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_);
}
@ -129,10 +129,10 @@ namespace gtsam {
}
/** Between using the default implementation */
inline Rot2 between(const Rot2& R, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none) const {
if (H1) *H1 = -eye(1);
if (H2) *H2 = eye(1);
inline Rot2 between(const Rot2& R, OptionalJacobian<1,1> H1 =
boost::none, OptionalJacobian<1,1> H2 = boost::none) const {
if (H1) *H1 = -I_1x1; // set to 1x1 identity matrix
if (H2) *H2 = I_1x1; // set to 1x1 identity matrix
return fromCosSin(c_ * R.c_ + s_ * R.s_, -s_ * R.c_ + c_ * R.s_);
}
@ -180,8 +180,8 @@ namespace gtsam {
/**
* rotate point from rotated coordinate frame to world \f$ p^w = R_c^w p^c \f$
*/
Point2 rotate(const Point2& p, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const;
Point2 rotate(const Point2& p, OptionalJacobian<2, 1> H1 = boost::none,
OptionalJacobian<2, 2> H2 = boost::none) const;
/** syntactic sugar for rotate */
inline Point2 operator*(const Point2& p) const {
@ -191,8 +191,8 @@ namespace gtsam {
/**
* rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$
*/
Point2 unrotate(const Point2& p, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const;
Point2 unrotate(const Point2& p, OptionalJacobian<2, 1> H1 = boost::none,
OptionalJacobian<2, 2> H2 = boost::none) const;
/// @}
/// @name Standard Interface
@ -225,10 +225,10 @@ namespace gtsam {
}
/** return 2*2 rotation matrix */
Matrix matrix() const;
Matrix2 matrix() const;
/** return 2*2 transpose (inverse) rotation matrix */
Matrix transpose() const;
Matrix2 transpose() const;
private:
/** Serialization function */

72
gtsam/geometry/Rot3.cpp
View File

@ -27,8 +27,6 @@ using namespace std;
namespace gtsam {
static const Matrix3 I3 = Matrix3::Identity();
/* ************************************************************************* */
void Rot3::print(const std::string& s) const {
gtsam::print((Matrix)matrix(), s);
@ -72,23 +70,21 @@ Point3 Rot3::operator*(const Point3& p) const {
/* ************************************************************************* */
Unit3 Rot3::rotate(const Unit3& p,
boost::optional<Matrix&> HR, boost::optional<Matrix&> Hp) const {
Unit3 q = Unit3(rotate(p.point3(Hp)));
if (Hp)
(*Hp) = q.basis().transpose() * matrix() * (*Hp);
if (HR)
(*HR) = -q.basis().transpose() * matrix() * p.skew();
OptionalJacobian<2,3> HR, OptionalJacobian<2,2> Hp) const {
Matrix32 Dp;
Unit3 q = Unit3(rotate(p.point3(Hp ? &Dp : 0)));
if (Hp) *Hp = q.basis().transpose() * matrix() * Dp;
if (HR) *HR = -q.basis().transpose() * matrix() * p.skew();
return q;
}
/* ************************************************************************* */
Unit3 Rot3::unrotate(const Unit3& p,
boost::optional<Matrix&> HR, boost::optional<Matrix&> Hp) const {
Unit3 q = Unit3(unrotate(p.point3(Hp)));
if (Hp)
(*Hp) = q.basis().transpose() * matrix().transpose () * (*Hp);
if (HR)
(*HR) = q.basis().transpose() * q.skew();
OptionalJacobian<2,3> HR, OptionalJacobian<2,2> Hp) const {
Matrix32 Dp;
Unit3 q = Unit3(unrotate(p.point3(Dp)));
if (Hp) *Hp = q.basis().transpose() * matrix().transpose () * Dp;
if (HR) *HR = q.basis().transpose() * q.skew();
return q;
}
@ -99,8 +95,8 @@ Unit3 Rot3::operator*(const Unit3& p) const {
/* ************************************************************************* */
// see doc/math.lyx, SO(3) section
Point3 Rot3::unrotate(const Point3& p, boost::optional<Matrix3&> H1,
boost::optional<Matrix3&> H2) const {
Point3 Rot3::unrotate(const Point3& p, OptionalJacobian<3,3> H1,
OptionalJacobian<3,3> H2) const {
const Matrix3& Rt = transpose();
Point3 q(Rt * p.vector()); // q = Rt*p
const double wx = q.x(), wy = q.y(), wz = q.z();
@ -111,32 +107,16 @@ Point3 Rot3::unrotate(const Point3& p, boost::optional<Matrix3&> H1,
return q;
}
/* ************************************************************************* */
// see doc/math.lyx, SO(3) section
Point3 Rot3::unrotate(const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
const Matrix3& Rt = transpose();
Point3 q(Rt * p.vector()); // q = Rt*p
const double wx = q.x(), wy = q.y(), wz = q.z();
if (H1) {
H1->resize(3,3);
*H1 << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0;
}
if (H2)
*H2 = Rt;
return q;
}
/* ************************************************************************* */
/// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version)
Matrix3 Rot3::dexpL(const Vector3& v) {
if(zero(v)) return eye(3);
Matrix x = skewSymmetric(v);
Matrix x2 = x*x;
Matrix3 x = skewSymmetric(v);
Matrix3 x2 = x*x;
double theta = v.norm(), vi = theta/2.0;
double s1 = sin(vi)/vi;
double s2 = (theta - sin(theta))/(theta*theta*theta);
Matrix res = eye(3) - 0.5*s1*s1*x + s2*x2;
Matrix3 res = I_3x3 - 0.5*s1*s1*x + s2*x2;
return res;
}
@ -144,11 +124,11 @@ Matrix3 Rot3::dexpL(const Vector3& v) {
/// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version)
Matrix3 Rot3::dexpInvL(const Vector3& v) {
if(zero(v)) return eye(3);
Matrix x = skewSymmetric(v);
Matrix x2 = x*x;
Matrix3 x = skewSymmetric(v);
Matrix3 x2 = x*x;
double theta = v.norm(), vi = theta/2.0;
double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta);
Matrix res = eye(3) + 0.5*x - s2*x2;
Matrix3 res = I_3x3 + 0.5*x - s2*x2;
return res;
}
@ -167,7 +147,7 @@ Point3 Rot3::column(int index) const{
/* ************************************************************************* */
Vector3 Rot3::xyz() const {
Matrix I;Vector3 q;
Matrix3 I;Vector3 q;
boost::tie(I,q)=RQ(matrix());
return q;
}
@ -200,11 +180,11 @@ Matrix3 Rot3::rightJacobianExpMapSO3(const Vector3& x) {
double normx = norm_2(x); // rotation angle
Matrix3 Jr;
if (normx < 10e-8){
Jr = Matrix3::Identity();
Jr = I_3x3;
}
else{
const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
Jr = Matrix3::Identity() - ((1-cos(normx))/(normx*normx)) * X +
Jr = I_3x3 - ((1-cos(normx))/(normx*normx)) * X +
((normx-sin(normx))/(normx*normx*normx)) * X * X; // right Jacobian
}
return Jr;
@ -217,11 +197,11 @@ Matrix3 Rot3::rightJacobianExpMapSO3inverse(const Vector3& x) {
Matrix3 Jrinv;
if (normx < 10e-8){
Jrinv = Matrix3::Identity();
Jrinv = I_3x3;
}
else{
const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
Jrinv = Matrix3::Identity() +
Jrinv = I_3x3 +
0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx)) ) * X * X;
}
return Jrinv;
@ -255,12 +235,6 @@ ostream &operator<<(ostream &os, const Rot3& R) {
return os;
}
/* ************************************************************************* */
Point3 Rot3::unrotate(const Point3& p) const {
// Eigen expression
return Point3(transpose()*p.vector()); // q = Rt*p
}
/* ************************************************************************* */
Rot3 Rot3::slerp(double t, const Rot3& other) const {
// amazingly simple in GTSAM :-)

40
gtsam/geometry/Rot3.h
View File

@ -215,18 +215,11 @@ namespace gtsam {
}
/// derivative of inverse rotation R^T s.t. inverse(R)*R = identity
Rot3 inverse(boost::optional<Matrix&> H1=boost::none) const;
Rot3 inverse(boost::optional<Matrix3&> H1=boost::none) const;
/// Compose two rotations i.e., R= (*this) * R2
Rot3 compose(const Rot3& R2) const;
/// Compose two rotations i.e., R= (*this) * R2
Rot3 compose(const Rot3& R2, boost::optional<Matrix3&> H1,
boost::optional<Matrix3&> H2) const;
/// Compose two rotations i.e., R= (*this) * R2
Rot3 compose(const Rot3& R2, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const;
Rot3 compose(const Rot3& R2, OptionalJacobian<3, 3> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) const;
/** compose two rotations */
Rot3 operator*(const Rot3& R2) const;
@ -245,8 +238,8 @@ namespace gtsam {
* Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1'
*/
Rot3 between(const Rot3& R2,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
OptionalJacobian<3,3> H1=boost::none,
OptionalJacobian<3,3> H2=boost::none) const;
/// @}
/// @name Manifold
@ -328,34 +321,27 @@ namespace gtsam {
/**
* rotate point from rotated coordinate frame to world \f$ p^w = R_c^w p^c \f$
*/
Point3 rotate(const Point3& p, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const;
Point3 rotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none,
OptionalJacobian<3,3> H2 = boost::none) const;
/// rotate point from rotated coordinate frame to world = R*p
Point3 operator*(const Point3& p) const;
/// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$
Point3 unrotate(const Point3& p) const;
/// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$
Point3 unrotate(const Point3& p, boost::optional<Matrix3&> H1,
boost::optional<Matrix3&> H2) const;
/// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$
Point3 unrotate(const Point3& p, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const;
Point3 unrotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none,
OptionalJacobian<3,3> H2=boost::none) const;
/// @}
/// @name Group Action on Unit3
/// @{
/// rotate 3D direction from rotated coordinate frame to world frame
Unit3 rotate(const Unit3& p, boost::optional<Matrix&> HR = boost::none,
boost::optional<Matrix&> Hp = boost::none) const;
Unit3 rotate(const Unit3& p, OptionalJacobian<2,3> HR = boost::none,
OptionalJacobian<2,2> Hp = boost::none) const;
/// unrotate 3D direction from world frame to rotated coordinate frame
Unit3 unrotate(const Unit3& p, boost::optional<Matrix&> HR = boost::none,
boost::optional<Matrix&> Hp = boost::none) const;
Unit3 unrotate(const Unit3& p, OptionalJacobian<2,3> HR = boost::none,
OptionalJacobian<2,2> Hp = boost::none) const;
/// rotate 3D direction from rotated coordinate frame to world frame
Unit3 operator*(const Unit3& p) const;

32
gtsam/geometry/Rot3M.cpp
View File

@ -30,10 +30,8 @@ using namespace std;
namespace gtsam {
static const Matrix3 I3 = Matrix3::Identity();
/* ************************************************************************* */
Rot3::Rot3() : rot_(Matrix3::Identity()) {}
Rot3::Rot3() : rot_(I_3x3) {}
/* ************************************************************************* */
Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) {
@ -142,23 +140,9 @@ Rot3 Rot3::rodriguez(const Vector& w, double theta) {
}
/* ************************************************************************* */
Rot3 Rot3::compose (const Rot3& R2) const {
return *this * R2;
}
/* ************************************************************************* */
Rot3 Rot3::compose (const Rot3& R2,
boost::optional<Matrix3&> H1, boost::optional<Matrix3&> H2) const {
Rot3 Rot3::compose(const Rot3& R2, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const {
if (H1) *H1 = R2.transpose();
if (H2) *H2 = I3;
return *this * R2;
}
/* ************************************************************************* */
Rot3 Rot3::compose (const Rot3& R2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) *H1 = R2.transpose();
if (H2) *H2 = I3;
if (H2) *H2 = I_3x3;
return *this * R2;
}
@ -174,23 +158,23 @@ Matrix3 Rot3::transpose() const {
}
/* ************************************************************************* */
Rot3 Rot3::inverse(boost::optional<Matrix&> H1) const {
Rot3 Rot3::inverse(boost::optional<Matrix3 &> H1) const {
if (H1) *H1 = -rot_;
return Rot3(Matrix3(transpose()));
}
/* ************************************************************************* */
Rot3 Rot3::between (const Rot3& R2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
if (H1) *H1 = -(R2.transpose()*rot_);
if (H2) *H2 = I3;
if (H2) *H2 = I_3x3;
Matrix3 R12 = transpose()*R2.rot_;
return Rot3(R12);
}
/* ************************************************************************* */
Point3 Rot3::rotate(const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
if (H1 || H2) {
if (H1) *H1 = rot_ * skewSymmetric(-p.x(), -p.y(), -p.z());
if (H2) *H2 = rot_;
@ -257,7 +241,7 @@ Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const {
} else if(mode == Rot3::CAYLEY) {
return retractCayley(omega);
} else if(mode == Rot3::SLOW_CAYLEY) {
Matrix Omega = skewSymmetric(omega);
Matrix3 Omega = skewSymmetric(omega);
return (*this)*CayleyFixed<3>(-Omega/2);
} else {
assert(false);

21
gtsam/geometry/Rot3Q.cpp
View File

@ -87,22 +87,9 @@ namespace gtsam {
Rot3 Rot3::rodriguez(const Vector& w, double theta) {
return Quaternion(Eigen::AngleAxisd(theta, w)); }
/* ************************************************************************* */
Rot3 Rot3::compose(const Rot3& R2) const {
return Rot3(quaternion_ * R2.quaternion_);
}
/* ************************************************************************* */
Rot3 Rot3::compose(const Rot3& R2,
boost::optional<Matrix3&> H1, boost::optional<Matrix3&> H2) const {
if (H1) *H1 = R2.transpose();
if (H2) *H2 = I3;
return Rot3(quaternion_ * R2.quaternion_);
}
/* ************************************************************************* */
Rot3 Rot3::compose(const Rot3& R2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
if (H1) *H1 = R2.transpose();
if (H2) *H2 = I3;
return Rot3(quaternion_ * R2.quaternion_);
@ -114,7 +101,7 @@ namespace gtsam {
}
/* ************************************************************************* */
Rot3 Rot3::inverse(boost::optional<Matrix&> H1) const {
Rot3 Rot3::inverse(boost::optional<Matrix3&> H1) const {
if (H1) *H1 = -matrix();
return Rot3(quaternion_.inverse());
}
@ -129,7 +116,7 @@ namespace gtsam {
/* ************************************************************************* */
Rot3 Rot3::between(const Rot3& R2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
if (H1) *H1 = -(R2.transpose()*matrix());
if (H2) *H2 = I3;
return between_default(*this, R2);
@ -137,7 +124,7 @@ namespace gtsam {
/* ************************************************************************* */
Point3 Rot3::rotate(const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
Matrix R = matrix();
if (H1) *H1 = R * skewSymmetric(-p.x(), -p.y(), -p.z());
if (H2) *H2 = R;

14
gtsam/geometry/SimpleCamera.cpp
View File

@ -21,27 +21,27 @@
namespace gtsam {
SimpleCamera simpleCamera(const Matrix& P) {
SimpleCamera simpleCamera(const Matrix34& P) {
// P = [A|a] = s K cRw [I|-T], with s the unknown scale
Matrix A = P.topLeftCorner(3, 3);
Vector a = P.col(3);
Matrix3 A = P.topLeftCorner(3, 3);
Vector3 a = P.col(3);
// do RQ decomposition to get s*K and cRw angles
Matrix sK;
Vector xyz;
Matrix3 sK;
Vector3 xyz;
boost::tie(sK, xyz) = RQ(A);
// Recover scale factor s and K
double s = sK(2, 2);
Matrix K = sK / s;
Matrix3 K = sK / s;
// Recover cRw itself, and its inverse
Rot3 cRw = Rot3::RzRyRx(xyz);
Rot3 wRc = cRw.inverse();
// Now, recover T from a = - s K cRw T = - A T
Vector T = -(A.inverse() * a);
Vector3 T = -(A.inverse() * a);
return SimpleCamera(Pose3(wRc, T),
Cal3_S2(K(0, 0), K(1, 1), K(0, 1), K(0, 2), K(1, 2)));
}

2
gtsam/geometry/SimpleCamera.h
View File

@ -27,5 +27,5 @@ namespace gtsam {
typedef PinholeCamera<Cal3_S2> SimpleCamera;
/// Recover camera from 3*4 camera matrix
GTSAM_EXPORT SimpleCamera simpleCamera(const Matrix& P);
GTSAM_EXPORT SimpleCamera simpleCamera(const Matrix34& P);
}

28
gtsam/geometry/StereoCamera.cpp
View File

@ -30,8 +30,8 @@ namespace gtsam {
/* ************************************************************************* */
StereoPoint2 StereoCamera::project(const Point3& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2,
boost::optional<Matrix&> H3) const {
OptionalJacobian<3,6> H1, OptionalJacobian<3,3> H2,
OptionalJacobian<3,6> H3) const {
#ifdef STEREOCAMERA_CHAIN_RULE
const Point3 q = leftCamPose_.transform_to(point, H1, H2);
@ -58,28 +58,28 @@ namespace gtsam {
if (H1 || H2) {
#ifdef STEREOCAMERA_CHAIN_RULE
// just implement chain rule
Matrix D_project_point = Dproject_to_stereo_camera1(q); // 3x3 Jacobian
Matrix3 D_project_point = Dproject_to_stereo_camera1(q); // 3x3 Jacobian
if (H1) *H1 = D_project_point*(*H1);
if (H2) *H2 = D_project_point*(*H2);
#else
// optimized version, see StereoCamera.nb
if (H1) {
const double v1 = v/fy, v2 = fx*v1, dx=d*x;
*H1 = (Matrix(3, 6) <<
uL*v1, -fx-dx*uL, v2, -dfx, 0.0, d*uL,
*H1 << uL*v1, -fx-dx*uL, v2, -dfx, 0.0, d*uL,
uR*v1, -fx-dx*uR, v2, -dfx, 0.0, d*uR,
fy + v*v1, -dx*v , -x*dfy, 0.0, -dfy, d*v
).finished();
fy + v*v1, -dx*v , -x*dfy, 0.0, -dfy, d*v;
}
if (H2) {
const Matrix R(leftCamPose_.rotation().matrix());
*H2 = d * (Matrix(3, 3) <<
fx*R(0, 0) - R(0, 2)*uL, fx*R(1, 0) - R(1, 2)*uL, fx*R(2, 0) - R(2, 2)*uL,
fx*R(0, 0) - R(0, 2)*uR, fx*R(1, 0) - R(1, 2)*uR, fx*R(2, 0) - R(2, 2)*uR,
fy*R(0, 1) - R(0, 2)*v , fy*R(1, 1) - R(1, 2)*v , fy*R(2, 1) - R(2, 2)*v
).finished();
const Matrix3 R(leftCamPose_.rotation().matrix());
*H2 << fx*R(0, 0) - R(0, 2)*uL, fx*R(1, 0) - R(1, 2)*uL, fx*R(2, 0) - R(2, 2)*uL,
fx*R(0, 0) - R(0, 2)*uR, fx*R(1, 0) - R(1, 2)*uR, fx*R(2, 0) - R(2, 2)*uR,
fy*R(0, 1) - R(0, 2)*v , fy*R(1, 1) - R(1, 2)*v , fy*R(2, 1) - R(2, 2)*v;
*H2 << d * (*H2);
}
#endif
if (H3)
// TODO, this is set to zero, as Cal3_S2Stereo cannot be optimized yet
H3->setZero();
}
// finally translate
@ -87,7 +87,7 @@ namespace gtsam {
}
/* ************************************************************************* */
Matrix StereoCamera::Dproject_to_stereo_camera1(const Point3& P) const {
Matrix3 StereoCamera::Dproject_to_stereo_camera1(const Point3& P) const {
double d = 1.0 / P.z(), d2 = d*d;
const Cal3_S2Stereo& K = *K_;
double f_x = K.fx(), f_y = K.fy(), b = K.baseline();

12
gtsam/geometry/StereoCamera.h
View File

@ -95,8 +95,8 @@ public:
}
Pose3 between(const StereoCamera &camera,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const {
OptionalJacobian<6,6> H1=boost::none,
OptionalJacobian<6,6> H2=boost::none) const {
return leftCamPose_.between(camera.pose(), H1, H2);
}
@ -119,9 +119,9 @@ public:
* @param H3 IGNORED (for calibration)
*/
StereoPoint2 project(const Point3& point,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const;
OptionalJacobian<3, 6> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none,
OptionalJacobian<3, 6> H3 = boost::none) const;
/**
*
@ -139,7 +139,7 @@ public:
private:
/** utility functions */
Matrix Dproject_to_stereo_camera1(const Point3& P) const;
Matrix3 Dproject_to_stereo_camera1(const Point3& P) const;
friend class boost::serialization::access;
template<class Archive>

42
gtsam/geometry/Unit3.cpp
View File

@ -39,14 +39,13 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
Unit3 Unit3::FromPoint3(const Point3& point, boost::optional<Matrix&> H) {
Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2,3> H) {
Unit3 direction(point);
if (H) {
// 3*3 Derivative of representation with respect to point is 3*3:
Matrix D_p_point;
Matrix3 D_p_point;
point.normalize(D_p_point); // TODO, this calculates norm a second time :-(
// Calculate the 2*3 Jacobian
H->resize(2, 3);
*H << direction.basis().transpose() * D_p_point;
}
return direction;
@ -67,7 +66,7 @@ Unit3 Unit3::Random(boost::mt19937 & rng) {
}
/* ************************************************************************* */
const Unit3::Matrix32& Unit3::basis() const {
const Matrix32& Unit3::basis() const {
// Return cached version if exists
if (B_)
@ -92,7 +91,7 @@ const Unit3::Matrix32& Unit3::basis() const {
b2 = b2 / b2.norm();
// Create the basis matrix
B_.reset(Unit3::Matrix32());
B_.reset(Matrix32());
(*B_) << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z();
return *B_;
}
@ -104,38 +103,39 @@ void Unit3::print(const std::string& s) const {
}
/* ************************************************************************* */
Matrix Unit3::skew() const {
Matrix3 Unit3::skew() const {
return skewSymmetric(p_.x(), p_.y(), p_.z());
}
/* ************************************************************************* */
Vector Unit3::error(const Unit3& q, boost::optional<Matrix&> H) const {
Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H) const {
// 2D error is equal to B'*q, as B is 3x2 matrix and q is 3x1
Matrix Bt = basis().transpose();
Vector xi = Bt * q.p_.vector();
Matrix23 Bt = basis().transpose();
Vector2 xi = Bt * q.p_.vector();
if (H)
*H = Bt * q.basis();
return xi;
}
/* ************************************************************************* */
double Unit3::distance(const Unit3& q, boost::optional<Matrix&> H) const {
Vector xi = error(q, H);
double Unit3::distance(const Unit3& q, OptionalJacobian<1,2> H) const {
Matrix2 H_;
Vector2 xi = error(q, H_);
double theta = xi.norm();
if (H)
*H = (xi.transpose() / theta) * (*H);
*H = (xi.transpose() / theta) * H_;
return theta;
}
/* ************************************************************************* */
Unit3 Unit3::retract(const Vector& v) const {
Unit3 Unit3::retract(const Vector2& v) const {
// Get the vector form of the point and the basis matrix
Vector p = Point3::Logmap(p_);
Matrix B = basis();
Vector3 p = Point3::Logmap(p_);
Matrix32 B = basis();
// Compute the 3D xi_hat vector
Vector xi_hat = v(0) * B.col(0) + v(1) * B.col(1);
Vector3 xi_hat = v(0) * B.col(0) + v(1) * B.col(1);
double xi_hat_norm = xi_hat.norm();
@ -147,17 +147,17 @@ Unit3 Unit3::retract(const Vector& v) const {
return Unit3(-point3());
}
Vector exp_p_xi_hat = cos(xi_hat_norm) * p
Vector3 exp_p_xi_hat = cos(xi_hat_norm) * p
+ sin(xi_hat_norm) * (xi_hat / xi_hat_norm);
return Unit3(exp_p_xi_hat);
}
/* ************************************************************************* */
Vector Unit3::localCoordinates(const Unit3& y) const {
Vector2 Unit3::localCoordinates(const Unit3& y) const {
Vector p = Point3::Logmap(p_);
Vector q = Point3::Logmap(y.p_);
Vector3 p = Point3::Logmap(p_);
Vector3 q = Point3::Logmap(y.p_);
double dot = p.dot(q);
// Check for special cases
@ -168,7 +168,7 @@ Vector Unit3::localCoordinates(const Unit3& y) const {
else {
// no special case
double theta = acos(dot);
Vector result_hat = (theta / sin(theta)) * (q - p * dot);
Vector3 result_hat = (theta / sin(theta)) * (q - p * dot);
return basis().transpose() * result_hat;
}
}

24
gtsam/geometry/Unit3.h
View File

@ -32,8 +32,6 @@ class GTSAM_EXPORT Unit3{
private:
typedef Eigen::Matrix<double,3,2> Matrix32;
Point3 p_; ///< The location of the point on the unit sphere
mutable boost::optional<Matrix32> B_; ///< Cached basis
@ -52,6 +50,11 @@ public:
p_(p / p.norm()) {
}
/// Construct from a vector3
explicit Unit3(const Vector3& p) :
p_(p / p.norm()) {
}
/// Construct from x,y,z
Unit3(double x, double y, double z) :
p_(x, y, z) {
@ -59,7 +62,7 @@ public:
}
/// Named constructor from Point3 with optional Jacobian
static Unit3 FromPoint3(const Point3& point, boost::optional<Matrix&> H =
static Unit3 FromPoint3(const Point3& point, OptionalJacobian<2,3> H =
boost::none);
/// Random direction, using boost::uniform_on_sphere
@ -90,10 +93,10 @@ public:
const Matrix32& basis() const;
/// Return skew-symmetric associated with 3D point on unit sphere
Matrix skew() const;
Matrix3 skew() const;
/// Return unit-norm Point3
const Point3& point3(boost::optional<Matrix&> H = boost::none) const {
const Point3& point3(OptionalJacobian<3,2> H = boost::none) const {
if (H)
*H = basis();
return p_;
@ -105,12 +108,12 @@ public:
}
/// Signed, vector-valued error between two directions
Vector error(const Unit3& q,
boost::optional<Matrix&> H = boost::none) const;
Vector2 error(const Unit3& q,
OptionalJacobian<2,2> H = boost::none) const;
/// Distance between two directions
double distance(const Unit3& q,
boost::optional<Matrix&> H = boost::none) const;
OptionalJacobian<1,2> H = boost::none) const;
/// @}
@ -133,10 +136,10 @@ public:
};
/// The retract function
Unit3 retract(const Vector& v) const;
Unit3 retract(const Vector2& v) const;
/// The local coordinates function
Vector localCoordinates(const Unit3& s) const;
Vector2 localCoordinates(const Unit3& s) const;
/// @}
@ -144,7 +147,6 @@ private:
/// @name Advanced Interface
/// @{
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>

2
gtsam/geometry/tests/testCal3_S2.cpp
View File

@ -60,7 +60,7 @@ TEST( Cal3_S2, calibrate_homogeneous) {
Point2 uncalibrate_(const Cal3_S2& k, const Point2& pt) { return k.uncalibrate(pt); }
TEST( Cal3_S2, Duncalibrate1)
{
Matrix computed; K.uncalibrate(p, computed, boost::none);
Matrix25 computed; K.uncalibrate(p, computed, boost::none);
Matrix numerical = numericalDerivative21(uncalibrate_, K, p);
CHECK(assert_equal(numerical,computed,1e-8));
}

10
gtsam/geometry/tests/testCalibratedCamera.cpp
View File

@ -15,12 +15,14 @@
* @brief test CalibratedCamera class
*/
#include <iostream>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <CppUnitLite/TestHarness.h>
#include <iostream>
using namespace std;
using namespace gtsam;

184
gtsam/geometry/tests/testPinholeCamera.cpp
View File

@ -15,29 +15,28 @@
* @brief test PinholeCamera class
*/
#include <cmath>
#include <iostream>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <cmath>
#include <iostream>
using namespace std;
using namespace gtsam;
typedef PinholeCamera<Cal3_S2> Camera;
static const Cal3_S2 K(625, 625, 0, 0, 0);
static const Pose3 pose1((Matrix)(Matrix(3,3) <<
1., 0., 0.,
0.,-1., 0.,
0., 0.,-1.
).finished(),
Point3(0,0,0.5));
static const Pose3 pose(Matrix3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5));
static const Camera camera(pose, K);
typedef PinholeCamera<Cal3_S2> Camera;
static const Camera camera(pose1, K);
static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5));
static const Camera camera1(pose1, K);
static const Point3 point1(-0.08,-0.08, 0.0);
static const Point3 point2(-0.08, 0.08, 0.0);
@ -52,8 +51,8 @@ static const Point3 point4_inf( 0.16,-0.16, -1.0);
/* ************************************************************************* */
TEST( PinholeCamera, constructor)
{
CHECK(assert_equal( camera.calibration(), K));
CHECK(assert_equal( camera.pose(), pose1));
EXPECT(assert_equal( camera.calibration(), K));
EXPECT(assert_equal( camera.pose(), pose));
}
/* ************************************************************************* */
@ -67,7 +66,7 @@ TEST( PinholeCamera, level2)
Point3 x(1,0,0),y(0,0,-1),z(0,1,0);
Rot3 wRc(x,y,z);
Pose3 expected(wRc,Point3(0.4,0.3,0.1));
CHECK(assert_equal( camera.pose(), expected));
EXPECT(assert_equal( camera.pose(), expected));
}
/* ************************************************************************* */
@ -80,72 +79,72 @@ TEST( PinholeCamera, lookat)
// expected
Point3 xc(0,1,0),yc(0,0,-1),zc(-1,0,0);
Pose3 expected(Rot3(xc,yc,zc),C);
CHECK(assert_equal( camera.pose(), expected));
EXPECT(assert_equal( camera.pose(), expected));
Point3 C2(30.0,0.0,10.0);
Camera camera2 = Camera::Lookat(C2, Point3(), Point3(0.0,0.0,1.0));
Matrix R = camera2.pose().rotation().matrix();
Matrix I = trans(R)*R;
CHECK(assert_equal(I, eye(3)));
EXPECT(assert_equal(I, eye(3)));
}
/* ************************************************************************* */
TEST( PinholeCamera, project)
{
CHECK(assert_equal( camera.project(point1), Point2(-100, 100) ));
CHECK(assert_equal( camera.project(point2), Point2(-100, -100) ));
CHECK(assert_equal( camera.project(point3), Point2( 100, -100) ));
CHECK(assert_equal( camera.project(point4), Point2( 100, 100) ));
EXPECT(assert_equal( camera.project(point1), Point2(-100, 100) ));
EXPECT(assert_equal( camera.project(point2), Point2(-100, -100) ));
EXPECT(assert_equal( camera.project(point3), Point2( 100, -100) ));
EXPECT(assert_equal( camera.project(point4), Point2( 100, 100) ));
}
/* ************************************************************************* */
TEST( PinholeCamera, backproject)
{
CHECK(assert_equal( camera.backproject(Point2(-100, 100), 0.5), point1));
CHECK(assert_equal( camera.backproject(Point2(-100, -100), 0.5), point2));
CHECK(assert_equal( camera.backproject(Point2( 100, -100), 0.5), point3));
CHECK(assert_equal( camera.backproject(Point2( 100, 100), 0.5), point4));
EXPECT(assert_equal( camera.backproject(Point2(-100, 100), 0.5), point1));
EXPECT(assert_equal( camera.backproject(Point2(-100, -100), 0.5), point2));
EXPECT(assert_equal( camera.backproject(Point2( 100, -100), 0.5), point3));
EXPECT(assert_equal( camera.backproject(Point2( 100, 100), 0.5), point4));
}
/* ************************************************************************* */
TEST( PinholeCamera, backprojectInfinity)
{
CHECK(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, 100)), point1_inf));
CHECK(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, -100)), point2_inf));
CHECK(assert_equal( camera.backprojectPointAtInfinity(Point2( 100, -100)), point3_inf));
CHECK(assert_equal( camera.backprojectPointAtInfinity(Point2( 100, 100)), point4_inf));
EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, 100)), point1_inf));
EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, -100)), point2_inf));
EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2( 100, -100)), point3_inf));
EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2( 100, 100)), point4_inf));
}
/* ************************************************************************* */
TEST( PinholeCamera, backproject2)
{
Point3 origin;
Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera looking down
Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down
Camera camera(Pose3(rot, origin), K);
Point3 actual = camera.backproject(Point2(), 1.);
Point3 expected(0., 1., 0.);
pair<Point2, bool> x = camera.projectSafe(expected);
CHECK(assert_equal(expected, actual));
CHECK(assert_equal(Point2(), x.first));
CHECK(x.second);
EXPECT(assert_equal(expected, actual));
EXPECT(assert_equal(Point2(), x.first));
EXPECT(x.second);
}
/* ************************************************************************* */
TEST( PinholeCamera, backprojectInfinity2)
{
Point3 origin;
Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera looking down
Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down
Camera camera(Pose3(rot, origin), K);
Point3 actual = camera.backprojectPointAtInfinity(Point2());
Point3 expected(0., 1., 0.);
Point2 x = camera.projectPointAtInfinity(expected);
CHECK(assert_equal(expected, actual));
CHECK(assert_equal(Point2(), x));
EXPECT(assert_equal(expected, actual));
EXPECT(assert_equal(Point2(), x));
}
/* ************************************************************************* */
@ -159,8 +158,8 @@ TEST( PinholeCamera, backprojectInfinity3)
Point3 expected(0., 0., 1.);
Point2 x = camera.projectPointAtInfinity(expected);
CHECK(assert_equal(expected, actual));
CHECK(assert_equal(Point2(), x));
EXPECT(assert_equal(expected, actual));
EXPECT(assert_equal(Point2(), x));
}
/* ************************************************************************* */
@ -173,13 +172,13 @@ TEST( PinholeCamera, Dproject)
{
Matrix Dpose, Dpoint, Dcal;
Point2 result = camera.project(point1, Dpose, Dpoint, Dcal);
Matrix numerical_pose = numericalDerivative31(project3, pose1, point1, K);
Matrix numerical_point = numericalDerivative32(project3, pose1, point1, K);
Matrix numerical_cal = numericalDerivative33(project3, pose1, point1, K);
CHECK(assert_equal(Point2(-100, 100), result));
CHECK(assert_equal(numerical_pose, Dpose, 1e-7));
CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
CHECK(assert_equal(numerical_cal, Dcal, 1e-7));
Matrix numerical_pose = numericalDerivative31(project3, pose, point1, K);
Matrix Hexpected2 = numericalDerivative32(project3, pose, point1, K);
Matrix numerical_cal = numericalDerivative33(project3, pose, point1, K);
EXPECT(assert_equal(Point2(-100, 100), result));
EXPECT(assert_equal(numerical_pose, Dpose, 1e-7));
EXPECT(assert_equal(Hexpected2, Dpoint, 1e-7));
EXPECT(assert_equal(numerical_cal, Dcal, 1e-7));
}
/* ************************************************************************* */
@ -190,21 +189,21 @@ static Point2 projectInfinity3(const Pose3& pose, const Point3& point3D, const C
TEST( PinholeCamera, Dproject_Infinity)
{
Matrix Dpose, Dpoint, Dcal;
Point3 point3D(point1.x(), point1.y(), -10.0); // a point in front of the camera
Point3 point3D(point1.x(), point1.y(), -10.0); // a point in front of the camera1
// test Projection
Point2 actual = camera.projectPointAtInfinity(point3D, Dpose, Dpoint, Dcal);
Point2 expected(-5.0, 5.0);
CHECK(assert_equal(actual, expected, 1e-7));
EXPECT(assert_equal(actual, expected, 1e-7));
// test Jacobians
Matrix numerical_pose = numericalDerivative31(projectInfinity3, pose1, point3D, K);
Matrix numerical_point = numericalDerivative32(projectInfinity3, pose1, point3D, K);
Matrix numerical_point2x2 = numerical_point.block(0,0,2,2); // only the direction to the point matters
Matrix numerical_cal = numericalDerivative33(projectInfinity3, pose1, point3D, K);
CHECK(assert_equal(numerical_pose, Dpose, 1e-7));
CHECK(assert_equal(numerical_point2x2, Dpoint, 1e-7));
CHECK(assert_equal(numerical_cal, Dcal, 1e-7));
Matrix numerical_pose = numericalDerivative31(projectInfinity3, pose, point3D, K);
Matrix Hexpected2 = numericalDerivative32(projectInfinity3, pose, point3D, K);
Matrix numerical_point2x2 = Hexpected2.block(0,0,2,2); // only the direction to the point matters
Matrix numerical_cal = numericalDerivative33(projectInfinity3, pose, point3D, K);
EXPECT(assert_equal(numerical_pose, Dpose, 1e-7));
EXPECT(assert_equal(numerical_point2x2, Dpoint, 1e-7));
EXPECT(assert_equal(numerical_cal, Dcal, 1e-7));
}
/* ************************************************************************* */
@ -217,11 +216,80 @@ TEST( PinholeCamera, Dproject2)
{
Matrix Dcamera, Dpoint;
Point2 result = camera.project2(point1, Dcamera, Dpoint);
Matrix numerical_camera = numericalDerivative21(project4, camera, point1);
Matrix numerical_point = numericalDerivative22(project4, camera, point1);
CHECK(assert_equal(result, Point2(-100, 100) ));
CHECK(assert_equal(numerical_camera, Dcamera, 1e-7));
CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
Matrix Hexpected1 = numericalDerivative21(project4, camera, point1);
Matrix Hexpected2 = numericalDerivative22(project4, camera, point1);
EXPECT(assert_equal(result, Point2(-100, 100) ));
EXPECT(assert_equal(Hexpected1, Dcamera, 1e-7));
EXPECT(assert_equal(Hexpected2, Dpoint, 1e-7));
}
/* ************************************************************************* */
static double range0(const Camera& camera, const Point3& point) {
return camera.range(point);
}
/* ************************************************************************* */
TEST( PinholeCamera, range0) {
Matrix D1; Matrix D2;
double result = camera.range(point1, D1, D2);
Matrix Hexpected1 = numericalDerivative21(range0, camera, point1);
Matrix Hexpected2 = numericalDerivative22(range0, camera, point1);
EXPECT_DOUBLES_EQUAL(point1.distance(camera.pose().translation()), result,
1e-9);
EXPECT(assert_equal(Hexpected1, D1, 1e-7));
EXPECT(assert_equal(Hexpected2, D2, 1e-7));
}
/* ************************************************************************* */
static double range1(const Camera& camera, const Pose3& pose) {
return camera.range(pose);
}
/* ************************************************************************* */
TEST( PinholeCamera, range1) {
Matrix D1; Matrix D2;
double result = camera.range(pose1, D1, D2);
Matrix Hexpected1 = numericalDerivative21(range1, camera, pose1);
Matrix Hexpected2 = numericalDerivative22(range1, camera, pose1);
EXPECT_DOUBLES_EQUAL(1, result, 1e-9);
EXPECT(assert_equal(Hexpected1, D1, 1e-7));
EXPECT(assert_equal(Hexpected2, D2, 1e-7));
}
/* ************************************************************************* */
typedef PinholeCamera<Cal3Bundler> Camera2;
static const Cal3Bundler K2(625, 1e-3, 1e-3);
static const Camera2 camera2(pose1, K2);
static double range2(const Camera& camera, const Camera2& camera2) {
return camera.range<Cal3Bundler>(camera2);
}
/* ************************************************************************* */
TEST( PinholeCamera, range2) {
Matrix D1; Matrix D2;
double result = camera.range<Cal3Bundler>(camera2, D1, D2);
Matrix Hexpected1 = numericalDerivative21(range2, camera, camera2);
Matrix Hexpected2 = numericalDerivative22(range2, camera, camera2);
EXPECT_DOUBLES_EQUAL(1, result, 1e-9);
EXPECT(assert_equal(Hexpected1, D1, 1e-7));
EXPECT(assert_equal(Hexpected2, D2, 1e-7));
}
/* ************************************************************************* */
static const CalibratedCamera camera3(pose1);
static double range3(const Camera& camera, const CalibratedCamera& camera3) {
return camera.range(camera3);
}
/* ************************************************************************* */
TEST( PinholeCamera, range3) {
Matrix D1; Matrix D2;
double result = camera.range(camera3, D1, D2);
Matrix Hexpected1 = numericalDerivative21(range3, camera, camera3);
Matrix Hexpected2 = numericalDerivative22(range3, camera, camera3);
EXPECT_DOUBLES_EQUAL(1, result, 1e-9);
EXPECT(assert_equal(Hexpected1, D1, 1e-7));
EXPECT(assert_equal(Hexpected2, D2, 1e-7));
}
/* ************************************************************************* */

6
gtsam/geometry/tests/testPose2.cpp
View File

@ -44,7 +44,7 @@ TEST(Pose2, constructors) {
Pose2 origin;
assert_equal(pose,origin);
Pose2 t(M_PI/2.0+0.018, Point2(1.015, 2.01));
EXPECT(assert_equal(t,Pose2(t.matrix())));
EXPECT(assert_equal(t,Pose2((Matrix)t.matrix())));
}
/* ************************************************************************* */
@ -519,7 +519,7 @@ TEST( Pose2, bearing )
expectedH1 = numericalDerivative21(bearing_proxy, x2, l3);
EXPECT(assert_equal(expectedH1,actualH1));
expectedH2 = numericalDerivative22(bearing_proxy, x2, l3);
EXPECT(assert_equal(expectedH1,actualH1));
EXPECT(assert_equal(expectedH2,actualH2));
// establish bearing is indeed 45 degrees even if rotated
Rot2 actual34 = x3.bearing(l4, actualH1, actualH2);
@ -529,7 +529,7 @@ TEST( Pose2, bearing )
expectedH1 = numericalDerivative21(bearing_proxy, x3, l4);
expectedH2 = numericalDerivative22(bearing_proxy, x3, l4);
EXPECT(assert_equal(expectedH1,actualH1));
EXPECT(assert_equal(expectedH1,actualH1));
EXPECT(assert_equal(expectedH2,actualH2));
}
/* ************************************************************************* */

4
gtsam/geometry/tests/testRot2.cpp
View File

@ -49,7 +49,9 @@ TEST( Rot2, unit)
/* ************************************************************************* */
TEST( Rot2, transpose)
{
CHECK(assert_equal(R.inverse().matrix(),R.transpose()));
Matrix expected = R.inverse().matrix();
Matrix actual = R.transpose();
CHECK(assert_equal(expected,actual));
}
/* ************************************************************************* */

13
gtsam/geometry/tests/testRot3.cpp
View File

@ -37,7 +37,6 @@ GTSAM_CONCEPT_LIE_INST(Rot3)
static Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2);
static Point3 P(0.2, 0.7, -2.0);
static double error = 1e-9, epsilon = 0.001;
static const Matrix I3 = eye(3);
/* ************************************************************************* */
TEST( Rot3, chart)
@ -50,7 +49,7 @@ TEST( Rot3, chart)
/* ************************************************************************* */
TEST( Rot3, constructor)
{
Rot3 expected(I3);
Rot3 expected((Matrix)I_3x3);
Point3 r1(1,0,0), r2(0,1,0), r3(0,0,1);
Rot3 actual(r1, r2, r3);
CHECK(assert_equal(actual,expected));
@ -95,7 +94,7 @@ Rot3 slow_but_correct_rodriguez(const Vector& w) {
double t = norm_2(w);
Matrix J = skewSymmetric(w / t);
if (t < 1e-5) return Rot3();
Matrix R = I3 + sin(t) * J + (1.0 - cos(t)) * (J * J);
Matrix R = I_3x3 + sin(t) * J + (1.0 - cos(t)) * (J * J);
return R;
}
@ -359,7 +358,7 @@ TEST( Rot3, inverse )
Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3);
Rot3 I;
Matrix actualH;
Matrix3 actualH;
Rot3 actual = R.inverse(actualH);
CHECK(assert_equal(I,R*actual));
CHECK(assert_equal(I,actual*R));
@ -482,7 +481,7 @@ TEST( Rot3, RQ)
Vector actual;
boost::tie(actualK, actual) = RQ(R.matrix());
Vector expected = Vector3(0.14715, 0.385821, 0.231671);
CHECK(assert_equal(I3,actualK));
CHECK(assert_equal(I_3x3,actualK));
CHECK(assert_equal(expected,actual,1e-6));
// Try using xyz call, asserting that Rot3::RzRyRx(x,y,z).xyz()==[x;y;z]
@ -516,7 +515,7 @@ TEST( Rot3, expmapStability ) {
w(2), 0.0, -w(0),
-w(1), w(0), 0.0 ).finished();
Matrix W2 = W*W;
Matrix Rmat = I3 + (1.0-theta2/6.0 + theta2*theta2/120.0
Matrix Rmat = I_3x3 + (1.0-theta2/6.0 + theta2*theta2/120.0
- theta2*theta2*theta2/5040.0)*W + (0.5 - theta2/24.0 + theta2*theta2/720.0)*W2 ;
Rot3 expectedR( Rmat );
CHECK(assert_equal(expectedR, actualR, 1e-10));
@ -578,7 +577,7 @@ TEST(Rot3, quaternion) {
TEST( Rot3, Cayley ) {
Matrix A = skewSymmetric(1,2,-3);
Matrix Q = Cayley(A);
EXPECT(assert_equal(I3, trans(Q)*Q));
EXPECT(assert_equal((Matrix)I_3x3, trans(Q)*Q));
EXPECT(assert_equal(A, Cayley(Q)));
}

1
gtsam/geometry/tests/testRot3M.cpp
View File

@ -37,7 +37,6 @@ GTSAM_CONCEPT_LIE_INST(Rot3)
static Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2);
static Point3 P(0.2, 0.7, -2.0);
static const Matrix I3 = eye(3);
/* ************************************************************************* */
TEST(Rot3, manifold_cayley)

19
gtsam/geometry/tests/testTriangulation.cpp
View File

@ -262,25 +262,6 @@ TEST( triangulation, twoIdenticalPoses) {
}
*/
//******************************************************************************
TEST( triangulation, TriangulationFactor ) {
// Create the factor with a measurement that is 3 pixels off in x
Key pointKey(1);
SharedNoiseModel model;
typedef TriangulationFactor<> Factor;
Factor factor(camera1, z1, model, pointKey);
// Use the factor to calculate the Jacobians
Matrix HActual;
factor.evaluateError(landmark, HActual);
Matrix HExpected = numericalDerivative11<Vector,Point3>(
boost::bind(&Factor::evaluateError, &factor, _1, boost::none), landmark);
// Verify the Jacobians are correct
CHECK(assert_equal(HExpected, HActual, 1e-3));
}
//******************************************************************************
int main() {
TestResult tr;

6
gtsam/geometry/tests/testUnit3.cpp
View File

@ -108,9 +108,9 @@ TEST(Unit3, unrotate) {
TEST(Unit3, error) {
Unit3 p(1, 0, 0), q = p.retract(Vector2(0.5, 0)), //
r = p.retract(Vector2(0.8, 0));
EXPECT(assert_equal(Vector2(0, 0), p.error(p), 1e-8));
EXPECT(assert_equal(Vector2(0.479426, 0), p.error(q), 1e-5));
EXPECT(assert_equal(Vector2(0.717356, 0), p.error(r), 1e-5));
EXPECT(assert_equal((Vector)(Vector2(0, 0)), p.error(p), 1e-8));
EXPECT(assert_equal((Vector)(Vector2(0.479426, 0)), p.error(q), 1e-5));
EXPECT(assert_equal((Vector)(Vector2(0.717356, 0)), p.error(r), 1e-5));
Matrix actual, expected;
// Use numerical derivatives to calculate the expected Jacobian

4
gtsam/geometry/triangulation.cpp
View File

@ -30,7 +30,7 @@ namespace gtsam {
* @param rank_tol SVD rank tolerance
* @return Triangulated Point3
*/
Point3 triangulateDLT(const std::vector<Matrix>& projection_matrices,
Point3 triangulateDLT(const std::vector<Matrix34>& projection_matrices,
const std::vector<Point2>& measurements, double rank_tol) {
// number of cameras
@ -41,7 +41,7 @@ Point3 triangulateDLT(const std::vector<Matrix>& projection_matrices,
for (size_t i = 0; i < m; i++) {
size_t row = i * 2;
const Matrix& projection = projection_matrices.at(i);
const Matrix34& projection = projection_matrices.at(i);
const Point2& p = measurements.at(i);
// build system of equations

24
gtsam/geometry/triangulation.h
View File

@ -19,12 +19,10 @@
#pragma once
#include <gtsam/geometry/TriangulationFactor.h>
#include <gtsam/slam/TriangulationFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/PriorFactor.h>
#include <vector>
namespace gtsam {
@ -53,7 +51,7 @@ public:
* @return Triangulated Point3
*/
GTSAM_EXPORT Point3 triangulateDLT(
const std::vector<Matrix>& projection_matrices,
const std::vector<Matrix34>& projection_matrices,
const std::vector<Point2>& measurements, double rank_tol);
///
@ -189,12 +187,11 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
throw(TriangulationUnderconstrainedException());
// construct projection matrices from poses & calibration
std::vector<Matrix> projection_matrices;
std::vector<Matrix34> projection_matrices;
BOOST_FOREACH(const Pose3& pose, poses) {
projection_matrices.push_back(
sharedCal->K() * sub(pose.inverse().matrix(), 0, 3, 0, 4));
sharedCal->K() * (pose.inverse().matrix()).block<3,4>(0,0));
}
// Triangulate linearly
Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol);
@ -240,12 +237,11 @@ Point3 triangulatePoint3(
// construct projection matrices from poses & calibration
typedef PinholeCamera<CALIBRATION> Camera;
std::vector<Matrix> projection_matrices;
BOOST_FOREACH(const Camera& camera, cameras)
projection_matrices.push_back(
camera.calibration().K()
* sub(camera.pose().inverse().matrix(), 0, 3, 0, 4));
std::vector<Matrix34> projection_matrices;
BOOST_FOREACH(const Camera& camera, cameras) {
Matrix P_ = (camera.pose().inverse().matrix());
projection_matrices.push_back(camera.calibration().K()* (P_.block<3,4>(0,0)) );
}
Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol);
// The n refine using non-linear optimization

8
gtsam/nonlinear/AdaptAutoDiff.h
View File

@ -20,6 +20,7 @@
#include <gtsam/3rdparty/ceres/autodiff.h>
#include <gtsam/base/Manifold.h>
#include <gtsam/base/OptionalJacobian.h>
namespace gtsam {
@ -50,11 +51,8 @@ class AdaptAutoDiff {
public:
typedef Eigen::Matrix<double, N, M1> JacobianTA1;
typedef Eigen::Matrix<double, N, M2> JacobianTA2;
T operator()(const A1& a1, const A2& a2, boost::optional<JacobianTA1&> H1 =
boost::none, boost::optional<JacobianTA2&> H2 = boost::none) {
T operator()(const A1& a1, const A2& a2, OptionalJacobian<N, M1> H1 = boost::none,
OptionalJacobian<N, M2> H2 = boost::none) {
using ceres::internal::AutoDiff;

64
gtsam/nonlinear/Expression-inl.h
View File

@ -23,6 +23,7 @@
#include <gtsam/nonlinear/Values.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/Manifold.h>
#include <gtsam/base/OptionalJacobian.h>
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
@ -42,21 +43,21 @@ namespace gtsam {
const unsigned TraceAlignment = 16;
template <typename T>
T & upAlign(T & value, unsigned requiredAlignment = TraceAlignment){
template<typename T>
T & upAlign(T & value, unsigned requiredAlignment = TraceAlignment) {
// right now only word sized types are supported.
// Easy to extend if needed,
// by somehow inferring the unsigned integer of same size
BOOST_STATIC_ASSERT(sizeof(T) == sizeof(size_t));
size_t & uiValue = reinterpret_cast<size_t &>(value);
size_t misAlignment = uiValue % requiredAlignment;
if(misAlignment) {
if (misAlignment) {
uiValue += requiredAlignment - misAlignment;
}
return value;
}
template <typename T>
T upAligned(T value, unsigned requiredAlignment = TraceAlignment){
template<typename T>
T upAligned(T value, unsigned requiredAlignment = TraceAlignment) {
return upAlign(value, requiredAlignment);
}
@ -88,19 +89,21 @@ public:
namespace internal {
template <bool UseBlock, typename Derived>
template<bool UseBlock, typename Derived>
struct UseBlockIf {
static void addToJacobian(const Eigen::MatrixBase<Derived>& dTdA,
JacobianMap& jacobians, Key key){
JacobianMap& jacobians, Key key) {
// block makes HUGE difference
jacobians(key).block<Derived::RowsAtCompileTime, Derived::ColsAtCompileTime>(0, 0) += dTdA;
};
jacobians(key).block<Derived::RowsAtCompileTime, Derived::ColsAtCompileTime>(
0, 0) += dTdA;
}
;
};
/// Handle Leaf Case for Dynamic Matrix type (slower)
template <typename Derived>
template<typename Derived>
struct UseBlockIf<false, Derived> {
static void addToJacobian(const Eigen::MatrixBase<Derived>& dTdA,
JacobianMap& jacobians, Key key) {
JacobianMap& jacobians, Key key) {
jacobians(key) += dTdA;
}
};
@ -111,10 +114,9 @@ template<typename Derived>
void handleLeafCase(const Eigen::MatrixBase<Derived>& dTdA,
JacobianMap& jacobians, Key key) {
internal::UseBlockIf<
Derived::RowsAtCompileTime != Eigen::Dynamic &&
Derived::ColsAtCompileTime != Eigen::Dynamic,
Derived>
::addToJacobian(dTdA, jacobians, key);
Derived::RowsAtCompileTime != Eigen::Dynamic
&& Derived::ColsAtCompileTime != Eigen::Dynamic, Derived>::addToJacobian(
dTdA, jacobians, key);
}
//-----------------------------------------------------------------------------
@ -265,7 +267,7 @@ public:
/// Construct an execution trace for reverse AD
virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
ExecutionTraceStorage* traceStorage) const = 0;
ExecutionTraceStorage* traceStorage) const = 0;
};
//-----------------------------------------------------------------------------
@ -336,7 +338,7 @@ public:
/// Construct an execution trace for reverse AD
virtual const value_type& traceExecution(const Values& values,
ExecutionTrace<value_type>& trace,
ExecutionTrace<value_type>& trace,
ExecutionTraceStorage* traceStorage) const {
trace.setLeaf(key_);
return dynamic_cast<const value_type&>(values.at(key_));
@ -454,10 +456,9 @@ struct Jacobian {
/// meta-function to generate JacobianTA optional reference
template<class T, class A>
struct OptionalJacobian {
typedef Eigen::Matrix<double, traits::dimension<T>::value,
traits::dimension<A>::value> Jacobian;
typedef boost::optional<Jacobian&> type;
struct MakeOptionalJacobian {
typedef OptionalJacobian<traits::dimension<T>::value,
traits::dimension<A>::value> type;
};
/**
@ -569,7 +570,7 @@ struct GenerateFunctionalNode: Argument<T, A, Base::N + 1>, Base {
};
/// Construct an execution trace for reverse AD
void trace(const Values& values, Record* record,
void trace(const Values& values, Record* record,
ExecutionTraceStorage*& traceStorage) const {
Base::trace(values, record, traceStorage); // recurse
// Write an Expression<A> execution trace in record->trace
@ -645,7 +646,7 @@ struct FunctionalNode {
};
/// Construct an execution trace for reverse AD
Record* trace(const Values& values,
Record* trace(const Values& values,
ExecutionTraceStorage* traceStorage) const {
assert(reinterpret_cast<size_t>(traceStorage) % TraceAlignment == 0);
@ -670,7 +671,8 @@ class UnaryExpression: public FunctionalNode<T, boost::mpl::vector<A1> >::type {
public:
typedef boost::function<T(const A1&, typename OptionalJacobian<T, A1>::type)> Function;
typedef boost::function<
T(const A1&, typename MakeOptionalJacobian<T, A1>::type)> Function;
typedef typename FunctionalNode<T, boost::mpl::vector<A1> >::type Base;
typedef typename Base::Record Record;
@ -715,8 +717,8 @@ class BinaryExpression: public FunctionalNode<T, boost::mpl::vector<A1, A2> >::t
public:
typedef boost::function<
T(const A1&, const A2&, typename OptionalJacobian<T, A1>::type,
typename OptionalJacobian<T, A2>::type)> Function;
T(const A1&, const A2&, typename MakeOptionalJacobian<T, A1>::type,
typename MakeOptionalJacobian<T, A2>::type)> Function;
typedef typename FunctionalNode<T, boost::mpl::vector<A1, A2> >::type Base;
typedef typename Base::Record Record;
@ -769,9 +771,10 @@ class TernaryExpression: public FunctionalNode<T, boost::mpl::vector<A1, A2, A3>
public:
typedef boost::function<
T(const A1&, const A2&, const A3&, typename OptionalJacobian<T, A1>::type,
typename OptionalJacobian<T, A2>::type,
typename OptionalJacobian<T, A3>::type)> Function;
T(const A1&, const A2&, const A3&,
typename MakeOptionalJacobian<T, A1>::type,
typename MakeOptionalJacobian<T, A2>::type,
typename MakeOptionalJacobian<T, A3>::type)> Function;
typedef typename FunctionalNode<T, boost::mpl::vector<A1, A2, A3> >::type Base;
typedef typename Base::Record Record;
@ -787,7 +790,8 @@ private:
this->template reset<A2, 2>(e2.root());
this->template reset<A3, 3>(e3.root());
ExpressionNode<T>::traceSize_ = //
upAligned(sizeof(Record)) + e1.traceSize() + e2.traceSize() + e3.traceSize();
upAligned(sizeof(Record)) + e1.traceSize() + e2.traceSize()
+ e3.traceSize();
}
friend class Expression<T> ;

13
gtsam/nonlinear/Expression.h
View File

@ -20,8 +20,8 @@
#pragma once
#include <gtsam/nonlinear/Expression-inl.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/base/FastVector.h>
#include <boost/bind.hpp>
#include <boost/range/adaptor/map.hpp>
@ -75,7 +75,7 @@ public:
/// Construct a nullary method expression
template<typename A>
Expression(const Expression<A>& expression,
T (A::*method)(typename OptionalJacobian<T, A>::type) const) :
T (A::*method)(typename MakeOptionalJacobian<T, A>::type) const) :
root_(new UnaryExpression<T, A>(boost::bind(method, _1, _2), expression)) {
}
@ -89,8 +89,8 @@ public:
/// Construct a unary method expression
template<typename A1, typename A2>
Expression(const Expression<A1>& expression1,
T (A1::*method)(const A2&, typename OptionalJacobian<T, A1>::type,
typename OptionalJacobian<T, A2>::type) const,
T (A1::*method)(const A2&, typename MakeOptionalJacobian<T, A1>::type,
typename MakeOptionalJacobian<T, A2>::type) const,
const Expression<A2>& expression2) :
root_(
new BinaryExpression<T, A1, A2>(boost::bind(method, _1, _2, _3, _4),
@ -224,9 +224,8 @@ template<class T>
struct apply_compose {
typedef T result_type;
static const int Dim = traits::dimension<T>::value;
typedef Eigen::Matrix<double, Dim, Dim> Jacobian;
T operator()(const T& x, const T& y, boost::optional<Jacobian&> H1,
boost::optional<Jacobian&> H2) const {
T operator()(const T& x, const T& y, OptionalJacobian<Dim, Dim> H1 =
boost::none, OptionalJacobian<Dim, Dim> H2 = boost::none) const {
return x.compose(y, H1, H2);
}
};

6
gtsam/nonlinear/NonlinearFactor.h
View File

@ -20,14 +20,14 @@
#pragma once
#include <boost/serialization/base_object.hpp>
#include <boost/assign/list_of.hpp>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/inference/Factor.h>
#include <gtsam/base/OptionalJacobian.h>
#include <boost/serialization/base_object.hpp>
#include <boost/assign/list_of.hpp>
/**
* Macro to add a standard clone function to a derived factor

20
gtsam/nonlinear/tests/testExpression.cpp
View File

@ -34,8 +34,8 @@ using namespace gtsam;
/* ************************************************************************* */
template<class CAL>
Point2 uncalibrate(const CAL& K, const Point2& p,
boost::optional<Matrix25&> Dcal, boost::optional<Matrix2&> Dp) {
Point2 uncalibrate(const CAL& K, const Point2& p, OptionalJacobian<2, 5> Dcal,
OptionalJacobian<2, 2> Dp) {
return K.uncalibrate(p, Dcal, Dp);
}
@ -75,13 +75,13 @@ TEST(Expression, Leaves) {
/* ************************************************************************* */
// Unary(Leaf)
namespace unary {
Point2 f0(const Point3& p, boost::optional<Matrix23&> H) {
Point2 f0(const Point3& p, OptionalJacobian<2,3> H) {
return Point2();
}
LieScalar f1(const Point3& p, boost::optional<Eigen::Matrix<double, 1, 3>&> H) {
LieScalar f1(const Point3& p, OptionalJacobian<1, 3> H) {
return LieScalar(0.0);
}
double f2(const Point3& p, boost::optional<Eigen::Matrix<double, 1, 3>&> H) {
double f2(const Point3& p, OptionalJacobian<1, 3> H) {
return 0.0;
}
Expression<Point3> p(1);
@ -117,7 +117,7 @@ TEST(Expression, NullaryMethod) {
// Check dims as map
std::map<Key, int> map;
norm.dims(map);
LONGS_EQUAL(1,map.size());
LONGS_EQUAL(1, map.size());
// Get value and Jacobians
std::vector<Matrix> H(1);
@ -133,9 +133,8 @@ TEST(Expression, NullaryMethod) {
// Binary(Leaf,Leaf)
namespace binary {
// Create leaves
double doubleF(const Pose3& pose, const Point3& point,
boost::optional<Eigen::Matrix<double, 1, 6>&> H1,
boost::optional<Eigen::Matrix<double, 1, 3>&> H2) {
double doubleF(const Pose3& pose, //
const Point3& point, OptionalJacobian<1, 6> H1, OptionalJacobian<1, 3> H2) {
return 0.0;
}
Expression<Pose3> x(1);
@ -244,8 +243,7 @@ TEST(Expression, compose3) {
/* ************************************************************************* */
// Test with ternary function
Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3,
boost::optional<Matrix3&> H1, boost::optional<Matrix3&> H2,
boost::optional<Matrix3&> H3) {
OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2, OptionalJacobian<3, 3> H3) {
// return dummy derivatives (not correct, but that's ok for testing here)
if (H1)
*H1 = eye(3);

233
gtsam/slam/RangeFactor.h
View File

@ -16,99 +16,176 @@
#pragma once
#include <boost/lexical_cast.hpp>
#include <gtsam/geometry/concepts.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/concepts.h>
#include <boost/lexical_cast.hpp>
namespace gtsam {
/**
* Binary factor for a range measurement
* @addtogroup SLAM
*/
template<class POSE, class POINT>
class RangeFactor: public NoiseModelFactor2<POSE, POINT> {
private:
/**
* Binary factor for a range measurement
* @addtogroup SLAM
*/
template<class T1, class T2 = T1>
class RangeFactor: public NoiseModelFactor2<T1, T2> {
private:
double measured_; /** measurement */
boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame
double measured_; /** measurement */
typedef RangeFactor<POSE, POINT> This;
typedef NoiseModelFactor2<POSE, POINT> Base;
typedef RangeFactor<T1, T2> This;
typedef NoiseModelFactor2<T1, T2> Base;
typedef POSE Pose;
typedef POINT Point;
// Concept requirements for this factor
GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(T1, T2)
// Concept requirements for this factor
GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(POSE, POINT)
public:
public:
RangeFactor() {
} /* Default constructor */
RangeFactor() {} /* Default constructor */
RangeFactor(Key key1, Key key2, double measured,
const SharedNoiseModel& model) :
Base(model, key1, key2), measured_(measured) {
}
RangeFactor(Key poseKey, Key pointKey, double measured,
const SharedNoiseModel& model, boost::optional<POSE> body_P_sensor = boost::none) :
Base(model, poseKey, pointKey), measured_(measured), body_P_sensor_(body_P_sensor) {
virtual ~RangeFactor() {
}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
/** h(x)-z */
Vector evaluateError(const T1& t1, const T2& t2, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none) const {
double hx;
hx = t1.range(t2, H1, H2);
return (Vector(1) << hx - measured_).finished();
}
/** return the measured */
double measured() const {
return measured_;
}
/** equals specialized to this factor */
virtual bool equals(const NonlinearFactor& expected,
double tol = 1e-9) const {
const This *e = dynamic_cast<const This*>(&expected);
return e != NULL && Base::equals(*e, tol)
&& fabs(this->measured_ - e->measured_) < tol;
}
/** print contents */
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const {
std::cout << s << "RangeFactor, range = " << measured_ << std::endl;
Base::print("", keyFormatter);
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar
& boost::serialization::make_nvp("NoiseModelFactor2",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_);
}
};
// RangeFactor
/**
* Binary factor for a range measurement, with a transform applied
* @addtogroup SLAM
*/
template<class POSE, class T2 = POSE>
class RangeFactorWithTransform: public NoiseModelFactor2<POSE, T2> {
private:
double measured_; /** measurement */
POSE body_P_sensor_; ///< The pose of the sensor in the body frame
typedef RangeFactorWithTransform<POSE, T2> This;
typedef NoiseModelFactor2<POSE, T2> Base;
// Concept requirements for this factor
GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(POSE, T2)
public:
RangeFactorWithTransform() {
} /* Default constructor */
RangeFactorWithTransform(Key key1, Key key2, double measured,
const SharedNoiseModel& model, const POSE& body_P_sensor) :
Base(model, key1, key2), measured_(measured), body_P_sensor_(
body_P_sensor) {
}
virtual ~RangeFactorWithTransform() {
}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
/** h(x)-z */
Vector evaluateError(const POSE& t1, const T2& t2,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
boost::none) const {
double hx;
if (H1) {
Matrix H0;
hx = t1.compose(body_P_sensor_, H0).range(t2, H1, H2);
*H1 = *H1 * H0;
} else {
hx = t1.compose(body_P_sensor_).range(t2, H1, H2);
}
return (Vector(1) << hx - measured_).finished();
}
virtual ~RangeFactor() {}
/** return the measured */
double measured() const {
return measured_;
}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/** equals specialized to this factor */
virtual bool equals(const NonlinearFactor& expected,
double tol = 1e-9) const {
const This *e = dynamic_cast<const This*>(&expected);
return e != NULL && Base::equals(*e, tol)
&& fabs(this->measured_ - e->measured_) < tol
&& body_P_sensor_.equals(e->body_P_sensor_);
}
/** h(x)-z */
Vector evaluateError(const POSE& pose, const POINT& point,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const {
double hx;
if(body_P_sensor_) {
if(H1) {
Matrix H0;
hx = pose.compose(*body_P_sensor_, H0).range(point, H1, H2);
*H1 = *H1 * H0;
} else {
hx = pose.compose(*body_P_sensor_).range(point, H1, H2);
}
} else {
hx = pose.range(point, H1, H2);
}
return (Vector(1) << hx - measured_).finished();
}
/** print contents */
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const {
std::cout << s << "RangeFactor, range = " << measured_ << std::endl;
this->body_P_sensor_.print(" sensor pose in body frame: ");
Base::print("", keyFormatter);
}
/** return the measured */
double measured() const {
return measured_;
}
private:
/** equals specialized to this factor */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL
&& Base::equals(*e, tol)
&& fabs(this->measured_ - e->measured_) < tol
&& ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
}
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar
& boost::serialization::make_nvp("NoiseModelFactor2",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
}
};
// RangeFactor
/** print contents */
void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "RangeFactor, range = " << measured_ << std::endl;
if(this->body_P_sensor_)
this->body_P_sensor_->print(" sensor pose in body frame: ");
Base::print("", keyFormatter);
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor2",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
}
}; // RangeFactor
} // namespace gtsam
}// namespace gtsam

3
gtsam/geometry/TriangulationFactor.h → gtsam/slam/TriangulationFactor.h
View File

@ -10,14 +10,13 @@
* -------------------------------------------------------------------------- */
/**
* testTriangulationFactor.h
* triangulationFactor.h
* @date March 2, 2014
* @author Frank Dellaert
*/
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/base/numericalDerivative.h>
#include <boost/optional.hpp>
#include <boost/make_shared.hpp>

91
gtsam/slam/tests/testRangeFactor.cpp
View File

@ -34,14 +34,30 @@ static SharedNoiseModel model(noiseModel::Unit::Create(1));
typedef RangeFactor<Pose2, Point2> RangeFactor2D;
typedef RangeFactor<Pose3, Point3> RangeFactor3D;
typedef RangeFactorWithTransform<Pose2, Point2> RangeFactorWithTransform2D;
typedef RangeFactorWithTransform<Pose3, Point3> RangeFactorWithTransform3D;
/* ************************************************************************* */
Vector factorError2D(const Pose2& pose, const Point2& point, const RangeFactor2D& factor) {
Vector factorError2D(const Pose2& pose, const Point2& point,
const RangeFactor2D& factor) {
return factor.evaluateError(pose, point);
}
/* ************************************************************************* */
Vector factorError3D(const Pose3& pose, const Point3& point, const RangeFactor3D& factor) {
Vector factorError3D(const Pose3& pose, const Point3& point,
const RangeFactor3D& factor) {
return factor.evaluateError(pose, point);
}
/* ************************************************************************* */
Vector factorErrorWithTransform2D(const Pose2& pose, const Point2& point,
const RangeFactorWithTransform2D& factor) {
return factor.evaluateError(pose, point);
}
/* ************************************************************************* */
Vector factorErrorWithTransform3D(const Pose3& pose, const Point3& point,
const RangeFactorWithTransform3D& factor) {
return factor.evaluateError(pose, point);
}
@ -61,10 +77,13 @@ TEST( RangeFactor, ConstructorWithTransform) {
Key pointKey(2);
double measurement(10.0);
Pose2 body_P_sensor_2D(0.25, -0.10, -M_PI_2);
Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
Point3(0.25, -0.10, 1.0));
RangeFactor2D factor2D(poseKey, pointKey, measurement, model, body_P_sensor_2D);
RangeFactor3D factor3D(poseKey, pointKey, measurement, model, body_P_sensor_3D);
RangeFactorWithTransform2D factor2D(poseKey, pointKey, measurement, model,
body_P_sensor_2D);
RangeFactorWithTransform3D factor3D(poseKey, pointKey, measurement, model,
body_P_sensor_3D);
}
/* ************************************************************************* */
@ -90,14 +109,19 @@ TEST( RangeFactor, EqualsWithTransform ) {
Key pointKey(2);
double measurement(10.0);
Pose2 body_P_sensor_2D(0.25, -0.10, -M_PI_2);
Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
Point3(0.25, -0.10, 1.0));
RangeFactor2D factor2D_1(poseKey, pointKey, measurement, model, body_P_sensor_2D);
RangeFactor2D factor2D_2(poseKey, pointKey, measurement, model, body_P_sensor_2D);
RangeFactorWithTransform2D factor2D_1(poseKey, pointKey, measurement, model,
body_P_sensor_2D);
RangeFactorWithTransform2D factor2D_2(poseKey, pointKey, measurement, model,
body_P_sensor_2D);
CHECK(assert_equal(factor2D_1, factor2D_2));
RangeFactor3D factor3D_1(poseKey, pointKey, measurement, model, body_P_sensor_3D);
RangeFactor3D factor3D_2(poseKey, pointKey, measurement, model, body_P_sensor_3D);
RangeFactorWithTransform3D factor3D_1(poseKey, pointKey, measurement, model,
body_P_sensor_3D);
RangeFactorWithTransform3D factor3D_2(poseKey, pointKey, measurement, model,
body_P_sensor_3D);
CHECK(assert_equal(factor3D_1, factor3D_2));
}
@ -130,7 +154,8 @@ TEST( RangeFactor, Error2DWithTransform ) {
Key pointKey(2);
double measurement(10.0);
Pose2 body_P_sensor(0.25, -0.10, -M_PI_2);
RangeFactor2D factor(poseKey, pointKey, measurement, model, body_P_sensor);
RangeFactorWithTransform2D factor(poseKey, pointKey, measurement, model,
body_P_sensor);
// Set the linearization point
Rot2 R(0.57);
@ -176,8 +201,10 @@ TEST( RangeFactor, Error3DWithTransform ) {
Key poseKey(1);
Key pointKey(2);
double measurement(10.0);
Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
RangeFactor3D factor(poseKey, pointKey, measurement, model, body_P_sensor);
Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
Point3(0.25, -0.10, 1.0));
RangeFactorWithTransform3D factor(poseKey, pointKey, measurement, model,
body_P_sensor);
// Set the linearization point
Rot3 R = Rot3::RzRyRx(0.2, -0.3, 1.75);
@ -213,8 +240,10 @@ TEST( RangeFactor, Jacobian2D ) {
// Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected;
H1Expected = numericalDerivative11<Vector, Pose2>(boost::bind(&factorError2D, _1, point, factor), pose);
H2Expected = numericalDerivative11<Vector, Point2>(boost::bind(&factorError2D, pose, _1, factor), point);
H1Expected = numericalDerivative11<Vector, Pose2>(
boost::bind(&factorError2D, _1, point, factor), pose);
H2Expected = numericalDerivative11<Vector, Point2>(
boost::bind(&factorError2D, pose, _1, factor), point);
// Verify the Jacobians are correct
CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
@ -228,7 +257,8 @@ TEST( RangeFactor, Jacobian2DWithTransform ) {
Key pointKey(2);
double measurement(10.0);
Pose2 body_P_sensor(0.25, -0.10, -M_PI_2);
RangeFactor2D factor(poseKey, pointKey, measurement, model, body_P_sensor);
RangeFactorWithTransform2D factor(poseKey, pointKey, measurement, model,
body_P_sensor);
// Set the linearization point
Rot2 R(0.57);
@ -242,8 +272,10 @@ TEST( RangeFactor, Jacobian2DWithTransform ) {
// Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected;
H1Expected = numericalDerivative11<Vector, Pose2>(boost::bind(&factorError2D, _1, point, factor), pose);
H2Expected = numericalDerivative11<Vector, Point2>(boost::bind(&factorError2D, pose, _1, factor), point);
H1Expected = numericalDerivative11<Vector, Pose2>(
boost::bind(&factorErrorWithTransform2D, _1, point, factor), pose);
H2Expected = numericalDerivative11<Vector, Point2>(
boost::bind(&factorErrorWithTransform2D, pose, _1, factor), point);
// Verify the Jacobians are correct
CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
@ -268,8 +300,10 @@ TEST( RangeFactor, Jacobian3D ) {
// Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected;
H1Expected = numericalDerivative11<Vector, Pose3>(boost::bind(&factorError3D, _1, point, factor), pose);
H2Expected = numericalDerivative11<Vector, Point3>(boost::bind(&factorError3D, pose, _1, factor), point);
H1Expected = numericalDerivative11<Vector, Pose3>(
boost::bind(&factorError3D, _1, point, factor), pose);
H2Expected = numericalDerivative11<Vector, Point3>(
boost::bind(&factorError3D, pose, _1, factor), point);
// Verify the Jacobians are correct
CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
@ -282,8 +316,10 @@ TEST( RangeFactor, Jacobian3DWithTransform ) {
Key poseKey(1);
Key pointKey(2);
double measurement(10.0);
Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
RangeFactor3D factor(poseKey, pointKey, measurement, model, body_P_sensor);
Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
Point3(0.25, -0.10, 1.0));
RangeFactorWithTransform3D factor(poseKey, pointKey, measurement, model,
body_P_sensor);
// Set the linearization point
Rot3 R = Rot3::RzRyRx(0.2, -0.3, 1.75);
@ -297,8 +333,10 @@ TEST( RangeFactor, Jacobian3DWithTransform ) {
// Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected;
H1Expected = numericalDerivative11<Vector, Pose3>(boost::bind(&factorError3D, _1, point, factor), pose);
H2Expected = numericalDerivative11<Vector, Point3>(boost::bind(&factorError3D, pose, _1, factor), point);
H1Expected = numericalDerivative11<Vector, Pose3>(
boost::bind(&factorErrorWithTransform3D, _1, point, factor), pose);
H2Expected = numericalDerivative11<Vector, Point3>(
boost::bind(&factorErrorWithTransform3D, pose, _1, factor), point);
// Verify the Jacobians are correct
CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
@ -306,6 +344,9 @@ TEST( RangeFactor, Jacobian3DWithTransform ) {
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

70
gtsam/slam/tests/testTriangulationFactor.cpp Normal file
View File

@ -0,0 +1,70 @@
/* ----------------------------------------------------------------------------
* 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
* -------------------------------------------------------------------------- */
/**
* testTriangulationFactor.cpp
*
* Created on: July 30th, 2013
* Author: cbeall3
*/
#include <gtsam/geometry/triangulation.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/assign.hpp>
#include <boost/assign/std/vector.hpp>
using namespace std;
using namespace gtsam;
using namespace boost::assign;
// Some common constants
static const boost::shared_ptr<Cal3_S2> sharedCal = //
boost::make_shared<Cal3_S2>(1500, 1200, 0, 640, 480);
// Looking along X-axis, 1 meter above ground plane (x-y)
static const Rot3 upright = Rot3::ypr(-M_PI / 2, 0., -M_PI / 2);
static const Pose3 pose1 = Pose3(upright, gtsam::Point3(0, 0, 1));
PinholeCamera<Cal3_S2> camera1(pose1, *sharedCal);
// landmark ~5 meters infront of camera
static const Point3 landmark(5, 0.5, 1.2);
// 1. Project two landmarks into two cameras and triangulate
Point2 z1 = camera1.project(landmark);
//******************************************************************************
TEST( triangulation, TriangulationFactor ) {
// Create the factor with a measurement that is 3 pixels off in x
Key pointKey(1);
SharedNoiseModel model;
typedef TriangulationFactor<> Factor;
Factor factor(camera1, z1, model, pointKey);
// Use the factor to calculate the Jacobians
Matrix HActual;
factor.evaluateError(landmark, HActual);
Matrix HExpected = numericalDerivative11<Vector,Point3>(
boost::bind(&Factor::evaluateError, &factor, _1, boost::none), landmark);
// Verify the Jacobians are correct
CHECK(assert_equal(HExpected, HActual, 1e-3));
}
//******************************************************************************
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
//******************************************************************************

231
gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp
View File

@ -1,231 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 testBasisDecompositions.cpp
* @date November 23, 2014
* @author Frank Dellaert
* @brief unit tests for Basis Decompositions w Expressions
*/
#include <gtsam/base/Matrix.h>
namespace gtsam {
/// Fourier
template<int N>
class Fourier {
public:
typedef Eigen::Matrix<double, N, 1> Coefficients;
typedef Eigen::Matrix<double, 1, N> Jacobian;
private:
double x_;
Jacobian H_;
public:
/// Constructor
Fourier(double x) :
x_(x) {
H_(0, 0) = 1;
for (size_t i = 1; i < N; i += 2) {
H_(0, i) = cos(i * x);
H_(0, i + 1) = sin(i * x);
}
}
/// Given coefficients c, predict value for x
double operator()(const Coefficients& c, boost::optional<Jacobian&> H) {
if (H)
(*H) = H_;
return H_ * c;
}
};
}
#include <gtsam_unstable/nonlinear/ExpressionFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>
namespace gtsam {
/// For now, this is our sequence representation
typedef std::map<double, double> Sequence;
/**
* Class that does Fourier Decomposition via least squares
*/
class FourierDecomposition {
public:
typedef Vector3 Coefficients; ///< Fourier coefficients
private:
Coefficients c_;
public:
/// Create nonlinear FG from Sequence
static NonlinearFactorGraph NonlinearGraph(const Sequence& sequence,
const SharedNoiseModel& model) {
NonlinearFactorGraph graph;
Expression<Coefficients> c(0);
typedef std::pair<double, double> Sample;
BOOST_FOREACH(Sample sample, sequence) {
Expression<double> expression(Fourier<3>(sample.first), c);
ExpressionFactor<double> factor(model, sample.second, expression);
graph.add(factor);
}
return graph;
}
/// Create linear FG from Sequence
static GaussianFactorGraph::shared_ptr LinearGraph(const Sequence& sequence,
const SharedNoiseModel& model) {
NonlinearFactorGraph graph = NonlinearGraph(sequence, model);
Values values;
values.insert<Coefficients>(0, Coefficients::Zero()); // does not matter
GaussianFactorGraph::shared_ptr gfg = graph.linearize(values);
return gfg;
}
/// Constructor
FourierDecomposition(const Sequence& sequence,
const SharedNoiseModel& model) {
GaussianFactorGraph::shared_ptr gfg = LinearGraph(sequence, model);
VectorValues solution = gfg->optimize();
c_ = solution.at(0);
}
/// Return Fourier coefficients
Coefficients coefficients() {
return c_;
}
};
}
#include <gtsam_unstable/nonlinear/expressionTesting.h>
#include <gtsam/base/Testable.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(1);
//******************************************************************************
TEST(BasisDecompositions, Fourier) {
Fourier<3> fx(0);
Eigen::Matrix<double, 1, 3> expectedH, actualH;
Vector3 c(1.5661, 1.2717, 1.2717);
expectedH = numericalDerivative11<double, Vector3>(
boost::bind(&Fourier<3>::operator(), fx, _1, boost::none), c);
EXPECT_DOUBLES_EQUAL(c[0]+c[1], fx(c,actualH), 1e-9);
EXPECT(assert_equal((Matrix)expectedH, actualH));
}
//******************************************************************************
TEST(BasisDecompositions, ManualFourier) {
// Create linear factor graph
GaussianFactorGraph g;
Key key(1);
Expression<Vector3> c(key);
Values values;
values.insert<Vector3>(key, Vector3::Zero()); // does not matter
for (size_t i = 0; i < 16; i++) {
double x = i * M_PI / 8, y = exp(sin(x) + cos(x));
// Manual JacobianFactor
Matrix A(1, 3);
A << 1, cos(x), sin(x);
Vector b(1);
b << y;
JacobianFactor f1(key, A, b);
g.add(f1);
// With ExpressionFactor
Expression<double> expression(Fourier<3>(x), c);
EXPECT_CORRECT_EXPRESSION_JACOBIANS(expression, values, 1e-5, 1e-9);
{
ExpressionFactor<double> f2(model, y, expression);
boost::shared_ptr<GaussianFactor> gf = f2.linearize(values);
boost::shared_ptr<JacobianFactor> jf = //
boost::dynamic_pointer_cast<JacobianFactor>(gf);
CHECK(jf);
EXPECT( assert_equal(f1, *jf, 1e-9));
}
{
ExpressionFactor<double> f2(model, y, expression);
boost::shared_ptr<GaussianFactor> gf = f2.linearize(values);
boost::shared_ptr<JacobianFactor> jf = //
boost::dynamic_pointer_cast<JacobianFactor>(gf);
CHECK(jf);
EXPECT( assert_equal(f1, *jf, 1e-9));
}
{
ExpressionFactor<double> f2(model, y, expression);
boost::shared_ptr<GaussianFactor> gf = f2.linearize(values);
boost::shared_ptr<JacobianFactor> jf = //
boost::dynamic_pointer_cast<JacobianFactor>(gf);
CHECK(jf);
EXPECT( assert_equal(f1, *jf, 1e-9));
}
{
ExpressionFactor<double> f2(model, y, expression);
boost::shared_ptr<GaussianFactor> gf = f2.linearize(values);
boost::shared_ptr<JacobianFactor> jf = //
boost::dynamic_pointer_cast<JacobianFactor>(gf);
CHECK(jf);
EXPECT( assert_equal(f1, *jf, 1e-9));
}
}
// Solve
VectorValues actual = g.optimize();
// Check
Vector3 expected(1.5661, 1.2717, 1.2717);
EXPECT(assert_equal((Vector) expected, actual.at(key),1e-4));
}
//******************************************************************************
TEST(BasisDecompositions, FourierDecomposition) {
// Create example sequence
Sequence sequence;
for (size_t i = 0; i < 16; i++) {
double x = i * M_PI / 8, y = exp(sin(x) + cos(x));
sequence[x] = y;
}
// Do Fourier Decomposition
FourierDecomposition actual(sequence, model);
// Check
Vector3 expected(1.5661, 1.2717, 1.2717);
EXPECT(assert_equal((Vector) expected, actual.coefficients(),1e-4));
}
//******************************************************************************
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
//******************************************************************************

9
gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp
View File

@ -131,14 +131,14 @@ TEST(ExpressionFactor, Unary) {
// Unary(Leaf)) and Unary(Unary(Leaf)))
// wide version (not handled in fixed-size pipeline)
typedef Eigen::Matrix<double,9,3> Matrix93;
Vector9 wide(const Point3& p, boost::optional<Matrix93&> H) {
Vector9 wide(const Point3& p, OptionalJacobian<9,3> H) {
Vector9 v;
v << p.vector(), p.vector(), p.vector();
if (H) *H << eye(3), eye(3), eye(3);
return v;
}
typedef Eigen::Matrix<double,9,9> Matrix9;
Vector9 id9(const Vector9& v, boost::optional<Matrix9&> H) {
Vector9 id9(const Vector9& v, OptionalJacobian<9,9> H) {
if (H) *H = Matrix9::Identity();
return v;
}
@ -161,7 +161,7 @@ TEST(ExpressionFactor, Wide) {
/* ************************************************************************* */
static Point2 myUncal(const Cal3_S2& K, const Point2& p,
boost::optional<Matrix25&> Dcal, boost::optional<Matrix2&> Dp) {
OptionalJacobian<2,5> Dcal, OptionalJacobian<2,2> Dp) {
return K.uncalibrate(p, Dcal, Dp);
}
@ -427,8 +427,7 @@ TEST(ExpressionFactor, compose3) {
/* ************************************************************************* */
// Test compose with three arguments
Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3,
boost::optional<Matrix3&> H1, boost::optional<Matrix3&> H2,
boost::optional<Matrix3&> H3) {
OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2, OptionalJacobian<3, 3> H3) {
// return dummy derivatives (not correct, but that's ok for testing here)
if (H1)
*H1 = eye(3);

42
gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp
View File

@ -179,29 +179,29 @@ TEST(ExpressionFactor, InvokeDerivatives) {
// Let's assign it it to a boost function object
// cast is needed because Pose3::transform_to is overloaded
typedef boost::function<Point3(const Pose3&, const Point3&)> F;
F f = static_cast<Point3 (Pose3::*)(
const Point3&) const >(&Pose3::transform_to);
// Create arguments
Pose3 pose;
Point3 point;
typedef boost::fusion::vector<Pose3, Point3> Arguments;
Arguments args = boost::fusion::make_vector(pose, point);
// Create fused function (takes fusion vector) and call it
boost::fusion::fused<F> g(f);
Point3 actual = g(args);
CHECK(assert_equal(point,actual));
// We can *immediately* do this using invoke
Point3 actual2 = boost::fusion::invoke(f, args);
CHECK(assert_equal(point,actual2));
// typedef boost::function<Point3(const Pose3&, const Point3&)> F;
// F f = static_cast<Point3 (Pose3::*)(
// const Point3&) const >(&Pose3::transform_to);
//
// // Create arguments
// Pose3 pose;
// Point3 point;
// typedef boost::fusion::vector<Pose3, Point3> Arguments;
// Arguments args = boost::fusion::make_vector(pose, point);
//
// // Create fused function (takes fusion vector) and call it
// boost::fusion::fused<F> g(f);
// Point3 actual = g(args);
// CHECK(assert_equal(point,actual));
//
// // We can *immediately* do this using invoke
// Point3 actual2 = boost::fusion::invoke(f, args);
// CHECK(assert_equal(point,actual2));
// Now, let's create the optional Jacobian arguments
typedef Point3 T;
typedef boost::mpl::vector<Pose3, Point3> TYPES;
typedef boost::mpl::transform<TYPES, OptionalJacobian<T, MPL::_1> >::type Optionals;
typedef boost::mpl::transform<TYPES, MakeOptionalJacobian<T, MPL::_1> >::type Optionals;
// Unfortunately this is moot: we need a pointer to a function with the
// optional derivatives; I don't see a way of calling a function that we
@ -215,8 +215,8 @@ struct proxy {
return pose.transform_to(point);
}
Point3 operator()(const Pose3& pose, const Point3& point,
boost::optional<Matrix36&> Dpose,
boost::optional<Matrix3&> Dpoint) const {
OptionalJacobian<3,6> Dpose,
OptionalJacobian<3,3> Dpoint) const {
return pose.transform_to(point, Dpose, Dpoint);
}
};

79
gtsam_unstable/slam/AHRS.cpp
View File

@ -12,17 +12,14 @@ using namespace std;
namespace gtsam {
Matrix cov(const Matrix& m) {
/* ************************************************************************* */
Matrix3 AHRS::Cov(const Vector3s& m) {
const double num_observations = m.cols();
const Vector mean = m.rowwise().sum() / num_observations;
Matrix D = m.colwise() - mean;
Matrix DDt = D * trans(D);
return DDt / (num_observations - 1);
const Vector3 mean = m.rowwise().sum() / num_observations;
Vector3s D = m.colwise() - mean;
return D * trans(D) / (num_observations - 1);
}
Matrix I3 = eye(3);
Matrix Z3 = zeros(3, 3);
/* ************************************************************************* */
AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e,
bool flat) :
@ -34,11 +31,11 @@ AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e,
size_t T = stationaryU.cols();
// estimate standard deviation on gyroscope readings
Pg_ = cov(stationaryU);
Vector sigmas_v_g = esqrt(Pg_.diagonal() * T);
Pg_ = Cov(stationaryU);
Vector3 sigmas_v_g = esqrt(Pg_.diagonal() * T);
// estimate standard deviation on accelerometer readings
Pa_ = cov(stationaryF);
Pa_ = Cov(stationaryF);
// Quantities needed for integration
@ -46,15 +43,13 @@ AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e,
double tau_g = 730; // correlation time for gyroscope
double tau_a = 730; // correlation time for accelerometer
F_g_ = -I3 / tau_g;
F_a_ = -I3 / tau_a;
F_g_ = -I_3x3 / tau_g;
F_a_ = -I_3x3 / tau_a;
Vector3 var_omega_w = 0 * ones(3); // TODO
Vector3 var_omega_g = (0.0034 * 0.0034) * ones(3);
Vector3 var_omega_a = (0.034 * 0.034) * ones(3);
Vector3 sigmas_v_g_sq = emul(sigmas_v_g, sigmas_v_g);
Vector vars(12);
vars << var_omega_w, var_omega_g, sigmas_v_g_sq, var_omega_a;
var_w_ = diag(vars);
Vector3 sigmas_v_g_sq = sigmas_v_g.cwiseProduct(sigmas_v_g);
var_w_ << var_omega_w, var_omega_g, sigmas_v_g_sq, var_omega_a;
// Quantities needed for aiding
sigmas_v_a_ = esqrt(T * Pa_.diagonal());
@ -95,15 +90,15 @@ std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::initialize(double g_e)
Matrix P_plus_k2 = Matrix(9, 9);
P_plus_k2.block<3,3>(0, 0) = P11;
P_plus_k2.block<3,3>(0, 3) = Z3;
P_plus_k2.block<3,3>(0, 3) = Z_3x3;
P_plus_k2.block<3,3>(0, 6) = P12;
P_plus_k2.block<3,3>(3, 0) = Z3;
P_plus_k2.block<3,3>(3, 0) = Z_3x3;
P_plus_k2.block<3,3>(3, 3) = Pg_;
P_plus_k2.block<3,3>(3, 6) = Z3;
P_plus_k2.block<3,3>(3, 6) = Z_3x3;
P_plus_k2.block<3,3>(6, 0) = trans(P12);
P_plus_k2.block<3,3>(6, 3) = Z3;
P_plus_k2.block<3,3>(6, 3) = Z_3x3;
P_plus_k2.block<3,3>(6, 6) = Pa;
Vector dx = zero(9);
@ -123,26 +118,26 @@ std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::integrate(
// FIXME:
//if nargout>1
Matrix bRn = mech.bRn().matrix();
Matrix I3 = eye(3);
Matrix Z3 = zeros(3, 3);
Matrix F_k = zeros(9, 9);
Matrix9 F_k; F_k.setZero();
F_k.block<3,3>(0, 3) = -bRn;
F_k.block<3,3>(3, 3) = F_g_;
F_k.block<3,3>(6, 6) = F_a_;
Matrix G_k = zeros(9, 12);
typedef Eigen::Matrix<double,9,12> Matrix9_12;
Matrix9_12 G_k; G_k.setZero();
G_k.block<3,3>(0, 0) = -bRn;
G_k.block<3,3>(0, 6) = -bRn;
G_k.block<3,3>(3, 3) = I3;
G_k.block<3,3>(6, 9) = I3;
G_k.block<3,3>(3, 3) = I_3x3;
G_k.block<3,3>(6, 9) = I_3x3;
Matrix Q_k = G_k * var_w_ * trans(G_k);
Matrix Psi_k = eye(9) + dt * F_k; // +dt*dt*F_k*F_k/2; // transition matrix
Matrix9 Q_k = G_k * var_w_.asDiagonal() * G_k.transpose();
Matrix9 Psi_k = I_9x9 + dt * F_k; // +dt*dt*F_k*F_k/2; // transition matrix
// This implements update in section 10.6
Matrix B = zeros(9, 9);
Vector u2 = zero(9);
Matrix9 B; B.setZero();
Vector9 u2; u2.setZero();
// TODO predictQ should be templated to also take fixed size matrices.
KalmanFilter::State newState = KF_.predictQ(state, Psi_k,B,u2,dt*Q_k);
return make_pair(newMech, newState);
}
@ -175,7 +170,7 @@ std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::aid(
if (Farrell) {
// calculate residual gravity measurement
z = n_g_ - trans(bRn) * measured_b_g;
H = collect(3, &n_g_cross_, &Z3, &bRn);
H = collect(3, &n_g_cross_, &Z_3x3, &bRn);
R = trans(bRn) * diag(emul(sigmas_v_a_, sigmas_v_a_)) * bRn;
} else {
// my measurement prediction (in body frame):
@ -189,7 +184,7 @@ std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::aid(
z = bRn * n_g_ - measured_b_g;
// Now the Jacobian H
Matrix b_g = bRn * n_g_cross_;
H = collect(3, &b_g, &Z3, &I3);
H = collect(3, &b_g, &Z_3x3, &I_3x3);
// And the measurement noise, TODO: should be created once where sigmas_v_a is given
R = diag(emul(sigmas_v_a_, sigmas_v_a_));
}
@ -219,7 +214,7 @@ std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::aidGeneral(
Vector z = f - increment * f_previous;
//Vector z = increment * f_previous - f;
Matrix b_g = skewSymmetric(increment* f_previous);
Matrix H = collect(3, &b_g, &I3, &Z3);
Matrix H = collect(3, &b_g, &I_3x3, &Z_3x3);
// Matrix R = diag(emul(sigmas_v_a_, sigmas_v_a_));
// Matrix R = diag(Vector3(1.0, 0.2, 1.0)); // good for L_twice
Matrix R = diag(Vector3(0.01, 0.0001, 0.01));
@ -240,16 +235,16 @@ std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::aidGeneral(
/* ************************************************************************* */
void AHRS::print(const std::string& s) const {
mech0_.print(s + ".mech0_");
gtsam::print(F_g_, s + ".F_g");
gtsam::print(F_a_, s + ".F_a");
gtsam::print(var_w_, s + ".var_w");
gtsam::print((Matrix)F_g_, s + ".F_g");
gtsam::print((Matrix)F_a_, s + ".F_a");
gtsam::print((Vector)var_w_, s + ".var_w");
gtsam::print(sigmas_v_a_, s + ".sigmas_v_a");
gtsam::print(n_g_, s + ".n_g");
gtsam::print(n_g_cross_, s + ".n_g_cross");
gtsam::print((Vector)sigmas_v_a_, s + ".sigmas_v_a");
gtsam::print((Vector)n_g_, s + ".n_g");
gtsam::print((Matrix)n_g_cross_, s + ".n_g_cross");
gtsam::print(Pg_, s + ".P_g");
gtsam::print(Pa_, s + ".P_a");
gtsam::print((Matrix)Pg_, s + ".P_g");
gtsam::print((Matrix)Pa_, s + ".P_a");
}

22
gtsam_unstable/slam/AHRS.h
View File

@ -14,8 +14,6 @@
namespace gtsam {
GTSAM_UNSTABLE_EXPORT Matrix cov(const Matrix& m);
class GTSAM_UNSTABLE_EXPORT AHRS {
private:
@ -25,18 +23,24 @@ private:
KalmanFilter KF_; ///< initial Kalman filter
// Quantities needed for integration
Matrix F_g_; ///< gyro bias dynamics
Matrix F_a_; ///< acc bias dynamics
Matrix var_w_; ///< dynamic noise variances
Matrix3 F_g_; ///< gyro bias dynamics
Matrix3 F_a_; ///< acc bias dynamics
typedef Eigen::Matrix<double,12,1> Variances;
Variances var_w_; ///< dynamic noise variances
// Quantities needed for aiding
Vector sigmas_v_a_; ///< measurement sigma
Vector n_g_; ///< gravity in nav frame
Matrix n_g_cross_; ///< and its skew-symmetric matrix
Vector3 sigmas_v_a_; ///< measurement sigma
Vector3 n_g_; ///< gravity in nav frame
Matrix3 n_g_cross_; ///< and its skew-symmetric matrix
Matrix Pg_, Pa_;
Matrix3 Pg_, Pa_;
public:
typedef Eigen::Matrix<double,3,Eigen::Dynamic> Vector3s;
static Matrix3 Cov(const Vector3s& m);
/**
* AHRS constructor
* @param stationaryU initial interval of gyro measurements, 3xn matrix

13
gtsam_unstable/slam/expressions.h
View File

@ -20,9 +20,8 @@ typedef Expression<Rot2> Rot2_;
typedef Expression<Pose2> Pose2_;
Point2_ transform_to(const Pose2_& x, const Point2_& p) {
Point2(Pose2::*transform)(const Point2& p,
boost::optional<Matrix23&> H1,
boost::optional<Matrix2&> H2) const = &Pose2::transform_to;
Point2 (Pose2::*transform)(const Point2& p, OptionalJacobian<2, 3> H1,
OptionalJacobian<2, 2> H2) const = &Pose2::transform_to;
return Point2_(x, transform, p);
}
@ -35,9 +34,8 @@ typedef Expression<Pose3> Pose3_;
Point3_ transform_to(const Pose3_& x, const Point3_& p) {
Point3(Pose3::*transform)(const Point3& p,
boost::optional<Matrix36&> Dpose,
boost::optional<Matrix3&> Dpoint) const = &Pose3::transform_to;
Point3 (Pose3::*transform)(const Point3& p, OptionalJacobian<3, 6> Dpose,
OptionalJacobian<3, 3> Dpoint) const = &Pose3::transform_to;
return Point3_(x, transform, p);
}
@ -51,8 +49,7 @@ Point2_ project(const Point3_& p_cam) {
}
Point2 project6(const Pose3& x, const Point3& p, const Cal3_S2& K,
boost::optional<Matrix26&> Dpose, boost::optional<Matrix23&> Dpoint,
boost::optional<Matrix25&> Dcal) {
OptionalJacobian<2, 6> Dpose, OptionalJacobian<2, 3> Dpoint, OptionalJacobian<2, 5> Dcal) {
return PinholeCamera<Cal3_S2>(x, K).project(p, Dpose, Dpoint, Dcal);
}

6
gtsam_unstable/slam/tests/testAHRS.cpp
View File

@ -30,7 +30,7 @@ TEST (AHRS, cov) {
9.0, 4.0, 7.0,
6.0, 3.0, 2.0).finished();
Matrix actual = cov(trans(A));
Matrix actual = AHRS::Cov(trans(A));
Matrix expected = (Matrix(3, 3) <<
10.9167, 2.3333, 5.0000,
2.3333, 4.6667, -2.6667,
@ -42,7 +42,7 @@ TEST (AHRS, cov) {
/* ************************************************************************* */
TEST (AHRS, covU) {
Matrix actual = cov(10000*stationaryU);
Matrix actual = AHRS::Cov(10000*stationaryU);
Matrix expected = (Matrix(3, 3) <<
33.3333333, -1.66666667, 53.3333333,
-1.66666667, 0.333333333, -5.16666667,
@ -54,7 +54,7 @@ TEST (AHRS, covU) {
/* ************************************************************************* */
TEST (AHRS, covF) {
Matrix actual = cov(100*stationaryF);
Matrix actual = AHRS::Cov(100*stationaryF);
Matrix expected = (Matrix(3, 3) <<
63.3808333, -0.432166667, -48.1706667,
-0.432166667, 8.31053333, -16.6792667,