From 4cd4023ef874e7b3ee610141def351a9dc4fe284 Mon Sep 17 00:00:00 2001 From: Luca Date: Sun, 17 Aug 2014 20:34:43 -0400 Subject: [PATCH] included normalization --- gtsam/slam/InitializePose3.cpp | 19 +++++++++++++++++-- 1 file changed, 17 insertions(+), 2 deletions(-) diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index 232733273..37b33204b 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -33,7 +33,7 @@ namespace InitializePose3 { //static const Matrix I = eye(1); static const Matrix I9 = eye(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); @@ -73,6 +73,9 @@ GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) { Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) { 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; BOOST_FOREACH(const VectorValues::value_type& it, relaxedRot3) { 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(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); - 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); } }