32 #include <boost/assign/list_of.hpp> 33 using boost::assign::list_of;
36 using namespace gtsam;
121 2, (
Matrix(2, 3) << 1, 0, 0, 0, 1, 0).finished(),
130 boost::shared_ptr<GaussianFactor> gf = f.
linearize(values);
131 boost::shared_ptr<JacobianFactor> jf =
143 if (H) *H << I_3x3, I_3x3, I_3x3;
148 if (H) *H = Matrix9::Identity();
178 typedef internal::BinaryExpression<Point2, Cal3_S2, Point2> Binary;
190 size_t size = binary.traceSize();
194 internal::ExecutionTrace<Point2> trace;
195 Point2 value = binary.traceExecution(values, trace, traceStorage);
201 expected25 << 0, 0, 0, 1, 0, 0, 0, 0, 0, 1;
203 expected22 << 1, 0, 0, 1;
206 boost::optional<Binary::Record*> r = trace.record<Binary::Record>();
222 boost::make_shared<Cal3_S2>());
223 double expected_error = old.
error(values);
245 typedef internal::UnaryExpression<Point2, Point3> Unary;
248 internal::ExecutionTrace<Point2> trace;
255 expected23 << 1, 0, 0, 0, 1, 0;
258 boost::optional<Unary::Record*> r = trace.record<Unary::Record>();
266 boost::shared_ptr<GaussianFactor> gf2 = f2.
linearize(values);
282 double expected_error = old.
error(values);
299 boost::shared_ptr<GaussianFactor> gf = f.
linearize(values);
307 boost::shared_ptr<GaussianFactor> gf2 = f2.
linearize(values);
314 boost::shared_ptr<GaussianFactor> gf3 = f3.
linearize(values);
334 std::vector<Matrix>
H(2);
341 boost::shared_ptr<GaussianFactor> gf = f.
linearize(values);
342 boost::shared_ptr<JacobianFactor> jf =
363 std::vector<Matrix>
H(1);
370 boost::shared_ptr<GaussianFactor> gf = f.
linearize(values);
371 boost::shared_ptr<JacobianFactor> jf =
392 std::vector<Matrix>
H(1);
399 boost::shared_ptr<GaussianFactor> gf = f.
linearize(values);
400 boost::shared_ptr<JacobianFactor> jf =
416 return R1 * (R2 *
R3);
435 std::vector<Matrix>
H(3);
444 boost::shared_ptr<GaussianFactor> gf = f.
linearize(values);
445 boost::shared_ptr<JacobianFactor> jf =
468 const double fd_step = 1
e-5;
469 const double tolerance = 1
e-5;
487 return a * x + b *
y;
492 const double tolerance = 1
e-5;
493 const double fd_step = 1
e-5;
534 static double combine3(
const double&
x,
const double&
y,
const double&
z,
537 if (H1) (*H1) << 1.0;
538 if (H2) (*H2) << 2.0;
539 if (H3) (*H3) << 3.0;
540 return x + 2.0 * y + 3.0 *
z;
544 const double tolerance = 1
e-5;
545 const double fd_step = 1
e-5;
576 auto model = noiseModel::Isotropic::Sigma(3, 1);
583 Matrix3
A =
Vector3(1, 2, 3).asDiagonal();
587 values.
insert<Matrix3>(0, A);
598 Matrix3
A =
Vector3(1, 2, 3).asDiagonal();
602 if (H1) *H1 << b.y(), b.z(), b.x(), 0, 0, 0;
609 auto model = noiseModel::Isotropic::Sigma(3, 1);
623 boost::bind(
f, _1, b, boost::none, boost::none), a),
638 gtsam::Rot3, gtsam::Point3,
639 gtsam::Rot3,gtsam::Point3> {
654 this->
initialize(expression({kR1, kV1, kR2, kV2}));
665 const std::array<gtsam::Key, NARY_EXPRESSION_SIZE> &
keys)
const override {
677 std::cout << s <<
"TestNaryFactor(" 678 << keyFormatter(Factor::keys_[0]) <<
"," 679 << keyFormatter(Factor::keys_[1]) <<
"," 680 << keyFormatter(Factor::keys_[2]) <<
"," 681 << keyFormatter(Factor::keys_[3]) <<
")\n";
683 this->noiseModel_->print(
" noise model: ");
688 double tol = 1
e-9)
const override {
690 return e !=
nullptr && Base::equals(*e,
tol) &&
696 friend class boost::serialization::access;
697 template <
class ARCHIVE>
699 ar &boost::serialization::make_nvp(
701 boost::serialization::base_object<Base>(*
this));
702 ar &BOOST_SERIALIZATION_NVP(measured_);
715 values.
insert(
R(0), Rot3::Ypr(0.1, 0.2, 0.3));
717 values.
insert(
R(1), Rot3::Ypr(0.2, 0.5, 0.2));
721 std::vector<Matrix>
H(4);
Combine(double a, double b)
boost::shared_ptr< GaussianFactor > linearize(const Values &x) const override
double error(const Values &c) const override
static Point2 project(const Pose3 &pose, const Unit3 &pointAtInfinity, const Cal3_S2::shared_ptr &cal)
size_t dim() const override
static const Eigen::MatrixBase< Vector3 >::ConstantReturnType Z_3x1
Vector9 id9(const Vector9 &v, OptionalJacobian< 9, 9 > H)
Concept check for values that can be used in unit tests.
static int runAllTests(TestResult &result)
Factor Graph consisting of non-linear factors.
void insert(Key j, const Value &val)
bool equals(const gtsam::NonlinearFactor &expected, double tol=1e-9) const override
Rot2 R(Rot2::fromAngle(0.1))
static Cal3_S2 K(500, 500, 0.1, 640/2, 480/2)
Vector unwhitenedError(const Values &x, boost::optional< std::vector< Matrix > & > H=boost::none) const override
noiseModel::Isotropic::shared_ptr Model
static double combine3(const double &x, const double &y, const double &z, OptionalJacobian< 1, 1 > H1, OptionalJacobian< 1, 1 > H2, OptionalJacobian< 1, 1 > H3)
double f2(const Vector2 &x)
void print(const std::string &s, const gtsam::KeyFormatter &keyFormatter=gtsam::DefaultKeyFormatter) const override
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
NonlinearFactorGraph graph
Matrix< SCALARB, Dynamic, Dynamic > B
static const KeyFormatter DefaultKeyFormatter
Point2 uncalibrate(const Point2 &p, OptionalJacobian< 2, 5 > Dcal=boost::none, OptionalJacobian< 2, 2 > Dp=boost::none) const
boost::aligned_storage< 1, TraceAlignment >::type ExecutionTraceStorage
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
TEST(ExpressionFactor, Leaf)
Rot3 composeThree(const Rot3 &R1, const Rot3 &R2, const Rot3 &R3, OptionalJacobian< 3, 3 > H1, OptionalJacobian< 3, 3 > H2, OptionalJacobian< 3, 3 > H3)
#define EXPECT_CORRECT_EXPRESSION_JACOBIANS(expression, values, numerical_derivative_step, tolerance)
Check the Jacobians produced by an expression against finite differences.
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
double operator()(const double &x, const double &y, OptionalJacobian< 1, 1 > H1, OptionalJacobian< 1, 1 > H2)
void addExpressionFactor(const SharedNoiseModel &R, const T &z, const Expression< T > &h)
Test harness methods for expressions.
KeysAndDims keysAndDims() const
Eigen::Matrix< double, 9, 3 > Matrix93
gtsam::NonlinearFactor::shared_ptr clone() const override
#define EXPECT(condition)
boost::shared_ptr< This > shared_ptr
Point3_ p_cam(x,&Pose3::transformTo, p)
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
void serialize(ARCHIVE &ar, const unsigned int)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Point3_ rotate(const Rot3_ &x, const Point3_ &p)
Eigen::Matrix< double, 9, 9 > Matrix9
Q R1(Eigen::AngleAxisd(1, Q_z_axis))
Basic bearing factor from 2D measurement.
std::set< Key > keys() const
Return keys that play in this expression.
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Expression< Point2 > uv_hat(uncalibrate< Cal3_S2 >, K, projection)
Matrix< Scalar, Dynamic, Dynamic > C
#define LONGS_EQUAL(expected, actual)
Vector9 wide(const Point3 &p, OptionalJacobian< 9, 3 > H)
noiseModel::Diagonal::shared_ptr SharedDiagonal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
TestNaryFactor(gtsam::Key kR1, gtsam::Key kV1, gtsam::Key kR2, gtsam::Key kV2, const gtsam::SharedNoiseModel &model, const gtsam::Point3 &measured)
#define EXPECT_LONGS_EQUAL(expected, actual)
T traceExecution(const Values &values, internal::ExecutionTrace< T > &trace, void *traceStorage) const
trace execution, very unsafe
Point2 f1(const Point3 &p, OptionalJacobian< 2, 3 > H)
#define EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, numerical_derivative_step, tolerance)
Check the Jacobians produced by a factor against finite differences.
Point2(* Project)(const Point3 &, OptionalJacobian< 2, 3 >)
double f3(double x1, double x2)
size_t traceSize() const
Return size needed for memory buffer in traceExecution.
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
boost::shared_ptr< GaussianFactor > linearize(const Values &x) const override
Line3 transformTo(const Pose3 &wTc, const Line3 &wL, OptionalJacobian< 4, 6 > Dpose, OptionalJacobian< 4, 4 > Dline)
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > Matrix
static Point2 myUncal(const Cal3_S2 &K, const Point2 &p, OptionalJacobian< 2, 5 > Dcal, OptionalJacobian< 2, 2 > Dp)
a general SFM factor with an unknown calibration
gtsam::Expression< gtsam::Point3 > expression(const std::array< gtsam::Key, NARY_EXPRESSION_SIZE > &keys) const override
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
static Point2 project3(const Pose3 &pose, const Point3 &point, const Cal3_S2 &cal)
Values initialize(const NonlinearFactorGraph &graph, bool useOdometricPath)
Q R2(Eigen::AngleAxisd(2, Vector3(0, 1, 0)))
std::uint64_t Key
Integer nonlinear key type.
void Print(const CONTAINER &keys, const string &s, const KeyFormatter &keyFormatter)
noiseModel::Base::shared_ptr SharedNoiseModel
Vector3 f(const Point2 &a, const Vector3 &b, OptionalJacobian< 3, 2 > H1, OptionalJacobian< 3, 3 > H2)
Point2 uncalibrate(const CAL &K, const Point2 &p, OptionalJacobian< 2, 5 > Dcal, OptionalJacobian< 2, 2 > Dp)
Expression< Vector3 > Vector3_