예제 #1
0
/* ************************************************************************* */
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);
}
예제 #2
0
/* ************************************************************************* */
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;
}
예제 #3
0
/* ************************************************************************* */
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);
    }
}
예제 #4
0
/* ************************************************************************* */
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;
}