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

renamed transformPose_to to transform_to, composeTransform to compose, and changed nr. of arguments of Dtransform_to2 (to two)

release/4.3a0
Frank Dellaert 2009-12-18 05:36:53 +00:00
parent eaf27af92d
commit ac72d1cc22
4 changed files with 18 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
cpp/CalibratedCamera.cpp
View File

@ -71,8 +71,8 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
Matrix Dproject_point(const CalibratedCamera& camera, const Point3& point) { Matrix Dproject_point(const CalibratedCamera& camera, const Point3& point) {
Point3 cameraPoint = transform_to(camera.pose(), point); Point3 cameraPoint = transform_to(camera.pose(),point);
Matrix D_cameraPoint_point = Dtransform_to2(camera.pose()); Matrix D_cameraPoint_point = Dtransform_to2(camera.pose(),point);
Point2 intrinsic = project_to_camera(cameraPoint); Point2 intrinsic = project_to_camera(cameraPoint);
Matrix D_intrinsic_cameraPoint = Dproject_to_camera1(cameraPoint); Matrix D_intrinsic_cameraPoint = Dproject_to_camera1(cameraPoint);
@ -86,8 +86,8 @@ namespace gtsam {
Matrix& D_intrinsic_pose, Matrix& D_intrinsic_point) { Matrix& D_intrinsic_pose, Matrix& D_intrinsic_point) {
Point3 cameraPoint = transform_to(camera.pose(), point); Point3 cameraPoint = transform_to(camera.pose(), point);
Matrix D_cameraPoint_pose = Dtransform_to1(camera.pose(), point); // 3*6 Matrix D_cameraPoint_pose = Dtransform_to1(camera.pose(),point); // 3*6
Matrix D_cameraPoint_point = Dtransform_to2(camera.pose()); // 3*3 Matrix D_cameraPoint_point = Dtransform_to2(camera.pose(),point); // 3*3
Point2 intrinsic = project_to_camera(cameraPoint); Point2 intrinsic = project_to_camera(cameraPoint);
Matrix D_intrinsic_cameraPoint = Dproject_to_camera1(cameraPoint); Matrix D_intrinsic_cameraPoint = Dproject_to_camera1(cameraPoint);

7
cpp/Pose3.cpp
View File

@ -75,7 +75,7 @@ Matrix Dtransform_to1(const Pose3& pose, const Point3& p) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix Dtransform_to2(const Pose3& pose) { Matrix Dtransform_to2(const Pose3& pose, const Point3& p) {
return Dunrotate2(pose.rotation()); return Dunrotate2(pose.rotation());
} }
@ -106,10 +106,9 @@ Pose3 Pose3::inverse() const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Pose3 Pose3::transformPose_to(const Pose3& pose) const { Pose3 Pose3::transform_to(const Pose3& pose) const {
Rot3 cRv = R_ * Rot3(pose.R_.inverse()); Rot3 cRv = R_ * Rot3(pose.R_.inverse());
Point3 t = transform_to(pose, t_); Point3 t = gtsam::transform_to(pose, t_);
return Pose3(cRv, t); return Pose3(cRv, t);
} }

8
cpp/Pose3.h
View File

@ -102,12 +102,12 @@ namespace gtsam {
Matrix matrix() const; Matrix matrix() const;
/** transforms */ /** transforms */
Pose3 transformPose_to(const Pose3& transform) const; Pose3 transform_to(const Pose3& transform) const;
/** friends */ /** friends */
friend Point3 transform_from(const Pose3& pose, const Point3& p); friend Point3 transform_from(const Pose3& pose, const Point3& p);
friend Point3 transform_to(const Pose3& pose, const Point3& p); friend Point3 transform_to(const Pose3& pose, const Point3& p);
friend Pose3 composeTransform(const Pose3& current, const Pose3& target); friend Pose3 compose(const Pose3& current, const Pose3& target);
private: private:
/** Serialization function */ /** Serialization function */
@ -122,7 +122,7 @@ namespace gtsam {
/** /**
* Finds a transform from the current frame dTx to the target frame sTx * Finds a transform from the current frame dTx to the target frame sTx
*/ */
inline Pose3 composeTransform(const Pose3& dTx, const Pose3& sTx) { inline Pose3 compose(const Pose3& dTx, const Pose3& sTx) {
return dTx * sTx.inverse(); return dTx * sTx.inverse();
} }
@ -134,7 +134,7 @@ namespace gtsam {
/** receives the point in world coordinates and transforms it to Pose coordinates */ /** 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);
Matrix Dtransform_to1(const Pose3& pose, const Point3& p); Matrix Dtransform_to1(const Pose3& pose, const Point3& p);
Matrix Dtransform_to2(const Pose3& pose); // does not depend on p ! Matrix Dtransform_to2(const Pose3& pose, const Point3& p);
/** /**
* Return relative pose between p1 and p2, in p1 coordinate frame * Return relative pose between p1 and p2, in p1 coordinate frame

14
cpp/testPose3.cpp
View File

@ -93,7 +93,7 @@ TEST( Pose3, Dtransform_to1)
TEST( Pose3, Dtransform_to2) TEST( Pose3, Dtransform_to2)
{ {
Matrix computed = Dtransform_to2(T); Matrix computed = Dtransform_to2(T,P);
Matrix numerical = numericalDerivative22(transform_to,T,P); Matrix numerical = numericalDerivative22(transform_to,T,P);
CHECK(assert_equal(numerical,computed,error)); CHECK(assert_equal(numerical,computed,error));
} }
@ -139,13 +139,13 @@ TEST( Pose3, transform_roundtrip)
TEST( Pose3, transformPose_to_origin) TEST( Pose3, transformPose_to_origin)
{ {
// transform to origin // transform to origin
Pose3 actual = pose1.transformPose_to(Pose3()); Pose3 actual = pose1.transform_to(Pose3());
CHECK(assert_equal(pose1, actual, error)); CHECK(assert_equal(pose1, actual, error));
} }
TEST( Pose3, transformPose_to_itself) TEST( Pose3, transformPose_to_itself)
{ {
// transform to itself // transform to itself
Pose3 actual = pose1.transformPose_to(pose1); Pose3 actual = pose1.transform_to(pose1);
CHECK(assert_equal(Pose3(), actual, error)); CHECK(assert_equal(Pose3(), actual, error));
} }
TEST( Pose3, transformPose_to_translation) TEST( Pose3, transformPose_to_translation)
@ -153,7 +153,7 @@ TEST( Pose3, transformPose_to_translation)
// transform translation only // transform translation only
Rot3 r = rodriguez(-1.570796,0,0); Rot3 r = rodriguez(-1.570796,0,0);
Pose3 pose2(r, Point3(21.,32.,13.)); Pose3 pose2(r, Point3(21.,32.,13.));
Pose3 actual = pose2.transformPose_to(Pose3(Rot3(), Point3(1,2,3))); Pose3 actual = pose2.transform_to(Pose3(Rot3(), Point3(1,2,3)));
Pose3 expected(r, Point3(20.,30.,10.)); Pose3 expected(r, Point3(20.,30.,10.));
CHECK(assert_equal(expected, actual, error)); CHECK(assert_equal(expected, actual, error));
} }
@ -163,7 +163,7 @@ TEST( Pose3, transformPose_to_simple_rotate)
Rot3 r = rodriguez(0,0,-1.570796); Rot3 r = rodriguez(0,0,-1.570796);
Pose3 pose2(r, Point3(21.,32.,13.)); Pose3 pose2(r, Point3(21.,32.,13.));
Pose3 transform(r, Point3(1,2,3)); Pose3 transform(r, Point3(1,2,3));
Pose3 actual = pose2.transformPose_to(transform); Pose3 actual = pose2.transform_to(transform);
Pose3 expected(Rot3(), Point3(-30.,20.,10.)); Pose3 expected(Rot3(), Point3(-30.,20.,10.));
CHECK(assert_equal(expected, actual, 0.001)); CHECK(assert_equal(expected, actual, 0.001));
} }
@ -174,7 +174,7 @@ TEST( Pose3, transformPose_to)
Rot3 r2 = rodriguez(0,0,0.698131701); //40 degree yaw Rot3 r2 = rodriguez(0,0,0.698131701); //40 degree yaw
Pose3 pose2(r2, Point3(21.,32.,13.)); Pose3 pose2(r2, Point3(21.,32.,13.));
Pose3 transform(r, Point3(1,2,3)); Pose3 transform(r, Point3(1,2,3));
Pose3 actual = pose2.transformPose_to(transform); Pose3 actual = pose2.transform_to(transform);
Pose3 expected(rodriguez(0,0,2.26892803), Point3(-30.,20.,10.)); Pose3 expected(rodriguez(0,0,2.26892803), Point3(-30.,20.,10.));
CHECK(assert_equal(expected, actual, 0.001)); CHECK(assert_equal(expected, actual, 0.001));
} }
@ -194,7 +194,7 @@ TEST( Pose3, composeTransform )
Pose3 target(rodriguez(0,0,2.26892803), Point3(-30.,20.,10.)); Pose3 target(rodriguez(0,0,2.26892803), Point3(-30.,20.,10.));
// calculate transform // calculate transform
Pose3 actual = composeTransform(current, target); Pose3 actual = compose(current, target);
//verify //verify
CHECK(assert_equal(exp_transform, actual, 0.001)); CHECK(assert_equal(exp_transform, actual, 0.001));