From 87a705468d19b29eb4cfbeb8da774a24c9dc79d8 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Wed, 6 Jun 2012 13:04:47 +0000 Subject: [PATCH] Removed unnecessary tests/examples, consolidated utility functions --- gtsam_unstable/dynamics/PoseRTV.cpp | 38 ++- gtsam_unstable/dynamics/PoseRTV.h | 14 + gtsam_unstable/dynamics/imu_examples.h | 273 ------------------ gtsam_unstable/dynamics/inertialUtils.cpp | 51 ---- gtsam_unstable/dynamics/inertialUtils.h | 32 -- .../dynamics/tests/testIMUSystem.cpp | 34 --- .../dynamics/tests/testInertialUtils.cpp | 34 --- gtsam_unstable/dynamics/tests/testPoseRTV.cpp | 215 +------------- 8 files changed, 61 insertions(+), 630 deletions(-) delete mode 100644 gtsam_unstable/dynamics/imu_examples.h delete mode 100644 gtsam_unstable/dynamics/inertialUtils.cpp delete mode 100644 gtsam_unstable/dynamics/inertialUtils.h delete mode 100644 gtsam_unstable/dynamics/tests/testInertialUtils.cpp diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp index e78f5a62d..c10885fc0 100644 --- a/gtsam_unstable/dynamics/PoseRTV.cpp +++ b/gtsam_unstable/dynamics/PoseRTV.cpp @@ -10,7 +10,6 @@ #include #include -#include #include namespace gtsam { @@ -201,7 +200,7 @@ Vector PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const { // rotation rates // just using euler angles based on matlab code // FIXME: this is silly - we shouldn't use differences in Euler angles - Matrix Enb = dynamics::RRTMnb(r1); + Matrix Enb = RRTMnb(r1); Vector euler1 = r1.xyz(), euler2 = r2.xyz(); Vector dR = euler2 - euler1; @@ -276,4 +275,39 @@ PoseRTV PoseRTV::transformed_from(const Pose3& trans, return PoseRTV(newpose, newvel); } +/* ************************************************************************* */ +Matrix PoseRTV::RRTMbn(const Vector& euler) { + assert(euler.size() == 3); + const double s1 = sin(euler(1-1)), c1 = cos(euler(1-1)); + const double t2 = tan(euler(2-1)), c2 = cos(euler(2-1)); + Matrix Ebn(3,3); + Ebn << 1.0, s1 * t2, c1 * t2, + 0.0, c1, -s1, + 0.0, s1 / c2, c1 / c2; + return Ebn; +} + +/* ************************************************************************* */ +Matrix PoseRTV::RRTMbn(const Rot3& att) { + return PoseRTV::RRTMbn(att.rpy()); +} + +/* ************************************************************************* */ +Matrix PoseRTV::RRTMnb(const Vector& euler) { + assert(euler.size() == 3); + Matrix Enb(3,3); + const double s1 = sin(euler(1-1)), c1 = cos(euler(1-1)); + const double s2 = sin(euler(2-1)), c2 = cos(euler(2-1)); + Enb << 1.0, 0.0, -s2, + 0.0, c1, s1*c2, + 0.0, -s1, c1*c2; + return Enb; +} + +/* ************************************************************************* */ +Matrix PoseRTV::RRTMnb(const Rot3& att) { + return PoseRTV::RRTMnb(att.rpy()); +} + +/* ************************************************************************* */ } // \namespace gtsam diff --git a/gtsam_unstable/dynamics/PoseRTV.h b/gtsam_unstable/dynamics/PoseRTV.h index b2e325f53..077756f56 100644 --- a/gtsam_unstable/dynamics/PoseRTV.h +++ b/gtsam_unstable/dynamics/PoseRTV.h @@ -153,6 +153,20 @@ public: boost::optional Dglobal=boost::none, boost::optional Dtrans=boost::none) const; + // Utility functions + + /// RRTMbn - Function computes the rotation rate transformation matrix from + /// body axis rates to euler angle (global) rates + static Matrix RRTMbn(const Vector& euler); + + static Matrix RRTMbn(const Rot3& att); + + /// RRTMnb - Function computes the rotation rate transformation matrix from + /// euler angle rates to body axis rates + static Matrix RRTMnb(const Vector& euler); + + static Matrix RRTMnb(const Rot3& att); + private: /** Serialization function */ friend class boost::serialization::access; diff --git a/gtsam_unstable/dynamics/imu_examples.h b/gtsam_unstable/dynamics/imu_examples.h deleted file mode 100644 index 90bbc9d8b..000000000 --- a/gtsam_unstable/dynamics/imu_examples.h +++ /dev/null @@ -1,273 +0,0 @@ -/** - * @file imu_examples.h - * @brief Example measurements from ACFR matlab simulation - * @author Alex Cunningham - */ - -#pragma once - -#include -#include - -namespace gtsam { -namespace examples { - -// examples pulled from simulated dataset - -// case pulled from dataset (frame 5000, no noise, flying robot) -namespace frame5000 { -double dt=0.010000; - -// previous frame -gtsam::Point3 posInit( - -34.959663877411039, - -36.312388041359441, - -9.929634578582256); -gtsam::Vector velInit = gtsam::Vector_(3, - -2.325950469365050, - -5.545469040035986, - 0.026939998635121); -gtsam::Vector attInit = gtsam::Vector_(3, - -0.005702574137157, - -0.029956547314287, - 1.625011216344428); - -// IMU measurement (accel (0-2), gyro (3-5)) - from current frame -gtsam::Vector accel = gtsam::Vector_(3, - 1.188806676070333, - -0.183885870345845, - -9.870747316422888); -gtsam::Vector gyro = gtsam::Vector_(3, - 0.0, - 0.026142019158168, - -2.617993877991494); - -// resulting frame -gtsam::Point3 posFinal( - -34.982904302954310, - -36.367693859767584, - -9.929367164045985); -gtsam::Vector velFinal = gtsam::Vector_(3, - -2.324042554327658, - -5.530581840815272, - 0.026741453627181); -gtsam::Vector attFinal = gtsam::Vector_(3, - -0.004918046965288, - -0.029844423605949, - 1.598818460739227); - -PoseRTV init (posInit, gtsam::Rot3::RzRyRx(attInit), velInit); -PoseRTV final(posFinal, gtsam::Rot3::RzRyRx(attFinal), velFinal); -} - -// case pulled from dataset (frame 10000, no noise, flying robot) -namespace frame10000 { -double dt=0.010000; - -// previous frame -gtsam::Point3 posInit( - -30.320731549352189, - -1.988742283760434, - -9.946212692656349); -gtsam::Vector velInit = gtsam::Vector_(3, - -0.094887047280235, - -5.200243574047883, - -0.006106911050672); -gtsam::Vector attInit = gtsam::Vector_(3, - -0.049884854704728, - -0.004630595995732, - -1.952691683647753); - -// IMU measurement (accel (0-2), gyro (3-5)) - from current frame -gtsam::Vector accel = gtsam::Vector_(3, - 0.127512027192321, - 0.508566822495083, - -9.987963604829643); -gtsam::Vector gyro = gtsam::Vector_(3, - 0.0, - 0.004040957322961, - 2.617993877991494); - -// resulting frame -gtsam::Point3 posFinal( - -30.321685191305157, - -2.040760054006146, - -9.946292804391417); -gtsam::Vector velFinal = gtsam::Vector_(3, - -0.095364195297074, - -5.201777024575911, - -0.008011173506779); -gtsam::Vector attFinal = gtsam::Vector_(3, - -0.050005924151669, - -0.003284795837998, - -1.926546047162884); - -PoseRTV init (posInit, gtsam::Rot3::RzRyRx(attInit), velInit); -PoseRTV final(posFinal, gtsam::Rot3::RzRyRx(attFinal), velFinal); -} - -// case pulled from dataset (frame at time 4.00, no noise, flying robot, poses from 3.99 to 4.0) -namespace flying400 { -double dt=0.010000; - -// start pose -// time 3.9900000e+00 -// pos (x,y,z) 1.8226188e+01 -6.7168156e+01 -9.8236334e+00 -// vel (dx,dy,dz) -5.2887267e+00 1.6117880e-01 -2.2665072e-02 -// att (r, p, y) 1.0928413e-02 -2.0811771e-02 2.7206011e+00 - -// previous frame -gtsam::Point3 posInit( - 1.8226188e+01, -6.7168156e+01, -9.8236334e+00); -gtsam::Vector velInit = gtsam::Vector_(3,-5.2887267e+00, 1.6117880e-01, -2.2665072e-02); -gtsam::Vector attInit = gtsam::Vector_(3, 1.0928413e-02, -2.0811771e-02, 2.7206011e+00); - -// time 4.0000000e+00 -// accel 3.4021509e-01 -3.4413998e-01 -9.8822495e+00 -// gyro 0.0000000e+00 1.8161697e-02 -2.6179939e+00 - -// IMU measurement (accel (0-2), gyro (3-5)) - ax, ay, az, r, p, y -gtsam::Vector accel = gtsam::Vector_(3, - 3.4021509e-01, -3.4413998e-01, -9.8822495e+00); -gtsam::Vector gyro = gtsam::Vector_(3, - 0.0000000e+00, 1.8161697e-02, -2.6179939e+00); // original version (possibly r, p, y) - -// final pose -// time 4.0000000e+00 -// pos (x,y,z) 1.8173262e+01 -6.7166500e+01 -9.8238667e+00 -// vel (vx,vy,vz) -5.2926092e+00 1.6559974e-01 -2.3330881e-02 -// att (r, p, y) 1.1473269e-02 -2.0344066e-02 2.6944191e+00 - -// resulting frame -gtsam::Point3 posFinal(1.8173262e+01, -6.7166500e+01, -9.8238667e+00); -gtsam::Vector velFinal = gtsam::Vector_(3,-5.2926092e+00, 1.6559974e-01, -2.3330881e-02); -gtsam::Vector attFinal = gtsam::Vector_(3, 1.1473269e-02, -2.0344066e-02, 2.6944191e+00); - -// full states -PoseRTV init (posInit, gtsam::Rot3::ypr( attInit(2), attInit(1), attInit(0)), velInit); -PoseRTV final(posFinal, gtsam::Rot3::ypr(attFinal(2), attFinal(1), attFinal(0)), velFinal); - -} // \namespace flying400 - -namespace flying650 { -double dt=0.010000; - -// from time 6.49 to 6.50 in robot F2 - - -// frame (6.49) -// 6.4900000e+00 -3.8065039e+01 -6.4788024e+01 -9.8947445e+00 -5.0099274e+00 1.5999675e-01 -1.7762191e-02 -5.7708025e-03 -1.0109000e-02 -3.0465662e+00 -gtsam::Point3 posInit( - -3.8065039e+01, -6.4788024e+01, -9.8947445e+00); -gtsam::Vector velInit = gtsam::Vector_(3,-5.0099274e+00, 1.5999675e-01, -1.7762191e-02); -gtsam::Vector attInit = gtsam::Vector_(3,-5.7708025e-03, -1.0109000e-02, -3.0465662e+00); - -// IMU measurement (accel (0-2), gyro (3-5)) - ax, ay, az, r, p, y -// 6.5000000e+00 -9.2017256e-02 8.6770069e-02 -9.8213017e+00 0.0000000e+00 1.0728860e-02 -2.6179939e+00 -gtsam::Vector accel = gtsam::Vector_(3, - -9.2017256e-02, 8.6770069e-02, -9.8213017e+00); -gtsam::Vector gyro = gtsam::Vector_(3, - 0.0000000e+00, 1.0728860e-02, -2.6179939e+00); // in r,p,y - -// resulting frame (6.50) -// 6.5000000e+00 -3.8115139e+01 -6.4786428e+01 -9.8949233e+00 -5.0099817e+00 1.5966531e-01 -1.7882777e-02 -5.5061386e-03 -1.0152792e-02 -3.0727477e+00 -gtsam::Point3 posFinal(-3.8115139e+01, -6.4786428e+01, -9.8949233e+00); -gtsam::Vector velFinal = gtsam::Vector_(3,-5.0099817e+00, 1.5966531e-01, -1.7882777e-02); -gtsam::Vector attFinal = gtsam::Vector_(3,-5.5061386e-03, -1.0152792e-02, -3.0727477e+00); - -// full states -PoseRTV init (posInit, gtsam::Rot3::ypr( attInit(2), attInit(1), attInit(0)), velInit); -PoseRTV final(posFinal, gtsam::Rot3::ypr(attFinal(2), attFinal(1), attFinal(0)), velFinal); - -} // \namespace flying650 - - -namespace flying39 { -double dt=0.010000; - -// from time 0.38 to 0.39 in robot F1 - -// frame (0.38) -// 3.8000000e-01 3.4204706e+01 -6.7413834e+01 -9.9996055e+00 -2.2484370e-02 -1.3603911e-03 1.5267496e-02 7.9033358e-04 -1.4946656e-02 -3.1174147e+00 -gtsam::Point3 posInit( 3.4204706e+01, -6.7413834e+01, -9.9996055e+00); -gtsam::Vector velInit = gtsam::Vector_(3,-2.2484370e-02, -1.3603911e-03, 1.5267496e-02); -gtsam::Vector attInit = gtsam::Vector_(3, 7.9033358e-04, -1.4946656e-02, -3.1174147e+00); - -// IMU measurement (accel (0-2), gyro (3-5)) - ax, ay, az, r, p, y -// 3.9000000e-01 7.2229967e-01 -9.5790214e-03 -9.3943087e+00 0.0000000e+00 -2.9157400e-01 -2.6179939e+00 -gtsam::Vector accel = gtsam::Vector_(3, 7.2229967e-01, -9.5790214e-03, -9.3943087e+00); -gtsam::Vector gyro = gtsam::Vector_(3, 0.0000000e+00, -2.9157400e-01, -2.6179939e+00); // in r,p,y - -// resulting frame (0.39) -// 3.9000000e-01 3.4204392e+01 -6.7413848e+01 -9.9994098e+00 -3.1382246e-02 -1.3577532e-03 1.9568177e-02 1.1816996e-03 -1.7841704e-02 3.1395854e+00 -gtsam::Point3 posFinal(3.4204392e+01, -6.7413848e+01, -9.9994098e+00); -gtsam::Vector velFinal = gtsam::Vector_(3,-3.1382246e-02, -1.3577532e-03, 1.9568177e-02); -gtsam::Vector attFinal = gtsam::Vector_(3, 1.1816996e-03, -1.7841704e-02, 3.1395854e+00); - -// full states -PoseRTV init (posInit, gtsam::Rot3::ypr( attInit(2), attInit(1), attInit(0)), velInit); -PoseRTV final(posFinal, gtsam::Rot3::ypr(attFinal(2), attFinal(1), attFinal(0)), velFinal); - -} // \namespace flying39 - -namespace ground200 { -double dt=0.010000; - -// from time 2.00 to 2.01 in robot G1 - -// frame (2.00) -// 2.0000000e+00 3.6863524e+01 -8.4874053e+01 0.0000000e+00 -7.9650687e-01 1.8345508e+00 0.0000000e+00 0.0000000e+00 0.0000000e+00 1.9804083e+00 -gtsam::Point3 posInit(3.6863524e+01, -8.4874053e+01, 0.0000000e+00); -gtsam::Vector velInit = gtsam::Vector_(3,-7.9650687e-01, 1.8345508e+00, 0.0000000e+00); -gtsam::Vector attInit = gtsam::Vector_(3, 0.0000000e+00, 0.0000000e+00, 1.9804083e+00); - -// IMU measurement (accel (0-2), gyro (3-5)) - ax, ay, az, r, p, y -// 2.0100000e+00 1.0000000e+00 8.2156504e-15 -9.8100000e+00 0.0000000e+00 0.0000000e+00 0.0000000e+00 -gtsam::Vector accel = gtsam::Vector_(3, - 1.0000000e+00, 8.2156504e-15, -9.8100000e+00); -gtsam::Vector gyro = gtsam::Vector_(3, - 0.0000000e+00, 0.0000000e+00, 0.0000000e+00); // in r,p,y - -// resulting frame (2.01) -// 2.0100000e+00 3.6855519e+01 -8.4855615e+01 0.0000000e+00 -8.0048940e-01 1.8437236e+00 0.0000000e+00 0.0000000e+00 0.0000000e+00 1.9804083e+00 -gtsam::Point3 posFinal(3.6855519e+01, -8.4855615e+01, 0.0000000e+00); -gtsam::Vector velFinal = gtsam::Vector_(3,-8.0048940e-01, 1.8437236e+00, 0.0000000e+00); -gtsam::Vector attFinal = gtsam::Vector_(3, 0.0000000e+00, 0.0000000e+00, 1.9804083e+00); - -// full states -PoseRTV init (posInit, gtsam::Rot3::ypr( attInit(2), attInit(1), attInit(0)), velInit); -PoseRTV final(posFinal, gtsam::Rot3::ypr(attFinal(2), attFinal(1), attFinal(0)), velFinal); - -} // \namespace ground200 - -namespace ground600 { -double dt=0.010000; - -// from time 6.00 to 6.01 in robot G1 - -// frame (6.00) -// 6.0000000e+00 3.0320057e+01 -7.0883904e+01 0.0000000e+00 -4.0020377e+00 2.9972811e+00 0.0000000e+00 0.0000000e+00 0.0000000e+00 2.4987711e+00 -gtsam::Point3 posInit(3.0320057e+01, -7.0883904e+01, 0.0000000e+00); -gtsam::Vector velInit = gtsam::Vector_(3,-4.0020377e+00, 2.9972811e+00, 0.0000000e+00); -gtsam::Vector attInit = gtsam::Vector_(3, 0.0000000e+00, 0.0000000e+00, 2.4987711e+00); - -// IMU measurement (accel (0-2), gyro (3-5)) - ax, ay, az, r, p, y -// 6.0100000e+00 6.1683759e-02 7.8536587e+00 -9.8100000e+00 0.0000000e+00 0.0000000e+00 1.5707963e+00 -gtsam::Vector accel = gtsam::Vector_(3, - 6.1683759e-02, 7.8536587e+00, -9.8100000e+00); -gtsam::Vector gyro = gtsam::Vector_(3, - 0.0000000e+00, 0.0000000e+00, 1.5707963e+00); // in r,p,y - -// resulting frame (6.01) -// 6.0100000e+00 3.0279571e+01 -7.0854564e+01 0.0000000e+00 -4.0486232e+00 2.9340501e+00 0.0000000e+00 0.0000000e+00 0.0000000e+00 2.5144791e+00 -gtsam::Point3 posFinal(3.0279571e+01, -7.0854564e+01, 0.0000000e+00); -gtsam::Vector velFinal = gtsam::Vector_(3,-4.0486232e+00, 2.9340501e+00, 0.0000000e+00); -gtsam::Vector attFinal = gtsam::Vector_(3, 0.0000000e+00, 0.0000000e+00, 2.5144791e+00); - -// full states -PoseRTV init (posInit, gtsam::Rot3::ypr( attInit(2), attInit(1), attInit(0)), velInit); -PoseRTV final(posFinal, gtsam::Rot3::ypr(attFinal(2), attFinal(1), attFinal(0)), velFinal); - -} // \namespace ground600 - -} // \namespace examples -} // \namespace gtsam diff --git a/gtsam_unstable/dynamics/inertialUtils.cpp b/gtsam_unstable/dynamics/inertialUtils.cpp deleted file mode 100644 index 94d99f690..000000000 --- a/gtsam_unstable/dynamics/inertialUtils.cpp +++ /dev/null @@ -1,51 +0,0 @@ -/** - * @file inertialUtils.cpp - * - * @date Nov 28, 2011 - * @author Alex Cunningham - */ - -#include - -namespace gtsam { -namespace dynamics { - -/* ************************************************************************* */ -Matrix RRTMbn(const Vector& euler) { - assert(euler.size() == 3); - const double s1 = sin(euler(1-1)), c1 = cos(euler(1-1)); - const double t2 = tan(euler(2-1)), c2 = cos(euler(2-1)); - Matrix Ebn(3,3); - Ebn << 1.0, s1 * t2, c1 * t2, - 0.0, c1, -s1, - 0.0, s1 / c2, c1 / c2; - return Ebn; -} - -/* ************************************************************************* */ -Matrix RRTMbn(const Rot3& att) { - return RRTMbn(att.rpy()); -} - -/* ************************************************************************* */ -Matrix RRTMnb(const Vector& euler) { - assert(euler.size() == 3); - Matrix Enb(3,3); - const double s1 = sin(euler(1-1)), c1 = cos(euler(1-1)); - const double s2 = sin(euler(2-1)), c2 = cos(euler(2-1)); - Enb << 1.0, 0.0, -s2, - 0.0, c1, s1*c2, - 0.0, -s1, c1*c2; - return Enb; -} - -/* ************************************************************************* */ -Matrix RRTMnb(const Rot3& att) { - return RRTMnb(att.rpy()); -} - -} // \namespace dynamics -} // \namespace gtsam - - - diff --git a/gtsam_unstable/dynamics/inertialUtils.h b/gtsam_unstable/dynamics/inertialUtils.h deleted file mode 100644 index 250ed3abe..000000000 --- a/gtsam_unstable/dynamics/inertialUtils.h +++ /dev/null @@ -1,32 +0,0 @@ -/** - * @file inertialUtils.h - * - * @brief Utility functions for working with dynamic systems - derived from Mitch's matlab code - * This is mostly just syntactic sugar for working with dynamic systems that use - * Euler angles to specify the orientation of a robot. - * - * @date Nov 28, 2011 - * @author Alex Cunningham - */ - -#pragma once - -#include - -namespace gtsam { -namespace dynamics { - -/// RRTMbn - Function computes the rotation rate transformation matrix from -/// body axis rates to euler angle (global) rates -Matrix RRTMbn(const Vector& euler); - -Matrix RRTMbn(const Rot3& att); - -/// RRTMnb - Function computes the rotation rate transformation matrix from -/// euler angle rates to body axis rates -Matrix RRTMnb(const Vector& euler); - -Matrix RRTMnb(const Rot3& att); - -} // \namespace dynamics -} // \namespace gtsam diff --git a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp index ad66b2b60..1749ded1a 100644 --- a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp +++ b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp @@ -6,7 +6,6 @@ #include #include -#include using namespace gtsam; using namespace imu; @@ -135,38 +134,6 @@ TEST( testIMUSystem, optimize_chain_fullfactor ) { // EXPECT(assert_equal(true_values, actual, tol)); // FAIL } -///* ************************************************************************* */ -//TEST( testIMUSystem, imu_factor_basics ) { -// using namespace examples; -// PoseKey x1(1), x2(2); -// SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector_(6, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6)); -// IMUMeasurement factor(frame5000::accel, frame5000::gyro, frame5000::dt, x1, x2, model); -// -// EXPECT(assert_equal(x1, factor.key1())); -// EXPECT(assert_equal(x2, factor.key2())); -// EXPECT(assert_equal(frame5000::accel, factor.accel(), tol)); -// EXPECT(assert_equal(frame5000::gyro, factor.gyro(), tol)); -// Vector full_meas = concatVectors(2, &frame5000::accel, &frame5000::gyro); -// EXPECT(assert_equal(full_meas, factor.z(), tol)); -//} -// -///* ************************************************************************* */ -//TEST( testIMUSystem, imu_factor_predict_function ) { -// using namespace examples; -// PoseKey x1(1), x2(2); -// SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector_(6, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6)); -// IMUMeasurement factor(frame5000::accel, frame5000::gyro, frame5000::dt, x1, x2, model); -// -// // verify zero error -// Vector actZeroError = factor.evaluateError(frame5000::init, frame5000::final); -// EXPECT(assert_equal(zero(6), actZeroError, tol)); -// -// // verify nonzero error - z-h(x) -// Vector actError = factor.evaluateError(frame10000::init, frame10000::final); -// Vector meas10k = concatVectors(2, &frame10000::accel, &frame10000::gyro); -// EXPECT(assert_equal(factor.z() - meas10k, actError, tol)); -//} - /* ************************************************************************* */ TEST( testIMUSystem, linear_trajectory) { // create a linear trajectory of poses @@ -195,7 +162,6 @@ TEST( testIMUSystem, linear_trajectory) { init_traj.insert(xB, PoseRTV()); } // EXPECT_DOUBLES_EQUAL(0, graph.error(true_traj), 1e-5); // FAIL - } /* ************************************************************************* */ diff --git a/gtsam_unstable/dynamics/tests/testInertialUtils.cpp b/gtsam_unstable/dynamics/tests/testInertialUtils.cpp deleted file mode 100644 index 5360aaae8..000000000 --- a/gtsam_unstable/dynamics/tests/testInertialUtils.cpp +++ /dev/null @@ -1,34 +0,0 @@ -/** - * @file testInertialUtils.cpp - * - * @brief Conversions for the utility functions from the matlab implementation - * - * @date Nov 28, 2011 - * @author Alex Cunningham - */ - -#include - -#include - -using namespace gtsam; - -static const double tol = 1e-5; - -/* ************************************************************************* */ -TEST(testInertialUtils, RRTMbn) { - EXPECT(assert_equal(Matrix::Identity(3,3), dynamics::RRTMbn(zero(3)), tol)); - EXPECT(assert_equal(Matrix::Identity(3,3), dynamics::RRTMbn(Rot3()), tol)); - EXPECT(assert_equal(dynamics::RRTMbn(Vector_(3, 0.3, 0.2, 0.1)), dynamics::RRTMbn(Rot3::ypr(0.1, 0.2, 0.3)), tol)); -} - -/* ************************************************************************* */ -TEST(testInertialUtils, RRTMnb) { - EXPECT(assert_equal(Matrix::Identity(3,3), dynamics::RRTMnb(zero(3)), tol)); - EXPECT(assert_equal(Matrix::Identity(3,3), dynamics::RRTMnb(Rot3()), tol)); - EXPECT(assert_equal(dynamics::RRTMnb(Vector_(3, 0.3, 0.2, 0.1)), dynamics::RRTMnb(Rot3::ypr(0.1, 0.2, 0.3)), tol)); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ diff --git a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp index c833ea116..1444f69fc 100644 --- a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp +++ b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp @@ -9,7 +9,6 @@ #include #include -#include using namespace gtsam; @@ -119,85 +118,6 @@ TEST( testPoseRTV, dynamics_identities ) { // EXPECT(assert_equal(x4.translation(), x3.imuPrediction(x4, dt).second, tol)); } -///* ************************************************************************* */ -//TEST( testPoseRTV, constant_velocity ) { -// double dt = 1.0; -// PoseRTV init(Rot3(), Point3(1.0, 2.0, 3.0), Vector_(3, 0.5, 0.0, 0.0)); -// PoseRTV final(Rot3(), Point3(1.5, 2.0, 3.0), Vector_(3, 0.5, 0.0, 0.0)); -// -// // constant velocity, so gyro is zero, but accel includes gravity -// Vector accel = delta(3, 2, -9.81), gyro = zero(3); -// -// // perform integration -// PoseRTV actFinal = init.integrate(accel, gyro, dt); -// EXPECT(assert_equal(final, actFinal, tol)); -// -// // perform prediction -// Vector actAccel, actGyro; -// boost::tie(actAccel, actGyro) = init.predict(final, dt); -// EXPECT(assert_equal(accel, actAccel, tol)); -// EXPECT(assert_equal(gyro, actGyro, tol)); -//} -// -///* ************************************************************************* */ -//TEST( testPoseRTV, frame10000_imu ) { -// using namespace examples; -// -// // perform integration -// PoseRTV actFinal = frame10000::init.integrate(frame10000::accel, frame10000::gyro, frame10000::dt); -// EXPECT(assert_equal(frame10000::final, actFinal, tol)); -// -// // perform prediction -// Vector actAccel, actGyro; -// boost::tie(actAccel, actGyro) = frame10000::init.predict(frame10000::final, frame10000::dt); -// EXPECT(assert_equal(frame10000::accel, actAccel, tol)); -// EXPECT(assert_equal(frame10000::gyro, actGyro, tol)); -//} -// -///* ************************************************************************* */ -//TEST( testPoseRTV, frame5000_imu ) { -// using namespace examples; -// -// // perform integration -// PoseRTV actFinal = frame5000::init.integrate(frame5000::accel, frame5000::gyro, frame5000::dt); -// EXPECT(assert_equal(frame5000::final, actFinal, tol)); -// -// // perform prediction -// Vector actAccel, actGyro; -// boost::tie(actAccel, actGyro) = frame5000::init.predict(frame5000::final, frame5000::dt); -// EXPECT(assert_equal(frame5000::accel, actAccel, tol)); -// EXPECT(assert_equal(frame5000::gyro, actGyro, tol)); -//} -// -///* ************************************************************************* */ -//TEST( testPoseRTV, time4_imu ) { -// using namespace examples::flying400; -// -// // perform integration -// PoseRTV actFinal = init.integrate(accel, gyro, dt); -// EXPECT(assert_equal(final, actFinal, tol)); -// -// // perform prediction -// Vector actAccel, actGyro; -// boost::tie(actAccel, actGyro) = init.predict(final, dt); -// EXPECT(assert_equal(accel, actAccel, tol)); -// EXPECT(assert_equal(gyro, actGyro, tol)); -//} -// -///* ************************************************************************* */ -//TEST( testPoseRTV, time65_imu ) { -// using namespace examples::flying650; -// -// // perform integration -// PoseRTV actFinal = init.integrate(accel, gyro, dt); -// EXPECT(assert_equal(final, actFinal, tol)); -// -// // perform prediction -// Vector actAccel, actGyro; -// boost::tie(actAccel, actGyro) = init.predict(final, dt); -// EXPECT(assert_equal(accel, actAccel, tol)); -// EXPECT(assert_equal(gyro, actGyro, tol)); -//} /* ************************************************************************* */ double range_proxy(const PoseRTV& A, const PoseRTV& B) { return A.range(B); } @@ -258,131 +178,18 @@ TEST( testPoseRTV, transformed_from_2 ) { } /* ************************************************************************* */ -// ground robot maximums -//const static double ground_max_accel = 1.0; // m/s^2 -//const static double ground_mag_vel = 5.0; // m/s - fixed in simulator +TEST(testPoseRTV, RRTMbn) { + EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMbn(zero(3)), tol)); + EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMbn(Rot3()), tol)); + EXPECT(assert_equal(PoseRTV::RRTMbn(Vector_(3, 0.3, 0.2, 0.1)), PoseRTV::RRTMbn(Rot3::ypr(0.1, 0.2, 0.3)), tol)); +} -///* ************************************************************************* */ -//TEST(testPoseRTV, flying_integration650) { -// using namespace examples; -// const PoseRTV &x1 = flying650::init, &x2 = flying650::final; -// Vector accel = flying650::accel, gyro = flying650::gyro; -// double dt = flying650::dt; -// -// // control inputs -// double pitch_rate = gyro(1), -// heading_rate = gyro(2), -// lift_control = 0.0; /// FIXME: need to find this value -// -// PoseRTV actual_x2; -// actual_x2 = x1.flyingDynamics(pitch_rate, heading_rate, lift_control, dt); -// -// // FIXME: enable remaining components when there the lift control value is known -// EXPECT(assert_equal(x2.R(), actual_x2.R(), tol)); -//// EXPECT(assert_equal(x2.t(), actual_x2.t(), tol)); -//// EXPECT(assert_equal(x2.v(), actual_x2.v(), tol)); -//} - -///* ************************************************************************* */ -//TEST(testPoseRTV, imu_prediction650) { -// using namespace examples; -// const PoseRTV &x1 = flying650::init, &x2 = flying650::final; -// Vector accel = flying650::accel, gyro = flying650::gyro; -// double dt = flying650::dt; -// -// // given states, predict the imu measurement and t2 (velocity constraint) -// Vector actual_imu; -// Point3 actual_t2; -// boost::tie(actual_imu, actual_t2) = x1.imuPrediction(x2, dt); -// -// EXPECT(assert_equal(x2.t(), actual_t2, tol)); -// EXPECT(assert_equal(accel, actual_imu.head(3), tol)); -// EXPECT(assert_equal(gyro, actual_imu.tail(3), tol)); -//} -// -///* ************************************************************************* */ -//TEST(testPoseRTV, imu_prediction39) { -// // This case was a known failure case for gyro prediction, returning [9.39091; 0.204952; 625.63] using -// // the general approach for reverse-engineering the gyro updates -// using namespace examples; -// const PoseRTV &x1 = flying39::init, &x2 = flying39::final; -// Vector accel = flying39::accel, gyro = flying39::gyro; -// double dt = flying39::dt; -// -// // given states, predict the imu measurement and t2 (velocity constraint) -// Vector actual_imu; -// Point3 actual_t2; -// boost::tie(actual_imu, actual_t2) = x1.imuPrediction(x2, dt); -// -// EXPECT(assert_equal(x2.t(), actual_t2, tol)); -// EXPECT(assert_equal(accel, actual_imu.head(3), tol)); -// EXPECT(assert_equal(gyro, actual_imu.tail(3), tol)); -//} -// -///* ************************************************************************* */ -//TEST(testPoseRTV, ground_integration200) { -// using namespace examples; -// const PoseRTV &x1 = ground200::init, &x2 = ground200::final; -// Vector accel = ground200::accel, gyro = ground200::gyro; -// double dt = ground200::dt; -// -// // integrates from one pose to the next with known measurements -// // No heading change in this example -// // Hits maximum accel bound in this example -// -// PoseRTV actual_x2; -// actual_x2 = x1.planarDynamics(ground_mag_vel, gyro(2), ground_max_accel, dt); -// -// EXPECT(assert_equal(x2, actual_x2, tol)); -//} -// -///* ************************************************************************* */ -//TEST(testPoseRTV, ground_prediction200) { -// using namespace examples; -// const PoseRTV &x1 = ground200::init, &x2 = ground200::final; -// Vector accel = ground200::accel, gyro = ground200::gyro; -// double dt = ground200::dt; -// -// // given states, predict the imu measurement and t2 (velocity constraint) -// Vector actual_imu; -// Point3 actual_t2; -// boost::tie(actual_imu, actual_t2) = x1.imuPrediction(x2, dt); -// -// EXPECT(assert_equal(x2.t(), actual_t2, tol)); -// EXPECT(assert_equal(accel, actual_imu.head(3), tol)); -// EXPECT(assert_equal(gyro, actual_imu.tail(3), tol)); -//} -// -///* ************************************************************************* */ -//TEST(testPoseRTV, ground_integration600) { -// using namespace examples; -// const PoseRTV &x1 = ground600::init, &x2 = ground600::final; -// Vector accel = ground600::accel, gyro = ground600::gyro; -// double dt = ground600::dt; -// -// // integrates from one pose to the next with known measurements -// PoseRTV actual_x2; -// actual_x2 = x1.planarDynamics(ground_mag_vel, gyro(2), ground_max_accel, dt); -// -// EXPECT(assert_equal(x2, actual_x2, tol)); -//} -// -///* ************************************************************************* */ -//TEST(testPoseRTV, ground_prediction600) { -// using namespace examples; -// const PoseRTV &x1 = ground600::init, &x2 = ground600::final; -// Vector accel = ground600::accel, gyro = ground600::gyro; -// double dt = ground600::dt; -// -// // given states, predict the imu measurement and t2 (velocity constraint) -// Vector actual_imu; -// Point3 actual_t2; -// boost::tie(actual_imu, actual_t2) = x1.imuPrediction(x2, dt); -// -// EXPECT(assert_equal(x2.t(), actual_t2, tol)); -// EXPECT(assert_equal(accel, actual_imu.head(3), tol)); -// EXPECT(assert_equal(gyro, actual_imu.tail(3), tol)); -//} +/* ************************************************************************* */ +TEST(testPoseRTV, RRTMnb) { + EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMnb(zero(3)), tol)); + EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMnb(Rot3()), tol)); + EXPECT(assert_equal(PoseRTV::RRTMnb(Vector_(3, 0.3, 0.2, 0.1)), PoseRTV::RRTMnb(Rot3::ypr(0.1, 0.2, 0.3)), tol)); +} /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); }