Merge pull request #1370 from borglab/feature/NoiseModelFactorN_undeprecate
commit
7bd4556b8a
|
@ -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->template key<1>();
|
KEY key1 = factor->key1();
|
||||||
KEY key2 = factor->template key<2>();
|
KEY key2 = factor->key2();
|
||||||
|
|
||||||
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->template key<1>();
|
KEY key1 = factor2->key1();
|
||||||
KEY key2 = factor2->template key<2>();
|
KEY key2 = factor2->key2();
|
||||||
// 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) ||
|
||||||
|
|
|
@ -101,7 +101,7 @@ class FunctorizedFactor : public NoiseModelFactorN<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->template key<1>()) << ")" << std::endl;
|
<< keyFormatter(this->key1()) << ")" << 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;
|
||||||
|
@ -208,8 +208,8 @@ class FunctorizedFactor2 : public NoiseModelFactorN<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->template key<1>()) << ", "
|
<< keyFormatter(this->key1()) << ", "
|
||||||
<< keyFormatter(this->template key<2>()) << ")" << std::endl;
|
<< keyFormatter(this->key2()) << ")" << 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;
|
||||||
|
|
|
@ -272,6 +272,64 @@ public:
|
||||||
|
|
||||||
}; // \class NoiseModelFactor
|
}; // \class NoiseModelFactor
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
namespace detail {
|
||||||
|
/** Convenience base class to add aliases `X1`, `X2`, ..., `X6` -> ValueType<N>.
|
||||||
|
* Usage example:
|
||||||
|
* ```
|
||||||
|
* class MyFactor : public NoiseModelFactorN<Pose3, Point3>,
|
||||||
|
* public NoiseModelFactorAliases<Pose3, Point3> {
|
||||||
|
* // class implementation ...
|
||||||
|
* };
|
||||||
|
*
|
||||||
|
* // MyFactor::X1 == Pose3
|
||||||
|
* // MyFactor::X2 == Point3
|
||||||
|
* ```
|
||||||
|
*/
|
||||||
|
template <typename, typename...>
|
||||||
|
struct NoiseModelFactorAliases {};
|
||||||
|
template <typename T1>
|
||||||
|
struct NoiseModelFactorAliases<T1> {
|
||||||
|
using X = T1;
|
||||||
|
using X1 = T1;
|
||||||
|
};
|
||||||
|
template <typename T1, typename T2>
|
||||||
|
struct NoiseModelFactorAliases<T1, T2> {
|
||||||
|
using X1 = T1;
|
||||||
|
using X2 = T2;
|
||||||
|
};
|
||||||
|
template <typename T1, typename T2, typename T3>
|
||||||
|
struct NoiseModelFactorAliases<T1, T2, T3> {
|
||||||
|
using X1 = T1;
|
||||||
|
using X2 = T2;
|
||||||
|
using X3 = T3;
|
||||||
|
};
|
||||||
|
template <typename T1, typename T2, typename T3, typename T4>
|
||||||
|
struct NoiseModelFactorAliases<T1, T2, T3, T4> {
|
||||||
|
using X1 = T1;
|
||||||
|
using X2 = T2;
|
||||||
|
using X3 = T3;
|
||||||
|
using X4 = T4;
|
||||||
|
};
|
||||||
|
template <typename T1, typename T2, typename T3, typename T4, typename T5>
|
||||||
|
struct NoiseModelFactorAliases<T1, T2, T3, T4, T5> {
|
||||||
|
using X1 = T1;
|
||||||
|
using X2 = T2;
|
||||||
|
using X3 = T3;
|
||||||
|
using X4 = T4;
|
||||||
|
using X5 = T5;
|
||||||
|
};
|
||||||
|
template <typename T1, typename T2, typename T3, typename T4, typename T5,
|
||||||
|
typename T6, typename... TExtra>
|
||||||
|
struct NoiseModelFactorAliases<T1, T2, T3, T4, T5, T6, TExtra...> {
|
||||||
|
using X1 = T1;
|
||||||
|
using X2 = T2;
|
||||||
|
using X3 = T3;
|
||||||
|
using X4 = T4;
|
||||||
|
using X5 = T5;
|
||||||
|
using X6 = T6;
|
||||||
|
};
|
||||||
|
} // namespace detail
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
/**
|
/**
|
||||||
|
@ -327,7 +385,9 @@ public:
|
||||||
* objects in non-linear manifolds (Lie groups).
|
* objects in non-linear manifolds (Lie groups).
|
||||||
*/
|
*/
|
||||||
template <class... ValueTypes>
|
template <class... ValueTypes>
|
||||||
class NoiseModelFactorN : public NoiseModelFactor {
|
class NoiseModelFactorN
|
||||||
|
: public NoiseModelFactor,
|
||||||
|
public detail::NoiseModelFactorAliases<ValueTypes...> {
|
||||||
public:
|
public:
|
||||||
/// N is the number of variables (N-way factor)
|
/// N is the number of variables (N-way factor)
|
||||||
enum { N = sizeof...(ValueTypes) };
|
enum { N = sizeof...(ValueTypes) };
|
||||||
|
@ -377,6 +437,14 @@ class NoiseModelFactorN : public NoiseModelFactor {
|
||||||
* // Factor::ValueType<0> // ERROR! Will not compile.
|
* // Factor::ValueType<0> // ERROR! Will not compile.
|
||||||
* // Factor::ValueType<3> // ERROR! Will not compile.
|
* // Factor::ValueType<3> // ERROR! Will not compile.
|
||||||
* ```
|
* ```
|
||||||
|
*
|
||||||
|
* You can also use the shortcuts `X1`, ..., `X6` which are the same as
|
||||||
|
* `ValueType<1>`, ..., `ValueType<6>` respectively (see
|
||||||
|
* detail::NoiseModelFactorAliases).
|
||||||
|
*
|
||||||
|
* Note that, if your class is templated AND you want to use `ValueType<1>`
|
||||||
|
* inside your class, due to dependent types you need the `template` keyword:
|
||||||
|
* `typename MyFactor<T>::template ValueType<1>`.
|
||||||
*/
|
*/
|
||||||
template <int I, typename = IndexIsValid<I>>
|
template <int I, typename = IndexIsValid<I>>
|
||||||
using ValueType =
|
using ValueType =
|
||||||
|
@ -431,6 +499,10 @@ class NoiseModelFactorN : public NoiseModelFactor {
|
||||||
* // key<0>() // ERROR! Will not compile
|
* // key<0>() // ERROR! Will not compile
|
||||||
* // key<3>() // ERROR! Will not compile
|
* // key<3>() // ERROR! Will not compile
|
||||||
* ```
|
* ```
|
||||||
|
*
|
||||||
|
* Note that, if your class is templated AND you are trying to call `key<1>`
|
||||||
|
* inside your class, due to dependent types you need the `template` keyword:
|
||||||
|
* `this->key1()`.
|
||||||
*/
|
*/
|
||||||
template <int I = 1>
|
template <int I = 1>
|
||||||
inline Key key() const {
|
inline Key key() const {
|
||||||
|
@ -555,37 +627,34 @@ class NoiseModelFactorN : public NoiseModelFactor {
|
||||||
}
|
}
|
||||||
|
|
||||||
public:
|
public:
|
||||||
/// @name Deprecated methods. Use `key<1>()`, `key<2>()`, ... instead of old
|
/// @name Shortcut functions `key1()` -> `key<1>()`
|
||||||
/// `key1()`, `key2()`, ...
|
|
||||||
/// If your class is templated AND you are trying to call `key<1>` inside your
|
|
||||||
/// class, due to dependent types you need to do `this->template key<1>()`.
|
|
||||||
/// @{
|
/// @{
|
||||||
|
|
||||||
inline Key GTSAM_DEPRECATED key1() const {
|
inline Key key1() const {
|
||||||
return key<1>();
|
return key<1>();
|
||||||
}
|
}
|
||||||
template <int I = 2>
|
template <int I = 2>
|
||||||
inline Key GTSAM_DEPRECATED key2() const {
|
inline Key key2() const {
|
||||||
static_assert(I <= N, "Index out of bounds");
|
static_assert(I <= N, "Index out of bounds");
|
||||||
return key<2>();
|
return key<2>();
|
||||||
}
|
}
|
||||||
template <int I = 3>
|
template <int I = 3>
|
||||||
inline Key GTSAM_DEPRECATED key3() const {
|
inline Key key3() const {
|
||||||
static_assert(I <= N, "Index out of bounds");
|
static_assert(I <= N, "Index out of bounds");
|
||||||
return key<3>();
|
return key<3>();
|
||||||
}
|
}
|
||||||
template <int I = 4>
|
template <int I = 4>
|
||||||
inline Key GTSAM_DEPRECATED key4() const {
|
inline Key key4() const {
|
||||||
static_assert(I <= N, "Index out of bounds");
|
static_assert(I <= N, "Index out of bounds");
|
||||||
return key<4>();
|
return key<4>();
|
||||||
}
|
}
|
||||||
template <int I = 5>
|
template <int I = 5>
|
||||||
inline Key GTSAM_DEPRECATED key5() const {
|
inline Key key5() const {
|
||||||
static_assert(I <= N, "Index out of bounds");
|
static_assert(I <= N, "Index out of bounds");
|
||||||
return key<5>();
|
return key<5>();
|
||||||
}
|
}
|
||||||
template <int I = 6>
|
template <int I = 6>
|
||||||
inline Key GTSAM_DEPRECATED key6() const {
|
inline Key key6() const {
|
||||||
static_assert(I <= N, "Index out of bounds");
|
static_assert(I <= N, "Index out of bounds");
|
||||||
return key<6>();
|
return key<6>();
|
||||||
}
|
}
|
||||||
|
@ -594,268 +663,11 @@ class NoiseModelFactorN : public NoiseModelFactor {
|
||||||
|
|
||||||
}; // \class NoiseModelFactorN
|
}; // \class NoiseModelFactorN
|
||||||
|
|
||||||
/* ************************************************************************* */
|
#define NoiseModelFactor1 NoiseModelFactorN
|
||||||
/** @deprecated: use NoiseModelFactorN, replacing .key() with .key<1>() and X1
|
#define NoiseModelFactor2 NoiseModelFactorN
|
||||||
* with ValueType<1>.
|
#define NoiseModelFactor3 NoiseModelFactorN
|
||||||
* If your class is templated AND you are trying to call `.key<1>()` or
|
#define NoiseModelFactor4 NoiseModelFactorN
|
||||||
* `ValueType<1>` inside your class, due to dependent types you need to do
|
#define NoiseModelFactor5 NoiseModelFactorN
|
||||||
* `this->template key<1>()` or `This::template ValueType<1>`.
|
#define NoiseModelFactor6 NoiseModelFactorN
|
||||||
* ~~~
|
|
||||||
* A convenient base class for creating your own NoiseModelFactor
|
|
||||||
* with 1 variable. To derive from this class, implement evaluateError().
|
|
||||||
*/
|
|
||||||
template <class VALUE>
|
|
||||||
class GTSAM_DEPRECATED NoiseModelFactor1 : public NoiseModelFactorN<VALUE> {
|
|
||||||
public:
|
|
||||||
/** Aliases for value types pulled from keys, for backwards compatibility.
|
|
||||||
* Note: in your code you can probably just do:
|
|
||||||
* `using X = ValueType<1>;`
|
|
||||||
* but this class is uglier due to dependent types.
|
|
||||||
* See e.g. testNonlinearFactor.cpp:TestFactorN.
|
|
||||||
*/
|
|
||||||
using X = typename NoiseModelFactor1::template ValueType<1>;
|
|
||||||
|
|
||||||
protected:
|
} // namespace gtsam
|
||||||
using Base = NoiseModelFactor; // grandparent, for backwards compatibility
|
|
||||||
using This = NoiseModelFactor1<VALUE>;
|
|
||||||
|
|
||||||
public:
|
|
||||||
// inherit NoiseModelFactorN's constructors
|
|
||||||
using NoiseModelFactorN<VALUE>::NoiseModelFactorN;
|
|
||||||
~NoiseModelFactor1() override {}
|
|
||||||
|
|
||||||
private:
|
|
||||||
/** Serialization function */
|
|
||||||
friend class boost::serialization::access;
|
|
||||||
template <class ARCHIVE>
|
|
||||||
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
|
|
||||||
ar& boost::serialization::make_nvp(
|
|
||||||
"NoiseModelFactor", boost::serialization::base_object<Base>(*this));
|
|
||||||
}
|
|
||||||
}; // \class NoiseModelFactor1
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1>() and X1
|
|
||||||
* with ValueType<1>.
|
|
||||||
* If your class is templated AND you are trying to call `.key<1>()` or
|
|
||||||
* `ValueType<1>` inside your class, due to dependent types you need to do
|
|
||||||
* `this->template key<1>()` or `This::template ValueType<1>`.
|
|
||||||
* ~~~
|
|
||||||
* A convenient base class for creating your own NoiseModelFactor
|
|
||||||
* with 2 variables. To derive from this class, implement evaluateError().
|
|
||||||
*/
|
|
||||||
template <class VALUE1, class VALUE2>
|
|
||||||
class GTSAM_DEPRECATED NoiseModelFactor2
|
|
||||||
: public NoiseModelFactorN<VALUE1, VALUE2> {
|
|
||||||
public:
|
|
||||||
/** Aliases for value types pulled from keys.
|
|
||||||
* Note: in your code you can probably just do:
|
|
||||||
* `using X1 = ValueType<1>;`
|
|
||||||
* but this class is uglier due to dependent types.
|
|
||||||
* See e.g. testNonlinearFactor.cpp:TestFactorN.
|
|
||||||
*/
|
|
||||||
using X1 = typename NoiseModelFactor2::template ValueType<1>;
|
|
||||||
using X2 = typename NoiseModelFactor2::template ValueType<2>;
|
|
||||||
|
|
||||||
protected:
|
|
||||||
using Base = NoiseModelFactor;
|
|
||||||
using This = NoiseModelFactor2<VALUE1, VALUE2>;
|
|
||||||
|
|
||||||
public:
|
|
||||||
// inherit NoiseModelFactorN's constructors
|
|
||||||
using NoiseModelFactorN<VALUE1, VALUE2>::NoiseModelFactorN;
|
|
||||||
~NoiseModelFactor2() override {}
|
|
||||||
|
|
||||||
private:
|
|
||||||
/** Serialization function */
|
|
||||||
friend class boost::serialization::access;
|
|
||||||
template <class ARCHIVE>
|
|
||||||
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
|
|
||||||
ar& boost::serialization::make_nvp(
|
|
||||||
"NoiseModelFactor", boost::serialization::base_object<Base>(*this));
|
|
||||||
}
|
|
||||||
}; // \class NoiseModelFactor2
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1>() and X1
|
|
||||||
* with ValueType<1>.
|
|
||||||
* If your class is templated AND you are trying to call `.key<1>()` or
|
|
||||||
* `ValueType<1>` inside your class, due to dependent types you need to do
|
|
||||||
* `this->template key<1>()` or `This::template ValueType<1>`.
|
|
||||||
* ~~~
|
|
||||||
* A convenient base class for creating your own NoiseModelFactor
|
|
||||||
* with 3 variables. To derive from this class, implement evaluateError().
|
|
||||||
*/
|
|
||||||
template <class VALUE1, class VALUE2, class VALUE3>
|
|
||||||
class GTSAM_DEPRECATED NoiseModelFactor3
|
|
||||||
: public NoiseModelFactorN<VALUE1, VALUE2, VALUE3> {
|
|
||||||
public:
|
|
||||||
/** Aliases for value types pulled from keys.
|
|
||||||
* Note: in your code you can probably just do:
|
|
||||||
* `using X1 = ValueType<1>;`
|
|
||||||
* but this class is uglier due to dependent types.
|
|
||||||
* See e.g. testNonlinearFactor.cpp:TestFactorN.
|
|
||||||
*/
|
|
||||||
using X1 = typename NoiseModelFactor3::template ValueType<1>;
|
|
||||||
using X2 = typename NoiseModelFactor3::template ValueType<2>;
|
|
||||||
using X3 = typename NoiseModelFactor3::template ValueType<3>;
|
|
||||||
|
|
||||||
protected:
|
|
||||||
using Base = NoiseModelFactor;
|
|
||||||
using This = NoiseModelFactor3<VALUE1, VALUE2, VALUE3>;
|
|
||||||
|
|
||||||
public:
|
|
||||||
// inherit NoiseModelFactorN's constructors
|
|
||||||
using NoiseModelFactorN<VALUE1, VALUE2, VALUE3>::NoiseModelFactorN;
|
|
||||||
~NoiseModelFactor3() override {}
|
|
||||||
|
|
||||||
private:
|
|
||||||
/** Serialization function */
|
|
||||||
friend class boost::serialization::access;
|
|
||||||
template <class ARCHIVE>
|
|
||||||
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
|
|
||||||
ar& boost::serialization::make_nvp(
|
|
||||||
"NoiseModelFactor", boost::serialization::base_object<Base>(*this));
|
|
||||||
}
|
|
||||||
}; // \class NoiseModelFactor3
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1>() and X1
|
|
||||||
* with ValueType<1>.
|
|
||||||
* If your class is templated AND you are trying to call `.key<1>()` or
|
|
||||||
* `ValueType<1>` inside your class, due to dependent types you need to do
|
|
||||||
* `this->template key<1>()` or `This::template ValueType<1>`.
|
|
||||||
* ~~~
|
|
||||||
* A convenient base class for creating your own NoiseModelFactor
|
|
||||||
* with 4 variables. To derive from this class, implement evaluateError().
|
|
||||||
*/
|
|
||||||
template <class VALUE1, class VALUE2, class VALUE3, class VALUE4>
|
|
||||||
class GTSAM_DEPRECATED NoiseModelFactor4
|
|
||||||
: public NoiseModelFactorN<VALUE1, VALUE2, VALUE3, VALUE4> {
|
|
||||||
public:
|
|
||||||
/** Aliases for value types pulled from keys.
|
|
||||||
* Note: in your code you can probably just do:
|
|
||||||
* `using X1 = ValueType<1>;`
|
|
||||||
* but this class is uglier due to dependent types.
|
|
||||||
* See e.g. testNonlinearFactor.cpp:TestFactorN.
|
|
||||||
*/
|
|
||||||
using X1 = typename NoiseModelFactor4::template ValueType<1>;
|
|
||||||
using X2 = typename NoiseModelFactor4::template ValueType<2>;
|
|
||||||
using X3 = typename NoiseModelFactor4::template ValueType<3>;
|
|
||||||
using X4 = typename NoiseModelFactor4::template ValueType<4>;
|
|
||||||
|
|
||||||
protected:
|
|
||||||
using Base = NoiseModelFactor;
|
|
||||||
using This = NoiseModelFactor4<VALUE1, VALUE2, VALUE3, VALUE4>;
|
|
||||||
|
|
||||||
public:
|
|
||||||
// inherit NoiseModelFactorN's constructors
|
|
||||||
using NoiseModelFactorN<VALUE1, VALUE2, VALUE3, VALUE4>::NoiseModelFactorN;
|
|
||||||
~NoiseModelFactor4() override {}
|
|
||||||
|
|
||||||
private:
|
|
||||||
/** Serialization function */
|
|
||||||
friend class boost::serialization::access;
|
|
||||||
template <class ARCHIVE>
|
|
||||||
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
|
|
||||||
ar& boost::serialization::make_nvp(
|
|
||||||
"NoiseModelFactor", boost::serialization::base_object<Base>(*this));
|
|
||||||
}
|
|
||||||
}; // \class NoiseModelFactor4
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1>() and X1
|
|
||||||
* with ValueType<1>.
|
|
||||||
* If your class is templated AND you are trying to call `.key<1>()` or
|
|
||||||
* `ValueType<1>` inside your class, due to dependent types you need to do
|
|
||||||
* `this->template key<1>()` or `This::template ValueType<1>`.
|
|
||||||
* ~~~
|
|
||||||
* A convenient base class for creating your own NoiseModelFactor
|
|
||||||
* with 5 variables. To derive from this class, implement evaluateError().
|
|
||||||
*/
|
|
||||||
template <class VALUE1, class VALUE2, class VALUE3, class VALUE4, class VALUE5>
|
|
||||||
class GTSAM_DEPRECATED NoiseModelFactor5
|
|
||||||
: public NoiseModelFactorN<VALUE1, VALUE2, VALUE3, VALUE4, VALUE5> {
|
|
||||||
public:
|
|
||||||
/** Aliases for value types pulled from keys.
|
|
||||||
* Note: in your code you can probably just do:
|
|
||||||
* `using X1 = ValueType<1>;`
|
|
||||||
* but this class is uglier due to dependent types.
|
|
||||||
* See e.g. testNonlinearFactor.cpp:TestFactorN.
|
|
||||||
*/
|
|
||||||
using X1 = typename NoiseModelFactor5::template ValueType<1>;
|
|
||||||
using X2 = typename NoiseModelFactor5::template ValueType<2>;
|
|
||||||
using X3 = typename NoiseModelFactor5::template ValueType<3>;
|
|
||||||
using X4 = typename NoiseModelFactor5::template ValueType<4>;
|
|
||||||
using X5 = typename NoiseModelFactor5::template ValueType<5>;
|
|
||||||
|
|
||||||
protected:
|
|
||||||
using Base = NoiseModelFactor;
|
|
||||||
using This = NoiseModelFactor5<VALUE1, VALUE2, VALUE3, VALUE4, VALUE5>;
|
|
||||||
|
|
||||||
public:
|
|
||||||
// inherit NoiseModelFactorN's constructors
|
|
||||||
using NoiseModelFactorN<VALUE1, VALUE2, VALUE3, VALUE4,
|
|
||||||
VALUE5>::NoiseModelFactorN;
|
|
||||||
~NoiseModelFactor5() override {}
|
|
||||||
|
|
||||||
private:
|
|
||||||
/** Serialization function */
|
|
||||||
friend class boost::serialization::access;
|
|
||||||
template <class ARCHIVE>
|
|
||||||
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
|
|
||||||
ar& boost::serialization::make_nvp(
|
|
||||||
"NoiseModelFactor", boost::serialization::base_object<Base>(*this));
|
|
||||||
}
|
|
||||||
}; // \class NoiseModelFactor5
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1>() and X1
|
|
||||||
* with ValueType<1>.
|
|
||||||
* If your class is templated AND you are trying to call `.key<1>()` or
|
|
||||||
* `ValueType<1>` inside your class, due to dependent types you need to do
|
|
||||||
* `this->template key<1>()` or `This::template ValueType<1>`.
|
|
||||||
* ~~~
|
|
||||||
* A convenient base class for creating your own NoiseModelFactor
|
|
||||||
* with 6 variables. To derive from this class, implement evaluateError().
|
|
||||||
*/
|
|
||||||
template <class VALUE1, class VALUE2, class VALUE3, class VALUE4, class VALUE5,
|
|
||||||
class VALUE6>
|
|
||||||
class GTSAM_DEPRECATED NoiseModelFactor6
|
|
||||||
: public NoiseModelFactorN<VALUE1, VALUE2, VALUE3, VALUE4, VALUE5, VALUE6> {
|
|
||||||
public:
|
|
||||||
/** Aliases for value types pulled from keys.
|
|
||||||
* Note: in your code you can probably just do:
|
|
||||||
* `using X1 = ValueType<1>;`
|
|
||||||
* but this class is uglier due to dependent types.
|
|
||||||
* See e.g. testNonlinearFactor.cpp:TestFactorN.
|
|
||||||
*/
|
|
||||||
using X1 = typename NoiseModelFactor6::template ValueType<1>;
|
|
||||||
using X2 = typename NoiseModelFactor6::template ValueType<2>;
|
|
||||||
using X3 = typename NoiseModelFactor6::template ValueType<3>;
|
|
||||||
using X4 = typename NoiseModelFactor6::template ValueType<4>;
|
|
||||||
using X5 = typename NoiseModelFactor6::template ValueType<5>;
|
|
||||||
using X6 = typename NoiseModelFactor6::template ValueType<6>;
|
|
||||||
|
|
||||||
protected:
|
|
||||||
using Base = NoiseModelFactor;
|
|
||||||
using This =
|
|
||||||
NoiseModelFactor6<VALUE1, VALUE2, VALUE3, VALUE4, VALUE5, VALUE6>;
|
|
||||||
|
|
||||||
public:
|
|
||||||
// inherit NoiseModelFactorN's constructors
|
|
||||||
using NoiseModelFactorN<VALUE1, VALUE2, VALUE3, VALUE4, VALUE5,
|
|
||||||
VALUE6>::NoiseModelFactorN;
|
|
||||||
~NoiseModelFactor6() override {}
|
|
||||||
|
|
||||||
private:
|
|
||||||
/** Serialization function */
|
|
||||||
friend class boost::serialization::access;
|
|
||||||
template <class ARCHIVE>
|
|
||||||
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
|
|
||||||
ar& boost::serialization::make_nvp(
|
|
||||||
"NoiseModelFactor", boost::serialization::base_object<Base>(*this));
|
|
||||||
}
|
|
||||||
}; // \class NoiseModelFactor6
|
|
||||||
|
|
||||||
} // \namespace gtsam
|
|
||||||
|
|
|
@ -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->template key<1>()) << ","
|
<< keyFormatter(this->key1()) << ","
|
||||||
<< keyFormatter(this->template key<2>()) << ")\n";
|
<< keyFormatter(this->key2()) << ")\n";
|
||||||
traits<T>::Print(measured_, " measured: ");
|
traits<T>::Print(measured_, " measured: ");
|
||||||
this->noiseModel_->print(" noise model: ");
|
this->noiseModel_->print(" noise model: ");
|
||||||
}
|
}
|
||||||
|
|
|
@ -129,7 +129,7 @@ struct BoundingConstraint2: public NoiseModelFactorN<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->template key<1>()), c.at<X2>(this->template key<2>()));
|
double x = value(c.at<X1>(this->key1()), c.at<X2>(this->key2()));
|
||||||
return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
|
return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -130,8 +130,8 @@ class FrobeniusBetweenFactor : public NoiseModelFactorN<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->template key<1>()) << ","
|
<< ">(" << keyFormatter(this->key1()) << ","
|
||||||
<< keyFormatter(this->template key<2>()) << ")\n";
|
<< keyFormatter(this->key2()) << ")\n";
|
||||||
traits<Rot>::Print(R12_, " R12: ");
|
traits<Rot>::Print(R12_, " R12: ");
|
||||||
this->noiseModel_->print(" noise model: ");
|
this->noiseModel_->print(" noise model: ");
|
||||||
}
|
}
|
||||||
|
|
|
@ -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->template key<1>(), key2 = this->template key<2>();
|
const Key key1 = this->key1(), key2 = this->key2();
|
||||||
JacobianC H1;
|
JacobianC H1;
|
||||||
JacobianL H2;
|
JacobianL H2;
|
||||||
Vector2 b;
|
Vector2 b;
|
||||||
|
@ -270,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->template key<2>())
|
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2())
|
||||||
<< " behind Camera " << DefaultKeyFormatter(this->template key<1>()) << std::endl;
|
<< " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl;
|
||||||
}
|
}
|
||||||
return Z_2x1;
|
return Z_2x1;
|
||||||
}
|
}
|
||||||
|
|
|
@ -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->template key<2>()) <<
|
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
|
||||||
" moved behind camera " << DefaultKeyFormatter(this->template key<1>()) << std::endl;
|
" moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
|
||||||
if (throwCheirality_)
|
if (throwCheirality_)
|
||||||
throw CheiralityException(this->template key<2>());
|
throw CheiralityException(this->key2());
|
||||||
}
|
}
|
||||||
return Vector2::Constant(2.0 * K_->fx());
|
return Vector2::Constant(2.0 * K_->fx());
|
||||||
}
|
}
|
||||||
|
|
|
@ -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->template key<1>()) << ","
|
<< "Global: " << keyFormatter(this->key1()) << ","
|
||||||
<< " Transform: " << keyFormatter(this->template key<2>()) << ","
|
<< " Transform: " << keyFormatter(this->key2()) << ","
|
||||||
<< " Local: " << keyFormatter(this->template key<3>()) << ")\n";
|
<< " Local: " << keyFormatter(this->key3()) << ")\n";
|
||||||
this->noiseModel_->print(" noise model");
|
this->noiseModel_->print(" noise model");
|
||||||
}
|
}
|
||||||
|
|
||||||
// access - convenience functions
|
// access - convenience functions
|
||||||
Key global_key() const { return this->template key<1>(); }
|
Key global_key() const { return this->key1(); }
|
||||||
Key transform_key() const { return this->template key<2>(); }
|
Key transform_key() const { return this->key2(); }
|
||||||
Key local_key() const { return this->template key<3>(); }
|
Key local_key() const { return this->key3(); }
|
||||||
|
|
||||||
private:
|
private:
|
||||||
/** Serialization function */
|
/** Serialization function */
|
||||||
|
|
|
@ -141,10 +141,10 @@ 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->template key<2>()) <<
|
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
|
||||||
" moved behind camera " << DefaultKeyFormatter(this->template key<1>()) << std::endl;
|
" moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
|
||||||
if (throwCheirality_)
|
if (throwCheirality_)
|
||||||
throw StereoCheiralityException(this->template key<2>());
|
throw StereoCheiralityException(this->key2());
|
||||||
}
|
}
|
||||||
return Vector3::Constant(2.0 * K_->fx());
|
return Vector3::Constant(2.0 * K_->fx());
|
||||||
}
|
}
|
||||||
|
|
|
@ -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->template key<1>()) << ","
|
<< keyFormatter(this->key1()) << ","
|
||||||
<< keyFormatter(this->template key<2>()) << ","
|
<< keyFormatter(this->key2()) << ","
|
||||||
<< keyFormatter(this->template key<3>()) << ","
|
<< keyFormatter(this->key3()) << ","
|
||||||
<< keyFormatter(this->template key<4>()) << ","
|
<< keyFormatter(this->key4()) << ","
|
||||||
<< keyFormatter(this->template key<5>()) << "\n";
|
<< keyFormatter(this->key5()) << "\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;
|
||||||
|
|
|
@ -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->template key<1>()) << ","
|
<< keyFormatter(this->key1()) << ","
|
||||||
<< keyFormatter(this->template key<2>()) << ")\n";
|
<< keyFormatter(this->key2()) << ")\n";
|
||||||
this->noiseModel_->print(" noise model");
|
this->noiseModel_->print(" noise model");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -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->template key<1>()) << ","
|
<< keyFormatter(this->key1()) << ","
|
||||||
<< keyFormatter(this->template key<2>()) << ","
|
<< keyFormatter(this->key2()) << ","
|
||||||
<< keyFormatter(this->template key<3>()) << ","
|
<< keyFormatter(this->key3()) << ","
|
||||||
<< keyFormatter(this->template key<4>()) << ","
|
<< keyFormatter(this->key4()) << ","
|
||||||
<< keyFormatter(this->template key<5>()) << "\n";
|
<< keyFormatter(this->key5()) << "\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;
|
||||||
|
|
|
@ -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->template key<2>()) <<
|
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
|
||||||
" moved behind camera " << DefaultKeyFormatter(this->template key<1>()) << std::endl;
|
" moved behind camera " << DefaultKeyFormatter(this->key1()) << 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();
|
||||||
|
|
|
@ -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->template key<1>()) << ","
|
<< keyFormatter(this->key1()) << ","
|
||||||
<< keyFormatter(this->template key<2>()) << ")\n";
|
<< keyFormatter(this->key2()) << ")\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: ");
|
||||||
|
|
|
@ -48,8 +48,8 @@ class PoseToPointFactor : public NoiseModelFactorN<POSE, POINT> {
|
||||||
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("
|
std::cout << s << "PoseToPointFactor("
|
||||||
<< keyFormatter(this->template key<1>()) << ","
|
<< keyFormatter(this->key1()) << ","
|
||||||
<< keyFormatter(this->template key<2>()) << ")\n"
|
<< keyFormatter(this->key2()) << ")\n"
|
||||||
<< " measured: " << measured_.transpose() << std::endl;
|
<< " measured: " << measured_.transpose() << std::endl;
|
||||||
this->noiseModel_->print(" noise model: ");
|
this->noiseModel_->print(" noise model: ");
|
||||||
}
|
}
|
||||||
|
|
|
@ -143,9 +143,9 @@ namespace gtsam {
|
||||||
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->template key<2>())
|
<< DefaultKeyFormatter(this->key2())
|
||||||
<< " moved behind camera "
|
<< " moved behind camera "
|
||||||
<< DefaultKeyFormatter(this->template key<1>())
|
<< DefaultKeyFormatter(this->key1())
|
||||||
<< std::endl;
|
<< std::endl;
|
||||||
if (throwCheirality_)
|
if (throwCheirality_)
|
||||||
throw e;
|
throw e;
|
||||||
|
|
|
@ -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->template key<2>()) <<
|
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
|
||||||
" moved behind camera " << DefaultKeyFormatter(this->template key<1>()) << std::endl;
|
" moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
|
||||||
if (throwCheirality_)
|
if (throwCheirality_)
|
||||||
throw e;
|
throw e;
|
||||||
}
|
}
|
||||||
|
|
|
@ -330,13 +330,6 @@ TEST( NonlinearFactor, cloneWithNewNoiseModel )
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
// Suppress deprecation warnings while we are testing backwards compatibility
|
|
||||||
#define IGNORE_DEPRECATED_PUSH \
|
|
||||||
CLANG_DIAGNOSTIC_PUSH_IGNORE("-Wdeprecated-declarations") \
|
|
||||||
GCC_DIAGNOSTIC_PUSH_IGNORE("-Wdeprecated-declarations") \
|
|
||||||
MSVC_DIAGNOSTIC_PUSH_IGNORE(4996)
|
|
||||||
/* ************************************************************************* */
|
|
||||||
IGNORE_DEPRECATED_PUSH
|
|
||||||
class TestFactor1 : public NoiseModelFactor1<double> {
|
class TestFactor1 : public NoiseModelFactor1<double> {
|
||||||
static_assert(std::is_same<Base, NoiseModelFactor>::value, "Base type wrong");
|
static_assert(std::is_same<Base, NoiseModelFactor>::value, "Base type wrong");
|
||||||
static_assert(std::is_same<This, NoiseModelFactor1<double>>::value,
|
static_assert(std::is_same<This, NoiseModelFactor1<double>>::value,
|
||||||
|
@ -358,7 +351,6 @@ class TestFactor1 : public NoiseModelFactor1<double> {
|
||||||
gtsam::NonlinearFactor::shared_ptr(new TestFactor1(*this)));
|
gtsam::NonlinearFactor::shared_ptr(new TestFactor1(*this)));
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
DIAGNOSTIC_POP()
|
|
||||||
|
|
||||||
/* ************************************ */
|
/* ************************************ */
|
||||||
TEST(NonlinearFactor, NoiseModelFactor1) {
|
TEST(NonlinearFactor, NoiseModelFactor1) {
|
||||||
|
@ -388,7 +380,6 @@ TEST(NonlinearFactor, NoiseModelFactor1) {
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
IGNORE_DEPRECATED_PUSH
|
|
||||||
class TestFactor4 : public NoiseModelFactor4<double, double, double, double> {
|
class TestFactor4 : public NoiseModelFactor4<double, double, double, double> {
|
||||||
static_assert(std::is_same<Base, NoiseModelFactor>::value, "Base type wrong");
|
static_assert(std::is_same<Base, NoiseModelFactor>::value, "Base type wrong");
|
||||||
static_assert(
|
static_assert(
|
||||||
|
@ -420,7 +411,6 @@ class TestFactor4 : public NoiseModelFactor4<double, double, double, double> {
|
||||||
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
|
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
|
||||||
gtsam::NonlinearFactor::shared_ptr(new TestFactor4(*this))); }
|
gtsam::NonlinearFactor::shared_ptr(new TestFactor4(*this))); }
|
||||||
};
|
};
|
||||||
DIAGNOSTIC_POP()
|
|
||||||
|
|
||||||
/* ************************************ */
|
/* ************************************ */
|
||||||
TEST(NonlinearFactor, NoiseModelFactor4) {
|
TEST(NonlinearFactor, NoiseModelFactor4) {
|
||||||
|
@ -444,7 +434,6 @@ TEST(NonlinearFactor, NoiseModelFactor4) {
|
||||||
EXPECT(assert_equal((Vector)(Vector(1) << 0.5 * -30.).finished(), jf.getb()));
|
EXPECT(assert_equal((Vector)(Vector(1) << 0.5 * -30.).finished(), jf.getb()));
|
||||||
|
|
||||||
// Test all functions/types for backwards compatibility
|
// Test all functions/types for backwards compatibility
|
||||||
IGNORE_DEPRECATED_PUSH
|
|
||||||
static_assert(std::is_same<TestFactor4::X1, double>::value,
|
static_assert(std::is_same<TestFactor4::X1, double>::value,
|
||||||
"X1 type incorrect");
|
"X1 type incorrect");
|
||||||
static_assert(std::is_same<TestFactor4::X2, double>::value,
|
static_assert(std::is_same<TestFactor4::X2, double>::value,
|
||||||
|
@ -463,7 +452,6 @@ TEST(NonlinearFactor, NoiseModelFactor4) {
|
||||||
EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.).finished(), H.at(1)));
|
EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.).finished(), H.at(1)));
|
||||||
EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 3.).finished(), H.at(2)));
|
EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 3.).finished(), H.at(2)));
|
||||||
EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 4.).finished(), H.at(3)));
|
EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 4.).finished(), H.at(3)));
|
||||||
DIAGNOSTIC_POP()
|
|
||||||
|
|
||||||
// And test "forward compatibility" using `key<N>` and `ValueType<N>` too
|
// And test "forward compatibility" using `key<N>` and `ValueType<N>` too
|
||||||
static_assert(std::is_same<TestFactor4::ValueType<1>, double>::value,
|
static_assert(std::is_same<TestFactor4::ValueType<1>, double>::value,
|
||||||
|
@ -489,7 +477,6 @@ TEST(NonlinearFactor, NoiseModelFactor4) {
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
IGNORE_DEPRECATED_PUSH
|
|
||||||
class TestFactor5 : public NoiseModelFactor5<double, double, double, double, double> {
|
class TestFactor5 : public NoiseModelFactor5<double, double, double, double, double> {
|
||||||
public:
|
public:
|
||||||
typedef NoiseModelFactor5<double, double, double, double, double> Base;
|
typedef NoiseModelFactor5<double, double, double, double, double> Base;
|
||||||
|
@ -513,7 +500,6 @@ public:
|
||||||
.finished();
|
.finished();
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
DIAGNOSTIC_POP()
|
|
||||||
|
|
||||||
/* ************************************ */
|
/* ************************************ */
|
||||||
TEST(NonlinearFactor, NoiseModelFactor5) {
|
TEST(NonlinearFactor, NoiseModelFactor5) {
|
||||||
|
@ -541,7 +527,6 @@ TEST(NonlinearFactor, NoiseModelFactor5) {
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
IGNORE_DEPRECATED_PUSH
|
|
||||||
class TestFactor6 : public NoiseModelFactor6<double, double, double, double, double, double> {
|
class TestFactor6 : public NoiseModelFactor6<double, double, double, double, double, double> {
|
||||||
public:
|
public:
|
||||||
typedef NoiseModelFactor6<double, double, double, double, double, double> Base;
|
typedef NoiseModelFactor6<double, double, double, double, double, double> Base;
|
||||||
|
@ -569,7 +554,6 @@ public:
|
||||||
}
|
}
|
||||||
|
|
||||||
};
|
};
|
||||||
DIAGNOSTIC_POP()
|
|
||||||
|
|
||||||
/* ************************************ */
|
/* ************************************ */
|
||||||
TEST(NonlinearFactor, NoiseModelFactor6) {
|
TEST(NonlinearFactor, NoiseModelFactor6) {
|
||||||
|
@ -673,11 +657,11 @@ TEST(NonlinearFactor, NoiseModelFactorN) {
|
||||||
EXPECT(assert_equal(H4_expected, H4));
|
EXPECT(assert_equal(H4_expected, H4));
|
||||||
|
|
||||||
// Test all functions/types for backwards compatibility
|
// Test all functions/types for backwards compatibility
|
||||||
IGNORE_DEPRECATED_PUSH
|
|
||||||
static_assert(std::is_same<TestFactor4::X1, double>::value,
|
static_assert(std::is_same<TestFactor4::X1, double>::value,
|
||||||
"X1 type incorrect");
|
"X1 type incorrect");
|
||||||
EXPECT(assert_equal(tf.key3(), X(3)));
|
EXPECT(assert_equal(tf.key3(), X(3)));
|
||||||
DIAGNOSTIC_POP()
|
|
||||||
|
|
||||||
// Test using `key<N>` and `ValueType<N>`
|
// Test using `key<N>` and `ValueType<N>`
|
||||||
static_assert(std::is_same<TestFactorN::ValueType<1>, double>::value,
|
static_assert(std::is_same<TestFactorN::ValueType<1>, double>::value,
|
||||||
|
|
Loading…
Reference in New Issue