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

Implemented is_manifold and dimension for all types in testExpressionFactor

release/4.3a0
dellaert 2014-10-19 00:35:25 +02:00
parent c32d2bb3b2
commit 6e142184cc
13 changed files with 116 additions and 78 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
gtsam/base/Manifold.h
View File

@ -53,9 +53,6 @@ struct is_manifold: public std::false_type {
// dimension, can return Eigen::Dynamic (-1) if not known at compile time
template<typename T>
struct dimension;
//: public std::integral_constant<int, T::dimension> {
// BOOST_STATIC_ASSERT(is_manifold<T>::value);
//};
// Chart is a map from T -> vector, retract is its inverse
template<typename T>
@ -82,7 +79,7 @@ struct is_manifold<double> : public std::true_type {
};
template<>
struct dimension<double> : public std::integral_constant<size_t, 1> {
struct dimension<double> : public std::integral_constant<int, 1> {
};
template<>
@ -111,7 +108,7 @@ struct is_manifold<Eigen::Matrix<double, M, N, Options> > : public std::true_typ
// TODO: Could be more sophisticated using Eigen traits and SFINAE?
typedef std::integral_constant<size_t, Eigen::Dynamic> Dynamic;
typedef std::integral_constant<int, Eigen::Dynamic> Dynamic;
template<int Options>
struct dimension<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Options> > : public Dynamic {
@ -129,7 +126,7 @@ struct dimension<Eigen::Matrix<double, Eigen::Dynamic, N, Options> > : public Dy
template<int M, int N, int Options>
struct dimension<Eigen::Matrix<double, M, N, Options> > : public std::integral_constant<
size_t, M * N> {
int, M * N> {
BOOST_STATIC_ASSERT(M!=Eigen::Dynamic && N!=Eigen::Dynamic);
};

14
gtsam/geometry/Cal3Bundler.h
View File

@ -36,8 +36,6 @@ private:
double u0_, v0_; ///< image center, not a parameter to be optimized but a constant
public:
/// dimension of the variable - used to autodetect sizes
static const size_t dimension = 3;
/// @name Standard Constructors
/// @{
@ -169,6 +167,14 @@ private:
/// @}
};
};
} // namespace gtsam
template<>
struct is_manifold<Cal3Bundler> : public std::true_type {
};
template<>
struct dimension<Cal3Bundler> : public std::integral_constant<size_t, 3> {
};
} // namespace gtsam

15
gtsam/geometry/Cal3_S2.h
View File

@ -36,8 +36,6 @@ private:
double fx_, fy_, s_, u0_, v0_;
public:
/// dimension of the variable - used to autodetect sizes
static const size_t dimension = 5;
typedef boost::shared_ptr<Cal3_S2> shared_ptr; ///< shared pointer to calibration object
@ -200,12 +198,12 @@ public:
/// return DOF, dimensionality of tangent space
inline size_t dim() const {
return dimension;
return 5;
}
/// return DOF, dimensionality of tangent space
static size_t Dim() {
return dimension;
return 5;
}
/// Given 5-dim tangent vector, create new calibration
@ -242,4 +240,13 @@ private:
};
template<>
struct is_manifold<Cal3_S2> : public std::true_type {
};
template<>
struct dimension<Cal3_S2> : public std::integral_constant<size_t, 5> {
};
} // \ namespace gtsam

2
gtsam/geometry/PinholeCamera.h
View File

@ -303,7 +303,7 @@ public:
return K_.uncalibrate(pn);
}
typedef Eigen::Matrix<double,2,Calibration::dimension> Matrix2K;
typedef Eigen::Matrix<double,2,dimension<Calibration>::value> Matrix2K;
/** project a point from world coordinate to the image
* @param pw is a point in world coordinates

17
gtsam/geometry/Point2.h
View File

@ -33,10 +33,9 @@ namespace gtsam {
* \nosubgrouping
*/
class GTSAM_EXPORT Point2 : public DerivedValue<Point2> {
public:
/// dimension of the variable - used to autodetect sizes
static const size_t dimension = 2;
private:
double x_, y_;
public:
@ -153,10 +152,10 @@ public:
/// @{
/// dimension of the variable - used to autodetect sizes
inline static size_t Dim() { return dimension; }
inline static size_t Dim() { return 2; }
/// Dimensionality of tangent space = 2 DOF
inline size_t dim() const { return dimension; }
inline size_t dim() const { return 2; }
/// Updates a with tangent space delta
inline Point2 retract(const Vector& v) const { return *this + Point2(v); }
@ -251,5 +250,13 @@ private:
/// multiply with scalar
inline Point2 operator*(double s, const Point2& p) {return p*s;}
template<>
struct is_manifold<Point2> : public std::true_type {
};
template<>
struct dimension<Point2> : public std::integral_constant<size_t, 2> {
};
}

16
gtsam/geometry/Point3.h
View File

@ -37,11 +37,9 @@ namespace gtsam {
* \nosubgrouping
*/
class GTSAM_EXPORT Point3 : public DerivedValue<Point3> {
public:
/// dimension of the variable - used to autodetect sizes
static const size_t dimension = 3;
private:
double x_, y_, z_;
public:
@ -122,10 +120,10 @@ namespace gtsam {
/// @{
/// dimension of the variable - used to autodetect sizes
inline static size_t Dim() { return dimension; }
inline static size_t Dim() { return 3; }
/// return dimensionality of tangent space, DOF = 3
inline size_t dim() const { return dimension; }
inline size_t dim() const { return 3; }
/// Updates a with tangent space delta
inline Point3 retract(const Vector& v) const { return Point3(*this + v); }
@ -244,4 +242,12 @@ namespace gtsam {
/// Syntactic sugar for multiplying coordinates by a scalar s*p
inline Point3 operator*(double s, const Point3& p) { return p*s;}
template<>
struct is_manifold<Point3> : public std::true_type {
};
template<>
struct dimension<Point3> : public std::integral_constant<size_t, 3> {
};
}

15
gtsam/geometry/Pose2.h
View File

@ -36,7 +36,6 @@ namespace gtsam {
class GTSAM_EXPORT Pose2 : public DerivedValue<Pose2> {
public:
static const size_t dimension = 3;
/** Pose Concept requirements */
typedef Rot2 Rotation;
@ -142,10 +141,10 @@ public:
/// @{
/// Dimensionality of tangent space = 3 DOF - used to autodetect sizes
inline static size_t Dim() { return dimension; }
inline static size_t Dim() { return 3; }
/// Dimensionality of tangent space = 3 DOF
inline size_t dim() const { return dimension; }
inline size_t dim() const { return 3; }
/// Retraction from R^3 \f$ [T_x,T_y,\theta] \f$ to Pose2 manifold neighborhood around current pose
Pose2 retract(const Vector& v) const;
@ -294,6 +293,8 @@ public:
*/
static std::pair<size_t, size_t> rotationInterval() { return std::make_pair(2, 2); }
/// @}
private:
// Serialization function
@ -320,7 +321,13 @@ inline Matrix wedge<Pose2>(const Vector& xi) {
typedef std::pair<Point2,Point2> Point2Pair;
GTSAM_EXPORT boost::optional<Pose2> align(const std::vector<Point2Pair>& pairs);
/// @}
template<>
struct is_manifold<Pose2> : public std::true_type {
};
template<>
struct dimension<Pose2> : public std::integral_constant<size_t, 3> {
};
} // namespace gtsam

13
gtsam/geometry/Pose3.h
View File

@ -41,7 +41,6 @@ class Pose2;
*/
class GTSAM_EXPORT Pose3: public DerivedValue<Pose3> {
public:
static const size_t dimension = 6;
/** Pose Concept requirements */
typedef Rot3 Rotation;
@ -132,12 +131,12 @@ public:
/// Dimensionality of tangent space = 6 DOF - used to autodetect sizes
static size_t Dim() {
return dimension;
return 6;
}
/// Dimensionality of the tangent space = 6 DOF
size_t dim() const {
return dimension;
return 6;
}
/// Retraction from R^6 \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ from R^ with fast first-order approximation to the exponential map
@ -355,4 +354,12 @@ inline Matrix wedge<Pose3>(const Vector& xi) {
typedef std::pair<Point3, Point3> Point3Pair;
GTSAM_EXPORT boost::optional<Pose3> align(const std::vector<Point3Pair>& pairs);
template<>
struct is_manifold<Pose3> : public std::true_type {
};
template<>
struct dimension<Pose3> : public std::integral_constant<size_t, 6> {
};
} // namespace gtsam

21
gtsam/geometry/Rot3.h
View File

@ -59,10 +59,9 @@ namespace gtsam {
* \nosubgrouping
*/
class GTSAM_EXPORT Rot3 : public DerivedValue<Rot3> {
public:
static const size_t dimension = 3;
private:
#ifdef GTSAM_USE_QUATERNIONS
/** Internal Eigen Quaternion */
Quaternion quaternion_;
@ -260,10 +259,10 @@ namespace gtsam {
/// @{
/// dimension of the variable - used to autodetect sizes
static size_t Dim() { return dimension; }
static size_t Dim() { return 3; }
/// return dimensionality of tangent space, DOF = 3
size_t dim() const { return dimension; }
size_t dim() const { return 3; }
/**
* The method retract() is used to map from the tangent space back to the manifold.
@ -449,6 +448,8 @@ namespace gtsam {
/// Output stream operator
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Rot3& p);
/// @}
private:
/** Serialization function */
@ -478,8 +479,6 @@ namespace gtsam {
};
/// @}
/**
* [RQ] receives a 3 by 3 matrix and returns an upper triangular matrix R
* and 3 rotation angles corresponding to the rotation matrix Q=Qz'*Qy'*Qx'
@ -491,4 +490,14 @@ namespace gtsam {
* @return a vector [thetax, thetay, thetaz] in radians.
*/
GTSAM_EXPORT std::pair<Matrix3,Vector3> RQ(const Matrix3& A);
template<>
struct is_manifold<Rot3> : public std::true_type {
};
template<>
struct dimension<Rot3> : public std::integral_constant<size_t, 3> {
};
}

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

@ -22,6 +22,8 @@
#include <gtsam/nonlinear/Values.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/Manifold.h>
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
@ -38,7 +40,6 @@
namespace MPL = boost::mpl::placeholders;
#include <new> // for placement new
class ExpressionFactorBinaryTest;
// Forward declare for testing
@ -75,14 +76,15 @@ struct CallRecord {
//-----------------------------------------------------------------------------
/// Handle Leaf Case: reverseAD ends here, by writing a matrix into Jacobians
template<int ROWS, int COLS>
void handleLeafCase(const Eigen::Matrix<double,ROWS,COLS>& dTdA,
void handleLeafCase(const Eigen::Matrix<double, ROWS, COLS>& dTdA,
JacobianMap& jacobians, Key key) {
JacobianMap::iterator it = jacobians.find(key);
it->second.block<ROWS,COLS>(0,0) += dTdA; // block makes HUGE difference
it->second.block<ROWS, COLS>(0, 0) += dTdA; // block makes HUGE difference
}
/// Handle Leaf Case for Dynamic Matrix type (slower)
template<>
void handleLeafCase(const Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic>& dTdA,
void handleLeafCase(
const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>& dTdA,
JacobianMap& jacobians, Key key) {
JacobianMap::iterator it = jacobians.find(key);
it->second += dTdA;
@ -98,12 +100,13 @@ void handleLeafCase(const Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic>& d
*/
template<class T>
class ExecutionTrace {
static const int Dim = dimension<T>::value;
enum {
Constant, Leaf, Function
} kind;
union {
Key key;
CallRecord<T::dimension>* ptr;
CallRecord<Dim>* ptr;
} content;
public:
/// Pointer always starts out as a Constant
@ -116,7 +119,7 @@ public:
content.key = key;
}
/// Take ownership of pointer to a Function Record
void setFunction(CallRecord<T::dimension>* record) {
void setFunction(CallRecord<Dim>* record) {
kind = Function;
content.ptr = record;
}
@ -145,7 +148,7 @@ public:
* *** This is the main entry point for reverseAD, called from Expression ***
* Called only once, either inserts I into Jacobians (Leaf) or starts AD (Function)
*/
typedef Eigen::Matrix<double, T::dimension, T::dimension> JacobianTT;
typedef Eigen::Matrix<double, Dim, Dim> JacobianTT;
void startReverseAD(JacobianMap& jacobians) const {
if (kind == Leaf) {
// This branch will only be called on trivial Leaf expressions, i.e. Priors
@ -164,7 +167,7 @@ public:
content.ptr->reverseAD(dTdA, jacobians);
}
// Either add to Jacobians (Leaf) or propagate (Function)
typedef Eigen::Matrix<double, 2, T::dimension> Jacobian2T;
typedef Eigen::Matrix<double, 2, Dim> Jacobian2T;
void reverseAD2(const Jacobian2T& dTdA, JacobianMap& jacobians) const {
if (kind == Leaf)
handleLeafCase(dTdA, jacobians, content.key);
@ -179,7 +182,7 @@ public:
/// Primary template calls the generic Matrix reverseAD pipeline
template<size_t ROWS, class A>
struct Select {
typedef Eigen::Matrix<double, ROWS, A::dimension> Jacobian;
typedef Eigen::Matrix<double, ROWS, dimension<A>::value> Jacobian;
static void reverseAD(const ExecutionTrace<A>& trace, const Jacobian& dTdA,
JacobianMap& jacobians) {
trace.reverseAD(dTdA, jacobians);
@ -189,7 +192,7 @@ struct Select {
/// Partially specialized template calls the 2-dimensional output version
template<class A>
struct Select<2, A> {
typedef Eigen::Matrix<double, 2, A::dimension> Jacobian;
typedef Eigen::Matrix<double, 2, dimension<A>::value> Jacobian;
static void reverseAD(const ExecutionTrace<A>& trace, const Jacobian& dTdA,
JacobianMap& jacobians) {
trace.reverseAD2(dTdA, jacobians);
@ -300,7 +303,7 @@ public:
/// Return dimensions for each argument
virtual void dims(std::map<Key, size_t>& map) const {
map[key_] = T::dimension;
map[key_] = dimension<T>::value;
}
/// Return value
@ -351,13 +354,13 @@ public:
/// meta-function to generate fixed-size JacobianTA type
template<class T, class A>
struct Jacobian {
typedef Eigen::Matrix<double, T::dimension, A::dimension> type;
typedef Eigen::Matrix<double, dimension<T>::value, dimension<A>::value> type;
};
/// meta-function to generate JacobianTA optional reference
template<class T, class A>
struct Optional {
typedef Eigen::Matrix<double, T::dimension, A::dimension> Jacobian;
typedef Eigen::Matrix<double, dimension<T>::value, dimension<A>::value> Jacobian;
typedef boost::optional<Jacobian&> type;
};
@ -368,7 +371,7 @@ template<class T>
struct FunctionalBase: ExpressionNode<T> {
static size_t const N = 0; // number of arguments
typedef CallRecord<T::dimension> Record;
typedef CallRecord<dimension<T>::value> Record;
/// Construct an execution trace for reverse AD
void trace(const Values& values, Record* record, char*& raw) const {
@ -437,7 +440,8 @@ 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<T::dimension, A>::reverseAD(This::trace, This::dTdA, jacobians);
Select<dimension<T>::value, A>::reverseAD(This::trace, This::dTdA,
jacobians);
}
/// Given df/dT, multiply in dT/dA and continue reverse AD process
@ -447,7 +451,7 @@ struct GenerateFunctionalNode: Argument<T, A, Base::N + 1>, Base {
}
/// Version specialized to 2-dimensional output
typedef Eigen::Matrix<double, 2, T::dimension> Jacobian2T;
typedef Eigen::Matrix<double, 2, dimension<T>::value> Jacobian2T;
virtual void reverseAD2(const Jacobian2T& dFdT,
JacobianMap& jacobians) const {
Base::Record::reverseAD2(dFdT, jacobians);

3
gtsam_unstable/nonlinear/Expression.h
View File

@ -154,7 +154,8 @@ public:
template<class T>
struct apply_compose {
typedef T result_type;
typedef Eigen::Matrix<double, T::dimension, T::dimension> Jacobian;
static const int Dim = 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 {
return x.compose(y, H1, H2);

14
gtsam_unstable/nonlinear/ExpressionFactor.h
View File

@ -45,7 +45,7 @@ public:
measurement_(measurement), expression_(expression) {
if (!noiseModel)
throw std::invalid_argument("ExpressionFactor: no NoiseModel.");
if (noiseModel->dim() != T::dimension)
if (noiseModel->dim() != dimension<T>::value)
throw std::invalid_argument(
"ExpressionFactor was created with a NoiseModel of incorrect dimension.");
noiseModel_ = noiseModel;
@ -68,7 +68,7 @@ public:
#ifdef DEBUG_ExpressionFactor
BOOST_FOREACH(size_t d, dimensions_)
std::cout << d << " ";
std::cout << " -> " << T::dimension << "x" << augmentedCols_ << std::endl;
std::cout << " -> " << dimension<T>::value << "x" << augmentedCols_ << std::endl;
#endif
}
@ -87,9 +87,9 @@ public:
JacobianMap blocks;
for (DenseIndex i = 0; i < size(); i++) {
Matrix& Hi = H->at(i);
Hi.resize(T::dimension, dimensions_[i]);
Hi.resize(dimension<T>::value, dimensions_[i]);
Hi.setZero(); // zero out
Eigen::Block<Matrix> block = Hi.block(0, 0, T::dimension,
Eigen::Block<Matrix> block = Hi.block(0, 0, dimension<T>::value,
dimensions_[i]);
blocks.insert(std::make_pair(keys_[i], block));
}
@ -105,9 +105,9 @@ public:
virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x) const {
// Allocate memory on stack and create a view on it (saves a malloc)
double memory[T::dimension * augmentedCols_];
Eigen::Map<Eigen::Matrix<double, T::dimension, Eigen::Dynamic> > //
matrix(memory, T::dimension, augmentedCols_);
double memory[dimension<T>::value * augmentedCols_];
Eigen::Map<Eigen::Matrix<double, dimension<T>::value, Eigen::Dynamic> > //
matrix(memory, dimension<T>::value, augmentedCols_);
matrix.setZero(); // zero out
// Construct block matrix, is of right size but un-initialized

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

@ -324,19 +324,6 @@ struct SnavelyReprojectionError {
/* ************************************************************************* */
// Point2
namespace gtsam {
template<>
struct is_manifold<Point2> : public true_type {
};
template<>
struct dimension<Point2> : public integral_constant<size_t, 2> {
};
}
// is_manifold
TEST(Expression, is_manifold) {
EXPECT(!is_manifold<int>::value);
@ -506,9 +493,9 @@ TEST(Expression, AutoDiff2) {
/* ************************************************************************* */
// keys
TEST(Expression, SnavelyKeys) {
// Expression<Vector2> expression(1);
// set<Key> expected = list_of(1)(2);
// EXPECT(expected == expression.keys());
Expression<Vector2> expression(1);
set<Key> expected = list_of(1)(2);
EXPECT(expected == expression.keys());
}
/* ************************************************************************* */
int main() {