Baker–Campbell–Hausdorff formula: in non-commutative Lie groups, when composing exp(Z) = exp(X)exp(Y) it is not true that Z = X+Y. Instead, Z can be calculated using the BCH formula:
Z = X + Y + [X,Y]/2 + [X-Y,[X,Y]]/12 - [Y,[X,[X,Y]]]/24 + ... See http://en.wikipedia.org/wiki/Baker–Campbell–Hausdorff_formularelease/4.3a0
Frank Dellaert
parent
67b4834bdb
commit
5e00c58ea7
15
cpp/Lie.h
15
cpp/Lie.h
|
|
@ -104,4 +104,19 @@ namespace gtsam {
|
|||
inline Vector logmap(const Vector& p) { return p;}
|
||||
inline Vector logmap(const Vector& p1,const Vector& p2) { return p2-p1;}
|
||||
|
||||
/**
|
||||
* Three term approximation of the BakerÐCampbellÐHausdorff formula
|
||||
* In non-commutative Lie groups, when composing exp(Z) = exp(X)exp(Y)
|
||||
* it is not true that Z = X+Y. Instead, Z can be calculated using the BCH
|
||||
* formula: Z = X + Y + [X,Y]/2 + [X-Y,[X,Y]]/12 - [Y,[X,[X,Y]]]/24
|
||||
* http://en.wikipedia.org/wiki/BakerÐCampbellÐHausdorff_formula
|
||||
*/
|
||||
template<class T>
|
||||
T BCH(const T& X, const T& Y) {
|
||||
static const double _2 = 1. / 2., _12 = 1. / 12., _24 = 1. / 24.;
|
||||
T X_Y = bracket(X, Y);
|
||||
return X + Y + _2 * X_Y + _12 * bracket(X - Y, X_Y) - _24 * bracket(Y,
|
||||
bracket(X, X_Y));
|
||||
}
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
285
cpp/testRot3.cpp
285
cpp/testRot3.cpp
|
|
@ -12,96 +12,106 @@
|
|||
|
||||
using namespace gtsam;
|
||||
|
||||
Rot3 R = rodriguez(0.1,0.4,0.2);
|
||||
Point3 P(0.2,0.7,-2.0);
|
||||
double error = 1e-9, epsilon=0.001;
|
||||
Rot3 R = rodriguez(0.1, 0.4, 0.2);
|
||||
Point3 P(0.2, 0.7, -2.0);
|
||||
double error = 1e-9, epsilon = 0.001;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3, constructor) {
|
||||
Rot3 expected(eye(3,3));
|
||||
Vector r1(3), r2(3), r3(3);
|
||||
r1(0)=1;r1(1)=0;r1(2)=0;
|
||||
r2(0)=0;r2(1)=1;r2(2)=0;
|
||||
r3(0)=0;r3(1)=0;r3(2)=1;
|
||||
Rot3 actual(r1,r2,r3);
|
||||
CHECK(assert_equal(actual,expected));
|
||||
TEST( Rot3, constructor)
|
||||
{
|
||||
Rot3 expected(eye(3, 3));
|
||||
Vector r1(3), r2(3), r3(3);
|
||||
r1(0) = 1;
|
||||
r1(1) = 0;
|
||||
r1(2) = 0;
|
||||
r2(0) = 0;
|
||||
r2(1) = 1;
|
||||
r2(2) = 0;
|
||||
r3(0) = 0;
|
||||
r3(1) = 0;
|
||||
r3(2) = 1;
|
||||
Rot3 actual(r1, r2, r3);
|
||||
CHECK(assert_equal(actual,expected));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3, constructor2) {
|
||||
Matrix R = Matrix_(3,3,
|
||||
11.,12.,13.,
|
||||
21.,22.,23.,
|
||||
31.,32.,33.);
|
||||
Rot3 actual(R);
|
||||
Rot3 expected(11,12,13,
|
||||
21,22,23,
|
||||
31,32,33);
|
||||
CHECK(assert_equal(actual,expected));
|
||||
TEST( Rot3, constructor2)
|
||||
{
|
||||
Matrix R = Matrix_(3, 3, 11., 12., 13., 21., 22., 23., 31., 32., 33.);
|
||||
Rot3 actual(R);
|
||||
Rot3 expected(11, 12, 13, 21, 22, 23, 31, 32, 33);
|
||||
CHECK(assert_equal(actual,expected));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3, constructor3) {
|
||||
Rot3 expected(1,2,3,4,5,6,7,8,9);
|
||||
Point3 r1(1,4,7), r2(2,5,8), r3(3,6,9);
|
||||
CHECK(assert_equal(Rot3(r1,r2,r3),expected));
|
||||
TEST( Rot3, constructor3)
|
||||
{
|
||||
Rot3 expected(1, 2, 3, 4, 5, 6, 7, 8, 9);
|
||||
Point3 r1(1, 4, 7), r2(2, 5, 8), r3(3, 6, 9);
|
||||
CHECK(assert_equal(Rot3(r1,r2,r3),expected));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3, transpose) {
|
||||
Rot3 R(1,2,3,4,5,6,7,8,9);
|
||||
Point3 r1(1,2,3), r2(4,5,6), r3(7,8,9);
|
||||
CHECK(assert_equal(inverse(R),Rot3(r1,r2,r3)));
|
||||
TEST( Rot3, transpose)
|
||||
{
|
||||
Rot3 R(1, 2, 3, 4, 5, 6, 7, 8, 9);
|
||||
Point3 r1(1, 2, 3), r2(4, 5, 6), r3(7, 8, 9);
|
||||
CHECK(assert_equal(inverse(R),Rot3(r1,r2,r3)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3, equals) {
|
||||
CHECK(R.equals(R));
|
||||
Rot3 zero;
|
||||
CHECK(!R.equals(zero));
|
||||
TEST( Rot3, equals)
|
||||
{
|
||||
CHECK(R.equals(R));
|
||||
Rot3 zero;
|
||||
CHECK(!R.equals(zero));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 slow_but_correct_rodriguez(const Vector& w) {
|
||||
double t = norm_2(w);
|
||||
Matrix J = skewSymmetric(w/t);
|
||||
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 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, rodriguez)
|
||||
{
|
||||
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 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, rodriguez2)
|
||||
{
|
||||
Vector v(3);
|
||||
v(0) = 0;
|
||||
v(1) = 1;
|
||||
v(2) = 0;
|
||||
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));
|
||||
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));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3, expmap)
|
||||
{
|
||||
Vector v(3);
|
||||
fill(v.begin(), v.end(), 0);
|
||||
CHECK(assert_equal(expmap(R,v), R));
|
||||
Vector v(3);
|
||||
fill(v.begin(), v.end(), 0);
|
||||
CHECK(assert_equal(expmap(R,v), R));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -131,17 +141,17 @@ TEST(Rot3, log)
|
|||
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 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)));
|
||||
Vector w8 = Vector_(3, 0.0, 0.0, boost::math::constants::pi<double>());
|
||||
Rot3 R8 = rodriguez(w8);
|
||||
CHECK(assert_equal(w8, logmap(R8)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Rot3, manifold)
|
||||
TEST(Rot3, manifold)
|
||||
{
|
||||
Rot3 gR1 = rodriguez(0.1, 0.4, 0.2);
|
||||
Rot3 gR2 = rodriguez(0.3, 0.1, 0.7);
|
||||
|
|
@ -159,46 +169,74 @@ TEST(Rot3, log)
|
|||
CHECK(assert_equal(d12,-d21));
|
||||
|
||||
// lines in canonical coordinates correspond to Abelian subgroups in SO(3)
|
||||
Vector d = Vector_(3,0.1,0.2,0.3);
|
||||
Vector d = Vector_(3, 0.1, 0.2, 0.3);
|
||||
// exp(-d)=inverse(exp(d))
|
||||
CHECK(assert_equal(expmap<Rot3>(-d),inverse(expmap<Rot3>(d))));
|
||||
// exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d)
|
||||
Rot3 R2 = expmap<Rot3>(2*d);
|
||||
Rot3 R3 = expmap<Rot3>(3*d);
|
||||
Rot3 R5 = expmap<Rot3>(5*d);
|
||||
Rot3 R2 = expmap<Rot3> (2 * d);
|
||||
Rot3 R3 = expmap<Rot3> (3 * d);
|
||||
Rot3 R5 = expmap<Rot3> (5 * d);
|
||||
CHECK(assert_equal(R5,R2*R3));
|
||||
CHECK(assert_equal(R5,R3*R2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
class AngularVelocity: public Point3 {
|
||||
public:
|
||||
AngularVelocity(const Point3& p) :
|
||||
Point3(p) {
|
||||
}
|
||||
AngularVelocity(double wx, double wy, double wz) :
|
||||
Point3(wx, wy, wz) {
|
||||
}
|
||||
};
|
||||
|
||||
AngularVelocity bracket(const AngularVelocity& X, const AngularVelocity& Y) {
|
||||
return cross(X, Y);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Rot3, BCH)
|
||||
{
|
||||
// Approximate exmap by BCH formula
|
||||
AngularVelocity w1(0.2, -0.1, 0.1);
|
||||
AngularVelocity w2(0.01, 0.02, -0.03);
|
||||
Rot3 R1 = expmap<Rot3> (w1.vector()), R2 = expmap<Rot3> (w2.vector());
|
||||
Rot3 R3 = R1 * R2;
|
||||
Vector expected = logmap(R3);
|
||||
Vector actual = BCH(w1, w2).vector();
|
||||
CHECK(assert_equal(expected, actual,1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// rotate derivatives
|
||||
|
||||
TEST( Rot3, Drotate1)
|
||||
{
|
||||
Matrix computed = Drotate1(R, P);
|
||||
Matrix numerical = numericalDerivative21(rotate,R,P);
|
||||
CHECK(assert_equal(numerical,computed,error));
|
||||
Matrix computed = Drotate1(R, P);
|
||||
Matrix numerical = numericalDerivative21(rotate, R, P);
|
||||
CHECK(assert_equal(numerical,computed,error));
|
||||
}
|
||||
|
||||
TEST( Rot3, Drotate1_)
|
||||
{
|
||||
Matrix computed = Drotate1(inverse(R), P);
|
||||
Matrix numerical = numericalDerivative21(rotate,inverse(R),P);
|
||||
CHECK(assert_equal(numerical,computed,error));
|
||||
{
|
||||
Matrix computed = Drotate1(inverse(R), P);
|
||||
Matrix numerical = numericalDerivative21(rotate, inverse(R), P);
|
||||
CHECK(assert_equal(numerical,computed,error));
|
||||
}
|
||||
|
||||
TEST( Rot3, Drotate2_DNrotate2)
|
||||
{
|
||||
Matrix computed = Drotate2(R);
|
||||
Matrix numerical = numericalDerivative22(rotate,R,P);
|
||||
CHECK(assert_equal(numerical,computed,error));
|
||||
Matrix computed = Drotate2(R);
|
||||
Matrix numerical = numericalDerivative22(rotate, R, P);
|
||||
CHECK(assert_equal(numerical,computed,error));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3, unrotate)
|
||||
{
|
||||
Point3 w = R*P;
|
||||
CHECK(assert_equal(unrotate(R,w),P));
|
||||
Point3 w = R * P;
|
||||
CHECK(assert_equal(unrotate(R,w),P));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -206,16 +244,16 @@ TEST( Rot3, unrotate)
|
|||
|
||||
TEST( Rot3, Dunrotate1)
|
||||
{
|
||||
Matrix computed = Dunrotate1(R, P);
|
||||
Matrix numerical = numericalDerivative21(unrotate,R,P);
|
||||
CHECK(assert_equal(numerical,computed,error));
|
||||
Matrix computed = Dunrotate1(R, P);
|
||||
Matrix numerical = numericalDerivative21(unrotate, R, P);
|
||||
CHECK(assert_equal(numerical,computed,error));
|
||||
}
|
||||
|
||||
TEST( Rot3, Dunrotate2_DNunrotate2)
|
||||
{
|
||||
Matrix computed = Dunrotate2(R);
|
||||
Matrix numerical = numericalDerivative22(unrotate,R,P);
|
||||
CHECK(assert_equal(numerical,computed,error));
|
||||
Matrix computed = Dunrotate2(R);
|
||||
Matrix numerical = numericalDerivative22(unrotate, R, P);
|
||||
CHECK(assert_equal(numerical,computed,error));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -224,16 +262,18 @@ TEST( Rot3, compose )
|
|||
Rot3 R1 = rodriguez(0.1, 0.2, 0.3);
|
||||
Rot3 R2 = rodriguez(0.2, 0.3, 0.5);
|
||||
|
||||
Rot3 expected = R1*R2;
|
||||
Rot3 expected = R1 * R2;
|
||||
Rot3 actual = compose(R1, R2);
|
||||
CHECK(assert_equal(expected,actual));
|
||||
|
||||
Matrix numericalH1 = numericalDerivative21<Rot3,Rot3,Rot3>(compose, R1, R2, 1e-5);
|
||||
Matrix numericalH1 = numericalDerivative21<Rot3, Rot3, Rot3> (compose, R1,
|
||||
R2, 1e-5);
|
||||
Matrix actualH1 = Dcompose1(R1, R2);
|
||||
CHECK(assert_equal(numericalH1,actualH1));
|
||||
|
||||
Matrix actualH2 = Dcompose2(R1, R2);
|
||||
Matrix numericalH2 = numericalDerivative22<Rot3,Rot3,Rot3>(compose, R1, R2, 1e-5);
|
||||
Matrix numericalH2 = numericalDerivative22<Rot3, Rot3, Rot3> (compose, R1,
|
||||
R2, 1e-5);
|
||||
CHECK(assert_equal(numericalH2,actualH2));
|
||||
}
|
||||
|
||||
|
|
@ -247,7 +287,7 @@ TEST( Rot3, inverse )
|
|||
CHECK(assert_equal(I,R*inverse(R)));
|
||||
CHECK(assert_equal(I,inverse(R)*R));
|
||||
|
||||
Matrix numericalH = numericalDerivative11<Rot3,Rot3>(inverse, R, 1e-5);
|
||||
Matrix numericalH = numericalDerivative11<Rot3, Rot3> (inverse, R, 1e-5);
|
||||
Matrix actualH = Dinverse(R);
|
||||
CHECK(assert_equal(numericalH,actualH));
|
||||
}
|
||||
|
|
@ -263,7 +303,7 @@ TEST( Rot3, between )
|
|||
Rot3 R1 = rodriguez(0.1, 0.2, 0.3);
|
||||
Rot3 R2 = rodriguez(0.2, 0.3, 0.5);
|
||||
|
||||
Rot3 expected = inverse(R1)*R2;
|
||||
Rot3 expected = inverse(R1) * R2;
|
||||
Rot3 actual = between(R1, R2);
|
||||
CHECK(assert_equal(expected,actual));
|
||||
|
||||
|
|
@ -287,32 +327,23 @@ TEST( Rot3, xyz )
|
|||
// z
|
||||
// | * Y=(ct,st)
|
||||
// x----y
|
||||
Rot3 expected1(
|
||||
1, 0, 0,
|
||||
0, ct,-st,
|
||||
0, st, ct);
|
||||
Rot3 expected1(1, 0, 0, 0, ct, -st, 0, st, ct);
|
||||
CHECK(assert_equal(expected1,Rot3::Rx(t)));
|
||||
|
||||
// x
|
||||
// | * Z=(ct,st)
|
||||
// y----z
|
||||
Rot3 expected2(
|
||||
ct, 0, st,
|
||||
0, 1, 0,
|
||||
-st, 0, ct);
|
||||
Rot3 expected2(ct, 0, st, 0, 1, 0, -st, 0, ct);
|
||||
CHECK(assert_equal(expected2,Rot3::Ry(t)));
|
||||
|
||||
// y
|
||||
// | X=* (ct,st)
|
||||
// z----x
|
||||
Rot3 expected3(
|
||||
ct, -st, 0,
|
||||
st, ct, 0,
|
||||
0, 0, 1);
|
||||
Rot3 expected3(ct, -st, 0, st, ct, 0, 0, 0, 1);
|
||||
CHECK(assert_equal(expected3,Rot3::Rz(t)));
|
||||
|
||||
// Check compound rotation
|
||||
Rot3 expected = Rot3::Rz(0.3)*Rot3::Ry(0.2)*Rot3::Rx(0.1);
|
||||
Rot3 expected = Rot3::Rz(0.3) * Rot3::Ry(0.2) * Rot3::Rx(0.1);
|
||||
CHECK(assert_equal(expected,Rot3::RzRyRx(0.1,0.2,0.3)));
|
||||
}
|
||||
|
||||
|
|
@ -331,7 +362,7 @@ TEST( Rot3, yaw_pitch_roll )
|
|||
CHECK(assert_equal(Rot3::Rx(t),Rot3::roll(t)));
|
||||
|
||||
// Check compound rotation
|
||||
Rot3 expected = Rot3::yaw(0.1)*Rot3::pitch(0.2)*Rot3::roll(0.3);
|
||||
Rot3 expected = Rot3::yaw(0.1) * Rot3::pitch(0.2) * Rot3::roll(0.3);
|
||||
CHECK(assert_equal(expected,Rot3::ypr(0.1,0.2,0.3)));
|
||||
}
|
||||
|
||||
|
|
@ -339,37 +370,37 @@ TEST( Rot3, yaw_pitch_roll )
|
|||
TEST( Rot3, RQ)
|
||||
{
|
||||
// Try RQ on a pure rotation
|
||||
Matrix actualK; Vector actual;
|
||||
boost::tie(actualK,actual) = RQ(R.matrix());
|
||||
Vector expected = Vector_(3,0.14715, 0.385821, 0.231671);
|
||||
CHECK(assert_equal(eye(3),actualK));
|
||||
CHECK(assert_equal(expected,actual,1e-6));
|
||||
Matrix actualK;
|
||||
Vector actual;
|
||||
boost::tie(actualK, actual) = RQ(R.matrix());
|
||||
Vector expected = Vector_(3, 0.14715, 0.385821, 0.231671);
|
||||
CHECK(assert_equal(eye(3),actualK));
|
||||
CHECK(assert_equal(expected,actual,1e-6));
|
||||
|
||||
// Try using xyz call, asserting that Rot3::RzRyRx(x,y,z).xyz()==[x;y;z]
|
||||
CHECK(assert_equal(expected,R.xyz(),1e-6));
|
||||
CHECK(assert_equal(Vector_(3,0.1,0.2,0.3),Rot3::RzRyRx(0.1,0.2,0.3).xyz()));
|
||||
// Try using xyz call, asserting that Rot3::RzRyRx(x,y,z).xyz()==[x;y;z]
|
||||
CHECK(assert_equal(expected,R.xyz(),1e-6));
|
||||
CHECK(assert_equal(Vector_(3,0.1,0.2,0.3),Rot3::RzRyRx(0.1,0.2,0.3).xyz()));
|
||||
|
||||
// Try using ypr call, asserting that Rot3::ypr(y,p,r).ypr()==[y;p;r]
|
||||
CHECK(assert_equal(Vector_(3,0.1,0.2,0.3),Rot3::ypr(0.1,0.2,0.3).ypr()));
|
||||
// Try using ypr call, asserting that Rot3::ypr(y,p,r).ypr()==[y;p;r]
|
||||
CHECK(assert_equal(Vector_(3,0.1,0.2,0.3),Rot3::ypr(0.1,0.2,0.3).ypr()));
|
||||
|
||||
// Try ypr for pure yaw-pitch-roll matrices
|
||||
CHECK(assert_equal(Vector_(3,0.1,0.0,0.0),Rot3::yaw (0.1).ypr()));
|
||||
CHECK(assert_equal(Vector_(3,0.0,0.1,0.0),Rot3::pitch(0.1).ypr()));
|
||||
CHECK(assert_equal(Vector_(3,0.0,0.0,0.1),Rot3::roll (0.1).ypr()));
|
||||
// Try ypr for pure yaw-pitch-roll matrices
|
||||
CHECK(assert_equal(Vector_(3,0.1,0.0,0.0),Rot3::yaw (0.1).ypr()));
|
||||
CHECK(assert_equal(Vector_(3,0.0,0.1,0.0),Rot3::pitch(0.1).ypr()));
|
||||
CHECK(assert_equal(Vector_(3,0.0,0.0,0.1),Rot3::roll (0.1).ypr()));
|
||||
|
||||
// Try RQ to recover calibration from 3*3 sub-block of projection matrix
|
||||
Matrix K = Matrix_(3,3,
|
||||
500.0, 0.0, 320.0,
|
||||
0.0, 500.0, 240.0,
|
||||
0.0, 0.0, 1.0
|
||||
);
|
||||
Matrix A = K*R.matrix();
|
||||
boost::tie(actualK,actual) = RQ(A);
|
||||
CHECK(assert_equal(K,actualK));
|
||||
CHECK(assert_equal(expected,actual,1e-6));
|
||||
// Try RQ to recover calibration from 3*3 sub-block of projection matrix
|
||||
Matrix K = Matrix_(3, 3, 500.0, 0.0, 320.0, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0);
|
||||
Matrix A = K * R.matrix();
|
||||
boost::tie(actualK, actual) = RQ(A);
|
||||
CHECK(assert_equal(K,actualK));
|
||||
CHECK(assert_equal(expected,actual,1e-6));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main(){ TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue