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

Some behavior changes. 

- maxIndex now does what it says
- id = size_t, Key is only for Values and Graph)
- re-admitted methods needed for wrapper.
release/4.3a0
Frank Dellaert 2020-08-15 13:05:58 -04:00
parent fc08562641
commit b1e3b5495c
5 changed files with 229 additions and 190 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
examples/Data/example_with_vertices.g2o
View File

@ -1,16 +1,16 @@
VERTEX_SE3:QUAT 8646911284551352320 40 -1.15443e-13 10 0.557345 0.557345 -0.435162 -0.435162 VERTEX_SE3:QUAT 0 40 -1.15443e-13 10 0.557345 0.557345 -0.435162 -0.435162
VERTEX_SE3:QUAT 8646911284551352321 28.2843 28.2843 10 0.301633 0.728207 -0.568567 -0.235508 VERTEX_SE3:QUAT 1 28.2843 28.2843 10 0.301633 0.728207 -0.568567 -0.235508
VERTEX_SE3:QUAT 8646911284551352322 -1.6986e-08 40 10 -3.89609e-10 0.788205 -0.615412 -2.07622e-10 VERTEX_SE3:QUAT 2 -1.6986e-08 40 10 -3.89609e-10 0.788205 -0.615412 -2.07622e-10
VERTEX_SE3:QUAT 8646911284551352323 -28.2843 28.2843 10 -0.301633 0.728207 -0.568567 0.235508 VERTEX_SE3:QUAT 3 -28.2843 28.2843 10 -0.301633 0.728207 -0.568567 0.235508
VERTEX_SE3:QUAT 8646911284551352324 -40 -2.32554e-10 10 -0.557345 0.557345 -0.435162 0.435162 VERTEX_SE3:QUAT 4 -40 -2.32554e-10 10 -0.557345 0.557345 -0.435162 0.435162
VERTEX_SE3:QUAT 8646911284551352325 -28.2843 -28.2843 10 -0.728207 0.301633 -0.235508 0.568567 VERTEX_SE3:QUAT 5 -28.2843 -28.2843 10 -0.728207 0.301633 -0.235508 0.568567
VERTEX_SE3:QUAT 8646911284551352326 -2.53531e-09 -40 10 -0.788205 -1.25891e-11 -3.82742e-13 0.615412 VERTEX_SE3:QUAT 6 -2.53531e-09 -40 10 -0.788205 -1.25891e-11 -3.82742e-13 0.615412
VERTEX_SE3:QUAT 8646911284551352327 28.2843 -28.2843 10 -0.728207 -0.301633 0.235508 0.568567 VERTEX_SE3:QUAT 7 28.2843 -28.2843 10 -0.728207 -0.301633 0.235508 0.568567
VERTEX_TRACKXYZ 7782220156096217088 10 10 10 VERTEX_TRACKXYZ 0 10 10 10
VERTEX_TRACKXYZ 7782220156096217089 -10 10 10 VERTEX_TRACKXYZ 1 -10 10 10
VERTEX_TRACKXYZ 7782220156096217090 -10 -10 10 VERTEX_TRACKXYZ 2 -10 -10 10
VERTEX_TRACKXYZ 7782220156096217091 10 -10 10 VERTEX_TRACKXYZ 3 10 -10 10
VERTEX_TRACKXYZ 7782220156096217092 10 10 -10 VERTEX_TRACKXYZ 4 10 10 -10
VERTEX_TRACKXYZ 7782220156096217093 -10 10 -10 VERTEX_TRACKXYZ 5 -10 10 -10
VERTEX_TRACKXYZ 7782220156096217094 -10 -10 -10 VERTEX_TRACKXYZ 6 -10 -10 -10
VERTEX_TRACKXYZ 7782220156096217095 10 -10 -10 VERTEX_TRACKXYZ 7 10 -10 -10

258
gtsam/slam/dataset.cpp
View File

@ -53,7 +53,9 @@
using namespace std; using namespace std;
namespace fs = boost::filesystem; namespace fs = boost::filesystem;
using namespace gtsam::symbol_shorthand; using gtsam::symbol_shorthand::L;
using gtsam::symbol_shorthand::P;
using gtsam::symbol_shorthand::X;
#define LINESIZE 81920 #define LINESIZE 81920
@ -127,19 +129,16 @@ static void parseLines(const string &filename, Parser<T> parse) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Parse types T into a Key-indexed map // Parse types T into a size_t-indexed map
template <typename T> template <typename T>
map<Key, T> parseToMap(const string &filename, Parser<pair<Key, T>> parse, map<size_t, T> parseToMap(const string &filename, Parser<pair<size_t, T>> parse,
Key maxKey) { size_t maxIndex) {
ifstream is(filename.c_str()); map<size_t, T> result;
if (!is) Parser<pair<size_t, T>> emplace = [&](istream &is, const string &tag) {
throw invalid_argument("parse: can not find file " + filename); if (auto t = parse(is, tag)) {
if (!maxIndex || t->first <= maxIndex)
map<Key, T> result;
Parser<pair<Key, T>> emplace = [&result, parse](istream &is,
const string &tag) {
if (auto t = parse(is, tag))
result.emplace(*t); result.emplace(*t);
}
return boost::none; return boost::none;
}; };
parseLines(filename, emplace); parseLines(filename, emplace);
@ -148,7 +147,6 @@ map<Key, T> parseToMap(const string &filename, Parser<pair<Key, T>> parse,
/* ************************************************************************* */ /* ************************************************************************* */
// Parse a file and push results on a vector // Parse a file and push results on a vector
// TODO(frank): do we need this *and* parseMeasurements?
template <typename T> template <typename T>
static vector<T> parseToVector(const string &filename, Parser<T> parse) { static vector<T> parseToVector(const string &filename, Parser<T> parse) {
vector<T> result; vector<T> result;
@ -164,7 +162,7 @@ static vector<T> parseToVector(const string &filename, Parser<T> parse) {
/* ************************************************************************* */ /* ************************************************************************* */
boost::optional<IndexedPose> parseVertexPose(istream &is, const string &tag) { boost::optional<IndexedPose> parseVertexPose(istream &is, const string &tag) {
if ((tag == "VERTEX2") || (tag == "VERTEX_SE2") || (tag == "VERTEX")) { if ((tag == "VERTEX2") || (tag == "VERTEX_SE2") || (tag == "VERTEX")) {
Key id; size_t id;
double x, y, yaw; double x, y, yaw;
is >> id >> x >> y >> yaw; is >> id >> x >> y >> yaw;
return IndexedPose(id, Pose2(x, y, yaw)); return IndexedPose(id, Pose2(x, y, yaw));
@ -174,16 +172,16 @@ boost::optional<IndexedPose> parseVertexPose(istream &is, const string &tag) {
} }
template <> template <>
std::map<Key, Pose2> parseVariables<Pose2>(const std::string &filename, std::map<size_t, Pose2> parseVariables<Pose2>(const std::string &filename,
Key maxKey) { size_t maxIndex) {
return parseToMap<Pose2>(filename, parseVertexPose, maxKey); return parseToMap<Pose2>(filename, parseVertexPose, maxIndex);
} }
/* ************************************************************************* */ /* ************************************************************************* */
boost::optional<IndexedLandmark> parseVertexLandmark(istream &is, boost::optional<IndexedLandmark> parseVertexLandmark(istream &is,
const string &tag) { const string &tag) {
if (tag == "VERTEX_XY") { if (tag == "VERTEX_XY") {
Key id; size_t id;
double x, y; double x, y;
is >> id >> x >> y; is >> id >> x >> y;
return IndexedLandmark(id, Point2(x, y)); return IndexedLandmark(id, Point2(x, y));
@ -193,9 +191,9 @@ boost::optional<IndexedLandmark> parseVertexLandmark(istream &is,
} }
template <> template <>
std::map<Key, Point2> parseVariables<Point2>(const std::string &filename, std::map<size_t, Point2> parseVariables<Point2>(const std::string &filename,
Key maxKey) { size_t maxIndex) {
return parseToMap<Point2>(filename, parseVertexLandmark, maxKey); return parseToMap<Point2>(filename, parseVertexLandmark, maxIndex);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -287,10 +285,10 @@ boost::optional<IndexedEdge> parseEdge(istream &is, const string &tag) {
if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "EDGE_SE2") || if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "EDGE_SE2") ||
(tag == "ODOMETRY")) { (tag == "ODOMETRY")) {
Key id1, id2; size_t id1, id2;
double x, y, yaw; double x, y, yaw;
is >> id1 >> id2 >> x >> y >> yaw; is >> id1 >> id2 >> x >> y >> yaw;
return IndexedEdge(pair<Key, Key>(id1, id2), Pose2(x, y, yaw)); return IndexedEdge({id1, id2}, Pose2(x, y, yaw));
} else { } else {
return boost::none; return boost::none;
} }
@ -323,7 +321,7 @@ template <typename T> struct ParseFactor : ParseMeasurement<T> {
template <> struct ParseMeasurement<Pose2> { template <> struct ParseMeasurement<Pose2> {
// The arguments // The arguments
boost::shared_ptr<Sampler> sampler; boost::shared_ptr<Sampler> sampler;
Key maxKey; size_t maxIndex;
// For noise model creation // For noise model creation
bool smart; bool smart;
@ -345,9 +343,9 @@ template <> struct ParseMeasurement<Pose2> {
is >> v(0) >> v(1) >> v(2) >> v(3) >> v(4) >> v(5); is >> v(0) >> v(1) >> v(2) >> v(3) >> v(4) >> v(5);
// optional filter // optional filter
Key id1, id2; size_t id1, id2;
tie(id1, id2) = edge->first; tie(id1, id2) = edge->first;
if (maxKey && (id1 >= maxKey || id2 >= maxKey)) if (maxIndex && (id1 > maxIndex || id2 > maxIndex))
return boost::none; return boost::none;
// Get pose and optionally add noise // Get pose and optionally add noise
@ -378,9 +376,11 @@ boost::shared_ptr<Sampler> createSampler(const SharedNoiseModel &model) {
template <> template <>
std::vector<BinaryMeasurement<Pose2>> std::vector<BinaryMeasurement<Pose2>>
parseMeasurements(const std::string &filename, parseMeasurements(const std::string &filename,
const noiseModel::Diagonal::shared_ptr &model, Key maxKey) { const noiseModel::Diagonal::shared_ptr &model,
ParseMeasurement<Pose2> parse{model ? createSampler(model) : nullptr, maxKey, size_t maxIndex) {
true, NoiseFormatAUTO, KernelFunctionTypeNONE}; ParseMeasurement<Pose2> parse{model ? createSampler(model) : nullptr,
maxIndex, true, NoiseFormatAUTO,
KernelFunctionTypeNONE};
return parseToVector<BinaryMeasurement<Pose2>>(filename, parse); return parseToVector<BinaryMeasurement<Pose2>>(filename, parse);
} }
@ -403,8 +403,9 @@ static BinaryMeasurement<Rot2> convert(const BinaryMeasurement<Pose2> &p) {
template <> template <>
std::vector<BinaryMeasurement<Rot2>> std::vector<BinaryMeasurement<Rot2>>
parseMeasurements(const std::string &filename, parseMeasurements(const std::string &filename,
const noiseModel::Diagonal::shared_ptr &model, Key maxKey) { const noiseModel::Diagonal::shared_ptr &model,
auto poses = parseMeasurements<Pose2>(filename, model, maxKey); size_t maxIndex) {
auto poses = parseMeasurements<Pose2>(filename, model, maxIndex);
std::vector<BinaryMeasurement<Rot2>> result; std::vector<BinaryMeasurement<Rot2>> result;
result.reserve(poses.size()); result.reserve(poses.size());
for (const auto &p : poses) for (const auto &p : poses)
@ -417,8 +418,9 @@ parseMeasurements(const std::string &filename,
template <> template <>
std::vector<BetweenFactor<Pose2>::shared_ptr> std::vector<BetweenFactor<Pose2>::shared_ptr>
parseFactors<Pose2>(const std::string &filename, parseFactors<Pose2>(const std::string &filename,
const noiseModel::Diagonal::shared_ptr &model, Key maxKey) { const noiseModel::Diagonal::shared_ptr &model,
ParseFactor<Pose2> parse({model ? createSampler(model) : nullptr, maxKey, size_t maxIndex) {
ParseFactor<Pose2> parse({model ? createSampler(model) : nullptr, maxIndex,
true, NoiseFormatAUTO, KernelFunctionTypeNONE, true, NoiseFormatAUTO, KernelFunctionTypeNONE,
nullptr}); nullptr});
return parseToVector<BetweenFactor<Pose2>::shared_ptr>(filename, parse); return parseToVector<BetweenFactor<Pose2>::shared_ptr>(filename, parse);
@ -429,7 +431,7 @@ using BearingRange2D = BearingRange<Pose2, Point2>;
template <> struct ParseMeasurement<BearingRange2D> { template <> struct ParseMeasurement<BearingRange2D> {
// arguments // arguments
Key maxKey; size_t maxIndex;
// The actual parser // The actual parser
boost::optional<BinaryMeasurement<BearingRange2D>> boost::optional<BinaryMeasurement<BearingRange2D>>
@ -437,7 +439,7 @@ template <> struct ParseMeasurement<BearingRange2D> {
if (tag != "BR" && tag != "LANDMARK") if (tag != "BR" && tag != "LANDMARK")
return boost::none; return boost::none;
Key id1, id2; size_t id1, id2;
is >> id1 >> id2; is >> id1 >> id2;
double bearing, range, bearing_std, range_std; double bearing, range, bearing_std, range_std;
@ -470,7 +472,7 @@ template <> struct ParseMeasurement<BearingRange2D> {
} }
// optional filter // optional filter
if (maxKey && id1 >= maxKey) if (maxIndex && id1 > maxIndex)
return boost::none; return boost::none;
// Create noise model // Create noise model
@ -484,30 +486,37 @@ template <> struct ParseMeasurement<BearingRange2D> {
/* ************************************************************************* */ /* ************************************************************************* */
GraphAndValues load2D(const string &filename, SharedNoiseModel model, GraphAndValues load2D(const string &filename, SharedNoiseModel model,
Key maxKey, bool addNoise, bool smart, size_t maxIndex, bool addNoise, bool smart,
NoiseFormat noiseFormat, NoiseFormat noiseFormat,
KernelFunctionType kernelFunctionType) { KernelFunctionType kernelFunctionType) {
auto graph = boost::make_shared<NonlinearFactorGraph>(); // Single pass for poses and landmarks.
auto initial = boost::make_shared<Values>(); auto initial = boost::make_shared<Values>();
Parser<int> insert = [maxIndex, &initial](istream &is, const string &tag) {
// TODO(frank): do a single pass if (auto indexedPose = parseVertexPose(is, tag)) {
const auto poses = parseVariables<Pose2>(filename, maxKey); if (!maxIndex || indexedPose->first <= maxIndex)
for (const auto &key_pose : poses) { initial->insert(indexedPose->first, indexedPose->second);
initial->insert(key_pose.first, key_pose.second); } else if (auto indexedLandmark = parseVertexLandmark(is, tag)) {
if (!maxIndex || indexedLandmark->first <= maxIndex)
initial->insert(L(indexedLandmark->first), indexedLandmark->second);
} }
return 0;
};
parseLines(filename, insert);
const auto landmarks = parseVariables<Point2>(filename, maxKey); // Single pass for Pose2 and bearing-range factors.
for (const auto &key_landmark : landmarks) { auto graph = boost::make_shared<NonlinearFactorGraph>();
initial->insert(L(key_landmark.first), key_landmark.second);
}
// Create parser for Pose2 and bearing/range // Instantiate factor parser
ParseFactor<Pose2> parseBetweenFactor( ParseFactor<Pose2> parseBetweenFactor(
{addNoise ? createSampler(model) : nullptr, maxKey, smart, noiseFormat, {addNoise ? createSampler(model) : nullptr, maxIndex, smart, noiseFormat,
kernelFunctionType, model}); kernelFunctionType, model});
ParseMeasurement<BearingRange2D> parseBearingRange{maxKey};
// Instantiate bearing-range parser
ParseMeasurement<BearingRange2D> parseBearingRange{maxIndex};
// Combine in a single parser that adds factors to `graph`, but also inserts
// new variables into `initial` when needed.
Parser<int> parse = [&](istream &is, const string &tag) { Parser<int> parse = [&](istream &is, const string &tag) {
if (auto f = parseBetweenFactor(is, tag)) { if (auto f = parseBetweenFactor(is, tag)) {
graph->push_back(*f); graph->push_back(*f);
@ -537,23 +546,24 @@ GraphAndValues load2D(const string &filename, SharedNoiseModel model,
return 0; return 0;
}; };
parseLines<int>(filename, parse); parseLines(filename, parse);
return make_pair(graph, initial); return make_pair(graph, initial);
} }
/* ************************************************************************* */ /* ************************************************************************* */
GraphAndValues load2D(pair<string, SharedNoiseModel> dataset, Key maxKey, GraphAndValues load2D(pair<string, SharedNoiseModel> dataset, size_t maxIndex,
bool addNoise, bool smart, NoiseFormat noiseFormat, bool addNoise, bool smart, NoiseFormat noiseFormat,
KernelFunctionType kernelFunctionType) { KernelFunctionType kernelFunctionType) {
return load2D(dataset.first, dataset.second, maxKey, addNoise, smart, return load2D(dataset.first, dataset.second, maxIndex, addNoise, smart,
noiseFormat, kernelFunctionType); noiseFormat, kernelFunctionType);
} }
/* ************************************************************************* */ /* ************************************************************************* */
GraphAndValues load2D_robust(const string &filename, GraphAndValues load2D_robust(const string &filename,
noiseModel::Base::shared_ptr &model, Key maxKey) { noiseModel::Base::shared_ptr &model,
return load2D(filename, model, maxKey); size_t maxIndex) {
return load2D(filename, model, maxIndex);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -596,10 +606,10 @@ GraphAndValues readG2o(const string &g2oFile, const bool is3D,
return load3D(g2oFile); return load3D(g2oFile);
} else { } else {
// just call load2D // just call load2D
Key maxKey = 0; size_t maxIndex = 0;
bool addNoise = false; bool addNoise = false;
bool smart = true; bool smart = true;
return load2D(g2oFile, SharedNoiseModel(), maxKey, addNoise, smart, return load2D(g2oFile, SharedNoiseModel(), maxIndex, addNoise, smart,
NoiseFormatG2O, kernelFunctionType); NoiseFormatG2O, kernelFunctionType);
} }
} }
@ -609,13 +619,16 @@ void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate,
const string &filename) { const string &filename) {
fstream stream(filename.c_str(), fstream::out); fstream stream(filename.c_str(), fstream::out);
// Use a lambda here to more easily modify behavior in future.
auto index = [](gtsam::Key key) { return Symbol(key).index(); };
// save 2D poses // save 2D poses
for (const auto key_value : estimate) { for (const auto key_value : estimate) {
auto p = dynamic_cast<const GenericValue<Pose2> *>(&key_value.value); auto p = dynamic_cast<const GenericValue<Pose2> *>(&key_value.value);
if (!p) if (!p)
continue; continue;
const Pose2 &pose = p->value(); const Pose2 &pose = p->value();
stream << "VERTEX_SE2 " << key_value.key << " " << pose.x() << " " stream << "VERTEX_SE2 " << index(key_value.key) << " " << pose.x() << " "
<< pose.y() << " " << pose.theta() << endl; << pose.y() << " " << pose.theta() << endl;
} }
@ -627,7 +640,7 @@ void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate,
const Pose3 &pose = p->value(); const Pose3 &pose = p->value();
const Point3 t = pose.translation(); const Point3 t = pose.translation();
const auto q = pose.rotation().toQuaternion(); const auto q = pose.rotation().toQuaternion();
stream << "VERTEX_SE3:QUAT " << key_value.key << " " << t.x() << " " stream << "VERTEX_SE3:QUAT " << index(key_value.key) << " " << t.x() << " "
<< t.y() << " " << t.z() << " " << q.x() << " " << q.y() << " " << t.y() << " " << t.z() << " " << q.x() << " " << q.y() << " "
<< q.z() << " " << q.w() << endl; << q.z() << " " << q.w() << endl;
} }
@ -638,7 +651,7 @@ void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate,
if (!p) if (!p)
continue; continue;
const Point2 &point = p->value(); const Point2 &point = p->value();
stream << "VERTEX_XY " << key_value.key << " " << point.x() << " " stream << "VERTEX_XY " << index(key_value.key) << " " << point.x() << " "
<< point.y() << endl; << point.y() << endl;
} }
@ -648,26 +661,25 @@ void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate,
if (!p) if (!p)
continue; continue;
const Point3 &point = p->value(); const Point3 &point = p->value();
stream << "VERTEX_TRACKXYZ " << key_value.key << " " << point.x() << " " stream << "VERTEX_TRACKXYZ " << index(key_value.key) << " " << point.x()
<< point.y() << " " << point.z() << endl; << " " << point.y() << " " << point.z() << endl;
} }
// save edges (2D or 3D) // save edges (2D or 3D)
for (const auto &factor_ : graph) { for (const auto &factor_ : graph) {
boost::shared_ptr<BetweenFactor<Pose2>> factor = auto factor = boost::dynamic_pointer_cast<BetweenFactor<Pose2>>(factor_);
boost::dynamic_pointer_cast<BetweenFactor<Pose2>>(factor_);
if (factor) { if (factor) {
SharedNoiseModel model = factor->noiseModel(); SharedNoiseModel model = factor->noiseModel();
boost::shared_ptr<noiseModel::Gaussian> gaussianModel = auto gaussianModel = boost::dynamic_pointer_cast<noiseModel::Gaussian>(model);
boost::dynamic_pointer_cast<noiseModel::Gaussian>(model);
if (!gaussianModel) { if (!gaussianModel) {
model->print("model\n"); model->print("model\n");
throw invalid_argument("writeG2o: invalid noise model!"); throw invalid_argument("writeG2o: invalid noise model!");
} }
Matrix3 Info = gaussianModel->R().transpose() * gaussianModel->R(); Matrix3 Info = gaussianModel->R().transpose() * gaussianModel->R();
Pose2 pose = factor->measured(); //.inverse(); Pose2 pose = factor->measured(); //.inverse();
stream << "EDGE_SE2 " << factor->key1() << " " << factor->key2() << " " stream << "EDGE_SE2 " << index(factor->key1()) << " "
<< pose.x() << " " << pose.y() << " " << pose.theta(); << index(factor->key2()) << " " << pose.x() << " " << pose.y()
<< " " << pose.theta();
for (size_t i = 0; i < 3; i++) { for (size_t i = 0; i < 3; i++) {
for (size_t j = i; j < 3; j++) { for (size_t j = i; j < 3; j++) {
stream << " " << Info(i, j); stream << " " << Info(i, j);
@ -676,8 +688,7 @@ void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate,
stream << endl; stream << endl;
} }
boost::shared_ptr<BetweenFactor<Pose3>> factor3D = auto factor3D = boost::dynamic_pointer_cast<BetweenFactor<Pose3>>(factor_);
boost::dynamic_pointer_cast<BetweenFactor<Pose3>>(factor_);
if (factor3D) { if (factor3D) {
SharedNoiseModel model = factor3D->noiseModel(); SharedNoiseModel model = factor3D->noiseModel();
@ -692,9 +703,10 @@ void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate,
const Pose3 pose3D = factor3D->measured(); const Pose3 pose3D = factor3D->measured();
const Point3 p = pose3D.translation(); const Point3 p = pose3D.translation();
const auto q = pose3D.rotation().toQuaternion(); const auto q = pose3D.rotation().toQuaternion();
stream << "EDGE_SE3:QUAT " << factor3D->key1() << " " << factor3D->key2() stream << "EDGE_SE3:QUAT " << index(factor3D->key1()) << " "
<< " " << p.x() << " " << p.y() << " " << p.z() << " " << q.x() << index(factor3D->key2()) << " " << p.x() << " " << p.y() << " "
<< " " << q.y() << " " << q.z() << " " << q.w(); << p.z() << " " << q.x() << " " << q.y() << " " << q.z() << " "
<< q.w();
Matrix6 InfoG2o = I_6x6; Matrix6 InfoG2o = I_6x6;
InfoG2o.block<3, 3>(0, 0) = Info.block<3, 3>(3, 3); // cov translation InfoG2o.block<3, 3>(0, 0) = Info.block<3, 3>(3, 3); // cov translation
@ -733,16 +745,16 @@ istream &operator>>(istream &is, Rot3 &R) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
boost::optional<pair<Key, Pose3>> parseVertexPose3(istream &is, boost::optional<pair<size_t, Pose3>> parseVertexPose3(istream &is,
const string &tag) { const string &tag) {
if (tag == "VERTEX3") { if (tag == "VERTEX3") {
Key id; size_t id;
double x, y, z; double x, y, z;
Rot3 R; Rot3 R;
is >> id >> x >> y >> z >> R; is >> id >> x >> y >> z >> R;
return make_pair(id, Pose3(R, {x, y, z})); return make_pair(id, Pose3(R, {x, y, z}));
} else if (tag == "VERTEX_SE3:QUAT") { } else if (tag == "VERTEX_SE3:QUAT") {
Key id; size_t id;
double x, y, z; double x, y, z;
Quaternion q; Quaternion q;
is >> id >> x >> y >> z >> q; is >> id >> x >> y >> z >> q;
@ -752,16 +764,16 @@ boost::optional<pair<Key, Pose3>> parseVertexPose3(istream &is,
} }
template <> template <>
std::map<Key, Pose3> parseVariables<Pose3>(const std::string &filename, std::map<size_t, Pose3> parseVariables<Pose3>(const std::string &filename,
Key maxKey) { size_t maxIndex) {
return parseToMap<Pose3>(filename, parseVertexPose3, maxKey); return parseToMap<Pose3>(filename, parseVertexPose3, maxIndex);
} }
/* ************************************************************************* */ /* ************************************************************************* */
boost::optional<pair<Key, Point3>> parseVertexPoint3(istream &is, boost::optional<pair<size_t, Point3>> parseVertexPoint3(istream &is,
const string &tag) { const string &tag) {
if (tag == "VERTEX_TRACKXYZ") { if (tag == "VERTEX_TRACKXYZ") {
Key id; size_t id;
double x, y, z; double x, y, z;
is >> id >> x >> y >> z; is >> id >> x >> y >> z;
return make_pair(id, Point3(x, y, z)); return make_pair(id, Point3(x, y, z));
@ -770,9 +782,9 @@ boost::optional<pair<Key, Point3>> parseVertexPoint3(istream &is,
} }
template <> template <>
std::map<Key, Point3> parseVariables<Point3>(const std::string &filename, std::map<size_t, Point3> parseVariables<Point3>(const std::string &filename,
Key maxKey) { size_t maxIndex) {
return parseToMap<Point3>(filename, parseVertexPoint3, maxKey); return parseToMap<Point3>(filename, parseVertexPoint3, maxIndex);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -791,7 +803,7 @@ istream &operator>>(istream &is, Matrix6 &m) {
template <> struct ParseMeasurement<Pose3> { template <> struct ParseMeasurement<Pose3> {
// The arguments // The arguments
boost::shared_ptr<Sampler> sampler; boost::shared_ptr<Sampler> sampler;
Key maxKey; size_t maxIndex;
// The actual parser // The actual parser
boost::optional<BinaryMeasurement<Pose3>> operator()(istream &is, boost::optional<BinaryMeasurement<Pose3>> operator()(istream &is,
@ -800,9 +812,9 @@ template <> struct ParseMeasurement<Pose3> {
return boost::none; return boost::none;
// parse ids and optionally filter // parse ids and optionally filter
Key id1, id2; size_t id1, id2;
is >> id1 >> id2; is >> id1 >> id2;
if (maxKey && (id1 >= maxKey || id2 >= maxKey)) if (maxIndex && (id1 > maxIndex || id2 > maxIndex))
return boost::none; return boost::none;
Matrix6 m; Matrix6 m;
@ -847,8 +859,10 @@ template <> struct ParseMeasurement<Pose3> {
template <> template <>
std::vector<BinaryMeasurement<Pose3>> std::vector<BinaryMeasurement<Pose3>>
parseMeasurements(const std::string &filename, parseMeasurements(const std::string &filename,
const noiseModel::Diagonal::shared_ptr &model, Key maxKey) { const noiseModel::Diagonal::shared_ptr &model,
ParseMeasurement<Pose3> parse{model ? createSampler(model) : nullptr, maxKey}; size_t maxIndex) {
ParseMeasurement<Pose3> parse{model ? createSampler(model) : nullptr,
maxIndex};
return parseToVector<BinaryMeasurement<Pose3>>(filename, parse); return parseToVector<BinaryMeasurement<Pose3>>(filename, parse);
} }
@ -872,8 +886,9 @@ static BinaryMeasurement<Rot3> convert(const BinaryMeasurement<Pose3> &p) {
template <> template <>
std::vector<BinaryMeasurement<Rot3>> std::vector<BinaryMeasurement<Rot3>>
parseMeasurements(const std::string &filename, parseMeasurements(const std::string &filename,
const noiseModel::Diagonal::shared_ptr &model, Key maxKey) { const noiseModel::Diagonal::shared_ptr &model,
auto poses = parseMeasurements<Pose3>(filename, model, maxKey); size_t maxIndex) {
auto poses = parseMeasurements<Pose3>(filename, model, maxIndex);
std::vector<BinaryMeasurement<Rot3>> result; std::vector<BinaryMeasurement<Rot3>> result;
result.reserve(poses.size()); result.reserve(poses.size());
for (const auto &p : poses) for (const auto &p : poses)
@ -886,8 +901,9 @@ parseMeasurements(const std::string &filename,
template <> template <>
std::vector<BetweenFactor<Pose3>::shared_ptr> std::vector<BetweenFactor<Pose3>::shared_ptr>
parseFactors<Pose3>(const std::string &filename, parseFactors<Pose3>(const std::string &filename,
const noiseModel::Diagonal::shared_ptr &model, Key maxKey) { const noiseModel::Diagonal::shared_ptr &model,
ParseFactor<Pose3> parse({model ? createSampler(model) : nullptr, maxKey}); size_t maxIndex) {
ParseFactor<Pose3> parse({model ? createSampler(model) : nullptr, maxIndex});
return parseToVector<BetweenFactor<Pose3>::shared_ptr>(filename, parse); return parseToVector<BetweenFactor<Pose3>::shared_ptr>(filename, parse);
} }
@ -896,21 +912,22 @@ GraphAndValues load3D(const string &filename) {
auto graph = boost::make_shared<NonlinearFactorGraph>(); auto graph = boost::make_shared<NonlinearFactorGraph>();
auto initial = boost::make_shared<Values>(); auto initial = boost::make_shared<Values>();
// TODO(frank): do a single pass // Instantiate factor parser. maxIndex is always zero for load3D.
const auto factors = parseFactors<Pose3>(filename); ParseFactor<Pose3> parseFactor({nullptr, 0});
for (const auto &factor : factors) {
graph->push_back(factor);
}
const auto poses = parseVariables<Pose3>(filename); // Single pass for variables and factors. Unlike 2D version, does *not* insert
for (const auto &key_pose : poses) { // variables into `initial` if referenced but not present.
initial->insert(key_pose.first, key_pose.second); Parser<int> parse = [&](istream &is, const string &tag) {
} if (auto indexedPose = parseVertexPose3(is, tag)) {
initial->insert(indexedPose->first, indexedPose->second);
const auto landmarks = parseVariables<Point3>(filename); } else if (auto indexedLandmark = parseVertexPoint3(is, tag)) {
for (const auto &key_landmark : landmarks) { initial->insert(L(indexedLandmark->first), indexedLandmark->second);
initial->insert(key_landmark.first, key_landmark.second); } else if (auto factor = parseFactor(is, tag)) {
graph->push_back(*factor);
} }
return 0;
};
parseLines(filename, parse);
return make_pair(graph, initial); return make_pair(graph, initial);
} }
@ -1187,8 +1204,7 @@ bool writeBALfromValues(const string &filename, const SfmData &data,
dataValues.number_cameras()) { // we only estimated camera poses dataValues.number_cameras()) { // we only estimated camera poses
for (size_t i = 0; i < dataValues.number_cameras(); for (size_t i = 0; i < dataValues.number_cameras();
i++) { // for each camera i++) { // for each camera
Key poseKey = symbol('x', i); Pose3 pose = values.at<Pose3>(X(i));
Pose3 pose = values.at<Pose3>(poseKey);
Cal3Bundler K = dataValues.cameras[i].calibration(); Cal3Bundler K = dataValues.cameras[i].calibration();
Camera camera(pose, K); Camera camera(pose, K);
dataValues.cameras[i] = camera; dataValues.cameras[i] = camera;
@ -1255,20 +1271,22 @@ Values initialCamerasAndPointsEstimate(const SfmData &db) {
return initial; return initial;
} }
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 // To be deprecated when pybind wrapper is merged:
std::map<Key, Pose3> parse3DPoses(const std::string &filename, Key maxKey) {
return parseVariables<Pose3>(filename, maxKey);
}
std::map<Key, Point3> parse3DLandmarks(const std::string &filename,
Key maxKey) {
return parseVariables<Point3>(filename, maxKey);
}
BetweenFactorPose3s BetweenFactorPose3s
parse3DFactors(const std::string &filename, parse3DFactors(const std::string &filename,
const noiseModel::Diagonal::shared_ptr &model, Key maxKey) { const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex) {
return parseFactors<Pose3>(filename, model, maxKey); return parseFactors<Pose3>(filename, model, maxIndex);
}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
std::map<size_t, Pose3> parse3DPoses(const std::string &filename,
size_t maxIndex) {
return parseVariables<Pose3>(filename, maxIndex);
}
std::map<size_t, Point3> parse3DLandmarks(const std::string &filename,
size_t maxIndex) {
return parseVariables<Point3>(filename, maxIndex);
} }
#endif #endif
} // namespace gtsam } // namespace gtsam

69
gtsam/slam/dataset.h
View File

@ -74,37 +74,41 @@ enum KernelFunctionType {
}; };
/** /**
* Parse variables in 2D g2o graph file into a map. * Parse variables in a line-based text format (like g2o) into a map.
* Instantiated in .cpp T equal to Pose2, Pose3 * Instantiated in .cpp Pose2, Point2, Pose3, and Point3.
* Note the map keys are integer indices, *not* gtsam::Keys. This is is
* different below where landmarks will use be L(index) symbols.
*/ */
template <typename T> template <typename T>
GTSAM_EXPORT std::map<Key, T> parseVariables(const std::string &filename, GTSAM_EXPORT std::map<size_t, T> parseVariables(const std::string &filename,
Key maxKey = 0); size_t maxIndex = 0);
/** /**
* Parse edges in 2D g2o graph file into a set of binary measuremnts. * Parse binary measurements in a line-based text format (like g2o) into a
* Instantiated in .cpp for T equal to Pose2, Pose3 * vector. Instantiated in .cpp for Pose2, Rot2, Pose3, and Rot3. The rotation
* versions parse poses and extract only the rotation part, using the marginal
* covariance as noise model.
*/ */
template <typename T> template <typename T>
GTSAM_EXPORT std::vector<BinaryMeasurement<T>> GTSAM_EXPORT std::vector<BinaryMeasurement<T>>
parseMeasurements(const std::string &filename, parseMeasurements(const std::string &filename,
const noiseModel::Diagonal::shared_ptr &model = nullptr, const noiseModel::Diagonal::shared_ptr &model = nullptr,
Key maxKey = 0); size_t maxIndex = 0);
/** /**
* Parse edges in 2D g2o graph file into a set of BetweenFactors. * Parse BetweenFactors in a line-based text format (like g2o) into a vector of
* Instantiated in .cpp T equal to Pose2, Pose3 * shared pointers. Instantiated in .cpp T equal to Pose2 and Pose3.
*/ */
template <typename T> template <typename T>
GTSAM_EXPORT std::vector<typename BetweenFactor<T>::shared_ptr> GTSAM_EXPORT std::vector<typename BetweenFactor<T>::shared_ptr>
parseFactors(const std::string &filename, parseFactors(const std::string &filename,
const noiseModel::Diagonal::shared_ptr &model = nullptr, const noiseModel::Diagonal::shared_ptr &model = nullptr,
Key maxKey = 0); size_t maxIndex = 0);
/// Return type for auxiliary functions /// Return type for auxiliary functions
typedef std::pair<Key, Pose2> IndexedPose; typedef std::pair<size_t, Pose2> IndexedPose;
typedef std::pair<Key, Point2> IndexedLandmark; typedef std::pair<size_t, Point2> IndexedLandmark;
typedef std::pair<std::pair<Key, Key>, Pose2> IndexedEdge; typedef std::pair<std::pair<size_t, size_t>, Pose2> IndexedEdge;
/** /**
* Parse TORO/G2O vertex "id x y yaw" * Parse TORO/G2O vertex "id x y yaw"
@ -130,19 +134,21 @@ GTSAM_EXPORT boost::optional<IndexedLandmark> parseVertexLandmark(std::istream&
GTSAM_EXPORT boost::optional<IndexedEdge> parseEdge(std::istream& is, GTSAM_EXPORT boost::optional<IndexedEdge> parseEdge(std::istream& is,
const std::string& tag); const std::string& tag);
/// Return type for load functions /// Return type for load functions, which return a graph and initial values. For
/// landmarks, the gtsam::Symbol L(index) is used to insert into the Values.
/// Bearing-range measurements also refer tio landmarks with L(index).
using GraphAndValues = using GraphAndValues =
std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr>; std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr>;
/** /**
* Load TORO 2D Graph * Load TORO 2D Graph
* @param dataset/model pair as constructed by [dataset] * @param dataset/model pair as constructed by [dataset]
* @param maxKey if non-zero cut out vertices >= maxKey * @param maxIndex if non-zero cut out vertices >= maxIndex
* @param addNoise add noise to the edges * @param addNoise add noise to the edges
* @param smart try to reduce complexity of covariance to cheapest model * @param smart try to reduce complexity of covariance to cheapest model
*/ */
GTSAM_EXPORT GraphAndValues load2D( GTSAM_EXPORT GraphAndValues load2D(
std::pair<std::string, SharedNoiseModel> dataset, Key maxKey = 0, std::pair<std::string, SharedNoiseModel> dataset, size_t maxIndex = 0,
bool addNoise = false, bool addNoise = false,
bool smart = true, // bool smart = true, //
NoiseFormat noiseFormat = NoiseFormatAUTO, NoiseFormat noiseFormat = NoiseFormatAUTO,
@ -152,7 +158,7 @@ GTSAM_EXPORT GraphAndValues load2D(
* Load TORO/G2O style graph files * Load TORO/G2O style graph files
* @param filename * @param filename
* @param model optional noise model to use instead of one specified by file * @param model optional noise model to use instead of one specified by file
* @param maxKey if non-zero cut out vertices >= maxKey * @param maxIndex if non-zero cut out vertices >= maxIndex
* @param addNoise add noise to the edges * @param addNoise add noise to the edges
* @param smart try to reduce complexity of covariance to cheapest model * @param smart try to reduce complexity of covariance to cheapest model
* @param noiseFormat how noise parameters are stored * @param noiseFormat how noise parameters are stored
@ -160,13 +166,13 @@ GTSAM_EXPORT GraphAndValues load2D(
* @return graph and initial values * @return graph and initial values
*/ */
GTSAM_EXPORT GraphAndValues load2D(const std::string& filename, GTSAM_EXPORT GraphAndValues load2D(const std::string& filename,
SharedNoiseModel model = SharedNoiseModel(), Key maxKey = 0, bool addNoise = SharedNoiseModel model = SharedNoiseModel(), size_t maxIndex = 0, bool addNoise =
false, bool smart = true, NoiseFormat noiseFormat = NoiseFormatAUTO, // false, bool smart = true, NoiseFormat noiseFormat = NoiseFormatAUTO, //
KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE); KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
/// @deprecated load2D now allows for arbitrary models and wrapping a robust kernel /// @deprecated load2D now allows for arbitrary models and wrapping a robust kernel
GTSAM_EXPORT GraphAndValues load2D_robust(const std::string& filename, GTSAM_EXPORT GraphAndValues load2D_robust(const std::string& filename,
noiseModel::Base::shared_ptr& model, Key maxKey = 0); noiseModel::Base::shared_ptr& model, size_t maxIndex = 0);
/** save 2d graph */ /** save 2d graph */
GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph, GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph,
@ -190,6 +196,11 @@ GTSAM_EXPORT GraphAndValues readG2o(const std::string& g2oFile, const bool is3D
* @param filename The name of the g2o file to write * @param filename The name of the g2o file to write
* @param graph NonlinearFactor graph storing the measurements * @param graph NonlinearFactor graph storing the measurements
* @param estimate Values * @param estimate Values
*
* Note:behavior change in PR #471: to be consistent with load2D and load3D, we
* write the *indices* to file and not the full Keys. This change really only
* affects landmarks, which get read as indices but stored in values with the
* symbol L(index).
*/ */
GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph, GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph,
const Values& estimate, const std::string& filename); const Values& estimate, const std::string& filename);
@ -330,22 +341,24 @@ GTSAM_EXPORT Values initialCamerasEstimate(const SfmData& db);
*/ */
GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfmData& db); GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfmData& db);
// To be deprecated when pybind wrapper is merged:
using BetweenFactorPose3s = std::vector<BetweenFactor<Pose3>::shared_ptr>;
GTSAM_EXPORT BetweenFactorPose3s
parse3DFactors(const std::string &filename,
const noiseModel::Diagonal::shared_ptr &model = nullptr,
size_t maxIndex = 0);
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
inline boost::optional<IndexedPose> parseVertex(std::istream &is, inline boost::optional<IndexedPose> parseVertex(std::istream &is,
const std::string &tag) { const std::string &tag) {
return parseVertexPose(is, tag); return parseVertexPose(is, tag);
} }
GTSAM_EXPORT std::map<Key, Pose3> parse3DPoses(const std::string &filename, GTSAM_EXPORT std::map<size_t, Pose3> parse3DPoses(const std::string &filename,
Key maxKey = 0); size_t maxIndex = 0);
GTSAM_EXPORT std::map<Key, Point3> parse3DLandmarks(const std::string &filename, GTSAM_EXPORT std::map<size_t, Point3>
Key maxKey = 0); parse3DLandmarks(const std::string &filename, size_t maxIndex = 0);
using BetweenFactorPose3s = std::vector<BetweenFactor<Pose3>::shared_ptr>;
GTSAM_EXPORT BetweenFactorPose3s parse3DFactors(
const std::string &filename,
const noiseModel::Diagonal::shared_ptr &model = nullptr, Key maxKey = 0);
#endif #endif
} // namespace gtsam } // namespace gtsam

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

@ -82,7 +82,7 @@ TEST( dataSet, parseEdge)
const auto actual = parseEdge(is, tag); const auto actual = parseEdge(is, tag);
EXPECT(actual); EXPECT(actual);
if (actual) { if (actual) {
pair<Key, Key> expected(0, 1); pair<size_t, size_t> expected(0, 1);
EXPECT(expected == actual->first); EXPECT(expected == actual->first);
EXPECT(assert_equal(Pose2(2, 3, 4), actual->second)); EXPECT(assert_equal(Pose2(2, 3, 4), actual->second));
} }
@ -105,8 +105,9 @@ TEST(dataSet, load2D) {
EXPECT(assert_equal(expected, *actual)); EXPECT(assert_equal(expected, *actual));
// Check binary measurements, Pose2 // Check binary measurements, Pose2
auto measurements = parseMeasurements<Pose2>(filename, nullptr, 5); size_t maxIndex = 5;
EXPECT_LONGS_EQUAL(4, measurements.size()); auto measurements = parseMeasurements<Pose2>(filename, nullptr, maxIndex);
EXPECT_LONGS_EQUAL(5, measurements.size());
// Check binary measurements, Rot2 // Check binary measurements, Rot2
auto measurements2 = parseMeasurements<Rot2>(filename); auto measurements2 = parseMeasurements<Rot2>(filename);
@ -132,14 +133,22 @@ TEST(dataSet, load2D) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( dataSet, load2DVictoriaPark) TEST(dataSet, load2DVictoriaPark) {
{
const string filename = findExampleDataFile("victoria_park.txt"); const string filename = findExampleDataFile("victoria_park.txt");
NonlinearFactorGraph::shared_ptr graph; NonlinearFactorGraph::shared_ptr graph;
Values::shared_ptr initial; Values::shared_ptr initial;
// Load all
boost::tie(graph, initial) = load2D(filename); boost::tie(graph, initial) = load2D(filename);
EXPECT_LONGS_EQUAL(10608, graph->size()); EXPECT_LONGS_EQUAL(10608, graph->size());
EXPECT_LONGS_EQUAL(7120, initial->size()); EXPECT_LONGS_EQUAL(7120, initial->size());
// Restrict keys
size_t maxIndex = 5;
boost::tie(graph, initial) = load2D(filename, nullptr, maxIndex);
EXPECT_LONGS_EQUAL(5, graph->size());
EXPECT_LONGS_EQUAL(6, initial->size()); // file has 0 as well
EXPECT_LONGS_EQUAL(L(5), graph->at(4)->keys()[1]);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -293,13 +302,13 @@ TEST(dataSet, readG2oCheckDeterminants) {
} }
// Check determinants in initial values // Check determinants in initial values
const map<Key, Pose3> poses = parseVariables<Pose3>(g2oFile); const map<size_t, Pose3> poses = parseVariables<Pose3>(g2oFile);
EXPECT_LONGS_EQUAL(5, poses.size()); EXPECT_LONGS_EQUAL(5, poses.size());
for (const auto& key_value : poses) { for (const auto& key_value : poses) {
const Rot3 R = key_value.second.rotation(); const Rot3 R = key_value.second.rotation();
EXPECT_DOUBLES_EQUAL(1.0, R.matrix().determinant(), 1e-9); EXPECT_DOUBLES_EQUAL(1.0, R.matrix().determinant(), 1e-9);
} }
const map<Key, Point3> landmarks = parseVariables<Point3>(g2oFile); const map<size_t, Point3> landmarks = parseVariables<Point3>(g2oFile);
EXPECT_LONGS_EQUAL(0, landmarks.size()); EXPECT_LONGS_EQUAL(0, landmarks.size());
} }
@ -308,10 +317,13 @@ TEST(dataSet, readG2oLandmarks) {
const string g2oFile = findExampleDataFile("example_with_vertices.g2o"); const string g2oFile = findExampleDataFile("example_with_vertices.g2o");
// Check number of poses and landmarks. Should be 8 each. // Check number of poses and landmarks. Should be 8 each.
const map<Key, Pose3> poses = parseVariables<Pose3>(g2oFile); const map<size_t, Pose3> poses = parseVariables<Pose3>(g2oFile);
EXPECT_LONGS_EQUAL(8, poses.size()); EXPECT_LONGS_EQUAL(8, poses.size());
const map<Key, Point3> landmarks = parseVariables<Point3>(g2oFile); const map<size_t, Point3> landmarks = parseVariables<Point3>(g2oFile);
EXPECT_LONGS_EQUAL(8, landmarks.size()); EXPECT_LONGS_EQUAL(8, landmarks.size());
auto graphAndValues = load3D(g2oFile);
EXPECT(graphAndValues.second->exists(L(0)));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -564,14 +576,12 @@ TEST( dataSet, writeBALfromValues_Dubrovnik){
Values value; Values value;
for(size_t i=0; i < readData.number_cameras(); i++){ // for each camera for(size_t i=0; i < readData.number_cameras(); i++){ // for each camera
Key poseKey = symbol('x',i);
Pose3 pose = poseChange.compose(readData.cameras[i].pose()); Pose3 pose = poseChange.compose(readData.cameras[i].pose());
value.insert(poseKey, pose); value.insert(X(i), pose);
} }
for(size_t j=0; j < readData.number_tracks(); j++){ // for each point for(size_t j=0; j < readData.number_tracks(); j++){ // for each point
Key pointKey = P(j);
Point3 point = poseChange.transformFrom( readData.tracks[j].p ); Point3 point = poseChange.transformFrom( readData.tracks[j].p );
value.insert(pointKey, point); value.insert(P(j), point);
} }
// Write values and readData to a file // Write values and readData to a file
@ -596,13 +606,11 @@ TEST( dataSet, writeBALfromValues_Dubrovnik){
EXPECT(assert_equal(expected,actual,12)); EXPECT(assert_equal(expected,actual,12));
Pose3 expectedPose = camera0.pose(); Pose3 expectedPose = camera0.pose();
Key poseKey = symbol('x',0); Pose3 actualPose = value.at<Pose3>(X(0));
Pose3 actualPose = value.at<Pose3>(poseKey);
EXPECT(assert_equal(expectedPose,actualPose, 1e-7)); EXPECT(assert_equal(expectedPose,actualPose, 1e-7));
Point3 expectedPoint = track0.p; Point3 expectedPoint = track0.p;
Key pointKey = P(0); Point3 actualPoint = value.at<Point3>(P(0));
Point3 actualPoint = value.at<Point3>(pointKey);
EXPECT(assert_equal(expectedPoint,actualPoint, 1e-6)); EXPECT(assert_equal(expectedPoint,actualPoint, 1e-6));
} }

2
tests/testNonlinearISAM.cpp
View File

@ -308,7 +308,7 @@ TEST(testNonlinearISAM, loop_closures ) {
// check if edge // check if edge
const auto betweenPose = parseEdge(is, tag); const auto betweenPose = parseEdge(is, tag);
if (betweenPose) { if (betweenPose) {
Key id1, id2; size_t id1, id2;
tie(id1, id2) = betweenPose->first; tie(id1, id2) = betweenPose->first;
graph.emplace_shared<BetweenFactor<Pose2> >(Symbol('x', id2), graph.emplace_shared<BetweenFactor<Pose2> >(Symbol('x', id2),
Symbol('x', id1), betweenPose->second, model); Symbol('x', id1), betweenPose->second, model);