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

Just refactoring

release/4.3a0
Frank Dellaert 2010-01-08 03:38:05 +00:00
parent f684becf1f
commit 8d22b3aca2
2 changed files with 31 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
cpp/Pose3.cpp
View File

@ -130,13 +130,8 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
/* // compose = Pose3(compose(R1,R2),transform_from(p1,t2);
Pose3 compose(const Pose3& p1, const Pose3& p2) {
return Pose3(compose(p1.rotation(),p2.rotation()),transform_from(p1,p2.translation());
}
*/
/* ************************************************************************* */
Matrix Dcompose1(const Pose3& p1, const Pose3& p2) { Matrix Dcompose1(const Pose3& p1, const Pose3& p2) {
Matrix DR_R1 = eye(3); Matrix DR_R1 = eye(3);
Matrix DR_t1 = zeros(3, 3); Matrix DR_t1 = zeros(3, 3);
@ -145,19 +140,15 @@ namespace gtsam {
return stack(2, &DR, &Dt); return stack(2, &DR, &Dt);
} }
/* ************************************************************************* */
Matrix Dcompose2(const Pose3& p1, const Pose3& p2) { Matrix Dcompose2(const Pose3& p1, const Pose3& p2) {
Matrix DR_R2 = p1.rotation().matrix(); Matrix R1 = p1.rotation().matrix(), Z3 = zeros(3, 3);
Matrix DR_t2 = zeros(3,3); Matrix DR = collect(2, &R1, &Z3);
Matrix DR = collect(2,&DR_R2,&DR_t2); Matrix Dt = collect(2, &Z3, &R1);
Matrix Dt_R2 = zeros(3,3);
Matrix Dt_t2 = Dtransform_from2(p1);
Matrix Dt = collect(2,&Dt_R2,&Dt_t2);
return stack(2, &DR, &Dt); return stack(2, &DR, &Dt);
} }
/* ************************************************************************* */ /* ************************************************************************* */
// inverse = Pose3(inverse(p.rotation()),-unrotate(p.rotation(),p.translation())); // inverse = Pose3(inverse(R),-unrotate(R,t));
Matrix Dinverse(const Pose3& p) { Matrix Dinverse(const Pose3& p) {
Matrix Rt = p.rotation().transpose(); Matrix Rt = p.rotation().transpose();
Matrix DR_R1 = -Rt; Matrix DR_R1 = -Rt;

4
cpp/testPose3.cpp
View File

@ -54,6 +54,10 @@ TEST(Pose3, expmap_b)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Pose3, compose ) TEST( Pose3, compose )
{ {
Rot3 R = rodriguez(0.3,0.2,0.1);
Point3 t(3.5,-8.2,4.2);
Pose3 T(R,t);
Matrix actual = (T*T).matrix(); Matrix actual = (T*T).matrix();
Matrix expected = T.matrix()*T.matrix(); Matrix expected = T.matrix()*T.matrix();
CHECK(assert_equal(actual,expected,error)); CHECK(assert_equal(actual,expected,error));