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

Special case for trace==1 in Rot3 logmap

release/4.3a0
Richard Roberts 2010-01-10 23:36:37 +00:00
parent b0fd5396ad
commit aa2aa4149c
3 changed files with 40 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
cpp/Rot3.cpp
View File

@ -109,6 +109,32 @@ namespace gtsam {
return Vector_(3,q(2),q(1),q(0));
}
/* ************************************************************************* */
// Log map at identity - return the canonical coordinates of this rotation
inline Vector logmap(const Rot3& R) {
double tr = R.r1().x()+R.r2().y()+R.r3().z();
if (tr==3.0) // when theta = 0, +-2pi, +-4pi, etc.
return zero(3);
else if (tr==-1.0) { // when theta = +-pi, +-3pi, +-5pi, etc.
if(R.r3().z() != -1.0)
return (boost::math::constants::pi<double>() / sqrt(2.0+2.0*R.r3().z())) *
Vector_(3, R.r3().x(), R.r3().y(), 1.0+R.r3().z());
else if(R.r2().y() != -1.0)
return (boost::math::constants::pi<double>() / sqrt(2.0+2.0*R.r2().y())) *
Vector_(3, R.r2().x(), 1.0+R.r2().y(), R.r2().z());
else if(R.r1().x() != -1.0)
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 {
double theta = acos((tr-1.0)/2.0);
return (theta/2.0/sin(theta))*Vector_(3,
R.r2().z()-R.r3().y(),
R.r3().x()-R.r1().z(),
R.r1().y()-R.r2().x());
}
}
/* ************************************************************************* */
Rot3 rodriguez(const Vector& n, double t) {
double n0 = n(0), n1=n(1), n2=n(2);

12
cpp/Rot3.h
View File

@ -10,6 +10,7 @@
#pragma once
#include <boost/math/constants/constants.hpp>
#include "Point3.h"
#include "Testable.h"
#include "Lie.h"
@ -156,16 +157,7 @@ namespace gtsam {
}
// Log map at identity - return the canonical coordinates of this rotation
inline Vector logmap(const Rot3& R) {
double tr = R.r1().x()+R.r2().y()+R.r3().z();
if (tr==3.0) return zero(3);
if (tr==-1.0) throw std::domain_error("Rot3::log: trace == -1 not yet handled :-(");;
double theta = acos((tr-1.0)/2.0);
return (theta/2.0/sin(theta))*Vector_(3,
R.r2().z()-R.r3().y(),
R.r3().x()-R.r1().z(),
R.r1().y()-R.r2().x());
}
Vector logmap(const Rot3& R);
// Compose two rotations
inline Rot3 compose(const Rot3& R1, const Rot3& R2) {

15
cpp/testRot3.cpp
View File

@ -5,6 +5,7 @@
*/
#include <CppUnitLite/TestHarness.h>
#include <boost/math/constants/constants.hpp>
#include "numericalDerivative.h"
#include "Point3.h"
#include "Rot3.h"
@ -126,9 +127,17 @@ TEST(Rot3, log)
Rot3 R5 = rodriguez(w5);
CHECK(assert_equal(w5, logmap(R5)));
// Vector w6 = Vector_(3, M_PI, 0.0, 0.0);
// Rot3 R6 = rodriguez(w6);
// CHECK(assert_equal(w6, logmap(R6)));
Vector w6 = Vector_(3, boost::math::constants::pi<double>(), 0.0, 0.0);
Rot3 R6 = rodriguez(w6);
CHECK(assert_equal(w6, logmap(R6)));
Vector w7 = Vector_(3, 0.0, boost::math::constants::pi<double>(), 0.0);
Rot3 R7 = rodriguez(w7);
CHECK(assert_equal(w7, logmap(R7)));
Vector w8 = Vector_(3, 0.0, 0.0, boost::math::constants::pi<double>());
Rot3 R8 = rodriguez(w8);
CHECK(assert_equal(w8, logmap(R8)));
}
/* ************************************************************************* */