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

revert from fixed size vectors to make it compile with existing numericalDerivatives

release/4.3a0
cbeall3 2014-10-22 22:00:25 -04:00
parent 941d6dfe07
commit a0be48ef75
1 changed files with 8 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
gtsam/geometry/tests/testRot3.cpp
View File

@ -138,12 +138,7 @@ TEST( Rot3, rodriguez4)
TEST( Rot3, retract)
{
Vector v = zero(3);
CHECK(assert_equal(R, R.retract(v)));
// test Canonical coordinates
Canonical<Rot3> chart;
Vector v2 = chart.apply(R);
CHECK(assert_equal(R, chart.retract(v2)));
CHECK(assert_equal(R.retract(v), R));
}
/* ************************************************************************* */
@ -206,9 +201,9 @@ Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta){
TEST( Rot3, rightJacobianExpMapSO3 )
{
// Linearization point
Vector3 thetahat; thetahat << 0.1, 0, 0;
Vector thetahat = (Vector(3) << 0.1, 0, 0);
Matrix expectedJacobian = numericalDerivative11<Rot3, Vector3>(
Matrix expectedJacobian = numericalDerivative11<Rot3, LieVector>(
boost::bind(&Rot3::Expmap, _1), thetahat);
Matrix actualJacobian = Rot3::rightJacobianExpMapSO3(thetahat);
CHECK(assert_equal(expectedJacobian, actualJacobian));
@ -218,10 +213,10 @@ TEST( Rot3, rightJacobianExpMapSO3 )
TEST( Rot3, rightJacobianExpMapSO3inverse )
{
// Linearization point
Vector3 thetahat; thetahat << 0.1,0.1,0; ///< Current estimate of rotation rate bias
Vector3 deltatheta; deltatheta << 0, 0, 0;
Vector thetahat = (Vector(3) << 0.1,0.1,0); ///< Current estimate of rotation rate bias
Vector deltatheta = (Vector(3) << 0, 0, 0);
Matrix expectedJacobian = numericalDerivative11<Vector3,Vector3>(
Matrix expectedJacobian = numericalDerivative11<LieVector>(
boost::bind(&evaluateLogRotation, thetahat, _1), deltatheta);
Matrix actualJacobian = Rot3::rightJacobianExpMapSO3inverse(thetahat);
EXPECT(assert_equal(expectedJacobian, actualJacobian));
@ -397,8 +392,8 @@ Vector3 testDexpL(const Vector3& dw) {
TEST( Rot3, dexpL) {
Matrix actualDexpL = Rot3::dexpL(w);
Matrix expectedDexpL = numericalDerivative11<Vector3, Vector3>(testDexpL,
Vector3::Zero(), 1e-2);
Matrix expectedDexpL = numericalDerivative11<LieVector>(testDexpL,
LieVector(zero(3)), 1e-2);
EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-5));
Matrix actualDexpInvL = Rot3::dexpInvL(w);