12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #1078 from borglab/feature/sfm_data

release/4.3a0
Frank Dellaert 2022-01-31 23:36:57 -05:00 committed by GitHub
parent ee4c75ffdf 762e8097bb
commit 236dbcb60b
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
27 changed files with 1106 additions and 890 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
examples/SFMExampleExpressions_bal.cpp
View File

@ -49,7 +49,7 @@ int main(int argc, char* argv[]) {
SfmData mydata; SfmData mydata;
readBAL(filename, mydata); readBAL(filename, mydata);
cout << boost::format("read %1% tracks on %2% cameras\n") % cout << boost::format("read %1% tracks on %2% cameras\n") %
mydata.number_tracks() % mydata.number_cameras(); mydata.numberTracks() % mydata.numberCameras();
// Create a factor graph // Create a factor graph
ExpressionFactorGraph graph; ExpressionFactorGraph graph;

2
examples/SFMExample_bal.cpp
View File

@ -43,7 +43,7 @@ int main (int argc, char* argv[]) {
// Load the SfM data from file // Load the SfM data from file
SfmData mydata; SfmData mydata;
readBAL(filename, mydata); readBAL(filename, mydata);
cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras(); cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.numberTracks() % mydata.numberCameras();
// Create a factor graph // Create a factor graph
NonlinearFactorGraph graph; NonlinearFactorGraph graph;

4
examples/SFMExample_bal_COLAMD_METIS.cpp
View File

@ -48,7 +48,7 @@ int main(int argc, char* argv[]) {
SfmData mydata; SfmData mydata;
readBAL(filename, mydata); readBAL(filename, mydata);
cout << boost::format("read %1% tracks on %2% cameras\n") % cout << boost::format("read %1% tracks on %2% cameras\n") %
mydata.number_tracks() % mydata.number_cameras(); mydata.numberTracks() % mydata.numberCameras();
// Create a factor graph // Create a factor graph
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
@ -131,7 +131,7 @@ int main(int argc, char* argv[]) {
cout << "Time comparison by solving " << filename << " results:" << endl; cout << "Time comparison by solving " << filename << " results:" << endl;
cout << boost::format("%1% point tracks and %2% cameras\n") % cout << boost::format("%1% point tracks and %2% cameras\n") %
mydata.number_tracks() % mydata.number_cameras() mydata.numberTracks() % mydata.numberCameras()
<< endl; << endl;
tictoc_print_(); tictoc_print_();

409
gtsam/sfm/SfmData.cpp Normal file
View File

@ -0,0 +1,409 @@
/* ----------------------------------------------------------------------------
* 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/inference/Symbol.h>
#include <gtsam/sfm/SfmData.h>
#include <fstream>
#include <iostream>
namespace gtsam {
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 = " << cameras.size() << std::endl;
std::cout << "Number of tracks = " << tracks.size() << std::endl;
}
/* ************************************************************************** */
bool SfmData::equals(const SfmData &sfmData, double tol) const {
// check number of cameras and tracks
if (cameras.size() != sfmData.cameras.size() ||
tracks.size() != sfmData.tracks.size()) {
return false;
}
// check each camera
for (size_t i = 0; i < cameras.size(); ++i) {
if (!camera(i).equals(sfmData.camera(i), tol)) {
return false;
}
}
// check each track
for (size_t j = 0; j < tracks.size(); ++j) {
if (!track(j).equals(sfmData.track(j), tol)) {
return false;
}
}
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.tracks.size(); j++) {
nrObservations += data.tracks[j].numberMeasurements();
}
// Write observations
os << data.cameras.size() << " " << data.tracks.size() << " "
<< nrObservations << endl;
os << endl;
for (size_t j = 0; j < data.tracks.size(); j++) { // for each 3D point j
const SfmTrack &track = data.tracks[j];
for (size_t k = 0; k < track.numberMeasurements();
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.cameras.size(); 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.tracks.size(); 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<Cal3Bundler>;
SfmData dataValues = data;
// Store poses or cameras in SfmData
size_t nrPoses = values.count<Pose3>();
if (nrPoses == dataValues.cameras.size()) { // we only estimated camera poses
for (size_t i = 0; i < dataValues.cameras.size(); i++) { // for each camera
Pose3 pose = values.at<Pose3>(X(i));
Cal3Bundler K = dataValues.cameras[i].calibration();
Camera camera(pose, K);
dataValues.cameras[i] = camera;
}
} else {
size_t nrCameras = values.count<Camera>();
if (nrCameras == dataValues.cameras.size()) { // 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<Camera>(cameraKey);
dataValues.cameras[i] = camera;
}
} else {
cout << "writeBALfromValues: different number of cameras in "
"SfM_dataValues (#cameras "
<< dataValues.cameras.size() << ") and values (#cameras " << nrPoses
<< ", #poses " << nrCameras << ")!!" << endl;
return false;
}
}
// Store 3D points in SfmData
size_t nrPoints = values.count<Point3>(), nrTracks = dataValues.tracks.size();
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<Point3>(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

195
gtsam/sfm/SfmData.h Normal file
View File

@ -0,0 +1,195 @@
/* ----------------------------------------------------------------------------
* 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/nonlinear/Values.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 numberTracks() const { return tracks.size(); }
/// The number of cameras
size_t numberCameras() 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> {};
/**
* @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<Cal3Bundler> 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

71
gtsam/sfm/SfmTrack.cpp Normal file
View File

@ -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 (numberMeasurements() != sfmTrack.numberMeasurements() ||
siftIndices.size() != sfmTrack.siftIndices.size()) {
return false;
}
// compare measurements (order sensitive)
for (size_t idx = 0; idx < numberMeasurements(); ++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

133
gtsam/sfm/SfmTrack.h Normal file
View File

@ -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 numberMeasurements() 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

2
gtsam/sfm/ShonanAveraging.h
View File

@ -165,7 +165,7 @@ class GTSAM_EXPORT ShonanAveraging {
size_t nrUnknowns() const { return nrUnknowns_; } size_t nrUnknowns() const { return nrUnknowns_; }
/// Return number of measurements /// Return number of measurements
size_t nrMeasurements() const { return measurements_.size(); } size_t numberMeasurements() const { return measurements_.size(); }
/// k^th binary measurement /// k^th binary measurement
const BinaryMeasurement<Rot> &measurement(size_t k) const { const BinaryMeasurement<Rot> &measurement(size_t k) const {

50
gtsam/sfm/sfm.i
View File

@ -4,7 +4,51 @@
namespace gtsam { namespace gtsam {
// ##### #include <gtsam/sfm/SfmTrack.h>
class SfmTrack {
SfmTrack();
SfmTrack(const gtsam::Point3& pt);
const Point3& point3() const;
double r;
double g;
double b;
std::vector<pair<size_t, gtsam::Point2>> measurements;
size_t numberMeasurements() const;
pair<size_t, gtsam::Point2> measurement(size_t idx) const;
pair<size_t, size_t> siftIndex(size_t idx) const;
void addMeasurement(size_t idx, const gtsam::Point2& m);
// enabling serialization functionality
void serialize() const;
// enabling function to compare objects
bool equals(const gtsam::SfmTrack& expected, double tol) const;
};
#include <gtsam/sfm/SfmData.h>
class SfmData {
SfmData();
size_t numberCameras() const;
size_t numberTracks() const;
gtsam::PinholeCamera<gtsam::Cal3Bundler> camera(size_t idx) const;
gtsam::SfmTrack track(size_t idx) const;
void addTrack(const gtsam::SfmTrack& t);
void addCamera(const gtsam::SfmCamera& cam);
// enabling serialization functionality
void serialize() const;
// enabling function to compare objects
bool equals(const gtsam::SfmData& expected, double tol) const;
};
gtsam::SfmData readBal(string filename);
bool writeBAL(string filename, gtsam::SfmData& data);
gtsam::Values initialCamerasEstimate(const gtsam::SfmData& db);
gtsam::Values initialCamerasAndPointsEstimate(const gtsam::SfmData& db);
#include <gtsam/sfm/ShonanFactor.h> #include <gtsam/sfm/ShonanFactor.h>
@ -92,7 +136,7 @@ class ShonanAveraging2 {
// Query properties // Query properties
size_t nrUnknowns() const; size_t nrUnknowns() const;
size_t nrMeasurements() const; size_t numberMeasurements() const;
gtsam::Rot2 measured(size_t i); gtsam::Rot2 measured(size_t i);
gtsam::KeyVector keys(size_t i); gtsam::KeyVector keys(size_t i);
@ -140,7 +184,7 @@ class ShonanAveraging3 {
// Query properties // Query properties
size_t nrUnknowns() const; size_t nrUnknowns() const;
size_t nrMeasurements() const; size_t numberMeasurements() const;
gtsam::Rot3 measured(size_t i); gtsam::Rot3 measured(size_t i);
gtsam::KeyVector keys(size_t i); gtsam::KeyVector keys(size_t i);

211
gtsam/sfm/tests/testSfmData.cpp Normal file
View File

@ -0,0 +1,211 @@
/* ----------------------------------------------------------------------------
* 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 TestSfmData.cpp
* @date January 2022
* @author Frank dellaert
* @brief tests for SfmData class and associated utilites
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/sfm/SfmData.h>
using namespace std;
using namespace gtsam;
using gtsam::symbol_shorthand::P;
using gtsam::symbol_shorthand::X;
namespace gtsam {
GTSAM_EXPORT std::string createRewrittenFileName(const std::string& name);
GTSAM_EXPORT std::string findExampleDataFile(const std::string& name);
} // namespace gtsam
/* ************************************************************************* */
TEST(dataSet, Balbianello) {
///< The structure where we will save the SfM data
const string filename = findExampleDataFile("Balbianello");
SfmData mydata;
CHECK(readBundler(filename, mydata));
// Check number of things
EXPECT_LONGS_EQUAL(5, mydata.numberCameras());
EXPECT_LONGS_EQUAL(544, mydata.numberTracks());
const SfmTrack& track0 = mydata.tracks[0];
EXPECT_LONGS_EQUAL(3, track0.numberMeasurements());
// Check projection of a given point
EXPECT_LONGS_EQUAL(0, track0.measurements[0].first);
const SfmCamera& camera0 = mydata.cameras[0];
Point2 expected = camera0.project(track0.p),
actual = track0.measurements[0].second;
EXPECT(assert_equal(expected, actual, 1));
}
/* ************************************************************************* */
TEST(dataSet, readBAL_Dubrovnik) {
///< The structure where we will save the SfM data
const string filename = findExampleDataFile("dubrovnik-3-7-pre");
SfmData mydata;
CHECK(readBAL(filename, mydata));
// Check number of things
EXPECT_LONGS_EQUAL(3, mydata.numberCameras());
EXPECT_LONGS_EQUAL(7, mydata.numberTracks());
const SfmTrack& track0 = mydata.tracks[0];
EXPECT_LONGS_EQUAL(3, track0.numberMeasurements());
// Check projection of a given point
EXPECT_LONGS_EQUAL(0, track0.measurements[0].first);
const SfmCamera& camera0 = mydata.cameras[0];
Point2 expected = camera0.project(track0.p),
actual = track0.measurements[0].second;
EXPECT(assert_equal(expected, actual, 12));
}
/* ************************************************************************* */
TEST(dataSet, openGL2gtsam) {
Vector3 rotVec(0.2, 0.7, 1.1);
Rot3 R = Rot3::Expmap(rotVec);
Point3 t = Point3(0.0, 0.0, 0.0);
Pose3 poseGTSAM = Pose3(R, t);
Pose3 expected = openGL2gtsam(R, t.x(), t.y(), t.z());
Point3 r1 = R.r1(), r2 = R.r2(), r3 = R.r3(); // columns!
Rot3 cRw(r1.x(), r2.x(), r3.x(), -r1.y(), -r2.y(), -r3.y(), -r1.z(), -r2.z(),
-r3.z());
Rot3 wRc = cRw.inverse();
Pose3 actual = Pose3(wRc, t);
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST(dataSet, gtsam2openGL) {
Vector3 rotVec(0.2, 0.7, 1.1);
Rot3 R = Rot3::Expmap(rotVec);
Point3 t = Point3(1.0, 20.0, 10.0);
Pose3 actual = Pose3(R, t);
Pose3 poseGTSAM = openGL2gtsam(R, t.x(), t.y(), t.z());
Pose3 expected = gtsam2openGL(poseGTSAM);
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST(dataSet, writeBAL_Dubrovnik) {
///< Read a file using the unit tested readBAL
const string filenameToRead = findExampleDataFile("dubrovnik-3-7-pre");
SfmData readData;
readBAL(filenameToRead, readData);
// Write readData to file filenameToWrite
const string filenameToWrite = createRewrittenFileName(filenameToRead);
CHECK(writeBAL(filenameToWrite, readData));
// Read what we wrote
SfmData writtenData;
CHECK(readBAL(filenameToWrite, writtenData));
// Check that what we read is the same as what we wrote
EXPECT_LONGS_EQUAL(readData.numberCameras(), writtenData.numberCameras());
EXPECT_LONGS_EQUAL(readData.numberTracks(), writtenData.numberTracks());
for (size_t i = 0; i < readData.numberCameras(); 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.numberTracks(); j++) {
// check point
SfmTrack expectedTrack = writtenData.tracks[j];
SfmTrack 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.numberMeasurements(); k++) {
EXPECT_LONGS_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");
SfmData readData;
readBAL(filenameToRead, readData);
Pose3 poseChange =
Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.3, 0.1, 0.3));
Values value;
for (size_t i = 0; i < readData.numberCameras(); i++) { // for each camera
Pose3 pose = poseChange.compose(readData.cameras[i].pose());
value.insert(X(i), pose);
}
for (size_t j = 0; j < readData.numberTracks(); j++) { // for each point
Point3 point = poseChange.transformFrom(readData.tracks[j].p);
value.insert(P(j), point);
}
// Write values and readData to a file
const string filenameToWrite = createRewrittenFileName(filenameToRead);
writeBALfromValues(filenameToWrite, readData, value);
// Read the file we wrote
SfmData 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.numberCameras());
EXPECT_LONGS_EQUAL(7, writtenData.numberTracks());
const SfmTrack& track0 = writtenData.tracks[0];
EXPECT_LONGS_EQUAL(3, track0.numberMeasurements());
// Check projection of a given point
EXPECT_LONGS_EQUAL(0, track0.measurements[0].first);
const SfmCamera& 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();
Pose3 actualPose = value.at<Pose3>(X(0));
EXPECT(assert_equal(expectedPose, actualPose, 1e-7));
Point3 expectedPoint = track0.p;
Point3 actualPoint = value.at<Point3>(P(0));
EXPECT(assert_equal(expectedPoint, actualPoint, 1e-6));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

2
gtsam/slam/SmartFactorBase.h
View File

@ -146,7 +146,7 @@ protected:
*/ */
template<class SFM_TRACK> template<class SFM_TRACK>
void add(const SFM_TRACK& trackToAdd) { void add(const SFM_TRACK& trackToAdd) {
for (size_t k = 0; k < trackToAdd.number_measurements(); k++) { for (size_t k = 0; k < trackToAdd.numberMeasurements(); k++) {
this->measured_.push_back(trackToAdd.measurements[k].second); this->measured_.push_back(trackToAdd.measurements[k].second);
this->keys_.push_back(trackToAdd.measurements[k].first); this->keys_.push_back(trackToAdd.measurements[k].first);
} }

348
gtsam/slam/dataset.cpp
View File

@ -54,8 +54,6 @@
using namespace std; using namespace std;
namespace fs = boost::filesystem; namespace fs = boost::filesystem;
using gtsam::symbol_shorthand::L; using gtsam::symbol_shorthand::L;
using gtsam::symbol_shorthand::P;
using gtsam::symbol_shorthand::X;
#define LINESIZE 81920 #define LINESIZE 81920
@ -945,352 +943,6 @@ GraphAndValues load3D(const string &filename) {
return make_pair(graph, initial); 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.number_tracks(); j++) {
nrObservations += data.tracks[j].number_measurements();
}
// Write observations
os << data.number_cameras() << " " << data.number_tracks() << " "
<< nrObservations << endl;
os << endl;
for (size_t j = 0; j < data.number_tracks(); j++) { // for each 3D point j
const SfmTrack &track = data.tracks[j];
for (size_t k = 0; k < track.number_measurements();
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.number_cameras(); 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.number_tracks(); 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<Cal3Bundler>;
SfmData dataValues = 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();
i++) { // for each camera
Pose3 pose = values.at<Pose3>(X(i));
Cal3Bundler K = dataValues.cameras[i].calibration();
Camera camera(pose, K);
dataValues.cameras[i] = camera;
}
} else {
size_t nrCameras = values.count<Camera>();
if (nrCameras == dataValues.number_cameras()) { // 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<Camera>(cameraKey);
dataValues.cameras[i] = camera;
}
} else {
cout << "writeBALfromValues: different number of cameras in "
"SfM_dataValues (#cameras "
<< dataValues.number_cameras() << ") and values (#cameras "
<< nrPoses << ", #poses " << nrCameras << ")!!" << endl;
return false;
}
}
// Store 3D points in SfmData
size_t nrPoints = values.count<Point3>(),
nrTracks = dataValues.number_tracks();
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<Point3>(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<Pose2> and parseFactors<Pose2> // Wrapper-friendly versions of parseFactors<Pose2> and parseFactors<Pose2>
BetweenFactorPose2s BetweenFactorPose2s
parse2DFactors(const std::string &filename, parse2DFactors(const std::string &filename,

281
gtsam/slam/dataset.h
View File

@ -22,6 +22,7 @@
#include <gtsam/sfm/BinaryMeasurement.h> #include <gtsam/sfm/BinaryMeasurement.h>
#include <gtsam/slam/BetweenFactor.h> #include <gtsam/slam/BetweenFactor.h>
#include <gtsam/sfm/SfmData.h>
#include <gtsam/geometry/Cal3Bundler.h> #include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/PinholeCamera.h> #include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Pose2.h> #include <gtsam/geometry/Pose2.h>
@ -208,286 +209,6 @@ GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph,
/// Load TORO 3D Graph /// Load TORO 3D Graph
GTSAM_EXPORT GraphAndValues load3D(const std::string& filename); 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
* @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<Cal3Bundler> 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<Pose2> and parseFactors<Pose2> // Wrapper-friendly versions of parseFactors<Pose2> and parseFactors<Pose2>
using BetweenFactorPose2s = std::vector<BetweenFactor<Pose2>::shared_ptr>; using BetweenFactorPose2s = std::vector<BetweenFactor<Pose2>::shared_ptr>;
GTSAM_EXPORT BetweenFactorPose2s GTSAM_EXPORT BetweenFactorPose2s

44
gtsam/slam/slam.i
View File

@ -209,50 +209,6 @@ virtual class EssentialMatrixConstraint : gtsam::NoiseModelFactor {
#include <gtsam/slam/dataset.h> #include <gtsam/slam/dataset.h>
class SfmTrack {
SfmTrack();
SfmTrack(const gtsam::Point3& pt);
const Point3& point3() const;
double r;
double g;
double b;
std::vector<pair<size_t, gtsam::Point2>> measurements;
size_t number_measurements() const;
pair<size_t, gtsam::Point2> measurement(size_t idx) const;
pair<size_t, size_t> siftIndex(size_t idx) const;
void add_measurement(size_t idx, const gtsam::Point2& m);
// enabling serialization functionality
void serialize() const;
// enabling function to compare objects
bool equals(const gtsam::SfmTrack& expected, double tol) const;
};
class SfmData {
SfmData();
size_t number_cameras() const;
size_t number_tracks() const;
gtsam::PinholeCamera<gtsam::Cal3Bundler> camera(size_t idx) const;
gtsam::SfmTrack track(size_t idx) const;
void add_track(const gtsam::SfmTrack& t);
void add_camera(const gtsam::SfmCamera& cam);
// enabling serialization functionality
void serialize() const;
// enabling function to compare objects
bool equals(const gtsam::SfmData& expected, double tol) const;
};
gtsam::SfmData readBal(string filename);
bool writeBAL(string filename, gtsam::SfmData& data);
gtsam::Values initialCamerasEstimate(const gtsam::SfmData& db);
gtsam::Values initialCamerasAndPointsEstimate(const gtsam::SfmData& db);
enum NoiseFormat { enum NoiseFormat {
NoiseFormatG2O, NoiseFormatG2O,
NoiseFormatTORO, NoiseFormatTORO,

175
gtsam/slam/tests/testDataset.cpp
View File

@ -151,27 +151,6 @@ TEST(dataSet, load2DVictoriaPark) {
EXPECT_LONGS_EQUAL(L(5), graph->at(4)->keys()[1]); EXPECT_LONGS_EQUAL(L(5), graph->at(4)->keys()[1]);
} }
/* ************************************************************************* */
TEST( dataSet, Balbianello)
{
///< The structure where we will save the SfM data
const string filename = findExampleDataFile("Balbianello");
SfmData mydata;
CHECK(readBundler(filename, mydata));
// Check number of things
EXPECT_LONGS_EQUAL(5,mydata.number_cameras());
EXPECT_LONGS_EQUAL(544,mydata.number_tracks());
const SfmTrack& 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 SfmCamera& camera0 = mydata.cameras[0];
Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second;
EXPECT(assert_equal(expected,actual,1));
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST(dataSet, readG2o3D) { TEST(dataSet, readG2o3D) {
const string g2oFile = findExampleDataFile("pose3example"); const string g2oFile = findExampleDataFile("pose3example");
@ -461,160 +440,6 @@ TEST( dataSet, writeG2o3DNonDiagonalNoise)
EXPECT(assert_equal(*expectedGraph,*actualGraph,1e-4)); EXPECT(assert_equal(*expectedGraph,*actualGraph,1e-4));
} }
/* ************************************************************************* */
TEST( dataSet, readBAL_Dubrovnik)
{
///< The structure where we will save the SfM data
const string filename = findExampleDataFile("dubrovnik-3-7-pre");
SfmData mydata;
CHECK(readBAL(filename, mydata));
// Check number of things
EXPECT_LONGS_EQUAL(3,mydata.number_cameras());
EXPECT_LONGS_EQUAL(7,mydata.number_tracks());
const SfmTrack& 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 SfmCamera& camera0 = mydata.cameras[0];
Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second;
EXPECT(assert_equal(expected,actual,12));
}
/* ************************************************************************* */
TEST( dataSet, openGL2gtsam)
{
Vector3 rotVec(0.2, 0.7, 1.1);
Rot3 R = Rot3::Expmap(rotVec);
Point3 t = Point3(0.0,0.0,0.0);
Pose3 poseGTSAM = Pose3(R,t);
Pose3 expected = openGL2gtsam(R, t.x(), t.y(), t.z());
Point3 r1 = R.r1(), r2 = R.r2(), r3 = R.r3(); //columns!
Rot3 cRw(
r1.x(), r2.x(), r3.x(),
-r1.y(), -r2.y(), -r3.y(),
-r1.z(), -r2.z(), -r3.z());
Rot3 wRc = cRw.inverse();
Pose3 actual = Pose3(wRc,t);
EXPECT(assert_equal(expected,actual));
}
/* ************************************************************************* */
TEST( dataSet, gtsam2openGL)
{
Vector3 rotVec(0.2, 0.7, 1.1);
Rot3 R = Rot3::Expmap(rotVec);
Point3 t = Point3(1.0,20.0,10.0);
Pose3 actual = Pose3(R,t);
Pose3 poseGTSAM = openGL2gtsam(R, t.x(), t.y(), t.z());
Pose3 expected = gtsam2openGL(poseGTSAM);
EXPECT(assert_equal(expected,actual));
}
/* ************************************************************************* */
TEST( dataSet, writeBAL_Dubrovnik)
{
///< Read a file using the unit tested readBAL
const string filenameToRead = findExampleDataFile("dubrovnik-3-7-pre");
SfmData readData;
readBAL(filenameToRead, readData);
// Write readData to file filenameToWrite
const string filenameToWrite = createRewrittenFileName(filenameToRead);
CHECK(writeBAL(filenameToWrite, readData));
// Read what we wrote
SfmData writtenData;
CHECK(readBAL(filenameToWrite, writtenData));
// Check that what we read is the same as what we wrote
EXPECT_LONGS_EQUAL(readData.number_cameras(),writtenData.number_cameras());
EXPECT_LONGS_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
SfmTrack expectedTrack = writtenData.tracks[j];
SfmTrack 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_LONGS_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");
SfmData readData;
readBAL(filenameToRead, readData);
Pose3 poseChange = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.3,0.1,0.3));
Values value;
for(size_t i=0; i < readData.number_cameras(); i++){ // for each camera
Pose3 pose = poseChange.compose(readData.cameras[i].pose());
value.insert(X(i), pose);
}
for(size_t j=0; j < readData.number_tracks(); j++){ // for each point
Point3 point = poseChange.transformFrom( readData.tracks[j].p );
value.insert(P(j), point);
}
// Write values and readData to a file
const string filenameToWrite = createRewrittenFileName(filenameToRead);
writeBALfromValues(filenameToWrite, readData, value);
// Read the file we wrote
SfmData 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 SfmTrack& 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 SfmCamera& 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();
Pose3 actualPose = value.at<Pose3>(X(0));
EXPECT(assert_equal(expectedPose,actualPose, 1e-7));
Point3 expectedPoint = track0.p;
Point3 actualPoint = value.at<Point3>(P(0));
EXPECT(assert_equal(expectedPoint,actualPoint, 1e-6));
}
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); } int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */ /* ************************************************************************* */

6
gtsam/slam/tests/testEssentialMatrixFactor.cpp
View File

@ -632,14 +632,14 @@ TEST(EssentialMatrixFactor2, extraMinimization) {
// We start with a factor graph and add constraints to it // We start with a factor graph and add constraints to it
// Noise sigma is 1, assuming pixel measurements // Noise sigma is 1, assuming pixel measurements
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
for (size_t i = 0; i < data.number_tracks(); i++) for (size_t i = 0; i < data.numberTracks(); i++)
graph.emplace_shared<EssentialMatrixFactor2>(100, i, pA(i), pB(i), model2, graph.emplace_shared<EssentialMatrixFactor2>(100, i, pA(i), pB(i), model2,
K); K);
// Check error at ground truth // Check error at ground truth
Values truth; Values truth;
truth.insert(100, trueE); truth.insert(100, trueE);
for (size_t i = 0; i < data.number_tracks(); i++) { for (size_t i = 0; i < data.numberTracks(); i++) {
Point3 P1 = data.tracks[i].p; Point3 P1 = data.tracks[i].p;
truth.insert(i, double(baseline / P1.z())); truth.insert(i, double(baseline / P1.z()));
} }
@ -654,7 +654,7 @@ TEST(EssentialMatrixFactor2, extraMinimization) {
// Check result // Check result
EssentialMatrix actual = result.at<EssentialMatrix>(100); EssentialMatrix actual = result.at<EssentialMatrix>(100);
EXPECT(assert_equal(trueE, actual, 1e-1)); EXPECT(assert_equal(trueE, actual, 1e-1));
for (size_t i = 0; i < data.number_tracks(); i++) for (size_t i = 0; i < data.numberTracks(); i++)
EXPECT_DOUBLES_EQUAL(truth.at<double>(i), result.at<double>(i), 1e-1); EXPECT_DOUBLES_EQUAL(truth.at<double>(i), result.at<double>(i), 1e-1);
// Check error at result // Check error at result

7
python/gtsam/examples/SFMExample.py
View File

@ -8,7 +8,6 @@ See LICENSE for the license information
A structure-from-motion problem on a simulated dataset A structure-from-motion problem on a simulated dataset
""" """
from __future__ import print_function
import gtsam import gtsam
import matplotlib.pyplot as plt import matplotlib.pyplot as plt
@ -89,7 +88,7 @@ def main():
point_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) point_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
factor = PriorFactorPoint3(L(0), points[0], point_noise) factor = PriorFactorPoint3(L(0), points[0], point_noise)
graph.push_back(factor) graph.push_back(factor)
graph.print_('Factor Graph:\n') graph.print('Factor Graph:\n')
# Create the data structure to hold the initial estimate to the solution # Create the data structure to hold the initial estimate to the solution
# Intentionally initialize the variables off from the ground truth # Intentionally initialize the variables off from the ground truth
@ -100,7 +99,7 @@ def main():
for j, point in enumerate(points): for j, point in enumerate(points):
transformed_point = point + 0.1*np.random.randn(3) transformed_point = point + 0.1*np.random.randn(3)
initial_estimate.insert(L(j), transformed_point) initial_estimate.insert(L(j), transformed_point)
initial_estimate.print_('Initial Estimates:\n') initial_estimate.print('Initial Estimates:\n')
# Optimize the graph and print results # Optimize the graph and print results
params = gtsam.DoglegParams() params = gtsam.DoglegParams()
@ -108,7 +107,7 @@ def main():
optimizer = DoglegOptimizer(graph, initial_estimate, params) optimizer = DoglegOptimizer(graph, initial_estimate, params)
print('Optimizing:') print('Optimizing:')
result = optimizer.optimize() result = optimizer.optimize()
result.print_('Final results:\n') result.print('Final results:\n')
print('initial error = {}'.format(graph.error(initial_estimate))) print('initial error = {}'.format(graph.error(initial_estimate)))
print('final error = {}'.format(graph.error(result))) print('final error = {}'.format(graph.error(result)))

16
python/gtsam/examples/SFMExample_bal.py
View File

@ -29,10 +29,10 @@ DEFAULT_BAL_DATASET = "dubrovnik-3-7-pre"
def plot_scene(scene_data: gtsam.SfmData, result: gtsam.Values) -> None: def plot_scene(scene_data: gtsam.SfmData, result: gtsam.Values) -> None:
"""Plot the SFM results.""" """Plot the SFM results."""
plot_vals = gtsam.Values() plot_vals = gtsam.Values()
for cam_idx in range(scene_data.number_cameras()): for cam_idx in range(scene_data.numberCameras()):
plot_vals.insert(C(cam_idx), plot_vals.insert(C(cam_idx),
result.atPinholeCameraCal3Bundler(C(cam_idx)).pose()) result.atPinholeCameraCal3Bundler(C(cam_idx)).pose())
for j in range(scene_data.number_tracks()): for j in range(scene_data.numberTracks()):
plot_vals.insert(P(j), result.atPoint3(P(j))) plot_vals.insert(P(j), result.atPoint3(P(j)))
plot.plot_3d_points(0, plot_vals, linespec="g.") plot.plot_3d_points(0, plot_vals, linespec="g.")
@ -47,8 +47,8 @@ def run(args: argparse.Namespace) -> None:
# Load the SfM data from file # Load the SfM data from file
scene_data = gtsam.readBal(input_file) scene_data = gtsam.readBal(input_file)
logging.info("read %d tracks on %d cameras\n", scene_data.number_tracks(), logging.info("read %d tracks on %d cameras\n", scene_data.numberTracks(),
scene_data.number_cameras()) scene_data.numberCameras())
# Create a factor graph # Create a factor graph
graph = gtsam.NonlinearFactorGraph() graph = gtsam.NonlinearFactorGraph()
@ -57,10 +57,10 @@ def run(args: argparse.Namespace) -> None:
noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v
# Add measurements to the factor graph # Add measurements to the factor graph
for j in range(scene_data.number_tracks()): for j in range(scene_data.numberTracks()):
track = scene_data.track(j) # SfmTrack track = scene_data.track(j) # SfmTrack
# retrieve the SfmMeasurement objects # retrieve the SfmMeasurement objects
for m_idx in range(track.number_measurements()): for m_idx in range(track.numberMeasurements()):
# i represents the camera index, and uv is the 2d measurement # i represents the camera index, and uv is the 2d measurement
i, uv = track.measurement(m_idx) i, uv = track.measurement(m_idx)
# note use of shorthand symbols C and P # note use of shorthand symbols C and P
@ -82,13 +82,13 @@ def run(args: argparse.Namespace) -> None:
i = 0 i = 0
# add each PinholeCameraCal3Bundler # add each PinholeCameraCal3Bundler
for cam_idx in range(scene_data.number_cameras()): for cam_idx in range(scene_data.numberCameras()):
camera = scene_data.camera(cam_idx) camera = scene_data.camera(cam_idx)
initial.insert(C(i), camera) initial.insert(C(i), camera)
i += 1 i += 1
# add each SfmTrack # add each SfmTrack
for j in range(scene_data.number_tracks()): for j in range(scene_data.numberTracks()):
track = scene_data.track(j) track = scene_data.track(j)
initial.insert(P(j), track.point3()) initial.insert(P(j), track.point3())

18
python/gtsam/tests/test_SfmData.py
View File

@ -39,10 +39,10 @@ class TestSfmData(GtsamTestCase):
# translating point uv_i1 along X-axis # translating point uv_i1 along X-axis
uv_i2 = gtsam.Point2(24.88, 82) uv_i2 = gtsam.Point2(24.88, 82)
# add measurements to the track # add measurements to the track
self.tracks.add_measurement(i1, uv_i1) self.tracks.addMeasurement(i1, uv_i1)
self.tracks.add_measurement(i2, uv_i2) self.tracks.addMeasurement(i2, uv_i2)
# Number of measurements in the track is 2 # Number of measurements in the track is 2
self.assertEqual(self.tracks.number_measurements(), 2) self.assertEqual(self.tracks.numberMeasurements(), 2)
# camera_idx in the first measurement of the track corresponds to i1 # camera_idx in the first measurement of the track corresponds to i1
cam_idx, img_measurement = self.tracks.measurement(0) cam_idx, img_measurement = self.tracks.measurement(0)
self.assertEqual(cam_idx, i1) self.assertEqual(cam_idx, i1)
@ -64,13 +64,13 @@ class TestSfmData(GtsamTestCase):
measurements = [(i1, uv_i1), (i2, uv_i2), (i3, uv_i3)] measurements = [(i1, uv_i1), (i2, uv_i2), (i3, uv_i3)]
pt = gtsam.Point3(1.0, 6.0, 2.0) pt = gtsam.Point3(1.0, 6.0, 2.0)
track2 = gtsam.SfmTrack(pt) track2 = gtsam.SfmTrack(pt)
track2.add_measurement(i1, uv_i1) track2.addMeasurement(i1, uv_i1)
track2.add_measurement(i2, uv_i2) track2.addMeasurement(i2, uv_i2)
track2.add_measurement(i3, uv_i3) track2.addMeasurement(i3, uv_i3)
self.data.add_track(self.tracks) self.data.addTrack(self.tracks)
self.data.add_track(track2) self.data.addTrack(track2)
# Number of tracks in SfmData is 2 # Number of tracks in SfmData is 2
self.assertEqual(self.data.number_tracks(), 2) self.assertEqual(self.data.numberTracks(), 2)
# camera idx of first measurement of second track corresponds to i1 # camera idx of first measurement of second track corresponds to i1
cam_idx, img_measurement = self.data.track(1).measurement(0) cam_idx, img_measurement = self.data.track(1).measurement(0)
self.assertEqual(cam_idx, i1) self.assertEqual(cam_idx, i1)

4
python/gtsam/tests/test_pickle.py
View File

@ -37,8 +37,8 @@ class TestPickle(GtsamTestCase):
def test_sfmTrack_roundtrip(self): def test_sfmTrack_roundtrip(self):
obj = SfmTrack(Point3(1, 1, 0)) obj = SfmTrack(Point3(1, 1, 0))
obj.add_measurement(0, Point2(-1, 5)) obj.addMeasurement(0, Point2(-1, 5))
obj.add_measurement(1, Point2(6, 2)) obj.addMeasurement(1, Point2(6, 2))
self.assertEqualityOnPickleRoundtrip(obj) self.assertEqualityOnPickleRoundtrip(obj)
def test_unit3_roundtrip(self): def test_unit3_roundtrip(self):

2
tests/testGeneralSFMFactorB.cpp
View File

@ -49,7 +49,7 @@ TEST(PinholeCamera, BAL) {
SharedNoiseModel unit2 = noiseModel::Unit::Create(2); SharedNoiseModel unit2 = noiseModel::Unit::Create(2);
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
for (size_t j = 0; j < db.number_tracks(); j++) { for (size_t j = 0; j < db.numberTracks(); j++) {
for (const SfmMeasurement& m: db.tracks[j].measurements) for (const SfmMeasurement& m: db.tracks[j].measurements)
graph.emplace_shared<sfmFactor>(m.second, unit2, m.first, P(j)); graph.emplace_shared<sfmFactor>(m.second, unit2, m.first, P(j));
} }

2
timing/timeSFMBAL.cpp
View File

@ -36,7 +36,7 @@ int main(int argc, char* argv[]) {
// Build graph using conventional GeneralSFMFactor // Build graph using conventional GeneralSFMFactor
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
for (size_t j = 0; j < db.number_tracks(); j++) { for (size_t j = 0; j < db.numberTracks(); j++) {
for (const SfmMeasurement& m: db.tracks[j].measurements) { for (const SfmMeasurement& m: db.tracks[j].measurements) {
size_t i = m.first; size_t i = m.first;
Point2 z = m.second; Point2 z = m.second;

4
timing/timeSFMBAL.h
View File

@ -73,8 +73,8 @@ int optimize(const SfmData& db, const NonlinearFactorGraph& graph,
if (gUseSchur) { if (gUseSchur) {
// Create Schur-complement ordering // Create Schur-complement ordering
Ordering ordering; Ordering ordering;
for (size_t j = 0; j < db.number_tracks(); j++) ordering.push_back(P(j)); for (size_t j = 0; j < db.numberTracks(); j++) ordering.push_back(P(j));
for (size_t i = 0; i < db.number_cameras(); i++) { for (size_t i = 0; i < db.numberCameras(); i++) {
ordering.push_back(C(i)); ordering.push_back(C(i));
if (separateCalibration) ordering.push_back(K(i)); if (separateCalibration) ordering.push_back(K(i));
} }

2
timing/timeSFMBALautodiff.cpp
View File

@ -44,7 +44,7 @@ int main(int argc, char* argv[]) {
// Build graph // Build graph
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
for (size_t j = 0; j < db.number_tracks(); j++) { for (size_t j = 0; j < db.numberTracks(); j++) {
for (const SfmMeasurement& m: db.tracks[j].measurements) { for (const SfmMeasurement& m: db.tracks[j].measurements) {
size_t i = m.first; size_t i = m.first;
Point2 z = m.second; Point2 z = m.second;

2
timing/timeSFMBALcamTnav.cpp
View File

@ -33,7 +33,7 @@ int main(int argc, char* argv[]) {
// Build graph using conventional GeneralSFMFactor // Build graph using conventional GeneralSFMFactor
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
for (size_t j = 0; j < db.number_tracks(); j++) { for (size_t j = 0; j < db.numberTracks(); j++) {
for (const SfmMeasurement& m: db.tracks[j].measurements) { for (const SfmMeasurement& m: db.tracks[j].measurements) {
size_t i = m.first; size_t i = m.first;
Point2 z = m.second; Point2 z = m.second;

2
timing/timeSFMBALnavTcam.cpp
View File

@ -33,7 +33,7 @@ int main(int argc, char* argv[]) {
// Build graph using conventional GeneralSFMFactor // Build graph using conventional GeneralSFMFactor
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
for (size_t j = 0; j < db.number_tracks(); j++) { for (size_t j = 0; j < db.numberTracks(); j++) {
Point3_ nav_point_(P(j)); Point3_ nav_point_(P(j));
for (const SfmMeasurement& m: db.tracks[j].measurements) { for (const SfmMeasurement& m: db.tracks[j].measurements) {
size_t i = m.first; size_t i = m.first;

2
timing/timeSFMBALsmart.cpp
View File

@ -35,7 +35,7 @@ int main(int argc, char* argv[]) {
// Add smart factors to graph // Add smart factors to graph
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
for (size_t j = 0; j < db.number_tracks(); j++) { for (size_t j = 0; j < db.numberTracks(); j++) {
auto smartFactor = boost::make_shared<SfmFactor>(gNoiseModel); auto smartFactor = boost::make_shared<SfmFactor>(gNoiseModel);
for (const SfmMeasurement& m : db.tracks[j].measurements) { for (const SfmMeasurement& m : db.tracks[j].measurements) {
size_t i = m.first; size_t i = m.first;