From 8be6890b206c761bb1f334f4f7ab6fcfa7ccbff0 Mon Sep 17 00:00:00 2001 From: Sushmita Date: Thu, 3 Dec 2020 21:10:10 -0500 Subject: [PATCH] code formatted --- gtsam/geometry/triangulation.h | 702 +-- gtsam/gtsam.i | 5372 +++++++++++----------- python/gtsam/preamble.h | 10 +- python/gtsam/specializations.h | 18 +- python/gtsam/tests/test_Triangulation.py | 14 +- 5 files changed, 3155 insertions(+), 2961 deletions(-) diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 1df9efd22..e22b35e56 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -27,49 +27,52 @@ #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!) @@ -78,27 +81,29 @@ GTSAM_EXPORT Point3 triangulateDLT( * @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); + 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); } - 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) @@ -107,35 +112,37 @@ std::pair triangulationGraph( * @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); + 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); } - 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 @@ -143,63 +150,69 @@ GTSAM_EXPORT Point3 optimize(const NonlinearFactorGraph& graph, * @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); - } -private: - const Matrix3 K_; -public: - GTSAM_MAKE_ALIGNED_OPERATOR_NEW -}; + 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 + }; + + /** * 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 @@ -211,43 +224,45 @@ public: * @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 @@ -259,72 +274,76 @@ Point3 triangulatePoint3(const std::vector& poses, * @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 @@ -332,173 +351,194 @@ struct GTSAM_EXPORT TriangulationParameters { * @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) { - } + 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; - } + // 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: + 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); + } + }; - /// 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: + 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() {} + 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(); + 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; } -} -// Vector of Cameras - used by the Python/MATLAB wrapper -using CameraSetCal3Bundler = CameraSet>; -using CameraSetCal3_S2 = CameraSet>; + private: + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE &ar, const unsigned int version) + { + ar &BOOST_SERIALIZATION_NVP(status_); + } + }; -} // \namespace gtsam + /// 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(); + } + } + + // Vector of Cameras - used by the Python/MATLAB wrapper + using CameraSetCal3Bundler = CameraSet>; + using CameraSetCal3_S2 = CameraSet>; + +} // namespace gtsam diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 127a6fe45..1ab2425ec 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -118,1140 +118,1176 @@ * - 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 { - // Standard Constructors - Cal3_S2(); - Cal3_S2(double fx, double fy, double s, double u0, double v0); - Cal3_S2(Vector v); - Cal3_S2(double fov, int w, int h); + class Cal3_S2 + { + // Standard Constructors + Cal3_S2(); + Cal3_S2(double fx, double fy, double s, double u0, double v0); + Cal3_S2(Vector v); + Cal3_S2(double fov, int w, int h); - // Testable - void print(string s) const; - bool equals(const gtsam::Cal3_S2& rhs, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::Cal3_S2 &rhs, double tol) const; - // Manifold - static size_t Dim(); - size_t dim() const; - gtsam::Cal3_S2 retract(Vector v) const; - Vector localCoordinates(const gtsam::Cal3_S2& c) const; + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::Cal3_S2 retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3_S2 &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 skew() const; - double px() const; - double py() const; - gtsam::Point2 principalPoint() const; - Vector vector() const; - Matrix K() const; - Matrix matrix() const; - Matrix matrix_inverse() const; + // Standard Interface + double fx() const; + double fy() const; + double skew() const; + double px() const; + double py() const; + gtsam::Point2 principalPoint() const; + Vector vector() const; + Matrix K() const; + Matrix matrix() const; + Matrix matrix_inverse() const; - // enabling serialization functionality - void serialize() const; -}; + // enabling serialization functionality + void serialize() const; + }; #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(); + template + class CameraSet + { + CameraSet(); + // structure specific methods + T at(size_t i) const; + void push_back(const T &cam); + }; - // // common STL methods - // size_t size() const; - // bool empty() const; - // void clear(); - - // structure specific methods - T at(size_t i) const; - void push_back(const T& cam); -}; - -// typedefs added here for shorter type name and to enforce uniformity in naming conventions -//typedef gtsam::CameraSet CameraSetCal3_S2; -//typedef gtsam::CameraSet CameraSetCal3Bundler; + // typedefs added here for shorter type name and to enforce uniformity in naming conventions + //typedef gtsam::CameraSet CameraSetCal3_S2; + //typedef gtsam::CameraSet CameraSetCal3Bundler; #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; @@ -1261,969 +1297,1019 @@ class SymbolicBayesTree { 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 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); + 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); + bool equals(gtsam::noiseModel::Base &expected, double tol); - // access to noise model - Matrix R() const; - Matrix information() const; - Matrix covariance() const; + // 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; + // Whitening operations + Vector whiten(Vector v) const; + Vector unwhiten(Vector v) const; + Matrix Whiten(Matrix H) const; - // enabling serialization functionality - void serializable() 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; + 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; + // access to noise model + Vector sigmas() const; + Vector invsigmas() const; + Vector precisions() const; - // enabling serialization functionality - void serializable() 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); + 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); + static gtsam::noiseModel::Constrained *All(size_t dim); + static gtsam::noiseModel::Constrained *All(size_t dim, double mu); - gtsam::noiseModel::Constrained* unit() const; + gtsam::noiseModel::Constrained *unit() const; - // enabling serialization functionality - void serializable() 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); + 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; + // access to noise model + double sigma() const; - // enabling serialization functionality - void serializable() const; -}; + // enabling serialization functionality + void serializable() const; + }; -virtual class Unit : gtsam::noiseModel::Isotropic { - static gtsam::noiseModel::Unit* Create(size_t dim); + virtual class Unit : gtsam::noiseModel::Isotropic + { + static gtsam::noiseModel::Unit *Create(size_t dim); - // enabling serialization functionality - void serializable() const; -}; + // enabling serialization functionality + void serializable() const; + }; -namespace mEstimator { -virtual class Base { - void print(string s) 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(); + virtual class Null : gtsam::noiseModel::mEstimator::Base + { + Null(); + static gtsam::noiseModel::mEstimator::Null *Create(); - // enabling serialization functionality - void serializable() const; + // enabling serialization functionality + void serializable() const; - double weight(double error) const; - double loss(double error) 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); + virtual class Fair : gtsam::noiseModel::mEstimator::Base + { + Fair(double c); + static gtsam::noiseModel::mEstimator::Fair *Create(double c); - // enabling serialization functionality - void serializable() const; + // enabling serialization functionality + void serializable() const; - double weight(double error) const; - double loss(double error) 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); + virtual class Huber : gtsam::noiseModel::mEstimator::Base + { + Huber(double k); + static gtsam::noiseModel::mEstimator::Huber *Create(double k); - // enabling serialization functionality - void serializable() const; + // enabling serialization functionality + void serializable() const; - double weight(double error) const; - double loss(double error) 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); + virtual class Cauchy : gtsam::noiseModel::mEstimator::Base + { + Cauchy(double k); + static gtsam::noiseModel::mEstimator::Cauchy *Create(double k); - // enabling serialization functionality - void serializable() const; + // enabling serialization functionality + void serializable() const; - double weight(double error) const; - double loss(double error) 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); + virtual class Tukey : gtsam::noiseModel::mEstimator::Base + { + Tukey(double k); + static gtsam::noiseModel::mEstimator::Tukey *Create(double k); - // enabling serialization functionality - void serializable() const; + // enabling serialization functionality + void serializable() const; - double weight(double error) const; - double loss(double error) 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); + virtual class Welsch : gtsam::noiseModel::mEstimator::Base + { + Welsch(double k); + static gtsam::noiseModel::mEstimator::Welsch *Create(double k); - // enabling serialization functionality - void serializable() const; + // enabling serialization functionality + void serializable() const; - double weight(double error) const; - double loss(double error) 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); + virtual class GemanMcClure : gtsam::noiseModel::mEstimator::Base + { + GemanMcClure(double c); + static gtsam::noiseModel::mEstimator::GemanMcClure *Create(double c); - // enabling serialization functionality - void serializable() const; + // enabling serialization functionality + void serializable() const; - double weight(double error) const; - double loss(double error) 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); + virtual class DCS : gtsam::noiseModel::mEstimator::Base + { + DCS(double c); + static gtsam::noiseModel::mEstimator::DCS *Create(double c); - // enabling serialization functionality - void serializable() const; + // enabling serialization functionality + void serializable() const; - double weight(double error) const; - double loss(double error) 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); + virtual class L2WithDeadZone : gtsam::noiseModel::mEstimator::Base + { + L2WithDeadZone(double k); + static gtsam::noiseModel::mEstimator::L2WithDeadZone *Create(double k); - // enabling serialization functionality - void serializable() const; + // enabling serialization functionality + void serializable() const; - double weight(double error) const; - double loss(double error) const; -}; + double weight(double error) const; + double loss(double error) const; + }; -}///\namespace mEstimator + } // 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); + 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; -}; + // enabling serialization functionality + void serializable() const; + }; -}///\namespace noiseModel + } // namespace noiseModel #include -class Sampler { - // Constructors - Sampler(gtsam::noiseModel::Diagonal* model, int seed); - Sampler(Vector sigmas, int seed); + 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(); -}; + // Standard Interface + size_t dim() const; + Vector sigmas() const; + gtsam::noiseModel::Diagonal *model() const; + Vector sample(); + }; #include -class VectorValues { + class VectorValues + { //Constructors - VectorValues(); - VectorValues(const gtsam::VectorValues& other); + VectorValues(); + VectorValues(const gtsam::VectorValues &other); - //Named Constructors - static gtsam::VectorValues Zero(const gtsam::VectorValues& model); + //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); + //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(); + //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); + 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; + 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; -}; + // 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; -}; + 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); + 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; + //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; + //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; + void transposeMultiplyAdd(double alpha, Vector e, gtsam::VectorValues &x) const; + gtsam::JacobianFactor whiten() const; - pair eliminate(const gtsam::Ordering& keys) const; + pair eliminate(const gtsam::Ordering &keys) const; - void setModel(bool anyConstrained, Vector sigmas); + void setModel(bool anyConstrained, Vector sigmas); - gtsam::noiseModel::Diagonal* get_model() const; + gtsam::noiseModel::Diagonal *get_model() const; - // enabling serialization functionality - void serialize() 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); + 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; + //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; + //Standard Interface + size_t rows() const; + Matrix information() const; + double constantTerm() const; + Vector linearTerm() const; - // enabling serialization functionality - void serialize() const; -}; + // enabling serialization functionality + void serialize() const; + }; #include -class GaussianFactorGraph { - GaussianFactorGraph(); - GaussianFactorGraph(const gtsam::GaussianBayesNet& bayesNet); - GaussianFactorGraph(const gtsam::GaussianBayesTree& bayesTree); + 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; + // 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); + // 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; + // 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; + 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; + // 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); + // 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; + // 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; -}; + // 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); + 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); + //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); + size_t name2, Matrix T); - //Standard Interface - void print(string s) const; - bool equals(const gtsam::GaussianConditional &cg, double tol) const; + //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; + //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; -}; + // enabling serialization functionality + void serialize() const; + }; #include -virtual class GaussianDensity : gtsam::GaussianConditional { + virtual class GaussianDensity : gtsam::GaussianConditional + { //Constructors - GaussianDensity(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas); + 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; -}; + //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 { + virtual class GaussianBayesNet + { //Constructors - GaussianBayesNet(); - GaussianBayesNet(const gtsam::GaussianConditional* conditional); + 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; + // 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; + // 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::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; -}; + 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; + 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; -}; + 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 { + class Errors + { //Constructors Errors(); - Errors(const gtsam::VectorValues& V); + Errors(const gtsam::VectorValues &V); //Testable void print(string s); - bool equals(const gtsam::Errors& expected, double tol) const; -}; + bool equals(const gtsam::Errors &expected, double tol) const; + }; #include -class GaussianISAM { - //Constructor - GaussianISAM(); + class GaussianISAM + { + //Constructor + GaussianISAM(); - //Standard Interface - void update(const gtsam::GaussianFactorGraph& newFactors); - void saveGraph(string s) const; - void clear(); -}; + //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 IterativeOptimizationParameters + { + string getVerbosity() const; + void setVerbosity(string s); + void print() const; + }; -//virtual class IterativeSolver { -// IterativeSolver(); -// gtsam::VectorValues optimize (); -//}; + //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; + 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; -}; + 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 PreconditionerParameters + { + PreconditionerParameters(); + }; -virtual class DummyPreconditionerParameters : gtsam::PreconditionerParameters { - DummyPreconditionerParameters(); -}; + virtual class DummyPreconditionerParameters : gtsam::PreconditionerParameters + { + DummyPreconditionerParameters(); + }; #include -virtual class PCGSolverParameters : gtsam::ConjugateGradientParameters { - PCGSolverParameters(); - void print(string s); - void setPreconditionerParams(gtsam::PreconditionerParameters* preconditioner); -}; + 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 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; -}; + 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); -}; + 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 -//************************************************************************* + //************************************************************************* + // nonlinear + //************************************************************************* #include -size_t symbol(char chr, size_t index); -char symbolChr(size_t key); -size_t symbolIndex(size_t key); + 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 + 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); + // 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); + 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; + 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; + 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; -}; + 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); + 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); + 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; + // 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); + // 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; -}; + // enabling serialization functionality + void serialize() const; + }; #include -class NonlinearFactorGraph { - NonlinearFactorGraph(); - NonlinearFactorGraph(const gtsam::NonlinearFactorGraph& graph); + 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; + // 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); + 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; + // 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; -}; + // 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 -}; + 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; -}; + 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); + class Values + { + Values(); + Values(const gtsam::Values &other); - size_t size() const; - bool empty() const; - void clear(); - size_t dim() const; + 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 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); + 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; + bool exists(size_t j) const; + gtsam::KeyVector keys() const; - gtsam::VectorValues zeroVectors() const; + gtsam::VectorValues zeroVectors() const; - gtsam::Values retract(const gtsam::VectorValues& delta) const; - gtsam::VectorValues localCoordinates(const gtsam::Values& cp) const; + gtsam::Values retract(const gtsam::VectorValues &delta) const; + gtsam::VectorValues localCoordinates(const gtsam::Values &cp) const; - // enabling serialization functionality - void serialize() 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); + // 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; - 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); + // 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); - template, gtsam::imuBias::ConstantBias, gtsam::NavState, Vector, Matrix}> - T at(size_t j); + 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); - /// version for double - void insertDouble(size_t j, double c); - double atDouble(size_t j) const; -}; + 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); + 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; -}; + 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; -}; + 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 { + virtual class LinearContainerFactor : gtsam::NonlinearFactor + { - LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::Values& linearizationPoint); - LinearContainerFactor(gtsam::GaussianFactor* factor); + LinearContainerFactor(gtsam::GaussianFactor *factor, const gtsam::Values &linearizationPoint); + LinearContainerFactor(gtsam::GaussianFactor *factor); - gtsam::GaussianFactor* factor() const; -// const boost::optional& linearizationPoint() const; + gtsam::GaussianFactor *factor() const; + // const boost::optional& linearizationPoint() const; - bool isJacobian() const; - gtsam::JacobianFactor* toJacobian() const; - gtsam::HessianFactor* toHessian() 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, + const gtsam::Values &linearizationPoint); - static gtsam::NonlinearFactorGraph ConvertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph); + static gtsam::NonlinearFactorGraph ConvertLinearGraph(const gtsam::GaussianFactorGraph &linear_graph); - // enabling serialization functionality - void serializable() const; -}; // \class LinearContainerFactor + // enabling serialization functionality + void serializable() const; + }; // \class LinearContainerFactor // Summarization functionality //#include @@ -2241,196 +2327,210 @@ virtual class LinearContainerFactor : gtsam::NonlinearFactor { // 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& params, - double currentError, double newError); + bool checkConvergence(double relativeErrorTreshold, + double absoluteErrorTreshold, double errorThreshold, + double currentError, double newError); + bool checkConvergence(const gtsam::NonlinearOptimizerParams ¶ms, + 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& params); -}; + 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); + }; #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& params); - 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 ¶ms); + 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& params); - 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 ¶ms); + 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(); @@ -2438,74 +2538,77 @@ class ISAM2Clique { //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& params); - ISAM2(const gtsam::ISAM2& other); + class ISAM2 + { + ISAM2(); + ISAM2(const gtsam::ISAM2Params ¶ms); + 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 @@ -2514,917 +2617,966 @@ class NonlinearISAM { #include #include -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; -}; + 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; + }; #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; -}; - -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; + // 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; #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; -}; - -typedef gtsam::BearingRangeFactor BearingRangeFactor2D; -typedef gtsam::BearingRangeFactor BearingRangeFactorPose2; + // enabling serialization functionality + void serialize() const; + }; + 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; - - // enabling serialization functionality - void serialize() const; -}; -typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3_S2; -typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3DS2; + 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; #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& params); - SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, - const CALIBRATION* K, - const gtsam::Pose3& body_P_sensor, - const gtsam::SmartProjectionParams& params); + 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); - 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; -}; - -typedef gtsam::SmartProjectionPoseFactor SmartProjectionPose3Factor; + // enabling serialization functionality + //void serialize() const; + }; + 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); - - // TODO(frank): deprecate once we land pybind wrapper - ShonanAveraging3(const gtsam::BetweenFactorPose3s &factors); - ShonanAveraging3(const gtsam::BetweenFactorPose3s &factors, - const gtsam::ShonanAveragingParameters3 ¶meters); + class ShonanAveraging3 + { + ShonanAveraging3(string g2oFile); + ShonanAveraging3(string g2oFile, + 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); + // TODO(frank): deprecate once we land pybind wrapper + ShonanAveraging3(const gtsam::BetweenFactorPose3s &factors); + ShonanAveraging3(const gtsam::BetweenFactorPose3s &factors, + const gtsam::ShonanAveragingParameters3 ¶meters); - // 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; + // Query properties + size_t nrUnknowns() const; + size_t nrMeasurements() const; + gtsam::Rot3 measured(size_t i); + gtsam::KeyVector keys(size_t i); - // 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; + // 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; - // 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; -}; + // 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; + }; #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); - - Matrix getBiasAccCovariance() const ; - Matrix getBiasOmegaCovariance() const ; - Matrix getBiasAccOmegaInt() const; - -}; + void setBiasAccCovariance(Matrix cov); + void setBiasOmegaCovariance(Matrix cov); + void setBiasAccOmegaInt(Matrix cov); -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); + Matrix getBiasAccCovariance() const; + Matrix getBiasOmegaCovariance() const; + Matrix getBiasAccOmegaInt() const; + }; - // Standard Interface - void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, - double deltaT); - void resetIntegration(); - void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat); + 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); - 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; -}; + // Standard Interface + void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, + double deltaT); + void resetIntegration(); + void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias &biasHat); -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); + 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; + }; - // 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); -}; + 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); + }; #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 { - - #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 + namespace utilities + { #include -class RedirectCout { - RedirectCout(); - string str(); -}; + 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 + class RedirectCout + { + RedirectCout(); + string str(); + }; + +} // namespace gtsam diff --git a/python/gtsam/preamble.h b/python/gtsam/preamble.h index c8a577431..fa98cd171 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 431697aac..63694f6f4 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 c04766804..6d7751356 100644 --- a/python/gtsam/tests/test_Triangulation.py +++ b/python/gtsam/tests/test_Triangulation.py @@ -14,11 +14,10 @@ import numpy as np import gtsam as g from gtsam.utils.test_case import GtsamTestCase -from gtsam import Cal3_S2, Cal3Bundler, Rot3, Pose3, \ - PinholeCameraCal3_S2, PinholeCameraCal3Bundler, Point3, \ - Point2Vector, Pose3Vector, triangulatePoint3, \ - CameraSetCal3_S2, CameraSetCal3Bundler -from numpy.core.records import array +from gtsam import Cal3_S2, Cal3Bundler, CameraSetCal3_S2,\ + CameraSetCal3Bundler, PinholeCameraCal3_S2, PinholeCameraCal3Bundler, \ + Point3, Pose3, Point2Vector, Pose3Vector, Rot3, triangulatePoint3 + class TestVisualISAMExample(GtsamTestCase): """ Tests for triangulation with shared and individual calibrations """ @@ -48,7 +47,6 @@ class TestVisualISAMExample(GtsamTestCase): Returns: vector of measurements and cameras """ - cameras = [] measurements = Point2Vector() for k, pose in zip(cal_params, self.poses): @@ -85,6 +83,8 @@ class TestVisualISAMExample(GtsamTestCase): K2 = (1600, 1300, 0, 650, 440) measurements, camera_list = self.generate_measurements(Cal3_S2, PinholeCameraCal3_S2, K1, K2) + + # convert list to CameraSet object cameras = CameraSetCal3_S2() for camera in camera_list: cameras.append(camera) @@ -99,6 +99,8 @@ class TestVisualISAMExample(GtsamTestCase): K2 = (1600, 0, 0, 650, 440) measurements, camera_list = self.generate_measurements(Cal3Bundler, PinholeCameraCal3Bundler, K1, K2) + + # convert list to CameraSet object cameras = CameraSetCal3Bundler() for camera in camera_list: cameras.append(camera)