diff --git a/gtsam/slam/SmartProjectionFactorP.h b/gtsam/slam/SmartProjectionFactorP.h index 34a8aa06a..a215890bc 100644 --- a/gtsam/slam/SmartProjectionFactorP.h +++ b/gtsam/slam/SmartProjectionFactorP.h @@ -46,15 +46,15 @@ namespace gtsam { * @addtogroup SLAM */ template -class SmartProjectionFactorP: public SmartProjectionFactor { +class SmartProjectionFactorP : public SmartProjectionFactor { -private: + private: typedef SmartProjectionFactor Base; typedef SmartProjectionFactorP This; typedef CAMERA Camera; typedef typename CAMERA::CalibrationType CALIBRATION; -protected: + protected: /// shared pointer to calibration object (one for each observation) std::vector > K_all_; @@ -62,22 +62,23 @@ protected: /// Pose of the camera in the body frame (one for each observation) std::vector body_P_sensors_; -public: + public: /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; /// Default constructor, only for serialization - SmartProjectionFactorP() {} + SmartProjectionFactorP() { + } /** * Constructor * @param sharedNoiseModel isotropic noise model for the 2D feature measurements * @param params parameters for the smart projection factors */ - SmartProjectionFactorP( - const SharedNoiseModel& sharedNoiseModel, - const SmartProjectionParams& params = SmartProjectionParams()) + SmartProjectionFactorP(const SharedNoiseModel& sharedNoiseModel, + const SmartProjectionParams& params = + SmartProjectionParams()) : Base(sharedNoiseModel, params) { } @@ -95,7 +96,8 @@ public: * @param body_P_sensor (fixed) camera extrinsic calibration */ void add(const Point2& measured, const Key& poseKey, - const boost::shared_ptr& K, const Pose3 body_P_sensor = Pose3::identity()) { + const boost::shared_ptr& K, const Pose3 body_P_sensor = + Pose3::identity()) { // store measurement and key this->measured_.push_back(measured); this->keys_.push_back(poseKey); @@ -113,15 +115,17 @@ public: * @param Ks vector of (fixed) intrinsic calibration objects * @param body_P_sensors vector of (fixed) extrinsic calibration objects */ - void add(const Point2Vector& measurements, - const std::vector& poseKeys, + void add(const Point2Vector& measurements, const std::vector& poseKeys, const std::vector>& Ks, - const std::vector body_P_sensors) { + const std::vector body_P_sensors = std::vector()) { assert(poseKeys.size() == measurements.size()); assert(poseKeys.size() == Ks.size()); - assert(poseKeys.size() == body_P_sensors.size()); for (size_t i = 0; i < measurements.size(); i++) { - add(measurements[i], poseKeys[i], Ks[i], body_P_sensors[i]); + if (poseKeys.size() == body_P_sensors.size()) { + add(measurements[i], poseKeys[i], Ks[i], body_P_sensors[i]); + } else { + add(measurements[i], poseKeys[i], Ks[i]); // use default extrinsics + } } } @@ -141,7 +145,7 @@ public: * @param keyFormatter optional formatter useful for printing Symbols */ void print(const std::string& s = "", const KeyFormatter& keyFormatter = - DefaultKeyFormatter) const override { + DefaultKeyFormatter) const override { std::cout << s << "SmartProjectionFactorP: \n "; for (size_t i = 0; i < K_all_.size(); i++) { std::cout << "-- Measurement nr " << i << std::endl; @@ -155,13 +159,16 @@ public: bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { const This *e = dynamic_cast(&p); double extrinsicCalibrationEqual = true; - if(this->body_P_sensors_.size() == e->body_P_sensors().size()){ - for(size_t i=0; i< this->body_P_sensors_.size(); i++){ - if (!body_P_sensors_[i].equals(e->body_P_sensors()[i])){ - extrinsicCalibrationEqual = false; break; + if (this->body_P_sensors_.size() == e->body_P_sensors().size()) { + for (size_t i = 0; i < this->body_P_sensors_.size(); i++) { + if (!body_P_sensors_[i].equals(e->body_P_sensors()[i])) { + extrinsicCalibrationEqual = false; + break; } } - }else{ extrinsicCalibrationEqual = false; } + } else { + extrinsicCalibrationEqual = false; + } return e && Base::equals(p, tol) && K_all_ == e->calibration() && extrinsicCalibrationEqual; @@ -173,7 +180,7 @@ public: double error(const Values& values) const override { if (this->active(values)) { return this->totalReprojectionError(cameras(values)); - } else { // else of active flag + } else { // else of active flag return 0.0; } } @@ -215,4 +222,4 @@ struct traits > : public Testable< SmartProjectionFactorP > { }; -} // \ namespace gtsam +} // \ namespace gtsam diff --git a/gtsam/slam/tests/testSmartProjectionFactorP.cpp b/gtsam/slam/tests/testSmartProjectionFactorP.cpp index 1f5457fb8..3ad4d62b6 100644 --- a/gtsam/slam/tests/testSmartProjectionFactorP.cpp +++ b/gtsam/slam/tests/testSmartProjectionFactorP.cpp @@ -114,77 +114,85 @@ TEST( SmartProjectionFactorP, noiseless ) { double expectedError = 0.0; EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); -// SmartFactorP::Cameras cameras = factor.cameras(values); -// double actualError2 = factor.totalReprojectionError(cameras); -// EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7); -// -// // Calculate expected derivative for point (easiest to check) -// std::function f = // -// std::bind(&SmartFactorP::whitenedError, factor, cameras, std::placeholders::_1); -// -// // Calculate using computeEP -// Matrix actualE; -// factor.triangulateAndComputeE(actualE, values); -// -// // get point -// boost::optional point = factor.point(); -// CHECK(point); -// -// // calculate numerical derivative with triangulated point -// Matrix expectedE = sigma * numericalDerivative11(f, *point); -// EXPECT(assert_equal(expectedE, actualE, 1e-7)); -// -// // Calculate using reprojectionError -// SmartFactorP::Cameras::FBlocks F; -// Matrix E; -// Vector actualErrors = factor.unwhitenedError(cameras, *point, F, E); -// EXPECT(assert_equal(expectedE, E, 1e-7)); -// -// EXPECT(assert_equal(Z_4x1, actualErrors, 1e-7)); -// -// // Calculate using computeJacobians -// Vector b; -// SmartFactorP::FBlocks Fs; -// factor.computeJacobians(Fs, E, b, cameras, *point); -// double actualError3 = b.squaredNorm(); -// EXPECT(assert_equal(expectedE, E, 1e-7)); -// EXPECT_DOUBLES_EQUAL(expectedError, actualError3, 1e-6); + SmartFactorP::Cameras cameras = factor.cameras(values); + double actualError2 = factor.totalReprojectionError(cameras); + EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7); + + // Calculate expected derivative for point (easiest to check) + std::function f = // + std::bind(&SmartFactorP::whitenedError, factor, cameras, std::placeholders::_1); + + // Calculate using computeEP + Matrix actualE; + factor.triangulateAndComputeE(actualE, values); + + // get point + boost::optional point = factor.point(); + CHECK(point); + + // calculate numerical derivative with triangulated point + Matrix expectedE = sigma * numericalDerivative11(f, *point); + EXPECT(assert_equal(expectedE, actualE, 1e-7)); + + // Calculate using reprojectionError + SmartFactorP::Cameras::FBlocks F; + Matrix E; + Vector actualErrors = factor.unwhitenedError(cameras, *point, F, E); + EXPECT(assert_equal(expectedE, E, 1e-7)); + + EXPECT(assert_equal(Z_4x1, actualErrors, 1e-7)); + + // Calculate using computeJacobians + Vector b; + SmartFactorP::FBlocks Fs; + factor.computeJacobians(Fs, E, b, cameras, *point); + double actualError3 = b.squaredNorm(); + EXPECT(assert_equal(expectedE, E, 1e-7)); + EXPECT_DOUBLES_EQUAL(expectedError, actualError3, 1e-6); +} + +/* *************************************************************************/ +TEST( SmartProjectionFactorP, noisy ) { + + using namespace vanillaPose; + + // Project two landmarks into two cameras + Point2 pixelError(0.2, 0.2); + Point2 level_uv = level_camera.project(landmark1) + pixelError; + Point2 level_uv_right = level_camera_right.project(landmark1); + + Values values; + values.insert(x1, cam1.pose()); + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), + Point3(0.5, 0.1, 0.3)); + values.insert(x2, pose_right.compose(noise_pose)); + + SmartFactorP::shared_ptr factor(new SmartFactorP(model)); + factor->add(level_uv, x1, sharedK); + factor->add(level_uv_right, x2, sharedK); + + double actualError1 = factor->error(values); + + SmartFactorP::shared_ptr factor2(new SmartFactorP(model)); + Point2Vector measurements; + measurements.push_back(level_uv); + measurements.push_back(level_uv_right); + + std::vector> sharedKs; + sharedKs.push_back(sharedK); + sharedKs.push_back(sharedK); + + std::vector body_P_sensors; + body_P_sensors.push_back(Pose3::identity()); + body_P_sensors.push_back(Pose3::identity()); + + KeyVector views {x1, x2}; + + factor2->add(measurements, views, sharedKs, body_P_sensors); + double actualError2 = factor2->error(values); + DOUBLES_EQUAL(actualError1, actualError2, 1e-7); } -///* *************************************************************************/ -//TEST( SmartProjectionFactorP, noisy ) { -// -// using namespace vanillaPose; -// -// // Project two landmarks into two cameras -// Point2 pixelError(0.2, 0.2); -// Point2 level_uv = level_camera.project(landmark1) + pixelError; -// Point2 level_uv_right = level_camera_right.project(landmark1); -// -// Values values; -// values.insert(x1, cam1.pose()); -// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), -// Point3(0.5, 0.1, 0.3)); -// values.insert(x2, pose_right.compose(noise_pose)); -// -// SmartFactorP::shared_ptr factor(new SmartFactorP(model, sharedK)); -// factor->add(level_uv, x1); -// factor->add(level_uv_right, x2); -// -// double actualError1 = factor->error(values); -// -// SmartFactorP::shared_ptr factor2(new SmartFactorP(model, sharedK)); -// Point2Vector measurements; -// measurements.push_back(level_uv); -// measurements.push_back(level_uv_right); -// -// KeyVector views {x1, x2}; -// -// factor2->add(measurements, views); -// double actualError2 = factor2->error(values); -// DOUBLES_EQUAL(actualError1, actualError2, 1e-7); -//} -// ///* *************************************************************************/ //TEST(SmartProjectionFactorP, smartFactorWithSensorBodyTransform) { // using namespace vanillaPose;