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

Fixed and double versions

release/4.3a0
dellaert 2014-12-19 15:40:43 +01:00
parent 9f2e6562c2
commit 959a002693
3 changed files with 55 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
gtsam.h
View File

@ -156,12 +156,6 @@ virtual class Value {
size_t dim() const;
};
class Vector3 {
Vector3(Vector v);
};
class Vector6 {
Vector6(Vector v);
};
#include <gtsam/base/LieScalar.h>
class LieScalar {
// Standard constructors
@ -758,10 +752,6 @@ class CalibratedCamera {
gtsam::CalibratedCamera retract(const Vector& d) const;
Vector localCoordinates(const gtsam::CalibratedCamera& T2) const;
// Group
gtsam::CalibratedCamera compose(const gtsam::CalibratedCamera& c) const;
gtsam::CalibratedCamera inverse() const;
// Action on Point3
gtsam::Point2 project(const gtsam::Point3& point) const;
static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint);
@ -1727,6 +1717,7 @@ class Values {
// 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 insert(size_t j, const gtsam::Point2& t);
void insert(size_t j, const gtsam::Point3& t);
void insert(size_t j, const gtsam::Rot2& t);
@ -1758,6 +1749,16 @@ class Values {
template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Pose2, gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::imuBias::ConstantBias, Vector, Matrix}>
T at(size_t j);
/// Fixed size versions, for n in 1..9
void insertFixed(size_t j, Vector t, size_t n);
/// Fixed size versions, for n in 1..9
Vector atFixed(size_t j, size_t n);
/// version for double
void insertDouble(size_t j, double c);
double atDouble(size_t j) const;
};
// Actually a FastList<Key>
@ -2199,10 +2200,14 @@ typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Point2> RangeFactorPosePoint2;
typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Point3> RangeFactorPosePoint3;
typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Pose2> RangeFactorPose2;
typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Pose3> RangeFactorPose3;
typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::Point3> RangeFactorCalibratedCameraPoint;
typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::Point3> RangeFactorSimpleCameraPoint;
typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::CalibratedCamera> RangeFactorCalibratedCamera;
typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::SimpleCamera> RangeFactorSimpleCamera;
// Commented out by Frank Dec 2014: not poses!
// If needed, we need a RangeFactor that takes a camera, extracts the pose
// Should be easy with Expressions
//typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::Point3> RangeFactorCalibratedCameraPoint;
//typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::Point3> RangeFactorSimpleCameraPoint;
//typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::CalibratedCamera> RangeFactorCalibratedCamera;
//typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::SimpleCamera> RangeFactorSimpleCamera;
#include <gtsam/slam/BearingFactor.h>
@ -2395,7 +2400,7 @@ class ConstantBias {
#include <gtsam/navigation/ImuFactor.h>
class PoseVelocity{
PoseVelocity(const gtsam::Pose3& pose, const gtsam::Vector3 velocity);
PoseVelocity(const gtsam::Pose3& pose, Vector velocity);
};
class ImuFactorPreintegratedMeasurements {
// Standard Constructor
@ -2434,7 +2439,7 @@ virtual class ImuFactor : gtsam::NonlinearFactor {
const gtsam::Pose3& body_P_sensor);
// Standard Interface
gtsam::ImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
gtsam::PoseVelocity Predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, const gtsam::imuBias::ConstantBias& bias,
gtsam::PoseVelocity Predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias,
const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements,
Vector gravity, Vector omegaCoriolis) const;
};
@ -2480,7 +2485,7 @@ virtual class AHRSFactor : gtsam::NonlinearFactor {
#include <gtsam/navigation/CombinedImuFactor.h>
class PoseVelocityBias{
PoseVelocityBias(const gtsam::Pose3& pose, const gtsam::Vector3 velocity, const gtsam::imuBias::ConstantBias& bias);
PoseVelocityBias(const gtsam::Pose3& pose, Vector velocity, const gtsam::imuBias::ConstantBias& bias);
};
class CombinedImuFactorPreintegratedMeasurements {
// Standard Constructor
@ -2531,7 +2536,7 @@ virtual class CombinedImuFactor : gtsam::NonlinearFactor {
// Standard Interface
gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
gtsam::PoseVelocityBias Predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, const gtsam::imuBias::ConstantBias& bias_i,
gtsam::PoseVelocityBias Predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias_i,
const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements,
Vector gravity, Vector omegaCoriolis);
};

18
gtsam/nonlinear/Values.cpp
View File

@ -112,6 +112,24 @@ namespace gtsam {
return result;
}
/* ************************************************************************* */
Vector Values::atFixed(Key j, size_t n) {
switch (n) {
case 1: return at<Vector1>(j);
case 2: return at<Vector2>(j);
case 3: return at<Vector3>(j);
case 4: return at<Vector4>(j);
case 5: return at<Vector5>(j);
case 6: return at<Vector6>(j);
case 7: return at<Vector7>(j);
case 8: return at<Vector8>(j);
case 9: return at<Vector9>(j);
default:
throw runtime_error(
"Values::at fixed size can only handle n in 1..9");
}
}
/* ************************************************************************* */
const Value& Values::at(Key j) const {
// Find the item

14
gtsam/nonlinear/Values.h
View File

@ -180,6 +180,13 @@ namespace gtsam {
template<typename ValueType>
const ValueType& at(Key j) const;
/// Special version for small fixed size vectors, for matlab/python
/// throws truntime error if n<1 || n>9
Vector atFixed(Key j, size_t n);
/// version for double
double atDouble(size_t key) const { return at<double>(key);}
/** Retrieve a variable by key \c j. This version returns a reference
* to the base Value class, and needs to be casted before use.
* @param j Retrieve the value associated with this key
@ -258,6 +265,13 @@ namespace gtsam {
template <typename ValueType>
void insert(Key j, const ValueType& val);
/// Special version for small fixed size vectors, for matlab/python
/// throws truntime error if n<1 || n>9
void insertFixed(Key j, const Vector& v, size_t n);
/// version for double
void insertDouble(Key j, double c) { insert<double>(j,c); }
/// overloaded insert version that also specifies a chart
template <typename ValueType, typename Chart>
void insert(Key j, const ValueType& val);