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

FixedDimension

release/4.3a0
dellaert 2014-12-22 17:22:09 +01:00
parent 65ae450abd
commit 0200e382b3
3 changed files with 53 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
gtsam/base/Manifold.h
View File

@ -127,7 +127,9 @@ check_manifold_invariants(const T& a, const T& b, double tol=1e-9) {
*/
template<typename M>
class IsManifold {
public:
typedef typename traits_x<M>::structure_category structure_category_tag;
static const size_t dim = traits_x<M>::dimension;
typedef typename traits_x<M>::ManifoldType ManifoldType;
@ -141,19 +143,30 @@ public:
BOOST_STATIC_ASSERT(TangentVector::SizeAtCompileTime == dim);
// make sure Chart methods are defined
v = traits_x<M>::Local(p,q);
q = traits_x<M>::Retract(p,v);
v = traits_x<M>::Local(p, q);
q = traits_x<M>::Retract(p, v);
// and the versions with Jacobians.
//v = traits_x<M>::Local(p,q,Hp,Hq);
//q = traits_x<M>::Retract(p,v,Hp,Hv);
}
private:
ManifoldType p,q;
ChartJacobian Hp,Hq,Hv;
ManifoldType p, q;
ChartJacobian Hp, Hq, Hv;
TangentVector v;
bool b;
};
/// Give fixed size dimension of a type, fails at compile time if dynamic
template<typename M>
struct FixedDimension {
typedef const int value_type;
static const int value = traits_x<M>::dimension;
BOOST_STATIC_ASSERT_MSG(value != Eigen::Dynamic,
"FixedDimension instantiated for dymanically-sized type.");
};
} // \ namespace gtsam
///**

25
gtsam/geometry/PinholeCamera.h
View File

@ -31,16 +31,19 @@ namespace gtsam {
*/
template<typename Calibration>
class PinholeCamera {
private:
Pose3 pose_;
Calibration K_;
GTSAM_CONCEPT_MANIFOLD_TYPE(Calibration)
// Get dimensions of calibration type and This at compile time
static const int DimK = traits_x<Calibration>::dimension, //
DimC = 6 + DimK;
static const int DimK = FixedDimension<Calibration>::value;
public:
enum { dimension = DimC };
enum { dimension = 6 + DimK };
/// @name Standard Constructors
/// @{
@ -169,15 +172,15 @@ public:
/// Manifold dimension
inline size_t dim() const {
return DimC;
return dimension;
}
/// Manifold dimension
inline static size_t Dim() {
return DimC;
return dimension;
}
typedef Eigen::Matrix<double, DimC, 1> VectorK6;
typedef Eigen::Matrix<double, dimension, 1> VectorK6;
/// move a cameras according to d
PinholeCamera retract(const Vector& d) const {
@ -316,7 +319,7 @@ public:
*/
Point2 project2(
const Point3& pw, //
OptionalJacobian<2, DimC> Dcamera = boost::none,
OptionalJacobian<2, dimension> Dcamera = boost::none,
OptionalJacobian<2, 3> Dpoint = boost::none) const {
const Point3 pc = pose_.transform_to(pw);
@ -366,7 +369,7 @@ public:
*/
double range(
const Point3& point, //
OptionalJacobian<1, DimC> Dcamera = boost::none,
OptionalJacobian<1, dimension> Dcamera = boost::none,
OptionalJacobian<1, 3> Dpoint = boost::none) const {
Matrix16 Dpose_;
double result = pose_.range(point, Dcamera ? &Dpose_ : 0, Dpoint);
@ -384,7 +387,7 @@ public:
*/
double range(
const Pose3& pose, //
OptionalJacobian<1, DimC> Dcamera = boost::none,
OptionalJacobian<1, dimension> Dcamera = boost::none,
OptionalJacobian<1, 6> Dpose = boost::none) const {
Matrix16 Dpose_;
double result = pose_.range(pose, Dcamera ? &Dpose_ : 0, Dpose);
@ -403,7 +406,7 @@ public:
template<class CalibrationB>
double range(
const PinholeCamera<CalibrationB>& camera, //
OptionalJacobian<1, DimC> Dcamera = boost::none,
OptionalJacobian<1, dimension> Dcamera = boost::none,
// OptionalJacobian<1, 6 + traits::dimension<CalibrationB>::value> Dother =
boost::optional<Matrix&> Dother =
boost::none) const {
@ -431,7 +434,7 @@ public:
*/
double range(
const CalibratedCamera& camera, //
OptionalJacobian<1, DimC> Dcamera = boost::none,
OptionalJacobian<1, dimension> Dcamera = boost::none,
OptionalJacobian<1, 6> Dother = boost::none) const {
return range(camera.pose(), Dcamera, Dother);
}

28
gtsam/slam/GeneralSFMFactor.h
View File

@ -36,15 +36,21 @@ namespace gtsam {
*/
template <class CAMERA, class LANDMARK>
class GeneralSFMFactor: public NoiseModelFactor2<CAMERA, LANDMARK> {
GTSAM_CONCEPT_MANIFOLD_TYPE(CAMERA)
GTSAM_CONCEPT_MANIFOLD_TYPE(LANDMARK)
static const int DimC = FixedDimension<CAMERA>::value;
static const int DimL = FixedDimension<LANDMARK>::value;
protected:
Point2 measured_; ///< the 2D measurement
public:
typedef CAMERA Cam; ///< typedef for camera type
typedef GeneralSFMFactor<CAMERA, LANDMARK> This; ///< typedef for this object
typedef NoiseModelFactor2<CAMERA, LANDMARK> Base; ///< typedef for the base class
typedef Point2 Measurement; ///< typedef for the measurement
// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
@ -89,7 +95,7 @@ namespace gtsam {
}
/** h(x)-z */
Vector evaluateError(const Cam& camera, const Point3& point,
Vector evaluateError(const CAMERA& camera, const LANDMARK& point,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const {
try {
@ -97,8 +103,8 @@ namespace gtsam {
return reprojError.vector();
}
catch( CheiralityException& e) {
if (H1) *H1 = zeros(2, camera.dim());
if (H2) *H2 = zeros(2, point.dim());
if (H1) *H1 = zeros(2, DimC);
if (H2) *H2 = zeros(2, DimL);
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2())
<< " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl;
return zero(2);
@ -132,7 +138,12 @@ namespace gtsam {
*/
template <class CALIBRATION>
class GeneralSFMFactor2: public NoiseModelFactor3<Pose3, Point3, CALIBRATION> {
GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION)
static const int DimK = FixedDimension<CALIBRATION>::value;
protected:
Point2 measured_; ///< the 2D measurement
public:
@ -140,7 +151,6 @@ namespace gtsam {
typedef GeneralSFMFactor2<CALIBRATION> This;
typedef PinholeCamera<CALIBRATION> Camera; ///< typedef for camera type
typedef NoiseModelFactor3<Pose3, Point3, CALIBRATION> Base; ///< typedef for the base class
typedef Point2 Measurement; ///< typedef for the measurement
// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
@ -194,9 +204,9 @@ namespace gtsam {
return reprojError.vector();
}
catch( CheiralityException& e) {
if (H1) *H1 = zeros(2, pose3.dim());
if (H2) *H2 = zeros(2, point.dim());
if (H3) *H3 = zeros(2, calib.dim());
if (H1) *H1 = zeros(2, 6);
if (H2) *H2 = zeros(2, 3);
if (H3) *H3 = zeros(2, DimK);
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2())
<< " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl;
}