Removed old Testable base class
parent
4962c8ed38
commit
3b5c6e8cef
|
@ -18,8 +18,6 @@
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <gtsam/base/Testable.h>
|
|
||||||
|
|
||||||
#include <set>
|
#include <set>
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
#include <string>
|
#include <string>
|
||||||
|
@ -45,8 +43,7 @@ struct FastSetTestableHelper;
|
||||||
* @ingroup base
|
* @ingroup base
|
||||||
*/
|
*/
|
||||||
template<typename VALUE, class ENABLE = void>
|
template<typename VALUE, class ENABLE = void>
|
||||||
class FastSet: public std::set<VALUE, std::less<VALUE>, boost::fast_pool_allocator<VALUE> >,
|
class FastSet: public std::set<VALUE, std::less<VALUE>, boost::fast_pool_allocator<VALUE> > {
|
||||||
public Testable<FastSet<VALUE> > {
|
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
|
|
|
@ -17,7 +17,6 @@
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <gtsam/base/Testable.h>
|
|
||||||
#include <stdarg.h>
|
#include <stdarg.h>
|
||||||
#include <gtsam/base/Vector.h>
|
#include <gtsam/base/Vector.h>
|
||||||
|
|
||||||
|
@ -28,8 +27,7 @@ namespace gtsam {
|
||||||
* size checking.
|
* size checking.
|
||||||
*/
|
*/
|
||||||
template<size_t N>
|
template<size_t N>
|
||||||
class FixedVector : public Eigen::Matrix<double, N, 1>,
|
class FixedVector : public Eigen::Matrix<double, N, 1> {
|
||||||
public Testable<FixedVector<N> > {
|
|
||||||
public:
|
public:
|
||||||
typedef Eigen::Matrix<double, N, 1> Base;
|
typedef Eigen::Matrix<double, N, 1> Base;
|
||||||
|
|
||||||
|
|
|
@ -11,7 +11,7 @@
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @file Testable.h
|
* @file Testable.h
|
||||||
* @brief Abstract base class for values that can be used in unit tests
|
* @brief Concept check for values that can be used in unit tests
|
||||||
* @author Frank Dellaert
|
* @author Frank Dellaert
|
||||||
*
|
*
|
||||||
* The necessary functions to implement for Testable are defined
|
* The necessary functions to implement for Testable are defined
|
||||||
|
@ -41,34 +41,12 @@
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* The Testable class should be templated with the derived class, e.g.
|
* A testable concept check that should be placed in applicable unit
|
||||||
* class Rot3 : public Testable<Rot3>. This allows us to define the
|
* tests and in generic algorithms.
|
||||||
* input type of equals as a Rot3 as well.
|
*
|
||||||
* @ingroup base
|
|
||||||
*/
|
|
||||||
template <class Derived>
|
|
||||||
class Testable {
|
|
||||||
|
|
||||||
private:
|
|
||||||
/**
|
|
||||||
* This concept check is to make sure these methods exist in derived classes
|
|
||||||
* This is as an alternative to declaring those methods virtual, which is slower
|
|
||||||
*/
|
|
||||||
static void concept(const Derived& d) {
|
|
||||||
const Derived *const_derived = static_cast<Derived*>(&d);
|
|
||||||
const_derived->print(std::string());
|
|
||||||
const_derived->print();
|
|
||||||
bool r;
|
|
||||||
r = const_derived->equals(*const_derived, 1.0);
|
|
||||||
r = const_derived->equals(*const_derived);
|
|
||||||
}
|
|
||||||
|
|
||||||
}; // Testable class
|
|
||||||
|
|
||||||
/**
|
|
||||||
* A testable concept check to be placed in unit tests, rather than subclassing
|
|
||||||
* See macros for details on using this structure
|
* See macros for details on using this structure
|
||||||
* @ingroup base
|
* @ingroup base
|
||||||
|
* @tparam T is the type this constrains to be testable - assumes print() and equals()
|
||||||
*/
|
*/
|
||||||
template <class T>
|
template <class T>
|
||||||
class TestableConcept {
|
class TestableConcept {
|
||||||
|
@ -83,7 +61,7 @@ namespace gtsam {
|
||||||
bool r2 = d.equals(d);
|
bool r2 = d.equals(d);
|
||||||
return r1 && r2;
|
return r1 && r2;
|
||||||
}
|
}
|
||||||
}; // Testable class
|
};
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* This template works for any type with equals
|
* This template works for any type with equals
|
||||||
|
@ -133,5 +111,6 @@ namespace gtsam {
|
||||||
* NOTE: intentionally not in the gtsam namespace to allow for classes not in
|
* NOTE: intentionally not in the gtsam namespace to allow for classes not in
|
||||||
* the gtsam namespace to be more easily enforced as testable
|
* the gtsam namespace to be more easily enforced as testable
|
||||||
*/
|
*/
|
||||||
|
/// TODO: find better name for "INST" macro, something like "UNIT" or similar
|
||||||
#define GTSAM_CONCEPT_TESTABLE_INST(T) template class gtsam::TestableConcept<T>;
|
#define GTSAM_CONCEPT_TESTABLE_INST(T) template class gtsam::TestableConcept<T>;
|
||||||
#define GTSAM_CONCEPT_TESTABLE_TYPE(T) typedef gtsam::TestableConcept<T> _gtsam_TestableConcept_##T;
|
#define GTSAM_CONCEPT_TESTABLE_TYPE(T) typedef gtsam::TestableConcept<T> _gtsam_TestableConcept_##T;
|
||||||
|
|
|
@ -24,7 +24,6 @@
|
||||||
#include <boost/serialization/nvp.hpp>
|
#include <boost/serialization/nvp.hpp>
|
||||||
|
|
||||||
#include <gtsam/base/Matrix.h>
|
#include <gtsam/base/Matrix.h>
|
||||||
#include <gtsam/base/Testable.h>
|
|
||||||
#include <gtsam/base/Lie.h>
|
#include <gtsam/base/Lie.h>
|
||||||
|
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
@ -33,7 +32,7 @@ namespace gtsam {
|
||||||
* A 3D point
|
* A 3D point
|
||||||
* @ingroup geometry
|
* @ingroup geometry
|
||||||
*/
|
*/
|
||||||
class Point3: Testable<Point3>, public Lie<Point3> {
|
class Point3 {
|
||||||
public:
|
public:
|
||||||
/// dimension of the variable - used to autodetect sizes
|
/// dimension of the variable - used to autodetect sizes
|
||||||
static const size_t dimension = 3;
|
static const size_t dimension = 3;
|
||||||
|
|
|
@ -27,7 +27,7 @@ namespace gtsam {
|
||||||
* A 3D pose (R,t) : (Rot3,Point3)
|
* A 3D pose (R,t) : (Rot3,Point3)
|
||||||
* @ingroup geometry
|
* @ingroup geometry
|
||||||
*/
|
*/
|
||||||
class Pose3 : Testable<Pose3>, public Lie<Pose3> {
|
class Pose3 : public Lie<Pose3> {
|
||||||
public:
|
public:
|
||||||
static const size_t dimension = 6;
|
static const size_t dimension = 6;
|
||||||
|
|
||||||
|
|
|
@ -32,7 +32,7 @@ typedef Eigen::Quaterniond Quaternion;
|
||||||
* @brief 3D Rotations represented as rotation matrices
|
* @brief 3D Rotations represented as rotation matrices
|
||||||
* @ingroup geometry
|
* @ingroup geometry
|
||||||
*/
|
*/
|
||||||
class Rot3: Testable<Rot3>, public Lie<Rot3> {
|
class Rot3: public Lie<Rot3> {
|
||||||
public:
|
public:
|
||||||
static const size_t dimension = 3;
|
static const size_t dimension = 3;
|
||||||
|
|
||||||
|
|
|
@ -18,6 +18,7 @@
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
|
|
||||||
#include <CppUnitLite/TestHarness.h>
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
#include <gtsam/base/Testable.h>
|
||||||
#include <gtsam/base/numericalDerivative.h>
|
#include <gtsam/base/numericalDerivative.h>
|
||||||
#include <gtsam/geometry/CalibratedCamera.h>
|
#include <gtsam/geometry/CalibratedCamera.h>
|
||||||
|
|
||||||
|
|
|
@ -15,6 +15,7 @@
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include <CppUnitLite/TestHarness.h>
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
#include <gtsam/base/Testable.h>
|
||||||
#include <gtsam/geometry/Point3.h>
|
#include <gtsam/geometry/Point3.h>
|
||||||
|
|
||||||
using namespace gtsam;
|
using namespace gtsam;
|
||||||
|
|
|
@ -16,6 +16,7 @@
|
||||||
|
|
||||||
#include <cmath>
|
#include <cmath>
|
||||||
#include <CppUnitLite/TestHarness.h>
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
#include <gtsam/base/Testable.h>
|
||||||
#include <gtsam/base/numericalDerivative.h>
|
#include <gtsam/base/numericalDerivative.h>
|
||||||
#include <gtsam/base/lieProxies.h>
|
#include <gtsam/base/lieProxies.h>
|
||||||
#include <gtsam/geometry/Pose3.h>
|
#include <gtsam/geometry/Pose3.h>
|
||||||
|
|
|
@ -16,6 +16,7 @@
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include <CppUnitLite/TestHarness.h>
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
#include <gtsam/base/Testable.h>
|
||||||
#include <boost/math/constants/constants.hpp>
|
#include <boost/math/constants/constants.hpp>
|
||||||
#include <gtsam/base/numericalDerivative.h>
|
#include <gtsam/base/numericalDerivative.h>
|
||||||
#include <gtsam/base/lieProxies.h>
|
#include <gtsam/base/lieProxies.h>
|
||||||
|
|
|
@ -19,6 +19,7 @@
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
|
|
||||||
#include <CppUnitLite/TestHarness.h>
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
#include <gtsam/base/Testable.h>
|
||||||
#include <gtsam/base/numericalDerivative.h>
|
#include <gtsam/base/numericalDerivative.h>
|
||||||
#include <gtsam/geometry/SimpleCamera.h>
|
#include <gtsam/geometry/SimpleCamera.h>
|
||||||
|
|
||||||
|
|
|
@ -17,6 +17,7 @@
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include <CppUnitLite/TestHarness.h>
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
#include <gtsam/base/Testable.h>
|
||||||
#include <gtsam/base/numericalDerivative.h>
|
#include <gtsam/base/numericalDerivative.h>
|
||||||
#include <gtsam/geometry/StereoCamera.h>
|
#include <gtsam/geometry/StereoCamera.h>
|
||||||
|
|
||||||
|
|
|
@ -17,6 +17,7 @@
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
|
#include <gtsam/base/Testable.h>
|
||||||
#include <gtsam/inference/BayesNet.h>
|
#include <gtsam/inference/BayesNet.h>
|
||||||
|
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
|
|
|
@ -26,7 +26,6 @@
|
||||||
|
|
||||||
#include <gtsam/base/types.h>
|
#include <gtsam/base/types.h>
|
||||||
#include <gtsam/base/FastList.h>
|
#include <gtsam/base/FastList.h>
|
||||||
#include <gtsam/base/Testable.h>
|
|
||||||
#include <gtsam/inference/Permutation.h>
|
#include <gtsam/inference/Permutation.h>
|
||||||
|
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
@ -41,7 +40,7 @@ namespace gtsam {
|
||||||
* todo: how to handle Bayes nets with an optimize function? Currently using global functions.
|
* todo: how to handle Bayes nets with an optimize function? Currently using global functions.
|
||||||
*/
|
*/
|
||||||
template<class CONDITIONAL>
|
template<class CONDITIONAL>
|
||||||
class BayesNet: public Testable<BayesNet<CONDITIONAL> > {
|
class BayesNet {
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
|
|
|
@ -37,7 +37,7 @@ namespace gtsam {
|
||||||
* \ingroup Multifrontal
|
* \ingroup Multifrontal
|
||||||
*/
|
*/
|
||||||
template<class CONDITIONAL>
|
template<class CONDITIONAL>
|
||||||
class BayesTree: public Testable<BayesTree<CONDITIONAL> > {
|
class BayesTree {
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
|
@ -147,7 +147,7 @@ namespace gtsam {
|
||||||
typedef boost::shared_ptr<Clique> sharedClique;
|
typedef boost::shared_ptr<Clique> sharedClique;
|
||||||
|
|
||||||
// A convenience class for a list of shared cliques
|
// A convenience class for a list of shared cliques
|
||||||
struct Cliques : public std::list<sharedClique>, public Testable<Cliques> {
|
struct Cliques : public std::list<sharedClique> {
|
||||||
void print(const std::string& s = "Cliques") const;
|
void print(const std::string& s = "Cliques") const;
|
||||||
bool equals(const Cliques& other, double tol = 1e-9) const;
|
bool equals(const Cliques& other, double tol = 1e-9) const;
|
||||||
};
|
};
|
||||||
|
|
|
@ -26,7 +26,6 @@
|
||||||
#include <boost/weak_ptr.hpp>
|
#include <boost/weak_ptr.hpp>
|
||||||
|
|
||||||
#include <gtsam/base/types.h>
|
#include <gtsam/base/types.h>
|
||||||
#include <gtsam/base/Testable.h>
|
|
||||||
|
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
||||||
|
@ -36,7 +35,7 @@ namespace gtsam {
|
||||||
* each factor f_i is associated with a single cluster and scope(f_i) \sub C_k.
|
* each factor f_i is associated with a single cluster and scope(f_i) \sub C_k.
|
||||||
*/
|
*/
|
||||||
template <class FG>
|
template <class FG>
|
||||||
class ClusterTree : public Testable<ClusterTree<FG> > {
|
class ClusterTree {
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
|
|
||||||
|
|
|
@ -34,7 +34,7 @@ namespace gtsam {
|
||||||
* additionally identifies cliques in the chordal Bayes net.
|
* additionally identifies cliques in the chordal Bayes net.
|
||||||
*/
|
*/
|
||||||
template<class FACTOR>
|
template<class FACTOR>
|
||||||
class EliminationTree: public Testable<EliminationTree<FACTOR> > {
|
class EliminationTree {
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
|
|
|
@ -25,7 +25,6 @@
|
||||||
#include <vector>
|
#include <vector>
|
||||||
#include <boost/serialization/nvp.hpp>
|
#include <boost/serialization/nvp.hpp>
|
||||||
#include <boost/foreach.hpp>
|
#include <boost/foreach.hpp>
|
||||||
#include <gtsam/base/Testable.h>
|
|
||||||
#include <gtsam/base/FastMap.h>
|
#include <gtsam/base/FastMap.h>
|
||||||
|
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
@ -51,7 +50,7 @@ template<class KEY> class Conditional;
|
||||||
* during symbolic elimination. GaussianFactor and NonlinearFactor are virtual.
|
* during symbolic elimination. GaussianFactor and NonlinearFactor are virtual.
|
||||||
*/
|
*/
|
||||||
template<typename KEY>
|
template<typename KEY>
|
||||||
class Factor : public Testable<Factor<KEY> > {
|
class Factor {
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
|
|
|
@ -22,6 +22,7 @@
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
|
#include <stdio.h>
|
||||||
#include <list>
|
#include <list>
|
||||||
#include <sstream>
|
#include <sstream>
|
||||||
#include <stdexcept>
|
#include <stdexcept>
|
||||||
|
|
|
@ -38,7 +38,7 @@ namespace gtsam {
|
||||||
* Templated on the type of factors and values structure.
|
* Templated on the type of factors and values structure.
|
||||||
*/
|
*/
|
||||||
template<class FACTOR>
|
template<class FACTOR>
|
||||||
class FactorGraph: public Testable<FactorGraph<FACTOR> > {
|
class FactorGraph {
|
||||||
public:
|
public:
|
||||||
typedef FACTOR FactorType;
|
typedef FACTOR FactorType;
|
||||||
typedef boost::shared_ptr<FactorGraph<FACTOR> > shared_ptr;
|
typedef boost::shared_ptr<FactorGraph<FACTOR> > shared_ptr;
|
||||||
|
|
|
@ -38,8 +38,7 @@ namespace gtsam {
|
||||||
* Additionally, the first step of MFQR is symbolic sequential elimination.
|
* Additionally, the first step of MFQR is symbolic sequential elimination.
|
||||||
*/
|
*/
|
||||||
template<class FACTOR>
|
template<class FACTOR>
|
||||||
class GenericSequentialSolver: public Testable<
|
class GenericSequentialSolver {
|
||||||
GenericSequentialSolver<FACTOR> > {
|
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
|
|
||||||
|
|
|
@ -19,7 +19,6 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <gtsam/base/types.h>
|
#include <gtsam/base/types.h>
|
||||||
#include <gtsam/base/Testable.h>
|
|
||||||
#include <gtsam/base/FastMap.h>
|
#include <gtsam/base/FastMap.h>
|
||||||
|
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
|
@ -55,7 +54,7 @@ namespace gtsam {
|
||||||
* is not performed by this class.
|
* is not performed by this class.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
class VariableSlots : public FastMap<Index, std::vector<Index> >, public Testable<VariableSlots> {
|
class VariableSlots : public FastMap<Index, std::vector<Index> > {
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
|
|
|
@ -36,7 +36,8 @@ typedef BayesTree<IndexConditional> SymbolicBayesTree;
|
||||||
|
|
||||||
///* ************************************************************************* */
|
///* ************************************************************************* */
|
||||||
//// SLAM example from RSS sqrtSAM paper
|
//// SLAM example from RSS sqrtSAM paper
|
||||||
static const Index _x3_=0, _x2_=1, _x1_=2, _l2_=3, _l1_=4;
|
static const Index _x3_=0, _x2_=1;
|
||||||
|
//static const Index _x1_=2, _l2_=3, _l1_=4; // unused
|
||||||
//IndexConditional::shared_ptr
|
//IndexConditional::shared_ptr
|
||||||
// x3(new IndexConditional(_x3_)),
|
// x3(new IndexConditional(_x3_)),
|
||||||
// x2(new IndexConditional(_x2_,_x3_)),
|
// x2(new IndexConditional(_x2_,_x3_)),
|
||||||
|
|
|
@ -21,6 +21,7 @@ using namespace boost::assign;
|
||||||
|
|
||||||
#include <CppUnitLite/TestHarness.h>
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
|
||||||
|
#include <gtsam/base/Testable.h>
|
||||||
#include <gtsam/inference/IndexConditional.h>
|
#include <gtsam/inference/IndexConditional.h>
|
||||||
#include <gtsam/inference/SymbolicFactorGraph.h>
|
#include <gtsam/inference/SymbolicFactorGraph.h>
|
||||||
|
|
||||||
|
|
|
@ -24,7 +24,7 @@
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
||||||
/** vector of errors */
|
/** vector of errors */
|
||||||
class Errors : public std::list<Vector>, public Testable<Errors> {
|
class Errors : public std::list<Vector> {
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
|
|
|
@ -22,10 +22,10 @@
|
||||||
#include <boost/utility.hpp>
|
#include <boost/utility.hpp>
|
||||||
|
|
||||||
#include <gtsam/base/types.h>
|
#include <gtsam/base/types.h>
|
||||||
|
#include <gtsam/base/blockMatrices.h>
|
||||||
#include <gtsam/inference/IndexConditional.h>
|
#include <gtsam/inference/IndexConditional.h>
|
||||||
#include <gtsam/linear/JacobianFactor.h>
|
#include <gtsam/linear/JacobianFactor.h>
|
||||||
#include <gtsam/linear/VectorValues.h>
|
#include <gtsam/linear/VectorValues.h>
|
||||||
#include <gtsam/base/blockMatrices.h>
|
|
||||||
|
|
||||||
// Forward declaration to friend unit tests
|
// Forward declaration to friend unit tests
|
||||||
class eliminate2JacobianFactorTest;
|
class eliminate2JacobianFactorTest;
|
||||||
|
|
|
@ -18,14 +18,14 @@
|
||||||
* @author Frank Dellaert
|
* @author Frank Dellaert
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
#include <gtsam/base/Testable.h>
|
||||||
|
#include <gtsam/base/debug.h>
|
||||||
|
#include <gtsam/base/timing.h>
|
||||||
|
#include <gtsam/base/cholesky.h>
|
||||||
#include <gtsam/linear/HessianFactor.h>
|
#include <gtsam/linear/HessianFactor.h>
|
||||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||||
#include <gtsam/inference/VariableSlots.h>
|
#include <gtsam/inference/VariableSlots.h>
|
||||||
#include <gtsam/inference/FactorGraph-inl.h>
|
#include <gtsam/inference/FactorGraph-inl.h>
|
||||||
#include <gtsam/base/debug.h>
|
|
||||||
#include <gtsam/base/timing.h>
|
|
||||||
#include <gtsam/base/cholesky.h>
|
|
||||||
|
|
||||||
|
|
||||||
using namespace std;
|
using namespace std;
|
||||||
using namespace gtsam;
|
using namespace gtsam;
|
||||||
|
|
|
@ -19,7 +19,6 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <boost/serialization/nvp.hpp>
|
#include <boost/serialization/nvp.hpp>
|
||||||
#include <gtsam/base/Testable.h>
|
|
||||||
#include <gtsam/base/Matrix.h>
|
#include <gtsam/base/Matrix.h>
|
||||||
|
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
@ -43,7 +42,7 @@ namespace gtsam {
|
||||||
* Noise models must implement a 'whiten' function to normalize an error vector,
|
* Noise models must implement a 'whiten' function to normalize an error vector,
|
||||||
* and an 'unwhiten' function to unnormalize an error vector.
|
* and an 'unwhiten' function to unnormalize an error vector.
|
||||||
*/
|
*/
|
||||||
class Base : public Testable<Base> {
|
class Base {
|
||||||
|
|
||||||
public:
|
public:
|
||||||
typedef boost::shared_ptr<Base> shared_ptr;
|
typedef boost::shared_ptr<Base> shared_ptr;
|
||||||
|
|
|
@ -17,7 +17,6 @@
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <gtsam/base/Testable.h>
|
|
||||||
#include <gtsam/base/Vector.h>
|
#include <gtsam/base/Vector.h>
|
||||||
#include <gtsam/base/types.h>
|
#include <gtsam/base/types.h>
|
||||||
|
|
||||||
|
@ -38,7 +37,7 @@ namespace gtsam {
|
||||||
* Access is through the variable index j, and returns a SubVector,
|
* Access is through the variable index j, and returns a SubVector,
|
||||||
* which is a view on the underlying data structure.
|
* which is a view on the underlying data structure.
|
||||||
*/
|
*/
|
||||||
class VectorValues: public Testable<VectorValues> {
|
class VectorValues {
|
||||||
protected:
|
protected:
|
||||||
Vector values_;
|
Vector values_;
|
||||||
std::vector<size_t> varStarts_; // start at 0 with size nVars + 1
|
std::vector<size_t> varStarts_; // start at 0 with size nVars + 1
|
||||||
|
|
|
@ -19,6 +19,7 @@
|
||||||
using namespace boost::assign;
|
using namespace boost::assign;
|
||||||
|
|
||||||
#include <CppUnitLite/TestHarness.h>
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
#include <gtsam/base/Testable.h>
|
||||||
#include <gtsam/linear/Errors.h>
|
#include <gtsam/linear/Errors.h>
|
||||||
|
|
||||||
using namespace std;
|
using namespace std;
|
||||||
|
|
|
@ -31,7 +31,9 @@ using namespace std;
|
||||||
using namespace gtsam;
|
using namespace gtsam;
|
||||||
using namespace boost;
|
using namespace boost;
|
||||||
|
|
||||||
|
#ifdef BROKEN
|
||||||
static const Index _x0_=0, _x1_=1, _x2_=2, _x3_=3, _x4_=4, _x_=5, _y_=6, _l1_=7, _l11_=8;
|
static const Index _x0_=0, _x1_=1, _x2_=2, _x3_=3, _x4_=4, _x_=5, _y_=6, _l1_=7, _l11_=8;
|
||||||
|
#endif
|
||||||
|
|
||||||
static SharedDiagonal
|
static SharedDiagonal
|
||||||
sigma0_1 = sharedSigma(2,0.1), sigma_02 = sharedSigma(2,0.2),
|
sigma0_1 = sharedSigma(2,0.1), sigma_02 = sharedSigma(2,0.2),
|
||||||
|
|
|
@ -41,10 +41,10 @@ static Matrix Sigma = Matrix_(3, 3,
|
||||||
var, 0.0, 0.0,
|
var, 0.0, 0.0,
|
||||||
0.0, var, 0.0,
|
0.0, var, 0.0,
|
||||||
0.0, 0.0, var);
|
0.0, 0.0, var);
|
||||||
static Matrix Q = Matrix_(3, 3,
|
//static Matrix Q = Matrix_(3, 3,
|
||||||
prc, 0.0, 0.0,
|
// prc, 0.0, 0.0,
|
||||||
0.0, prc, 0.0,
|
// 0.0, prc, 0.0,
|
||||||
0.0, 0.0, prc);
|
// 0.0, 0.0, prc);
|
||||||
|
|
||||||
static double inf = std::numeric_limits<double>::infinity();
|
static double inf = std::numeric_limits<double>::infinity();
|
||||||
|
|
||||||
|
|
|
@ -18,6 +18,7 @@
|
||||||
|
|
||||||
#include <boost/assign/std/vector.hpp>
|
#include <boost/assign/std/vector.hpp>
|
||||||
|
|
||||||
|
#include <gtsam/base/Testable.h>
|
||||||
#include <gtsam/linear/VectorValues.h>
|
#include <gtsam/linear/VectorValues.h>
|
||||||
#include <gtsam/inference/Permutation.h>
|
#include <gtsam/inference/Permutation.h>
|
||||||
|
|
||||||
|
|
|
@ -22,12 +22,9 @@
|
||||||
#include <map>
|
#include <map>
|
||||||
#include <list>
|
#include <list>
|
||||||
#include <vector>
|
#include <vector>
|
||||||
//#include <boost/serialization/map.hpp>
|
|
||||||
//#include <boost/serialization/list.hpp>
|
|
||||||
#include <stdexcept>
|
#include <stdexcept>
|
||||||
|
|
||||||
#include <gtsam/base/types.h>
|
#include <gtsam/base/types.h>
|
||||||
#include <gtsam/base/Testable.h>
|
|
||||||
#include <gtsam/base/FastSet.h>
|
#include <gtsam/base/FastSet.h>
|
||||||
#include <gtsam/base/FastList.h>
|
#include <gtsam/base/FastList.h>
|
||||||
#include <gtsam/inference/FactorGraph.h>
|
#include <gtsam/inference/FactorGraph.h>
|
||||||
|
|
|
@ -27,8 +27,6 @@
|
||||||
#include <boost/lexical_cast.hpp>
|
#include <boost/lexical_cast.hpp>
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#include <gtsam/base/Testable.h>
|
|
||||||
|
|
||||||
#define ALPHA '\224'
|
#define ALPHA '\224'
|
||||||
|
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
@ -39,7 +37,7 @@ namespace gtsam {
|
||||||
* 2) the character constant used for its string representation
|
* 2) the character constant used for its string representation
|
||||||
*/
|
*/
|
||||||
template<class T, char C>
|
template<class T, char C>
|
||||||
class TypedSymbol: Testable<TypedSymbol<T, C> > {
|
class TypedSymbol {
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
size_t j_;
|
size_t j_;
|
||||||
|
@ -59,6 +57,8 @@ public:
|
||||||
j_(j) {
|
j_(j) {
|
||||||
}
|
}
|
||||||
|
|
||||||
|
virtual ~TypedSymbol() {}
|
||||||
|
|
||||||
// Get stuff:
|
// Get stuff:
|
||||||
|
|
||||||
size_t index() const {
|
size_t index() const {
|
||||||
|
@ -120,7 +120,7 @@ class TypedLabeledSymbol;
|
||||||
* keys when linearizing a nonlinear factor graph. This key is not type
|
* keys when linearizing a nonlinear factor graph. This key is not type
|
||||||
* safe, so cannot be used with any Nonlinear* classes.
|
* safe, so cannot be used with any Nonlinear* classes.
|
||||||
*/
|
*/
|
||||||
class Symbol: Testable<Symbol> {
|
class Symbol {
|
||||||
protected:
|
protected:
|
||||||
unsigned char c_;
|
unsigned char c_;
|
||||||
size_t j_;
|
size_t j_;
|
||||||
|
@ -251,8 +251,7 @@ template<class KEY> std::list<Symbol> keys2symbols(std::list<KEY> keys) {
|
||||||
* index
|
* index
|
||||||
*/
|
*/
|
||||||
template<class T, char C, typename L>
|
template<class T, char C, typename L>
|
||||||
class TypedLabeledSymbol: public TypedSymbol<T, C> , Testable<
|
class TypedLabeledSymbol: public TypedSymbol<T, C> {
|
||||||
TypedLabeledSymbol<T, C, L> > {
|
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
// Label
|
// Label
|
||||||
|
|
|
@ -28,7 +28,6 @@
|
||||||
|
|
||||||
#include <gtsam/base/FastMap.h>
|
#include <gtsam/base/FastMap.h>
|
||||||
#include <gtsam/base/Vector.h>
|
#include <gtsam/base/Vector.h>
|
||||||
#include <gtsam/base/Testable.h>
|
|
||||||
#include <gtsam/nonlinear/Ordering.h>
|
#include <gtsam/nonlinear/Ordering.h>
|
||||||
|
|
||||||
namespace boost { template<class T> class optional; }
|
namespace boost { template<class T> class optional; }
|
||||||
|
@ -47,7 +46,7 @@ namespace gtsam {
|
||||||
* labels (example: Pose2, Point2, etc)
|
* labels (example: Pose2, Point2, etc)
|
||||||
*/
|
*/
|
||||||
template<class J>
|
template<class J>
|
||||||
class LieValues : public Testable<LieValues<J> > {
|
class LieValues {
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
|
|
|
@ -19,7 +19,6 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <map>
|
#include <map>
|
||||||
#include <gtsam/base/Testable.h>
|
|
||||||
#include <gtsam/nonlinear/Key.h>
|
#include <gtsam/nonlinear/Key.h>
|
||||||
#include <gtsam/inference/inference.h>
|
#include <gtsam/inference/inference.h>
|
||||||
|
|
||||||
|
@ -32,7 +31,7 @@ namespace gtsam {
|
||||||
/**
|
/**
|
||||||
* An ordering is a map from symbols (non-typed keys) to integer indices
|
* An ordering is a map from symbols (non-typed keys) to integer indices
|
||||||
*/
|
*/
|
||||||
class Ordering : Testable<Ordering> {
|
class Ordering {
|
||||||
protected:
|
protected:
|
||||||
typedef boost::fast_pool_allocator<std::pair<const Symbol, Index> > Allocator;
|
typedef boost::fast_pool_allocator<std::pair<const Symbol, Index> > Allocator;
|
||||||
typedef std::map<Symbol, Index, std::less<Symbol>, Allocator> Map;
|
typedef std::map<Symbol, Index, std::less<Symbol>, Allocator> Map;
|
||||||
|
@ -204,7 +203,7 @@ private:
|
||||||
* @class Unordered
|
* @class Unordered
|
||||||
* @brief a set of unordered indices
|
* @brief a set of unordered indices
|
||||||
*/
|
*/
|
||||||
class Unordered: public std::set<Index>, public Testable<Unordered> {
|
class Unordered: public std::set<Index> {
|
||||||
public:
|
public:
|
||||||
/** Default constructor creates empty ordering */
|
/** Default constructor creates empty ordering */
|
||||||
Unordered() { }
|
Unordered() { }
|
||||||
|
|
|
@ -47,7 +47,7 @@ namespace gtsam {
|
||||||
* operates on the "first" values. TupleValuesEnd contains only the specialized version.
|
* operates on the "first" values. TupleValuesEnd contains only the specialized version.
|
||||||
*/
|
*/
|
||||||
template<class VALUES1, class VALUES2>
|
template<class VALUES1, class VALUES2>
|
||||||
class TupleValues : public Testable<TupleValues<VALUES1, VALUES2> > {
|
class TupleValues {
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
// Data for internal valuess
|
// Data for internal valuess
|
||||||
|
@ -254,7 +254,7 @@ namespace gtsam {
|
||||||
* of a recursive structure
|
* of a recursive structure
|
||||||
*/
|
*/
|
||||||
template<class VALUES>
|
template<class VALUES>
|
||||||
class TupleValuesEnd : public Testable<TupleValuesEnd<VALUES> > {
|
class TupleValuesEnd {
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
// Data for internal valuess
|
// Data for internal valuess
|
||||||
|
|
|
@ -20,8 +20,9 @@
|
||||||
#include <boost/assign/std/list.hpp> // for operator +=
|
#include <boost/assign/std/list.hpp> // for operator +=
|
||||||
using namespace boost::assign;
|
using namespace boost::assign;
|
||||||
|
|
||||||
#include <gtsam/nonlinear/LieValues-inl.h>
|
#include <gtsam/base/Testable.h>
|
||||||
#include <gtsam/base/LieVector.h>
|
#include <gtsam/base/LieVector.h>
|
||||||
|
#include <gtsam/nonlinear/LieValues-inl.h>
|
||||||
|
|
||||||
using namespace gtsam;
|
using namespace gtsam;
|
||||||
using namespace std;
|
using namespace std;
|
||||||
|
|
|
@ -15,6 +15,7 @@ using namespace boost;
|
||||||
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
|
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
|
||||||
#define GTSAM_MAGIC_KEY
|
#define GTSAM_MAGIC_KEY
|
||||||
|
|
||||||
|
#include <gtsam/base/Testable.h>
|
||||||
#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
|
#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
|
||||||
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
|
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
|
||||||
#include <gtsam/inference/graph-inl.h>
|
#include <gtsam/inference/graph-inl.h>
|
||||||
|
|
|
@ -15,6 +15,7 @@ using namespace boost;
|
||||||
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
|
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
|
||||||
#define GTSAM_MAGIC_KEY
|
#define GTSAM_MAGIC_KEY
|
||||||
|
|
||||||
|
#include <gtsam/base/Testable.h>
|
||||||
#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
|
#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
|
||||||
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
|
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
|
||||||
#include <gtsam/inference/graph-inl.h>
|
#include <gtsam/inference/graph-inl.h>
|
||||||
|
|
|
@ -40,7 +40,8 @@ static noiseModel::Gaussian::shared_ptr covariance(
|
||||||
sx*sx, 0.0, 0.0,
|
sx*sx, 0.0, 0.0,
|
||||||
0.0, sy*sy, 0.0,
|
0.0, sy*sy, 0.0,
|
||||||
0.0, 0.0, st*st
|
0.0, 0.0, st*st
|
||||||
))), I3(noiseModel::Unit::Create(3));
|
)));
|
||||||
|
//static noiseModel::Gaussian::shared_ptr I3(noiseModel::Unit::Create(3));
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
TEST( Pose2Graph, constructor )
|
TEST( Pose2Graph, constructor )
|
||||||
|
|
|
@ -19,6 +19,7 @@
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
#include <CppUnitLite/TestHarness.h>
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
|
||||||
|
#include <gtsam/base/Testable.h>
|
||||||
#include <gtsam/base/numericalDerivative.h>
|
#include <gtsam/base/numericalDerivative.h>
|
||||||
#include <gtsam/slam/simulated2D.h>
|
#include <gtsam/slam/simulated2D.h>
|
||||||
|
|
||||||
|
|
|
@ -19,6 +19,7 @@
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
#include <CppUnitLite/TestHarness.h>
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
|
||||||
|
#include <gtsam/base/Testable.h>
|
||||||
#include <gtsam/base/numericalDerivative.h>
|
#include <gtsam/base/numericalDerivative.h>
|
||||||
#include <gtsam/slam/simulated2D.h>
|
#include <gtsam/slam/simulated2D.h>
|
||||||
#include <gtsam/slam/simulated2DOriented.h>
|
#include <gtsam/slam/simulated2DOriented.h>
|
||||||
|
|
|
@ -18,8 +18,9 @@
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
#include <CppUnitLite/TestHarness.h>
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
|
||||||
#include <gtsam/geometry/Pose3.h>
|
#include <gtsam/base/Testable.h>
|
||||||
#include <gtsam/base/numericalDerivative.h>
|
#include <gtsam/base/numericalDerivative.h>
|
||||||
|
#include <gtsam/geometry/Pose3.h>
|
||||||
#include <gtsam/slam/Simulated3D.h>
|
#include <gtsam/slam/Simulated3D.h>
|
||||||
|
|
||||||
using namespace gtsam;
|
using namespace gtsam;
|
||||||
|
|
|
@ -28,8 +28,9 @@ using namespace boost::assign;
|
||||||
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
|
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
|
||||||
#define GTSAM_MAGIC_KEY
|
#define GTSAM_MAGIC_KEY
|
||||||
|
|
||||||
#include <gtsam/linear/GaussianBayesNet.h>
|
#include <gtsam/base/Testable.h>
|
||||||
#include <gtsam/inference/BayesNet.h>
|
#include <gtsam/inference/BayesNet.h>
|
||||||
|
#include <gtsam/linear/GaussianBayesNet.h>
|
||||||
#include <gtsam/linear/GaussianSequentialSolver.h>
|
#include <gtsam/linear/GaussianSequentialSolver.h>
|
||||||
#include <gtsam/slam/smallExample.h>
|
#include <gtsam/slam/smallExample.h>
|
||||||
|
|
||||||
|
|
|
@ -30,6 +30,7 @@ using namespace boost::assign;
|
||||||
#define GTSAM_MAGIC_KEY
|
#define GTSAM_MAGIC_KEY
|
||||||
|
|
||||||
#include <gtsam/base/Matrix.h>
|
#include <gtsam/base/Matrix.h>
|
||||||
|
#include <gtsam/base/Testable.h>
|
||||||
#include <gtsam/linear/GaussianConditional.h>
|
#include <gtsam/linear/GaussianConditional.h>
|
||||||
#include <gtsam/slam/smallExample.h>
|
#include <gtsam/slam/smallExample.h>
|
||||||
#include <gtsam/nonlinear/Ordering.h>
|
#include <gtsam/nonlinear/Ordering.h>
|
||||||
|
|
|
@ -32,11 +32,12 @@ using namespace boost::assign;
|
||||||
#define GTSAM_MAGIC_KEY
|
#define GTSAM_MAGIC_KEY
|
||||||
|
|
||||||
#include <gtsam/base/Matrix.h>
|
#include <gtsam/base/Matrix.h>
|
||||||
#include <gtsam/slam/smallExample.h>
|
#include <gtsam/base/Testable.h>
|
||||||
#include <gtsam/linear/GaussianBayesNet.h>
|
|
||||||
#include <gtsam/base/numericalDerivative.h>
|
#include <gtsam/base/numericalDerivative.h>
|
||||||
#include <gtsam/inference/SymbolicFactorGraph.h>
|
#include <gtsam/inference/SymbolicFactorGraph.h>
|
||||||
|
#include <gtsam/linear/GaussianBayesNet.h>
|
||||||
#include <gtsam/linear/GaussianSequentialSolver.h>
|
#include <gtsam/linear/GaussianSequentialSolver.h>
|
||||||
|
#include <gtsam/slam/smallExample.h>
|
||||||
|
|
||||||
using namespace gtsam;
|
using namespace gtsam;
|
||||||
using namespace example;
|
using namespace example;
|
||||||
|
|
|
@ -28,6 +28,7 @@
|
||||||
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
|
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
|
||||||
#define GTSAM_MAGIC_KEY
|
#define GTSAM_MAGIC_KEY
|
||||||
|
|
||||||
|
#include <gtsam/base/Testable.h>
|
||||||
#include <gtsam/base/Matrix.h>
|
#include <gtsam/base/Matrix.h>
|
||||||
#include <gtsam/slam/smallExample.h>
|
#include <gtsam/slam/smallExample.h>
|
||||||
#include <gtsam/slam/simulated2D.h>
|
#include <gtsam/slam/simulated2D.h>
|
||||||
|
|
|
@ -30,6 +30,7 @@ using namespace boost::assign;
|
||||||
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
|
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
|
||||||
#define GTSAM_MAGIC_KEY
|
#define GTSAM_MAGIC_KEY
|
||||||
|
|
||||||
|
#include <gtsam/base/Testable.h>
|
||||||
#include <gtsam/base/Matrix.h>
|
#include <gtsam/base/Matrix.h>
|
||||||
#include <gtsam/slam/smallExample.h>
|
#include <gtsam/slam/smallExample.h>
|
||||||
#include <gtsam/inference/FactorGraph-inl.h>
|
#include <gtsam/inference/FactorGraph-inl.h>
|
||||||
|
|
|
@ -24,6 +24,7 @@ using namespace boost::assign;
|
||||||
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
|
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
|
||||||
#define GTSAM_MAGIC_KEY
|
#define GTSAM_MAGIC_KEY
|
||||||
|
|
||||||
|
#include <gtsam/base/Testable.h>
|
||||||
#include <gtsam/slam/smallExample.h>
|
#include <gtsam/slam/smallExample.h>
|
||||||
#include <gtsam/inference/SymbolicFactorGraph.h>
|
#include <gtsam/inference/SymbolicFactorGraph.h>
|
||||||
#include <gtsam/inference/SymbolicSequentialSolver.h>
|
#include <gtsam/inference/SymbolicSequentialSolver.h>
|
||||||
|
|
Loading…
Reference in New Issue