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

add Align() for pose3pairs

release/4.3a0
John Lambert 2021-02-25 20:51:33 -05:00
parent 149b0adfb1
commit 7d90e5040b
4 changed files with 16 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
gtsam/geometry/Pose3.h
View File

@ -389,6 +389,7 @@ inline Matrix wedge<Pose3>(const Vector& xi) {
// Convenience typedef // Convenience typedef
using Pose3Pair = std::pair<Pose3, Pose3>; using Pose3Pair = std::pair<Pose3, Pose3>;
using Pose3Pairs = std::vector<std::pair<Pose3, Pose3> >;
// For MATLAB wrapper // For MATLAB wrapper
typedef std::vector<Pose3> Pose3Vector; typedef std::vector<Pose3> Pose3Vector;

13
gtsam/gtsam.i
View File

@ -766,6 +766,16 @@ class Pose3 {
void serialize() const; void serialize() const;
}; };
#include <gtsam/geometry/Pose3.h>
class Pose3Pairs
{
Pose3Pairs();
size_t size() const;
bool empty() const;
gtsam::Pose3Pair at(size_t n) const;
void push_back(const gtsam::Pose3Pair& pose_pair);
};
// std::vector<gtsam::Pose3> // std::vector<gtsam::Pose3>
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
class Pose3Vector class Pose3Vector
@ -1077,6 +1087,9 @@ class Similarity3 {
Similarity3(const Matrix& T); Similarity3(const Matrix& T);
static Similarity3 Align(const gtsam::PointPairs & abPointPairs); static Similarity3 Align(const gtsam::PointPairs & abPointPairs);
static Similarity3 Align(const gtsam::Pose3Pairs & abPosePairs);
}; };

1
python/CMakeLists.txt
View File

@ -42,6 +42,7 @@ set(ignore
gtsam::BetweenFactorPose3s gtsam::BetweenFactorPose3s
gtsam::Point2Vector gtsam::Point2Vector
gtsam::PointPairs gtsam::PointPairs
gtsam::Pose3Pairs
gtsam::Pose3Vector gtsam::Pose3Vector
gtsam::KeyVector gtsam::KeyVector
gtsam::BinaryMeasurementsUnit3 gtsam::BinaryMeasurementsUnit3

1
python/gtsam/specializations.h
View File

@ -7,6 +7,7 @@ py::bind_vector<std::vector<gtsam::Key> >(m_, "KeyVector");
#endif #endif
py::bind_vector<std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::Point2> > >(m_, "Point2Vector"); py::bind_vector<std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::Point2> > >(m_, "Point2Vector");
py::bind_vector<std::vector<gtsam::Point3Pair> >(m_, "PointPairs"); py::bind_vector<std::vector<gtsam::Point3Pair> >(m_, "PointPairs");
py::bind_vector<std::vector<gtsam::Pose3Pair> >(m_, "Pose3Pairs");
py::bind_vector<std::vector<gtsam::Pose3> >(m_, "Pose3Vector"); 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"); py::bind_vector<std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2> > > >(m_, "BetweenFactorPose2s");