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