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

new-style combined unrotate

release/4.3a0
Frank Dellaert 2010-03-01 01:36:27 +00:00
parent 20ad08e48f
commit 619190a818
3 changed files with 25 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
cpp/Rot3.cpp
View File

@ -182,24 +182,19 @@ namespace gtsam {
/* ************************************************************************* */
Point3 unrotate(const Rot3& R, const Point3& p) {
return Point3(
R.r1().x() * p.x() + R.r1().y() * p.y() + R.r1().z() * p.z(),
R.r2().x() * p.x() + R.r2().y() * p.y() + R.r2().z() * p.z(),
R.r3().x() * p.x() + R.r3().y() * p.y() + R.r3().z() * p.z()
);
const Matrix Rt(R.transpose());
return Rt*p.vector(); // q = Rt*p
}
/* ************************************************************************* */
/** see libraries/caml/geometry/math.lyx, derivative of unrotate */
/* ************************************************************************* */
Matrix Dunrotate1(const Rot3 & R, const Point3 & p) {
Point3 q = unrotate(R,p);
return skewSymmetric(q.x(), q.y(), q.z());
}
/* ************************************************************************* */
Matrix Dunrotate2(const Rot3 & R) {
return R.transpose();
// see doc/math.lyx, SO(3) section
Point3 unrotate(const Rot3& R, const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
const Matrix Rt(R.transpose());
Point3 q(Rt*p.vector()); // q = Rt*p
if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z());
if (H2) *H2 = Rt;
return q;
}
/* ************************************************************************* */

4
cpp/Rot3.h
View File

@ -188,8 +188,8 @@ namespace gtsam {
* frame = R'*p
*/
Point3 unrotate(const Rot3& R, const Point3& p);
Matrix Dunrotate1(const Rot3& R, const Point3& p);
Matrix Dunrotate2(const Rot3& R); // does not depend on p !
Point3 unrotate(const Rot3& R, const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
/**
* compose two rotations i.e., R=R1*R2

35
cpp/testRot3.cpp
View File

@ -213,47 +213,38 @@ TEST(Rot3, BCH)
TEST( Rot3, Drotate1)
{
Matrix computed = Drotate1(R, P);
Matrix actualDrotate1 = Drotate1(R, P);
Matrix numerical = numericalDerivative21(rotate, R, P);
CHECK(assert_equal(numerical,computed,error));
CHECK(assert_equal(numerical,actualDrotate1,error));
}
TEST( Rot3, Drotate1_)
{
Matrix computed = Drotate1(inverse(R), P);
Matrix actualDrotate1 = Drotate1(inverse(R), P);
Matrix numerical = numericalDerivative21(rotate, inverse(R), P);
CHECK(assert_equal(numerical,computed,error));
CHECK(assert_equal(numerical,actualDrotate1,error));
}
TEST( Rot3, Drotate2_DNrotate2)
{
Matrix computed = Drotate2(R);
Matrix actualDrotate2 = Drotate2(R);
Matrix numerical = numericalDerivative22(rotate, R, P);
CHECK(assert_equal(numerical,computed,error));
CHECK(assert_equal(numerical,actualDrotate2,error));
}
/* ************************************************************************* */
TEST( Rot3, unrotate)
{
Point3 w = R * P;
CHECK(assert_equal(unrotate(R,w),P));
}
Matrix H1,H2;
Point3 actual = unrotate(R,w,H1,H2);
CHECK(assert_equal(P,actual));
/* ************************************************************************* */
// unrotate derivatives
Matrix numerical1 = numericalDerivative21(unrotate, R, w);
CHECK(assert_equal(numerical1,H1,error));
TEST( Rot3, Dunrotate1)
{
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 numerical2 = numericalDerivative22(unrotate, R, w);
CHECK(assert_equal(numerical2,H2,error));
}
/* ************************************************************************* */