Go to the documentation of this file.
16 using namespace gtsam;
23 const double radius = 1.0;
24 for (
int i = 0;
i < 3; ++
i) {
25 double angle =
i * 2.0 *
M_PI / 3.0;
26 double c =
cos(angle),
s =
sin(angle);
37 std::array<SimpleFundamentalMatrix, 3>
F;
38 for (
size_t i = 0;
i < 3; ++
i) {
39 size_t j = (
i + 1) % 3;
42 F[
i] = {
E, 1000.0, 1000.0,
Point2(640 / 2, 480 / 2),
45 return {
F[0],
F[1],
F[2]};
59 std::array<PinholeCamera<Cal3_S2>, 3>
cameras = {
72 EXPECT(assert_equal<Matrix3>(
triplet.Fbc.matrix().transpose(), Fkj));
82 std::array<Point2, 3>
p;
83 for (
size_t i = 0;
i < 3; ++
i) {
88 EdgeKey key01(0, 1), key12(1, 2), key20(2, 0);
90 factor0{key01, key20, {{
p[1],
p[2],
p[0]}}},
114 const size_t numPoints = 5;
115 std::vector<std::array<Point2, 3>> projectedPoints;
118 for (
size_t n = 0;
n < numPoints; ++
n) {
120 std::array<Point2, 3>
p;
121 for (
size_t i = 0;
i < 3; ++
i) {
124 projectedPoints.push_back(
p);
128 EdgeKey key01(0, 1), key12(1, 2), key20(2, 0);
129 std::vector<std::tuple<Point2, Point2, Point2>> tuples;
131 for (
size_t n = 0;
n < numPoints; ++
n) {
132 const auto&
p = projectedPoints[
n];
133 tuples.emplace_back(
p[1],
p[2],
p[0]);
150 EXPECT(assert_equal<Vector>(Vector::Zero(2 * numPoints),
error, 1
e-9));
169 std::array<Point2, 3>
p;
170 for (
size_t i = 0;
i < 3; ++
i) {
189 Matrix H1, H2, H3, H4, H5;
192 EXPECT(H1.rows() == 2 && H1.cols() == 5)
193 EXPECT(H2.rows() == 2 && H2.cols() == 5)
213 std::array<Point2, 3>
p;
214 for (
size_t i = 0;
i < 3; ++
i) {
234 Matrix H1, H2, H3, H4, H5;
237 &H1, &H2, &H3, &H4, &H5);
238 EXPECT(H1.rows() == 2 && H1.cols() == 5)
239 EXPECT(H2.rows() == 2 && H2.cols() == 5)
240 EXPECT(H3.rows() == 2 && H3.cols() == 5)
241 EXPECT(H4.rows() == 2 && H4.cols() == 5)
242 EXPECT(H5.rows() == 2 && H5.cols() == 5)
static int runAllTests(TestResult &result)
#define EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, numerical_derivative_step, tolerance)
Check the Jacobians produced by a factor against finite differences.
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
Expression< T > between(const Expression< T > &t1, const Expression< T > &t2)
std::array< PinholeCamera< Cal3_S2 >, 3 > cameras
Array< double, 1, 3 > e(1./3., 0.5, 2.)
#define EXPECT_LONGS_EQUAL(expected, actual)
Jet< T, N > sin(const Jet< T, N > &f)
#define EXPECT(condition)
The most common 5DOF 3D->2D calibration.
std::array< Pose3, 3 > generateCameraPoses()
Generate three cameras on a circle, looking inwards.
TripleF< SimpleFundamentalMatrix > generateTripleF(const std::array< Pose3, 3 > &cameraPoses)
Transfers points between views using essential matrices, optimizes for calibrations of the views,...
const Cal3_S2 cal(focalLength, focalLength, 0.0, principalPoint.x(), principalPoint.y())
Jet< T, N > cos(const Jet< T, N > &f)
Transfers points between views using essential matrices with a shared calibration.
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
const Point3 & translation(OptionalJacobian< 3, 6 > Hself={}) const
get translation
Evaluate derivatives of a nonlinear factor numerically.
Point2 principalPoint(640/2, 480/2)
void insert(Key j, const Vector &value)
static const std::shared_ptr< Cal3_S2 > sharedCal
std::array< Pose3, 3 > cameraPoses
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself={}) const
get rotation
TEST(SmartFactorBase, Pinhole)
static const Eigen::MatrixBase< Vector2 >::ConstantReturnType Z_2x1
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
The most common 5DOF 3D->2D calibration.
A simple camera class with a Cal3_S2 calibration.
Represents a 3D point on a unit sphere.
std::pair< Matrix3, Matrix3 > getMatrices(const F &F1, const F &F2) const
Create Matrix3 objects based on EdgeKey configurations.
static GTSAM_EXPORT EssentialMatrix FromPose3(const Pose3 &_1P2_, OptionalJacobian< 5, 6 > H={})
Named constructor converting a Pose3 with scale to EssentialMatrix (no scale)
gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:08:47