Go to the documentation of this file.
30 using namespace std::placeholders;
33 using namespace gtsam;
68 std::shared_ptr<GaussianFactor> gf2 =
f.linearize(
values);
88 std::shared_ptr<GaussianFactor> gf2 =
f.linearize(
values);
107 std::shared_ptr<GaussianFactor> gf2 =
f.linearize(
values);
120 2, (
Matrix(2, 3) << 1, 0, 0, 0, 1, 0).finished(),
129 std::shared_ptr<GaussianFactor> gf =
f.linearize(
values);
130 std::shared_ptr<JacobianFactor> jf =
131 std::dynamic_pointer_cast<JacobianFactor>(gf);
142 if (
H) *
H << I_3x3, I_3x3, I_3x3;
148 if (
H) *
H = Matrix9::Identity();
173 return K.uncalibrate(
p, Dcal, Dp);
179 typedef internal::BinaryExpression<Point2, Cal3_S2, Point2> Binary;
192 internal::ExecutionTrace<Point2> trace;
199 expected25 << 0, 0, 0, 1, 0, 0, 0, 0, 0, 1;
201 expected22 << 1, 0, 0, 1;
204 std::optional<Binary::Record*> r = trace.record<Binary::Record>();
221 std::make_shared<Cal3_S2>());
242 typedef internal::UnaryExpression<Point2, Point3> Unary;
244 internal::ExecutionTrace<Point2> trace;
251 expected23 << 1, 0, 0, 0, 1, 0;
254 std::optional<Unary::Record*> r = trace.record<Unary::Record>();
262 std::shared_ptr<GaussianFactor> gf2 =
f2.linearize(
values);
295 std::shared_ptr<GaussianFactor> gf =
f.linearize(
values);
303 std::shared_ptr<GaussianFactor> gf2 =
f2.linearize(
values);
310 std::shared_ptr<GaussianFactor> gf3 =
f3.linearize(
values);
330 std::vector<Matrix>
H(2);
337 std::shared_ptr<GaussianFactor> gf =
f.linearize(
values);
338 std::shared_ptr<JacobianFactor> jf =
339 std::dynamic_pointer_cast<JacobianFactor>(gf);
359 std::vector<Matrix>
H(1);
366 std::shared_ptr<GaussianFactor> gf =
f.linearize(
values);
367 std::shared_ptr<JacobianFactor> jf =
368 std::dynamic_pointer_cast<JacobianFactor>(gf);
388 std::vector<Matrix>
H(1);
395 std::shared_ptr<GaussianFactor> gf =
f.linearize(
values);
396 std::shared_ptr<JacobianFactor> jf =
397 std::dynamic_pointer_cast<JacobianFactor>(gf);
431 std::vector<Matrix>
H(3);
440 std::shared_ptr<GaussianFactor> gf =
f.linearize(
values);
441 std::shared_ptr<JacobianFactor> jf =
442 std::dynamic_pointer_cast<JacobianFactor>(gf);
464 const double fd_step = 1
e-5;
465 const double tolerance = 1
e-5;
483 return a *
x +
b *
y;
488 const double tolerance = 1
e-5;
489 const double fd_step = 1
e-5;
530 static double combine3(
const double&
x,
const double&
y,
const double&
z,
533 if (H1) (*H1) << 1.0;
534 if (H2) (*H2) << 2.0;
535 if (H3) (*H3) << 3.0;
536 return x + 2.0 *
y + 3.0 *
z;
540 const double tolerance = 1
e-5;
541 const double fd_step = 1
e-5;
557 EXPECT((sum2_.
keys() == std::set<Key>{1, 2, 3}))
561 EXPECT((sum3_.
keys() == std::set<Key>{1, 2, 3}))
565 EXPECT((sum4_.
keys() == std::set<Key>{1, 2, 3}))
572 auto model = noiseModel::Isotropic::Sigma(3, 1);
579 Matrix3
A =
Vector3(1, 2, 3).asDiagonal();
594 Matrix3
A =
Vector3(1, 2, 3).asDiagonal();
598 if (H1) *H1 <<
b.y(),
b.z(),
b.x(), 0, 0, 0;
605 auto model = noiseModel::Isotropic::Sigma(3, 1);
619 numericalDerivative11<Vector3, Point2>(
620 [&](
const Point2&
a) {
return f(
a,
b, {}, {}); },
a),
635 gtsam::Rot3, gtsam::Point3,
636 gtsam::Rot3, gtsam::Point3> {
650 this->
initialize(expression({kR1, kV1, kR2, kV2}));
655 return std::static_pointer_cast<gtsam::NonlinearFactor>(
661 const std::array<gtsam::Key, NARY_EXPRESSION_SIZE> &
keys)
const override {
673 std::cout <<
s <<
"TestNaryFactor("
674 << keyFormatter(Factor::keys_[0]) <<
","
675 << keyFormatter(Factor::keys_[1]) <<
","
676 << keyFormatter(Factor::keys_[2]) <<
","
677 << keyFormatter(Factor::keys_[3]) <<
")\n";
679 this->noiseModel_->print(
" noise model: ");
684 double tol = 1
e-9)
const override {
686 return e !=
nullptr && Base::equals(*
e,
tol) &&
691 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
693 friend class boost::serialization::access;
694 template <
class ARCHIVE>
695 void serialize(ARCHIVE &ar,
const unsigned int ) {
696 ar &boost::serialization::make_nvp(
698 boost::serialization::base_object<Base>(*
this));
699 ar &BOOST_SERIALIZATION_NVP(measured_);
719 std::vector<Matrix>
H(4);
728 auto model = noiseModel::Isotropic::Sigma(3, 1);
742 auto model = noiseModel::Isotropic::Sigma(3, 1);
758 auto model = noiseModel::Isotropic::Sigma(1, 1);
static int runAllTests(TestResult &result)
KeysAndDims keysAndDims() const
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
std::shared_ptr< This > shared_ptr
Vector9 id9(const Vector9 &v, OptionalJacobian< 9, 9 > H)
Rot3 composeThree(const Rot3 &R1, const Rot3 &R2, const Rot3 &R3, OptionalJacobian< 3, 3 > H1, OptionalJacobian< 3, 3 > H2, OptionalJacobian< 3, 3 > H3)
Reprojection of a LANDMARK to a 2D point.
Matrix< SCALARB, Dynamic, Dynamic, opt_B > B
#define EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, numerical_derivative_step, tolerance)
Check the Jacobians produced by a factor against finite differences.
static sharedNode Leaf(Key key, const SymbolicFactorGraph &factors)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
#define EXPECT_LONGS_EQUAL(expected, actual)
double error(const Values &c) const override
Concept check for values that can be used in unit tests.
#define EXPECT(condition)
static const Eigen::MatrixBase< Vector3 >::ConstantReturnType Z_3x1
noiseModel::Isotropic::shared_ptr Model
Point3_ p_cam(x, &Pose3::transformTo, p)
Eigen::Matrix< double, 9, 9 > Matrix9
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
Combine(double a, double b)
Vector9 wide(const Point3 &p, OptionalJacobian< 9, 3 > H)
double f2(const Vector2 &x)
a general SFM factor with an unknown calibration
double operator()(const double &x, const double &y, OptionalJacobian< 1, 1 > H1, OptionalJacobian< 1, 1 > H2)
static Point2 myUncal(const Cal3_S2 &K, const Point2 &p, OptionalJacobian< 2, 5 > Dcal, OptionalJacobian< 2, 2 > Dp)
Point2 uncalibrate(const CAL &K, const Point2 &p, OptionalJacobian< 2, 5 > Dcal, OptionalJacobian< 2, 2 > Dp)
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
Scalar EIGEN_BLAS_FUNC() dot(int *n, RealScalar *px, int *incx, RealScalar *py, int *incy)
std::shared_ptr< GaussianFactor > linearize(const Values &x) const override
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > Matrix
std::set< Key > keys() const
Return keys that play in this expression.
static void normalize(Signature::Row &row)
Point3_ rotate(const Rot3_ &x, const Point3_ &p)
static Point2 project3(const Pose3 &pose, const Point3 &point, const Cal3_S2 &cal)
Point3 cross(const Point3 &p, const Point3 &q, OptionalJacobian< 3, 3 > H1, OptionalJacobian< 3, 3 > H2)
cross product
Line3 transformTo(const Pose3 &wTc, const Line3 &wL, OptionalJacobian< 4, 6 > Dpose, OptionalJacobian< 4, 4 > Dline)
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
std::unique_ptr< internal::ExecutionTraceStorage[]> allocAligned(size_t size)
void print(const std::string &s, const gtsam::KeyFormatter &keyFormatter=gtsam::DefaultKeyFormatter) const override
std::shared_ptr< This > shared_ptr
shared_ptr to this class
noiseModel::Diagonal::shared_ptr SharedDiagonal
bool equals(const gtsam::NonlinearFactor &expected, double tol=1e-9) const override
gtsam::Expression< gtsam::Point3 > expression(const std::array< gtsam::Key, NARY_EXPRESSION_SIZE > &keys) const override
gtsam::NonlinearFactor::shared_ptr clone() const override
noiseModel::Base::shared_ptr SharedNoiseModel
TestNaryFactor(gtsam::Key kR1, gtsam::Key kV1, gtsam::Key kR2, gtsam::Key kV2, const gtsam::SharedNoiseModel &model, const gtsam::Point3 &measured)
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Test harness methods for expressions.
Vector3 f(const Point2 &a, const Vector3 &b, OptionalJacobian< 3, 2 > H1, OptionalJacobian< 3, 3 > H2)
Matrix< Scalar, Dynamic, Dynamic > C
static Point2 project(const Pose3 &pose, const Unit3 &pointAtInfinity, const Cal3_S2::shared_ptr &cal)
Factor Graph consisting of non-linear factors.
double f3(double x1, double x2)
Common expressions for solving geometry/slam/sfm problems.
size_t traceSize() const
Return size needed for memory buffer in traceExecution.
void insert(Key j, const Value &val)
void Print(const CONTAINER &keys, const string &s, const KeyFormatter &keyFormatter)
Array< int, Dynamic, 1 > v
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
static double combine3(const double &x, const double &y, const double &z, OptionalJacobian< 1, 1 > H1, OptionalJacobian< 1, 1 > H2, OptionalJacobian< 1, 1 > H3)
The most common 5DOF 3D->2D calibration.
T traceExecution(const Values &values, internal::ExecutionTrace< T > &trace, char *traceStorage) const
trace execution, very unsafe
Expression< Vector3 > Vector3_
Expression< Point2 > uv_hat(uncalibrate< Cal3_S2 >, K, projection)
#define EXPECT_CORRECT_EXPRESSION_JACOBIANS(expression, values, numerical_derivative_step, tolerance)
Check the Jacobians produced by an expression against finite differences.
Point2(* Project)(const Point3 &, OptionalJacobian< 2, 3 >)
TEST(ExpressionFactor, Leaf)
NonlinearFactorGraph graph
std::uint64_t Key
Integer nonlinear key type.
#define LONGS_EQUAL(expected, actual)
Jet< T, N > sqrt(const Jet< T, N > &f)
Values initialize(const NonlinearFactorGraph &graph, bool useOdometricPath)
Eigen::Matrix< double, 9, 3 > Matrix93
Rot2 R(Rot2::fromAngle(0.1))
void addExpressionFactor(const SharedNoiseModel &R, const T &z, const Expression< T > &h)
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:39:57