diff --git a/examples/SelfCalibrationExample.cpp b/examples/SelfCalibrationExample.cpp index afd63beba..58f59b7e2 100644 --- a/examples/SelfCalibrationExample.cpp +++ b/examples/SelfCalibrationExample.cpp @@ -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(Symbol('x', 0), poses[0], poseNoise)); // add directly to graph + graph.push_back(PriorFactor(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(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(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)