Go to the documentation of this file.
29 using namespace gtsam;
39 return K.uncalibrate(
p, Dcal, Dp);
70 const Point3 somePoint(1, 2, 3);
72 std::vector<Point3_> pointExpressions = createUnknowns<Point3>(10,
'p', 1);
94 using namespace unary;
101 using namespace unary;
108 using namespace unary;
118 enum {dimension = 3};
121 inline static Class
Identity() {
return Class(0,0,0); }
128 void print(
const string&
s)
const { cout <<
s << *
this << endl;}
150 std::map<Key, int> map;
155 std::vector<Matrix>
H(1);
159 const double norm =
sqrt(3*3 + 4*4 + 5*5);
162 expected << 3.0 / norm, 4.0 / norm, 5.0 / norm;
196 map<Key, int> actual,
expected{{1, 6}, {2, 3}};
204 typedef internal::BinaryExpression<Point3, Pose3, Point3> Binary;
205 size_t expectedTraceSize =
sizeof(Binary::Record);
233 map<Key, int> actual,
expected{{1, 6}, {2, 3}, {3, 5}};
241 typedef internal::BinaryExpression<Point3, Pose3, Point3> Binary1;
244 typedef internal::UnaryExpression<Point2, Point3> Unary;
250 typedef internal::BinaryExpression<Point2, Cal3_S2, Point2> Binary2;
335 const set<Key> expected_keys{
key};
338 map<Key, int> actual_dims, expected_dims {{
key, 3}};
339 expr.
dims(actual_dims);
340 EXPECT(actual_dims == expected_dims)
343 std::map<Key, int> map;
355 std::vector<Matrix>
H(1);
366 const set<Key> expected_keys{
key};
369 map<Key, int> actual_dims, expected_dims {{
key, 3}};
370 sum_.
dims(actual_dims);
371 EXPECT(actual_dims == expected_dims)
374 std::map<Key, int> map;
386 std::vector<Matrix>
H(1);
408 std::vector<Matrix>
H(1);
432 std::vector<Matrix>
H(1);
441 const Double_ sum_ = norm_ + norm_;
450 std::vector<Matrix>
H(1);
461 map<Key, int> actual_dims, expected_dims = {{
key1, 3}, {
key2, 3}};
462 norm_.
dims(actual_dims);
463 EXPECT(actual_dims == expected_dims)
473 std::vector<Matrix>
H(2);
484 map<Key, int> actual_dims, expected_dims {{
key1, 3}, {
key2, 3}};
485 weighted_sum_.
dims(actual_dims);
486 EXPECT(actual_dims == expected_dims)
498 std::vector<Matrix>
H(2);
506 const Vector3 p = Vector3::Random(),
q = Vector3::Random();
511 set<Key> expected_keys = {0, 1};
515 std::vector<Matrix>
H(2);
536 std::vector<Matrix>
H(1);
const Symbol key1('v', 1)
static int runAllTests(TestResult &result)
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 y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange[*:*] noreverse nowriteback set trange[*:*] noreverse nowriteback set urange[*:*] noreverse nowriteback set vrange[*:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
Expression< Point3 > Point3_
Matrix< SCALARB, Dynamic, Dynamic, opt_B > B
T value(const Values &values, std::vector< Matrix > *H=nullptr) const
Return value and optional derivatives, reverse AD version Notes: this is not terribly efficient,...
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
static sharedNode Leaf(Key key, const SymbolicFactorGraph &factors)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
#define EXPECT_LONGS_EQUAL(expected, actual)
Concept check for values that can be used in unit tests.
#define EXPECT(condition)
The most common 5DOF 3D->2D calibration.
Point3_ pointExpression(1)
const set< Key > expected
Point3_ p_cam(x, &Pose3::transformTo, p)
VectorSpace provides both Testable and VectorSpaceTraits.
Expression< Point2 > projection(f, p_cam)
T upAligned(T value, unsigned requiredAlignment=TraceAlignment)
double f2(const Vector2 &x)
Rot3 composeThree(const Rot3 &R1, const Rot3 &R2, const Rot3 &R3, OptionalJacobian< 3, 3 > H1, OptionalJacobian< 3, 3 > H2, OptionalJacobian< 3, 3 > H3)
static const Point3 point2(-0.08, 0.08, 0.0)
Point2 uncalibrate(const CAL &K, const Point2 &p, OptionalJacobian< 2, 5 > Dcal, OptionalJacobian< 2, 2 > Dp)
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
std::set< Key > keys() const
Return keys that play in this expression.
TEST(Expression, Constant)
Common expressions, both linear and non-linear.
const Symbol key2('v', 2)
Line3 transformTo(const Pose3 &wTc, const Line3 &wL, OptionalJacobian< 4, 6 > Dpose, OptionalJacobian< 4, 4 > Dline)
EIGEN_DEVICE_FUNC const Scalar & q
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Expression< Cal3_S2 > K(3)
double doubleF(const Pose3 &pose, const Point3 &point, OptionalJacobian< 1, 6 > H1, OptionalJacobian< 1, 3 > H2)
Base class for all pinhole cameras.
void print(const string &s) const
void insert(Key j, const Vector &value)
double norm(OptionalJacobian< 1, 3 > H={}) const
static const NavState kIdentity
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
void dims(std::map< Key, int > &map) const
Return dimensions for each argument, as a map.
const gtsam::Symbol key('X', 0)
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
double norm3(const Point3 &p, OptionalJacobian< 1, 3 > H)
Distance of the point from the origin, with Jacobian.
Matrix< Scalar, Dynamic, Dynamic > C
double f3(double x1, double x2)
Expression< Pose3 > Pose3_
bool equals(const Class &q, double tol) const
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Expression< Vector3 > Vector3_
T & upAlign(T &value, unsigned requiredAlignment=TraceAlignment)
const Point3 point1(3.0, 4.0, 2.0)
Point2 f1(const Point3 &p, OptionalJacobian< 2, 3 > H)
Expression< Point2 > uv_hat(uncalibrate< Cal3_S2 >, K, projection)
Point2(* Project)(const Point3 &, OptionalJacobian< 2, 3 >)
std::uint64_t Key
Integer nonlinear key type.
const Vector3 & vector() const
#define LONGS_EQUAL(expected, actual)
Jet< T, N > sqrt(const Jet< T, N > &f)
Rot2 R(Rot2::fromAngle(0.1))
Expression< T > linearExpression(const std::function< T(A)> &f, const Expression< A > &expression, const Eigen::Matrix< double, traits< T >::dimension, traits< A >::dimension > &dTdA)
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:39:55