int main(int argc, char* argv[]) { // parse options and read BAL file SfM_data db = preamble(argc, argv); // Build graph using conventional GeneralSFMFactor NonlinearFactorGraph graph; for (size_t j = 0; j < db.number_tracks(); j++) { BOOST_FOREACH (const SfM_Measurement& m, db.tracks[j].measurements) { size_t i = m.first; Point2 z = m.second; Pose3_ camTnav_(C(i)); Cal3Bundler_ calibration_(K(i)); Point3_ nav_point_(P(j)); graph.addExpressionFactor( gNoiseModel, z, uncalibrate(calibration_, // now using transform_from !!!: project(transform_from(camTnav_, nav_point_)))); } } Values initial; size_t i = 0, j = 0; BOOST_FOREACH (const SfM_Camera& camera, db.cameras) { initial.insert(C(i), camera.pose().inverse()); // inverse !!! initial.insert(K(i), camera.calibration()); i += 1; } BOOST_FOREACH (const SfM_Track& track, db.tracks) initial.insert(P(j++), track.p); bool separateCalibration = true; return optimize(db, graph, initial, separateCalibration); }
//************************************************************************* TEST (EssentialMatrixFactor2, extraMinimization) { // Additional test with camera moving in positive X direction // We start with a factor graph and add constraints to it // Noise sigma is 1, assuming pixel measurements NonlinearFactorGraph graph; for (size_t i = 0; i < data.number_tracks(); i++) graph.add(EssentialMatrixFactor2(100, i, pA(i), pB(i), model2, K)); // Check error at ground truth Values truth; truth.insert(100, trueE); for (size_t i = 0; i < data.number_tracks(); i++) { Point3 P1 = data.tracks[i].p; truth.insert(i, double(baseline / P1.z())); } EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8); // Optimize LevenbergMarquardtParams parameters; // parameters.setVerbosity("ERROR"); LevenbergMarquardtOptimizer optimizer(graph, truth, parameters); Values result = optimizer.optimize(); // Check result EssentialMatrix actual = result.at<EssentialMatrix>(100); EXPECT(assert_equal(trueE, actual, 1e-1)); for (size_t i = 0; i < data.number_tracks(); i++) EXPECT_DOUBLES_EQUAL(truth.at<double>(i), result.at<double>(i), 1e-1); // Check error at result EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); }
/* ************************************************************************* */ int main (int argc, char* argv[]) { // Find default file, but if an argument is given, try loading a file string filename = findExampleDataFile("dubrovnik-3-7-pre"); if (argc>1) filename = string(argv[1]); // Load the SfM data from file SfM_data mydata; assert(readBAL(filename, mydata)); cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras(); // Create a factor graph NonlinearFactorGraph graph; // We share *one* noiseModel between all projection factors noiseModel::Isotropic::shared_ptr noise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v // Add measurements to the factor graph size_t j = 0; BOOST_FOREACH(const SfM_Track& track, mydata.tracks) { BOOST_FOREACH(const SfM_Measurement& m, track.measurements) { size_t i = m.first; Point2 uv = m.second; graph.push_back(MyFactor(uv, noise, C(i), P(j))); // note use of shorthand symbols C and P } j += 1; } // Add a prior on pose x1. This indirectly specifies where the origin is. // and a prior on the position of the first landmark to fix the scale graph.push_back(PriorFactor<SfM_Camera>(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1))); graph.push_back(PriorFactor<Point3> (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1))); // Create initial estimate Values initial; size_t i = 0; j = 0; BOOST_FOREACH(const SfM_Camera& camera, mydata.cameras) initial.insert(C(i++), camera); BOOST_FOREACH(const SfM_Track& track, mydata.tracks) initial.insert(P(j++), track.p); /* Optimize the graph and print results */ Values result; try { LevenbergMarquardtParams params; params.setVerbosity("ERROR"); LevenbergMarquardtOptimizer lm(graph, initial, params); result = lm.optimize(); } catch (exception& e) { cout << e.what(); } cout << "final error: " << graph.error(result) << endl; return 0; }
/* ************************************************************************* */ TEST( dataSet, Balbianello) { ///< The structure where we will save the SfM data const string filename = findExampleDataFile("Balbianello"); SfM_data mydata; CHECK(readBundler(filename, mydata)); // Check number of things EXPECT_LONGS_EQUAL(5,mydata.number_cameras()); EXPECT_LONGS_EQUAL(544,mydata.number_tracks()); const SfM_Track& track0 = mydata.tracks[0]; EXPECT_LONGS_EQUAL(3,track0.number_measurements()); // Check projection of a given point EXPECT_LONGS_EQUAL(0,track0.measurements[0].first); const SfM_Camera& camera0 = mydata.cameras[0]; Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second; EXPECT(assert_equal(expected,actual,1)); }
/* ************************************************************************* */ TEST(PinholeCamera, BAL) { string filename = findExampleDataFile("dubrovnik-3-7-pre"); SfM_data db; bool success = readBAL(filename, db); if (!success) throw runtime_error("Could not access file!"); SharedNoiseModel unit2 = noiseModel::Unit::Create(2); NonlinearFactorGraph graph; for (size_t j = 0; j < db.number_tracks(); j++) { for (const SfM_Measurement& m: db.tracks[j].measurements) graph.push_back(sfmFactor(m.second, unit2, m.first, P(j))); } Values initial = initialCamerasAndPointsEstimate(db); LevenbergMarquardtOptimizer lm(graph, initial); Values actual = lm.optimize(); double actualError = graph.error(actual); EXPECT_DOUBLES_EQUAL(0.0199833, actualError, 1e-5); }
/* ************************************************************************* */ TEST( dataSet, writeBAL_Dubrovnik) { ///< Read a file using the unit tested readBAL const string filenameToRead = findExampleDataFile("dubrovnik-3-7-pre"); SfM_data readData; readBAL(filenameToRead, readData); // Write readData to file filenameToWrite const string filenameToWrite = createRewrittenFileName(filenameToRead); CHECK(writeBAL(filenameToWrite, readData)); // Read what we wrote SfM_data writtenData; CHECK(readBAL(filenameToWrite, writtenData)); // Check that what we read is the same as what we wrote EXPECT(assert_equal(readData.number_cameras(),writtenData.number_cameras())); EXPECT(assert_equal(readData.number_tracks(),writtenData.number_tracks())); for (size_t i = 0; i < readData.number_cameras(); i++){ PinholeCamera<Cal3Bundler> expectedCamera = writtenData.cameras[i]; PinholeCamera<Cal3Bundler> actualCamera = readData.cameras[i]; EXPECT(assert_equal(expectedCamera,actualCamera)); } for (size_t j = 0; j < readData.number_tracks(); j++){ // check point SfM_Track expectedTrack = writtenData.tracks[j]; SfM_Track actualTrack = readData.tracks[j]; Point3 expectedPoint = expectedTrack.p; Point3 actualPoint = actualTrack.p; EXPECT(assert_equal(expectedPoint,actualPoint)); // check rgb Point3 expectedRGB = Point3( expectedTrack.r, expectedTrack.g, expectedTrack.b ); Point3 actualRGB = Point3( actualTrack.r, actualTrack.g, actualTrack.b); EXPECT(assert_equal(expectedRGB,actualRGB)); // check measurements for (size_t k = 0; k < actualTrack.number_measurements(); k++){ EXPECT(assert_equal(expectedTrack.measurements[k].first,actualTrack.measurements[k].first)); EXPECT(assert_equal(expectedTrack.measurements[k].second,actualTrack.measurements[k].second)); } } }
/* ************************************************************************* */ TEST( dataSet, writeBALfromValues_Dubrovnik){ ///< Read a file using the unit tested readBAL const string filenameToRead = findExampleDataFile("dubrovnik-3-7-pre"); SfM_data readData; readBAL(filenameToRead, readData); Pose3 poseChange = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.3,0.1,0.3)); Values value; for(size_t i=0; i < readData.number_cameras(); i++){ // for each camera Key poseKey = symbol('x',i); Pose3 pose = poseChange.compose(readData.cameras[i].pose()); value.insert(poseKey, pose); } for(size_t j=0; j < readData.number_tracks(); j++){ // for each point Key pointKey = P(j); Point3 point = poseChange.transform_from( readData.tracks[j].p ); value.insert(pointKey, point); } // Write values and readData to a file const string filenameToWrite = createRewrittenFileName(filenameToRead); writeBALfromValues(filenameToWrite, readData, value); // Read the file we wrote SfM_data writtenData; readBAL(filenameToWrite, writtenData); // Check that the reprojection errors are the same and the poses are correct // Check number of things EXPECT_LONGS_EQUAL(3,writtenData.number_cameras()); EXPECT_LONGS_EQUAL(7,writtenData.number_tracks()); const SfM_Track& track0 = writtenData.tracks[0]; EXPECT_LONGS_EQUAL(3,track0.number_measurements()); // Check projection of a given point EXPECT_LONGS_EQUAL(0,track0.measurements[0].first); const SfM_Camera& camera0 = writtenData.cameras[0]; Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second; EXPECT(assert_equal(expected,actual,12)); Pose3 expectedPose = camera0.pose(); Key poseKey = symbol('x',0); Pose3 actualPose = value.at<Pose3>(poseKey); EXPECT(assert_equal(expectedPose,actualPose, 1e-7)); Point3 expectedPoint = track0.p; Key pointKey = P(0); Point3 actualPoint = value.at<Point3>(pointKey); EXPECT(assert_equal(expectedPoint,actualPoint, 1e-6)); }
/* ************************************************************************* */ int main (int argc, char* argv[]) { // Find default file, but if an argument is given, try loading a file string filename = findExampleDataFile("dubrovnik-3-7-pre"); if (argc>1) filename = string(argv[1]); // Load the SfM data from file SfM_data mydata; readBAL(filename, mydata); cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras(); // Create a factor graph NonlinearFactorGraph graph; // We share *one* noiseModel between all projection factors noiseModel::Isotropic::shared_ptr noise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v // Add measurements to the factor graph size_t j = 0; BOOST_FOREACH(const SfM_Track& track, mydata.tracks) { BOOST_FOREACH(const SfM_Measurement& m, track.measurements) { size_t i = m.first; Point2 uv = m.second; graph.push_back(MyFactor(uv, noise, C(i), P(j))); // note use of shorthand symbols C and P } j += 1; } // Add a prior on pose x1. This indirectly specifies where the origin is. // and a prior on the position of the first landmark to fix the scale graph.push_back(PriorFactor<SfM_Camera>(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1))); graph.push_back(PriorFactor<Point3> (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1))); // Create initial estimate Values initial; size_t i = 0; j = 0; BOOST_FOREACH(const SfM_Camera& camera, mydata.cameras) initial.insert(C(i++), camera); BOOST_FOREACH(const SfM_Track& track, mydata.tracks) initial.insert(P(j++), track.p); /** --------------- COMPARISON -----------------------**/ /** ----------------------------------------------------**/ LevenbergMarquardtParams params_using_COLAMD, params_using_METIS; try { params_using_METIS.setVerbosity("ERROR"); gttic_(METIS_ORDERING); params_using_METIS.ordering = Ordering::Create(Ordering::METIS, graph); gttoc_(METIS_ORDERING); params_using_COLAMD.setVerbosity("ERROR"); gttic_(COLAMD_ORDERING); params_using_COLAMD.ordering = Ordering::Create(Ordering::COLAMD, graph); gttoc_(COLAMD_ORDERING); } catch (exception& e) { cout << e.what(); } // expect they have different ordering results if(params_using_COLAMD.ordering == params_using_METIS.ordering) { cout << "COLAMD and METIS produce the same ordering. " << "Problem here!!!" << endl; } /* Optimize the graph with METIS and COLAMD and time the results */ Values result_METIS, result_COLAMD; try { gttic_(OPTIMIZE_WITH_METIS); LevenbergMarquardtOptimizer lm_METIS(graph, initial, params_using_METIS); result_METIS = lm_METIS.optimize(); gttoc_(OPTIMIZE_WITH_METIS); gttic_(OPTIMIZE_WITH_COLAMD); LevenbergMarquardtOptimizer lm_COLAMD(graph, initial, params_using_COLAMD); result_COLAMD = lm_COLAMD.optimize(); gttoc_(OPTIMIZE_WITH_COLAMD); } catch (exception& e) { cout << e.what(); } { // printing the result cout << "COLAMD final error: " << graph.error(result_COLAMD) << endl; cout << "METIS final error: " << graph.error(result_METIS) << endl; cout << endl << endl; cout << "Time comparison by solving " << filename << " results:" << endl; cout << boost::format("%1% point tracks and %2% cameras\n") \ % mydata.number_tracks() % mydata.number_cameras() \ << endl; tictoc_print_(); } return 0; }
namespace example2 { const string filename = findExampleDataFile("5pointExample2.txt"); SfM_data data; bool readOK = readBAL(filename, data); Rot3 aRb = data.cameras[1].pose().rotation(); Point3 aTb = data.cameras[1].pose().translation(); EssentialMatrix trueE(aRb, Unit3(aTb)); double baseline = 10; // actual baseline of the camera Point2 pA(size_t i) { return data.tracks[i].measurements[0].second; } Point2 pB(size_t i) { return data.tracks[i].measurements[1].second; } boost::shared_ptr<Cal3Bundler> // K = boost::make_shared<Cal3Bundler>(500, 0, 0); PinholeCamera<Cal3Bundler> camera2(data.cameras[1].pose(), *K); Vector vA(size_t i) { Point2 xy = K->calibrate(pA(i)); return EssentialMatrix::Homogeneous(xy); } Vector vB(size_t i) { Point2 xy = K->calibrate(pB(i)); return EssentialMatrix::Homogeneous(xy); } //************************************************************************* TEST (EssentialMatrixFactor, extraMinimization) { // Additional test with camera moving in positive X direction NonlinearFactorGraph graph; for (size_t i = 0; i < 5; i++) graph.add(EssentialMatrixFactor(1, pA(i), pB(i), model1, K)); // Check error at ground truth Values truth; truth.insert(1, trueE); EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8); // Check error at initial estimate Values initial; EssentialMatrix initialE = trueE.retract( (Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); initial.insert(1, initialE); #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) EXPECT_DOUBLES_EQUAL(643.26, graph.error(initial), 1e-2); #else EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); #endif // Optimize LevenbergMarquardtParams parameters; LevenbergMarquardtOptimizer optimizer(graph, initial, parameters); Values result = optimizer.optimize(); // Check result EssentialMatrix actual = result.at<EssentialMatrix>(1); EXPECT(assert_equal(trueE, actual, 1e-1)); // Check error at result EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); // Check errors individually for (size_t i = 0; i < 5; i++) EXPECT_DOUBLES_EQUAL(0, actual.error(vA(i), vB(i)), 1e-6); } //************************************************************************* TEST (EssentialMatrixFactor2, extraTest) { for (size_t i = 0; i < 5; i++) { EssentialMatrixFactor2 factor(100, i, pA(i), pB(i), model2, K); // Check evaluation Point3 P1 = data.tracks[i].p; const Point2 pi = camera2.project(P1); Point2 reprojectionError(pi - pB(i)); Vector expected = reprojectionError.vector(); Matrix Hactual1, Hactual2; double d(baseline / P1.z()); Vector actual = factor.evaluateError(trueE, d, Hactual1, Hactual2); EXPECT(assert_equal(expected, actual, 1e-7)); // Use numerical derivatives to calculate the expected Jacobian Matrix Hexpected1, Hexpected2; boost::function<Vector(const EssentialMatrix&, double)> f = boost::bind( &EssentialMatrixFactor2::evaluateError, &factor, _1, _2, boost::none, boost::none); Hexpected1 = numericalDerivative21<Vector2, EssentialMatrix, double>(f, trueE, d); Hexpected2 = numericalDerivative22<Vector2, EssentialMatrix, double>(f, trueE, d); // Verify the Jacobian is correct EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6)); EXPECT(assert_equal(Hexpected2, Hactual2, 1e-8)); } } //************************************************************************* TEST (EssentialMatrixFactor2, extraMinimization) { // Additional test with camera moving in positive X direction // We start with a factor graph and add constraints to it // Noise sigma is 1, assuming pixel measurements NonlinearFactorGraph graph; for (size_t i = 0; i < data.number_tracks(); i++) graph.add(EssentialMatrixFactor2(100, i, pA(i), pB(i), model2, K)); // Check error at ground truth Values truth; truth.insert(100, trueE); for (size_t i = 0; i < data.number_tracks(); i++) { Point3 P1 = data.tracks[i].p; truth.insert(i, double(baseline / P1.z())); } EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8); // Optimize LevenbergMarquardtParams parameters; // parameters.setVerbosity("ERROR"); LevenbergMarquardtOptimizer optimizer(graph, truth, parameters); Values result = optimizer.optimize(); // Check result EssentialMatrix actual = result.at<EssentialMatrix>(100); EXPECT(assert_equal(trueE, actual, 1e-1)); for (size_t i = 0; i < data.number_tracks(); i++) EXPECT_DOUBLES_EQUAL(truth.at<double>(i), result.at<double>(i), 1e-1); // Check error at result EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); } //************************************************************************* TEST (EssentialMatrixFactor3, extraTest) { // The "true E" in the body frame is EssentialMatrix bodyE = cRb.inverse() * trueE; for (size_t i = 0; i < 5; i++) { EssentialMatrixFactor3 factor(100, i, pA(i), pB(i), cRb, model2, K); // Check evaluation Point3 P1 = data.tracks[i].p; const Point2 pi = camera2.project(P1); Point2 reprojectionError(pi - pB(i)); Vector expected = reprojectionError.vector(); Matrix Hactual1, Hactual2; double d(baseline / P1.z()); Vector actual = factor.evaluateError(bodyE, d, Hactual1, Hactual2); EXPECT(assert_equal(expected, actual, 1e-7)); // Use numerical derivatives to calculate the expected Jacobian Matrix Hexpected1, Hexpected2; boost::function<Vector(const EssentialMatrix&, double)> f = boost::bind( &EssentialMatrixFactor3::evaluateError, &factor, _1, _2, boost::none, boost::none); Hexpected1 = numericalDerivative21<Vector2, EssentialMatrix, double>(f, bodyE, d); Hexpected2 = numericalDerivative22<Vector2, EssentialMatrix, double>(f, bodyE, d); // Verify the Jacobian is correct EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6)); EXPECT(assert_equal(Hexpected2, Hactual2, 1e-8)); } } } // namespace example2