/* ************************************************************************* */ Pose2 Pose2betweenOptimized(const Pose2& r1, const Pose2& r2, boost::optional<Matrix3&> H1, boost::optional<Matrix3&> H2) { // get cosines and sines from rotation matrices const Rot2& R1 = r1.r(), R2 = r2.r(); double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s(); // Assert that R1 and R2 are normalized assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5); // Calculate delta rotation = between(R1,R2) double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2; Rot2 R(Rot2::atan2(s,c)); // normalizes // Calculate delta translation = unrotate(R1, dt); Point2 dt = r2.t() - r1.t(); double x = dt.x(), y = dt.y(); Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y); // FD: This is just -AdjointMap(between(p2,p1)) inlined and re-using above if (H1) { double dt1 = -s2 * x + c2 * y; double dt2 = -c2 * x - s2 * y; *H1 = Matrix3(); (*H1) << -c, -s, dt1, s, -c, dt2, 0.0, 0.0,-1.0; } if (H2) *H2 = Matrix3::Identity(); return Pose2(R,t); }
/* ************************************************************************* */ Rot2 Pose2::bearing(const Pose2& point, boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { Rot2 result = bearing(point.t(), H1, H2); if (H2) { Matrix H2_ = *H2 * point.r().matrix(); *H2 = zeros(1, 3); insertSub(*H2, H2_, 0, 0); } return result; }
/* ************************************************************************* */ Vector Pose2::Logmap(const Pose2& p) { const Rot2& R = p.r(); const Point2& t = p.t(); double w = R.theta(); if (std::abs(w) < 1e-10) return (Vector(3) << t.x(), t.y(), w); else { double c_1 = R.c()-1.0, s = R.s(); double det = c_1*c_1 + s*s; Point2 p = R_PI_2 * (R.unrotate(t) - t); Point2 v = (w/det) * p; return (Vector(3) << v.x(), v.y(), w); } }
/* ************************************************************************* */ double Pose2::range(const Pose2& pose2, boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { Point2 d = pose2.t() - t_; if (!H1 && !H2) return d.norm(); Matrix H; double r = d.norm(H); if (H1) *H1 = H * (Matrix(2, 3) << -r_.c(), r_.s(), 0.0, -r_.s(), -r_.c(), 0.0); if (H2) *H2 = H * (Matrix(2, 3) << pose2.r_.c(), -pose2.r_.s(), 0.0, pose2.r_.s(), pose2.r_.c(), 0.0); return r; }