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) {
Point3 cameraPoint = transform_to(camera.pose(), point);
Matrix D_cameraPoint_point = Dtransform_to2(camera.pose());
Point3 cameraPoint = transform_to(camera.pose(),point);
Matrix D_cameraPoint_point = Dtransform_to2(camera.pose(),point);
Point2 intrinsic = project_to_camera(cameraPoint);
Matrix D_intrinsic_cameraPoint = Dproject_to_camera1(cameraPoint);
@ -86,8 +86,8 @@ namespace gtsam {
Matrix& D_intrinsic_pose, Matrix& D_intrinsic_point) {
Point3 cameraPoint = transform_to(camera.pose(), point);
Matrix D_cameraPoint_pose = Dtransform_to1(camera.pose(), point); // 3*6
Matrix D_cameraPoint_point = Dtransform_to2(camera.pose()); // 3*3
Matrix D_cameraPoint_pose = Dtransform_to1(camera.pose(),point); // 3*6
Matrix D_cameraPoint_point = Dtransform_to2(camera.pose(),point); // 3*3
Point2 intrinsic = project_to_camera(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());
}
@ -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());
Point3 t = transform_to(pose, t_);
Point3 t = gtsam::transform_to(pose, t_);
return Pose3(cRv, t);
}

8
cpp/Pose3.h
View File

@ -102,12 +102,12 @@ namespace gtsam {
Matrix matrix() const;
/** transforms */
Pose3 transformPose_to(const Pose3& transform) const;
Pose3 transform_to(const Pose3& transform) const;
/** friends */
friend Point3 transform_from(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:
/** Serialization function */
@ -122,7 +122,7 @@ namespace gtsam {
/**
* 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();
}
@ -134,7 +134,7 @@ namespace gtsam {
/** receives the point in world coordinates and transforms it to Pose coordinates */
Point3 transform_to(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

14
cpp/testPose3.cpp
View File

@ -93,7 +93,7 @@ TEST( Pose3, Dtransform_to1)
TEST( Pose3, Dtransform_to2)
{
Matrix computed = Dtransform_to2(T);
Matrix computed = Dtransform_to2(T,P);
Matrix numerical = numericalDerivative22(transform_to,T,P);
CHECK(assert_equal(numerical,computed,error));
}
@ -139,13 +139,13 @@ TEST( Pose3, transform_roundtrip)
TEST( Pose3, transformPose_to_origin)
{
// transform to origin
Pose3 actual = pose1.transformPose_to(Pose3());
Pose3 actual = pose1.transform_to(Pose3());
CHECK(assert_equal(pose1, actual, error));
}
TEST( Pose3, transformPose_to_itself)
{
// transform to itself
Pose3 actual = pose1.transformPose_to(pose1);
Pose3 actual = pose1.transform_to(pose1);
CHECK(assert_equal(Pose3(), actual, error));
}
TEST( Pose3, transformPose_to_translation)
@ -153,7 +153,7 @@ TEST( Pose3, transformPose_to_translation)
// transform translation only
Rot3 r = rodriguez(-1.570796,0,0);
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.));
CHECK(assert_equal(expected, actual, error));
}
@ -163,7 +163,7 @@ TEST( Pose3, transformPose_to_simple_rotate)
Rot3 r = rodriguez(0,0,-1.570796);
Pose3 pose2(r, Point3(21.,32.,13.));
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.));
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
Pose3 pose2(r2, Point3(21.,32.,13.));
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.));
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.));
// calculate transform
Pose3 actual = composeTransform(current, target);
Pose3 actual = compose(current, target);
//verify
CHECK(assert_equal(exp_transform, actual, 0.001));