Moved Sfm classes to their own files
Frank Dellaert
parent
cd94e7dda2
commit
0fd49efaba
|
|
@ -0,0 +1,51 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 SfmData.cpp
|
||||
* @date January 2022
|
||||
* @author Frank dellaert
|
||||
* @brief Data structure for dealing with Structure from Motion data
|
||||
*/
|
||||
|
||||
#include <gtsam/sfm/SfmData.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
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 {
|
||||
// check number of cameras and tracks
|
||||
if (nrCameras() != sfmData.nrCameras() || nrTracks() != sfmData.nrTracks()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// check each camera
|
||||
for (size_t i = 0; i < nrCameras(); ++i) {
|
||||
if (!camera(i).equals(sfmData.camera(i), tol)) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
// check each track
|
||||
for (size_t j = 0; j < nrTracks(); ++j) {
|
||||
if (!track(j).equals(sfmData.track(j), tol)) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -0,0 +1,103 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 SfmData.h
|
||||
* @date January 2022
|
||||
* @author Frank dellaert
|
||||
* @brief Data structure for dealing with Structure from Motion data
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/geometry/Cal3Bundler.h>
|
||||
#include <gtsam/geometry/PinholeCamera.h>
|
||||
#include <gtsam/sfm/SfmTrack.h>
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/// Define the structure for the camera poses
|
||||
typedef PinholeCamera<Cal3Bundler> SfmCamera;
|
||||
|
||||
/**
|
||||
* @brief SfmData stores a bunch of SfmTracks
|
||||
* @addtogroup sfm
|
||||
*/
|
||||
struct SfmData {
|
||||
std::vector<SfmCamera> cameras; ///< Set of cameras
|
||||
|
||||
std::vector<SfmTrack> tracks; ///< Sparse set of points
|
||||
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/// Add a track to SfmData
|
||||
void addTrack(const SfmTrack& t) { tracks.push_back(t); }
|
||||
|
||||
/// Add a camera to SfmData
|
||||
void addCamera(const SfmCamera& cam) { cameras.push_back(cam); }
|
||||
|
||||
/// The number of reconstructed 3D points
|
||||
size_t nrTracks() const { return tracks.size(); }
|
||||
|
||||
/// The number of cameras
|
||||
size_t nrCameras() const { return cameras.size(); }
|
||||
|
||||
/// The track formed by series of landmark measurements
|
||||
SfmTrack track(size_t idx) const { return tracks[idx]; }
|
||||
|
||||
/// The camera pose at frame index `idx`
|
||||
SfmCamera camera(size_t idx) const { return cameras[idx]; }
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/// print
|
||||
void print(const std::string& s = "") const;
|
||||
|
||||
/// assert equality up to a tolerance
|
||||
bool equals(const SfmData& sfmData, double tol = 1e-9) const;
|
||||
|
||||
/// @}
|
||||
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
|
||||
/// @name Deprecated
|
||||
/// @{
|
||||
void GTSAM_DEPRECATED add_track(const SfmTrack& t) { tracks.push_back(t); }
|
||||
void GTSAM_DEPRECATED add_camera(const SfmCamera& cam) {
|
||||
cameras.push_back(cam);
|
||||
}
|
||||
size_t GTSAM_DEPRECATED number_tracks() const { return tracks.size(); }
|
||||
size_t GTSAM_DEPRECATED number_cameras() const { return cameras.size(); }
|
||||
/// @}
|
||||
#endif
|
||||
/// @name Serialization
|
||||
/// @{
|
||||
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template <class Archive>
|
||||
void serialize(Archive& ar, const unsigned int /*version*/) {
|
||||
ar& BOOST_SERIALIZATION_NVP(cameras);
|
||||
ar& BOOST_SERIALIZATION_NVP(tracks);
|
||||
}
|
||||
|
||||
/// @}
|
||||
};
|
||||
|
||||
/// traits
|
||||
template <>
|
||||
struct traits<SfmData> : public Testable<SfmData> {};
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -0,0 +1,71 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 SfmTrack.cpp
|
||||
* @date January 2022
|
||||
* @author Frank Dellaert
|
||||
* @brief A simple data structure for a track in Structure from Motion
|
||||
*/
|
||||
|
||||
#include <gtsam/sfm/SfmTrack.h>
|
||||
|
||||
#include <iostream>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
void SfmTrack::print(const std::string& s) const {
|
||||
std::cout << "Track with " << measurements.size();
|
||||
std::cout << " measurements of point " << p << std::endl;
|
||||
}
|
||||
|
||||
bool SfmTrack::equals(const SfmTrack& sfmTrack, double tolÏ) const {
|
||||
// check the 3D point
|
||||
if (!p.isApprox(sfmTrack.p)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// check the RGB values
|
||||
if (r != sfmTrack.r || g != sfmTrack.g || b != sfmTrack.b) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// compare size of vectors for measurements and siftIndices
|
||||
if (nrMeasurements() != sfmTrack.nrMeasurements() ||
|
||||
siftIndices.size() != sfmTrack.siftIndices.size()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// compare measurements (order sensitive)
|
||||
for (size_t idx = 0; idx < nrMeasurements(); ++idx) {
|
||||
SfmMeasurement measurement = measurements[idx];
|
||||
SfmMeasurement otherMeasurement = sfmTrack.measurements[idx];
|
||||
|
||||
if (measurement.first != otherMeasurement.first ||
|
||||
!measurement.second.isApprox(otherMeasurement.second)) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
// compare sift indices (order sensitive)
|
||||
for (size_t idx = 0; idx < siftIndices.size(); ++idx) {
|
||||
SiftIndex index = siftIndices[idx];
|
||||
SiftIndex otherIndex = sfmTrack.siftIndices[idx];
|
||||
|
||||
if (index.first != otherIndex.first || index.second != otherIndex.second) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -0,0 +1,133 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 SfmTrack.h
|
||||
* @date January 2022
|
||||
* @author Frank Dellaert
|
||||
* @brief A simple data structure for a track in Structure from Motion
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/base/serialization.h>
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
|
||||
#include <string>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/// A measurement with its camera index
|
||||
typedef std::pair<size_t, Point2> SfmMeasurement;
|
||||
|
||||
/// Sift index for SfmTrack
|
||||
typedef std::pair<size_t, size_t> SiftIndex;
|
||||
|
||||
/**
|
||||
* @brief An SfmTrack stores SfM measurements grouped in a track
|
||||
* @addtogroup sfm
|
||||
*/
|
||||
struct SfmTrack {
|
||||
Point3 p; ///< 3D position of the point
|
||||
float r, g, b; ///< RGB color of the 3D point
|
||||
|
||||
/// The 2D image projections (id,(u,v))
|
||||
std::vector<SfmMeasurement> measurements;
|
||||
|
||||
/// The feature descriptors
|
||||
std::vector<SiftIndex> siftIndices;
|
||||
|
||||
/// @name Constructors
|
||||
/// @{
|
||||
|
||||
explicit SfmTrack(float r = 0, float g = 0, float b = 0)
|
||||
: p(0, 0, 0), r(r), g(g), b(b) {}
|
||||
|
||||
explicit SfmTrack(const gtsam::Point3& pt, float r = 0, float g = 0,
|
||||
float b = 0)
|
||||
: p(pt), r(r), g(g), b(b) {}
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/// Add measurement (camera_idx, Point2) to track
|
||||
void addMeasurement(size_t idx, const gtsam::Point2& m) {
|
||||
measurements.emplace_back(idx, m);
|
||||
}
|
||||
|
||||
/// Total number of measurements in this track
|
||||
size_t nrMeasurements() const { return measurements.size(); }
|
||||
|
||||
/// Get the measurement (camera index, Point2) at pose index `idx`
|
||||
const SfmMeasurement& measurement(size_t idx) const {
|
||||
return measurements[idx];
|
||||
}
|
||||
|
||||
/// Get the SIFT feature index corresponding to the measurement at `idx`
|
||||
const SiftIndex& siftIndex(size_t idx) const { return siftIndices[idx]; }
|
||||
|
||||
/// Get 3D point
|
||||
const Point3& point3() const { return p; }
|
||||
|
||||
/// Get RGB values describing 3d point
|
||||
Point3 rgb() const { return Point3(r, g, b); }
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/// print
|
||||
void print(const std::string& s = "") const;
|
||||
|
||||
/// assert equality up to a tolerance
|
||||
bool equals(const SfmTrack& sfmTrack, double tol = 1e-9) const;
|
||||
|
||||
/// @}
|
||||
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
|
||||
/// @name Deprecated
|
||||
/// @{
|
||||
void GTSAM_DEPRECATED add_measurement(size_t idx, const gtsam::Point2& m) {
|
||||
measurements.emplace_back(idx, m);
|
||||
}
|
||||
|
||||
size_t GTSAM_DEPRECATED number_measurements() const {
|
||||
return measurements.size();
|
||||
}
|
||||
/// @}
|
||||
#endif
|
||||
/// @name Serialization
|
||||
/// @{
|
||||
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template <class ARCHIVE>
|
||||
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
|
||||
ar& BOOST_SERIALIZATION_NVP(p);
|
||||
ar& BOOST_SERIALIZATION_NVP(r);
|
||||
ar& BOOST_SERIALIZATION_NVP(g);
|
||||
ar& BOOST_SERIALIZATION_NVP(b);
|
||||
ar& BOOST_SERIALIZATION_NVP(measurements);
|
||||
ar& BOOST_SERIALIZATION_NVP(siftIndices);
|
||||
}
|
||||
/// @}
|
||||
};
|
||||
|
||||
template <typename T>
|
||||
struct traits;
|
||||
|
||||
template <>
|
||||
struct traits<SfmTrack> : public Testable<SfmTrack> {};
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -1149,19 +1149,19 @@ bool writeBAL(const string &filename, SfmData &data) {
|
|||
|
||||
// Write the number of camera poses and 3D points
|
||||
size_t nrObservations = 0;
|
||||
for (size_t j = 0; j < data.number_tracks(); j++) {
|
||||
nrObservations += data.tracks[j].number_measurements();
|
||||
for (size_t j = 0; j < data.nrTracks(); j++) {
|
||||
nrObservations += data.tracks[j].nrMeasurements();
|
||||
}
|
||||
|
||||
// Write observations
|
||||
os << data.number_cameras() << " " << data.number_tracks() << " "
|
||||
os << data.nrCameras() << " " << data.nrTracks() << " "
|
||||
<< nrObservations << endl;
|
||||
os << endl;
|
||||
|
||||
for (size_t j = 0; j < data.number_tracks(); j++) { // for each 3D point j
|
||||
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.number_measurements();
|
||||
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();
|
||||
|
|
@ -1186,7 +1186,7 @@ bool writeBAL(const string &filename, SfmData &data) {
|
|||
os << endl;
|
||||
|
||||
// Write cameras
|
||||
for (size_t i = 0; i < data.number_cameras(); i++) { // for each camera
|
||||
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);
|
||||
|
|
@ -1201,7 +1201,7 @@ bool writeBAL(const string &filename, SfmData &data) {
|
|||
}
|
||||
|
||||
// Write the points
|
||||
for (size_t j = 0; j < data.number_tracks(); j++) { // for each 3D point j
|
||||
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;
|
||||
|
|
@ -1221,8 +1221,8 @@ bool writeBALfromValues(const string &filename, const SfmData &data,
|
|||
// Store poses or cameras in SfmData
|
||||
size_t nrPoses = values.count<Pose3>();
|
||||
if (nrPoses ==
|
||||
dataValues.number_cameras()) { // we only estimated camera poses
|
||||
for (size_t i = 0; i < dataValues.number_cameras();
|
||||
dataValues.nrCameras()) { // we only estimated camera poses
|
||||
for (size_t i = 0; i < dataValues.nrCameras();
|
||||
i++) { // for each camera
|
||||
Pose3 pose = values.at<Pose3>(X(i));
|
||||
Cal3Bundler K = dataValues.cameras[i].calibration();
|
||||
|
|
@ -1231,7 +1231,7 @@ bool writeBALfromValues(const string &filename, const SfmData &data,
|
|||
}
|
||||
} else {
|
||||
size_t nrCameras = values.count<Camera>();
|
||||
if (nrCameras == dataValues.number_cameras()) { // we only estimated camera
|
||||
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);
|
||||
|
|
@ -1241,7 +1241,7 @@ bool writeBALfromValues(const string &filename, const SfmData &data,
|
|||
} else {
|
||||
cout << "writeBALfromValues: different number of cameras in "
|
||||
"SfM_dataValues (#cameras "
|
||||
<< dataValues.number_cameras() << ") and values (#cameras "
|
||||
<< dataValues.nrCameras() << ") and values (#cameras "
|
||||
<< nrPoses << ", #poses " << nrCameras << ")!!" << endl;
|
||||
return false;
|
||||
}
|
||||
|
|
@ -1249,7 +1249,7 @@ bool writeBALfromValues(const string &filename, const SfmData &data,
|
|||
|
||||
// Store 3D points in SfmData
|
||||
size_t nrPoints = values.count<Point3>(),
|
||||
nrTracks = dataValues.number_tracks();
|
||||
nrTracks = dataValues.nrTracks();
|
||||
if (nrPoints != nrTracks) {
|
||||
cout << "writeBALfromValues: different number of points in "
|
||||
"SfM_dataValues (#points= "
|
||||
|
|
|
|||
|
|
@ -22,6 +22,7 @@
|
|||
|
||||
#include <gtsam/sfm/BinaryMeasurement.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/sfm/SfmData.h>
|
||||
#include <gtsam/geometry/Cal3Bundler.h>
|
||||
#include <gtsam/geometry/PinholeCamera.h>
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
|
|
@ -208,196 +209,6 @@ GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph,
|
|||
/// Load TORO 3D Graph
|
||||
GTSAM_EXPORT GraphAndValues load3D(const std::string& filename);
|
||||
|
||||
/// A measurement with its camera index
|
||||
typedef std::pair<size_t, Point2> SfmMeasurement;
|
||||
|
||||
/// Sift index for SfmTrack
|
||||
typedef std::pair<size_t, size_t> SiftIndex;
|
||||
|
||||
/// Define the structure for the 3D points
|
||||
struct SfmTrack {
|
||||
SfmTrack(float r = 0, float g = 0, float b = 0): p(0,0,0), r(r), g(g), b(b) {}
|
||||
SfmTrack(const gtsam::Point3& pt, float r = 0, float g = 0, float b = 0) : p(pt), r(r), g(g), b(b) {}
|
||||
|
||||
Point3 p; ///< 3D position of the point
|
||||
float r, g, b; ///< RGB color of the 3D point
|
||||
std::vector<SfmMeasurement> measurements; ///< The 2D image projections (id,(u,v))
|
||||
std::vector<SiftIndex> siftIndices;
|
||||
|
||||
/// Get RGB values describing 3d point
|
||||
const Point3 rgb() const { return Point3(r, g, b); }
|
||||
|
||||
/// Total number of measurements in this track
|
||||
size_t number_measurements() const {
|
||||
return measurements.size();
|
||||
}
|
||||
/// Get the measurement (camera index, Point2) at pose index `idx`
|
||||
SfmMeasurement measurement(size_t idx) const {
|
||||
return measurements[idx];
|
||||
}
|
||||
/// Get the SIFT feature index corresponding to the measurement at `idx`
|
||||
SiftIndex siftIndex(size_t idx) const {
|
||||
return siftIndices[idx];
|
||||
}
|
||||
/// Get 3D point
|
||||
const Point3& point3() const {
|
||||
return p;
|
||||
}
|
||||
/// Add measurement (camera_idx, Point2) to track
|
||||
void add_measurement(size_t idx, const gtsam::Point2& m) {
|
||||
measurements.emplace_back(idx, m);
|
||||
}
|
||||
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
|
||||
ar & BOOST_SERIALIZATION_NVP(p);
|
||||
ar & BOOST_SERIALIZATION_NVP(r);
|
||||
ar & BOOST_SERIALIZATION_NVP(g);
|
||||
ar & BOOST_SERIALIZATION_NVP(b);
|
||||
ar & BOOST_SERIALIZATION_NVP(measurements);
|
||||
ar & BOOST_SERIALIZATION_NVP(siftIndices);
|
||||
}
|
||||
|
||||
/// assert equality up to a tolerance
|
||||
bool equals(const SfmTrack &sfmTrack, double tol = 1e-9) const {
|
||||
// check the 3D point
|
||||
if (!p.isApprox(sfmTrack.p)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// check the RGB values
|
||||
if (r!=sfmTrack.r || g!=sfmTrack.g || b!=sfmTrack.b) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// compare size of vectors for measurements and siftIndices
|
||||
if (number_measurements() != sfmTrack.number_measurements() ||
|
||||
siftIndices.size() != sfmTrack.siftIndices.size()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// compare measurements (order sensitive)
|
||||
for (size_t idx = 0; idx < number_measurements(); ++idx) {
|
||||
SfmMeasurement measurement = measurements[idx];
|
||||
SfmMeasurement otherMeasurement = sfmTrack.measurements[idx];
|
||||
|
||||
if (measurement.first != otherMeasurement.first ||
|
||||
!measurement.second.isApprox(otherMeasurement.second)) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
// compare sift indices (order sensitive)
|
||||
for (size_t idx = 0; idx < siftIndices.size(); ++idx) {
|
||||
SiftIndex index = siftIndices[idx];
|
||||
SiftIndex otherIndex = sfmTrack.siftIndices[idx];
|
||||
|
||||
if (index.first != otherIndex.first ||
|
||||
index.second != otherIndex.second) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/// print
|
||||
void print(const std::string& s = "") const {
|
||||
std::cout << "Track with " << measurements.size();
|
||||
std::cout << " measurements of point " << p << std::endl;
|
||||
}
|
||||
};
|
||||
|
||||
/* ************************************************************************* */
|
||||
/// traits
|
||||
template<>
|
||||
struct traits<SfmTrack> : public Testable<SfmTrack> {
|
||||
};
|
||||
|
||||
|
||||
/// Define the structure for the camera poses
|
||||
typedef PinholeCamera<Cal3Bundler> SfmCamera;
|
||||
|
||||
/// Define the structure for SfM data
|
||||
struct SfmData {
|
||||
std::vector<SfmCamera> cameras; ///< Set of cameras
|
||||
std::vector<SfmTrack> tracks; ///< Sparse set of points
|
||||
size_t number_cameras() const {
|
||||
return cameras.size();
|
||||
}
|
||||
/// The number of reconstructed 3D points
|
||||
size_t number_tracks() const {
|
||||
return tracks.size();
|
||||
}
|
||||
/// The camera pose at frame index `idx`
|
||||
SfmCamera camera(size_t idx) const {
|
||||
return cameras[idx];
|
||||
}
|
||||
/// The track formed by series of landmark measurements
|
||||
SfmTrack track(size_t idx) const {
|
||||
return tracks[idx];
|
||||
}
|
||||
/// Add a track to SfmData
|
||||
void add_track(const SfmTrack& t) {
|
||||
tracks.push_back(t);
|
||||
}
|
||||
/// Add a camera to SfmData
|
||||
void add_camera(const SfmCamera& cam) {
|
||||
cameras.push_back(cam);
|
||||
}
|
||||
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
void serialize(Archive & ar, const unsigned int /*version*/) {
|
||||
ar & BOOST_SERIALIZATION_NVP(cameras);
|
||||
ar & BOOST_SERIALIZATION_NVP(tracks);
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/// assert equality up to a tolerance
|
||||
bool equals(const SfmData &sfmData, double tol = 1e-9) const {
|
||||
// check number of cameras and tracks
|
||||
if (number_cameras() != sfmData.number_cameras() ||
|
||||
number_tracks() != sfmData.number_tracks()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// check each camera
|
||||
for (size_t i = 0; i < number_cameras(); ++i) {
|
||||
if (!camera(i).equals(sfmData.camera(i), tol)) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
// check each track
|
||||
for (size_t j = 0; j < number_tracks(); ++j) {
|
||||
if (!track(j).equals(sfmData.track(j), tol)) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/// print
|
||||
void print(const std::string& s = "") const {
|
||||
std::cout << "Number of cameras = " << number_cameras() << std::endl;
|
||||
std::cout << "Number of tracks = " << number_tracks() << std::endl;
|
||||
}
|
||||
};
|
||||
|
||||
/* ************************************************************************* */
|
||||
/// traits
|
||||
template<>
|
||||
struct traits<SfmData> : public Testable<SfmData> {
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief This function parses a bundler output file and stores the data into a
|
||||
* SfmData structure
|
||||
|
|
|
|||
Loading…
Reference in New Issue