From e4936df80a85bffadf800cfae6be17a0551f4579 Mon Sep 17 00:00:00 2001 From: Mike Bosse Date: Fri, 7 Nov 2014 22:41:21 +0100 Subject: [PATCH] matlab wrappers compile, but need testing --- gtsam.h | 16 +++++++++++----- gtsam/geometry/Cal3DS2.h | 4 +--- gtsam/geometry/Cal3DS2_Base.cpp | 16 ++++++++++++++-- gtsam/geometry/Cal3DS2_Base.h | 7 +++++-- gtsam/geometry/Cal3Unified.cpp | 1 + gtsam/geometry/Cal3Unified.h | 4 +--- gtsam_unstable/gtsam_unstable.h | 31 ++++++++++++++++--------------- 7 files changed, 49 insertions(+), 30 deletions(-) diff --git a/gtsam.h b/gtsam.h index 731767c37..c48bc825c 100644 --- a/gtsam.h +++ b/gtsam.h @@ -156,6 +156,12 @@ virtual class Value { size_t dim() const; }; +class Vector3 { + Vector3(Vector v); +}; +class Vector6 { + Vector6(Vector v); +}; #include class LieScalar { // Standard constructors @@ -2077,7 +2083,7 @@ class NonlinearISAM { #include #include -template +template virtual class PriorFactor : gtsam::NoiseModelFactor { PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); T prior() const; @@ -2088,7 +2094,7 @@ virtual class PriorFactor : gtsam::NoiseModelFactor { #include -template +template virtual class BetweenFactor : gtsam::NoiseModelFactor { BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel); T measured() const; @@ -2099,7 +2105,7 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor { #include -template +template virtual class NonlinearEquality : gtsam::NoiseModelFactor { // Constructor - forces exact evaluation NonlinearEquality(size_t j, const T& feasible); @@ -2340,7 +2346,7 @@ virtual class ImuFactor : gtsam::NonlinearFactor { // Standard Interface gtsam::ImuFactorPreintegratedMeasurements preintegratedMeasurements() const; - void Predict(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i, gtsam::Pose3& pose_j, gtsam::LieVector& vel_j, + void Predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, gtsam::Pose3& pose_j, gtsam::Vector3& vel_j, const gtsam::imuBias::ConstantBias& bias, const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis) const; @@ -2383,7 +2389,7 @@ virtual class CombinedImuFactor : gtsam::NonlinearFactor { // Standard Interface gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const; - void Predict(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i, gtsam::Pose3& pose_j, gtsam::LieVector& vel_j, + void Predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, gtsam::Pose3& pose_j, gtsam::Vector3& vel_j, const gtsam::imuBias::ConstantBias& bias_i, const gtsam::imuBias::ConstantBias& bias_j, const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis) const; diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index 39f4b7996..3b80b5b95 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -37,7 +37,7 @@ namespace gtsam { * k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ] * pi = K*pn */ -class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base, public DerivedValue { +class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base { typedef Cal3DS2_Base Base; @@ -96,8 +96,6 @@ private: template void serialize(Archive & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("Cal3DS2", - boost::serialization::base_object(*this)); ar & boost::serialization::make_nvp("Cal3DS2", boost::serialization::base_object(*this)); } diff --git a/gtsam/geometry/Cal3DS2_Base.cpp b/gtsam/geometry/Cal3DS2_Base.cpp index b8181ab4d..d50f56b89 100644 --- a/gtsam/geometry/Cal3DS2_Base.cpp +++ b/gtsam/geometry/Cal3DS2_Base.cpp @@ -90,8 +90,9 @@ static Eigen::Matrix D2dintrinsic(double x, double y, double rr, } /* ************************************************************************* */ -Point2 Cal3DS2_Base::uncalibrate(const Point2& p, boost::optional H1, - boost::optional H2) const { +Point2 Cal3DS2_Base::uncalibrate(const Point2& p, + boost::optional&> H1, + boost::optional&> H2) const { // rr = x^2 + y^2; // g = (1 + k(1)*rr + k(2)*rr^2); @@ -125,6 +126,17 @@ Point2 Cal3DS2_Base::uncalibrate(const Point2& p, boost::optional H1, return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_); } +Point2 Cal3DS2_Base::uncalibrate(const Point2& p, boost::optional H1, + boost::optional H2) const { + Eigen::Matrix H1f; + Eigen::Matrix H2f; + Point2 u = uncalibrate(p,H1f,H2f); + *H1 = H1f; + *H2 = H2f; + return u; +} + + /* ************************************************************************* */ Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const { // Use the following fixed point iteration to invert the radial distortion. diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h index 7be5a6874..09dc27f91 100644 --- a/gtsam/geometry/Cal3DS2_Base.h +++ b/gtsam/geometry/Cal3DS2_Base.h @@ -115,8 +115,11 @@ public: * @return point in (distorted) image coordinates */ Point2 uncalibrate(const Point2& p, - boost::optional Dcal = boost::none, - boost::optional Dp = boost::none) const ; + boost::optional&> Dcal = boost::none, + boost::optional&> Dp = boost::none) const ; + Point2 uncalibrate(const Point2& p, + boost::optional Dcal, + boost::optional Dp) const ; /// Convert (distorted) image coordinates uv to intrinsic coordinates xy Point2 calibrate(const Point2& p, const double tol=1e-5) const; diff --git a/gtsam/geometry/Cal3Unified.cpp b/gtsam/geometry/Cal3Unified.cpp index e7b408982..6312981a1 100644 --- a/gtsam/geometry/Cal3Unified.cpp +++ b/gtsam/geometry/Cal3Unified.cpp @@ -50,6 +50,7 @@ bool Cal3Unified::equals(const Cal3Unified& K, double tol) const { } /* ************************************************************************* */ +// todo: make a fixed sized jacobian version of this Point2 Cal3Unified::uncalibrate(const Point2& p, boost::optional H1, boost::optional H2) const { diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h index dc167173e..fb99592f7 100644 --- a/gtsam/geometry/Cal3Unified.h +++ b/gtsam/geometry/Cal3Unified.h @@ -40,7 +40,7 @@ namespace gtsam { * k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ] * pi = K*pn */ -class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base, public DerivedValue { +class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base { typedef Cal3Unified This; typedef Cal3DS2_Base Base; @@ -129,8 +129,6 @@ private: template void serialize(Archive & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("Cal3Unified", - boost::serialization::base_object(*this)); ar & boost::serialization::make_nvp("Cal3Unified", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(xi_); diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index 27460bd72..95e635516 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -4,14 +4,15 @@ // specify the classes from gtsam we are using virtual class gtsam::Value; -virtual class gtsam::LieScalar; -virtual class gtsam::LieVector; -virtual class gtsam::Point2; -virtual class gtsam::Rot2; -virtual class gtsam::Pose2; -virtual class gtsam::Point3; -virtual class gtsam::Rot3; -virtual class gtsam::Pose3; +class gtsam::Vector6; +class gtsam::LieScalar; +class gtsam::LieVector; +class gtsam::Point2; +class gtsam::Rot2; +class gtsam::Pose2; +class gtsam::Point3; +class gtsam::Rot3; +class gtsam::Pose3; virtual class gtsam::noiseModel::Base; virtual class gtsam::noiseModel::Gaussian; virtual class gtsam::imuBias::ConstantBias; @@ -23,8 +24,8 @@ virtual class gtsam::NoiseModelFactor4; virtual class gtsam::GaussianFactor; virtual class gtsam::HessianFactor; virtual class gtsam::JacobianFactor; -virtual class gtsam::Cal3_S2; -virtual class gtsam::Cal3DS2; +class gtsam::Cal3_S2; +class gtsam::Cal3DS2; class gtsam::GaussianFactorGraph; class gtsam::NonlinearFactorGraph; class gtsam::Ordering; @@ -48,7 +49,7 @@ class Dummy { }; #include -virtual class PoseRTV : gtsam::Value { +class PoseRTV { PoseRTV(); PoseRTV(Vector rtv); PoseRTV(const gtsam::Point3& pt, const gtsam::Rot3& rot, const gtsam::Point3& vel); @@ -99,7 +100,7 @@ virtual class PoseRTV : gtsam::Value { }; #include -virtual class Pose3Upright : gtsam::Value { +class Pose3Upright { Pose3Upright(); Pose3Upright(const gtsam::Pose3Upright& x); Pose3Upright(const gtsam::Rot2& bearing, const gtsam::Point3& t); @@ -137,7 +138,7 @@ virtual class Pose3Upright : gtsam::Value { }; // \class Pose3Upright #include -virtual class BearingS2 : gtsam::Value { +class BearingS2 { BearingS2(); BearingS2(double azimuth, double elevation); BearingS2(const gtsam::Rot2& azimuth, const gtsam::Rot2& elevation); @@ -520,14 +521,14 @@ virtual class PendulumFactorPk1 : gtsam::NonlinearFactor { virtual class Reconstruction : gtsam::NonlinearFactor { Reconstruction(size_t gKey1, size_t gKey, size_t xiKey, double h); - Vector evaluateError(const gtsam::Pose3& gK1, const gtsam::Pose3& gK, const gtsam::LieVector& xiK) const; + Vector evaluateError(const gtsam::Pose3& gK1, const gtsam::Pose3& gK, const gtsam::Vector6& xiK) const; }; virtual class DiscreteEulerPoincareHelicopter : gtsam::NonlinearFactor { DiscreteEulerPoincareHelicopter(size_t xiKey, size_t xiKey_1, size_t gKey, double h, Matrix Inertia, Vector Fu, double m); - Vector evaluateError(const gtsam::LieVector& xiK, const gtsam::LieVector& xiK_1, const gtsam::Pose3& gK) const; + Vector evaluateError(const gtsam::Vector6& xiK, const gtsam::Vector6& xiK_1, const gtsam::Pose3& gK) const; }; //*************************************************************************