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

Some cleanup/cleanup flags, capitalization

release/4.3a0
Rahul Sawhney 2010-10-21 18:02:17 +00:00
parent fbcbea5f61
commit 125c612c09
15 changed files with 65 additions and 106 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
geometry/Point2.h
View File

@ -118,8 +118,8 @@ namespace gtsam {
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
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_NVP(x_); ar & BOOST_SERIALIZATION_NVP(x_);
ar & BOOST_SERIALIZATION_NVP(y_); ar & BOOST_SERIALIZATION_NVP(y_);

5
geometry/Point3.h
View File

@ -85,7 +85,6 @@ namespace gtsam {
/** return vectorized form (column-wise)*/ /** return vectorized form (column-wise)*/
Vector vector() const { Vector vector() const {
//double r[] = { x_, y_, z_ };
Vector v(3); v(0)=x_; v(1)=y_; v(2)=z_; Vector v(3); v(0)=x_; v(1)=y_; v(2)=z_;
return v; return v;
} }
@ -128,8 +127,8 @@ namespace gtsam {
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
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_NVP(x_); ar & BOOST_SERIALIZATION_NVP(x_);
ar & BOOST_SERIALIZATION_NVP(y_); ar & BOOST_SERIALIZATION_NVP(y_);

2
geometry/Pose3.cpp
View File

@ -171,7 +171,7 @@ namespace gtsam {
Point3 Pose3::transform_to(const Point3& p, Point3 Pose3::transform_to(const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
const Point3 result = R_.unrotate(p - t_); const Point3 result = R_.unrotate(p - t_);
if (H1) { // *H1 = Dtransform_to1(pose, p); if (H1) {
const Point3& q = result; const Point3& q = result;
Matrix DR = skewSymmetric(q.x(), q.y(), q.z()); Matrix DR = skewSymmetric(q.x(), q.y(), q.z());
#ifdef CORRECT_POSE3_EXMAP #ifdef CORRECT_POSE3_EXMAP

4
geometry/Rot2.h
View File

@ -166,8 +166,8 @@ namespace gtsam {
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
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_NVP(c_); ar & BOOST_SERIALIZATION_NVP(c_);
ar & BOOST_SERIALIZATION_NVP(s_); ar & BOOST_SERIALIZATION_NVP(s_);
} }

4
geometry/Rot3.h
View File

@ -216,8 +216,8 @@ namespace gtsam {
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
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_NVP(r1_); ar & BOOST_SERIALIZATION_NVP(r1_);
ar & BOOST_SERIALIZATION_NVP(r2_); ar & BOOST_SERIALIZATION_NVP(r2_);

36
geometry/StereoCamera.cpp
View File

@ -46,13 +46,11 @@ StereoPoint2 StereoCamera::project(const Point3& point,
//**** above function call inlined //**** above function call inlined
Matrix D_cameraPoint_pose; Matrix D_cameraPoint_pose;
Point3 cameraPoint = pose().transform_to(point, D_cameraPoint_pose, boost::none); Point3 cameraPoint = pose().transform_to(point, D_cameraPoint_pose, boost::none);
//cout << "D_cameraPoint_pose" << endl;
//print(D_cameraPoint_pose);
//Point2 intrinsic = project_to_camera(cameraPoint); // unused //Point2 intrinsic = project_to_camera(cameraPoint); // unused
Matrix D_intrinsic_cameraPoint = Dproject_to_stereo_camera1(cameraPoint); // 3x3 Jacobian Matrix D_intrinsic_cameraPoint = Dproject_to_stereo_camera1(cameraPoint); // 3x3 Jacobian
//cout << "myJacobian" << endl;
//print(D_intrinsic_cameraPoint);
Matrix D_intrinsic_pose = D_intrinsic_cameraPoint * D_cameraPoint_pose; Matrix D_intrinsic_pose = D_intrinsic_cameraPoint * D_cameraPoint_pose;
@ -106,32 +104,8 @@ Matrix StereoCamera::Duncalibrate2(const Cal3_S2& K) {
calibration2(1) = calibration(0); calibration2(1) = calibration(0);
calibration2(2) = calibration(1); calibration2(2) = calibration(1);
return diag(calibration2); return diag(calibration2);
//return Matrix_(2, 2, K.fx_, K.s_, 0.000, K.fy_);
} }
// calibrated cameras
/*
Matrix Dproject_pose(const CalibratedCamera& camera, const Point3& point) {
Point3 cameraPoint = transform_to(camera.pose(), point);
Matrix D_cameraPoint_pose = Dtransform_to1(camera.pose(), point);
Point2 intrinsic = project_to_camera(cameraPoint);
Matrix D_intrinsic_cameraPoint = Dproject_to_camera1(cameraPoint);
Matrix D_intrinsic_pose = D_intrinsic_cameraPoint * D_cameraPoint_pose;
return D_intrinsic_pose;
}
Matrix Dproject_point(const CalibratedCamera& camera, const Point3& point) {
Point3 cameraPoint = transform_to(camera.pose(), point);
Matrix D_cameraPoint_point = Dtransform_to2(camera.pose());
Point2 intrinsic = project_to_camera(cameraPoint);
Matrix D_intrinsic_cameraPoint = Dproject_to_camera1(cameraPoint);
Matrix D_intrinsic_point = D_intrinsic_cameraPoint * D_cameraPoint_point;
return D_intrinsic_point;
}
*/
} }

8
slam/BearingFactor.h
View File

@ -26,18 +26,18 @@ namespace gtsam {
/** /**
* Binary factor for a bearing measurement * Binary factor for a bearing measurement
*/ */
template<class Values, class PoseKey, class PointKey> template<class VALUES, class POSEKEY, class POINTKEY>
class BearingFactor: public NonlinearFactor2<Values, PoseKey, PointKey> { class BearingFactor: public NonlinearFactor2<VALUES, POSEKEY, POINTKEY> {
private: private:
Rot2 z_; /** measurement */ Rot2 z_; /** measurement */
typedef NonlinearFactor2<Values, PoseKey, PointKey> Base; typedef NonlinearFactor2<VALUES, POSEKEY, POINTKEY> Base;
public: public:
BearingFactor(); /* Default constructor */ BearingFactor(); /* Default constructor */
BearingFactor(const PoseKey& i, const PointKey& j, const Rot2& z, BearingFactor(const POSEKEY& i, const POINTKEY& j, const Rot2& z,
const SharedGaussian& model) : const SharedGaussian& model) :
Base(model, i, j), z_(z) { Base(model, i, j), z_(z) {
} }

8
slam/BearingRangeFactor.h
View File

@ -27,20 +27,20 @@ namespace gtsam {
/** /**
* Binary factor for a bearing measurement * Binary factor for a bearing measurement
*/ */
template<class Values, class PoseKey, class PointKey> template<class VALUES, class POSEKEY, class POINTKEY>
class BearingRangeFactor: public NonlinearFactor2<Values, PoseKey, PointKey> { class BearingRangeFactor: public NonlinearFactor2<VALUES, POSEKEY, POINTKEY> {
private: private:
// the measurement // the measurement
Rot2 bearing_; Rot2 bearing_;
double range_; double range_;
typedef NonlinearFactor2<Values, PoseKey, PointKey> Base; typedef NonlinearFactor2<VALUES, POSEKEY, POINTKEY> Base;
public: public:
BearingRangeFactor(); /* Default constructor */ BearingRangeFactor(); /* Default constructor */
BearingRangeFactor(const PoseKey& i, const PointKey& j, const Rot2& bearing, const double range, BearingRangeFactor(const POSEKEY& i, const POINTKEY& j, const Rot2& bearing, const double range,
const SharedGaussian& model) : const SharedGaussian& model) :
Base(model, i, j), bearing_(bearing), range_(range) { Base(model, i, j), bearing_(bearing), range_(range) {
} }

16
slam/BetweenFactor.h
View File

@ -30,15 +30,15 @@ namespace gtsam {
* *
* FIXME: This should only need one key type, as we can't have different types * FIXME: This should only need one key type, as we can't have different types
*/ */
template<class Values, class Key1, class Key2 = Key1> template<class VALUES, class KEY1, class KEY2 = KEY1>
class BetweenFactor: public NonlinearFactor2<Values, Key1, Key2> { class BetweenFactor: public NonlinearFactor2<VALUES, KEY1, KEY2> {
public: public:
typedef typename Key1::Value T; typedef typename KEY1::Value T;
private: private:
typedef NonlinearFactor2<Values, Key1, Key2> Base; typedef NonlinearFactor2<VALUES, KEY1, KEY2> Base;
T measured_; /** The measurement */ T measured_; /** The measurement */
@ -48,7 +48,7 @@ namespace gtsam {
typedef typename boost::shared_ptr<BetweenFactor> shared_ptr; typedef typename boost::shared_ptr<BetweenFactor> shared_ptr;
/** Constructor */ /** Constructor */
BetweenFactor(const Key1& key1, const Key2& key2, const T& measured, BetweenFactor(const KEY1& key1, const KEY2& key2, const T& measured,
const SharedGaussian& model) : const SharedGaussian& model) :
Base(model, key1, key2), measured_(measured) { Base(model, key1, key2), measured_(measured) {
} }
@ -62,9 +62,9 @@ namespace gtsam {
} }
/** equals */ /** equals */
bool equals(const NonlinearFactor<Values>& expected, double tol) const { bool equals(const NonlinearFactor<VALUES>& expected, double tol) const {
const BetweenFactor<Values, Key1, Key2> *e = const BetweenFactor<VALUES, KEY1, KEY2> *e =
dynamic_cast<const BetweenFactor<Values, Key1, Key2>*> (&expected); dynamic_cast<const BetweenFactor<VALUES, KEY1, KEY2>*> (&expected);
return e != NULL && Base::equals(expected, tol) && this->measured_.equals( return e != NULL && Base::equals(expected, tol) && this->measured_.equals(
e->measured_, tol); e->measured_, tol);
} }

16
slam/PriorFactor.h
View File

@ -31,15 +31,15 @@ namespace gtsam {
* The Key type is not arbitrary: we need to cast to a Symbol at linearize, so * The Key type is not arbitrary: we need to cast to a Symbol at linearize, so
* a simple type like int will not work * a simple type like int will not work
*/ */
template<class Values, class Key> template<class VALUES, class KEY>
class PriorFactor: public NonlinearFactor1<Values, Key> { class PriorFactor: public NonlinearFactor1<VALUES, KEY> {
public: public:
typedef typename Key::Value T; typedef typename KEY::Value T;
private: private:
typedef NonlinearFactor1<Values, Key> Base; typedef NonlinearFactor1<VALUES, KEY> Base;
T prior_; /** The measurement */ T prior_; /** The measurement */
@ -49,7 +49,7 @@ namespace gtsam {
typedef typename boost::shared_ptr<PriorFactor> shared_ptr; typedef typename boost::shared_ptr<PriorFactor> shared_ptr;
/** Constructor */ /** Constructor */
PriorFactor(const Key& key, const T& prior, PriorFactor(const KEY& key, const T& prior,
const SharedGaussian& model) : const SharedGaussian& model) :
Base(model, key), prior_(prior) { Base(model, key), prior_(prior) {
} }
@ -63,9 +63,9 @@ namespace gtsam {
} }
/** equals */ /** equals */
bool equals(const NonlinearFactor<Values>& expected, double tol) const { bool equals(const NonlinearFactor<VALUES>& expected, double tol) const {
const PriorFactor<Values, Key> *e = dynamic_cast<const PriorFactor< const PriorFactor<VALUES, KEY> *e = dynamic_cast<const PriorFactor<
Values, Key>*> (&expected); VALUES, KEY>*> (&expected);
return e != NULL && Base::equals(expected, tol) && this->prior_.equals( return e != NULL && Base::equals(expected, tol) && this->prior_.equals(
e->prior_, tol); e->prior_, tol);
} }

12
slam/dataset.cpp
View File

@ -46,17 +46,7 @@ pair<string, boost::optional<SharedDiagonal> > dataset(const string& dataset, c
if (path.empty()) path = string(getenv("HOME")) + "/"; if (path.empty()) path = string(getenv("HOME")) + "/";
if (set.empty()) set = string(getenv("DATASET")); if (set.empty()) set = string(getenv("DATASET"));
/*if (set == "intel") return make_pair(path + "data/iSAM/Laser/intel.graph", null_model);
if (set == "intel-gfs") return make_pair(path + "data/iSAM/Laser/intel.gfs.graph", null_model);
if (set == "Killian-gfs") return make_pair(path + "data/iSAM/Laser/Killian.gfs.graph", null_model);
if (set == "Killian") return make_pair(path + "data/iSAM/Laser/Killian.graph", small);
if (set == "Killian-noised") return make_pair(path + "data/iSAM/Laser/Killian-noised.graph", null_model);
if (set == "3") return make_pair(path + "borg/toro/data/2D/w3-odom.graph", identity);
if (set == "100") return make_pair(path + "borg/toro/data/2D/w100-odom.graph", identity);
if (set == "10K") return make_pair(path + "borg/toro/data/2D/w10000-odom.graph", identity);
if (set == "olson") return make_pair(path + "data/iSAM/ISAM2/olson06icra.txt", null_model);
if (set == "victoria") return make_pair(path + "data/iSAM/ISAM2/victoria_park.txt", null_model);
if (set == "beijing") return make_pair(path + "data/BeijingData/beijingData_trips.log", null_model);*/
if (set == "intel") return make_pair(path + "borg/CitySLAM/data/Intel/intel.graph", null_model); if (set == "intel") return make_pair(path + "borg/CitySLAM/data/Intel/intel.graph", null_model);
if (set == "intel-gfs") return make_pair(path + "borg/CitySLAM/data/Intel/intel.gfs.graph", null_model); if (set == "intel-gfs") return make_pair(path + "borg/CitySLAM/data/Intel/intel.gfs.graph", null_model);

4
slam/simulated2D.cpp
View File

@ -22,11 +22,9 @@
namespace gtsam { namespace gtsam {
using namespace simulated2D; using namespace simulated2D;
// INSTANTIATE_LIE_CONFIG(PointKey)
INSTANTIATE_LIE_CONFIG(PoseKey) INSTANTIATE_LIE_CONFIG(PoseKey)
INSTANTIATE_TUPLE_CONFIG2(PoseValues, PointValues) INSTANTIATE_TUPLE_CONFIG2(PoseValues, PointValues)
// INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Values)
// INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Values)
namespace simulated2D { namespace simulated2D {

38
slam/simulated2D.h
View File

@ -65,14 +65,14 @@ namespace gtsam {
/** /**
* Unary factor encoding a soft prior on a vector * Unary factor encoding a soft prior on a vector
*/ */
template<class Cfg = Values, class Key = PoseKey> template<class CFG = Values, class KEY = PoseKey>
struct GenericPrior: public NonlinearFactor1<Cfg, Key> { struct GenericPrior: public NonlinearFactor1<CFG, KEY> {
typedef boost::shared_ptr<GenericPrior<Cfg, Key> > shared_ptr; typedef boost::shared_ptr<GenericPrior<CFG, KEY> > shared_ptr;
typedef typename Key::Value Pose; typedef typename KEY::Value Pose;
Pose z_; Pose z_;
GenericPrior(const Pose& z, const SharedGaussian& model, const Key& key) : GenericPrior(const Pose& z, const SharedGaussian& model, const KEY& key) :
NonlinearFactor1<Cfg, Key> (model, key), z_(z) { NonlinearFactor1<CFG, KEY> (model, key), z_(z) {
} }
Vector evaluateError(const Pose& x, boost::optional<Matrix&> H = Vector evaluateError(const Pose& x, boost::optional<Matrix&> H =
@ -85,15 +85,15 @@ namespace gtsam {
/** /**
* Binary factor simulating "odometry" between two Vectors * Binary factor simulating "odometry" between two Vectors
*/ */
template<class Cfg = Values, class Key = PoseKey> template<class CFG = Values, class KEY = PoseKey>
struct GenericOdometry: public NonlinearFactor2<Cfg, Key, Key> { struct GenericOdometry: public NonlinearFactor2<CFG, KEY, KEY> {
typedef boost::shared_ptr<GenericOdometry<Cfg, Key> > shared_ptr; typedef boost::shared_ptr<GenericOdometry<CFG, KEY> > shared_ptr;
typedef typename Key::Value Pose; typedef typename KEY::Value Pose;
Pose z_; Pose z_;
GenericOdometry(const Pose& z, const SharedGaussian& model, GenericOdometry(const Pose& z, const SharedGaussian& model,
const Key& i1, const Key& i2) : const KEY& i1, const KEY& i2) :
NonlinearFactor2<Cfg, Key, Key> (model, i1, i2), z_(z) { NonlinearFactor2<CFG, KEY, KEY> (model, i1, i2), z_(z) {
} }
Vector evaluateError(const Pose& x1, const Pose& x2, boost::optional< Vector evaluateError(const Pose& x1, const Pose& x2, boost::optional<
@ -106,18 +106,18 @@ namespace gtsam {
/** /**
* Binary factor simulating "measurement" between two Vectors * Binary factor simulating "measurement" between two Vectors
*/ */
template<class Cfg = Values, class XKey = PoseKey, class LKey = PointKey> template<class CFG = Values, class XKEY = PoseKey, class LKEY = PointKey>
class GenericMeasurement: public NonlinearFactor2<Cfg, XKey, LKey> { class GenericMeasurement: public NonlinearFactor2<CFG, XKEY, LKEY> {
public: public:
typedef boost::shared_ptr<GenericMeasurement<Cfg, XKey, LKey> > shared_ptr; typedef boost::shared_ptr<GenericMeasurement<CFG, XKEY, LKEY> > shared_ptr;
typedef typename XKey::Value Pose; typedef typename XKEY::Value Pose;
typedef typename LKey::Value Point; typedef typename LKEY::Value Point;
Point z_; Point z_;
GenericMeasurement(const Point& z, const SharedGaussian& model, GenericMeasurement(const Point& z, const SharedGaussian& model,
const XKey& i, const LKey& j) : const XKEY& i, const LKEY& j) :
NonlinearFactor2<Cfg, XKey, LKey> (model, i, j), z_(z) { NonlinearFactor2<CFG, XKEY, LKEY> (model, i, j), z_(z) {
} }
Vector evaluateError(const Pose& x1, const Point& x2, boost::optional< Vector evaluateError(const Pose& x1, const Point& x2, boost::optional<

4
slam/simulated2DOriented.cpp
View File

@ -21,9 +21,7 @@
namespace gtsam { namespace gtsam {
using namespace simulated2DOriented; using namespace simulated2DOriented;
// INSTANTIATE_LIE_CONFIG(PointKey)
// INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Values)
// INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Values)
namespace simulated2DOriented { namespace simulated2DOriented {

14
slam/simulated2DOriented.h
View File

@ -58,13 +58,13 @@ namespace gtsam {
/** /**
* Unary factor encoding a soft prior on a vector * Unary factor encoding a soft prior on a vector
*/ */
template<class Cfg = Values, class Key = PoseKey> template<class CFG = Values, class Key = PoseKey>
struct GenericPosePrior: public NonlinearFactor1<Cfg, Key> { struct GenericPosePrior: public NonlinearFactor1<CFG, Key> {
Pose2 z_; Pose2 z_;
GenericPosePrior(const Pose2& z, const SharedGaussian& model, const Key& key) : GenericPosePrior(const Pose2& z, const SharedGaussian& model, const Key& key) :
NonlinearFactor1<Cfg, Key> (model, key), z_(z) { NonlinearFactor1<CFG, Key> (model, key), z_(z) {
} }
Vector evaluateError(const Pose2& x, boost::optional<Matrix&> H = Vector evaluateError(const Pose2& x, boost::optional<Matrix&> H =
@ -77,13 +77,13 @@ namespace gtsam {
/** /**
* Binary factor simulating "odometry" between two Vectors * Binary factor simulating "odometry" between two Vectors
*/ */
template<class Cfg = Values, class Key = PoseKey> template<class CFG = Values, class KEY = PoseKey>
struct GenericOdometry: public NonlinearFactor2<Cfg, Key, Key> { struct GenericOdometry: public NonlinearFactor2<CFG, KEY, KEY> {
Pose2 z_; Pose2 z_;
GenericOdometry(const Pose2& z, const SharedGaussian& model, GenericOdometry(const Pose2& z, const SharedGaussian& model,
const Key& i1, const Key& i2) : const KEY& i1, const KEY& i2) :
NonlinearFactor2<Cfg, Key, Key> (model, i1, i2), z_(z) { NonlinearFactor2<CFG, KEY, KEY> (model, i1, i2), z_(z) {
} }
Vector evaluateError(const Pose2& x1, const Pose2& x2, boost::optional< Vector evaluateError(const Pose2& x1, const Pose2& x2, boost::optional<