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

More slow progress. Added a variable-size specialization for OptionalJacobian

release/4.3a0
Paul Furgale 2014-12-16 12:30:11 +01:00
parent e9ae4f3c4d
commit 37fc86b595
7 changed files with 269 additions and 191 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
gtsam/base/LieVector.cpp
View File

@ -35,5 +35,6 @@ void LieVector::print(const std::string& name) const {
gtsam::print(vector(), name); gtsam::print(vector(), name);
} }
GTSAM_CONCEPT_LIE_INST(LieVector) // Does not compile because LieVector is not fixed size.
// GTSAM_CONCEPT_LIE_INST(LieVector)
} // \namespace gtsam } // \namespace gtsam

96
gtsam/base/OptionalJacobian.h
View File

@ -48,8 +48,8 @@ private:
// Trick from http://eigen.tuxfamily.org/dox/group__TutorialMapClass.html // Trick from http://eigen.tuxfamily.org/dox/group__TutorialMapClass.html
// uses "placement new" to make map_ usurp the memory of the fixed size matrix // uses "placement new" to make map_ usurp the memory of the fixed size matrix
void usurp(double* data) { void usurp(double* data, int rows, int cols) {
new (&map_) Eigen::Map<Fixed>(data); new (&map_) Eigen::Map<Fixed>(data, rows, cols);
} }
public: public:
@ -62,22 +62,22 @@ public:
/// Constructor that will usurp data of a fixed-size matrix /// Constructor that will usurp data of a fixed-size matrix
OptionalJacobian(Fixed& fixed) : OptionalJacobian(Fixed& fixed) :
map_(NULL) { map_(NULL) {
usurp(fixed.data()); usurp(fixed.data(), fixed.rows(), fixed.cols());
} }
/// Constructor that will usurp data of a fixed-size matrix, pointer version /// Constructor that will usurp data of a fixed-size matrix, pointer version
OptionalJacobian(Fixed* fixedPtr) : OptionalJacobian(Fixed* fixedPtr) :
map_(NULL) { map_(NULL) {
if (fixedPtr) if (fixedPtr)
usurp(fixedPtr->data()); usurp(fixedPtr->data(), fixedPtr->rows(), fixedPtr->cols());
} }
// Disabled to deal with dynamic types.
/// Constructor that will resize a dynamic matrix (unless already correct) /// Constructor that will resize a dynamic matrix (unless already correct)
OptionalJacobian(Eigen::MatrixXd& dynamic) : OptionalJacobian(Eigen::MatrixXd& dynamic) :
map_(NULL) { map_(NULL) {
dynamic.resize(Rows, Cols); // no malloc if correct size dynamic.resize(Rows, Cols); // no malloc if correct size
usurp(dynamic.data()); usurp(dynamic.data(), dynamic.rows(), dynamic.cols());
} }
#ifndef OPTIONALJACOBIAN_NOBOOST #ifndef OPTIONALJACOBIAN_NOBOOST
@ -92,7 +92,7 @@ public:
map_(NULL) { map_(NULL) {
if (optional) { if (optional) {
optional->resize(Rows, Cols); optional->resize(Rows, Cols);
usurp(optional->data()); usurp(optional->data(), optional->rows(), optional->cols());
} }
} }
@ -112,11 +112,83 @@ public:
Eigen::Map<Fixed>* operator->(){ return &map_; } Eigen::Map<Fixed>* operator->(){ return &map_; }
}; };
// these make sure that no dynamic sized matrices are compiled. // The pure dynamic specialization of this is needed to support
template<int Cols> // variable-sized types.
class OptionalJacobian<-1, Cols> {}; template<>
template<int Rows> class OptionalJacobian<-1, -1> {
class OptionalJacobian<Rows, -1> {};
public:
/// Fixed size type
typedef Eigen::Matrix<double, -1, -1> 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, int rows, int cols) {
new (&map_) Eigen::Map<Fixed>(data, rows, cols);
}
public:
/// Default constructor acts like boost::none
OptionalJacobian() :
map_(NULL, 0, 0) {
}
/// Constructor that will usurp data of a fixed-size matrix
OptionalJacobian(Fixed& fixed) :
map_(NULL, 0, 0) {
usurp(fixed.data(), fixed.rows(), fixed.cols());
}
/// Constructor that will usurp data of a fixed-size matrix, pointer version
OptionalJacobian(Fixed* fixedPtr) :
map_(NULL, 0, 0) {
if (fixedPtr)
usurp(fixedPtr->data(), fixedPtr->rows(), fixedPtr->cols());
}
#ifndef OPTIONALJACOBIAN_NOBOOST
/// Constructor with boost::none just makes empty
OptionalJacobian(boost::none_t none) :
map_(NULL, 0, 0) {
}
/// Constructor compatible with old-style derivatives
OptionalJacobian(const boost::optional<Eigen::MatrixXd&> optional) :
map_(NULL, 0, 0) {
if (optional) {
usurp(optional->data(), optional->rows(), optional->cols());
}
}
#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_; }
};
//// these make sure that no dynamic sized matrices are compiled.
//template<int Cols>
//class OptionalJacobian<-1, Cols> {};
//template<int Rows>
//class OptionalJacobian<Rows, -1> {};
} // namespace gtsam } // namespace gtsam

4
gtsam/geometry/Point3.h
View File

@ -132,6 +132,10 @@ namespace gtsam {
/// Returns inverse retraction /// Returns inverse retraction
inline Vector3 localCoordinates(const Point3& q) const { return (q -*this).vector(); } inline Vector3 localCoordinates(const Point3& q) const { return (q -*this).vector(); }
/// Returns inverse retraction
inline Vector3 localCoordinates(const Point3& q, OptionalJacobian<3,3> Horigin, OptionalJacobian<3,3> Ha) const {
CONCEPT_NOT_IMPLEMENTED; return (q -*this).vector();
}
/// @} /// @}
/// @name Lie Group /// @name Lie Group
/// @{ /// @{

9
gtsam/navigation/AttitudeFactor.cpp
View File

@ -24,13 +24,14 @@ namespace gtsam {
//*************************************************************************** //***************************************************************************
Vector AttitudeFactor::attitudeError(const Rot3& nRb, Vector AttitudeFactor::attitudeError(const Rot3& nRb,
boost::optional<Matrix&> H) const { OptionalJacobian<2, 3> H) const {
if (H) { if (H) {
Matrix D_nRef_R, D_e_nRef; Matrix23 D_nRef_R;
Matrix22 D_e_nRef;
Unit3 nRef = nRb.rotate(bRef_, D_nRef_R); Unit3 nRef = nRb.rotate(bRef_, D_nRef_R);
Vector e = nZ_.error(nRef, D_e_nRef); Vector e = nZ_.error(nRef, D_e_nRef);
H->resize(2, 3);
H->block < 2, 3 > (0, 0) = D_e_nRef * D_nRef_R; (*H) = D_e_nRef * D_nRef_R;
return e; return e;
} else { } else {
Unit3 nRef = nRb * bRef_; Unit3 nRef = nRb * bRef_;

2
gtsam/navigation/AttitudeFactor.h
View File

@ -54,7 +54,7 @@ public:
/** vector of errors */ /** vector of errors */
Vector attitudeError(const Rot3& p, Vector attitudeError(const Rot3& p,
boost::optional<Matrix&> H = boost::none) const; OptionalJacobian<2,3> H = boost::none) const;
}; };
/** /**

10
gtsam/navigation/tests/testAHRSFactor.cpp
View File

@ -134,19 +134,19 @@ TEST(AHRSFactor, Error) {
// Create factor // Create factor
AHRSFactor factor(X(1), X(2), B(1), pre_int_data, omegaCoriolis, false); AHRSFactor factor(X(1), X(2), B(1), pre_int_data, omegaCoriolis, false);
Vector errorActual = factor.evaluateError(x1, x2, bias); Vector3 errorActual = factor.evaluateError(x1, x2, bias);
// Expected error // Expected error
Vector errorExpected(3); Vector3 errorExpected(3);
errorExpected << 0, 0, 0; errorExpected << 0, 0, 0;
EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); EXPECT(assert_equal(errorExpected, errorActual, 1e-6));
// Expected Jacobians // Expected Jacobians
Matrix H1e = numericalDerivative11<Vector, Rot3>( Matrix H1e = numericalDerivative11<Vector3, Rot3>(
boost::bind(&callEvaluateError, factor, _1, x2, bias), x1); boost::bind(&callEvaluateError, factor, _1, x2, bias), x1);
Matrix H2e = numericalDerivative11<Vector, Rot3>( Matrix H2e = numericalDerivative11<Vector3, Rot3>(
boost::bind(&callEvaluateError, factor, x1, _1, bias), x2); boost::bind(&callEvaluateError, factor, x1, _1, bias), x2);
Matrix H3e = numericalDerivative11<Vector, Vector3>( Matrix H3e = numericalDerivative11<Vector3, Vector3>(
boost::bind(&callEvaluateError, factor, x1, x2, _1), bias); boost::bind(&callEvaluateError, factor, x1, x2, _1), bias);
// Check rotation Jacobians // Check rotation Jacobians

336
gtsam/nonlinear/tests/testAdaptAutoDiff.cpp
View File

@ -37,175 +37,175 @@ using boost::assign::map_list_of;
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
// The DefaultChart of Camera below is laid out like Snavely's 9-dim vector //// The DefaultChart of Camera below is laid out like Snavely's 9-dim vector
typedef PinholeCamera<Cal3Bundler> Camera; //typedef PinholeCamera<Cal3Bundler> Camera;
//
/* ************************************************************************* */ ///* ************************************************************************* */
// Some Ceres Snippets copied for testing //// Some Ceres Snippets copied for testing
// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. //// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
template<typename T> inline T &RowMajorAccess(T *base, int rows, int cols, //template<typename T> inline T &RowMajorAccess(T *base, int rows, int cols,
int i, int j) { // int i, int j) {
return base[cols * i + j]; // return base[cols * i + j];
} //}
//
inline double RandDouble() { //inline double RandDouble() {
double r = static_cast<double>(rand()); // double r = static_cast<double>(rand());
return r / RAND_MAX; // return r / RAND_MAX;
} //}
//
// A structure for projecting a 3x4 camera matrix and a //// A structure for projecting a 3x4 camera matrix and a
// homogeneous 3D point, to a 2D inhomogeneous point. //// homogeneous 3D point, to a 2D inhomogeneous point.
struct Projective { //struct Projective {
// Function that takes P and X as separate vectors: // // Function that takes P and X as separate vectors:
// P, X -> x // // P, X -> x
template<typename A> // template<typename A>
bool operator()(A const P[12], A const X[4], A x[2]) const { // bool operator()(A const P[12], A const X[4], A x[2]) const {
A PX[3]; // A PX[3];
for (int i = 0; i < 3; ++i) { // for (int i = 0; i < 3; ++i) {
PX[i] = RowMajorAccess(P, 3, 4, i, 0) * X[0] // PX[i] = RowMajorAccess(P, 3, 4, i, 0) * X[0]
+ RowMajorAccess(P, 3, 4, i, 1) * X[1] // + RowMajorAccess(P, 3, 4, i, 1) * X[1]
+ RowMajorAccess(P, 3, 4, i, 2) * X[2] // + RowMajorAccess(P, 3, 4, i, 2) * X[2]
+ RowMajorAccess(P, 3, 4, i, 3) * X[3]; // + RowMajorAccess(P, 3, 4, i, 3) * X[3];
} // }
if (PX[2] != 0.0) { // if (PX[2] != 0.0) {
x[0] = PX[0] / PX[2]; // x[0] = PX[0] / PX[2];
x[1] = PX[1] / PX[2]; // x[1] = PX[1] / PX[2];
return true; // return true;
} // }
return false; // return false;
} // }
//
// Adapt to eigen types // // Adapt to eigen types
Vector2 operator()(const MatrixRowMajor& P, const Vector4& X) const { // Vector2 operator()(const MatrixRowMajor& P, const Vector4& X) const {
Vector2 x; // Vector2 x;
if (operator()(P.data(), X.data(), x.data())) // if (operator()(P.data(), X.data(), x.data()))
return x; // return x;
else // else
throw std::runtime_error("Projective fail"); // throw std::runtime_error("Projective fail");
} // }
}; //};
//
/* ************************************************************************* */ ///* ************************************************************************* */
// Test Ceres AutoDiff //// Test Ceres AutoDiff
TEST(Expression, AutoDiff) { //TEST(Expression, AutoDiff) {
using ceres::internal::AutoDiff; // using ceres::internal::AutoDiff;
//
// Instantiate function // // Instantiate function
Projective projective; // Projective projective;
//
// Make arguments // // Make arguments
typedef Eigen::Matrix<double, 3, 4, Eigen::RowMajor> M; // typedef Eigen::Matrix<double, 3, 4, Eigen::RowMajor> M;
M P; // M P;
P << 1, 0, 0, 0, 0, 1, 0, 5, 0, 0, 1, 0; // P << 1, 0, 0, 0, 0, 1, 0, 5, 0, 0, 1, 0;
Vector4 X(10, 0, 5, 1); // Vector4 X(10, 0, 5, 1);
//
// Apply the mapping, to get image point b_x. // // Apply the mapping, to get image point b_x.
Vector expected = Vector2(2, 1); // Vector expected = Vector2(2, 1);
Vector2 actual = projective(P, X); // Vector2 actual = projective(P, X);
EXPECT(assert_equal(expected,actual,1e-9)); // EXPECT(assert_equal(expected,actual,1e-9));
//
// Get expected derivatives
Matrix E1 = numericalDerivative21<Vector2, M, Vector4>(Projective(), P, X);
Matrix E2 = numericalDerivative22<Vector2, M, Vector4>(Projective(), P, X);
// Get derivatives with AutoDiff
Vector2 actual2;
MatrixRowMajor H1(2, 12), H2(2, 4);
double *parameters[] = { P.data(), X.data() };
double *jacobians[] = { H1.data(), H2.data() };
CHECK(
(AutoDiff<Projective, double, 12, 4>::Differentiate( projective, parameters, 2, actual2.data(), jacobians)));
EXPECT(assert_equal(E1,H1,1e-8));
EXPECT(assert_equal(E2,H2,1e-8));
}
/* ************************************************************************* */
// Test Ceres AutoDiff on Snavely, defined in ceres_example.h
// Adapt to GTSAM types
Vector2 adapted(const Vector9& P, const Vector3& X) {
SnavelyProjection snavely;
Vector2 x;
if (snavely(P.data(), X.data(), x.data()))
return x;
else
throw std::runtime_error("Snavely fail");
}
TEST(Expression, AutoDiff2) {
using ceres::internal::AutoDiff;
// Instantiate function
SnavelyProjection snavely;
// Make arguments
Vector9 P; // zero rotation, (0,5,0) translation, focal length 1
P << 0, 0, 0, 0, 5, 0, 1, 0, 0;
Vector3 X(10, 0, -5); // negative Z-axis convention of Snavely!
// Apply the mapping, to get image point b_x.
Vector expected = Vector2(2, 1);
Vector2 actual = adapted(P, X);
EXPECT(assert_equal(expected,actual,1e-9));
// Get expected derivatives
Matrix E1 = numericalDerivative21<Vector2, Vector9, Vector3>(adapted, P, X);
Matrix E2 = numericalDerivative22<Vector2, Vector9, Vector3>(adapted, P, X);
// Get derivatives with AutoDiff
Vector2 actual2;
MatrixRowMajor H1(2, 9), H2(2, 3);
double *parameters[] = { P.data(), X.data() };
double *jacobians[] = { H1.data(), H2.data() };
CHECK(
(AutoDiff<SnavelyProjection, double, 9, 3>::Differentiate( snavely, parameters, 2, actual2.data(), jacobians)));
EXPECT(assert_equal(E1,H1,1e-8));
EXPECT(assert_equal(E2,H2,1e-8));
}
/* ************************************************************************* */
// Test AutoDiff wrapper Snavely
TEST(Expression, AutoDiff3) {
// Make arguments
Camera P(Pose3(Rot3(), Point3(0, 5, 0)), Cal3Bundler(1, 0, 0));
Point3 X(10, 0, -5); // negative Z-axis convention of Snavely!
typedef AdaptAutoDiff<SnavelyProjection, Point2, Camera, Point3> Adaptor;
Adaptor snavely;
// Apply the mapping, to get image point b_x.
Point2 expected(2, 1);
Point2 actual = snavely(P, X);
EXPECT(assert_equal(expected,actual,1e-9));
// // Get expected derivatives // // Get expected derivatives
Matrix E1 = numericalDerivative21<Point2, Camera, Point3>(Adaptor(), P, X); // Matrix E1 = numericalDerivative21<Vector2, M, Vector4>(Projective(), P, X);
Matrix E2 = numericalDerivative22<Point2, Camera, Point3>(Adaptor(), P, X); // Matrix E2 = numericalDerivative22<Vector2, M, Vector4>(Projective(), P, X);
//
// Get derivatives with AutoDiff, not gives RowMajor results! // // Get derivatives with AutoDiff
OptionalJacobian<2,9> H1; // Vector2 actual2;
OptionalJacobian<2,3> H2; // MatrixRowMajor H1(2, 12), H2(2, 4);
Point2 actual2 = snavely(P, X, H1, H2); // double *parameters[] = { P.data(), X.data() };
EXPECT(assert_equal(expected,actual2,1e-9)); // double *jacobians[] = { H1.data(), H2.data() };
EXPECT(assert_equal(E1,*H1,1e-8)); // CHECK(
EXPECT(assert_equal(E2,*H2,1e-8)); // (AutoDiff<Projective, double, 12, 4>::Differentiate( projective, parameters, 2, actual2.data(), jacobians)));
} // EXPECT(assert_equal(E1,H1,1e-8));
// EXPECT(assert_equal(E2,H2,1e-8));
/* ************************************************************************* */ //}
// Test AutoDiff wrapper in an expression //
TEST(Expression, Snavely) { ///* ************************************************************************* */
Expression<Camera> P(1); //// Test Ceres AutoDiff on Snavely, defined in ceres_example.h
Expression<Point3> X(2); //// Adapt to GTSAM types
typedef AdaptAutoDiff<SnavelyProjection, Point2, Camera, Point3> Adaptor; //Vector2 adapted(const Vector9& P, const Vector3& X) {
Expression<Point2> expression(Adaptor(), P, X); // SnavelyProjection snavely;
#ifdef GTSAM_USE_QUATERNIONS // Vector2 x;
EXPECT_LONGS_EQUAL(400,expression.traceSize()); // Todo, should be zero // if (snavely(P.data(), X.data(), x.data()))
#else // return x;
EXPECT_LONGS_EQUAL(432,expression.traceSize()); // Todo, should be zero // else
#endif // throw std::runtime_error("Snavely fail");
set<Key> expected = list_of(1)(2); //}
EXPECT(expected == expression.keys()); //
} //TEST(Expression, AutoDiff2) {
// using ceres::internal::AutoDiff;
//
// // Instantiate function
// SnavelyProjection snavely;
//
// // Make arguments
// Vector9 P; // zero rotation, (0,5,0) translation, focal length 1
// P << 0, 0, 0, 0, 5, 0, 1, 0, 0;
// Vector3 X(10, 0, -5); // negative Z-axis convention of Snavely!
//
// // Apply the mapping, to get image point b_x.
// Vector expected = Vector2(2, 1);
// Vector2 actual = adapted(P, X);
// EXPECT(assert_equal(expected,actual,1e-9));
//
// // Get expected derivatives
// Matrix E1 = numericalDerivative21<Vector2, Vector9, Vector3>(adapted, P, X);
// Matrix E2 = numericalDerivative22<Vector2, Vector9, Vector3>(adapted, P, X);
//
// // Get derivatives with AutoDiff
// Vector2 actual2;
// MatrixRowMajor H1(2, 9), H2(2, 3);
// double *parameters[] = { P.data(), X.data() };
// double *jacobians[] = { H1.data(), H2.data() };
// CHECK(
// (AutoDiff<SnavelyProjection, double, 9, 3>::Differentiate( snavely, parameters, 2, actual2.data(), jacobians)));
// EXPECT(assert_equal(E1,H1,1e-8));
// EXPECT(assert_equal(E2,H2,1e-8));
//}
//
///* ************************************************************************* */
//// Test AutoDiff wrapper Snavely
//TEST(Expression, AutoDiff3) {
//
// // Make arguments
// Camera P(Pose3(Rot3(), Point3(0, 5, 0)), Cal3Bundler(1, 0, 0));
// Point3 X(10, 0, -5); // negative Z-axis convention of Snavely!
//
// typedef AdaptAutoDiff<SnavelyProjection, Point2, Camera, Point3> Adaptor;
// Adaptor snavely;
//
// // Apply the mapping, to get image point b_x.
// Point2 expected(2, 1);
// Point2 actual = snavely(P, X);
// EXPECT(assert_equal(expected,actual,1e-9));
//
//// // Get expected derivatives
// Matrix E1 = numericalDerivative21<Point2, Camera, Point3>(Adaptor(), P, X);
// Matrix E2 = numericalDerivative22<Point2, Camera, Point3>(Adaptor(), P, X);
//
// // Get derivatives with AutoDiff, not gives RowMajor results!
// OptionalJacobian<2,9> H1;
// OptionalJacobian<2,3> H2;
// Point2 actual2 = snavely(P, X, H1, H2);
// EXPECT(assert_equal(expected,actual2,1e-9));
// EXPECT(assert_equal(E1,*H1,1e-8));
// EXPECT(assert_equal(E2,*H2,1e-8));
//}
//
///* ************************************************************************* */
//// Test AutoDiff wrapper in an expression
//TEST(Expression, Snavely) {
// Expression<Camera> P(1);
// Expression<Point3> X(2);
// typedef AdaptAutoDiff<SnavelyProjection, Point2, Camera, Point3> Adaptor;
// Expression<Point2> expression(Adaptor(), P, X);
//#ifdef GTSAM_USE_QUATERNIONS
// EXPECT_LONGS_EQUAL(400,expression.traceSize()); // Todo, should be zero
//#else
// EXPECT_LONGS_EQUAL(432,expression.traceSize()); // Todo, should be zero
//#endif
// set<Key> expected = list_of(1)(2);
// EXPECT(expected == expression.keys());
//}
/* ************************************************************************* */ /* ************************************************************************* */
int main() { int main() {