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

Deal with traits changes

release/4.3a0
dellaert 2014-10-21 01:26:17 +02:00
parent e60ad9d2b2
commit bf16446f92
4 changed files with 29 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
gtsam_unstable/nonlinear/Expression-inl.h
View File

@ -114,7 +114,7 @@ void handleLeafCase(
*/
template<class T>
class ExecutionTrace {
static const int Dim = dimension<T>::value;
static const int Dim = traits::dimension<T>::value;
enum {
Constant, Leaf, Function
} kind;
@ -196,7 +196,7 @@ public:
/// Primary template calls the generic Matrix reverseAD pipeline
template<size_t ROWS, class A>
struct Select {
typedef Eigen::Matrix<double, ROWS, dimension<A>::value> Jacobian;
typedef Eigen::Matrix<double, ROWS, traits::dimension<A>::value> Jacobian;
static void reverseAD(const ExecutionTrace<A>& trace, const Jacobian& dTdA,
JacobianMap& jacobians) {
trace.reverseAD(dTdA, jacobians);
@ -206,7 +206,7 @@ struct Select {
/// Partially specialized template calls the 2-dimensional output version
template<class A>
struct Select<2, A> {
typedef Eigen::Matrix<double, 2, dimension<A>::value> Jacobian;
typedef Eigen::Matrix<double, 2, traits::dimension<A>::value> Jacobian;
static void reverseAD(const ExecutionTrace<A>& trace, const Jacobian& dTdA,
JacobianMap& jacobians) {
trace.reverseAD2(dTdA, jacobians);
@ -317,7 +317,7 @@ public:
/// Return dimensions for each argument
virtual void dims(std::map<Key, size_t>& map) const {
map[key_] = dimension<T>::value;
map[key_] = traits::dimension<T>::value;
}
/// Return value
@ -368,13 +368,15 @@ public:
/// meta-function to generate fixed-size JacobianTA type
template<class T, class A>
struct Jacobian {
typedef Eigen::Matrix<double, dimension<T>::value, dimension<A>::value> type;
typedef Eigen::Matrix<double, traits::dimension<T>::value,
traits::dimension<A>::value> type;
};
/// meta-function to generate JacobianTA optional reference
template<class T, class A>
struct Optional {
typedef Eigen::Matrix<double, dimension<T>::value, dimension<A>::value> Jacobian;
typedef Eigen::Matrix<double, traits::dimension<T>::value,
traits::dimension<A>::value> Jacobian;
typedef boost::optional<Jacobian&> type;
};
@ -385,7 +387,7 @@ template<class T>
struct FunctionalBase: ExpressionNode<T> {
static size_t const N = 0; // number of arguments
typedef CallRecord<dimension<T>::value> Record;
typedef CallRecord<traits::dimension<T>::value> Record;
/// Construct an execution trace for reverse AD
void trace(const Values& values, Record* record, char*& raw) const {
@ -454,7 +456,7 @@ struct GenerateFunctionalNode: Argument<T, A, Base::N + 1>, Base {
/// Start the reverse AD process
virtual void startReverseAD(JacobianMap& jacobians) const {
Base::Record::startReverseAD(jacobians);
Select<dimension<T>::value, A>::reverseAD(This::trace, This::dTdA,
Select<traits::dimension<T>::value, A>::reverseAD(This::trace, This::dTdA,
jacobians);
}
@ -465,7 +467,7 @@ struct GenerateFunctionalNode: Argument<T, A, Base::N + 1>, Base {
}
/// Version specialized to 2-dimensional output
typedef Eigen::Matrix<double, 2, dimension<T>::value> Jacobian2T;
typedef Eigen::Matrix<double, 2, traits::dimension<T>::value> Jacobian2T;
virtual void reverseAD2(const Jacobian2T& dFdT,
JacobianMap& jacobians) const {
Base::Record::reverseAD2(dFdT, jacobians);

2
gtsam_unstable/nonlinear/Expression.h
View File

@ -159,7 +159,7 @@ public:
template<class T>
struct apply_compose {
typedef T result_type;
static const int Dim = dimension<T>::value;
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 {

16
gtsam_unstable/nonlinear/ExpressionFactor.h
View File

@ -37,6 +37,8 @@ class ExpressionFactor: public NoiseModelFactor {
std::vector<size_t> dimensions_; ///< dimensions of the Jacobian matrices
size_t augmentedCols_; ///< total number of columns + 1 (for RHS)
static const int Dim = traits::dimension<T>::value;
public:
/// Constructor
@ -45,7 +47,7 @@ public:
measurement_(measurement), expression_(expression) {
if (!noiseModel)
throw std::invalid_argument("ExpressionFactor: no NoiseModel.");
if (noiseModel->dim() != dimension<T>::value)
if (noiseModel->dim() != Dim)
throw std::invalid_argument(
"ExpressionFactor was created with a NoiseModel of incorrect dimension.");
noiseModel_ = noiseModel;
@ -68,7 +70,7 @@ public:
#ifdef DEBUG_ExpressionFactor
BOOST_FOREACH(size_t d, dimensions_)
std::cout << d << " ";
std::cout << " -> " << dimension<T>::value << "x" << augmentedCols_ << std::endl;
std::cout << " -> " << Dim << "x" << augmentedCols_ << std::endl;
#endif
}
@ -87,10 +89,9 @@ public:
JacobianMap blocks;
for (DenseIndex i = 0; i < size(); i++) {
Matrix& Hi = H->at(i);
Hi.resize(dimension<T>::value, dimensions_[i]);
Hi.resize(Dim, dimensions_[i]);
Hi.setZero(); // zero out
Eigen::Block<Matrix> block = Hi.block(0, 0, dimension<T>::value,
dimensions_[i]);
Eigen::Block<Matrix> block = Hi.block(0, 0, Dim, dimensions_[i]);
blocks.insert(std::make_pair(keys_[i], block));
}
@ -110,10 +111,9 @@ public:
// to [expression_.value] below, which writes directly into Ab_.
// Another malloc saved by creating a Matrix on the stack
static const int Dim = dimension<T>::value;
double memory[Dim * augmentedCols_];
Eigen::Map<Eigen::Matrix<double, dimension<T>::value, Eigen::Dynamic> > //
matrix(memory, dimension<T>::value, augmentedCols_);
Eigen::Map<Eigen::Matrix<double, Dim, Eigen::Dynamic> > //
matrix(memory, Dim, augmentedCols_);
matrix.setZero(); // zero out
// Construct block matrix, is of right size but un-initialized

52
gtsam_unstable/nonlinear/tests/testExpression.cpp
View File

@ -327,6 +327,7 @@ struct SnavelyProjection {
// is_manifold
TEST(Expression, is_manifold) {
using namespace traits;
EXPECT(!is_manifold<int>::value);
EXPECT(is_manifold<Point2>::value);
EXPECT(is_manifold<Matrix24>::value);
@ -337,6 +338,7 @@ TEST(Expression, is_manifold) {
// dimension
TEST(Expression, dimension) {
using namespace traits;
EXPECT_LONGS_EQUAL(2, dimension<Point2>::value);
EXPECT_LONGS_EQUAL(8, dimension<Matrix24>::value);
EXPECT_LONGS_EQUAL(1, dimension<double>::value);
@ -374,6 +376,7 @@ TEST(Expression, Charts) {
template<typename Y, typename X>
Matrix numericalDerivative(boost::function<Y(const X&)> h, const X& x,
double delta = 1e-5) {
using namespace traits;
BOOST_STATIC_ASSERT(is_manifold<Y>::value);
static const size_t M = dimension<Y>::value;
@ -491,33 +494,14 @@ TEST(Expression, AutoDiff2) {
EXPECT(assert_equal(E2,H2,1e-8));
}
/* ************************************************************************* */
// zero<T>::value is intended to be the origin of a canonical coordinate system
// with canonical(t) == DefaultChart<T>(zero<T>::value).apply(t)
template<typename T> struct zero;
template<typename T> class Canonical {
DefaultChart<T> chart;
public:
typedef T type;
typedef typename DefaultChart<T>::vector vector;
Canonical() :
chart(zero<T>::value) {
}
vector vee(const T& t) {
return chart.apply(t);
}
T hat(const vector& v) {
return chart.retract(v);
}
};
/* ************************************************************************* */
// Adapt ceres-style autodiff
template<typename F, typename T, typename A1, typename A2>
class AdaptAutoDiff {
static const int N = dimension<T>::value;
static const int M1 = dimension<A1>::value;
static const int M2 = dimension<A2>::value;
static const int N = traits::dimension<T>::value;
static const int M1 = traits::dimension<A1>::value;
static const int M2 = traits::dimension<A2>::value;
typedef Eigen::Matrix<double, N, M1, Eigen::RowMajor> RowMajor1;
typedef Eigen::Matrix<double, N, M2, Eigen::RowMajor> RowMajor2;
@ -547,8 +531,8 @@ public:
using ceres::internal::AutoDiff;
// Make arguments
Vector1 v1 = chart1.vee(a1);
Vector2 v2 = chart2.vee(a2);
Vector1 v1 = chart1.apply(a1);
Vector2 v2 = chart2.apply(a2);
bool success;
VectorT result;
@ -574,7 +558,7 @@ public:
if (!success)
throw std::runtime_error(
"AdaptAutoDiff: function call resulted in failure");
return chartT.hat(result);
return chartT.retract(result);
}
};
@ -582,24 +566,6 @@ public:
// The DefaultChart of Camera below is laid out like Snavely's 9-dim vector
typedef PinholeCamera<Cal3Bundler> Camera;
template<>
struct zero<Camera> {
static const Camera value;
};
const Camera zero<Camera>::value(Camera(Pose3(), Cal3Bundler(0, 0, 0)));
template<>
struct zero<Point3> {
static const Point3 value;
};
const Point3 zero<Point3>::value(Point3(0, 0, 0));
template<>
struct zero<Point2> {
static const Point2 value;
};
const Point2 zero<Point2>::value(Point2(0, 0));
/* ************************************************************************* */
// Test AutoDiff wrapper Snavely
TEST(Expression, AutoDiff3) {