diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index b3a47ad2b..0d5d71602 100644 --- a/gtsam/slam/StereoFactor.h +++ b/gtsam/slam/StereoFactor.h @@ -34,6 +34,7 @@ private: // Keep a copy of measurement and calibration for I/O StereoPoint2 measured_; ///< the measurement boost::shared_ptr K_; ///< shared pointer to calibration + boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame public: @@ -56,8 +57,10 @@ public: * @param landmarkKey the landmark variable key * @param K the constant calibration */ - GenericStereoFactor(const StereoPoint2& measured, const SharedNoiseModel& model, Key poseKey, Key landmarkKey, const shared_ptrKStereo& K) : - Base(model, poseKey, landmarkKey), measured_(measured), K_(K) { + GenericStereoFactor(const StereoPoint2& measured, const SharedNoiseModel& model, + Key poseKey, Key landmarkKey, const shared_ptrKStereo& K, + boost::optional body_P_sensor = boost::none) : + Base(model, poseKey, landmarkKey), measured_(measured), K_(K), body_P_sensor_(body_P_sensor) { } virtual ~GenericStereoFactor() {} ///< Virtual destructor @@ -75,22 +78,40 @@ public: void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { Base::print(s, keyFormatter); measured_.print(s + ".z"); + if(this->body_P_sensor_) + this->body_P_sensor_->print(" sensor pose in body frame: "); } /** * equals */ virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { - const GenericStereoFactor* p = dynamic_cast (&f); - return p && Base::equals(f) && measured_.equals(p->measured_, tol); + const GenericStereoFactor* e = dynamic_cast (&f); + return e + && Base::equals(f) + && measured_.equals(e->measured_, tol) + && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); } /** h(x)-z */ Vector evaluateError(const Pose3& pose, const Point3& point, - boost::optional H1, boost::optional H2) const { + boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { try { - StereoCamera stereoCam(pose, K_); - return (stereoCam.project(point, H1, H2) - measured_).vector(); + if(body_P_sensor_) { + if(H1) { + gtsam::Matrix H0; + StereoCamera stereoCam(pose.compose(*body_P_sensor_, H0), K_); + StereoPoint2 reprojectionError(stereoCam.project(point, H1, H2) - measured_); + *H1 = *H1 * H0; + return reprojectionError.vector(); + } else { + StereoCamera stereoCam(pose.compose(*body_P_sensor_), K_); + return (stereoCam.project(point, H1, H2) - measured_).vector(); + } + } else { + StereoCamera stereoCam(pose, K_); + return (stereoCam.project(point, H1, H2) - measured_).vector(); + } } catch(StereoCheiralityException& e) { if (H1) *H1 = zeros(3,6); if (H2) *H2 = zeros(3,3); @@ -119,6 +140,7 @@ private: boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(K_); + ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); } }; diff --git a/gtsam/slam/tests/testStereoFactor.cpp b/gtsam/slam/tests/testStereoFactor.cpp index f8a17522e..3936d8f45 100644 --- a/gtsam/slam/tests/testStereoFactor.cpp +++ b/gtsam/slam/tests/testStereoFactor.cpp @@ -42,12 +42,148 @@ static boost::shared_ptr K(new Cal3_S2Stereo(625, 625, 0, 320, 24 // point X Y Z in meters static Point3 p(0, 0, 5); -static SharedNoiseModel sigma(noiseModel::Unit::Create(3)); +static SharedNoiseModel model(noiseModel::Unit::Create(3)); // Convenience for named keys using symbol_shorthand::X; using symbol_shorthand::L; +typedef GenericStereoFactor TestStereoFactor; + +/* ************************************************************************* */ +TEST( StereoFactor, Constructor) { + StereoPoint2 measurement(323, 318-50, 241); + + TestStereoFactor factor(measurement, model, X(1), L(1), K); +} + +/* ************************************************************************* */ +TEST( StereoFactor, ConstructorWithTransform) { + StereoPoint2 measurement(323, 318-50, 241); + Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); + + TestStereoFactor factor(measurement, model, X(1), L(1), K, body_P_sensor); +} + +/* ************************************************************************* */ +TEST( StereoFactor, Equals ) { + // Create two identical factors and make sure they're equal + StereoPoint2 measurement(323, 318-50, 241); + + TestStereoFactor factor1(measurement, model, X(1), L(1), K); + TestStereoFactor factor2(measurement, model, X(1), L(1), K); + + CHECK(assert_equal(factor1, factor2)); +} + +/* ************************************************************************* */ +TEST( StereoFactor, EqualsWithTransform ) { + // Create two identical factors and make sure they're equal + StereoPoint2 measurement(323, 318-50, 241); + Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); + + TestStereoFactor factor1(measurement, model, X(1), L(1), K, body_P_sensor); + TestStereoFactor factor2(measurement, model, X(1), L(1), K, body_P_sensor); + + CHECK(assert_equal(factor1, factor2)); +} + +/* ************************************************************************* */ +TEST( StereoFactor, Error ) { + // Create the factor with a measurement that is 3 pixels off in x + StereoPoint2 measurement(323, 318-50, 241); + TestStereoFactor factor(measurement, model, X(1), L(1), K); + + // Set the linearization point + Pose3 pose(Rot3(), Point3(0.0, 0.0, -6.25)); + Point3 point(0.0, 0.0, 0.0); + + // Use the factor to calculate the error + Vector actualError(factor.evaluateError(pose, point)); + + // The expected error is (-3.0, +2.0, -1.0) pixels / UnitCovariance + Vector expectedError = Vector_(3, -3.0, +2.0, -1.0); + + // Verify we get the expected error + CHECK(assert_equal(expectedError, actualError, 1e-9)); +} + +/* ************************************************************************* */ +TEST( StereoFactor, ErrorWithTransform ) { + // Create the factor with a measurement that is 3 pixels off in x + StereoPoint2 measurement(323, 318-50, 241); + Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); + TestStereoFactor factor(measurement, model, X(1), L(1), K, body_P_sensor); + + // Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0) + Pose3 pose(Rot3(), Point3(-6.50, 0.10 , -1.0)); + Point3 point(0.0, 0.0, 0.0); + + // Use the factor to calculate the error + Vector actualError(factor.evaluateError(pose, point)); + + // The expected error is (-3.0, +2.0, -1.0) pixels / UnitCovariance + Vector expectedError = Vector_(3, -3.0, +2.0, -1.0); + + // Verify we get the expected error + CHECK(assert_equal(expectedError, actualError, 1e-9)); +} + +/* ************************************************************************* */ +TEST( StereoFactor, Jacobian ) { + // Create the factor with a measurement that is 3 pixels off in x + StereoPoint2 measurement(323, 318-50, 241); + TestStereoFactor factor(measurement, model, X(1), L(1), K); + + // Set the linearization point + Pose3 pose(Rot3(), Point3(0.0, 0.0, -6.25)); + Point3 point(0.0, 0.0, 0.0); + + // Use the factor to calculate the Jacobians + Matrix H1Actual, H2Actual; + factor.evaluateError(pose, point, H1Actual, H2Actual); + + // The expected Jacobians + Matrix H1Expected = Matrix_(3, 6, 0.0, -625.0, 0.0, -100.0, 0.0, 0.0, + 0.0, -625.0, 0.0, -100.0, 0.0, -8.0, + 625.0, 0.0, 0.0, 0.0, -100.0, 0.0); + Matrix H2Expected = Matrix_(3, 3, 100.0, 0.0, 0.0, + 100.0, 0.0, 8.0, + 0.0, 100.0, 0.0); + + // Verify the Jacobians are correct + CHECK(assert_equal(H1Expected, H1Actual, 1e-3)); + CHECK(assert_equal(H2Expected, H2Actual, 1e-3)); +} + +/* ************************************************************************* */ +TEST( StereoFactor, JacobianWithTransform ) { + // Create the factor with a measurement that is 3 pixels off in x + StereoPoint2 measurement(323, 318-50, 241); + Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); + TestStereoFactor factor(measurement, model, X(1), L(1), K, body_P_sensor); + + // Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0) + Pose3 pose(Rot3(), Point3(-6.50, 0.10 , -1.0)); + Point3 point(0.0, 0.0, 0.0); + + // Use the factor to calculate the Jacobians + Matrix H1Actual, H2Actual; + factor.evaluateError(pose, point, H1Actual, H2Actual); + + // The expected Jacobians + Matrix H1Expected = Matrix_(3, 6, -100.0, 0.0, 650.0, 0.0, 100.0, 0.0, + -100.0, -8.0, 649.2, -8.0, 100.0, 0.0, + -10.0, -650.0, 0.0, 0.0, 0.0, 100.0); + Matrix H2Expected = Matrix_(3, 3, 0.0, -100.0, 0.0, + 8.0, -100.0, 0.0, + 0.0, 0.0, -100.0); + + // Verify the Jacobians are correct + CHECK(assert_equal(H1Expected, H1Actual, 1e-3)); + CHECK(assert_equal(H2Expected, H2Actual, 1e-3)); +} + /* ************************************************************************* */ TEST( StereoFactor, singlePoint) { @@ -55,9 +191,9 @@ TEST( StereoFactor, singlePoint) graph.add(NonlinearEquality(X(1), camera1)); - StereoPoint2 z14(320, 320.0-50, 240); + StereoPoint2 measurement(320, 320.0-50, 240); // arguments: measurement, sigma, cam#, measurement #, K, baseline (m) - graph.add(GenericStereoFactor(z14, sigma, X(1), L(1), K)); + graph.add(GenericStereoFactor(measurement, model, X(1), L(1), K)); // Create a configuration corresponding to the ground truth Values values;