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

Added various unit tests for the class ImuBias.

release/4.3a0
krunalchande 2015-03-05 11:44:08 -05:00
parent 447ebd1233
commit 2f27aa2d16
1 changed files with 92 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

98
gtsam/navigation/tests/testImuBias.cpp
View File

@ -21,23 +21,109 @@
using namespace std;
using namespace gtsam;
Vector biasAcc1(Vector3(0.2, -0.1, 0));
Vector biasGyro1(Vector3(0.1, -0.3, -0.2));
imuBias::ConstantBias bias1(biasAcc1, biasGyro1);
Vector biasAcc2(Vector3(0.1,0.2,0.04));
Vector biasGyro2(Vector3(-0.002, 0.005, 0.03));
imuBias::ConstantBias bias2(biasAcc2, biasGyro2);
/* ************************************************************************* */
TEST( ImuBias, Constructor)
{
Vector bias_acc(Vector3(0.1,0.2,0.4));
Vector bias_gyro(Vector3(-0.2, 0.5, 0.03));
// Default Constructor
gtsam::imuBias::ConstantBias bias1;
imuBias::ConstantBias bias1;
// Acc + Gyro Constructor
gtsam::imuBias::ConstantBias bias2(bias_acc, bias_gyro);
imuBias::ConstantBias bias2(biasAcc2, biasGyro2);
// Copy Constructor
gtsam::imuBias::ConstantBias bias3(bias2);
imuBias::ConstantBias bias3(bias2);
}
/* ************************************************************************* */
TEST( ImuBias, inverse)
{
imuBias::ConstantBias biasActual = bias1.inverse();
imuBias::ConstantBias biasExpected = imuBias::ConstantBias(-biasAcc1, -biasGyro1);
EXPECT(assert_equal(biasExpected, biasActual));
}
/* ************************************************************************* */
TEST( ImuBias, compose)
{
imuBias::ConstantBias biasActual = bias2.compose(bias1);
imuBias::ConstantBias biasExpected = imuBias::ConstantBias(biasAcc1+biasAcc2, biasGyro1+biasGyro2);
EXPECT(assert_equal(biasExpected, biasActual));
}
/* ************************************************************************* */
TEST( ImuBias, between)
{
// p.between(q) == q - p
imuBias::ConstantBias biasActual = bias2.between(bias1);
imuBias::ConstantBias biasExpected = imuBias::ConstantBias(biasAcc1-biasAcc2, biasGyro1-biasGyro2);
EXPECT(assert_equal(biasExpected, biasActual));
}
/* ************************************************************************* */
TEST( ImuBias, localCoordinates)
{
Vector deltaActual = Vector(bias2.localCoordinates(bias1));
Vector deltaExpected = (imuBias::ConstantBias(biasAcc1-biasAcc2, biasGyro1-biasGyro2)).vector();
EXPECT(assert_equal(deltaExpected, deltaActual));
}
/* ************************************************************************* */
TEST( ImuBias, retract)
{
Vector6 delta; delta << 0.1, 0.2, -0.3, 0.1, -0.1, 0.2;
imuBias::ConstantBias biasActual = bias2.retract(delta);
imuBias::ConstantBias biasExpected = imuBias::ConstantBias(biasAcc2+delta.block<3,1>(0,0), biasGyro2+delta.block<3,1>(3,0));
EXPECT(assert_equal(biasExpected, biasActual));
}
/* ************************************************************************* */
TEST( ImuBias, Logmap)
{
Vector deltaActual = bias2.Logmap(bias1);
Vector deltaExpected = bias1.vector();
EXPECT(assert_equal(deltaExpected, deltaActual));
}
/* ************************************************************************* */
TEST( ImuBias, Expmap)
{
Vector6 delta; delta << 0.1, 0.2, -0.3, 0.1, -0.1, 0.2;
imuBias::ConstantBias biasActual = bias2.Expmap(delta);
imuBias::ConstantBias biasExpected = imuBias::ConstantBias(delta);
EXPECT(assert_equal(biasExpected, biasActual));
}
/* ************************************************************************* */
TEST( ImuBias, operatorSub)
{
imuBias::ConstantBias biasActual = -bias1;
imuBias::ConstantBias biasExpected(-biasAcc1, -biasGyro1);
EXPECT(assert_equal(biasExpected, biasActual));
}
/* ************************************************************************* */
TEST( ImuBias, operatorAdd)
{
imuBias::ConstantBias biasActual = bias2 + bias1;
imuBias::ConstantBias biasExpected(biasAcc2 + biasAcc1, biasGyro2 + biasGyro1);
EXPECT(assert_equal(biasExpected, biasActual));
}
/* ************************************************************************* */
TEST( ImuBias, operatorSubB)
{
imuBias::ConstantBias biasActual = bias2 - bias1;
imuBias::ConstantBias biasExpected(biasAcc2 - biasAcc1, biasGyro2 - biasGyro1);
EXPECT(assert_equal(biasExpected, biasActual));}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */