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

Cleaned up and tested dynamic version

release/4.3a0
dellaert 2014-12-23 14:54:42 +01:00
parent 36f8510d7d
commit 6d05069ef7
2 changed files with 79 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

86
gtsam/base/OptionalJacobian.h
View File

@ -11,7 +11,7 @@
/**
* @file OptionalJacobian.h
* @brief Special class for optional Matrix arguments
* @brief Special class for optional Jacobian arguments
* @author Frank Dellaert
* @author Natesh Srinivasan
* @date Nov 28, 2014
@ -33,23 +33,24 @@ namespace gtsam {
* matrix will be resized. Finally, there is a constructor that takes
* boost::none, the default constructor acts like boost::none, and
* boost::optional<Eigen::MatrixXd&> is also supported for backwards compatibility.
* Below this class, a dynamic version is also implemented.
*/
template<int Rows, int Cols>
class OptionalJacobian {
public:
/// ::Fixed size type
typedef Eigen::Matrix<double, Rows, Cols> Matrix;
/// ::Jacobian size type
typedef Eigen::Matrix<double, Rows, Cols> Jacobian;
private:
Eigen::Map<Matrix> map_; /// View on constructor argument, if given
Eigen::Map<Jacobian> 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<Matrix>(data, rows, cols);
void usurp(double* data) {
new (&map_) Eigen::Map<Jacobian>(data);
}
public:
@ -60,24 +61,23 @@ public:
}
/// Constructor that will usurp data of a fixed-size matrix
OptionalJacobian(Matrix& fixed) :
OptionalJacobian(Jacobian& fixed) :
map_(NULL) {
usurp(fixed.data(), fixed.rows(), fixed.cols());
usurp(fixed.data());
}
/// Constructor that will usurp data of a fixed-size matrix, pointer version
OptionalJacobian(Matrix* fixedPtr) :
OptionalJacobian(Jacobian* fixedPtr) :
map_(NULL) {
if (fixedPtr)
usurp(fixedPtr->data(), fixedPtr->rows(), fixedPtr->cols());
usurp(fixedPtr->data());
}
// Disabled to deal with dynamic types.
/// Constructor that will resize a dynamic matrix (unless already correct)
OptionalJacobian(Eigen::MatrixXd& dynamic) :
map_(NULL) {
dynamic.resize(Rows, Cols); // no malloc if correct size
usurp(dynamic.data(), dynamic.rows(), dynamic.cols());
usurp(dynamic.data());
}
#ifndef OPTIONALJACOBIAN_NOBOOST
@ -92,7 +92,7 @@ public:
map_(NULL) {
if (optional) {
optional->resize(Rows, Cols);
usurp(optional->data(), optional->rows(), optional->cols());
usurp(optional->data());
}
}
@ -104,91 +104,69 @@ public:
}
/// De-reference, like boost optional
Eigen::Map<Matrix>& operator*() {
Eigen::Map<Jacobian>& operator*() {
return map_;
}
/// TODO: operator->()
Eigen::Map<Matrix>* operator->(){ return &map_; }
Eigen::Map<Jacobian>* operator->(){ return &map_; }
};
// The pure dynamic specialization of this is needed to support
// variable-sized types.
// variable-sized types. Note that this is designed to work like the
// boost optional scheme from GTSAM 3.
template<>
class OptionalJacobian<-1, -1> {
class OptionalJacobian<Eigen::Dynamic, Eigen::Dynamic> {
public:
/// Fixed size type
typedef Eigen::Matrix<double, -1, -1> Fixed;
/// Jacobian size type
typedef Eigen::MatrixXd Jacobian;
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);
}
Jacobian* pointer_; /// View on constructor argument, if given
public:
/// Default constructor acts like boost::none
OptionalJacobian() :
map_(NULL, 0, 0) {
pointer_(NULL) {
}
/// 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());
/// Constructor that will resize a dynamic matrix (unless already correct)
OptionalJacobian(Eigen::MatrixXd& dynamic) :
pointer_(&dynamic) {
}
#ifndef OPTIONALJACOBIAN_NOBOOST
/// Constructor with boost::none just makes empty
OptionalJacobian(boost::none_t none) :
map_(NULL, 0, 0) {
pointer_(NULL) {
}
/// 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());
}
pointer_(NULL) {
if (optional) pointer_ = &(*optional);
}
#endif
/// Return true is allocated, false if default constructor was used
operator bool() const {
return map_.data();
return pointer_;
}
/// De-reference, like boost optional
Eigen::Map<Fixed>& operator*() {
return map_;
Jacobian& operator*() {
return *pointer_;
}
/// TODO: operator->()
Eigen::Map<Fixed>* operator->(){ return &map_; }
Jacobian* operator->(){ return pointer_; }
};
//// 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

52
gtsam/base/tests/testOptionalJacobian.cpp
View File

@ -46,6 +46,18 @@ TEST( OptionalJacobian, Constructors ) {
boost::optional<Matrix&> optional(dynamic);
OptionalJacobian<2, 3> H6(optional);
EXPECT(H6);
OptionalJacobian<-1, -1> H7;
EXPECT(!H7);
OptionalJacobian<-1, -1> H8(dynamic);
EXPECT(H8);
OptionalJacobian<-1, -1> H9(boost::none);
EXPECT(!H9);
OptionalJacobian<-1, -1> H10(optional);
EXPECT(H10);
}
//******************************************************************************
@ -59,10 +71,9 @@ void testPtr(Matrix23* H = NULL) {
*H = Matrix23::Zero();
}
TEST( OptionalJacobian, Ref2) {
TEST( OptionalJacobian, Fixed) {
Matrix expected;
expected = Matrix23::Zero();
Matrix expected = Matrix23::Zero();
// Default argument does nothing
test();
@ -88,13 +99,44 @@ TEST( OptionalJacobian, Ref2) {
Matrix dynamic1(3, 5);
dynamic1.setOnes();
test(dynamic1);
EXPECT(assert_equal(expected,dynamic0));
EXPECT(assert_equal(expected,dynamic1));
// Dynamic right size
Matrix dynamic2(2, 5);
dynamic2.setOnes();
test(dynamic2);
EXPECT(assert_equal(dynamic2,dynamic0));
EXPECT(assert_equal(expected, dynamic2));
}
//******************************************************************************
void test2(OptionalJacobian<-1,-1> H = boost::none) {
if (H)
*H = Matrix23::Zero(); // resizes
}
TEST( OptionalJacobian, Dynamic) {
Matrix expected = Matrix23::Zero();
// Default argument does nothing
test2();
// Empty is no longer a sign we don't want a matrix, we want it resized
Matrix dynamic0;
test2(dynamic0);
EXPECT(assert_equal(expected,dynamic0));
// Dynamic wrong size
Matrix dynamic1(3, 5);
dynamic1.setOnes();
test2(dynamic1);
EXPECT(assert_equal(expected,dynamic1));
// Dynamic right size
Matrix dynamic2(2, 5);
dynamic2.setOnes();
test2(dynamic2);
EXPECT(assert_equal(expected, dynamic2));
}
//******************************************************************************