Removed unnecessary tests/examples, consolidated utility functions
Alex Cunningham
parent
55ff10a1c2
commit
87a705468d
|
|
@ -10,7 +10,6 @@
|
|||
#include <gtsam/base/Lie-inl.h>
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
|
||||
#include <gtsam_unstable/dynamics/inertialUtils.h>
|
||||
#include <gtsam_unstable/dynamics/PoseRTV.h>
|
||||
|
||||
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
|
||||
|
|
|
|||
|
|
@ -153,6 +153,20 @@ public:
|
|||
boost::optional<Matrix&> Dglobal=boost::none,
|
||||
boost::optional<Matrix&> 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;
|
||||
|
|
|
|||
|
|
@ -1,273 +0,0 @@
|
|||
/**
|
||||
* @file imu_examples.h
|
||||
* @brief Example measurements from ACFR matlab simulation
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/base/Vector.h>
|
||||
#include <gtsam_unstable/dynamics/PoseRTV.h>
|
||||
|
||||
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
|
||||
|
|
@ -1,51 +0,0 @@
|
|||
/**
|
||||
* @file inertialUtils.cpp
|
||||
*
|
||||
* @date Nov 28, 2011
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#include <gtsam_unstable/dynamics/inertialUtils.h>
|
||||
|
||||
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
|
||||
|
||||
|
||||
|
||||
|
|
@ -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 <gtsam/geometry/Rot3.h>
|
||||
|
||||
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
|
||||
|
|
@ -6,7 +6,6 @@
|
|||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <gtsam_unstable/dynamics/imuSystem.h>
|
||||
#include <gtsam_unstable/dynamics/imu_examples.h>
|
||||
|
||||
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
|
||||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -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 <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <gtsam_unstable/dynamics/inertialUtils.h>
|
||||
|
||||
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); }
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -9,7 +9,6 @@
|
|||
#include <gtsam/base/numericalDerivative.h>
|
||||
|
||||
#include <gtsam_unstable/dynamics/PoseRTV.h>
|
||||
#include <gtsam_unstable/dynamics/imu_examples.h>
|
||||
|
||||
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); }
|
||||
|
|
|
|||
Loading…
Reference in New Issue