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

add given marginals to update_noiseModel to improve speed

release/4.3a0
Jing Dong 2014-09-15 03:03:25 -04:00
parent 1a7653730c
commit be68cc0047
1 changed files with 17 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h
View File

@ -26,7 +26,6 @@
#include <gtsam/linear/GaussianFactor.h> #include <gtsam/linear/GaussianFactor.h>
#include <gtsam/slam/BetweenFactor.h> #include <gtsam/slam/BetweenFactor.h>
namespace gtsam { namespace gtsam {
/** /**
@ -325,6 +324,20 @@ namespace gtsam {
return (model_outlier_->R().transpose()*model_outlier_->R()).inverse(); return (model_outlier_->R().transpose()*model_outlier_->R()).inverse();
} }
/* ************************************************************************* */
void updateNoiseModels(const gtsam::Values& values, const gtsam::Marginals& marginals) {
/* given marginals version, don't need to marginal multiple times if update a lot */
std::vector<gtsam::Key> Keys;
Keys.push_back(keyA_);
Keys.push_back(keyB_);
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(const gtsam::Values& values, const gtsam::NonlinearFactorGraph& graph){ void updateNoiseModels(const gtsam::Values& values, const gtsam::NonlinearFactorGraph& graph){
@ -338,19 +351,12 @@ namespace gtsam {
*/ */
// get joint covariance of the involved states // 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); 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); this->updateNoiseModels(values, marginals);
} }
/* ************************************************************************* */ /* ************************************************************************* */
void updateNoiseModels_givenCovs(const gtsam::Values& values, const Matrix& cov1, const Matrix& cov2, const Matrix& 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 /* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories