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

Added unit tests for Predict

release/4.3a0
krunalchande 2014-11-21 20:02:39 -05:00
parent 1bc9f3ac06
commit 1ab1323a33
4 changed files with 88 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
gtsam/navigation/AHRSFactor.h
View File

@ -83,7 +83,7 @@ public:
tol);
}
Matrix measurementCovariance() const {return measurementCovariance_;}
Matrix deltaRij() const {return deltaRij_.matrix();}
Rot3 deltaRij() const {return deltaRij_;}
double deltaTij() const {return deltaTij_;}
Vector biasHat() const {return biasHat_.vector();}
Matrix3 delRdelBiasOmega() {return delRdelBiasOmega_;}
@ -349,7 +349,7 @@ public:
}
/** predicted states from IMU */
static void predict(const Rot3& rot_i, const Rot3& rot_j,
static Rot3 predict(const Rot3& rot_i,
const imuBias::ConstantBias& bias,
const PreintegratedMeasurements preintegratedMeasurements,
const Vector3& omegaCoriolis,
@ -376,7 +376,8 @@ public:
- Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term
const Rot3 deltaRij_biascorrected_corioliscorrected = Rot3::Expmap(
theta_biascorrected_corioliscorrected);
const Rot3 Rot_j = Rot_i.compose(deltaRij_biascorrected_corioliscorrected);
// const Rot3 Rot_j =
return (Rot_i.compose(deltaRij_biascorrected_corioliscorrected));
}

17
gtsam/navigation/ImuFactor.h
View File

@ -28,6 +28,16 @@
namespace gtsam {
/**
* Struct to hold return variables by the Predict Function
*/
struct PoseVelocity {
Pose3 pose;
Vector3 velocity;
PoseVelocity(const Pose3& _pose, const Vector3& _velocity) :
pose(_pose), velocity(_velocity) {
}
};
/**
*
@ -547,7 +557,7 @@ namespace gtsam {
/** predicted states from IMU */
static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, Vector3& vel_j,
static PoseVelocity Predict(const Pose3& pose_i, const Vector3& vel_i,
const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements,
const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor = boost::none,
const bool use2ndOrderCoriolis = false)
@ -569,7 +579,7 @@ namespace gtsam {
- skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper
+ 0.5 * gravity * deltaTij*deltaTij;
vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_
Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_
+ preintegratedMeasurements.delVdelBiasAcc_ * biasAccIncr
+ preintegratedMeasurements.delVdelBiasOmega_ * biasOmegaIncr)
- 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term
@ -589,7 +599,8 @@ namespace gtsam {
Rot3::Expmap( theta_biascorrected_corioliscorrected );
const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected );
pose_j = Pose3( Rot_j, Point3(pos_j) );
Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) );
return PoseVelocity(pose_j, vel_j);
}

10
gtsam/navigation/tests/testAHRSFactor.cpp
View File

@ -69,7 +69,7 @@ Rot3 evaluatePreintegratedMeasurementsRotation(
const list<double>& deltaTs,
const Vector3& initialRotationRate = Vector3(0.0, 0.0, 0.0)) {
return evaluatePreintegratedMeasurements(bias, measuredOmegas,
deltaTs, initialRotationRate).deltaRij_;
deltaTs, initialRotationRate).deltaRij();
}
Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega,
@ -99,8 +99,8 @@ TEST( AHRSFactor, PreintegratedMeasurements ) {
AHRSFactor::PreintegratedMeasurements actual1(bias, Matrix3::Zero());
actual1.integrateMeasurement(measuredOmega, deltaT);
EXPECT(assert_equal(expectedDeltaR1, actual1.deltaRij_, 1e-6));
DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij_, 1e-6);
EXPECT(assert_equal(expectedDeltaR1, actual1.deltaRij(), 1e-6));
DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij(), 1e-6);
// Integrate again
Rot3 expectedDeltaR2 = Rot3::RzRyRx(2.0 * 0.5 * M_PI / 100.0, 0.0, 0.0);
@ -110,8 +110,8 @@ TEST( AHRSFactor, PreintegratedMeasurements ) {
AHRSFactor::PreintegratedMeasurements actual2 = actual1;
actual2.integrateMeasurement(measuredOmega, deltaT);
EXPECT(assert_equal(expectedDeltaR2, actual2.deltaRij_, 1e-6));
DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij_, 1e-6);
EXPECT(assert_equal(expectedDeltaR2, actual2.deltaRij(), 1e-6));
DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij(), 1e-6);
}
/* ************************************************************************* */

65
gtsam/navigation/tests/testImuFactor.cpp
View File

@ -560,6 +560,71 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement )
EXPECT(assert_equal(H5e, H5a));
}
TEST(ImuFactor, PredictPositionAndVelocity){
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
// Measurements
Vector3 gravity; gravity << 0, 0, 9.81;
Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0;
Vector3 measuredOmega; measuredOmega << 0, 0, 0;//M_PI/10.0+0.3;
Vector3 measuredAcc; measuredAcc << 0,1,-9.81;
double deltaT = 0.001;
double tol = 1e-6;
Matrix I6x6(6,6);
I6x6 = Matrix::Identity(6,6);
ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0),
Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), true);
for (int i = 0; i<1000; ++i) pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis);
// Predict
Pose3 x1;
Vector3 v1(0, 0.0, 0.0);
PoseVelocity poseVelocity = factor.Predict(x1, v1, bias, pre_int_data, gravity, omegaCoriolis);
Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0));
Vector3 expectedVelocity; expectedVelocity<<0,1,0;
EXPECT(assert_equal(expectedPose, poseVelocity.pose));
EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocity.velocity)));
}
TEST(ImuFactor, PredictRotation) {
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
// Measurements
Vector3 gravity; gravity << 0, 0, 9.81;
Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0;
Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10;//M_PI/10.0+0.3;
Vector3 measuredAcc; measuredAcc << 0,0,-9.81;
double deltaT = 0.001;
double tol = 1e-6;
Matrix I6x6(6,6);
I6x6 = Matrix::Identity(6,6);
ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0),
Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), true);
for (int i = 0; i<1000; ++i) pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis);
// Predict
Pose3 x1;
Vector3 v1(0, 0.0, 0.0);
PoseVelocity poseVelocity = factor.Predict(x1, v1, bias, pre_int_data, gravity, omegaCoriolis);
Pose3 expectedPose(Rot3().ypr(M_PI/10, 0, 0), Point3(0, 0, 0));
Vector3 expectedVelocity; expectedVelocity<<0,0,0;
EXPECT(assert_equal(expectedPose, poseVelocity.pose));
EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocity.velocity)));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}