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

Added prior, now no longer segfaults...

release/4.3a0
Frank Dellaert 2013-08-30 16:18:41 +00:00
parent 642e486ba9
commit c9a7796fc3
1 changed files with 7 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
examples/SelfCalibrationExample.cpp
View File

@ -54,11 +54,11 @@ int main(int argc, char* argv[]) {
// Add a prior on pose x1.
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(Vector_(6, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1)); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
graph.push_back(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise)); // add directly to graph
graph.push_back(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise));
// Simulated measurements from each camera pose, adding them to the factor graph
Cal3_S2 K(50.0, 50.0, 0.0, 50.0, 50.0);
noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0);
for (size_t i = 0; i < poses.size(); ++i) {
for (size_t j = 0; j < points.size(); ++j) {
SimpleCamera camera(poses[i], K);
@ -73,8 +73,12 @@ int main(int argc, char* argv[]) {
noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
graph.push_back(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise)); // add directly to graph
// Add a prior on the calibration.
noiseModel::Diagonal::shared_ptr calNoise = noiseModel::Diagonal::Sigmas(Vector_(5, 500, 500, 0.1, 100, 100));
graph.push_back(PriorFactor<Cal3_S2>(Symbol('K', 0), K, calNoise));
// Create the initial estimate to the solution
// Including an estimate on the camera calibration parameters
// now including an estimate on the camera calibration parameters
Values initialEstimate;
initialEstimate.insert(Symbol('K', 0), Cal3_S2(60.0, 60.0, 0.0, 45.0, 45.0));
for (size_t i = 0; i < poses.size(); ++i)