diff --git a/gtsam_unstable/slam/SmartProjectionFactor.h b/gtsam_unstable/slam/SmartProjectionFactor.h index 4f3337e19..5572f04e8 100644 --- a/gtsam_unstable/slam/SmartProjectionFactor.h +++ b/gtsam_unstable/slam/SmartProjectionFactor.h @@ -42,6 +42,7 @@ namespace gtsam { ///< (important that the order is the same as the keys that we use to create the factor) boost::shared_ptr K_; ///< shared pointer to calibration object const SharedNoiseModel noise_; ///< noise model used + boost::optional point_; boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame // verbosity handling for Cheirality Exceptions @@ -73,8 +74,9 @@ namespace gtsam { */ SmartProjectionFactor(const std::vector measured, const SharedNoiseModel& model, std::vector poseKeys, const boost::shared_ptr& K, + boost::optional point = boost::none, boost::optional body_P_sensor = boost::none) : - measured_(measured), K_(K), noise_(model), body_P_sensor_(body_P_sensor), + measured_(measured), K_(K), noise_(model), point_(point), body_P_sensor_(body_P_sensor), throwCheirality_(false), verboseCheirality_(false) { keys_.assign(poseKeys.begin(), poseKeys.end()); } @@ -93,8 +95,9 @@ namespace gtsam { SmartProjectionFactor(const std::vector measured, const SharedNoiseModel& model, std::vector poseKeys, const boost::shared_ptr& K, bool throwCheirality, bool verboseCheirality, + boost::optional point = boost::none, boost::optional body_P_sensor = boost::none) : - measured_(measured), K_(K), noise_(model), body_P_sensor_(body_P_sensor), + measured_(measured), K_(K), noise_(model), point_(point), body_P_sensor_(body_P_sensor), throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {} /** Virtual destructor */ @@ -159,8 +162,23 @@ namespace gtsam { } // We triangulate the 3D position of the landmark - boost::optional point = triangulatePoint3(cameraPoses, measured_, *K_); - + if (debug) { + BOOST_FOREACH(const Pose3& pose, cameraPoses) { + std::cout << "Pose: " << pose << std::endl; + } + BOOST_FOREACH(const Point2& point, measured_) { + std::cout << "Point: " << point << std::endl; + } + } + boost::optional point; + if (point_) { + point = point_; + //std::cout << "Using existing point " << *point << std::endl; + } else { + //std::cout << "Triangulating in linearize " << std::endl; + point = triangulatePoint3(cameraPoses, measured_, *K_); + } + if (debug) std::cout << "Result: " << *point << std::endl; if (debug) { @@ -179,6 +197,7 @@ namespace gtsam { // point is behind one of the cameras, turn factor off by setting everything to 0 if (!point) { + std::cout << "WARNING: Could not triangulate during linearize" << std::endl; BOOST_FOREACH(gtsam::Matrix& m, Gs) m = zeros(6,6); BOOST_FOREACH(Vector& v, gs) v = zero(6); return HessianFactor::shared_ptr(new HessianFactor(js, Gs, gs, f)); @@ -365,6 +384,7 @@ namespace gtsam { * to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5. */ virtual double error(const Values& values) const { + bool debug = false; if (this->active(values)) { double overallError=0; @@ -379,7 +399,23 @@ namespace gtsam { } // We triangulate the 3D position of the landmark - boost::optional point = triangulatePoint3(cameraPoses, measured_, *K_); + if (debug) { + BOOST_FOREACH(const Pose3& pose, cameraPoses) { + std::cout << "Pose: " << pose << std::endl; + } + BOOST_FOREACH(const Point2& point, measured_) { + std::cout << "Point: " << point << std::endl; + } + } + boost::optional point; + if (point_) { + point = point_; + //std::cout << "Using existing point " << *point << std::endl; + } else { + //std::cout << "Triangulate during error calc" << std::endl; + point = triangulatePoint3(cameraPoses, measured_, *K_); + } + if (debug) std::cout << "Result: " << *point << std::endl; if(point) { // triangulation produced a good estimate of landmark position @@ -392,7 +428,8 @@ namespace gtsam { overallError += noise_->distance( reprojectionError.vector() ); } return std::sqrt(overallError); - }else{ // triangulation failed: we deactivate the factor, then the error should not contribute to the overall error + } else{ // triangulation failed: we deactivate the factor, then the error should not contribute to the overall error + std::cout << "WARNING: Could not triangulate during error calc" << std::endl; return 0.0; } } else { @@ -429,5 +466,6 @@ namespace gtsam { ar & BOOST_SERIALIZATION_NVP(throwCheirality_); ar & BOOST_SERIALIZATION_NVP(verboseCheirality_); } + }; } // \ namespace gtsam diff --git a/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp b/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp index 5aa3b3540..2e277ab76 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp @@ -32,6 +32,7 @@ TEST(SmartProjectionFactor, disabled) #include #include #include +#include #include #include #include @@ -89,7 +90,7 @@ TEST( SmartProjectionFactor, ConstructorWithTransform) { measurements.push_back(Point2(323.0, 240.0)); Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); - TestSmartProjectionFactor factor(measurements, model, views, K, body_P_sensor); + TestSmartProjectionFactor factor(measurements, model, views, K, boost::none, body_P_sensor); } /* ************************************************************************* */ @@ -115,8 +116,8 @@ TEST( SmartProjectionFactor, EqualsWithTransform ) { std::vector views; views += X(1); - TestSmartProjectionFactor factor1(measurements, model, views, K, body_P_sensor); - TestSmartProjectionFactor factor2(measurements, model, views, K, body_P_sensor); + TestSmartProjectionFactor factor1(measurements, model, views, K, boost::none, body_P_sensor); + TestSmartProjectionFactor factor2(measurements, model, views, K, boost::none, body_P_sensor); CHECK(assert_equal(factor1, factor2)); } @@ -171,6 +172,145 @@ TEST( SmartProjectionFactor, noisy ){ // DOUBLES_EQUAL(expectedError, actualError, 1e-7); } +/* ************************************************************************* */ +TEST( SmartProjectionFactor, 3poses_1iteration_projection_factor_comparison ){ + cout << " ************************ SmartProjectionFactor: 3 cams + 3 landmarks, 1 iteration, comparison b/w Generic and Smart Projection Factors **********************" << endl; + + Symbol x1('X', 1); + Symbol x2('X', 2); + Symbol x3('X', 3); + + const SharedDiagonal noiseProjection = noiseModel::Isotropic::Sigma(2, 1); + + std::vector views; + views += x1, x2, x3; + + Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480)); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera cam1(pose1, *K); + + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + SimpleCamera cam2(pose2, *K); + + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + SimpleCamera cam3(pose3, *K); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + Point3 landmark3(3, 0, 3.0); + + vector measurements_cam1, measurements_cam2, measurements_cam3; + + // 1. Project three landmarks into three cameras and triangulate + Point2 cam1_uv1 = cam1.project(landmark1); + Point2 cam2_uv1 = cam2.project(landmark1); + Point2 cam3_uv1 = cam3.project(landmark1); + measurements_cam1 += cam1_uv1, cam2_uv1, cam3_uv1; + + // + Point2 cam1_uv2 = cam1.project(landmark2); + Point2 cam2_uv2 = cam2.project(landmark2); + Point2 cam3_uv2 = cam3.project(landmark2); + measurements_cam2 += cam1_uv2, cam2_uv2, cam3_uv2; + + + Point2 cam1_uv3 = cam1.project(landmark3); + Point2 cam2_uv3 = cam2.project(landmark3); + Point2 cam3_uv3 = cam3.project(landmark3); + measurements_cam3 += cam1_uv3, cam2_uv3, cam3_uv3; + + typedef SmartProjectionFactor SmartFactor; + typedef GenericProjectionFactor ProjectionFactor; + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(measurements_cam1, noiseProjection, views, K, boost::make_optional(landmark1) )); + SmartFactor::shared_ptr smartFactor2(new SmartFactor(measurements_cam2, noiseProjection, views, K, boost::make_optional(landmark2) )); + SmartFactor::shared_ptr smartFactor3(new SmartFactor(measurements_cam3, noiseProjection, views, K, boost::make_optional(landmark3) )); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graphWithOriginalFactor; + graphWithOriginalFactor.add(ProjectionFactor(cam1.project(landmark1), noiseProjection, x1, L(1), K)); + graphWithOriginalFactor.add(ProjectionFactor(cam2.project(landmark1), noiseProjection, x2, L(1), K)); + graphWithOriginalFactor.add(ProjectionFactor(cam3.project(landmark1), noiseProjection, x3, L(1), K)); + + graphWithOriginalFactor.add(ProjectionFactor(cam1.project(landmark2), noiseProjection, x1, L(2), K)); + graphWithOriginalFactor.add(ProjectionFactor(cam2.project(landmark2), noiseProjection, x2, L(2), K)); + graphWithOriginalFactor.add(ProjectionFactor(cam3.project(landmark2), noiseProjection, x3, L(2), K)); + + graphWithOriginalFactor.add(ProjectionFactor(cam1.project(landmark3), noiseProjection, x1, L(3), K)); + graphWithOriginalFactor.add(ProjectionFactor(cam2.project(landmark3), noiseProjection, x2, L(3), K)); + graphWithOriginalFactor.add(ProjectionFactor(cam3.project(landmark3), noiseProjection, x3, L(3), K)); + + graphWithOriginalFactor.add(PriorFactor(x1, pose1, noisePrior)); + graphWithOriginalFactor.add(PriorFactor(x2, pose2, noisePrior)); + + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); + Values valuesOriginalFactor; + valuesOriginalFactor.insert(x1, pose1); + valuesOriginalFactor.insert(x2, pose2); + valuesOriginalFactor.insert(x3, pose3* noise_pose); + valuesOriginalFactor.insert(L(1), landmark1); + valuesOriginalFactor.insert(L(2), landmark2); + valuesOriginalFactor.insert(L(3), landmark3); + + NonlinearFactorGraph graphWithSmartFactor; + graphWithSmartFactor.push_back(smartFactor1); + graphWithSmartFactor.push_back(smartFactor2); + graphWithSmartFactor.push_back(smartFactor3); + graphWithSmartFactor.add(PriorFactor(x1, pose1, noisePrior)); + graphWithSmartFactor.add(PriorFactor(x2, pose2, noisePrior)); + + Values valuesSmartFactor; + valuesSmartFactor.insert(x1, pose1); + valuesSmartFactor.insert(x2, pose2); + // initialize third pose with some noise, we expect it to move back to original pose3 + valuesSmartFactor.insert(x3, pose3*noise_pose); + valuesSmartFactor.at(x3).print("Pose3 before optimization: "); + pose3.print("Pose3 ground truth: "); + + LevenbergMarquardtParams params; + params.maxIterations = 1; + params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + params.verbosity = NonlinearOptimizerParams::ERROR; + + Values resultWithOriginalFactor; + std::cout << "\n=========================================" << std::endl; + std::cout << "Optimizing GenericProjectionFactor" << std::endl; + LevenbergMarquardtOptimizer optimizerForOriginalFactor(graphWithOriginalFactor, valuesOriginalFactor, params); + resultWithOriginalFactor = optimizerForOriginalFactor.optimize(); + + Values resultWithSmartFactor; + std::cout << "\n=========================================" << std::endl; + std::cout << "Optimizing SmartProjectionfactor" << std::endl; + LevenbergMarquardtOptimizer optimizerForSmartFactor(graphWithSmartFactor, valuesSmartFactor, params); + resultWithSmartFactor = optimizerForSmartFactor.optimize(); + + std::cout << "\n=========================================" << std::endl; + // result.print("results of 3 camera, 3 landmark optimization \n"); + resultWithOriginalFactor.at(x3).print("Original: Pose3 after optimization: "); + resultWithSmartFactor.at(x3).print("\nSmart: Pose3 after optimization: "); + EXPECT(assert_equal(resultWithOriginalFactor.at(x3),resultWithSmartFactor.at(x3))); + + GaussNewtonParams params2; + params2.maxIterations = 1; + Values resultWithOriginalFactor2; + GaussNewtonOptimizer optimizerForOriginalFactor2(graphWithOriginalFactor, valuesOriginalFactor, params2); + resultWithOriginalFactor2 = optimizerForOriginalFactor2.optimize(); + + Values resultWithSmartFactor2; + GaussNewtonOptimizer optimizerForSmartFactor2(graphWithSmartFactor, valuesSmartFactor, params2); + resultWithSmartFactor2 = optimizerForSmartFactor2.optimize(); + + resultWithOriginalFactor2.at(x3).print("Original: Pose3 after optimization (GaussNewton): "); + resultWithSmartFactor2.at(x3).print("\nSmart: Pose3 after optimization (GaussNewton): "); + +} + /* ************************************************************************* */ TEST( SmartProjectionFactor, 3poses_smart_projection_factor ){ @@ -341,6 +481,7 @@ TEST( SmartProjectionFactor, 3poses_projection_factor ){ } + /* ************************************************************************* */ TEST( SmartProjectionFactor, Hessian ){ cout << " ************************ SmartProjectionFactor: Hessian **********************" << endl;