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

log and between

release/4.3a0
Frank Dellaert 2010-01-06 19:29:41 +00:00
parent 8fb2c00fc1
commit b20ed42134
3 changed files with 88 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
cpp/Rot3.cpp
View File

@ -23,6 +23,18 @@ namespace gtsam {
return rodriguez(v) * (*this);
}
/* ************************************************************************* */
Vector Rot3::log() const {
double tr = r1_.x()+r2_.y()+r3_.z();
if (tr==3.0) return ones(3);
if (tr==-1.0) throw 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,
r2_.z()-r3_.y(),
r3_.x()-r1_.z(),
r1_.y()-r2_.x());
}
/* ************************************************************************* */
Vector Rot3::vector() const {
double r[] = { r1_.x(), r1_.y(), r1_.z(),
@ -118,6 +130,10 @@ namespace gtsam {
}
/* ************************************************************************* */
Vector log(const Rot3& R, const Rot3& S) {
return between(R,S).log();
}
/* ************************************************************************* */
Point3 rotate(const Rot3& R, const Point3& p) {
return R * p;
}
@ -151,6 +167,11 @@ namespace gtsam {
return R.transpose();
}
/* ************************************************************************* */
Rot3 between(const Rot3& R1, const Rot3& R2) {
return R2 * R1.inverse();
}
/* ************************************************************************* */
pair<Matrix,Vector> RQ(const Matrix& A) {
double A21 = A(2, 1), A22 = A(2, 2), a = sqrt(A21 * A21 + A22 * A22);

22
cpp/Rot3.h
View File

@ -64,9 +64,17 @@ namespace gtsam {
/** return DOF, dimensionality of tangent space */
size_t dim() const { return 3;}
/** Given 3-dim tangent vector, create new rotation */
/**
* @param a 3-dim tangent vector d (canonical coordinates of between(R,S))
* @return new rotation S=exp(d)*R
*/
Rot3 exmap(const Vector& d) const;
/**
* @return log(R), i.e. canonical coordinates of R
*/
Vector log() const;
/** return vectorized form (column-wise)*/
Vector vector() const;
@ -143,6 +151,13 @@ namespace gtsam {
*/
Rot3 exmap(const Rot3& R, const Vector& v);
/**
* @param a rotation R
* @param a rotation S
* @return log(S*R'), i.e. canonical coordinates of between(R,S)
*/
Vector log(const Rot3& R, const Rot3& S);
/**
* rotate point from rotated coordinate frame to
* world = R*p
@ -159,6 +174,11 @@ namespace gtsam {
Matrix Dunrotate1(const Rot3& R, const Point3& p);
Matrix Dunrotate2(const Rot3& R); // does not depend on p !
/**
* Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1'
*/
Rot3 between(const Rot3& R1, const Rot3& R2);
/**
* [RQ] receives a 3 by 3 matrix and returns an upper triangular matrix R
* and 3 rotation angles corresponding to the rotation matrix Q=Qz'*Qy'*Qx'

62
cpp/testRot3.cpp
View File

@ -95,25 +95,55 @@ TEST( Rot3, rodriguez3) {
CHECK(assert_equal(R1,R2));
}
/* ************************************************************************* */
//TEST(Rot3, manifold) {
// Rot3 t1 = rodriguez(0.1,0.4,0.2);
// Rot3 t2 = rodriguez(0.3,0.1,0.7);
// Rot3 origin;
// Vector d12 = t1.log(t2);
// CHECK(assert_equal(t2, t1.exmap(d12)));
// CHECK(assert_equal(t2, origin.exmap(d12)*t1));
// Vector d21 = t2.log(t1);
// CHECK(assert_equal(t1, t2.exmap(d21)));
// CHECK(assert_equal(t1, origin.exmap(d21)*t2));
//}
/* ************************************************************************* */
TEST( Rot3, exmap)
{
Vector v(3);
fill(v.begin(), v.end(), 0);
CHECK(assert_equal(R.exmap(v), R));
Vector v(3);
fill(v.begin(), v.end(), 0);
CHECK(assert_equal(R.exmap(v), R));
}
/* ************************************************************************* */
TEST(Rot3, log)
{
Vector w1 = Vector_(3, 0.1, 0.0, 0.0);
Rot3 R1 = rodriguez(w1);
CHECK(assert_equal(w1, R1.log()));
Vector w2 = Vector_(3, 0.0, 0.1, 0.0);
Rot3 R2 = rodriguez(w2);
CHECK(assert_equal(w2, R2.log()));
Vector w3 = Vector_(3, 0.0, 0.0, 0.1);
Rot3 R3 = rodriguez(w3);
CHECK(assert_equal(w3, R3.log()));
Vector w = Vector_(3, 0.1, 0.4, 0.2);
Rot3 R = rodriguez(w);
CHECK(assert_equal(w, R.log()));
}
/* ************************************************************************* */
TEST(Rot3, between)
{
Rot3 R = rodriguez(0.1, 0.4, 0.2);
Rot3 origin;
CHECK(assert_equal(R, between(origin,R)));
CHECK(assert_equal(R.inverse(), between(R,origin)));
}
/* ************************************************************************* */
TEST(Rot3, manifold)
{
Rot3 t1 = rodriguez(0.1, 0.4, 0.2);
Rot3 t2 = rodriguez(0.3, 0.1, 0.7);
Rot3 origin;
Vector d12 = log(t1, t2);
CHECK(assert_equal(t2, t1.exmap(d12)));
CHECK(assert_equal(t2, origin.exmap(d12)*t1));
Vector d21 = log(t2, t1);
CHECK(assert_equal(t1, t2.exmap(d21)));
CHECK(assert_equal(t1, origin.exmap(d21)*t2));
}
/* ************************************************************************* */