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

Fixed issues with Closest and << in SOn.h

release/4.3a0
Frank Dellaert 2019-04-20 14:28:43 -04:00 committed by Fan Jiang
parent b0e4075089
commit ad0bb79533
2 changed files with 4 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/geometry/SO3.cpp
View File

@ -136,7 +136,7 @@ SO3 SO3::AxisAngle(const Vector3& axis, double theta) {
/* ************************************************************************* */
SO3 SO3::ClosestTo(const Matrix3& M) {
Eigen::JacobiSVD<Matrix3> svd(M, Eigen::ComputeThinU | Eigen::ComputeThinV);
Eigen::JacobiSVD<Matrix3> svd(M, Eigen::ComputeFullU | Eigen::ComputeFullV);
const auto& U = svd.matrixU();
const auto& V = svd.matrixV();
const double det = (U * V.transpose()).determinant();

5
gtsam/geometry/SOn.h
View File

@ -75,12 +75,13 @@ class SOnBase {
Matrix G(n2, d);
for (size_t j = 0; j < d; j++) {
// TODO(frank): this can't be right. Think about fixed vs dynamic.
G.col(j) << Derived::Hat(n, Eigen::VectorXd::Unit(d, j));
const auto X = derived().Hat(n, Eigen::VectorXd::Unit(d, j));
G.col(j) = Eigen::Map<const Matrix>(X.data(), n2, 1);
}
// Vectorize
Vector X(n2);
X << derived();
X << Eigen::Map<const Matrix>(derived().data(), n2, 1);
// If requested, calculate H as (I \oplus Q) * P
if (H) {