12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

added updateNoiseModels_givenCovs to BetweenFactorEM.

release/4.3a0
Vadim Indelman 2014-07-31 16:22:54 +03:00
parent fc58bf36fb
commit 89410fe1ee
2 changed files with 43 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
gtsam_unstable/gtsam_unstable.h
View File

@ -333,6 +333,7 @@ virtual class BetweenFactorEM : gtsam::NonlinearFactor {
bool get_flag_bump_up_near_zero_probs() const; bool get_flag_bump_up_near_zero_probs() const;
void updateNoiseModels(const gtsam::Values& values, const gtsam::NonlinearFactorGraph& graph); 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_inlier_cov();
Matrix get_model_outlier_cov(); Matrix get_model_outlier_cov();

26
gtsam_unstable/slam/BetweenFactorEM.h
View File

@ -324,12 +324,6 @@ namespace gtsam {
* TODO: improve efficiency (info form) * 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 // get joint covariance of the involved states
std::vector<gtsam::Key> Keys; std::vector<gtsam::Key> Keys;
Keys.push_back(key1_); Keys.push_back(key1_);
@ -340,6 +334,26 @@ namespace gtsam {
Matrix cov2 = joint_marginal12(key2_, key2_); Matrix cov2 = joint_marginal12(key2_, key2_);
Matrix cov12 = joint_marginal12(key1_, key2_); Matrix cov12 = joint_marginal12(key1_, key2_);
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>(key1_);
const T& p2 = values.at<T>(key2_);
Matrix H1, H2;
T hx = p1.between(p2, H1, H2); // h(x)
Matrix H; Matrix H;
H.resize(H1.rows(), H1.rows()+H2.rows()); H.resize(H1.rows(), H1.rows()+H2.rows());
H << H1, H2; // H = [H1 H2] H << H1, H2; // H = [H1 H2]