From 6b890cec0eca7e1754de372063c20bec1f2c181c Mon Sep 17 00:00:00 2001
From: Vadim Indelman <vadim.indelman@gmail.com>
Date: Wed, 10 Sep 2014 00:22:59 +0300
Subject: [PATCH] added updateNoiseModels functionality

---
 gtsam_unstable/gtsam_unstable.h               |  5 ++
 .../slam/TransformBtwRobotsUnaryFactorEM.h    | 88 +++++++++++++++++++
 2 files changed, 93 insertions(+)

diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h
index c9a9e0f0f..f2223c2f4 100644
--- a/gtsam_unstable/gtsam_unstable.h
+++ b/gtsam_unstable/gtsam_unstable.h
@@ -359,6 +359,11 @@ virtual class TransformBtwRobotsUnaryFactorEM : gtsam::NonlinearFactor {
   Vector calcIndicatorProb(const gtsam::Values& x);
   void setValAValB(const gtsam::Values valA, const gtsam::Values valB);
 
+  void updateNoiseModels(const gtsam::Values& values, const gtsam::NonlinearFactorGraph& graph);
+  void updateNoiseModels_givenCovs(const gtsam::Values& values, Matrix cov1, Matrix cov2, Matrix cov12);
+  Matrix get_model_inlier_cov();
+  Matrix get_model_outlier_cov();
+
   void serializable() const; // enabling serialization functionality
 };
 
diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h
index 48071f355..d41d895a6 100644
--- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h
+++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h
@@ -302,6 +302,94 @@ namespace gtsam {
       return currA_T_currB_msr.localCoordinates(currA_T_currB_pred);
     }
 
+    /* ************************************************************************* */
+    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)
+       */
+
+       // get joint covariance of the involved states
+       std::vector<gtsam::Key> Keys;
+       Keys.push_back(keyA_);
+       Keys.push_back(keyB_);
+       Marginals marginals( graph, values, Marginals::QR );
+       JointMarginal joint_marginal12 = marginals.jointMarginalCovariance(Keys);
+       Matrix cov1 = joint_marginal12(keyA_, keyA_);
+       Matrix cov2 = joint_marginal12(keyB_, keyB_);
+       Matrix cov12 = joint_marginal12(keyA_, keyB_);
+
+       updateNoiseModels_givenCovs(values, cov1, cov2, cov12);
+    }
+
+
+    /* ************************************************************************* */
+    void updateNoiseModels_givenCovs(const gtsam::Values& values, const Matrix& cov1, const Matrix& cov2, const Matrix& cov12){
+      /* 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>(keyA_);
+      const T& p2 = values.at<T>(keyB_);
+
+      Matrix H1, H2;
+      T hx = p1.between(p2, H1, H2); // h(x)
+
+      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;
+    }
+
+
     /* ************************************************************************* */
 
     /** number of variables attached to this factor */