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

Fixed issue with << operator (surfaced in debug mode)

release/4.3a0
Frank Dellaert 2019-04-10 22:25:33 -04:00
parent 35d8ebca22
commit d8570ce09b
1 changed files with 5 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/slam/InitializePose3.cpp
View File

@ -82,7 +82,11 @@ Values InitializePose3::normalizeRelaxedRotations(
for(const auto& it: relaxedRot3) { for(const auto& it: relaxedRot3) {
Key key = it.first; Key key = it.first;
if (key != kAnchorKey) { if (key != kAnchorKey) {
Matrix3 R; R << it.second; Matrix3 R;
R << Eigen::Map<const Matrix3>(it.second.data()); // Recover M from vectorized
// ClosestTo finds rotation matrix closest to H in Frobenius sense
// Rot3 initRot = Rot3::ClosestTo(M.transpose());
Matrix U, V; Vector s; Matrix U, V; Vector s;
svd(R.transpose(), U, s, V); svd(R.transpose(), U, s, V);