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

Merged in feature/OptionalJacobianBlocks (pull request #177) 

New OptionalJacobian functionality
release/4.3a0
Frank Dellaert 2015-07-19 20:20:53 -07:00
parent 07577189d9 2deac4b88f
commit 6058d045ae
4 changed files with 156 additions and 105 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
gtsam/base/OptionalJacobian.h
View File

@ -40,7 +40,8 @@ class OptionalJacobian {
public:
/// ::Jacobian size type
/// Jacobian size type
/// TODO(frank): how to enforce RowMajor? Or better, make it work with any storage order?
typedef Eigen::Matrix<double, Rows, Cols> Jacobian;
private:
@ -53,6 +54,14 @@ private:
new (&map_) Eigen::Map<Jacobian>(data);
}
// Private and very dangerous constructor straight from memory
OptionalJacobian(double* data) : map_(NULL) {
if (data) usurp(data);
}
template<int M, int N>
friend class OptionalJacobian;
public:
/// Default constructor acts like boost::none
@ -98,6 +107,11 @@ public:
#endif
/// Constructor that will usurp data of a block expression
/// TODO(frank): unfortunately using a Map makes usurping non-contiguous memory impossible
// template <typename Derived, bool InnerPanel>
// OptionalJacobian(Eigen::Block<Derived,Rows,Cols,InnerPanel> block) : map_(NULL) { ?? }
/// Return true is allocated, false if default constructor was used
operator bool() const {
return map_.data() != NULL;
@ -108,8 +122,36 @@ public:
return map_;
}
/// TODO: operator->()
Eigen::Map<Jacobian>* operator->(){ return &map_; }
/// operator->()
Eigen::Map<Jacobian>* operator->() {
return &map_;
}
/// Access M*N sub-block if we are allocated, otherwise none
/// TODO(frank): this could work as is below if only constructor above worked
// template <int M, int N>
// OptionalJacobian<M, N> block(int startRow, int startCol) {
// if (*this)
// OptionalJacobian<M, N>(map_.block<M, N>(startRow, startCol));
// else
// return OptionalJacobian<M, N>();
// }
/// Access Rows*N sub-block if we are allocated, otherwise return an empty OptionalJacobian
/// The use case is functions with arguments that are dissected, e.g. Pose3 into Rot3, Point3
/// TODO(frank): ideally, we'd like full block functionality, but see note above.
template <int N>
OptionalJacobian<Rows, N> cols(int startCol) {
if (*this)
return OptionalJacobian<Rows, N>(&map_(0,startCol));
else
return OptionalJacobian<Rows, N>();
}
/// Access M*Cols sub-block if we are allocated, otherwise return empty OptionalJacobian
/// The use case is functions that create their return value piecemeal by calling other functions
/// TODO(frank): Unfortunately we assume column-major storage order and hence this can't work
/// template <int M> OptionalJacobian<M, Cols> rows(int startRow) { ?? }
};
// The pure dynamic specialization of this is needed to support

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

@ -61,20 +61,15 @@ TEST( OptionalJacobian, Constructors ) {
}
//******************************************************************************
Matrix kTestMatrix = (Matrix23() << 11,12,13,21,22,23).finished();
void test(OptionalJacobian<2, 3> H = boost::none) {
if (H)
*H = Matrix23::Zero();
}
void testPtr(Matrix23* H = NULL) {
if (H)
*H = Matrix23::Zero();
*H = kTestMatrix;
}
TEST( OptionalJacobian, Fixed) {
Matrix expected = Matrix23::Zero();
// Default argument does nothing
test();
@ -82,61 +77,83 @@ TEST( OptionalJacobian, Fixed) {
Matrix23 fixed1;
fixed1.setOnes();
test(fixed1);
EXPECT(assert_equal(expected,fixed1));
EXPECT(assert_equal(kTestMatrix,fixed1));
// Fixed size, no copy, pointer style
Matrix23 fixed2;
fixed2.setOnes();
test(&fixed2);
EXPECT(assert_equal(expected,fixed2));
EXPECT(assert_equal(kTestMatrix,fixed2));
// Empty is no longer a sign we don't want a matrix, we want it resized
// Passing in an empty matrix means we want it resized
Matrix dynamic0;
test(dynamic0);
EXPECT(assert_equal(expected,dynamic0));
EXPECT(assert_equal(kTestMatrix,dynamic0));
// Dynamic wrong size
Matrix dynamic1(3, 5);
dynamic1.setOnes();
test(dynamic1);
EXPECT(assert_equal(expected,dynamic1));
EXPECT(assert_equal(kTestMatrix,dynamic1));
// Dynamic right size
Matrix dynamic2(2, 5);
dynamic2.setOnes();
test(dynamic2);
EXPECT(assert_equal(expected, dynamic2));
EXPECT(assert_equal(kTestMatrix, dynamic2));
}
//******************************************************************************
void test2(OptionalJacobian<-1,-1> H = boost::none) {
if (H)
*H = Matrix23::Zero(); // resizes
*H = kTestMatrix; // 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
// Passing in an empty matrix means we want it resized
Matrix dynamic0;
test2(dynamic0);
EXPECT(assert_equal(expected,dynamic0));
EXPECT(assert_equal(kTestMatrix,dynamic0));
// Dynamic wrong size
Matrix dynamic1(3, 5);
dynamic1.setOnes();
test2(dynamic1);
EXPECT(assert_equal(expected,dynamic1));
EXPECT(assert_equal(kTestMatrix,dynamic1));
// Dynamic right size
Matrix dynamic2(2, 5);
dynamic2.setOnes();
test2(dynamic2);
EXPECT(assert_equal(expected, dynamic2));
EXPECT(assert_equal(kTestMatrix, dynamic2));
}
//******************************************************************************
void test3(double add, OptionalJacobian<2,1> H = boost::none) {
if (H) *H << add + 10, add + 20;
}
// This function calls the above function three times, one for each column
void test4(OptionalJacobian<2, 3> H = boost::none) {
if (H) {
test3(1, H.cols<1>(0));
test3(2, H.cols<1>(1));
test3(3, H.cols<1>(2));
}
}
TEST(OptionalJacobian, Block) {
// Default argument does nothing
test4();
Matrix23 fixed;
fixed.setOnes();
test4(fixed);
EXPECT(assert_equal(kTestMatrix, fixed));
}
//******************************************************************************

97
gtsam/geometry/Pose2.cpp
View File

@ -201,95 +201,88 @@ Pose2 Pose2::inverse() const {
/* ************************************************************************* */
// see doc/math.lyx, SE(2) section
Point2 Pose2::transform_to(const Point2& point,
OptionalJacobian<2, 3> H1, OptionalJacobian<2, 2> H2) const {
Point2 d = point - t_;
Point2 q = r_.unrotate(d);
if (!H1 && !H2) return q;
if (H1) *H1 <<
-1.0, 0.0, q.y(),
0.0, -1.0, -q.x();
if (H2) *H2 << r_.transpose();
OptionalJacobian<2, 3> Hpose, OptionalJacobian<2, 2> Hpoint) const {
OptionalJacobian<2, 2> Htranslation = Hpose.cols<2>(0);
OptionalJacobian<2, 1> Hrotation = Hpose.cols<1>(2);
const Point2 q = r_.unrotate(point - t_, Hrotation, Hpoint);
if (Htranslation) *Htranslation << -1.0, 0.0, 0.0, -1.0;
return q;
}
/* ************************************************************************* */
// see doc/math.lyx, SE(2) section
Point2 Pose2::transform_from(const Point2& p,
OptionalJacobian<2, 3> H1, OptionalJacobian<2, 2> H2) const {
const Point2 q = r_ * p;
if (H1 || H2) {
const Matrix2 R = r_.matrix();
Matrix21 Drotate1;
Drotate1 << -q.y(), q.x();
if (H1) *H1 << R, Drotate1;
if (H2) *H2 = R; // R
}
Point2 Pose2::transform_from(const Point2& point,
OptionalJacobian<2, 3> Hpose, OptionalJacobian<2, 2> Hpoint) const {
OptionalJacobian<2, 2> Htranslation = Hpose.cols<2>(0);
OptionalJacobian<2, 1> Hrotation = Hpose.cols<1>(2);
const Point2 q = r_.rotate(point, Hrotation, Hpoint);
if (Htranslation) *Htranslation = Hpoint ? *Hpoint : r_.matrix();
return q + t_;
}
/* ************************************************************************* */
Rot2 Pose2::bearing(const Point2& point,
OptionalJacobian<1, 3> H1, OptionalJacobian<1, 2> H2) const {
OptionalJacobian<1, 3> Hpose, OptionalJacobian<1, 2> Hpoint) const {
// make temporary matrices
Matrix23 D1; Matrix2 D2;
Point2 d = transform_to(point, H1 ? &D1 : 0, H2 ? &D2 : 0); // uses pointer version
if (!H1 && !H2) return Rot2::relativeBearing(d);
Matrix23 D_d_pose; Matrix2 D_d_point;
Point2 d = transform_to(point, Hpose ? &D_d_pose : 0, Hpoint ? &D_d_point : 0);
if (!Hpose && !Hpoint) return Rot2::relativeBearing(d);
Matrix12 D_result_d;
Rot2 result = Rot2::relativeBearing(d, D_result_d);
if (H1) *H1 = D_result_d * (D1);
if (H2) *H2 = D_result_d * (D2);
Rot2 result = Rot2::relativeBearing(d, Hpose || Hpoint ? &D_result_d : 0);
if (Hpose) *Hpose = D_result_d * D_d_pose;
if (Hpoint) *Hpoint = D_result_d * D_d_point;
return result;
}
/* ************************************************************************* */
Rot2 Pose2::bearing(const Pose2& pose,
OptionalJacobian<1, 3> H1, OptionalJacobian<1, 3> H2) const {
OptionalJacobian<1, 3> Hpose, OptionalJacobian<1, 3> Hother) const {
Matrix12 D2;
Rot2 result = bearing(pose.t(), H1, H2 ? &D2 : 0);
if (H2) {
Rot2 result = bearing(pose.t(), Hpose, Hother ? &D2 : 0);
if (Hother) {
Matrix12 H2_ = D2 * pose.r().matrix();
*H2 << H2_, Z_1x1;
*Hother << H2_, Z_1x1;
}
return result;
}
/* ************************************************************************* */
double Pose2::range(const Point2& point,
OptionalJacobian<1,3> H1, OptionalJacobian<1,2> H2) const {
OptionalJacobian<1,3> Hpose, OptionalJacobian<1,2> Hpoint) const {
Point2 d = point - t_;
if (!H1 && !H2) return d.norm();
Matrix12 H;
double r = d.norm(H);
if (H1) {
Matrix23 H1_;
H1_ << -r_.c(), r_.s(), 0.0,
-r_.s(), -r_.c(), 0.0;
*H1 = H * H1_;
if (!Hpose && !Hpoint) return d.norm();
Matrix12 D_r_d;
double r = d.norm(D_r_d);
if (Hpose) {
Matrix23 D_d_pose;
D_d_pose << -r_.c(), r_.s(), 0.0,
-r_.s(), -r_.c(), 0.0;
*Hpose = D_r_d * D_d_pose;
}
if (H2) *H2 = H;
if (Hpoint) *Hpoint = D_r_d;
return r;
}
/* ************************************************************************* */
double Pose2::range(const Pose2& pose,
OptionalJacobian<1,3> H1,
OptionalJacobian<1,3> H2) const {
OptionalJacobian<1,3> Hpose,
OptionalJacobian<1,3> Hother) const {
Point2 d = pose.t() - t_;
if (!H1 && !H2) return d.norm();
Matrix12 H;
double r = d.norm(H);
if (H1) {
Matrix23 H1_;
H1_ <<
if (!Hpose && !Hother) return d.norm();
Matrix12 D_r_d;
double r = d.norm(D_r_d);
if (Hpose) {
Matrix23 D_d_pose;
D_d_pose <<
-r_.c(), r_.s(), 0.0,
-r_.s(), -r_.c(), 0.0;
*H1 = H * H1_;
*Hpose = D_r_d * D_d_pose;
}
if (H2) {
Matrix23 H2_;
H2_ <<
if (Hother) {
Matrix23 D_d_other;
D_d_other <<
pose.r_.c(), -pose.r_.s(), 0.0,
pose.r_.s(), pose.r_.c(), 0.0;
*H2 = H * H2_;
*Hother = D_r_d * D_d_other;
}
return r;
}

57
gtsam/geometry/Pose3.cpp
View File

@ -193,10 +193,10 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& T, ChartJacobian H) {
* The closed-form formula is similar to formula 102 in Barfoot14tro)
*/
static Matrix3 computeQforExpmapDerivative(const Vector6& xi) {
Vector3 w(sub(xi, 0, 3));
Vector3 v(sub(xi, 3, 6));
Matrix3 V = skewSymmetric(v);
Matrix3 W = skewSymmetric(w);
const Vector3 w = xi.head<3>();
const Vector3 v = xi.tail<3>();
const Matrix3 V = skewSymmetric(v);
const Matrix3 W = skewSymmetric(w);
Matrix3 Q;
@ -215,8 +215,8 @@ static Matrix3 computeQforExpmapDerivative(const Vector6& xi) {
// The closed-form formula in Barfoot14tro eq. (102)
double phi = w.norm();
if (fabs(phi)>1e-5) {
double sinPhi = sin(phi), cosPhi = cos(phi);
double phi2 = phi * phi, phi3 = phi2 * phi, phi4 = phi3 * phi, phi5 = phi4 * phi;
const double sinPhi = sin(phi), cosPhi = cos(phi);
const double phi2 = phi * phi, phi3 = phi2 * phi, phi4 = phi3 * phi, phi5 = phi4 * phi;
// Invert the sign of odd-order terms to have the right Jacobian
Q = -0.5*V + (phi-sinPhi)/phi3*(W*V + V*W - W*V*W)
+ (1-phi2/2-cosPhi)/phi4*(W*W*V + V*W*W - 3*W*V*W)
@ -234,40 +234,37 @@ static Matrix3 computeQforExpmapDerivative(const Vector6& xi) {
/* ************************************************************************* */
Matrix6 Pose3::ExpmapDerivative(const Vector6& xi) {
Vector3 w(sub(xi, 0, 3));
Matrix3 Jw = Rot3::ExpmapDerivative(w);
Matrix3 Q = computeQforExpmapDerivative(xi);
Matrix6 J = (Matrix(6,6) << Jw, Z_3x3, Q, Jw).finished();
const Vector3 w = xi.head<3>();
const Matrix3 Jw = Rot3::ExpmapDerivative(w);
const Matrix3 Q = computeQforExpmapDerivative(xi);
Matrix6 J;
J << Jw, Z_3x3, Q, Jw;
return J;
}
/* ************************************************************************* */
Matrix6 Pose3::LogmapDerivative(const Pose3& pose) {
Vector6 xi = Logmap(pose);
Vector3 w(sub(xi, 0, 3));
Matrix3 Jw = Rot3::LogmapDerivative(w);
Matrix3 Q = computeQforExpmapDerivative(xi);
Matrix3 Q2 = -Jw*Q*Jw;
Matrix6 J = (Matrix(6,6) << Jw, Z_3x3, Q2, Jw).finished();
const Vector6 xi = Logmap(pose);
const Vector3 w = xi.head<3>();
const Matrix3 Jw = Rot3::LogmapDerivative(w);
const Matrix3 Q = computeQforExpmapDerivative(xi);
const Matrix3 Q2 = -Jw*Q*Jw;
Matrix6 J;
J << Jw, Z_3x3, Q2, Jw;
return J;
}
/* ************************************************************************* */
const Point3& Pose3::translation(OptionalJacobian<3, 6> H) const {
if (H) {
*H << Z_3x3, rotation().matrix();
}
if (H) *H << Z_3x3, rotation().matrix();
return t_;
}
/* ************************************************************************* */
Matrix4 Pose3::matrix() const {
const Matrix3 R = R_.matrix();
const Vector3 T = t_.vector();
Matrix14 A14;
A14 << 0.0, 0.0, 0.0, 1.0;
static const Matrix14 A14 = (Matrix14() << 0.0, 0.0, 0.0, 1.0).finished();
Matrix4 mat;
mat << R, T, A14;
mat << R_.matrix(), t_.vector(), A14;
return mat;
}
@ -281,15 +278,17 @@ Pose3 Pose3::transform_to(const Pose3& pose) const {
/* ************************************************************************* */
Point3 Pose3::transform_from(const Point3& p, OptionalJacobian<3,6> Dpose,
OptionalJacobian<3,3> Dpoint) const {
// Only get matrix once, to avoid multiple allocations,
// as well as multiple conversions in the Quaternion case
const Matrix3 R = R_.matrix();
if (Dpose) {
const Matrix3 R = R_.matrix();
Matrix3 DR = R * skewSymmetric(-p.x(), -p.y(), -p.z());
(*Dpose) << DR, R;
Dpose->leftCols<3>() = R * skewSymmetric(-p.x(), -p.y(), -p.z());
Dpose->rightCols<3>() = R;
}
if (Dpoint) {
*Dpoint = R_.matrix();
*Dpoint = R;
}
return R_ * p + t_;
return Point3(R * p.vector()) + t_;
}
/* ************************************************************************* */