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

added more comments here and there

release/4.3a0
lcarlone 2021-10-04 20:29:40 -04:00
parent 4a88574f67
commit 222380ce48
1 changed files with 19 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
gtsam/slam/SmartProjectionRigFactor.h
View File

@ -36,10 +36,10 @@ namespace gtsam {
*/ */
/** /**
* This factor assumes that camera calibration is fixed (but each camera * This factor assumes that camera calibration is fixed (but each measurement
* measurement can have a different extrinsic and intrinsic calibration). * can be taken by a different camera in the rig, hence can have a different
* The factor only constrains poses (variable dimension is 6 for each pose). * extrinsic and intrinsic calibration). The factor only constrains poses (variable dimension
* This factor requires that values contains the involved poses (Pose3). * is 6 for each pose). This factor requires that values contains the involved poses (Pose3).
* If all measurements share the same calibration (i.e., are from the same camera), use SmartProjectionPoseFactor instead! * If all measurements share the same calibration (i.e., are from the same camera), use SmartProjectionPoseFactor instead!
* If the calibration should be optimized, as well, use SmartProjectionFactor instead! * If the calibration should be optimized, as well, use SmartProjectionFactor instead!
* @addtogroup SLAM * @addtogroup SLAM
@ -53,7 +53,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
typedef typename CAMERA::CalibrationType CALIBRATION; typedef typename CAMERA::CalibrationType CALIBRATION;
static const int DimPose = 6; ///< Pose3 dimension static const int DimPose = 6; ///< Pose3 dimension
static const int ZDim = 2; ///< Measurement dimension (Point2) static const int ZDim = 2; ///< Measurement dimension
protected: protected:
@ -63,7 +63,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
/// cameras in the rig (fixed poses wrt body + fixed intrinsics, for each camera) /// cameras in the rig (fixed poses wrt body + fixed intrinsics, for each camera)
typename Base::Cameras cameraRig_; typename Base::Cameras cameraRig_;
/// vector of camera Ids (one for each observation), identifying which camera took the measurement /// vector of camera Ids (one for each observation, in the same order), identifying which camera took the measurement
FastVector<size_t> cameraIds_; FastVector<size_t> cameraIds_;
public: public:
@ -84,10 +84,11 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
* @param params parameters for the smart projection factors * @param params parameters for the smart projection factors
*/ */
SmartProjectionRigFactor(const SharedNoiseModel& sharedNoiseModel, SmartProjectionRigFactor(const SharedNoiseModel& sharedNoiseModel,
const Cameras& cameraRig, const Cameras& cameraRig,
const SmartProjectionParams& params = const SmartProjectionParams& params =
SmartProjectionParams()) SmartProjectionParams())
: Base(sharedNoiseModel, params), cameraRig_(cameraRig) { : Base(sharedNoiseModel, params),
cameraRig_(cameraRig) {
// use only configuration that works with this factor // use only configuration that works with this factor
Base::params_.degeneracyMode = gtsam::ZERO_ON_DEGENERACY; Base::params_.degeneracyMode = gtsam::ZERO_ON_DEGENERACY;
Base::params_.linearizationMode = gtsam::HESSIAN; Base::params_.linearizationMode = gtsam::HESSIAN;
@ -123,7 +124,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
* @param measurements vector of the 2m dimensional location of the projection * @param measurements vector of the 2m dimensional location of the projection
* of a single landmark in the m views (the measurements) * of a single landmark in the m views (the measurements)
* @param poseKeys keys corresponding to the body poses of the cameras taking the measurements * @param poseKeys keys corresponding to the body poses of the cameras taking the measurements
* @param cameraIds IDs of the cameras in the rig taking each measurement (same order as measurements) * @param cameraIds IDs of the cameras in the rig taking each measurement (same order as the measurements)
*/ */
void add(const Point2Vector& measurements, const KeyVector& poseKeys, void add(const Point2Vector& measurements, const KeyVector& poseKeys,
const FastVector<size_t>& cameraIds) { const FastVector<size_t>& cameraIds) {
@ -185,12 +186,14 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
*/ */
typename Base::Cameras cameras(const Values& values) const override { typename Base::Cameras cameras(const Values& values) const override {
typename Base::Cameras cameras; typename Base::Cameras cameras;
cameras.reserve(nonUniqueKeys_.size()); // preallocate cameras.reserve(nonUniqueKeys_.size()); // preallocate
for (size_t i = 0; i < nonUniqueKeys_.size(); i++) { for (size_t i = 0; i < nonUniqueKeys_.size(); i++) {
const Pose3 world_P_sensor_i = values.at<Pose3>(nonUniqueKeys_[i]) const Pose3 world_P_sensor_i = values.at<Pose3>(nonUniqueKeys_[i]) // = world_P_body
* cameraRig_[ cameraIds_[i] ].pose(); // cameraRig_[ cameraIds_[i] ].pose() is body_P_cam_i * cameraRig_[cameraIds_[i]].pose(); // = body_P_cam_i
cameras.emplace_back(world_P_sensor_i, cameras.emplace_back(
make_shared<typename CAMERA::CalibrationType>(cameraRig_[ cameraIds_[i] ].calibration())); world_P_sensor_i,
make_shared<typename CAMERA::CalibrationType>(
cameraRig_[cameraIds_[i]].calibration()));
} }
return cameras; return cameras;
} }