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

included normalization

release/4.3a0
Luca 2014-08-17 20:34:43 -04:00
parent e031ba036d
commit 4cd4023ef8
1 changed files with 17 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
gtsam/slam/InitializePose3.cpp
View File

@ -33,7 +33,7 @@ namespace InitializePose3 {
//static const Matrix I = eye(1); //static const Matrix I = eye(1);
static const Matrix I9 = eye(9); static const Matrix I9 = eye(9);
static const Vector zero9 = Vector::Zero(9); static const Vector zero9 = Vector::Zero(9);
static const Vector zero33= Matrix::Zero(3,3); static const Matrix zero33= Matrix::Zero(3,3);
static const Key keyAnchor = symbol('Z', 9999999); static const Key keyAnchor = symbol('Z', 9999999);
@ -73,6 +73,9 @@ GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) {
Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) { Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) {
gttic(InitializePose3_computeOrientations); gttic(InitializePose3_computeOrientations);
Matrix ppm = Matrix::Zero(3,3); // plus plus minus
ppm(0,0) = 1; ppm(1,1) = 1; ppm(2,2) = -1;
Values validRot3; Values validRot3;
BOOST_FOREACH(const VectorValues::value_type& it, relaxedRot3) { BOOST_FOREACH(const VectorValues::value_type& it, relaxedRot3) {
Key key = it.first; Key key = it.first;
@ -82,7 +85,19 @@ Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) {
rotMat(0,0) = rotVector(0); rotMat(0,1) = rotVector(1); rotMat(0,2) = rotVector(2); rotMat(0,0) = rotVector(0); rotMat(0,1) = rotVector(1); rotMat(0,2) = rotVector(2);
rotMat(1,0) = rotVector(3); rotMat(1,1) = rotVector(4); rotMat(1,2) = rotVector(5); rotMat(1,0) = rotVector(3); rotMat(1,1) = rotVector(4); rotMat(1,2) = rotVector(5);
rotMat(2,0) = rotVector(6); rotMat(2,1) = rotVector(7); rotMat(2,2) = rotVector(8); rotMat(2,0) = rotVector(6); rotMat(2,1) = rotVector(7); rotMat(2,2) = rotVector(8);
Rot3 initRot = Rot3(rotMat);
Matrix U, V; Vector s;
svd(rotMat, U, s, V);
Matrix3 normalizedRotMat = U * V.transpose();
// std::cout << "rotMat \n" << rotMat << std::endl;
// std::cout << "U V' \n" << U * V.transpose() << std::endl;
// std::cout << "V \n" << V << std::endl;
if(normalizedRotMat.determinant() < 0)
normalizedRotMat = U * ppm * V.transpose();
Rot3 initRot = Rot3(normalizedRotMat);
validRot3.insert(key, initRot); validRot3.insert(key, initRot);
} }
} }