C++ (Cpp) Point2::norm Examples

Example #1

File: Pose2.cpp Project: rubinasultan/gtsam

/* ************************************************************************* */
double Pose2::range(const Point2& point,
                    boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
    Point2 d = point - 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;
    return r;
}