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

Cleaned up Pose2 derivatives and used OptionalJacobian::cols in some places

release/4.3a0
Frank Dellaert 2015-07-19 18:28:18 -07:00
parent f2df547d86
commit c3dfa3ab10
1 changed files with 45 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

97
gtsam/geometry/Pose2.cpp
View File

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