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

Added a test that diagnoses problem with Rot3 Local/Retract

release/4.3a0
dellaert 2015-02-11 14:09:20 +01:00
parent 6f34725cff
commit 0d2bb1bb01
1 changed files with 50 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

55
gtsam/geometry/tests/testRot3Q.cpp
View File

@ -15,20 +15,65 @@
* @author Alireza Fathi
*/
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Quaternion.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/lieProxies.h>
#include <boost/math/constants/constants.hpp>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
#ifdef GTSAM_USE_QUATERNIONS
// No quaternion only tests
//******************************************************************************
TEST(Rot3Q , Compare) {
using boost::none;
// We set up expected values with quaternions
typedef Quaternion Q;
typedef traits<Q> TQ;
typedef TQ::ChartJacobian OJ;
// We check Rot3 expressions
typedef Rot3 R;
typedef traits<R> TR;
// Define test values
Q q1(Eigen::AngleAxisd(1, Vector3(0, 0, 1)));
Q q2(Eigen::AngleAxisd(2, Vector3(0, 1, 0)));
R R1(q1), R2(q2);
// Check Compose
Q q3 = TQ::Compose(q1, q2, none, none);
R R3 = TR::Compose(R1, R2, none, none);
EXPECT(assert_equal(R(q3), R3));
// Check Retract/Local
Vector3 v(1e-5, 0, 0);
Vector3 vQ = TQ::Local(q3, TQ::Retract(q3, v));
Vector3 vR = TR::Local(R3, TR::Retract(R3, v));
EXPECT(assert_equal(vQ, vR));
// Check Retract/Local of Compose
Vector3 vQ1 = TQ::Local(q3, TQ::Compose(q1, TQ::Retract(q2, v)));
Vector3 vR1 = TR::Local(R3, TR::Compose(R1, TR::Retract(R2, v)));
EXPECT(assert_equal(vQ1, vR1));
Vector3 vQ2 = TQ::Local(q3, TQ::Compose(q1, TQ::Retract(q2, -v)));
Vector3 vR2 = TR::Local(R3, TR::Compose(R1, TR::Retract(R2, -v)));
EXPECT(assert_equal(vQ2, vR2));
EXPECT(assert_equal<Vector3>((vQ1 - vQ2) / 0.2, (vR1 - vR2) / 0.2));
cout << (vR1 - vR2) / 0.2 << endl;
// Check Compose Derivatives
Matrix HQ, HR;
HQ = numericalDerivative42<Q, Q, Q, OJ, OJ>(TQ::Compose, q1, q2, none, none);
HR = numericalDerivative42<R, R, R, OJ, OJ>(TR::Compose, R1, R2, none, none);
EXPECT(assert_equal(HQ, HR));
}
#endif