diff --git a/.cproject b/.cproject index 80a7c399e..95c0f2a96 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 @@ -797,6 +791,14 @@ true true + + make + -j5 + testPoseRotationPrior.run + true + true + true + make -j5 @@ -983,7 +985,6 @@ make - testGraph.run true false @@ -991,7 +992,6 @@ make - testJunctionTree.run true false @@ -999,7 +999,6 @@ make - testSymbolicBayesNetB.run true false @@ -1135,7 +1134,6 @@ make - testErrors.run true false @@ -1181,10 +1179,10 @@ true true - + make - -j5 - testLinearContainerFactor.run + -j2 + testGaussianFactor.run true true true @@ -1269,10 +1267,10 @@ true true - + make - -j2 - testGaussianFactor.run + -j5 + testLinearContainerFactor.run true true true @@ -1607,6 +1605,7 @@ make + testSimulated2DOriented.run true false @@ -1646,6 +1645,7 @@ make + testSimulated2D.run true false @@ -1653,6 +1653,7 @@ make + testSimulated3D.run true false @@ -1844,6 +1845,7 @@ make + tests/testGaussianISAM2 true false @@ -1865,6 +1867,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 @@ -2066,7 +2164,6 @@ cpack - -G DEB true false @@ -2074,7 +2171,6 @@ cpack - -G RPM true false @@ -2082,7 +2178,6 @@ cpack - -G TGZ true false @@ -2090,7 +2185,6 @@ cpack - --config CPackSourceConfig.cmake true false @@ -2224,98 +2318,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 @@ -2359,38 +2389,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/PoseRotationPrior.h b/gtsam_unstable/slam/PoseRotationPrior.h index d6efc0cc4..60a29b3e4 100644 --- a/gtsam_unstable/slam/PoseRotationPrior.h +++ b/gtsam_unstable/slam/PoseRotationPrior.h @@ -9,63 +9,82 @@ #pragma once -#include -#include +#include namespace gtsam { template -class PoseRotationPrior : public PartialPriorFactor { +class PoseRotationPrior : public NoiseModelFactor1 { public: typedef PoseRotationPrior 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(Rotation) +protected: + + Rotation measured_; + +public: + /** standard constructor */ PoseRotationPrior(Key key, const Rotation& rot_z, const SharedNoiseModel& model) - : Base(key, model) { - initialize(rot_z); - } + : Base(model, key), measured_(rot_z) {} /** Constructor that pulls the translation from an incoming POSE */ PoseRotationPrior(Key key, const POSE& pose_z, const SharedNoiseModel& model) - : Base(key, model) { - initialize(pose_z.rotation()); + : Base(model, key), measured_(pose_z.rotation()) {} + + virtual ~PoseRotationPrior() {} + + // access + const Rotation& measured() const { return measured_; } + + // testable + + /** 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); } - /** get the rotation used to create the measurement */ - Rotation priorRotation() const { return Rotation::Expmap(this->prior_); } - -protected: - /** loads the underlying partial prior factor */ - void initialize(const Rotation& rot_z) { - assert(rot_z.dim() == this->noiseModel_->dim()); - - // Calculate the prior applied - this->prior_ = Rotation::Logmap(rot_z); - - // Create the mask for partial prior - size_t pose_dim = Pose::identity().dim(); - size_t rot_dim = rot_z.dim(); - - // get the interval of the lie coordinates corresponding to rotation - std::pair interval = Pose::rotationInterval(); - - std::vector mask; - for (size_t i=interval.first; i<=interval.second; ++i) - mask.push_back(i); - this->mask_ = mask; - - 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 + "PoseRotationPrior", keyFormatter); + measured_.print("Measured Rotation"); } + /** h(x)-z */ + Vector evaluateError(const Pose& pose, boost::optional H = boost::none) const { + const Rotation& newR = pose.rotation(); + const size_t rDim = newR.dim(), xDim = pose.dim(); + if (H) { + *H = gtsam::zeros(rDim, xDim); + if (pose_traits::isRotFirst()) + (*H).leftCols(rDim) = eye(rDim); + else + (*H).rightCols(rDim) = eye(rDim); + } + + return Rotation::Logmap(newR) - Rotation::Logmap(measured_); + } + +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_); + } }; } // \namespace gtsam diff --git a/gtsam_unstable/slam/tests/testPoseRotationPrior.cpp b/gtsam_unstable/slam/tests/testPoseRotationPrior.cpp index cdded40e7..2f60e114a 100644 --- a/gtsam_unstable/slam/tests/testPoseRotationPrior.cpp +++ b/gtsam_unstable/slam/tests/testPoseRotationPrior.cpp @@ -9,42 +9,85 @@ #include +#include + #include #include #include -#include - -#include using namespace gtsam; -Key x1 = symbol_shorthand::X(1); - const SharedNoiseModel model1 = noiseModel::Diagonal::Sigmas(Vector_(1, 0.1)); const SharedNoiseModel model3 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3)); -/* ************************************************************************* */ -TEST( testPoseRotationPrior, planar) { +typedef PoseRotationPrior Pose2RotationPrior; +typedef PoseRotationPrior Pose3RotationPrior; - typedef PoseRotationPrior PlanarRPrior; - PlanarRPrior actRprior(x1, Rot2::fromDegrees(30.0), model1); - EXPECT(assert_equal(Rot2::fromDegrees(30.0), actRprior.priorRotation())); - Matrix expH(1,3); expH << 0.0, 0.0, 1.0; - EXPECT(assert_equal(expH, actRprior.H())); +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::Expmap(Vector_(3, 0.1, 0.2, 0.3)); + +// Pose2 examples +const Point2 point2A(1.0, 2.0), point2B(4.0, 6.0); +const Rot2 rot2A, rot2B = Rot2::fromAngle(M_PI_2); + +/* ************************************************************************* */ +LieVector evalFactorError3(const Pose3RotationPrior& factor, const Pose3& x) { + return LieVector(factor.evaluateError(x)); } /* ************************************************************************* */ -TEST( testPoseRotationPrior, visual) { +LieVector evalFactorError2(const Pose2RotationPrior& factor, const Pose2& x) { + return LieVector(factor.evaluateError(x)); +} - typedef PoseRotationPrior VisualRPrior; - VisualRPrior actPose3Rprior(x1, Rot3::RzRyRx(0.1, 0.2, 0.3), model3); - EXPECT(assert_equal(Rot3::RzRyRx(0.1, 0.2, 0.3), actPose3Rprior.priorRotation())); - Matrix expH(3,6); expH << eye(3,3), zeros(3,3); - EXPECT(assert_equal(expH, actPose3Rprior.H())); +/* ************************************************************************* */ +TEST( testPoseRotationFactor, level3_zero_error ) { + Pose3 pose1(rot3A, point3A); + Pose3RotationPrior factor(poseKey, rot3A, 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)); +} -// typedef testPoseRotationPrior CamRPrior; -// CamRPrior actCamRprior(x1, Rot3::RzRyRx(0.1, 0.2, 0.3), model3); +/* ************************************************************************* */ +TEST( testPoseRotationFactor, level3_error ) { + Pose3 pose1(rot3A, point3A); + Pose3RotationPrior factor(poseKey, rot3C, model3); + Matrix actH1; + EXPECT(assert_equal(Vector_(3,-0.1,-0.2,-0.3), factor.evaluateError(pose1, actH1))); + Matrix expH1 = numericalDerivative11( + boost::bind(evalFactorError3, factor, _1), pose1, 1e-5); + EXPECT(assert_equal(expH1, actH1, tol)); +} + +/* ************************************************************************* */ +TEST( testPoseRotationFactor, level2_zero_error ) { + Pose2 pose1(rot2A, point2A); + Pose2RotationPrior factor(poseKey, rot2A, model1); + Matrix actH1; + EXPECT(assert_equal(zero(1), factor.evaluateError(pose1, actH1))); + Matrix expH1 = numericalDerivative11( + boost::bind(evalFactorError2, factor, _1), pose1, 1e-5); + EXPECT(assert_equal(expH1, actH1, tol)); +} + +/* ************************************************************************* */ +TEST( testPoseRotationFactor, level2_error ) { + Pose2 pose1(rot2A, point2A); + Pose2RotationPrior factor(poseKey, rot2B, model1); + Matrix actH1; + EXPECT(assert_equal(Vector_(1,-M_PI_2), factor.evaluateError(pose1, actH1))); + Matrix expH1 = numericalDerivative11( + boost::bind(evalFactorError2, factor, _1), pose1, 1e-5); + EXPECT(assert_equal(expH1, actH1, tol)); } /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/tests/testPoseTranslationPrior.cpp b/gtsam_unstable/slam/tests/testPoseTranslationPrior.cpp index 0821e566b..9e11555ee 100644 --- a/gtsam_unstable/slam/tests/testPoseTranslationPrior.cpp +++ b/gtsam_unstable/slam/tests/testPoseTranslationPrior.cpp @@ -44,7 +44,7 @@ LieVector evalFactorError2(const Pose2TranslationPrior& factor, const Pose2& x) } /* ************************************************************************* */ -TEST( testRelativeElevationFactor, level3_zero_error ) { +TEST( testPoseTranslationFactor, level3_zero_error ) { Pose3 pose1(rot3A, point3A); Pose3TranslationPrior factor(poseKey, point3A, model3); Matrix actH1; @@ -55,7 +55,7 @@ TEST( testRelativeElevationFactor, level3_zero_error ) { } /* ************************************************************************* */ -TEST( testRelativeElevationFactor, level3_error ) { +TEST( testPoseTranslationFactor, level3_error ) { Pose3 pose1(rot3A, point3A); Pose3TranslationPrior factor(poseKey, point3B, model3); Matrix actH1; @@ -66,7 +66,7 @@ TEST( testRelativeElevationFactor, level3_error ) { } /* ************************************************************************* */ -TEST( testRelativeElevationFactor, pitched3_zero_error ) { +TEST( testPoseTranslationFactor, pitched3_zero_error ) { Pose3 pose1(rot3B, point3A); Pose3TranslationPrior factor(poseKey, point3A, model3); Matrix actH1; @@ -77,7 +77,7 @@ TEST( testRelativeElevationFactor, pitched3_zero_error ) { } /* ************************************************************************* */ -TEST( testRelativeElevationFactor, pitched3_error ) { +TEST( testPoseTranslationFactor, pitched3_error ) { Pose3 pose1(rot3B, point3A); Pose3TranslationPrior factor(poseKey, point3B, model3); Matrix actH1; @@ -88,7 +88,7 @@ TEST( testRelativeElevationFactor, pitched3_error ) { } /* ************************************************************************* */ -TEST( testRelativeElevationFactor, smallrot3_zero_error ) { +TEST( testPoseTranslationFactor, smallrot3_zero_error ) { Pose3 pose1(rot3C, point3A); Pose3TranslationPrior factor(poseKey, point3A, model3); Matrix actH1; @@ -99,7 +99,7 @@ TEST( testRelativeElevationFactor, smallrot3_zero_error ) { } /* ************************************************************************* */ -TEST( testRelativeElevationFactor, smallrot3_error ) { +TEST( testPoseTranslationFactor, smallrot3_error ) { Pose3 pose1(rot3C, point3A); Pose3TranslationPrior factor(poseKey, point3B, model3); Matrix actH1; @@ -110,7 +110,7 @@ TEST( testRelativeElevationFactor, smallrot3_error ) { } /* ************************************************************************* */ -TEST( testRelativeElevationFactor, level2_zero_error ) { +TEST( testPoseTranslationFactor, level2_zero_error ) { Pose2 pose1(rot2A, point2A); Pose2TranslationPrior factor(poseKey, point2A, model2); Matrix actH1; @@ -121,7 +121,7 @@ TEST( testRelativeElevationFactor, level2_zero_error ) { } /* ************************************************************************* */ -TEST( testRelativeElevationFactor, level2_error ) { +TEST( testPoseTranslationFactor, level2_error ) { Pose2 pose1(rot2A, point2A); Pose2TranslationPrior factor(poseKey, point2B, model2); Matrix actH1;