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

Added more support for Pose2 data

release/4.3a0
Frank dellaert 2020-08-20 23:25:16 -04:00
parent 3d477f3a38
commit 84e0bc5351
6 changed files with 43 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
gtsam/gtsam.i
View File

@ -2816,7 +2816,19 @@ void save2D(const gtsam::NonlinearFactorGraph& graph,
const gtsam::Values& config, gtsam::noiseModel::Diagonal* model,
string filename);
// std::vector<gtsam::BetweenFactor<Pose2>::shared_ptr>
// Ignored by pybind -> will be List[BetweenFactorPose2]
class BetweenFactorPose2s
{
BetweenFactorPose2s();
size_t size() const;
gtsam::BetweenFactor<gtsam::Pose2>* at(size_t i) const;
void push_back(const gtsam::BetweenFactor<gtsam::Pose2>* factor);
};
gtsam::BetweenFactorPose2s parse2DFactors(string filename);
// std::vector<gtsam::BetweenFactor<Pose3>::shared_ptr>
// Ignored by pybind -> will be List[BetweenFactorPose3]
class BetweenFactorPose3s
{
BetweenFactorPose3s();
@ -2824,6 +2836,14 @@ class BetweenFactorPose3s
gtsam::BetweenFactor<gtsam::Pose3>* at(size_t i) const;
void push_back(const gtsam::BetweenFactor<gtsam::Pose3>* factor);
};
gtsam::BetweenFactorPose3s parse3DFactors(string filename);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load3D(string filename);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename, bool is3D);
void writeG2o(const gtsam::NonlinearFactorGraph& graph,
const gtsam::Values& estimate, string filename);
#include <gtsam/slam/InitializePose3.h>
class InitializePose3 {
@ -2845,14 +2865,6 @@ class InitializePose3 {
static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph);
};
gtsam::BetweenFactorPose3s parse3DFactors(string filename);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load3D(string filename);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename, bool is3D);
void writeG2o(const gtsam::NonlinearFactorGraph& graph,
const gtsam::Values& estimate, string filename);
#include <gtsam/slam/KarcherMeanFactor-inl.h>
template<T = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3, gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose3}>
virtual class KarcherMeanFactor : gtsam::NonlinearFactor {
@ -3376,6 +3388,7 @@ namespace utilities {
gtsam::KeySet createKeySet(string s, Vector I);
Matrix extractPoint2(const gtsam::Values& values);
Matrix extractPoint3(const gtsam::Values& values);
gtsam::Values allPose2s(gtsam::Values& values);
Matrix extractPose2(const gtsam::Values& values);
gtsam::Values allPose3s(gtsam::Values& values);
Matrix extractPose3(const gtsam::Values& values);

5
gtsam/nonlinear/utilities.h
View File

@ -107,6 +107,11 @@ Matrix extractPoint3(const Values& values) {
return result;
}
/// Extract all Pose3 values
Values allPose2s(const Values& values) {
return values.filter<Pose2>();
}
/// Extract all Pose2 values into a single matrix [x y theta]
Matrix extractPose2(const Values& values) {
Values::ConstFiltered<Pose2> poses = values.filter<Pose2>();

8
gtsam/slam/dataset.cpp
View File

@ -1284,7 +1284,13 @@ Values initialCamerasAndPointsEstimate(const SfmData &db) {
return initial;
}
// To be deprecated when pybind wrapper is merged:
// Wrapper-friendly versions of parseFactors<Pose2> and parseFactors<Pose2>
BetweenFactorPose2s
parse2DFactors(const std::string &filename,
const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex) {
return parseFactors<Pose2>(filename, model, maxIndex);
}
BetweenFactorPose3s
parse3DFactors(const std::string &filename,
const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex) {

8
gtsam/slam/dataset.h
View File

@ -341,7 +341,13 @@ GTSAM_EXPORT Values initialCamerasEstimate(const SfmData& db);
*/
GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfmData& db);
// To be deprecated when pybind wrapper is merged:
// Wrapper-friendly versions of parseFactors<Pose2> and parseFactors<Pose2>
using BetweenFactorPose2s = std::vector<BetweenFactor<Pose2>::shared_ptr>;
GTSAM_EXPORT BetweenFactorPose2s
parse2DFactors(const std::string &filename,
const noiseModel::Diagonal::shared_ptr &model = nullptr,
size_t maxIndex = 0);
using BetweenFactorPose3s = std::vector<BetweenFactor<Pose3>::shared_ptr>;
GTSAM_EXPORT BetweenFactorPose3s
parse3DFactors(const std::string &filename,

1
python/CMakeLists.txt
View File

@ -26,6 +26,7 @@ set(ignore
gtsam::ISAM2ThresholdMapValue
gtsam::FactorIndices
gtsam::FactorIndexSet
gtsam::BetweenFactorPose2s
gtsam::BetweenFactorPose3s
gtsam::Point2Vector
gtsam::Pose3Vector

3
python/gtsam/specializations.h
View File

@ -3,4 +3,5 @@
py::bind_vector<std::vector<gtsam::Key> >(m_, "KeyVector");
py::bind_vector<std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::Point2> > >(m_, "Point2Vector");
py::bind_vector<std::vector<gtsam::Pose3> >(m_, "Pose3Vector");
py::bind_vector<std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3> > > >(m_, "BetweenFactorPose3s");
py::bind_vector<std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3> > > >(m_, "BetweenFactorPose3s");
py::bind_vector<std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2> > > >(m_, "BetweenFactorPose2s");