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

Fix examples and Matlab wrapper

release/4.3a0
cbeall3 2015-08-26 13:25:12 -04:00
parent 29ad9478f7
commit 92e210b893
4 changed files with 14 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
examples/SFMExample_SmartFactor.cpp
View File

@ -58,7 +58,7 @@ int main(int argc, char* argv[]) {
for (size_t j = 0; j < points.size(); ++j) {
// every landmark represent a single landmark, we use shared pointer to init the factor, and then insert measurements.
SmartFactor::shared_ptr smartfactor(new SmartFactor(K));
SmartFactor::shared_ptr smartfactor(new SmartFactor(measurementNoise, K));
for (size_t i = 0; i < poses.size(); ++i) {
@ -71,7 +71,7 @@ int main(int argc, char* argv[]) {
// 2. the corresponding camera's key
// 3. camera noise model
// 4. camera calibration
smartfactor->add(measurement, i, measurementNoise);
smartfactor->add(measurement, i);
}
// insert the smart factor in the graph

4
examples/SFMExample_SmartFactorPCG.cpp
View File

@ -54,7 +54,7 @@ int main(int argc, char* argv[]) {
for (size_t j = 0; j < points.size(); ++j) {
// every landmark represent a single landmark, we use shared pointer to init the factor, and then insert measurements.
SmartFactor::shared_ptr smartfactor(new SmartFactor(K));
SmartFactor::shared_ptr smartfactor(new SmartFactor(measurementNoise, K));
for (size_t i = 0; i < poses.size(); ++i) {
@ -63,7 +63,7 @@ int main(int argc, char* argv[]) {
Point2 measurement = camera.project(points[j]);
// call add() function to add measurement into a single factor, here we need to add:
smartfactor->add(measurement, i, measurementNoise);
smartfactor->add(measurement, i);
}
// insert the smart factor in the graph

12
gtsam.h
View File

@ -2364,15 +2364,17 @@ class SmartProjectionParams {
template<CALIBRATION>
virtual class SmartProjectionPoseFactor: gtsam::NonlinearFactor {
SmartProjectionPoseFactor(const CALIBRATION* K);
SmartProjectionPoseFactor(const CALIBRATION* K,
SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise,
const CALIBRATION* K);
SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise,
const CALIBRATION* K,
const gtsam::Pose3& body_P_sensor);
SmartProjectionPoseFactor(const CALIBRATION* K,
SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise,
const CALIBRATION* K,
const gtsam::Pose3& body_P_sensor,
const gtsam::SmartProjectionParams& params);
void add(const gtsam::Point2& measured_i, size_t poseKey_i,
const gtsam::noiseModel::Base* noise_i);
void add(const gtsam::Point2& measured_i, size_t poseKey_i);
// enabling serialization functionality
//void serialize() const;

6
gtsam_unstable/examples/SmartProjectionFactorExample.cpp
View File

@ -87,17 +87,17 @@ int main(int argc, char** argv){
//read stereo measurements and construct smart factors
SmartFactor::shared_ptr factor(new SmartFactor(K));
SmartFactor::shared_ptr factor(new SmartFactor(model, K));
size_t current_l = 3; // hardcoded landmark ID from first measurement
while (factor_file >> i >> l >> uL >> uR >> v >> X >> Y >> Z) {
if(current_l != l) {
graph.push_back(factor);
factor = SmartFactor::shared_ptr(new SmartFactor(K));
factor = SmartFactor::shared_ptr(new SmartFactor(model, K));
current_l = l;
}
factor->add(Point2(uL,v), i, model);
factor->add(Point2(uL,v), i);
}
Pose3 firstPose = initial_estimate.at<Pose3>(1);