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

transform_to and between, with derivatives, unit-tested

release/4.3a0
Frank Dellaert 2009-12-08 19:12:20 +00:00
parent 2f1b176fe9
commit d1ab2c7288
3 changed files with 134 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

55
cpp/Pose2.cpp
View File

@ -40,6 +40,59 @@ namespace gtsam {
return Pose2(c * x_ - s * y_, s * x_ + c * y_, theta + theta_);
}
/* ************************************************************************* */
/* ************************************************************************* */
Point2 transform_to(const Pose2& pose, const Point2& point) {
double dx = point.x()-pose.x(), dy = point.y()-pose.y();
double ct=cos(pose.theta()), st=sin(pose.theta());
return Point2(ct*dx + st*dy, -st*dx + ct*dy);
}
// TODO, have a combined function that returns both function value and derivative
Matrix Dtransform_to1(const Pose2& pose, const Point2& point) {
double dx = point.x()-pose.x(), dy = point.y()-pose.y();
double ct=cos(pose.theta()), st=sin(pose.theta());
double transformed_x = ct*dx + st*dy, transformed_y = -st*dx + ct*dy;
return Matrix_(2,3,
-ct, -st, transformed_y,
st, -ct, -transformed_x
);
}
Matrix Dtransform_to2(const Pose2& pose, const Point2& point) {
double ct=cos(pose.theta()), st=sin(pose.theta());
return Matrix_(2,2,
ct, st,
-st, ct
);
}
/* ************************************************************************* */
Pose2 between(const Pose2& p1, const Pose2& p2) {
double dx = p2.x()-p1.x(), dy = p2.y()-p1.y();
double ct=cos(p1.theta()), st=sin(p1.theta());
return Pose2(ct*dx + st*dy, -st*dx + ct*dy, p2.theta()-p1.theta());
}
Matrix Dbetween1(const Pose2& p1, const Pose2& p2) {
double dx = p2.x()-p1.x(), dy = p2.y()-p1.y();
double ct=cos(p1.theta()), st=sin(p1.theta());
double transformed_x = ct*dx + st*dy, transformed_y = -st*dx + ct*dy;
return Matrix_(3,3,
-ct, -st, transformed_y,
st, -ct, -transformed_x,
0.0, 0.0, -1.0
);
}
Matrix Dbetween2(const Pose2& p1, const Pose2& p2) {
double ct=cos(p1.theta()), st=sin(p1.theta());
return Matrix_(3,3,
ct, st, 0.0,
-st, ct, 0.0,
0.0, 0.0, 1.0
);
}
/* ************************************************************************* */
} // namespace gtsam

16
cpp/Pose2.h
View File

@ -9,7 +9,7 @@
#pragma once
#include "Point2.h"
#include "Vector.h"
#include "Matrix.h"
#include "Testable.h"
namespace gtsam {
@ -84,4 +84,18 @@ namespace gtsam {
}
}; // Pose2
/**
* Return point coordinates in pose coordinate frame
*/
Point2 transform_to(const Pose2& pose, const Point2& point);
Matrix Dtransform_to1(const Pose2& pose, const Point2& point);
Matrix Dtransform_to2(const Pose2& pose, const Point2& point);
/**
* Return relative pose between p1 and p2, in p1 coordinate frame
*/
Pose2 between(const Pose2& p1, const Pose2& p2);
Matrix Dbetween1(const Pose2& p1, const Pose2& p2);
Matrix Dbetween2(const Pose2& p1, const Pose2& p2);
} // namespace gtsam

65
cpp/testPose2.cpp
View File

@ -3,8 +3,11 @@
* @brief Unit tests for Pose2 class
*/
#include <math.h>
#include <CppUnitLite/TestHarness.h>
#include "numericalDerivative.h"
#include "Pose2.h"
#include "Point2.h"
using namespace gtsam;
@ -30,6 +33,68 @@ TEST(Pose2, operators) {
CHECK(assert_equal(Pose2(0,0,0),Pose2(1,1,1)-Pose2(1,1,1)));
}
/* ************************************************************************* */
TEST( Pose2, transform_to )
{
Pose2 pose(1,2,M_PI_2); // robot at (1,2) looking towards y
Point2 point(-1,4); // landmark at (-1,4)
// expected
Point2 expected(2,2);
Matrix expectedH1 = Matrix_(2,3, 0.0, -1.0, 2.0, 1.0, 0.0, -2.0);
Matrix expectedH2 = Matrix_(2,2, 0.0, 1.0, -1.0, 0.0);
// actual
Point2 actual = transform_to(pose,point);
Matrix actualH1 = Dtransform_to1(pose,point);
Matrix actualH2 = Dtransform_to2(pose,point);
CHECK(assert_equal(expected,actual));
CHECK(assert_equal(expectedH1,actualH1));
CHECK(assert_equal(expectedH2,actualH2));
Matrix numericalH1 = numericalDerivative21(transform_to, pose, point, 1e-5);
CHECK(assert_equal(numericalH1,actualH1));
Matrix numericalH2 = numericalDerivative22(transform_to, pose, point, 1e-5);
CHECK(assert_equal(numericalH2,actualH2));
}
/* ************************************************************************* */
TEST( Pose2, between )
{
Pose2 p1(1,2,M_PI_2); // robot at (1,2) looking towards y
Pose2 p2(-1,4,M_PI); // robot at (-1,4) loooking at negative x
// expected
Pose2 expected(2,2,M_PI_2);
Matrix expectedH1 = Matrix_(3,3,
0.0,-1.0,2.0,
1.0,0.0,-2.0,
0.0,0.0,-1.0
);
Matrix expectedH2 = Matrix_(3,3,
0.0,1.0,0.0,
-1.0,0.0,0.0,
0.0,0.0,1.0
);
// actual
Pose2 actual = between(p1,p2);
Matrix actualH1 = Dbetween1(p1,p2);
Matrix actualH2 = Dbetween2(p1,p2);
CHECK(assert_equal(expected,actual));
CHECK(assert_equal(expectedH1,actualH1));
CHECK(assert_equal(expectedH2,actualH2));
Matrix numericalH1 = numericalDerivative21(between, p1, p2, 1e-5);
CHECK(assert_equal(numericalH1,actualH1));
Matrix numericalH2 = numericalDerivative22(between, p1, p2, 1e-5);
CHECK(assert_equal(numericalH2,actualH2));
}
/* ************************************************************************* */
int main() {
TestResult tr;