12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #1004 from borglab/fix/projection-factor-pppc-documentation

release/4.3a0
Varun Agrawal 2022-01-03 09:31:51 -05:00 committed by GitHub
parent 3c9ace628a 25c82c9878
commit 6bfad39343
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 11 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
gtsam_unstable/slam/ProjectionFactorPPPC.h
View File

@ -57,34 +57,25 @@ class GTSAM_UNSTABLE_EXPORT ProjectionFactorPPPC
measured_(0.0, 0.0), throwCheirality_(false), verboseCheirality_(false) { measured_(0.0, 0.0), throwCheirality_(false), verboseCheirality_(false) {
} }
/**
* Constructor
* TODO: Mark argument order standard (keys, measurement, parameters)
* @param measured is the 2 dimensional location of point in image (the measurement)
* @param model is the standard deviation
* @param poseKey is the index of the camera
* @param pointKey is the index of the landmark
* @param K shared pointer to the constant calibration
*/
ProjectionFactorPPPC(const Point2& measured, const SharedNoiseModel& model,
Key poseKey, Key transformKey, Key pointKey, Key calibKey) :
Base(model, poseKey, transformKey, pointKey, calibKey), measured_(measured),
throwCheirality_(false), verboseCheirality_(false) {}
/** /**
* Constructor with exception-handling flags * Constructor with exception-handling flags
* TODO: Mark argument order standard (keys, measurement, parameters) * TODO: Mark argument order standard (keys, measurement, parameters)
* @param measured is the 2 dimensional location of point in image (the measurement) * @param measured is the 2 dimensional location of point in image (the
* measurement)
* @param model is the standard deviation * @param model is the standard deviation
* @param poseKey is the index of the camera * @param poseKey is the index of the camera
* @param transformKey is the index of the extrinsic calibration
* @param pointKey is the index of the landmark * @param pointKey is the index of the landmark
* @param K shared pointer to the constant calibration * @param calibKey is the index of the intrinsic calibration
* @param throwCheirality determines whether Cheirality exceptions are rethrown * @param throwCheirality determines whether Cheirality exceptions are
* @param verboseCheirality determines whether exceptions are printed for Cheirality * rethrown
* @param verboseCheirality determines whether exceptions are printed for
* Cheirality
*/ */
ProjectionFactorPPPC(const Point2& measured, const SharedNoiseModel& model, ProjectionFactorPPPC(const Point2& measured, const SharedNoiseModel& model,
Key poseKey, Key transformKey, Key pointKey, Key calibKey, Key poseKey, Key transformKey, Key pointKey, Key calibKey,
bool throwCheirality, bool verboseCheirality) : bool throwCheirality = false, bool verboseCheirality = false) :
Base(model, poseKey, transformKey, pointKey, calibKey), measured_(measured), Base(model, poseKey, transformKey, pointKey, calibKey), measured_(measured),
throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {} throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {}
@ -124,8 +115,8 @@ class GTSAM_UNSTABLE_EXPORT ProjectionFactorPPPC
try { try {
if(H1 || H2 || H3 || H4) { if(H1 || H2 || H3 || H4) {
Matrix H0, H02; Matrix H0, H02;
PinholeCamera<CALIBRATION> camera(pose.compose(transform, H0, H02), K); const PinholeCamera<CALIBRATION> camera(pose.compose(transform, H0, H02), K);
Point2 reprojectionError(camera.project(point, H1, H3, H4) - measured_); const Point2 reprojectionError(camera.project(point, H1, H3, H4) - measured_);
*H2 = *H1 * H02; *H2 = *H1 * H02;
*H1 = *H1 * H0; *H1 = *H1 * H0;
return reprojectionError; return reprojectionError;