From 3d6a7bf970a1e1a28f269a80ad86449ed0bd7604 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 16 Feb 2022 09:43:12 -0500 Subject: [PATCH] Fix more stuff in check.slam --- .../testSmartStereoProjectionFactorPP.cpp | 21 ++++++++++++------- .../testSmartStereoProjectionPoseFactor.cpp | 19 ++++++++++------- 2 files changed, 24 insertions(+), 16 deletions(-) diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp index 61836d098..c71c19038 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp @@ -31,12 +31,13 @@ using namespace std; using namespace boost::assign; using namespace gtsam; +namespace { // make a realistic calibration matrix static double b = 1; static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fov, w, h, b)); -static Cal3_S2Stereo::shared_ptr K2( - new Cal3_S2Stereo(1500, 1200, 0, 640, 480, b)); +static Cal3_S2Stereo::shared_ptr K2(new Cal3_S2Stereo(1500, 1200, 0, 640, 480, + b)); static SmartStereoProjectionParams params; @@ -45,8 +46,8 @@ static SmartStereoProjectionParams params; static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.1)); // Convenience for named keys -using symbol_shorthand::X; using symbol_shorthand::L; +using symbol_shorthand::X; // tests data static Symbol x1('X', 1); @@ -59,16 +60,19 @@ static Symbol body_P_cam3_key('P', 3); static Key poseKey1(x1); static Key poseExtrinsicKey1(body_P_cam1_key); static Key poseExtrinsicKey2(body_P_cam2_key); -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 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), - Point3(0.25, -0.10, 1.0)); + Point3(0.25, -0.10, 1.0)); static double missing_uR = std::numeric_limits::quiet_NaN(); vector stereo_projectToMultipleCameras(const StereoCamera& cam1, - const StereoCamera& cam2, const StereoCamera& cam3, Point3 landmark) { - + const StereoCamera& cam2, + const StereoCamera& cam3, + Point3 landmark) { vector measurements_cam; StereoPoint2 cam1_uv1 = cam1.project(landmark); @@ -82,6 +86,7 @@ vector stereo_projectToMultipleCameras(const StereoCamera& cam1, } LevenbergMarquardtParams lm_params; +} // namespace /* ************************************************************************* */ TEST( SmartStereoProjectionFactorPP, params) { diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index a0bfc3649..fc56b1a9f 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -32,13 +32,13 @@ using namespace std; using namespace boost::assign; using namespace gtsam; +namespace { // make a realistic calibration matrix static double b = 1; static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fov, w, h, b)); -static Cal3_S2Stereo::shared_ptr K2( - new Cal3_S2Stereo(1500, 1200, 0, 640, 480, b)); - +static Cal3_S2Stereo::shared_ptr K2(new Cal3_S2Stereo(1500, 1200, 0, 640, 480, + b)); static SmartStereoProjectionParams params; @@ -47,8 +47,8 @@ static SmartStereoProjectionParams params; static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.1)); // Convenience for named keys -using symbol_shorthand::X; using symbol_shorthand::L; +using symbol_shorthand::X; // tests data static Symbol x1('X', 1); @@ -56,15 +56,17 @@ static Symbol x2('X', 2); static Symbol x3('X', 3); static Key poseKey1(x1); -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 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)); static double missing_uR = std::numeric_limits::quiet_NaN(); vector stereo_projectToMultipleCameras(const StereoCamera& cam1, - const StereoCamera& cam2, const StereoCamera& cam3, Point3 landmark) { - + const StereoCamera& cam2, + const StereoCamera& cam3, + Point3 landmark) { vector measurements_cam; StereoPoint2 cam1_uv1 = cam1.project(landmark); @@ -78,6 +80,7 @@ vector stereo_projectToMultipleCameras(const StereoCamera& cam1, } LevenbergMarquardtParams lm_params; +} // namespace /* ************************************************************************* */ TEST( SmartStereoProjectionPoseFactor, params) {