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

103
gtsam_unstable/slam/BetweenFactorEM.h
View File

@ -56,8 +56,7 @@ 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:
@ -65,32 +64,34 @@ namespace gtsam {
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);
} }
@ -100,10 +101,12 @@ namespace gtsam {
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,11 +168,13 @@ 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
@ -219,7 +225,6 @@ namespace gtsam {
// std::cout<<"===="<<std::endl; // std::cout<<"===="<<std::endl;
} }
return err_wh_eq; return err_wh_eq;
} }
@ -235,18 +240,27 @@ 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;
@ -314,7 +328,8 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
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).
* *
@ -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,7 +368,7 @@ 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());
@ -360,19 +376,22 @@ namespace gtsam {
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;
@ -399,10 +418,12 @@ namespace gtsam {
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