SfmData.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
19 #include <gtsam/inference/Symbol.h>
20 #include <gtsam/sfm/SfmData.h>
22 
23 #include <fstream>
24 #include <iostream>
25 
26 namespace gtsam {
27 
28 using std::cout;
29 using std::endl;
30 
32 
33 /* ************************************************************************** */
34 void SfmData::print(const std::string &s) const {
35  std::cout << "Number of cameras = " << cameras.size() << std::endl;
36  std::cout << "Number of tracks = " << tracks.size() << std::endl;
37 }
38 
39 /* ************************************************************************** */
40 bool SfmData::equals(const SfmData &sfmData, double tol) const {
41  // check number of cameras and tracks
42  if (cameras.size() != sfmData.cameras.size() ||
43  tracks.size() != sfmData.tracks.size()) {
44  return false;
45  }
46 
47  // check each camera
48  for (size_t i = 0; i < cameras.size(); ++i) {
49  if (!camera(i).equals(sfmData.camera(i), tol)) {
50  return false;
51  }
52  }
53 
54  // check each track
55  for (size_t j = 0; j < tracks.size(); ++j) {
56  if (!track(j).equals(sfmData.track(j), tol)) {
57  return false;
58  }
59  }
60 
61  return true;
62 }
63 
64 /* ************************************************************************* */
65 Rot3 openGLFixedRotation() { // this is due to different convention for
66  // cameras in gtsam and openGL
67  /* R = [ 1 0 0
68  * 0 -1 0
69  * 0 0 -1]
70  */
71  Matrix3 R_mat = Matrix3::Zero(3, 3);
72  R_mat(0, 0) = 1.0;
73  R_mat(1, 1) = -1.0;
74  R_mat(2, 2) = -1.0;
75  return Rot3(R_mat);
76 }
77 
78 /* ************************************************************************* */
79 Pose3 openGL2gtsam(const Rot3 &R, double tx, double ty, double tz) {
80  Rot3 R90 = openGLFixedRotation();
81  Rot3 wRc = (R.inverse()).compose(R90);
82 
83  // Our camera-to-world translation wTc = -R'*t
84  return Pose3(wRc, R.unrotate(Point3(-tx, -ty, -tz)));
85 }
86 
87 /* ************************************************************************* */
88 Pose3 gtsam2openGL(const Rot3 &R, double tx, double ty, double tz) {
89  Rot3 R90 = openGLFixedRotation();
90  Rot3 cRw_openGL = R90.compose(R.inverse());
91  Point3 t_openGL = cRw_openGL.rotate(Point3(-tx, -ty, -tz));
92  return Pose3(cRw_openGL, t_openGL);
93 }
94 
95 /* ************************************************************************* */
96 Pose3 gtsam2openGL(const Pose3 &PoseGTSAM) {
97  return gtsam2openGL(PoseGTSAM.rotation(), PoseGTSAM.x(), PoseGTSAM.y(),
98  PoseGTSAM.z());
99 }
100 
101 /* ************************************************************************** */
103  // Load the data file
104  std::ifstream is(filename.c_str(), std::ifstream::in);
105  if (!is) {
106  throw std::runtime_error(
107  "Error in FromBundlerFile: can not find the file!!");
108  }
109 
110  SfmData sfmData;
111 
112  // Ignore the first line
113  char aux[500];
114  is.getline(aux, 500);
115 
116  // Get the number of camera poses and 3D points
117  size_t nrPoses, nrPoints;
118  is >> nrPoses >> nrPoints;
119 
120  // Get the information for the camera poses
121  for (size_t i = 0; i < nrPoses; i++) {
122  // Get the focal length and the radial distortion parameters
123  float f, k1, k2;
124  is >> f >> k1 >> k2;
125  Cal3Bundler K(f, k1, k2);
126 
127  // Get the rotation matrix
128  float r11, r12, r13;
129  float r21, r22, r23;
130  float r31, r32, r33;
131  is >> r11 >> r12 >> r13 >> r21 >> r22 >> r23 >> r31 >> r32 >> r33;
132 
133  // Bundler-OpenGL rotation matrix
134  Rot3 R(r11, r12, r13, r21, r22, r23, r31, r32, r33);
135 
136  // Check for all-zero R, in which case quit
137  if (r11 == 0 && r12 == 0 && r13 == 0) {
138  throw std::runtime_error(
139  "Error in FromBundlerFile: zero rotation matrix");
140  }
141 
142  // Get the translation vector
143  float tx, ty, tz;
144  is >> tx >> ty >> tz;
145 
146  Pose3 pose = openGL2gtsam(R, tx, ty, tz);
147 
148  sfmData.cameras.emplace_back(pose, K);
149  }
150 
151  // Get the information for the 3D points
152  sfmData.tracks.reserve(nrPoints);
153  for (size_t j = 0; j < nrPoints; j++) {
154  SfmTrack track;
155 
156  // Get the 3D position
157  float x, y, z;
158  is >> x >> y >> z;
159  track.p = Point3(x, y, z);
160 
161  // Get the color information
162  float r, g, b;
163  is >> r >> g >> b;
164  track.r = r / 255.f;
165  track.g = g / 255.f;
166  track.b = b / 255.f;
167 
168  // Now get the visibility information
169  size_t nvisible = 0;
170  is >> nvisible;
171 
172  track.measurements.reserve(nvisible);
173  track.siftIndices.reserve(nvisible);
174  for (size_t k = 0; k < nvisible; k++) {
175  size_t cam_idx = 0, point_idx = 0;
176  float u, v;
177  is >> cam_idx >> point_idx >> u >> v;
178  track.measurements.emplace_back(cam_idx, Point2(u, -v));
179  track.siftIndices.emplace_back(cam_idx, point_idx);
180  }
181 
182  sfmData.tracks.push_back(track);
183  }
184 
185  return sfmData;
186 }
187 
188 /* ************************************************************************** */
189 SfmData SfmData::FromBalFile(const std::string &filename) {
190  // Load the data file
191  std::ifstream is(filename.c_str(), std::ifstream::in);
192  if (!is) {
193  throw std::runtime_error("Error in FromBalFile: can not find the file!!");
194  }
195 
196  SfmData sfmData;
197 
198  // Get the number of camera poses and 3D points
199  size_t nrPoses, nrPoints, nrObservations;
200  is >> nrPoses >> nrPoints >> nrObservations;
201 
202  sfmData.tracks.resize(nrPoints);
203 
204  // Get the information for the observations
205  for (size_t k = 0; k < nrObservations; k++) {
206  size_t i = 0, j = 0;
207  float u, v;
208  is >> i >> j >> u >> v;
209  sfmData.tracks[j].measurements.emplace_back(i, Point2(u, -v));
210  }
211 
212  // Get the information for the camera poses
213  for (size_t i = 0; i < nrPoses; i++) {
214  // Get the Rodrigues vector
215  float wx, wy, wz;
216  is >> wx >> wy >> wz;
217  Rot3 R = Rot3::Rodrigues(wx, wy, wz); // BAL-OpenGL rotation matrix
218 
219  // Get the translation vector
220  float tx, ty, tz;
221  is >> tx >> ty >> tz;
222 
223  Pose3 pose = openGL2gtsam(R, tx, ty, tz);
224 
225  // Get the focal length and the radial distortion parameters
226  float f, k1, k2;
227  is >> f >> k1 >> k2;
228  Cal3Bundler K(f, k1, k2);
229 
230  sfmData.cameras.emplace_back(pose, K);
231  }
232 
233  // Get the information for the 3D points
234  for (size_t j = 0; j < nrPoints; j++) {
235  // Get the 3D position
236  float x, y, z;
237  is >> x >> y >> z;
238  SfmTrack &track = sfmData.tracks[j];
239  track.p = Point3(x, y, z);
240  track.r = 0.4f;
241  track.g = 0.4f;
242  track.b = 0.4f;
243  }
244 
245  return sfmData;
246 }
247 
248 /* ************************************************************************** */
249 bool writeBAL(const std::string &filename, const SfmData &data) {
250  // Open the output file
251  std::ofstream os;
252  os.open(filename.c_str());
253  os.precision(20);
254  if (!os.is_open()) {
255  cout << "Error in writeBAL: can not open the file!!" << endl;
256  return false;
257  }
258 
259  // Write the number of camera poses and 3D points
260  size_t nrObservations = 0;
261  for (size_t j = 0; j < data.tracks.size(); j++) {
262  nrObservations += data.tracks[j].numberMeasurements();
263  }
264 
265  // Write observations
266  os << data.cameras.size() << " " << data.tracks.size() << " "
267  << nrObservations << endl;
268  os << endl;
269 
270  for (size_t j = 0; j < data.tracks.size(); j++) { // for each 3D point j
271  const SfmTrack &track = data.tracks[j];
272 
273  for (size_t k = 0; k < track.numberMeasurements();
274  k++) { // for each observation of the 3D point j
275  size_t i = track.measurements[k].first; // camera id
276  double u0 = data.cameras[i].calibration().px();
277  double v0 = data.cameras[i].calibration().py();
278 
279  if (u0 != 0 || v0 != 0) {
280  cout << "writeBAL has not been tested for calibration with nonzero "
281  "(u0,v0)"
282  << endl;
283  }
284 
285  double pixelBALx = track.measurements[k].second.x() -
286  u0; // center of image is the origin
287  double pixelBALy = -(track.measurements[k].second.y() -
288  v0); // center of image is the origin
289  Point2 pixelMeasurement(pixelBALx, pixelBALy);
290  os << i /*camera id*/ << " " << j /*point id*/ << " "
291  << pixelMeasurement.x() /*u of the pixel*/ << " "
292  << pixelMeasurement.y() /*v of the pixel*/ << endl;
293  }
294  }
295  os << endl;
296 
297  // Write cameras
298  for (size_t i = 0; i < data.cameras.size(); i++) { // for each camera
299  Pose3 poseGTSAM = data.cameras[i].pose();
300  Cal3Bundler cameraCalibration = data.cameras[i].calibration();
301  Pose3 poseOpenGL = gtsam2openGL(poseGTSAM);
302  os << Rot3::Logmap(poseOpenGL.rotation()) << endl;
303  os << poseOpenGL.translation().x() << endl;
304  os << poseOpenGL.translation().y() << endl;
305  os << poseOpenGL.translation().z() << endl;
306  os << cameraCalibration.fx() << endl;
307  os << cameraCalibration.k1() << endl;
308  os << cameraCalibration.k2() << endl;
309  os << endl;
310  }
311 
312  // Write the points
313  for (size_t j = 0; j < data.tracks.size(); j++) { // for each 3D point j
314  Point3 point = data.tracks[j].p;
315  os << point.x() << endl;
316  os << point.y() << endl;
317  os << point.z() << endl;
318  os << endl;
319  }
320 
321  os.close();
322  return true;
323 }
324 
325 SfmData readBal(const std::string &filename) {
326  return SfmData::FromBalFile(filename);
327 }
328 
329 /* ************************************************************************** */
330 bool writeBALfromValues(const std::string &filename, const SfmData &data,
331  const Values &values) {
333  SfmData dataValues = data;
334 
335  // Store poses or cameras in SfmData
336  size_t nrPoses = values.count<Pose3>();
337  if (nrPoses == dataValues.cameras.size()) { // we only estimated camera poses
338  for (size_t i = 0; i < dataValues.cameras.size(); i++) { // for each camera
339  Pose3 pose = values.at<Pose3>(i);
340  Cal3Bundler K = dataValues.cameras[i].calibration();
341  Camera camera(pose, K);
342  dataValues.cameras[i] = camera;
343  }
344  } else {
345  size_t nrCameras = values.count<Camera>();
346  if (nrCameras == dataValues.cameras.size()) { // we only estimated camera
347  // poses and calibration
348  for (size_t i = 0; i < nrCameras; i++) { // for each camera
349  Key cameraKey = i; // symbol('c',i);
350  Camera camera = values.at<Camera>(cameraKey);
351  dataValues.cameras[i] = camera;
352  }
353  } else {
354  cout << "writeBALfromValues: different number of cameras in "
355  "SfM_dataValues (#cameras "
356  << dataValues.cameras.size() << ") and values (#cameras " << nrPoses
357  << ", #poses " << nrCameras << ")!!" << endl;
358  return false;
359  }
360  }
361 
362  // Store 3D points in SfmData
363  size_t nrPoints = values.count<Point3>(), nrTracks = dataValues.tracks.size();
364  if (nrPoints != nrTracks) {
365  cout << "writeBALfromValues: different number of points in "
366  "SfM_dataValues (#points= "
367  << nrTracks << ") and values (#points " << nrPoints << ")!!" << endl;
368  }
369 
370  for (size_t j = 0; j < nrTracks; j++) { // for each point
371  Key pointKey = P(j);
372  if (values.exists(pointKey)) {
373  Point3 point = values.at<Point3>(pointKey);
374  dataValues.tracks[j].p = point;
375  } else {
376  dataValues.tracks[j].r = 1.0;
377  dataValues.tracks[j].g = 0.0;
378  dataValues.tracks[j].b = 0.0;
379  dataValues.tracks[j].p = Point3(0, 0, 0);
380  }
381  }
382 
383  // Write SfmData to file
384  return writeBAL(filename, dataValues);
385 }
386 
387 /* ************************************************************************** */
389  const SharedNoiseModel &model) const {
390  using ProjectionFactor = GeneralSFMFactor<SfmCamera, Point3>;
392 
393  size_t j = 0;
394  for (const SfmTrack &track : tracks) {
395  for (const SfmMeasurement &m : track.measurements) {
396  size_t i = m.first;
397  Point2 uv = m.second;
398  factors.emplace_shared<ProjectionFactor>(uv, model, i, P(j));
399  }
400  j += 1;
401  }
402 
403  return factors;
404 }
405 
406 /* ************************************************************************** */
408  const SharedNoiseModel &model, std::optional<size_t> fixedCamera,
409  std::optional<size_t> fixedPoint) const {
412  if (fixedCamera) {
413  graph.addPrior(*fixedCamera, cameras[0], Constrained::All(9));
414  }
415  if (fixedPoint) {
416  graph.addPrior(P(*fixedPoint), tracks[0].p, Constrained::All(3));
417  }
418  return graph;
419 }
420 
421 /* ************************************************************************** */
423  Values initial;
424  size_t i = 0; // NO POINTS: j = 0;
425  for (const SfmCamera &camera : db.cameras) initial.insert(i++, camera);
426  return initial;
427 }
428 
429 /* ************************************************************************** */
431  Values initial;
432  size_t i = 0, j = 0;
433  for (const SfmCamera &camera : db.cameras) initial.insert(i++, camera);
434  for (const SfmTrack &track : db.tracks) initial.insert(P(j++), track.p);
435  return initial;
436 }
437 
438 /* ************************************************************************** */
439 
440 } // namespace gtsam
Matrix3f m
SfmData readBal(const std::string &filename)
This function parses a "Bundle Adjustment in the Large" (BAL) file and returns the data as a SfmData ...
Definition: SfmData.cpp:325
const SfmCamera & camera(size_t idx) const
The camera pose at frame index idx
Definition: SfmData.h:83
const Point3 & translation(OptionalJacobian< 3, 6 > Hself={}) const
get translation
Definition: Pose3.cpp:308
Scalar * y
SfmData stores a bunch of SfmTracks.
Definition: SfmData.h:39
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:196
noiseModel::Diagonal::shared_ptr model
const ValueType at(Key j) const
Definition: Values-inl.h:204
Key P(std::uint64_t j)
Vector2 Point2
Definition: Point2.h:32
std::vector< SiftIndex > siftIndices
The feature descriptors (optional)
Definition: SfmTrack.h:47
double z() const
get z
Definition: Pose3.h:302
Rot2 R(Rot2::fromAngle(0.1))
leaf::MyValues values
NonlinearFactorGraph generalSfmFactors(const SharedNoiseModel &model=noiseModel::Isotropic::Sigma(2, 1.0)) const
Create projection factors using keys i and P(j)
Definition: SfmData.cpp:388
const GaussianFactorGraph factors
static Cal3_S2 K(500, 500, 0.1, 640/2, 480/2)
Values initialCamerasAndPointsEstimate(const SfmData &db)
This function creates initial values for cameras and points from db.
Definition: SfmData.cpp:430
Values initial
const SfmTrack & track(size_t idx) const
The track formed by series of landmark measurements.
Definition: SfmData.h:80
static SfmData FromBalFile(const std::string &filename)
Parse a "Bundle Adjustment in the Large" (BAL) file and return result as SfmData instance.
Definition: SfmData.cpp:189
Pose3 openGL2gtsam(const Rot3 &R, double tx, double ty, double tz)
This function converts an openGL camera pose to an GTSAM camera pose.
Definition: SfmData.cpp:79
NonlinearFactorGraph graph
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
const gtsam::Key pointKey
static SfmData FromBundlerFile(const std::string &filename)
Parses a bundler output file and return result as SfmData instance.
Definition: SfmData.cpp:102
void g(const string &key, int i)
Definition: testBTree.cpp:41
std::vector< SfmTrack > tracks
Sparse set of points.
Definition: SfmData.h:42
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Rot3 inverse() const
inverse of a rotation
Definition: Rot3.h:308
std::pair< size_t, Point2 > SfmMeasurement
A measurement with its camera index.
Definition: SfmTrack.h:32
bool writeBALfromValues(const std::string &filename, const SfmData &data, const Values &values)
This function writes a "Bundle Adjustment in the Large" (BAL) file from a SfmData structure and a val...
Definition: SfmData.cpp:330
static Rot3 Rodrigues(const Vector3 &w)
Definition: Rot3.h:240
size_t numberMeasurements() const
Total number of measurements in this track.
Definition: SfmTrack.h:69
Point3 unrotate(const Point3 &p, OptionalJacobian< 3, 3 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
rotate point from world to rotated frame
Definition: Rot3.cpp:135
Rot3 openGLFixedRotation()
Definition: SfmData.cpp:65
static Vector3 Logmap(const Rot3 &R, OptionalJacobian< 3, 3 > H={})
Definition: Rot3M.cpp:157
double k1() const
distorsion parameter k1
Definition: Cal3Bundler.h:87
double fx() const
focal length x
Definition: Cal3.h:139
Array< int, Dynamic, 1 > v
Data structure for dealing with Structure from Motion data.
int data[]
std::vector< SfmMeasurement > measurements
The 2D image projections (id,(u,v))
Definition: SfmTrack.h:44
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
RealScalar s
bool equals(const Base &camera, double tol=1e-9) const
assert equality up to a tolerance
Values initialCamerasEstimate(const SfmData &db)
This function creates initial values for cameras from db.
Definition: SfmData.cpp:422
Class compose(const Class &g) const
Definition: Lie.h:48
static const double u0
bool equals(const SfmTrack &sfmTrack, double tol=1e-9) const
assert equality up to a tolerance
Definition: SfmTrack.cpp:30
const G & b
Definition: Group.h:86
traits
Definition: chartTesting.h:28
size_t count() const
Definition: Values-inl.h:94
double k2() const
distorsion parameter k2
Definition: Cal3Bundler.h:90
double x() const
get x
Definition: Pose3.h:292
std::vector< SfmCamera > cameras
Set of cameras.
Definition: SfmData.h:40
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
Calibration used by Bundler.
Definition: Cal3Bundler.h:32
ofstream os("timeSchurFactors.csv")
bool equals(const SfmData &sfmData, double tol=1e-9) const
assert equality up to a tolerance
Definition: SfmData.cpp:40
static const double v0
Expression< T > compose(const Expression< T > &t1, const Expression< T > &t2)
Point3 p
3D position of the point
Definition: SfmTrack.h:126
float * p
float b
RGB color of the 3D point.
Definition: SfmTrack.h:127
void insert(Key j, const Value &val)
Definition: Values.cpp:155
void print(const std::string &s="") const
print
Definition: SfmData.cpp:34
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself={}) const
get rotation
Definition: Pose3.cpp:315
const G double tol
Definition: Group.h:86
Vector3 Point3
Definition: Point3.h:38
Pose3 gtsam2openGL(const Rot3 &R, double tx, double ty, double tz)
This function converts a GTSAM camera pose to an openGL camera pose.
Definition: SfmData.cpp:88
Point3 rotate(const Point3 &p, OptionalJacobian< 3, 3 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
Definition: Rot3M.cpp:148
NonlinearFactorGraph sfmFactorGraph(const SharedNoiseModel &model=noiseModel::Isotropic::Sigma(2, 1.0), std::optional< size_t > fixedCamera=0, std::optional< size_t > fixedPoint=0) const
Create factor graph with projection factors and gauge fix.
Definition: SfmData.cpp:407
a general SFM factor with an unknown calibration
bool exists(Key j) const
Definition: Values.cpp:93
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
std::ptrdiff_t j
bool writeBAL(const std::string &filename, const SfmData &data)
This function writes a "Bundle Adjustment in the Large" (BAL) file from a SfmData structure...
Definition: SfmData.cpp:249
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:741
double y() const
get y
Definition: Pose3.h:297


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:35:42