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

fixed bug in derivatives in transform_from

release/4.3a0
Alex Cunningham 2010-09-27 12:34:01 +00:00
parent b6009028d8
commit 87465cb04b
2 changed files with 37 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
geometry/Point2.cpp
View File

@ -40,7 +40,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
Point2 Point2::transform_from(const Point2& point, Point2 Point2::transform_from(const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) *H1 = eye(2); if (H1) *H1 = -eye(2);
if (H2) *H2 = eye(2); if (H2) *H2 = eye(2);
return point + *this; return point + *this;
} }

37
geometry/tests/testPoint2.cpp
View File

@ -5,11 +5,14 @@
**/ **/
#include <gtsam/CppUnitLite/TestHarness.h> #include <gtsam/CppUnitLite/TestHarness.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Point2.h> #include <gtsam/geometry/Point2.h>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
const double tol = 1e-5;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Point2, expmap) TEST( Point2, expmap)
{ {
@ -41,15 +44,47 @@ TEST( Point2, norm)
DOUBLES_EQUAL( 5,(p2-p1).norm(),1e-6); DOUBLES_EQUAL( 5,(p2-p1).norm(),1e-6);
} }
/* ************************************************************************* */
Point2 transform_from_proxy(const Point2& pose, const Point2& point) {
return pose.transform_to(point);
}
/* ************************************************************************* */
Point2 transform_to_proxy(const Point2& pose, const Point2& point) {
return pose.transform_to(point);
}
Point2 offset(3.0, 4.0), pt(5.0, 6.0);
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Point2, transforms ) { 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(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(Point2(2.0, 2.0))));
EXPECT(assert_equal(Point2(1.0, 2.0), offset.transform_to( EXPECT(assert_equal(Point2(1.0, 2.0), offset.transform_to(
offset.transform_from(Point2(1.0, 2.0))))); offset.transform_from(Point2(1.0, 2.0)))));
} }
/* ************************************************************************* */
TEST( Point2, transform_to_derivatives ) {
Matrix actH1, actH2;
offset.transform_to(pt, actH1, actH2);
Matrix numericalH1 = numericalDerivative21(transform_to_proxy, offset, pt, 1e-5);
Matrix numericalH2 = numericalDerivative22(transform_to_proxy, offset, pt, 1e-5);
EXPECT(assert_equal(numericalH1, actH1, tol));
EXPECT(assert_equal(numericalH2, actH2, tol));
}
/* ************************************************************************* */
TEST( Point2, transform_from_derivatives ) {
Matrix actH1, actH2;
offset.transform_from(pt, actH1, actH2);
Matrix numericalH1 = numericalDerivative21(transform_from_proxy, offset, pt, 1e-5);
Matrix numericalH2 = numericalDerivative22(transform_from_proxy, offset, pt, 1e-5);
EXPECT(assert_equal(numericalH1, actH1, tol));
EXPECT(assert_equal(numericalH2, actH2, tol));
}
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); } int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */ /* ************************************************************************* */