diff --git a/gtsam/geometry/SphericalCamera.h b/gtsam/geometry/SphericalCamera.h index 01b749df4..b60639985 100644 --- a/gtsam/geometry/SphericalCamera.h +++ b/gtsam/geometry/SphericalCamera.h @@ -30,15 +30,34 @@ namespace gtsam { class GTSAM_EXPORT EmptyCal { + protected: + Matrix3 K_; public: - EmptyCal(){} + + ///< shared pointer to calibration object + EmptyCal() + : K_(Matrix3::Identity()) { + } + /// Default destructor virtual ~EmptyCal() = default; using shared_ptr = boost::shared_ptr; void print(const std::string& s) const { std::cout << "empty calibration: " << s << std::endl; } + Matrix3 K() const {return K_;} }; +// +//class GTSAM_EXPORT EmptyCal { +// public: +// EmptyCal(){} +// virtual ~EmptyCal() = default; +// using shared_ptr = boost::shared_ptr; +// void print(const std::string& s) const { +// std::cout << "empty calibration: " << s << std::endl; +// } +//}; + /** * A spherical camera class that has a Pose3 and measures bearing vectors * @addtogroup geometry diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index f67ca0740..1a4632d2c 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -70,6 +70,7 @@ public: typedef boost::shared_ptr shared_ptr; /// shorthand for a set of cameras + typedef CAMERA Camera; typedef CameraSet Cameras; /** diff --git a/gtsam/slam/SmartProjectionFactorP.h b/gtsam/slam/SmartProjectionFactorP.h index d1c4fd8ec..370df31bb 100644 --- a/gtsam/slam/SmartProjectionFactorP.h +++ b/gtsam/slam/SmartProjectionFactorP.h @@ -60,7 +60,6 @@ class SmartProjectionFactorP : public SmartProjectionFactor { static const int ZDim = 2; ///< Measurement dimension (Point2) protected: - /// vector of keys (one for each observation) with potentially repeated keys std::vector nonUniqueKeys_; diff --git a/gtsam/slam/tests/smartFactorScenarios.h b/gtsam/slam/tests/smartFactorScenarios.h index 8a3bc81f9..24a8fb86e 100644 --- a/gtsam/slam/tests/smartFactorScenarios.h +++ b/gtsam/slam/tests/smartFactorScenarios.h @@ -110,6 +110,7 @@ Camera cam2(pose_right, K); Camera cam3(pose_above, K); typedef GeneralSFMFactor SFMFactor; } + /* *************************************************************************/ // Cal3Bundler poses namespace bundlerPose { diff --git a/gtsam/slam/tests/testSmartProjectionFactorP.cpp b/gtsam/slam/tests/testSmartProjectionFactorP.cpp index 557a220df..99fdd51c8 100644 --- a/gtsam/slam/tests/testSmartProjectionFactorP.cpp +++ b/gtsam/slam/tests/testSmartProjectionFactorP.cpp @@ -1107,7 +1107,7 @@ TEST( SmartProjectionFactorP, optimization_3poses_sphericalCamera ) { emptyKs.push_back(emptyK); SmartProjectionParams params; - params.setRankTolerance(0.01); + params.setRankTolerance(0.1); SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model,params)); smartFactor1->add(measurements_lmk1, keys, emptyKs); diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index 524943a3f..7842f37df 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -40,8 +40,16 @@ namespace gtsam { template class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor { - public: + private: + typedef SmartProjectionFactor Base; + typedef SmartProjectionPoseFactorRollingShutter This; typedef typename CAMERA::CalibrationType CALIBRATION; + typedef typename CAMERA::Measurement MEASUREMENT; + typedef typename CAMERA::MeasurementVector MEASUREMENTS; + + static const int DimBlock = 12; ///< size of the variable stacking 2 poses from which the observation pose is interpolated + static const int DimPose = 6; ///< Pose3 dimension + static const int ZDim = 2; ///< Measurement dimension (Point2) protected: /// shared pointer to calibration object (one for each observation) @@ -57,22 +65,17 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor body_P_sensors_; public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - /// shorthand for base class type - typedef SmartProjectionFactor > Base; - - /// shorthand for this class - typedef SmartProjectionPoseFactorRollingShutter This; + typedef CAMERA Camera; + typedef CameraSet Cameras; + typedef Eigen::Matrix MatrixZD; // F blocks (derivatives wrt block of 2 poses) + typedef std::vector > FBlocks; // vector of F blocks /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; - static const int DimBlock = 12; ///< size of the variable stacking 2 poses from which the observation pose is interpolated - static const int DimPose = 6; ///< Pose3 dimension - static const int ZDim = 2; ///< Measurement dimension (Point2) - typedef Eigen::Matrix MatrixZD; // F blocks (derivatives wrt block of 2 poses) - typedef std::vector > FBlocks; // vector of F blocks + /// Default constructor, only for serialization + SmartProjectionPoseFactorRollingShutter() { + } /** * Constructor @@ -83,6 +86,9 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor& K, const Pose3 body_P_sensor = Pose3::identity()) { // store measurements in base class @@ -134,7 +140,7 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor>& world_P_body_key_pairs, const std::vector& alphas, const std::vector>& Ks, @@ -160,7 +166,7 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor>& world_P_body_key_pairs, const std::vector& alphas, const boost::shared_ptr& K, const Pose3 body_P_sensor = Pose3::identity()) { @@ -244,6 +250,43 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactoralphas() && keyPairsEqual && extrinsicCalibrationEqual; } + /** + * Collect all cameras involved in this factor + * @param values Values structure which must contain camera poses + * corresponding to keys involved in this factor + * @return Cameras + */ + typename Base::Cameras cameras(const Values& values) const override { + size_t numViews = this->measured_.size(); + assert(numViews == K_all_.size()); + assert(numViews == alphas_.size()); + assert(numViews == body_P_sensors_.size()); + assert(numViews == world_P_body_key_pairs_.size()); + + typename Base::Cameras cameras; + for (size_t i = 0; i < numViews; i++) { // for each measurement + const Pose3& w_P_body1 = values.at(world_P_body_key_pairs_[i].first); + const Pose3& w_P_body2 = values.at(world_P_body_key_pairs_[i].second); + double interpolationFactor = alphas_[i]; + const Pose3& w_P_body = interpolate(w_P_body1, w_P_body2, interpolationFactor); + const Pose3& body_P_cam = body_P_sensors_[i]; + const Pose3& w_P_cam = w_P_body.compose(body_P_cam); + cameras.emplace_back(w_P_cam, K_all_[i]); + } + return cameras; + } + + /** + * error calculates the error of the factor. + */ + double error(const Values& values) const override { + if (this->active(values)) { + return this->totalReprojectionError(this->cameras(values)); + } else { // else of active flag + return 0.0; + } + } + /** * Compute jacobian F, E and error vector at a given linearization point * @param values Values structure which must contain camera poses @@ -274,12 +317,11 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor(w_P_body1, w_P_body2,interpolationFactor, dInterpPose_dPoseBody1, dInterpPose_dPoseBody2); const Pose3& body_P_cam = body_P_sensors_[i]; const Pose3& w_P_cam = w_P_body.compose(body_P_cam, dPoseCam_dInterpPose); - PinholeCamera camera(w_P_cam, *K_all_[i]); + typename Base::Camera camera(w_P_cam, K_all_[i]); // get jacobians and error vector for current measurement - Point2 reprojectionError_i = Point2( - camera.project(*this->result_, dProject_dPoseCam, Ei) - - this->measured_.at(i)); + Point2 reprojectionError_i = camera.reprojectionError( + *this->result_, this->measured_.at(i), dProject_dPoseCam, Ei); Eigen::Matrix J; // 2 x 12 J.block(0, 0, ZDim, 6) = dProject_dPoseCam * dPoseCam_dInterpPose * dInterpPose_dPoseBody1; // (2x6) * (6x6) * (6x6) @@ -340,7 +382,7 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactornoiseModel_->Whiten(Fs[i]); - Matrix3 P = Base::Cameras::PointCov(E, lambda, diagonalDamping); + Matrix3 P = Cameras::PointCov(E, lambda, diagonalDamping); // Collect all the key pairs: these are the keys that correspond to the blocks in Fs (on which we apply the Schur Complement) KeyVector nonuniqueKeys; @@ -352,50 +394,13 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactorkeys_); return boost::make_shared < RegularHessianFactor > (this->keys_, augmentedHessianUniqueKeys); } - /** - * error calculates the error of the factor. - */ - double error(const Values& values) const override { - if (this->active(values)) { - return this->totalReprojectionError(this->cameras(values)); - } else { // else of active flag - return 0.0; - } - } - - /** - * Collect all cameras involved in this factor - * @param values Values structure which must contain camera poses - * corresponding to keys involved in this factor - * @return Cameras - */ - typename Base::Cameras cameras(const Values& values) const override { - size_t numViews = this->measured_.size(); - assert(numViews == K_all_.size()); - assert(numViews == alphas_.size()); - assert(numViews == body_P_sensors_.size()); - assert(numViews == world_P_body_key_pairs_.size()); - - typename Base::Cameras cameras; - for (size_t i = 0; i < numViews; i++) { // for each measurement - const Pose3& w_P_body1 = values.at(world_P_body_key_pairs_[i].first); - const Pose3& w_P_body2 = values.at(world_P_body_key_pairs_[i].second); - double interpolationFactor = alphas_[i]; - const Pose3& w_P_body = interpolate(w_P_body1, w_P_body2, interpolationFactor); - const Pose3& body_P_cam = body_P_sensors_[i]; - const Pose3& w_P_cam = w_P_body.compose(body_P_cam); - cameras.emplace_back(w_P_cam, K_all_[i]); - } - return cameras; - } - /** * Linearize to Gaussian Factor (possibly adding a damping factor Lambda for LM) * @param values Values structure which must contain camera poses and extrinsic pose for this factor diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index bf8a8c4ab..adf49e8cb 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -19,6 +19,7 @@ #include "gtsam/slam/tests/smartFactorScenarios.h" #include #include +#include #include #include #include @@ -72,6 +73,20 @@ Camera cam2(interp_pose2, sharedK); Camera cam3(interp_pose3, sharedK); } +/* ************************************************************************* */ +// default Cal3_S2 poses with rolling shutter effect +namespace sphericalCameraRS { +typedef SphericalCamera Camera; +typedef SmartProjectionPoseFactorRollingShutter SmartFactorRS_spherical; +Pose3 interp_pose1 = interpolate(level_pose,pose_right,interp_factor1); +Pose3 interp_pose2 = interpolate(pose_right,pose_above,interp_factor2); +Pose3 interp_pose3 = interpolate(pose_above,level_pose,interp_factor3); +static EmptyCal::shared_ptr emptyK; +Camera cam1(interp_pose1, emptyK); +Camera cam2(interp_pose2, emptyK); +Camera cam3(interp_pose3, emptyK); +} + LevenbergMarquardtParams lmParams; typedef SmartProjectionPoseFactorRollingShutter< PinholePose > SmartFactorRS; @@ -1040,6 +1055,79 @@ TEST( SmartProjectionPoseFactorRollingShutter, timing ) { } #endif +/* *************************************************************************/ +TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_sphericalCameras ) { + + using namespace sphericalCameraRS; + std::vector measurements_lmk1, measurements_lmk2, measurements_lmk3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); + + // create inputs + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1,x2)); + key_pairs.push_back(std::make_pair(x2,x3)); + key_pairs.push_back(std::make_pair(x3,x1)); + + std::vector interp_factors; + interp_factors.push_back(interp_factor1); + interp_factors.push_back(interp_factor2); + interp_factors.push_back(interp_factor3); + + SmartProjectionParams params; + params.setRankTolerance(0.1); + + SmartFactorRS_spherical::shared_ptr smartFactor1( + new SmartFactorRS_spherical(model,params)); + smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, emptyK); + + SmartFactorRS_spherical::shared_ptr smartFactor2( + new SmartFactorRS_spherical(model,params)); + smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, emptyK); + + SmartFactorRS_spherical::shared_ptr smartFactor3( + new SmartFactorRS_spherical(model,params)); + smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, emptyK); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.addPrior(x1, level_pose, noisePrior); + graph.addPrior(x2, pose_right, noisePrior); + + Values groundTruth; + groundTruth.insert(x1, level_pose); + groundTruth.insert(x2, pose_right); + groundTruth.insert(x3, pose_above); + DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); + + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, level_pose); + values.insert(x2, pose_right); + // initialize third pose with some noise, we expect it to move back to original pose_above + values.insert(x3, pose_above * noise_pose); + EXPECT( // check that the pose is actually noisy + assert_equal( + Pose3( + Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, + -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), values.at(x3))); + + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); +} + /* ************************************************************************* */ int main() { TestResult tr;