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_;
/** 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:
@ -65,32 +64,34 @@ namespace gtsam {
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);
}
@ -100,10 +101,12 @@ namespace gtsam {
const This *t = dynamic_cast<const This*>(&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_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,11 +168,13 @@ 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 << sqrt(p_inlier) * err_wh_inlier.array(), sqrt(p_outlier)
* err_wh_outlier.array();
if (H) {
// stack Jacobians for the two indicators for each of the key
@ -219,7 +225,6 @@ namespace gtsam {
// std::cout<<"===="<<std::endl;
}
return err_wh_eq;
}
@ -235,18 +240,27 @@ 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;
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;
@ -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
* (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
* (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_);
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());
@ -360,19 +376,22 @@ namespace gtsam {
Matrix joint_cov;
joint_cov.resize(cov1.rows() + cov2.rows(), cov1.cols() + cov2.cols());
joint_cov << cov1, cov12,
cov12.transpose(), cov2;
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 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;
@ -399,10 +418,12 @@ namespace gtsam {
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