diff --git a/gtsam/sfm/SfmData.cpp b/gtsam/sfm/SfmData.cpp index 715dda2ed..1e3d53601 100644 --- a/gtsam/sfm/SfmData.cpp +++ b/gtsam/sfm/SfmData.cpp @@ -16,16 +16,28 @@ * @brief Data structure for dealing with Structure from Motion data */ +#include #include +#include +#include + namespace gtsam { -void SfmData::print(const std::string& s) const { +using std::cout; +using std::endl; + +using gtsam::symbol_shorthand::P; +using gtsam::symbol_shorthand::X; + +/* ************************************************************************** */ +void SfmData::print(const std::string &s) const { std::cout << "Number of cameras = " << nrCameras() << std::endl; std::cout << "Number of tracks = " << nrTracks() << std::endl; } -bool SfmData::equals(const SfmData& sfmData, double tol) const { +/* ************************************************************************** */ +bool SfmData::equals(const SfmData &sfmData, double tol) const { // check number of cameras and tracks if (nrCameras() != sfmData.nrCameras() || nrTracks() != sfmData.nrTracks()) { return false; @@ -48,4 +60,349 @@ bool SfmData::equals(const SfmData& sfmData, double tol) const { return true; } +/* ************************************************************************* */ +Rot3 openGLFixedRotation() { // this is due to different convention for + // cameras in gtsam and openGL + /* R = [ 1 0 0 + * 0 -1 0 + * 0 0 -1] + */ + Matrix3 R_mat = Matrix3::Zero(3, 3); + R_mat(0, 0) = 1.0; + R_mat(1, 1) = -1.0; + R_mat(2, 2) = -1.0; + return Rot3(R_mat); +} + +/* ************************************************************************* */ +Pose3 openGL2gtsam(const Rot3 &R, double tx, double ty, double tz) { + Rot3 R90 = openGLFixedRotation(); + Rot3 wRc = (R.inverse()).compose(R90); + + // Our camera-to-world translation wTc = -R'*t + return Pose3(wRc, R.unrotate(Point3(-tx, -ty, -tz))); +} + +/* ************************************************************************* */ +Pose3 gtsam2openGL(const Rot3 &R, double tx, double ty, double tz) { + Rot3 R90 = openGLFixedRotation(); + Rot3 cRw_openGL = R90.compose(R.inverse()); + Point3 t_openGL = cRw_openGL.rotate(Point3(-tx, -ty, -tz)); + return Pose3(cRw_openGL, t_openGL); +} + +/* ************************************************************************* */ +Pose3 gtsam2openGL(const Pose3 &PoseGTSAM) { + return gtsam2openGL(PoseGTSAM.rotation(), PoseGTSAM.x(), PoseGTSAM.y(), + PoseGTSAM.z()); +} + +/* ************************************************************************** */ +bool readBundler(const std::string &filename, SfmData &data) { + // Load the data file + std::ifstream is(filename.c_str(), std::ifstream::in); + if (!is) { + cout << "Error in readBundler: can not find the file!!" << endl; + return false; + } + + // Ignore the first line + char aux[500]; + is.getline(aux, 500); + + // Get the number of camera poses and 3D points + size_t nrPoses, nrPoints; + is >> nrPoses >> nrPoints; + + // Get the information for the camera poses + for (size_t i = 0; i < nrPoses; i++) { + // Get the focal length and the radial distortion parameters + float f, k1, k2; + is >> f >> k1 >> k2; + Cal3Bundler K(f, k1, k2); + + // Get the rotation matrix + float r11, r12, r13; + float r21, r22, r23; + float r31, r32, r33; + is >> r11 >> r12 >> r13 >> r21 >> r22 >> r23 >> r31 >> r32 >> r33; + + // Bundler-OpenGL rotation matrix + Rot3 R(r11, r12, r13, r21, r22, r23, r31, r32, r33); + + // Check for all-zero R, in which case quit + if (r11 == 0 && r12 == 0 && r13 == 0) { + cout << "Error in readBundler: zero rotation matrix for pose " << i + << endl; + return false; + } + + // Get the translation vector + float tx, ty, tz; + is >> tx >> ty >> tz; + + Pose3 pose = openGL2gtsam(R, tx, ty, tz); + + data.cameras.emplace_back(pose, K); + } + + // Get the information for the 3D points + data.tracks.reserve(nrPoints); + for (size_t j = 0; j < nrPoints; j++) { + SfmTrack track; + + // Get the 3D position + float x, y, z; + is >> x >> y >> z; + track.p = Point3(x, y, z); + + // Get the color information + float r, g, b; + is >> r >> g >> b; + track.r = r / 255.f; + track.g = g / 255.f; + track.b = b / 255.f; + + // Now get the visibility information + size_t nvisible = 0; + is >> nvisible; + + track.measurements.reserve(nvisible); + track.siftIndices.reserve(nvisible); + for (size_t k = 0; k < nvisible; k++) { + size_t cam_idx = 0, point_idx = 0; + float u, v; + is >> cam_idx >> point_idx >> u >> v; + track.measurements.emplace_back(cam_idx, Point2(u, -v)); + track.siftIndices.emplace_back(cam_idx, point_idx); + } + + data.tracks.push_back(track); + } + + is.close(); + return true; +} + +/* ************************************************************************** */ +bool readBAL(const std::string &filename, SfmData &data) { + // Load the data file + std::ifstream is(filename.c_str(), std::ifstream::in); + if (!is) { + cout << "Error in readBAL: can not find the file!!" << endl; + return false; + } + + // Get the number of camera poses and 3D points + size_t nrPoses, nrPoints, nrObservations; + is >> nrPoses >> nrPoints >> nrObservations; + + data.tracks.resize(nrPoints); + + // Get the information for the observations + for (size_t k = 0; k < nrObservations; k++) { + size_t i = 0, j = 0; + float u, v; + is >> i >> j >> u >> v; + data.tracks[j].measurements.emplace_back(i, Point2(u, -v)); + } + + // Get the information for the camera poses + for (size_t i = 0; i < nrPoses; i++) { + // Get the Rodrigues vector + float wx, wy, wz; + is >> wx >> wy >> wz; + Rot3 R = Rot3::Rodrigues(wx, wy, wz); // BAL-OpenGL rotation matrix + + // Get the translation vector + float tx, ty, tz; + is >> tx >> ty >> tz; + + Pose3 pose = openGL2gtsam(R, tx, ty, tz); + + // Get the focal length and the radial distortion parameters + float f, k1, k2; + is >> f >> k1 >> k2; + Cal3Bundler K(f, k1, k2); + + data.cameras.emplace_back(pose, K); + } + + // Get the information for the 3D points + for (size_t j = 0; j < nrPoints; j++) { + // Get the 3D position + float x, y, z; + is >> x >> y >> z; + SfmTrack &track = data.tracks[j]; + track.p = Point3(x, y, z); + track.r = 0.4f; + track.g = 0.4f; + track.b = 0.4f; + } + + is.close(); + return true; +} + +/* ************************************************************************** */ +SfmData readBal(const std::string &filename) { + SfmData data; + readBAL(filename, data); + return data; +} + +/* ************************************************************************** */ +bool writeBAL(const std::string &filename, SfmData &data) { + // Open the output file + std::ofstream os; + os.open(filename.c_str()); + os.precision(20); + if (!os.is_open()) { + cout << "Error in writeBAL: can not open the file!!" << endl; + return false; + } + + // Write the number of camera poses and 3D points + size_t nrObservations = 0; + for (size_t j = 0; j < data.nrTracks(); j++) { + nrObservations += data.tracks[j].nrMeasurements(); + } + + // Write observations + os << data.nrCameras() << " " << data.nrTracks() << " " << nrObservations + << endl; + os << endl; + + for (size_t j = 0; j < data.nrTracks(); j++) { // for each 3D point j + const SfmTrack &track = data.tracks[j]; + + for (size_t k = 0; k < track.nrMeasurements(); + k++) { // for each observation of the 3D point j + size_t i = track.measurements[k].first; // camera id + double u0 = data.cameras[i].calibration().px(); + double v0 = data.cameras[i].calibration().py(); + + if (u0 != 0 || v0 != 0) { + cout << "writeBAL has not been tested for calibration with nonzero " + "(u0,v0)" + << endl; + } + + double pixelBALx = track.measurements[k].second.x() - + u0; // center of image is the origin + double pixelBALy = -(track.measurements[k].second.y() - + v0); // center of image is the origin + Point2 pixelMeasurement(pixelBALx, pixelBALy); + os << i /*camera id*/ << " " << j /*point id*/ << " " + << pixelMeasurement.x() /*u of the pixel*/ << " " + << pixelMeasurement.y() /*v of the pixel*/ << endl; + } + } + os << endl; + + // Write cameras + for (size_t i = 0; i < data.nrCameras(); i++) { // for each camera + Pose3 poseGTSAM = data.cameras[i].pose(); + Cal3Bundler cameraCalibration = data.cameras[i].calibration(); + Pose3 poseOpenGL = gtsam2openGL(poseGTSAM); + os << Rot3::Logmap(poseOpenGL.rotation()) << endl; + os << poseOpenGL.translation().x() << endl; + os << poseOpenGL.translation().y() << endl; + os << poseOpenGL.translation().z() << endl; + os << cameraCalibration.fx() << endl; + os << cameraCalibration.k1() << endl; + os << cameraCalibration.k2() << endl; + os << endl; + } + + // Write the points + for (size_t j = 0; j < data.nrTracks(); j++) { // for each 3D point j + Point3 point = data.tracks[j].p; + os << point.x() << endl; + os << point.y() << endl; + os << point.z() << endl; + os << endl; + } + + os.close(); + return true; +} + +/* ************************************************************************** */ +bool writeBALfromValues(const std::string &filename, const SfmData &data, + Values &values) { + using Camera = PinholeCamera; + SfmData dataValues = data; + + // Store poses or cameras in SfmData + size_t nrPoses = values.count(); + if (nrPoses == dataValues.nrCameras()) { // we only estimated camera poses + for (size_t i = 0; i < dataValues.nrCameras(); i++) { // for each camera + Pose3 pose = values.at(X(i)); + Cal3Bundler K = dataValues.cameras[i].calibration(); + Camera camera(pose, K); + dataValues.cameras[i] = camera; + } + } else { + size_t nrCameras = values.count(); + if (nrCameras == dataValues.nrCameras()) { // we only estimated camera + // poses and calibration + for (size_t i = 0; i < nrCameras; i++) { // for each camera + Key cameraKey = i; // symbol('c',i); + Camera camera = values.at(cameraKey); + dataValues.cameras[i] = camera; + } + } else { + cout << "writeBALfromValues: different number of cameras in " + "SfM_dataValues (#cameras " + << dataValues.nrCameras() << ") and values (#cameras " << nrPoses + << ", #poses " << nrCameras << ")!!" << endl; + return false; + } + } + + // Store 3D points in SfmData + size_t nrPoints = values.count(), nrTracks = dataValues.nrTracks(); + if (nrPoints != nrTracks) { + cout << "writeBALfromValues: different number of points in " + "SfM_dataValues (#points= " + << nrTracks << ") and values (#points " << nrPoints << ")!!" << endl; + } + + for (size_t j = 0; j < nrTracks; j++) { // for each point + Key pointKey = P(j); + if (values.exists(pointKey)) { + Point3 point = values.at(pointKey); + dataValues.tracks[j].p = point; + } else { + dataValues.tracks[j].r = 1.0; + dataValues.tracks[j].g = 0.0; + dataValues.tracks[j].b = 0.0; + dataValues.tracks[j].p = Point3(0, 0, 0); + } + } + + // Write SfmData to file + return writeBAL(filename, dataValues); +} + +/* ************************************************************************** */ +Values initialCamerasEstimate(const SfmData &db) { + Values initial; + size_t i = 0; // NO POINTS: j = 0; + for (const SfmCamera &camera : db.cameras) initial.insert(i++, camera); + return initial; +} + +/* ************************************************************************** */ +Values initialCamerasAndPointsEstimate(const SfmData &db) { + Values initial; + size_t i = 0, j = 0; + for (const SfmCamera &camera : db.cameras) initial.insert((i++), camera); + for (const SfmTrack &track : db.tracks) initial.insert(P(j++), track.p); + return initial; +} + +/* ************************************************************************** */ + } // namespace gtsam diff --git a/gtsam/sfm/SfmData.h b/gtsam/sfm/SfmData.h index 91b7a5d1e..8770fcd4a 100644 --- a/gtsam/sfm/SfmData.h +++ b/gtsam/sfm/SfmData.h @@ -20,6 +20,7 @@ #include #include +#include #include #include @@ -100,4 +101,95 @@ struct SfmData { template <> struct traits : public Testable {}; +/** + * @brief This function parses a bundler output file and stores the data into a + * SfmData structure + * @param filename The name of the bundler file + * @param data SfM structure where the data is stored + * @return true if the parsing was successful, false otherwise + */ +GTSAM_EXPORT bool readBundler(const std::string& filename, SfmData& data); + +/** + * @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and + * stores the data into a SfmData structure + * @param filename The name of the BAL file + * @param data SfM structure where the data is stored + * @return true if the parsing was successful, false otherwise + */ +GTSAM_EXPORT bool readBAL(const std::string& filename, SfmData& data); + +/** + * @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and + * returns the data as a SfmData structure. Mainly used by wrapped code. + * @param filename The name of the BAL file. + * @return SfM structure where the data is stored. + */ +GTSAM_EXPORT SfmData readBal(const std::string& filename); + +/** + * @brief This function writes a "Bundle Adjustment in the Large" (BAL) file + * from a SfmData structure + * @param filename The name of the BAL file to write + * @param data SfM structure where the data is stored + * @return true if the parsing was successful, false otherwise + */ +GTSAM_EXPORT bool writeBAL(const std::string& filename, SfmData& data); + +/** + * @brief This function writes a "Bundle Adjustment in the Large" (BAL) file + * from a SfmData structure and a value structure (measurements are the same as + * the SfM input data, while camera poses and values are read from Values) + * @param filename The name of the BAL file to write + * @param data SfM structure where the data is stored + * @param values structure where the graph values are stored (values can be + * either Pose3 or PinholeCamera for the cameras, and should be + * Point3 for the 3D points). Note that the current version assumes that the + * keys are "x1" for pose 1 (or "c1" for camera 1) and "l1" for landmark 1 + * @return true if the parsing was successful, false otherwise + */ +GTSAM_EXPORT bool writeBALfromValues(const std::string& filename, + const SfmData& data, Values& values); + +/** + * @brief This function converts an openGL camera pose to an GTSAM camera pose + * @param R rotation in openGL + * @param tx x component of the translation in openGL + * @param ty y component of the translation in openGL + * @param tz z component of the translation in openGL + * @return Pose3 in GTSAM format + */ +GTSAM_EXPORT Pose3 openGL2gtsam(const Rot3& R, double tx, double ty, double tz); + +/** + * @brief This function converts a GTSAM camera pose to an openGL camera pose + * @param R rotation in GTSAM + * @param tx x component of the translation in GTSAM + * @param ty y component of the translation in GTSAM + * @param tz z component of the translation in GTSAM + * @return Pose3 in openGL format + */ +GTSAM_EXPORT Pose3 gtsam2openGL(const Rot3& R, double tx, double ty, double tz); + +/** + * @brief This function converts a GTSAM camera pose to an openGL camera pose + * @param PoseGTSAM pose in GTSAM format + * @return Pose3 in openGL format + */ +GTSAM_EXPORT Pose3 gtsam2openGL(const Pose3& PoseGTSAM); + +/** + * @brief This function creates initial values for cameras from db + * @param SfmData + * @return Values + */ +GTSAM_EXPORT Values initialCamerasEstimate(const SfmData& db); + +/** + * @brief This function creates initial values for cameras and points from db + * @param SfmData + * @return Values + */ +GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfmData& db); + } // namespace gtsam diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 8acdbfc14..a2b96efab 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -54,8 +54,6 @@ using namespace std; namespace fs = boost::filesystem; using gtsam::symbol_shorthand::L; -using gtsam::symbol_shorthand::P; -using gtsam::symbol_shorthand::X; #define LINESIZE 81920 @@ -945,352 +943,6 @@ GraphAndValues load3D(const string &filename) { return make_pair(graph, initial); } -/* ************************************************************************* */ -Rot3 openGLFixedRotation() { // this is due to different convention for - // cameras in gtsam and openGL - /* R = [ 1 0 0 - * 0 -1 0 - * 0 0 -1] - */ - Matrix3 R_mat = Matrix3::Zero(3, 3); - R_mat(0, 0) = 1.0; - R_mat(1, 1) = -1.0; - R_mat(2, 2) = -1.0; - return Rot3(R_mat); -} - -/* ************************************************************************* */ -Pose3 openGL2gtsam(const Rot3 &R, double tx, double ty, double tz) { - Rot3 R90 = openGLFixedRotation(); - Rot3 wRc = (R.inverse()).compose(R90); - - // Our camera-to-world translation wTc = -R'*t - return Pose3(wRc, R.unrotate(Point3(-tx, -ty, -tz))); -} - -/* ************************************************************************* */ -Pose3 gtsam2openGL(const Rot3 &R, double tx, double ty, double tz) { - Rot3 R90 = openGLFixedRotation(); - Rot3 cRw_openGL = R90.compose(R.inverse()); - Point3 t_openGL = cRw_openGL.rotate(Point3(-tx, -ty, -tz)); - return Pose3(cRw_openGL, t_openGL); -} - -/* ************************************************************************* */ -Pose3 gtsam2openGL(const Pose3 &PoseGTSAM) { - return gtsam2openGL(PoseGTSAM.rotation(), PoseGTSAM.x(), PoseGTSAM.y(), - PoseGTSAM.z()); -} - -/* ************************************************************************* */ -bool readBundler(const string &filename, SfmData &data) { - // Load the data file - ifstream is(filename.c_str(), ifstream::in); - if (!is) { - cout << "Error in readBundler: can not find the file!!" << endl; - return false; - } - - // Ignore the first line - char aux[500]; - is.getline(aux, 500); - - // Get the number of camera poses and 3D points - size_t nrPoses, nrPoints; - is >> nrPoses >> nrPoints; - - // Get the information for the camera poses - for (size_t i = 0; i < nrPoses; i++) { - // Get the focal length and the radial distortion parameters - float f, k1, k2; - is >> f >> k1 >> k2; - Cal3Bundler K(f, k1, k2); - - // Get the rotation matrix - float r11, r12, r13; - float r21, r22, r23; - float r31, r32, r33; - is >> r11 >> r12 >> r13 >> r21 >> r22 >> r23 >> r31 >> r32 >> r33; - - // Bundler-OpenGL rotation matrix - Rot3 R(r11, r12, r13, r21, r22, r23, r31, r32, r33); - - // Check for all-zero R, in which case quit - if (r11 == 0 && r12 == 0 && r13 == 0) { - cout << "Error in readBundler: zero rotation matrix for pose " << i - << endl; - return false; - } - - // Get the translation vector - float tx, ty, tz; - is >> tx >> ty >> tz; - - Pose3 pose = openGL2gtsam(R, tx, ty, tz); - - data.cameras.emplace_back(pose, K); - } - - // Get the information for the 3D points - data.tracks.reserve(nrPoints); - for (size_t j = 0; j < nrPoints; j++) { - SfmTrack track; - - // Get the 3D position - float x, y, z; - is >> x >> y >> z; - track.p = Point3(x, y, z); - - // Get the color information - float r, g, b; - is >> r >> g >> b; - track.r = r / 255.f; - track.g = g / 255.f; - track.b = b / 255.f; - - // Now get the visibility information - size_t nvisible = 0; - is >> nvisible; - - track.measurements.reserve(nvisible); - track.siftIndices.reserve(nvisible); - for (size_t k = 0; k < nvisible; k++) { - size_t cam_idx = 0, point_idx = 0; - float u, v; - is >> cam_idx >> point_idx >> u >> v; - track.measurements.emplace_back(cam_idx, Point2(u, -v)); - track.siftIndices.emplace_back(cam_idx, point_idx); - } - - data.tracks.push_back(track); - } - - is.close(); - return true; -} - -/* ************************************************************************* */ -bool readBAL(const string &filename, SfmData &data) { - // Load the data file - ifstream is(filename.c_str(), ifstream::in); - if (!is) { - cout << "Error in readBAL: can not find the file!!" << endl; - return false; - } - - // Get the number of camera poses and 3D points - size_t nrPoses, nrPoints, nrObservations; - is >> nrPoses >> nrPoints >> nrObservations; - - data.tracks.resize(nrPoints); - - // Get the information for the observations - for (size_t k = 0; k < nrObservations; k++) { - size_t i = 0, j = 0; - float u, v; - is >> i >> j >> u >> v; - data.tracks[j].measurements.emplace_back(i, Point2(u, -v)); - } - - // Get the information for the camera poses - for (size_t i = 0; i < nrPoses; i++) { - // Get the Rodrigues vector - float wx, wy, wz; - is >> wx >> wy >> wz; - Rot3 R = Rot3::Rodrigues(wx, wy, wz); // BAL-OpenGL rotation matrix - - // Get the translation vector - float tx, ty, tz; - is >> tx >> ty >> tz; - - Pose3 pose = openGL2gtsam(R, tx, ty, tz); - - // Get the focal length and the radial distortion parameters - float f, k1, k2; - is >> f >> k1 >> k2; - Cal3Bundler K(f, k1, k2); - - data.cameras.emplace_back(pose, K); - } - - // Get the information for the 3D points - for (size_t j = 0; j < nrPoints; j++) { - // Get the 3D position - float x, y, z; - is >> x >> y >> z; - SfmTrack &track = data.tracks[j]; - track.p = Point3(x, y, z); - track.r = 0.4f; - track.g = 0.4f; - track.b = 0.4f; - } - - is.close(); - return true; -} - -/* ************************************************************************* */ -SfmData readBal(const string &filename) { - SfmData data; - readBAL(filename, data); - return data; -} - -/* ************************************************************************* */ -bool writeBAL(const string &filename, SfmData &data) { - // Open the output file - ofstream os; - os.open(filename.c_str()); - os.precision(20); - if (!os.is_open()) { - cout << "Error in writeBAL: can not open the file!!" << endl; - return false; - } - - // Write the number of camera poses and 3D points - size_t nrObservations = 0; - for (size_t j = 0; j < data.nrTracks(); j++) { - nrObservations += data.tracks[j].nrMeasurements(); - } - - // Write observations - os << data.nrCameras() << " " << data.nrTracks() << " " - << nrObservations << endl; - os << endl; - - for (size_t j = 0; j < data.nrTracks(); j++) { // for each 3D point j - const SfmTrack &track = data.tracks[j]; - - for (size_t k = 0; k < track.nrMeasurements(); - k++) { // for each observation of the 3D point j - size_t i = track.measurements[k].first; // camera id - double u0 = data.cameras[i].calibration().px(); - double v0 = data.cameras[i].calibration().py(); - - if (u0 != 0 || v0 != 0) { - cout << "writeBAL has not been tested for calibration with nonzero " - "(u0,v0)" - << endl; - } - - double pixelBALx = track.measurements[k].second.x() - - u0; // center of image is the origin - double pixelBALy = -(track.measurements[k].second.y() - - v0); // center of image is the origin - Point2 pixelMeasurement(pixelBALx, pixelBALy); - os << i /*camera id*/ << " " << j /*point id*/ << " " - << pixelMeasurement.x() /*u of the pixel*/ << " " - << pixelMeasurement.y() /*v of the pixel*/ << endl; - } - } - os << endl; - - // Write cameras - for (size_t i = 0; i < data.nrCameras(); i++) { // for each camera - Pose3 poseGTSAM = data.cameras[i].pose(); - Cal3Bundler cameraCalibration = data.cameras[i].calibration(); - Pose3 poseOpenGL = gtsam2openGL(poseGTSAM); - os << Rot3::Logmap(poseOpenGL.rotation()) << endl; - os << poseOpenGL.translation().x() << endl; - os << poseOpenGL.translation().y() << endl; - os << poseOpenGL.translation().z() << endl; - os << cameraCalibration.fx() << endl; - os << cameraCalibration.k1() << endl; - os << cameraCalibration.k2() << endl; - os << endl; - } - - // Write the points - for (size_t j = 0; j < data.nrTracks(); j++) { // for each 3D point j - Point3 point = data.tracks[j].p; - os << point.x() << endl; - os << point.y() << endl; - os << point.z() << endl; - os << endl; - } - - os.close(); - return true; -} - -bool writeBALfromValues(const string &filename, const SfmData &data, - Values &values) { - using Camera = PinholeCamera; - SfmData dataValues = data; - - // Store poses or cameras in SfmData - size_t nrPoses = values.count(); - if (nrPoses == - dataValues.nrCameras()) { // we only estimated camera poses - for (size_t i = 0; i < dataValues.nrCameras(); - i++) { // for each camera - Pose3 pose = values.at(X(i)); - Cal3Bundler K = dataValues.cameras[i].calibration(); - Camera camera(pose, K); - dataValues.cameras[i] = camera; - } - } else { - size_t nrCameras = values.count(); - if (nrCameras == dataValues.nrCameras()) { // we only estimated camera - // poses and calibration - for (size_t i = 0; i < nrCameras; i++) { // for each camera - Key cameraKey = i; // symbol('c',i); - Camera camera = values.at(cameraKey); - dataValues.cameras[i] = camera; - } - } else { - cout << "writeBALfromValues: different number of cameras in " - "SfM_dataValues (#cameras " - << dataValues.nrCameras() << ") and values (#cameras " - << nrPoses << ", #poses " << nrCameras << ")!!" << endl; - return false; - } - } - - // Store 3D points in SfmData - size_t nrPoints = values.count(), - nrTracks = dataValues.nrTracks(); - if (nrPoints != nrTracks) { - cout << "writeBALfromValues: different number of points in " - "SfM_dataValues (#points= " - << nrTracks << ") and values (#points " << nrPoints << ")!!" << endl; - } - - for (size_t j = 0; j < nrTracks; j++) { // for each point - Key pointKey = P(j); - if (values.exists(pointKey)) { - Point3 point = values.at(pointKey); - dataValues.tracks[j].p = point; - } else { - dataValues.tracks[j].r = 1.0; - dataValues.tracks[j].g = 0.0; - dataValues.tracks[j].b = 0.0; - dataValues.tracks[j].p = Point3(0, 0, 0); - } - } - - // Write SfmData to file - return writeBAL(filename, dataValues); -} - -Values initialCamerasEstimate(const SfmData &db) { - Values initial; - size_t i = 0; // NO POINTS: j = 0; - for (const SfmCamera &camera : db.cameras) - initial.insert(i++, camera); - return initial; -} - -Values initialCamerasAndPointsEstimate(const SfmData &db) { - Values initial; - size_t i = 0, j = 0; - for (const SfmCamera &camera : db.cameras) - initial.insert((i++), camera); - for (const SfmTrack &track : db.tracks) - initial.insert(P(j++), track.p); - return initial; -} - // Wrapper-friendly versions of parseFactors and parseFactors BetweenFactorPose2s parse2DFactors(const std::string &filename, diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index d5c93c7f8..dc450a9f7 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -209,96 +209,6 @@ GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph, /// Load TORO 3D Graph GTSAM_EXPORT GraphAndValues load3D(const std::string& filename); -/** - * @brief This function parses a bundler output file and stores the data into a - * SfmData structure - * @param filename The name of the bundler file - * @param data SfM structure where the data is stored - * @return true if the parsing was successful, false otherwise - */ -GTSAM_EXPORT bool readBundler(const std::string& filename, SfmData &data); - -/** - * @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and stores the data into a - * SfmData structure - * @param filename The name of the BAL file - * @param data SfM structure where the data is stored - * @return true if the parsing was successful, false otherwise - */ -GTSAM_EXPORT bool readBAL(const std::string& filename, SfmData &data); - -/** - * @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and returns the data - * as a SfmData structure. Mainly used by wrapped code. - * @param filename The name of the BAL file. - * @return SfM structure where the data is stored. - */ -GTSAM_EXPORT SfmData readBal(const std::string& filename); - -/** - * @brief This function writes a "Bundle Adjustment in the Large" (BAL) file from a - * SfmData structure - * @param filename The name of the BAL file to write - * @param data SfM structure where the data is stored - * @return true if the parsing was successful, false otherwise - */ -GTSAM_EXPORT bool writeBAL(const std::string& filename, SfmData &data); - -/** - * @brief This function writes a "Bundle Adjustment in the Large" (BAL) file from a - * SfmData structure and a value structure (measurements are the same as the SfM input data, - * while camera poses and values are read from Values) - * @param filename The name of the BAL file to write - * @param data SfM structure where the data is stored - * @param values structure where the graph values are stored (values can be either Pose3 or PinholeCamera for the - * cameras, and should be Point3 for the 3D points). Note that the current version - * assumes that the keys are "x1" for pose 1 (or "c1" for camera 1) and "l1" for landmark 1 - * @return true if the parsing was successful, false otherwise - */ -GTSAM_EXPORT bool writeBALfromValues(const std::string& filename, - const SfmData &data, Values& values); - -/** - * @brief This function converts an openGL camera pose to an GTSAM camera pose - * @param R rotation in openGL - * @param tx x component of the translation in openGL - * @param ty y component of the translation in openGL - * @param tz z component of the translation in openGL - * @return Pose3 in GTSAM format - */ -GTSAM_EXPORT Pose3 openGL2gtsam(const Rot3& R, double tx, double ty, double tz); - -/** - * @brief This function converts a GTSAM camera pose to an openGL camera pose - * @param R rotation in GTSAM - * @param tx x component of the translation in GTSAM - * @param ty y component of the translation in GTSAM - * @param tz z component of the translation in GTSAM - * @return Pose3 in openGL format - */ -GTSAM_EXPORT Pose3 gtsam2openGL(const Rot3& R, double tx, double ty, double tz); - -/** - * @brief This function converts a GTSAM camera pose to an openGL camera pose - * @param PoseGTSAM pose in GTSAM format - * @return Pose3 in openGL format - */ -GTSAM_EXPORT Pose3 gtsam2openGL(const Pose3& PoseGTSAM); - -/** - * @brief This function creates initial values for cameras from db - * @param SfmData - * @return Values - */ -GTSAM_EXPORT Values initialCamerasEstimate(const SfmData& db); - -/** - * @brief This function creates initial values for cameras and points from db - * @param SfmData - * @return Values - */ -GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfmData& db); - // Wrapper-friendly versions of parseFactors and parseFactors using BetweenFactorPose2s = std::vector::shared_ptr>; GTSAM_EXPORT BetweenFactorPose2s