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

Avoid warning and re-formatted with BORG template

release/4.3a0
dellaert 2014-11-10 16:00:44 +01:00
parent 9391decc91
commit e976aae38a
1 changed files with 356 additions and 335 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

161
gtsam_unstable/slam/BetweenFactorEM.h
View File

@ -25,19 +25,19 @@
namespace gtsam {
/**
/**
* A class for a measurement predicted by "between(config[key1],config[key2])"
* @tparam VALUE the Value type
* @addtogroup SLAM
*/
template<class VALUE>
class BetweenFactorEM: public NonlinearFactor {
template<class VALUE>
class BetweenFactorEM: public NonlinearFactor {
public:
public:
typedef VALUE T;
private:
private:
typedef BetweenFactorEM<VALUE> This;
typedef gtsam::NonlinearFactor Base;
@ -56,54 +56,57 @@ namespace gtsam {
bool flag_bump_up_near_zero_probs_;
/** concept check by type */
GTSAM_CONCEPT_LIE_TYPE(T)
GTSAM_CONCEPT_TESTABLE_TYPE(T)
GTSAM_CONCEPT_LIE_TYPE(T)GTSAM_CONCEPT_TESTABLE_TYPE(T)
public:
public:
// shorthand for a smart pointer to a factor
typedef typename boost::shared_ptr<BetweenFactorEM> shared_ptr;
/** default constructor - only use for serialization */
BetweenFactorEM() {}
BetweenFactorEM() {
}
/** Constructor */
BetweenFactorEM(Key key1, Key key2, const VALUE& measured,
const SharedGaussian& model_inlier, const SharedGaussian& model_outlier,
const double prior_inlier, const double prior_outlier, const bool flag_bump_up_near_zero_probs = false) :
Base(cref_list_of<2>(key1)(key2)), key1_(key1), key2_(key2), measured_(measured),
model_inlier_(model_inlier), model_outlier_(model_outlier),
prior_inlier_(prior_inlier), prior_outlier_(prior_outlier), flag_bump_up_near_zero_probs_(flag_bump_up_near_zero_probs){
const double prior_inlier, const double prior_outlier,
const bool flag_bump_up_near_zero_probs = false) :
Base(cref_list_of<2>(key1)(key2)), key1_(key1), key2_(key2), measured_(
measured), model_inlier_(model_inlier), model_outlier_(model_outlier), prior_inlier_(
prior_inlier), prior_outlier_(prior_outlier), flag_bump_up_near_zero_probs_(
flag_bump_up_near_zero_probs) {
}
virtual ~BetweenFactorEM() {}
virtual ~BetweenFactorEM() {
}
/** implement functions needed for Testable */
/** print */
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "BetweenFactorEM("
<< keyFormatter(key1_) << ","
virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const {
std::cout << s << "BetweenFactorEM(" << keyFormatter(key1_) << ","
<< keyFormatter(key2_) << ")\n";
measured_.print(" measured: ");
model_inlier_->print(" noise model inlier: ");
model_outlier_->print(" noise model outlier: ");
std::cout << "(prior_inlier, prior_outlier_) = ("
<< prior_inlier_ << ","
std::cout << "(prior_inlier, prior_outlier_) = (" << prior_inlier_ << ","
<< prior_outlier_ << ")\n";
// Base::print(s, keyFormatter);
}
/** equals */
virtual bool equals(const NonlinearFactor& f, double tol=1e-9) const {
const This *t = dynamic_cast<const This*> (&f);
virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
const This *t = dynamic_cast<const This*>(&f);
if(t && Base::equals(f))
return key1_ == t->key1_ && key2_ == t->key2_ &&
if (t && Base::equals(f))
return key1_ == t->key1_ && key2_ == t->key2_
&&
// model_inlier_->equals(t->model_inlier_ ) && // TODO: fix here
// model_outlier_->equals(t->model_outlier_ ) &&
prior_outlier_ == t->prior_outlier_ && prior_inlier_ == t->prior_inlier_ && measured_.equals(t->measured_);
prior_outlier_ == t->prior_outlier_
&& prior_inlier_ == t->prior_inlier_ && measured_.equals(t->measured_);
else
return false;
}
@ -122,7 +125,8 @@ namespace gtsam {
* Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$
*/
/* This version of linearize recalculates the noise model each time */
virtual boost::shared_ptr<gtsam::GaussianFactor> linearize(const gtsam::Values& x) const {
virtual boost::shared_ptr<gtsam::GaussianFactor> linearize(
const gtsam::Values& x) const {
// Only linearize if the factor is active
if (!this->active(x))
return boost::shared_ptr<gtsam::JacobianFactor>();
@ -135,10 +139,10 @@ namespace gtsam {
A2 = A[1];
return gtsam::GaussianFactor::shared_ptr(
new gtsam::JacobianFactor(key1_, A1, key2_, A2, b, gtsam::noiseModel::Unit::Create(b.size())));
new gtsam::JacobianFactor(key1_, A1, key2_, A2, b,
gtsam::noiseModel::Unit::Create(b.size())));
}
/* ************************************************************************* */
gtsam::Vector whitenedError(const gtsam::Values& x,
boost::optional<std::vector<gtsam::Matrix>&> H = boost::none) const {
@ -164,31 +168,33 @@ namespace gtsam {
Vector err_wh_outlier = model_outlier_->whiten(err);
Matrix invCov_inlier = model_inlier_->R().transpose() * model_inlier_->R();
Matrix invCov_outlier = model_outlier_->R().transpose() * model_outlier_->R();
Matrix invCov_outlier = model_outlier_->R().transpose()
* model_outlier_->R();
Vector err_wh_eq;
err_wh_eq.resize(err_wh_inlier.rows()*2);
err_wh_eq << sqrt(p_inlier) * err_wh_inlier.array() , sqrt(p_outlier) * err_wh_outlier.array();
err_wh_eq.resize(err_wh_inlier.rows() * 2);
err_wh_eq << sqrt(p_inlier) * err_wh_inlier.array(), sqrt(p_outlier)
* err_wh_outlier.array();
if (H){
if (H) {
// stack Jacobians for the two indicators for each of the key
Matrix H1_inlier = sqrt(p_inlier)*model_inlier_->Whiten(H1);
Matrix H1_outlier = sqrt(p_outlier)*model_outlier_->Whiten(H1);
Matrix H1_inlier = sqrt(p_inlier) * model_inlier_->Whiten(H1);
Matrix H1_outlier = sqrt(p_outlier) * model_outlier_->Whiten(H1);
Matrix H1_aug = gtsam::stack(2, &H1_inlier, &H1_outlier);
Matrix H2_inlier = sqrt(p_inlier)*model_inlier_->Whiten(H2);
Matrix H2_outlier = sqrt(p_outlier)*model_outlier_->Whiten(H2);
Matrix H2_inlier = sqrt(p_inlier) * model_inlier_->Whiten(H2);
Matrix H2_outlier = sqrt(p_outlier) * model_outlier_->Whiten(H2);
Matrix H2_aug = gtsam::stack(2, &H2_inlier, &H2_outlier);
(*H)[0].resize(H1_aug.rows(),H1_aug.cols());
(*H)[1].resize(H2_aug.rows(),H2_aug.cols());
(*H)[0].resize(H1_aug.rows(), H1_aug.cols());
(*H)[1].resize(H2_aug.rows(), H2_aug.cols());
(*H)[0] = H1_aug;
(*H)[1] = H2_aug;
}
if (debug){
if (debug) {
// std::cout<<"unwhitened error: "<<err[0]<<" "<<err[1]<<" "<<err[2]<<std::endl;
// std::cout<<"err_wh_inlier: "<<err_wh_inlier[0]<<" "<<err_wh_inlier[1]<<" "<<err_wh_inlier[2]<<std::endl;
// std::cout<<"err_wh_outlier: "<<err_wh_outlier[0]<<" "<<err_wh_outlier[1]<<" "<<err_wh_outlier[2]<<std::endl;
@ -219,7 +225,6 @@ namespace gtsam {
// std::cout<<"===="<<std::endl;
}
return err_wh_eq;
}
@ -235,28 +240,37 @@ namespace gtsam {
Vector err_wh_outlier = model_outlier_->whiten(err);
Matrix invCov_inlier = model_inlier_->R().transpose() * model_inlier_->R();
Matrix invCov_outlier = model_outlier_->R().transpose() * model_outlier_->R();
Matrix invCov_outlier = model_outlier_->R().transpose()
* model_outlier_->R();
double p_inlier = prior_inlier_ * std::sqrt(invCov_inlier.determinant()) * exp( -0.5 * err_wh_inlier.dot(err_wh_inlier) );
double p_outlier = prior_outlier_ * std::sqrt(invCov_outlier.determinant()) * exp( -0.5 * err_wh_outlier.dot(err_wh_outlier) );
double p_inlier = prior_inlier_ * std::sqrt(invCov_inlier.determinant())
* exp(-0.5 * err_wh_inlier.dot(err_wh_inlier));
double p_outlier = prior_outlier_ * std::sqrt(invCov_outlier.determinant())
* exp(-0.5 * err_wh_outlier.dot(err_wh_outlier));
if (debug){
std::cout<<"in calcIndicatorProb. err_unwh: "<<err[0]<<", "<<err[1]<<", "<<err[2]<<std::endl;
std::cout<<"in calcIndicatorProb. err_wh_inlier: "<<err_wh_inlier[0]<<", "<<err_wh_inlier[1]<<", "<<err_wh_inlier[2]<<std::endl;
std::cout<<"in calcIndicatorProb. err_wh_inlier.dot(err_wh_inlier): "<<err_wh_inlier.dot(err_wh_inlier)<<std::endl;
std::cout<<"in calcIndicatorProb. err_wh_outlier.dot(err_wh_outlier): "<<err_wh_outlier.dot(err_wh_outlier)<<std::endl;
if (debug) {
std::cout << "in calcIndicatorProb. err_unwh: " << err[0] << ", "
<< err[1] << ", " << err[2] << std::endl;
std::cout << "in calcIndicatorProb. err_wh_inlier: " << err_wh_inlier[0]
<< ", " << err_wh_inlier[1] << ", " << err_wh_inlier[2] << std::endl;
std::cout << "in calcIndicatorProb. err_wh_inlier.dot(err_wh_inlier): "
<< err_wh_inlier.dot(err_wh_inlier) << std::endl;
std::cout << "in calcIndicatorProb. err_wh_outlier.dot(err_wh_outlier): "
<< err_wh_outlier.dot(err_wh_outlier) << std::endl;
std::cout<<"in calcIndicatorProb. p_inlier, p_outlier before normalization: "<<p_inlier<<", "<<p_outlier<<std::endl;
std::cout
<< "in calcIndicatorProb. p_inlier, p_outlier before normalization: "
<< p_inlier << ", " << p_outlier << std::endl;
}
double sumP = p_inlier + p_outlier;
p_inlier /= sumP;
p_outlier /= sumP;
if (flag_bump_up_near_zero_probs_){
if (flag_bump_up_near_zero_probs_) {
// Bump up near-zero probabilities (as in linerFlow.h)
double minP = 0.05; // == 0.1 / 2 indicator variables
if (p_inlier < minP || p_outlier < minP){
if (p_inlier < minP || p_outlier < minP) {
if (p_inlier < minP)
p_inlier = minP;
if (p_outlier < minP)
@ -305,16 +319,17 @@ namespace gtsam {
/* ************************************************************************* */
Matrix get_model_inlier_cov() const {
return (model_inlier_->R().transpose()*model_inlier_->R()).inverse();
return (model_inlier_->R().transpose() * model_inlier_->R()).inverse();
}
/* ************************************************************************* */
Matrix get_model_outlier_cov() const {
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::NonlinearFactorGraph& graph){
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).
*
@ -328,7 +343,7 @@ namespace gtsam {
std::vector<gtsam::Key> Keys;
Keys.push_back(key1_);
Keys.push_back(key2_);
Marginals marginals( graph, values, Marginals::QR );
Marginals marginals(graph, values, Marginals::QR);
JointMarginal joint_marginal12 = marginals.jointMarginalCovariance(Keys);
Matrix cov1 = joint_marginal12(key1_, key1_);
Matrix cov2 = joint_marginal12(key2_, key2_);
@ -338,7 +353,8 @@ namespace gtsam {
}
/* ************************************************************************* */
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
* (note these are given in the E step, where indicator probabilities are calculated).
*
@ -352,27 +368,30 @@ namespace gtsam {
const T& p2 = values.at<T>(key2_);
Matrix H1, H2;
T hx = p1.between(p2, H1, H2); // h(x)
p1.between(p2, H1, H2); // h(x)
Matrix H;
H.resize(H1.rows(), H1.rows()+H2.rows());
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;
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();
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 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);
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;
@ -393,16 +412,18 @@ namespace gtsam {
return model_inlier_->R().rows() + model_inlier_->R().cols();
}
private:
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearFactor",
ar
& boost::serialization::make_nvp("NonlinearFactor",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_);
}
}; // \class BetweenFactorEM
};
// \class BetweenFactorEM
} /// namespace gtsam
}/// namespace gtsam