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

Merged in fix/ExpmapFunctor (pull request #420) 

fix bug in ExpmapFunctor caused by square root of theta2

Approved-by: Frank Dellaert <dellaert@cc.gatech.edu>
release/4.3a0
Mandy Xie 2019-05-08 03:01:36 +00:00 committed by Frank Dellaert
parent a5844cbdcd 543938e219
commit 92bb0d71a4
3 changed files with 44 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam/geometry/SO3.cpp
View File

@ -31,7 +31,6 @@ namespace so3 {
void ExpmapFunctor::init(bool nearZeroApprox) {
nearZero = nearZeroApprox || (theta2 <= std::numeric_limits<double>::epsilon());
if (!nearZero) {
theta = std::sqrt(theta2); // rotation angle
sin_theta = std::sin(theta);
const double s2 = std::sin(theta / 2.0);
one_minus_cos = 2.0 * s2 * s2; // numerically better than [1 - cos(theta)]
@ -39,7 +38,7 @@ void ExpmapFunctor::init(bool nearZeroApprox) {
}
ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox)
: theta2(omega.dot(omega)) {
: theta2(omega.dot(omega)), theta(std::sqrt(theta2)) {
const double wx = omega.x(), wy = omega.y(), wz = omega.z();
W << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0;
init(nearZeroApprox);
@ -50,7 +49,7 @@ ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox)
}
ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox)
: theta2(angle * angle) {
: theta2(angle * angle), theta(angle) {
const double ax = axis.x(), ay = axis.y(), az = axis.z();
K << 0.0, -az, +ay, +az, 0.0, -ax, -ay, +ax, 0.0;
W = K * angle;

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

@ -103,6 +103,20 @@ Rot3 slow_but_correct_Rodrigues(const Vector& w) {
return Rot3(R);
}
/* ************************************************************************* */
TEST( Rot3, AxisAngle)
{
Vector axis = Vector3(0., 1., 0.); // rotation around Y
double angle = 3.14 / 4.0;
Rot3 expected(0.707388, 0, 0.706825,
0, 1, 0,
-0.706825, 0, 0.707388);
Rot3 actual = Rot3::AxisAngle(axis, angle);
CHECK(assert_equal(expected,actual,1e-5));
Rot3 actual2 = Rot3::AxisAngle(axis, angle-2*M_PI);
CHECK(assert_equal(expected,actual2,1e-5));
}
/* ************************************************************************* */
TEST( Rot3, Rodrigues)
{

28
gtsam/geometry/tests/testSO3.cpp
View File

@ -82,6 +82,34 @@ TEST(SO3, ChartDerivatives) {
CHECK_CHART_DERIVATIVES(R2, R1);
}
/* ************************************************************************* */
TEST(SO3, Expmap) {
Vector axis = Vector3(0., 1., 0.); // rotation around Y
double angle = 3.14 / 4.0;
Matrix expected(3,3);
expected << 0.707388, 0, 0.706825, 0, 1, 0, -0.706825, 0, 0.707388;
// axis angle version
so3::ExpmapFunctor f1(axis, angle);
SO3 actual1 = f1.expmap();
CHECK(assert_equal(expected, actual1.matrix(), 1e-5));
// axis angle version, negative angle
so3::ExpmapFunctor f2(axis, angle - 2*M_PI);
SO3 actual2 = f2.expmap();
CHECK(assert_equal(expected, actual2.matrix(), 1e-5));
// omega version
so3::ExpmapFunctor f3(axis * angle);
SO3 actual3 = f3.expmap();
CHECK(assert_equal(expected, actual3.matrix(), 1e-5));
// omega version, negative angle
so3::ExpmapFunctor f4(axis * (angle - 2*M_PI));
SO3 actual4 = f4.expmap();
CHECK(assert_equal(expected, actual4.matrix(), 1e-5));
}
/* ************************************************************************* */
namespace exmap_derivative {
static const Vector3 w(0.1, 0.27, -0.2);