Frank Dellaert
commit
ed9938b70c
|
|
@ -32,18 +32,23 @@ SO3 SO3::Rodrigues(const Vector3& axis, double theta) {
|
|||
// get components of axis \omega
|
||||
double wx = axis(0), wy = axis(1), wz = axis(2);
|
||||
|
||||
double c = cos(theta), s = sin(theta), c_1 = 1 - c;
|
||||
double wwTxx = wx * wx, wwTyy = wy * wy, wwTzz = wz * wz;
|
||||
double swx = wx * s, swy = wy * s, swz = wz * s;
|
||||
double costheta = cos(theta), sintheta = sin(theta), c_1 = 1 - costheta;
|
||||
double wx_sintheta = wx * sintheta, wy_sintheta = wy * sintheta, wz_sintheta = wz * sintheta;
|
||||
|
||||
double C00 = c_1 * wwTxx, C01 = c_1 * wx * wy, C02 = c_1 * wx * wz;
|
||||
double C11 = c_1 * wwTyy, C12 = c_1 * wy * wz;
|
||||
double C22 = c_1 * wwTzz;
|
||||
double C00 = c_1 * wx * wx, C01 = c_1 * wx * wy, C02 = c_1 * wx * wz;
|
||||
double C11 = c_1 * wy * wy, C12 = c_1 * wy * wz;
|
||||
double C22 = c_1 * wz * wz;
|
||||
|
||||
Matrix3 R;
|
||||
R << c + C00, -swz + C01, swy + C02, //
|
||||
swz + C01, c + C11, -swx + C12, //
|
||||
-swy + C02, swx + C12, c + C22;
|
||||
R(0, 0) = costheta + C00;
|
||||
R(1, 0) = wz_sintheta + C01;
|
||||
R(2, 0) = -wy_sintheta + C02;
|
||||
R(0, 1) = -wz_sintheta + C01;
|
||||
R(1, 1) = costheta + C11;
|
||||
R(2, 1) = wx_sintheta + C12;
|
||||
R(0, 2) = wy_sintheta + C02;
|
||||
R(1, 2) = -wx_sintheta + C12;
|
||||
R(2, 2) = costheta + C22;
|
||||
|
||||
return R;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
@ -27,97 +27,46 @@
|
|||
|
||||
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
|
||||
* Function evaluation, i.e., a function F that defines an operator
|
||||
* template<typename T> bool operator()(const T* const, const T* const, T* predicted) const;
|
||||
* 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;
|
||||
* 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 {
|
||||
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;
|
||||
static const int M1 = traits<A1>::dimension;
|
||||
static const int M2 = traits<A2>::dimension;
|
||||
typedef Eigen::Matrix<double, M, 1> VectorT;
|
||||
typedef Eigen::Matrix<double, N1, 1> Vector1;
|
||||
typedef Eigen::Matrix<double, N2, 1> Vector2;
|
||||
|
||||
typedef Eigen::Matrix<double, N, M1, Eigen::RowMajor> RowMajor1;
|
||||
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:
|
||||
|
||||
T operator()(const A1& a1, const A2& a2, OptionalJacobian<N, M1> H1 = boost::none,
|
||||
OptionalJacobian<N, M2> H2 = boost::none) {
|
||||
FUNCTOR f;
|
||||
|
||||
public:
|
||||
VectorT operator()(const Vector1& v1, const Vector2& v2,
|
||||
OptionalJacobian<M, N1> H1 = boost::none,
|
||||
OptionalJacobian<M, N2> H2 = boost::none) {
|
||||
using ceres::internal::AutoDiff;
|
||||
|
||||
// Make arguments
|
||||
Vector1 v1 = Canonical1::Local(a1);
|
||||
Vector2 v2 = Canonical2::Local(a2);
|
||||
|
||||
bool success;
|
||||
VectorT result;
|
||||
|
||||
if (H1 || H2) {
|
||||
|
||||
// Get derivatives with AutoDiff
|
||||
double *parameters[] = { v1.data(), v2.data() };
|
||||
double rowMajor1[N * M1], rowMajor2[N * M2]; // on the stack
|
||||
double *jacobians[] = { rowMajor1, rowMajor2 };
|
||||
success = AutoDiff<F, double, 9, 3>::Differentiate(f, parameters, 2,
|
||||
result.data(), jacobians);
|
||||
const double* parameters[] = {v1.data(), v2.data()};
|
||||
double rowMajor1[M * N1], rowMajor2[M * N2]; // on the stack
|
||||
double* jacobians[] = {rowMajor1, rowMajor2};
|
||||
success = AutoDiff<FUNCTOR, double, N1, N2>::Differentiate(
|
||||
f, parameters, M, result.data(), jacobians);
|
||||
|
||||
// Convert from row-major to columnn-major
|
||||
// TODO: if this is a bottleneck (probably not!) fix Autodiff to be Column-Major
|
||||
*H1 = Eigen::Map<RowMajor1>(rowMajor1);
|
||||
*H2 = Eigen::Map<RowMajor2>(rowMajor2);
|
||||
// TODO: if this is a bottleneck (probably not!) fix Autodiff to be
|
||||
// Column-Major
|
||||
if (H1) *H1 = Eigen::Map<RowMajor1>(rowMajor1);
|
||||
if (H2) *H2 = Eigen::Map<RowMajor2>(rowMajor2);
|
||||
|
||||
} else {
|
||||
// Apply the mapping, to get result
|
||||
|
|
@ -126,9 +75,8 @@ public:
|
|||
if (!success)
|
||||
throw std::runtime_error(
|
||||
"AdaptAutoDiff: function call resulted in failure");
|
||||
return CanonicalT::Retract(result);
|
||||
return result;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -25,6 +25,9 @@
|
|||
|
||||
#include <boost/algorithm/string.hpp>
|
||||
#include <boost/range/adaptor/map.hpp>
|
||||
#include <boost/format.hpp>
|
||||
#include <boost/timer/timer.hpp>
|
||||
|
||||
#include <fstream>
|
||||
#include <limits>
|
||||
#include <string>
|
||||
|
|
@ -236,7 +239,7 @@ void LevenbergMarquardtOptimizer::iterate() {
|
|||
|
||||
// Keep increasing lambda until we make make progress
|
||||
while (true) {
|
||||
|
||||
boost::timer::cpu_timer timer;
|
||||
if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
|
||||
cout << "trying lambda = " << state_.lambda << endl;
|
||||
|
||||
|
|
@ -248,7 +251,7 @@ void LevenbergMarquardtOptimizer::iterate() {
|
|||
double modelFidelity = 0.0;
|
||||
bool step_is_successful = false;
|
||||
bool stopSearchingLambda = false;
|
||||
double newError = numeric_limits<double>::infinity();
|
||||
double newError = numeric_limits<double>::infinity(), costChange;
|
||||
Values newValues;
|
||||
VectorValues delta;
|
||||
|
||||
|
|
@ -261,8 +264,6 @@ void LevenbergMarquardtOptimizer::iterate() {
|
|||
systemSolvedSuccessfully = false;
|
||||
}
|
||||
|
||||
double linearizedCostChange = 0,
|
||||
newlinearizedError = 0;
|
||||
if (systemSolvedSuccessfully) {
|
||||
state_.reuseDiagonal = true;
|
||||
|
||||
|
|
@ -272,9 +273,9 @@ void LevenbergMarquardtOptimizer::iterate() {
|
|||
delta.print("delta");
|
||||
|
||||
// cost change in the linearized system (old - new)
|
||||
newlinearizedError = linear->error(delta);
|
||||
double newlinearizedError = linear->error(delta);
|
||||
|
||||
linearizedCostChange = state_.error - newlinearizedError;
|
||||
double linearizedCostChange = state_.error - newlinearizedError;
|
||||
if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
|
||||
cout << "newlinearizedError = " << newlinearizedError <<
|
||||
" linearizedCostChange = " << linearizedCostChange << endl;
|
||||
|
|
@ -299,7 +300,7 @@ void LevenbergMarquardtOptimizer::iterate() {
|
|||
<< ") new (tentative) error (" << newError << ")" << endl;
|
||||
|
||||
// cost change in the original, nonlinear system (old - new)
|
||||
double costChange = state_.error - newError;
|
||||
costChange = state_.error - newError;
|
||||
|
||||
if (linearizedCostChange > 1e-20) { // the (linear) error has to decrease to satisfy this condition
|
||||
// fidelity of linearized model VS original system between
|
||||
|
|
@ -322,9 +323,13 @@ void LevenbergMarquardtOptimizer::iterate() {
|
|||
}
|
||||
|
||||
if (lmVerbosity == LevenbergMarquardtParams::SUMMARY) {
|
||||
cout << "[" << state_.iterations << "]: " << "new error = " << newlinearizedError
|
||||
<< ", delta = " << linearizedCostChange << ", lambda = " << state_.lambda
|
||||
<< ", success = " << systemSolvedSuccessfully << std::endl;
|
||||
// do timing
|
||||
double iterationTime = 1e-9 * timer.elapsed().wall;
|
||||
if (state_.iterations == 0)
|
||||
cout << "iter cost cost_change lambda success iter_time" << endl;
|
||||
cout << boost::format("% 4d % 8e % 3.2e % 3.2e % 4d % 3.2e") %
|
||||
state_.iterations % newError % costChange % state_.lambda %
|
||||
systemSolvedSuccessfully % iterationTime << endl;
|
||||
}
|
||||
|
||||
++state_.totalNumberInnerIterations;
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
@ -36,81 +36,43 @@ using boost::assign::map_list_of;
|
|||
namespace gtsam {
|
||||
|
||||
// Special version of Cal3Bundler so that default constructor = 0,0,0
|
||||
struct Cal: public Cal3Bundler {
|
||||
Cal(double f = 0, double k1 = 0, double k2 = 0, double u0 = 0, double v0 = 0) :
|
||||
Cal3Bundler(f, k1, k2, u0, v0) {
|
||||
struct Cal3Bundler0 : public Cal3Bundler {
|
||||
Cal3Bundler0(double f = 0, double k1 = 0, double k2 = 0, double u0 = 0,
|
||||
double v0 = 0)
|
||||
: Cal3Bundler(f, k1, k2, u0, v0) {}
|
||||
Cal3Bundler0 retract(const Vector& d) const {
|
||||
return Cal3Bundler0(fx() + d(0), k1() + d(1), k2() + d(2), u0(), v0());
|
||||
}
|
||||
Cal retract(const Vector& d) const {
|
||||
return Cal(fx() + d(0), k1() + d(1), k2() + d(2), u0(), v0());
|
||||
}
|
||||
Vector3 localCoordinates(const Cal& T2) const {
|
||||
Vector3 localCoordinates(const Cal3Bundler0& T2) const {
|
||||
return T2.vector() - vector();
|
||||
}
|
||||
};
|
||||
|
||||
template<>
|
||||
struct traits<Cal> : public internal::Manifold<Cal> {};
|
||||
template <>
|
||||
struct traits<Cal3Bundler0> : public internal::Manifold<Cal3Bundler0> {};
|
||||
|
||||
// With that, camera below behaves like Snavely's 9-dim vector
|
||||
typedef PinholeCamera<Cal> Camera;
|
||||
|
||||
typedef PinholeCamera<Cal3Bundler0> Camera;
|
||||
}
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
/* ************************************************************************* */
|
||||
// charts
|
||||
TEST(AdaptAutoDiff, Canonical) {
|
||||
|
||||
Canonical<Point2> chart1;
|
||||
EXPECT(chart1.Local(Point2(1, 0))==Vector2(1, 0));
|
||||
EXPECT(chart1.Retract(Vector2(1, 0))==Point2(1, 0));
|
||||
|
||||
Vector v2(2);
|
||||
v2 << 1, 0;
|
||||
Canonical<Vector2> chart2;
|
||||
EXPECT(assert_equal(v2, chart2.Local(Vector2(1, 0))));
|
||||
EXPECT(chart2.Retract(v2)==Vector2(1, 0));
|
||||
|
||||
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);
|
||||
|
||||
Canonical<Point3> chart4;
|
||||
Point3 point(1, 2, 3);
|
||||
Vector v3(3);
|
||||
v3 << 1, 2, 3;
|
||||
EXPECT(assert_equal(v3, chart4.Local(point)));
|
||||
EXPECT(assert_equal(chart4.Retract(v3), point));
|
||||
|
||||
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));
|
||||
|
||||
Canonical<Cal> chart6;
|
||||
Cal cal0;
|
||||
Vector z3 = Vector3::Zero();
|
||||
EXPECT(assert_equal(z3, chart6.Local(cal0)));
|
||||
EXPECT(assert_equal(chart6.Retract(z3), cal0));
|
||||
|
||||
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));
|
||||
// Check that ceres rotation convention is the same
|
||||
TEST(AdaptAutoDiff, Rotation) {
|
||||
Vector3 axisAngle(0.1, 0.2, 0.3);
|
||||
Matrix3 expected = Rot3::rodriguez(axisAngle).matrix();
|
||||
Matrix3 actual;
|
||||
ceres::AngleAxisToRotationMatrix(axisAngle.data(), actual.data());
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Some Ceres Snippets copied for testing
|
||||
// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
|
||||
template<typename T> inline T &RowMajorAccess(T *base, int rows, int cols,
|
||||
int i, int j) {
|
||||
template <typename T>
|
||||
inline T& RowMajorAccess(T* base, int rows, int cols, int i, int j) {
|
||||
return base[cols * i + j];
|
||||
}
|
||||
|
||||
|
|
@ -124,14 +86,14 @@ inline double RandDouble() {
|
|||
struct Projective {
|
||||
// Function that takes P and X as separate vectors:
|
||||
// P, X -> x
|
||||
template<typename A>
|
||||
template <typename A>
|
||||
bool operator()(A const P[12], A const X[4], A x[2]) const {
|
||||
A PX[3];
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
PX[i] = RowMajorAccess(P, 3, 4, i, 0) * X[0]
|
||||
+ RowMajorAccess(P, 3, 4, i, 1) * X[1]
|
||||
+ RowMajorAccess(P, 3, 4, i, 2) * X[2]
|
||||
+ RowMajorAccess(P, 3, 4, i, 3) * X[3];
|
||||
PX[i] = RowMajorAccess(P, 3, 4, i, 0) * X[0] +
|
||||
RowMajorAccess(P, 3, 4, i, 1) * X[1] +
|
||||
RowMajorAccess(P, 3, 4, i, 2) * X[2] +
|
||||
RowMajorAccess(P, 3, 4, i, 3) * X[3];
|
||||
}
|
||||
if (PX[2] != 0.0) {
|
||||
x[0] = PX[0] / PX[2];
|
||||
|
|
@ -160,29 +122,31 @@ TEST(AdaptAutoDiff, AutoDiff) {
|
|||
Projective projective;
|
||||
|
||||
// Make arguments
|
||||
typedef Eigen::Matrix<double, 3, 4, Eigen::RowMajor> M;
|
||||
M P;
|
||||
typedef Eigen::Matrix<double, 3, 4, Eigen::RowMajor> RowMajorMatrix34;
|
||||
RowMajorMatrix34 P;
|
||||
P << 1, 0, 0, 0, 0, 1, 0, 5, 0, 0, 1, 0;
|
||||
Vector4 X(10, 0, 5, 1);
|
||||
|
||||
// Apply the mapping, to get image point b_x.
|
||||
Vector expected = Vector2(2, 1);
|
||||
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);
|
||||
Matrix E1 = numericalDerivative21<Vector2, RowMajorMatrix34, Vector4>(
|
||||
Projective(), P, X);
|
||||
Matrix E2 = numericalDerivative22<Vector2, RowMajorMatrix34, 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));
|
||||
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));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -197,78 +161,85 @@ Vector2 adapted(const Vector9& P, const Vector3& X) {
|
|||
throw std::runtime_error("Snavely fail");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
namespace example {
|
||||
Camera camera(Pose3(Rot3().retract(Vector3(0.1, 0.2, 0.3)), Point3(0, 5, 0)),
|
||||
Cal3Bundler0(1, 0, 0));
|
||||
Point3 point(10, 0, -5); // negative Z-axis convention of Snavely!
|
||||
Vector9 P = Camera().localCoordinates(camera);
|
||||
Vector3 X = Point3::Logmap(point);
|
||||
Vector2 expectedMeasurement(1.2431567, 1.2525694);
|
||||
Matrix E1 = numericalDerivative21<Vector2, Vector9, Vector3>(adapted, P, X);
|
||||
Matrix E2 = numericalDerivative22<Vector2, Vector9, Vector3>(adapted, P, X);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Check that Local worked as expected
|
||||
TEST(AdaptAutoDiff, Local) {
|
||||
using namespace example;
|
||||
Vector9 expectedP = (Vector9() << 0.1, 0.2, 0.3, 0, 5, 0, 1, 0, 0).finished();
|
||||
EXPECT(equal_with_abs_tol(expectedP, P));
|
||||
Vector3 expectedX(10, 0, -5); // negative Z-axis convention of Snavely!
|
||||
EXPECT(equal_with_abs_tol(expectedX, X));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Test Ceres AutoDiff
|
||||
TEST(AdaptAutoDiff, AutoDiff2) {
|
||||
using namespace example;
|
||||
using ceres::internal::AutoDiff;
|
||||
|
||||
// Apply the mapping, to get image point b_x.
|
||||
Vector2 actual = adapted(P, X);
|
||||
EXPECT(assert_equal(expectedMeasurement, actual, 1e-6));
|
||||
|
||||
// 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));
|
||||
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(AdaptAutoDiff, AutoDiff3) {
|
||||
TEST(AdaptAutoDiff, AdaptAutoDiff) {
|
||||
using namespace example;
|
||||
|
||||
// Make arguments
|
||||
Camera P(Pose3(Rot3(), Point3(0, 5, 0)), Cal(1, 0, 0));
|
||||
Point3 X(10, 0, -5); // negative Z-axis convention of Snavely!
|
||||
|
||||
typedef AdaptAutoDiff<SnavelyProjection, Point2, Camera, Point3> Adaptor;
|
||||
typedef AdaptAutoDiff<SnavelyProjection, 2, 9, 3> 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);
|
||||
Vector2 actual = snavely(P, X);
|
||||
EXPECT(assert_equal(expectedMeasurement, actual, 1e-6));
|
||||
|
||||
// Get derivatives with AutoDiff, not gives RowMajor results!
|
||||
Matrix29 H1;
|
||||
Matrix23 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));
|
||||
Vector2 actual2 = snavely(P, X, H1, H2);
|
||||
EXPECT(assert_equal(expectedMeasurement, actual2, 1e-6));
|
||||
EXPECT(assert_equal(E1, H1, 1e-8));
|
||||
EXPECT(assert_equal(E2, H2, 1e-8));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Test AutoDiff wrapper in an expression
|
||||
TEST(AdaptAutoDiff, Snavely) {
|
||||
Expression<Camera> P(1);
|
||||
Expression<Point3> X(2);
|
||||
typedef AdaptAutoDiff<SnavelyProjection, Point2, Camera, Point3> Adaptor;
|
||||
Expression<Point2> expression(Adaptor(), P, X);
|
||||
TEST(AdaptAutoDiff, SnavelyExpression) {
|
||||
Expression<Vector9> P(1);
|
||||
Expression<Vector3> X(2);
|
||||
typedef AdaptAutoDiff<SnavelyProjection, 2, 9, 3> Adaptor;
|
||||
Expression<Vector2> expression(Adaptor(), P, X);
|
||||
EXPECT_LONGS_EQUAL(
|
||||
sizeof(internal::BinaryExpression<Vector2, Vector9, Vector3>::Record),
|
||||
expression.traceSize());
|
||||
#ifdef GTSAM_USE_QUATERNIONS
|
||||
EXPECT_LONGS_EQUAL(384,expression.traceSize()); // Todo, should be zero
|
||||
#else
|
||||
EXPECT_LONGS_EQUAL(sizeof(internal::BinaryExpression<Point2, Camera, Point3>::Record),
|
||||
expression.traceSize()); // Todo, should be zero
|
||||
EXPECT_LONGS_EQUAL(336, expression.traceSize());
|
||||
#endif
|
||||
set<Key> expected = list_of(1)(2);
|
||||
EXPECT(expected == expression.keys());
|
||||
|
|
@ -280,4 +251,3 @@ int main() {
|
|||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
|
|||
|
|
@ -57,16 +57,19 @@ int main() {
|
|||
f1 = boost::make_shared<GeneralSFMFactor<Camera, Point3> >(z, model, 1, 2);
|
||||
time("GeneralSFMFactor<Camera> : ", f1, values);
|
||||
|
||||
// AdaptAutoDiff
|
||||
typedef AdaptAutoDiff<SnavelyProjection, Point2, Camera, Point3> AdaptedSnavely;
|
||||
Point2_ expression(AdaptedSnavely(), camera, point);
|
||||
f2 = boost::make_shared<ExpressionFactor<Point2> >(model, z, expression);
|
||||
time("Point2_(AdaptedSnavely(), camera, point): ", f2, values);
|
||||
|
||||
// ExpressionFactor
|
||||
Point2_ expression2(camera, &Camera::project2, point);
|
||||
f3 = boost::make_shared<ExpressionFactor<Point2> >(model, z, expression2);
|
||||
time("Point2_(camera, &Camera::project, point): ", f3, values);
|
||||
|
||||
// AdaptAutoDiff
|
||||
values.clear();
|
||||
values.insert(1,Vector9(Vector9::Zero()));
|
||||
values.insert(2,Vector3(0,0,1));
|
||||
typedef AdaptAutoDiff<SnavelyProjection, 2, 9, 3> AdaptedSnavely;
|
||||
Expression<Vector2> expression(AdaptedSnavely(), Expression<Vector9>(1), Expression<Vector3>(2));
|
||||
f2 = boost::make_shared<ExpressionFactor<Vector2> >(model, z.vector(), expression);
|
||||
time("Point2_(AdaptedSnavely(), camera, point): ", f2, values);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -34,7 +34,7 @@
|
|||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
#define USE_GTSAM_FACTOR
|
||||
//#define USE_GTSAM_FACTOR
|
||||
#ifdef USE_GTSAM_FACTOR
|
||||
#include <gtsam/slam/GeneralSFMFactor.h>
|
||||
#include <gtsam/geometry/Cal3Bundler.h>
|
||||
|
|
@ -45,6 +45,7 @@ typedef GeneralSFMFactor<Camera, Point3> SfmFactor;
|
|||
#include <gtsam/3rdparty/ceres/example.h>
|
||||
#include <gtsam/nonlinear/ExpressionFactor.h>
|
||||
#include <gtsam/nonlinear/AdaptAutoDiff.h>
|
||||
// See http://www.cs.cornell.edu/~snavely/bundler/bundler-v0.3-manual.html for conventions
|
||||
// Special version of Cal3Bundler so that default constructor = 0,0,0
|
||||
struct CeresCalibration: public Cal3Bundler {
|
||||
CeresCalibration(double f = 0, double k1 = 0, double k2 = 0, double u0 = 0,
|
||||
|
|
@ -76,14 +77,15 @@ int main(int argc, char* argv[]) {
|
|||
using symbol_shorthand::P;
|
||||
|
||||
// Load BAL file (default is tiny)
|
||||
string defaultFilename = findExampleDataFile("dubrovnik-3-7-pre");
|
||||
//string defaultFilename = findExampleDataFile("dubrovnik-3-7-pre");
|
||||
string defaultFilename = "/home/frank/problem-16-22106-pre.txt";
|
||||
SfM_data db;
|
||||
bool success = readBAL(argc > 1 ? argv[1] : defaultFilename, db);
|
||||
if (!success)
|
||||
throw runtime_error("Could not access file!");
|
||||
|
||||
#ifndef USE_GTSAM_FACTOR
|
||||
AdaptAutoDiff<SnavelyProjection, Point2, Camera, Point3> snavely;
|
||||
AdaptAutoDiff<SnavelyProjection, 2, 9, 3> snavely;
|
||||
#endif
|
||||
|
||||
// Build graph
|
||||
|
|
@ -92,14 +94,15 @@ int main(int argc, char* argv[]) {
|
|||
for (size_t j = 0; j < db.number_tracks(); j++) {
|
||||
BOOST_FOREACH (const SfM_Measurement& m, db.tracks[j].measurements) {
|
||||
size_t i = m.first;
|
||||
Point2 measurement = m.second;
|
||||
Point2 z = m.second;
|
||||
#ifdef USE_GTSAM_FACTOR
|
||||
graph.push_back(SfmFactor(measurement, unit2, i, P(j)));
|
||||
graph.push_back(SfmFactor(z, unit2, i, P(j)));
|
||||
#else
|
||||
Expression<Camera> camera_(i);
|
||||
Expression<Point3> point_(P(j));
|
||||
graph.addExpressionFactor(unit2, measurement,
|
||||
Expression<Point2>(snavely, camera_, point_));
|
||||
Expression<Vector9> camera_(i);
|
||||
Expression<Vector3> point_(P(j));
|
||||
// Snavely expects measurements in OpenGL format, with y increasing upwards
|
||||
graph.addExpressionFactor(unit2, Vector2(z.x(), -z.y()),
|
||||
Expression<Vector2>(snavely, camera_, point_));
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
|
@ -110,21 +113,33 @@ int main(int argc, char* argv[]) {
|
|||
#ifdef USE_GTSAM_FACTOR
|
||||
initial.insert((i++), camera);
|
||||
#else
|
||||
Camera ceresCamera(camera.pose(), camera.calibration());
|
||||
initial.insert((i++), ceresCamera);
|
||||
// readBAL converts to GTSAM format, so we need to convert back !
|
||||
Camera ceresCamera(gtsam2openGL(camera.pose()), camera.calibration());
|
||||
Vector9 v9 = Camera().localCoordinates(ceresCamera);
|
||||
initial.insert((i++), v9);
|
||||
#endif
|
||||
}
|
||||
BOOST_FOREACH(const SfM_Track& track, db.tracks)
|
||||
BOOST_FOREACH(const SfM_Track& track, db.tracks) {
|
||||
#ifdef USE_GTSAM_FACTOR
|
||||
initial.insert(P(j++), track.p);
|
||||
#else
|
||||
Vector3 v3 = track.p.vector();
|
||||
initial.insert(P(j++), v3);
|
||||
#endif
|
||||
}
|
||||
|
||||
// Check projection
|
||||
// Check projection of first point in first camera
|
||||
Point2 expected = db.tracks.front().measurements.front().second;
|
||||
#ifdef USE_GTSAM_FACTOR
|
||||
Camera camera = initial.at<Camera>(0);
|
||||
Point3 point = initial.at<Point3>(P(0));
|
||||
#ifdef USE_GTSAM_FACTOR
|
||||
Point2 actual = camera.project(point);
|
||||
#else
|
||||
Point2 actual = snavely(camera, point);
|
||||
Vector9 camera = initial.at<Vector9>(0);
|
||||
Vector3 point = initial.at<Vector3>(P(0));
|
||||
Point2 z = snavely(camera, point);
|
||||
// Again: fix y to increase upwards
|
||||
Point2 actual(z.x(), -z.y());
|
||||
#endif
|
||||
assert_equal(expected,actual,10);
|
||||
|
||||
|
|
@ -146,8 +161,7 @@ int main(int argc, char* argv[]) {
|
|||
LevenbergMarquardtParams params;
|
||||
LevenbergMarquardtParams::SetCeresDefaults(¶ms);
|
||||
params.setOrdering(ordering);
|
||||
params.setVerbosity("ERROR");
|
||||
params.setVerbosityLM("TRYLAMBDA");
|
||||
params.setVerbosityLM("SUMMARY");
|
||||
LevenbergMarquardtOptimizer lm(graph, initial, params);
|
||||
Values result = lm.optimize();
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue