12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

matlab wrappers compile, but need testing

release/4.3a0
Mike Bosse 2014-11-07 22:41:21 +01:00
parent 0ead01af92
commit e4936df80a
7 changed files with 49 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
gtsam.h
View File

@ -156,6 +156,12 @@ virtual class Value {
size_t dim() const; size_t dim() const;
}; };
class Vector3 {
Vector3(Vector v);
};
class Vector6 {
Vector6(Vector v);
};
#include <gtsam/base/LieScalar.h> #include <gtsam/base/LieScalar.h>
class LieScalar { class LieScalar {
// Standard constructors // Standard constructors
@ -2077,7 +2083,7 @@ class NonlinearISAM {
#include <gtsam/geometry/StereoPoint2.h> #include <gtsam/geometry/StereoPoint2.h>
#include <gtsam/slam/PriorFactor.h> #include <gtsam/slam/PriorFactor.h>
template<T = {gtsam::LieScalar, gtsam::LieVector, gtsam::LieMatrix, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}> template<T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
virtual class PriorFactor : gtsam::NoiseModelFactor { virtual class PriorFactor : gtsam::NoiseModelFactor {
PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
T prior() const; T prior() const;
@ -2088,7 +2094,7 @@ virtual class PriorFactor : gtsam::NoiseModelFactor {
#include <gtsam/slam/BetweenFactor.h> #include <gtsam/slam/BetweenFactor.h>
template<T = {gtsam::LieScalar, gtsam::LieVector, gtsam::LieMatrix, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::imuBias::ConstantBias}> template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::imuBias::ConstantBias}>
virtual class BetweenFactor : gtsam::NoiseModelFactor { virtual class BetweenFactor : gtsam::NoiseModelFactor {
BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel); BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel);
T measured() const; T measured() const;
@ -2099,7 +2105,7 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor {
#include <gtsam/nonlinear/NonlinearEquality.h> #include <gtsam/nonlinear/NonlinearEquality.h>
template<T = {gtsam::LieScalar, gtsam::LieVector, gtsam::LieMatrix, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}> template<T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
virtual class NonlinearEquality : gtsam::NoiseModelFactor { virtual class NonlinearEquality : gtsam::NoiseModelFactor {
// Constructor - forces exact evaluation // Constructor - forces exact evaluation
NonlinearEquality(size_t j, const T& feasible); NonlinearEquality(size_t j, const T& feasible);
@ -2340,7 +2346,7 @@ virtual class ImuFactor : gtsam::NonlinearFactor {
// Standard Interface // Standard Interface
gtsam::ImuFactorPreintegratedMeasurements preintegratedMeasurements() const; 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::imuBias::ConstantBias& bias,
const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements,
Vector gravity, Vector omegaCoriolis) const; Vector gravity, Vector omegaCoriolis) const;
@ -2383,7 +2389,7 @@ virtual class CombinedImuFactor : gtsam::NonlinearFactor {
// Standard Interface // Standard Interface
gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const; 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::imuBias::ConstantBias& bias_i, const gtsam::imuBias::ConstantBias& bias_j,
const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements, const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements,
Vector gravity, Vector omegaCoriolis) const; Vector gravity, Vector omegaCoriolis) const;

4
gtsam/geometry/Cal3DS2.h
View File

@ -37,7 +37,7 @@ namespace gtsam {
* k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ] * k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ]
* pi = K*pn * pi = K*pn
*/ */
class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base, public DerivedValue<Cal3DS2> { class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base {
typedef Cal3DS2_Base Base; typedef Cal3DS2_Base Base;
@ -96,8 +96,6 @@ private:
template<class Archive> template<class Archive>
void serialize(Archive & ar, const unsigned int version) void serialize(Archive & ar, const unsigned int version)
{ {
ar & boost::serialization::make_nvp("Cal3DS2",
boost::serialization::base_object<Value>(*this));
ar & boost::serialization::make_nvp("Cal3DS2", ar & boost::serialization::make_nvp("Cal3DS2",
boost::serialization::base_object<Cal3DS2_Base>(*this)); boost::serialization::base_object<Cal3DS2_Base>(*this));
} }

16
gtsam/geometry/Cal3DS2_Base.cpp
View File

@ -90,8 +90,9 @@ static Eigen::Matrix<double, 2, 2> D2dintrinsic(double x, double y, double rr,
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point2 Cal3DS2_Base::uncalibrate(const Point2& p, boost::optional<Matrix&> H1, Point2 Cal3DS2_Base::uncalibrate(const Point2& p,
boost::optional<Matrix&> H2) const { boost::optional<Eigen::Matrix<double, 2, 9>&> H1,
boost::optional<Eigen::Matrix<double, 2, 2>&> H2) const {
// rr = x^2 + y^2; // rr = x^2 + y^2;
// g = (1 + k(1)*rr + k(2)*rr^2); // g = (1 + k(1)*rr + k(2)*rr^2);
@ -125,6 +126,17 @@ Point2 Cal3DS2_Base::uncalibrate(const Point2& p, boost::optional<Matrix&> H1,
return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_); return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_);
} }
Point2 Cal3DS2_Base::uncalibrate(const Point2& p, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
Eigen::Matrix<double, 2, 9> H1f;
Eigen::Matrix<double, 2, 2> H2f;
Point2 u = uncalibrate(p,H1f,H2f);
*H1 = H1f;
*H2 = H2f;
return u;
}
/* ************************************************************************* */ /* ************************************************************************* */
Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const { Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const {
// Use the following fixed point iteration to invert the radial distortion. // Use the following fixed point iteration to invert the radial distortion.

7
gtsam/geometry/Cal3DS2_Base.h
View File

@ -115,8 +115,11 @@ public:
* @return point in (distorted) image coordinates * @return point in (distorted) image coordinates
*/ */
Point2 uncalibrate(const Point2& p, Point2 uncalibrate(const Point2& p,
boost::optional<Matrix&> Dcal = boost::none, boost::optional<Eigen::Matrix<double, 2, 9>&> Dcal = boost::none,
boost::optional<Matrix&> Dp = boost::none) const ; boost::optional<Eigen::Matrix<double, 2, 2>&> Dp = boost::none) const ;
Point2 uncalibrate(const Point2& p,
boost::optional<Matrix&> Dcal,
boost::optional<Matrix&> Dp) const ;
/// Convert (distorted) image coordinates uv to intrinsic coordinates xy /// Convert (distorted) image coordinates uv to intrinsic coordinates xy
Point2 calibrate(const Point2& p, const double tol=1e-5) const; Point2 calibrate(const Point2& p, const double tol=1e-5) const;

1
gtsam/geometry/Cal3Unified.cpp
View File

@ -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, Point2 Cal3Unified::uncalibrate(const Point2& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H2) const {

4
gtsam/geometry/Cal3Unified.h
View File

@ -40,7 +40,7 @@ namespace gtsam {
* k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ] * k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ]
* pi = K*pn * pi = K*pn
*/ */
class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base, public DerivedValue<Cal3Unified> { class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
typedef Cal3Unified This; typedef Cal3Unified This;
typedef Cal3DS2_Base Base; typedef Cal3DS2_Base Base;
@ -129,8 +129,6 @@ private:
template<class Archive> template<class Archive>
void serialize(Archive & ar, const unsigned int version) void serialize(Archive & ar, const unsigned int version)
{ {
ar & boost::serialization::make_nvp("Cal3Unified",
boost::serialization::base_object<Value>(*this));
ar & boost::serialization::make_nvp("Cal3Unified", ar & boost::serialization::make_nvp("Cal3Unified",
boost::serialization::base_object<Cal3DS2_Base>(*this)); boost::serialization::base_object<Cal3DS2_Base>(*this));
ar & BOOST_SERIALIZATION_NVP(xi_); ar & BOOST_SERIALIZATION_NVP(xi_);

31
gtsam_unstable/gtsam_unstable.h
View File

@ -4,14 +4,15 @@
// specify the classes from gtsam we are using // specify the classes from gtsam we are using
virtual class gtsam::Value; virtual class gtsam::Value;
virtual class gtsam::LieScalar; class gtsam::Vector6;
virtual class gtsam::LieVector; class gtsam::LieScalar;
virtual class gtsam::Point2; class gtsam::LieVector;
virtual class gtsam::Rot2; class gtsam::Point2;
virtual class gtsam::Pose2; class gtsam::Rot2;
virtual class gtsam::Point3; class gtsam::Pose2;
virtual class gtsam::Rot3; class gtsam::Point3;
virtual class gtsam::Pose3; class gtsam::Rot3;
class gtsam::Pose3;
virtual class gtsam::noiseModel::Base; virtual class gtsam::noiseModel::Base;
virtual class gtsam::noiseModel::Gaussian; virtual class gtsam::noiseModel::Gaussian;
virtual class gtsam::imuBias::ConstantBias; virtual class gtsam::imuBias::ConstantBias;
@ -23,8 +24,8 @@ virtual class gtsam::NoiseModelFactor4;
virtual class gtsam::GaussianFactor; virtual class gtsam::GaussianFactor;
virtual class gtsam::HessianFactor; virtual class gtsam::HessianFactor;
virtual class gtsam::JacobianFactor; virtual class gtsam::JacobianFactor;
virtual class gtsam::Cal3_S2; class gtsam::Cal3_S2;
virtual class gtsam::Cal3DS2; class gtsam::Cal3DS2;
class gtsam::GaussianFactorGraph; class gtsam::GaussianFactorGraph;
class gtsam::NonlinearFactorGraph; class gtsam::NonlinearFactorGraph;
class gtsam::Ordering; class gtsam::Ordering;
@ -48,7 +49,7 @@ class Dummy {
}; };
#include <gtsam_unstable/dynamics/PoseRTV.h> #include <gtsam_unstable/dynamics/PoseRTV.h>
virtual class PoseRTV : gtsam::Value { class PoseRTV {
PoseRTV(); PoseRTV();
PoseRTV(Vector rtv); PoseRTV(Vector rtv);
PoseRTV(const gtsam::Point3& pt, const gtsam::Rot3& rot, const gtsam::Point3& vel); PoseRTV(const gtsam::Point3& pt, const gtsam::Rot3& rot, const gtsam::Point3& vel);
@ -99,7 +100,7 @@ virtual class PoseRTV : gtsam::Value {
}; };
#include <gtsam_unstable/geometry/Pose3Upright.h> #include <gtsam_unstable/geometry/Pose3Upright.h>
virtual class Pose3Upright : gtsam::Value { class Pose3Upright {
Pose3Upright(); Pose3Upright();
Pose3Upright(const gtsam::Pose3Upright& x); Pose3Upright(const gtsam::Pose3Upright& x);
Pose3Upright(const gtsam::Rot2& bearing, const gtsam::Point3& t); Pose3Upright(const gtsam::Rot2& bearing, const gtsam::Point3& t);
@ -137,7 +138,7 @@ virtual class Pose3Upright : gtsam::Value {
}; // \class Pose3Upright }; // \class Pose3Upright
#include <gtsam_unstable/geometry/BearingS2.h> #include <gtsam_unstable/geometry/BearingS2.h>
virtual class BearingS2 : gtsam::Value { class BearingS2 {
BearingS2(); BearingS2();
BearingS2(double azimuth, double elevation); BearingS2(double azimuth, double elevation);
BearingS2(const gtsam::Rot2& azimuth, const gtsam::Rot2& elevation); BearingS2(const gtsam::Rot2& azimuth, const gtsam::Rot2& elevation);
@ -520,14 +521,14 @@ virtual class PendulumFactorPk1 : gtsam::NonlinearFactor {
virtual class Reconstruction : gtsam::NonlinearFactor { virtual class Reconstruction : gtsam::NonlinearFactor {
Reconstruction(size_t gKey1, size_t gKey, size_t xiKey, double h); 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 { virtual class DiscreteEulerPoincareHelicopter : gtsam::NonlinearFactor {
DiscreteEulerPoincareHelicopter(size_t xiKey, size_t xiKey_1, size_t gKey, DiscreteEulerPoincareHelicopter(size_t xiKey, size_t xiKey_1, size_t gKey,
double h, Matrix Inertia, Vector Fu, double m); 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;
}; };
//************************************************************************* //*************************************************************************