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

addMeasurements adds a bunch of measurements at the same time

release/4.3a0
Frank Dellaert 2012-07-04 15:16:03 +00:00
parent 2c4278491f
commit 989c71e9a2
3 changed files with 41 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam.h
View File

@ -1288,10 +1288,14 @@ class Graph {
void addRangeFactor(size_t poseKey, size_t pointKey, double range, const gtsam::noiseModel::Base* model);
// Measurements
void addMeasurement(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model,
size_t poseKey, size_t pointKey, const gtsam::Cal3_S2* K);
void addStereoMeasurement(const gtsam::StereoPoint2& measured, const gtsam::noiseModel::Base* model,
size_t poseKey, size_t pointKey, const gtsam::Cal3_S2Stereo* K);
void addMeasurement(const gtsam::Point2& measured,
const gtsam::noiseModel::Base* model, size_t poseKey, size_t pointKey,
const gtsam::Cal3_S2* K);
void addMeasurements(size_t i, const gtsam::KeyVector& J, Matrix Z,
const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K);
void addStereoMeasurement(const gtsam::StereoPoint2& measured,
const gtsam::noiseModel::Base* model, size_t poseKey, size_t pointKey,
const gtsam::Cal3_S2Stereo* K);
};
class ISAM {

15
gtsam/slam/visualSLAM.cpp
View File

@ -47,7 +47,20 @@ namespace visualSLAM {
/* ************************************************************************* */
void Graph::addMeasurement(const Point2& measured, const SharedNoiseModel& model,
Key poseKey, Key pointKey, const shared_ptrK K) {
push_back(make_shared<GenericProjectionFactor<Pose3, Point3> >(measured, model, poseKey, pointKey, K));
push_back(
make_shared<GenericProjectionFactor<Pose3, Point3> >
(measured, model, poseKey, pointKey, K));
}
/* ************************************************************************* */
void Graph::addMeasurements(Key i, const KeyVector& J, const Matrix& Z,
const SharedNoiseModel& model, const shared_ptrK K) {
if ( Z.rows()!=2) throw std::invalid_argument("addMeasurements: Z must be 2*K");
if (Z.cols() != J.size())
throw std::invalid_argument(
"addMeasurements: J and Z must have same number of entries");
for(size_t k=0;k<Z.cols();k++)
addMeasurement(Point2(Z(0,k),Z(1,k)),model,i, J[k], K);
}
/* ************************************************************************* */

27
gtsam/slam/visualSLAM.h
View File

@ -94,14 +94,14 @@ namespace visualSLAM {
/**
* Add a constraint on a point (for now, *must* be satisfied in any Values)
* @param key variable key of the landmark
* @param key variable key of the point
* @param p point around which soft prior is defined
*/
void addPointConstraint(Key pointKey, const Point3& p = Point3());
/**
* Add a prior on a landmark
* @param key variable key of the landmark
* Add a prior on a point
* @param key variable key of the point
* @param p to which point to constrain it to
* @param model uncertainty model of this prior
*/
@ -109,10 +109,10 @@ namespace visualSLAM {
const SharedNoiseModel& model = noiseModel::Unit::Create(3));
/**
* Add a range prior to a landmark
* Add a range prior to a point
* @param poseKey variable key of the camera pose
* @param pointKey variable key of the landmark
* @param range approximate range to landmark
* @param pointKey variable key of the point
* @param range approximate range to point
* @param model uncertainty model of this prior
*/
void addRangeFactor(Key poseKey, Key pointKey, double range,
@ -123,18 +123,29 @@ namespace visualSLAM {
* @param measured the measurement
* @param model the noise model for the measurement
* @param poseKey variable key for the camera pose
* @param pointKey variable key for the landmark
* @param pointKey variable key for the point
* @param K shared pointer to calibration object
*/
void addMeasurement(const Point2& measured, const SharedNoiseModel& model,
Key poseKey, Key pointKey, const shared_ptrK K);
/**
* Add a number of measurements at the same time
* @param i variable key for the camera pose
* @param J variable keys for the point, KeyVector of size K
* @param Z the 2*K measurements as a matrix
* @param model the noise model for the measurement
* @param K shared pointer to calibration object
*/
void addMeasurements(Key i, const KeyVector& J, const Matrix& Z,
const SharedNoiseModel& model, const shared_ptrK K);
/**
* Add a stereo factor measurement
* @param measured the measurement
* @param model the noise model for the measurement
* @param poseKey variable key for the camera pose
* @param pointKey variable key for the landmark
* @param pointKey variable key for the point
* @param K shared pointer to stereo calibration object
*/
void addStereoMeasurement(const StereoPoint2& measured, const SharedNoiseModel& model,