From 7b01a7806077cf84ccf05fa73c0b1603481057e8 Mon Sep 17 00:00:00 2001
From: asantamaria <asantamaria@iri.upc.edu>
Date: Wed, 27 Sep 2017 17:36:08 +0200
Subject: [PATCH] Fix odd bug about rotations...
Bug was: in Eigen matrix functions, passing a matrix expression by reference means
that each time the reference is needed the expression was reevaluated.
This produced that when calling a function
e.g. f( Matrix::Random() ),
the parameter passed is an expression and not a matrix. Therefore, each
time the matrix is used in f(), a new Random was created.
Solution: use the .eval() method to evaluate the expression:
as in
e.g. f( Matrix::Random().eval() )
---
src/rotations.h | 17 ++++++++++++++---
src/test/gtest_rotation.cpp | 15 ++++++++-------
2 files changed, 22 insertions(+), 10 deletions(-)
diff --git a/src/rotations.h b/src/rotations.h
index 916025606..b43b84a08 100644
--- a/src/rotations.h
+++ b/src/rotations.h
@@ -67,7 +67,13 @@ inline Eigen::Matrix<typename Derived::Scalar, 3, 3> skew(const Eigen::MatrixBas
typedef typename Derived::Scalar T;
- return (Eigen::Matrix<T, 3, 3>() << 0.0, -_v(2), +_v(1), +_v(2), 0.0, -_v(0), -_v(1), +_v(0), 0.0).finished();
+ Eigen::Matrix<T, 3, 3> sk;
+
+ sk << 0.0 , -_v(2), +_v(1),
+ +_v(2), 0.0 , -_v(0),
+ -_v(1), +_v(0), 0.0;
+
+ return sk;
}
/** \brief Inverse of skew symmetric matrix
@@ -156,12 +162,17 @@ inline Eigen::Matrix<typename Derived::Scalar, 3, 3> exp_R(const Eigen::MatrixBa
typedef typename Derived::Scalar T;
+ Eigen::Matrix<typename Derived::Scalar, 3, 3> R;
+
T angle_squared = _v.squaredNorm();
T angle = sqrt(angle_squared);
+
if (angle > wolf::Constants::EPS)
- return Eigen::AngleAxis<T>(angle, _v / angle).matrix();
+ R = Eigen::AngleAxis<T>(angle, _v.normalized()).toRotationMatrix();
else
- return Eigen::Matrix<T, 3, 3>::Identity() + skew(_v);
+ R = Eigen::Matrix<T, 3, 3>::Identity() + skew(_v);
+
+ return R;
}
/** \brief Rotation matrix logarithmic map
diff --git a/src/test/gtest_rotation.cpp b/src/test/gtest_rotation.cpp
index 407318408..79932e9c1 100644
--- a/src/test/gtest_rotation.cpp
+++ b/src/test/gtest_rotation.cpp
@@ -258,7 +258,7 @@ TEST(rotations, R2v_v2R_limits)
//Vector3s rv;
for(int i = 0; i<8; i++){
- initial_matrix = v2R(Vector3s::Random() * scale);
+ initial_matrix = v2R(Vector3s::Random().eval() * scale);
R_to_v = R2v(initial_matrix);
v_to_R = v2R(R_to_v);
@@ -277,7 +277,7 @@ TEST(rotations, R2v_v2R_AAlimits)
Vector3s rv;
for(int i = 0; i<8; i++){
- rotation_mat = v2R(Vector3s::Random() * scale);
+ rotation_mat = v2R(Vector3s::Random().eval() * scale);
//rotation_mat(0,0) = 1.0;
//rotation_mat(1,1) = 1.0;
//rotation_mat(2,2) = 1.0;
@@ -321,7 +321,7 @@ TEST(rotations, AngleAxis_limits)
Vector3s rv;
for(int i = 0; i<8; i++){ //FIX ME : Random() will not create a rotation matrix. Then, R2v(Random_matrix()) makes no sense at all.
- rotation_mat = v2R(Vector3s::Random() * scale);
+ rotation_mat = v2R(Vector3s::Random().eval() * scale);
rotation_mati = rotation_mat;
//rv = R2v(rotation_mat); //decomposing R2v below
@@ -352,7 +352,8 @@ TEST(rotations, AngleAxis_limits2)
//FIX ME : 5. Building a rot mat doing this is not safe; You cannot guarantee that R is valid.
// Highlight limitation of AngleAxis
- rotation_mat = skew(Vector3s::Random()) *0.0001;
+ rotation_mat = skew(Vector3s::Random().eval()) * 1e-8;
+
rotation_mat(0,0) = 1;
rotation_mat(1,1) = 0.999999;
rotation_mat(2,2) = 1;
@@ -367,9 +368,9 @@ TEST(rotations, AngleAxis_limits2)
aa0 = AngleAxis<wolf::Scalar>(rotation_mat);
rv = aa0.axis() * aa0.angle();
//checking if rv is 0 vector
- ASSERT_TRUE(rv.isMuchSmallerThan(1, wolf::Constants::EPS_SMALL));
+ ASSERT_MATRIX_APPROX(rv, Eigen::Vector3s::Zero(), wolf::Constants::EPS);
- rotation_mat = skew(Vector3s::Random()) *0.1;
+ rotation_mat = skew(Vector3s::Random().eval()) *0.1;
rotation_mat(0,0) = 1;
rotation_mat(1,1) = 0.9999999;
rotation_mat(2,2) = 1;
@@ -384,7 +385,7 @@ TEST(rotations, AngleAxis_limits2)
aa0 = AngleAxis<wolf::Scalar>(rotation_mat);
rv = aa0.axis() * aa0.angle();
//checking if rv is 0 vector
- ASSERT_TRUE(rv.isMuchSmallerThan(1, wolf::Constants::EPS_SMALL));
+ ASSERT_FALSE(rv.isMuchSmallerThan(1, wolf::Constants::EPS_SMALL));
}
TEST(rotations, Quat_compos_const_rateOfTurn)
--
GitLab