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

refactored load2d

release/4.3a0
Frank Dellaert 2020-08-12 23:59:57 -04:00
parent d67afa8a3d
commit aa3c0f8c5d
2 changed files with 96 additions and 119 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

189
gtsam/slam/dataset.cpp
View File

@ -108,10 +108,10 @@ string createRewrittenFileName(const string &name) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
GraphAndValues load2D(pair<string, SharedNoiseModel> dataset, Key maxNr, GraphAndValues load2D(pair<string, SharedNoiseModel> dataset, Key maxKey,
bool addNoise, bool smart, NoiseFormat noiseFormat, bool addNoise, bool smart, NoiseFormat noiseFormat,
KernelFunctionType kernelFunctionType) { KernelFunctionType kernelFunctionType) {
return load2D(dataset.first, dataset.second, maxNr, addNoise, smart, return load2D(dataset.first, dataset.second, maxKey, addNoise, smart,
noiseFormat, kernelFunctionType); noiseFormat, kernelFunctionType);
} }
@ -199,16 +199,6 @@ createNoiseModel(const Vector6 v, bool smart, NoiseFormat noiseFormat,
} }
} }
/* ************************************************************************* */
// Read noise parameters and interpret them according to flags
static SharedNoiseModel readNoiseModel(istream &is, bool smart,
NoiseFormat noiseFormat,
KernelFunctionType kernelFunctionType) {
Vector6 v;
is >> v(0) >> v(1) >> v(2) >> v(3) >> v(4) >> v(5);
return createNoiseModel(v, smart, noiseFormat, kernelFunctionType);
}
/* ************************************************************************* */ /* ************************************************************************* */
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")) {
@ -237,7 +227,7 @@ boost::optional<IndexedLandmark> parseVertexLandmark(istream &is,
/* ************************************************************************* */ /* ************************************************************************* */
// Parse types T into a Key-indexed map // Parse types T into a Key-indexed map
template <typename T> template <typename T>
static map<Key, T> parseIntoMap(const string &filename, Key maxNr = 0) { static map<Key, T> parseIntoMap(const string &filename, Key maxKey = 0) {
ifstream is(filename.c_str()); ifstream is(filename.c_str());
if (!is) if (!is)
throw invalid_argument("parse: can not find file " + filename); throw invalid_argument("parse: can not find file " + filename);
@ -249,7 +239,7 @@ static map<Key, T> parseIntoMap(const string &filename, Key maxNr = 0) {
is >> indexed_t; is >> indexed_t;
if (indexed_t) { if (indexed_t) {
// optional filter // optional filter
if (maxNr && indexed_t->first >= maxNr) if (maxKey && indexed_t->first >= maxKey)
continue; continue;
result.insert(*indexed_t); result.insert(*indexed_t);
} }
@ -266,8 +256,8 @@ istream &operator>>(istream &is, boost::optional<IndexedPose> &indexed) {
return is; return is;
} }
map<Key, Pose2> parse2DPoses(const string &filename, Key maxNr) { map<Key, Pose2> parse2DPoses(const string &filename, Key maxKey) {
return parseIntoMap<Pose2>(filename, maxNr); return parseIntoMap<Pose2>(filename, maxKey);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -278,8 +268,8 @@ istream &operator>>(istream &is, boost::optional<IndexedLandmark> &indexed) {
return is; return is;
} }
map<Key, Point2> parse2DLandmarks(const string &filename, Key maxNr) { map<Key, Point2> parse2DLandmarks(const string &filename, Key maxKey) {
return parseIntoMap<Point2>(filename, maxNr); return parseIntoMap<Point2>(filename, maxKey);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -360,19 +350,26 @@ boost::shared_ptr<Sampler> createSampler(const SharedNoiseModel &model) {
// Small functor to process the edge into a BetweenFactor<Pose2>::shared_ptr // Small functor to process the edge into a BetweenFactor<Pose2>::shared_ptr
struct ProcessPose2 { struct ProcessPose2 {
// The arguments // The arguments
Key maxNr = 0; Key maxKey;
SharedNoiseModel model; SharedNoiseModel model;
boost::shared_ptr<Sampler> sampler; boost::shared_ptr<Sampler> sampler;
// Arguments for parsing noise model // Arguments for parsing noise model
bool smart = true; bool smart;
NoiseFormat noiseFormat = NoiseFormatAUTO; NoiseFormat noiseFormat;
KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE; KernelFunctionType kernelFunctionType;
ProcessPose2(Key maxKey = 0, SharedNoiseModel model = nullptr,
boost::shared_ptr<Sampler> sampler = nullptr, bool smart = true,
NoiseFormat noiseFormat = NoiseFormatAUTO,
KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE)
: maxKey(maxKey), model(model), sampler(sampler), smart(smart),
noiseFormat(noiseFormat), kernelFunctionType(kernelFunctionType) {}
// The actual function // The actual function
BetweenFactor<Pose2>::shared_ptr operator()(const Measurement2 &edge) { BetweenFactor<Pose2>::shared_ptr operator()(const Measurement2 &edge) {
// optional filter // optional filter
if (maxNr && (edge.id1 >= maxNr || edge.id2 >= maxNr)) if (maxKey && (edge.id1 >= maxKey || edge.id2 >= maxKey))
return nullptr; return nullptr;
// Get pose and optionally add noise // Get pose and optionally add noise
@ -394,90 +391,32 @@ struct ProcessPose2 {
BetweenFactorPose2s BetweenFactorPose2s
parse2DFactors(const string &filename, parse2DFactors(const string &filename,
const noiseModel::Diagonal::shared_ptr &corruptingNoise, const noiseModel::Diagonal::shared_ptr &corruptingNoise,
Key maxNr) { Key maxKey) {
ProcessPose2 process; ProcessPose2 process(maxKey, nullptr,
process.maxNr = maxNr; corruptingNoise ? createSampler(corruptingNoise)
if (corruptingNoise) { : nullptr);
process.sampler = createSampler(corruptingNoise);
}
return parseToVector<Measurement2, BetweenFactor<Pose2>::shared_ptr>(filename, return parseToVector<Measurement2, BetweenFactor<Pose2>::shared_ptr>(filename,
process); process);
} }
/* ************************************************************************* */ /* ************************************************************************* */
GraphAndValues load2D(const string &filename, SharedNoiseModel model, Key maxNr, static void parseOthers(const string &filename, Key maxKey,
bool addNoise, bool smart, NoiseFormat noiseFormat, NonlinearFactorGraph::shared_ptr graph,
KernelFunctionType kernelFunctionType) { Values::shared_ptr initial) {
// Do a pass to get other types of measurements
Values::shared_ptr initial(new Values);
const auto poses = parse2DPoses(filename, maxNr);
for (const auto &key_pose : poses) {
initial->insert(key_pose.first, key_pose.second);
}
const auto landmarks = parse2DLandmarks(filename, maxNr);
for (const auto &key_landmark : landmarks) {
initial->insert(key_landmark.first, key_landmark.second);
}
ifstream is(filename.c_str()); ifstream is(filename.c_str());
if (!is) if (!is)
throw invalid_argument("load2D: can not find file " + filename); throw invalid_argument("load2D: can not find file " + filename);
NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph);
// If asked, create a sampler with random number generator
boost::shared_ptr<Sampler> sampler;
if (addNoise) {
noiseModel::Diagonal::shared_ptr noise;
if (model)
noise = boost::dynamic_pointer_cast<noiseModel::Diagonal>(model);
if (!noise)
throw invalid_argument(
"gtsam::load2D: invalid noise model for adding noise"
"(current version assumes diagonal noise model)!");
sampler.reset(new Sampler(noise));
}
// Parse the pose constraints
Key id1, id2; Key id1, id2;
bool haveLandmark = false;
const bool useModelInFile = !model;
while (!is.eof()) { while (!is.eof()) {
string tag; string tag;
if (!(is >> tag)) if (!(is >> tag))
break; break;
auto between_pose = parseEdge(is, tag);
if (between_pose) {
std::tie(id1, id2) = between_pose->first;
Pose2 l1Xl2 = between_pose->second;
// read noise model
SharedNoiseModel modelInFile =
readNoiseModel(is, smart, noiseFormat, kernelFunctionType);
// optional filter
if (maxNr && (id1 >= maxNr || id2 >= maxNr))
continue;
if (useModelInFile)
model = modelInFile;
if (addNoise)
l1Xl2 = l1Xl2.retract(sampler->sample());
// Insert vertices if pure odometry file
if (!initial->exists(id1))
initial->insert(id1, Pose2());
if (!initial->exists(id2))
initial->insert(id2, initial->at<Pose2>(id1) * l1Xl2);
NonlinearFactor::shared_ptr factor(
new BetweenFactor<Pose2>(id1, id2, l1Xl2, model));
graph->push_back(factor);
}
// Parse measurements // Parse measurements
bool haveLandmark = false;
double bearing, range, bearing_std, range_std; double bearing, range, bearing_std, range_std;
// A bearing-range measurement // A bearing-range measurement
@ -485,8 +424,7 @@ GraphAndValues load2D(const string &filename, SharedNoiseModel model, Key maxNr,
is >> id1 >> id2 >> bearing >> range >> bearing_std >> range_std; is >> id1 >> id2 >> bearing >> range >> bearing_std >> range_std;
} }
// A landmark measurement, TODO Frank says: don't know why is converted to // A landmark measurement, converted to bearing-range
// bearing-range
if (tag == "LANDMARK") { if (tag == "LANDMARK") {
double lmx, lmy; double lmx, lmy;
double v1, v2, v3; double v1, v2, v3;
@ -521,7 +459,7 @@ GraphAndValues load2D(const string &filename, SharedNoiseModel model, Key maxNr,
if (tag == "LANDMARK" || tag == "BR") { if (tag == "LANDMARK" || tag == "BR") {
// optional filter // optional filter
if (maxNr && id1 >= maxNr) if (maxKey && id1 >= maxKey)
continue; continue;
// Create noise model // Create noise model
@ -545,14 +483,53 @@ GraphAndValues load2D(const string &filename, SharedNoiseModel model, Key maxNr,
} }
is.ignore(LINESIZE, '\n'); is.ignore(LINESIZE, '\n');
} }
}
/* ************************************************************************* */
GraphAndValues load2D(const string &filename, SharedNoiseModel model, Key maxKey,
bool addNoise, bool smart, NoiseFormat noiseFormat,
KernelFunctionType kernelFunctionType) {
Values::shared_ptr initial(new Values);
const auto poses = parse2DPoses(filename, maxKey);
for (const auto &key_pose : poses) {
initial->insert(key_pose.first, key_pose.second);
}
const auto landmarks = parse2DLandmarks(filename, maxKey);
for (const auto &key_landmark : landmarks) {
initial->insert(key_landmark.first, key_landmark.second);
}
// Parse the pose constraints
ProcessPose2 process(maxKey, model, addNoise ? createSampler(model) : nullptr,
smart, noiseFormat, kernelFunctionType);
auto factors = parseToVector<Measurement2, BetweenFactor<Pose2>::shared_ptr>(
filename, process);
// Add factors into the graph and add poses if necessary
auto graph = boost::make_shared<NonlinearFactorGraph>();
for (const auto &f : factors) {
graph->push_back(f);
// Insert vertices if pure odometry file
Key id1 = f->key1(), id2 = f->key2();
if (!initial->exists(id1))
initial->insert(id1, Pose2());
if (!initial->exists(id2))
initial->insert(id2, initial->at<Pose2>(id1) * f->measured());
}
// Do a pass to parse landmarks and bearing-range measurements
parseOthers(filename, maxKey, graph, initial);
return make_pair(graph, initial); return make_pair(graph, initial);
} }
/* ************************************************************************* */ /* ************************************************************************* */
GraphAndValues load2D_robust(const string &filename, GraphAndValues load2D_robust(const string &filename,
noiseModel::Base::shared_ptr &model, Key maxNr) { noiseModel::Base::shared_ptr &model, Key maxKey) {
return load2D(filename, model, maxNr); return load2D(filename, model, maxKey);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -595,10 +572,10 @@ GraphAndValues readG2o(const string &g2oFile, const bool is3D,
return load3D(g2oFile); return load3D(g2oFile);
} else { } else {
// just call load2D // just call load2D
Key maxNr = 0; Key maxKey = 0;
bool addNoise = false; bool addNoise = false;
bool smart = true; bool smart = true;
return load2D(g2oFile, SharedNoiseModel(), maxNr, addNoise, smart, return load2D(g2oFile, SharedNoiseModel(), maxKey, addNoise, smart,
NoiseFormatG2O, kernelFunctionType); NoiseFormatG2O, kernelFunctionType);
} }
} }
@ -751,8 +728,8 @@ istream &operator>>(istream &is, boost::optional<pair<Key, Pose3>> &indexed) {
return is; return is;
} }
map<Key, Pose3> parse3DPoses(const string &filename, Key maxNr) { map<Key, Pose3> parse3DPoses(const string &filename, Key maxKey) {
return parseIntoMap<Pose3>(filename, maxNr); return parseIntoMap<Pose3>(filename, maxKey);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -768,8 +745,8 @@ istream &operator>>(istream &is, boost::optional<pair<Key, Point3>> &indexed) {
return is; return is;
} }
map<Key, Point3> parse3DLandmarks(const string &filename, Key maxNr) { map<Key, Point3> parse3DLandmarks(const string &filename, Key maxKey) {
return parseIntoMap<Point3>(filename, maxNr); return parseIntoMap<Point3>(filename, maxKey);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -826,14 +803,14 @@ istream &operator>>(istream &is, boost::optional<Measurement3> &edge) {
// Small functor to process the edge into a BetweenFactor<Pose3>::shared_ptr // Small functor to process the edge into a BetweenFactor<Pose3>::shared_ptr
struct ProcessPose3 { struct ProcessPose3 {
// The arguments // The arguments
Key maxNr = 0; Key maxKey = 0;
SharedNoiseModel model; SharedNoiseModel model;
boost::shared_ptr<Sampler> sampler; boost::shared_ptr<Sampler> sampler;
// The actual function // The actual function
BetweenFactor<Pose3>::shared_ptr operator()(const Measurement3 &edge) { BetweenFactor<Pose3>::shared_ptr operator()(const Measurement3 &edge) {
// optional filter // optional filter
if (maxNr && (edge.id1 >= maxNr || edge.id2 >= maxNr)) if (maxKey && (edge.id1 >= maxKey || edge.id2 >= maxKey))
return nullptr; return nullptr;
// Get pose and optionally add noise // Get pose and optionally add noise
@ -851,9 +828,9 @@ struct ProcessPose3 {
BetweenFactorPose3s BetweenFactorPose3s
parse3DFactors(const string &filename, parse3DFactors(const string &filename,
const noiseModel::Diagonal::shared_ptr &corruptingNoise, const noiseModel::Diagonal::shared_ptr &corruptingNoise,
Key maxNr) { Key maxKey) {
ProcessPose3 process; ProcessPose3 process;
process.maxNr = maxNr; process.maxKey = maxKey;
if (corruptingNoise) { if (corruptingNoise) {
process.sampler = createSampler(corruptingNoise); process.sampler = createSampler(corruptingNoise);
} }

24
gtsam/slam/dataset.h
View File

@ -109,15 +109,15 @@ using BetweenFactorPose2s =
GTSAM_EXPORT BetweenFactorPose2s parse2DFactors( GTSAM_EXPORT BetweenFactorPose2s parse2DFactors(
const std::string &filename, const std::string &filename,
const noiseModel::Diagonal::shared_ptr &corruptingNoise = nullptr, const noiseModel::Diagonal::shared_ptr &corruptingNoise = nullptr,
Key maxNr = 0); Key maxKey = 0);
/// Parse vertices in 2D g2o graph file into a map of Pose2s. /// Parse vertices in 2D g2o graph file into a map of Pose2s.
GTSAM_EXPORT std::map<Key, Pose2> parse2DPoses(const std::string &filename, GTSAM_EXPORT std::map<Key, Pose2> parse2DPoses(const std::string &filename,
Key maxNr = 0); Key maxKey = 0);
/// Parse landmarks in 2D g2o graph file into a map of Point2s. /// Parse landmarks in 2D g2o graph file into a map of Point2s.
GTSAM_EXPORT std::map<Key, Point2> parse2DLandmarks(const string &filename, GTSAM_EXPORT std::map<Key, Point2> parse2DLandmarks(const string &filename,
Key maxNr = 0); Key maxKey = 0);
/// Return type for load functions /// Return type for load functions
using GraphAndValues = using GraphAndValues =
@ -126,12 +126,12 @@ using GraphAndValues =
/** /**
* 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 maxNr if non-zero cut out vertices >= maxNr * @param maxKey if non-zero cut out vertices >= maxKey
* @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 maxNr = 0, std::pair<std::string, SharedNoiseModel> dataset, Key maxKey = 0,
bool addNoise = false, bool addNoise = false,
bool smart = true, // bool smart = true, //
NoiseFormat noiseFormat = NoiseFormatAUTO, NoiseFormat noiseFormat = NoiseFormatAUTO,
@ -141,7 +141,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 maxNr if non-zero cut out vertices >= maxNr * @param maxKey if non-zero cut out vertices >= maxKey
* @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
@ -149,13 +149,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 maxNr = 0, bool addNoise = SharedNoiseModel model = SharedNoiseModel(), Key maxKey = 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 maxNr = 0); noiseModel::Base::shared_ptr& model, Key maxKey = 0);
/** save 2d graph */ /** save 2d graph */
GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph, GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph,
@ -188,15 +188,15 @@ using BetweenFactorPose3s = std::vector<gtsam::BetweenFactor<Pose3>::shared_ptr>
GTSAM_EXPORT BetweenFactorPose3s parse3DFactors( GTSAM_EXPORT BetweenFactorPose3s parse3DFactors(
const std::string &filename, const std::string &filename,
const noiseModel::Diagonal::shared_ptr &corruptingNoise = nullptr, const noiseModel::Diagonal::shared_ptr &corruptingNoise = nullptr,
Key maxNr = 0); Key maxKey = 0);
/// Parse vertices in 3D TORO/g2o graph file into a map of Pose3s. /// Parse vertices in 3D TORO/g2o graph file into a map of Pose3s.
GTSAM_EXPORT std::map<Key, Pose3> parse3DPoses(const std::string &filename, GTSAM_EXPORT std::map<Key, Pose3> parse3DPoses(const std::string &filename,
Key maxNr = 0); Key maxKey = 0);
/// Parse landmarks in 3D g2o graph file into a map of Point3s. /// Parse landmarks in 3D g2o graph file into a map of Point3s.
GTSAM_EXPORT std::map<Key, Point3> parse3DLandmarks(const std::string &filename, GTSAM_EXPORT std::map<Key, Point3> parse3DLandmarks(const std::string &filename,
Key maxNr = 0); Key maxKey = 0);
/// Load TORO 3D Graph /// Load TORO 3D Graph
GTSAM_EXPORT GraphAndValues load3D(const std::string& filename); GTSAM_EXPORT GraphAndValues load3D(const std::string& filename);
@ -340,7 +340,7 @@ GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfmData& db);
* @param is input stream * @param is input stream
* @param tag string parsed from input stream, will only parse if vertex type * @param tag string parsed from input stream, will only parse if vertex type
*/ */
GTSAM_EXPORT 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);
} }