diff --git a/gtsam/base/OptionalJacobian.h b/gtsam/base/OptionalJacobian.h index c0ac8cd7f..eb1c1bbcc 100644 --- a/gtsam/base/OptionalJacobian.h +++ b/gtsam/base/OptionalJacobian.h @@ -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 Jacobian; private: @@ -53,6 +54,14 @@ private: new (&map_) Eigen::Map(data); } + // Private and very dangerous constructor straight from memory + OptionalJacobian(double* data) : map_(NULL) { + if (data) usurp(data); + } + + template + 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 + // OptionalJacobian(Eigen::Block 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* operator->(){ return &map_; } + /// operator->() + Eigen::Map* 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 + // OptionalJacobian block(int startRow, int startCol) { + // if (*this) + // OptionalJacobian(map_.block(startRow, startCol)); + // else + // return OptionalJacobian(); + // } + + /// 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 + OptionalJacobian cols(int startCol) { + if (*this) + return OptionalJacobian(&map_(0,startCol)); + else + return OptionalJacobian(); + } + + /// 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 OptionalJacobian rows(int startRow) { ?? } }; // The pure dynamic specialization of this is needed to support diff --git a/gtsam/base/tests/testOptionalJacobian.cpp b/gtsam/base/tests/testOptionalJacobian.cpp index 331fb49eb..de1c07dcf 100644 --- a/gtsam/base/tests/testOptionalJacobian.cpp +++ b/gtsam/base/tests/testOptionalJacobian.cpp @@ -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)); } //****************************************************************************** diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 9e87407f4..89f6b3754 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -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; } diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index ac08f0797..3f46df40d 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -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_; } /* ************************************************************************* */