diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index e22b35e56..1df9efd22 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -27,52 +27,49 @@ #include #include -namespace gtsam -{ +namespace gtsam { - /// Exception thrown by triangulateDLT when SVD returns rank < 3 - class GTSAM_EXPORT TriangulationUnderconstrainedException : public std::runtime_error - { - public: - TriangulationUnderconstrainedException() : std::runtime_error("Triangulation Underconstrained Exception.") - { - } - }; +/// Exception thrown by triangulateDLT when SVD returns rank < 3 +class GTSAM_EXPORT TriangulationUnderconstrainedException: public std::runtime_error { +public: + TriangulationUnderconstrainedException() : + std::runtime_error("Triangulation Underconstrained Exception.") { + } +}; - /// Exception thrown by triangulateDLT when landmark is behind one or more of the cameras - class GTSAM_EXPORT TriangulationCheiralityException : public std::runtime_error - { - public: - TriangulationCheiralityException() : std::runtime_error( - "Triangulation Cheirality Exception: The resulting landmark is behind one or more cameras.") - { - } - }; +/// Exception thrown by triangulateDLT when landmark is behind one or more of the cameras +class GTSAM_EXPORT TriangulationCheiralityException: public std::runtime_error { +public: + TriangulationCheiralityException() : + std::runtime_error( + "Triangulation Cheirality Exception: The resulting landmark is behind one or more cameras.") { + } +}; - /** +/** * DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312 * @param projection_matrices Projection matrices (K*P^-1) * @param measurements 2D measurements * @param rank_tol SVD rank tolerance * @return Triangulated point, in homogeneous coordinates */ - GTSAM_EXPORT Vector4 triangulateHomogeneousDLT( - const std::vector> &projection_matrices, - const Point2Vector &measurements, double rank_tol = 1e-9); +GTSAM_EXPORT Vector4 triangulateHomogeneousDLT( + const std::vector>& projection_matrices, + const Point2Vector& measurements, double rank_tol = 1e-9); - /** +/** * DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312 * @param projection_matrices Projection matrices (K*P^-1) * @param measurements 2D measurements * @param rank_tol SVD rank tolerance * @return Triangulated Point3 */ - GTSAM_EXPORT Point3 triangulateDLT( - const std::vector> &projection_matrices, - const Point2Vector &measurements, - double rank_tol = 1e-9); +GTSAM_EXPORT Point3 triangulateDLT( + const std::vector>& projection_matrices, + const Point2Vector& measurements, + double rank_tol = 1e-9); - /** +/** * Create a factor graph with projection factors from poses and one calibration * @param poses Camera poses * @param sharedCal shared pointer to single calibration object (monocular only!) @@ -81,29 +78,27 @@ namespace gtsam * @param initialEstimate * @return graph and initial values */ - template - std::pair triangulationGraph( - const std::vector &poses, boost::shared_ptr sharedCal, - const Point2Vector &measurements, Key landmarkKey, - const Point3 &initialEstimate) - { - Values values; - values.insert(landmarkKey, initialEstimate); // Initial landmark value - NonlinearFactorGraph graph; - static SharedNoiseModel unit2(noiseModel::Unit::Create(2)); - static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6)); - for (size_t i = 0; i < measurements.size(); i++) - { - const Pose3 &pose_i = poses[i]; - typedef PinholePose Camera; - Camera camera_i(pose_i, sharedCal); - graph.emplace_shared> // - (camera_i, measurements[i], unit2, landmarkKey); - } - return std::make_pair(graph, values); +template +std::pair triangulationGraph( + const std::vector& poses, boost::shared_ptr sharedCal, + const Point2Vector& measurements, Key landmarkKey, + const Point3& initialEstimate) { + Values values; + values.insert(landmarkKey, initialEstimate); // Initial landmark value + NonlinearFactorGraph graph; + static SharedNoiseModel unit2(noiseModel::Unit::Create(2)); + static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6)); + for (size_t i = 0; i < measurements.size(); i++) { + const Pose3& pose_i = poses[i]; + typedef PinholePose Camera; + Camera camera_i(pose_i, sharedCal); + graph.emplace_shared > // + (camera_i, measurements[i], unit2, landmarkKey); } + return std::make_pair(graph, values); +} - /** +/** * Create a factor graph with projection factors from pinhole cameras * (each camera has a pose and calibration) * @param cameras pinhole cameras (monocular or stereo) @@ -112,37 +107,35 @@ namespace gtsam * @param initialEstimate * @return graph and initial values */ - template - std::pair triangulationGraph( - const CameraSet &cameras, - const typename CAMERA::MeasurementVector &measurements, Key landmarkKey, - const Point3 &initialEstimate) - { - Values values; - values.insert(landmarkKey, initialEstimate); // Initial landmark value - NonlinearFactorGraph graph; - static SharedNoiseModel unit(noiseModel::Unit::Create( - traits::dimension)); - for (size_t i = 0; i < measurements.size(); i++) - { - const CAMERA &camera_i = cameras[i]; - graph.emplace_shared> // - (camera_i, measurements[i], unit, landmarkKey); - } - return std::make_pair(graph, values); +template +std::pair triangulationGraph( + const CameraSet& cameras, + const typename CAMERA::MeasurementVector& measurements, Key landmarkKey, + const Point3& initialEstimate) { + Values values; + values.insert(landmarkKey, initialEstimate); // Initial landmark value + NonlinearFactorGraph graph; + static SharedNoiseModel unit(noiseModel::Unit::Create( + traits::dimension)); + for (size_t i = 0; i < measurements.size(); i++) { + const CAMERA& camera_i = cameras[i]; + graph.emplace_shared > // + (camera_i, measurements[i], unit, landmarkKey); } + return std::make_pair(graph, values); +} - /** +/** * Optimize for triangulation * @param graph nonlinear factors for projection * @param values initial values * @param landmarkKey to refer to landmark * @return refined Point3 */ - GTSAM_EXPORT Point3 optimize(const NonlinearFactorGraph &graph, - const Values &values, Key landmarkKey); +GTSAM_EXPORT Point3 optimize(const NonlinearFactorGraph& graph, + const Values& values, Key landmarkKey); - /** +/** * Given an initial estimate , refine a point using measurements in several cameras * @param poses Camera poses * @param sharedCal shared pointer to single calibration object @@ -150,69 +143,63 @@ namespace gtsam * @param initialEstimate * @return refined Point3 */ - template - Point3 triangulateNonlinear(const std::vector &poses, - boost::shared_ptr sharedCal, - const Point2Vector &measurements, const Point3 &initialEstimate) - { +template +Point3 triangulateNonlinear(const std::vector& poses, + boost::shared_ptr sharedCal, + const Point2Vector& measurements, const Point3& initialEstimate) { - // Create a factor graph and initial values - Values values; - NonlinearFactorGraph graph; - boost::tie(graph, values) = triangulationGraph // - (poses, sharedCal, measurements, Symbol('p', 0), initialEstimate); + // Create a factor graph and initial values + Values values; + NonlinearFactorGraph graph; + boost::tie(graph, values) = triangulationGraph // + (poses, sharedCal, measurements, Symbol('p', 0), initialEstimate); - return optimize(graph, values, Symbol('p', 0)); - } + return optimize(graph, values, Symbol('p', 0)); +} - /** +/** * Given an initial estimate , refine a point using measurements in several cameras * @param cameras pinhole cameras (monocular or stereo) * @param measurements 2D measurements * @param initialEstimate * @return refined Point3 */ - template - Point3 triangulateNonlinear( - const CameraSet &cameras, - const typename CAMERA::MeasurementVector &measurements, const Point3 &initialEstimate) - { +template +Point3 triangulateNonlinear( + const CameraSet& cameras, + const typename CAMERA::MeasurementVector& measurements, const Point3& initialEstimate) { - // Create a factor graph and initial values - Values values; - NonlinearFactorGraph graph; - boost::tie(graph, values) = triangulationGraph // - (cameras, measurements, Symbol('p', 0), initialEstimate); + // Create a factor graph and initial values + Values values; + NonlinearFactorGraph graph; + boost::tie(graph, values) = triangulationGraph // + (cameras, measurements, Symbol('p', 0), initialEstimate); - return optimize(graph, values, Symbol('p', 0)); - } + return optimize(graph, values, Symbol('p', 0)); +} - /** +/** * Create a 3*4 camera projection matrix from calibration and pose. * Functor for partial application on calibration * @param pose The camera pose * @param cal The calibration * @return Returns a Matrix34 */ - template - struct CameraProjectionMatrix - { - CameraProjectionMatrix(const CALIBRATION &calibration) : K_(calibration.K()) - { - } - Matrix34 operator()(const Pose3 &pose) const - { - return K_ * (pose.inverse().matrix()).block<3, 4>(0, 0); - } +template +struct CameraProjectionMatrix { + CameraProjectionMatrix(const CALIBRATION& calibration) : + K_(calibration.K()) { + } + Matrix34 operator()(const Pose3& pose) const { + return K_ * (pose.inverse().matrix()).block<3, 4>(0, 0); + } +private: + const Matrix3 K_; +public: + GTSAM_MAKE_ALIGNED_OPERATOR_NEW +}; - private: - const Matrix3 K_; - - public: - GTSAM_MAKE_ALIGNED_OPERATOR_NEW - }; - - /** +/** * Function to triangulate 3D landmark point from an arbitrary number * of poses (at least 2) using the DLT. The function checks that the * resulting point lies in front of all cameras, but has no other checks @@ -224,45 +211,43 @@ namespace gtsam * @param optimize Flag to turn on nonlinear refinement of triangulation * @return Returns a Point3 */ - template - Point3 triangulatePoint3(const std::vector &poses, - boost::shared_ptr sharedCal, - const Point2Vector &measurements, double rank_tol = 1e-9, - bool optimize = false) - { +template +Point3 triangulatePoint3(const std::vector& poses, + boost::shared_ptr sharedCal, + const Point2Vector& measurements, double rank_tol = 1e-9, + bool optimize = false) { - assert(poses.size() == measurements.size()); - if (poses.size() < 2) - throw(TriangulationUnderconstrainedException()); + assert(poses.size() == measurements.size()); + if (poses.size() < 2) + throw(TriangulationUnderconstrainedException()); - // construct projection matrices from poses & calibration - std::vector> projection_matrices; - CameraProjectionMatrix createP(*sharedCal); // partially apply - for (const Pose3 &pose : poses) - projection_matrices.push_back(createP(pose)); + // construct projection matrices from poses & calibration + std::vector> projection_matrices; + CameraProjectionMatrix createP(*sharedCal); // partially apply + for(const Pose3& pose: poses) + projection_matrices.push_back(createP(pose)); - // Triangulate linearly - Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); + // Triangulate linearly + Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); - // Then refine using non-linear optimization - if (optimize) - point = triangulateNonlinear // - (poses, sharedCal, measurements, point); + // Then refine using non-linear optimization + if (optimize) + point = triangulateNonlinear // + (poses, sharedCal, measurements, point); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - // verify that the triangulated point lies in front of all cameras - for (const Pose3 &pose : poses) - { - const Point3 &p_local = pose.transformTo(point); - if (p_local.z() <= 0) - throw(TriangulationCheiralityException()); - } + // verify that the triangulated point lies in front of all cameras + for(const Pose3& pose: poses) { + const Point3& p_local = pose.transformTo(point); + if (p_local.z() <= 0) + throw(TriangulationCheiralityException()); + } #endif - return point; - } + return point; +} - /** +/** * Function to triangulate 3D landmark point from an arbitrary number * of poses (at least 2) using the DLT. This function is similar to the one * above, except that each camera has its own calibration. The function @@ -274,76 +259,72 @@ namespace gtsam * @param optimize Flag to turn on nonlinear refinement of triangulation * @return Returns a Point3 */ - template - Point3 triangulatePoint3( - const CameraSet &cameras, - const typename CAMERA::MeasurementVector &measurements, double rank_tol = 1e-9, - bool optimize = false) - { +template +Point3 triangulatePoint3( + const CameraSet& cameras, + const typename CAMERA::MeasurementVector& measurements, double rank_tol = 1e-9, + bool optimize = false) { - size_t m = cameras.size(); - assert(measurements.size() == m); + size_t m = cameras.size(); + assert(measurements.size() == m); - if (m < 2) - throw(TriangulationUnderconstrainedException()); + if (m < 2) + throw(TriangulationUnderconstrainedException()); - // construct projection matrices from poses & calibration - std::vector> projection_matrices; - for (const CAMERA &camera : cameras) - projection_matrices.push_back( - CameraProjectionMatrix(camera.calibration())( - camera.pose())); - Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); + // construct projection matrices from poses & calibration + std::vector> projection_matrices; + for(const CAMERA& camera: cameras) + projection_matrices.push_back( + CameraProjectionMatrix(camera.calibration())( + camera.pose())); + Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); - // The n refine using non-linear optimization - if (optimize) - point = triangulateNonlinear(cameras, measurements, point); + // The n refine using non-linear optimization + if (optimize) + point = triangulateNonlinear(cameras, measurements, point); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - // verify that the triangulated point lies in front of all cameras - for (const CAMERA &camera : cameras) - { - const Point3 &p_local = camera.pose().transformTo(point); - if (p_local.z() <= 0) - throw(TriangulationCheiralityException()); - } + // verify that the triangulated point lies in front of all cameras + for(const CAMERA& camera: cameras) { + const Point3& p_local = camera.pose().transformTo(point); + if (p_local.z() <= 0) + throw(TriangulationCheiralityException()); + } #endif - return point; - } + return point; +} - /// Pinhole-specific version - template - Point3 triangulatePoint3( - const CameraSet> &cameras, - const Point2Vector &measurements, double rank_tol = 1e-9, - bool optimize = false) - { - return triangulatePoint3> // - (cameras, measurements, rank_tol, optimize); - } +/// Pinhole-specific version +template +Point3 triangulatePoint3( + const CameraSet >& cameras, + const Point2Vector& measurements, double rank_tol = 1e-9, + bool optimize = false) { + return triangulatePoint3 > // + (cameras, measurements, rank_tol, optimize); +} - struct GTSAM_EXPORT TriangulationParameters - { +struct GTSAM_EXPORT TriangulationParameters { - double rankTolerance; ///< threshold to decide whether triangulation is result.degenerate - ///< (the rank is the number of singular values of the triangulation matrix which are larger than rankTolerance) - bool enableEPI; ///< if set to true, will refine triangulation using LM + double rankTolerance; ///< threshold to decide whether triangulation is result.degenerate + ///< (the rank is the number of singular values of the triangulation matrix which are larger than rankTolerance) + bool enableEPI; ///< if set to true, will refine triangulation using LM - /** + /** * if the landmark is triangulated at distance larger than this, * result is flagged as degenerate. */ - double landmarkDistanceThreshold; // + double landmarkDistanceThreshold; // - /** + /** * If this is nonnegative the we will check if the average reprojection error * is smaller than this threshold after triangulation, otherwise result is * flagged as degenerate. */ - double dynamicOutlierRejectionThreshold; + double dynamicOutlierRejectionThreshold; - /** + /** * Constructor * @param rankTol tolerance used to check if point triangulation is degenerate * @param enableEPI if true refine triangulation with embedded LM iterations @@ -351,194 +332,173 @@ namespace gtsam * @param dynamicOutlierRejectionThreshold or if average error larger than this * */ - TriangulationParameters(const double _rankTolerance = 1.0, - const bool _enableEPI = false, double _landmarkDistanceThreshold = -1, - double _dynamicOutlierRejectionThreshold = -1) : rankTolerance(_rankTolerance), enableEPI(_enableEPI), // - landmarkDistanceThreshold(_landmarkDistanceThreshold), // - dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold) - { - } - - // stream to output - friend std::ostream &operator<<(std::ostream &os, - const TriangulationParameters &p) - { - os << "rankTolerance = " << p.rankTolerance << std::endl; - os << "enableEPI = " << p.enableEPI << std::endl; - os << "landmarkDistanceThreshold = " << p.landmarkDistanceThreshold - << std::endl; - os << "dynamicOutlierRejectionThreshold = " - << p.dynamicOutlierRejectionThreshold << std::endl; - return os; - } - - private: - /// Serialization function - friend class boost::serialization::access; - template - void serialize(ARCHIVE &ar, const unsigned int version) - { - ar &BOOST_SERIALIZATION_NVP(rankTolerance); - ar &BOOST_SERIALIZATION_NVP(enableEPI); - ar &BOOST_SERIALIZATION_NVP(landmarkDistanceThreshold); - ar &BOOST_SERIALIZATION_NVP(dynamicOutlierRejectionThreshold); - } - }; - - /** - * TriangulationResult is an optional point, along with the reasons why it is invalid. - */ - class TriangulationResult : public boost::optional - { - enum Status - { - VALID, - DEGENERATE, - BEHIND_CAMERA, - OUTLIER, - FAR_POINT - }; - Status status_; - TriangulationResult(Status s) : status_(s) - { - } - - public: - /** - * Default constructor, only for serialization - */ - TriangulationResult() {} - - /** - * Constructor - */ - TriangulationResult(const Point3 &p) : status_(VALID) - { - reset(p); - } - static TriangulationResult Degenerate() - { - return TriangulationResult(DEGENERATE); - } - static TriangulationResult Outlier() - { - return TriangulationResult(OUTLIER); - } - static TriangulationResult FarPoint() - { - return TriangulationResult(FAR_POINT); - } - static TriangulationResult BehindCamera() - { - return TriangulationResult(BEHIND_CAMERA); - } - bool valid() const - { - return status_ == VALID; - } - bool degenerate() const - { - return status_ == DEGENERATE; - } - bool outlier() const - { - return status_ == OUTLIER; - } - bool farPoint() const - { - return status_ == FAR_POINT; - } - bool behindCamera() const - { - return status_ == BEHIND_CAMERA; - } - // stream to output - friend std::ostream &operator<<(std::ostream &os, - const TriangulationResult &result) - { - if (result) - os << "point = " << *result << std::endl; - else - os << "no point, status = " << result.status_ << std::endl; - return os; - } - - private: - /// Serialization function - friend class boost::serialization::access; - template - void serialize(ARCHIVE &ar, const unsigned int version) - { - ar &BOOST_SERIALIZATION_NVP(status_); - } - }; - - /// triangulateSafe: extensive checking of the outcome - template - TriangulationResult triangulateSafe(const CameraSet &cameras, - const typename CAMERA::MeasurementVector &measured, - const TriangulationParameters ¶ms) - { - - size_t m = cameras.size(); - - // if we have a single pose the corresponding factor is uninformative - if (m < 2) - return TriangulationResult::Degenerate(); - else - // We triangulate the 3D position of the landmark - try - { - Point3 point = triangulatePoint3(cameras, measured, - params.rankTolerance, params.enableEPI); - - // Check landmark distance and re-projection errors to avoid outliers - size_t i = 0; - double maxReprojError = 0.0; - for (const CAMERA &camera : cameras) - { - const Pose3 &pose = camera.pose(); - if (params.landmarkDistanceThreshold > 0 && distance3(pose.translation(), point) > params.landmarkDistanceThreshold) - return TriangulationResult::FarPoint(); -#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - // verify that the triangulated point lies in front of all cameras - // Only needed if this was not yet handled by exception - const Point3 &p_local = pose.transformTo(point); - if (p_local.z() <= 0) - return TriangulationResult::BehindCamera(); -#endif - // Check reprojection error - if (params.dynamicOutlierRejectionThreshold > 0) - { - const Point2 &zi = measured.at(i); - Point2 reprojectionError(camera.project(point) - zi); - maxReprojError = std::max(maxReprojError, reprojectionError.norm()); - } - i += 1; - } - // Flag as degenerate if average reprojection error is too large - if (params.dynamicOutlierRejectionThreshold > 0 && maxReprojError > params.dynamicOutlierRejectionThreshold) - return TriangulationResult::Outlier(); - - // all good! - return TriangulationResult(point); - } - catch (TriangulationUnderconstrainedException &) - { - // This exception is thrown if - // 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before - // 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark) - return TriangulationResult::Degenerate(); - } - catch (TriangulationCheiralityException &) - { - // point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers - return TriangulationResult::BehindCamera(); - } + TriangulationParameters(const double _rankTolerance = 1.0, + const bool _enableEPI = false, double _landmarkDistanceThreshold = -1, + double _dynamicOutlierRejectionThreshold = -1) : + rankTolerance(_rankTolerance), enableEPI(_enableEPI), // + landmarkDistanceThreshold(_landmarkDistanceThreshold), // + dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold) { } - // Vector of Cameras - used by the Python/MATLAB wrapper - using CameraSetCal3Bundler = CameraSet>; - using CameraSetCal3_S2 = CameraSet>; + // stream to output + friend std::ostream &operator<<(std::ostream &os, + const TriangulationParameters& p) { + os << "rankTolerance = " << p.rankTolerance << std::endl; + os << "enableEPI = " << p.enableEPI << std::endl; + os << "landmarkDistanceThreshold = " << p.landmarkDistanceThreshold + << std::endl; + os << "dynamicOutlierRejectionThreshold = " + << p.dynamicOutlierRejectionThreshold << std::endl; + return os; + } + +private: + + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(rankTolerance); + ar & BOOST_SERIALIZATION_NVP(enableEPI); + ar & BOOST_SERIALIZATION_NVP(landmarkDistanceThreshold); + ar & BOOST_SERIALIZATION_NVP(dynamicOutlierRejectionThreshold); + } +}; + +/** + * TriangulationResult is an optional point, along with the reasons why it is invalid. + */ +class TriangulationResult: public boost::optional { + enum Status { + VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT + }; + Status status_; + TriangulationResult(Status s) : + status_(s) { + } +public: + + /** + * Default constructor, only for serialization + */ + TriangulationResult() {} + + /** + * Constructor + */ + TriangulationResult(const Point3& p) : + status_(VALID) { + reset(p); + } + static TriangulationResult Degenerate() { + return TriangulationResult(DEGENERATE); + } + static TriangulationResult Outlier() { + return TriangulationResult(OUTLIER); + } + static TriangulationResult FarPoint() { + return TriangulationResult(FAR_POINT); + } + static TriangulationResult BehindCamera() { + return TriangulationResult(BEHIND_CAMERA); + } + bool valid() const { + return status_ == VALID; + } + bool degenerate() const { + return status_ == DEGENERATE; + } + bool outlier() const { + return status_ == OUTLIER; + } + bool farPoint() const { + return status_ == FAR_POINT; + } + bool behindCamera() const { + return status_ == BEHIND_CAMERA; + } + // stream to output + friend std::ostream &operator<<(std::ostream &os, + const TriangulationResult& result) { + if (result) + os << "point = " << *result << std::endl; + else + os << "no point, status = " << result.status_ << std::endl; + return os; + } + +private: + + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(status_); + } +}; + +/// triangulateSafe: extensive checking of the outcome +template +TriangulationResult triangulateSafe(const CameraSet& cameras, + const typename CAMERA::MeasurementVector& measured, + const TriangulationParameters& params) { + + size_t m = cameras.size(); + + // if we have a single pose the corresponding factor is uninformative + if (m < 2) + return TriangulationResult::Degenerate(); + else + // We triangulate the 3D position of the landmark + try { + Point3 point = triangulatePoint3(cameras, measured, + params.rankTolerance, params.enableEPI); + + // Check landmark distance and re-projection errors to avoid outliers + size_t i = 0; + double maxReprojError = 0.0; + for(const CAMERA& camera: cameras) { + const Pose3& pose = camera.pose(); + if (params.landmarkDistanceThreshold > 0 + && distance3(pose.translation(), point) + > params.landmarkDistanceThreshold) + return TriangulationResult::FarPoint(); +#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION + // verify that the triangulated point lies in front of all cameras + // Only needed if this was not yet handled by exception + const Point3& p_local = pose.transformTo(point); + if (p_local.z() <= 0) + return TriangulationResult::BehindCamera(); +#endif + // Check reprojection error + if (params.dynamicOutlierRejectionThreshold > 0) { + const Point2& zi = measured.at(i); + Point2 reprojectionError(camera.project(point) - zi); + maxReprojError = std::max(maxReprojError, reprojectionError.norm()); + } + i += 1; + } + // Flag as degenerate if average reprojection error is too large + if (params.dynamicOutlierRejectionThreshold > 0 + && maxReprojError > params.dynamicOutlierRejectionThreshold) + return TriangulationResult::Outlier(); + + // all good! + return TriangulationResult(point); + } catch (TriangulationUnderconstrainedException&) { + // This exception is thrown if + // 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before + // 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark) + return TriangulationResult::Degenerate(); + } catch (TriangulationCheiralityException&) { + // point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers + return TriangulationResult::BehindCamera(); + } +} + +// Vector of Cameras - used by the Python/MATLAB wrapper +using CameraSetCal3Bundler = CameraSet>; +using CameraSetCal3_S2 = CameraSet>; + +} // \namespace gtsam -} // namespace gtsam diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index b33e7ad6e..64e57a1d5 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -118,735 +118,708 @@ * - TODO: Add generalized serialization support via boost.serialization with hooks to matlab save/load */ -namespace gtsam -{ +namespace gtsam { // Actually a FastList #include - class KeyList - { - KeyList(); - KeyList(const gtsam::KeyList &other); +class KeyList { + KeyList(); + KeyList(const gtsam::KeyList& other); - // Note: no print function + // Note: no print function - // common STL methods - size_t size() const; - bool empty() const; - void clear(); + // common STL methods + size_t size() const; + bool empty() const; + void clear(); - // structure specific methods - size_t front() const; - size_t back() const; - void push_back(size_t key); - void push_front(size_t key); - void pop_back(); - void pop_front(); - void sort(); - void remove(size_t key); + // structure specific methods + size_t front() const; + size_t back() const; + void push_back(size_t key); + void push_front(size_t key); + void pop_back(); + void pop_front(); + void sort(); + void remove(size_t key); - void serialize() const; - }; + void serialize() const; +}; - // Actually a FastSet - class KeySet - { - KeySet(); - KeySet(const gtsam::KeySet &set); - KeySet(const gtsam::KeyVector &vector); - KeySet(const gtsam::KeyList &list); +// Actually a FastSet +class KeySet { + KeySet(); + KeySet(const gtsam::KeySet& set); + KeySet(const gtsam::KeyVector& vector); + KeySet(const gtsam::KeyList& list); - // Testable - void print(string s) const; - bool equals(const gtsam::KeySet &other) const; + // Testable + void print(string s) const; + bool equals(const gtsam::KeySet& other) const; - // common STL methods - size_t size() const; - bool empty() const; - void clear(); + // common STL methods + size_t size() const; + bool empty() const; + void clear(); - // structure specific methods - void insert(size_t key); - void merge(const gtsam::KeySet &other); - bool erase(size_t key); // returns true if value was removed - bool count(size_t key) const; // returns true if value exists + // structure specific methods + void insert(size_t key); + void merge(const gtsam::KeySet& other); + bool erase(size_t key); // returns true if value was removed + bool count(size_t key) const; // returns true if value exists - void serialize() const; - }; + void serialize() const; +}; - // Actually a vector - class KeyVector - { - KeyVector(); - KeyVector(const gtsam::KeyVector &other); +// Actually a vector +class KeyVector { + KeyVector(); + KeyVector(const gtsam::KeyVector& other); - // Note: no print function + // Note: no print function - // common STL methods - size_t size() const; - bool empty() const; - void clear(); + // common STL methods + size_t size() const; + bool empty() const; + void clear(); - // structure specific methods - size_t at(size_t i) const; - size_t front() const; - size_t back() const; - void push_back(size_t key) const; + // structure specific methods + size_t at(size_t i) const; + size_t front() const; + size_t back() const; + void push_back(size_t key) const; - void serialize() const; - }; + void serialize() const; +}; - // Actually a FastMap - class KeyGroupMap - { - KeyGroupMap(); +// Actually a FastMap +class KeyGroupMap { + KeyGroupMap(); - // Note: no print function + // Note: no print function - // common STL methods - size_t size() const; - bool empty() const; - void clear(); + // common STL methods + size_t size() const; + bool empty() const; + void clear(); - // structure specific methods - size_t at(size_t key) const; - int erase(size_t key); - bool insert2(size_t key, int val); - }; + // structure specific methods + size_t at(size_t key) const; + int erase(size_t key); + bool insert2(size_t key, int val); +}; - // Actually a FastSet - class FactorIndexSet - { - FactorIndexSet(); - FactorIndexSet(const gtsam::FactorIndexSet &set); +// Actually a FastSet +class FactorIndexSet { + FactorIndexSet(); + FactorIndexSet(const gtsam::FactorIndexSet& set); - // common STL methods - size_t size() const; - bool empty() const; - void clear(); + // common STL methods + size_t size() const; + bool empty() const; + void clear(); - // structure specific methods - void insert(size_t factorIndex); - bool erase(size_t factorIndex); // returns true if value was removed - bool count(size_t factorIndex) const; // returns true if value exists - }; + // structure specific methods + void insert(size_t factorIndex); + bool erase(size_t factorIndex); // returns true if value was removed + bool count(size_t factorIndex) const; // returns true if value exists +}; - // Actually a vector - class FactorIndices - { - FactorIndices(); - FactorIndices(const gtsam::FactorIndices &other); +// Actually a vector +class FactorIndices { + FactorIndices(); + FactorIndices(const gtsam::FactorIndices& other); - // common STL methods - size_t size() const; - bool empty() const; - void clear(); + // common STL methods + size_t size() const; + bool empty() const; + void clear(); - // structure specific methods - size_t at(size_t i) const; - size_t front() const; - size_t back() const; - void push_back(size_t factorIndex) const; - }; + // structure specific methods + size_t at(size_t i) const; + size_t front() const; + size_t back() const; + void push_back(size_t factorIndex) const; +}; - //************************************************************************* - // base - //************************************************************************* +//************************************************************************* +// base +//************************************************************************* - /** gtsam namespace functions */ +/** gtsam namespace functions */ #include - bool isDebugVersion(); +bool isDebugVersion(); #include - class IndexPair - { - IndexPair(); - IndexPair(size_t i, size_t j); - size_t i() const; - size_t j() const; - }; +class IndexPair { + IndexPair(); + IndexPair(size_t i, size_t j); + size_t i() const; + size_t j() const; +}; - // template - // class DSFMap { - // DSFMap(); - // KEY find(const KEY& key) const; - // void merge(const KEY& x, const KEY& y); - // std::map sets(); - // }; +// template +// class DSFMap { +// DSFMap(); +// KEY find(const KEY& key) const; +// void merge(const KEY& x, const KEY& y); +// std::map sets(); +// }; - class IndexPairSet - { - IndexPairSet(); - // common STL methods - size_t size() const; - bool empty() const; - void clear(); +class IndexPairSet { + IndexPairSet(); + // common STL methods + size_t size() const; + bool empty() const; + void clear(); - // structure specific methods - void insert(gtsam::IndexPair key); - bool erase(gtsam::IndexPair key); // returns true if value was removed - bool count(gtsam::IndexPair key) const; // returns true if value exists - }; + // structure specific methods + void insert(gtsam::IndexPair key); + bool erase(gtsam::IndexPair key); // returns true if value was removed + bool count(gtsam::IndexPair key) const; // returns true if value exists +}; - class IndexPairVector - { - IndexPairVector(); - IndexPairVector(const gtsam::IndexPairVector &other); +class IndexPairVector { + IndexPairVector(); + IndexPairVector(const gtsam::IndexPairVector& other); - // common STL methods - size_t size() const; - bool empty() const; - void clear(); + // common STL methods + size_t size() const; + bool empty() const; + void clear(); - // structure specific methods - gtsam::IndexPair at(size_t i) const; - void push_back(gtsam::IndexPair key) const; - }; + // structure specific methods + gtsam::IndexPair at(size_t i) const; + void push_back(gtsam::IndexPair key) const; +}; - gtsam::IndexPairVector IndexPairSetAsArray(gtsam::IndexPairSet &set); +gtsam::IndexPairVector IndexPairSetAsArray(gtsam::IndexPairSet& set); - class IndexPairSetMap - { - IndexPairSetMap(); - // common STL methods - size_t size() const; - bool empty() const; - void clear(); +class IndexPairSetMap { + IndexPairSetMap(); + // common STL methods + size_t size() const; + bool empty() const; + void clear(); - // structure specific methods - gtsam::IndexPairSet at(gtsam::IndexPair &key); - }; + // structure specific methods + gtsam::IndexPairSet at(gtsam::IndexPair& key); +}; - class DSFMapIndexPair - { - DSFMapIndexPair(); - gtsam::IndexPair find(const gtsam::IndexPair &key) const; - void merge(const gtsam::IndexPair &x, const gtsam::IndexPair &y); - gtsam::IndexPairSetMap sets(); - }; +class DSFMapIndexPair { + DSFMapIndexPair(); + gtsam::IndexPair find(const gtsam::IndexPair& key) const; + void merge(const gtsam::IndexPair& x, const gtsam::IndexPair& y); + gtsam::IndexPairSetMap sets(); +}; #include - bool linear_independent(Matrix A, Matrix B, double tol); +bool linear_independent(Matrix A, Matrix B, double tol); #include - virtual class Value - { - // No constructors because this is an abstract class +virtual class Value { + // No constructors because this is an abstract class - // Testable - void print(string s) const; + // Testable + void print(string s) const; - // Manifold - size_t dim() const; - }; + // Manifold + size_t dim() const; +}; #include - template - virtual class GenericValue : gtsam::Value - { - void serializable() const; - }; +template +virtual class GenericValue : gtsam::Value { + void serializable() const; +}; - //************************************************************************* - // geometry - //************************************************************************* +//************************************************************************* +// geometry +//************************************************************************* #include - class Point2 - { - // Standard Constructors - Point2(); - Point2(double x, double y); - Point2(Vector v); +class Point2 { + // Standard Constructors + Point2(); + Point2(double x, double y); + Point2(Vector v); - // Testable - void print(string s) const; - bool equals(const gtsam::Point2 &point, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::Point2& point, double tol) const; - // Group - static gtsam::Point2 identity(); + // Group + static gtsam::Point2 identity(); - // Standard Interface - double x() const; - double y() const; - Vector vector() const; - double distance(const gtsam::Point2 &p2) const; - double norm() const; + // Standard Interface + double x() const; + double y() const; + Vector vector() const; + double distance(const gtsam::Point2& p2) const; + double norm() const; - // enabling serialization functionality - void serialize() const; - }; + // enabling serialization functionality + void serialize() const; +}; // std::vector #include - class Point2Vector - { - // Constructors - Point2Vector(); - Point2Vector(const gtsam::Point2Vector &v); +class Point2Vector +{ + // Constructors + Point2Vector(); + Point2Vector(const gtsam::Point2Vector& v); - //Capacity - size_t size() const; - size_t max_size() const; - void resize(size_t sz); - size_t capacity() const; - bool empty() const; - void reserve(size_t n); + //Capacity + size_t size() const; + size_t max_size() const; + void resize(size_t sz); + size_t capacity() const; + bool empty() const; + void reserve(size_t n); - //Element access - gtsam::Point2 at(size_t n) const; - gtsam::Point2 front() const; - gtsam::Point2 back() const; + //Element access + gtsam::Point2 at(size_t n) const; + gtsam::Point2 front() const; + gtsam::Point2 back() const; - //Modifiers - void assign(size_t n, const gtsam::Point2 &u); - void push_back(const gtsam::Point2 &x); - void pop_back(); - }; + //Modifiers + void assign(size_t n, const gtsam::Point2& u); + void push_back(const gtsam::Point2& x); + void pop_back(); +}; #include - class StereoPoint2 - { - // Standard Constructors - StereoPoint2(); - StereoPoint2(double uL, double uR, double v); +class StereoPoint2 { + // Standard Constructors + StereoPoint2(); + StereoPoint2(double uL, double uR, double v); - // Testable - void print(string s) const; - bool equals(const gtsam::StereoPoint2 &point, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::StereoPoint2& point, double tol) const; - // Group - static gtsam::StereoPoint2 identity(); - gtsam::StereoPoint2 inverse() const; - gtsam::StereoPoint2 compose(const gtsam::StereoPoint2 &p2) const; - gtsam::StereoPoint2 between(const gtsam::StereoPoint2 &p2) const; + // Group + static gtsam::StereoPoint2 identity(); + gtsam::StereoPoint2 inverse() const; + gtsam::StereoPoint2 compose(const gtsam::StereoPoint2& p2) const; + gtsam::StereoPoint2 between(const gtsam::StereoPoint2& p2) const; - // Manifold - gtsam::StereoPoint2 retract(Vector v) const; - Vector localCoordinates(const gtsam::StereoPoint2 &p) const; + // Manifold + gtsam::StereoPoint2 retract(Vector v) const; + Vector localCoordinates(const gtsam::StereoPoint2& p) const; - // Lie Group - static gtsam::StereoPoint2 Expmap(Vector v); - static Vector Logmap(const gtsam::StereoPoint2 &p); + // Lie Group + static gtsam::StereoPoint2 Expmap(Vector v); + static Vector Logmap(const gtsam::StereoPoint2& p); - // Standard Interface - Vector vector() const; - double uL() const; - double uR() const; - double v() const; + // Standard Interface + Vector vector() const; + double uL() const; + double uR() const; + double v() const; - // enabling serialization functionality - void serialize() const; - }; + // enabling serialization functionality + void serialize() const; +}; #include - class Point3 - { - // Standard Constructors - Point3(); - Point3(double x, double y, double z); - Point3(Vector v); +class Point3 { + // Standard Constructors + Point3(); + Point3(double x, double y, double z); + Point3(Vector v); - // Testable - void print(string s) const; - bool equals(const gtsam::Point3 &p, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::Point3& p, double tol) const; - // Group - static gtsam::Point3 identity(); + // Group + static gtsam::Point3 identity(); - // Standard Interface - Vector vector() const; - double x() const; - double y() const; - double z() const; + // Standard Interface + Vector vector() const; + double x() const; + double y() const; + double z() const; - // enabling serialization functionality - void serialize() const; - }; + // enabling serialization functionality + void serialize() const; +}; #include - class Rot2 - { - // Standard Constructors and Named Constructors - Rot2(); - Rot2(double theta); - static gtsam::Rot2 fromAngle(double theta); - static gtsam::Rot2 fromDegrees(double theta); - static gtsam::Rot2 fromCosSin(double c, double s); +class Rot2 { + // Standard Constructors and Named Constructors + Rot2(); + Rot2(double theta); + static gtsam::Rot2 fromAngle(double theta); + static gtsam::Rot2 fromDegrees(double theta); + static gtsam::Rot2 fromCosSin(double c, double s); - // Testable - void print(string s) const; - bool equals(const gtsam::Rot2 &rot, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::Rot2& rot, double tol) const; - // Group - static gtsam::Rot2 identity(); - gtsam::Rot2 inverse(); - gtsam::Rot2 compose(const gtsam::Rot2 &p2) const; - gtsam::Rot2 between(const gtsam::Rot2 &p2) const; + // Group + static gtsam::Rot2 identity(); + gtsam::Rot2 inverse(); + gtsam::Rot2 compose(const gtsam::Rot2& p2) const; + gtsam::Rot2 between(const gtsam::Rot2& p2) const; - // Manifold - gtsam::Rot2 retract(Vector v) const; - Vector localCoordinates(const gtsam::Rot2 &p) const; + // Manifold + gtsam::Rot2 retract(Vector v) const; + Vector localCoordinates(const gtsam::Rot2& p) const; - // Lie Group - static gtsam::Rot2 Expmap(Vector v); - static Vector Logmap(const gtsam::Rot2 &p); - Vector logmap(const gtsam::Rot2 &p); + // Lie Group + static gtsam::Rot2 Expmap(Vector v); + static Vector Logmap(const gtsam::Rot2& p); + Vector logmap(const gtsam::Rot2& p); - // Group Action on Point2 - gtsam::Point2 rotate(const gtsam::Point2 &point) const; - gtsam::Point2 unrotate(const gtsam::Point2 &point) const; + // Group Action on Point2 + gtsam::Point2 rotate(const gtsam::Point2& point) const; + gtsam::Point2 unrotate(const gtsam::Point2& point) const; - // Standard Interface - static gtsam::Rot2 relativeBearing(const gtsam::Point2 &d); // Ignoring derivative - static gtsam::Rot2 atan2(double y, double x); - double theta() const; - double degrees() const; - double c() const; - double s() const; - Matrix matrix() const; + // Standard Interface + static gtsam::Rot2 relativeBearing(const gtsam::Point2& d); // Ignoring derivative + static gtsam::Rot2 atan2(double y, double x); + double theta() const; + double degrees() const; + double c() const; + double s() const; + Matrix matrix() const; - // enabling serialization functionality - void serialize() const; - }; + // enabling serialization functionality + void serialize() const; +}; #include - class SO3 - { - // Standard Constructors - SO3(); - SO3(Matrix R); - static gtsam::SO3 FromMatrix(Matrix R); - static gtsam::SO3 AxisAngle(const Vector axis, double theta); - static gtsam::SO3 ClosestTo(const Matrix M); +class SO3 { + // Standard Constructors + SO3(); + SO3(Matrix R); + static gtsam::SO3 FromMatrix(Matrix R); + static gtsam::SO3 AxisAngle(const Vector axis, double theta); + static gtsam::SO3 ClosestTo(const Matrix M); - // Testable - void print(string s) const; - bool equals(const gtsam::SO3 &other, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::SO3& other, double tol) const; - // Group - static gtsam::SO3 identity(); - gtsam::SO3 inverse() const; - gtsam::SO3 between(const gtsam::SO3 &R) const; - gtsam::SO3 compose(const gtsam::SO3 &R) const; + // Group + static gtsam::SO3 identity(); + gtsam::SO3 inverse() const; + gtsam::SO3 between(const gtsam::SO3& R) const; + gtsam::SO3 compose(const gtsam::SO3& R) const; - // Manifold - gtsam::SO3 retract(Vector v) const; - Vector localCoordinates(const gtsam::SO3 &R) const; - static gtsam::SO3 Expmap(Vector v); + // Manifold + gtsam::SO3 retract(Vector v) const; + Vector localCoordinates(const gtsam::SO3& R) const; + static gtsam::SO3 Expmap(Vector v); - // Other methods - Vector vec() const; - Matrix matrix() const; - }; + // Other methods + Vector vec() const; + Matrix matrix() const; +}; #include - class SO4 - { - // Standard Constructors - SO4(); - SO4(Matrix R); - static gtsam::SO4 FromMatrix(Matrix R); +class SO4 { + // Standard Constructors + SO4(); + SO4(Matrix R); + static gtsam::SO4 FromMatrix(Matrix R); - // Testable - void print(string s) const; - bool equals(const gtsam::SO4 &other, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::SO4& other, double tol) const; - // Group - static gtsam::SO4 identity(); - gtsam::SO4 inverse() const; - gtsam::SO4 between(const gtsam::SO4 &Q) const; - gtsam::SO4 compose(const gtsam::SO4 &Q) const; + // Group + static gtsam::SO4 identity(); + gtsam::SO4 inverse() const; + gtsam::SO4 between(const gtsam::SO4& Q) const; + gtsam::SO4 compose(const gtsam::SO4& Q) const; - // Manifold - gtsam::SO4 retract(Vector v) const; - Vector localCoordinates(const gtsam::SO4 &Q) const; - static gtsam::SO4 Expmap(Vector v); + // Manifold + gtsam::SO4 retract(Vector v) const; + Vector localCoordinates(const gtsam::SO4& Q) const; + static gtsam::SO4 Expmap(Vector v); - // Other methods - Vector vec() const; - Matrix matrix() const; - }; + // Other methods + Vector vec() const; + Matrix matrix() const; +}; #include - class SOn - { - // Standard Constructors - SOn(size_t n); - static gtsam::SOn FromMatrix(Matrix R); - static gtsam::SOn Lift(size_t n, Matrix R); +class SOn { + // Standard Constructors + SOn(size_t n); + static gtsam::SOn FromMatrix(Matrix R); + static gtsam::SOn Lift(size_t n, Matrix R); - // Testable - void print(string s) const; - bool equals(const gtsam::SOn &other, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::SOn& other, double tol) const; - // Group - static gtsam::SOn identity(); - gtsam::SOn inverse() const; - gtsam::SOn between(const gtsam::SOn &Q) const; - gtsam::SOn compose(const gtsam::SOn &Q) const; + // Group + static gtsam::SOn identity(); + gtsam::SOn inverse() const; + gtsam::SOn between(const gtsam::SOn& Q) const; + gtsam::SOn compose(const gtsam::SOn& Q) const; - // Manifold - gtsam::SOn retract(Vector v) const; - Vector localCoordinates(const gtsam::SOn &Q) const; - static gtsam::SOn Expmap(Vector v); + // Manifold + gtsam::SOn retract(Vector v) const; + Vector localCoordinates(const gtsam::SOn& Q) const; + static gtsam::SOn Expmap(Vector v); - // Other methods - Vector vec() const; - Matrix matrix() const; + // Other methods + Vector vec() const; + Matrix matrix() const; - // enabling serialization functionality - void serialize() const; - }; + // enabling serialization functionality + void serialize() const; +}; #include - class Quaternion - { - double w() const; - double x() const; - double y() const; - double z() const; - Vector coeffs() const; - }; +class Quaternion { + double w() const; + double x() const; + double y() const; + double z() const; + Vector coeffs() const; +}; #include - class Rot3 - { - // Standard Constructors and Named Constructors - Rot3(); - Rot3(Matrix R); - Rot3(const gtsam::Point3 &col1, const gtsam::Point3 &col2, const gtsam::Point3 &col3); - Rot3(double R11, double R12, double R13, - double R21, double R22, double R23, - double R31, double R32, double R33); - Rot3(double w, double x, double y, double z); +class Rot3 { + // Standard Constructors and Named Constructors + Rot3(); + Rot3(Matrix R); + Rot3(const gtsam::Point3& col1, const gtsam::Point3& col2, const gtsam::Point3& col3); + Rot3(double R11, double R12, double R13, + double R21, double R22, double R23, + double R31, double R32, double R33); + Rot3(double w, double x, double y, double z); - static gtsam::Rot3 Rx(double t); - static gtsam::Rot3 Ry(double t); - static gtsam::Rot3 Rz(double t); - static gtsam::Rot3 RzRyRx(double x, double y, double z); - static gtsam::Rot3 RzRyRx(Vector xyz); - static gtsam::Rot3 Yaw(double t); // positive yaw is to right (as in aircraft heading) - static gtsam::Rot3 Pitch(double t); // positive pitch is up (increasing aircraft altitude) - static gtsam::Rot3 Roll(double t); // positive roll is to right (increasing yaw in aircraft) - static gtsam::Rot3 Ypr(double y, double p, double r); - static gtsam::Rot3 Quaternion(double w, double x, double y, double z); - static gtsam::Rot3 AxisAngle(const gtsam::Point3 &axis, double angle); - static gtsam::Rot3 Rodrigues(Vector v); - static gtsam::Rot3 Rodrigues(double wx, double wy, double wz); - static gtsam::Rot3 ClosestTo(const Matrix M); + static gtsam::Rot3 Rx(double t); + static gtsam::Rot3 Ry(double t); + static gtsam::Rot3 Rz(double t); + static gtsam::Rot3 RzRyRx(double x, double y, double z); + static gtsam::Rot3 RzRyRx(Vector xyz); + static gtsam::Rot3 Yaw(double t); // positive yaw is to right (as in aircraft heading) + static gtsam::Rot3 Pitch(double t); // positive pitch is up (increasing aircraft altitude) + static gtsam::Rot3 Roll(double t); // positive roll is to right (increasing yaw in aircraft) + static gtsam::Rot3 Ypr(double y, double p, double r); + static gtsam::Rot3 Quaternion(double w, double x, double y, double z); + static gtsam::Rot3 AxisAngle(const gtsam::Point3& axis, double angle); + static gtsam::Rot3 Rodrigues(Vector v); + static gtsam::Rot3 Rodrigues(double wx, double wy, double wz); + static gtsam::Rot3 ClosestTo(const Matrix M); - // Testable - void print(string s) const; - bool equals(const gtsam::Rot3 &rot, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::Rot3& rot, double tol) const; - // Group - static gtsam::Rot3 identity(); + // Group + static gtsam::Rot3 identity(); gtsam::Rot3 inverse() const; - gtsam::Rot3 compose(const gtsam::Rot3 &p2) const; - gtsam::Rot3 between(const gtsam::Rot3 &p2) const; + gtsam::Rot3 compose(const gtsam::Rot3& p2) const; + gtsam::Rot3 between(const gtsam::Rot3& p2) const; - // Manifold - //gtsam::Rot3 retractCayley(Vector v) const; // TODO, does not exist in both Matrix and Quaternion options - gtsam::Rot3 retract(Vector v) const; - Vector localCoordinates(const gtsam::Rot3 &p) const; + // Manifold + //gtsam::Rot3 retractCayley(Vector v) const; // TODO, does not exist in both Matrix and Quaternion options + gtsam::Rot3 retract(Vector v) const; + Vector localCoordinates(const gtsam::Rot3& p) const; - // Group Action on Point3 - gtsam::Point3 rotate(const gtsam::Point3 &p) const; - gtsam::Point3 unrotate(const gtsam::Point3 &p) const; + // Group Action on Point3 + gtsam::Point3 rotate(const gtsam::Point3& p) const; + gtsam::Point3 unrotate(const gtsam::Point3& p) const; - // Standard Interface - static gtsam::Rot3 Expmap(Vector v); - static Vector Logmap(const gtsam::Rot3 &p); - Vector logmap(const gtsam::Rot3 &p); - Matrix matrix() const; - Matrix transpose() const; - gtsam::Point3 column(size_t index) const; - Vector xyz() const; - Vector ypr() const; - Vector rpy() const; - double roll() const; - double pitch() const; - double yaw() const; - pair axisAngle() const; - gtsam::Quaternion toQuaternion() const; - Vector quaternion() const; - gtsam::Rot3 slerp(double t, const gtsam::Rot3 &other) const; + // Standard Interface + static gtsam::Rot3 Expmap(Vector v); + static Vector Logmap(const gtsam::Rot3& p); + Vector logmap(const gtsam::Rot3& p); + Matrix matrix() const; + Matrix transpose() const; + gtsam::Point3 column(size_t index) const; + Vector xyz() const; + Vector ypr() const; + Vector rpy() const; + double roll() const; + double pitch() const; + double yaw() const; + pair axisAngle() const; + gtsam::Quaternion toQuaternion() const; + Vector quaternion() const; + gtsam::Rot3 slerp(double t, const gtsam::Rot3& other) const; - // enabling serialization functionality - void serialize() const; - }; + // enabling serialization functionality + void serialize() const; +}; #include - class Pose2 - { - // Standard Constructor - Pose2(); - Pose2(const gtsam::Pose2 &other); - Pose2(double x, double y, double theta); - Pose2(double theta, const gtsam::Point2 &t); - Pose2(const gtsam::Rot2 &r, const gtsam::Point2 &t); - Pose2(Vector v); +class Pose2 { + // Standard Constructor + Pose2(); + Pose2(const gtsam::Pose2& other); + Pose2(double x, double y, double theta); + Pose2(double theta, const gtsam::Point2& t); + Pose2(const gtsam::Rot2& r, const gtsam::Point2& t); + Pose2(Vector v); - // Testable - void print(string s) const; - bool equals(const gtsam::Pose2 &pose, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::Pose2& pose, double tol) const; - // Group - static gtsam::Pose2 identity(); - gtsam::Pose2 inverse() const; - gtsam::Pose2 compose(const gtsam::Pose2 &p2) const; - gtsam::Pose2 between(const gtsam::Pose2 &p2) const; + // Group + static gtsam::Pose2 identity(); + gtsam::Pose2 inverse() const; + gtsam::Pose2 compose(const gtsam::Pose2& p2) const; + gtsam::Pose2 between(const gtsam::Pose2& p2) const; - // Manifold - gtsam::Pose2 retract(Vector v) const; - Vector localCoordinates(const gtsam::Pose2 &p) const; + // Manifold + gtsam::Pose2 retract(Vector v) const; + Vector localCoordinates(const gtsam::Pose2& p) const; - // Lie Group - static gtsam::Pose2 Expmap(Vector v); - static Vector Logmap(const gtsam::Pose2 &p); - Vector logmap(const gtsam::Pose2 &p); - static Matrix ExpmapDerivative(Vector v); - static Matrix LogmapDerivative(const gtsam::Pose2 &v); - Matrix AdjointMap() const; - Vector Adjoint(Vector xi) const; - static Matrix adjointMap_(Vector v); - static Vector adjoint_(Vector xi, Vector y); - static Vector adjointTranspose(Vector xi, Vector y); - static Matrix wedge(double vx, double vy, double w); + // Lie Group + static gtsam::Pose2 Expmap(Vector v); + static Vector Logmap(const gtsam::Pose2& p); + Vector logmap(const gtsam::Pose2& p); + static Matrix ExpmapDerivative(Vector v); + static Matrix LogmapDerivative(const gtsam::Pose2& v); + Matrix AdjointMap() const; + Vector Adjoint(Vector xi) const; + static Matrix adjointMap_(Vector v); + static Vector adjoint_(Vector xi, Vector y); + static Vector adjointTranspose(Vector xi, Vector y); + static Matrix wedge(double vx, double vy, double w); - // Group Actions on Point2 - gtsam::Point2 transformFrom(const gtsam::Point2 &p) const; - gtsam::Point2 transformTo(const gtsam::Point2 &p) const; + // Group Actions on Point2 + gtsam::Point2 transformFrom(const gtsam::Point2& p) const; + gtsam::Point2 transformTo(const gtsam::Point2& p) const; - // Standard Interface - double x() const; - double y() const; - double theta() const; - gtsam::Rot2 bearing(const gtsam::Point2 &point) const; - double range(const gtsam::Point2 &point) const; - gtsam::Point2 translation() const; - gtsam::Rot2 rotation() const; - Matrix matrix() const; + // Standard Interface + double x() const; + double y() const; + double theta() const; + gtsam::Rot2 bearing(const gtsam::Point2& point) const; + double range(const gtsam::Point2& point) const; + gtsam::Point2 translation() const; + gtsam::Rot2 rotation() const; + Matrix matrix() const; - // enabling serialization functionality - void serialize() const; - }; + // enabling serialization functionality + void serialize() const; +}; #include - class Pose3 - { - // Standard Constructors - Pose3(); - Pose3(const gtsam::Pose3 &other); - Pose3(const gtsam::Rot3 &r, const gtsam::Point3 &t); - Pose3(const gtsam::Pose2 &pose2); - Pose3(Matrix mat); +class Pose3 { + // Standard Constructors + Pose3(); + Pose3(const gtsam::Pose3& other); + Pose3(const gtsam::Rot3& r, const gtsam::Point3& t); + Pose3(const gtsam::Pose2& pose2); + Pose3(Matrix mat); - // Testable - void print(string s) const; - bool equals(const gtsam::Pose3 &pose, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::Pose3& pose, double tol) const; - // Group - static gtsam::Pose3 identity(); - gtsam::Pose3 inverse() const; - gtsam::Pose3 compose(const gtsam::Pose3 &pose) const; - gtsam::Pose3 between(const gtsam::Pose3 &pose) const; + // Group + static gtsam::Pose3 identity(); + gtsam::Pose3 inverse() const; + gtsam::Pose3 compose(const gtsam::Pose3& pose) const; + gtsam::Pose3 between(const gtsam::Pose3& pose) const; - // Manifold - gtsam::Pose3 retract(Vector v) const; - Vector localCoordinates(const gtsam::Pose3 &pose) const; + // Manifold + gtsam::Pose3 retract(Vector v) const; + Vector localCoordinates(const gtsam::Pose3& pose) const; - // Lie Group - static gtsam::Pose3 Expmap(Vector v); - static Vector Logmap(const gtsam::Pose3 &pose); - Vector logmap(const gtsam::Pose3 &pose); - Matrix AdjointMap() const; - Vector Adjoint(Vector xi) const; - static Matrix adjointMap_(Vector xi); - static Vector adjoint_(Vector xi, Vector y); - static Vector adjointTranspose(Vector xi, Vector y); - static Matrix ExpmapDerivative(Vector xi); - static Matrix LogmapDerivative(const gtsam::Pose3 &xi); - static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz); + // Lie Group + static gtsam::Pose3 Expmap(Vector v); + static Vector Logmap(const gtsam::Pose3& pose); + Vector logmap(const gtsam::Pose3& pose); + Matrix AdjointMap() const; + Vector Adjoint(Vector xi) const; + static Matrix adjointMap_(Vector xi); + static Vector adjoint_(Vector xi, Vector y); + static Vector adjointTranspose(Vector xi, Vector y); + static Matrix ExpmapDerivative(Vector xi); + static Matrix LogmapDerivative(const gtsam::Pose3& xi); + static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz); - // Group Action on Point3 - gtsam::Point3 transformFrom(const gtsam::Point3 &point) const; - gtsam::Point3 transformTo(const gtsam::Point3 &point) const; + // Group Action on Point3 + gtsam::Point3 transformFrom(const gtsam::Point3& point) const; + gtsam::Point3 transformTo(const gtsam::Point3& point) const; - // Standard Interface - gtsam::Rot3 rotation() const; - gtsam::Point3 translation() const; - double x() const; - double y() const; - double z() const; - Matrix matrix() const; - gtsam::Pose3 transformPoseFrom(const gtsam::Pose3 &pose) const; - gtsam::Pose3 transformPoseTo(const gtsam::Pose3 &pose) const; - double range(const gtsam::Point3 &point); - double range(const gtsam::Pose3 &pose); + // Standard Interface + gtsam::Rot3 rotation() const; + gtsam::Point3 translation() const; + double x() const; + double y() const; + double z() const; + Matrix matrix() const; + gtsam::Pose3 transformPoseFrom(const gtsam::Pose3& pose) const; + gtsam::Pose3 transformPoseTo(const gtsam::Pose3& pose) const; + double range(const gtsam::Point3& point); + double range(const gtsam::Pose3& pose); - // enabling serialization functionality - void serialize() const; - }; + // enabling serialization functionality + void serialize() const; +}; // std::vector #include - class Pose3Vector - { - Pose3Vector(); - size_t size() const; - bool empty() const; - gtsam::Pose3 at(size_t n) const; - void push_back(const gtsam::Pose3 &pose); - }; +class Pose3Vector +{ + Pose3Vector(); + size_t size() const; + bool empty() const; + gtsam::Pose3 at(size_t n) const; + void push_back(const gtsam::Pose3& pose); +}; #include - class Unit3 - { - // Standard Constructors - Unit3(); - Unit3(const gtsam::Point3 &pose); +class Unit3 { + // Standard Constructors + Unit3(); + Unit3(const gtsam::Point3& pose); - // Testable - void print(string s) const; - bool equals(const gtsam::Unit3 &pose, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::Unit3& pose, double tol) const; - // Other functionality - Matrix basis() const; - Matrix skew() const; - gtsam::Point3 point3() const; + // Other functionality + Matrix basis() const; + Matrix skew() const; + gtsam::Point3 point3() const; - // Manifold - static size_t Dim(); - size_t dim() const; - gtsam::Unit3 retract(Vector v) const; - Vector localCoordinates(const gtsam::Unit3 &s) const; - }; + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::Unit3 retract(Vector v) const; + Vector localCoordinates(const gtsam::Unit3& s) const; +}; #include - class EssentialMatrix - { - // Standard Constructors - EssentialMatrix(const gtsam::Rot3 &aRb, const gtsam::Unit3 &aTb); +class EssentialMatrix { + // Standard Constructors + EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb); - // Testable - void print(string s) const; - bool equals(const gtsam::EssentialMatrix &pose, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::EssentialMatrix& pose, double tol) const; - // Manifold - static size_t Dim(); - size_t dim() const; - gtsam::EssentialMatrix retract(Vector v) const; - Vector localCoordinates(const gtsam::EssentialMatrix &s) const; + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::EssentialMatrix retract(Vector v) const; + Vector localCoordinates(const gtsam::EssentialMatrix& s) const; - // Other methods: - gtsam::Rot3 rotation() const; - gtsam::Unit3 direction() const; - Matrix matrix() const; - double error(Vector vA, Vector vB); - }; + // Other methods: + gtsam::Rot3 rotation() const; + gtsam::Unit3 direction() const; + Matrix matrix() const; + double error(Vector vA, Vector vB); +}; #include class Cal3_S2 { @@ -886,402 +859,389 @@ class Cal3_S2 { }; #include - virtual class Cal3DS2_Base - { - // Standard Constructors - Cal3DS2_Base(); +virtual class Cal3DS2_Base { + // Standard Constructors + Cal3DS2_Base(); - // Testable - void print(string s) const; + // Testable + void print(string s) const; - // Standard Interface - double fx() const; - double fy() const; - double skew() const; - double px() const; - double py() const; - double k1() const; - double k2() const; - Matrix K() const; - Vector k() const; - Vector vector() const; + // Standard Interface + double fx() const; + double fy() const; + double skew() const; + double px() const; + double py() const; + double k1() const; + double k2() const; + Matrix K() const; + Vector k() const; + Vector vector() const; - // Action on Point2 - gtsam::Point2 uncalibrate(const gtsam::Point2 &p) const; - gtsam::Point2 calibrate(const gtsam::Point2 &p) const; + // Action on Point2 + gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + gtsam::Point2 calibrate(const gtsam::Point2& p) const; - // enabling serialization functionality - void serialize() const; - }; + // enabling serialization functionality + void serialize() const; +}; #include - virtual class Cal3DS2 : gtsam::Cal3DS2_Base - { - // Standard Constructors - Cal3DS2(); - Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2); - Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2); - Cal3DS2(Vector v); +virtual class Cal3DS2 : gtsam::Cal3DS2_Base { + // Standard Constructors + Cal3DS2(); + Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2); + Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2); + Cal3DS2(Vector v); - // Testable - bool equals(const gtsam::Cal3DS2 &rhs, double tol) const; + // Testable + bool equals(const gtsam::Cal3DS2& rhs, double tol) const; - // Manifold - size_t dim() const; - static size_t Dim(); - gtsam::Cal3DS2 retract(Vector v) const; - Vector localCoordinates(const gtsam::Cal3DS2 &c) const; + // Manifold + size_t dim() const; + static size_t Dim(); + gtsam::Cal3DS2 retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3DS2& c) const; - // enabling serialization functionality - void serialize() const; - }; + // enabling serialization functionality + void serialize() const; +}; #include - virtual class Cal3Unified : gtsam::Cal3DS2_Base - { - // Standard Constructors - Cal3Unified(); - Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2); - Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2, double xi); - Cal3Unified(Vector v); +virtual class Cal3Unified : gtsam::Cal3DS2_Base { + // Standard Constructors + Cal3Unified(); + Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2); + Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2, double xi); + Cal3Unified(Vector v); - // Testable - bool equals(const gtsam::Cal3Unified &rhs, double tol) const; + // Testable + bool equals(const gtsam::Cal3Unified& rhs, double tol) const; - // Standard Interface - double xi() const; - gtsam::Point2 spaceToNPlane(const gtsam::Point2 &p) const; - gtsam::Point2 nPlaneToSpace(const gtsam::Point2 &p) const; + // Standard Interface + double xi() const; + gtsam::Point2 spaceToNPlane(const gtsam::Point2& p) const; + gtsam::Point2 nPlaneToSpace(const gtsam::Point2& p) const; - // Manifold - size_t dim() const; - static size_t Dim(); - gtsam::Cal3Unified retract(Vector v) const; - Vector localCoordinates(const gtsam::Cal3Unified &c) const; + // Manifold + size_t dim() const; + static size_t Dim(); + gtsam::Cal3Unified retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3Unified& c) const; - // enabling serialization functionality - void serialize() const; - }; + // enabling serialization functionality + void serialize() const; +}; #include - class Cal3_S2Stereo - { - // Standard Constructors - Cal3_S2Stereo(); - Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b); - Cal3_S2Stereo(Vector v); +class Cal3_S2Stereo { + // Standard Constructors + Cal3_S2Stereo(); + Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b); + Cal3_S2Stereo(Vector v); - // Testable - void print(string s) const; - bool equals(const gtsam::Cal3_S2Stereo &K, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::Cal3_S2Stereo& K, double tol) const; - // Standard Interface - double fx() const; - double fy() const; - double skew() const; - double px() const; - double py() const; - gtsam::Point2 principalPoint() const; - double baseline() const; - }; + // Standard Interface + double fx() const; + double fy() const; + double skew() const; + double px() const; + double py() const; + gtsam::Point2 principalPoint() const; + double baseline() const; +}; #include - class Cal3Bundler - { - // Standard Constructors - Cal3Bundler(); - Cal3Bundler(double fx, double k1, double k2, double u0, double v0); - Cal3Bundler(double fx, double k1, double k2, double u0, double v0, double tol); +class Cal3Bundler { + // Standard Constructors + Cal3Bundler(); + Cal3Bundler(double fx, double k1, double k2, double u0, double v0); + Cal3Bundler(double fx, double k1, double k2, double u0, double v0, double tol); - // Testable - void print(string s) const; - bool equals(const gtsam::Cal3Bundler &rhs, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::Cal3Bundler& rhs, double tol) const; - // Manifold - static size_t Dim(); - size_t dim() const; - gtsam::Cal3Bundler retract(Vector v) const; - Vector localCoordinates(const gtsam::Cal3Bundler &c) const; + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::Cal3Bundler retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3Bundler& c) const; - // Action on Point2 - gtsam::Point2 calibrate(const gtsam::Point2 &p) const; - gtsam::Point2 uncalibrate(const gtsam::Point2 &p) const; + // Action on Point2 + gtsam::Point2 calibrate(const gtsam::Point2& p) const; + gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; - // Standard Interface - double fx() const; - double fy() const; - double k1() const; - double k2() const; - double px() const; - double py() const; - Vector vector() const; - Vector k() const; - Matrix K() const; + // Standard Interface + double fx() const; + double fy() const; + double k1() const; + double k2() const; + double px() const; + double py() const; + Vector vector() const; + Vector k() const; + Matrix K() const; - // enabling serialization functionality - void serialize() const; - }; + // enabling serialization functionality + void serialize() const; +}; #include - class CalibratedCamera - { - // Standard Constructors and Named Constructors - CalibratedCamera(); - CalibratedCamera(const gtsam::Pose3 &pose); - CalibratedCamera(Vector v); - static gtsam::CalibratedCamera Level(const gtsam::Pose2 &pose2, double height); +class CalibratedCamera { + // Standard Constructors and Named Constructors + CalibratedCamera(); + CalibratedCamera(const gtsam::Pose3& pose); + CalibratedCamera(Vector v); + static gtsam::CalibratedCamera Level(const gtsam::Pose2& pose2, double height); - // Testable - void print(string s) const; - bool equals(const gtsam::CalibratedCamera &camera, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::CalibratedCamera& camera, double tol) const; - // Manifold - static size_t Dim(); - size_t dim() const; - gtsam::CalibratedCamera retract(Vector d) const; - Vector localCoordinates(const gtsam::CalibratedCamera &T2) const; + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::CalibratedCamera retract(Vector d) const; + Vector localCoordinates(const gtsam::CalibratedCamera& T2) const; - // Action on Point3 - gtsam::Point2 project(const gtsam::Point3 &point) const; - static gtsam::Point2 Project(const gtsam::Point3 &cameraPoint); + // Action on Point3 + gtsam::Point2 project(const gtsam::Point3& point) const; + static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); - // Standard Interface - gtsam::Pose3 pose() const; - double range(const gtsam::Point3 &p) const; // TODO: Other overloaded range methods + // Standard Interface + gtsam::Pose3 pose() const; + double range(const gtsam::Point3& p) const; // TODO: Other overloaded range methods - // enabling serialization functionality - void serialize() const; - }; + // enabling serialization functionality + void serialize() const; +}; #include - template - class PinholeCamera - { - // Standard Constructors and Named Constructors - PinholeCamera(); - PinholeCamera(const gtsam::Pose3 &pose); - PinholeCamera(const gtsam::Pose3 &pose, const CALIBRATION &K); - static This Level(const CALIBRATION &K, const gtsam::Pose2 &pose, double height); - static This Level(const gtsam::Pose2 &pose, double height); - static This Lookat(const gtsam::Point3 &eye, const gtsam::Point3 &target, - const gtsam::Point3 &upVector, const CALIBRATION &K); +template +class PinholeCamera { + // Standard Constructors and Named Constructors + PinholeCamera(); + PinholeCamera(const gtsam::Pose3& pose); + PinholeCamera(const gtsam::Pose3& pose, const CALIBRATION& K); + static This Level(const CALIBRATION& K, const gtsam::Pose2& pose, double height); + static This Level(const gtsam::Pose2& pose, double height); + static This Lookat(const gtsam::Point3& eye, const gtsam::Point3& target, + const gtsam::Point3& upVector, const CALIBRATION& K); - // Testable - void print(string s) const; - bool equals(const This &camera, double tol) const; + // Testable + void print(string s) const; + bool equals(const This& camera, double tol) const; - // Standard Interface - gtsam::Pose3 pose() const; - CALIBRATION calibration() const; + // Standard Interface + gtsam::Pose3 pose() const; + CALIBRATION calibration() const; - // Manifold - This retract(Vector d) const; - Vector localCoordinates(const This &T2) const; - size_t dim() const; - static size_t Dim(); + // Manifold + This retract(Vector d) const; + Vector localCoordinates(const This& T2) const; + size_t dim() const; + static size_t Dim(); - // Transformations and measurement functions - static gtsam::Point2 Project(const gtsam::Point3 &cameraPoint); - pair projectSafe(const gtsam::Point3 &pw) const; - gtsam::Point2 project(const gtsam::Point3 &point); - gtsam::Point3 backproject(const gtsam::Point2 &p, double depth) const; - double range(const gtsam::Point3 &point); - double range(const gtsam::Pose3 &pose); + // Transformations and measurement functions + static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); + pair projectSafe(const gtsam::Point3& pw) const; + gtsam::Point2 project(const gtsam::Point3& point); + gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; + double range(const gtsam::Point3& point); + double range(const gtsam::Pose3& pose); - // enabling serialization functionality - void serialize() const; - }; + // enabling serialization functionality + void serialize() const; +}; // Forward declaration of PinholeCameraCalX is defined here. #include - // Some typedefs for common camera types - // PinholeCameraCal3_S2 is the same as SimpleCamera above - typedef gtsam::PinholeCamera PinholeCameraCal3_S2; - typedef gtsam::PinholeCamera PinholeCameraCal3DS2; - typedef gtsam::PinholeCamera PinholeCameraCal3Unified; - typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; +// Some typedefs for common camera types +// PinholeCameraCal3_S2 is the same as SimpleCamera above +typedef gtsam::PinholeCamera PinholeCameraCal3_S2; +typedef gtsam::PinholeCamera PinholeCameraCal3DS2; +typedef gtsam::PinholeCamera PinholeCameraCal3Unified; +typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; - template - class CameraSet - { - CameraSet(); - // structure specific methods - T at(size_t i) const; - void push_back(const T &cam); - }; +template +class CameraSet { + CameraSet(); + + // structure specific methods + T at(size_t i) const; + void push_back(const T& cam); +}; #include - class StereoCamera - { - // Standard Constructors and Named Constructors - StereoCamera(); - StereoCamera(const gtsam::Pose3 &pose, const gtsam::Cal3_S2Stereo *K); +class StereoCamera { + // Standard Constructors and Named Constructors + StereoCamera(); + StereoCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2Stereo* K); - // Testable - void print(string s) const; - bool equals(const gtsam::StereoCamera &camera, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::StereoCamera& camera, double tol) const; - // Standard Interface - gtsam::Pose3 pose() const; - double baseline() const; - gtsam::Cal3_S2Stereo calibration() const; + // Standard Interface + gtsam::Pose3 pose() const; + double baseline() const; + gtsam::Cal3_S2Stereo calibration() const; - // Manifold - gtsam::StereoCamera retract(Vector d) const; - Vector localCoordinates(const gtsam::StereoCamera &T2) const; - size_t dim() const; - static size_t Dim(); + // Manifold + gtsam::StereoCamera retract(Vector d) const; + Vector localCoordinates(const gtsam::StereoCamera& T2) const; + size_t dim() const; + static size_t Dim(); - // Transformations and measurement functions - gtsam::StereoPoint2 project(const gtsam::Point3 &point); - gtsam::Point3 backproject(const gtsam::StereoPoint2 &p) const; + // Transformations and measurement functions + gtsam::StereoPoint2 project(const gtsam::Point3& point); + gtsam::Point3 backproject(const gtsam::StereoPoint2& p) const; - // enabling serialization functionality - void serialize() const; - }; + // enabling serialization functionality + void serialize() const; +}; #include - // Templates appear not yet supported for free functions - issue raised at borglab/wrap#14 to add support - gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector &poses, - gtsam::Cal3_S2 *sharedCal, const gtsam::Point2Vector &measurements, - double rank_tol, bool optimize); - gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector &poses, - gtsam::Cal3DS2 *sharedCal, const gtsam::Point2Vector &measurements, - double rank_tol, bool optimize); - gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector &poses, - gtsam::Cal3Bundler *sharedCal, const gtsam::Point2Vector &measurements, - double rank_tol, bool optimize); - gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3_S2 &cameras, - const gtsam::Point2Vector &measurements, double rank_tol, - bool optimize); - gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Bundler &cameras, - const gtsam::Point2Vector &measurements, double rank_tol, - bool optimize); - - //************************************************************************* - // Symbolic - //************************************************************************* +// Templates appear not yet supported for free functions - issue raised at borglab/wrap#14 to add support +gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, + gtsam::Cal3_S2* sharedCal, const gtsam::Point2Vector& measurements, + double rank_tol, bool optimize); +gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, + gtsam::Cal3DS2* sharedCal, const gtsam::Point2Vector& measurements, + double rank_tol, bool optimize); +gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, + gtsam::Cal3Bundler* sharedCal, const gtsam::Point2Vector& measurements, + double rank_tol, bool optimize); +gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3_S2& cameras, + const gtsam::Point2Vector& measurements, double rank_tol, + bool optimize); +gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Bundler& cameras, + const gtsam::Point2Vector& measurements, double rank_tol, + bool optimize); + +//************************************************************************* +// Symbolic +//************************************************************************* #include - virtual class SymbolicFactor - { - // Standard Constructors and Named Constructors - SymbolicFactor(const gtsam::SymbolicFactor &f); - SymbolicFactor(); - SymbolicFactor(size_t j); - SymbolicFactor(size_t j1, size_t j2); - SymbolicFactor(size_t j1, size_t j2, size_t j3); - SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4); - SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4, size_t j5); - SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4, size_t j5, size_t j6); - static gtsam::SymbolicFactor FromKeys(const gtsam::KeyVector &js); +virtual class SymbolicFactor { + // Standard Constructors and Named Constructors + SymbolicFactor(const gtsam::SymbolicFactor& f); + SymbolicFactor(); + SymbolicFactor(size_t j); + SymbolicFactor(size_t j1, size_t j2); + SymbolicFactor(size_t j1, size_t j2, size_t j3); + SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4); + SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4, size_t j5); + SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4, size_t j5, size_t j6); + static gtsam::SymbolicFactor FromKeys(const gtsam::KeyVector& js); - // From Factor - size_t size() const; - void print(string s) const; - bool equals(const gtsam::SymbolicFactor &other, double tol) const; - gtsam::KeyVector keys(); - }; + // From Factor + size_t size() const; + void print(string s) const; + bool equals(const gtsam::SymbolicFactor& other, double tol) const; + gtsam::KeyVector keys(); +}; #include - virtual class SymbolicFactorGraph - { - SymbolicFactorGraph(); - SymbolicFactorGraph(const gtsam::SymbolicBayesNet &bayesNet); - SymbolicFactorGraph(const gtsam::SymbolicBayesTree &bayesTree); +virtual class SymbolicFactorGraph { + SymbolicFactorGraph(); + SymbolicFactorGraph(const gtsam::SymbolicBayesNet& bayesNet); + SymbolicFactorGraph(const gtsam::SymbolicBayesTree& bayesTree); - // From FactorGraph - void push_back(gtsam::SymbolicFactor *factor); - void print(string s) const; - bool equals(const gtsam::SymbolicFactorGraph &rhs, double tol) const; - size_t size() const; - bool exists(size_t idx) const; + // From FactorGraph + void push_back(gtsam::SymbolicFactor* factor); + void print(string s) const; + bool equals(const gtsam::SymbolicFactorGraph& rhs, double tol) const; + size_t size() const; + bool exists(size_t idx) const; - // Standard interface - gtsam::KeySet keys() const; - void push_back(const gtsam::SymbolicFactorGraph &graph); - void push_back(const gtsam::SymbolicBayesNet &bayesNet); - void push_back(const gtsam::SymbolicBayesTree &bayesTree); + // Standard interface + gtsam::KeySet keys() const; + void push_back(const gtsam::SymbolicFactorGraph& graph); + void push_back(const gtsam::SymbolicBayesNet& bayesNet); + void push_back(const gtsam::SymbolicBayesTree& bayesTree); - //Advanced Interface - void push_factor(size_t key); - void push_factor(size_t key1, size_t key2); - void push_factor(size_t key1, size_t key2, size_t key3); - void push_factor(size_t key1, size_t key2, size_t key3, size_t key4); + //Advanced Interface + void push_factor(size_t key); + void push_factor(size_t key1, size_t key2); + void push_factor(size_t key1, size_t key2, size_t key3); + void push_factor(size_t key1, size_t key2, size_t key3, size_t key4); - gtsam::SymbolicBayesNet *eliminateSequential(); - gtsam::SymbolicBayesNet *eliminateSequential(const gtsam::Ordering &ordering); - gtsam::SymbolicBayesTree *eliminateMultifrontal(); - gtsam::SymbolicBayesTree *eliminateMultifrontal(const gtsam::Ordering &ordering); - pair eliminatePartialSequential( - const gtsam::Ordering &ordering); - pair eliminatePartialSequential( - const gtsam::KeyVector &keys); - pair eliminatePartialMultifrontal( - const gtsam::Ordering &ordering); - pair eliminatePartialMultifrontal( - const gtsam::KeyVector &keys); - gtsam::SymbolicBayesNet *marginalMultifrontalBayesNet(const gtsam::Ordering &ordering); - gtsam::SymbolicBayesNet *marginalMultifrontalBayesNet(const gtsam::KeyVector &key_vector); - gtsam::SymbolicBayesNet *marginalMultifrontalBayesNet(const gtsam::Ordering &ordering, - const gtsam::Ordering &marginalizedVariableOrdering); - gtsam::SymbolicBayesNet *marginalMultifrontalBayesNet(const gtsam::KeyVector &key_vector, - const gtsam::Ordering &marginalizedVariableOrdering); - gtsam::SymbolicFactorGraph *marginal(const gtsam::KeyVector &key_vector); - }; + gtsam::SymbolicBayesNet* eliminateSequential(); + gtsam::SymbolicBayesNet* eliminateSequential(const gtsam::Ordering& ordering); + gtsam::SymbolicBayesTree* eliminateMultifrontal(); + gtsam::SymbolicBayesTree* eliminateMultifrontal(const gtsam::Ordering& ordering); + pair eliminatePartialSequential( + const gtsam::Ordering& ordering); + pair eliminatePartialSequential( + const gtsam::KeyVector& keys); + pair eliminatePartialMultifrontal( + const gtsam::Ordering& ordering); + pair eliminatePartialMultifrontal( + const gtsam::KeyVector& keys); + gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering); + gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector); + gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering, + const gtsam::Ordering& marginalizedVariableOrdering); + gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector, + const gtsam::Ordering& marginalizedVariableOrdering); + gtsam::SymbolicFactorGraph* marginal(const gtsam::KeyVector& key_vector); +}; #include - virtual class SymbolicConditional : gtsam::SymbolicFactor - { - // Standard Constructors and Named Constructors - SymbolicConditional(); - SymbolicConditional(const gtsam::SymbolicConditional &other); - SymbolicConditional(size_t key); - SymbolicConditional(size_t key, size_t parent); - SymbolicConditional(size_t key, size_t parent1, size_t parent2); - SymbolicConditional(size_t key, size_t parent1, size_t parent2, size_t parent3); - static gtsam::SymbolicConditional FromKeys(const gtsam::KeyVector &keys, size_t nrFrontals); +virtual class SymbolicConditional : gtsam::SymbolicFactor { + // Standard Constructors and Named Constructors + SymbolicConditional(); + SymbolicConditional(const gtsam::SymbolicConditional& other); + SymbolicConditional(size_t key); + SymbolicConditional(size_t key, size_t parent); + SymbolicConditional(size_t key, size_t parent1, size_t parent2); + SymbolicConditional(size_t key, size_t parent1, size_t parent2, size_t parent3); + static gtsam::SymbolicConditional FromKeys(const gtsam::KeyVector& keys, size_t nrFrontals); - // Testable - void print(string s) const; - bool equals(const gtsam::SymbolicConditional &other, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::SymbolicConditional& other, double tol) const; - // Standard interface - size_t nrFrontals() const; - size_t nrParents() const; - }; + // Standard interface + size_t nrFrontals() const; + size_t nrParents() const; +}; #include - class SymbolicBayesNet - { - SymbolicBayesNet(); - SymbolicBayesNet(const gtsam::SymbolicBayesNet &other); - // Testable - void print(string s) const; - bool equals(const gtsam::SymbolicBayesNet &other, double tol) const; +class SymbolicBayesNet { + SymbolicBayesNet(); + SymbolicBayesNet(const gtsam::SymbolicBayesNet& other); + // Testable + void print(string s) const; + bool equals(const gtsam::SymbolicBayesNet& other, double tol) const; - // Standard interface - size_t size() const; - void saveGraph(string s) const; - gtsam::SymbolicConditional *at(size_t idx) const; - gtsam::SymbolicConditional *front() const; - gtsam::SymbolicConditional *back() const; - void push_back(gtsam::SymbolicConditional *conditional); - void push_back(const gtsam::SymbolicBayesNet &bayesNet); - }; + // Standard interface + size_t size() const; + void saveGraph(string s) const; + gtsam::SymbolicConditional* at(size_t idx) const; + gtsam::SymbolicConditional* front() const; + gtsam::SymbolicConditional* back() const; + void push_back(gtsam::SymbolicConditional* conditional); + void push_back(const gtsam::SymbolicBayesNet& bayesNet); +}; #include - class SymbolicBayesTree - { +class SymbolicBayesTree { //Constructors SymbolicBayesTree(); - SymbolicBayesTree(const gtsam::SymbolicBayesTree &other); + SymbolicBayesTree(const gtsam::SymbolicBayesTree& other); // Testable void print(string s); - bool equals(const gtsam::SymbolicBayesTree &other, double tol) const; + bool equals(const gtsam::SymbolicBayesTree& other, double tol) const; //Standard Interface //size_t findParentClique(const gtsam::IndexVector& parents) const; @@ -1291,1019 +1251,969 @@ class Cal3_S2 { void deleteCachedShortcuts(); size_t numCachedSeparatorMarginals() const; - gtsam::SymbolicConditional *marginalFactor(size_t key) const; - gtsam::SymbolicFactorGraph *joint(size_t key1, size_t key2) const; - gtsam::SymbolicBayesNet *jointBayesNet(size_t key1, size_t key2) const; - }; + gtsam::SymbolicConditional* marginalFactor(size_t key) const; + gtsam::SymbolicFactorGraph* joint(size_t key1, size_t key2) const; + gtsam::SymbolicBayesNet* jointBayesNet(size_t key1, size_t key2) const; +}; - // class SymbolicBayesTreeClique { - // BayesTreeClique(); - // BayesTreeClique(CONDITIONAL* conditional); - // // BayesTreeClique(const pair& result) : Base(result) {} - // - // bool equals(const This& other, double tol) const; - // void print(string s) const; - // void printTree() const; // Default indent of "" - // void printTree(string indent) const; - // size_t numCachedSeparatorMarginals() const; - // - // CONDITIONAL* conditional() const; - // bool isRoot() const; - // size_t treeSize() const; - // // const std::list& children() const { return children_; } - // // derived_ptr parent() const { return parent_.lock(); } - // - // // TODO: need wrapped versions graphs, BayesNet - // // BayesNet shortcut(derived_ptr root, Eliminate function) const; - // // FactorGraph marginal(derived_ptr root, Eliminate function) const; - // // FactorGraph joint(derived_ptr C2, derived_ptr root, Eliminate function) const; - // - // void deleteCachedShortcuts(); - // }; +// class SymbolicBayesTreeClique { +// BayesTreeClique(); +// BayesTreeClique(CONDITIONAL* conditional); +// // BayesTreeClique(const pair& result) : Base(result) {} +// +// bool equals(const This& other, double tol) const; +// void print(string s) const; +// void printTree() const; // Default indent of "" +// void printTree(string indent) const; +// size_t numCachedSeparatorMarginals() const; +// +// CONDITIONAL* conditional() const; +// bool isRoot() const; +// size_t treeSize() const; +// // const std::list& children() const { return children_; } +// // derived_ptr parent() const { return parent_.lock(); } +// +// // TODO: need wrapped versions graphs, BayesNet +// // BayesNet shortcut(derived_ptr root, Eliminate function) const; +// // FactorGraph marginal(derived_ptr root, Eliminate function) const; +// // FactorGraph joint(derived_ptr C2, derived_ptr root, Eliminate function) const; +// +// void deleteCachedShortcuts(); +// }; #include - class VariableIndex - { - // Standard Constructors and Named Constructors - VariableIndex(); - // TODO: Templetize constructor when wrap supports it - //template - //VariableIndex(const T& factorGraph, size_t nVariables); - //VariableIndex(const T& factorGraph); - VariableIndex(const gtsam::SymbolicFactorGraph &sfg); - VariableIndex(const gtsam::GaussianFactorGraph &gfg); - VariableIndex(const gtsam::NonlinearFactorGraph &fg); - VariableIndex(const gtsam::VariableIndex &other); +class VariableIndex { + // Standard Constructors and Named Constructors + VariableIndex(); + // TODO: Templetize constructor when wrap supports it + //template + //VariableIndex(const T& factorGraph, size_t nVariables); + //VariableIndex(const T& factorGraph); + VariableIndex(const gtsam::SymbolicFactorGraph& sfg); + VariableIndex(const gtsam::GaussianFactorGraph& gfg); + VariableIndex(const gtsam::NonlinearFactorGraph& fg); + VariableIndex(const gtsam::VariableIndex& other); - // Testable - bool equals(const gtsam::VariableIndex &other, double tol) const; - void print(string s) const; + // Testable + bool equals(const gtsam::VariableIndex& other, double tol) const; + void print(string s) const; - // Standard interface - size_t size() const; - size_t nFactors() const; - size_t nEntries() const; - }; + // Standard interface + size_t size() const; + size_t nFactors() const; + size_t nEntries() const; +}; - //************************************************************************* - // linear - //************************************************************************* +//************************************************************************* +// linear +//************************************************************************* - namespace noiseModel - { +namespace noiseModel { #include - virtual class Base - { - void print(string s) const; - // Methods below are available for all noise models. However, can't add them - // because wrap (incorrectly) thinks robust classes derive from this Base as well. - // bool isConstrained() const; - // bool isUnit() const; - // size_t dim() const; - // Vector sigmas() const; - }; - - virtual class Gaussian : gtsam::noiseModel::Base - { - static gtsam::noiseModel::Gaussian *Information(Matrix R); - static gtsam::noiseModel::Gaussian *SqrtInformation(Matrix R); - static gtsam::noiseModel::Gaussian *Covariance(Matrix R); - - bool equals(gtsam::noiseModel::Base &expected, double tol); - - // access to noise model - Matrix R() const; - Matrix information() const; - Matrix covariance() const; - - // Whitening operations - Vector whiten(Vector v) const; - Vector unwhiten(Vector v) const; - Matrix Whiten(Matrix H) const; - - // enabling serialization functionality - void serializable() const; - }; - - virtual class Diagonal : gtsam::noiseModel::Gaussian - { - static gtsam::noiseModel::Diagonal *Sigmas(Vector sigmas); - static gtsam::noiseModel::Diagonal *Variances(Vector variances); - static gtsam::noiseModel::Diagonal *Precisions(Vector precisions); - Matrix R() const; - - // access to noise model - Vector sigmas() const; - Vector invsigmas() const; - Vector precisions() const; - - // enabling serialization functionality - void serializable() const; - }; - - virtual class Constrained : gtsam::noiseModel::Diagonal - { - static gtsam::noiseModel::Constrained *MixedSigmas(Vector mu, Vector sigmas); - static gtsam::noiseModel::Constrained *MixedSigmas(double m, Vector sigmas); - static gtsam::noiseModel::Constrained *MixedVariances(Vector mu, Vector variances); - static gtsam::noiseModel::Constrained *MixedVariances(Vector variances); - static gtsam::noiseModel::Constrained *MixedPrecisions(Vector mu, Vector precisions); - static gtsam::noiseModel::Constrained *MixedPrecisions(Vector precisions); - - static gtsam::noiseModel::Constrained *All(size_t dim); - static gtsam::noiseModel::Constrained *All(size_t dim, double mu); - - gtsam::noiseModel::Constrained *unit() const; - - // enabling serialization functionality - void serializable() const; - }; - - virtual class Isotropic : gtsam::noiseModel::Diagonal - { - static gtsam::noiseModel::Isotropic *Sigma(size_t dim, double sigma); - static gtsam::noiseModel::Isotropic *Variance(size_t dim, double varianace); - static gtsam::noiseModel::Isotropic *Precision(size_t dim, double precision); - - // access to noise model - double sigma() const; - - // enabling serialization functionality - void serializable() const; - }; - - virtual class Unit : gtsam::noiseModel::Isotropic - { - static gtsam::noiseModel::Unit *Create(size_t dim); - - // enabling serialization functionality - void serializable() const; - }; - - namespace mEstimator - { - virtual class Base - { - void print(string s) const; - }; - - virtual class Null : gtsam::noiseModel::mEstimator::Base - { - Null(); - static gtsam::noiseModel::mEstimator::Null *Create(); - - // enabling serialization functionality - void serializable() const; - - double weight(double error) const; - double loss(double error) const; - }; - - virtual class Fair : gtsam::noiseModel::mEstimator::Base - { - Fair(double c); - static gtsam::noiseModel::mEstimator::Fair *Create(double c); - - // enabling serialization functionality - void serializable() const; - - double weight(double error) const; - double loss(double error) const; - }; - - virtual class Huber : gtsam::noiseModel::mEstimator::Base - { - Huber(double k); - static gtsam::noiseModel::mEstimator::Huber *Create(double k); - - // enabling serialization functionality - void serializable() const; - - double weight(double error) const; - double loss(double error) const; - }; - - virtual class Cauchy : gtsam::noiseModel::mEstimator::Base - { - Cauchy(double k); - static gtsam::noiseModel::mEstimator::Cauchy *Create(double k); - - // enabling serialization functionality - void serializable() const; - - double weight(double error) const; - double loss(double error) const; - }; - - virtual class Tukey : gtsam::noiseModel::mEstimator::Base - { - Tukey(double k); - static gtsam::noiseModel::mEstimator::Tukey *Create(double k); - - // enabling serialization functionality - void serializable() const; - - double weight(double error) const; - double loss(double error) const; - }; - - virtual class Welsch : gtsam::noiseModel::mEstimator::Base - { - Welsch(double k); - static gtsam::noiseModel::mEstimator::Welsch *Create(double k); - - // enabling serialization functionality - void serializable() const; - - double weight(double error) const; - double loss(double error) const; - }; - - virtual class GemanMcClure : gtsam::noiseModel::mEstimator::Base - { - GemanMcClure(double c); - static gtsam::noiseModel::mEstimator::GemanMcClure *Create(double c); - - // enabling serialization functionality - void serializable() const; - - double weight(double error) const; - double loss(double error) const; - }; - - virtual class DCS : gtsam::noiseModel::mEstimator::Base - { - DCS(double c); - static gtsam::noiseModel::mEstimator::DCS *Create(double c); - - // enabling serialization functionality - void serializable() const; - - double weight(double error) const; - double loss(double error) const; - }; - - virtual class L2WithDeadZone : gtsam::noiseModel::mEstimator::Base - { - L2WithDeadZone(double k); - static gtsam::noiseModel::mEstimator::L2WithDeadZone *Create(double k); - - // enabling serialization functionality - void serializable() const; - - double weight(double error) const; - double loss(double error) const; - }; - - } // namespace mEstimator - - virtual class Robust : gtsam::noiseModel::Base - { - Robust(const gtsam::noiseModel::mEstimator::Base *robust, const gtsam::noiseModel::Base *noise); - static gtsam::noiseModel::Robust *Create(const gtsam::noiseModel::mEstimator::Base *robust, const gtsam::noiseModel::Base *noise); - - // enabling serialization functionality - void serializable() const; - }; - - } // namespace noiseModel - -#include - class Sampler - { - // Constructors - Sampler(gtsam::noiseModel::Diagonal *model, int seed); - Sampler(Vector sigmas, int seed); - - // Standard Interface - size_t dim() const; - Vector sigmas() const; - gtsam::noiseModel::Diagonal *model() const; - Vector sample(); - }; - -#include - class VectorValues - { - //Constructors - VectorValues(); - VectorValues(const gtsam::VectorValues &other); - - //Named Constructors - static gtsam::VectorValues Zero(const gtsam::VectorValues &model); - - //Standard Interface - size_t size() const; - size_t dim(size_t j) const; - bool exists(size_t j) const; - void print(string s) const; - bool equals(const gtsam::VectorValues &expected, double tol) const; - void insert(size_t j, Vector value); - Vector vector() const; - Vector at(size_t j) const; - void update(const gtsam::VectorValues &values); - - //Advanced Interface - void setZero(); - - gtsam::VectorValues add(const gtsam::VectorValues &c) const; - void addInPlace(const gtsam::VectorValues &c); - gtsam::VectorValues subtract(const gtsam::VectorValues &c) const; - gtsam::VectorValues scale(double a) const; - void scaleInPlace(double a); - - bool hasSameStructure(const gtsam::VectorValues &other) const; - double dot(const gtsam::VectorValues &V) const; - double norm() const; - double squaredNorm() const; - - // enabling serialization functionality - void serialize() const; - }; - -#include - virtual class GaussianFactor - { - gtsam::KeyVector keys() const; - void print(string s) const; - bool equals(const gtsam::GaussianFactor &lf, double tol) const; - double error(const gtsam::VectorValues &c) const; - gtsam::GaussianFactor *clone() const; - gtsam::GaussianFactor *negate() const; - Matrix augmentedInformation() const; - Matrix information() const; - Matrix augmentedJacobian() const; - pair jacobian() const; - size_t size() const; - bool empty() const; - }; - -#include - virtual class JacobianFactor : gtsam::GaussianFactor - { - //Constructors - JacobianFactor(); - JacobianFactor(const gtsam::GaussianFactor &factor); - JacobianFactor(Vector b_in); - JacobianFactor(size_t i1, Matrix A1, Vector b, - const gtsam::noiseModel::Diagonal *model); - JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, Vector b, - const gtsam::noiseModel::Diagonal *model); - JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, size_t i3, Matrix A3, - Vector b, const gtsam::noiseModel::Diagonal *model); - JacobianFactor(const gtsam::GaussianFactorGraph &graph); - - //Testable - void print(string s) const; - void printKeys(string s) const; - bool equals(const gtsam::GaussianFactor &lf, double tol) const; - size_t size() const; - Vector unweighted_error(const gtsam::VectorValues &c) const; - Vector error_vector(const gtsam::VectorValues &c) const; - double error(const gtsam::VectorValues &c) const; - - //Standard Interface - Matrix getA() const; - Vector getb() const; - size_t rows() const; - size_t cols() const; - bool isConstrained() const; - pair jacobianUnweighted() const; - Matrix augmentedJacobianUnweighted() const; - - void transposeMultiplyAdd(double alpha, Vector e, gtsam::VectorValues &x) const; - gtsam::JacobianFactor whiten() const; - - pair eliminate(const gtsam::Ordering &keys) const; - - void setModel(bool anyConstrained, Vector sigmas); - - gtsam::noiseModel::Diagonal *get_model() const; - - // enabling serialization functionality - void serialize() const; - }; - -#include - virtual class HessianFactor : gtsam::GaussianFactor - { - //Constructors - HessianFactor(); - HessianFactor(const gtsam::GaussianFactor &factor); - HessianFactor(size_t j, Matrix G, Vector g, double f); - HessianFactor(size_t j, Vector mu, Matrix Sigma); - HessianFactor(size_t j1, size_t j2, Matrix G11, Matrix G12, Vector g1, Matrix G22, - Vector g2, double f); - HessianFactor(size_t j1, size_t j2, size_t j3, Matrix G11, Matrix G12, Matrix G13, - Vector g1, Matrix G22, Matrix G23, Vector g2, Matrix G33, Vector g3, - double f); - HessianFactor(const gtsam::GaussianFactorGraph &factors); - - //Testable - size_t size() const; - void print(string s) const; - void printKeys(string s) const; - bool equals(const gtsam::GaussianFactor &lf, double tol) const; - double error(const gtsam::VectorValues &c) const; - - //Standard Interface - size_t rows() const; - Matrix information() const; - double constantTerm() const; - Vector linearTerm() const; - - // enabling serialization functionality - void serialize() const; - }; - -#include - class GaussianFactorGraph - { - GaussianFactorGraph(); - GaussianFactorGraph(const gtsam::GaussianBayesNet &bayesNet); - GaussianFactorGraph(const gtsam::GaussianBayesTree &bayesTree); - - // From FactorGraph - void print(string s) const; - bool equals(const gtsam::GaussianFactorGraph &lfgraph, double tol) const; - size_t size() const; - gtsam::GaussianFactor *at(size_t idx) const; - gtsam::KeySet keys() const; - gtsam::KeyVector keyVector() const; - bool exists(size_t idx) const; - - // Building the graph - void push_back(const gtsam::GaussianFactor *factor); - void push_back(const gtsam::GaussianConditional *conditional); - void push_back(const gtsam::GaussianFactorGraph &graph); - void push_back(const gtsam::GaussianBayesNet &bayesNet); - void push_back(const gtsam::GaussianBayesTree &bayesTree); - void add(const gtsam::GaussianFactor &factor); - void add(Vector b); - void add(size_t key1, Matrix A1, Vector b, const gtsam::noiseModel::Diagonal *model); - void add(size_t key1, Matrix A1, size_t key2, Matrix A2, Vector b, - const gtsam::noiseModel::Diagonal *model); - void add(size_t key1, Matrix A1, size_t key2, Matrix A2, size_t key3, Matrix A3, - Vector b, const gtsam::noiseModel::Diagonal *model); - - // error and probability - double error(const gtsam::VectorValues &c) const; - double probPrime(const gtsam::VectorValues &c) const; - - gtsam::GaussianFactorGraph clone() const; - gtsam::GaussianFactorGraph negate() const; - - // Optimizing and linear algebra - gtsam::VectorValues optimize() const; - gtsam::VectorValues optimize(const gtsam::Ordering &ordering) const; - gtsam::VectorValues optimizeGradientSearch() const; - gtsam::VectorValues gradient(const gtsam::VectorValues &x0) const; - gtsam::VectorValues gradientAtZero() const; - - // Elimination and marginals - gtsam::GaussianBayesNet *eliminateSequential(); - gtsam::GaussianBayesNet *eliminateSequential(const gtsam::Ordering &ordering); - gtsam::GaussianBayesTree *eliminateMultifrontal(); - gtsam::GaussianBayesTree *eliminateMultifrontal(const gtsam::Ordering &ordering); - pair eliminatePartialSequential( - const gtsam::Ordering &ordering); - pair eliminatePartialSequential( - const gtsam::KeyVector &keys); - pair eliminatePartialMultifrontal( - const gtsam::Ordering &ordering); - pair eliminatePartialMultifrontal( - const gtsam::KeyVector &keys); - gtsam::GaussianBayesNet *marginalMultifrontalBayesNet(const gtsam::Ordering &ordering); - gtsam::GaussianBayesNet *marginalMultifrontalBayesNet(const gtsam::KeyVector &key_vector); - gtsam::GaussianBayesNet *marginalMultifrontalBayesNet(const gtsam::Ordering &ordering, - const gtsam::Ordering &marginalizedVariableOrdering); - gtsam::GaussianBayesNet *marginalMultifrontalBayesNet(const gtsam::KeyVector &key_vector, - const gtsam::Ordering &marginalizedVariableOrdering); - gtsam::GaussianFactorGraph *marginal(const gtsam::KeyVector &key_vector); - - // Conversion to matrices - Matrix sparseJacobian_() const; - Matrix augmentedJacobian() const; - Matrix augmentedJacobian(const gtsam::Ordering &ordering) const; - pair jacobian() const; - pair jacobian(const gtsam::Ordering &ordering) const; - Matrix augmentedHessian() const; - Matrix augmentedHessian(const gtsam::Ordering &ordering) const; - pair hessian() const; - pair hessian(const gtsam::Ordering &ordering) const; - - // enabling serialization functionality - void serialize() const; - }; - -#include - virtual class GaussianConditional : gtsam::GaussianFactor - { - //Constructors - GaussianConditional(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal *sigmas); - GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, - const gtsam::noiseModel::Diagonal *sigmas); - GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, - size_t name2, Matrix T, const gtsam::noiseModel::Diagonal *sigmas); - - //Constructors with no noise model - GaussianConditional(size_t key, Vector d, Matrix R); - GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S); - GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, - size_t name2, Matrix T); - - //Standard Interface - void print(string s) const; - bool equals(const gtsam::GaussianConditional &cg, double tol) const; - - //Advanced Interface - gtsam::VectorValues solve(const gtsam::VectorValues &parents) const; - gtsam::VectorValues solveOtherRHS(const gtsam::VectorValues &parents, const gtsam::VectorValues &rhs) const; - void solveTransposeInPlace(gtsam::VectorValues &gy) const; - void scaleFrontalsBySigma(gtsam::VectorValues &gy) const; - Matrix R() const; - Matrix S() const; - Vector d() const; - - // enabling serialization functionality - void serialize() const; - }; - -#include - virtual class GaussianDensity : gtsam::GaussianConditional - { - //Constructors - GaussianDensity(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal *sigmas); - - //Standard Interface - void print(string s) const; - bool equals(const gtsam::GaussianDensity &cg, double tol) const; - Vector mean() const; - Matrix covariance() const; - }; - -#include - virtual class GaussianBayesNet - { - //Constructors - GaussianBayesNet(); - GaussianBayesNet(const gtsam::GaussianConditional *conditional); - - // Testable - void print(string s) const; - bool equals(const gtsam::GaussianBayesNet &other, double tol) const; - size_t size() const; - - // FactorGraph derived interface - // size_t size() const; - gtsam::GaussianConditional *at(size_t idx) const; - gtsam::KeySet keys() const; - bool exists(size_t idx) const; - - gtsam::GaussianConditional *front() const; - gtsam::GaussianConditional *back() const; - void push_back(gtsam::GaussianConditional *conditional); - void push_back(const gtsam::GaussianBayesNet &bayesNet); - - gtsam::VectorValues optimize() const; - gtsam::VectorValues optimize(gtsam::VectorValues &solutionForMissing) const; - gtsam::VectorValues optimizeGradientSearch() const; - gtsam::VectorValues gradient(const gtsam::VectorValues &x0) const; - gtsam::VectorValues gradientAtZero() const; - double error(const gtsam::VectorValues &x) const; - double determinant() const; - double logDeterminant() const; - gtsam::VectorValues backSubstitute(const gtsam::VectorValues &gx) const; - gtsam::VectorValues backSubstituteTranspose(const gtsam::VectorValues &gx) const; - }; - -#include - virtual class GaussianBayesTree - { - // Standard Constructors and Named Constructors - GaussianBayesTree(); - GaussianBayesTree(const gtsam::GaussianBayesTree &other); - bool equals(const gtsam::GaussianBayesTree &other, double tol) const; - void print(string s); - size_t size() const; - bool empty() const; - size_t numCachedSeparatorMarginals() const; - void saveGraph(string s) const; - - gtsam::VectorValues optimize() const; - gtsam::VectorValues optimizeGradientSearch() const; - gtsam::VectorValues gradient(const gtsam::VectorValues &x0) const; - gtsam::VectorValues gradientAtZero() const; - double error(const gtsam::VectorValues &x) const; - double determinant() const; - double logDeterminant() const; - Matrix marginalCovariance(size_t key) const; - gtsam::GaussianConditional *marginalFactor(size_t key) const; - gtsam::GaussianFactorGraph *joint(size_t key1, size_t key2) const; - gtsam::GaussianBayesNet *jointBayesNet(size_t key1, size_t key2) const; - }; - -#include - class Errors - { - //Constructors - Errors(); - Errors(const gtsam::VectorValues &V); - - //Testable - void print(string s); - bool equals(const gtsam::Errors &expected, double tol) const; - }; - -#include - class GaussianISAM - { - //Constructor - GaussianISAM(); - - //Standard Interface - void update(const gtsam::GaussianFactorGraph &newFactors); - void saveGraph(string s) const; - void clear(); - }; - -#include - virtual class IterativeOptimizationParameters - { - string getVerbosity() const; - void setVerbosity(string s); - void print() const; - }; - - //virtual class IterativeSolver { - // IterativeSolver(); - // gtsam::VectorValues optimize (); - //}; - -#include - virtual class ConjugateGradientParameters : gtsam::IterativeOptimizationParameters - { - ConjugateGradientParameters(); - int getMinIterations() const; - int getMaxIterations() const; - int getReset() const; - double getEpsilon_rel() const; - double getEpsilon_abs() const; - - void setMinIterations(int value); - void setMaxIterations(int value); - void setReset(int value); - void setEpsilon_rel(double value); - void setEpsilon_abs(double value); - void print() const; - }; - -#include - virtual class PreconditionerParameters - { - PreconditionerParameters(); - }; - - virtual class DummyPreconditionerParameters : gtsam::PreconditionerParameters - { - DummyPreconditionerParameters(); - }; - -#include - virtual class PCGSolverParameters : gtsam::ConjugateGradientParameters - { - PCGSolverParameters(); - void print(string s); - void setPreconditionerParams(gtsam::PreconditionerParameters *preconditioner); - }; - -#include - virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters - { - SubgraphSolverParameters(); - void print() const; - }; - - virtual class SubgraphSolver - { - SubgraphSolver(const gtsam::GaussianFactorGraph &A, const gtsam::SubgraphSolverParameters ¶meters, const gtsam::Ordering &ordering); - SubgraphSolver(const gtsam::GaussianFactorGraph &Ab1, const gtsam::GaussianFactorGraph *Ab2, const gtsam::SubgraphSolverParameters ¶meters, const gtsam::Ordering &ordering); - gtsam::VectorValues optimize() const; - }; - -#include - class KalmanFilter - { - KalmanFilter(size_t n); - // gtsam::GaussianDensity* init(Vector x0, const gtsam::SharedDiagonal& P0); - gtsam::GaussianDensity *init(Vector x0, Matrix P0); - void print(string s) const; - static size_t step(gtsam::GaussianDensity *p); - gtsam::GaussianDensity *predict(gtsam::GaussianDensity *p, Matrix F, - Matrix B, Vector u, const gtsam::noiseModel::Diagonal *modelQ); - gtsam::GaussianDensity *predictQ(gtsam::GaussianDensity *p, Matrix F, - Matrix B, Vector u, Matrix Q); - gtsam::GaussianDensity *predict2(gtsam::GaussianDensity *p, Matrix A0, - Matrix A1, Vector b, const gtsam::noiseModel::Diagonal *model); - gtsam::GaussianDensity *update(gtsam::GaussianDensity *p, Matrix H, - Vector z, const gtsam::noiseModel::Diagonal *model); - gtsam::GaussianDensity *updateQ(gtsam::GaussianDensity *p, Matrix H, - Vector z, Matrix Q); - }; - - //************************************************************************* - // nonlinear - //************************************************************************* - -#include - size_t symbol(char chr, size_t index); - char symbolChr(size_t key); - size_t symbolIndex(size_t key); - - namespace symbol_shorthand - { - size_t A(size_t j); - size_t B(size_t j); - size_t C(size_t j); - size_t D(size_t j); - size_t E(size_t j); - size_t F(size_t j); - size_t G(size_t j); - size_t H(size_t j); - size_t I(size_t j); - size_t J(size_t j); - size_t K(size_t j); - size_t L(size_t j); - size_t M(size_t j); - size_t N(size_t j); - size_t O(size_t j); - size_t P(size_t j); - size_t Q(size_t j); - size_t R(size_t j); - size_t S(size_t j); - size_t T(size_t j); - size_t U(size_t j); - size_t V(size_t j); - size_t W(size_t j); - size_t X(size_t j); - size_t Y(size_t j); - size_t Z(size_t j); - } // namespace symbol_shorthand - - // Default keyformatter - void PrintKeyList(const gtsam::KeyList &keys); - void PrintKeyList(const gtsam::KeyList &keys, string s); - void PrintKeyVector(const gtsam::KeyVector &keys); - void PrintKeyVector(const gtsam::KeyVector &keys, string s); - void PrintKeySet(const gtsam::KeySet &keys); - void PrintKeySet(const gtsam::KeySet &keys, string s); - -#include - class LabeledSymbol - { - LabeledSymbol(size_t full_key); - LabeledSymbol(const gtsam::LabeledSymbol &key); - LabeledSymbol(unsigned char valType, unsigned char label, size_t j); - - size_t key() const; - unsigned char label() const; - unsigned char chr() const; - size_t index() const; - - gtsam::LabeledSymbol upper() const; - gtsam::LabeledSymbol lower() const; - gtsam::LabeledSymbol newChr(unsigned char c) const; - gtsam::LabeledSymbol newLabel(unsigned char label) const; - - void print(string s) const; - }; - - size_t mrsymbol(unsigned char c, unsigned char label, size_t j); - unsigned char mrsymbolChr(size_t key); - unsigned char mrsymbolLabel(size_t key); - size_t mrsymbolIndex(size_t key); - -#include - class Ordering - { - // Standard Constructors and Named Constructors - Ordering(); - Ordering(const gtsam::Ordering &other); - - // Testable - void print(string s) const; - bool equals(const gtsam::Ordering &ord, double tol) const; - - // Standard interface - size_t size() const; - size_t at(size_t key) const; - void push_back(size_t key); - - // enabling serialization functionality - void serialize() const; - }; - -#include - class NonlinearFactorGraph - { - NonlinearFactorGraph(); - NonlinearFactorGraph(const gtsam::NonlinearFactorGraph &graph); - - // FactorGraph - void print(string s) const; - bool equals(const gtsam::NonlinearFactorGraph &fg, double tol) const; - size_t size() const; - bool empty() const; - void remove(size_t i); - void replace(size_t i, gtsam::NonlinearFactor *factors); - void resize(size_t size); - size_t nrFactors() const; - gtsam::NonlinearFactor *at(size_t idx) const; - void push_back(const gtsam::NonlinearFactorGraph &factors); - void push_back(gtsam::NonlinearFactor *factor); - void add(gtsam::NonlinearFactor *factor); - bool exists(size_t idx) const; - gtsam::KeySet keys() const; - gtsam::KeyVector keyVector() const; - - template , gtsam::imuBias::ConstantBias}> - void addPrior(size_t key, const T &prior, const gtsam::noiseModel::Base *noiseModel); - - // NonlinearFactorGraph - void printErrors(const gtsam::Values &values) const; - double error(const gtsam::Values &values) const; - double probPrime(const gtsam::Values &values) const; - gtsam::Ordering orderingCOLAMD() const; - // Ordering* orderingCOLAMDConstrained(const gtsam::Values& c, const std::map& constraints) const; - gtsam::GaussianFactorGraph *linearize(const gtsam::Values &values) const; - gtsam::NonlinearFactorGraph clone() const; - - // enabling serialization functionality - void serialize() const; - }; - -#include - virtual class NonlinearFactor - { - // Factor base class - size_t size() const; - gtsam::KeyVector keys() const; - void print(string s) const; - void printKeys(string s) const; - // NonlinearFactor - bool equals(const gtsam::NonlinearFactor &other, double tol) const; - double error(const gtsam::Values &c) const; - size_t dim() const; - bool active(const gtsam::Values &c) const; - gtsam::GaussianFactor *linearize(const gtsam::Values &c) const; - gtsam::NonlinearFactor *clone() const; - // gtsam::NonlinearFactor* rekey(const gtsam::KeyVector& newKeys) const; //TODO: Conversion from KeyVector to std::vector does not happen - }; - -#include - virtual class NoiseModelFactor : gtsam::NonlinearFactor - { - bool equals(const gtsam::NoiseModelFactor &other, double tol) const; - gtsam::noiseModel::Base *noiseModel() const; - Vector unwhitenedError(const gtsam::Values &x) const; - Vector whitenedError(const gtsam::Values &x) const; - }; - -#include - class Values - { - Values(); - Values(const gtsam::Values &other); - - size_t size() const; - bool empty() const; - void clear(); - size_t dim() const; - - void print(string s) const; - bool equals(const gtsam::Values &other, double tol) const; - - void insert(const gtsam::Values &values); - void update(const gtsam::Values &values); - void erase(size_t j); - void swap(gtsam::Values &values); - - bool exists(size_t j) const; - gtsam::KeyVector keys() const; - - gtsam::VectorValues zeroVectors() const; - - gtsam::Values retract(const gtsam::VectorValues &delta) const; - gtsam::VectorValues localCoordinates(const gtsam::Values &cp) const; - - // enabling serialization functionality - void serialize() const; - - // New in 4.0, we have to specialize every insert/update/at to generate wrappers - // Instead of the old: - // void insert(size_t j, const gtsam::Value& value); - // void update(size_t j, const gtsam::Value& val); - // gtsam::Value at(size_t j) const; - - // The order is important: Vector has to precede Point2/Point3 so `atVector` - // can work for those fixed-size vectors. - void insert(size_t j, Vector vector); - void insert(size_t j, Matrix matrix); - void insert(size_t j, const gtsam::Point2 &point2); - void insert(size_t j, const gtsam::Point3 &point3); - void insert(size_t j, const gtsam::Rot2 &rot2); - void insert(size_t j, const gtsam::Pose2 &pose2); - void insert(size_t j, const gtsam::SO3 &R); - void insert(size_t j, const gtsam::SO4 &Q); - void insert(size_t j, const gtsam::SOn &P); - void insert(size_t j, const gtsam::Rot3 &rot3); - void insert(size_t j, const gtsam::Pose3 &pose3); - void insert(size_t j, const gtsam::Unit3 &unit3); - void insert(size_t j, const gtsam::Cal3_S2 &cal3_s2); - void insert(size_t j, const gtsam::Cal3DS2 &cal3ds2); - void insert(size_t j, const gtsam::Cal3Bundler &cal3bundler); - void insert(size_t j, const gtsam::EssentialMatrix &essential_matrix); - void insert(size_t j, const gtsam::PinholeCameraCal3_S2 &simple_camera); - void insert(size_t j, const gtsam::PinholeCamera &camera); - void insert(size_t j, const gtsam::imuBias::ConstantBias &constant_bias); - void insert(size_t j, const gtsam::NavState &nav_state); - - void update(size_t j, const gtsam::Point2 &point2); - void update(size_t j, const gtsam::Point3 &point3); - void update(size_t j, const gtsam::Rot2 &rot2); - void update(size_t j, const gtsam::Pose2 &pose2); - void update(size_t j, const gtsam::SO3 &R); - void update(size_t j, const gtsam::SO4 &Q); - void update(size_t j, const gtsam::SOn &P); - void update(size_t j, const gtsam::Rot3 &rot3); - void update(size_t j, const gtsam::Pose3 &pose3); - void update(size_t j, const gtsam::Unit3 &unit3); - void update(size_t j, const gtsam::Cal3_S2 &cal3_s2); - void update(size_t j, const gtsam::Cal3DS2 &cal3ds2); - void update(size_t j, const gtsam::Cal3Bundler &cal3bundler); - void update(size_t j, const gtsam::EssentialMatrix &essential_matrix); - void update(size_t j, const gtsam::PinholeCameraCal3_S2 &simple_camera); - void update(size_t j, const gtsam::PinholeCamera &camera); - void update(size_t j, const gtsam::imuBias::ConstantBias &constant_bias); - void update(size_t j, const gtsam::NavState &nav_state); - void update(size_t j, Vector vector); - void update(size_t j, Matrix matrix); - - template , gtsam::imuBias::ConstantBias, gtsam::NavState, Vector, Matrix}> - T at(size_t j); - - /// version for double - void insertDouble(size_t j, double c); - double atDouble(size_t j) const; - }; - -#include - class Marginals - { - Marginals(const gtsam::NonlinearFactorGraph &graph, - const gtsam::Values &solution); - Marginals(const gtsam::GaussianFactorGraph &gfgraph, - const gtsam::Values &solution); - Marginals(const gtsam::GaussianFactorGraph &gfgraph, - const gtsam::VectorValues &solutionvec); - - void print(string s) const; - Matrix marginalCovariance(size_t variable) const; - Matrix marginalInformation(size_t variable) const; - gtsam::JointMarginal jointMarginalCovariance(const gtsam::KeyVector &variables) const; - gtsam::JointMarginal jointMarginalInformation(const gtsam::KeyVector &variables) const; - }; - - class JointMarginal - { - Matrix at(size_t iVariable, size_t jVariable) const; - Matrix fullMatrix() const; - void print(string s) const; - void print() const; - }; - -#include - virtual class LinearContainerFactor : gtsam::NonlinearFactor - { - - LinearContainerFactor(gtsam::GaussianFactor *factor, const gtsam::Values &linearizationPoint); - LinearContainerFactor(gtsam::GaussianFactor *factor); - - gtsam::GaussianFactor *factor() const; - // const boost::optional& linearizationPoint() const; - - bool isJacobian() const; - gtsam::JacobianFactor *toJacobian() const; - gtsam::HessianFactor *toHessian() const; - - static gtsam::NonlinearFactorGraph ConvertLinearGraph(const gtsam::GaussianFactorGraph &linear_graph, - const gtsam::Values &linearizationPoint); - - static gtsam::NonlinearFactorGraph ConvertLinearGraph(const gtsam::GaussianFactorGraph &linear_graph); +virtual class Base { + void print(string s) const; + // Methods below are available for all noise models. However, can't add them + // because wrap (incorrectly) thinks robust classes derive from this Base as well. + // bool isConstrained() const; + // bool isUnit() const; + // size_t dim() const; + // Vector sigmas() const; +}; + +virtual class Gaussian : gtsam::noiseModel::Base { + static gtsam::noiseModel::Gaussian* Information(Matrix R); + static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R); + static gtsam::noiseModel::Gaussian* Covariance(Matrix R); + + bool equals(gtsam::noiseModel::Base& expected, double tol); + + // access to noise model + Matrix R() const; + Matrix information() const; + Matrix covariance() const; + + // Whitening operations + Vector whiten(Vector v) const; + Vector unwhiten(Vector v) const; + Matrix Whiten(Matrix H) const; + + // enabling serialization functionality + void serializable() const; +}; + +virtual class Diagonal : gtsam::noiseModel::Gaussian { + static gtsam::noiseModel::Diagonal* Sigmas(Vector sigmas); + static gtsam::noiseModel::Diagonal* Variances(Vector variances); + static gtsam::noiseModel::Diagonal* Precisions(Vector precisions); + Matrix R() const; + + // access to noise model + Vector sigmas() const; + Vector invsigmas() const; + Vector precisions() const; + + // enabling serialization functionality + void serializable() const; +}; + +virtual class Constrained : gtsam::noiseModel::Diagonal { + static gtsam::noiseModel::Constrained* MixedSigmas(Vector mu, Vector sigmas); + static gtsam::noiseModel::Constrained* MixedSigmas(double m, Vector sigmas); + static gtsam::noiseModel::Constrained* MixedVariances(Vector mu, Vector variances); + static gtsam::noiseModel::Constrained* MixedVariances(Vector variances); + static gtsam::noiseModel::Constrained* MixedPrecisions(Vector mu, Vector precisions); + static gtsam::noiseModel::Constrained* MixedPrecisions(Vector precisions); + + static gtsam::noiseModel::Constrained* All(size_t dim); + static gtsam::noiseModel::Constrained* All(size_t dim, double mu); + + gtsam::noiseModel::Constrained* unit() const; // enabling serialization functionality void serializable() const; - }; // \class LinearContainerFactor +}; + +virtual class Isotropic : gtsam::noiseModel::Diagonal { + static gtsam::noiseModel::Isotropic* Sigma(size_t dim, double sigma); + static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace); + static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision); + + // access to noise model + double sigma() const; + + // enabling serialization functionality + void serializable() const; +}; + +virtual class Unit : gtsam::noiseModel::Isotropic { + static gtsam::noiseModel::Unit* Create(size_t dim); + + // enabling serialization functionality + void serializable() const; +}; + +namespace mEstimator { +virtual class Base { + void print(string s) const; +}; + +virtual class Null: gtsam::noiseModel::mEstimator::Base { + Null(); + static gtsam::noiseModel::mEstimator::Null* Create(); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; + double loss(double error) const; +}; + +virtual class Fair: gtsam::noiseModel::mEstimator::Base { + Fair(double c); + static gtsam::noiseModel::mEstimator::Fair* Create(double c); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; + double loss(double error) const; +}; + +virtual class Huber: gtsam::noiseModel::mEstimator::Base { + Huber(double k); + static gtsam::noiseModel::mEstimator::Huber* Create(double k); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; + double loss(double error) const; +}; + +virtual class Cauchy: gtsam::noiseModel::mEstimator::Base { + Cauchy(double k); + static gtsam::noiseModel::mEstimator::Cauchy* Create(double k); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; + double loss(double error) const; +}; + +virtual class Tukey: gtsam::noiseModel::mEstimator::Base { + Tukey(double k); + static gtsam::noiseModel::mEstimator::Tukey* Create(double k); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; + double loss(double error) const; +}; + +virtual class Welsch: gtsam::noiseModel::mEstimator::Base { + Welsch(double k); + static gtsam::noiseModel::mEstimator::Welsch* Create(double k); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; + double loss(double error) const; +}; + +virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base { + GemanMcClure(double c); + static gtsam::noiseModel::mEstimator::GemanMcClure* Create(double c); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; + double loss(double error) const; +}; + +virtual class DCS: gtsam::noiseModel::mEstimator::Base { + DCS(double c); + static gtsam::noiseModel::mEstimator::DCS* Create(double c); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; + double loss(double error) const; +}; + +virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base { + L2WithDeadZone(double k); + static gtsam::noiseModel::mEstimator::L2WithDeadZone* Create(double k); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; + double loss(double error) const; +}; + +}///\namespace mEstimator + +virtual class Robust : gtsam::noiseModel::Base { + Robust(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise); + static gtsam::noiseModel::Robust* Create(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise); + + // enabling serialization functionality + void serializable() const; +}; + +}///\namespace noiseModel + +#include +class Sampler { + // Constructors + Sampler(gtsam::noiseModel::Diagonal* model, int seed); + Sampler(Vector sigmas, int seed); + + // Standard Interface + size_t dim() const; + Vector sigmas() const; + gtsam::noiseModel::Diagonal* model() const; + Vector sample(); +}; + +#include +class VectorValues { + //Constructors + VectorValues(); + VectorValues(const gtsam::VectorValues& other); + + //Named Constructors + static gtsam::VectorValues Zero(const gtsam::VectorValues& model); + + //Standard Interface + size_t size() const; + size_t dim(size_t j) const; + bool exists(size_t j) const; + void print(string s) const; + bool equals(const gtsam::VectorValues& expected, double tol) const; + void insert(size_t j, Vector value); + Vector vector() const; + Vector at(size_t j) const; + void update(const gtsam::VectorValues& values); + + //Advanced Interface + void setZero(); + + gtsam::VectorValues add(const gtsam::VectorValues& c) const; + void addInPlace(const gtsam::VectorValues& c); + gtsam::VectorValues subtract(const gtsam::VectorValues& c) const; + gtsam::VectorValues scale(double a) const; + void scaleInPlace(double a); + + bool hasSameStructure(const gtsam::VectorValues& other) const; + double dot(const gtsam::VectorValues& V) const; + double norm() const; + double squaredNorm() const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +virtual class GaussianFactor { + gtsam::KeyVector keys() const; + void print(string s) const; + bool equals(const gtsam::GaussianFactor& lf, double tol) const; + double error(const gtsam::VectorValues& c) const; + gtsam::GaussianFactor* clone() const; + gtsam::GaussianFactor* negate() const; + Matrix augmentedInformation() const; + Matrix information() const; + Matrix augmentedJacobian() const; + pair jacobian() const; + size_t size() const; + bool empty() const; +}; + +#include +virtual class JacobianFactor : gtsam::GaussianFactor { + //Constructors + JacobianFactor(); + JacobianFactor(const gtsam::GaussianFactor& factor); + JacobianFactor(Vector b_in); + JacobianFactor(size_t i1, Matrix A1, Vector b, + const gtsam::noiseModel::Diagonal* model); + JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, Vector b, + const gtsam::noiseModel::Diagonal* model); + JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, size_t i3, Matrix A3, + Vector b, const gtsam::noiseModel::Diagonal* model); + JacobianFactor(const gtsam::GaussianFactorGraph& graph); + + //Testable + void print(string s) const; + void printKeys(string s) const; + bool equals(const gtsam::GaussianFactor& lf, double tol) const; + size_t size() const; + Vector unweighted_error(const gtsam::VectorValues& c) const; + Vector error_vector(const gtsam::VectorValues& c) const; + double error(const gtsam::VectorValues& c) const; + + //Standard Interface + Matrix getA() const; + Vector getb() const; + size_t rows() const; + size_t cols() const; + bool isConstrained() const; + pair jacobianUnweighted() const; + Matrix augmentedJacobianUnweighted() const; + + void transposeMultiplyAdd(double alpha, Vector e, gtsam::VectorValues& x) const; + gtsam::JacobianFactor whiten() const; + + pair eliminate(const gtsam::Ordering& keys) const; + + void setModel(bool anyConstrained, Vector sigmas); + + gtsam::noiseModel::Diagonal* get_model() const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +virtual class HessianFactor : gtsam::GaussianFactor { + //Constructors + HessianFactor(); + HessianFactor(const gtsam::GaussianFactor& factor); + HessianFactor(size_t j, Matrix G, Vector g, double f); + HessianFactor(size_t j, Vector mu, Matrix Sigma); + HessianFactor(size_t j1, size_t j2, Matrix G11, Matrix G12, Vector g1, Matrix G22, + Vector g2, double f); + HessianFactor(size_t j1, size_t j2, size_t j3, Matrix G11, Matrix G12, Matrix G13, + Vector g1, Matrix G22, Matrix G23, Vector g2, Matrix G33, Vector g3, + double f); + HessianFactor(const gtsam::GaussianFactorGraph& factors); + + //Testable + size_t size() const; + void print(string s) const; + void printKeys(string s) const; + bool equals(const gtsam::GaussianFactor& lf, double tol) const; + double error(const gtsam::VectorValues& c) const; + + //Standard Interface + size_t rows() const; + Matrix information() const; + double constantTerm() const; + Vector linearTerm() const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +class GaussianFactorGraph { + GaussianFactorGraph(); + GaussianFactorGraph(const gtsam::GaussianBayesNet& bayesNet); + GaussianFactorGraph(const gtsam::GaussianBayesTree& bayesTree); + + // From FactorGraph + void print(string s) const; + bool equals(const gtsam::GaussianFactorGraph& lfgraph, double tol) const; + size_t size() const; + gtsam::GaussianFactor* at(size_t idx) const; + gtsam::KeySet keys() const; + gtsam::KeyVector keyVector() const; + bool exists(size_t idx) const; + + // Building the graph + void push_back(const gtsam::GaussianFactor* factor); + void push_back(const gtsam::GaussianConditional* conditional); + void push_back(const gtsam::GaussianFactorGraph& graph); + void push_back(const gtsam::GaussianBayesNet& bayesNet); + void push_back(const gtsam::GaussianBayesTree& bayesTree); + void add(const gtsam::GaussianFactor& factor); + void add(Vector b); + void add(size_t key1, Matrix A1, Vector b, const gtsam::noiseModel::Diagonal* model); + void add(size_t key1, Matrix A1, size_t key2, Matrix A2, Vector b, + const gtsam::noiseModel::Diagonal* model); + void add(size_t key1, Matrix A1, size_t key2, Matrix A2, size_t key3, Matrix A3, + Vector b, const gtsam::noiseModel::Diagonal* model); + + // error and probability + double error(const gtsam::VectorValues& c) const; + double probPrime(const gtsam::VectorValues& c) const; + + gtsam::GaussianFactorGraph clone() const; + gtsam::GaussianFactorGraph negate() const; + + // Optimizing and linear algebra + gtsam::VectorValues optimize() const; + gtsam::VectorValues optimize(const gtsam::Ordering& ordering) const; + gtsam::VectorValues optimizeGradientSearch() const; + gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; + gtsam::VectorValues gradientAtZero() const; + + // Elimination and marginals + gtsam::GaussianBayesNet* eliminateSequential(); + gtsam::GaussianBayesNet* eliminateSequential(const gtsam::Ordering& ordering); + gtsam::GaussianBayesTree* eliminateMultifrontal(); + gtsam::GaussianBayesTree* eliminateMultifrontal(const gtsam::Ordering& ordering); + pair eliminatePartialSequential( + const gtsam::Ordering& ordering); + pair eliminatePartialSequential( + const gtsam::KeyVector& keys); + pair eliminatePartialMultifrontal( + const gtsam::Ordering& ordering); + pair eliminatePartialMultifrontal( + const gtsam::KeyVector& keys); + gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering); + gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector); + gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering, + const gtsam::Ordering& marginalizedVariableOrdering); + gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector, + const gtsam::Ordering& marginalizedVariableOrdering); + gtsam::GaussianFactorGraph* marginal(const gtsam::KeyVector& key_vector); + + // Conversion to matrices + Matrix sparseJacobian_() const; + Matrix augmentedJacobian() const; + Matrix augmentedJacobian(const gtsam::Ordering& ordering) const; + pair jacobian() const; + pair jacobian(const gtsam::Ordering& ordering) const; + Matrix augmentedHessian() const; + Matrix augmentedHessian(const gtsam::Ordering& ordering) const; + pair hessian() const; + pair hessian(const gtsam::Ordering& ordering) const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +virtual class GaussianConditional : gtsam::GaussianFactor { + //Constructors + GaussianConditional(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas); + GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, + const gtsam::noiseModel::Diagonal* sigmas); + GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, + size_t name2, Matrix T, const gtsam::noiseModel::Diagonal* sigmas); + + //Constructors with no noise model + GaussianConditional(size_t key, Vector d, Matrix R); + GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S); + GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, + size_t name2, Matrix T); + + //Standard Interface + void print(string s) const; + bool equals(const gtsam::GaussianConditional &cg, double tol) const; + + //Advanced Interface + gtsam::VectorValues solve(const gtsam::VectorValues& parents) const; + gtsam::VectorValues solveOtherRHS(const gtsam::VectorValues& parents, const gtsam::VectorValues& rhs) const; + void solveTransposeInPlace(gtsam::VectorValues& gy) const; + void scaleFrontalsBySigma(gtsam::VectorValues& gy) const; + Matrix R() const; + Matrix S() const; + Vector d() const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +virtual class GaussianDensity : gtsam::GaussianConditional { + //Constructors + GaussianDensity(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas); + + //Standard Interface + void print(string s) const; + bool equals(const gtsam::GaussianDensity &cg, double tol) const; + Vector mean() const; + Matrix covariance() const; +}; + +#include +virtual class GaussianBayesNet { + //Constructors + GaussianBayesNet(); + GaussianBayesNet(const gtsam::GaussianConditional* conditional); + + // Testable + void print(string s) const; + bool equals(const gtsam::GaussianBayesNet& other, double tol) const; + size_t size() const; + + // FactorGraph derived interface + // size_t size() const; + gtsam::GaussianConditional* at(size_t idx) const; + gtsam::KeySet keys() const; + bool exists(size_t idx) const; + + gtsam::GaussianConditional* front() const; + gtsam::GaussianConditional* back() const; + void push_back(gtsam::GaussianConditional* conditional); + void push_back(const gtsam::GaussianBayesNet& bayesNet); + + gtsam::VectorValues optimize() const; + gtsam::VectorValues optimize(gtsam::VectorValues& solutionForMissing) const; + gtsam::VectorValues optimizeGradientSearch() const; + gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; + gtsam::VectorValues gradientAtZero() const; + double error(const gtsam::VectorValues& x) const; + double determinant() const; + double logDeterminant() const; + gtsam::VectorValues backSubstitute(const gtsam::VectorValues& gx) const; + gtsam::VectorValues backSubstituteTranspose(const gtsam::VectorValues& gx) const; +}; + +#include +virtual class GaussianBayesTree { + // Standard Constructors and Named Constructors + GaussianBayesTree(); + GaussianBayesTree(const gtsam::GaussianBayesTree& other); + bool equals(const gtsam::GaussianBayesTree& other, double tol) const; + void print(string s); + size_t size() const; + bool empty() const; + size_t numCachedSeparatorMarginals() const; + void saveGraph(string s) const; + + gtsam::VectorValues optimize() const; + gtsam::VectorValues optimizeGradientSearch() const; + gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; + gtsam::VectorValues gradientAtZero() const; + double error(const gtsam::VectorValues& x) const; + double determinant() const; + double logDeterminant() const; + Matrix marginalCovariance(size_t key) const; + gtsam::GaussianConditional* marginalFactor(size_t key) const; + gtsam::GaussianFactorGraph* joint(size_t key1, size_t key2) const; + gtsam::GaussianBayesNet* jointBayesNet(size_t key1, size_t key2) const; +}; + +#include +class Errors { + //Constructors + Errors(); + Errors(const gtsam::VectorValues& V); + + //Testable + void print(string s); + bool equals(const gtsam::Errors& expected, double tol) const; +}; + +#include +class GaussianISAM { + //Constructor + GaussianISAM(); + + //Standard Interface + void update(const gtsam::GaussianFactorGraph& newFactors); + void saveGraph(string s) const; + void clear(); +}; + +#include +virtual class IterativeOptimizationParameters { + string getVerbosity() const; + void setVerbosity(string s) ; + void print() const; +}; + +//virtual class IterativeSolver { +// IterativeSolver(); +// gtsam::VectorValues optimize (); +//}; + +#include +virtual class ConjugateGradientParameters : gtsam::IterativeOptimizationParameters { + ConjugateGradientParameters(); + int getMinIterations() const ; + int getMaxIterations() const ; + int getReset() const; + double getEpsilon_rel() const; + double getEpsilon_abs() const; + + void setMinIterations(int value); + void setMaxIterations(int value); + void setReset(int value); + void setEpsilon_rel(double value); + void setEpsilon_abs(double value); + void print() const; +}; + +#include +virtual class PreconditionerParameters { + PreconditionerParameters(); +}; + +virtual class DummyPreconditionerParameters : gtsam::PreconditionerParameters { + DummyPreconditionerParameters(); +}; + +#include +virtual class PCGSolverParameters : gtsam::ConjugateGradientParameters { + PCGSolverParameters(); + void print(string s); + void setPreconditionerParams(gtsam::PreconditionerParameters* preconditioner); +}; + +#include +virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters { + SubgraphSolverParameters(); + void print() const; +}; + +virtual class SubgraphSolver { + SubgraphSolver(const gtsam::GaussianFactorGraph &A, const gtsam::SubgraphSolverParameters ¶meters, const gtsam::Ordering& ordering); + SubgraphSolver(const gtsam::GaussianFactorGraph &Ab1, const gtsam::GaussianFactorGraph* Ab2, const gtsam::SubgraphSolverParameters ¶meters, const gtsam::Ordering& ordering); + gtsam::VectorValues optimize() const; +}; + +#include +class KalmanFilter { + KalmanFilter(size_t n); + // gtsam::GaussianDensity* init(Vector x0, const gtsam::SharedDiagonal& P0); + gtsam::GaussianDensity* init(Vector x0, Matrix P0); + void print(string s) const; + static size_t step(gtsam::GaussianDensity* p); + gtsam::GaussianDensity* predict(gtsam::GaussianDensity* p, Matrix F, + Matrix B, Vector u, const gtsam::noiseModel::Diagonal* modelQ); + gtsam::GaussianDensity* predictQ(gtsam::GaussianDensity* p, Matrix F, + Matrix B, Vector u, Matrix Q); + gtsam::GaussianDensity* predict2(gtsam::GaussianDensity* p, Matrix A0, + Matrix A1, Vector b, const gtsam::noiseModel::Diagonal* model); + gtsam::GaussianDensity* update(gtsam::GaussianDensity* p, Matrix H, + Vector z, const gtsam::noiseModel::Diagonal* model); + gtsam::GaussianDensity* updateQ(gtsam::GaussianDensity* p, Matrix H, + Vector z, Matrix Q); +}; + +//************************************************************************* +// nonlinear +//************************************************************************* + +#include +size_t symbol(char chr, size_t index); +char symbolChr(size_t key); +size_t symbolIndex(size_t key); + +namespace symbol_shorthand { + size_t A(size_t j); + size_t B(size_t j); + size_t C(size_t j); + size_t D(size_t j); + size_t E(size_t j); + size_t F(size_t j); + size_t G(size_t j); + size_t H(size_t j); + size_t I(size_t j); + size_t J(size_t j); + size_t K(size_t j); + size_t L(size_t j); + size_t M(size_t j); + size_t N(size_t j); + size_t O(size_t j); + size_t P(size_t j); + size_t Q(size_t j); + size_t R(size_t j); + size_t S(size_t j); + size_t T(size_t j); + size_t U(size_t j); + size_t V(size_t j); + size_t W(size_t j); + size_t X(size_t j); + size_t Y(size_t j); + size_t Z(size_t j); +}///\namespace symbol + +// Default keyformatter +void PrintKeyList (const gtsam::KeyList& keys); +void PrintKeyList (const gtsam::KeyList& keys, string s); +void PrintKeyVector(const gtsam::KeyVector& keys); +void PrintKeyVector(const gtsam::KeyVector& keys, string s); +void PrintKeySet (const gtsam::KeySet& keys); +void PrintKeySet (const gtsam::KeySet& keys, string s); + +#include +class LabeledSymbol { + LabeledSymbol(size_t full_key); + LabeledSymbol(const gtsam::LabeledSymbol& key); + LabeledSymbol(unsigned char valType, unsigned char label, size_t j); + + size_t key() const; + unsigned char label() const; + unsigned char chr() const; + size_t index() const; + + gtsam::LabeledSymbol upper() const; + gtsam::LabeledSymbol lower() const; + gtsam::LabeledSymbol newChr(unsigned char c) const; + gtsam::LabeledSymbol newLabel(unsigned char label) const; + + void print(string s) const; +}; + +size_t mrsymbol(unsigned char c, unsigned char label, size_t j); +unsigned char mrsymbolChr(size_t key); +unsigned char mrsymbolLabel(size_t key); +size_t mrsymbolIndex(size_t key); + +#include +class Ordering { + // Standard Constructors and Named Constructors + Ordering(); + Ordering(const gtsam::Ordering& other); + + // Testable + void print(string s) const; + bool equals(const gtsam::Ordering& ord, double tol) const; + + // Standard interface + size_t size() const; + size_t at(size_t key) const; + void push_back(size_t key); + + // enabling serialization functionality + void serialize() const; +}; + +#include +class NonlinearFactorGraph { + NonlinearFactorGraph(); + NonlinearFactorGraph(const gtsam::NonlinearFactorGraph& graph); + + // FactorGraph + void print(string s) const; + bool equals(const gtsam::NonlinearFactorGraph& fg, double tol) const; + size_t size() const; + bool empty() const; + void remove(size_t i); + void replace(size_t i, gtsam::NonlinearFactor* factors); + void resize(size_t size); + size_t nrFactors() const; + gtsam::NonlinearFactor* at(size_t idx) const; + void push_back(const gtsam::NonlinearFactorGraph& factors); + void push_back(gtsam::NonlinearFactor* factor); + void add(gtsam::NonlinearFactor* factor); + bool exists(size_t idx) const; + gtsam::KeySet keys() const; + gtsam::KeyVector keyVector() const; + + template, gtsam::imuBias::ConstantBias}> + void addPrior(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); + + // NonlinearFactorGraph + void printErrors(const gtsam::Values& values) const; + double error(const gtsam::Values& values) const; + double probPrime(const gtsam::Values& values) const; + gtsam::Ordering orderingCOLAMD() const; + // Ordering* orderingCOLAMDConstrained(const gtsam::Values& c, const std::map& constraints) const; + gtsam::GaussianFactorGraph* linearize(const gtsam::Values& values) const; + gtsam::NonlinearFactorGraph clone() const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +virtual class NonlinearFactor { + // Factor base class + size_t size() const; + gtsam::KeyVector keys() const; + void print(string s) const; + void printKeys(string s) const; + // NonlinearFactor + bool equals(const gtsam::NonlinearFactor& other, double tol) const; + double error(const gtsam::Values& c) const; + size_t dim() const; + bool active(const gtsam::Values& c) const; + gtsam::GaussianFactor* linearize(const gtsam::Values& c) const; + gtsam::NonlinearFactor* clone() const; + // gtsam::NonlinearFactor* rekey(const gtsam::KeyVector& newKeys) const; //TODO: Conversion from KeyVector to std::vector does not happen +}; + +#include +virtual class NoiseModelFactor: gtsam::NonlinearFactor { + bool equals(const gtsam::NoiseModelFactor& other, double tol) const; + gtsam::noiseModel::Base* noiseModel() const; + Vector unwhitenedError(const gtsam::Values& x) const; + Vector whitenedError(const gtsam::Values& x) const; +}; + +#include +class Values { + Values(); + Values(const gtsam::Values& other); + + size_t size() const; + bool empty() const; + void clear(); + size_t dim() const; + + void print(string s) const; + bool equals(const gtsam::Values& other, double tol) const; + + void insert(const gtsam::Values& values); + void update(const gtsam::Values& values); + void erase(size_t j); + void swap(gtsam::Values& values); + + bool exists(size_t j) const; + gtsam::KeyVector keys() const; + + gtsam::VectorValues zeroVectors() const; + + gtsam::Values retract(const gtsam::VectorValues& delta) const; + gtsam::VectorValues localCoordinates(const gtsam::Values& cp) const; + + // enabling serialization functionality + void serialize() const; + + // New in 4.0, we have to specialize every insert/update/at to generate wrappers + // Instead of the old: + // void insert(size_t j, const gtsam::Value& value); + // void update(size_t j, const gtsam::Value& val); + // gtsam::Value at(size_t j) const; + + // The order is important: Vector has to precede Point2/Point3 so `atVector` + // can work for those fixed-size vectors. + void insert(size_t j, Vector vector); + void insert(size_t j, Matrix matrix); + void insert(size_t j, const gtsam::Point2& point2); + void insert(size_t j, const gtsam::Point3& point3); + void insert(size_t j, const gtsam::Rot2& rot2); + void insert(size_t j, const gtsam::Pose2& pose2); + void insert(size_t j, const gtsam::SO3& R); + void insert(size_t j, const gtsam::SO4& Q); + void insert(size_t j, const gtsam::SOn& P); + void insert(size_t j, const gtsam::Rot3& rot3); + void insert(size_t j, const gtsam::Pose3& pose3); + void insert(size_t j, const gtsam::Unit3& unit3); + void insert(size_t j, const gtsam::Cal3_S2& cal3_s2); + void insert(size_t j, const gtsam::Cal3DS2& cal3ds2); + void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler); + void insert(size_t j, const gtsam::EssentialMatrix& essential_matrix); + void insert(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera); + void insert(size_t j, const gtsam::PinholeCamera& camera); + void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); + void insert(size_t j, const gtsam::NavState& nav_state); + + void update(size_t j, const gtsam::Point2& point2); + void update(size_t j, const gtsam::Point3& point3); + void update(size_t j, const gtsam::Rot2& rot2); + void update(size_t j, const gtsam::Pose2& pose2); + void update(size_t j, const gtsam::SO3& R); + void update(size_t j, const gtsam::SO4& Q); + void update(size_t j, const gtsam::SOn& P); + void update(size_t j, const gtsam::Rot3& rot3); + void update(size_t j, const gtsam::Pose3& pose3); + void update(size_t j, const gtsam::Unit3& unit3); + void update(size_t j, const gtsam::Cal3_S2& cal3_s2); + void update(size_t j, const gtsam::Cal3DS2& cal3ds2); + void update(size_t j, const gtsam::Cal3Bundler& cal3bundler); + void update(size_t j, const gtsam::EssentialMatrix& essential_matrix); + void update(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera); + void update(size_t j, const gtsam::PinholeCamera& camera); + void update(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); + void update(size_t j, const gtsam::NavState& nav_state); + void update(size_t j, Vector vector); + void update(size_t j, Matrix matrix); + + template, gtsam::imuBias::ConstantBias, gtsam::NavState, Vector, Matrix}> + T at(size_t j); + + /// version for double + void insertDouble(size_t j, double c); + double atDouble(size_t j) const; +}; + +#include +class Marginals { + Marginals(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& solution); + Marginals(const gtsam::GaussianFactorGraph& gfgraph, + const gtsam::Values& solution); + Marginals(const gtsam::GaussianFactorGraph& gfgraph, + const gtsam::VectorValues& solutionvec); + + void print(string s) const; + Matrix marginalCovariance(size_t variable) const; + Matrix marginalInformation(size_t variable) const; + gtsam::JointMarginal jointMarginalCovariance(const gtsam::KeyVector& variables) const; + gtsam::JointMarginal jointMarginalInformation(const gtsam::KeyVector& variables) const; +}; + +class JointMarginal { + Matrix at(size_t iVariable, size_t jVariable) const; + Matrix fullMatrix() const; + void print(string s) const; + void print() const; +}; + +#include +virtual class LinearContainerFactor : gtsam::NonlinearFactor { + + LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::Values& linearizationPoint); + LinearContainerFactor(gtsam::GaussianFactor* factor); + + gtsam::GaussianFactor* factor() const; +// const boost::optional& linearizationPoint() const; + + bool isJacobian() const; + gtsam::JacobianFactor* toJacobian() const; + gtsam::HessianFactor* toHessian() const; + + static gtsam::NonlinearFactorGraph ConvertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph, + const gtsam::Values& linearizationPoint); + + static gtsam::NonlinearFactorGraph ConvertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph); + + // enabling serialization functionality + void serializable() const; +}; // \class LinearContainerFactor // Summarization functionality //#include @@ -2321,210 +2231,196 @@ class Cal3_S2 { // Nonlinear optimizers //************************************************************************* #include - virtual class NonlinearOptimizerParams - { - NonlinearOptimizerParams(); - void print(string s) const; +virtual class NonlinearOptimizerParams { + NonlinearOptimizerParams(); + void print(string s) const; - int getMaxIterations() const; - double getRelativeErrorTol() const; - double getAbsoluteErrorTol() const; - double getErrorTol() const; - string getVerbosity() const; + int getMaxIterations() const; + double getRelativeErrorTol() const; + double getAbsoluteErrorTol() const; + double getErrorTol() const; + string getVerbosity() const; - void setMaxIterations(int value); - void setRelativeErrorTol(double value); - void setAbsoluteErrorTol(double value); - void setErrorTol(double value); - void setVerbosity(string s); + void setMaxIterations(int value); + void setRelativeErrorTol(double value); + void setAbsoluteErrorTol(double value); + void setErrorTol(double value); + void setVerbosity(string s); - string getLinearSolverType() const; - void setLinearSolverType(string solver); + string getLinearSolverType() const; + void setLinearSolverType(string solver); - void setIterativeParams(gtsam::IterativeOptimizationParameters *params); - void setOrdering(const gtsam::Ordering &ordering); - string getOrderingType() const; - void setOrderingType(string ordering); + void setIterativeParams(gtsam::IterativeOptimizationParameters* params); + void setOrdering(const gtsam::Ordering& ordering); + string getOrderingType() const; + void setOrderingType(string ordering); - bool isMultifrontal() const; - bool isSequential() const; - bool isCholmod() const; - bool isIterative() const; - }; + bool isMultifrontal() const; + bool isSequential() const; + bool isCholmod() const; + bool isIterative() const; +}; - bool checkConvergence(double relativeErrorTreshold, - double absoluteErrorTreshold, double errorThreshold, - double currentError, double newError); - bool checkConvergence(const gtsam::NonlinearOptimizerParams ¶ms, - double currentError, double newError); +bool checkConvergence(double relativeErrorTreshold, + double absoluteErrorTreshold, double errorThreshold, + double currentError, double newError); +bool checkConvergence(const gtsam::NonlinearOptimizerParams& params, + double currentError, double newError); #include - virtual class GaussNewtonParams : gtsam::NonlinearOptimizerParams - { - GaussNewtonParams(); - }; +virtual class GaussNewtonParams : gtsam::NonlinearOptimizerParams { + GaussNewtonParams(); +}; #include - virtual class LevenbergMarquardtParams : gtsam::NonlinearOptimizerParams - { - LevenbergMarquardtParams(); +virtual class LevenbergMarquardtParams : gtsam::NonlinearOptimizerParams { + LevenbergMarquardtParams(); - bool getDiagonalDamping() const; - double getlambdaFactor() const; - double getlambdaInitial() const; - double getlambdaLowerBound() const; - double getlambdaUpperBound() const; - bool getUseFixedLambdaFactor(); - string getLogFile() const; - string getVerbosityLM() const; + bool getDiagonalDamping() const; + double getlambdaFactor() const; + double getlambdaInitial() const; + double getlambdaLowerBound() const; + double getlambdaUpperBound() const; + bool getUseFixedLambdaFactor(); + string getLogFile() const; + string getVerbosityLM() const; - void setDiagonalDamping(bool flag); - void setlambdaFactor(double value); - void setlambdaInitial(double value); - void setlambdaLowerBound(double value); - void setlambdaUpperBound(double value); - void setUseFixedLambdaFactor(bool flag); - void setLogFile(string s); - void setVerbosityLM(string s); + void setDiagonalDamping(bool flag); + void setlambdaFactor(double value); + void setlambdaInitial(double value); + void setlambdaLowerBound(double value); + void setlambdaUpperBound(double value); + void setUseFixedLambdaFactor(bool flag); + void setLogFile(string s); + void setVerbosityLM(string s); - static gtsam::LevenbergMarquardtParams LegacyDefaults(); - static gtsam::LevenbergMarquardtParams CeresDefaults(); + static gtsam::LevenbergMarquardtParams LegacyDefaults(); + static gtsam::LevenbergMarquardtParams CeresDefaults(); - static gtsam::LevenbergMarquardtParams EnsureHasOrdering( - gtsam::LevenbergMarquardtParams params, - const gtsam::NonlinearFactorGraph &graph); - static gtsam::LevenbergMarquardtParams ReplaceOrdering( - gtsam::LevenbergMarquardtParams params, const gtsam::Ordering &ordering); - }; + static gtsam::LevenbergMarquardtParams EnsureHasOrdering( + gtsam::LevenbergMarquardtParams params, + const gtsam::NonlinearFactorGraph& graph); + static gtsam::LevenbergMarquardtParams ReplaceOrdering( + gtsam::LevenbergMarquardtParams params, const gtsam::Ordering& ordering); +}; #include - virtual class DoglegParams : gtsam::NonlinearOptimizerParams - { - DoglegParams(); +virtual class DoglegParams : gtsam::NonlinearOptimizerParams { + DoglegParams(); - double getDeltaInitial() const; - string getVerbosityDL() const; + double getDeltaInitial() const; + string getVerbosityDL() const; - void setDeltaInitial(double deltaInitial) const; - void setVerbosityDL(string verbosityDL) const; - }; + void setDeltaInitial(double deltaInitial) const; + void setVerbosityDL(string verbosityDL) const; +}; #include - virtual class NonlinearOptimizer - { - gtsam::Values optimize(); - gtsam::Values optimizeSafely(); - double error() const; - int iterations() const; - gtsam::Values values() const; - gtsam::NonlinearFactorGraph graph() const; - gtsam::GaussianFactorGraph *iterate() const; - }; +virtual class NonlinearOptimizer { + gtsam::Values optimize(); + gtsam::Values optimizeSafely(); + double error() const; + int iterations() const; + gtsam::Values values() const; + gtsam::NonlinearFactorGraph graph() const; + gtsam::GaussianFactorGraph* iterate() const; +}; #include - virtual class GaussNewtonOptimizer : gtsam::NonlinearOptimizer - { - GaussNewtonOptimizer(const gtsam::NonlinearFactorGraph &graph, const gtsam::Values &initialValues); - GaussNewtonOptimizer(const gtsam::NonlinearFactorGraph &graph, const gtsam::Values &initialValues, const gtsam::GaussNewtonParams ¶ms); - }; +virtual class GaussNewtonOptimizer : gtsam::NonlinearOptimizer { + GaussNewtonOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); + GaussNewtonOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::GaussNewtonParams& params); +}; #include - virtual class DoglegOptimizer : gtsam::NonlinearOptimizer - { - DoglegOptimizer(const gtsam::NonlinearFactorGraph &graph, const gtsam::Values &initialValues); - DoglegOptimizer(const gtsam::NonlinearFactorGraph &graph, const gtsam::Values &initialValues, const gtsam::DoglegParams ¶ms); - double getDelta() const; - }; +virtual class DoglegOptimizer : gtsam::NonlinearOptimizer { + DoglegOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); + DoglegOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::DoglegParams& params); + double getDelta() const; +}; #include - virtual class LevenbergMarquardtOptimizer : gtsam::NonlinearOptimizer - { - LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph &graph, const gtsam::Values &initialValues); - LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph &graph, const gtsam::Values &initialValues, const gtsam::LevenbergMarquardtParams ¶ms); - double lambda() const; - void print(string str) const; - }; +virtual class LevenbergMarquardtOptimizer : gtsam::NonlinearOptimizer { + LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); + LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::LevenbergMarquardtParams& params); + double lambda() const; + void print(string str) const; +}; #include - class ISAM2GaussNewtonParams - { - ISAM2GaussNewtonParams(); +class ISAM2GaussNewtonParams { + ISAM2GaussNewtonParams(); - void print(string str) const; + void print(string str) const; - /** Getters and Setters for all properties */ - double getWildfireThreshold() const; - void setWildfireThreshold(double wildfireThreshold); - }; + /** Getters and Setters for all properties */ + double getWildfireThreshold() const; + void setWildfireThreshold(double wildfireThreshold); +}; - class ISAM2DoglegParams - { - ISAM2DoglegParams(); +class ISAM2DoglegParams { + ISAM2DoglegParams(); - void print(string str) const; + void print(string str) const; - /** Getters and Setters for all properties */ - double getWildfireThreshold() const; - void setWildfireThreshold(double wildfireThreshold); - double getInitialDelta() const; - void setInitialDelta(double initialDelta); - string getAdaptationMode() const; - void setAdaptationMode(string adaptationMode); - bool isVerbose() const; - void setVerbose(bool verbose); - }; + /** Getters and Setters for all properties */ + double getWildfireThreshold() const; + void setWildfireThreshold(double wildfireThreshold); + double getInitialDelta() const; + void setInitialDelta(double initialDelta); + string getAdaptationMode() const; + void setAdaptationMode(string adaptationMode); + bool isVerbose() const; + void setVerbose(bool verbose); +}; - class ISAM2ThresholdMapValue - { - ISAM2ThresholdMapValue(char c, Vector thresholds); - ISAM2ThresholdMapValue(const gtsam::ISAM2ThresholdMapValue &other); - }; +class ISAM2ThresholdMapValue { + ISAM2ThresholdMapValue(char c, Vector thresholds); + ISAM2ThresholdMapValue(const gtsam::ISAM2ThresholdMapValue& other); +}; - class ISAM2ThresholdMap - { - ISAM2ThresholdMap(); - ISAM2ThresholdMap(const gtsam::ISAM2ThresholdMap &other); +class ISAM2ThresholdMap { + ISAM2ThresholdMap(); + ISAM2ThresholdMap(const gtsam::ISAM2ThresholdMap& other); - // Note: no print function + // Note: no print function - // common STL methods - size_t size() const; - bool empty() const; - void clear(); + // common STL methods + size_t size() const; + bool empty() const; + void clear(); - // structure specific methods - void insert(const gtsam::ISAM2ThresholdMapValue &value) const; - }; + // structure specific methods + void insert(const gtsam::ISAM2ThresholdMapValue& value) const; +}; - class ISAM2Params - { - ISAM2Params(); +class ISAM2Params { + ISAM2Params(); - void print(string str) const; + void print(string str) const; - /** Getters and Setters for all properties */ - void setOptimizationParams(const gtsam::ISAM2GaussNewtonParams &gauss_newton__params); - void setOptimizationParams(const gtsam::ISAM2DoglegParams &dogleg_params); - void setRelinearizeThreshold(double threshold); - void setRelinearizeThreshold(const gtsam::ISAM2ThresholdMap &threshold_map); - int getRelinearizeSkip() const; - void setRelinearizeSkip(int relinearizeSkip); - bool isEnableRelinearization() const; - void setEnableRelinearization(bool enableRelinearization); - bool isEvaluateNonlinearError() const; - void setEvaluateNonlinearError(bool evaluateNonlinearError); - string getFactorization() const; - void setFactorization(string factorization); - bool isCacheLinearizedFactors() const; - void setCacheLinearizedFactors(bool cacheLinearizedFactors); - bool isEnableDetailedResults() const; - void setEnableDetailedResults(bool enableDetailedResults); - bool isEnablePartialRelinearizationCheck() const; - void setEnablePartialRelinearizationCheck(bool enablePartialRelinearizationCheck); - }; + /** Getters and Setters for all properties */ + void setOptimizationParams(const gtsam::ISAM2GaussNewtonParams& gauss_newton__params); + void setOptimizationParams(const gtsam::ISAM2DoglegParams& dogleg_params); + void setRelinearizeThreshold(double threshold); + void setRelinearizeThreshold(const gtsam::ISAM2ThresholdMap& threshold_map); + int getRelinearizeSkip() const; + void setRelinearizeSkip(int relinearizeSkip); + bool isEnableRelinearization() const; + void setEnableRelinearization(bool enableRelinearization); + bool isEvaluateNonlinearError() const; + void setEvaluateNonlinearError(bool evaluateNonlinearError); + string getFactorization() const; + void setFactorization(string factorization); + bool isCacheLinearizedFactors() const; + void setCacheLinearizedFactors(bool cacheLinearizedFactors); + bool isEnableDetailedResults() const; + void setEnableDetailedResults(bool enableDetailedResults); + bool isEnablePartialRelinearizationCheck() const; + void setEnablePartialRelinearizationCheck(bool enablePartialRelinearizationCheck); +}; - class ISAM2Clique - { +class ISAM2Clique { //Constructors ISAM2Clique(); @@ -2532,77 +2428,74 @@ class Cal3_S2 { //Standard Interface Vector gradientContribution() const; void print(string s); - }; +}; - class ISAM2Result - { - ISAM2Result(); +class ISAM2Result { + ISAM2Result(); - void print(string str) const; + void print(string str) const; - /** Getters and Setters for all properties */ - size_t getVariablesRelinearized() const; - size_t getVariablesReeliminated() const; - size_t getCliques() const; - double getErrorBefore() const; - double getErrorAfter() const; - }; + /** Getters and Setters for all properties */ + size_t getVariablesRelinearized() const; + size_t getVariablesReeliminated() const; + size_t getCliques() const; + double getErrorBefore() const; + double getErrorAfter() const; +}; - class ISAM2 - { - ISAM2(); - ISAM2(const gtsam::ISAM2Params ¶ms); - ISAM2(const gtsam::ISAM2 &other); +class ISAM2 { + ISAM2(); + ISAM2(const gtsam::ISAM2Params& params); + ISAM2(const gtsam::ISAM2& other); - bool equals(const gtsam::ISAM2 &other, double tol) const; - void print(string s) const; - void printStats() const; - void saveGraph(string s) const; + bool equals(const gtsam::ISAM2& other, double tol) const; + void print(string s) const; + void printStats() const; + void saveGraph(string s) const; - gtsam::ISAM2Result update(); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph &newFactors, const gtsam::Values &newTheta); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph &newFactors, const gtsam::Values &newTheta, const gtsam::FactorIndices &removeFactorIndices); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph &newFactors, const gtsam::Values &newTheta, const gtsam::FactorIndices &removeFactorIndices, const gtsam::KeyGroupMap &constrainedKeys); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph &newFactors, const gtsam::Values &newTheta, const gtsam::FactorIndices &removeFactorIndices, gtsam::KeyGroupMap &constrainedKeys, const gtsam::KeyList &noRelinKeys); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph &newFactors, const gtsam::Values &newTheta, const gtsam::FactorIndices &removeFactorIndices, gtsam::KeyGroupMap &constrainedKeys, const gtsam::KeyList &noRelinKeys, const gtsam::KeyList &extraReelimKeys); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph &newFactors, const gtsam::Values &newTheta, const gtsam::FactorIndices &removeFactorIndices, gtsam::KeyGroupMap &constrainedKeys, const gtsam::KeyList &noRelinKeys, const gtsam::KeyList &extraReelimKeys, bool force_relinearize); + gtsam::ISAM2Result update(); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, const gtsam::KeyGroupMap& constrainedKeys); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, gtsam::KeyGroupMap& constrainedKeys, const gtsam::KeyList& noRelinKeys); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, gtsam::KeyGroupMap& constrainedKeys, const gtsam::KeyList& noRelinKeys, const gtsam::KeyList& extraReelimKeys); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, gtsam::KeyGroupMap& constrainedKeys, const gtsam::KeyList& noRelinKeys, const gtsam::KeyList& extraReelimKeys, bool force_relinearize); - gtsam::Values getLinearizationPoint() const; - gtsam::Values calculateEstimate() const; - template , - Vector, Matrix}> - VALUE calculateEstimate(size_t key) const; - gtsam::Values calculateBestEstimate() const; - Matrix marginalCovariance(size_t key) const; - gtsam::VectorValues getDelta() const; - gtsam::NonlinearFactorGraph getFactorsUnsafe() const; - gtsam::VariableIndex getVariableIndex() const; - gtsam::ISAM2Params params() const; - }; + gtsam::Values getLinearizationPoint() const; + gtsam::Values calculateEstimate() const; + template , + Vector, Matrix}> + VALUE calculateEstimate(size_t key) const; + gtsam::Values calculateBestEstimate() const; + Matrix marginalCovariance(size_t key) const; + gtsam::VectorValues getDelta() const; + gtsam::NonlinearFactorGraph getFactorsUnsafe() const; + gtsam::VariableIndex getVariableIndex() const; + gtsam::ISAM2Params params() const; +}; #include - class NonlinearISAM - { - NonlinearISAM(); - NonlinearISAM(int reorderInterval); - void print(string s) const; - void printStats() const; - void saveGraph(string s) const; - gtsam::Values estimate() const; - Matrix marginalCovariance(size_t key) const; - int reorderInterval() const; - int reorderCounter() const; - void update(const gtsam::NonlinearFactorGraph &newFactors, const gtsam::Values &initialValues); - void reorder_relinearize(); +class NonlinearISAM { + NonlinearISAM(); + NonlinearISAM(int reorderInterval); + void print(string s) const; + void printStats() const; + void saveGraph(string s) const; + gtsam::Values estimate() const; + Matrix marginalCovariance(size_t key) const; + int reorderInterval() const; + int reorderCounter() const; + void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& initialValues); + void reorder_relinearize(); - // These might be expensive as instead of a reference the wrapper will make a copy - gtsam::GaussianISAM bayesTree() const; - gtsam::Values getLinearizationPoint() const; - gtsam::NonlinearFactorGraph getFactorsUnsafe() const; - }; + // These might be expensive as instead of a reference the wrapper will make a copy + gtsam::GaussianISAM bayesTree() const; + gtsam::Values getLinearizationPoint() const; + gtsam::NonlinearFactorGraph getFactorsUnsafe() const; +}; //************************************************************************* // Nonlinear factor types @@ -2611,966 +2504,917 @@ class Cal3_S2 { #include #include - template }> - virtual class PriorFactor : gtsam::NoiseModelFactor - { - PriorFactor(size_t key, const T &prior, const gtsam::noiseModel::Base *noiseModel); - T prior() const; +template}> +virtual class PriorFactor : gtsam::NoiseModelFactor { + PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); + T prior() const; + + // enabling serialization functionality + void serialize() const; +}; - // enabling serialization functionality - void serialize() const; - }; #include - template - virtual class BetweenFactor : gtsam::NoiseModelFactor - { - BetweenFactor(size_t key1, size_t key2, const T &relativePose, const gtsam::noiseModel::Base *noiseModel); - T measured() const; +template +virtual class BetweenFactor : gtsam::NoiseModelFactor { + BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel); + T measured() const; - // enabling serialization functionality - void serialize() const; - }; + // enabling serialization functionality + void serialize() const; +}; #include - template - virtual class NonlinearEquality : gtsam::NoiseModelFactor - { - // Constructor - forces exact evaluation - NonlinearEquality(size_t j, const T &feasible); - // Constructor - allows inexact evaluation - NonlinearEquality(size_t j, const T &feasible, double error_gain); +template +virtual class NonlinearEquality : gtsam::NoiseModelFactor { + // Constructor - forces exact evaluation + NonlinearEquality(size_t j, const T& feasible); + // Constructor - allows inexact evaluation + NonlinearEquality(size_t j, const T& feasible, double error_gain); - // enabling serialization functionality - void serialize() const; - }; + // enabling serialization functionality + void serialize() const; +}; #include - template - virtual class RangeFactor : gtsam::NoiseModelFactor - { - RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base *noiseModel); +template +virtual class RangeFactor : gtsam::NoiseModelFactor { + RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel); - // enabling serialization functionality - void serialize() const; - }; + // enabling serialization functionality + void serialize() const; +}; + +typedef gtsam::RangeFactor RangeFactor2D; +typedef gtsam::RangeFactor RangeFactor3D; +typedef gtsam::RangeFactor RangeFactorPose2; +typedef gtsam::RangeFactor RangeFactorPose3; +typedef gtsam::RangeFactor RangeFactorCalibratedCameraPoint; +typedef gtsam::RangeFactor RangeFactorSimpleCameraPoint; +typedef gtsam::RangeFactor RangeFactorCalibratedCamera; +typedef gtsam::RangeFactor RangeFactorSimpleCamera; - typedef gtsam::RangeFactor RangeFactor2D; - typedef gtsam::RangeFactor RangeFactor3D; - typedef gtsam::RangeFactor RangeFactorPose2; - typedef gtsam::RangeFactor RangeFactorPose3; - typedef gtsam::RangeFactor RangeFactorCalibratedCameraPoint; - typedef gtsam::RangeFactor RangeFactorSimpleCameraPoint; - typedef gtsam::RangeFactor RangeFactorCalibratedCamera; - typedef gtsam::RangeFactor RangeFactorSimpleCamera; #include - template - virtual class RangeFactorWithTransform : gtsam::NoiseModelFactor - { - RangeFactorWithTransform(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base *noiseModel, const POSE &body_T_sensor); +template +virtual class RangeFactorWithTransform : gtsam::NoiseModelFactor { + RangeFactorWithTransform(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel, const POSE& body_T_sensor); - // enabling serialization functionality - void serialize() const; - }; + // enabling serialization functionality + void serialize() const; +}; - typedef gtsam::RangeFactorWithTransform RangeFactorWithTransform2D; - typedef gtsam::RangeFactorWithTransform RangeFactorWithTransform3D; - typedef gtsam::RangeFactorWithTransform RangeFactorWithTransformPose2; - typedef gtsam::RangeFactorWithTransform RangeFactorWithTransformPose3; +typedef gtsam::RangeFactorWithTransform RangeFactorWithTransform2D; +typedef gtsam::RangeFactorWithTransform RangeFactorWithTransform3D; +typedef gtsam::RangeFactorWithTransform RangeFactorWithTransformPose2; +typedef gtsam::RangeFactorWithTransform RangeFactorWithTransformPose3; #include - template - virtual class BearingFactor : gtsam::NoiseModelFactor - { - BearingFactor(size_t key1, size_t key2, const BEARING &measured, const gtsam::noiseModel::Base *noiseModel); +template +virtual class BearingFactor : gtsam::NoiseModelFactor { + BearingFactor(size_t key1, size_t key2, const BEARING& measured, const gtsam::noiseModel::Base* noiseModel); - // enabling serialization functionality - void serialize() const; - }; + // enabling serialization functionality + void serialize() const; +}; - typedef gtsam::BearingFactor BearingFactor2D; - typedef gtsam::BearingFactor BearingFactorPose2; +typedef gtsam::BearingFactor BearingFactor2D; +typedef gtsam::BearingFactor BearingFactorPose2; #include - template - class BearingRange - { - BearingRange(const BEARING &b, const RANGE &r); - BEARING bearing() const; - RANGE range() const; - static This Measure(const POSE &pose, const POINT &point); - static BEARING MeasureBearing(const POSE &pose, const POINT &point); - static RANGE MeasureRange(const POSE &pose, const POINT &point); - void print(string s) const; - }; +template +class BearingRange { + BearingRange(const BEARING& b, const RANGE& r); + BEARING bearing() const; + RANGE range() const; + static This Measure(const POSE& pose, const POINT& point); + static BEARING MeasureBearing(const POSE& pose, const POINT& point); + static RANGE MeasureRange(const POSE& pose, const POINT& point); + void print(string s) const; +}; - typedef gtsam::BearingRange BearingRange2D; +typedef gtsam::BearingRange BearingRange2D; #include - template - virtual class BearingRangeFactor : gtsam::NoiseModelFactor - { - BearingRangeFactor(size_t poseKey, size_t pointKey, - const BEARING &measuredBearing, const RANGE &measuredRange, - const gtsam::noiseModel::Base *noiseModel); +template +virtual class BearingRangeFactor : gtsam::NoiseModelFactor { + BearingRangeFactor(size_t poseKey, size_t pointKey, + const BEARING& measuredBearing, const RANGE& measuredRange, + const gtsam::noiseModel::Base* noiseModel); - // enabling serialization functionality - void serialize() const; - }; + // enabling serialization functionality + void serialize() const; +}; + +typedef gtsam::BearingRangeFactor BearingRangeFactor2D; +typedef gtsam::BearingRangeFactor BearingRangeFactorPose2; - typedef gtsam::BearingRangeFactor BearingRangeFactor2D; - typedef gtsam::BearingRangeFactor BearingRangeFactorPose2; #include - template - virtual class GenericProjectionFactor : gtsam::NoiseModelFactor - { - GenericProjectionFactor(const gtsam::Point2 &measured, const gtsam::noiseModel::Base *noiseModel, - size_t poseKey, size_t pointKey, const CALIBRATION *k); - GenericProjectionFactor(const gtsam::Point2 &measured, const gtsam::noiseModel::Base *noiseModel, - size_t poseKey, size_t pointKey, const CALIBRATION *k, const POSE &body_P_sensor); +template +virtual class GenericProjectionFactor : gtsam::NoiseModelFactor { + GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, + size_t poseKey, size_t pointKey, const CALIBRATION* k); + GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, + size_t poseKey, size_t pointKey, const CALIBRATION* k, const POSE& body_P_sensor); - GenericProjectionFactor(const gtsam::Point2 &measured, const gtsam::noiseModel::Base *noiseModel, - size_t poseKey, size_t pointKey, const CALIBRATION *k, bool throwCheirality, bool verboseCheirality); - GenericProjectionFactor(const gtsam::Point2 &measured, const gtsam::noiseModel::Base *noiseModel, - size_t poseKey, size_t pointKey, const CALIBRATION *k, bool throwCheirality, bool verboseCheirality, - const POSE &body_P_sensor); + GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, + size_t poseKey, size_t pointKey, const CALIBRATION* k, bool throwCheirality, bool verboseCheirality); + GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, + size_t poseKey, size_t pointKey, const CALIBRATION* k, bool throwCheirality, bool verboseCheirality, + const POSE& body_P_sensor); - gtsam::Point2 measured() const; - CALIBRATION *calibration() const; - bool verboseCheirality() const; - bool throwCheirality() const; + gtsam::Point2 measured() const; + CALIBRATION* calibration() const; + bool verboseCheirality() const; + bool throwCheirality() const; + + // enabling serialization functionality + void serialize() const; +}; +typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3_S2; +typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3DS2; - // enabling serialization functionality - void serialize() const; - }; - typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3_S2; - typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3DS2; #include - template - virtual class GeneralSFMFactor : gtsam::NoiseModelFactor - { - GeneralSFMFactor(const gtsam::Point2 &measured, const gtsam::noiseModel::Base *model, size_t cameraKey, size_t landmarkKey); - gtsam::Point2 measured() const; - }; - typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; - typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3DS2; - typedef gtsam::GeneralSFMFactor, gtsam::Point3> GeneralSFMFactorCal3Bundler; +template +virtual class GeneralSFMFactor : gtsam::NoiseModelFactor { + GeneralSFMFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t cameraKey, size_t landmarkKey); + gtsam::Point2 measured() const; +}; +typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; +typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3DS2; +typedef gtsam::GeneralSFMFactor, gtsam::Point3> GeneralSFMFactorCal3Bundler; - template - virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor - { - GeneralSFMFactor2(const gtsam::Point2 &measured, const gtsam::noiseModel::Base *model, size_t poseKey, size_t landmarkKey, size_t calibKey); - gtsam::Point2 measured() const; +template +virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { + GeneralSFMFactor2(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t poseKey, size_t landmarkKey, size_t calibKey); + gtsam::Point2 measured() const; - // enabling serialization functionality - void serialize() const; - }; + // enabling serialization functionality + void serialize() const; +}; #include - class SmartProjectionParams - { - SmartProjectionParams(); - // TODO(frank): make these work: - // void setLinearizationMode(LinearizationMode linMode); - // void setDegeneracyMode(DegeneracyMode degMode); - void setRankTolerance(double rankTol); - void setEnableEPI(bool enableEPI); - void setLandmarkDistanceThreshold(bool landmarkDistanceThreshold); - void setDynamicOutlierRejectionThreshold(bool dynOutRejectionThreshold); - }; +class SmartProjectionParams { + SmartProjectionParams(); + // TODO(frank): make these work: + // void setLinearizationMode(LinearizationMode linMode); + // void setDegeneracyMode(DegeneracyMode degMode); + void setRankTolerance(double rankTol); + void setEnableEPI(bool enableEPI); + void setLandmarkDistanceThreshold(bool landmarkDistanceThreshold); + void setDynamicOutlierRejectionThreshold(bool dynOutRejectionThreshold); +}; #include - template - virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor - { +template +virtual class SmartProjectionPoseFactor: gtsam::NonlinearFactor { - SmartProjectionPoseFactor(const gtsam::noiseModel::Base *noise, - const CALIBRATION *K); - SmartProjectionPoseFactor(const gtsam::noiseModel::Base *noise, - const CALIBRATION *K, - const gtsam::Pose3 &body_P_sensor); - SmartProjectionPoseFactor(const gtsam::noiseModel::Base *noise, - const CALIBRATION *K, - const gtsam::SmartProjectionParams ¶ms); - SmartProjectionPoseFactor(const gtsam::noiseModel::Base *noise, - const CALIBRATION *K, - const gtsam::Pose3 &body_P_sensor, - const gtsam::SmartProjectionParams ¶ms); + SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, + const CALIBRATION* K); + SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, + const CALIBRATION* K, + const gtsam::Pose3& body_P_sensor); + SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, + const CALIBRATION* K, + const gtsam::SmartProjectionParams& params); + SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, + const CALIBRATION* K, + const gtsam::Pose3& body_P_sensor, + const gtsam::SmartProjectionParams& params); - void add(const gtsam::Point2 &measured_i, size_t poseKey_i); + void add(const gtsam::Point2& measured_i, size_t poseKey_i); - // enabling serialization functionality - //void serialize() const; - }; + // enabling serialization functionality + //void serialize() const; +}; + +typedef gtsam::SmartProjectionPoseFactor SmartProjectionPose3Factor; - typedef gtsam::SmartProjectionPoseFactor SmartProjectionPose3Factor; #include - template - virtual class GenericStereoFactor : gtsam::NoiseModelFactor - { - GenericStereoFactor(const gtsam::StereoPoint2 &measured, const gtsam::noiseModel::Base *noiseModel, - size_t poseKey, size_t landmarkKey, const gtsam::Cal3_S2Stereo *K); - gtsam::StereoPoint2 measured() const; - gtsam::Cal3_S2Stereo *calibration() const; +template +virtual class GenericStereoFactor : gtsam::NoiseModelFactor { + GenericStereoFactor(const gtsam::StereoPoint2& measured, const gtsam::noiseModel::Base* noiseModel, + size_t poseKey, size_t landmarkKey, const gtsam::Cal3_S2Stereo* K); + gtsam::StereoPoint2 measured() const; + gtsam::Cal3_S2Stereo* calibration() const; - // enabling serialization functionality - void serialize() const; - }; - typedef gtsam::GenericStereoFactor GenericStereoFactor3D; + // enabling serialization functionality + void serialize() const; +}; +typedef gtsam::GenericStereoFactor GenericStereoFactor3D; #include - template - virtual class PoseTranslationPrior : gtsam::NoiseModelFactor - { - PoseTranslationPrior(size_t key, const POSE &pose_z, const gtsam::noiseModel::Base *noiseModel); - }; +template +virtual class PoseTranslationPrior : gtsam::NoiseModelFactor { + PoseTranslationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel); +}; - typedef gtsam::PoseTranslationPrior PoseTranslationPrior2D; - typedef gtsam::PoseTranslationPrior PoseTranslationPrior3D; +typedef gtsam::PoseTranslationPrior PoseTranslationPrior2D; +typedef gtsam::PoseTranslationPrior PoseTranslationPrior3D; #include - template - virtual class PoseRotationPrior : gtsam::NoiseModelFactor - { - PoseRotationPrior(size_t key, const POSE &pose_z, const gtsam::noiseModel::Base *noiseModel); - }; +template +virtual class PoseRotationPrior : gtsam::NoiseModelFactor { + PoseRotationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel); +}; - typedef gtsam::PoseRotationPrior PoseRotationPrior2D; - typedef gtsam::PoseRotationPrior PoseRotationPrior3D; +typedef gtsam::PoseRotationPrior PoseRotationPrior2D; +typedef gtsam::PoseRotationPrior PoseRotationPrior3D; #include - virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor - { - EssentialMatrixFactor(size_t key, const gtsam::Point2 &pA, const gtsam::Point2 &pB, - const gtsam::noiseModel::Base *noiseModel); - }; +virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor { + EssentialMatrixFactor(size_t key, const gtsam::Point2& pA, const gtsam::Point2& pB, + const gtsam::noiseModel::Base* noiseModel); +}; #include - class SfmTrack - { - SfmTrack(); - SfmTrack(const gtsam::Point3 &pt); - const Point3 &point3() const; +class SfmTrack { + SfmTrack(); + SfmTrack(const gtsam::Point3& pt); + const Point3& point3() const; - double r; - double g; - double b; - // TODO Need to close wrap#10 to allow this to work. - // std::vector> measurements; + double r; + double g; + double b; + // TODO Need to close wrap#10 to allow this to work. + // std::vector> measurements; - size_t number_measurements() const; - pair measurement(size_t idx) const; - pair siftIndex(size_t idx) const; - void add_measurement(size_t idx, const gtsam::Point2 &m); - }; + size_t number_measurements() const; + pair measurement(size_t idx) const; + pair siftIndex(size_t idx) const; + void add_measurement(size_t idx, const gtsam::Point2& m); +}; - class SfmData - { - SfmData(); - size_t number_cameras() const; - size_t number_tracks() const; - gtsam::PinholeCamera camera(size_t idx) const; - gtsam::SfmTrack track(size_t idx) const; - void add_track(const gtsam::SfmTrack &t); - void add_camera(const gtsam::SfmCamera &cam); - }; +class SfmData { + SfmData(); + size_t number_cameras() const; + size_t number_tracks() const; + gtsam::PinholeCamera camera(size_t idx) const; + gtsam::SfmTrack track(size_t idx) const; + void add_track(const gtsam::SfmTrack& t) ; + void add_camera(const gtsam::SfmCamera& cam); +}; - gtsam::SfmData readBal(string filename); - bool writeBAL(string filename, gtsam::SfmData &data); - gtsam::Values initialCamerasEstimate(const gtsam::SfmData &db); - gtsam::Values initialCamerasAndPointsEstimate(const gtsam::SfmData &db); +gtsam::SfmData readBal(string filename); +bool writeBAL(string filename, gtsam::SfmData& data); +gtsam::Values initialCamerasEstimate(const gtsam::SfmData& db); +gtsam::Values initialCamerasAndPointsEstimate(const gtsam::SfmData& db); - pair load2D(string filename, - gtsam::noiseModel::Diagonal *model, int maxIndex, bool addNoise, bool smart); - pair load2D(string filename, - gtsam::noiseModel::Diagonal *model, int maxIndex, bool addNoise); - pair load2D(string filename, - gtsam::noiseModel::Diagonal *model, int maxIndex); - pair load2D(string filename, - gtsam::noiseModel::Diagonal *model); - pair load2D(string filename); - pair load2D_robust(string filename, - gtsam::noiseModel::Base *model, int maxIndex); - void save2D(const gtsam::NonlinearFactorGraph &graph, - const gtsam::Values &config, gtsam::noiseModel::Diagonal *model, - string filename); +pair load2D(string filename, + gtsam::noiseModel::Diagonal* model, int maxIndex, bool addNoise, bool smart); +pair load2D(string filename, + gtsam::noiseModel::Diagonal* model, int maxIndex, bool addNoise); +pair load2D(string filename, + gtsam::noiseModel::Diagonal* model, int maxIndex); +pair load2D(string filename, + gtsam::noiseModel::Diagonal* model); +pair load2D(string filename); +pair load2D_robust(string filename, + gtsam::noiseModel::Base* model, int maxIndex); +void save2D(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& config, gtsam::noiseModel::Diagonal* model, + string filename); - // std::vector::shared_ptr> - // Ignored by pybind -> will be List[BetweenFactorPose2] - class BetweenFactorPose2s - { - BetweenFactorPose2s(); - size_t size() const; - gtsam::BetweenFactor *at(size_t i) const; - void push_back(const gtsam::BetweenFactor *factor); - }; - gtsam::BetweenFactorPose2s parse2DFactors(string filename); +// std::vector::shared_ptr> +// Ignored by pybind -> will be List[BetweenFactorPose2] +class BetweenFactorPose2s +{ + BetweenFactorPose2s(); + size_t size() const; + gtsam::BetweenFactor* at(size_t i) const; + void push_back(const gtsam::BetweenFactor* factor); +}; +gtsam::BetweenFactorPose2s parse2DFactors(string filename); - // std::vector::shared_ptr> - // Ignored by pybind -> will be List[BetweenFactorPose3] - class BetweenFactorPose3s - { - BetweenFactorPose3s(); - size_t size() const; - gtsam::BetweenFactor *at(size_t i) const; - void push_back(const gtsam::BetweenFactor *factor); - }; - gtsam::BetweenFactorPose3s parse3DFactors(string filename); +// std::vector::shared_ptr> +// Ignored by pybind -> will be List[BetweenFactorPose3] +class BetweenFactorPose3s +{ + BetweenFactorPose3s(); + size_t size() const; + gtsam::BetweenFactor* at(size_t i) const; + void push_back(const gtsam::BetweenFactor* factor); +}; +gtsam::BetweenFactorPose3s parse3DFactors(string filename); - pair load3D(string filename); +pair load3D(string filename); - pair readG2o(string filename); - pair readG2o(string filename, bool is3D); - void writeG2o(const gtsam::NonlinearFactorGraph &graph, - const gtsam::Values &estimate, string filename); +pair readG2o(string filename); +pair readG2o(string filename, bool is3D); +void writeG2o(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& estimate, string filename); #include - class InitializePose3 - { - static gtsam::Values computeOrientationsChordal( - const gtsam::NonlinearFactorGraph &pose3Graph); - static gtsam::Values computeOrientationsGradient( - const gtsam::NonlinearFactorGraph &pose3Graph, - const gtsam::Values &givenGuess, size_t maxIter, const bool setRefFrame); - static gtsam::Values computeOrientationsGradient( - const gtsam::NonlinearFactorGraph &pose3Graph, - const gtsam::Values &givenGuess); - static gtsam::NonlinearFactorGraph buildPose3graph( - const gtsam::NonlinearFactorGraph &graph); - static gtsam::Values initializeOrientations( - const gtsam::NonlinearFactorGraph &graph); - static gtsam::Values initialize(const gtsam::NonlinearFactorGraph &graph, - const gtsam::Values &givenGuess, - bool useGradient); - static gtsam::Values initialize(const gtsam::NonlinearFactorGraph &graph); - }; +class InitializePose3 { + static gtsam::Values computeOrientationsChordal( + const gtsam::NonlinearFactorGraph& pose3Graph); + static gtsam::Values computeOrientationsGradient( + const gtsam::NonlinearFactorGraph& pose3Graph, + const gtsam::Values& givenGuess, size_t maxIter, const bool setRefFrame); + static gtsam::Values computeOrientationsGradient( + const gtsam::NonlinearFactorGraph& pose3Graph, + const gtsam::Values& givenGuess); + static gtsam::NonlinearFactorGraph buildPose3graph( + const gtsam::NonlinearFactorGraph& graph); + static gtsam::Values initializeOrientations( + const gtsam::NonlinearFactorGraph& graph); + static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& givenGuess, + bool useGradient); + static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph); +}; #include - template - virtual class KarcherMeanFactor : gtsam::NonlinearFactor - { - KarcherMeanFactor(const gtsam::KeyVector &keys); - }; +template +virtual class KarcherMeanFactor : gtsam::NonlinearFactor { + KarcherMeanFactor(const gtsam::KeyVector& keys); +}; #include - gtsam::noiseModel::Isotropic *ConvertNoiseModel( - gtsam::noiseModel::Base *model, size_t d); +gtsam::noiseModel::Isotropic* ConvertNoiseModel( + gtsam::noiseModel::Base* model, size_t d); - template - virtual class FrobeniusFactor : gtsam::NoiseModelFactor - { - FrobeniusFactor(size_t key1, size_t key2); - FrobeniusFactor(size_t key1, size_t key2, gtsam::noiseModel::Base *model); +template +virtual class FrobeniusFactor : gtsam::NoiseModelFactor { + FrobeniusFactor(size_t key1, size_t key2); + FrobeniusFactor(size_t key1, size_t key2, gtsam::noiseModel::Base* model); - Vector evaluateError(const T &R1, const T &R2); - }; + Vector evaluateError(const T& R1, const T& R2); +}; - template - virtual class FrobeniusBetweenFactor : gtsam::NoiseModelFactor - { - FrobeniusBetweenFactor(size_t key1, size_t key2, const T &R12); - FrobeniusBetweenFactor(size_t key1, size_t key2, const T &R12, gtsam::noiseModel::Base *model); +template +virtual class FrobeniusBetweenFactor : gtsam::NoiseModelFactor { + FrobeniusBetweenFactor(size_t key1, size_t key2, const T& R12); + FrobeniusBetweenFactor(size_t key1, size_t key2, const T& R12, gtsam::noiseModel::Base* model); - Vector evaluateError(const T &R1, const T &R2); - }; + Vector evaluateError(const T& R1, const T& R2); +}; #include - virtual class ShonanFactor3 : gtsam::NoiseModelFactor - { - ShonanFactor3(size_t key1, size_t key2, const gtsam::Rot3 &R12, - size_t p); - ShonanFactor3(size_t key1, size_t key2, const gtsam::Rot3 &R12, - size_t p, gtsam::noiseModel::Base *model); - Vector evaluateError(const gtsam::SOn &Q1, const gtsam::SOn &Q2); - }; +virtual class ShonanFactor3 : gtsam::NoiseModelFactor { + ShonanFactor3(size_t key1, size_t key2, const gtsam::Rot3 &R12, + size_t p); + ShonanFactor3(size_t key1, size_t key2, const gtsam::Rot3 &R12, + size_t p, gtsam::noiseModel::Base *model); + Vector evaluateError(const gtsam::SOn &Q1, const gtsam::SOn &Q2); +}; #include - template - class BinaryMeasurement - { - BinaryMeasurement(size_t key1, size_t key2, const T &measured, - const gtsam::noiseModel::Base *model); - size_t key1() const; - size_t key2() const; - T measured() const; - gtsam::noiseModel::Base *noiseModel() const; - }; +template +class BinaryMeasurement { + BinaryMeasurement(size_t key1, size_t key2, const T& measured, + const gtsam::noiseModel::Base* model); + size_t key1() const; + size_t key2() const; + T measured() const; + gtsam::noiseModel::Base* noiseModel() const; +}; - typedef gtsam::BinaryMeasurement BinaryMeasurementUnit3; - typedef gtsam::BinaryMeasurement BinaryMeasurementRot3; +typedef gtsam::BinaryMeasurement BinaryMeasurementUnit3; +typedef gtsam::BinaryMeasurement BinaryMeasurementRot3; - class BinaryMeasurementsUnit3 - { - BinaryMeasurementsUnit3(); - size_t size() const; - gtsam::BinaryMeasurement at(size_t idx) const; - void push_back(const gtsam::BinaryMeasurement &measurement); - }; +class BinaryMeasurementsUnit3 { + BinaryMeasurementsUnit3(); + size_t size() const; + gtsam::BinaryMeasurement at(size_t idx) const; + void push_back(const gtsam::BinaryMeasurement& measurement); +}; #include - // TODO(frank): copy/pasta below until we have integer template paremeters in wrap! +// TODO(frank): copy/pasta below until we have integer template paremeters in wrap! - class ShonanAveragingParameters2 - { - ShonanAveragingParameters2(const gtsam::LevenbergMarquardtParams &lm); - ShonanAveragingParameters2(const gtsam::LevenbergMarquardtParams &lm, string method); - gtsam::LevenbergMarquardtParams getLMParams() const; - void setOptimalityThreshold(double value); - double getOptimalityThreshold() const; - void setAnchor(size_t index, const gtsam::Rot2 &value); - pair getAnchor(); - void setAnchorWeight(double value); - double getAnchorWeight() const; - void setKarcherWeight(double value); - double getKarcherWeight(); - void setGaugesWeight(double value); - double getGaugesWeight(); - }; +class ShonanAveragingParameters2 { + ShonanAveragingParameters2(const gtsam::LevenbergMarquardtParams& lm); + ShonanAveragingParameters2(const gtsam::LevenbergMarquardtParams& lm, string method); + gtsam::LevenbergMarquardtParams getLMParams() const; + void setOptimalityThreshold(double value); + double getOptimalityThreshold() const; + void setAnchor(size_t index, const gtsam::Rot2& value); + pair getAnchor(); + void setAnchorWeight(double value); + double getAnchorWeight() const; + void setKarcherWeight(double value); + double getKarcherWeight(); + void setGaugesWeight(double value); + double getGaugesWeight(); +}; - class ShonanAveragingParameters3 - { - ShonanAveragingParameters3(const gtsam::LevenbergMarquardtParams &lm); - ShonanAveragingParameters3(const gtsam::LevenbergMarquardtParams &lm, string method); - gtsam::LevenbergMarquardtParams getLMParams() const; - void setOptimalityThreshold(double value); - double getOptimalityThreshold() const; - void setAnchor(size_t index, const gtsam::Rot3 &value); - pair getAnchor(); - void setAnchorWeight(double value); - double getAnchorWeight() const; - void setKarcherWeight(double value); - double getKarcherWeight(); - void setGaugesWeight(double value); - double getGaugesWeight(); - }; +class ShonanAveragingParameters3 { + ShonanAveragingParameters3(const gtsam::LevenbergMarquardtParams& lm); + ShonanAveragingParameters3(const gtsam::LevenbergMarquardtParams& lm, string method); + gtsam::LevenbergMarquardtParams getLMParams() const; + void setOptimalityThreshold(double value); + double getOptimalityThreshold() const; + void setAnchor(size_t index, const gtsam::Rot3& value); + pair getAnchor(); + void setAnchorWeight(double value); + double getAnchorWeight() const; + void setKarcherWeight(double value); + double getKarcherWeight(); + void setGaugesWeight(double value); + double getGaugesWeight(); +}; - class ShonanAveraging2 - { - ShonanAveraging2(string g2oFile); - ShonanAveraging2(string g2oFile, - const gtsam::ShonanAveragingParameters2 ¶meters); +class ShonanAveraging2 { + ShonanAveraging2(string g2oFile); + ShonanAveraging2(string g2oFile, + const gtsam::ShonanAveragingParameters2 ¶meters); - // Query properties - size_t nrUnknowns() const; - size_t nrMeasurements() const; - gtsam::Rot2 measured(size_t i); - gtsam::KeyVector keys(size_t i); + // Query properties + size_t nrUnknowns() const; + size_t nrMeasurements() const; + gtsam::Rot2 measured(size_t i); + gtsam::KeyVector keys(size_t i); - // Matrix API (advanced use, debugging) - Matrix denseD() const; - Matrix denseQ() const; - Matrix denseL() const; - // Matrix computeLambda_(Matrix S) const; - Matrix computeLambda_(const gtsam::Values &values) const; - Matrix computeA_(const gtsam::Values &values) const; - double computeMinEigenValue(const gtsam::Values &values) const; - gtsam::Values initializeWithDescent(size_t p, const gtsam::Values &values, - const Vector &minEigenVector, double minEigenValue) const; + // Matrix API (advanced use, debugging) + Matrix denseD() const; + Matrix denseQ() const; + Matrix denseL() const; + // Matrix computeLambda_(Matrix S) const; + Matrix computeLambda_(const gtsam::Values& values) const; + Matrix computeA_(const gtsam::Values& values) const; + double computeMinEigenValue(const gtsam::Values& values) const; + gtsam::Values initializeWithDescent(size_t p, const gtsam::Values& values, + const Vector& minEigenVector, double minEigenValue) const; - // Advanced API - gtsam::NonlinearFactorGraph buildGraphAt(size_t p) const; - gtsam::Values initializeRandomlyAt(size_t p) const; - double costAt(size_t p, const gtsam::Values &values) const; - pair computeMinEigenVector(const gtsam::Values &values) const; - bool checkOptimality(const gtsam::Values &values) const; - gtsam::LevenbergMarquardtOptimizer *createOptimizerAt(size_t p, const gtsam::Values &initial); - // gtsam::Values tryOptimizingAt(size_t p) const; - gtsam::Values tryOptimizingAt(size_t p, const gtsam::Values &initial) const; - gtsam::Values projectFrom(size_t p, const gtsam::Values &values) const; - gtsam::Values roundSolution(const gtsam::Values &values) const; + // Advanced API + gtsam::NonlinearFactorGraph buildGraphAt(size_t p) const; + gtsam::Values initializeRandomlyAt(size_t p) const; + double costAt(size_t p, const gtsam::Values& values) const; + pair computeMinEigenVector(const gtsam::Values& values) const; + bool checkOptimality(const gtsam::Values& values) const; + gtsam::LevenbergMarquardtOptimizer* createOptimizerAt(size_t p, const gtsam::Values& initial); + // gtsam::Values tryOptimizingAt(size_t p) const; + gtsam::Values tryOptimizingAt(size_t p, const gtsam::Values& initial) const; + gtsam::Values projectFrom(size_t p, const gtsam::Values& values) const; + gtsam::Values roundSolution(const gtsam::Values& values) const; - // Basic API - double cost(const gtsam::Values &values) const; - gtsam::Values initializeRandomly() const; - pair run(const gtsam::Values &initial, size_t min_p, size_t max_p) const; - }; + // Basic API + double cost(const gtsam::Values& values) const; + gtsam::Values initializeRandomly() const; + pair run(const gtsam::Values& initial, size_t min_p, size_t max_p) const; +}; - class ShonanAveraging3 - { - ShonanAveraging3(string g2oFile); - ShonanAveraging3(string g2oFile, - const gtsam::ShonanAveragingParameters3 ¶meters); +class ShonanAveraging3 { + ShonanAveraging3(string g2oFile); + ShonanAveraging3(string g2oFile, + const gtsam::ShonanAveragingParameters3 ¶meters); + + // TODO(frank): deprecate once we land pybind wrapper + ShonanAveraging3(const gtsam::BetweenFactorPose3s &factors); + ShonanAveraging3(const gtsam::BetweenFactorPose3s &factors, + const gtsam::ShonanAveragingParameters3 ¶meters); - // TODO(frank): deprecate once we land pybind wrapper - ShonanAveraging3(const gtsam::BetweenFactorPose3s &factors); - ShonanAveraging3(const gtsam::BetweenFactorPose3s &factors, - const gtsam::ShonanAveragingParameters3 ¶meters); + // Query properties + size_t nrUnknowns() const; + size_t nrMeasurements() const; + gtsam::Rot3 measured(size_t i); + gtsam::KeyVector keys(size_t i); - // Query properties - size_t nrUnknowns() const; - size_t nrMeasurements() const; - gtsam::Rot3 measured(size_t i); - gtsam::KeyVector keys(size_t i); + // Matrix API (advanced use, debugging) + Matrix denseD() const; + Matrix denseQ() const; + Matrix denseL() const; + // Matrix computeLambda_(Matrix S) const; + Matrix computeLambda_(const gtsam::Values& values) const; + Matrix computeA_(const gtsam::Values& values) const; + double computeMinEigenValue(const gtsam::Values& values) const; + gtsam::Values initializeWithDescent(size_t p, const gtsam::Values& values, + const Vector& minEigenVector, double minEigenValue) const; - // Matrix API (advanced use, debugging) - Matrix denseD() const; - Matrix denseQ() const; - Matrix denseL() const; - // Matrix computeLambda_(Matrix S) const; - Matrix computeLambda_(const gtsam::Values &values) const; - Matrix computeA_(const gtsam::Values &values) const; - double computeMinEigenValue(const gtsam::Values &values) const; - gtsam::Values initializeWithDescent(size_t p, const gtsam::Values &values, - const Vector &minEigenVector, double minEigenValue) const; + // Advanced API + gtsam::NonlinearFactorGraph buildGraphAt(size_t p) const; + gtsam::Values initializeRandomlyAt(size_t p) const; + double costAt(size_t p, const gtsam::Values& values) const; + pair computeMinEigenVector(const gtsam::Values& values) const; + bool checkOptimality(const gtsam::Values& values) const; + gtsam::LevenbergMarquardtOptimizer* createOptimizerAt(size_t p, const gtsam::Values& initial); + // gtsam::Values tryOptimizingAt(size_t p) const; + gtsam::Values tryOptimizingAt(size_t p, const gtsam::Values& initial) const; + gtsam::Values projectFrom(size_t p, const gtsam::Values& values) const; + gtsam::Values roundSolution(const gtsam::Values& values) const; - // Advanced API - gtsam::NonlinearFactorGraph buildGraphAt(size_t p) const; - gtsam::Values initializeRandomlyAt(size_t p) const; - double costAt(size_t p, const gtsam::Values &values) const; - pair computeMinEigenVector(const gtsam::Values &values) const; - bool checkOptimality(const gtsam::Values &values) const; - gtsam::LevenbergMarquardtOptimizer *createOptimizerAt(size_t p, const gtsam::Values &initial); - // gtsam::Values tryOptimizingAt(size_t p) const; - gtsam::Values tryOptimizingAt(size_t p, const gtsam::Values &initial) const; - gtsam::Values projectFrom(size_t p, const gtsam::Values &values) const; - gtsam::Values roundSolution(const gtsam::Values &values) const; - - // Basic API - double cost(const gtsam::Values &values) const; - gtsam::Values initializeRandomly() const; - pair run(const gtsam::Values &initial, size_t min_p, size_t max_p) const; - }; + // Basic API + double cost(const gtsam::Values& values) const; + gtsam::Values initializeRandomly() const; + pair run(const gtsam::Values& initial, size_t min_p, size_t max_p) const; +}; #include - class KeyPairDoubleMap - { - KeyPairDoubleMap(); - KeyPairDoubleMap(const gtsam::KeyPairDoubleMap &other); +class KeyPairDoubleMap { + KeyPairDoubleMap(); + KeyPairDoubleMap(const gtsam::KeyPairDoubleMap& other); - size_t size() const; - bool empty() const; - void clear(); - size_t at(const pair &keypair) const; - }; + size_t size() const; + bool empty() const; + void clear(); + size_t at(const pair& keypair) const; +}; - class MFAS - { - MFAS(const gtsam::BinaryMeasurementsUnit3 &relativeTranslations, - const gtsam::Unit3 &projectionDirection); +class MFAS { + MFAS(const gtsam::BinaryMeasurementsUnit3& relativeTranslations, + const gtsam::Unit3& projectionDirection); - gtsam::KeyPairDoubleMap computeOutlierWeights() const; - gtsam::KeyVector computeOrdering() const; - }; + gtsam::KeyPairDoubleMap computeOutlierWeights() const; + gtsam::KeyVector computeOrdering() const; +}; #include - class TranslationRecovery - { - TranslationRecovery(const gtsam::BinaryMeasurementsUnit3 &relativeTranslations, - const gtsam::LevenbergMarquardtParams &lmParams); - TranslationRecovery( - const gtsam::BinaryMeasurementsUnit3 &relativeTranslations); // default LevenbergMarquardtParams - gtsam::Values run(const double scale) const; - gtsam::Values run() const; // default scale = 1.0 - }; +class TranslationRecovery { + TranslationRecovery(const gtsam::BinaryMeasurementsUnit3 &relativeTranslations, + const gtsam::LevenbergMarquardtParams &lmParams); + TranslationRecovery( + const gtsam::BinaryMeasurementsUnit3 & relativeTranslations); // default LevenbergMarquardtParams + gtsam::Values run(const double scale) const; + gtsam::Values run() const; // default scale = 1.0 +}; - //************************************************************************* - // Navigation - //************************************************************************* - namespace imuBias - { +//************************************************************************* +// Navigation +//************************************************************************* +namespace imuBias { #include - class ConstantBias - { - // Constructors - ConstantBias(); - ConstantBias(Vector biasAcc, Vector biasGyro); +class ConstantBias { + // Constructors + ConstantBias(); + ConstantBias(Vector biasAcc, Vector biasGyro); - // Testable - void print(string s) const; - bool equals(const gtsam::imuBias::ConstantBias &expected, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::imuBias::ConstantBias& expected, double tol) const; - // Group - static gtsam::imuBias::ConstantBias identity(); - gtsam::imuBias::ConstantBias inverse() const; - gtsam::imuBias::ConstantBias compose(const gtsam::imuBias::ConstantBias &b) const; - gtsam::imuBias::ConstantBias between(const gtsam::imuBias::ConstantBias &b) const; + // Group + static gtsam::imuBias::ConstantBias identity(); + gtsam::imuBias::ConstantBias inverse() const; + gtsam::imuBias::ConstantBias compose(const gtsam::imuBias::ConstantBias& b) const; + gtsam::imuBias::ConstantBias between(const gtsam::imuBias::ConstantBias& b) const; - // Manifold - gtsam::imuBias::ConstantBias retract(Vector v) const; - Vector localCoordinates(const gtsam::imuBias::ConstantBias &b) const; + // Manifold + gtsam::imuBias::ConstantBias retract(Vector v) const; + Vector localCoordinates(const gtsam::imuBias::ConstantBias& b) const; - // Lie Group - static gtsam::imuBias::ConstantBias Expmap(Vector v); - static Vector Logmap(const gtsam::imuBias::ConstantBias &b); + // Lie Group + static gtsam::imuBias::ConstantBias Expmap(Vector v); + static Vector Logmap(const gtsam::imuBias::ConstantBias& b); - // Standard Interface - Vector vector() const; - Vector accelerometer() const; - Vector gyroscope() const; - Vector correctAccelerometer(Vector measurement) const; - Vector correctGyroscope(Vector measurement) const; - }; + // Standard Interface + Vector vector() const; + Vector accelerometer() const; + Vector gyroscope() const; + Vector correctAccelerometer(Vector measurement) const; + Vector correctGyroscope(Vector measurement) const; +}; - } // namespace imuBias +}///\namespace imuBias #include - class NavState - { - // Constructors - NavState(); - NavState(const gtsam::Rot3 &R, const gtsam::Point3 &t, Vector v); - NavState(const gtsam::Pose3 &pose, Vector v); +class NavState { + // Constructors + NavState(); + NavState(const gtsam::Rot3& R, const gtsam::Point3& t, Vector v); + NavState(const gtsam::Pose3& pose, Vector v); - // Testable - void print(string s) const; - bool equals(const gtsam::NavState &expected, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::NavState& expected, double tol) const; - // Access - gtsam::Rot3 attitude() const; - gtsam::Point3 position() const; - Vector velocity() const; - gtsam::Pose3 pose() const; - }; + // Access + gtsam::Rot3 attitude() const; + gtsam::Point3 position() const; + Vector velocity() const; + gtsam::Pose3 pose() const; +}; #include - virtual class PreintegratedRotationParams - { - PreintegratedRotationParams(); +virtual class PreintegratedRotationParams { + PreintegratedRotationParams(); - // Testable - void print(string s) const; - bool equals(const gtsam::PreintegratedRotationParams &expected, double tol); + // Testable + void print(string s) const; + bool equals(const gtsam::PreintegratedRotationParams& expected, double tol); - void setGyroscopeCovariance(Matrix cov); - void setOmegaCoriolis(Vector omega); - void setBodyPSensor(const gtsam::Pose3 &pose); + void setGyroscopeCovariance(Matrix cov); + void setOmegaCoriolis(Vector omega); + void setBodyPSensor(const gtsam::Pose3& pose); - Matrix getGyroscopeCovariance() const; + Matrix getGyroscopeCovariance() const; - // TODO(frank): allow optional - // boost::optional getOmegaCoriolis() const; - // boost::optional getBodyPSensor() const; - }; + // TODO(frank): allow optional + // boost::optional getOmegaCoriolis() const; + // boost::optional getBodyPSensor() const; +}; #include - virtual class PreintegrationParams : gtsam::PreintegratedRotationParams - { - PreintegrationParams(Vector n_gravity); +virtual class PreintegrationParams : gtsam::PreintegratedRotationParams { + PreintegrationParams(Vector n_gravity); - static gtsam::PreintegrationParams *MakeSharedD(double g); - static gtsam::PreintegrationParams *MakeSharedU(double g); - static gtsam::PreintegrationParams *MakeSharedD(); // default g = 9.81 - static gtsam::PreintegrationParams *MakeSharedU(); // default g = 9.81 + static gtsam::PreintegrationParams* MakeSharedD(double g); + static gtsam::PreintegrationParams* MakeSharedU(double g); + static gtsam::PreintegrationParams* MakeSharedD(); // default g = 9.81 + static gtsam::PreintegrationParams* MakeSharedU(); // default g = 9.81 - // Testable - void print(string s) const; - bool equals(const gtsam::PreintegrationParams &expected, double tol); + // Testable + void print(string s) const; + bool equals(const gtsam::PreintegrationParams& expected, double tol); - void setAccelerometerCovariance(Matrix cov); - void setIntegrationCovariance(Matrix cov); - void setUse2ndOrderCoriolis(bool flag); + void setAccelerometerCovariance(Matrix cov); + void setIntegrationCovariance(Matrix cov); + void setUse2ndOrderCoriolis(bool flag); - Matrix getAccelerometerCovariance() const; - Matrix getIntegrationCovariance() const; - bool getUse2ndOrderCoriolis() const; - }; + Matrix getAccelerometerCovariance() const; + Matrix getIntegrationCovariance() const; + bool getUse2ndOrderCoriolis() const; +}; #include - class PreintegratedImuMeasurements - { - // Constructors - PreintegratedImuMeasurements(const gtsam::PreintegrationParams *params); - PreintegratedImuMeasurements(const gtsam::PreintegrationParams *params, - const gtsam::imuBias::ConstantBias &bias); +class PreintegratedImuMeasurements { + // Constructors + PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params); + PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params, + const gtsam::imuBias::ConstantBias& bias); - // Testable - void print(string s) const; - bool equals(const gtsam::PreintegratedImuMeasurements &expected, double tol); + // Testable + void print(string s) const; + bool equals(const gtsam::PreintegratedImuMeasurements& expected, double tol); - // Standard Interface - void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, - double deltaT); - void resetIntegration(); - void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias &biasHat); + // Standard Interface + void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, + double deltaT); + void resetIntegration(); + void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat); - Matrix preintMeasCov() const; - Vector preintegrated() const; - double deltaTij() const; - gtsam::Rot3 deltaRij() const; - Vector deltaPij() const; - Vector deltaVij() const; - gtsam::imuBias::ConstantBias biasHat() const; - Vector biasHatVector() const; - gtsam::NavState predict(const gtsam::NavState &state_i, - const gtsam::imuBias::ConstantBias &bias) const; - }; + Matrix preintMeasCov() const; + Vector preintegrated() const; + double deltaTij() const; + gtsam::Rot3 deltaRij() const; + Vector deltaPij() const; + Vector deltaVij() const; + gtsam::imuBias::ConstantBias biasHat() const; + Vector biasHatVector() const; + gtsam::NavState predict(const gtsam::NavState& state_i, + const gtsam::imuBias::ConstantBias& bias) const; +}; - virtual class ImuFactor : gtsam::NonlinearFactor - { - ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, - size_t bias, - const gtsam::PreintegratedImuMeasurements &preintegratedMeasurements); +virtual class ImuFactor: gtsam::NonlinearFactor { + ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, + size_t bias, + const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements); - // Standard Interface - gtsam::PreintegratedImuMeasurements preintegratedMeasurements() const; - Vector evaluateError(const gtsam::Pose3 &pose_i, Vector vel_i, - const gtsam::Pose3 &pose_j, Vector vel_j, - const gtsam::imuBias::ConstantBias &bias); - }; + // Standard Interface + gtsam::PreintegratedImuMeasurements preintegratedMeasurements() const; + Vector evaluateError(const gtsam::Pose3& pose_i, Vector vel_i, + const gtsam::Pose3& pose_j, Vector vel_j, + const gtsam::imuBias::ConstantBias& bias); +}; #include - virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams - { - PreintegrationCombinedParams(Vector n_gravity); +virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams { + PreintegrationCombinedParams(Vector n_gravity); - static gtsam::PreintegrationCombinedParams *MakeSharedD(double g); - static gtsam::PreintegrationCombinedParams *MakeSharedU(double g); - static gtsam::PreintegrationCombinedParams *MakeSharedD(); // default g = 9.81 - static gtsam::PreintegrationCombinedParams *MakeSharedU(); // default g = 9.81 + static gtsam::PreintegrationCombinedParams* MakeSharedD(double g); + static gtsam::PreintegrationCombinedParams* MakeSharedU(double g); + static gtsam::PreintegrationCombinedParams* MakeSharedD(); // default g = 9.81 + static gtsam::PreintegrationCombinedParams* MakeSharedU(); // default g = 9.81 - // Testable - void print(string s) const; - bool equals(const gtsam::PreintegrationCombinedParams &expected, double tol); + // Testable + void print(string s) const; + bool equals(const gtsam::PreintegrationCombinedParams& expected, double tol); - void setBiasAccCovariance(Matrix cov); - void setBiasOmegaCovariance(Matrix cov); - void setBiasAccOmegaInt(Matrix cov); + void setBiasAccCovariance(Matrix cov); + void setBiasOmegaCovariance(Matrix cov); + void setBiasAccOmegaInt(Matrix cov); + + Matrix getBiasAccCovariance() const ; + Matrix getBiasOmegaCovariance() const ; + Matrix getBiasAccOmegaInt() const; + +}; - Matrix getBiasAccCovariance() const; - Matrix getBiasOmegaCovariance() const; - Matrix getBiasAccOmegaInt() const; - }; +class PreintegratedCombinedMeasurements { +// Constructors + PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams* params); + PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams* params, + const gtsam::imuBias::ConstantBias& bias); + // Testable + void print(string s) const; + bool equals(const gtsam::PreintegratedCombinedMeasurements& expected, + double tol); - class PreintegratedCombinedMeasurements - { - // Constructors - PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams *params); - PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams *params, - const gtsam::imuBias::ConstantBias &bias); - // Testable - void print(string s) const; - bool equals(const gtsam::PreintegratedCombinedMeasurements &expected, - double tol); + // Standard Interface + void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, + double deltaT); + void resetIntegration(); + void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat); - // Standard Interface - void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, - double deltaT); - void resetIntegration(); - void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias &biasHat); + Matrix preintMeasCov() const; + double deltaTij() const; + gtsam::Rot3 deltaRij() const; + Vector deltaPij() const; + Vector deltaVij() const; + gtsam::imuBias::ConstantBias biasHat() const; + Vector biasHatVector() const; + gtsam::NavState predict(const gtsam::NavState& state_i, + const gtsam::imuBias::ConstantBias& bias) const; +}; - Matrix preintMeasCov() const; - double deltaTij() const; - gtsam::Rot3 deltaRij() const; - Vector deltaPij() const; - Vector deltaVij() const; - gtsam::imuBias::ConstantBias biasHat() const; - Vector biasHatVector() const; - gtsam::NavState predict(const gtsam::NavState &state_i, - const gtsam::imuBias::ConstantBias &bias) const; - }; +virtual class CombinedImuFactor: gtsam::NonlinearFactor { + CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, + size_t bias_i, size_t bias_j, + const gtsam::PreintegratedCombinedMeasurements& CombinedPreintegratedMeasurements); - virtual class CombinedImuFactor : gtsam::NonlinearFactor - { - CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, - size_t bias_i, size_t bias_j, - const gtsam::PreintegratedCombinedMeasurements &CombinedPreintegratedMeasurements); - - // Standard Interface - gtsam::PreintegratedCombinedMeasurements preintegratedMeasurements() const; - Vector evaluateError(const gtsam::Pose3 &pose_i, Vector vel_i, - const gtsam::Pose3 &pose_j, Vector vel_j, - const gtsam::imuBias::ConstantBias &bias_i, - const gtsam::imuBias::ConstantBias &bias_j); - }; + // Standard Interface + gtsam::PreintegratedCombinedMeasurements preintegratedMeasurements() const; + Vector evaluateError(const gtsam::Pose3& pose_i, Vector vel_i, + const gtsam::Pose3& pose_j, Vector vel_j, + const gtsam::imuBias::ConstantBias& bias_i, + const gtsam::imuBias::ConstantBias& bias_j); +}; #include - class PreintegratedAhrsMeasurements - { - // Standard Constructor - PreintegratedAhrsMeasurements(Vector bias, Matrix measuredOmegaCovariance); - PreintegratedAhrsMeasurements(const gtsam::PreintegratedAhrsMeasurements &rhs); +class PreintegratedAhrsMeasurements { + // Standard Constructor + PreintegratedAhrsMeasurements(Vector bias, Matrix measuredOmegaCovariance); + PreintegratedAhrsMeasurements(const gtsam::PreintegratedAhrsMeasurements& rhs); - // Testable - void print(string s) const; - bool equals(const gtsam::PreintegratedAhrsMeasurements &expected, double tol); + // Testable + void print(string s) const; + bool equals(const gtsam::PreintegratedAhrsMeasurements& expected, double tol); - // get Data - gtsam::Rot3 deltaRij() const; - double deltaTij() const; - Vector biasHat() const; + // get Data + gtsam::Rot3 deltaRij() const; + double deltaTij() const; + Vector biasHat() const; - // Standard Interface - void integrateMeasurement(Vector measuredOmega, double deltaT); - void resetIntegration(); - }; + // Standard Interface + void integrateMeasurement(Vector measuredOmega, double deltaT); + void resetIntegration() ; +}; - virtual class AHRSFactor : gtsam::NonlinearFactor - { - AHRSFactor(size_t rot_i, size_t rot_j, size_t bias, - const gtsam::PreintegratedAhrsMeasurements &preintegratedMeasurements, Vector omegaCoriolis); - AHRSFactor(size_t rot_i, size_t rot_j, size_t bias, - const gtsam::PreintegratedAhrsMeasurements &preintegratedMeasurements, Vector omegaCoriolis, - const gtsam::Pose3 &body_P_sensor); +virtual class AHRSFactor : gtsam::NonlinearFactor { + AHRSFactor(size_t rot_i, size_t rot_j,size_t bias, + const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis); + AHRSFactor(size_t rot_i, size_t rot_j, size_t bias, + const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis, + const gtsam::Pose3& body_P_sensor); - // Standard Interface - gtsam::PreintegratedAhrsMeasurements preintegratedMeasurements() const; - Vector evaluateError(const gtsam::Rot3 &rot_i, const gtsam::Rot3 &rot_j, - Vector bias) const; - gtsam::Rot3 predict(const gtsam::Rot3 &rot_i, Vector bias, - const gtsam::PreintegratedAhrsMeasurements &preintegratedMeasurements, - Vector omegaCoriolis) const; - }; + // Standard Interface + gtsam::PreintegratedAhrsMeasurements preintegratedMeasurements() const; + Vector evaluateError(const gtsam::Rot3& rot_i, const gtsam::Rot3& rot_j, + Vector bias) const; + gtsam::Rot3 predict(const gtsam::Rot3& rot_i, Vector bias, + const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, + Vector omegaCoriolis) const; +}; #include - //virtual class AttitudeFactor : gtsam::NonlinearFactor { - // AttitudeFactor(const Unit3& nZ, const Unit3& bRef); - // AttitudeFactor(); - //}; - virtual class Rot3AttitudeFactor : gtsam::NonlinearFactor - { - Rot3AttitudeFactor(size_t key, const gtsam::Unit3 &nZ, const gtsam::noiseModel::Diagonal *model, - const gtsam::Unit3 &bRef); - Rot3AttitudeFactor(size_t key, const gtsam::Unit3 &nZ, const gtsam::noiseModel::Diagonal *model); - Rot3AttitudeFactor(); - void print(string s) const; - bool equals(const gtsam::NonlinearFactor &expected, double tol) const; - gtsam::Unit3 nZ() const; - gtsam::Unit3 bRef() const; - }; +//virtual class AttitudeFactor : gtsam::NonlinearFactor { +// AttitudeFactor(const Unit3& nZ, const Unit3& bRef); +// AttitudeFactor(); +//}; +virtual class Rot3AttitudeFactor : gtsam::NonlinearFactor{ + Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model, + const gtsam::Unit3& bRef); + Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model); + Rot3AttitudeFactor(); + void print(string s) const; + bool equals(const gtsam::NonlinearFactor& expected, double tol) const; + gtsam::Unit3 nZ() const; + gtsam::Unit3 bRef() const; +}; - virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor - { - Pose3AttitudeFactor(size_t key, const gtsam::Unit3 &nZ, - const gtsam::noiseModel::Diagonal *model, - const gtsam::Unit3 &bRef); - Pose3AttitudeFactor(size_t key, const gtsam::Unit3 &nZ, - const gtsam::noiseModel::Diagonal *model); - Pose3AttitudeFactor(); - void print(string s) const; - bool equals(const gtsam::NonlinearFactor &expected, double tol) const; - gtsam::Unit3 nZ() const; - gtsam::Unit3 bRef() const; - }; +virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor { + Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, + const gtsam::noiseModel::Diagonal* model, + const gtsam::Unit3& bRef); + Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, + const gtsam::noiseModel::Diagonal* model); + Pose3AttitudeFactor(); + void print(string s) const; + bool equals(const gtsam::NonlinearFactor& expected, double tol) const; + gtsam::Unit3 nZ() const; + gtsam::Unit3 bRef() const; +}; #include - virtual class GPSFactor : gtsam::NonlinearFactor - { - GPSFactor(size_t key, const gtsam::Point3 &gpsIn, - const gtsam::noiseModel::Base *model); +virtual class GPSFactor : gtsam::NonlinearFactor{ + GPSFactor(size_t key, const gtsam::Point3& gpsIn, + const gtsam::noiseModel::Base* model); - // Testable - void print(string s) const; - bool equals(const gtsam::GPSFactor &expected, double tol); + // Testable + void print(string s) const; + bool equals(const gtsam::GPSFactor& expected, double tol); - // Standard Interface - gtsam::Point3 measurementIn() const; - }; + // Standard Interface + gtsam::Point3 measurementIn() const; +}; - virtual class GPSFactor2 : gtsam::NonlinearFactor - { - GPSFactor2(size_t key, const gtsam::Point3 &gpsIn, - const gtsam::noiseModel::Base *model); +virtual class GPSFactor2 : gtsam::NonlinearFactor { + GPSFactor2(size_t key, const gtsam::Point3& gpsIn, + const gtsam::noiseModel::Base* model); - // Testable - void print(string s) const; - bool equals(const gtsam::GPSFactor2 &expected, double tol); + // Testable + void print(string s) const; + bool equals(const gtsam::GPSFactor2& expected, double tol); - // Standard Interface - gtsam::Point3 measurementIn() const; - }; + // Standard Interface + gtsam::Point3 measurementIn() const; +}; #include - virtual class Scenario - { - gtsam::Pose3 pose(double t) const; - Vector omega_b(double t) const; - Vector velocity_n(double t) const; - Vector acceleration_n(double t) const; - gtsam::Rot3 rotation(double t) const; - gtsam::NavState navState(double t) const; - Vector velocity_b(double t) const; - Vector acceleration_b(double t) const; - }; +virtual class Scenario { + gtsam::Pose3 pose(double t) const; + Vector omega_b(double t) const; + Vector velocity_n(double t) const; + Vector acceleration_n(double t) const; + gtsam::Rot3 rotation(double t) const; + gtsam::NavState navState(double t) const; + Vector velocity_b(double t) const; + Vector acceleration_b(double t) const; +}; - virtual class ConstantTwistScenario : gtsam::Scenario - { - ConstantTwistScenario(Vector w, Vector v); - ConstantTwistScenario(Vector w, Vector v, - const gtsam::Pose3 &nTb0); - }; +virtual class ConstantTwistScenario : gtsam::Scenario { + ConstantTwistScenario(Vector w, Vector v); + ConstantTwistScenario(Vector w, Vector v, + const gtsam::Pose3& nTb0); +}; - virtual class AcceleratingScenario : gtsam::Scenario - { - AcceleratingScenario(const gtsam::Rot3 &nRb, const gtsam::Point3 &p0, - Vector v0, Vector a_n, - Vector omega_b); - }; +virtual class AcceleratingScenario : gtsam::Scenario { + AcceleratingScenario(const gtsam::Rot3& nRb, const gtsam::Point3& p0, + Vector v0, Vector a_n, + Vector omega_b); +}; #include - class ScenarioRunner - { - ScenarioRunner(const gtsam::Scenario &scenario, - const gtsam::PreintegrationParams *p, - double imuSampleTime, - const gtsam::imuBias::ConstantBias &bias); - Vector gravity_n() const; - Vector actualAngularVelocity(double t) const; - Vector actualSpecificForce(double t) const; - Vector measuredAngularVelocity(double t) const; - Vector measuredSpecificForce(double t) const; - double imuSampleTime() const; - gtsam::PreintegratedImuMeasurements integrate( - double T, const gtsam::imuBias::ConstantBias &estimatedBias, - bool corrupted) const; - gtsam::NavState predict( - const gtsam::PreintegratedImuMeasurements &pim, - const gtsam::imuBias::ConstantBias &estimatedBias) const; - Matrix estimateCovariance( - double T, size_t N, - const gtsam::imuBias::ConstantBias &estimatedBias) const; - Matrix estimateNoiseCovariance(size_t N) const; - }; +class ScenarioRunner { + ScenarioRunner(const gtsam::Scenario& scenario, + const gtsam::PreintegrationParams* p, + double imuSampleTime, + const gtsam::imuBias::ConstantBias& bias); + Vector gravity_n() const; + Vector actualAngularVelocity(double t) const; + Vector actualSpecificForce(double t) const; + Vector measuredAngularVelocity(double t) const; + Vector measuredSpecificForce(double t) const; + double imuSampleTime() const; + gtsam::PreintegratedImuMeasurements integrate( + double T, const gtsam::imuBias::ConstantBias& estimatedBias, + bool corrupted) const; + gtsam::NavState predict( + const gtsam::PreintegratedImuMeasurements& pim, + const gtsam::imuBias::ConstantBias& estimatedBias) const; + Matrix estimateCovariance( + double T, size_t N, + const gtsam::imuBias::ConstantBias& estimatedBias) const; + Matrix estimateNoiseCovariance(size_t N) const; +}; - //************************************************************************* - // Utilities - //************************************************************************* +//************************************************************************* +// Utilities +//************************************************************************* - namespace utilities - { +namespace utilities { + + #include + gtsam::KeyList createKeyList(Vector I); + gtsam::KeyList createKeyList(string s, Vector I); + gtsam::KeyVector createKeyVector(Vector I); + gtsam::KeyVector createKeyVector(string s, Vector I); + gtsam::KeySet createKeySet(Vector I); + gtsam::KeySet createKeySet(string s, Vector I); + Matrix extractPoint2(const gtsam::Values& values); + Matrix extractPoint3(const gtsam::Values& values); + gtsam::Values allPose2s(gtsam::Values& values); + Matrix extractPose2(const gtsam::Values& values); + gtsam::Values allPose3s(gtsam::Values& values); + Matrix extractPose3(const gtsam::Values& values); + void perturbPoint2(gtsam::Values& values, double sigma, int seed); + void perturbPose2 (gtsam::Values& values, double sigmaT, double sigmaR, int seed); + void perturbPoint3(gtsam::Values& values, double sigma, int seed); + void insertBackprojections(gtsam::Values& values, const gtsam::PinholeCameraCal3_S2& c, Vector J, Matrix Z, double depth); + void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K); + void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K, const gtsam::Pose3& body_P_sensor); + Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values); + gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base); + gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base, const gtsam::KeyVector& keys); + +} //\namespace utilities #include - gtsam::KeyList createKeyList(Vector I); - gtsam::KeyList createKeyList(string s, Vector I); - gtsam::KeyVector createKeyVector(Vector I); - gtsam::KeyVector createKeyVector(string s, Vector I); - gtsam::KeySet createKeySet(Vector I); - gtsam::KeySet createKeySet(string s, Vector I); - Matrix extractPoint2(const gtsam::Values &values); - Matrix extractPoint3(const gtsam::Values &values); - gtsam::Values allPose2s(gtsam::Values &values); - Matrix extractPose2(const gtsam::Values &values); - gtsam::Values allPose3s(gtsam::Values &values); - Matrix extractPose3(const gtsam::Values &values); - void perturbPoint2(gtsam::Values &values, double sigma, int seed); - void perturbPose2(gtsam::Values &values, double sigmaT, double sigmaR, int seed); - void perturbPoint3(gtsam::Values &values, double sigma, int seed); - void insertBackprojections(gtsam::Values &values, const gtsam::PinholeCameraCal3_S2 &c, Vector J, Matrix Z, double depth); - void insertProjectionFactors(gtsam::NonlinearFactorGraph &graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base *model, const gtsam::Cal3_S2 *K); - void insertProjectionFactors(gtsam::NonlinearFactorGraph &graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base *model, const gtsam::Cal3_S2 *K, const gtsam::Pose3 &body_P_sensor); - Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph &graph, const gtsam::Values &values); - gtsam::Values localToWorld(const gtsam::Values &local, const gtsam::Pose2 &base); - gtsam::Values localToWorld(const gtsam::Values &local, const gtsam::Pose2 &base, const gtsam::KeyVector &keys); +class RedirectCout { + RedirectCout(); + string str(); +}; - } // namespace utilities - -#include - class RedirectCout - { - RedirectCout(); - string str(); - }; - -} // namespace gtsam +} diff --git a/python/gtsam/preamble.h b/python/gtsam/preamble.h index fa98cd171..c8a577431 100644 --- a/python/gtsam/preamble.h +++ b/python/gtsam/preamble.h @@ -5,10 +5,10 @@ PYBIND11_MAKE_OPAQUE(std::vector>); #else PYBIND11_MAKE_OPAQUE(std::vector); #endif -PYBIND11_MAKE_OPAQUE(std::vector>); +PYBIND11_MAKE_OPAQUE(std::vector >); PYBIND11_MAKE_OPAQUE(std::vector); -PYBIND11_MAKE_OPAQUE(std::vector>>); -PYBIND11_MAKE_OPAQUE(std::vector>>); +PYBIND11_MAKE_OPAQUE(std::vector > >); +PYBIND11_MAKE_OPAQUE(std::vector > >); PYBIND11_MAKE_OPAQUE(std::vector); -PYBIND11_MAKE_OPAQUE(gtsam::CameraSet>); -PYBIND11_MAKE_OPAQUE(gtsam::CameraSet>); \ No newline at end of file +PYBIND11_MAKE_OPAQUE(gtsam::CameraSet >); +PYBIND11_MAKE_OPAQUE(gtsam::CameraSet >); \ No newline at end of file diff --git a/python/gtsam/specializations.h b/python/gtsam/specializations.h index 63694f6f4..431697aac 100644 --- a/python/gtsam/specializations.h +++ b/python/gtsam/specializations.h @@ -1,17 +1,17 @@ // Please refer to: https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html // These are required to save one copy operation on Python calls #ifdef GTSAM_ALLOCATOR_TBB -py::bind_vector>>(m_, "KeyVector"); +py::bind_vector > >(m_, "KeyVector"); #else -py::bind_vector>(m_, "KeyVector"); +py::bind_vector >(m_, "KeyVector"); #endif -py::bind_vector>>(m_, "Point2Vector"); -py::bind_vector>(m_, "Pose3Vector"); -py::bind_vector>>>(m_, "BetweenFactorPose3s"); -py::bind_vector>>>(m_, "BetweenFactorPose2s"); -py::bind_vector>>(m_, "BinaryMeasurementsUnit3"); +py::bind_vector > >(m_, "Point2Vector"); +py::bind_vector >(m_, "Pose3Vector"); +py::bind_vector > > >(m_, "BetweenFactorPose3s"); +py::bind_vector > > >(m_, "BetweenFactorPose2s"); +py::bind_vector > >(m_, "BinaryMeasurementsUnit3"); py::bind_map(m_, "IndexPairSetMap"); py::bind_vector(m_, "IndexPairVector"); py::bind_map(m_, "KeyPairDoubleMap"); -py::bind_vector>>(m_, "CameraSetCal3_S2"); -py::bind_vector>>(m_, "CameraSetCal3Bundler"); +py::bind_vector > >(m_, "CameraSetCal3_S2"); +py::bind_vector > >(m_, "CameraSetCal3Bundler"); diff --git a/python/gtsam/tests/test_Triangulation.py b/python/gtsam/tests/test_Triangulation.py index 901aad0b9..574452afa 100644 --- a/python/gtsam/tests/test_Triangulation.py +++ b/python/gtsam/tests/test_Triangulation.py @@ -14,10 +14,11 @@ import numpy as np import gtsam as g from gtsam.utils.test_case import GtsamTestCase -from gtsam import Cal3_S2, Cal3Bundler, CameraSetCal3_S2,\ - CameraSetCal3Bundler, PinholeCameraCal3_S2, PinholeCameraCal3Bundler, \ - Point3, Pose3, Point2Vector, Pose3Vector, Rot3, triangulatePoint3 - +from gtsam import Cal3_S2, Cal3Bundler, Rot3, Pose3, \ + PinholeCameraCal3_S2, PinholeCameraCal3Bundler, Point3, \ + Point2Vector, Pose3Vector, triangulatePoint3, \ + CameraSetCal3_S2, CameraSetCal3Bundler +from numpy.core.records import array class TestVisualISAMExample(GtsamTestCase): """ Tests for triangulation with shared and individual calibrations """