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

Sped up rodriguez and replaced slow implementation with call to fast implementation. Also moved some functions to cpp.

release/4.3a0
Frank Dellaert 2010-01-02 14:28:18 +00:00
parent a1c55b9f62
commit f82b46f96c
6 changed files with 195 additions and 107 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
.cproject
View File

@ -663,6 +663,22 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="timeRot3.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>timeRot3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="install" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>install</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>

5
cpp/Makefile.am
View File

@ -176,6 +176,11 @@ testRot3_LDADD = libgtsam.la
testPose3_LDADD = libgtsam.la
testCal3_S2_LDADD = libgtsam.la
# not the correct way, I'm sure: Kai ?
timeRot3: timeRot3.cpp
timeRot3: CXXFLAGS += -I /opt/local/include
timeRot3: LDFLAGS += -L.libs -lgtsam
# simulated2D example
sources += simulated2D.cpp
testSimulated2D_SOURCES = testSimulated2D.cpp

90
cpp/Rot3.cpp
View File

@ -12,7 +12,7 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
/* ************************************************************************* */
bool Rot3::equals(const Rot3 & R, double tol) const {
return equal_with_abs_tol(matrix(), R.matrix(), tol);
}
@ -23,37 +23,77 @@ namespace gtsam {
return rodriguez(v) * (*this);
}
/* ************************************************************************* */
/** faster than below ? */
/* ************************************************************************* */
Rot3 rodriguez(const Vector& w, double t) {
double l_w = 0.0;
for (int i = 0; i < 3; i++)
l_w += pow(w(i), 2.0);
if (l_w != 1.0) throw domain_error("rodriguez: length of w should be 1");
/* ************************************************************************* */
Vector Rot3::vector() const {
double r[] = { r1_.x(), r1_.y(), r1_.z(),
r2_.x(), r2_.y(), r2_.z(),
r3_.x(), r3_.y(), r3_.z() };
Vector v(9);
copy(r,r+9,v.begin());
return v;
}
double ct = cos(t), st = sin(t);
/* ************************************************************************* */
Matrix Rot3::matrix() const {
double r[] = { r1_.x(), r2_.x(), r3_.x(),
r1_.y(), r2_.y(), r3_.y(),
r1_.z(), r2_.z(), r3_.z() };
return Matrix_(3,3, r);
}
Point3 r1 = Point3(ct + w(0) * w(0) * (1 - ct), w(2) * st + w(0) * w(1) * (1 - ct), -w(1) * st + w(0) * w(2) * (1 - ct));
Point3 r2 = Point3(w(1) * w(0) * (1 - ct) - w(2) * st, w(1) * w(1) * (1 - ct) + ct, w(1) * w(2) * (1 - ct) + w(0) * st);
Point3 r3 = Point3(w(1) * st + w(2) * w(0) * (1 - ct), -w(0) * st + w(2) * w(1) * (1 - ct), ct + w(2) * w(2) * (1 - ct));
/* ************************************************************************* */
Matrix Rot3::transpose() const {
double r[] = { r1_.x(), r1_.y(), r1_.z(),
r2_.x(), r2_.y(), r2_.z(),
r3_.x(), r3_.y(), r3_.z()};
return Matrix_(3,3, r);
}
/* ************************************************************************* */
Point3 Rot3::column(int index) const{
if(index == 3)
return r3_;
else if (index == 2)
return r2_;
else
return r1_; // default returns r1
}
/* ************************************************************************* */
Rot3 Rot3::inverse() const {
return Rot3(
r1_.x(), r1_.y(), r1_.z(),
r2_.x(), r2_.y(), r2_.z(),
r3_.x(), r3_.y(), r3_.z());
}
/* ************************************************************************* */
Rot3 rodriguez(const Vector& n, double t) {
double n0 = n(0), n1=n(1), n2=n(2);
double n00 = n0*n0, n11 = n1*n1, n22 = n2*n2;
#ifndef NDEBUG
double l_n = n00+n11+n22;
if (fabs(l_n-1.0)>1e-9) throw domain_error("rodriguez: length of n should be 1");
#endif
double ct = cos(t), st = sin(t), ct_1 = 1 - ct;
double s0 = n0 * st, s1 = n1 * st, s2 = n2 * st;
double C01 = ct_1*n0*n1, C02 = ct_1*n0*n2, C12 = ct_1*n1*n2;
double C00 = ct_1*n00, C11 = ct_1*n11, C22 = ct_1*n22;
Point3 r1 = Point3( ct + C00, s2 + C01, -s1 + C02);
Point3 r2 = Point3(-s2 + C01, ct + C11, s0 + C12);
Point3 r3 = Point3( s1 + C02, -s0 + C12, ct + C22);
return Rot3(r1, r2, r3);
}
/* ************************************************************************* */
Rot3 rodriguez(double wx, double wy, double wz) {
Matrix J = skewSymmetric(wx, wy, wz);
double t2 = wx * wx + wy * wy + wz * wz;
if (t2 < 1e-10) return Rot3();
double t = sqrt(t2);
Matrix R = eye(3, 3) + sin(t) / t * J + (1.0 - cos(t)) / t2 * (J * J);
return R; // matrix constructor will be tripped
}
/* ************************************************************************* */
Rot3 rodriguez(const Vector& v) {
return rodriguez(v(0), v(1), v(2));
Rot3 rodriguez(const Vector& w) {
double t = norm_2(w);
if (t < 1e-5) return Rot3();
return rodriguez(w/t, t);
}
/* ************************************************************************* */

121
cpp/Rot3.h
View File

@ -23,43 +23,43 @@ namespace gtsam {
public:
/** default coonstructor, unit rotation */
Rot3() : r1_(Point3(1.0,0.0,0.0)),
r2_(Point3(0.0,1.0,0.0)),
r3_(Point3(0.0,0.0,1.0)) {
}
/** default constructor, unit rotation */
Rot3() : r1_(Point3(1.0,0.0,0.0)),
r2_(Point3(0.0,1.0,0.0)),
r3_(Point3(0.0,0.0,1.0)) {
}
/** constructor from columns */
Rot3(const Point3& r1, const Point3& r2, const Point3& r3) :
r1_(r1), r2_(r2), r3_(r3) {
}
/** constructor from vector */
Rot3(const Vector &v) :
r1_(Point3(v(0),v(1),v(2))),
r2_(Point3(v(3),v(4),v(5))),
r3_(Point3(v(6),v(7),v(8)))
{ }
/** constructor from columns */
Rot3(const Point3& r1, const Point3& r2, const Point3& r3) :
r1_(r1), r2_(r2), r3_(r3) {
}
/** constructor from doubles in *row* order !!! */
Rot3(double R11, double R12, double R13,
double R21, double R22, double R23,
double R31, double R32, double R33) :
r1_(Point3(R11, R21, R31)),
r2_(Point3(R12, R22, R32)),
r3_(Point3(R13, R23, R33)) {}
/** constructor from vector */
Rot3(const Vector &v) :
r1_(Point3(v(0),v(1),v(2))),
r2_(Point3(v(3),v(4),v(5))),
r3_(Point3(v(6),v(7),v(8)))
{ }
/** constructor from matrix */
Rot3(const Matrix& R):
r1_(Point3(R(0,0), R(1,0), R(2,0))),
r2_(Point3(R(0,1), R(1,1), R(2,1))),
r3_(Point3(R(0,2), R(1,2), R(2,2))) {}
/** constructor from doubles in *row* order !!! */
Rot3(double R11, double R12, double R13,
double R21, double R22, double R23,
double R31, double R32, double R33) :
r1_(Point3(R11, R21, R31)),
r2_(Point3(R12, R22, R32)),
r3_(Point3(R13, R23, R33)) {}
/** print */
void print(const std::string& s="R") const { gtsam::print(matrix(), s);}
/** constructor from matrix */
Rot3(const Matrix& R):
r1_(Point3(R(0,0), R(1,0), R(2,0))),
r2_(Point3(R(0,1), R(1,1), R(2,1))),
r3_(Point3(R(0,2), R(1,2), R(2,2))) {}
/** equals with an tolerance */
bool equals(const Rot3& p, double tol = 1e-9) const;
/** print */
void print(const std::string& s="R") const { gtsam::print(matrix(), s);}
/** equals with an tolerance */
bool equals(const Rot3& p, double tol = 1e-9) const;
/** return DOF, dimensionality of tangent space */
size_t dim() const { return 3;}
@ -68,48 +68,19 @@ namespace gtsam {
Rot3 exmap(const Vector& d) const;
/** return vectorized form (column-wise)*/
Vector vector() const {
double r[] = { r1_.x(), r1_.y(), r1_.z(),
r2_.x(), r2_.y(), r2_.z(),
r3_.x(), r3_.y(), r3_.z() };
Vector v(9);
copy(r,r+9,v.begin());
return v;
}
Vector vector() const;
/** return 3*3 rotation matrix */
Matrix matrix() const {
double r[] = { r1_.x(), r2_.x(), r3_.x(),
r1_.y(), r2_.y(), r3_.y(),
r1_.z(), r2_.z(), r3_.z() };
return Matrix_(3,3, r);
}
Matrix matrix() const;
/** return 3*3 transpose (inverse) rotation matrix */
Matrix transpose() const {
double r[] = { r1_.x(), r1_.y(), r1_.z(),
r2_.x(), r2_.y(), r2_.z(),
r3_.x(), r3_.y(), r3_.z()};
return Matrix_(3,3, r);
}
Matrix transpose() const;
/** returns column vector specified by index */
Point3 column(int index) const{
if(index == 3)
return r3_;
else if (index == 2)
return r2_;
else
return r1_; // default returns r1
}
Point3 column(int index) const;
/** inverse transformation */
Rot3 inverse() const {
return Rot3(
r1_.x(), r1_.y(), r1_.z(),
r2_.x(), r2_.y(), r2_.z(),
r3_.x(), r3_.y(), r3_.z());
}
Rot3 inverse() const;
/** composition */
inline Rot3 operator*(const Rot3& B) const { return Rot3(matrix()*B.matrix());}
@ -151,15 +122,6 @@ namespace gtsam {
*/
Rot3 rodriguez(const Vector& w, double theta);
/**
* Rodriguez' formula to compute an incremental rotation matrix
* @param wx
* @param wy
* @param wz
* @return incremental rotation matrix
*/
Rot3 rodriguez(double wx, double wy, double wz);
/**
* Rodriguez' formula to compute an incremental rotation matrix
* @param v a vector of incremental roll,pitch,yaw
@ -167,6 +129,15 @@ namespace gtsam {
*/
Rot3 rodriguez(const Vector& v);
/**
* Rodriguez' formula to compute an incremental rotation matrix
* @param wx
* @param wy
* @param wz
* @return incremental rotation matrix
*/
inline Rot3 rodriguez(double wx, double wy, double wz) { return rodriguez(Vector_(3,wx,wy,wz));}
/**
* Update Rotation with incremental rotation
* @param v a vector of incremental roll,pitch,yaw

33
cpp/testRot3.cpp
View File

@ -60,20 +60,39 @@ TEST( Rot3, equals) {
CHECK(!R.equals(zero));
}
/* ************************************************************************* */
Rot3 slow_but_correct_rodriguez(const Vector& w) {
double t = norm_2(w);
Matrix J = skewSymmetric(w/t);
if (t < 1e-5) return Rot3();
Matrix R = eye(3, 3) + sin(t) * J + (1.0 - cos(t)) * (J * J);
return R; // matrix constructor will be tripped
}
/* ************************************************************************* */
TEST( Rot3, rodriguez) {
Rot3 rd1 = rodriguez(epsilon, 0, 0);
Vector temp(3); temp(0) = epsilon; temp(1) = 0; temp(2) = 0;
Rot3 rd2 = rodriguez(temp);
CHECK(assert_equal(rd1,rd2));
Rot3 R1 = rodriguez(epsilon, 0, 0);
Vector w = Vector_(3,epsilon,0.,0.);
Rot3 R2 = slow_but_correct_rodriguez(w);
CHECK(assert_equal(R1,R2));
}
/* ************************************************************************* */
TEST( Rot3, rodriguez2) {
Vector v(3); v(0) = 0; v(1) = 1; v(2) = 0;
Rot3 rd1 = rodriguez(v, 3.14/4.0);
Rot3 rd2(0.707388,0,0.706825,0,1,0,-0.706825,0,0.707388);
CHECK(rd1.equals(rd2,0.0001));
Rot3 R1 = rodriguez(v, 3.14/4.0);
Rot3 R2(0.707388,0,0.706825,
0,1,0,
-0.706825,0,0.707388);
CHECK(assert_equal(R1,R2,1e-5));
}
/* ************************************************************************* */
TEST( Rot3, rodriguez3) {
Vector w = Vector_(3,0.1,0.2,0.3);
Rot3 R1 = rodriguez(w/norm_2(w), norm_2(w));
Rot3 R2 = slow_but_correct_rodriguez(w);
CHECK(assert_equal(R1,R2));
}
/* ************************************************************************* */

37
cpp/timeRot3.cpp Normal file
View File

@ -0,0 +1,37 @@
/**
* @file timeRot3.cpp
* @brief time Rot3 functions
* @author Frank Dellaert
*/
#include <time.h>
#include <iostream>
#include "Rot3.h"
using namespace std;
using namespace gtsam;
int main()
{
int n = 3000000;
Vector v = Vector_(3,1.,0.,0.);
long timeLog = clock();
for(int i = 0; i < n; i++)
rodriguez(v,0.001);
long timeLog2 = clock();
double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
cout << seconds << " seconds" << endl;
cout << ((double)n/seconds) << " calls/second" << endl;
timeLog = clock();
for(int i = 0; i < n; i++)
rodriguez(v);
timeLog2 = clock();
seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
cout << seconds << " seconds" << endl;
cout << ((double)n/seconds) << " calls/second" << endl;
return 0;
}