Go to the documentation of this file.
35 std::cout <<
"Number of cameras = " <<
cameras.size() << std::endl;
36 std::cout <<
"Number of tracks = " <<
tracks.size() << std::endl;
55 for (
size_t j = 0;
j <
tracks.size(); ++
j) {
71 Matrix3 R_mat = Matrix3::Zero(3, 3);
92 return Pose3(cRw_openGL, t_openGL);
104 std::ifstream is(
filename.c_str(), std::ifstream::in);
106 throw std::runtime_error(
107 "Error in FromBundlerFile: can not find the file!!");
114 is.getline(aux, 500);
117 size_t nrPoses, nrPoints;
118 is >> nrPoses >> nrPoints;
121 for (
size_t i = 0;
i < nrPoses;
i++) {
131 is >> r11 >> r12 >> r13 >> r21 >> r22 >> r23 >> r31 >> r32 >> r33;
134 Rot3 R(r11, r12, r13, r21, r22, r23, r31, r32, r33);
137 if (r11 == 0 && r12 == 0 && r13 == 0) {
138 throw std::runtime_error(
139 "Error in FromBundlerFile: zero rotation matrix");
144 is >> tx >> ty >> tz;
152 sfmData.
tracks.reserve(nrPoints);
153 for (
size_t j = 0;
j < nrPoints;
j++) {
174 for (
size_t k = 0; k < nvisible; k++) {
175 size_t cam_idx = 0, point_idx = 0;
177 is >> cam_idx >> point_idx >> u >>
v;
191 std::ifstream is(
filename.c_str(), std::ifstream::in);
193 throw std::runtime_error(
"Error in FromBalFile: can not find the file!!");
199 size_t nrPoses, nrPoints, nrObservations;
200 is >> nrPoses >> nrPoints >> nrObservations;
202 sfmData.
tracks.resize(nrPoints);
205 for (
size_t k = 0; k < nrObservations; k++) {
208 is >>
i >>
j >> u >>
v;
213 for (
size_t i = 0;
i < nrPoses;
i++) {
216 is >> wx >> wy >> wz;
221 is >> tx >> ty >> tz;
234 for (
size_t j = 0;
j < nrPoints;
j++) {
255 cout <<
"Error in writeBAL: can not open the file!!" << endl;
260 size_t nrObservations = 0;
261 for (
size_t j = 0;
j <
data.tracks.size();
j++) {
262 nrObservations +=
data.tracks[
j].numberMeasurements();
266 os <<
data.cameras.size() <<
" " <<
data.tracks.size() <<
" "
267 << nrObservations << endl;
270 for (
size_t j = 0;
j <
data.tracks.size();
j++) {
276 double u0 =
data.cameras[
i].calibration().px();
277 double v0 =
data.cameras[
i].calibration().py();
279 if (
u0 != 0 ||
v0 != 0) {
280 cout <<
"writeBAL has not been tested for calibration with nonzero "
289 Point2 pixelMeasurement(pixelBALx, pixelBALy);
290 os <<
i <<
" " <<
j <<
" "
291 << pixelMeasurement.x() <<
" "
292 << pixelMeasurement.y() << endl;
298 for (
size_t i = 0;
i <
data.cameras.size();
i++) {
306 os << cameraCalibration.
fx() << endl;
307 os << cameraCalibration.
k1() << endl;
308 os << cameraCalibration.
k2() << endl;
313 for (
size_t j = 0;
j <
data.tracks.size();
j++) {
337 if (nrPoses == dataValues.
cameras.size()) {
338 for (
size_t i = 0;
i < dataValues.
cameras.size();
i++) {
346 if (nrCameras == dataValues.
cameras.size()) {
348 for (
size_t i = 0;
i < nrCameras;
i++) {
354 cout <<
"writeBALfromValues: different number of cameras in "
355 "SfM_dataValues (#cameras "
356 << dataValues.
cameras.size() <<
") and values (#cameras " << nrPoses
357 <<
", #poses " << nrCameras <<
")!!" << endl;
364 if (nrPoints != nrTracks) {
365 cout <<
"writeBALfromValues: different number of points in "
366 "SfM_dataValues (#points= "
367 << nrTracks <<
") and values (#points " << nrPoints <<
")!!" << endl;
370 for (
size_t j = 0;
j < nrTracks;
j++) {
409 std::optional<size_t> fixedPoint)
const {
413 graph.addPrior(*fixedCamera,
cameras[0], Constrained::All(9));
416 graph.addPrior(
P(*fixedPoint),
tracks[0].
p, Constrained::All(3));
const SfmCamera & camera(size_t idx) const
The camera pose at frame index idx
bool exists(Key j)
Check whether a variable with key j exists.
Values initialCamerasAndPointsEstimate(const SfmData &db)
This function creates initial values for cameras and points from db.
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
const gtsam::Key pointKey
Point2 unrotate(const Point2 &p, OptionalJacobian< 2, 1 > H1={}, OptionalJacobian< 2, 2 > H2={}) const
Pose3 openGL2gtsam(const Rot3 &R, double tx, double ty, double tz)
This function converts an openGL camera pose to an GTSAM camera pose.
const SfmTrack & track(size_t idx) const
The track formed by series of landmark measurements.
const GaussianFactorGraph factors
Class compose(const Class &g) const
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
SfmData readBal(const std::string &filename)
This function parses a "Bundle Adjustment in the Large" (BAL) file and returns the data as a SfmData ...
std::vector< SfmMeasurement > measurements
The 2D image projections (id,(u,v))
a general SFM factor with an unknown calibration
NonlinearFactorGraph generalSfmFactors(const SharedNoiseModel &model=noiseModel::Isotropic::Sigma(2, 1.0)) const
Create projection factors using keys i and P(j)
static SfmData FromBalFile(const std::string &filename)
Parse a "Bundle Adjustment in the Large" (BAL) file and return result as SfmData instance.
ofstream os("timeSchurFactors.csv")
float b
RGB color of the 3D point.
SfmData stores a bunch of SfmTracks.
Expression< T > compose(const Expression< T > &t1, const Expression< T > &t2)
bool writeBAL(const std::string &filename, const SfmData &data)
This function writes a "Bundle Adjustment in the Large" (BAL) file from a SfmData structure.
static SfmData FromBundlerFile(const std::string &filename)
Parses a bundler output file and return result as SfmData instance.
std::vector< SfmTrack > tracks
Sparse set of points.
double k1() const
distortion parameter k1
Point3 rotate(const Point3 &p, OptionalJacobian< 3, 3 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
Rot3 openGLFixedRotation()
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
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...
static Rot3 Rodrigues(const Vector3 &w)
const Point3 & translation(OptionalJacobian< 3, 6 > Hself={}) const
get translation
Calibration used by Bundler.
noiseModel::Base::shared_ptr SharedNoiseModel
noiseModel::Diagonal::shared_ptr model
void g(const string &key, int i)
double k2() const
distortion parameter k2
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Data structure for dealing with Structure from Motion data.
double fx() const
focal length x
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself={}) const
get rotation
bool equals(const SfmData &sfmData, double tol=1e-9) const
assert equality up to a tolerance
std::vector< SiftIndex > siftIndices
The feature descriptors (optional)
std::vector< SfmCamera > cameras
Set of cameras.
void print(const std::string &s="") const
print
Array< int, Dynamic, 1 > v
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.
static Vector3 Logmap(const Rot3 &R, OptionalJacobian< 3, 3 > H={})
Pose3 gtsam2openGL(const Rot3 &R, double tx, double ty, double tz)
This function converts a GTSAM camera pose to an openGL camera pose.
static const CalibratedCamera camera(kDefaultPose)
Point3 p
3D position of the point
size_t numberMeasurements() const
Total number of measurements in this track.
NonlinearFactorGraph graph
std::uint64_t Key
Integer nonlinear key type.
Rot2 R(Rot2::fromAngle(0.1))
Values initialCamerasEstimate(const SfmData &db)
This function creates initial values for cameras from db.
std::pair< size_t, Point2 > SfmMeasurement
A measurement with its camera index.
gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:04:11