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) {
}
/**
* 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
* 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 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 K shared pointer to the constant calibration
* @param throwCheirality determines whether Cheirality exceptions are rethrown
* @param verboseCheirality determines whether exceptions are printed for Cheirality
* @param calibKey is the index of the intrinsic calibration
* @param throwCheirality determines whether Cheirality exceptions are
* rethrown
* @param verboseCheirality determines whether exceptions are printed for
* Cheirality
*/
ProjectionFactorPPPC(const Point2& measured, const SharedNoiseModel& model,
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),
throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {}
@ -124,8 +115,8 @@ class GTSAM_UNSTABLE_EXPORT ProjectionFactorPPPC
try {
if(H1 || H2 || H3 || H4) {
Matrix H0, H02;
PinholeCamera<CALIBRATION> camera(pose.compose(transform, H0, H02), K);
Point2 reprojectionError(camera.project(point, H1, H3, H4) - measured_);
const PinholeCamera<CALIBRATION> camera(pose.compose(transform, H0, H02), K);
const Point2 reprojectionError(camera.project(point, H1, H3, H4) - measured_);
*H2 = *H1 * H02;
*H1 = *H1 * H0;
return reprojectionError;