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;
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();

70
gtsam_unstable/slam/BetweenFactorEM.h
View File

@ -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;
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;
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;
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 {