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

No this-> needed

release/4.3a0
dellaert 2014-11-02 14:35:22 +01:00
parent 90a0fa6e45
commit d0e004c05f
1 changed files with 10 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
gtsam/nonlinear/NonlinearFactor.cpp
View File

@ -25,7 +25,7 @@ namespace gtsam {
void NonlinearFactor::print(const std::string& s, void NonlinearFactor::print(const std::string& s,
const KeyFormatter& keyFormatter) const { const KeyFormatter& keyFormatter) const {
std::cout << s << " keys = { "; std::cout << s << " keys = { ";
BOOST_FOREACH(Key key, this->keys()) { BOOST_FOREACH(Key key, keys()) {
std::cout << keyFormatter(key) << " "; std::cout << keyFormatter(key) << " ";
} }
std::cout << "}" << std::endl; std::cout << "}" << std::endl;
@ -52,7 +52,7 @@ NonlinearFactor::shared_ptr NonlinearFactor::rekey(
/* ************************************************************************* */ /* ************************************************************************* */
NonlinearFactor::shared_ptr NonlinearFactor::rekey( NonlinearFactor::shared_ptr NonlinearFactor::rekey(
const std::vector<Key>& new_keys) const { const std::vector<Key>& new_keys) const {
assert(new_keys.size() == this->keys().size()); assert(new_keys.size() == keys().size());
shared_ptr new_factor = clone(); shared_ptr new_factor = clone();
new_factor->keys() = new_keys; new_factor->keys() = new_keys;
return new_factor; return new_factor;
@ -62,7 +62,7 @@ NonlinearFactor::shared_ptr NonlinearFactor::rekey(
void NoiseModelFactor::print(const std::string& s, void NoiseModelFactor::print(const std::string& s,
const KeyFormatter& keyFormatter) const { const KeyFormatter& keyFormatter) const {
Base::print(s, keyFormatter); Base::print(s, keyFormatter);
this->noiseModel_->print(" noise model: "); noiseModel_->print(" noise model: ");
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -92,7 +92,7 @@ Vector NoiseModelFactor::whitenedError(const Values& c) const {
/* ************************************************************************* */ /* ************************************************************************* */
double NoiseModelFactor::error(const Values& c) const { double NoiseModelFactor::error(const Values& c) const {
if (this->active(c)) { if (active(c)) {
const Vector b = unwhitenedError(c); const Vector b = unwhitenedError(c);
check(noiseModel_, b.size()); check(noiseModel_, b.size());
return 0.5 * noiseModel_->distance(b); return 0.5 * noiseModel_->distance(b);
@ -106,21 +106,21 @@ boost::shared_ptr<GaussianFactor> NoiseModelFactor::linearize(
const Values& x) const { const Values& x) const {
// Only linearize if the factor is active // Only linearize if the factor is active
if (!this->active(x)) if (!active(x))
return boost::shared_ptr<JacobianFactor>(); return boost::shared_ptr<JacobianFactor>();
// Call evaluate error to get Jacobians and RHS vector b // Call evaluate error to get Jacobians and RHS vector b
std::vector<Matrix> A(this->size()); std::vector<Matrix> A(size());
Vector b = -unwhitenedError(x, A); Vector b = -unwhitenedError(x, A);
check(noiseModel_, b.size()); check(noiseModel_, b.size());
// Whiten the corresponding system now // Whiten the corresponding system now
this->noiseModel_->WhitenSystem(A, b); noiseModel_->WhitenSystem(A, b);
// Fill in terms, needed to create JacobianFactor below // Fill in terms, needed to create JacobianFactor below
std::vector<std::pair<Key, Matrix> > terms(this->size()); std::vector<std::pair<Key, Matrix> > terms(size());
for (size_t j = 0; j < this->size(); ++j) { for (size_t j = 0; j < size(); ++j) {
terms[j].first = this->keys()[j]; terms[j].first = keys()[j];
terms[j].second.swap(A[j]); terms[j].second.swap(A[j]);
} }