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

hack EssentialMatrix transform_to Jacobian 

git-svn-id: https://svn.cc.gatech.edu/borg/gtsam/trunk@20411 898a188c-9671-0410-8e00-e3fd810bbb7f
release/4.3a0
Duy-Nguyen Ta 2013-12-20 20:32:42 +00:00 committed by Richard Roberts
parent 191d18290d
commit 6cede01c79
2 changed files with 5 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/geometry/EssentialMatrix.h
View File

@ -129,7 +129,7 @@ public:
Matrix H(3, 5);
std::cout << *DE << std::endl << std::endl;
std::cout << aTb_.basis() << std::endl << std::endl;
H << DE->block < 3, 3 > (0, 0), DE->block < 3, 3 > (0, 3) * aTb_.basis();
H << DE->block < 3, 3 > (0, 0), -aRb_.inverse().matrix() * aTb_.basis();
std::cout << H << std::endl << std::endl;
*DE = H;
}

5
gtsam/geometry/tests/testEssentialMatrix.cpp
View File

@ -47,7 +47,10 @@ TEST (EssentialMatrix, retract2) {
//*************************************************************************
Point3 transform_to_(const EssentialMatrix& E, const Point3& point) { return E.transform_to(point); }
TEST (EssentialMatrix, transform_to) {
EssentialMatrix E(aRb, aTb);
// test with a more complicated EssentialMatrix
Rot3 aRb2 = Rot3::yaw(M_PI/3.0)*Rot3::pitch(M_PI_4)*Rot3::roll(M_PI/6.0);
Point3 aTb2(19.2, 3.7, 5.9);
EssentialMatrix E(aRb2, aTb2);
//EssentialMatrix E(aRb, Sphere2(aTb).retract((Vector(2) << 0.1, 0)));
static Point3 P(0.2,0.7,-2);
Matrix actH1, actH2;