Skip to content
Open
Changes from 1 commit
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
Update closest_rotation.cpp
  • Loading branch information
jnyao authored Oct 9, 2018
commit 480fd1f436b97bf4bfc696189324a00e78c01873
13 changes: 13 additions & 0 deletions src/closest_rotation.cpp
Original file line number Diff line number Diff line change
@@ -1,9 +1,22 @@
#include "closest_rotation.h"
#include <Eigen/SVD>
#include <Eigen/Dense>
#include <iostream>

using namespace Eigen;

void closest_rotation(
const Eigen::Matrix3d & M,
Eigen::Matrix3d & R)
{
// Replace with your code
R = Eigen::Matrix3d::Identity();
JacobiSVD<MatrixXd> svd(M, ComputeThinU | ComputeThinV);
Matrix3d omega;
omega.setZero();
omega(0, 0) = 1;
omega(1, 1) = 1;
omega(2, 2) = (svd.matrixU() * svd.matrixV().transpose()).determinant();
//std::cout<<"det: "<<omega(2,2)<<std::endl;
R = svd.matrixU() * omega * svd.matrixV().transpose();
}