revert confusing `->template key<N>()` notations
revert to using shortcut form: `key1()`, `key2()`release/4.3a0
parent
69226edd81
commit
97fcb409b9
|
@ -200,8 +200,8 @@ boost::shared_ptr<Values> composePoses(const G& graph, const PredecessorMap<KEY>
|
|||
boost::shared_ptr<Factor> factor = boost::dynamic_pointer_cast<Factor>(nl_factor);
|
||||
if (!factor) continue;
|
||||
|
||||
KEY key1 = factor->template key<1>();
|
||||
KEY key2 = factor->template key<2>();
|
||||
KEY key1 = factor->key1();
|
||||
KEY key2 = factor->key2();
|
||||
|
||||
PoseVertex v1 = key2vertex.find(key1)->second;
|
||||
PoseVertex v2 = key2vertex.find(key2)->second;
|
||||
|
@ -270,8 +270,8 @@ void split(const G& g, const PredecessorMap<KEY>& tree, G& Ab1, G& Ab2) {
|
|||
FACTOR2>(factor);
|
||||
if (!factor2) continue;
|
||||
|
||||
KEY key1 = factor2->template key<1>();
|
||||
KEY key2 = factor2->template key<2>();
|
||||
KEY key1 = factor2->key1();
|
||||
KEY key2 = factor2->key2();
|
||||
// if the tree contains the key
|
||||
if ((tree.find(key1) != tree.end() &&
|
||||
tree.find(key1)->second.compare(key2) == 0) ||
|
||||
|
|
|
@ -101,7 +101,7 @@ class FunctorizedFactor : public NoiseModelFactorN<T> {
|
|||
const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override {
|
||||
Base::print(s, keyFormatter);
|
||||
std::cout << s << (s != "" ? " " : "") << "FunctorizedFactor("
|
||||
<< keyFormatter(this->template key<1>()) << ")" << std::endl;
|
||||
<< keyFormatter(key1()) << ")" << std::endl;
|
||||
traits<R>::Print(measured_, " measurement: ");
|
||||
std::cout << " noise model sigmas: " << noiseModel_->sigmas().transpose()
|
||||
<< std::endl;
|
||||
|
@ -208,8 +208,8 @@ class FunctorizedFactor2 : public NoiseModelFactorN<T1, T2> {
|
|||
const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override {
|
||||
Base::print(s, keyFormatter);
|
||||
std::cout << s << (s != "" ? " " : "") << "FunctorizedFactor2("
|
||||
<< keyFormatter(this->template key<1>()) << ", "
|
||||
<< keyFormatter(this->template key<2>()) << ")" << std::endl;
|
||||
<< keyFormatter(key1()) << ", "
|
||||
<< keyFormatter(key2()) << ")" << std::endl;
|
||||
traits<R>::Print(measured_, " measurement: ");
|
||||
std::cout << " noise model sigmas: " << noiseModel_->sigmas().transpose()
|
||||
<< std::endl;
|
||||
|
|
|
@ -88,8 +88,8 @@ namespace gtsam {
|
|||
const std::string& s = "",
|
||||
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
|
||||
std::cout << s << "BetweenFactor("
|
||||
<< keyFormatter(this->template key<1>()) << ","
|
||||
<< keyFormatter(this->template key<2>()) << ")\n";
|
||||
<< keyFormatter(key1()) << ","
|
||||
<< keyFormatter(key2()) << ")\n";
|
||||
traits<T>::Print(measured_, " measured: ");
|
||||
this->noiseModel_->print(" noise model: ");
|
||||
}
|
||||
|
|
|
@ -129,7 +129,7 @@ struct BoundingConstraint2: public NoiseModelFactorN<VALUE1, VALUE2> {
|
|||
/** active when constraint *NOT* met */
|
||||
bool active(const Values& c) const override {
|
||||
// note: still active at equality to avoid zigzagging
|
||||
double x = value(c.at<X1>(this->template key<1>()), c.at<X2>(this->template key<2>()));
|
||||
double x = value(c.at<X1>(key1()), c.at<X2>(key2()));
|
||||
return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
|
||||
}
|
||||
|
||||
|
|
|
@ -130,8 +130,8 @@ class FrobeniusBetweenFactor : public NoiseModelFactorN<Rot, Rot> {
|
|||
print(const std::string &s,
|
||||
const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override {
|
||||
std::cout << s << "FrobeniusBetweenFactor<" << demangle(typeid(Rot).name())
|
||||
<< ">(" << keyFormatter(this->template key<1>()) << ","
|
||||
<< keyFormatter(this->template key<2>()) << ")\n";
|
||||
<< ">(" << keyFormatter(key1()) << ","
|
||||
<< keyFormatter(key2()) << ")\n";
|
||||
traits<Rot>::Print(R12_, " R12: ");
|
||||
this->noiseModel_->print(" noise model: ");
|
||||
}
|
||||
|
|
|
@ -140,7 +140,7 @@ public:
|
|||
// Only linearize if the factor is active
|
||||
if (!this->active(values)) return boost::shared_ptr<JacobianFactor>();
|
||||
|
||||
const Key key1 = this->template key<1>(), key2 = this->template key<2>();
|
||||
const Key key1 = key1(), key2 = key2();
|
||||
JacobianC H1;
|
||||
JacobianL H2;
|
||||
Vector2 b;
|
||||
|
@ -270,8 +270,8 @@ public:
|
|||
if (H1) *H1 = Matrix::Zero(2, 6);
|
||||
if (H2) *H2 = Matrix::Zero(2, 3);
|
||||
if (H3) *H3 = Matrix::Zero(2, DimK);
|
||||
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->template key<2>())
|
||||
<< " behind Camera " << DefaultKeyFormatter(this->template key<1>()) << std::endl;
|
||||
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(key2())
|
||||
<< " behind Camera " << DefaultKeyFormatter(key1()) << std::endl;
|
||||
}
|
||||
return Z_2x1;
|
||||
}
|
||||
|
|
|
@ -154,10 +154,10 @@ namespace gtsam {
|
|||
if (H1) *H1 = Matrix::Zero(2,6);
|
||||
if (H2) *H2 = Matrix::Zero(2,3);
|
||||
if (verboseCheirality_)
|
||||
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->template key<2>()) <<
|
||||
" moved behind camera " << DefaultKeyFormatter(this->template key<1>()) << std::endl;
|
||||
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(key2()) <<
|
||||
" moved behind camera " << DefaultKeyFormatter(key1()) << std::endl;
|
||||
if (throwCheirality_)
|
||||
throw CheiralityException(this->template key<2>());
|
||||
throw CheiralityException(key2());
|
||||
}
|
||||
return Vector2::Constant(2.0 * K_->fx());
|
||||
}
|
||||
|
|
|
@ -107,16 +107,16 @@ public:
|
|||
void print(const std::string& s="",
|
||||
const gtsam::KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
|
||||
std::cout << s << ": ReferenceFrameFactor("
|
||||
<< "Global: " << keyFormatter(this->template key<1>()) << ","
|
||||
<< " Transform: " << keyFormatter(this->template key<2>()) << ","
|
||||
<< " Local: " << keyFormatter(this->template key<3>()) << ")\n";
|
||||
<< "Global: " << keyFormatter(key1()) << ","
|
||||
<< " Transform: " << keyFormatter(key2()) << ","
|
||||
<< " Local: " << keyFormatter(key3()) << ")\n";
|
||||
this->noiseModel_->print(" noise model");
|
||||
}
|
||||
|
||||
// access - convenience functions
|
||||
Key global_key() const { return this->template key<1>(); }
|
||||
Key transform_key() const { return this->template key<2>(); }
|
||||
Key local_key() const { return this->template key<3>(); }
|
||||
Key global_key() const { return key1(); }
|
||||
Key transform_key() const { return key2(); }
|
||||
Key local_key() const { return key3(); }
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
|
|
|
@ -141,10 +141,10 @@ public:
|
|||
if (H1) *H1 = Matrix::Zero(3,6);
|
||||
if (H2) *H2 = Z_3x3;
|
||||
if (verboseCheirality_)
|
||||
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->template key<2>()) <<
|
||||
" moved behind camera " << DefaultKeyFormatter(this->template key<1>()) << std::endl;
|
||||
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(key2()) <<
|
||||
" moved behind camera " << DefaultKeyFormatter(key1()) << std::endl;
|
||||
if (throwCheirality_)
|
||||
throw StereoCheiralityException(this->template key<2>());
|
||||
throw StereoCheiralityException(key2());
|
||||
}
|
||||
return Vector3::Constant(2.0 * K_->fx());
|
||||
}
|
||||
|
|
|
@ -136,11 +136,11 @@ public:
|
|||
/** print */
|
||||
void print(const std::string& s = "EquivInertialNavFactor_GlobalVel", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
|
||||
std::cout << s << "("
|
||||
<< keyFormatter(this->template key<1>()) << ","
|
||||
<< keyFormatter(this->template key<2>()) << ","
|
||||
<< keyFormatter(this->template key<3>()) << ","
|
||||
<< keyFormatter(this->template key<4>()) << ","
|
||||
<< keyFormatter(this->template key<5>()) << "\n";
|
||||
<< keyFormatter(key1()) << ","
|
||||
<< keyFormatter(key2()) << ","
|
||||
<< keyFormatter(key3()) << ","
|
||||
<< keyFormatter(key4()) << ","
|
||||
<< keyFormatter(key5()) << "\n";
|
||||
std::cout << "delta_pos_in_t0: " << this->delta_pos_in_t0_.transpose() << std::endl;
|
||||
std::cout << "delta_vel_in_t0: " << this->delta_vel_in_t0_.transpose() << std::endl;
|
||||
std::cout << "delta_angles: " << this->delta_angles_ << std::endl;
|
||||
|
|
|
@ -73,8 +73,8 @@ public:
|
|||
/** print */
|
||||
void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
|
||||
std::cout << s << "GaussMarkov1stOrderFactor("
|
||||
<< keyFormatter(this->template key<1>()) << ","
|
||||
<< keyFormatter(this->template key<2>()) << ")\n";
|
||||
<< keyFormatter(key1()) << ","
|
||||
<< keyFormatter(key2()) << ")\n";
|
||||
this->noiseModel_->print(" noise model");
|
||||
}
|
||||
|
||||
|
|
|
@ -117,11 +117,11 @@ public:
|
|||
/** print */
|
||||
void print(const std::string& s = "InertialNavFactor_GlobalVelocity", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
|
||||
std::cout << s << "("
|
||||
<< keyFormatter(this->template key<1>()) << ","
|
||||
<< keyFormatter(this->template key<2>()) << ","
|
||||
<< keyFormatter(this->template key<3>()) << ","
|
||||
<< keyFormatter(this->template key<4>()) << ","
|
||||
<< keyFormatter(this->template key<5>()) << "\n";
|
||||
<< keyFormatter(key1()) << ","
|
||||
<< keyFormatter(key2()) << ","
|
||||
<< keyFormatter(key3()) << ","
|
||||
<< keyFormatter(key4()) << ","
|
||||
<< keyFormatter(key5()) << "\n";
|
||||
std::cout << "acc measurement: " << this->measurement_acc_.transpose() << std::endl;
|
||||
std::cout << "gyro measurement: " << this->measurement_gyro_.transpose() << std::endl;
|
||||
std::cout << "dt: " << this->dt_ << std::endl;
|
||||
|
|
|
@ -93,8 +93,8 @@ public:
|
|||
if (H1) *H1 = Matrix::Zero(2,6);
|
||||
if (H2) *H2 = Matrix::Zero(2,5);
|
||||
if (H3) *H3 = Matrix::Zero(2,1);
|
||||
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->template key<2>()) <<
|
||||
" moved behind camera " << DefaultKeyFormatter(this->template key<1>()) << std::endl;
|
||||
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(key2()) <<
|
||||
" moved behind camera " << DefaultKeyFormatter(key1()) << std::endl;
|
||||
return Vector::Ones(2) * 2.0 * K_->fx();
|
||||
}
|
||||
return (Vector(1) << 0.0).finished();
|
||||
|
|
|
@ -68,8 +68,8 @@ namespace gtsam {
|
|||
/** print */
|
||||
void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
|
||||
std::cout << s << "BetweenFactor("
|
||||
<< keyFormatter(this->template key<1>()) << ","
|
||||
<< keyFormatter(this->template key<2>()) << ")\n";
|
||||
<< keyFormatter(key1()) << ","
|
||||
<< keyFormatter(key2()) << ")\n";
|
||||
measured_.print(" measured: ");
|
||||
if(this->body_P_sensor_)
|
||||
this->body_P_sensor_->print(" sensor pose in body frame: ");
|
||||
|
|
|
@ -48,8 +48,8 @@ class PoseToPointFactor : public NoiseModelFactorN<POSE, POINT> {
|
|||
void print(const std::string& s, const KeyFormatter& keyFormatter =
|
||||
DefaultKeyFormatter) const override {
|
||||
std::cout << s << "PoseToPointFactor("
|
||||
<< keyFormatter(this->template key<1>()) << ","
|
||||
<< keyFormatter(this->template key<2>()) << ")\n"
|
||||
<< keyFormatter(key1()) << ","
|
||||
<< keyFormatter(key2()) << ")\n"
|
||||
<< " measured: " << measured_.transpose() << std::endl;
|
||||
this->noiseModel_->print(" noise model: ");
|
||||
}
|
||||
|
|
|
@ -143,9 +143,9 @@ namespace gtsam {
|
|||
if (H3) *H3 = Matrix::Zero(2,3);
|
||||
if (verboseCheirality_)
|
||||
std::cout << e.what() << ": Landmark "
|
||||
<< DefaultKeyFormatter(this->template key<2>())
|
||||
<< DefaultKeyFormatter(key2())
|
||||
<< " moved behind camera "
|
||||
<< DefaultKeyFormatter(this->template key<1>())
|
||||
<< DefaultKeyFormatter(key1())
|
||||
<< std::endl;
|
||||
if (throwCheirality_)
|
||||
throw e;
|
||||
|
|
|
@ -130,8 +130,8 @@ class GTSAM_UNSTABLE_EXPORT ProjectionFactorPPPC
|
|||
if (H3) *H3 = Matrix::Zero(2,3);
|
||||
if (H4) *H4 = Matrix::Zero(2,CALIBRATION::Dim());
|
||||
if (verboseCheirality_)
|
||||
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->template key<2>()) <<
|
||||
" moved behind camera " << DefaultKeyFormatter(this->template key<1>()) << std::endl;
|
||||
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(key2()) <<
|
||||
" moved behind camera " << DefaultKeyFormatter(key1()) << std::endl;
|
||||
if (throwCheirality_)
|
||||
throw e;
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue