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

Minor fixes and documentation cleanup

release/4.3a0
cbeall3 2014-05-30 13:31:35 -04:00
parent 625b7b0743
commit a7a209ce0c
2 changed files with 23 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam_unstable/slam/SmartProjectionFactor.h
View File

@ -54,7 +54,7 @@ public:
double f;
};
enum linearizationType {
enum LinearizationMode {
HESSIAN, JACOBIAN_SVD, JACOBIAN_Q
};

30
gtsam_unstable/slam/SmartProjectionPoseFactor.h
View File

@ -41,9 +41,8 @@ template<class POSE, class LANDMARK, class CALIBRATION>
class SmartProjectionPoseFactor: public SmartProjectionFactor<POSE, LANDMARK, CALIBRATION, 6> {
protected:
linearizationType linearizeTo_;
LinearizationMode linearizeTo_; ///< How to linearize the factor (HESSIAN, JACOBIAN_SVD, JACOBIAN_Q)
// Known calibration
std::vector<boost::shared_ptr<CALIBRATION> > K_all_; ///< shared pointer to calibration object (one for each camera)
public:
@ -69,7 +68,7 @@ public:
SmartProjectionPoseFactor(const double rankTol = 1,
const double linThreshold = -1, const bool manageDegeneracy = false,
const bool enableEPI = false, boost::optional<POSE> body_P_sensor = boost::none,
linearizationType linearizeTo = HESSIAN, double landmarkDistanceThreshold = 1e10,
LinearizationMode linearizeTo = HESSIAN, double landmarkDistanceThreshold = 1e10,
double dynamicOutlierRejectionThreshold = -1) :
Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor,
landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), linearizeTo_(linearizeTo) {}
@ -80,7 +79,7 @@ public:
/**
* add a new measurement and pose key
* @param measured is the 2m dimensional location of the projection of a single landmark in the m view (the measurement)
* @param poseKey is the index corresponding to the camera observing the same landmark
* @param poseKey is key corresponding to the camera observing the same landmark
* @param noise_i is the measurement noise
* @param K_i is the (known) camera calibration
*/
@ -92,8 +91,11 @@ public:
}
/**
* add a new measurements and pose keys
* Variant of the previous one in which we include a set of measurements
* @param measurements vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement)
* @param poseKeys vector of keys corresponding to the camera observing the same landmark
* @param noises vector of measurement noises
* @param Ks vector of calibration objects
*/
void add(std::vector<Point2> measurements, std::vector<Key> poseKeys,
std::vector<SharedNoiseModel> noises,
@ -105,8 +107,11 @@ public:
}
/**
* add a new measurements and pose keys
* Variant of the previous one in which we include a set of measurements with the same noise and calibration
* @param mmeasurements vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement)
* @param poseKeys vector of keys corresponding to the camera observing the same landmark
* @param noise measurement noise (same for all measurements)
* @param K the (known) camera calibration (same for all measurements)
*/
void add(std::vector<Point2> measurements, std::vector<Key> poseKeys,
const SharedNoiseModel noise, const boost::shared_ptr<CALIBRATION> K) {
@ -141,7 +146,12 @@ public:
return 6 * this->keys_.size();
}
// Collect all cameras
/**
* Collect all cameras involved in this factor
* @param values Values structure which must contain camera poses corresponding
* to keys involved in this factor
* @return vector of Values
*/
typename Base::Cameras cameras(const Values& values) const {
typename Base::Cameras cameras;
size_t i=0;
@ -154,7 +164,9 @@ public:
}
/**
* linear factor on the poses
* Linearize to Gaussian Factor
* @param values Values structure which must contain camera poses for this factor
* @return
*/
virtual boost::shared_ptr<GaussianFactor> linearize(
const Values& values) const {
@ -184,7 +196,7 @@ public:
}
/** return the calibration object */
inline const boost::shared_ptr<CALIBRATION> calibration() const {
inline const std::vector<boost::shared_ptr<CALIBRATION> > calibration() const {
return K_all_;
}