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

simple tests are passing, but now we start on the serious ones

release/4.3a0
lcarlone 2021-03-13 14:03:10 -05:00
parent 273d2da567
commit 266d8248d0
1 changed files with 25 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp
View File

@ -51,10 +51,15 @@ using symbol_shorthand::L;
static Symbol x1('X', 1); static Symbol x1('X', 1);
static Symbol x2('X', 2); static Symbol x2('X', 2);
static Symbol x3('X', 3); static Symbol x3('X', 3);
static Symbol body_P_sensor1_sym('P', 0); static Symbol body_P_sensor1_sym('P', 1);
static Symbol body_P_sensor2_sym('P', 2);
static Symbol body_P_sensor3_sym('P', 3);
static Key poseKey1(x1); static Key poseKey1(x1);
static Key poseExtrinsicKey1(body_P_sensor1_sym);
static Key poseExtrinsicKey2(body_P_sensor2_sym);
static StereoPoint2 measurement1(323.0, 300.0, 240.0); //potentially use more reasonable measurement value? static StereoPoint2 measurement1(323.0, 300.0, 240.0); //potentially use more reasonable measurement value?
static StereoPoint2 measurement2(350.0, 200.0, 240.0); //potentially use more reasonable measurement value?
static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
Point3(0.25, -0.10, 1.0)); Point3(0.25, -0.10, 1.0));
@ -77,7 +82,7 @@ vector<StereoPoint2> stereo_projectToMultipleCameras(const StereoCamera& cam1,
LevenbergMarquardtParams lm_params; LevenbergMarquardtParams lm_params;
/* ************************************************************************* * /* ************************************************************************* */
TEST( SmartStereoProjectionFactorPP, params) { TEST( SmartStereoProjectionFactorPP, params) {
SmartStereoProjectionParams p; SmartStereoProjectionParams p;
@ -111,32 +116,42 @@ TEST( SmartStereoProjectionFactorPP, Constructor) {
SmartStereoProjectionFactorPP::shared_ptr factor1(new SmartStereoProjectionFactorPP(model)); SmartStereoProjectionFactorPP::shared_ptr factor1(new SmartStereoProjectionFactorPP(model));
} }
/* ************************************************************************* * /* ************************************************************************* */
TEST( SmartStereoProjectionFactorPP, Constructor2) { TEST( SmartStereoProjectionFactorPP, Constructor2) {
SmartStereoProjectionFactorPP factor1(model, params); SmartStereoProjectionFactorPP factor1(model, params);
} }
/* ************************************************************************* * /* ************************************************************************* */
TEST( SmartStereoProjectionFactorPP, Constructor3) { TEST( SmartStereoProjectionFactorPP, Constructor3) {
SmartStereoProjectionFactorPP::shared_ptr factor1(new SmartStereoProjectionFactorPP(model)); SmartStereoProjectionFactorPP::shared_ptr factor1(new SmartStereoProjectionFactorPP(model));
factor1->add(measurement1, poseKey1, K); factor1->add(measurement1, poseKey1, poseExtrinsicKey1, K);
} }
/* ************************************************************************* * /* ************************************************************************* */
TEST( SmartStereoProjectionFactorPP, Constructor4) { TEST( SmartStereoProjectionFactorPP, Constructor4) {
SmartStereoProjectionFactorPP factor1(model, params); SmartStereoProjectionFactorPP factor1(model, params);
factor1.add(measurement1, poseKey1, K); factor1.add(measurement1, poseKey1, poseExtrinsicKey1, K);
} }
/* ************************************************************************* * /* ************************************************************************* *
TEST( SmartStereoProjectionFactorPP, Equals ) { TEST( SmartStereoProjectionFactorPP, Equals ) {
SmartStereoProjectionFactorPP::shared_ptr factor1(new SmartStereoProjectionFactorPP(model)); SmartStereoProjectionFactorPP::shared_ptr factor1(new SmartStereoProjectionFactorPP(model));
factor1->add(measurement1, poseKey1, K); factor1->add(measurement1, poseKey1, poseExtrinsicKey1, K);
SmartStereoProjectionFactorPP::shared_ptr factor2(new SmartStereoProjectionFactorPP(model)); SmartStereoProjectionFactorPP::shared_ptr factor2(new SmartStereoProjectionFactorPP(model));
factor2->add(measurement1, poseKey1, K); factor2->add(measurement1, poseKey1, poseExtrinsicKey1, K);
// check these are equal
CHECK(assert_equal(*factor1, *factor2)); CHECK(assert_equal(*factor1, *factor2));
SmartStereoProjectionFactorPP::shared_ptr factor3(new SmartStereoProjectionFactorPP(model));
factor3->add(measurement2, poseKey1, poseExtrinsicKey1, K);
// check these are different
CHECK(!assert_equal(*factor1, *factor3));
SmartStereoProjectionFactorPP::shared_ptr factor4(new SmartStereoProjectionFactorPP(model));
factor3->add(measurement1, poseKey1, poseExtrinsicKey2, K);
// check these are different
CHECK(!assert_equal(*factor1, *factor4));
} }
/* ************************************************************************* /* *************************************************************************