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

Did more extensive testing on Logmap, cleaned that up, and replaced Taylor expansion on theta with one on (trace-3)

release/4.3a0
Frank Dellaert 2012-01-07 19:40:42 +00:00
parent aa31be1649
commit f9f3b1c47d
2 changed files with 81 additions and 55 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

64
gtsam/geometry/Rot3M.cpp
View File

@ -20,8 +20,9 @@
#ifndef GTSAM_DEFAULT_QUATERNIONS #ifndef GTSAM_DEFAULT_QUATERNIONS
#include <boost/math/constants/constants.hpp>
#include <gtsam/geometry/Rot3.h> #include <gtsam/geometry/Rot3.h>
#include <boost/math/constants/constants.hpp>
#include <cmath>
using namespace std; using namespace std;
@ -119,7 +120,7 @@ Rot3 Rot3::rodriguez(const Vector& w, double theta) {
double wwTxx = wx*wx, wwTyy = wy*wy, wwTzz = wz*wz; double wwTxx = wx*wx, wwTyy = wy*wy, wwTzz = wz*wz;
#ifndef NDEBUG #ifndef NDEBUG
double l_n = wwTxx + wwTyy + wwTzz; double l_n = wwTxx + wwTyy + wwTzz;
if (fabs(l_n-1.0)>1e-9) throw domain_error("rodriguez: length of n should be 1"); if (std::abs(l_n-1.0)>1e-9) throw domain_error("rodriguez: length of n should be 1");
#endif #endif
double c = cos(theta), s = sin(theta), c_1 = 1 - c; double c = cos(theta), s = sin(theta), c_1 = 1 - c;
@ -205,34 +206,39 @@ Point3 Rot3::unrotate(const Point3& p,
/* ************************************************************************* */ /* ************************************************************************* */
// Log map at identity - return the canonical coordinates of this rotation // Log map at identity - return the canonical coordinates of this rotation
Vector Rot3::Logmap(const Rot3& R) { Vector Rot3::Logmap(const Rot3& R) {
double tr = R.r1().x()+R.r2().y()+R.r3().z();
// FIXME should tr in statement below be absolute value? static const double PI = boost::math::constants::pi<double>();
if (tr > 3.0 - 1e-17) { // when theta = 0, +-2pi, +-4pi, etc. (or tr > 3 + 1E-10)
return zero(3); // Get trace(R)
} else if (tr > 3.0 - 1e-10) { // when theta near 0, +-2pi, +-4pi, etc. (or tr > 3 + 1E-3) double tr = R.r1_.x()+R.r2_.y()+R.r3_.z();
double theta = acos((tr-1.0)/2.0);
// Using Taylor expansion: theta/(2*sin(theta)) \approx 1/2+theta^2/12 + O(theta^4) // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc.
return (0.5 + theta*theta/12)*Vector_(3, // we do something special
R.r2().z()-R.r3().y(), if (std::abs(tr+1.0) < 1e-10) {
R.r3().x()-R.r1().z(), if(std::abs(R.r3_.z()+1.0) > 1e-10)
R.r1().y()-R.r2().x()); return (PI / sqrt(2.0+2.0*R.r3_.z())) *
// FIXME: in statement below, is this the right comparision? Vector_(3, R.r3_.x(), R.r3_.y(), 1.0+R.r3_.z());
} else if (fabs(tr - -1.0) < 1e-10) { // when theta = +-pi, +-3pi, +-5pi, etc. else if(std::abs(R.r2_.y()+1.0) > 1e-10)
if(fabs(R.r3().z() - -1.0) > 1e-10) return (PI / sqrt(2.0+2.0*R.r2_.y())) *
return (boost::math::constants::pi<double>() / sqrt(2.0+2.0*R.r3().z())) * Vector_(3, R.r2_.x(), 1.0+R.r2_.y(), R.r2_.z());
Vector_(3, R.r3().x(), R.r3().y(), 1.0+R.r3().z()); else // if(std::abs(R.r1_.x()+1.0) > 1e-10) This is implicit
else if(fabs(R.r2().y() - -1.0) > 1e-10) return (PI / sqrt(2.0+2.0*R.r1_.x())) *
return (boost::math::constants::pi<double>() / sqrt(2.0+2.0*R.r2().y())) * Vector_(3, 1.0+R.r1_.x(), R.r1_.y(), R.r1_.z());
Vector_(3, R.r2().x(), 1.0+R.r2().y(), R.r2().z());
else // if(fabs(R.r1().x() - -1.0) > 1e-10) This is implicit
return (boost::math::constants::pi<double>() / sqrt(2.0+2.0*R.r1().x())) *
Vector_(3, 1.0+R.r1().x(), R.r1().y(), R.r1().z());
} else { } else {
double theta = acos((tr-1.0)/2.0); double magnitude;
return (theta/2.0/sin(theta))*Vector_(3, double tr_3 = tr-3.0; // always negative
R.r2().z()-R.r3().y(), if (tr_3<-1e-7) {
R.r3().x()-R.r1().z(), double theta = acos((tr-1.0)/2.0);
R.r1().y()-R.r2().x()); magnitude = theta/(2.0*sin(theta));
} else {
// when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0)
// use Taylor expansion: magnitude \approx 1/2-(t-3)/12 + O((t-3)^2)
magnitude = 0.5 - tr_3*tr_3/12.0;
}
return magnitude*Vector_(3,
R.r2_.z()-R.r3_.y(),
R.r3_.x()-R.r1_.z(),
R.r1_.y()-R.r2_.x());
} }
} }

72
gtsam/geometry/tests/testRot3M.cpp
View File

@ -15,16 +15,20 @@
* @author Alireza Fathi * @author Alireza Fathi
*/ */
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <boost/math/constants/constants.hpp>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/lieProxies.h> #include <gtsam/base/lieProxies.h>
#include <gtsam/geometry/Point3.h> #include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Rot3.h> #include <gtsam/geometry/Rot3.h>
#include <boost/math/constants/constants.hpp>
#include <CppUnitLite/TestHarness.h>
#ifndef GTSAM_DEFAULT_QUATERNIONS #ifndef GTSAM_DEFAULT_QUATERNIONS
using namespace std;
using namespace gtsam; using namespace gtsam;
Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2); Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2);
@ -147,37 +151,53 @@ TEST( Rot3, expmap)
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Rot3, log) TEST(Rot3, log)
{ {
Vector w1 = Vector_(3, 0.1, 0.0, 0.0); static const double PI = boost::math::constants::pi<double>();
Rot3 R1 = Rot3::rodriguez(w1); Vector w;
CHECK(assert_equal(w1, Rot3::Logmap(R1))); Rot3 R;
Vector w2 = Vector_(3, 0.0, 0.1, 0.0); #define CHECK_OMEGA(X,Y,Z) \
Rot3 R2 = Rot3::rodriguez(w2); w = Vector_(3, (double)X, (double)Y, double(Z)); \
CHECK(assert_equal(w2, Rot3::Logmap(R2))); R = Rot3::rodriguez(w); \
EXPECT(assert_equal(w, Rot3::Logmap(R),1e-12));
Vector w3 = Vector_(3, 0.0, 0.0, 0.1); // Check zero
Rot3 R3 = Rot3::rodriguez(w3); CHECK_OMEGA( 0, 0, 0)
CHECK(assert_equal(w3, Rot3::Logmap(R3)));
Vector w = Vector_(3, 0.1, 0.4, 0.2); // create a random direction:
Rot3 R = Rot3::rodriguez(w); double norm=sqrt(1.0+16.0+4.0);
CHECK(assert_equal(w, Rot3::Logmap(R))); double x=1.0/norm, y=4.0/norm, z=2.0/norm;
Vector w5 = Vector_(3, 0.0, 0.0, 0.0); // Check very small rotation for Taylor expansion
Rot3 R5 = Rot3::rodriguez(w5); // Note that tolerance above is 1e-12, so Taylor is pretty good !
CHECK(assert_equal(w5, Rot3::Logmap(R5))); double d = 0.0001;
CHECK_OMEGA( d, 0, 0)
CHECK_OMEGA( 0, d, 0)
CHECK_OMEGA( 0, 0, d)
CHECK_OMEGA(x*d, y*d, z*d)
Vector w6 = Vector_(3, boost::math::constants::pi<double>(), 0.0, 0.0); // check normal rotation
Rot3 R6 = Rot3::rodriguez(w6); d = 0.1;
CHECK(assert_equal(w6, Rot3::Logmap(R6))); CHECK_OMEGA( d, 0, 0)
CHECK_OMEGA( 0, d, 0)
CHECK_OMEGA( 0, 0, d)
CHECK_OMEGA(x*d, y*d, z*d)
Vector w7 = Vector_(3, 0.0, boost::math::constants::pi<double>(), 0.0); // Check 180 degree rotations
Rot3 R7 = Rot3::rodriguez(w7); CHECK_OMEGA( PI, 0, 0)
CHECK(assert_equal(w7, Rot3::Logmap(R7))); CHECK_OMEGA( 0, PI, 0)
CHECK_OMEGA( 0, 0, PI)
CHECK_OMEGA(x*PI,y*PI,z*PI)
Vector w8 = Vector_(3, 0.0, 0.0, boost::math::constants::pi<double>()); // Check 360 degree rotations
Rot3 R8 = Rot3::rodriguez(w8); #define CHECK_OMEGA_ZERO(X,Y,Z) \
CHECK(assert_equal(w8, Rot3::Logmap(R8))); w = Vector_(3, (double)X, (double)Y, double(Z)); \
R = Rot3::rodriguez(w); \
EXPECT(assert_equal(zero(3), Rot3::Logmap(R)));
CHECK_OMEGA_ZERO( 2.0*PI, 0, 0)
CHECK_OMEGA_ZERO( 0, 2.0*PI, 0)
CHECK_OMEGA_ZERO( 0, 0, 2.0*PI)
CHECK_OMEGA_ZERO(x*2.*PI,y*2.*PI,z*2.*PI)
} }
/* ************************************************************************* */ /* ************************************************************************* */