Updating EM factor (to accout for uncertainty in states)

gtsam_unstable/gtsam_unstable.h

@@ -332,6 +332,10 @@ virtual class BetweenFactorEM : gtsam::NonlinearFactor {
   void set_flag_bump_up_near_zero_probs(bool flag);
   bool get_flag_bump_up_near_zero_probs() const;
 
+  void updateNoiseModels(const gtsam::Values& values, const gtsam::NonlinearFactorGraph& graph);
+  Matrix get_model_inlier_cov();
+  Matrix get_model_outlier_cov();
+
   void serializable() const; // enabling serialization functionality
 };
 

gtsam_unstable/slam/BetweenFactorEM.h

@@ -21,6 +21,7 @@
 #include <gtsam/base/Lie.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/linear/GaussianFactor.h>
+#include <gtsam/nonlinear/Marginals.h>
 
 namespace gtsam {
 
@@ -292,6 +293,77 @@ namespace gtsam {
       return flag_bump_up_near_zero_probs_;
     }
 
+    /* ************************************************************************* */
+    SharedGaussian get_model_inlier() const {
+    	return model_inlier_;
+    }
+
+    /* ************************************************************************* */
+    SharedGaussian get_model_outlier() const {
+    	return model_outlier_;
+    }
+
+    /* ************************************************************************* */
+    Matrix get_model_inlier_cov() const {
+    	return (model_inlier_->R().transpose()*model_inlier_->R()).inverse();
+    }
+
+    /* ************************************************************************* */
+    Matrix get_model_outlier_cov() const {
+    	return (model_outlier_->R().transpose()*model_outlier_->R()).inverse();
+    }
+
+    /* ************************************************************************* */
+    void updateNoiseModels(const gtsam::Values& values, const gtsam::NonlinearFactorGraph& graph){
+    	/* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories
+    	 * (note these are given in the E step, where indicator probabilities are calculated).
+    	 *
+    	 * Principle: R += [H1 H2] * joint_cov12 * [H1 H2]', where H1, H2 are Jacobians of the
+    	 * unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes).
+    	 *
+    	 * TODO: improve efficiency (info form)
+    	 */
+
+    	 const T& p1 = values.at<T>(key1_);
+    	 const T& p2 = values.at<T>(key2_);
+
+    	 Matrix H1, H2;
+    	 T hx = p1.between(p2, H1, H2); // h(x)
+
+    	 // get joint covariance of the involved states
+    	 std::vector<gtsam::Key> Keys;
+    	 Keys.push_back(key1_);
+    	 Keys.push_back(key2_);
+    	 Marginals marginals( graph, values, Marginals::QR );
+    	 JointMarginal joint_marginal12 = marginals.jointMarginalCovariance(Keys);
+    	 Matrix cov1 = joint_marginal12(key1_, key1_);
+    	 Matrix cov2 = joint_marginal12(key2_, key2_);
+    	 Matrix cov12 = joint_marginal12(key1_, key2_);
+
+    	 Matrix H;
+    	 H.resize(H1.rows(), H1.rows()+H2.rows());
+    	 H << H1, H2; // H = [H1 H2]
+
+    	 Matrix joint_cov;
+    	 joint_cov.resize(cov1.rows()+cov2.rows(), cov1.cols()+cov2.cols());
+    	 joint_cov << cov1, cov12,
+    			 cov12.transpose(), cov2;
+
+    	 Matrix cov_state = H*joint_cov*H.transpose();
+
+    	 //    	 model_inlier_->print("before:");
+
+    	 // update inlier and outlier noise models
+    	 Matrix covRinlier = (model_inlier_->R().transpose()*model_inlier_->R()).inverse();
+    	 model_inlier_ = gtsam::noiseModel::Gaussian::Covariance(covRinlier + cov_state);
+
+    	 Matrix covRoutlier = (model_outlier_->R().transpose()*model_outlier_->R()).inverse();
+    	 model_outlier_ = gtsam::noiseModel::Gaussian::Covariance(covRoutlier + cov_state);
+
+    	 //    	 model_inlier_->print("after:");
+    	 //    	 std::cout<<"covRinlier + cov_state: "<<covRinlier + cov_state<<std::endl;
+    }
+
     /* ************************************************************************* */
     /** return the measured */
     const VALUE& measured() const {

gtsam_unstable/slam/tests/testBetweenFactorEM.cpp

@@ -14,11 +14,13 @@
 #include <gtsam/base/numericalDerivative.h>
 
 #include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/slam/PriorFactor.h>
 
 
 using namespace std;
 using namespace gtsam;
 
+
 // Disabled this test because it is currently failing - remove the lines "#if 0" and "#endif" below
 // to reenable the test.
 #if 0
@@ -251,6 +253,49 @@ TEST( BetweenFactorEM, CaseStudy)
   }
 }
 
+
+///* ************************************************************************** */
+TEST (BetweenFactorEM, updateNoiseModel ) {
+	gtsam::Key key1(1);
+	gtsam::Key key2(2);
+
+	gtsam::Pose2 p1(10.0, 15.0, 0.1);
+	gtsam::Pose2 p2(15.0, 15.0, 0.3);
+	gtsam::Pose2 noise(0.5, 0.4, 0.01);
+	gtsam::Pose2 rel_pose_ideal = p1.between(p2);
+	gtsam::Pose2 rel_pose_msr   = rel_pose_ideal.compose(noise);
+
+	SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas( (gtsam::Vector(3) << 1.5, 2.5, 4.05)));
+	SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas( (gtsam::Vector(3) << 50.0, 50.0, 10.0)));
+
+	gtsam::Values values;
+	values.insert(key1, p1);
+	values.insert(key2, p2);
+
+	double prior_outlier = 0.0;
+	double prior_inlier = 1.0;
+
+	BetweenFactorEM<gtsam::Pose2> f(key1, key2, rel_pose_msr, model_inlier, model_outlier,
+			prior_inlier, prior_outlier);
+
+	SharedGaussian model = SharedGaussian(noiseModel::Isotropic::Sigma(3, 1e2));
+
+	NonlinearFactorGraph graph;
+	graph.push_back(gtsam::PriorFactor<Pose2>(key1, p1, model));
+	graph.push_back(gtsam::PriorFactor<Pose2>(key2, p2, model));
+
+	f.updateNoiseModels(values, graph);
+
+	SharedGaussian model_inlier_new = f.get_model_inlier();
+	SharedGaussian model_outlier_new = f.get_model_outlier();
+
+	model_inlier->print("model_inlier:");
+	model_outlier->print("model_outlier:");
+	model_inlier_new->print("model_inlier_new:");
+	model_outlier_new->print("model_outlier_new:");
+}
+
+
 #endif
 
 /* ************************************************************************* */