testSfmData.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 
20 #include <gtsam/inference/Symbol.h>
22 #include <gtsam/sfm/SfmData.h>
23 
24 using namespace std;
25 using namespace gtsam;
26 
28 
29 namespace gtsam {
30 GTSAM_EXPORT std::string createRewrittenFileName(const std::string& name);
31 GTSAM_EXPORT std::string findExampleDataFile(const std::string& name);
32 } // namespace gtsam
33 
34 /* ************************************************************************* */
35 TEST(dataSet, Balbianello) {
36  // The structure where we will save the SfM data
37  const string filename = findExampleDataFile("Balbianello");
38  SfmData sfmData = SfmData::FromBundlerFile(filename);
39 
40  // Check number of things
41  EXPECT_LONGS_EQUAL(5, sfmData.numberCameras());
42  EXPECT_LONGS_EQUAL(544, sfmData.numberTracks());
43  const SfmTrack& track0 = sfmData.tracks[0];
45 
46  // Check projection of a given point
47  EXPECT_LONGS_EQUAL(0, track0.measurements[0].first);
48  const SfmCamera& camera0 = sfmData.cameras[0];
49  Point2 expected = camera0.project(track0.p),
50  actual = track0.measurements[0].second;
51  EXPECT(assert_equal(expected, actual, 1));
52 
53  // We share *one* noiseModel between all projection factors
54  auto model = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
55 
56  // Convert to NonlinearFactorGraph
58  EXPECT_LONGS_EQUAL(1419, graph.size()); // regression
59 
60  // Get initial estimate
62  EXPECT_LONGS_EQUAL(549, values.size()); // regression
63 }
64 
65 /* ************************************************************************* */
66 TEST(dataSet, readBAL_Dubrovnik) {
67  // The structure where we will save the SfM data
68  const string filename = findExampleDataFile("dubrovnik-3-7-pre");
69  SfmData sfmData = SfmData::FromBalFile(filename);
70 
71  // Check number of things
72  EXPECT_LONGS_EQUAL(3, sfmData.numberCameras());
73  EXPECT_LONGS_EQUAL(7, sfmData.numberTracks());
74  const SfmTrack& track0 = sfmData.tracks[0];
76 
77  // Check projection of a given point
78  EXPECT_LONGS_EQUAL(0, track0.measurements[0].first);
79  const SfmCamera& camera0 = sfmData.cameras[0];
80  Point2 expected = camera0.project(track0.p),
81  actual = track0.measurements[0].second;
82  EXPECT(assert_equal(expected, actual, 12));
83 }
84 
85 /* ************************************************************************* */
86 TEST(dataSet, openGL2gtsam) {
87  Vector3 rotVec(0.2, 0.7, 1.1);
88  Rot3 R = Rot3::Expmap(rotVec);
89  Point3 t = Point3(0.0, 0.0, 0.0);
90  Pose3 poseGTSAM = Pose3(R, t);
91 
92  Pose3 expected = openGL2gtsam(R, t.x(), t.y(), t.z());
93 
94  Point3 r1 = R.r1(), r2 = R.r2(), r3 = R.r3(); // columns!
95  Rot3 cRw(r1.x(), r2.x(), r3.x(), -r1.y(), -r2.y(), -r3.y(), -r1.z(), -r2.z(),
96  -r3.z());
97  Rot3 wRc = cRw.inverse();
98  Pose3 actual = Pose3(wRc, t);
99 
100  EXPECT(assert_equal(expected, actual));
101 }
102 
103 /* ************************************************************************* */
104 TEST(dataSet, gtsam2openGL) {
105  Vector3 rotVec(0.2, 0.7, 1.1);
106  Rot3 R = Rot3::Expmap(rotVec);
107  Point3 t = Point3(1.0, 20.0, 10.0);
108  Pose3 actual = Pose3(R, t);
109  Pose3 poseGTSAM = openGL2gtsam(R, t.x(), t.y(), t.z());
110 
111  Pose3 expected = gtsam2openGL(poseGTSAM);
112  EXPECT(assert_equal(expected, actual));
113 }
114 
115 /* ************************************************************************* */
116 TEST(dataSet, writeBAL_Dubrovnik) {
117  const string filenameToRead = findExampleDataFile("dubrovnik-3-7-pre");
118  SfmData readData = SfmData::FromBalFile(filenameToRead);
119 
120  // Write readData to file filenameToWrite
121  const string filenameToWrite = createRewrittenFileName(filenameToRead);
122  CHECK(writeBAL(filenameToWrite, readData));
123 
124  // Read what we wrote
125  SfmData writtenData = SfmData::FromBalFile(filenameToWrite);
126 
127  // Check that what we read is the same as what we wrote
128  EXPECT_LONGS_EQUAL(readData.numberCameras(), writtenData.numberCameras());
129  EXPECT_LONGS_EQUAL(readData.numberTracks(), writtenData.numberTracks());
130 
131  for (size_t i = 0; i < readData.numberCameras(); i++) {
132  PinholeCamera<Cal3Bundler> expectedCamera = writtenData.cameras[i];
133  PinholeCamera<Cal3Bundler> actualCamera = readData.cameras[i];
134  EXPECT(assert_equal(expectedCamera, actualCamera));
135  }
136 
137  for (size_t j = 0; j < readData.numberTracks(); j++) {
138  // check point
139  SfmTrack expectedTrack = writtenData.tracks[j];
140  SfmTrack actualTrack = readData.tracks[j];
141  Point3 expectedPoint = expectedTrack.p;
142  Point3 actualPoint = actualTrack.p;
143  EXPECT(assert_equal(expectedPoint, actualPoint));
144 
145  // check rgb
146  Point3 expectedRGB =
147  Point3(expectedTrack.r, expectedTrack.g, expectedTrack.b);
148  Point3 actualRGB = Point3(actualTrack.r, actualTrack.g, actualTrack.b);
149  EXPECT(assert_equal(expectedRGB, actualRGB));
150 
151  // check measurements
152  for (size_t k = 0; k < actualTrack.numberMeasurements(); k++) {
153  EXPECT_LONGS_EQUAL(expectedTrack.measurements[k].first,
154  actualTrack.measurements[k].first);
155  EXPECT(assert_equal(expectedTrack.measurements[k].second,
156  actualTrack.measurements[k].second));
157  }
158  }
159 }
160 
161 /* ************************************************************************* */
162 TEST(dataSet, writeBALfromValues_Dubrovnik) {
163  const string filenameToRead = findExampleDataFile("dubrovnik-3-7-pre");
164  SfmData readData = SfmData::FromBalFile(filenameToRead);
165 
166  Pose3 poseChange =
167  Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.3, 0.1, 0.3));
168 
169  Values values;
170  for (size_t i = 0; i < readData.numberCameras(); i++) { // for each camera
171  Pose3 pose = poseChange.compose(readData.cameras[i].pose());
172  values.insert(i, pose);
173  }
174  for (size_t j = 0; j < readData.numberTracks(); j++) { // for each point
175  Point3 point = poseChange.transformFrom(readData.tracks[j].p);
176  values.insert(P(j), point);
177  }
178 
179  // Write values and readData to a file
180  const string filenameToWrite = createRewrittenFileName(filenameToRead);
181  writeBALfromValues(filenameToWrite, readData, values);
182 
183  // Read the file we wrote
184  SfmData writtenData = SfmData::FromBalFile(filenameToWrite);
185 
186  // Check that the reprojection errors are the same and the poses are correct
187  // Check number of things
188  EXPECT_LONGS_EQUAL(3, writtenData.numberCameras());
189  EXPECT_LONGS_EQUAL(7, writtenData.numberTracks());
190  const SfmTrack& track0 = writtenData.tracks[0];
192 
193  // Check projection of a given point
194  EXPECT_LONGS_EQUAL(0, track0.measurements[0].first);
195  const SfmCamera& camera0 = writtenData.cameras[0];
196  Point2 expected = camera0.project(track0.p),
197  actual = track0.measurements[0].second;
198  EXPECT(assert_equal(expected, actual, 12));
199 
200  Pose3 expectedPose = camera0.pose();
201  Pose3 actualPose = values.at<Pose3>(0);
202  EXPECT(assert_equal(expectedPose, actualPose, 1e-7));
203 
204  Point3 expectedPoint = track0.p;
205  Point3 actualPoint = values.at<Point3>(P(0));
206  EXPECT(assert_equal(expectedPoint, actualPoint, 1e-6));
207 }
208 
209 /* ************************************************************************* */
210 int main() {
211  TestResult tr;
212  return TestRegistry::runAllTests(tr);
213 }
214 /* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::createRewrittenFileName
GTSAM_EXPORT std::string createRewrittenFileName(const std::string &name)
Definition: dataset.cpp:105
gtsam::symbol_shorthand::P
Key P(std::uint64_t j)
Definition: inference/Symbol.h:163
name
Annotation for function names.
Definition: attr.h:51
gtsam::initialCamerasAndPointsEstimate
Values initialCamerasAndPointsEstimate(const SfmData &db)
This function creates initial values for cameras and points from db.
Definition: SfmData.cpp:430
pose
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
EXPECT_LONGS_EQUAL
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:154
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
TestHarness.h
gtsam::openGL2gtsam
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
r2
static const double r2
Definition: testSmartRangeFactor.cpp:32
gtsam::LieGroup::compose
Class compose(const Class &g) const
Definition: Lie.h:48
gtsam::SfmTrack2d::measurements
std::vector< SfmMeasurement > measurements
The 2D image projections (id,(u,v))
Definition: SfmTrack.h:44
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
r1
static const double r1
Definition: testSmartRangeFactor.cpp:32
gtsam::SfmTrack::b
float b
RGB color of the 3D point.
Definition: SfmTrack.h:127
gtsam::PinholeBaseK::project
Point2 project(const Point3 &pw, OptionalJacobian< 2, 6 > Dpose={}, OptionalJacobian< 2, 3 > Dpoint={}, OptionalJacobian< 2, DimK > Dcal={}) const
project a 3D point from world coordinates into the image
Definition: PinholePose.h:112
gtsam::SfmData
SfmData stores a bunch of SfmTracks.
Definition: SfmData.h:39
gtsam::SfmTrack
Definition: SfmTrack.h:125
main
int main()
Definition: testSfmData.cpp:210
gtsam::writeBAL
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
gtsam_unstable.tests.test_ProjectionFactorRollingShutter.point
point
Definition: test_ProjectionFactorRollingShutter.py:25
Expmap
Pose2_ Expmap(const Vector3_ &xi)
Definition: InverseKinematicsExampleExpressions.cpp:47
gtsam::SfmData::tracks
std::vector< SfmTrack > tracks
Sparse set of points.
Definition: SfmData.h:42
TEST
TEST(dataSet, Balbianello)
Definition: testSfmData.cpp:35
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
gtsam::writeBALfromValues
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
relicense.filename
filename
Definition: relicense.py:57
gtsam::Pose3
Definition: Pose3.h:37
gtsam::PinholeCamera< Cal3Bundler >
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::PinholeCamera::pose
const Pose3 & pose() const
return pose
Definition: PinholeCamera.h:164
r3
static const double r3
Definition: testSmartRangeFactor.cpp:32
gtsam::SfmTrack::r
float r
Definition: SfmTrack.h:127
Symbol.h
gtsam::HybridValues::insert
void insert(Key j, const Vector &value)
Definition: HybridValues.cpp:85
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
gtsam::Rot3::inverse
Rot3 inverse() const
inverse of a rotation
Definition: Rot3.h:308
gtsam::SfmTrack::g
float g
Definition: SfmTrack.h:127
gtsam::SfmData::numberCameras
size_t numberCameras() const
The number of cameras.
Definition: SfmData.h:77
TestResult
Definition: TestResult.h:26
gtsam::FactorGraph::size
size_t size() const
Definition: FactorGraph.h:297
SfmData.h
Data structure for dealing with Structure from Motion data.
gtsam::HybridValues::at
Vector & at(Key j)
Definition: HybridValues.cpp:129
gtsam
traits
Definition: SFMdata.h:40
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
gtsam::SfmData::cameras
std::vector< SfmCamera > cameras
Set of cameras.
Definition: SfmData.h:40
gtsam::Values
Definition: Values.h:65
CHECK
#define CHECK(condition)
Definition: Test.h:108
std
Definition: BFloat16.h:88
gtsam::Pose3::transformFrom
Point3 transformFrom(const Point3 &point, OptionalJacobian< 3, 6 > Hself={}, OptionalJacobian< 3, 3 > Hpoint={}) const
takes point in Pose coordinates and transforms it to world coordinates
Definition: Pose3.cpp:347
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
P
static double P[]
Definition: ellpe.c:68
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
gtsam::SfmData::sfmFactorGraph
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
gtsam::gtsam2openGL
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
gtsam::findExampleDataFile
GTSAM_EXPORT std::string findExampleDataFile(const std::string &name)
Definition: dataset.cpp:70
M_PI
#define M_PI
Definition: mconf.h:117
gtsam::SfmTrack::p
Point3 p
3D position of the point
Definition: SfmTrack.h:126
gtsam::SfmTrack2d::numberMeasurements
size_t numberMeasurements() const
Total number of measurements in this track.
Definition: SfmTrack.h:69
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
align_3::t
Point2 t(10, 10)
gtsam::SfmData::numberTracks
size_t numberTracks() const
The number of reconstructed 3D points.
Definition: SfmData.h:74
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
R
Rot2 R(Rot2::fromAngle(0.1))


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:41:33