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

Merge pull request #884 from borglab/feature/OptionalJacobian/dynamic_pointer 

Add constructor for OptionalJacobian
release/4.3a0
Gerry Chen 2021-10-04 09:49:43 -04:00 committed by GitHub
parent d2bb299507 658effa999
commit e08800c4dc
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 45 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
gtsam/base/OptionalJacobian.h
View File

@ -89,6 +89,13 @@ public:
usurp(dynamic.data());
}
/// Constructor that will resize a dynamic matrix (unless already correct)
OptionalJacobian(Eigen::MatrixXd* dynamic) :
map_(nullptr) {
dynamic->resize(Rows, Cols); // no malloc if correct size
usurp(dynamic->data());
}
#ifndef OPTIONALJACOBIAN_NOBOOST
/// Constructor with boost::none just makes empty

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

@ -24,40 +24,33 @@ using namespace std;
using namespace gtsam;
//******************************************************************************
#define TEST_CONSTRUCTOR(DIM1, DIM2, X, TRUTHY) \
{ \
OptionalJacobian<DIM1, DIM2> H(X); \
EXPECT(H == TRUTHY); \
}
TEST( OptionalJacobian, Constructors ) {
Matrix23 fixed;
OptionalJacobian<2, 3> H1;
EXPECT(!H1);
OptionalJacobian<2, 3> H2(fixed);
EXPECT(H2);
OptionalJacobian<2, 3> H3(&fixed);
EXPECT(H3);
Matrix dynamic;
OptionalJacobian<2, 3> H4(dynamic);
EXPECT(H4);
OptionalJacobian<2, 3> H5(boost::none);
EXPECT(!H5);
boost::optional<Matrix&> optional(dynamic);
OptionalJacobian<2, 3> H6(optional);
EXPECT(H6);
OptionalJacobian<2, 3> H;
EXPECT(!H);
TEST_CONSTRUCTOR(2, 3, fixed, true);
TEST_CONSTRUCTOR(2, 3, &fixed, true);
TEST_CONSTRUCTOR(2, 3, dynamic, true);
TEST_CONSTRUCTOR(2, 3, &dynamic, true);
TEST_CONSTRUCTOR(2, 3, boost::none, false);
TEST_CONSTRUCTOR(2, 3, optional, true);
// Test dynamic
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);
TEST_CONSTRUCTOR(-1, -1, dynamic, true);
TEST_CONSTRUCTOR(-1, -1, boost::none, false);
TEST_CONSTRUCTOR(-1, -1, optional, true);
}
//******************************************************************************
@ -101,6 +94,25 @@ TEST( OptionalJacobian, Fixed) {
dynamic2.setOnes();
test(dynamic2);
EXPECT(assert_equal(kTestMatrix, dynamic2));
{ // Dynamic pointer
// Passing in an empty matrix means we want it resized
Matrix dynamic0;
test(&dynamic0);
EXPECT(assert_equal(kTestMatrix, dynamic0));
// Dynamic wrong size
Matrix dynamic1(3, 5);
dynamic1.setOnes();
test(&dynamic1);
EXPECT(assert_equal(kTestMatrix, dynamic1));
// Dynamic right size
Matrix dynamic2(2, 5);
dynamic2.setOnes();
test(&dynamic2);
EXPECT(assert_equal(kTestMatrix, dynamic2));
}
}
//******************************************************************************