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

Switch from optional to (possibly empty) shared_ptr

release/4.3a0
dellaert 2014-05-31 19:01:54 -04:00
parent 204ddbee5e
commit 0e1b52150d
2 changed files with 80 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

77
gtsam/slam/dataset.cpp
View File

@ -16,18 +16,18 @@
* @brief utility functions for loading datasets
*/
#include <fstream>
#include <sstream>
#include <cstdlib>
#include <boost/filesystem.hpp>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/linear/Sampler.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/BearingRangeFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/linear/Sampler.h>
#include <gtsam/inference/Symbol.h>
#include <boost/filesystem.hpp>
#include <fstream>
#include <sstream>
#include <cstdlib>
using namespace std;
namespace fs = boost::filesystem;
@ -90,16 +90,16 @@ string createRewrittenFileName(const string& name) {
/* ************************************************************************* */
pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
pair<string, boost::optional<noiseModel::Diagonal::shared_ptr> > dataset,
int maxID, bool addNoise, bool smart) {
pair<string, SharedNoiseModel> dataset, int maxID, bool addNoise,
bool smart) {
return load2D(dataset.first, dataset.second, maxID, addNoise, smart);
}
/* ************************************************************************* */
pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
const string& filename,
boost::optional<noiseModel::Diagonal::shared_ptr> model, int maxID,
bool addNoise, bool smart) {
const string& filename, SharedNoiseModel model, int maxID, bool addNoise,
bool smart) {
cout << "Will try to read " << filename << endl;
ifstream is(filename.c_str());
if (!is)
@ -119,9 +119,11 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
int id;
double x, y, yaw;
is >> id >> x >> y >> yaw;
// optional filter
if (maxID && id >= maxID)
continue;
initial->insert(id, Pose2(x, y, yaw));
}
is.ignore(LINESIZE, '\n');
@ -130,7 +132,17 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
is.seekg(0, ios::beg);
// Create a sampler with random number generator
Sampler sampler(42u);
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 = Sampler(noise);
}
// Parse the pose constraints
int id1, id2;
@ -146,21 +158,6 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
is >> id1 >> id2 >> x >> y >> yaw;
is >> v1 >> v2 >> v3 >> v4 >> v5 >> v6;
// Try to guess covariance matrix layout
Matrix m(3, 3);
if (v1 != 0.0 && v2 == 0.0 && v3 != 0.0 && v4 != 0.0 && v5 == 0.0
&& v6 == 0.0) {
// Looks like [ v1 v2 v5; v2' v3 v6; v5' v6' v4 ]
m << v1, v2, v5, v2, v3, v6, v5, v6, v4;
} else if (v1 != 0.0 && v2 == 0.0 && v3 == 0.0 && v4 != 0.0 && v5 == 0.0
&& v6 != 0.0) {
// Looks like [ v1 v2 v3; v2' v4 v5; v3' v5' v6 ]
m << v1, v2, v3, v2, v4, v5, v3, v5, v6;
} else {
throw std::invalid_argument(
"load2D: unrecognized covariance matrix format in dataset file");
}
// optional filter
if (maxID && (id1 >= maxID || id2 >= maxID))
continue;
@ -169,12 +166,28 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
// SharedNoiseModel noise = noiseModel::Gaussian::Covariance(m, smart);
if (!model) {
// Try to guess covariance matrix layout
Matrix m(3, 3);
if (v1 != 0.0 && v2 == 0.0 && v3 != 0.0 && v4 != 0.0 && v5 == 0.0
&& v6 == 0.0) {
// Looks like [ v1 v2 v5; v2' v3 v6; v5' v6' v4 ]
m << v1, v2, v5, v2, v3, v6, v5, v6, v4;
} else if (v1 != 0.0 && v2 == 0.0 && v3 == 0.0 && v4 != 0.0 && v5 == 0.0
&& v6 != 0.0) {
// Looks like [ v1 v2 v3; v2' v4 v5; v3' v5' v6 ]
m << v1, v2, v3, v2, v4, v5, v3, v5, v6;
} else {
throw std::invalid_argument(
"load2D: unrecognized covariance matrix format in dataset file");
}
Vector variances = (Vector(3) << m(0, 0), m(1, 1), m(2, 2));
model = noiseModel::Diagonal::Variances(variances, smart);
}
if (addNoise)
l1Xl2 = l1Xl2.retract(sampler.sampleNewModel(*model));
l1Xl2 = l1Xl2.retract(sampler.sample());
// Insert vertices if pure odometry file
if (!initial->exists(id1))
@ -183,7 +196,7 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
initial->insert(id2, initial->at<Pose2>(id1) * l1Xl2);
NonlinearFactor::shared_ptr factor(
new BetweenFactor<Pose2>(id1, id2, l1Xl2, *model));
new BetweenFactor<Pose2>(id1, id2, l1Xl2, model));
graph->push_back(factor);
}

62
gtsam/slam/dataset.h
View File

@ -60,8 +60,8 @@ GTSAM_EXPORT std::string createRewrittenFileName(const std::string& name);
* @param smart try to reduce complexity of covariance to cheapest model
*/
GTSAM_EXPORT std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
std::pair<std::string, boost::optional<noiseModel::Diagonal::shared_ptr> > dataset,
int maxID = 0, bool addNoise = false, bool smart = true);
std::pair<std::string, SharedNoiseModel> dataset, int maxID = 0,
bool addNoise = false, bool smart = true);
/**
* Load TORO 2D Graph
@ -72,18 +72,17 @@ GTSAM_EXPORT std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> loa
* @param smart try to reduce complexity of covariance to cheapest model
*/
GTSAM_EXPORT std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
const std::string& filename,
boost::optional<gtsam::SharedDiagonal> model = boost::optional<
noiseModel::Diagonal::shared_ptr>(), int maxID = 0, bool addNoise = false,
bool smart = true);
const std::string& filename, SharedNoiseModel model = SharedNoiseModel(),
int maxID = 0, bool addNoise = false, bool smart = true);
GTSAM_EXPORT std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D_robust(
const std::string& filename,
gtsam::noiseModel::Base::shared_ptr& model, int maxID = 0);
const std::string& filename, noiseModel::Base::shared_ptr& model,
int maxID = 0);
/** save 2d graph */
GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph, const Values& config,
const noiseModel::Diagonal::shared_ptr model, const std::string& filename);
GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph,
const Values& config, const noiseModel::Diagonal::shared_ptr model,
const std::string& filename);
/**
* Load TORO 3D Graph
@ -91,27 +90,31 @@ GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph, const Values& config
GTSAM_EXPORT bool load3D(const std::string& filename);
/// A measurement with its camera index
typedef std::pair<size_t,gtsam::Point2> SfM_Measurement;
typedef std::pair<size_t, Point2> SfM_Measurement;
/// Define the structure for the 3D points
struct SfM_Track
{
gtsam::Point3 p; ///< 3D position of the point
float r,g,b; ///< RGB color of the 3D point
struct SfM_Track {
Point3 p; ///< 3D position of the point
float r, g, b; ///< RGB color of the 3D point
std::vector<SfM_Measurement> measurements; ///< The 2D image projections (id,(u,v))
size_t number_measurements() const { return measurements.size();}
size_t number_measurements() const {
return measurements.size();
}
};
/// Define the structure for the camera poses
typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> SfM_Camera;
typedef PinholeCamera<Cal3Bundler> SfM_Camera;
/// Define the structure for SfM data
struct SfM_data
{
std::vector<SfM_Camera> cameras; ///< Set of cameras
struct SfM_data {
std::vector<SfM_Camera> cameras; ///< Set of cameras
std::vector<SfM_Track> tracks; ///< Sparse set of points
size_t number_cameras() const { return cameras.size();} ///< The number of camera poses
size_t number_tracks() const { return tracks.size();} ///< The number of reconstructed 3D points
size_t number_cameras() const {
return cameras.size();
} ///< The number of camera poses
size_t number_tracks() const {
return tracks.size();
} ///< The number of reconstructed 3D points
};
/**
@ -130,8 +133,12 @@ GTSAM_EXPORT bool readBundler(const std::string& filename, SfM_data &data);
* @param graph NonlinearFactor graph storing the measurements (EDGE_SE2). NOTE: information matrix is assumed diagonal.
* @return initial Values containing the initial guess (VERTEX_SE2)
*/
enum kernelFunctionType { QUADRATIC, HUBER, TUKEY };
GTSAM_EXPORT bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& initial, const kernelFunctionType kernelFunction = QUADRATIC);
enum kernelFunctionType {
QUADRATIC, HUBER, TUKEY
};
GTSAM_EXPORT bool readG2o(const std::string& g2oFile,
NonlinearFactorGraph& graph, Values& initial,
const kernelFunctionType kernelFunction = QUADRATIC);
/**
* @brief This function writes a g2o file from
@ -140,7 +147,8 @@ GTSAM_EXPORT bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& grap
* @param graph NonlinearFactor graph storing the measurements (EDGE_SE2)
* @return estimate Values containing the values (VERTEX_SE2)
*/
GTSAM_EXPORT bool writeG2o(const std::string& filename, const NonlinearFactorGraph& graph, const Values& estimate);
GTSAM_EXPORT bool writeG2o(const std::string& filename,
const NonlinearFactorGraph& graph, const Values& estimate);
/**
* @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and stores the data into a
@ -171,7 +179,8 @@ GTSAM_EXPORT bool writeBAL(const std::string& filename, SfM_data &data);
* assumes that the keys are "x1" for pose 1 (or "c1" for camera 1) and "l1" for landmark 1
* @return true if the parsing was successful, false otherwise
*/
GTSAM_EXPORT bool writeBALfromValues(const std::string& filename, const SfM_data &data, Values& values);
GTSAM_EXPORT bool writeBALfromValues(const std::string& filename,
const SfM_data &data, Values& values);
/**
* @brief This function converts an openGL camera pose to an GTSAM camera pose
@ -214,5 +223,4 @@ GTSAM_EXPORT Values initialCamerasEstimate(const SfM_data& db);
*/
GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfM_data& db);
} // namespace gtsam