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

normalized needs to be called explicitly

release/4.3a0
Varun Agrawal 2020-10-05 13:25:30 -04:00
parent 06ac627249
commit 2e1cc3ca35
4 changed files with 15 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/geometry/Rot3.h
View File

@ -435,7 +435,7 @@ namespace gtsam {
* This means either re-orthogonalizing the Matrix representation or
* normalizing the quaternion representation.
*/
Rot3 normalize(const Rot3& R) const;
Rot3 normalized() const;
/// @deprecated, this is base 1, and was just confusing
Point3 column(int index) const;

18
gtsam/geometry/Rot3M.cpp
View File

@ -109,7 +109,7 @@ Rot3 Rot3::RzRyRx(double x, double y, double z, OptionalJacobian<3, 1> Hx,
}
/* ************************************************************************* */
Rot3 Rot3::normalize(const Rot3& R) const {
Rot3 Rot3::normalized() const {
/// Implementation from here: https://stackoverflow.com/a/23082112/1236990
/// Theory: https://drive.google.com/file/d/0B9rLLz1XQKmaZTlQdV81QjNoZTA/view
@ -117,9 +117,11 @@ Rot3 Rot3::normalize(const Rot3& R) const {
/// error to the x and y rows, and then performs a Taylor expansion to
/// orthogonalize.
Matrix3 rot = R.matrix(), rot_new;
Matrix3 rot = rot_.matrix(), rot_orth;
if (std::fabs(rot.determinant()-1) < 1e-12) return R;
// Check if determinant is already 1.
// If yes, then return the current Rot3.
if (std::fabs(rot.determinant()-1) < 1e-12) return Rot3(rot_);
Vector3 x = rot.block<1, 3>(0, 0), y = rot.block<1, 3>(1, 0);
double error = x.dot(y);
@ -127,16 +129,16 @@ Rot3 Rot3::normalize(const Rot3& R) const {
Vector3 x_ort = x - (error / 2) * y, y_ort = y - (error / 2) * x;
Vector3 z_ort = x_ort.cross(y_ort);
rot_new.block<1, 3>(0, 0) = 0.5 * (3 - x_ort.dot(x_ort)) * x_ort;
rot_new.block<1, 3>(1, 0) = 0.5 * (3 - y_ort.dot(y_ort)) * y_ort;
rot_new.block<1, 3>(2, 0) = 0.5 * (3 - z_ort.dot(z_ort)) * z_ort;
rot_orth.block<1, 3>(0, 0) = 0.5 * (3 - x_ort.dot(x_ort)) * x_ort;
rot_orth.block<1, 3>(1, 0) = 0.5 * (3 - y_ort.dot(y_ort)) * y_ort;
rot_orth.block<1, 3>(2, 0) = 0.5 * (3 - z_ort.dot(z_ort)) * z_ort;
return Rot3(rot_new);
return Rot3(rot_orth);
}
/* ************************************************************************* */
Rot3 Rot3::operator*(const Rot3& R2) const {
return normalize(Rot3(rot_*R2.rot_));
return Rot3(rot_*R2.rot_);
}
/* ************************************************************************* */

6
gtsam/geometry/Rot3Q.cpp
View File

@ -87,12 +87,12 @@ namespace gtsam {
}
/* ************************************************************************* */
Rot3 Rot3::normalize(const Rot3& R) const {
return Rot3(R.quaternion_.normalized());
Rot3 Rot3::normalized() const {
return Rot3(quaternion_.normalized());
}
/* ************************************************************************* */
Rot3 Rot3::operator*(const Rot3& R2) const {
return normalize(Rot3(quaternion_ * R2.quaternion_));
return Rot3(quaternion_ * R2.quaternion_);
}
/* ************************************************************************* */

2
gtsam/geometry/tests/testRot3.cpp
View File

@ -923,7 +923,7 @@ TEST(Rot3, determinant) {
R_01 = R_w0.between(R_w1);
R_w2 = R_w1 * R_01;
R_w0 = R_w1;
R_w1 = R_w2;
R_w1 = R_w2.normalized();
actual = R_w2.matrix().determinant();
EXPECT_DOUBLES_EQUAL(expected, actual, 1e-7);