diff --git a/.cproject b/.cproject index 316c5c30c..7f13b1809 100644 --- a/.cproject +++ b/.cproject @@ -309,6 +309,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -335,7 +343,6 @@ make - tests/testBayesTree.run true false @@ -343,7 +350,6 @@ make - testBinaryBayesNet.run true false @@ -391,7 +397,6 @@ make - testSymbolicBayesNet.run true false @@ -399,7 +404,6 @@ make - tests/testSymbolicFactor.run true false @@ -407,7 +411,6 @@ make - testSymbolicFactorGraph.run true false @@ -423,20 +426,11 @@ make - tests/testBayesTree true false true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j5 @@ -525,22 +519,6 @@ false true - - make - -j2 - tests/testPose2.run - true - true - true - - - make - -j2 - tests/testPose3.run - true - true - true - make -j2 @@ -557,6 +535,22 @@ true true + + make + -j2 + tests/testPose2.run + true + true + true + + + make + -j2 + tests/testPose3.run + true + true + true + make -j2 @@ -581,26 +575,26 @@ true true - + make - -j2 - all + -j5 + testValues.run true true true - + make - -j2 - check + -j5 + testOrdering.run true true true - + make - -j2 - clean + -j5 + testKey.run true true true @@ -685,26 +679,26 @@ true true - + make - -j5 - testValues.run + -j2 + all true true true - + make - -j5 - testOrdering.run + -j2 + check true true true - + make - -j5 - testKey.run + -j2 + clean true true true @@ -789,6 +783,14 @@ true true + + make + -j5 + testPoseTranslationPrior.run + true + true + true + make -j5 @@ -975,7 +977,6 @@ make - testGraph.run true false @@ -983,7 +984,6 @@ make - testJunctionTree.run true false @@ -991,7 +991,6 @@ make - testSymbolicBayesNetB.run true false @@ -1127,7 +1126,6 @@ make - testErrors.run true false @@ -1173,6 +1171,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -1253,14 +1259,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -1591,6 +1589,7 @@ make + testSimulated2DOriented.run true false @@ -1630,6 +1629,7 @@ make + testSimulated2D.run true false @@ -1637,6 +1637,7 @@ make + testSimulated3D.run true false @@ -1828,6 +1829,7 @@ make + tests/testGaussianISAM2 true false @@ -1849,6 +1851,102 @@ true true + + make + -j2 + testRot3.run + true + true + true + + + make + -j2 + testRot2.run + true + true + true + + + make + -j2 + testPose3.run + true + true + true + + + make + -j2 + timeRot3.run + true + true + true + + + make + -j2 + testPose2.run + true + true + true + + + make + -j2 + testCal3_S2.run + true + true + true + + + make + -j2 + testSimpleCamera.run + true + true + true + + + make + -j2 + testHomography2.run + true + true + true + + + make + -j2 + testCalibratedCamera.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + testPoint2.run + true + true + true + make -j1 @@ -2050,7 +2148,6 @@ cpack - -G DEB true false @@ -2058,7 +2155,6 @@ cpack - -G RPM true false @@ -2066,7 +2162,6 @@ cpack - -G TGZ true false @@ -2074,7 +2169,6 @@ cpack - --config CPackSourceConfig.cmake true false @@ -2208,98 +2302,34 @@ true true - + make - -j2 - testRot3.run + -j5 + testSpirit.run true true true - + make - -j2 - testRot2.run + -j5 + testWrap.run true true true - + make - -j2 - testPose3.run + -j5 + check.wrap true true true - + make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - testPose2.run - true - true - true - - - make - -j2 - testCal3_S2.run - true - true - true - - - make - -j2 - testSimpleCamera.run - true - true - true - - - make - -j2 - testHomography2.run - true - true - true - - - make - -j2 - testCalibratedCamera.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testPoint2.run + -j5 + wrap true true true @@ -2343,38 +2373,6 @@ false true - - make - -j5 - testSpirit.run - true - true - true - - - make - -j5 - testWrap.run - true - true - true - - - make - -j5 - check.wrap - true - true - true - - - make - -j5 - wrap - true - true - true - diff --git a/gtsam_unstable/slam/PoseTranslationPrior.h b/gtsam_unstable/slam/PoseTranslationPrior.h index 4219e3252..61cf77263 100644 --- a/gtsam_unstable/slam/PoseTranslationPrior.h +++ b/gtsam_unstable/slam/PoseTranslationPrior.h @@ -9,61 +9,105 @@ #pragma once -#include #include +#include namespace gtsam { -template -class PoseTranslationPrior : public PartialPriorFactor { -public: +/** simple pose traits for building factors */ +namespace pose_traits { +/** checks whether parameterization of rotation is before or after translation in Lie algebra */ +template +bool isRotFirst() { + throw std::invalid_argument("PoseTrait: no implementation for this pose type"); + return false; +} + +// Instantiate for common poses +template<> bool isRotFirst() { return true; } +template<> bool isRotFirst() { return false; } + +} // \namespace pose_traits + +/** + * A prior on the translation part of a pose + */ +template +class PoseTranslationPrior : public NoiseModelFactor1 { +public: typedef PoseTranslationPrior This; - typedef PartialPriorFactor Base; + typedef NoiseModelFactor1 Base; typedef POSE Pose; typedef typename POSE::Translation Translation; + typedef typename POSE::Rotation Rotation; GTSAM_CONCEPT_POSE_TYPE(Pose) GTSAM_CONCEPT_GROUP_TYPE(Pose) GTSAM_CONCEPT_LIE_TYPE(Translation) +protected: + + Translation measured_; + +public: + /** standard constructor */ - PoseTranslationPrior(Key key, const Translation& trans_z, const SharedNoiseModel& model) - : Base(key, model) { - initialize(trans_z); + PoseTranslationPrior(Key key, const Translation& measured, const noiseModel::Base::shared_ptr& model) + : Base(model, key), measured_(measured) { } /** Constructor that pulls the translation from an incoming POSE */ - PoseTranslationPrior(Key key, const POSE& pose_z, const SharedNoiseModel& model) - : Base(key, model) { - initialize(pose_z.translation()); + PoseTranslationPrior(Key key, const POSE& pose_z, const noiseModel::Base::shared_ptr& model) + : Base(key, model), measured_(pose_z.translation()) { } - /** get the rotation used to create the measurement */ - Translation priorTranslation() const { return Translation::Expmap(this->prior_); } + virtual ~PoseTranslationPrior() {} -protected: - /** loads the underlying partial prior factor */ - void initialize(const Translation& trans_z) { - assert(trans_z.dim() == this->noiseModel_->dim()); + const Translation& measured() const { return measured_; } - // Calculate the prior applied - this->prior_ = Translation::Logmap(trans_z); + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); } - // Create the mask for partial prior - size_t pose_dim = Pose::identity().dim(); - size_t rot_dim = trans_z.dim(); + /** h(x)-z */ + Vector evaluateError(const Pose& pose, boost::optional H = boost::none) const { + const Translation& newTrans = pose.translation(); + const Rotation& R = pose.rotation(); + const size_t tDim = newTrans.dim(), xDim = pose.dim(); + if (H) { + *H = gtsam::zeros(tDim, xDim); + if (pose_traits::isRotFirst()) + (*H).rightCols(tDim) = R.matrix(); + else + (*H).leftCols(tDim) = R.matrix(); + } - // get the interval of the lie coordinates corresponding to rotation - std::pair interval = Pose::translationInterval(); + return newTrans.vector() - measured_.vector(); + } - std::vector mask; - for (size_t i=interval.first; i<=interval.second; ++i) - mask.push_back(i); - this->mask_ = mask; + /** equals specialized to this factor */ + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + const This *e = dynamic_cast (&expected); + return e != NULL && Base::equals(*e, tol) && measured_.equals(e->measured_, tol); + } - this->H_ = zeros(rot_dim, pose_dim); - this->fillH(); + /** print contents */ + void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + Base::print(s + "PoseTranslationPrior", keyFormatter); + measured_.print("Measured Translation"); + } + +private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NoiseModelFactor1", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(measured_); } }; diff --git a/gtsam_unstable/slam/tests/testPosePriors.cpp b/gtsam_unstable/slam/tests/testPoseRotationPrior.cpp similarity index 61% rename from gtsam_unstable/slam/tests/testPosePriors.cpp rename to gtsam_unstable/slam/tests/testPoseRotationPrior.cpp index 1a40dd335..cdded40e7 100644 --- a/gtsam_unstable/slam/tests/testPosePriors.cpp +++ b/gtsam_unstable/slam/tests/testPoseRotationPrior.cpp @@ -1,7 +1,7 @@ /** - * @file testPosePriors.cpp + * @file testPoseRotationPrior.cpp * - * @brief Tests partial priors on poses + * @brief Tests rotation priors on poses * * @date Jun 14, 2012 * @author Alex Cunningham @@ -22,32 +22,8 @@ using namespace gtsam; Key x1 = symbol_shorthand::X(1); const SharedNoiseModel model1 = noiseModel::Diagonal::Sigmas(Vector_(1, 0.1)); -const SharedNoiseModel model2 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.2)); const SharedNoiseModel model3 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3)); -/* ************************************************************************* */ -TEST( testPoseTranslationPrior, planar) { - - typedef PoseTranslationPrior PlanarTPrior; - PlanarTPrior actTprior(x1, Point2(2.0, 3.0), model2); - EXPECT(assert_equal(Point2(2.0, 3.0), actTprior.priorTranslation())); - Matrix expH(2,3); expH << eye(2,2), zeros(2,1); - EXPECT(assert_equal(eye(2,3), actTprior.H())); -} - -/* ************************************************************************* */ -TEST( testPoseTranslationPrior, visual) { - - typedef PoseTranslationPrior VisualTPrior; - VisualTPrior actPose3Tprior(x1, Point3(2.0, 3.0, 4.0), model3); - EXPECT(assert_equal(Point3(2.0, 3.0, 4.0), actPose3Tprior.priorTranslation())); - Matrix expH(3,6); expH << zeros(3,3), eye(3,3); - EXPECT(assert_equal(expH, actPose3Tprior.H())); - -// typedef PoseTranslationPrior CamTPrior; -// CamTPrior actCamTprior(x1, Point3(2.0, 3.0, 4.0), model3); -} - /* ************************************************************************* */ TEST( testPoseRotationPrior, planar) { diff --git a/gtsam_unstable/slam/tests/testPoseTranslationPrior.cpp b/gtsam_unstable/slam/tests/testPoseTranslationPrior.cpp new file mode 100644 index 000000000..1db155fc1 --- /dev/null +++ b/gtsam_unstable/slam/tests/testPoseTranslationPrior.cpp @@ -0,0 +1,105 @@ +/** + * @file testPoseTranslationPrior.cpp + * + * @date Aug 19, 2012 + * @author Alex Cunningham + */ + +#include + +#include +#include +#include + +#include + +using namespace gtsam; + +const SharedNoiseModel model2 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.2)); +const SharedNoiseModel model3 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3)); + +typedef PoseTranslationPrior Pose2TranslationPrior; +typedef PoseTranslationPrior Pose3TranslationPrior; + +const double tol = 1e-5; + +const gtsam::Key poseKey = 1; + +// Pose3 examples +const Point3 point3A(1.0, 2.0, 3.0), point3B(4.0, 6.0, 8.0); +const Rot3 rot3A, rot3B = Rot3::pitch(-M_PI_2), rot3C = Rot3::RzRyRx(0.1, 0.2, 0.3); + +/* ************************************************************************* */ +LieVector evalFactorError3(const Pose3TranslationPrior& factor, const Pose3& x) { + return LieVector(factor.evaluateError(x)); +} + +/* ************************************************************************* */ +TEST( testRelativeElevationFactor, level3_zero_error ) { + Pose3 pose1(rot3A, point3A); + Pose3TranslationPrior factor(poseKey, point3A, model3); + Matrix actH1; + EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1))); + Matrix expH1 = numericalDerivative11( + boost::bind(evalFactorError3, factor, _1), pose1, 1e-5); + EXPECT(assert_equal(expH1, actH1, tol)); +} + +/* ************************************************************************* */ +TEST( testRelativeElevationFactor, level3_error ) { + Pose3 pose1(rot3A, point3A); + Pose3TranslationPrior factor(poseKey, point3B, model3); + Matrix actH1; + EXPECT(assert_equal(Vector_(3,-3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1))); + Matrix expH1 = numericalDerivative11( + boost::bind(evalFactorError3, factor, _1), pose1, 1e-5); + EXPECT(assert_equal(expH1, actH1, tol)); +} + +/* ************************************************************************* */ +TEST( testRelativeElevationFactor, pitched3_zero_error ) { + Pose3 pose1(rot3B, point3A); + Pose3TranslationPrior factor(poseKey, point3A, model3); + Matrix actH1; + EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1))); + Matrix expH1 = numericalDerivative11( + boost::bind(evalFactorError3, factor, _1), pose1, 1e-5); + EXPECT(assert_equal(expH1, actH1, tol)); +} + +/* ************************************************************************* */ +TEST( testRelativeElevationFactor, pitched3_error ) { + Pose3 pose1(rot3B, point3A); + Pose3TranslationPrior factor(poseKey, point3B, model3); + Matrix actH1; + EXPECT(assert_equal(Vector_(3,-3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1))); + Matrix expH1 = numericalDerivative11( + boost::bind(evalFactorError3, factor, _1), pose1, 1e-5); + EXPECT(assert_equal(expH1, actH1, tol)); +} + +/* ************************************************************************* */ +TEST( testRelativeElevationFactor, smallrot3_zero_error ) { + Pose3 pose1(rot3C, point3A); + Pose3TranslationPrior factor(poseKey, point3A, model3); + Matrix actH1; + EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1))); + Matrix expH1 = numericalDerivative11( + boost::bind(evalFactorError3, factor, _1), pose1, 1e-5); + EXPECT(assert_equal(expH1, actH1, tol)); +} + +/* ************************************************************************* */ +TEST( testRelativeElevationFactor, smallrot3_error ) { + Pose3 pose1(rot3C, point3A); + Pose3TranslationPrior factor(poseKey, point3B, model3); + Matrix actH1; + EXPECT(assert_equal(Vector_(3,-3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1))); + Matrix expH1 = numericalDerivative11( + boost::bind(evalFactorError3, factor, _1), pose1, 1e-5); + EXPECT(assert_equal(expH1, actH1, tol)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */