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

Added version of transform_from and transform_to to Pose3 with optional derivatives

release/4.3a0
Alex Cunningham 2010-05-18 14:51:09 +00:00
parent 1c0dcbc438
commit fa954ab55e
3 changed files with 44 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
cpp/Pose3.cpp
View File

@ -142,6 +142,14 @@ namespace gtsam {
return pose.rotation() * p + pose.translation();
}
/* ************************************************************************* */
Point3 transform_from(const Pose3& pose, const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
if (H1) *H1 = Dtransform_from1(pose, p);
if (H2) *H2 = Dtransform_from2(pose);
return transform_from(pose, p);
}
/* ************************************************************************* */
Matrix Dtransform_from1(const Pose3& pose, const Point3& p) {
#ifdef CORRECT_POSE3_EXMAP
@ -165,6 +173,14 @@ namespace gtsam {
return unrotate(pose.rotation(), sub);
}
/* ************************************************************************* */
Point3 transform_to(const Pose3& pose, const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
if (H1) *H1 = Dtransform_to1(pose, p);
if (H2) *H2 = Dtransform_to2(pose, p);
return transform_to(pose, p);
}
/* ************************************************************************* */
// see math.lyx, derivative of action
Matrix Dtransform_to1(const Pose3& pose, const Point3& p) {

6
cpp/Pose3.h
View File

@ -117,11 +117,17 @@ namespace gtsam {
/** receives the point in Pose coordinates and transforms it to world coordinates */
Point3 transform_from(const Pose3& pose, const Point3& p);
inline Point3 operator*(const Pose3& pose, const Point3& p) { return transform_from(pose, p); }
Point3 transform_from(const Pose3& pose, const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
// Older transform functions
Matrix Dtransform_from1(const Pose3& pose, const Point3& p);
Matrix Dtransform_from2(const Pose3& pose); // does not depend on p !
/** receives the point in world coordinates and transforms it to Pose coordinates */
Point3 transform_to(const Pose3& pose, const Point3& p);
Point3 transform_to(const Pose3& pose, const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
Matrix Dtransform_to1(const Pose3& pose, const Point3& p);
Matrix Dtransform_to2(const Pose3& pose, const Point3& p);

22
cpp/testPose3.cpp
View File

@ -271,6 +271,28 @@ TEST( Pose3, Dtransform_to2)
CHECK(assert_equal(numerical,computed,1e-8));
}
/* ************************************************************************* */
TEST( Pose3, transform_to_with_derivatives)
{
Matrix actH1, actH2;
transform_to(T,P,actH1,actH2);
Matrix expH1 = numericalDerivative21(transform_to, T,P),
expH2 = numericalDerivative22(transform_to, T,P);
CHECK(assert_equal(expH1, actH1, 1e-8));
CHECK(assert_equal(expH2, actH2, 1e-8));
}
/* ************************************************************************* */
TEST( Pose3, transform_from_with_derivatives)
{
Matrix actH1, actH2;
transform_from(T,P,actH1,actH2);
Matrix expH1 = numericalDerivative21(transform_from, T,P),
expH2 = numericalDerivative22(transform_from, T,P);
CHECK(assert_equal(expH1, actH1, 1e-8));
CHECK(assert_equal(expH2, actH2, 1e-8));
}
/* ************************************************************************* */
TEST( Pose3, transform_to_translate)
{