30 #include <type_traits> 33 using namespace gtsam;
35 static double inf = std::numeric_limits<double>::infinity();
58 void print(
const std::string&
str =
"")
const {}
60 size_t dim()
const {
return 0; }
190 Matrix23
M1 = Matrix23::Zero(),
M2 = Matrix23::Zero();
204 for (
const auto& [
key,
value] : values) {
288 poseconfig.insert(
key2,
Pose2(0.3, 0.4, 0.5));
291 CHECK(poseconfig.equals(poseconfig));
323 KeyVector::const_iterator itAct = actual.begin(), itExp =
expected.begin();
324 for (; itAct != actual.end() && itExp !=
expected.end(); ++itAct, ++itExp) {
394 auto extracted_pose3s = values.
extract<
Pose3>(Symbol::ChrTest(
'y'));
509 Vector v(3); v << 5.0, 6.0, 7.0;
520 Vector v(3); v << 5.0, 6.0, 7.0;
557 Matrix v(1,3); v << 5.0, 6.0, 7.0;
566 Matrix13
v; v << 5.0, 6.0, 7.0;
568 string expected =
"Values with 1 values:\nValue v1: (Eigen::Matrix<double, 1, 3, 1, 1, 3>)\n[\n 5, 6, 7\n]\n\n";
575 const Pose2 poseA(1.0, 2.0, 0.3), poseC(.0, .0, .0);
const gtsam::Symbol key('X', 0)
Provides additional testing facilities for common data structures.
void print(const std::string &str="") const
Both ManifoldTraits and Testable.
const Symbol key3('v', 3)
Concept check for values that can be used in unit tests.
static int runAllTests(TestResult &result)
A non-templated config holding any types of Manifold-group elements.
bool assert_print_equal(const std::string &expected, const V &actual, const std::string &s="")
const ValueType at(Key j) const
#define DOUBLES_EQUAL(expected, actual, threshold)
std::map< Key, ValueType > extract(const std::function< bool(Key)> &filterFcn=&_truePredicate< Key >) const
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
void update(Key j, const Value &val)
TestValues(const TestValue &value1, const TestValue &value2)
VectorValues localCoordinates(const Values &cp) const
#define CHECK_EXCEPTION(condition, exception_name)
bool equals(const TestValue &other, double tol=1e-9) const
Values retract(const VectorValues &delta) const
const Symbol key1('v', 1)
M1<< 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12;Map< MatrixXf > M2(M1.data(), 6, 2)
#define EXPECT(condition)
static Pose3 pose3(rt3, pt3)
const Symbol key4('v', 4)
Array< int, Dynamic, 1 > v
static int DestructorCount
void retractMasked(const VectorValues &delta, const KeySet &mask)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
bool equals(const Values &other, double tol=1e-9) const
static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5))
#define LONGS_EQUAL(expected, actual)
GenericValue< T > genericValue(const T &v)
TestValueData(const TestValueData &other)
std::vector< float > Values
void insert_or_assign(Key j, const Value &val)
If key j exists, update value, else perform an insert.
Vector localCoordinates(const TestValue &, OptionalJacobian< dimension, dimension > H1={}, OptionalJacobian< dimension, dimension > H2={}) const
#define EXPECT_LONGS_EQUAL(expected, actual)
TestValue retract(const Vector &, OptionalJacobian< dimension, dimension > H1={}, OptionalJacobian< dimension, dimension > H2={}) const
void insert(Key j, const Value &val)
static int ConstructorCount
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
bool equal(const T &obj1, const T &obj2, double tol)
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
std::uint64_t Key
Integer nonlinear key type.
const Symbol key2('v', 2)