25 #include <boost/algorithm/string.hpp> 34 using namespace gtsam;
38 const string expected_end =
"examples/Data/example.graph";
40 string actual_end = actual.substr(actual.size() - expected_end.size(), expected_end.size());
41 boost::replace_all(actual_end,
"\\",
"/");
48 const string str =
"VERTEX2 1 2.000000 3.000000 4.000000";
49 istringstream is(str);
63 const string str =
"VERTEX_XY 1 2.000000 3.000000";
64 istringstream is(str);
78 const string str =
"EDGE2 0 1 2.000000 3.000000 4.000000";
79 istringstream is(str);
86 EXPECT(expected == actual->first);
97 boost::tie(graph, initial) =
load2D(filename);
109 auto measurements = parseMeasurements<Pose2>(
filename,
nullptr, maxIndex);
113 auto measurements2 = parseMeasurements<Rot2>(
filename);
118 for (
size_t i : {0, 1, 2, 3, 4, 5}) {
121 *actualFactors[
i], 1
e-5));
126 for (
size_t j : {0, 1, 2, 3, 4}) {
136 TEST(dataSet, load2DVictoriaPark) {
142 boost::tie(graph, initial) =
load2D(filename);
148 boost::tie(graph, initial) =
load2D(filename,
nullptr, maxIndex);
181 const std::vector<Pose3> relative_poses = {
182 {{0.854230, 0.190253, 0.283162, -0.392318},
183 {1.001367, 0.015390, 0.004948}},
184 {{0.105373, 0.311512, 0.656877, -0.678505},
185 {0.523923, 0.776654, 0.326659}},
186 {{0.568551, 0.595795, -0.561677, 0.079353},
187 {0.910927, 0.055169, -0.411761}},
188 {{0.542221, -0.592077, 0.303380, -0.513226},
189 {0.775288, 0.228798, -0.596923}},
190 {{0.327419, -0.125250, -0.534379, 0.769122},
191 {-0.577841, 0.628016, -0.543592}},
192 {{0.083672, 0.104639, 0.627755, 0.766795},
193 {-0.623267, 0.086928, 0.773222}},
197 const std::vector<Pose3> poses = {
198 {{1.000000, 0.000000, 0.000000, 0.000000}, {0, 0, 0}},
199 {{0.854230, 0.190253, 0.283162, -0.392318},
200 {1.001367, 0.015390, 0.004948}},
201 {{0.421446, -0.351729, -0.597838, 0.584174},
202 {1.993500, 0.023275, 0.003793}},
203 {{0.067024, 0.331798, -0.200659, 0.919323},
204 {2.004291, 1.024305, 0.018047}},
205 {{0.765488, -0.035697, -0.462490, 0.445933},
206 {0.999908, 1.055073, 0.020212}},
210 using KeyPair = pair<Key, Key>;
211 std::vector<KeyPair>
edges = {{0, 1}, {1, 2}, {2, 3}, {3, 4}, {1, 4}, {3, 0}};
216 for (
const auto&
keys : edges) {
223 for (
size_t i : {0, 1, 2, 3, 4, 5}) {
226 *actualFactors[i], 1
e-5));
231 for (
size_t j : {0, 1, 2, 3, 4}) {
237 for (
size_t j : {0, 1, 2, 3, 4}) {
245 boost::tie(actualGraph, actualValues) =
readG2o(g2oFile, is3D);
247 for (
size_t j : {0, 1, 2, 3, 4}) {
253 TEST( dataSet, readG2o3DNonDiagonalNoise)
259 boost::tie(actualGraph, actualValues) =
readG2o(g2oFile, is3D);
273 for (
int i = 0;
i < 6;
i++){
274 for (
int j =
i;
j < 6;
j++){
293 TEST(dataSet, readG2oCheckDeterminants) {
299 for (
const auto& factor :
factors) {
300 const Rot3 R = factor->measured().rotation();
307 for (
const auto& key_value : poses) {
308 const Rot3 R = key_value.second.rotation();
316 TEST(dataSet, readG2oLandmarks) {
325 auto graphAndValues =
load3D(g2oFile);
326 EXPECT(graphAndValues.second->exists(
L(0)));
353 boost::tie(actualGraph, actualValues) =
readG2o(g2oFile);
356 Vector3(44.721360, 44.721360, 30.901699));
360 expectedValues.
insert(0,
Pose2(0.000000, 0.000000, 0.000000));
361 expectedValues.
insert(1,
Pose2(1.030390, 0.011350, -0.081596));
362 expectedValues.
insert(2,
Pose2(2.036137, -0.129733, -0.301887));
363 expectedValues.
insert(3,
Pose2(3.015097, -0.442395, -0.345514));
364 expectedValues.
insert(4,
Pose2(3.343949, 0.506678, 1.214715));
365 expectedValues.
insert(5,
Pose2(3.684491, 1.464049, 1.183785));
366 expectedValues.
insert(6,
Pose2(4.064626, 2.414783, 1.176333));
367 expectedValues.
insert(7,
Pose2(4.429778, 3.300180, 1.259169));
368 expectedValues.
insert(8,
Pose2(4.128877, 2.321481, -1.825391));
369 expectedValues.
insert(9,
Pose2(3.884653, 1.327509, -1.953016));
370 expectedValues.
insert(10,
Pose2(3.531067, 0.388263, -2.148934));
380 boost::tie(actualGraph, actualValues) =
384 Vector3(44.721360, 44.721360, 30.901699));
397 boost::tie(actualGraph, actualValues) =
401 Vector3(44.721360, 44.721360, 30.901699));
414 boost::tie(expectedGraph, expectedValues) =
readG2o(g2oFile);
417 writeG2o(*expectedGraph, *expectedValues, filenameToWrite);
421 boost::tie(actualGraph, actualValues) =
readG2o(filenameToWrite);
433 boost::tie(expectedGraph, expectedValues) =
readG2o(g2oFile, is3D);
436 writeG2o(*expectedGraph, *expectedValues, filenameToWrite);
440 boost::tie(actualGraph, actualValues) =
readG2o(filenameToWrite, is3D);
446 TEST( dataSet, writeG2o3DNonDiagonalNoise)
452 boost::tie(expectedGraph, expectedValues) =
readG2o(g2oFile, is3D);
455 writeG2o(*expectedGraph, *expectedValues, filenameToWrite);
459 boost::tie(actualGraph, actualValues) =
readG2o(filenameToWrite, is3D);
465 TEST( dataSet, readBAL_Dubrovnik)
497 r1.x(),
r2.x(),
r3.x(),
498 -r1.y(), -
r2.y(), -
r3.y(),
499 -r1.z(), -
r2.z(), -
r3.z());
520 TEST( dataSet, writeBAL_Dubrovnik)
525 readBAL(filenameToRead, readData);
549 Point3 expectedPoint = expectedTrack.
p;
550 Point3 actualPoint = actualTrack.
p;
554 Point3 expectedRGB =
Point3( expectedTrack.
r, expectedTrack.
g, expectedTrack.
b );
555 Point3 actualRGB =
Point3( actualTrack.
r, actualTrack.
g, actualTrack.
b);
568 TEST( dataSet, writeBALfromValues_Dubrovnik){
573 readBAL(filenameToRead, readData);
593 readBAL(filenameToWrite, writtenData);
612 Point3 expectedPoint = track0.p;
vector< MFAS::KeyPair > edges
Provides additional testing facilities for common data structures.
void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate, const string &filename)
This function writes a g2o file from NonlinearFactorGraph and a Values structure. ...
Point3 r1() const
first column
std::vector< BetweenFactor< Pose2 >::shared_ptr > parseFactors< Pose2 >(const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
const Pose3 & pose() const
return pose
bool writeBAL(const string &filename, SfmData &data)
This function writes a "Bundle Adjustment in the Large" (BAL) file from a SfmData structure...
static shared_ptr Covariance(const Matrix &covariance, bool smart=true)
std::vector< BetweenFactor< Pose3 >::shared_ptr > parseFactors< Pose3 >(const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
Define the structure for SfM data.
static int runAllTests(TestResult &result)
noiseModel::Diagonal::shared_ptr model
void insert(Key j, const Value &val)
GraphAndValues load2D(const string &filename, SharedNoiseModel model, size_t maxIndex, bool addNoise, bool smart, NoiseFormat noiseFormat, KernelFunctionType kernelFunctionType)
Pose3 gtsam2openGL(const Rot3 &R, double tx, double ty, double tz)
This function converts a GTSAM camera pose to an openGL camera pose.
boost::optional< IndexedEdge > parseEdge(istream &is, const string &tag)
GaussianFactorGraph factors(list_of(factor1)(factor2)(factor3))
static shared_ptr Precisions(const Vector &precisions, bool smart=true)
static shared_ptr Create(const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise)
NonlinearFactorGraph graph
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
static Rot3 Ypr(double y, double p, double r, OptionalJacobian< 3, 1 > Hy=boost::none, OptionalJacobian< 3, 1 > Hp=boost::none, OptionalJacobian< 3, 1 > Hr=boost::none)
void g(const string &key, int i)
static shared_ptr Create(size_t dim)
Rot3 inverse() const
inverse of a rotation
std::vector< SfmTrack > tracks
Sparse set of points.
static NonlinearFactorGraph expectedGraph(const SharedNoiseModel &model)
string findExampleDataFile(const string &name)
static Rot3 Quaternion(double w, double x, double y, double z)
boost::shared_ptr< BetweenFactor > shared_ptr
Point3 transformFrom(const Point3 &point, OptionalJacobian< 3, 6 > Hself=boost::none, OptionalJacobian< 3, 3 > Hpoint=boost::none) const
takes point in Pose coordinates and transforms it to world coordinates
size_t number_tracks() const
The number of reconstructed 3D points.
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&...args)
Emplace a shared pointer to factor of given type.
boost::shared_ptr< This > shared_ptr
Class compose(const Class &g) const
Define the structure for the 3D points.
const ValueType at(Key j) const
boost::shared_ptr< Values > shared_ptr
A shared_ptr to this class.
Point3 r2() const
second column
static const Pose3 pose(Rot3(Vector3(1,-1,-1).asDiagonal()), Point3(0, 0, 0.5))
Point3 r3() const
third column
static shared_ptr Create(double k, const ReweightScheme reweight=Block)
#define EXPECT(condition)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
static Rot3 Expmap(const Vector3 &v, OptionalJacobian< 3, 3 > H=boost::none)
size_t number_measurements() const
Total number of measurements in this track.
string createRewrittenFileName(const string &name)
Q R1(Eigen::AngleAxisd(1, Q_z_axis))
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
std::vector< SfmCamera > cameras
Set of cameras.
#define EXPECT_LONGS_EQUAL(expected, actual)
Pose3 openGL2gtsam(const Rot3 &R, double tx, double ty, double tz)
This function converts an openGL camera pose to an GTSAM camera pose.
std::map< size_t, Point3 > parseVariables< Point3 >(const std::string &filename, size_t maxIndex)
Point3 p
3D position of the point
float b
RGB color of the 3D point.
TEST(LPInitSolver, InfiniteLoopSingleVar)
std::map< size_t, Pose2 > parseVariables< Pose2 >(const std::string &filename, size_t maxIndex)
Point2 project(const Point3 &pw, OptionalJacobian< 2, 6 > Dpose=boost::none, OptionalJacobian< 2, 3 > Dpoint=boost::none, OptionalJacobian< 2, DimK > Dcal=boost::none) const
project a 3D point from world coordinates into the image
std::map< size_t, Pose3 > parseVariables< Pose3 >(const std::string &filename, size_t maxIndex)
boost::shared_ptr< Gaussian > shared_ptr
GraphAndValues readG2o(const string &g2oFile, const bool is3D, KernelFunctionType kernelFunctionType)
This function parses a g2o file and stores the measurements into a NonlinearFactorGraph and the initi...
bool readBundler(const string &filename, SfmData &data)
This function parses a bundler output file and stores the data into a SfmData structure.
GraphAndValues load3D(const string &filename)
Load TORO 3D Graph.
static shared_ptr Precision(size_t dim, double precision, bool smart=true)
std::map< size_t, Point2 > parseVariables< Point2 >(const std::string &filename, size_t maxIndex)
boost::optional< IndexedLandmark > parseVertexLandmark(istream &is, const string &tag)
utility functions for loading datasets
std::vector< SfmMeasurement > measurements
The 2D image projections (id,(u,v))
static shared_ptr Create(double k, const ReweightScheme reweight=Block)
noiseModel::Base::shared_ptr SharedNoiseModel
boost::optional< IndexedPose > parseVertexPose(istream &is, const string &tag)
bool readBAL(const string &filename, SfmData &data)
This function parses a "Bundle Adjustment in the Large" (BAL) file and stores the data into a SfmData...
bool writeBALfromValues(const string &filename, const SfmData &data, Values &values)
This function writes a "Bundle Adjustment in the Large" (BAL) file from a SfmData structure and a val...
size_t number_cameras() const