49 lines
1.4 KiB
C++
49 lines
1.4 KiB
C++
/**
|
|
* @file Point2.cpp
|
|
* @brief 2D Point
|
|
* @author Frank Dellaert
|
|
*/
|
|
|
|
#include <gtsam/geometry/Point2.h>
|
|
#include <gtsam/base/Lie-inl.h>
|
|
|
|
using namespace std;
|
|
|
|
namespace gtsam {
|
|
|
|
/** Explicit instantiation of base class to export members */
|
|
INSTANTIATE_LIE(Point2);
|
|
|
|
/* ************************************************************************* */
|
|
void Point2::print(const string& s) const {
|
|
cout << s << "(" << x_ << ", " << y_ << ")" << endl;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
bool Point2::equals(const Point2& q, double tol) const {
|
|
return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
double Point2::norm() const {
|
|
return sqrt(x_*x_ + y_*y_);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Point2 Point2::transform_to(const Point2& point,
|
|
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
|
if (H1) *H1 = -eye(2);
|
|
if (H2) *H2 = eye(2);
|
|
return point - *this;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Point2 Point2::transform_from(const Point2& point,
|
|
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
|
if (H1) *H1 = eye(2);
|
|
if (H2) *H2 = eye(2);
|
|
return point + *this;
|
|
}
|
|
|
|
} // namespace gtsam
|