Merge branch 'master' into retraction_name

gtsam.h

@@ -45,10 +45,13 @@ class Pose2 {
 
 class SharedGaussian {
 	SharedGaussian(Matrix covariance);
+	void print(string s) const;
 };
 
 class SharedDiagonal {
 	SharedDiagonal(Vector sigmas);
+	void print(string s) const;
+	Vector sample() const;
 };
 
 class VectorValues {

gtsam/linear/NoiseModel.cpp

@@ -535,7 +535,7 @@ void Base::reweight(Matrix &A1, Matrix &A2, Matrix &A3, Vector &error) const {
 // Null model
 /* ************************************************************************* */
 
-void Null::print(const std::string &s) const
+void Null::print(const std::string &s="") const
 { cout << s << ": null ()" << endl; }
 
 Null::shared_ptr Null::Create()
@@ -556,7 +556,7 @@ Fair::Fair(const double c, const ReweightScheme reweight)
 double Fair::weight(const double &error) const
 { return 1.0 / (1.0 + fabs(error)/c_); }
 
-void Fair::print(const std::string &s) const
+void Fair::print(const std::string &s="") const
 { cout << s << ": fair (" << c_ << ")" << endl; }
 
 bool Fair::equals(const Base &expected, const double tol) const {
@@ -584,7 +584,7 @@ double Huber::weight(const double &error) const {
 	return (error < k_) ? (1.0) : (k_ / fabs(error));
 }
 
-void Huber::print(const std::string &s) const {
+void Huber::print(const std::string &s="") const {
 	cout << s << ": huber (" << k_ << ")" << endl;
 }
 

gtsam/linear/SharedDiagonal.h

@@ -44,6 +44,12 @@ namespace gtsam { // note, deliberately not in noiseModel namespace
 			noiseModel::Diagonal::shared_ptr(noiseModel::Diagonal::Sigmas(sigmas)) {
 		}
 
+		/// Print
+		inline void print(const std::string &s) const { (*this)->print(s); }
+
+		/// Generate a sample
+		inline Vector sample() const { return (*this)->sample(); }
+
 		/** Serialization function */
 		friend class boost::serialization::access;
 		template<class ARCHIVE>

gtsam/linear/SharedGaussian.h

@@ -55,6 +55,9 @@ namespace gtsam { // note, deliberately not in noiseModel namespace
 			Base(noiseModel::Isotropic::Sigma(GTSAM_DANGEROUS_GAUSSIAN, s)) {}
 #endif
 
+		/// Print
+		inline void print(const std::string &s) const { (*this)->print(s); }
+
 		/** Serialization function */
 		friend class boost::serialization::access;
 		template<class ARCHIVE>

gtsam/linear/tests/testNoiseModel.cpp

@@ -134,6 +134,14 @@ TEST(NoiseModel, equals)
 	EXPECT(assert_inequal(*i1,*i2));
 }
 
+/* ************************************************************************* */
+TEST(NoiseModel, sample) {
+	Vector s = Vector_(3,1.0,2.0,3.0);
+	SharedDiagonal model = sharedSigmas(s);
+	Vector v = model->sample();
+	// no check as no way yet to set random seed
+}
+
 /* ************************************************************************* */
 TEST(NoiseModel, ConstrainedMixed )
 {

gtsam/slam/tests/testPose2SLAM.cpp

@@ -44,13 +44,37 @@ static noiseModel::Gaussian::shared_ptr covariance(
 //static noiseModel::Gaussian::shared_ptr I3(noiseModel::Unit::Create(3));
 
 /* ************************************************************************* */
-Vector f(const Pose2& pose1, const Pose2& pose2) {
+// Test constraint, small displacement
+Vector f1(const Pose2& pose1, const Pose2& pose2) {
+	Pose2 z(2.1130913087, 0.468461064817, 0.436332312999);
+	Pose2Factor constraint(1, 2, z, covariance);
+	return constraint.evaluateError(pose1, pose2);
+}
+
+TEST( Pose2SLAM, constraint1 )
+{
+	// create a factor between unknown poses p1 and p2
+	Pose2 pose1, pose2(2.1130913087, 0.468461064817, 0.436332312999);
+	Pose2Factor constraint(1, 2, pose2, covariance);
+	Matrix H1, H2;
+	Vector actual = constraint.evaluateError(pose1, pose2, H1, H2);
+
+	Matrix numericalH1 = numericalDerivative21(&f1 , pose1, pose2);
+	EXPECT(assert_equal(numericalH1,H1,5e-3));
+
+	Matrix numericalH2 = numericalDerivative22(&f1 , pose1, pose2);
+	EXPECT(assert_equal(numericalH2,H2));
+}
+
+/* ************************************************************************* */
+// Test constraint, large displacement
+Vector f2(const Pose2& pose1, const Pose2& pose2) {
 	Pose2 z(2,2,M_PI_2);
 	Pose2Factor constraint(1, 2, z, covariance);
 	return constraint.evaluateError(pose1, pose2);
 }
 
-TEST( Pose2SLAM, constraint )
+TEST( Pose2SLAM, constraint2 )
 {
 	// create a factor between unknown poses p1 and p2
 	Pose2 pose1, pose2(2,2,M_PI_2);
@@ -58,10 +82,10 @@ TEST( Pose2SLAM, constraint )
 	Matrix H1, H2;
 	Vector actual = constraint.evaluateError(pose1, pose2, H1, H2);
 
-	Matrix numericalH1 = numericalDerivative21(&f , pose1, pose2);
+	Matrix numericalH1 = numericalDerivative21(&f2 , pose1, pose2);
 	EXPECT(assert_equal(numericalH1,H1,5e-3));
 
-	Matrix numericalH2 = numericalDerivative22(&f , pose1, pose2);
+	Matrix numericalH2 = numericalDerivative22(&f2 , pose1, pose2);
 	EXPECT(assert_equal(numericalH2,H2));
 }