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) { 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. // 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) { 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 // 2. the corresponding camera's key
// 3. camera noise model // 3. camera noise model
// 4. camera calibration // 4. camera calibration
smartfactor->add(measurement, i, measurementNoise); smartfactor->add(measurement, i);
} }
// insert the smart factor in the graph // 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) { 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. // 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) { 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]); Point2 measurement = camera.project(points[j]);
// call add() function to add measurement into a single factor, here we need to add: // 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 // insert the smart factor in the graph

12
gtsam.h
View File

@ -2364,15 +2364,17 @@ class SmartProjectionParams {
template<CALIBRATION> template<CALIBRATION>
virtual class SmartProjectionPoseFactor: gtsam::NonlinearFactor { virtual class SmartProjectionPoseFactor: gtsam::NonlinearFactor {
SmartProjectionPoseFactor(const CALIBRATION* K); SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise,
SmartProjectionPoseFactor(const CALIBRATION* K, const CALIBRATION* K);
SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise,
const CALIBRATION* K,
const gtsam::Pose3& body_P_sensor); 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::Pose3& body_P_sensor,
const gtsam::SmartProjectionParams& params); const gtsam::SmartProjectionParams& params);
void add(const gtsam::Point2& measured_i, size_t poseKey_i, void add(const gtsam::Point2& measured_i, size_t poseKey_i);
const gtsam::noiseModel::Base* noise_i);
// enabling serialization functionality // enabling serialization functionality
//void serialize() const; //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 //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 size_t current_l = 3; // hardcoded landmark ID from first measurement
while (factor_file >> i >> l >> uL >> uR >> v >> X >> Y >> Z) { while (factor_file >> i >> l >> uL >> uR >> v >> X >> Y >> Z) {
if(current_l != l) { if(current_l != l) {
graph.push_back(factor); graph.push_back(factor);
factor = SmartFactor::shared_ptr(new SmartFactor(K)); factor = SmartFactor::shared_ptr(new SmartFactor(model, K));
current_l = l; current_l = l;
} }
factor->add(Point2(uL,v), i, model); factor->add(Point2(uL,v), i);
} }
Pose3 firstPose = initial_estimate.at<Pose3>(1); Pose3 firstPose = initial_estimate.at<Pose3>(1);