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

Fixed and simplified (quite broken) AdaptAutoDiff, now works with fixed Vectors

release/4.3a0
Frank Dellaert 2015-07-04 18:06:01 -07:00
parent bded06f97f
commit b752f8446c
2 changed files with 68 additions and 172 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

94
gtsam/nonlinear/AdaptAutoDiff.h
View File

@ -27,95 +27,44 @@
namespace gtsam { namespace gtsam {
namespace detail {
// By default, we assume an Identity element
template<typename T, typename structure_category>
struct Origin { T operator()() { return traits<T>::Identity();} };
// but simple manifolds don't have one, so we just use the default constructor
template<typename T>
struct Origin<T, manifold_tag> { T operator()() { return T();} };
} // \ detail
/**
* Canonical is a template that creates canonical coordinates for a given type.
* A simple manifold type (i.e., not a Lie Group) has no concept of identity,
* hence in that case we use the value given by the default constructor T() as
* the origin of a "canonical coordinate" parameterization.
*/
template<typename T>
struct Canonical {
GTSAM_CONCEPT_MANIFOLD_TYPE(T)
typedef traits<T> Traits;
enum { dimension = Traits::dimension };
typedef typename Traits::TangentVector TangentVector;
typedef typename Traits::structure_category Category;
typedef detail::Origin<T, Category> Origin;
static TangentVector Local(const T& other) {
return Traits::Local(Origin()(), other);
}
static T Retract(const TangentVector& v) {
return Traits::Retract(Origin()(), v);
}
};
/** /**
* The AdaptAutoDiff class uses ceres-style autodiff to adapt a ceres-style * The AdaptAutoDiff class uses ceres-style autodiff to adapt a ceres-style
* Function evaluation, i.e., a function F that defines an operator * Function evaluation, i.e., a function FUNCTOR that defines an operator
* template<typename T> bool operator()(const T* const, const T* const, T* predicted) const; * template<typename T> bool operator()(const T* const, const T* const, T*
* predicted) const;
* For now only binary operators are supported. * For now only binary operators are supported.
*/ */
template<typename F, typename T, typename A1, typename A2> template <typename FUNCTOR, int M, int N1, int N2>
class AdaptAutoDiff { class AdaptAutoDiff {
typedef Eigen::Matrix<double, M, N1, Eigen::RowMajor> RowMajor1;
typedef Eigen::Matrix<double, M, N2, Eigen::RowMajor> RowMajor2;
static const int N = traits<T>::dimension; typedef Eigen::Matrix<double, M, 1> VectorT;
static const int M1 = traits<A1>::dimension; typedef Eigen::Matrix<double, N1, 1> Vector1;
static const int M2 = traits<A2>::dimension; typedef Eigen::Matrix<double, N2, 1> Vector2;
typedef Eigen::Matrix<double, N, M1, Eigen::RowMajor> RowMajor1; FUNCTOR f;
typedef Eigen::Matrix<double, N, M2, Eigen::RowMajor> RowMajor2;
typedef Canonical<T> CanonicalT;
typedef Canonical<A1> Canonical1;
typedef Canonical<A2> Canonical2;
typedef typename CanonicalT::TangentVector VectorT;
typedef typename Canonical1::TangentVector Vector1;
typedef typename Canonical2::TangentVector Vector2;
F f;
public: public:
VectorT operator()(const Vector1& v1, const Vector2& v2,
T operator()(const A1& a1, const A2& a2, OptionalJacobian<N, M1> H1 = boost::none, OptionalJacobian<M, N1> H1 = boost::none,
OptionalJacobian<N, M2> H2 = boost::none) { OptionalJacobian<M, N2> H2 = boost::none) {
using ceres::internal::AutoDiff; using ceres::internal::AutoDiff;
// Make arguments
Vector1 v1 = Canonical1::Local(a1);
Vector2 v2 = Canonical2::Local(a2);
bool success; bool success;
VectorT result; VectorT result;
if (H1 || H2) { if (H1 || H2) {
// Get derivatives with AutoDiff // Get derivatives with AutoDiff
double *parameters[] = { v1.data(), v2.data() }; const double* parameters[] = {v1.data(), v2.data()};
double rowMajor1[N * M1], rowMajor2[N * M2]; // on the stack double rowMajor1[M * N1], rowMajor2[M * N2]; // on the stack
double* jacobians[] = {rowMajor1, rowMajor2}; double* jacobians[] = {rowMajor1, rowMajor2};
success = AutoDiff<F, double, 9, 3>::Differentiate(f, parameters, 2, success = AutoDiff<FUNCTOR, double, N1, N2>::Differentiate(
result.data(), jacobians); f, parameters, M, result.data(), jacobians);
// Convert from row-major to columnn-major // Convert from row-major to columnn-major
// TODO: if this is a bottleneck (probably not!) fix Autodiff to be Column-Major // TODO: if this is a bottleneck (probably not!) fix Autodiff to be
// Column-Major
*H1 = Eigen::Map<RowMajor1>(rowMajor1); *H1 = Eigen::Map<RowMajor1>(rowMajor1);
*H2 = Eigen::Map<RowMajor2>(rowMajor2); *H2 = Eigen::Map<RowMajor2>(rowMajor2);
@ -126,9 +75,8 @@ public:
if (!success) if (!success)
throw std::runtime_error( throw std::runtime_error(
"AdaptAutoDiff: function call resulted in failure"); "AdaptAutoDiff: function call resulted in failure");
return CanonicalT::Retract(result); return result;
} }
}; };
} } // namespace gtsam

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

@ -37,9 +37,9 @@ namespace gtsam {
// Special version of Cal3Bundler so that default constructor = 0,0,0 // Special version of Cal3Bundler so that default constructor = 0,0,0
struct Cal3Bundler0 : public Cal3Bundler { struct Cal3Bundler0 : public Cal3Bundler {
Cal3Bundler0(double f = 0, double k1 = 0, double k2 = 0, double u0 = 0, double v0 = 0) : Cal3Bundler0(double f = 0, double k1 = 0, double k2 = 0, double u0 = 0,
Cal3Bundler(f, k1, k2, u0, v0) { double v0 = 0)
} : Cal3Bundler(f, k1, k2, u0, v0) {}
Cal3Bundler0 retract(const Vector& d) const { Cal3Bundler0 retract(const Vector& d) const {
return Cal3Bundler0(fx() + d(0), k1() + d(1), k2() + d(2), u0(), v0()); return Cal3Bundler0(fx() + d(0), k1() + d(1), k2() + d(2), u0(), v0());
} }
@ -53,7 +53,6 @@ struct traits<Cal3Bundler0> : public internal::Manifold<Cal3Bundler0> {};
// With that, camera below behaves like Snavely's 9-dim vector // With that, camera below behaves like Snavely's 9-dim vector
typedef PinholeCamera<Cal3Bundler0> Camera; typedef PinholeCamera<Cal3Bundler0> Camera;
} }
using namespace std; using namespace std;
@ -69,57 +68,11 @@ TEST(AdaptAutoDiff, Rotation) {
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
} }
/* ************************************************************************* */
// Canonical<T> sets up Local/Retract around the default-constructed value
// The tests below check this for all types that play a role in SFM
TEST(AdaptAutoDiff, Canonical) {
typedef Canonical<Point2> Chart1;
EXPECT(Chart1::Local(Point2(1, 0))==Vector2(1, 0));
EXPECT(Chart1::Retract(Vector2(1, 0))==Point2(1, 0));
Vector2 v2(1, 0);
typedef Canonical<Vector2> Chart2;
EXPECT(assert_equal(v2, Chart2::Local(Vector2(1, 0))));
EXPECT(Chart2::Retract(v2)==Vector2(1, 0));
typedef Canonical<double> Chart3;
Eigen::Matrix<double, 1, 1> v1;
v1 << 1;
EXPECT(Chart3::Local(1)==v1);
EXPECT_DOUBLES_EQUAL(Chart3::Retract(v1), 1, 1e-9);
typedef Canonical<Point3> Chart4;
Point3 point(1, 2, 3);
Vector3 v3(1, 2, 3);
EXPECT(assert_equal(v3, Chart4::Local(point)));
EXPECT(assert_equal(Chart4::Retract(v3), point));
typedef Canonical<Pose3> Chart5;
Pose3 pose(Rot3::identity(), point);
Vector v6(6);
v6 << 0, 0, 0, 1, 2, 3;
EXPECT(assert_equal(v6, Chart5::Local(pose)));
EXPECT(assert_equal(Chart5::Retract(v6), pose));
typedef Canonical<Cal3Bundler0> Chart6;
Cal3Bundler0 cal0;
Vector z3 = Vector3::Zero();
EXPECT(assert_equal(z3, Chart6::Local(cal0)));
EXPECT(assert_equal(Chart6::Retract(z3), cal0));
typedef Canonical<Camera> Chart7;
Camera camera(Pose3(), cal0);
Vector z9 = Vector9::Zero();
EXPECT(assert_equal(z9, Chart7::Local(camera)));
EXPECT(assert_equal(Chart7::Retract(z9), 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>
int i, int j) { inline T& RowMajorAccess(T* base, int rows, int cols, int i, int j) {
return base[cols * i + j]; return base[cols * i + j];
} }
@ -137,10 +90,10 @@ struct Projective {
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];
@ -169,8 +122,8 @@ TEST(AdaptAutoDiff, AutoDiff) {
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> RowMajorMatrix34;
M P; RowMajorMatrix34 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);
@ -180,16 +133,18 @@ TEST(AdaptAutoDiff, AutoDiff) {
EXPECT(assert_equal(expected, actual, 1e-9)); EXPECT(assert_equal(expected, actual, 1e-9));
// Get expected derivatives // Get expected derivatives
Matrix E1 = numericalDerivative21<Vector2, M, Vector4>(Projective(), P, X); Matrix E1 = numericalDerivative21<Vector2, RowMajorMatrix34, Vector4>(
Matrix E2 = numericalDerivative22<Vector2, M, Vector4>(Projective(), P, X); Projective(), P, X);
Matrix E2 = numericalDerivative22<Vector2, RowMajorMatrix34, Vector4>(
Projective(), P, X);
// Get derivatives with AutoDiff // Get derivatives with AutoDiff
Vector2 actual2; Vector2 actual2;
MatrixRowMajor H1(2, 12), H2(2, 4); MatrixRowMajor H1(2, 12), H2(2, 4);
double* parameters[] = {P.data(), X.data()}; double* parameters[] = {P.data(), X.data()};
double* jacobians[] = {H1.data(), H2.data()}; double* jacobians[] = {H1.data(), H2.data()};
CHECK( CHECK((AutoDiff<Projective, double, 12, 4>::Differentiate(
(AutoDiff<Projective, double, 12, 4>::Differentiate( projective, parameters, 2, actual2.data(), jacobians))); projective, parameters, 2, actual2.data(), jacobians)));
EXPECT(assert_equal(E1, H1, 1e-8)); EXPECT(assert_equal(E1, H1, 1e-8));
EXPECT(assert_equal(E2, H2, 1e-8)); EXPECT(assert_equal(E2, H2, 1e-8));
} }
@ -211,9 +166,11 @@ namespace example {
Camera camera(Pose3(Rot3::rodriguez(0.1, 0.2, 0.3), Point3(0, 5, 0)), Camera camera(Pose3(Rot3::rodriguez(0.1, 0.2, 0.3), Point3(0, 5, 0)),
Cal3Bundler0(1, 0, 0)); Cal3Bundler0(1, 0, 0));
Point3 point(10, 0, -5); // negative Z-axis convention of Snavely! Point3 point(10, 0, -5); // negative Z-axis convention of Snavely!
Vector9 P = Canonical<Camera>::Local(camera); Vector9 P = Camera().localCoordinates(camera);
Vector3 X = Canonical<Point3>::Local(point); Vector3 X = Point3::Logmap(point);
Point2 expectedMeasurement(1.2431567, 1.2525694); Vector2 expectedMeasurement(1.2431567, 1.2525694);
Matrix E1 = numericalDerivative21<Vector2, Vector9, Vector3>(adapted, P, X);
Matrix E2 = numericalDerivative22<Vector2, Vector9, Vector3>(adapted, P, X);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -234,11 +191,7 @@ TEST(AdaptAutoDiff, AutoDiff2) {
// Apply the mapping, to get image point b_x. // Apply the mapping, to get image point b_x.
Vector2 actual = adapted(P, X); Vector2 actual = adapted(P, X);
EXPECT(assert_equal(expectedMeasurement.vector(), actual, 1e-6)); EXPECT(assert_equal(expectedMeasurement, actual, 1e-6));
// Get expected derivatives
Matrix E1 = numericalDerivative21<Vector2, Vector9, Vector3>(adapted, P, X);
Matrix E2 = numericalDerivative22<Vector2, Vector9, Vector3>(adapted, P, X);
// Instantiate function // Instantiate function
SnavelyProjection snavely; SnavelyProjection snavely;
@ -259,21 +212,17 @@ TEST(AdaptAutoDiff, AutoDiff2) {
TEST(AdaptAutoDiff, AdaptAutoDiff) { TEST(AdaptAutoDiff, AdaptAutoDiff) {
using namespace example; using namespace example;
typedef AdaptAutoDiff<SnavelyProjection, Point2, Camera, Point3> Adaptor; typedef AdaptAutoDiff<SnavelyProjection, 2, 9, 3> Adaptor;
Adaptor snavely; Adaptor snavely;
// Apply the mapping, to get image point b_x. // Apply the mapping, to get image point b_x.
Point2 actual = snavely(camera, point); Vector2 actual = snavely(P, X);
EXPECT(assert_equal(expectedMeasurement, actual, 1e-6)); EXPECT(assert_equal(expectedMeasurement, actual, 1e-6));
// // Get expected derivatives
Matrix E1 = numericalDerivative21<Point2, Camera, Point3>(Adaptor(), camera, point);
Matrix E2 = numericalDerivative22<Point2, Camera, Point3>(Adaptor(), camera, point);
// Get derivatives with AutoDiff, not gives RowMajor results! // Get derivatives with AutoDiff, not gives RowMajor results!
Matrix29 H1; Matrix29 H1;
Matrix23 H2; Matrix23 H2;
Point2 actual2 = snavely(camera, point, H1, H2); Vector2 actual2 = snavely(P, X, H1, H2);
EXPECT(assert_equal(expectedMeasurement, actual2, 1e-6)); EXPECT(assert_equal(expectedMeasurement, actual2, 1e-6));
EXPECT(assert_equal(E1, H1, 1e-8)); EXPECT(assert_equal(E1, H1, 1e-8));
EXPECT(assert_equal(E2, H2, 1e-8)); EXPECT(assert_equal(E2, H2, 1e-8));
@ -282,15 +231,15 @@ TEST(AdaptAutoDiff, AdaptAutoDiff) {
/* ************************************************************************* */ /* ************************************************************************* */
// Test AutoDiff wrapper in an expression // Test AutoDiff wrapper in an expression
TEST(AdaptAutoDiff, SnavelyExpression) { TEST(AdaptAutoDiff, SnavelyExpression) {
Expression<Camera> P(1); Expression<Vector9> P(1);
Expression<Point3> X(2); Expression<Vector3> X(2);
typedef AdaptAutoDiff<SnavelyProjection, Point2, Camera, Point3> Adaptor; typedef AdaptAutoDiff<SnavelyProjection, 2, 9, 3> Adaptor;
Expression<Point2> expression(Adaptor(), P, X); Expression<Vector2> expression(Adaptor(), P, X);
EXPECT_LONGS_EQUAL(
sizeof(internal::BinaryExpression<Vector2, Vector9, Vector3>::Record),
expression.traceSize());
#ifdef GTSAM_USE_QUATERNIONS #ifdef GTSAM_USE_QUATERNIONS
EXPECT_LONGS_EQUAL(384,expression.traceSize()); // TODO(frank): should be zero EXPECT_LONGS_EQUAL(336, expression.traceSize());
#else
EXPECT_LONGS_EQUAL(sizeof(internal::BinaryExpression<Point2, Camera, Point3>::Record),
expression.traceSize()); // TODO(frank): should be zero
#endif #endif
set<Key> expected = list_of(1)(2); set<Key> expected = list_of(1)(2);
EXPECT(expected == expression.keys()); EXPECT(expected == expression.keys());
@ -302,4 +251,3 @@ int main() {
return TestRegistry::runAllTests(tr); return TestRegistry::runAllTests(tr);
} }
/* ************************************************************************* */ /* ************************************************************************* */