/* ---------------------------------------------------------------------------- * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) * See LICENSE for the license information * -------------------------------------------------------------------------- */ /** * @file CreateSFMExampleData.cpp * @brief Create some example data that for inclusion in the data folder * @author Frank Dellaert */ #include #include using namespace std; using namespace gtsam; /* ************************************************************************* */ void createExampleBALFile(const string& filename, const vector& points, const Pose3& pose1, const Pose3& pose2, const Cal3Bundler& K = Cal3Bundler()) { // Class that will gather all data SfmData data; // Create two cameras and add them to data data.cameras.push_back(SfmCamera(pose1, K)); data.cameras.push_back(SfmCamera(pose2, K)); for (const Point3& p : points) { // Create the track SfmTrack track; track.p = p; track.r = 1; track.g = 1; track.b = 1; // Project points in both cameras for (size_t i = 0; i < 2; i++) track.measurements.push_back(make_pair(i, data.cameras[i].project(p))); // Add track to data data.tracks.push_back(track); } writeBAL(filename, data); } /* ************************************************************************* */ void create5PointExample1() { // Create two cameras poses Rot3 aRb = Rot3::Yaw(M_PI_2); Point3 aTb(0.1, 0, 0); Pose3 pose1, pose2(aRb, aTb); // Create test data, we need at least 5 points vector points = { {0, 0, 1}, {-0.1, 0, 1}, {0.1, 0, 1}, {0, 0.5, 0.5}, {0, -0.5, 0.5}}; // Assumes example is run in ${GTSAM_TOP}/build/examples const string filename = "../../examples/Data/5pointExample1.txt"; createExampleBALFile(filename, points, pose1, pose2); } /* ************************************************************************* */ void create5PointExample2() { // Create two cameras poses Rot3 aRb = Rot3::Yaw(M_PI_2); Point3 aTb(10, 0, 0); Pose3 pose1, pose2(aRb, aTb); // Create test data, we need at least 5 points vector points = {{0, 0, 100}, {-10, 0, 100}, {10, 0, 100}, // {0, 50, 50}, {0, -50, 50}, {-20, 0, 80}, // {20, -50, 80}}; // Assumes example is run in ${GTSAM_TOP}/build/examples const string filename = "../../examples/Data/5pointExample2.txt"; Cal3Bundler K(500, 0, 0); createExampleBALFile(filename, points, pose1, pose2, K); } /* ************************************************************************* */ void create18PointExample1() { // Create two cameras poses Rot3 aRb = Rot3::Yaw(M_PI_2); Point3 aTb(0.1, 0, 0); Pose3 pose1, pose2(aRb, aTb); // Create test data, we need 15 points vector points = { {0, 0, 1}, {-0.1, 0, 1}, {0.1, 0, 1}, // {0, 0.5, 0.5}, {0, -0.5, 0.5}, {-1, -0.5, 2}, // {-1, 0.5, 2}, {0.25, -0.5, 1.5}, {0.25, 0.5, 1.5}, // {-0.1, -0.5, 0.5}, {0.1, -0.5, 1}, {0.1, 0.5, 1}, // {-0.1, 0, 0.5}, {-0.1, 0.5, 0.5}, {0, 0, 0.5}, // {0.1, -0.5, 0.5}, {0.1, 0, 0.5}, {0.1, 0.5, 0.5}}; // Assumes example is run in ${GTSAM_TOP}/build/examples const string filename = "../../examples/Data/18pointExample1.txt"; createExampleBALFile(filename, points, pose1, pose2); } int main(int argc, char* argv[]) { create5PointExample1(); create5PointExample2(); create18PointExample1(); return 0; } /* ************************************************************************* */