diff --git a/gtsam/slam/tests/smartFactorScenarios.h b/gtsam/slam/tests/smartFactorScenarios.h index c5df4af7e..fc68ba7d9 100644 --- a/gtsam/slam/tests/smartFactorScenarios.h +++ b/gtsam/slam/tests/smartFactorScenarios.h @@ -17,8 +17,9 @@ */ #pragma once -#include #include +#include +#include using namespace std; using namespace gtsam; @@ -40,13 +41,13 @@ Pose3 pose_up = level_pose * Pose3(Rot3(), Point3(0, -1, 0)); // Create a noise unit2 for the pixel error static SharedNoiseModel unit2(noiseModel::Unit::Create(2)); +static double fov = 60; // degrees +static size_t w = 640, h = 480; + /* ************************************************************************* */ // default Cal3_S2 cameras namespace vanilla { typedef PinholeCamera Camera; -// make a realistic calibration matrix -static double fov = 60; // degrees -static size_t w = 640, h = 480; static Cal3_S2 K(fov, w, h); static Cal3_S2 K2(1500, 1200, 0, w, h); Camera level_camera(level_pose, K2); @@ -57,7 +58,30 @@ Camera cam1(level_pose, K2); Camera cam2(pose_right, K2); Camera cam3(pose_up, K2); typedef GeneralSFMFactor SFMFactor; -typedef SmartProjectionCameraFactor SmartFactor; +} + +/* ************************************************************************* */ +// default Cal3_S2 poses +namespace vanillaPose { +typedef PinholePose Camera; +static Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h)); +Camera level_camera(level_pose, sharedK); +Camera level_camera_right(pose_right, sharedK); +Camera cam1(level_pose, sharedK); +Camera cam2(pose_right, sharedK); +Camera cam3(pose_up, sharedK); +} + +/* ************************************************************************* */ +// default Cal3_S2 poses +namespace vanillaPose2 { +typedef PinholePose Camera; +static Cal3_S2::shared_ptr sharedK2(new Cal3_S2(1500, 1200, 0, 640, 480)); +Camera level_camera(level_pose, sharedK2); +Camera level_camera_right(pose_right, sharedK2); +Camera cam1(level_pose, sharedK2); +Camera cam2(pose_right, sharedK2); +Camera cam3(pose_up, sharedK2); } /* *************************************************************************/ @@ -74,40 +98,33 @@ Camera cam1(level_pose, K); Camera cam2(pose_right, K); Camera cam3(pose_up, K); typedef GeneralSFMFactor SFMFactor; -typedef SmartProjectionCameraFactor SmartFactorBundler; +} +/* *************************************************************************/ +// Cal3Bundler poses +namespace bundlerPose { +typedef PinholePose Camera; +static boost::shared_ptr sharedBundlerK( + new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); +Camera level_camera(level_pose, sharedBundlerK); +Camera level_camera_right(pose_right, sharedBundlerK); +Camera cam1(level_pose, sharedBundlerK); +Camera cam2(pose_right, sharedBundlerK); +Camera cam3(pose_up, sharedBundlerK); } /* *************************************************************************/ -template -PinholeCamera perturbCameraPose( - const PinholeCamera& camera) { +template +CAMERA perturbCameraPose(const CAMERA& camera) { Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3)); Pose3 cameraPose = camera.pose(); Pose3 perturbedCameraPose = cameraPose.compose(noise_pose); - return PinholeCamera(perturbedCameraPose, camera.calibration()); + return CAMERA(perturbedCameraPose, camera.calibration()); } -template -PinholeCamera perturbCameraPoseAndCalibration( - const PinholeCamera& camera) { - GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION) - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), - Point3(0.5, 0.1, 0.3)); - Pose3 cameraPose = camera.pose(); - Pose3 perturbedCameraPose = cameraPose.compose(noise_pose); - typename gtsam::traits::TangentVector d; - d.setRandom(); - d *= 0.1; - CALIBRATION perturbedCalibration = camera.calibration().retract(d); - return PinholeCamera(perturbedCameraPose, perturbedCalibration); -} - -template -void projectToMultipleCameras(const PinholeCamera& cam1, - const PinholeCamera& cam2, - const PinholeCamera& cam3, Point3 landmark, - vector& measurements_cam) { +template +void projectToMultipleCameras(const CAMERA& cam1, const CAMERA& cam2, + const CAMERA& cam3, Point3 landmark, vector& measurements_cam) { Point2 cam1_uv1 = cam1.project(landmark); Point2 cam2_uv1 = cam2.project(landmark); Point2 cam3_uv1 = cam3.project(landmark); diff --git a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp index af1ee3ea9..3ac95f7e2 100644 --- a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp @@ -19,11 +19,12 @@ */ #include "smartFactorScenarios.h" +#include #include -#include #include +#include #include -// + using namespace boost::assign; static bool isDebugTest = false; @@ -43,6 +44,24 @@ static Point2 measurement1(323.0, 240.0); static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); +typedef SmartProjectionCameraFactor SmartFactor; +typedef SmartProjectionCameraFactor SmartFactorBundler; + +template +PinholeCamera perturbCameraPoseAndCalibration( + const PinholeCamera& camera) { + GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION) + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Point3(0.5, 0.1, 0.3)); + Pose3 cameraPose = camera.pose(); + Pose3 perturbedCameraPose = cameraPose.compose(noise_pose); + typename gtsam::traits::TangentVector d; + d.setRandom(); + d *= 0.1; + CALIBRATION perturbedCalibration = camera.calibration().retract(d); + return PinholeCamera(perturbedCameraPose, perturbedCalibration); +} + /* ************************************************************************* */ TEST( SmartProjectionCameraFactor, perturbCameraPose) { using namespace vanilla;