diff --git a/.cproject b/.cproject index 05fb247ed..cf5f88a63 100644 --- a/.cproject +++ b/.cproject @@ -322,6 +322,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -348,7 +356,6 @@ make - tests/testBayesTree.run true false @@ -356,7 +363,6 @@ make - testBinaryBayesNet.run true false @@ -404,7 +410,6 @@ make - testSymbolicBayesNet.run true false @@ -412,7 +417,6 @@ make - tests/testSymbolicFactor.run true false @@ -420,7 +424,6 @@ make - testSymbolicFactorGraph.run true false @@ -436,20 +439,11 @@ make - tests/testBayesTree true false true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -474,16 +468,9 @@ true true - - make - -j2 - testGaussianISAM2.run - true - true - true - make + testGraph.run true false @@ -529,14 +516,6 @@ true true - - make - -j2 - testSQP.run - true - true - true - make -j2 @@ -561,38 +540,17 @@ true true - - make - -j2 - testBayesNetPreconditioner.run - true - true - true - - - make - -j2 - testConstraintOptimizer.run - true - true - true - make + testInference.run true false true - - make - testGaussianBayesNet.run - true - false - true - make + testGaussianFactor.run true false @@ -600,6 +558,7 @@ make + testJunctionTree.run true false @@ -607,6 +566,7 @@ make + testSymbolicBayesNet.run true false @@ -614,6 +574,7 @@ make + testSymbolicFactorGraph.run true false @@ -635,6 +596,14 @@ true true + + make + -j2 + testSerialization.run + true + true + true + make -j2 @@ -675,6 +644,14 @@ true true + + make + -j2 + check + true + true + true + make -j2 @@ -699,14 +676,6 @@ true true - - make - -j2 - check - true - true - true - make -j2 @@ -763,14 +732,6 @@ true true - - make - -j2 - vSFMexample.run - true - true - true - make -j2 @@ -779,6 +740,14 @@ true true + + make + -j2 + vSFMexample.run + true + true + true + make -j2 @@ -981,7 +950,6 @@ make - testErrors.run true false @@ -1365,6 +1333,7 @@ make + testSimulated2DOriented.run true false @@ -1404,6 +1373,7 @@ make + testSimulated2D.run true false @@ -1411,6 +1381,7 @@ make + testSimulated3D.run true false @@ -1602,6 +1573,7 @@ make + tests/testGaussianISAM2 true false @@ -1623,46 +1595,6 @@ true true - - make - -j2 - install - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - dist - true - true - true - make -j2 @@ -1759,6 +1691,54 @@ true true + + make + -j2 + install + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + dist + true + true + true + + + make + -j2 + check + true + true + true + make -j2 @@ -1791,14 +1771,6 @@ true true - - make - -j2 - check - true - true - true - @@ -2121,6 +2093,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -2147,7 +2127,6 @@ make - tests/testBayesTree.run true false @@ -2155,7 +2134,6 @@ make - testBinaryBayesNet.run true false @@ -2203,7 +2181,6 @@ make - testSymbolicBayesNet.run true false @@ -2211,7 +2188,6 @@ make - tests/testSymbolicFactor.run true false @@ -2219,7 +2195,6 @@ make - testSymbolicFactorGraph.run true false @@ -2235,20 +2210,11 @@ make - tests/testBayesTree true false true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -2273,16 +2239,9 @@ true true - - make - -j2 - testGaussianISAM2.run - true - true - true - make + testGraph.run true false @@ -2328,14 +2287,6 @@ true true - - make - -j2 - testSQP.run - true - true - true - make -j2 @@ -2360,38 +2311,17 @@ true true - - make - -j2 - testBayesNetPreconditioner.run - true - true - true - - - make - -j2 - testConstraintOptimizer.run - true - true - true - make + testInference.run true false true - - make - testGaussianBayesNet.run - true - false - true - make + testGaussianFactor.run true false @@ -2399,6 +2329,7 @@ make + testJunctionTree.run true false @@ -2406,6 +2337,7 @@ make + testSymbolicBayesNet.run true false @@ -2413,6 +2345,7 @@ make + testSymbolicFactorGraph.run true false @@ -2434,6 +2367,14 @@ true true + + make + -j2 + testSerialization.run + true + true + true + make -j2 @@ -2474,6 +2415,14 @@ true true + + make + -j2 + check + true + true + true + make -j2 @@ -2498,14 +2447,6 @@ true true - - make - -j2 - check - true - true - true - make -j2 @@ -2562,14 +2503,6 @@ true true - - make - -j2 - vSFMexample.run - true - true - true - make -j2 @@ -2578,6 +2511,14 @@ true true + + make + -j2 + vSFMexample.run + true + true + true + make -j2 @@ -2780,7 +2721,6 @@ make - testErrors.run true false @@ -3164,6 +3104,7 @@ make + testSimulated2DOriented.run true false @@ -3203,6 +3144,7 @@ make + testSimulated2D.run true false @@ -3210,6 +3152,7 @@ make + testSimulated3D.run true false @@ -3401,6 +3344,7 @@ make + tests/testGaussianISAM2 true false @@ -3422,46 +3366,6 @@ true true - - make - -j2 - install - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - dist - true - true - true - make -j2 @@ -3558,6 +3462,54 @@ true true + + make + -j2 + install + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + dist + true + true + true + + + make + -j2 + check + true + true + true + make -j2 @@ -3590,14 +3542,6 @@ true true - - make - -j2 - check - true - true - true - diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 972cd6339..c827af8b4 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -489,7 +489,7 @@ Matrix expm(const Matrix& A, size_t K=7); namespace boost { namespace serialization { -// split version - sends sizes ahead +// split version for Row-major matrix - sends sizes ahead template void save(Archive & ar, const Matrix & m, unsigned int version) { @@ -512,8 +512,32 @@ void load(Archive & ar, Matrix & m, unsigned int version) std::copy(raw_data.begin(), raw_data.end(), m.data()); } +// split version for Column-major matrix - sends sizes ahead +template +void save(Archive & ar, const MatrixColMajor & m, unsigned int version) +{ + const int rows = m.rows(), cols = m.cols(), elements = rows*cols; + std::vector raw_data(elements); + std::copy(m.data(), m.data()+elements, raw_data.begin()); + ar << make_nvp("rows", rows); + ar << make_nvp("cols", cols); + ar << make_nvp("data", raw_data); +} +template +void load(Archive & ar, MatrixColMajor & m, unsigned int version) +{ + size_t rows, cols; + std::vector raw_data; + ar >> make_nvp("rows", rows); + ar >> make_nvp("cols", cols); + ar >> make_nvp("data", raw_data); + m = MatrixColMajor(rows, cols); + std::copy(raw_data.begin(), raw_data.end(), m.data()); +} + } // namespace serialization } // namespace boost BOOST_SERIALIZATION_SPLIT_FREE(Matrix) +BOOST_SERIALIZATION_SPLIT_FREE(MatrixColMajor) diff --git a/gtsam/base/blockMatrices.h b/gtsam/base/blockMatrices.h index e1a8fdb53..ad3f97059 100644 --- a/gtsam/base/blockMatrices.h +++ b/gtsam/base/blockMatrices.h @@ -288,6 +288,18 @@ protected: template friend class SymmetricBlockView; template friend class VerticalBlockView; + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(matrix_); + ar & BOOST_SERIALIZATION_NVP(variableColOffsets_); + ar & BOOST_SERIALIZATION_NVP(rowStart_); + ar & BOOST_SERIALIZATION_NVP(rowEnd_); + ar & BOOST_SERIALIZATION_NVP(blockStart_); + } }; /** @@ -571,6 +583,16 @@ protected: template friend class SymmetricBlockView; template friend class VerticalBlockView; + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(matrix_); + ar & BOOST_SERIALIZATION_NVP(variableColOffsets_); + ar & BOOST_SERIALIZATION_NVP(blockStart_); + } }; diff --git a/gtsam/geometry/SimpleCamera.h b/gtsam/geometry/SimpleCamera.h index c76c76cc1..252af900f 100644 --- a/gtsam/geometry/SimpleCamera.h +++ b/gtsam/geometry/SimpleCamera.h @@ -38,6 +38,10 @@ namespace gtsam { public: SimpleCamera(const Cal3_S2& K, const CalibratedCamera& calibrated); SimpleCamera(const Cal3_S2& K, const Pose3& pose); + + /** constructor for serialization */ + SimpleCamera(){} + virtual ~SimpleCamera(); const Pose3& pose() const { @@ -73,6 +77,19 @@ namespace gtsam { boost::optional H1 = boost::none, boost::optional H2 = boost::none) const; + bool equals(const SimpleCamera& X, double tol=1e-9) const { + return calibrated_.equals(X.calibrated_, tol) && K_.equals(X.K_, tol); + } + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(calibrated_); + ar & BOOST_SERIALIZATION_NVP(K_); + } + }; } diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index 604e86e20..cd7d6166b 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -55,7 +55,7 @@ namespace gtsam { } /** equals */ - bool equals(const StereoPoint2& q, double tol) const { + bool equals(const StereoPoint2& q, double tol=1e-9) const { return (fabs(uL_ - q.uL_) < tol && fabs(uR_ - q.uR_) < tol && fabs(v_ - q.v_) < tol); } @@ -108,18 +108,28 @@ namespace gtsam { return p.vector(); } - /** default implementations of binary functions */ - inline StereoPoint2 expmap(const Vector& v) const { - return gtsam::expmap_default(*this, v); - } + /** default implementations of binary functions */ + inline StereoPoint2 expmap(const Vector& v) const { + return gtsam::expmap_default(*this, v); + } - inline Vector logmap(const StereoPoint2& p2) const { - return gtsam::logmap_default(*this, p2); - } + inline Vector logmap(const StereoPoint2& p2) const { + return gtsam::logmap_default(*this, p2); + } - inline StereoPoint2 between(const StereoPoint2& p2) const { - return gtsam::between_default(*this, p2); - } + inline StereoPoint2 between(const StereoPoint2& p2) const { + return gtsam::between_default(*this, p2); + } + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(uL_); + ar & BOOST_SERIALIZATION_NVP(uR_); + ar & BOOST_SERIALIZATION_NVP(v_); + } }; } diff --git a/gtsam/inference/IndexConditional.h b/gtsam/inference/IndexConditional.h index 8037d9e42..747a805f0 100644 --- a/gtsam/inference/IndexConditional.h +++ b/gtsam/inference/IndexConditional.h @@ -88,6 +88,13 @@ namespace gtsam { */ void permuteWithInverse(const Permutation& inversePermutation); + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + } }; } diff --git a/gtsam/inference/tests/testBinaryBayesNet.cpp b/gtsam/inference/tests/testBinaryBayesNet.cpp index 26a48b4f4..80c35ed30 100644 --- a/gtsam/inference/tests/testBinaryBayesNet.cpp +++ b/gtsam/inference/tests/testBinaryBayesNet.cpp @@ -26,11 +26,6 @@ #include // for operator += using namespace boost::assign; -#ifdef HAVE_BOOST_SERIALIZATION -#include -#include -#endif //HAVE_BOOST_SERIALIZATION - #define GTSAM_MAGIC_KEY #include diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index 69381a5b5..3619e653c 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -149,20 +149,18 @@ protected: friend class JacobianFactor; private: -// /** Serialization function */ -// friend class boost::serialization::access; -// template -// void serialize(Archive & ar, const unsigned int version) { -// ar & boost::serialization::make_nvp("Conditional", boost::serialization::base_object(*this)); -// ar & BOOST_SERIALIZATION_NVP(R_); -// ar & BOOST_SERIALIZATION_NVP(parents_); -// ar & BOOST_SERIALIZATION_NVP(d_); -// ar & BOOST_SERIALIZATION_NVP(sigmas_); -// } + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(IndexConditional); + ar & BOOST_SERIALIZATION_NVP(matrix_); + ar & BOOST_SERIALIZATION_NVP(rsd_); + ar & BOOST_SERIALIZATION_NVP(sigmas_); + } }; // GaussianConditional /* ************************************************************************* */ -// TODO: constructor outside class??? template GaussianConditional::GaussianConditional(ITERATOR firstKey, ITERATOR lastKey, size_t nrFrontals, const VerticalBlockView& matrices, diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index d5628d7d3..afc66497f 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -156,6 +156,7 @@ namespace gtsam { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(GaussianFactor); ar & BOOST_SERIALIZATION_NVP(info_); ar & BOOST_SERIALIZATION_NVP(matrix_); } diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index d5c8fde41..d79d58205 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -250,6 +250,7 @@ namespace gtsam { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(GaussianFactor); ar & BOOST_SERIALIZATION_NVP(firstNonzeroBlocks_); ar & BOOST_SERIALIZATION_NVP(Ab_); ar & BOOST_SERIALIZATION_NVP(model_); diff --git a/gtsam/linear/tests/testGaussianConditional.cpp b/gtsam/linear/tests/testGaussianConditional.cpp index f9a63a187..6e4028b24 100644 --- a/gtsam/linear/tests/testGaussianConditional.cpp +++ b/gtsam/linear/tests/testGaussianConditional.cpp @@ -18,13 +18,6 @@ #include #include -#ifdef HAVE_BOOST_SERIALIZATION -#include -#include -#endif //HAVE_BOOST_SERIALIZATION - -//#define GTSAM_MAGIC_KEY - #include #include @@ -162,44 +155,6 @@ TEST( GaussianConditional, solve ) } -/* ************************************************************************* */ -#ifdef HAVE_BOOST_SERIALIZATION -TEST( GaussianConditional, serialize ) -{ - // create a conditional gaussion node - Matrix A1(2,2); - A1(0,0) = 1 ; A1(1,0) = 2; - A1(0,1) = 3 ; A1(1,1) = 4; - - Matrix A2(2,2); - A2(0,0) = 6 ; A2(1,0) = 0.2; - A2(0,1) = 8 ; A2(1,1) = 0.4; - - Matrix R(2,2); - R(0,0) = 0.1 ; R(1,0) = 0.3; - R(0,1) = 0.0 ; R(1,1) = 0.34; - - Vector d(2); - d(0) = 0.2; d(1) = 0.5; - - GaussianConditional cg(_x2_, d, R, _x1_, A1, _l1_, A2); - - //serialize the CG - std::ostringstream in_archive_stream; - boost::archive::text_oarchive in_archive(in_archive_stream); - in_archive << cg; - std::string serialized = in_archive_stream.str(); - - //deserialize the CGg - std::istringstream out_archive_stream(serialized); - boost::archive::text_iarchive out_archive(out_archive_stream); - GaussianConditional output; - out_archive >> output; - - //check for equality - EXPECT(cg.equals(output)); -} -#endif //HAVE_BOOST_SERIALIZATION /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testVectorBTree.cpp b/gtsam/linear/tests/testVectorBTree.cpp index bc685767a..b12871c91 100644 --- a/gtsam/linear/tests/testVectorBTree.cpp +++ b/gtsam/linear/tests/testVectorBTree.cpp @@ -22,13 +22,6 @@ #include using namespace boost::assign; // bring 'operator+=()' into scope -//#include TEST_AC_DEFINE - -#ifdef HAVE_BOOST_SERIALIZATION -#include -#include -#endif //HAVE_BOOST_SERIALIZATION - #include #include #include @@ -285,33 +278,6 @@ TEST( VectorBTree, subVector) { CHECK(assert_equal(expected,c)); } -/* ************************************************************************* */ -#ifdef HAVE_BOOST_SERIALIZATION -TEST( VectorBTree, serialize) -{ - //DEBUG: - cout << "VectorBTree: Running Serialization Test" << endl; - - //create an VectorBTree - VectorBTree fg = createValues(); - - //serialize the config - std::ostringstream in_archive_stream; - boost::archive::text_oarchive in_archive(in_archive_stream); - in_archive << fg; - std::string serialized_fgc = in_archive_stream.str(); - - //deserialize the config - std::istringstream out_archive_stream(serialized_fgc); - boost::archive::text_iarchive out_archive(out_archive_stream); - VectorBTree output; - out_archive >> output; - - //check for equality - CHECK(fg.equals(output)); -} -#endif //HAVE_BOOST_SERIALIZATION - /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/linear/tests/testVectorMap.cpp b/gtsam/linear/tests/testVectorMap.cpp index e72dfadd5..79263bd0e 100644 --- a/gtsam/linear/tests/testVectorMap.cpp +++ b/gtsam/linear/tests/testVectorMap.cpp @@ -19,13 +19,6 @@ #include #include -//#include TEST_AC_DEFINE - -#ifdef HAVE_BOOST_SERIALIZATION -#include -#include -#endif //HAVE_BOOST_SERIALIZATION - #define GTSAM_MAGIC_KEY #include @@ -224,32 +217,6 @@ TEST( VectorMap, getReference) { CHECK(assert_equal(expected,c)); } -/* ************************************************************************* */ -#ifdef HAVE_BOOST_SERIALIZATION -TEST( VectorMap, serialize) -{ - //DEBUG: - cout << "VectorMap: Running Serialization Test" << endl; - - //create an VectorMap - VectorMap fg = createValues(); - - //serialize the config - std::ostringstream in_archive_stream; - boost::archive::text_oarchive in_archive(in_archive_stream); - in_archive << fg; - std::string serialized_fgc = in_archive_stream.str(); - - //deserialize the config - std::istringstream out_archive_stream(serialized_fgc); - boost::archive::text_iarchive out_archive(out_archive_stream); - VectorMap output; - out_archive >> output; - - //check for equality - CHECK(fg.equals(output)); -} -#endif //HAVE_BOOST_SERIALIZATION /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/slam/Makefile.am b/gtsam/slam/Makefile.am index 79a4e5773..6fcd5ad12 100644 --- a/gtsam/slam/Makefile.am +++ b/gtsam/slam/Makefile.am @@ -50,7 +50,7 @@ sources += pose3SLAM.cpp check_PROGRAMS += tests/testPose3Factor tests/testPose3Values tests/testPose3SLAM # Visual SLAM -headers += GeneralSFMFactor.h +headers += GeneralSFMFactor.h ProjectionFactor.h sources += visualSLAM.cpp check_PROGRAMS += tests/testVSLAMFactor tests/testVSLAMGraph tests/testVSLAMValues tests/testGeneralSFMFactor tests/testGeneralSFMFactor_Cal3Bundler diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h new file mode 100644 index 000000000..b936c6785 --- /dev/null +++ b/gtsam/slam/ProjectionFactor.h @@ -0,0 +1,90 @@ +/** + * @file ProjectionFactor.h + * @brief Basic bearing factor from 2D measurement + * @author Alex Cunningham + */ + +#pragma once + +#include +#include + +namespace gtsam { + + /** + * Non-linear factor for a constraint derived from a 2D measurement. The calibration is known here. + * i.e. the main building block for visual SLAM. + */ + template + class GenericProjectionFactor : public NonlinearFactor2, Testable > { + protected: + + // Keep a copy of measurement and calibration for I/O + Point2 z_; + boost::shared_ptr K_; + + public: + + // shorthand for base class type + typedef NonlinearFactor2 Base; + + // shorthand for a smart pointer to a factor + typedef boost::shared_ptr > shared_ptr; + + /** + * Default constructor + */ + GenericProjectionFactor() : K_(new Cal3_S2(444, 555, 666, 777, 888)) {} + + /** + * Constructor + * @param z is the 2 dimensional location of point in image (the measurement) + * @param sigma is the standard deviation + * @param cameraFrameNumber is basically the frame number + * @param landmarkNumber is the index of the landmark + * @param K the constant calibration + */ + GenericProjectionFactor(const Point2& z, + const SharedGaussian& model, POSK j_pose, + LMK j_landmark, const shared_ptrK& K) : + Base(model, j_pose, j_landmark), z_(z), K_(K) { + } + + /** + * print + * @param s optional string naming the factor + */ + void print(const std::string& s = "ProjectionFactor") const { + Base::print(s); + z_.print(s + ".z"); + } + + /** + * equals + */ + bool equals(const GenericProjectionFactor& p, double tol = 1e-9) const { + return Base::equals(p, tol) && this->z_.equals(p.z_, tol) + && this->K_->equals(*p.K_, tol); + } + + + + /** h(x)-z */ + Vector evaluateError(const Pose3& pose, const Point3& point, + boost::optional H1, boost::optional H2) const { + SimpleCamera camera(*K_, pose); + Point2 reprojectionError(camera.project(point, H1, H2) - z_); + return reprojectionError.vector(); + } + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar & BOOST_SERIALIZATION_NVP(z_); + ar & BOOST_SERIALIZATION_NVP(K_); + } + }; +} // \ namespace gtsam diff --git a/gtsam/slam/visualSLAM.h b/gtsam/slam/visualSLAM.h index e8cdd929e..34c590761 100644 --- a/gtsam/slam/visualSLAM.h +++ b/gtsam/slam/visualSLAM.h @@ -27,87 +27,11 @@ #include #include #include +#include #include namespace gtsam { - /** - * Non-linear factor for a constraint derived from a 2D measurement. The calibration is known here. - * i.e. the main building block for visual SLAM. - */ - template - class GenericProjectionFactor : public NonlinearFactor2, Testable > { - protected: - - // Keep a copy of measurement and calibration for I/O - Point2 z_; - boost::shared_ptr K_; - - public: - - // shorthand for base class type - typedef NonlinearFactor2 Base; - - // shorthand for a smart pointer to a factor - typedef boost::shared_ptr > shared_ptr; - - /** - * Default constructor - */ - GenericProjectionFactor() : K_(new Cal3_S2(444, 555, 666, 777, 888)) {} - - /** - * Constructor - * @param z is the 2 dimensional location of point in image (the measurement) - * @param sigma is the standard deviation - * @param cameraFrameNumber is basically the frame number - * @param landmarkNumber is the index of the landmark - * @param K the constant calibration - */ - GenericProjectionFactor(const Point2& z, - const SharedGaussian& model, POSK j_pose, - LMK j_landmark, const shared_ptrK& K) : - Base(model, j_pose, j_landmark), z_(z), K_(K) { - } - - /** - * print - * @param s optional string naming the factor - */ - void print(const std::string& s = "ProjectionFactor") const { - Base::print(s); - z_.print(s + ".z"); - } - - /** - * equals - */ - bool equals(const GenericProjectionFactor& p, double tol = 1e-9) const { - return Base::equals(p, tol) && this->z_.equals(p.z_, tol) - && this->K_->equals(*p.K_, tol); - } - - - - /** h(x)-z */ - Vector evaluateError(const Pose3& pose, const Point3& point, - boost::optional H1, boost::optional H2) const { - SimpleCamera camera(*K_, pose); - Point2 reprojectionError(camera.project(point, H1, H2) - z_); - return reprojectionError.vector(); - } - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(z_); - ar & BOOST_SERIALIZATION_NVP(K_); - } - }; - - namespace visualSLAM { /** diff --git a/tests/testGaussianBayesNet.cpp b/tests/testGaussianBayesNet.cpp index f772c3aac..2fbdcf381 100644 --- a/tests/testGaussianBayesNet.cpp +++ b/tests/testGaussianBayesNet.cpp @@ -25,11 +25,6 @@ #include // for operator += using namespace boost::assign; -#ifdef HAVE_BOOST_SERIALIZATION -#include -#include -#endif //HAVE_BOOST_SERIALIZATION - #define GTSAM_MAGIC_KEY #include @@ -203,47 +198,6 @@ TEST( GaussianBayesNet, backSubstituteTranspose ) CHECK(assert_equal(y,actual)); } -/* ************************************************************************* */ -#ifdef HAVE_BOOST_SERIALIZATION -TEST( GaussianBayesNet, serialize ) -{ - //create a starting CBN - GaussianBayesNet cbn = createSmallGaussianBayesNet(); - - //serialize the CBN - ostringstream in_archive_stream; - boost::archive::text_oarchive in_archive(in_archive_stream); - in_archive << cbn; - string serialized = in_archive_stream.str(); - - //DEBUG - cout << "CBN Raw string: [" << serialized << "]" << endl; - - //remove newlines/carriage returns - string clean; - BOOST_FOREACH(char s, serialized) { - if (s != '\n') { - //copy in character - clean.append(string(1,s)); - } - else { - cout << " Newline character found!" << endl; - //replace with an identifiable string - clean.append(string(1,' ')); - } - } - - cout << "Cleaned CBN String: [" << clean << "]" << endl; - - //deserialize the CBN - istringstream out_archive_stream(clean); - boost::archive::text_iarchive out_archive(out_archive_stream); - GaussianBayesNet output; - out_archive >> output; - CHECK(cbn.equals(output)); -} -#endif //HAVE_BOOST_SERIALIZATION - /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/tests/testSerialization.cpp b/tests/testSerialization.cpp index d48c88c44..bcbab4480 100644 --- a/tests/testSerialization.cpp +++ b/tests/testSerialization.cpp @@ -137,22 +137,23 @@ bool equalsDereferencedXML(const T& input = T()) { #include #include -#include #include -#include -#include -#include -#include #include +#include +#include +#include +#include +#include #include +#include +#include #include #include using namespace std; using namespace gtsam; -using namespace planarSLAM; /* ************************************************************************* */ TEST (Serialization, matrix_vector) { @@ -163,17 +164,40 @@ TEST (Serialization, matrix_vector) { EXPECT(equalityXML(Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0))); } +Point3 pt3(1.0, 2.0, 3.0); +Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0); +Pose3 pose3(rt3, pt3); + +Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5); +Cal3DS2 cal2(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0); +Cal3Bundler cal3(1.0, 2.0, 3.0); +Cal3_S2Stereo cal4(1.0, 2.0, 3.0, 4.0, 5.0, 6.0); +CalibratedCamera cal5(Pose3(rt3, pt3)); + +SimpleCamera cam1(cal1, cal5); +StereoCamera cam2(pose3, cal4); +StereoPoint2 spt(1.0, 2.0, 3.0); + + /* ************************************************************************* */ TEST (Serialization, text_geometry) { EXPECT(equalsObj(Point2(1.0, 2.0))); EXPECT(equalsObj(Pose2(1.0, 2.0, 0.3))); EXPECT(equalsObj(Rot2::fromDegrees(30.0))); - Point3 pt3(1.0, 2.0, 3.0); - Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0); - EXPECT(equalsObj(pt3)); + EXPECT(equalsObj(pt3)); EXPECT(equalsObj(rt3)); EXPECT(equalsObj(Pose3(rt3, pt3))); + + EXPECT(equalsObj(cal1)); + EXPECT(equalsObj(cal2)); + EXPECT(equalsObj(cal3)); + EXPECT(equalsObj(cal4)); + EXPECT(equalsObj(cal5)); + + EXPECT(equalsObj(cam1)); + EXPECT(equalsObj(cam2)); + EXPECT(equalsObj(spt)); } /* ************************************************************************* */ @@ -182,11 +206,19 @@ TEST (Serialization, xml_geometry) { EXPECT(equalsXML(Pose2(1.0, 2.0, 0.3))); EXPECT(equalsXML(Rot2::fromDegrees(30.0))); - Point3 pt3(1.0, 2.0, 3.0); - Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0); EXPECT(equalsXML(pt3)); EXPECT(equalsXML(rt3)); EXPECT(equalsXML(Pose3(rt3, pt3))); + + EXPECT(equalsXML(cal1)); + EXPECT(equalsXML(cal2)); + EXPECT(equalsXML(cal3)); + EXPECT(equalsXML(cal4)); + EXPECT(equalsXML(cal5)); + + EXPECT(equalsXML(cam1)); + EXPECT(equalsXML(cam2)); + EXPECT(equalsXML(spt)); } /* ************************************************************************* */ @@ -273,7 +305,7 @@ TEST (Serialization, SharedDiagonal_noiseModels) { #include /* ************************************************************************* */ -TEST (Serialization, text_linear) { +TEST (Serialization, linear) { vector dims; dims.push_back(1); dims.push_back(2); @@ -281,26 +313,6 @@ TEST (Serialization, text_linear) { double v[] = {1., 2., 3., 4., 5.}; VectorValues values(dims, v); EXPECT(equalsObj(values)); - - Index i1 = 4, i2 = 7; - Matrix A1 = eye(3), A2 = -1.0 * eye(3); - Vector b = ones(3); - SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector_(3, 1.0, 2.0, 3.0)); - JacobianFactor jacobianfactor(i1, A1, i2, A2, b, model); - EXPECT(equalsObj(jacobianfactor)); - - HessianFactor hessianfactor(jacobianfactor); - EXPECT(equalsObj(hessianfactor)); -} - -/* ************************************************************************* */ -TEST (Serialization, xml_linear) { - vector dims; - dims.push_back(1); - dims.push_back(2); - dims.push_back(2); - double v[] = {1., 2., 3., 4., 5.}; - VectorValues values(dims, v); EXPECT(equalsXML(values)); Index i1 = 4, i2 = 7; @@ -308,10 +320,21 @@ TEST (Serialization, xml_linear) { Vector b = ones(3); SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector_(3, 1.0, 2.0, 3.0)); JacobianFactor jacobianfactor(i1, A1, i2, A2, b, model); - EXPECT(equalsObj(jacobianfactor)); + EXPECT(equalsObj(jacobianfactor)); + EXPECT(equalsXML(jacobianfactor)); HessianFactor hessianfactor(jacobianfactor); - EXPECT(equalsObj(hessianfactor)); + EXPECT(equalsObj(hessianfactor)); + EXPECT(equalsXML(hessianfactor)); + { + Matrix A1 = Matrix_(2,2, 1., 2., 3., 4.); + Matrix A2 = Matrix_(2,2, 6., 0.2, 8., 0.4); + Matrix R = Matrix_(2,2, 0.1, 0.3, 0.0, 0.34); + Vector d(2); d << 0.2, 0.5; + GaussianConditional cg(0, d, R, 1, A1, 2, A2, ones(2)); +// EXPECT(equalsObj(cg)); // FAILS: does not match +// EXPECT(equalsXML(cg)); // FAILS: does not match + } } /* ************************************************************************* */ @@ -325,7 +348,7 @@ BOOST_CLASS_EXPORT_GUID(gtsam::planarSLAM::Constraint, "gtsam::planarSLAM::Cons /* ************************************************************************* */ TEST (Serialization, planar_system) { - + using namespace planarSLAM; Values values; values.insert(PointKey(3), Point2(1.0, 2.0)); values.insert(PoseKey(4), Pose2(1.0, 2.0, 0.3)); @@ -375,8 +398,43 @@ TEST (Serialization, planar_system) { } /* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} +/* Create GUIDs for factors */ +BOOST_CLASS_EXPORT_GUID(gtsam::visualSLAM::PoseConstraint, "gtsam::visualSLAM::PoseConstraint"); +BOOST_CLASS_EXPORT_GUID(gtsam::visualSLAM::PointConstraint, "gtsam::visualSLAM::PointConstraint"); +BOOST_CLASS_EXPORT_GUID(gtsam::visualSLAM::PosePrior, "gtsam::visualSLAM::PosePrior"); +BOOST_CLASS_EXPORT_GUID(gtsam::visualSLAM::PointPrior, "gtsam::visualSLAM::PointPrior"); +BOOST_CLASS_EXPORT_GUID(gtsam::visualSLAM::ProjectionFactor,"gtsam::visualSLAM::ProjectionFactor"); +BOOST_CLASS_EXPORT_GUID(gtsam::visualSLAM::StereoFactor, "gtsam::visualSLAM::StereoFactor"); + +/* ************************************************************************* */ +TEST (Serialization, visual_system) { + using namespace visualSLAM; + Values values; + PoseKey x1(1), x2(2); + PointKey l1(1), l2(2); + Pose3 pose1 = pose3, pose2 = pose3.inverse(); + Point3 pt1(1.0, 2.0, 3.0), pt2(4.0, 5.0, 6.0); + values.insert(x1, pose1); + values.insert(l1, pt1); + SharedGaussian model2 = noiseModel::Isotropic::Sigma(2, 0.3); + SharedGaussian model3 = noiseModel::Isotropic::Sigma(3, 0.3); + SharedGaussian model6 = noiseModel::Isotropic::Sigma(6, 0.3); + boost::shared_ptr K(new Cal3_S2(cal1)); + + Graph graph; + graph.addMeasurement(Point2(1.0, 2.0), model2, x1, l1, K); + graph.addPointConstraint(1, pt1); + graph.addPointPrior(1, pt2, model3); + graph.addPoseConstraint(1, pose1); + graph.addPosePrior(1, pose3, model6); + + EXPECT(equalsObj(values)); + EXPECT(equalsObj(graph)); + + EXPECT(equalsXML(values)); + EXPECT(equalsXML(graph)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */