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

combined evaluation and derivatives

release/4.3a0
Frank Dellaert 2010-01-12 02:08:41 +00:00
parent 2c8d8dbde4
commit 1aed18717b
3 changed files with 22 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
cpp/Pose2.cpp
View File

@ -24,15 +24,25 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
// TODO, have a combined function that returns both function value and derivative Point2 transform_to(const Pose2& pose, const Point2& point, boost::optional<
Matrix&> H1, boost::optional<Matrix&> H2) {
const Rot2& R = pose.r();
Point2 d = point - pose.t();
Point2 q = R.unrotate(d);
if (!H1 && !H2) return q;
if (H1) *H1 = Matrix_(2, 3,
-1.0, 0.0, q.y(),
0.0, -1.0, -q.x());
if (H2) *H2 = R.transpose();
return q;
}
Matrix Dtransform_to1(const Pose2& pose, const Point2& point) { Matrix Dtransform_to1(const Pose2& pose, const Point2& point) {
Matrix dx_dt = Matrix_(2,2, -1.0, 0.0, 0.0, -1.0); Matrix H; transform_to(pose, point, H, boost::none); return H;
Matrix dx_dr = Dunrotate1(pose.r(), point-pose.t());
return collect(2, &dx_dt, &dx_dr);
} }
Matrix Dtransform_to2(const Pose2& pose, const Point2& point) { Matrix Dtransform_to2(const Pose2& pose, const Point2& point) {
return pose.r().transpose(); Matrix H; transform_to(pose, point, boost::none, H); return H;
} }
/* ************************************************************************* */ /* ************************************************************************* */

3
cpp/Pose2.h
View File

@ -9,6 +9,7 @@
#pragma once #pragma once
#include <boost/optional.hpp>
#include "Matrix.h" #include "Matrix.h"
#include "Testable.h" #include "Testable.h"
#include "Lie.h" #include "Lie.h"
@ -87,6 +88,8 @@ namespace gtsam {
/** Return point coordinates in pose coordinate frame */ /** Return point coordinates in pose coordinate frame */
inline Point2 transform_to(const Pose2& pose, const Point2& point) { inline Point2 transform_to(const Pose2& pose, const Point2& point) {
return unrotate(pose.r(), point-pose.t()); } return unrotate(pose.r(), point-pose.t()); }
Point2 transform_to(const Pose2& pose, const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
Matrix Dtransform_to1(const Pose2& pose, const Point2& point); Matrix Dtransform_to1(const Pose2& pose, const Point2& point);
Matrix Dtransform_to2(const Pose2& pose, const Point2& point); Matrix Dtransform_to2(const Pose2& pose, const Point2& point);

13
cpp/testPose2.cpp
View File

@ -65,15 +65,6 @@ TEST(Pose2, logmap) {
CHECK(assert_equal(expected, actual)); CHECK(assert_equal(expected, actual));
} }
/* ************************************************************************* */
//TEST(Pose2, rotate) {
// std::cout << "rotate\n";
// double theta = 0.1, c=cos(theta),s=sin(theta);
// Pose2 p1(1,0,0.2), p2(0,1,0.4);
// CHECK(assert_equal(Pose2( c,s,0.3),p1.rotate(theta)));
// CHECK(assert_equal(Pose2(-s,c,0.5),p2.rotate(theta)));
//}
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Pose2, transform_to ) TEST( Pose2, transform_to )
{ {
@ -100,6 +91,10 @@ TEST( Pose2, transform_to )
Matrix numericalH2 = numericalDerivative22(transform_to, pose, point, 1e-5); Matrix numericalH2 = numericalDerivative22(transform_to, pose, point, 1e-5);
CHECK(assert_equal(numericalH2,actualH2)); CHECK(assert_equal(numericalH2,actualH2));
transform_to(pose,point,actualH1,actualH2);
CHECK(assert_equal(numericalH1,actualH1));
CHECK(assert_equal(numericalH2,actualH2));
} }
/* ************************************************************************* */ /* ************************************************************************* */