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

replace all NoiseModelFactor1, 2, ... with NoiseModelFactorN

release/4.3a0
Gerry Chen 2022-12-22 17:25:48 -05:00
parent 2a7efc729a
commit 885eed33d1
No known key found for this signature in database
GPG Key ID: E9845092D3A57286
80 changed files with 407 additions and 333 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
examples/CameraResectioning.cpp
View File

@ -30,8 +30,8 @@ using symbol_shorthand::X;
* Unary factor on the unknown pose, resulting from meauring the projection of * Unary factor on the unknown pose, resulting from meauring the projection of
* a known 3D point in the image * a known 3D point in the image
*/ */
class ResectioningFactor: public NoiseModelFactor1<Pose3> { class ResectioningFactor: public NoiseModelFactorN<Pose3> {
typedef NoiseModelFactor1<Pose3> Base; typedef NoiseModelFactorN<Pose3> Base;
Cal3_S2::shared_ptr K_; ///< camera's intrinsic parameters Cal3_S2::shared_ptr K_; ///< camera's intrinsic parameters
Point3 P_; ///< 3D point on the calibration rig Point3 P_; ///< 3D point on the calibration rig

8
examples/LocalizationExample.cpp
View File

@ -62,10 +62,10 @@ using namespace gtsam;
// //
// The factor will be a unary factor, affect only a single system variable. It will // The factor will be a unary factor, affect only a single system variable. It will
// also use a standard Gaussian noise model. Hence, we will derive our new factor from // also use a standard Gaussian noise model. Hence, we will derive our new factor from
// the NoiseModelFactor1. // the NoiseModelFactorN.
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
class UnaryFactor: public NoiseModelFactor1<Pose2> { class UnaryFactor: public NoiseModelFactorN<Pose2> {
// The factor will hold a measurement consisting of an (X,Y) location // The factor will hold a measurement consisting of an (X,Y) location
// We could this with a Point2 but here we just use two doubles // We could this with a Point2 but here we just use two doubles
double mx_, my_; double mx_, my_;
@ -76,11 +76,11 @@ class UnaryFactor: public NoiseModelFactor1<Pose2> {
// The constructor requires the variable key, the (X, Y) measurement value, and the noise model // The constructor requires the variable key, the (X, Y) measurement value, and the noise model
UnaryFactor(Key j, double x, double y, const SharedNoiseModel& model): UnaryFactor(Key j, double x, double y, const SharedNoiseModel& model):
NoiseModelFactor1<Pose2>(model, j), mx_(x), my_(y) {} NoiseModelFactorN<Pose2>(model, j), mx_(x), my_(y) {}
~UnaryFactor() override {} ~UnaryFactor() override {}
// Using the NoiseModelFactor1 base class there are two functions that must be overridden. // Using the NoiseModelFactorN base class there are two functions that must be overridden.
// The first is the 'evaluateError' function. This function implements the desired measurement // The first is the 'evaluateError' function. This function implements the desired measurement
// function, returning a vector of errors when evaluated at the provided variable value. It // function, returning a vector of errors when evaluated at the provided variable value. It
// must also calculate the Jacobians for this measurement function, if requested. // must also calculate the Jacobians for this measurement function, if requested.

8
examples/Pose3SLAMExample_changeKeys.cpp
View File

@ -71,11 +71,11 @@ int main(const int argc, const char *argv[]) {
if (pose3Between){ if (pose3Between){
Key key1, key2; Key key1, key2;
if(add){ if(add){
key1 = pose3Between->key1() + firstKey; key1 = pose3Between->key<1>() + firstKey;
key2 = pose3Between->key2() + firstKey; key2 = pose3Between->key<2>() + firstKey;
}else{ }else{
key1 = pose3Between->key1() - firstKey; key1 = pose3Between->key<1>() - firstKey;
key2 = pose3Between->key2() - firstKey; key2 = pose3Between->key<2>() - firstKey;
} }
NonlinearFactor::shared_ptr simpleFactor( NonlinearFactor::shared_ptr simpleFactor(
new BetweenFactor<Pose3>(key1, key2, pose3Between->measured(), pose3Between->noiseModel())); new BetweenFactor<Pose3>(key1, key2, pose3Between->measured(), pose3Between->noiseModel()));

28
examples/SolverComparer.cpp
View File

@ -69,8 +69,8 @@ namespace br = boost::range;
typedef Pose2 Pose; typedef Pose2 Pose;
typedef NoiseModelFactor1<Pose> NM1; typedef NoiseModelFactorN<Pose> NM1;
typedef NoiseModelFactor2<Pose,Pose> NM2; typedef NoiseModelFactorN<Pose,Pose> NM2;
typedef BearingRangeFactor<Pose,Point2> BR; typedef BearingRangeFactor<Pose,Point2> BR;
double chi2_red(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& config) { double chi2_red(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& config) {
@ -261,7 +261,7 @@ void runIncremental()
if(BetweenFactor<Pose>::shared_ptr factor = if(BetweenFactor<Pose>::shared_ptr factor =
boost::dynamic_pointer_cast<BetweenFactor<Pose> >(datasetMeasurements[nextMeasurement])) boost::dynamic_pointer_cast<BetweenFactor<Pose> >(datasetMeasurements[nextMeasurement]))
{ {
Key key1 = factor->key1(), key2 = factor->key2(); Key key1 = factor->key<1>(), key2 = factor->key<2>();
if(((int)key1 >= firstStep && key1 < key2) || ((int)key2 >= firstStep && key2 < key1)) { if(((int)key1 >= firstStep && key1 < key2) || ((int)key2 >= firstStep && key2 < key1)) {
// We found an odometry starting at firstStep // We found an odometry starting at firstStep
firstPose = std::min(key1, key2); firstPose = std::min(key1, key2);
@ -313,11 +313,11 @@ void runIncremental()
boost::dynamic_pointer_cast<BetweenFactor<Pose> >(measurementf)) boost::dynamic_pointer_cast<BetweenFactor<Pose> >(measurementf))
{ {
// Stop collecting measurements that are for future steps // Stop collecting measurements that are for future steps
if(factor->key1() > step || factor->key2() > step) if(factor->key<1>() > step || factor->key<2>() > step)
break; break;
// Require that one of the nodes is the current one // Require that one of the nodes is the current one
if(factor->key1() != step && factor->key2() != step) if(factor->key<1>() != step && factor->key<2>() != step)
throw runtime_error("Problem in data file, out-of-sequence measurements"); throw runtime_error("Problem in data file, out-of-sequence measurements");
// Add a new factor // Add a new factor
@ -325,22 +325,22 @@ void runIncremental()
const auto& measured = factor->measured(); const auto& measured = factor->measured();
// Initialize the new variable // Initialize the new variable
if(factor->key1() > factor->key2()) { if(factor->key<1>() > factor->key<2>()) {
if(!newVariables.exists(factor->key1())) { // Only need to check newVariables since loop closures come after odometry if(!newVariables.exists(factor->key<1>())) { // Only need to check newVariables since loop closures come after odometry
if(step == 1) if(step == 1)
newVariables.insert(factor->key1(), measured.inverse()); newVariables.insert(factor->key<1>(), measured.inverse());
else { else {
Pose prevPose = isam2.calculateEstimate<Pose>(factor->key2()); Pose prevPose = isam2.calculateEstimate<Pose>(factor->key<2>());
newVariables.insert(factor->key1(), prevPose * measured.inverse()); newVariables.insert(factor->key<1>(), prevPose * measured.inverse());
} }
} }
} else { } else {
if(!newVariables.exists(factor->key2())) { // Only need to check newVariables since loop closures come after odometry if(!newVariables.exists(factor->key<2>())) { // Only need to check newVariables since loop closures come after odometry
if(step == 1) if(step == 1)
newVariables.insert(factor->key2(), measured); newVariables.insert(factor->key<2>(), measured);
else { else {
Pose prevPose = isam2.calculateEstimate<Pose>(factor->key1()); Pose prevPose = isam2.calculateEstimate<Pose>(factor->key<1>());
newVariables.insert(factor->key2(), prevPose * measured); newVariables.insert(factor->key<2>(), prevPose * measured);
} }
} }
} }

2
gtsam/base/Testable.h
View File

@ -23,7 +23,7 @@
* void print(const std::string& name) const = 0; * void print(const std::string& name) const = 0;
* *
* equality up to tolerance * equality up to tolerance
* tricky to implement, see NoiseModelFactor1 for an example * tricky to implement, see PriorFactor for an example
* equals is not supposed to print out *anything*, just return true|false * equals is not supposed to print out *anything*, just return true|false
* bool equals(const Derived& expected, double tol) const = 0; * bool equals(const Derived& expected, double tol) const = 0;
* *

8
gtsam/inference/graph-inl.h
View File

@ -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); boost::shared_ptr<Factor> factor = boost::dynamic_pointer_cast<Factor>(nl_factor);
if (!factor) continue; if (!factor) continue;
KEY key1 = factor->key1(); KEY key1 = factor->template key<1>();
KEY key2 = factor->key2(); KEY key2 = factor->template key<2>();
PoseVertex v1 = key2vertex.find(key1)->second; PoseVertex v1 = key2vertex.find(key1)->second;
PoseVertex v2 = key2vertex.find(key2)->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); FACTOR2>(factor);
if (!factor2) continue; if (!factor2) continue;
KEY key1 = factor2->key1(); KEY key1 = factor2->template key<1>();
KEY key2 = factor2->key2(); KEY key2 = factor2->template key<2>();
// if the tree contains the key // if the tree contains the key
if ((tree.find(key1) != tree.end() && if ((tree.find(key1) != tree.end() &&
tree.find(key1)->second.compare(key2) == 0) || tree.find(key1)->second.compare(key2) == 0) ||

4
gtsam/navigation/AHRSFactor.cpp
View File

@ -105,8 +105,8 @@ gtsam::NonlinearFactor::shared_ptr AHRSFactor::clone() const {
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
void AHRSFactor::print(const string& s, void AHRSFactor::print(const string& s,
const KeyFormatter& keyFormatter) const { const KeyFormatter& keyFormatter) const {
cout << s << "AHRSFactor(" << keyFormatter(this->key1()) << "," cout << s << "AHRSFactor(" << keyFormatter(this->key<1>()) << ","
<< keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << ","; << keyFormatter(this->key<2>()) << "," << keyFormatter(this->key<3>()) << ",";
_PIM_.print(" preintegrated measurements:"); _PIM_.print(" preintegrated measurements:");
noiseModel_->print(" noise model: "); noiseModel_->print(" noise model: ");
} }

5
gtsam/navigation/AHRSFactor.h
View File

@ -128,10 +128,10 @@ private:
} }
}; };
class GTSAM_EXPORT AHRSFactor: public NoiseModelFactor3<Rot3, Rot3, Vector3> { class GTSAM_EXPORT AHRSFactor: public NoiseModelFactorN<Rot3, Rot3, Vector3> {
typedef AHRSFactor This; typedef AHRSFactor This;
typedef NoiseModelFactor3<Rot3, Rot3, Vector3> Base; typedef NoiseModelFactorN<Rot3, Rot3, Vector3> Base;
PreintegratedAhrsMeasurements _PIM_; PreintegratedAhrsMeasurements _PIM_;
@ -212,6 +212,7 @@ private:
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*/) {
// NoiseModelFactor3 instead of NoiseModelFactorN for backward compatibility
ar ar
& boost::serialization::make_nvp("NoiseModelFactor3", & boost::serialization::make_nvp("NoiseModelFactor3",
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));

10
gtsam/navigation/AttitudeFactor.h
View File

@ -76,9 +76,9 @@ public:
* Version of AttitudeFactor for Rot3 * Version of AttitudeFactor for Rot3
* @ingroup navigation * @ingroup navigation
*/ */
class GTSAM_EXPORT Rot3AttitudeFactor: public NoiseModelFactor1<Rot3>, public AttitudeFactor { class GTSAM_EXPORT Rot3AttitudeFactor: public NoiseModelFactorN<Rot3>, public AttitudeFactor {
typedef NoiseModelFactor1<Rot3> Base; typedef NoiseModelFactorN<Rot3> Base;
public: public:
@ -132,6 +132,7 @@ private:
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*/) {
// NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
ar & boost::serialization::make_nvp("NoiseModelFactor1", ar & boost::serialization::make_nvp("NoiseModelFactor1",
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));
ar & boost::serialization::make_nvp("AttitudeFactor", ar & boost::serialization::make_nvp("AttitudeFactor",
@ -149,10 +150,10 @@ template<> struct traits<Rot3AttitudeFactor> : public Testable<Rot3AttitudeFacto
* Version of AttitudeFactor for Pose3 * Version of AttitudeFactor for Pose3
* @ingroup navigation * @ingroup navigation
*/ */
class GTSAM_EXPORT Pose3AttitudeFactor: public NoiseModelFactor1<Pose3>, class GTSAM_EXPORT Pose3AttitudeFactor: public NoiseModelFactorN<Pose3>,
public AttitudeFactor { public AttitudeFactor {
typedef NoiseModelFactor1<Pose3> Base; typedef NoiseModelFactorN<Pose3> Base;
public: public:
@ -212,6 +213,7 @@ private:
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*/) {
// NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
ar & boost::serialization::make_nvp("NoiseModelFactor1", ar & boost::serialization::make_nvp("NoiseModelFactor1",
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));
ar & boost::serialization::make_nvp("AttitudeFactor", ar & boost::serialization::make_nvp("AttitudeFactor",

5
gtsam/navigation/BarometricFactor.h
View File

@ -31,9 +31,9 @@ namespace gtsam {
* https://www.grc.nasa.gov/www/k-12/airplane/atmosmet.html * https://www.grc.nasa.gov/www/k-12/airplane/atmosmet.html
* @ingroup navigation * @ingroup navigation
*/ */
class GTSAM_EXPORT BarometricFactor : public NoiseModelFactor2<Pose3, double> { class GTSAM_EXPORT BarometricFactor : public NoiseModelFactorN<Pose3, double> {
private: private:
typedef NoiseModelFactor2<Pose3, double> Base; typedef NoiseModelFactorN<Pose3, double> Base;
double nT_; ///< Height Measurement based on a standard atmosphere double nT_; ///< Height Measurement based on a standard atmosphere
@ -99,6 +99,7 @@ class GTSAM_EXPORT BarometricFactor : public NoiseModelFactor2<Pose3, double> {
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*/) {
// NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
ar& boost::serialization::make_nvp( ar& boost::serialization::make_nvp(
"NoiseModelFactor1", "NoiseModelFactor1",
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));

6
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -204,9 +204,9 @@ gtsam::NonlinearFactor::shared_ptr CombinedImuFactor::clone() const {
void CombinedImuFactor::print(const string& s, void CombinedImuFactor::print(const string& s,
const KeyFormatter& keyFormatter) const { const KeyFormatter& keyFormatter) const {
cout << (s.empty() ? s : s + "\n") << "CombinedImuFactor(" cout << (s.empty() ? s : s + "\n") << "CombinedImuFactor("
<< keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << "," << keyFormatter(this->key<1>()) << "," << keyFormatter(this->key<2>()) << ","
<< keyFormatter(this->key3()) << "," << keyFormatter(this->key4()) << "," << keyFormatter(this->key<3>()) << "," << keyFormatter(this->key<4>()) << ","
<< keyFormatter(this->key5()) << "," << keyFormatter(this->key6()) << keyFormatter(this->key<5>()) << "," << keyFormatter(this->key<6>())
<< ")\n"; << ")\n";
_PIM_.print(" preintegrated measurements:"); _PIM_.print(" preintegrated measurements:");
this->noiseModel_->print(" noise model: "); this->noiseModel_->print(" noise model: ");

8
gtsam/navigation/CombinedImuFactor.h
View File

@ -255,14 +255,14 @@ public:
* *
* @ingroup navigation * @ingroup navigation
*/ */
class GTSAM_EXPORT CombinedImuFactor: public NoiseModelFactor6<Pose3, Vector3, Pose3, class GTSAM_EXPORT CombinedImuFactor: public NoiseModelFactorN<Pose3, Vector3, Pose3,
Vector3, imuBias::ConstantBias, imuBias::ConstantBias> { Vector3, imuBias::ConstantBias, imuBias::ConstantBias> {
public: public:
private: private:
typedef CombinedImuFactor This; typedef CombinedImuFactor This;
typedef NoiseModelFactor6<Pose3, Vector3, Pose3, Vector3, typedef NoiseModelFactorN<Pose3, Vector3, Pose3, Vector3,
imuBias::ConstantBias, imuBias::ConstantBias> Base; imuBias::ConstantBias, imuBias::ConstantBias> Base;
PreintegratedCombinedMeasurements _PIM_; PreintegratedCombinedMeasurements _PIM_;
@ -334,7 +334,9 @@ public:
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_BASE_OBJECT_NVP(NoiseModelFactor6); // NoiseModelFactor6 instead of NoiseModelFactorN for backward compatibility
ar& boost::serialization::make_nvp(
"NoiseModelFactor6", boost::serialization::base_object<Base>(*this));
ar& BOOST_SERIALIZATION_NVP(_PIM_); ar& BOOST_SERIALIZATION_NVP(_PIM_);
} }

6
gtsam/navigation/ConstantVelocityFactor.h
View File

@ -26,15 +26,15 @@ namespace gtsam {
* Binary factor for applying a constant velocity model to a moving body represented as a NavState. * Binary factor for applying a constant velocity model to a moving body represented as a NavState.
* The only measurement is dt, the time delta between the states. * The only measurement is dt, the time delta between the states.
*/ */
class ConstantVelocityFactor : public NoiseModelFactor2<NavState, NavState> { class ConstantVelocityFactor : public NoiseModelFactorN<NavState, NavState> {
double dt_; double dt_;
public: public:
using Base = NoiseModelFactor2<NavState, NavState>; using Base = NoiseModelFactorN<NavState, NavState>;
public: public:
ConstantVelocityFactor(Key i, Key j, double dt, const SharedNoiseModel &model) ConstantVelocityFactor(Key i, Key j, double dt, const SharedNoiseModel &model)
: NoiseModelFactor2<NavState, NavState>(model, i, j), dt_(dt) {} : NoiseModelFactorN<NavState, NavState>(model, i, j), dt_(dt) {}
~ConstantVelocityFactor() override{}; ~ConstantVelocityFactor() override{};
/** /**

10
gtsam/navigation/GPSFactor.h
View File

@ -32,11 +32,11 @@ namespace gtsam {
* See Farrell08book or e.g. http://www.dirsig.org/docs/new/coordinates.html * See Farrell08book or e.g. http://www.dirsig.org/docs/new/coordinates.html
* @ingroup navigation * @ingroup navigation
*/ */
class GTSAM_EXPORT GPSFactor: public NoiseModelFactor1<Pose3> { class GTSAM_EXPORT GPSFactor: public NoiseModelFactorN<Pose3> {
private: private:
typedef NoiseModelFactor1<Pose3> Base; typedef NoiseModelFactorN<Pose3> Base;
Point3 nT_; ///< Position measurement in cartesian coordinates Point3 nT_; ///< Position measurement in cartesian coordinates
@ -99,6 +99,7 @@ private:
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*/) {
// NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
ar ar
& boost::serialization::make_nvp("NoiseModelFactor1", & boost::serialization::make_nvp("NoiseModelFactor1",
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));
@ -110,11 +111,11 @@ private:
* Version of GPSFactor for NavState * Version of GPSFactor for NavState
* @ingroup navigation * @ingroup navigation
*/ */
class GTSAM_EXPORT GPSFactor2: public NoiseModelFactor1<NavState> { class GTSAM_EXPORT GPSFactor2: public NoiseModelFactorN<NavState> {
private: private:
typedef NoiseModelFactor1<NavState> Base; typedef NoiseModelFactorN<NavState> Base;
Point3 nT_; ///< Position measurement in cartesian coordinates Point3 nT_; ///< Position measurement in cartesian coordinates
@ -163,6 +164,7 @@ private:
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*/) {
// NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
ar ar
& boost::serialization::make_nvp("NoiseModelFactor1", & boost::serialization::make_nvp("NoiseModelFactor1",
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));

24
gtsam/navigation/ImuFactor.cpp
View File

@ -133,9 +133,9 @@ std::ostream& operator<<(std::ostream& os, const ImuFactor& f) {
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const { void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const {
cout << (s.empty() ? s : s + "\n") << "ImuFactor(" << keyFormatter(this->key1()) cout << (s.empty() ? s : s + "\n") << "ImuFactor(" << keyFormatter(this->key<1>())
<< "," << keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << "," << keyFormatter(this->key<2>()) << "," << keyFormatter(this->key<3>())
<< "," << keyFormatter(this->key4()) << "," << keyFormatter(this->key5()) << "," << keyFormatter(this->key<4>()) << "," << keyFormatter(this->key<5>())
<< ")\n"; << ")\n";
cout << *this << endl; cout << *this << endl;
} }
@ -184,22 +184,22 @@ PreintegratedImuMeasurements ImuFactor::Merge(
ImuFactor::shared_ptr ImuFactor::Merge(const shared_ptr& f01, ImuFactor::shared_ptr ImuFactor::Merge(const shared_ptr& f01,
const shared_ptr& f12) { const shared_ptr& f12) {
// IMU bias keys must be the same. // IMU bias keys must be the same.
if (f01->key5() != f12->key5()) if (f01->key<5>() != f12->key<5>())
throw std::domain_error("ImuFactor::Merge: IMU bias keys must be the same"); throw std::domain_error("ImuFactor::Merge: IMU bias keys must be the same");
// expect intermediate pose, velocity keys to matchup. // expect intermediate pose, velocity keys to matchup.
if (f01->key3() != f12->key1() || f01->key4() != f12->key2()) if (f01->key<3>() != f12->key<1>() || f01->key<4>() != f12->key<2>())
throw std::domain_error( throw std::domain_error(
"ImuFactor::Merge: intermediate pose, velocity keys need to match up"); "ImuFactor::Merge: intermediate pose, velocity keys need to match up");
// return new factor // return new factor
auto pim02 = auto pim02 =
Merge(f01->preintegratedMeasurements(), f12->preintegratedMeasurements()); Merge(f01->preintegratedMeasurements(), f12->preintegratedMeasurements());
return boost::make_shared<ImuFactor>(f01->key1(), // P0 return boost::make_shared<ImuFactor>(f01->key<1>(), // P0
f01->key2(), // V0 f01->key<2>(), // V0
f12->key3(), // P2 f12->key<3>(), // P2
f12->key4(), // V2 f12->key<4>(), // V2
f01->key5(), // B f01->key<5>(), // B
pim02); pim02);
} }
#endif #endif
@ -230,8 +230,8 @@ std::ostream& operator<<(std::ostream& os, const ImuFactor2& f) {
void ImuFactor2::print(const string& s, void ImuFactor2::print(const string& s,
const KeyFormatter& keyFormatter) const { const KeyFormatter& keyFormatter) const {
cout << (s.empty() ? s : s + "\n") << "ImuFactor2(" cout << (s.empty() ? s : s + "\n") << "ImuFactor2("
<< keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << "," << keyFormatter(this->key<1>()) << "," << keyFormatter(this->key<2>()) << ","
<< keyFormatter(this->key3()) << ")\n"; << keyFormatter(this->key<3>()) << ")\n";
cout << *this << endl; cout << *this << endl;
} }

10
gtsam/navigation/ImuFactor.h
View File

@ -168,12 +168,12 @@ public:
* *
* @ingroup navigation * @ingroup navigation
*/ */
class GTSAM_EXPORT ImuFactor: public NoiseModelFactor5<Pose3, Vector3, Pose3, Vector3, class GTSAM_EXPORT ImuFactor: public NoiseModelFactorN<Pose3, Vector3, Pose3, Vector3,
imuBias::ConstantBias> { imuBias::ConstantBias> {
private: private:
typedef ImuFactor This; typedef ImuFactor This;
typedef NoiseModelFactor5<Pose3, Vector3, Pose3, Vector3, typedef NoiseModelFactorN<Pose3, Vector3, Pose3, Vector3,
imuBias::ConstantBias> Base; imuBias::ConstantBias> Base;
PreintegratedImuMeasurements _PIM_; PreintegratedImuMeasurements _PIM_;
@ -248,6 +248,7 @@ public:
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*/) {
// NoiseModelFactor5 instead of NoiseModelFactorN for backward compatibility
ar & boost::serialization::make_nvp("NoiseModelFactor5", ar & boost::serialization::make_nvp("NoiseModelFactor5",
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(_PIM_); ar & BOOST_SERIALIZATION_NVP(_PIM_);
@ -259,11 +260,11 @@ public:
* ImuFactor2 is a ternary factor that uses NavStates rather than Pose/Velocity. * ImuFactor2 is a ternary factor that uses NavStates rather than Pose/Velocity.
* @ingroup navigation * @ingroup navigation
*/ */
class GTSAM_EXPORT ImuFactor2 : public NoiseModelFactor3<NavState, NavState, imuBias::ConstantBias> { class GTSAM_EXPORT ImuFactor2 : public NoiseModelFactorN<NavState, NavState, imuBias::ConstantBias> {
private: private:
typedef ImuFactor2 This; typedef ImuFactor2 This;
typedef NoiseModelFactor3<NavState, NavState, imuBias::ConstantBias> Base; typedef NoiseModelFactorN<NavState, NavState, imuBias::ConstantBias> Base;
PreintegratedImuMeasurements _PIM_; PreintegratedImuMeasurements _PIM_;
@ -316,6 +317,7 @@ private:
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*/) {
// NoiseModelFactor3 instead of NoiseModelFactorN for backward compatibility
ar & boost::serialization::make_nvp("NoiseModelFactor3", ar & boost::serialization::make_nvp("NoiseModelFactor3",
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(_PIM_); ar & BOOST_SERIALIZATION_NVP(_PIM_);

16
gtsam/navigation/MagFactor.h
View File

@ -30,7 +30,7 @@ namespace gtsam {
* and assumes scale, direction, and the bias are given. * and assumes scale, direction, and the bias are given.
* Rotation is around negative Z axis, i.e. positive is yaw to right! * Rotation is around negative Z axis, i.e. positive is yaw to right!
*/ */
class MagFactor: public NoiseModelFactor1<Rot2> { class MagFactor: public NoiseModelFactorN<Rot2> {
const Point3 measured_; ///< The measured magnetometer values const Point3 measured_; ///< The measured magnetometer values
const Point3 nM_; ///< Local magnetic field (mag output units) const Point3 nM_; ///< Local magnetic field (mag output units)
@ -50,7 +50,7 @@ public:
MagFactor(Key key, const Point3& measured, double scale, MagFactor(Key key, const Point3& measured, double scale,
const Unit3& direction, const Point3& bias, const Unit3& direction, const Point3& bias,
const SharedNoiseModel& model) : const SharedNoiseModel& model) :
NoiseModelFactor1<Rot2>(model, key), // NoiseModelFactorN<Rot2>(model, key), //
measured_(measured), nM_(scale * direction), bias_(bias) { measured_(measured), nM_(scale * direction), bias_(bias) {
} }
@ -87,7 +87,7 @@ public:
* This version uses model measured bM = scale * bRn * direction + bias * This version uses model measured bM = scale * bRn * direction + bias
* and assumes scale, direction, and the bias are given * and assumes scale, direction, and the bias are given
*/ */
class MagFactor1: public NoiseModelFactor1<Rot3> { class MagFactor1: public NoiseModelFactorN<Rot3> {
const Point3 measured_; ///< The measured magnetometer values const Point3 measured_; ///< The measured magnetometer values
const Point3 nM_; ///< Local magnetic field (mag output units) const Point3 nM_; ///< Local magnetic field (mag output units)
@ -99,7 +99,7 @@ public:
MagFactor1(Key key, const Point3& measured, double scale, MagFactor1(Key key, const Point3& measured, double scale,
const Unit3& direction, const Point3& bias, const Unit3& direction, const Point3& bias,
const SharedNoiseModel& model) : const SharedNoiseModel& model) :
NoiseModelFactor1<Rot3>(model, key), // NoiseModelFactorN<Rot3>(model, key), //
measured_(measured), nM_(scale * direction), bias_(bias) { measured_(measured), nM_(scale * direction), bias_(bias) {
} }
@ -125,7 +125,7 @@ public:
* This version uses model measured bM = bRn * nM + bias * This version uses model measured bM = bRn * nM + bias
* and optimizes for both nM and the bias, where nM is in units defined by magnetometer * and optimizes for both nM and the bias, where nM is in units defined by magnetometer
*/ */
class MagFactor2: public NoiseModelFactor2<Point3, Point3> { class MagFactor2: public NoiseModelFactorN<Point3, Point3> {
const Point3 measured_; ///< The measured magnetometer values const Point3 measured_; ///< The measured magnetometer values
const Rot3 bRn_; ///< The assumed known rotation from nav to body const Rot3 bRn_; ///< The assumed known rotation from nav to body
@ -135,7 +135,7 @@ public:
/** Constructor */ /** Constructor */
MagFactor2(Key key1, Key key2, const Point3& measured, const Rot3& nRb, MagFactor2(Key key1, Key key2, const Point3& measured, const Rot3& nRb,
const SharedNoiseModel& model) : const SharedNoiseModel& model) :
NoiseModelFactor2<Point3, Point3>(model, key1, key2), // NoiseModelFactorN<Point3, Point3>(model, key1, key2), //
measured_(measured), bRn_(nRb.inverse()) { measured_(measured), bRn_(nRb.inverse()) {
} }
@ -166,7 +166,7 @@ public:
* This version uses model measured bM = scale * bRn * direction + bias * This version uses model measured bM = scale * bRn * direction + bias
* and optimizes for both scale, direction, and the bias. * and optimizes for both scale, direction, and the bias.
*/ */
class MagFactor3: public NoiseModelFactor3<double, Unit3, Point3> { class MagFactor3: public NoiseModelFactorN<double, Unit3, Point3> {
const Point3 measured_; ///< The measured magnetometer values const Point3 measured_; ///< The measured magnetometer values
const Rot3 bRn_; ///< The assumed known rotation from nav to body const Rot3 bRn_; ///< The assumed known rotation from nav to body
@ -176,7 +176,7 @@ public:
/** Constructor */ /** Constructor */
MagFactor3(Key key1, Key key2, Key key3, const Point3& measured, MagFactor3(Key key1, Key key2, Key key3, const Point3& measured,
const Rot3& nRb, const SharedNoiseModel& model) : const Rot3& nRb, const SharedNoiseModel& model) :
NoiseModelFactor3<double, Unit3, Point3>(model, key1, key2, key3), // NoiseModelFactorN<double, Unit3, Point3>(model, key1, key2, key3), //
measured_(measured), bRn_(nRb.inverse()) { measured_(measured), bRn_(nRb.inverse()) {
} }

5
gtsam/navigation/MagPoseFactor.h
View File

@ -25,10 +25,10 @@ namespace gtsam {
* expressed in the sensor frame. * expressed in the sensor frame.
*/ */
template <class POSE> template <class POSE>
class MagPoseFactor: public NoiseModelFactor1<POSE> { class MagPoseFactor: public NoiseModelFactorN<POSE> {
private: private:
using This = MagPoseFactor<POSE>; using This = MagPoseFactor<POSE>;
using Base = NoiseModelFactor1<POSE>; using Base = NoiseModelFactorN<POSE>;
using Point = typename POSE::Translation; ///< Could be a Vector2 or Vector3 depending on POSE. using Point = typename POSE::Translation; ///< Could be a Vector2 or Vector3 depending on POSE.
using Rot = typename POSE::Rotation; using Rot = typename POSE::Rotation;
@ -129,6 +129,7 @@ class MagPoseFactor: public NoiseModelFactor1<POSE> {
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*/) {
// NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
ar & boost::serialization::make_nvp("NoiseModelFactor1", ar & boost::serialization::make_nvp("NoiseModelFactor1",
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(measured_);

4
gtsam/nonlinear/ExtendedKalmanFilter.h
View File

@ -53,8 +53,8 @@ class ExtendedKalmanFilter {
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
//@deprecated: any NoiseModelFactor will do, as long as they have the right keys //@deprecated: any NoiseModelFactor will do, as long as they have the right keys
typedef NoiseModelFactor2<VALUE, VALUE> MotionFactor; typedef NoiseModelFactorN<VALUE, VALUE> MotionFactor;
typedef NoiseModelFactor1<VALUE> MeasurementFactor; typedef NoiseModelFactorN<VALUE> MeasurementFactor;
#endif #endif
protected: protected:

16
gtsam/nonlinear/FunctorizedFactor.h
View File

@ -56,9 +56,9 @@ namespace gtsam {
* MultiplyFunctor(multiplier)); * MultiplyFunctor(multiplier));
*/ */
template <typename R, typename T> template <typename R, typename T>
class FunctorizedFactor : public NoiseModelFactor1<T> { class FunctorizedFactor : public NoiseModelFactorN<T> {
private: private:
using Base = NoiseModelFactor1<T>; using Base = NoiseModelFactorN<T>;
R measured_; ///< value that is compared with functor return value R measured_; ///< value that is compared with functor return value
SharedNoiseModel noiseModel_; ///< noise model SharedNoiseModel noiseModel_; ///< noise model
@ -101,7 +101,7 @@ class FunctorizedFactor : public NoiseModelFactor1<T> {
const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override { const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override {
Base::print(s, keyFormatter); Base::print(s, keyFormatter);
std::cout << s << (s != "" ? " " : "") << "FunctorizedFactor(" std::cout << s << (s != "" ? " " : "") << "FunctorizedFactor("
<< keyFormatter(this->key()) << ")" << std::endl; << keyFormatter(this->template key<1>()) << ")" << std::endl;
traits<R>::Print(measured_, " measurement: "); traits<R>::Print(measured_, " measurement: ");
std::cout << " noise model sigmas: " << noiseModel_->sigmas().transpose() std::cout << " noise model sigmas: " << noiseModel_->sigmas().transpose()
<< std::endl; << std::endl;
@ -120,6 +120,7 @@ class FunctorizedFactor : public NoiseModelFactor1<T> {
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*/) {
// NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
ar &boost::serialization::make_nvp( ar &boost::serialization::make_nvp(
"NoiseModelFactor1", boost::serialization::base_object<Base>(*this)); "NoiseModelFactor1", boost::serialization::base_object<Base>(*this));
ar &BOOST_SERIALIZATION_NVP(measured_); ar &BOOST_SERIALIZATION_NVP(measured_);
@ -155,9 +156,9 @@ FunctorizedFactor<R, T> MakeFunctorizedFactor(Key key, const R &z,
* @param T2: The second argument type for the functor. * @param T2: The second argument type for the functor.
*/ */
template <typename R, typename T1, typename T2> template <typename R, typename T1, typename T2>
class FunctorizedFactor2 : public NoiseModelFactor2<T1, T2> { class FunctorizedFactor2 : public NoiseModelFactorN<T1, T2> {
private: private:
using Base = NoiseModelFactor2<T1, T2>; using Base = NoiseModelFactorN<T1, T2>;
R measured_; ///< value that is compared with functor return value R measured_; ///< value that is compared with functor return value
SharedNoiseModel noiseModel_; ///< noise model SharedNoiseModel noiseModel_; ///< noise model
@ -207,8 +208,8 @@ class FunctorizedFactor2 : public NoiseModelFactor2<T1, T2> {
const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override { const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override {
Base::print(s, keyFormatter); Base::print(s, keyFormatter);
std::cout << s << (s != "" ? " " : "") << "FunctorizedFactor2(" std::cout << s << (s != "" ? " " : "") << "FunctorizedFactor2("
<< keyFormatter(this->key1()) << ", " << keyFormatter(this->template key<1>()) << ", "
<< keyFormatter(this->key2()) << ")" << std::endl; << keyFormatter(this->template key<2>()) << ")" << std::endl;
traits<R>::Print(measured_, " measurement: "); traits<R>::Print(measured_, " measurement: ");
std::cout << " noise model sigmas: " << noiseModel_->sigmas().transpose() std::cout << " noise model sigmas: " << noiseModel_->sigmas().transpose()
<< std::endl; << std::endl;
@ -227,6 +228,7 @@ class FunctorizedFactor2 : public NoiseModelFactor2<T1, T2> {
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*/) {
// NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
ar &boost::serialization::make_nvp( ar &boost::serialization::make_nvp(
"NoiseModelFactor2", boost::serialization::base_object<Base>(*this)); "NoiseModelFactor2", boost::serialization::base_object<Base>(*this));
ar &BOOST_SERIALIZATION_NVP(measured_); ar &BOOST_SERIALIZATION_NVP(measured_);

15
gtsam/nonlinear/NonlinearEquality.h
View File

@ -42,7 +42,7 @@ namespace gtsam {
* \nosubgrouping * \nosubgrouping
*/ */
template<class VALUE> template<class VALUE>
class NonlinearEquality: public NoiseModelFactor1<VALUE> { class NonlinearEquality: public NoiseModelFactorN<VALUE> {
public: public:
typedef VALUE T; typedef VALUE T;
@ -62,7 +62,7 @@ private:
using This = NonlinearEquality<VALUE>; using This = NonlinearEquality<VALUE>;
// typedef to base class // typedef to base class
using Base = NoiseModelFactor1<VALUE>; using Base = NoiseModelFactorN<VALUE>;
public: public:
@ -184,6 +184,7 @@ private:
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*/) {
// NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
ar ar
& boost::serialization::make_nvp("NoiseModelFactor1", & boost::serialization::make_nvp("NoiseModelFactor1",
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));
@ -203,13 +204,13 @@ struct traits<NonlinearEquality<VALUE>> : Testable<NonlinearEquality<VALUE>> {};
* Simple unary equality constraint - fixes a value for a variable * Simple unary equality constraint - fixes a value for a variable
*/ */
template<class VALUE> template<class VALUE>
class NonlinearEquality1: public NoiseModelFactor1<VALUE> { class NonlinearEquality1: public NoiseModelFactorN<VALUE> {
public: public:
typedef VALUE X; typedef VALUE X;
protected: protected:
typedef NoiseModelFactor1<VALUE> Base; typedef NoiseModelFactorN<VALUE> Base;
typedef NonlinearEquality1<VALUE> This; typedef NonlinearEquality1<VALUE> This;
/// Default constructor to allow for serialization /// Default constructor to allow for serialization
@ -272,6 +273,7 @@ private:
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*/) {
// NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
ar ar
& boost::serialization::make_nvp("NoiseModelFactor1", & boost::serialization::make_nvp("NoiseModelFactor1",
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));
@ -290,9 +292,9 @@ struct traits<NonlinearEquality1<VALUE> >
* be the same. * be the same.
*/ */
template <class T> template <class T>
class NonlinearEquality2 : public NoiseModelFactor2<T, T> { class NonlinearEquality2 : public NoiseModelFactorN<T, T> {
protected: protected:
using Base = NoiseModelFactor2<T, T>; using Base = NoiseModelFactorN<T, T>;
using This = NonlinearEquality2<T>; using This = NonlinearEquality2<T>;
GTSAM_CONCEPT_MANIFOLD_TYPE(T) GTSAM_CONCEPT_MANIFOLD_TYPE(T)
@ -337,6 +339,7 @@ class NonlinearEquality2 : public NoiseModelFactor2<T, T> {
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*/) {
// NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
ar& boost::serialization::make_nvp( ar& boost::serialization::make_nvp(
"NoiseModelFactor2", boost::serialization::base_object<Base>(*this)); "NoiseModelFactor2", boost::serialization::base_object<Base>(*this));
} }

49
gtsam/nonlinear/NonlinearFactor.h
View File

@ -14,6 +14,7 @@
* @brief Non-linear factor base classes * @brief Non-linear factor base classes
* @author Frank Dellaert * @author Frank Dellaert
* @author Richard Roberts * @author Richard Roberts
* @author Gerry Chen
*/ */
// \callgraph // \callgraph
@ -431,8 +432,9 @@ class NoiseModelFactorN : public NoiseModelFactor {
* // key<3>() // ERROR! Will not compile * // key<3>() // ERROR! Will not compile
* ``` * ```
*/ */
template <int I, typename = IndexIsValid<I>> template <int I = 1>
inline Key key() const { inline Key key() const {
static_assert(I <= N, "Index out of bounds");
return keys_[I - 1]; return keys_[I - 1];
} }
@ -492,6 +494,7 @@ class NoiseModelFactorN : public NoiseModelFactor {
OptionalMatrix<ValueTypes>... H) const = 0; OptionalMatrix<ValueTypes>... H) const = 0;
/// @} /// @}
/// @name Convenience method overloads /// @name Convenience method overloads
/// @{ /// @{
@ -550,11 +553,43 @@ class NoiseModelFactorN : public NoiseModelFactor {
ar& boost::serialization::make_nvp( ar& boost::serialization::make_nvp(
"NoiseModelFactor", boost::serialization::base_object<Base>(*this)); "NoiseModelFactor", boost::serialization::base_object<Base>(*this));
} }
public:
/// @name Deprecated methods
/// @{
template <int I = N, typename = typename std::enable_if<(I > 1), void>::type>
inline Key GTSAM_DEPRECATED key1() const {
return key<1>();
}
template <int I = 2, typename = IndexIsValid<I>>
inline Key GTSAM_DEPRECATED key2() const {
return key<2>();
}
template <int I = 3, typename = IndexIsValid<I>>
inline Key GTSAM_DEPRECATED key3() const {
return key<3>();
}
template <int I = 4, typename = IndexIsValid<I>>
inline Key GTSAM_DEPRECATED key4() const {
return key<4>();
}
template <int I = 5, typename = IndexIsValid<I>>
inline Key GTSAM_DEPRECATED key5() const {
return key<5>();
}
template <int I = 6, typename = IndexIsValid<I>>
inline Key GTSAM_DEPRECATED key6() const {
return key<6>();
}
/// @}
}; // \class NoiseModelFactorN }; // \class NoiseModelFactorN
/* ************************************************************************* */ /* ************************************************************************* */
/** @deprecated: use NoiseModelFactorN, replacing .key() with .key<1> and X1 /** @deprecated: use NoiseModelFactorN, replacing .key() with .key<1> and X1
* with ValueType<1>. * with ValueType<1>. If your class is templated, use `this->template key<1>()`
* A convenient base class for creating your own NoiseModelFactor * A convenient base class for creating your own NoiseModelFactor
* with 1 variable. To derive from this class, implement evaluateError(). * with 1 variable. To derive from this class, implement evaluateError().
*/ */
@ -595,7 +630,7 @@ class GTSAM_DEPRECATED NoiseModelFactor1 : public NoiseModelFactorN<VALUE> {
/* ************************************************************************* */ /* ************************************************************************* */
/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 /** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1
* with ValueType<1>. * with ValueType<1>. If your class is templated, use `this->template key<1>()`
* A convenient base class for creating your own NoiseModelFactor * A convenient base class for creating your own NoiseModelFactor
* with 2 variables. To derive from this class, implement evaluateError(). * with 2 variables. To derive from this class, implement evaluateError().
*/ */
@ -639,7 +674,7 @@ class GTSAM_DEPRECATED NoiseModelFactor2
/* ************************************************************************* */ /* ************************************************************************* */
/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 /** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1
* with ValueType<1>. * with ValueType<1>. If your class is templated, use `this->template key<1>()`
* A convenient base class for creating your own NoiseModelFactor * A convenient base class for creating your own NoiseModelFactor
* with 3 variables. To derive from this class, implement evaluateError(). * with 3 variables. To derive from this class, implement evaluateError().
*/ */
@ -685,7 +720,7 @@ class GTSAM_DEPRECATED NoiseModelFactor3
/* ************************************************************************* */ /* ************************************************************************* */
/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 /** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1
* with ValueType<1>. * with ValueType<1>. If your class is templated, use `this->template key<1>()`
* A convenient base class for creating your own NoiseModelFactor * A convenient base class for creating your own NoiseModelFactor
* with 4 variables. To derive from this class, implement evaluateError(). * with 4 variables. To derive from this class, implement evaluateError().
*/ */
@ -733,7 +768,7 @@ class GTSAM_DEPRECATED NoiseModelFactor4
/* ************************************************************************* */ /* ************************************************************************* */
/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 /** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1
* with ValueType<1>. * with ValueType<1>. If your class is templated, use `this->template key<1>()`
* A convenient base class for creating your own NoiseModelFactor * A convenient base class for creating your own NoiseModelFactor
* with 5 variables. To derive from this class, implement evaluateError(). * with 5 variables. To derive from this class, implement evaluateError().
*/ */
@ -784,7 +819,7 @@ class GTSAM_DEPRECATED NoiseModelFactor5
/* ************************************************************************* */ /* ************************************************************************* */
/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 /** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1
* with ValueType<1>. * with ValueType<1>. If your class is templated, use `this->template key<1>()`
* A convenient base class for creating your own NoiseModelFactor * A convenient base class for creating your own NoiseModelFactor
* with 6 variables. To derive from this class, implement evaluateError(). * with 6 variables. To derive from this class, implement evaluateError().
*/ */

5
gtsam/nonlinear/PriorFactor.h
View File

@ -27,14 +27,14 @@ namespace gtsam {
* @ingroup nonlinear * @ingroup nonlinear
*/ */
template<class VALUE> template<class VALUE>
class PriorFactor: public NoiseModelFactor1<VALUE> { class PriorFactor: public NoiseModelFactorN<VALUE> {
public: public:
typedef VALUE T; typedef VALUE T;
private: private:
typedef NoiseModelFactor1<VALUE> Base; typedef NoiseModelFactorN<VALUE> Base;
VALUE prior_; /** The measurement */ VALUE prior_; /** The measurement */
@ -105,6 +105,7 @@ 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*/) {
// NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
ar & boost::serialization::make_nvp("NoiseModelFactor1", ar & boost::serialization::make_nvp("NoiseModelFactor1",
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(prior_); ar & BOOST_SERIALIZATION_NVP(prior_);

4
gtsam/sfm/ShonanAveraging.cpp
View File

@ -958,7 +958,7 @@ static BinaryMeasurement<Rot2> convertPose2ToBinaryMeasurementRot2(
// the (2,2) entry of Pose2's covariance corresponds to Rot2's covariance // the (2,2) entry of Pose2's covariance corresponds to Rot2's covariance
// because the tangent space of Pose2 is ordered as (vx, vy, w) // because the tangent space of Pose2 is ordered as (vx, vy, w)
auto model = noiseModel::Isotropic::Variance(1, M(2, 2)); auto model = noiseModel::Isotropic::Variance(1, M(2, 2));
return BinaryMeasurement<Rot2>(f->key1(), f->key2(), f->measured().rotation(), return BinaryMeasurement<Rot2>(f->key<1>(), f->key<2>(), f->measured().rotation(),
model); model);
} }
@ -1006,7 +1006,7 @@ static BinaryMeasurement<Rot3> convert(
// the upper-left 3x3 sub-block of Pose3's covariance corresponds to Rot3's covariance // the upper-left 3x3 sub-block of Pose3's covariance corresponds to Rot3's covariance
// because the tangent space of Pose3 is ordered as (w,T) where w and T are both Vector3's // because the tangent space of Pose3 is ordered as (w,T) where w and T are both Vector3's
auto model = noiseModel::Gaussian::Covariance(M.block<3, 3>(0, 0)); auto model = noiseModel::Gaussian::Covariance(M.block<3, 3>(0, 0));
return BinaryMeasurement<Rot3>(f->key1(), f->key2(), f->measured().rotation(), return BinaryMeasurement<Rot3>(f->key<1>(), f->key<2>(), f->measured().rotation(),
model); model);
} }

8
gtsam/sfm/ShonanFactor.cpp
View File

@ -35,7 +35,7 @@ template <size_t d>
ShonanFactor<d>::ShonanFactor(Key j1, Key j2, const Rot &R12, size_t p, ShonanFactor<d>::ShonanFactor(Key j1, Key j2, const Rot &R12, size_t p,
const SharedNoiseModel &model, const SharedNoiseModel &model,
const boost::shared_ptr<Matrix> &G) const boost::shared_ptr<Matrix> &G)
: NoiseModelFactor2<SOn, SOn>(ConvertNoiseModel(model, p * d), j1, j2), : NoiseModelFactorN<SOn, SOn>(ConvertNoiseModel(model, p * d), j1, j2),
M_(R12.matrix()), // d*d in all cases M_(R12.matrix()), // d*d in all cases
p_(p), // 4 for SO(4) p_(p), // 4 for SO(4)
pp_(p * p), // 16 for SO(4) pp_(p * p), // 16 for SO(4)
@ -57,8 +57,8 @@ ShonanFactor<d>::ShonanFactor(Key j1, Key j2, const Rot &R12, size_t p,
template <size_t d> template <size_t d>
void ShonanFactor<d>::print(const std::string &s, void ShonanFactor<d>::print(const std::string &s,
const KeyFormatter &keyFormatter) const { const KeyFormatter &keyFormatter) const {
std::cout << s << "ShonanFactor<" << p_ << ">(" << keyFormatter(key1()) << "," std::cout << s << "ShonanFactor<" << p_ << ">(" << keyFormatter(key<1>()) << ","
<< keyFormatter(key2()) << ")\n"; << keyFormatter(key<2>()) << ")\n";
traits<Matrix>::Print(M_, " M: "); traits<Matrix>::Print(M_, " M: ");
noiseModel_->print(" noise model: "); noiseModel_->print(" noise model: ");
} }
@ -68,7 +68,7 @@ template <size_t d>
bool ShonanFactor<d>::equals(const NonlinearFactor &expected, bool ShonanFactor<d>::equals(const NonlinearFactor &expected,
double tol) const { double tol) const {
auto e = dynamic_cast<const ShonanFactor *>(&expected); auto e = dynamic_cast<const ShonanFactor *>(&expected);
return e != nullptr && NoiseModelFactor2<SOn, SOn>::equals(*e, tol) && return e != nullptr && NoiseModelFactorN<SOn, SOn>::equals(*e, tol) &&
p_ == e->p_ && M_ == e->M_; p_ == e->p_ && M_ == e->M_;
} }

4
gtsam/sfm/ShonanFactor.h
View File

@ -33,7 +33,7 @@ namespace gtsam {
* the SO(p) matrices down to a Stiefel manifold of p*d matrices. * the SO(p) matrices down to a Stiefel manifold of p*d matrices.
*/ */
template <size_t d> template <size_t d>
class GTSAM_EXPORT ShonanFactor : public NoiseModelFactor2<SOn, SOn> { class GTSAM_EXPORT ShonanFactor : public NoiseModelFactorN<SOn, SOn> {
Matrix M_; ///< measured rotation between R1 and R2 Matrix M_; ///< measured rotation between R1 and R2
size_t p_, pp_; ///< dimensionality constants size_t p_, pp_; ///< dimensionality constants
boost::shared_ptr<Matrix> G_; ///< matrix of vectorized generators boost::shared_ptr<Matrix> G_; ///< matrix of vectorized generators
@ -66,7 +66,7 @@ public:
double tol = 1e-9) const override; double tol = 1e-9) const override;
/// @} /// @}
/// @name NoiseModelFactor2 methods /// @name NoiseModelFactorN methods
/// @{ /// @{
/// Error is Frobenius norm between Q1*P*R12 and Q2*P, where P=[I_3x3;0] /// Error is Frobenius norm between Q1*P*R12 and Q2*P, where P=[I_3x3;0]

4
gtsam/sfm/TranslationFactor.h
View File

@ -39,9 +39,9 @@ namespace gtsam {
* *
* *
*/ */
class TranslationFactor : public NoiseModelFactor2<Point3, Point3> { class TranslationFactor : public NoiseModelFactorN<Point3, Point3> {
private: private:
typedef NoiseModelFactor2<Point3, Point3> Base; typedef NoiseModelFactorN<Point3, Point3> Base;
Point3 measured_w_aZb_; Point3 measured_w_aZb_;
public: public:

11
gtsam/slam/BetweenFactor.h
View File

@ -37,7 +37,7 @@ namespace gtsam {
* @ingroup slam * @ingroup slam
*/ */
template<class VALUE> template<class VALUE>
class BetweenFactor: public NoiseModelFactor2<VALUE, VALUE> { class BetweenFactor: public NoiseModelFactorN<VALUE, VALUE> {
// Check that VALUE type is a testable Lie group // Check that VALUE type is a testable Lie group
BOOST_CONCEPT_ASSERT((IsTestable<VALUE>)); BOOST_CONCEPT_ASSERT((IsTestable<VALUE>));
@ -50,7 +50,7 @@ namespace gtsam {
private: private:
typedef BetweenFactor<VALUE> This; typedef BetweenFactor<VALUE> This;
typedef NoiseModelFactor2<VALUE, VALUE> Base; typedef NoiseModelFactorN<VALUE, VALUE> Base;
VALUE measured_; /** The measurement */ VALUE measured_; /** The measurement */
@ -88,8 +88,8 @@ namespace gtsam {
const std::string& s = "", const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
std::cout << s << "BetweenFactor(" std::cout << s << "BetweenFactor("
<< keyFormatter(this->key1()) << "," << keyFormatter(this->template key<1>()) << ","
<< keyFormatter(this->key2()) << ")\n"; << keyFormatter(this->template key<2>()) << ")\n";
traits<T>::Print(measured_, " measured: "); traits<T>::Print(measured_, " measured: ");
this->noiseModel_->print(" noise model: "); this->noiseModel_->print(" noise model: ");
} }
@ -101,7 +101,7 @@ namespace gtsam {
} }
/// @} /// @}
/// @name NoiseModelFactor2 methods /// @name NoiseModelFactorN methods
/// @{ /// @{
/// evaluate error, returns vector of errors size of tangent space /// evaluate error, returns vector of errors size of tangent space
@ -136,6 +136,7 @@ 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*/) {
// NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
ar & boost::serialization::make_nvp("NoiseModelFactor2", ar & boost::serialization::make_nvp("NoiseModelFactor2",
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(measured_);

12
gtsam/slam/BoundingConstraint.h
View File

@ -30,9 +30,9 @@ namespace gtsam {
* @ingroup slam * @ingroup slam
*/ */
template<class VALUE> template<class VALUE>
struct BoundingConstraint1: public NoiseModelFactor1<VALUE> { struct BoundingConstraint1: public NoiseModelFactorN<VALUE> {
typedef VALUE X; typedef VALUE X;
typedef NoiseModelFactor1<VALUE> Base; typedef NoiseModelFactorN<VALUE> Base;
typedef boost::shared_ptr<BoundingConstraint1<VALUE> > shared_ptr; typedef boost::shared_ptr<BoundingConstraint1<VALUE> > shared_ptr;
double threshold_; double threshold_;
@ -85,6 +85,7 @@ private:
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*/) {
// NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
ar & boost::serialization::make_nvp("NoiseModelFactor1", ar & boost::serialization::make_nvp("NoiseModelFactor1",
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(threshold_); ar & BOOST_SERIALIZATION_NVP(threshold_);
@ -97,11 +98,11 @@ private:
* to implement for specific systems * to implement for specific systems
*/ */
template<class VALUE1, class VALUE2> template<class VALUE1, class VALUE2>
struct BoundingConstraint2: public NoiseModelFactor2<VALUE1, VALUE2> { struct BoundingConstraint2: public NoiseModelFactorN<VALUE1, VALUE2> {
typedef VALUE1 X1; typedef VALUE1 X1;
typedef VALUE2 X2; typedef VALUE2 X2;
typedef NoiseModelFactor2<VALUE1, VALUE2> Base; typedef NoiseModelFactorN<VALUE1, VALUE2> Base;
typedef boost::shared_ptr<BoundingConstraint2<VALUE1, VALUE2> > shared_ptr; typedef boost::shared_ptr<BoundingConstraint2<VALUE1, VALUE2> > shared_ptr;
double threshold_; double threshold_;
@ -128,7 +129,7 @@ struct BoundingConstraint2: public NoiseModelFactor2<VALUE1, VALUE2> {
/** active when constraint *NOT* met */ /** active when constraint *NOT* met */
bool active(const Values& c) const override { bool active(const Values& c) const override {
// note: still active at equality to avoid zigzagging // note: still active at equality to avoid zigzagging
double x = value(c.at<X1>(this->key1()), c.at<X2>(this->key2())); double x = value(c.at<X1>(this->template key<1>()), c.at<X2>(this->template key<2>()));
return (isGreaterThan_) ? x <= threshold_ : x >= threshold_; return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
} }
@ -158,6 +159,7 @@ private:
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*/) {
// NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
ar & boost::serialization::make_nvp("NoiseModelFactor2", ar & boost::serialization::make_nvp("NoiseModelFactor2",
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(threshold_); ar & BOOST_SERIALIZATION_NVP(threshold_);

4
gtsam/slam/EssentialMatrixConstraint.cpp
View File

@ -27,8 +27,8 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
void EssentialMatrixConstraint::print(const std::string& s, void EssentialMatrixConstraint::print(const std::string& s,
const KeyFormatter& keyFormatter) const { const KeyFormatter& keyFormatter) const {
std::cout << s << "EssentialMatrixConstraint(" << keyFormatter(this->key1()) std::cout << s << "EssentialMatrixConstraint(" << keyFormatter(this->key<1>())
<< "," << keyFormatter(this->key2()) << ")\n"; << "," << keyFormatter(this->key<2>()) << ")\n";
measuredE_.print(" measured: "); measuredE_.print(" measured: ");
this->noiseModel_->print(" noise model: "); this->noiseModel_->print(" noise model: ");
} }

5
gtsam/slam/EssentialMatrixConstraint.h
View File

@ -27,12 +27,12 @@ namespace gtsam {
* Binary factor between two Pose3 variables induced by an EssentialMatrix measurement * Binary factor between two Pose3 variables induced by an EssentialMatrix measurement
* @ingroup slam * @ingroup slam
*/ */
class GTSAM_EXPORT EssentialMatrixConstraint: public NoiseModelFactor2<Pose3, Pose3> { class GTSAM_EXPORT EssentialMatrixConstraint: public NoiseModelFactorN<Pose3, Pose3> {
private: private:
typedef EssentialMatrixConstraint This; typedef EssentialMatrixConstraint This;
typedef NoiseModelFactor2<Pose3, Pose3> Base; typedef NoiseModelFactorN<Pose3, Pose3> Base;
EssentialMatrix measuredE_; /** The measurement is an essential matrix */ EssentialMatrix measuredE_; /** The measurement is an essential matrix */
@ -93,6 +93,7 @@ private:
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*/) {
// NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
ar ar
& boost::serialization::make_nvp("NoiseModelFactor2", & boost::serialization::make_nvp("NoiseModelFactor2",
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));

12
gtsam/slam/EssentialMatrixFactor.h
View File

@ -31,10 +31,10 @@ namespace gtsam {
/** /**
* Factor that evaluates epipolar error p'Ep for given essential matrix * Factor that evaluates epipolar error p'Ep for given essential matrix
*/ */
class EssentialMatrixFactor : public NoiseModelFactor1<EssentialMatrix> { class EssentialMatrixFactor : public NoiseModelFactorN<EssentialMatrix> {
Vector3 vA_, vB_; ///< Homogeneous versions, in ideal coordinates Vector3 vA_, vB_; ///< Homogeneous versions, in ideal coordinates
typedef NoiseModelFactor1<EssentialMatrix> Base; typedef NoiseModelFactorN<EssentialMatrix> Base;
typedef EssentialMatrixFactor This; typedef EssentialMatrixFactor This;
public: public:
@ -106,12 +106,12 @@ class EssentialMatrixFactor : public NoiseModelFactor1<EssentialMatrix> {
* in image 2 is perfect, and returns re-projection error in image 1 * in image 2 is perfect, and returns re-projection error in image 1
*/ */
class EssentialMatrixFactor2 class EssentialMatrixFactor2
: public NoiseModelFactor2<EssentialMatrix, double> { : public NoiseModelFactorN<EssentialMatrix, double> {
Point3 dP1_; ///< 3D point corresponding to measurement in image 1 Point3 dP1_; ///< 3D point corresponding to measurement in image 1
Point2 pn_; ///< Measurement in image 2, in ideal coordinates Point2 pn_; ///< Measurement in image 2, in ideal coordinates
double f_; ///< approximate conversion factor for error scaling double f_; ///< approximate conversion factor for error scaling
typedef NoiseModelFactor2<EssentialMatrix, double> Base; typedef NoiseModelFactorN<EssentialMatrix, double> Base;
typedef EssentialMatrixFactor2 This; typedef EssentialMatrixFactor2 This;
public: public:
@ -321,11 +321,11 @@ class EssentialMatrixFactor3 : public EssentialMatrixFactor2 {
*/ */
template <class CALIBRATION> template <class CALIBRATION>
class EssentialMatrixFactor4 class EssentialMatrixFactor4
: public NoiseModelFactor2<EssentialMatrix, CALIBRATION> { : public NoiseModelFactorN<EssentialMatrix, CALIBRATION> {
private: private:
Point2 pA_, pB_; ///< points in pixel coordinates Point2 pA_, pB_; ///< points in pixel coordinates
typedef NoiseModelFactor2<EssentialMatrix, CALIBRATION> Base; typedef NoiseModelFactorN<EssentialMatrix, CALIBRATION> Base;
typedef EssentialMatrixFactor4 This; typedef EssentialMatrixFactor4 This;
static constexpr int DimK = FixedDimension<CALIBRATION>::value; static constexpr int DimK = FixedDimension<CALIBRATION>::value;

20
gtsam/slam/FrobeniusFactor.h
View File

@ -48,7 +48,7 @@ ConvertNoiseModel(const SharedNoiseModel &model, size_t n,
* element of SO(3) or SO(4). * element of SO(3) or SO(4).
*/ */
template <class Rot> template <class Rot>
class FrobeniusPrior : public NoiseModelFactor1<Rot> { class FrobeniusPrior : public NoiseModelFactorN<Rot> {
enum { Dim = Rot::VectorN2::RowsAtCompileTime }; enum { Dim = Rot::VectorN2::RowsAtCompileTime };
using MatrixNN = typename Rot::MatrixNN; using MatrixNN = typename Rot::MatrixNN;
Eigen::Matrix<double, Dim, 1> vecM_; ///< vectorized matrix to approximate Eigen::Matrix<double, Dim, 1> vecM_; ///< vectorized matrix to approximate
@ -59,7 +59,7 @@ class FrobeniusPrior : public NoiseModelFactor1<Rot> {
/// Constructor /// Constructor
FrobeniusPrior(Key j, const MatrixNN& M, FrobeniusPrior(Key j, const MatrixNN& M,
const SharedNoiseModel& model = nullptr) const SharedNoiseModel& model = nullptr)
: NoiseModelFactor1<Rot>(ConvertNoiseModel(model, Dim), j) { : NoiseModelFactorN<Rot>(ConvertNoiseModel(model, Dim), j) {
vecM_ << Eigen::Map<const Matrix>(M.data(), Dim, 1); vecM_ << Eigen::Map<const Matrix>(M.data(), Dim, 1);
} }
@ -75,13 +75,13 @@ class FrobeniusPrior : public NoiseModelFactor1<Rot> {
* The template argument can be any fixed-size SO<N>. * The template argument can be any fixed-size SO<N>.
*/ */
template <class Rot> template <class Rot>
class FrobeniusFactor : public NoiseModelFactor2<Rot, Rot> { class FrobeniusFactor : public NoiseModelFactorN<Rot, Rot> {
enum { Dim = Rot::VectorN2::RowsAtCompileTime }; enum { Dim = Rot::VectorN2::RowsAtCompileTime };
public: public:
/// Constructor /// Constructor
FrobeniusFactor(Key j1, Key j2, const SharedNoiseModel& model = nullptr) FrobeniusFactor(Key j1, Key j2, const SharedNoiseModel& model = nullptr)
: NoiseModelFactor2<Rot, Rot>(ConvertNoiseModel(model, Dim), j1, : NoiseModelFactorN<Rot, Rot>(ConvertNoiseModel(model, Dim), j1,
j2) {} j2) {}
/// Error is just Frobenius norm between rotation matrices. /// Error is just Frobenius norm between rotation matrices.
@ -101,7 +101,7 @@ class FrobeniusFactor : public NoiseModelFactor2<Rot, Rot> {
* and in fact only SO3 and SO4 really work, as we need SO<N>::AdjointMap. * and in fact only SO3 and SO4 really work, as we need SO<N>::AdjointMap.
*/ */
template <class Rot> template <class Rot>
class FrobeniusBetweenFactor : public NoiseModelFactor2<Rot, Rot> { class FrobeniusBetweenFactor : public NoiseModelFactorN<Rot, Rot> {
Rot R12_; ///< measured rotation between R1 and R2 Rot R12_; ///< measured rotation between R1 and R2
Eigen::Matrix<double, Rot::dimension, Rot::dimension> Eigen::Matrix<double, Rot::dimension, Rot::dimension>
R2hat_H_R1_; ///< fixed derivative of R2hat wrpt R1 R2hat_H_R1_; ///< fixed derivative of R2hat wrpt R1
@ -116,7 +116,7 @@ class FrobeniusBetweenFactor : public NoiseModelFactor2<Rot, Rot> {
/// Construct from two keys and measured rotation /// Construct from two keys and measured rotation
FrobeniusBetweenFactor(Key j1, Key j2, const Rot& R12, FrobeniusBetweenFactor(Key j1, Key j2, const Rot& R12,
const SharedNoiseModel& model = nullptr) const SharedNoiseModel& model = nullptr)
: NoiseModelFactor2<Rot, Rot>( : NoiseModelFactorN<Rot, Rot>(
ConvertNoiseModel(model, Dim), j1, j2), ConvertNoiseModel(model, Dim), j1, j2),
R12_(R12), R12_(R12),
R2hat_H_R1_(R12.inverse().AdjointMap()) {} R2hat_H_R1_(R12.inverse().AdjointMap()) {}
@ -130,8 +130,8 @@ class FrobeniusBetweenFactor : public NoiseModelFactor2<Rot, Rot> {
print(const std::string &s, print(const std::string &s,
const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override { const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override {
std::cout << s << "FrobeniusBetweenFactor<" << demangle(typeid(Rot).name()) std::cout << s << "FrobeniusBetweenFactor<" << demangle(typeid(Rot).name())
<< ">(" << keyFormatter(this->key1()) << "," << ">(" << keyFormatter(this->template key<1>()) << ","
<< keyFormatter(this->key2()) << ")\n"; << keyFormatter(this->template key<2>()) << ")\n";
traits<Rot>::Print(R12_, " R12: "); traits<Rot>::Print(R12_, " R12: ");
this->noiseModel_->print(" noise model: "); this->noiseModel_->print(" noise model: ");
} }
@ -140,12 +140,12 @@ class FrobeniusBetweenFactor : public NoiseModelFactor2<Rot, Rot> {
bool equals(const NonlinearFactor &expected, bool equals(const NonlinearFactor &expected,
double tol = 1e-9) const override { double tol = 1e-9) const override {
auto e = dynamic_cast<const FrobeniusBetweenFactor *>(&expected); auto e = dynamic_cast<const FrobeniusBetweenFactor *>(&expected);
return e != nullptr && NoiseModelFactor2<Rot, Rot>::equals(*e, tol) && return e != nullptr && NoiseModelFactorN<Rot, Rot>::equals(*e, tol) &&
traits<Rot>::Equals(this->R12_, e->R12_, tol); traits<Rot>::Equals(this->R12_, e->R12_, tol);
} }
/// @} /// @}
/// @name NoiseModelFactor2 methods /// @name NoiseModelFactorN methods
/// @{ /// @{
/// Error is Frobenius norm between R1*R12 and R2. /// Error is Frobenius norm between R1*R12 and R2.

16
gtsam/slam/GeneralSFMFactor.h
View File

@ -57,7 +57,7 @@ namespace gtsam {
* @ingroup slam * @ingroup slam
*/ */
template<class CAMERA, class LANDMARK> template<class CAMERA, class LANDMARK>
class GeneralSFMFactor: public NoiseModelFactor2<CAMERA, LANDMARK> { class GeneralSFMFactor: public NoiseModelFactorN<CAMERA, LANDMARK> {
GTSAM_CONCEPT_MANIFOLD_TYPE(CAMERA) GTSAM_CONCEPT_MANIFOLD_TYPE(CAMERA)
GTSAM_CONCEPT_MANIFOLD_TYPE(LANDMARK) GTSAM_CONCEPT_MANIFOLD_TYPE(LANDMARK)
@ -74,7 +74,7 @@ protected:
public: public:
typedef GeneralSFMFactor<CAMERA, LANDMARK> This;///< typedef for this object typedef GeneralSFMFactor<CAMERA, LANDMARK> This;///< typedef for this object
typedef NoiseModelFactor2<CAMERA, LANDMARK> Base;///< typedef for the base class typedef NoiseModelFactorN<CAMERA, LANDMARK> Base;///< typedef for the base class
// shorthand for a smart pointer to a factor // shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr; typedef boost::shared_ptr<This> shared_ptr;
@ -140,7 +140,7 @@ public:
// Only linearize if the factor is active // Only linearize if the factor is active
if (!this->active(values)) return boost::shared_ptr<JacobianFactor>(); if (!this->active(values)) return boost::shared_ptr<JacobianFactor>();
const Key key1 = this->key1(), key2 = this->key2(); const Key key1 = this->template key<1>(), key2 = this->template key<2>();
JacobianC H1; JacobianC H1;
JacobianL H2; JacobianL H2;
Vector2 b; Vector2 b;
@ -184,6 +184,7 @@ private:
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*/) {
// NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
ar & boost::serialization::make_nvp("NoiseModelFactor2", ar & boost::serialization::make_nvp("NoiseModelFactor2",
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(measured_);
@ -200,7 +201,7 @@ struct traits<GeneralSFMFactor<CAMERA, LANDMARK> > : Testable<
* Compared to GeneralSFMFactor, it is a ternary-factor because the calibration is isolated from camera.. * Compared to GeneralSFMFactor, it is a ternary-factor because the calibration is isolated from camera..
*/ */
template<class CALIBRATION> template<class CALIBRATION>
class GeneralSFMFactor2: public NoiseModelFactor3<Pose3, Point3, CALIBRATION> { class GeneralSFMFactor2: public NoiseModelFactorN<Pose3, Point3, CALIBRATION> {
GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION) GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION)
static const int DimK = FixedDimension<CALIBRATION>::value; static const int DimK = FixedDimension<CALIBRATION>::value;
@ -213,7 +214,7 @@ public:
typedef GeneralSFMFactor2<CALIBRATION> This; typedef GeneralSFMFactor2<CALIBRATION> This;
typedef PinholeCamera<CALIBRATION> Camera;///< typedef for camera type typedef PinholeCamera<CALIBRATION> Camera;///< typedef for camera type
typedef NoiseModelFactor3<Pose3, Point3, CALIBRATION> Base;///< typedef for the base class typedef NoiseModelFactorN<Pose3, Point3, CALIBRATION> Base;///< typedef for the base class
// shorthand for a smart pointer to a factor // shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr; typedef boost::shared_ptr<This> shared_ptr;
@ -269,8 +270,8 @@ public:
if (H1) *H1 = Matrix::Zero(2, 6); if (H1) *H1 = Matrix::Zero(2, 6);
if (H2) *H2 = Matrix::Zero(2, 3); if (H2) *H2 = Matrix::Zero(2, 3);
if (H3) *H3 = Matrix::Zero(2, DimK); if (H3) *H3 = Matrix::Zero(2, DimK);
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->template key<2>())
<< " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl; << " behind Camera " << DefaultKeyFormatter(this->template key<1>()) << std::endl;
} }
return Z_2x1; return Z_2x1;
} }
@ -285,6 +286,7 @@ private:
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*/) {
// NoiseModelFactor3 instead of NoiseModelFactorN for backward compatibility
ar & boost::serialization::make_nvp("NoiseModelFactor3", ar & boost::serialization::make_nvp("NoiseModelFactor3",
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(measured_);

4
gtsam/slam/InitializePose3.cpp
View File

@ -228,7 +228,7 @@ void InitializePose3::createSymbolicGraph(
Rot3 Rij = pose3Between->measured().rotation(); Rot3 Rij = pose3Between->measured().rotation();
factorId2RotMap->emplace(factorId, Rij); factorId2RotMap->emplace(factorId, Rij);
Key key1 = pose3Between->key1(); Key key1 = pose3Between->key<1>();
if (adjEdgesMap->find(key1) != adjEdgesMap->end()) { // key is already in if (adjEdgesMap->find(key1) != adjEdgesMap->end()) { // key is already in
adjEdgesMap->at(key1).push_back(factorId); adjEdgesMap->at(key1).push_back(factorId);
} else { } else {
@ -236,7 +236,7 @@ void InitializePose3::createSymbolicGraph(
edge_id.push_back(factorId); edge_id.push_back(factorId);
adjEdgesMap->emplace(key1, edge_id); adjEdgesMap->emplace(key1, edge_id);
} }
Key key2 = pose3Between->key2(); Key key2 = pose3Between->key<2>();
if (adjEdgesMap->find(key2) != adjEdgesMap->end()) { // key is already in if (adjEdgesMap->find(key2) != adjEdgesMap->end()) { // key is already in
adjEdgesMap->at(key2).push_back(factorId); adjEdgesMap->at(key2).push_back(factorId);
} else { } else {

4
gtsam/slam/OrientedPlane3Factor.cpp
View File

@ -15,8 +15,8 @@ namespace gtsam {
void OrientedPlane3Factor::print(const string& s, void OrientedPlane3Factor::print(const string& s,
const KeyFormatter& keyFormatter) const { const KeyFormatter& keyFormatter) const {
cout << s << (s == "" ? "" : "\n"); cout << s << (s == "" ? "" : "\n");
cout << "OrientedPlane3Factor Factor (" << keyFormatter(key1()) << ", " cout << "OrientedPlane3Factor Factor (" << keyFormatter(key<1>()) << ", "
<< keyFormatter(key2()) << ")\n"; << keyFormatter(key<2>()) << ")\n";
measured_p_.print("Measured Plane"); measured_p_.print("Measured Plane");
this->noiseModel_->print(" noise model: "); this->noiseModel_->print(" noise model: ");
} }

8
gtsam/slam/OrientedPlane3Factor.h
View File

@ -15,10 +15,10 @@ namespace gtsam {
/** /**
* Factor to measure a planar landmark from a given pose * Factor to measure a planar landmark from a given pose
*/ */
class GTSAM_EXPORT OrientedPlane3Factor: public NoiseModelFactor2<Pose3, OrientedPlane3> { class GTSAM_EXPORT OrientedPlane3Factor: public NoiseModelFactorN<Pose3, OrientedPlane3> {
protected: protected:
OrientedPlane3 measured_p_; OrientedPlane3 measured_p_;
typedef NoiseModelFactor2<Pose3, OrientedPlane3> Base; typedef NoiseModelFactorN<Pose3, OrientedPlane3> Base;
public: public:
/// Constructor /// Constructor
@ -49,10 +49,10 @@ class GTSAM_EXPORT OrientedPlane3Factor: public NoiseModelFactor2<Pose3, Oriente
}; };
// TODO: Convert this factor to dimension two, three dimensions is redundant for direction prior // TODO: Convert this factor to dimension two, three dimensions is redundant for direction prior
class GTSAM_EXPORT OrientedPlane3DirectionPrior : public NoiseModelFactor1<OrientedPlane3> { class GTSAM_EXPORT OrientedPlane3DirectionPrior : public NoiseModelFactorN<OrientedPlane3> {
protected: protected:
OrientedPlane3 measured_p_; /// measured plane parameters OrientedPlane3 measured_p_; /// measured plane parameters
typedef NoiseModelFactor1<OrientedPlane3> Base; typedef NoiseModelFactorN<OrientedPlane3> Base;
public: public:
typedef OrientedPlane3DirectionPrior This; typedef OrientedPlane3DirectionPrior This;

5
gtsam/slam/PoseRotationPrior.h
View File

@ -16,11 +16,11 @@
namespace gtsam { namespace gtsam {
template<class POSE> template<class POSE>
class PoseRotationPrior : public NoiseModelFactor1<POSE> { class PoseRotationPrior : public NoiseModelFactorN<POSE> {
public: public:
typedef PoseRotationPrior<POSE> This; typedef PoseRotationPrior<POSE> This;
typedef NoiseModelFactor1<POSE> Base; typedef NoiseModelFactorN<POSE> Base;
typedef POSE Pose; typedef POSE Pose;
typedef typename POSE::Translation Translation; typedef typename POSE::Translation Translation;
typedef typename POSE::Rotation Rotation; typedef typename POSE::Rotation Rotation;
@ -92,6 +92,7 @@ private:
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*/) {
// NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
ar & boost::serialization::make_nvp("NoiseModelFactor1", ar & boost::serialization::make_nvp("NoiseModelFactor1",
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(measured_);

5
gtsam/slam/PoseTranslationPrior.h
View File

@ -18,10 +18,10 @@ namespace gtsam {
* A prior on the translation part of a pose * A prior on the translation part of a pose
*/ */
template<class POSE> template<class POSE>
class PoseTranslationPrior : public NoiseModelFactor1<POSE> { class PoseTranslationPrior : public NoiseModelFactorN<POSE> {
public: public:
typedef PoseTranslationPrior<POSE> This; typedef PoseTranslationPrior<POSE> This;
typedef NoiseModelFactor1<POSE> Base; typedef NoiseModelFactorN<POSE> Base;
typedef POSE Pose; typedef POSE Pose;
typedef typename POSE::Translation Translation; typedef typename POSE::Translation Translation;
typedef typename POSE::Rotation Rotation; typedef typename POSE::Rotation Rotation;
@ -91,6 +91,7 @@ private:
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*/) {
// NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
ar & boost::serialization::make_nvp("NoiseModelFactor1", ar & boost::serialization::make_nvp("NoiseModelFactor1",
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(measured_);

10
gtsam/slam/ProjectionFactor.h
View File

@ -37,7 +37,7 @@ namespace gtsam {
*/ */
template <class POSE = Pose3, class LANDMARK = Point3, template <class POSE = Pose3, class LANDMARK = Point3,
class CALIBRATION = Cal3_S2> class CALIBRATION = Cal3_S2>
class GenericProjectionFactor: public NoiseModelFactor2<POSE, LANDMARK> { class GenericProjectionFactor: public NoiseModelFactorN<POSE, LANDMARK> {
protected: protected:
// Keep a copy of measurement and calibration for I/O // Keep a copy of measurement and calibration for I/O
@ -52,7 +52,7 @@ namespace gtsam {
public: public:
/// shorthand for base class type /// shorthand for base class type
typedef NoiseModelFactor2<POSE, LANDMARK> Base; typedef NoiseModelFactorN<POSE, LANDMARK> Base;
/// shorthand for this class /// shorthand for this class
typedef GenericProjectionFactor<POSE, LANDMARK, CALIBRATION> This; typedef GenericProjectionFactor<POSE, LANDMARK, CALIBRATION> This;
@ -154,10 +154,10 @@ namespace gtsam {
if (H1) *H1 = Matrix::Zero(2,6); if (H1) *H1 = Matrix::Zero(2,6);
if (H2) *H2 = Matrix::Zero(2,3); if (H2) *H2 = Matrix::Zero(2,3);
if (verboseCheirality_) if (verboseCheirality_)
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->template key<2>()) <<
" moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; " moved behind camera " << DefaultKeyFormatter(this->template key<1>()) << std::endl;
if (throwCheirality_) if (throwCheirality_)
throw CheiralityException(this->key2()); throw CheiralityException(this->template key<2>());
} }
return Vector2::Constant(2.0 * K_->fx()); return Vector2::Constant(2.0 * K_->fx());
} }

16
gtsam/slam/ReferenceFrameFactor.h
View File

@ -54,13 +54,13 @@ P transform_point(
* specific classes of landmarks * specific classes of landmarks
*/ */
template<class POINT, class TRANSFORM> template<class POINT, class TRANSFORM>
class ReferenceFrameFactor : public NoiseModelFactor3<POINT, TRANSFORM, POINT> { class ReferenceFrameFactor : public NoiseModelFactorN<POINT, TRANSFORM, POINT> {
protected: protected:
/** default constructor for serialization only */ /** default constructor for serialization only */
ReferenceFrameFactor() {} ReferenceFrameFactor() {}
public: public:
typedef NoiseModelFactor3<POINT, TRANSFORM, POINT> Base; typedef NoiseModelFactorN<POINT, TRANSFORM, POINT> Base;
typedef ReferenceFrameFactor<POINT, TRANSFORM> This; typedef ReferenceFrameFactor<POINT, TRANSFORM> This;
typedef POINT Point; typedef POINT Point;
@ -107,16 +107,16 @@ public:
void print(const std::string& s="", void print(const std::string& s="",
const gtsam::KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { const gtsam::KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
std::cout << s << ": ReferenceFrameFactor(" std::cout << s << ": ReferenceFrameFactor("
<< "Global: " << keyFormatter(this->key1()) << "," << "Global: " << keyFormatter(this->template key<1>()) << ","
<< " Transform: " << keyFormatter(this->key2()) << "," << " Transform: " << keyFormatter(this->template key<2>()) << ","
<< " Local: " << keyFormatter(this->key3()) << ")\n"; << " Local: " << keyFormatter(this->template key<3>()) << ")\n";
this->noiseModel_->print(" noise model"); this->noiseModel_->print(" noise model");
} }
// access - convenience functions // access - convenience functions
Key global_key() const { return this->key1(); } Key global_key() const { return this->template key<1>(); }
Key transform_key() const { return this->key2(); } Key transform_key() const { return this->template key<2>(); }
Key local_key() const { return this->key3(); } Key local_key() const { return this->template key<3>(); }
private: private:
/** Serialization function */ /** Serialization function */

8
gtsam/slam/RotateFactor.h
View File

@ -20,11 +20,11 @@ namespace gtsam {
* with z and p measured and predicted angular velocities, and hence * with z and p measured and predicted angular velocities, and hence
* p = iRc * z * p = iRc * z
*/ */
class RotateFactor: public NoiseModelFactor1<Rot3> { class RotateFactor: public NoiseModelFactorN<Rot3> {
Point3 p_, z_; ///< Predicted and measured directions, p = iRc * z Point3 p_, z_; ///< Predicted and measured directions, p = iRc * z
typedef NoiseModelFactor1<Rot3> Base; typedef NoiseModelFactorN<Rot3> Base;
typedef RotateFactor This; typedef RotateFactor This;
public: public:
@ -64,11 +64,11 @@ public:
* Factor on unknown rotation iRc that relates two directions c * Factor on unknown rotation iRc that relates two directions c
* Directions provide less constraints than a full rotation * Directions provide less constraints than a full rotation
*/ */
class RotateDirectionsFactor: public NoiseModelFactor1<Rot3> { class RotateDirectionsFactor: public NoiseModelFactorN<Rot3> {
Unit3 i_p_, c_z_; ///< Predicted and measured directions, i_p = iRc * c_z Unit3 i_p_, c_z_; ///< Predicted and measured directions, i_p = iRc * c_z
typedef NoiseModelFactor1<Rot3> Base; typedef NoiseModelFactorN<Rot3> Base;
typedef RotateDirectionsFactor This; typedef RotateDirectionsFactor This;
public: public:

9
gtsam/slam/StereoFactor.h
View File

@ -28,7 +28,7 @@ namespace gtsam {
* @ingroup slam * @ingroup slam
*/ */
template<class POSE, class LANDMARK> template<class POSE, class LANDMARK>
class GenericStereoFactor: public NoiseModelFactor2<POSE, LANDMARK> { class GenericStereoFactor: public NoiseModelFactorN<POSE, LANDMARK> {
private: private:
// Keep a copy of measurement and calibration for I/O // Keep a copy of measurement and calibration for I/O
@ -43,7 +43,7 @@ private:
public: public:
// shorthand for base class type // shorthand for base class type
typedef NoiseModelFactor2<POSE, LANDMARK> Base; ///< typedef for base class typedef NoiseModelFactorN<POSE, LANDMARK> Base; ///< typedef for base class
typedef GenericStereoFactor<POSE, LANDMARK> This; ///< typedef for this class (with templates) typedef GenericStereoFactor<POSE, LANDMARK> This; ///< typedef for this class (with templates)
typedef boost::shared_ptr<GenericStereoFactor> shared_ptr; ///< typedef for shared pointer to this object typedef boost::shared_ptr<GenericStereoFactor> shared_ptr; ///< typedef for shared pointer to this object
typedef POSE CamPose; ///< typedef for Pose Lie Value type typedef POSE CamPose; ///< typedef for Pose Lie Value type
@ -141,8 +141,8 @@ public:
if (H1) *H1 = Matrix::Zero(3,6); if (H1) *H1 = Matrix::Zero(3,6);
if (H2) *H2 = Z_3x3; if (H2) *H2 = Z_3x3;
if (verboseCheirality_) if (verboseCheirality_)
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->template key<2>()) <<
" moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; " moved behind camera " << DefaultKeyFormatter(this->template key<1>()) << std::endl;
if (throwCheirality_) if (throwCheirality_)
throw StereoCheiralityException(this->key2()); throw StereoCheiralityException(this->key2());
} }
@ -170,6 +170,7 @@ private:
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*/) {
// NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
ar & boost::serialization::make_nvp("NoiseModelFactor2", ar & boost::serialization::make_nvp("NoiseModelFactor2",
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(measured_);

4
gtsam/slam/TriangulationFactor.h
View File

@ -30,7 +30,7 @@ namespace gtsam {
* @ingroup slam * @ingroup slam
*/ */
template<class CAMERA> template<class CAMERA>
class TriangulationFactor: public NoiseModelFactor1<Point3> { class TriangulationFactor: public NoiseModelFactorN<Point3> {
public: public:
@ -40,7 +40,7 @@ public:
protected: protected:
/// shorthand for base class type /// shorthand for base class type
using Base = NoiseModelFactor1<Point3>; using Base = NoiseModelFactorN<Point3>;
/// shorthand for this class /// shorthand for this class
using This = TriangulationFactor<CAMERA>; using This = TriangulationFactor<CAMERA>;

12
gtsam/slam/dataset.cpp
View File

@ -535,7 +535,7 @@ GraphAndValues load2D(const string &filename, SharedNoiseModel model,
graph->push_back(*f); graph->push_back(*f);
// Insert vertices if pure odometry file // Insert vertices if pure odometry file
Key key1 = (*f)->key1(), key2 = (*f)->key2(); Key key1 = (*f)->key<1>(), key2 = (*f)->key<2>();
if (!initial->exists(key1)) if (!initial->exists(key1))
initial->insert(key1, Pose2()); initial->insert(key1, Pose2());
if (!initial->exists(key2)) if (!initial->exists(key2))
@ -603,7 +603,7 @@ void save2D(const NonlinearFactorGraph &graph, const Values &config,
continue; continue;
const Pose2 pose = factor->measured().inverse(); const Pose2 pose = factor->measured().inverse();
stream << "EDGE2 " << factor->key2() << " " << factor->key1() << " " stream << "EDGE2 " << factor->key<2>() << " " << factor->key<1>() << " "
<< pose.x() << " " << pose.y() << " " << pose.theta() << " " << pose.x() << " " << pose.y() << " " << pose.theta() << " "
<< RR(0, 0) << " " << RR(0, 1) << " " << RR(1, 1) << " " << RR(2, 2) << RR(0, 0) << " " << RR(0, 1) << " " << RR(1, 1) << " " << RR(2, 2)
<< " " << RR(0, 2) << " " << RR(1, 2) << endl; << " " << RR(0, 2) << " " << RR(1, 2) << endl;
@ -691,8 +691,8 @@ void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate,
} }
Matrix3 Info = gaussianModel->R().transpose() * gaussianModel->R(); Matrix3 Info = gaussianModel->R().transpose() * gaussianModel->R();
Pose2 pose = factor->measured(); //.inverse(); Pose2 pose = factor->measured(); //.inverse();
stream << "EDGE_SE2 " << index(factor->key1()) << " " stream << "EDGE_SE2 " << index(factor->key<1>()) << " "
<< index(factor->key2()) << " " << pose.x() << " " << pose.y() << index(factor->key<2>()) << " " << pose.x() << " " << pose.y()
<< " " << pose.theta(); << " " << pose.theta();
for (size_t i = 0; i < 3; i++) { for (size_t i = 0; i < 3; i++) {
for (size_t j = i; j < 3; j++) { for (size_t j = i; j < 3; j++) {
@ -717,8 +717,8 @@ void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate,
const Pose3 pose3D = factor3D->measured(); const Pose3 pose3D = factor3D->measured();
const Point3 p = pose3D.translation(); const Point3 p = pose3D.translation();
const auto q = pose3D.rotation().toQuaternion(); const auto q = pose3D.rotation().toQuaternion();
stream << "EDGE_SE3:QUAT " << index(factor3D->key1()) << " " stream << "EDGE_SE3:QUAT " << index(factor3D->key<1>()) << " "
<< index(factor3D->key2()) << " " << p.x() << " " << p.y() << " " << index(factor3D->key<2>()) << " " << p.x() << " " << p.y() << " "
<< p.z() << " " << q.x() << " " << q.y() << " " << q.z() << " " << p.z() << " " << q.x() << " " << q.y() << " " << q.z() << " "
<< q.w(); << q.w();

4
gtsam_unstable/dynamics/FullIMUFactor.h
View File

@ -22,9 +22,9 @@ namespace gtsam {
* assumed to be PoseRTV * assumed to be PoseRTV
*/ */
template<class POSE> template<class POSE>
class FullIMUFactor : public NoiseModelFactor2<POSE, POSE> { class FullIMUFactor : public NoiseModelFactorN<POSE, POSE> {
public: public:
typedef NoiseModelFactor2<POSE, POSE> Base; typedef NoiseModelFactorN<POSE, POSE> Base;
typedef FullIMUFactor<POSE> This; typedef FullIMUFactor<POSE> This;
protected: protected:

4
gtsam_unstable/dynamics/IMUFactor.h
View File

@ -20,9 +20,9 @@ namespace gtsam {
* assumed to be PoseRTV * assumed to be PoseRTV
*/ */
template<class POSE> template<class POSE>
class IMUFactor : public NoiseModelFactor2<POSE, POSE> { class IMUFactor : public NoiseModelFactorN<POSE, POSE> {
public: public:
typedef NoiseModelFactor2<POSE, POSE> Base; typedef NoiseModelFactorN<POSE, POSE> Base;
typedef IMUFactor<POSE> This; typedef IMUFactor<POSE> This;
protected: protected:

16
gtsam_unstable/dynamics/Pendulum.h
View File

@ -20,11 +20,11 @@ namespace gtsam {
* - For implicit Euler method: q_{k+1} = q_k + h*v_{k+1} * - For implicit Euler method: q_{k+1} = q_k + h*v_{k+1}
* - For sympletic Euler method: q_{k+1} = q_k + h*v_{k+1} * - For sympletic Euler method: q_{k+1} = q_k + h*v_{k+1}
*/ */
class PendulumFactor1: public NoiseModelFactor3<double, double, double> { class PendulumFactor1: public NoiseModelFactorN<double, double, double> {
public: public:
protected: protected:
typedef NoiseModelFactor3<double, double, double> Base; typedef NoiseModelFactorN<double, double, double> Base;
/** default constructor to allow for serialization */ /** default constructor to allow for serialization */
PendulumFactor1() {} PendulumFactor1() {}
@ -66,11 +66,11 @@ public:
* - For implicit Euler method: v_{k+1} = v_k - h*g/L*sin(q_{k+1}) * - For implicit Euler method: v_{k+1} = v_k - h*g/L*sin(q_{k+1})
* - For sympletic Euler method: v_{k+1} = v_k - h*g/L*sin(q_k) * - For sympletic Euler method: v_{k+1} = v_k - h*g/L*sin(q_k)
*/ */
class PendulumFactor2: public NoiseModelFactor3<double, double, double> { class PendulumFactor2: public NoiseModelFactorN<double, double, double> {
public: public:
protected: protected:
typedef NoiseModelFactor3<double, double, double> Base; typedef NoiseModelFactorN<double, double, double> Base;
/** default constructor to allow for serialization */ /** default constructor to allow for serialization */
PendulumFactor2() {} PendulumFactor2() {}
@ -113,11 +113,11 @@ public:
* \f$ p_k = -D_1 L_d(q_k,q_{k+1},h) = \frac{1}{h}mr^{2}\left(q_{k+1}-q_{k}\right)+mgrh(1-\alpha)\,\sin\left((1-\alpha)q_{k}+\alpha q_{k+1}\right) \f$ * \f$ p_k = -D_1 L_d(q_k,q_{k+1},h) = \frac{1}{h}mr^{2}\left(q_{k+1}-q_{k}\right)+mgrh(1-\alpha)\,\sin\left((1-\alpha)q_{k}+\alpha q_{k+1}\right) \f$
* \f$ = (1/h)mr^2 (q_{k+1}-q_k) + mgrh(1-alpha) sin ((1-alpha)q_k+\alpha q_{k+1}) \f$ * \f$ = (1/h)mr^2 (q_{k+1}-q_k) + mgrh(1-alpha) sin ((1-alpha)q_k+\alpha q_{k+1}) \f$
*/ */
class PendulumFactorPk: public NoiseModelFactor3<double, double, double> { class PendulumFactorPk: public NoiseModelFactorN<double, double, double> {
public: public:
protected: protected:
typedef NoiseModelFactor3<double, double, double> Base; typedef NoiseModelFactorN<double, double, double> Base;
/** default constructor to allow for serialization */ /** default constructor to allow for serialization */
PendulumFactorPk() {} PendulumFactorPk() {}
@ -169,11 +169,11 @@ public:
* \f$ p_k1 = D_2 L_d(q_k,q_{k+1},h) = \frac{1}{h}mr^{2}\left(q_{k+1}-q_{k}\right)-mgrh\alpha\sin\left((1-\alpha)q_{k}+\alpha q_{k+1}\right) \f$ * \f$ p_k1 = D_2 L_d(q_k,q_{k+1},h) = \frac{1}{h}mr^{2}\left(q_{k+1}-q_{k}\right)-mgrh\alpha\sin\left((1-\alpha)q_{k}+\alpha q_{k+1}\right) \f$
* \f$ = (1/h)mr^2 (q_{k+1}-q_k) - mgrh alpha sin ((1-alpha)q_k+\alpha q_{k+1}) \f$ * \f$ = (1/h)mr^2 (q_{k+1}-q_k) - mgrh alpha sin ((1-alpha)q_k+\alpha q_{k+1}) \f$
*/ */
class PendulumFactorPk1: public NoiseModelFactor3<double, double, double> { class PendulumFactorPk1: public NoiseModelFactorN<double, double, double> {
public: public:
protected: protected:
typedef NoiseModelFactor3<double, double, double> Base; typedef NoiseModelFactorN<double, double, double> Base;
/** default constructor to allow for serialization */ /** default constructor to allow for serialization */
PendulumFactorPk1() {} PendulumFactorPk1() {}

8
gtsam_unstable/dynamics/SimpleHelicopter.h
View File

@ -24,10 +24,10 @@ namespace gtsam {
* Note: this factor is necessary if one needs to smooth the entire graph. It's not needed * Note: this factor is necessary if one needs to smooth the entire graph. It's not needed
* in sequential update method. * in sequential update method.
*/ */
class Reconstruction : public NoiseModelFactor3<Pose3, Pose3, Vector6> { class Reconstruction : public NoiseModelFactorN<Pose3, Pose3, Vector6> {
double h_; // time step double h_; // time step
typedef NoiseModelFactor3<Pose3, Pose3, Vector6> Base; typedef NoiseModelFactorN<Pose3, Pose3, Vector6> Base;
public: public:
Reconstruction(Key gKey1, Key gKey, Key xiKey, double h, double mu = 1000.0) : Reconstruction(Key gKey1, Key gKey, Key xiKey, double h, double mu = 1000.0) :
Base(noiseModel::Constrained::All(6, std::abs(mu)), gKey1, gKey, Base(noiseModel::Constrained::All(6, std::abs(mu)), gKey1, gKey,
@ -73,7 +73,7 @@ public:
/** /**
* Implement the Discrete Euler-Poincare' equation: * Implement the Discrete Euler-Poincare' equation:
*/ */
class DiscreteEulerPoincareHelicopter : public NoiseModelFactor3<Vector6, Vector6, Pose3> { class DiscreteEulerPoincareHelicopter : public NoiseModelFactorN<Vector6, Vector6, Pose3> {
double h_; /// time step double h_; /// time step
Matrix Inertia_; /// Inertia tensors Inertia = [ J 0; 0 M ] Matrix Inertia_; /// Inertia tensors Inertia = [ J 0; 0 M ]
@ -86,7 +86,7 @@ class DiscreteEulerPoincareHelicopter : public NoiseModelFactor3<Vector6, Vector
// This might be needed in control or system identification problems. // This might be needed in control or system identification problems.
// We treat them as constant here, since the control inputs are to specify. // We treat them as constant here, since the control inputs are to specify.
typedef NoiseModelFactor3<Vector6, Vector6, Pose3> Base; typedef NoiseModelFactorN<Vector6, Vector6, Pose3> Base;
public: public:
DiscreteEulerPoincareHelicopter(Key xiKey1, Key xiKey_1, Key gKey, DiscreteEulerPoincareHelicopter(Key xiKey1, Key xiKey_1, Key gKey,

4
gtsam_unstable/dynamics/VelocityConstraint.h
View File

@ -32,9 +32,9 @@ typedef enum {
* NOTE: this approximation is insufficient for large timesteps, but is accurate * NOTE: this approximation is insufficient for large timesteps, but is accurate
* if timesteps are small. * if timesteps are small.
*/ */
class VelocityConstraint : public gtsam::NoiseModelFactor2<PoseRTV,PoseRTV> { class VelocityConstraint : public gtsam::NoiseModelFactorN<PoseRTV,PoseRTV> {
public: public:
typedef gtsam::NoiseModelFactor2<PoseRTV,PoseRTV> Base; typedef gtsam::NoiseModelFactorN<PoseRTV,PoseRTV> Base;
protected: protected:

5
gtsam_unstable/dynamics/VelocityConstraint3.h
View File

@ -10,11 +10,11 @@
namespace gtsam { namespace gtsam {
class VelocityConstraint3 : public NoiseModelFactor3<double, double, double> { class VelocityConstraint3 : public NoiseModelFactorN<double, double, double> {
public: public:
protected: protected:
typedef NoiseModelFactor3<double, double, double> Base; typedef NoiseModelFactorN<double, double, double> Base;
/** default constructor to allow for serialization */ /** default constructor to allow for serialization */
VelocityConstraint3() {} VelocityConstraint3() {}
@ -53,6 +53,7 @@ private:
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*/) {
// NoiseModelFactor3 instead of NoiseModelFactorN for backward compatibility
ar & boost::serialization::make_nvp("NoiseModelFactor3", ar & boost::serialization::make_nvp("NoiseModelFactor3",
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));
} }

9
gtsam_unstable/slam/BiasedGPSFactor.h
View File

@ -27,12 +27,12 @@ namespace gtsam {
* common-mode errors and that can be partially corrected if other sensors are used * common-mode errors and that can be partially corrected if other sensors are used
* @ingroup slam * @ingroup slam
*/ */
class BiasedGPSFactor: public NoiseModelFactor2<Pose3, Point3> { class BiasedGPSFactor: public NoiseModelFactorN<Pose3, Point3> {
private: private:
typedef BiasedGPSFactor This; typedef BiasedGPSFactor This;
typedef NoiseModelFactor2<Pose3, Point3> Base; typedef NoiseModelFactorN<Pose3, Point3> Base;
Point3 measured_; /** The measurement */ Point3 measured_; /** The measurement */
@ -57,8 +57,8 @@ namespace gtsam {
/** print */ /** print */
void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
std::cout << s << "BiasedGPSFactor(" std::cout << s << "BiasedGPSFactor("
<< keyFormatter(this->key1()) << "," << keyFormatter(this->key<1>()) << ","
<< keyFormatter(this->key2()) << ")\n" << keyFormatter(this->key<2>()) << ")\n"
<< " measured: " << measured_.transpose() << std::endl; << " measured: " << measured_.transpose() << std::endl;
this->noiseModel_->print(" noise model: "); this->noiseModel_->print(" noise model: ");
} }
@ -96,6 +96,7 @@ 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*/) {
// NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
ar & boost::serialization::make_nvp("NoiseModelFactor2", ar & boost::serialization::make_nvp("NoiseModelFactor2",
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(measured_);

14
gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h
View File

@ -88,12 +88,12 @@ namespace gtsam {
*/ */
template<class POSE, class VELOCITY, class IMUBIAS> template<class POSE, class VELOCITY, class IMUBIAS>
class EquivInertialNavFactor_GlobalVel : public NoiseModelFactor5<POSE, VELOCITY, IMUBIAS, POSE, VELOCITY> { class EquivInertialNavFactor_GlobalVel : public NoiseModelFactorN<POSE, VELOCITY, IMUBIAS, POSE, VELOCITY> {
private: private:
typedef EquivInertialNavFactor_GlobalVel<POSE, VELOCITY, IMUBIAS> This; typedef EquivInertialNavFactor_GlobalVel<POSE, VELOCITY, IMUBIAS> This;
typedef NoiseModelFactor5<POSE, VELOCITY, IMUBIAS, POSE, VELOCITY> Base; typedef NoiseModelFactorN<POSE, VELOCITY, IMUBIAS, POSE, VELOCITY> Base;
Vector delta_pos_in_t0_; Vector delta_pos_in_t0_;
Vector delta_vel_in_t0_; Vector delta_vel_in_t0_;
@ -136,11 +136,11 @@ public:
/** print */ /** print */
void print(const std::string& s = "EquivInertialNavFactor_GlobalVel", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { void print(const std::string& s = "EquivInertialNavFactor_GlobalVel", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
std::cout << s << "(" std::cout << s << "("
<< keyFormatter(this->key1()) << "," << keyFormatter(this->template key<1>()) << ","
<< keyFormatter(this->key2()) << "," << keyFormatter(this->template key<2>()) << ","
<< keyFormatter(this->key3()) << "," << keyFormatter(this->template key<3>()) << ","
<< keyFormatter(this->key4()) << "," << keyFormatter(this->template key<4>()) << ","
<< keyFormatter(this->key5()) << "\n"; << keyFormatter(this->template key<5>()) << "\n";
std::cout << "delta_pos_in_t0: " << this->delta_pos_in_t0_.transpose() << std::endl; 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_vel_in_t0: " << this->delta_vel_in_t0_.transpose() << std::endl;
std::cout << "delta_angles: " << this->delta_angles_ << std::endl; std::cout << "delta_angles: " << this->delta_angles_ << std::endl;

12
gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h
View File

@ -87,12 +87,12 @@ namespace gtsam {
*/ */
template<class POSE, class VELOCITY> template<class POSE, class VELOCITY>
class EquivInertialNavFactor_GlobalVel_NoBias : public NoiseModelFactor4<POSE, VELOCITY, POSE, VELOCITY> { class EquivInertialNavFactor_GlobalVel_NoBias : public NoiseModelFactorN<POSE, VELOCITY, POSE, VELOCITY> {
private: private:
typedef EquivInertialNavFactor_GlobalVel_NoBias<POSE, VELOCITY> This; typedef EquivInertialNavFactor_GlobalVel_NoBias<POSE, VELOCITY> This;
typedef NoiseModelFactor4<POSE, VELOCITY, POSE, VELOCITY> Base; typedef NoiseModelFactorN<POSE, VELOCITY, POSE, VELOCITY> Base;
Vector delta_pos_in_t0_; Vector delta_pos_in_t0_;
Vector delta_vel_in_t0_; Vector delta_vel_in_t0_;
@ -136,10 +136,10 @@ public:
const std::string& s = "EquivInertialNavFactor_GlobalVel_NoBias", const std::string& s = "EquivInertialNavFactor_GlobalVel_NoBias",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "(" std::cout << s << "("
<< keyFormatter(this->key1()) << "," << keyFormatter(this->key<1>()) << ","
<< keyFormatter(this->key2()) << "," << keyFormatter(this->key<2>()) << ","
<< keyFormatter(this->key3()) << "," << keyFormatter(this->key<3>()) << ","
<< keyFormatter(this->key4()) << "\n"; << keyFormatter(this->key<4>()) << "\n";
std::cout << "delta_pos_in_t0: " << this->delta_pos_in_t0_.transpose() << std::endl; 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_vel_in_t0: " << this->delta_vel_in_t0_.transpose() << std::endl;
std::cout << "delta_angles: " << this->delta_angles_ << std::endl; std::cout << "delta_angles: " << this->delta_angles_ << std::endl;

8
gtsam_unstable/slam/GaussMarkov1stOrderFactor.h
View File

@ -42,12 +42,12 @@ namespace gtsam {
* T is the measurement type, by default the same * T is the measurement type, by default the same
*/ */
template<class VALUE> template<class VALUE>
class GaussMarkov1stOrderFactor: public NoiseModelFactor2<VALUE, VALUE> { class GaussMarkov1stOrderFactor: public NoiseModelFactorN<VALUE, VALUE> {
private: private:
typedef GaussMarkov1stOrderFactor<VALUE> This; typedef GaussMarkov1stOrderFactor<VALUE> This;
typedef NoiseModelFactor2<VALUE, VALUE> Base; typedef NoiseModelFactorN<VALUE, VALUE> Base;
double dt_; double dt_;
Vector tau_; Vector tau_;
@ -73,8 +73,8 @@ public:
/** print */ /** print */
void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
std::cout << s << "GaussMarkov1stOrderFactor(" std::cout << s << "GaussMarkov1stOrderFactor("
<< keyFormatter(this->key1()) << "," << keyFormatter(this->template key<1>()) << ","
<< keyFormatter(this->key2()) << ")\n"; << keyFormatter(this->template key<2>()) << ")\n";
this->noiseModel_->print(" noise model"); this->noiseModel_->print(" noise model");
} }

14
gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h
View File

@ -77,12 +77,12 @@ namespace gtsam {
* vehicle * vehicle
*/ */
template<class POSE, class VELOCITY, class IMUBIAS> template<class POSE, class VELOCITY, class IMUBIAS>
class InertialNavFactor_GlobalVelocity : public NoiseModelFactor5<POSE, VELOCITY, IMUBIAS, POSE, VELOCITY> { class InertialNavFactor_GlobalVelocity : public NoiseModelFactorN<POSE, VELOCITY, IMUBIAS, POSE, VELOCITY> {
private: private:
typedef InertialNavFactor_GlobalVelocity<POSE, VELOCITY, IMUBIAS> This; typedef InertialNavFactor_GlobalVelocity<POSE, VELOCITY, IMUBIAS> This;
typedef NoiseModelFactor5<POSE, VELOCITY, IMUBIAS, POSE, VELOCITY> Base; typedef NoiseModelFactorN<POSE, VELOCITY, IMUBIAS, POSE, VELOCITY> Base;
Vector measurement_acc_; Vector measurement_acc_;
Vector measurement_gyro_; Vector measurement_gyro_;
@ -117,11 +117,11 @@ public:
/** print */ /** print */
void print(const std::string& s = "InertialNavFactor_GlobalVelocity", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { void print(const std::string& s = "InertialNavFactor_GlobalVelocity", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
std::cout << s << "(" std::cout << s << "("
<< keyFormatter(this->key1()) << "," << keyFormatter(this->template key<1>()) << ","
<< keyFormatter(this->key2()) << "," << keyFormatter(this->template key<2>()) << ","
<< keyFormatter(this->key3()) << "," << keyFormatter(this->template key<3>()) << ","
<< keyFormatter(this->key4()) << "," << keyFormatter(this->template key<4>()) << ","
<< keyFormatter(this->key5()) << "\n"; << keyFormatter(this->template key<5>()) << "\n";
std::cout << "acc measurement: " << this->measurement_acc_.transpose() << std::endl; std::cout << "acc measurement: " << this->measurement_acc_.transpose() << std::endl;
std::cout << "gyro measurement: " << this->measurement_gyro_.transpose() << std::endl; std::cout << "gyro measurement: " << this->measurement_gyro_.transpose() << std::endl;
std::cout << "dt: " << this->dt_ << std::endl; std::cout << "dt: " << this->dt_ << std::endl;

8
gtsam_unstable/slam/InvDepthFactor3.h
View File

@ -24,7 +24,7 @@ namespace gtsam {
* Ternary factor representing a visual measurement that includes inverse depth * Ternary factor representing a visual measurement that includes inverse depth
*/ */
template<class POSE, class LANDMARK, class INVDEPTH> template<class POSE, class LANDMARK, class INVDEPTH>
class InvDepthFactor3: public NoiseModelFactor3<POSE, LANDMARK, INVDEPTH> { class InvDepthFactor3: public NoiseModelFactorN<POSE, LANDMARK, INVDEPTH> {
protected: protected:
// Keep a copy of measurement and calibration for I/O // Keep a copy of measurement and calibration for I/O
@ -34,7 +34,7 @@ protected:
public: public:
/// shorthand for base class type /// shorthand for base class type
typedef NoiseModelFactor3<POSE, LANDMARK, INVDEPTH> Base; typedef NoiseModelFactorN<POSE, LANDMARK, INVDEPTH> Base;
/// shorthand for this class /// shorthand for this class
typedef InvDepthFactor3<POSE, LANDMARK, INVDEPTH> This; typedef InvDepthFactor3<POSE, LANDMARK, INVDEPTH> This;
@ -93,8 +93,8 @@ public:
if (H1) *H1 = Matrix::Zero(2,6); if (H1) *H1 = Matrix::Zero(2,6);
if (H2) *H2 = Matrix::Zero(2,5); if (H2) *H2 = Matrix::Zero(2,5);
if (H3) *H3 = Matrix::Zero(2,1); if (H3) *H3 = Matrix::Zero(2,1);
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->template key<2>()) <<
" moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; " moved behind camera " << DefaultKeyFormatter(this->template key<1>()) << std::endl;
return Vector::Ones(2) * 2.0 * K_->fx(); return Vector::Ones(2) * 2.0 * K_->fx();
} }
return (Vector(1) << 0.0).finished(); return (Vector(1) << 0.0).finished();

8
gtsam_unstable/slam/InvDepthFactorVariant1.h
View File

@ -24,7 +24,7 @@ namespace gtsam {
/** /**
* Binary factor representing a visual measurement using an inverse-depth parameterization * Binary factor representing a visual measurement using an inverse-depth parameterization
*/ */
class InvDepthFactorVariant1: public NoiseModelFactor2<Pose3, Vector6> { class InvDepthFactorVariant1: public NoiseModelFactorN<Pose3, Vector6> {
protected: protected:
// Keep a copy of measurement and calibration for I/O // Keep a copy of measurement and calibration for I/O
@ -34,7 +34,7 @@ protected:
public: public:
/// shorthand for base class type /// shorthand for base class type
typedef NoiseModelFactor2<Pose3, Vector6> Base; typedef NoiseModelFactorN<Pose3, Vector6> Base;
/// shorthand for this class /// shorthand for this class
typedef InvDepthFactorVariant1 This; typedef InvDepthFactorVariant1 This;
@ -93,8 +93,8 @@ public:
return camera.project(world_P_landmark) - measured_; return camera.project(world_P_landmark) - measured_;
} catch( CheiralityException& e) { } catch( CheiralityException& e) {
std::cout << e.what() std::cout << e.what()
<< ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key2()) << "]" << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key<2>()) << "]"
<< " moved behind camera [" << DefaultKeyFormatter(this->key1()) <<"]" << " moved behind camera [" << DefaultKeyFormatter(this->key<1>()) <<"]"
<< std::endl; << std::endl;
return Vector::Ones(2) * 2.0 * K_->fx(); return Vector::Ones(2) * 2.0 * K_->fx();
} }

8
gtsam_unstable/slam/InvDepthFactorVariant2.h
View File

@ -25,7 +25,7 @@ namespace gtsam {
/** /**
* Binary factor representing a visual measurement using an inverse-depth parameterization * Binary factor representing a visual measurement using an inverse-depth parameterization
*/ */
class InvDepthFactorVariant2: public NoiseModelFactor2<Pose3, Vector3> { class InvDepthFactorVariant2: public NoiseModelFactorN<Pose3, Vector3> {
protected: protected:
// Keep a copy of measurement and calibration for I/O // Keep a copy of measurement and calibration for I/O
@ -36,7 +36,7 @@ protected:
public: public:
/// shorthand for base class type /// shorthand for base class type
typedef NoiseModelFactor2<Pose3, Vector3> Base; typedef NoiseModelFactorN<Pose3, Vector3> Base;
/// shorthand for this class /// shorthand for this class
typedef InvDepthFactorVariant2 This; typedef InvDepthFactorVariant2 This;
@ -96,8 +96,8 @@ public:
return camera.project(world_P_landmark) - measured_; return camera.project(world_P_landmark) - measured_;
} catch( CheiralityException& e) { } catch( CheiralityException& e) {
std::cout << e.what() std::cout << e.what()
<< ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key2()) << "]" << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key<2>()) << "]"
<< " moved behind camera [" << DefaultKeyFormatter(this->key1()) <<"]" << " moved behind camera [" << DefaultKeyFormatter(this->key<1>()) <<"]"
<< std::endl; << std::endl;
return Vector::Ones(2) * 2.0 * K_->fx(); return Vector::Ones(2) * 2.0 * K_->fx();
} }

16
gtsam_unstable/slam/InvDepthFactorVariant3.h
View File

@ -24,7 +24,7 @@ namespace gtsam {
/** /**
* Binary factor representing the first visual measurement using an inverse-depth parameterization * Binary factor representing the first visual measurement using an inverse-depth parameterization
*/ */
class InvDepthFactorVariant3a: public NoiseModelFactor2<Pose3, Vector3> { class InvDepthFactorVariant3a: public NoiseModelFactorN<Pose3, Vector3> {
protected: protected:
// Keep a copy of measurement and calibration for I/O // Keep a copy of measurement and calibration for I/O
@ -34,7 +34,7 @@ protected:
public: public:
/// shorthand for base class type /// shorthand for base class type
typedef NoiseModelFactor2<Pose3, Vector3> Base; typedef NoiseModelFactorN<Pose3, Vector3> Base;
/// shorthand for this class /// shorthand for this class
typedef InvDepthFactorVariant3a This; typedef InvDepthFactorVariant3a This;
@ -96,8 +96,8 @@ public:
return camera.project(world_P_landmark) - measured_; return camera.project(world_P_landmark) - measured_;
} catch( CheiralityException& e) { } catch( CheiralityException& e) {
std::cout << e.what() std::cout << e.what()
<< ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key1()) << "," << DefaultKeyFormatter(this->key2()) << "]" << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key<1>()) << "," << DefaultKeyFormatter(this->key<2>()) << "]"
<< " moved behind camera [" << DefaultKeyFormatter(this->key1()) << "]" << " moved behind camera [" << DefaultKeyFormatter(this->key<1>()) << "]"
<< std::endl; << std::endl;
return Vector::Ones(2) * 2.0 * K_->fx(); return Vector::Ones(2) * 2.0 * K_->fx();
} }
@ -150,7 +150,7 @@ private:
/** /**
* Ternary factor representing a visual measurement using an inverse-depth parameterization * Ternary factor representing a visual measurement using an inverse-depth parameterization
*/ */
class InvDepthFactorVariant3b: public NoiseModelFactor3<Pose3, Pose3, Vector3> { class InvDepthFactorVariant3b: public NoiseModelFactorN<Pose3, Pose3, Vector3> {
protected: protected:
// Keep a copy of measurement and calibration for I/O // Keep a copy of measurement and calibration for I/O
@ -160,7 +160,7 @@ protected:
public: public:
/// shorthand for base class type /// shorthand for base class type
typedef NoiseModelFactor3<Pose3, Pose3, Vector3> Base; typedef NoiseModelFactorN<Pose3, Pose3, Vector3> Base;
/// shorthand for this class /// shorthand for this class
typedef InvDepthFactorVariant3b This; typedef InvDepthFactorVariant3b This;
@ -222,8 +222,8 @@ public:
return camera.project(world_P_landmark) - measured_; return camera.project(world_P_landmark) - measured_;
} catch( CheiralityException& e) { } catch( CheiralityException& e) {
std::cout << e.what() std::cout << e.what()
<< ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key1()) << "," << DefaultKeyFormatter(this->key3()) << "]" << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key<1>()) << "," << DefaultKeyFormatter(this->key<3>()) << "]"
<< " moved behind camera " << DefaultKeyFormatter(this->key2()) << " moved behind camera " << DefaultKeyFormatter(this->key<2>())
<< std::endl; << std::endl;
return Vector::Ones(2) * 2.0 * K_->fx(); return Vector::Ones(2) * 2.0 * K_->fx();
} }

4
gtsam_unstable/slam/LocalOrientedPlane3Factor.h
View File

@ -35,10 +35,10 @@ namespace gtsam {
* optimized in x1 frame in the optimization. * optimized in x1 frame in the optimization.
*/ */
class GTSAM_UNSTABLE_EXPORT LocalOrientedPlane3Factor class GTSAM_UNSTABLE_EXPORT LocalOrientedPlane3Factor
: public NoiseModelFactor3<Pose3, Pose3, OrientedPlane3> { : public NoiseModelFactorN<Pose3, Pose3, OrientedPlane3> {
protected: protected:
OrientedPlane3 measured_p_; OrientedPlane3 measured_p_;
typedef NoiseModelFactor3<Pose3, Pose3, OrientedPlane3> Base; typedef NoiseModelFactorN<Pose3, Pose3, OrientedPlane3> Base;
public: public:
/// Constructor /// Constructor
LocalOrientedPlane3Factor() {} LocalOrientedPlane3Factor() {}

4
gtsam_unstable/slam/MultiProjectionFactor.h
View File

@ -187,8 +187,8 @@ namespace gtsam {
if (H1) *H1 = Matrix::Zero(2,6); if (H1) *H1 = Matrix::Zero(2,6);
if (H2) *H2 = Matrix::Zero(2,3); if (H2) *H2 = Matrix::Zero(2,3);
if (verboseCheirality_) if (verboseCheirality_)
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->keys_.at(1)) <<
" moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; " moved behind camera " << DefaultKeyFormatter(this->keys_.at(0)) << std::endl;
if (throwCheirality_) if (throwCheirality_)
throw e; throw e;
} }

5
gtsam_unstable/slam/PartialPriorFactor.h
View File

@ -35,7 +35,7 @@ namespace gtsam {
* @tparam VALUE is the type of variable the prior effects * @tparam VALUE is the type of variable the prior effects
*/ */
template<class VALUE> template<class VALUE>
class PartialPriorFactor: public NoiseModelFactor1<VALUE> { class PartialPriorFactor: public NoiseModelFactorN<VALUE> {
public: public:
typedef VALUE T; typedef VALUE T;
@ -44,7 +44,7 @@ namespace gtsam {
// Concept checks on the variable type - currently requires Lie // Concept checks on the variable type - currently requires Lie
GTSAM_CONCEPT_LIE_TYPE(VALUE) GTSAM_CONCEPT_LIE_TYPE(VALUE)
typedef NoiseModelFactor1<VALUE> Base; typedef NoiseModelFactorN<VALUE> Base;
typedef PartialPriorFactor<VALUE> This; typedef PartialPriorFactor<VALUE> This;
Vector prior_; ///< Measurement on tangent space parameters, in compressed form. Vector prior_; ///< Measurement on tangent space parameters, in compressed form.
@ -141,6 +141,7 @@ 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*/) {
// NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
ar & boost::serialization::make_nvp("NoiseModelFactor1", ar & boost::serialization::make_nvp("NoiseModelFactor1",
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(prior_); ar & BOOST_SERIALIZATION_NVP(prior_);

8
gtsam_unstable/slam/PoseBetweenFactor.h
View File

@ -29,12 +29,12 @@ namespace gtsam {
* @ingroup slam * @ingroup slam
*/ */
template<class POSE> template<class POSE>
class PoseBetweenFactor: public NoiseModelFactor2<POSE, POSE> { class PoseBetweenFactor: public NoiseModelFactorN<POSE, POSE> {
private: private:
typedef PoseBetweenFactor<POSE> This; typedef PoseBetweenFactor<POSE> This;
typedef NoiseModelFactor2<POSE, POSE> Base; typedef NoiseModelFactorN<POSE, POSE> Base;
POSE measured_; /** The measurement */ POSE measured_; /** The measurement */
boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame
@ -68,8 +68,8 @@ namespace gtsam {
/** print */ /** print */
void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
std::cout << s << "BetweenFactor(" std::cout << s << "BetweenFactor("
<< keyFormatter(this->key1()) << "," << keyFormatter(this->template key<1>()) << ","
<< keyFormatter(this->key2()) << ")\n"; << keyFormatter(this->template key<2>()) << ")\n";
measured_.print(" measured: "); measured_.print(" measured: ");
if(this->body_P_sensor_) if(this->body_P_sensor_)
this->body_P_sensor_->print(" sensor pose in body frame: "); this->body_P_sensor_->print(" sensor pose in body frame: ");

4
gtsam_unstable/slam/PosePriorFactor.h
View File

@ -26,12 +26,12 @@ namespace gtsam {
* @ingroup slam * @ingroup slam
*/ */
template<class POSE> template<class POSE>
class PosePriorFactor: public NoiseModelFactor1<POSE> { class PosePriorFactor: public NoiseModelFactorN<POSE> {
private: private:
typedef PosePriorFactor<POSE> This; typedef PosePriorFactor<POSE> This;
typedef NoiseModelFactor1<POSE> Base; typedef NoiseModelFactorN<POSE> Base;
POSE prior_; /** The measurement */ POSE prior_; /** The measurement */
boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame

13
gtsam_unstable/slam/PoseToPointFactor.h
View File

@ -21,10 +21,10 @@ namespace gtsam {
* @ingroup slam * @ingroup slam
*/ */
template<typename POSE = Pose3, typename POINT = Point3> template<typename POSE = Pose3, typename POINT = Point3>
class PoseToPointFactor : public NoiseModelFactor2<POSE, POINT> { class PoseToPointFactor : public NoiseModelFactorN<POSE, POINT> {
private: private:
typedef PoseToPointFactor This; typedef PoseToPointFactor This;
typedef NoiseModelFactor2<POSE, POINT> Base; typedef NoiseModelFactorN<POSE, POINT> Base;
POINT measured_; /** the point measurement in local coordinates */ POINT measured_; /** the point measurement in local coordinates */
@ -47,8 +47,9 @@ class PoseToPointFactor : public NoiseModelFactor2<POSE, POINT> {
/** print */ /** print */
void print(const std::string& s, const KeyFormatter& keyFormatter = void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override { DefaultKeyFormatter) const override {
std::cout << s << "PoseToPointFactor(" << keyFormatter(this->key1()) << "," std::cout << s << "PoseToPointFactor("
<< keyFormatter(this->key2()) << ")\n" << keyFormatter(this->template key<1>()) << ","
<< keyFormatter(this->template key<2>()) << ")\n"
<< " measured: " << measured_.transpose() << std::endl; << " measured: " << measured_.transpose() << std::endl;
this->noiseModel_->print(" noise model: "); this->noiseModel_->print(" noise model: ");
} }
@ -91,8 +92,10 @@ class PoseToPointFactor : public NoiseModelFactor2<POSE, POINT> {
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*/) {
// NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
ar& boost::serialization::make_nvp( ar& boost::serialization::make_nvp(
"NoiseModelFactor2", boost::serialization::base_object<Base>(*this)); "NoiseModelFactor2",
boost::serialization::base_object<Base>(*this));
ar& BOOST_SERIALIZATION_NVP(measured_); ar& BOOST_SERIALIZATION_NVP(measured_);
} }

11
gtsam_unstable/slam/ProjectionFactorPPP.h
View File

@ -31,7 +31,7 @@ namespace gtsam {
* @ingroup slam * @ingroup slam
*/ */
template<class POSE, class LANDMARK, class CALIBRATION = Cal3_S2> template<class POSE, class LANDMARK, class CALIBRATION = Cal3_S2>
class ProjectionFactorPPP: public NoiseModelFactor3<POSE, POSE, LANDMARK> { class ProjectionFactorPPP: public NoiseModelFactorN<POSE, POSE, LANDMARK> {
protected: protected:
// Keep a copy of measurement and calibration for I/O // Keep a copy of measurement and calibration for I/O
@ -45,7 +45,7 @@ namespace gtsam {
public: public:
/// shorthand for base class type /// shorthand for base class type
typedef NoiseModelFactor3<POSE, POSE, LANDMARK> Base; typedef NoiseModelFactorN<POSE, POSE, LANDMARK> Base;
/// shorthand for this class /// shorthand for this class
typedef ProjectionFactorPPP<POSE, LANDMARK, CALIBRATION> This; typedef ProjectionFactorPPP<POSE, LANDMARK, CALIBRATION> This;
@ -142,8 +142,11 @@ namespace gtsam {
if (H2) *H2 = Matrix::Zero(2,6); if (H2) *H2 = Matrix::Zero(2,6);
if (H3) *H3 = Matrix::Zero(2,3); if (H3) *H3 = Matrix::Zero(2,3);
if (verboseCheirality_) if (verboseCheirality_)
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << std::cout << e.what() << ": Landmark "
" moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; << DefaultKeyFormatter(this->template key<2>())
<< " moved behind camera "
<< DefaultKeyFormatter(this->template key<1>())
<< std::endl;
if (throwCheirality_) if (throwCheirality_)
throw e; throw e;
} }

8
gtsam_unstable/slam/ProjectionFactorPPPC.h
View File

@ -34,7 +34,7 @@ namespace gtsam {
*/ */
template <class POSE, class LANDMARK, class CALIBRATION = Cal3_S2> template <class POSE, class LANDMARK, class CALIBRATION = Cal3_S2>
class GTSAM_UNSTABLE_EXPORT ProjectionFactorPPPC class GTSAM_UNSTABLE_EXPORT ProjectionFactorPPPC
: public NoiseModelFactor4<POSE, POSE, LANDMARK, CALIBRATION> { : public NoiseModelFactorN<POSE, POSE, LANDMARK, CALIBRATION> {
protected: protected:
Point2 measured_; ///< 2D measurement Point2 measured_; ///< 2D measurement
@ -44,7 +44,7 @@ class GTSAM_UNSTABLE_EXPORT ProjectionFactorPPPC
public: public:
/// shorthand for base class type /// shorthand for base class type
typedef NoiseModelFactor4<POSE, POSE, LANDMARK, CALIBRATION> Base; typedef NoiseModelFactorN<POSE, POSE, LANDMARK, CALIBRATION> Base;
/// shorthand for this class /// shorthand for this class
typedef ProjectionFactorPPPC<POSE, LANDMARK, CALIBRATION> This; typedef ProjectionFactorPPPC<POSE, LANDMARK, CALIBRATION> This;
@ -130,8 +130,8 @@ class GTSAM_UNSTABLE_EXPORT ProjectionFactorPPPC
if (H3) *H3 = Matrix::Zero(2,3); if (H3) *H3 = Matrix::Zero(2,3);
if (H4) *H4 = Matrix::Zero(2,CALIBRATION::Dim()); if (H4) *H4 = Matrix::Zero(2,CALIBRATION::Dim());
if (verboseCheirality_) if (verboseCheirality_)
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->template key<2>()) <<
" moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; " moved behind camera " << DefaultKeyFormatter(this->template key<1>()) << std::endl;
if (throwCheirality_) if (throwCheirality_)
throw e; throw e;
} }

6
gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp
View File

@ -54,9 +54,9 @@ Vector ProjectionFactorRollingShutter::evaluateError(
if (H3) *H3 = Matrix::Zero(2, 3); if (H3) *H3 = Matrix::Zero(2, 3);
if (verboseCheirality_) if (verboseCheirality_)
std::cout << e.what() << ": Landmark " std::cout << e.what() << ": Landmark "
<< DefaultKeyFormatter(this->key2()) << " moved behind camera " << DefaultKeyFormatter(this->key<2>()) << " moved behind camera "
<< DefaultKeyFormatter(this->key1()) << std::endl; << DefaultKeyFormatter(this->key<1>()) << std::endl;
if (throwCheirality_) throw CheiralityException(this->key2()); if (throwCheirality_) throw CheiralityException(this->key<2>());
} }
return Vector2::Constant(2.0 * K_->fx()); return Vector2::Constant(2.0 * K_->fx());
} }

4
gtsam_unstable/slam/ProjectionFactorRollingShutter.h
View File

@ -42,7 +42,7 @@ namespace gtsam {
*/ */
class GTSAM_UNSTABLE_EXPORT ProjectionFactorRollingShutter class GTSAM_UNSTABLE_EXPORT ProjectionFactorRollingShutter
: public NoiseModelFactor3<Pose3, Pose3, Point3> { : public NoiseModelFactorN<Pose3, Pose3, Point3> {
protected: protected:
// Keep a copy of measurement and calibration for I/O // Keep a copy of measurement and calibration for I/O
Point2 measured_; ///< 2D measurement Point2 measured_; ///< 2D measurement
@ -60,7 +60,7 @@ class GTSAM_UNSTABLE_EXPORT ProjectionFactorRollingShutter
public: public:
/// shorthand for base class type /// shorthand for base class type
typedef NoiseModelFactor3<Pose3, Pose3, Point3> Base; typedef NoiseModelFactorN<Pose3, Pose3, Point3> Base;
/// shorthand for this class /// shorthand for this class
typedef ProjectionFactorRollingShutter This; typedef ProjectionFactorRollingShutter This;

5
gtsam_unstable/slam/RelativeElevationFactor.h
View File

@ -25,13 +25,13 @@ namespace gtsam {
* *
* TODO: enable use of a Pose3 for the target as well * TODO: enable use of a Pose3 for the target as well
*/ */
class GTSAM_UNSTABLE_EXPORT RelativeElevationFactor: public NoiseModelFactor2<Pose3, Point3> { class GTSAM_UNSTABLE_EXPORT RelativeElevationFactor: public NoiseModelFactorN<Pose3, Point3> {
private: private:
double measured_; /** measurement */ double measured_; /** measurement */
typedef RelativeElevationFactor This; typedef RelativeElevationFactor This;
typedef NoiseModelFactor2<Pose3, Point3> Base; typedef NoiseModelFactorN<Pose3, Point3> Base;
public: public:
@ -66,6 +66,7 @@ private:
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*/) {
// NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
ar & boost::serialization::make_nvp("NoiseModelFactor2", ar & boost::serialization::make_nvp("NoiseModelFactor2",
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(measured_);

12
gtsam_unstable/slam/TSAMFactors.h
View File

@ -26,11 +26,11 @@ namespace gtsam {
/** /**
* DeltaFactor: relative 2D measurement between Pose2 and Point2 * DeltaFactor: relative 2D measurement between Pose2 and Point2
*/ */
class DeltaFactor: public NoiseModelFactor2<Pose2, Point2> { class DeltaFactor: public NoiseModelFactorN<Pose2, Point2> {
public: public:
typedef DeltaFactor This; typedef DeltaFactor This;
typedef NoiseModelFactor2<Pose2, Point2> Base; typedef NoiseModelFactorN<Pose2, Point2> Base;
typedef boost::shared_ptr<This> shared_ptr; typedef boost::shared_ptr<This> shared_ptr;
private: private:
@ -55,11 +55,11 @@ public:
/** /**
* DeltaFactorBase: relative 2D measurement between Pose2 and Point2, with Basenodes * DeltaFactorBase: relative 2D measurement between Pose2 and Point2, with Basenodes
*/ */
class DeltaFactorBase: public NoiseModelFactor4<Pose2, Pose2, Pose2, Point2> { class DeltaFactorBase: public NoiseModelFactorN<Pose2, Pose2, Pose2, Point2> {
public: public:
typedef DeltaFactorBase This; typedef DeltaFactorBase This;
typedef NoiseModelFactor4<Pose2, Pose2, Pose2, Point2> Base; typedef NoiseModelFactorN<Pose2, Pose2, Pose2, Point2> Base;
typedef boost::shared_ptr<This> shared_ptr; typedef boost::shared_ptr<This> shared_ptr;
private: private:
@ -110,11 +110,11 @@ public:
/** /**
* OdometryFactorBase: Pose2 odometry, with Basenodes * OdometryFactorBase: Pose2 odometry, with Basenodes
*/ */
class OdometryFactorBase: public NoiseModelFactor4<Pose2, Pose2, Pose2, Pose2> { class OdometryFactorBase: public NoiseModelFactorN<Pose2, Pose2, Pose2, Pose2> {
public: public:
typedef OdometryFactorBase This; typedef OdometryFactorBase This;
typedef NoiseModelFactor4<Pose2, Pose2, Pose2, Pose2> Base; typedef NoiseModelFactorN<Pose2, Pose2, Pose2, Pose2> Base;
typedef boost::shared_ptr<This> shared_ptr; typedef boost::shared_ptr<This> shared_ptr;
private: private:

2
gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h
View File

@ -32,7 +32,7 @@ namespace gtsam {
* @ingroup slam * @ingroup slam
*/ */
template<class VALUE> template<class VALUE>
class TransformBtwRobotsUnaryFactor: public NonlinearFactor { // TODO why not NoiseModelFactor1 ? class TransformBtwRobotsUnaryFactor: public NonlinearFactor { // TODO why not NoiseModelFactorN ?
public: public:

12
tests/simulated2D.h
View File

@ -124,9 +124,9 @@ namespace simulated2D {
* Unary factor encoding a soft prior on a vector * Unary factor encoding a soft prior on a vector
*/ */
template<class VALUE = Point2> template<class VALUE = Point2>
class GenericPrior: public NoiseModelFactor1<VALUE> { class GenericPrior: public NoiseModelFactorN<VALUE> {
public: public:
typedef NoiseModelFactor1<VALUE> Base; ///< base class typedef NoiseModelFactorN<VALUE> Base; ///< base class
typedef GenericPrior<VALUE> This; typedef GenericPrior<VALUE> This;
typedef boost::shared_ptr<GenericPrior<VALUE> > shared_ptr; typedef boost::shared_ptr<GenericPrior<VALUE> > shared_ptr;
typedef VALUE Pose; ///< shortcut to Pose type typedef VALUE Pose; ///< shortcut to Pose type
@ -168,9 +168,9 @@ namespace simulated2D {
* Binary factor simulating "odometry" between two Vectors * Binary factor simulating "odometry" between two Vectors
*/ */
template<class VALUE = Point2> template<class VALUE = Point2>
class GenericOdometry: public NoiseModelFactor2<VALUE, VALUE> { class GenericOdometry: public NoiseModelFactorN<VALUE, VALUE> {
public: public:
typedef NoiseModelFactor2<VALUE, VALUE> Base; ///< base class typedef NoiseModelFactorN<VALUE, VALUE> Base; ///< base class
typedef GenericOdometry<VALUE> This; typedef GenericOdometry<VALUE> This;
typedef boost::shared_ptr<GenericOdometry<VALUE> > shared_ptr; typedef boost::shared_ptr<GenericOdometry<VALUE> > shared_ptr;
typedef VALUE Pose; ///< shortcut to Pose type typedef VALUE Pose; ///< shortcut to Pose type
@ -214,9 +214,9 @@ namespace simulated2D {
* Binary factor simulating "measurement" between two Vectors * Binary factor simulating "measurement" between two Vectors
*/ */
template<class POSE, class LANDMARK> template<class POSE, class LANDMARK>
class GenericMeasurement: public NoiseModelFactor2<POSE, LANDMARK> { class GenericMeasurement: public NoiseModelFactorN<POSE, LANDMARK> {
public: public:
typedef NoiseModelFactor2<POSE, LANDMARK> Base; ///< base class typedef NoiseModelFactorN<POSE, LANDMARK> Base; ///< base class
typedef GenericMeasurement<POSE, LANDMARK> This; typedef GenericMeasurement<POSE, LANDMARK> This;
typedef boost::shared_ptr<GenericMeasurement<POSE, LANDMARK> > shared_ptr; typedef boost::shared_ptr<GenericMeasurement<POSE, LANDMARK> > shared_ptr;
typedef POSE Pose; ///< shortcut to Pose type typedef POSE Pose; ///< shortcut to Pose type

8
tests/simulated2DOriented.h
View File

@ -80,13 +80,13 @@ namespace simulated2DOriented {
/// Unary factor encoding a soft prior on a vector /// Unary factor encoding a soft prior on a vector
template<class VALUE = Pose2> template<class VALUE = Pose2>
struct GenericPosePrior: public NoiseModelFactor1<VALUE> { struct GenericPosePrior: public NoiseModelFactorN<VALUE> {
Pose2 measured_; ///< measurement Pose2 measured_; ///< measurement
/// Create generic pose prior /// Create generic pose prior
GenericPosePrior(const Pose2& measured, const SharedNoiseModel& model, Key key) : GenericPosePrior(const Pose2& measured, const SharedNoiseModel& model, Key key) :
NoiseModelFactor1<VALUE>(model, key), measured_(measured) { NoiseModelFactorN<VALUE>(model, key), measured_(measured) {
} }
/// Evaluate error and optionally derivative /// Evaluate error and optionally derivative
@ -101,7 +101,7 @@ namespace simulated2DOriented {
* Binary factor simulating "odometry" between two Vectors * Binary factor simulating "odometry" between two Vectors
*/ */
template<class VALUE = Pose2> template<class VALUE = Pose2>
struct GenericOdometry: public NoiseModelFactor2<VALUE, VALUE> { struct GenericOdometry: public NoiseModelFactorN<VALUE, VALUE> {
Pose2 measured_; ///< Between measurement for odometry factor Pose2 measured_; ///< Between measurement for odometry factor
typedef GenericOdometry<VALUE> This; typedef GenericOdometry<VALUE> This;
@ -111,7 +111,7 @@ namespace simulated2DOriented {
*/ */
GenericOdometry(const Pose2& measured, const SharedNoiseModel& model, GenericOdometry(const Pose2& measured, const SharedNoiseModel& model,
Key i1, Key i2) : Key i1, Key i2) :
NoiseModelFactor2<VALUE, VALUE>(model, i1, i2), measured_(measured) { NoiseModelFactorN<VALUE, VALUE>(model, i1, i2), measured_(measured) {
} }
~GenericOdometry() override {} ~GenericOdometry() override {}

8
tests/simulated3D.h
View File

@ -68,7 +68,7 @@ Point3 mea(const Point3& x, const Point3& l,
/** /**
* A prior factor on a single linear robot pose * A prior factor on a single linear robot pose
*/ */
struct PointPrior3D: public NoiseModelFactor1<Point3> { struct PointPrior3D: public NoiseModelFactorN<Point3> {
Point3 measured_; ///< The prior pose value for the variable attached to this factor Point3 measured_; ///< The prior pose value for the variable attached to this factor
@ -79,7 +79,7 @@ struct PointPrior3D: public NoiseModelFactor1<Point3> {
* @param key is the key for the pose * @param key is the key for the pose
*/ */
PointPrior3D(const Point3& measured, const SharedNoiseModel& model, Key key) : PointPrior3D(const Point3& measured, const SharedNoiseModel& model, Key key) :
NoiseModelFactor1<Point3> (model, key), measured_(measured) { NoiseModelFactorN<Point3> (model, key), measured_(measured) {
} }
/** /**
@ -98,7 +98,7 @@ struct PointPrior3D: public NoiseModelFactor1<Point3> {
/** /**
* Models a linear 3D measurement between 3D points * Models a linear 3D measurement between 3D points
*/ */
struct Simulated3DMeasurement: public NoiseModelFactor2<Point3, Point3> { struct Simulated3DMeasurement: public NoiseModelFactorN<Point3, Point3> {
Point3 measured_; ///< Linear displacement between a pose and landmark Point3 measured_; ///< Linear displacement between a pose and landmark
@ -110,7 +110,7 @@ struct Simulated3DMeasurement: public NoiseModelFactor2<Point3, Point3> {
* @param pointKey is the point key for the landmark * @param pointKey is the point key for the landmark
*/ */
Simulated3DMeasurement(const Point3& measured, const SharedNoiseModel& model, Key i, Key j) : Simulated3DMeasurement(const Point3& measured, const SharedNoiseModel& model, Key i, Key j) :
NoiseModelFactor2<Point3, Point3>(model, i, j), measured_(measured) {} NoiseModelFactorN<Point3, Point3>(model, i, j), measured_(measured) {}
/** /**
* Error function with optional derivatives * Error function with optional derivatives

4
tests/smallExample.h
View File

@ -329,12 +329,12 @@ inline Matrix H(const Point2& v) {
0.0, cos(v.y())).finished(); 0.0, cos(v.y())).finished();
} }
struct UnaryFactor: public gtsam::NoiseModelFactor1<Point2> { struct UnaryFactor: public gtsam::NoiseModelFactorN<Point2> {
Point2 z_; Point2 z_;
UnaryFactor(const Point2& z, const SharedNoiseModel& model, Key key) : UnaryFactor(const Point2& z, const SharedNoiseModel& model, Key key) :
gtsam::NoiseModelFactor1<Point2>(model, key), z_(z) { gtsam::NoiseModelFactorN<Point2>(model, key), z_(z) {
} }
Vector evaluateError(const Point2& x, boost::optional<Matrix&> A = boost::none) const override { Vector evaluateError(const Point2& x, boost::optional<Matrix&> A = boost::none) const override {

27
tests/testExtendedKalmanFilter.cpp
View File

@ -91,9 +91,9 @@ TEST( ExtendedKalmanFilter, linear ) {
// Create Motion Model Factor // Create Motion Model Factor
class NonlinearMotionModel : public NoiseModelFactor2<Point2,Point2> { class NonlinearMotionModel : public NoiseModelFactorN<Point2,Point2> {
typedef NoiseModelFactor2<Point2, Point2> Base; typedef NoiseModelFactorN<Point2, Point2> Base;
typedef NonlinearMotionModel This; typedef NonlinearMotionModel This;
protected: protected:
@ -155,14 +155,14 @@ public:
/* print */ /* print */
void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
std::cout << s << ": NonlinearMotionModel\n"; std::cout << s << ": NonlinearMotionModel\n";
std::cout << " TestKey1: " << keyFormatter(key1()) << std::endl; std::cout << " TestKey1: " << keyFormatter(key<1>()) << std::endl;
std::cout << " TestKey2: " << keyFormatter(key2()) << std::endl; std::cout << " TestKey2: " << keyFormatter(key<2>()) << std::endl;
} }
/** Check if two factors are equal. Note type is IndexFactor and needs cast. */ /** Check if two factors are equal. Note type is IndexFactor and needs cast. */
bool equals(const NonlinearFactor& f, double tol = 1e-9) const override { bool equals(const NonlinearFactor& f, double tol = 1e-9) const override {
const This *e = dynamic_cast<const This*> (&f); const This *e = dynamic_cast<const This*> (&f);
return (e != nullptr) && (key1() == e->key1()) && (key2() == e->key2()); return (e != nullptr) && (key<1>() == e->key<1>()) && (key<2>() == e->key<2>());
} }
/** /**
@ -181,7 +181,7 @@ public:
/** Vector of errors, whitened ! */ /** Vector of errors, whitened ! */
Vector whitenedError(const Values& c) const { Vector whitenedError(const Values& c) const {
return QInvSqrt(c.at<Point2>(key1()))*unwhitenedError(c); return QInvSqrt(c.at<Point2>(key<1>()))*unwhitenedError(c);
} }
/** /**
@ -190,14 +190,16 @@ public:
* Hence b = z - h(x1,x2) = - error_vector(x) * Hence b = z - h(x1,x2) = - error_vector(x)
*/ */
boost::shared_ptr<GaussianFactor> linearize(const Values& c) const override { boost::shared_ptr<GaussianFactor> linearize(const Values& c) const override {
const X1& x1 = c.at<X1>(key1()); using X1 = ValueType<1>;
const X2& x2 = c.at<X2>(key2()); using X2 = ValueType<2>;
const X1& x1 = c.at<X1>(key<1>());
const X2& x2 = c.at<X2>(key<2>());
Matrix A1, A2; Matrix A1, A2;
Vector b = -evaluateError(x1, x2, A1, A2); Vector b = -evaluateError(x1, x2, A1, A2);
SharedDiagonal constrained = SharedDiagonal constrained =
boost::dynamic_pointer_cast<noiseModel::Constrained>(this->noiseModel_); boost::dynamic_pointer_cast<noiseModel::Constrained>(this->noiseModel_);
if (constrained.get() != nullptr) { if (constrained.get() != nullptr) {
return JacobianFactor::shared_ptr(new JacobianFactor(key1(), A1, key2(), return JacobianFactor::shared_ptr(new JacobianFactor(key<1>(), A1, key<2>(),
A2, b, constrained)); A2, b, constrained));
} }
// "Whiten" the system before converting to a Gaussian Factor // "Whiten" the system before converting to a Gaussian Factor
@ -205,7 +207,7 @@ public:
A1 = Qinvsqrt*A1; A1 = Qinvsqrt*A1;
A2 = Qinvsqrt*A2; A2 = Qinvsqrt*A2;
b = Qinvsqrt*b; b = Qinvsqrt*b;
return GaussianFactor::shared_ptr(new JacobianFactor(key1(), A1, key2(), return GaussianFactor::shared_ptr(new JacobianFactor(key<1>(), A1, key<2>(),
A2, b, noiseModel::Unit::Create(b.size()))); A2, b, noiseModel::Unit::Create(b.size())));
} }
@ -232,9 +234,9 @@ public:
}; };
// Create Measurement Model Factor // Create Measurement Model Factor
class NonlinearMeasurementModel : public NoiseModelFactor1<Point2> { class NonlinearMeasurementModel : public NoiseModelFactorN<Point2> {
typedef NoiseModelFactor1<Point2> Base; typedef NoiseModelFactorN<Point2> Base;
typedef NonlinearMeasurementModel This; typedef NonlinearMeasurementModel This;
protected: protected:
@ -320,6 +322,7 @@ public:
* Hence b = z - h(x1) = - error_vector(x) * Hence b = z - h(x1) = - error_vector(x)
*/ */
boost::shared_ptr<GaussianFactor> linearize(const Values& c) const override { boost::shared_ptr<GaussianFactor> linearize(const Values& c) const override {
using X = ValueType<1>;
const X& x1 = c.at<X>(key()); const X& x1 = c.at<X>(key());
Matrix A1; Matrix A1;
Vector b = -evaluateError(x1, A1); Vector b = -evaluateError(x1, A1);

12
timing/timeIncremental.cpp
View File

@ -35,8 +35,8 @@ using namespace gtsam::symbol_shorthand;
typedef Pose2 Pose; typedef Pose2 Pose;
typedef NoiseModelFactor1<Pose> NM1; typedef NoiseModelFactorN<Pose> NM1;
typedef NoiseModelFactor2<Pose,Pose> NM2; typedef NoiseModelFactorN<Pose,Pose> NM2;
typedef BearingRangeFactor<Pose,Point2> BR; typedef BearingRangeFactor<Pose,Point2> BR;
//GTSAM_VALUE_EXPORT(Value); //GTSAM_VALUE_EXPORT(Value);
@ -107,18 +107,18 @@ int main(int argc, char *argv[]) {
boost::dynamic_pointer_cast<BetweenFactor<Pose> >(measurementf)) boost::dynamic_pointer_cast<BetweenFactor<Pose> >(measurementf))
{ {
// Stop collecting measurements that are for future steps // Stop collecting measurements that are for future steps
if(measurement->key1() > step || measurement->key2() > step) if(measurement->key<1>() > step || measurement->key<2>() > step)
break; break;
// Require that one of the nodes is the current one // Require that one of the nodes is the current one
if(measurement->key1() != step && measurement->key2() != step) if(measurement->key<1>() != step && measurement->key<2>() != step)
throw runtime_error("Problem in data file, out-of-sequence measurements"); throw runtime_error("Problem in data file, out-of-sequence measurements");
// Add a new factor // Add a new factor
newFactors.push_back(measurement); newFactors.push_back(measurement);
// Initialize the new variable // Initialize the new variable
if(measurement->key1() == step && measurement->key2() == step-1) { if(measurement->key<1>() == step && measurement->key<2>() == step-1) {
if(step == 1) if(step == 1)
newVariables.insert(step, measurement->measured().inverse()); newVariables.insert(step, measurement->measured().inverse());
else { else {
@ -126,7 +126,7 @@ int main(int argc, char *argv[]) {
newVariables.insert(step, prevPose * measurement->measured().inverse()); newVariables.insert(step, prevPose * measurement->measured().inverse());
} }
// cout << "Initializing " << step << endl; // cout << "Initializing " << step << endl;
} else if(measurement->key2() == step && measurement->key1() == step-1) { } else if(measurement->key<2>() == step && measurement->key<1>() == step-1) {
if(step == 1) if(step == 1)
newVariables.insert(step, measurement->measured()); newVariables.insert(step, measurement->measured());
else { else {

4
timing/timeiSAM2Chain.cpp
View File

@ -36,8 +36,8 @@ using namespace gtsam::symbol_shorthand;
typedef Pose2 Pose; typedef Pose2 Pose;
typedef NoiseModelFactor1<Pose> NM1; typedef NoiseModelFactorN<Pose> NM1;
typedef NoiseModelFactor2<Pose,Pose> NM2; typedef NoiseModelFactorN<Pose,Pose> NM2;
noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(3); noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(3);
int main(int argc, char *argv[]) { int main(int argc, char *argv[]) {