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

Added some simple transform_[to|from] functions to Point2 to allow for linear systems to still use transforms in generic code

release/4.3a0
Alex Cunningham 2010-09-25 22:31:48 +00:00
parent c1fee1ab88
commit b6009028d8
3 changed files with 34 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
geometry/Point2.cpp
View File

@ -30,5 +30,19 @@ namespace gtsam {
}
/* ************************************************************************* */
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

11
geometry/Point2.h
View File

@ -41,6 +41,17 @@ namespace gtsam {
/** equals with an tolerance, prints out message if unequal*/
bool equals(const Point2& q, double tol = 1e-9) const;
/** simple transform_[to|from] to allow for generic algorithms */
/** Return point coordinates in pose coordinate frame */
Point2 transform_to(const Point2& point,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
/** Return point coordinates in global frame */
Point2 transform_from(const Point2& point,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
/** Lie requirements */
/** Size of the tangent space of the Lie type */

12
geometry/tests/testPoint2.cpp
View File

@ -42,8 +42,14 @@ TEST( Point2, norm)
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
TEST( Point2, transforms ) {
Point2 offset(3.0, 4.0);
EXPECT(assert_equal(Point2(5.0, 6.0), offset.transform_from(Point2(2.0, 2.0))));
EXPECT(assert_equal(Point2(-1.0, -2.0), offset.transform_to(Point2(2.0, 2.0))));
EXPECT(assert_equal(Point2(1.0, 2.0), offset.transform_to(
offset.transform_from(Point2(1.0, 2.0)))));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */