added updateNoiseModels_givenCovs to BetweenFactorEM.

gtsam_unstable/gtsam_unstable.h

@@ -333,6 +333,7 @@ virtual class BetweenFactorEM : gtsam::NonlinearFactor {
   bool get_flag_bump_up_near_zero_probs() const;
 
   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();
 

gtsam_unstable/slam/BetweenFactorEM.h

@@ -324,12 +324,6 @@ namespace gtsam {
     	 * 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_);
@@ -340,30 +334,50 @@ namespace gtsam {
     	 Matrix cov2 = joint_marginal12(key2_, key2_);
     	 Matrix cov12 = joint_marginal12(key1_, key2_);
 
-    	 Matrix H;
+    	 updateNoiseModels_givenCovs(values, cov1, cov2, cov12);
-    	 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;
     }
 
+    /* ************************************************************************* */
+        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>(key1_);
+        	 const T& p2 = values.at<T>(key2_);
+
+        	 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;
+        }
+
     /* ************************************************************************* */
     /** return the measured */
     const VALUE& measured() const {