diff --git a/geometry/Point2.h b/geometry/Point2.h index d273e1453..3d7dc4d78 100644 --- a/geometry/Point2.h +++ b/geometry/Point2.h @@ -118,8 +118,8 @@ namespace gtsam { private: /** Serialization function */ friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) + template + void serialize(ARCHIVE & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_NVP(x_); ar & BOOST_SERIALIZATION_NVP(y_); diff --git a/geometry/Point3.h b/geometry/Point3.h index d2dd0e466..cccd59c5d 100644 --- a/geometry/Point3.h +++ b/geometry/Point3.h @@ -85,7 +85,6 @@ namespace gtsam { /** return vectorized form (column-wise)*/ Vector vector() const { - //double r[] = { x_, y_, z_ }; Vector v(3); v(0)=x_; v(1)=y_; v(2)=z_; return v; } @@ -128,8 +127,8 @@ namespace gtsam { private: /** Serialization function */ friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) + template + void serialize(ARCHIVE & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_NVP(x_); ar & BOOST_SERIALIZATION_NVP(y_); diff --git a/geometry/Pose3.cpp b/geometry/Pose3.cpp index 79e182173..a85539894 100644 --- a/geometry/Pose3.cpp +++ b/geometry/Pose3.cpp @@ -171,7 +171,7 @@ namespace gtsam { Point3 Pose3::transform_to(const Point3& p, boost::optional H1, boost::optional H2) const { const Point3 result = R_.unrotate(p - t_); - if (H1) { // *H1 = Dtransform_to1(pose, p); + if (H1) { const Point3& q = result; Matrix DR = skewSymmetric(q.x(), q.y(), q.z()); #ifdef CORRECT_POSE3_EXMAP diff --git a/geometry/Rot2.h b/geometry/Rot2.h index 00600234e..40f77d414 100644 --- a/geometry/Rot2.h +++ b/geometry/Rot2.h @@ -166,8 +166,8 @@ namespace gtsam { private: /** Serialization function */ friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) { + template + void serialize(ARCHIVE & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_NVP(c_); ar & BOOST_SERIALIZATION_NVP(s_); } diff --git a/geometry/Rot3.h b/geometry/Rot3.h index 45e03b37e..b79afedb8 100644 --- a/geometry/Rot3.h +++ b/geometry/Rot3.h @@ -216,8 +216,8 @@ namespace gtsam { private: /** Serialization function */ friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) + template + void serialize(ARCHIVE & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_NVP(r1_); ar & BOOST_SERIALIZATION_NVP(r2_); diff --git a/geometry/StereoCamera.cpp b/geometry/StereoCamera.cpp index 9300c3c6c..d6afea925 100644 --- a/geometry/StereoCamera.cpp +++ b/geometry/StereoCamera.cpp @@ -46,13 +46,11 @@ StereoPoint2 StereoCamera::project(const Point3& point, //**** above function call inlined Matrix D_cameraPoint_pose; 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 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; @@ -106,32 +104,8 @@ Matrix StereoCamera::Duncalibrate2(const Cal3_S2& K) { calibration2(1) = calibration(0); calibration2(2) = calibration(1); 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; - } - */ } diff --git a/slam/BearingFactor.h b/slam/BearingFactor.h index 4a1d0d5d3..cc3c2428d 100644 --- a/slam/BearingFactor.h +++ b/slam/BearingFactor.h @@ -26,18 +26,18 @@ namespace gtsam { /** * Binary factor for a bearing measurement */ - template - class BearingFactor: public NonlinearFactor2 { + template + class BearingFactor: public NonlinearFactor2 { private: Rot2 z_; /** measurement */ - typedef NonlinearFactor2 Base; + typedef NonlinearFactor2 Base; public: 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) : Base(model, i, j), z_(z) { } diff --git a/slam/BearingRangeFactor.h b/slam/BearingRangeFactor.h index d0032bb15..38353c0ca 100644 --- a/slam/BearingRangeFactor.h +++ b/slam/BearingRangeFactor.h @@ -27,20 +27,20 @@ namespace gtsam { /** * Binary factor for a bearing measurement */ - template - class BearingRangeFactor: public NonlinearFactor2 { + template + class BearingRangeFactor: public NonlinearFactor2 { private: // the measurement Rot2 bearing_; double range_; - typedef NonlinearFactor2 Base; + typedef NonlinearFactor2 Base; public: 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) : Base(model, i, j), bearing_(bearing), range_(range) { } diff --git a/slam/BetweenFactor.h b/slam/BetweenFactor.h index a91552e02..e3a74e59d 100644 --- a/slam/BetweenFactor.h +++ b/slam/BetweenFactor.h @@ -30,15 +30,15 @@ namespace gtsam { * * FIXME: This should only need one key type, as we can't have different types */ - template - class BetweenFactor: public NonlinearFactor2 { + template + class BetweenFactor: public NonlinearFactor2 { public: - typedef typename Key1::Value T; + typedef typename KEY1::Value T; private: - typedef NonlinearFactor2 Base; + typedef NonlinearFactor2 Base; T measured_; /** The measurement */ @@ -48,7 +48,7 @@ namespace gtsam { typedef typename boost::shared_ptr shared_ptr; /** Constructor */ - BetweenFactor(const Key1& key1, const Key2& key2, const T& measured, + BetweenFactor(const KEY1& key1, const KEY2& key2, const T& measured, const SharedGaussian& model) : Base(model, key1, key2), measured_(measured) { } @@ -62,9 +62,9 @@ namespace gtsam { } /** equals */ - bool equals(const NonlinearFactor& expected, double tol) const { - const BetweenFactor *e = - dynamic_cast*> (&expected); + bool equals(const NonlinearFactor& expected, double tol) const { + const BetweenFactor *e = + dynamic_cast*> (&expected); return e != NULL && Base::equals(expected, tol) && this->measured_.equals( e->measured_, tol); } diff --git a/slam/PriorFactor.h b/slam/PriorFactor.h index 2151d5a5d..d2b7e233f 100644 --- a/slam/PriorFactor.h +++ b/slam/PriorFactor.h @@ -31,15 +31,15 @@ namespace gtsam { * The Key type is not arbitrary: we need to cast to a Symbol at linearize, so * a simple type like int will not work */ - template - class PriorFactor: public NonlinearFactor1 { + template + class PriorFactor: public NonlinearFactor1 { public: - typedef typename Key::Value T; + typedef typename KEY::Value T; private: - typedef NonlinearFactor1 Base; + typedef NonlinearFactor1 Base; T prior_; /** The measurement */ @@ -49,7 +49,7 @@ namespace gtsam { typedef typename boost::shared_ptr shared_ptr; /** Constructor */ - PriorFactor(const Key& key, const T& prior, + PriorFactor(const KEY& key, const T& prior, const SharedGaussian& model) : Base(model, key), prior_(prior) { } @@ -63,9 +63,9 @@ namespace gtsam { } /** equals */ - bool equals(const NonlinearFactor& expected, double tol) const { - const PriorFactor *e = dynamic_cast*> (&expected); + bool equals(const NonlinearFactor& expected, double tol) const { + const PriorFactor *e = dynamic_cast*> (&expected); return e != NULL && Base::equals(expected, tol) && this->prior_.equals( e->prior_, tol); } diff --git a/slam/dataset.cpp b/slam/dataset.cpp index 17c2179c3..6fc3b4c3a 100644 --- a/slam/dataset.cpp +++ b/slam/dataset.cpp @@ -46,17 +46,7 @@ pair > dataset(const string& dataset, c if (path.empty()) path = string(getenv("HOME")) + "/"; 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-gfs") return make_pair(path + "borg/CitySLAM/data/Intel/intel.gfs.graph", null_model); diff --git a/slam/simulated2D.cpp b/slam/simulated2D.cpp index 07b818e9d..427be5a8a 100644 --- a/slam/simulated2D.cpp +++ b/slam/simulated2D.cpp @@ -22,11 +22,9 @@ namespace gtsam { using namespace simulated2D; -// INSTANTIATE_LIE_CONFIG(PointKey) + INSTANTIATE_LIE_CONFIG(PoseKey) INSTANTIATE_TUPLE_CONFIG2(PoseValues, PointValues) -// INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Values) -// INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Values) namespace simulated2D { diff --git a/slam/simulated2D.h b/slam/simulated2D.h index 8a7d6a380..ed74b6f5f 100644 --- a/slam/simulated2D.h +++ b/slam/simulated2D.h @@ -65,14 +65,14 @@ namespace gtsam { /** * Unary factor encoding a soft prior on a vector */ - template - struct GenericPrior: public NonlinearFactor1 { - typedef boost::shared_ptr > shared_ptr; - typedef typename Key::Value Pose; + template + struct GenericPrior: public NonlinearFactor1 { + typedef boost::shared_ptr > shared_ptr; + typedef typename KEY::Value Pose; Pose z_; - GenericPrior(const Pose& z, const SharedGaussian& model, const Key& key) : - NonlinearFactor1 (model, key), z_(z) { + GenericPrior(const Pose& z, const SharedGaussian& model, const KEY& key) : + NonlinearFactor1 (model, key), z_(z) { } Vector evaluateError(const Pose& x, boost::optional H = @@ -85,15 +85,15 @@ namespace gtsam { /** * Binary factor simulating "odometry" between two Vectors */ - template - struct GenericOdometry: public NonlinearFactor2 { - typedef boost::shared_ptr > shared_ptr; - typedef typename Key::Value Pose; + template + struct GenericOdometry: public NonlinearFactor2 { + typedef boost::shared_ptr > shared_ptr; + typedef typename KEY::Value Pose; Pose z_; GenericOdometry(const Pose& z, const SharedGaussian& model, - const Key& i1, const Key& i2) : - NonlinearFactor2 (model, i1, i2), z_(z) { + const KEY& i1, const KEY& i2) : + NonlinearFactor2 (model, i1, i2), z_(z) { } Vector evaluateError(const Pose& x1, const Pose& x2, boost::optional< @@ -106,18 +106,18 @@ namespace gtsam { /** * Binary factor simulating "measurement" between two Vectors */ - template - class GenericMeasurement: public NonlinearFactor2 { + template + class GenericMeasurement: public NonlinearFactor2 { public: - typedef boost::shared_ptr > shared_ptr; - typedef typename XKey::Value Pose; - typedef typename LKey::Value Point; + typedef boost::shared_ptr > shared_ptr; + typedef typename XKEY::Value Pose; + typedef typename LKEY::Value Point; Point z_; GenericMeasurement(const Point& z, const SharedGaussian& model, - const XKey& i, const LKey& j) : - NonlinearFactor2 (model, i, j), z_(z) { + const XKEY& i, const LKEY& j) : + NonlinearFactor2 (model, i, j), z_(z) { } Vector evaluateError(const Pose& x1, const Point& x2, boost::optional< diff --git a/slam/simulated2DOriented.cpp b/slam/simulated2DOriented.cpp index fe9bf18d2..5dc1b2b22 100644 --- a/slam/simulated2DOriented.cpp +++ b/slam/simulated2DOriented.cpp @@ -21,9 +21,7 @@ namespace gtsam { using namespace simulated2DOriented; -// INSTANTIATE_LIE_CONFIG(PointKey) -// INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Values) -// INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Values) + namespace simulated2DOriented { diff --git a/slam/simulated2DOriented.h b/slam/simulated2DOriented.h index 1742fba97..115a1f71d 100644 --- a/slam/simulated2DOriented.h +++ b/slam/simulated2DOriented.h @@ -58,13 +58,13 @@ namespace gtsam { /** * Unary factor encoding a soft prior on a vector */ - template - struct GenericPosePrior: public NonlinearFactor1 { + template + struct GenericPosePrior: public NonlinearFactor1 { Pose2 z_; GenericPosePrior(const Pose2& z, const SharedGaussian& model, const Key& key) : - NonlinearFactor1 (model, key), z_(z) { + NonlinearFactor1 (model, key), z_(z) { } Vector evaluateError(const Pose2& x, boost::optional H = @@ -77,13 +77,13 @@ namespace gtsam { /** * Binary factor simulating "odometry" between two Vectors */ - template - struct GenericOdometry: public NonlinearFactor2 { + template + struct GenericOdometry: public NonlinearFactor2 { Pose2 z_; GenericOdometry(const Pose2& z, const SharedGaussian& model, - const Key& i1, const Key& i2) : - NonlinearFactor2 (model, i1, i2), z_(z) { + const KEY& i1, const KEY& i2) : + NonlinearFactor2 (model, i1, i2), z_(z) { } Vector evaluateError(const Pose2& x1, const Pose2& x2, boost::optional<