diff --git a/gtsam/slam/SmartProjectionRigFactor.h b/gtsam/slam/SmartProjectionRigFactor.h index 71ef396f6..1bc43b2b5 100644 --- a/gtsam/slam/SmartProjectionRigFactor.h +++ b/gtsam/slam/SmartProjectionRigFactor.h @@ -73,6 +73,8 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { FastVector cameraIds_; public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + typedef CAMERA Camera; typedef CameraSet Cameras; @@ -129,7 +131,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { } /** Virtual destructor */ - ~SmartProjectionRigFactor() override {} + ~SmartProjectionRigFactor() override = default; /** * add a new measurement, corresponding to an observation from pose "poseKey" @@ -178,8 +180,8 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { if (cameraIds.size() == 0 && cameraRig_.size() > 1) { throw std::runtime_error( "SmartProjectionRigFactor: " - "camera rig includes multiple camera but add did not input " - "cameraIds"); + "camera rig includes multiple camera " + "but add did not input cameraIds"); } for (size_t i = 0; i < measurements.size(); i++) { add(measurements[i], poseKeys[i], @@ -378,9 +380,9 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar& BOOST_SERIALIZATION_NVP(nonUniqueKeys_); - ar& BOOST_SERIALIZATION_NVP(cameraRig_); - ar& BOOST_SERIALIZATION_NVP(cameraIds_); + //ar& BOOST_SERIALIZATION_NVP(nonUniqueKeys_); + // ar& BOOST_SERIALIZATION_NVP(cameraRig_); + //ar& BOOST_SERIALIZATION_NVP(cameraIds_); } }; // end of class declaration diff --git a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp index 280e4c28b..7e80eb009 100644 --- a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp @@ -1225,16 +1225,16 @@ TEST(SmartProjectionRigFactor, timing) { size_t nrTests = 10000; for (size_t i = 0; i < nrTests; i++) { - SmartRigFactor::shared_ptr smartFactorP( + SmartRigFactor::shared_ptr smartRigFactor( new SmartRigFactor(model, cameraRig, params)); - smartFactorP->add(measurements_lmk1[0], x1, cameraId1); - smartFactorP->add(measurements_lmk1[1], x1, cameraId1); + smartRigFactor->add(measurements_lmk1[0], x1, cameraId1); + smartRigFactor->add(measurements_lmk1[1], x1, cameraId1); Values values; values.insert(x1, pose1); values.insert(x2, pose2); gttic_(SmartRigFactor_LINEARIZE); - smartFactorP->linearize(values); + smartRigFactor->linearize(values); gttoc_(SmartRigFactor_LINEARIZE); } diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index 0a3bd4039..e46f76607 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -50,10 +50,6 @@ class SmartProjectionPoseFactorRollingShutter 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: /// The keys of the pose of the body (with respect to an external world /// frame): two consecutive poses for each observation @@ -72,12 +68,20 @@ class SmartProjectionPoseFactorRollingShutter FastVector cameraIds_; public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + 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 + 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; @@ -198,8 +202,8 @@ class SmartProjectionPoseFactorRollingShutter if (cameraIds.size() == 0 && cameraRig_.size() > 1) { throw std::runtime_error( "SmartProjectionPoseFactorRollingShutter: " - "camera rig includes multiple camera but add did not input " - "cameraIds"); + "camera rig includes multiple camera " + "but add did not input cameraIds"); } for (size_t i = 0; i < measurements.size(); i++) { add(measurements[i], world_P_body_key_pairs[i].first, @@ -276,6 +280,44 @@ class SmartProjectionPoseFactorRollingShutter e->cameraIds().begin()); } + /** + * 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 { + typename Base::Cameras cameras; + for (size_t i = 0; i < this->measured_.size(); + 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 typename Base::Camera& camera_i = cameraRig_[cameraIds_[i]]; + const Pose3& body_P_cam = camera_i.pose(); + const Pose3& w_P_cam = w_P_body.compose(body_P_cam); + cameras.emplace_back(w_P_cam, + make_shared( + camera_i.calibration())); + } + 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 @@ -406,43 +448,6 @@ class SmartProjectionPoseFactorRollingShutter 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 { - typename Base::Cameras cameras; - for (size_t i = 0; i < this->measured_.size(); - 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 = cameraRig_[cameraIds_[i]].pose(); - const Pose3& w_P_cam = w_P_body.compose(body_P_cam); - cameras.emplace_back(w_P_cam, - make_shared( - cameraRig_[cameraIds_[i]].calibration())); - } - return cameras; - } - /** * Linearize to Gaussian Factor (possibly adding a damping factor Lambda for * LM) @@ -459,8 +464,8 @@ class SmartProjectionPoseFactorRollingShutter return this->createHessianFactor(values, lambda); default: throw std::runtime_error( - "SmartProjectionPoseFactorRollingShutter: unknown linearization " - "mode"); + "SmartProjectionPoseFactorRollingShutter: " + "unknown linearization mode"); } }