ProjectionFactorRollingShutter.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
19 
20 namespace gtsam {
21 
23  const Pose3& pose_a, const Pose3& pose_b, const Point3& point,
25  OptionalMatrixType H3) const {
26  try {
27  Pose3 pose = interpolate<Pose3>(pose_a, pose_b, alpha_, H1, H2);
28  gtsam::Matrix Hprj;
29  if (body_P_sensor_) {
30  if (H1 || H2 || H3) {
31  gtsam::Matrix HbodySensor;
33  pose.compose(*body_P_sensor_, HbodySensor), *K_);
34  Point2 reprojectionError(camera.project(point, Hprj, H3, {}) -
35  measured_);
36  if (H1) *H1 = Hprj * HbodySensor * (*H1);
37  if (H2) *H2 = Hprj * HbodySensor * (*H2);
38  return reprojectionError;
39  } else {
41  return camera.project(point) - measured_;
42  }
43  } else {
45  Point2 reprojectionError(camera.project(point, Hprj, H3, {}) -
46  measured_);
47  if (H1) *H1 = Hprj * (*H1);
48  if (H2) *H2 = Hprj * (*H2);
49  return reprojectionError;
50  }
51  } catch (CheiralityException& e) {
52  if (H1) *H1 = Matrix::Zero(2, 6);
53  if (H2) *H2 = Matrix::Zero(2, 6);
54  if (H3) *H3 = Matrix::Zero(2, 3);
56  std::cout << e.what() << ": Landmark "
57  << DefaultKeyFormatter(this->key<2>()) << " moved behind camera "
58  << DefaultKeyFormatter(this->key<1>()) << std::endl;
59  if (throwCheirality_) throw CheiralityException(this->key<2>());
60  }
61  return Vector2::Constant(2.0 * K_->fx());
62 }
63 
64 } // namespace gtsam
gtsam::ProjectionFactorRollingShutter::measured_
Point2 measured_
2D measurement
Definition: ProjectionFactorRollingShutter.h:47
pose
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::ProjectionFactorRollingShutter::alpha_
double alpha_
Definition: ProjectionFactorRollingShutter.h:48
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::DefaultKeyFormatter
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Definition: Key.cpp:30
gtsam::CheiralityException
Definition: CalibratedCamera.h:34
gtsam::ProjectionFactorRollingShutter::body_P_sensor_
std::optional< Pose3 > body_P_sensor_
The pose of the sensor in the body frame.
Definition: ProjectionFactorRollingShutter.h:52
gtsam_unstable.tests.test_ProjectionFactorRollingShutter.point
point
Definition: test_ProjectionFactorRollingShutter.py:25
gtsam::ProjectionFactorRollingShutter::K_
std::shared_ptr< Cal3_S2 > K_
shared pointer to calibration object
Definition: ProjectionFactorRollingShutter.h:50
gtsam::Pose3
Definition: Pose3.h:37
gtsam::PinholeCamera
Definition: PinholeCamera.h:33
gtsam::ProjectionFactorRollingShutter::verboseCheirality_
bool verboseCheirality_
Definition: ProjectionFactorRollingShutter.h:57
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam
traits
Definition: SFMdata.h:40
ProjectionFactorRollingShutter.h
Basic projection factor for rolling shutter cameras.
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
gtsam::OptionalMatrixType
Matrix * OptionalMatrixType
Definition: NonlinearFactor.h:55
camera
static const CalibratedCamera camera(kDefaultPose)
gtsam::ProjectionFactorRollingShutter::evaluateError
Vector evaluateError(const Pose3 &pose_a, const Pose3 &pose_b, const Point3 &point, OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3) const override
Evaluate error h(x)-z and optionally derivatives.
Definition: ProjectionFactorRollingShutter.cpp:22
gtsam::ProjectionFactorRollingShutter::throwCheirality_
bool throwCheirality_
Definition: ProjectionFactorRollingShutter.h:55


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:03:53