diff --git a/python/gtsam/__init__.py b/python/gtsam/__init__.py index 4b18783d3..f1b07905b 100644 --- a/python/gtsam/__init__.py +++ b/python/gtsam/__init__.py @@ -1 +1 @@ -from libgtsam_python import * \ No newline at end of file +from libgtsam_python import * diff --git a/python/gtsam/geometry/.gitignore b/python/gtsam/geometry/.gitignore deleted file mode 100644 index 7e7c8fdb2..000000000 --- a/python/gtsam/geometry/.gitignore +++ /dev/null @@ -1 +0,0 @@ -/libgeometry_python.so diff --git a/python/gtsam/geometry/__init__.py b/python/gtsam/geometry/__init__.py deleted file mode 100644 index 32a465828..000000000 --- a/python/gtsam/geometry/__init__.py +++ /dev/null @@ -1 +0,0 @@ -from libgeometry_python import * \ No newline at end of file diff --git a/python/gtsam/noiseModel/.gitignore b/python/gtsam/noiseModel/.gitignore deleted file mode 100644 index e054d5cfe..000000000 --- a/python/gtsam/noiseModel/.gitignore +++ /dev/null @@ -1 +0,0 @@ -/libnoiseModel_python.so diff --git a/python/gtsam/noiseModel/__init__.py b/python/gtsam/noiseModel/__init__.py deleted file mode 100644 index 492937407..000000000 --- a/python/gtsam/noiseModel/__init__.py +++ /dev/null @@ -1 +0,0 @@ -from libnoiseModel_python import * \ No newline at end of file diff --git a/python/gtsam/nonlinear/.gitignore b/python/gtsam/nonlinear/.gitignore deleted file mode 100644 index d2353be92..000000000 --- a/python/gtsam/nonlinear/.gitignore +++ /dev/null @@ -1 +0,0 @@ -/libnonlinear_python.so diff --git a/python/gtsam/nonlinear/__init__.py b/python/gtsam/nonlinear/__init__.py deleted file mode 100644 index 74a227d48..000000000 --- a/python/gtsam/nonlinear/__init__.py +++ /dev/null @@ -1 +0,0 @@ -from libnonlinear_python import * \ No newline at end of file diff --git a/python/gtsam/registernumpyeigen/.gitignore b/python/gtsam/registernumpyeigen/.gitignore deleted file mode 100644 index 7ed4e4844..000000000 --- a/python/gtsam/registernumpyeigen/.gitignore +++ /dev/null @@ -1 +0,0 @@ -/libregisternumpyeigen_python.so diff --git a/python/gtsam/registernumpyeigen/__init__.py b/python/gtsam/registernumpyeigen/__init__.py deleted file mode 100644 index ac6856e8c..000000000 --- a/python/gtsam/registernumpyeigen/__init__.py +++ /dev/null @@ -1 +0,0 @@ -from libregisternumpyeigen_python import * \ No newline at end of file diff --git a/python/gtsam/slam/.gitignore b/python/gtsam/slam/.gitignore deleted file mode 100644 index 4fbe984da..000000000 --- a/python/gtsam/slam/.gitignore +++ /dev/null @@ -1 +0,0 @@ -/libslam_python.so diff --git a/python/gtsam/slam/__init__.py b/python/gtsam/slam/__init__.py deleted file mode 100644 index 8c327e67f..000000000 --- a/python/gtsam/slam/__init__.py +++ /dev/null @@ -1 +0,0 @@ -from libslam_python import * \ No newline at end of file diff --git a/python/handwritten/exportgtsam.cpp b/python/handwritten/exportgtsam.cpp index 3799ef584..2424cd4da 100644 --- a/python/handwritten/exportgtsam.cpp +++ b/python/handwritten/exportgtsam.cpp @@ -1,63 +1,77 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief exports the python module + * @author Andrew Melim + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + #include #include -#include -#include -#include -#include -#include +#include +#include using namespace boost::python; using namespace gtsam; using namespace std; -// Geometery +// Geometry void exportPoint2(); +void exportPoint3(); void exportRot2(); +void exportRot3(); void exportPose2(); +void exportPose3(); // Linear void exportNoiseModels(); // Nonlinear void exportValues(); +void exportNonlinearFactor(); void exportNonlinearFactorGraph(); void exportLevenbergMarquardtOptimizer(); +void exportISAM2(); // Slam -template< class FACTOR, class VALUE > -void exportPriorFactor(const std::string& name){ - class_< FACTOR >(name.c_str(), init<>()) - .def(init< Key, VALUE&, SharedNoiseModel >()) - ; -} +void exportPriorFactors(); +void exportBetweenFactors(); -template -void exportBetweenFactor(const std::string& name){ - class_(name.c_str(), init<>()) - .def(init()) - ; -} - -typedef gtsam::PriorFactor Point2PriorFactor; -typedef gtsam::PriorFactor Pose2PriorFactor; -typedef gtsam::BetweenFactor Pose2BetweenFactor; +// Utils (or Python wrapper specific functions) +void registerNumpyEigenConversions(); //-----------------------------------// BOOST_PYTHON_MODULE(libgtsam_python){ - using namespace boost::python; + + // Should be the first thing to be done + registerNumpyEigenConversions(); + exportPoint2(); + exportPoint3(); exportRot2(); + exportRot3(); exportPose2(); + exportPose3(); exportNoiseModels(); exportValues(); + exportNonlinearFactor(); exportNonlinearFactorGraph(); exportLevenbergMarquardtOptimizer(); + exportISAM2(); - exportPriorFactor< Point2PriorFactor, Point2 >("Point2PriorFactor"); - exportPriorFactor< Pose2PriorFactor, Pose2 >("Pose2PriorFactor"); - exportBetweenFactor< Pose2BetweenFactor, Pose2 >("Pose2BetweenFactor"); + exportPriorFactors(); + exportBetweenFactors(); } \ No newline at end of file diff --git a/python/handwritten/geometry/Point2.cpp b/python/handwritten/geometry/Point2.cpp index 9e1a4d6b8..77abca636 100644 --- a/python/handwritten/geometry/Point2.cpp +++ b/python/handwritten/geometry/Point2.cpp @@ -1,3 +1,20 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps Point2 class to python + * @author Andrew Melim + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + #include #include "gtsam/geometry/Point2.h" @@ -12,15 +29,26 @@ void exportPoint2(){ class_("Point2", init<>()) .def(init()) - .def("print", &Point2::print, print_overloads(args("s"))) + .def(init()) + .def("identity", &Point2::identity) + .def("dist", &Point2::dist) + .def("distance", &Point2::distance) .def("equals", &Point2::equals, equals_overloads(args("q","tol"))) - .def("inverse", &Point2::inverse) - // .def("compose", &Point2::compose, compose_overloads(args("q", "H1", "H2"))) - .def("between", &Point2::between) - // .def("dim", &Point2::dim) - .def("retract", &Point2::retract) + .def("norm", &Point2::norm) + .def("print", &Point2::print, print_overloads(args("s"))) + .def("unit", &Point2::unit) + .def("vector", &Point2::vector) .def("x", &Point2::x) .def("y", &Point2::y) + .def(self * other()) // __mult__ + .def(other() * self) // __mult__ + .def(self + self) + .def(-self) + .def(self - self) + .def(self / other()) + .def(self_ns::str(self)) + .def(repr(self)) + .def(self == self) ; } \ No newline at end of file diff --git a/python/handwritten/geometry/Point3.cpp b/python/handwritten/geometry/Point3.cpp new file mode 100644 index 000000000..fd703f3ce --- /dev/null +++ b/python/handwritten/geometry/Point3.cpp @@ -0,0 +1,60 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps Point3 class to python + * @author Andrew Melim + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include +#include "gtsam/geometry/Point3.h" + +using namespace boost::python; +using namespace gtsam; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Point3::print, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Point3::equals, 1, 2) + +void exportPoint3(){ + +class_("Point3") + .def(init<>()) + .def(init()) + .def(init()) + .def("identity", &Point3::identity) + .staticmethod("identity") + .def("add", &Point3::add) + .def("cross", &Point3::cross) + .def("dist", &Point3::dist) + .def("distance", &Point3::distance) + .def("dot", &Point3::dot) + .def("equals", &Point3::equals, equals_overloads(args("q","tol"))) + .def("norm", &Point3::norm) + .def("normalize", &Point3::normalize) + .def("print", &Point3::print, print_overloads(args("s"))) + .def("sub", &Point3::sub) + .def("vector", &Point3::vector) + .def("x", &Point3::x) + .def("y", &Point3::y) + .def("z", &Point3::z) + .def(self * other()) + .def(other() * self) + .def(self + self) + .def(-self) + .def(self - self) + .def(self / other()) + .def(self_ns::str(self)) + .def(repr(self)) + .def(self == self) +; + +} \ No newline at end of file diff --git a/python/handwritten/geometry/Pose2.cpp b/python/handwritten/geometry/Pose2.cpp index 133a6f05f..6640d47a9 100644 --- a/python/handwritten/geometry/Pose2.cpp +++ b/python/handwritten/geometry/Pose2.cpp @@ -1,3 +1,20 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps Pose2 class to python + * @author Andrew Melim + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + #include #include "gtsam/geometry/Pose2.h" diff --git a/python/handwritten/geometry/Pose3.cpp b/python/handwritten/geometry/Pose3.cpp new file mode 100644 index 000000000..a7e3ae2fd --- /dev/null +++ b/python/handwritten/geometry/Pose3.cpp @@ -0,0 +1,74 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps Pose3 class to python + * @author Andrew Melim + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include +#include "gtsam/geometry/Pose3.h" + +#include "gtsam/geometry/Pose2.h" +#include "gtsam/geometry/Point3.h" +#include "gtsam/geometry/Rot3.h" + +using namespace boost::python; +using namespace gtsam; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Pose3::print, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Pose3::equals, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(transform_to_overloads, Pose3::transform_to, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(transform_from_overloads, Pose3::transform_from, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(translation_overloads, Pose3::translation, 0, 1) +// BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(bearing_overloads, Pose3::bearing, 1, 3) +// BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(range_overloads, Pose3::range, 1, 3) +// BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(compose_overloads, Pose3::compose, 1, 3) +// BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(between_overloads, Pose3::between, 1, 3) + +void exportPose3(){ + + Point3 (Pose3::*transform_to1)(const Point3&, OptionalJacobian< 3, 6 >, OptionalJacobian< 3, 3 > ) const + = &Pose3::transform_to; + Pose3 (Pose3::*transform_to2)(const Pose3&) const + = &Pose3::transform_to; + + class_("Pose3") + .def(init<>()) + .def(init()) + .def(init()) + .def(init()) + .def(init()) + .def(init()) + .def("print", &Pose3::print, print_overloads(args("s"))) + .def("equals", &Pose3::equals, equals_overloads(args("pose","tol"))) + .def("identity", &Pose3::identity) + .staticmethod("identity") + .def("bearing", &Pose3::bearing) + .def("matrix", &Pose3::matrix) + .def("transform_from", &Pose3::transform_from, + transform_from_overloads(args("point", "H1", "H2"))) + .def("transform_to", transform_to1, + transform_to_overloads(args("point", "H1", "H2"))) + .def("transform_to", transform_to2) + .def("x", &Pose3::x) + .def("y", &Pose3::y) + .def("z", &Pose3::z) + .def("translation", &Pose3::translation, + translation_overloads()[return_value_policy()]) + .def("rotation", &Pose3::rotation, return_value_policy()) + .def(self * self) // __mult__ + .def(self * other()) // __mult__ + .def(self_ns::str(self)) // __str__ + .def(repr(self)) // __repr__ + ; +} \ No newline at end of file diff --git a/python/handwritten/geometry/Rot2.cpp b/python/handwritten/geometry/Rot2.cpp index e79fabd9d..d7bcf8cf1 100644 --- a/python/handwritten/geometry/Rot2.cpp +++ b/python/handwritten/geometry/Rot2.cpp @@ -1,3 +1,20 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps Rot2 class to python + * @author Andrew Melim + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + #include #include "gtsam/geometry/Rot2.h" @@ -13,35 +30,31 @@ void exportRot2(){ class_("Rot2", init<>()) .def(init()) - - .def("fromAngle", &Rot2::fromAngle) - .staticmethod("fromAngle") - - .def("fromDegrees", &Rot2::fromDegrees) - .staticmethod("fromDegrees") - - .def("fromCosSin", &Rot2::fromCosSin) - .staticmethod("fromCosSin") - - .def("atan2", &Rot2::atan2) - .staticmethod("atan2") - - .def("print", &Rot2::print, print_overloads(args("s"))) - .def("equals", &Rot2::equals, equals_overloads(args("q","tol"))) - // .def("inverse", &Rot2::inverse) - // .def("compose", &Rot2::compose, compose_overloads(args("q", "H1", "H2"))) - // .def("between", &Rot2::between) - // .def("dim", &Rot2::dim) - // .def("retract", &Rot2::retract) - .def("Expmap", &Rot2::Expmap) .staticmethod("Expmap") - - .def("theta", &Rot2::theta) - .def("degrees", &Rot2::degrees) + .def("Logmap", &Rot2::Logmap) + .staticmethod("Logmap") + .def("atan2", &Rot2::atan2) + .staticmethod("atan2") + .def("fromAngle", &Rot2::fromAngle) + .staticmethod("fromAngle") + .def("fromCosSin", &Rot2::fromCosSin) + .staticmethod("fromCosSin") + .def("fromDegrees", &Rot2::fromDegrees) + .staticmethod("fromDegrees") + .def("identity", &Rot2::identity) + .staticmethod("identity") + .def("relativeBearing", &Rot2::relativeBearing) + .staticmethod("relativeBearing") .def("c", &Rot2::c) + .def("degrees", &Rot2::degrees) + .def("equals", &Rot2::equals, equals_overloads(args("q","tol"))) + .def("matrix", &Rot2::matrix) + .def("print", &Rot2::print, print_overloads(args("s"))) + .def("rotate", &Rot2::rotate) .def("s", &Rot2::s) - + .def("theta", &Rot2::theta) + .def("unrotate", &Rot2::unrotate) .def(self * self) // __mult__ ; diff --git a/python/handwritten/geometry/Rot3.cpp b/python/handwritten/geometry/Rot3.cpp new file mode 100644 index 000000000..8f1728214 --- /dev/null +++ b/python/handwritten/geometry/Rot3.cpp @@ -0,0 +1,91 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps Rot3 class to python + * @author Andrew Melim + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include +#include "gtsam/geometry/Rot3.h" + +using namespace boost::python; +using namespace gtsam; + +// Prototypes used to perform overloading +// See: http://www.boost.org/doc/libs/1_59_0/libs/python/doc/tutorial/doc/html/python/functions.html +gtsam::Rot3 (*AxisAngle_0)(const Vector3&, double) = &Rot3::AxisAngle; +gtsam::Rot3 (*AxisAngle_1)(const gtsam::Point3&, double) = &Rot3::AxisAngle; +gtsam::Rot3 (*Rodrigues_0)(const Vector3&) = &Rot3::Rodrigues; +gtsam::Rot3 (*Rodrigues_1)(double, double, double) = &Rot3::Rodrigues; +gtsam::Rot3 (*RzRyRx_0)(double, double, double) = &Rot3::RzRyRx; +gtsam::Rot3 (*RzRyRx_1)(const Vector&) = &Rot3::RzRyRx; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Rot3::print, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Rot3::equals, 1, 2) + +void exportRot3(){ + + class_("Rot3") + .def(init<>()) + .def(init()) + .def(init()) + .def(init()) + .def(init()) + .def("AxisAngle", AxisAngle_0) + .def("AxisAngle", AxisAngle_1) + .staticmethod("AxisAngle") + .def("Expmap", &Rot3::Expmap) + .staticmethod("Expmap") + .def("ExpmapDerivative", &Rot3::ExpmapDerivative) + .staticmethod("ExpmapDerivative") + .def("Logmap", &Rot3::Logmap) + .staticmethod("Logmap") + .def("LogmapDerivative", &Rot3::LogmapDerivative) + .staticmethod("LogmapDerivative") + .def("Rodrigues", Rodrigues_0) + .def("Rodrigues", Rodrigues_1) + .staticmethod("Rodrigues") + .def("Rx", &Rot3::Rx) + .staticmethod("Rx") + .def("Ry", &Rot3::Ry) + .staticmethod("Ry") + .def("Rz", &Rot3::Rz) + .staticmethod("Rz") + .def("RzRyRx", RzRyRx_0) + .def("RzRyRx", RzRyRx_1) + .staticmethod("RzRyRx") + .def("identity", &Rot3::identity) + .staticmethod("identity") + .def("AdjointMap", &Rot3::AdjointMap) + .def("column", &Rot3::column) + .def("conjugate", &Rot3::conjugate) + .def("equals", &Rot3::equals, equals_overloads(args("q","tol"))) + .def("localCayley", &Rot3::localCayley) + .def("matrix", &Rot3::matrix) + .def("print", &Rot3::print, print_overloads(args("s"))) + .def("r1", &Rot3::r1) + .def("r2", &Rot3::r2) + .def("r3", &Rot3::r3) + .def("retractCayley", &Rot3::retractCayley) + .def("rpy", &Rot3::rpy) + .def("slerp", &Rot3::slerp) + .def("transpose", &Rot3::transpose) + .def("xyz", &Rot3::xyz) + .def(self * self) + .def(self * other()) + .def(self * other()) + .def(self_ns::str(self)) // __str__ + .def(repr(self)) // __repr__ + ; + +} \ No newline at end of file diff --git a/python/handwritten/linear/NoiseModel.cpp b/python/handwritten/linear/NoiseModel.cpp index 89ee26e9a..2b492b7e5 100644 --- a/python/handwritten/linear/NoiseModel.cpp +++ b/python/handwritten/linear/NoiseModel.cpp @@ -1,13 +1,134 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps the noise model classes into the noiseModel module + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + + /** TODOs Summary: + * + * TODO(Ellon): Don't know yet it it's worth/needed to add 'Wrap structs' for each of the noise models. + * I think it's only worthy if we want to access virtual the virtual functions from python. + * TODO(Ellon): Wrap non-pure virtual methods of Base on BaseWrap + */ + #include -#include + +#include "gtsam/linear/NoiseModel.h" using namespace boost::python; using namespace gtsam; +using namespace gtsam::noiseModel; + +// Wrap around pure virtual class Base. +// All pure virtual methods should be wrapped. Non-pure may be wrapped if we want to mimic the +// overloading through inheritance in Python. +// See: http://www.boost.org/doc/libs/1_59_0/libs/python/doc/tutorial/doc/html/python/exposing.html#python.class_virtual_functions +struct BaseCallback : Base, wrapper +{ + void print (const std::string & name="") const { + this->get_override("print")(); + } + bool equals (const Base & expected, double tol=1e-9) const { + return this->get_override("equals")(); + } + Vector whiten (const Vector & v) const { + return this->get_override("whiten")(); + } + Matrix Whiten (const Matrix & v) const { + return this->get_override("Whiten")(); + } + Vector unwhiten (const Vector & v) const { + return this->get_override("unwhiten")(); + } + double distance (const Vector & v) const { + return this->get_override("distance")(); + } + void WhitenSystem (std::vector< Matrix > &A, Vector &b) const { + this->get_override("WhitenSystem")(); + } + void WhitenSystem (Matrix &A, Vector &b) const { + this->get_override("WhitenSystem")(); + } + void WhitenSystem (Matrix &A1, Matrix &A2, Vector &b) const { + this->get_override("WhitenSystem")(); + } + void WhitenSystem (Matrix &A1, Matrix &A2, Matrix &A3, Vector &b) const { + this->get_override("WhitenSystem")(); + } + + // TODO(Ellon): Wrap non-pure virtual methods should go here. + // See: http://www.boost.org/doc/libs/1_59_0/libs/python/doc/tutorial/doc/html/python/exposing.html#python.virtual_functions_with_default_implementations + +}; + +// Overloads for named constructors. Named constructors are static, so we declare them +// using BOOST_PYTHON_FUNCTION_OVERLOADS instead of BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS +// See: http://www.boost.org/doc/libs/1_59_0/libs/python/doc/tutorial/doc/html/python/functions.html#python.default_arguments +BOOST_PYTHON_FUNCTION_OVERLOADS(Gaussian_SqrtInformation_overloads, Gaussian::SqrtInformation, 1, 2) +BOOST_PYTHON_FUNCTION_OVERLOADS(Gaussian_Information_overloads, Gaussian::Information, 1, 2) +BOOST_PYTHON_FUNCTION_OVERLOADS(Gaussian_Covariance_overloads, Gaussian::Covariance, 1, 2) +BOOST_PYTHON_FUNCTION_OVERLOADS(Diagonal_Sigmas_overloads, Diagonal::Sigmas, 1, 2) +BOOST_PYTHON_FUNCTION_OVERLOADS(Diagonal_Variances_overloads, Diagonal::Variances, 1, 2) +BOOST_PYTHON_FUNCTION_OVERLOADS(Diagonal_Precisions_overloads, Diagonal::Precisions, 1, 2) +BOOST_PYTHON_FUNCTION_OVERLOADS(Isotropic_Sigma_overloads, Isotropic::Sigma, 2, 3) +BOOST_PYTHON_FUNCTION_OVERLOADS(Isotropic_Variance_overloads, Isotropic::Variance, 2, 3) +BOOST_PYTHON_FUNCTION_OVERLOADS(Isotropic_Precision_overloads, Isotropic::Precision, 2, 3) + void exportNoiseModels(){ - // Diagonal Noise Model, no constructor - class_("DiagonalNoiseModel", no_init) - .def("Sigmas", &noiseModel::Diagonal::Sigmas) - .staticmethod("Sigmas") + + // Create a scope "noiseModel". See: http://isolation-nation.blogspot.fr/2008/09/packages-in-python-extension-modules.html + std::string noiseModel_name = extract(scope().attr("__name__") + ".noiseModel"); + object noiseModel_module(handle<>(borrowed(PyImport_AddModule(noiseModel_name.c_str())))); + scope().attr("noiseModel") = noiseModel_module; + scope noiseModel_scope = noiseModel_module; + + // Then export our classes in the noiseModel scope + class_("Base") + .def("print", pure_virtual(&Base::print)) ; + + // NOTE: We should use "Base" in "bases<...>", and not "BaseCallback" (it was not clear at the begining) + class_, bases >("Gaussian", no_init) + .def("SqrtInformation",&Gaussian::SqrtInformation, Gaussian_SqrtInformation_overloads()) + .staticmethod("SqrtInformation") + .def("Information",&Gaussian::Information, Gaussian_Information_overloads()) + .staticmethod("Information") + .def("Covariance",&Gaussian::Covariance, Gaussian_Covariance_overloads()) + .staticmethod("Covariance") + ; + + class_, bases >("Diagonal", no_init) + .def("Sigmas",&Diagonal::Sigmas, Diagonal_Sigmas_overloads()) + .staticmethod("Sigmas") + .def("Variances",&Diagonal::Variances, Diagonal_Variances_overloads()) + .staticmethod("Variances") + .def("Precisions",&Diagonal::Precisions, Diagonal_Precisions_overloads()) + .staticmethod("Precisions") + ; + + class_, bases >("Isotropic", no_init) + .def("Sigma",&Isotropic::Sigma, Isotropic_Sigma_overloads()) + .staticmethod("Sigma") + .def("Variance",&Isotropic::Variance, Isotropic_Variance_overloads()) + .staticmethod("Variance") + .def("Precision",&Isotropic::Precision, Isotropic_Precision_overloads()) + .staticmethod("Precision") + ; + + class_, bases >("Unit", no_init) + .def("Create",&Unit::Create) + .staticmethod("Create") + ; + } \ No newline at end of file diff --git a/python/handwritten/nonlinear/ISAM2.cpp b/python/handwritten/nonlinear/ISAM2.cpp new file mode 100644 index 000000000..d55da3752 --- /dev/null +++ b/python/handwritten/nonlinear/ISAM2.cpp @@ -0,0 +1,44 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief exports ISAM2 class to python + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include +#include "gtsam/nonlinear/ISAM2.h" + +using namespace boost::python; +using namespace gtsam; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(update_overloads, ISAM2::update, 0, 7) + +void exportISAM2(){ + +// TODO(Ellon): Export all properties of ISAM2Params +class_("ISAM2Params") +; + +// TODO(Ellon): Export useful methods/properties of ISAM2Result +class_("ISAM2Result") +; + +// Function pointers for overloads in ISAM2 +Values (ISAM2::*calculateEstimate_0)() const = &ISAM2::calculateEstimate; + +class_("ISAM2") + // TODO(Ellon): wrap all optional values of update + .def("update",&ISAM2::update, update_overloads()) + .def("calculate_estimate", calculateEstimate_0) +; + +} \ No newline at end of file diff --git a/python/handwritten/nonlinear/NonlinearFactor.cpp b/python/handwritten/nonlinear/NonlinearFactor.cpp new file mode 100644 index 000000000..e111f65f7 --- /dev/null +++ b/python/handwritten/nonlinear/NonlinearFactor.cpp @@ -0,0 +1,45 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief exports virtual class NonlinearFactor to python + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include +#include "gtsam/nonlinear/NonlinearFactor.h" + +using namespace boost::python; +using namespace gtsam; + +// Wrap around pure virtual class NonlinearFactor. +// All pure virtual methods should be wrapped. Non-pure may be wrapped if we want to mimic the +// overloading through inheritance in Python. +// See: http://www.boost.org/doc/libs/1_59_0/libs/python/doc/tutorial/doc/html/python/exposing.html#python.class_virtual_functions +struct NonlinearFactorCallback : NonlinearFactor, wrapper +{ + double error (const Values & values) const { + return this->get_override("error")(values); + } + size_t dim () const { + return this->get_override("dim")(); + } + boost::shared_ptr linearize(const Values & values) const { + return this->get_override("linearize")(values); + } +}; + +void exportNonlinearFactor(){ + + class_("NonlinearFactor") + ; + +} \ No newline at end of file diff --git a/python/handwritten/nonlinear/NonlinearFactorGraph.cpp b/python/handwritten/nonlinear/NonlinearFactorGraph.cpp index 16aa4c6e4..a2262f9fd 100644 --- a/python/handwritten/nonlinear/NonlinearFactorGraph.cpp +++ b/python/handwritten/nonlinear/NonlinearFactorGraph.cpp @@ -1,6 +1,23 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief exports NonlinearFactorGraph class to python + * @author Andrew Melim + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + #include #include "gtsam/nonlinear/NonlinearFactorGraph.h" -#include +#include "gtsam/nonlinear/NonlinearFactor.h" using namespace boost::python; using namespace gtsam; @@ -8,11 +25,15 @@ using namespace gtsam; void exportNonlinearFactorGraph(){ - typedef boost::shared_ptr shared_factor; + typedef NonlinearFactorGraph::sharedFactor sharedFactor; - void (NonlinearFactorGraph::*push_back1)(const shared_factor& factor) = &NonlinearFactorGraph::push_back; + void (NonlinearFactorGraph::*push_back1)(const sharedFactor&) = &NonlinearFactorGraph::push_back; + void (NonlinearFactorGraph::*add1)(const sharedFactor&) = &NonlinearFactorGraph::add; class_("NonlinearFactorGraph", init<>()) - .def("push_back", push_back1) + .def("size",&NonlinearFactorGraph::size) + .def("push_back", push_back1) + .def("add", add1) ; + } \ No newline at end of file diff --git a/python/handwritten/nonlinear/Values.cpp b/python/handwritten/nonlinear/Values.cpp index c1451cef1..7d68e88c8 100644 --- a/python/handwritten/nonlinear/Values.cpp +++ b/python/handwritten/nonlinear/Values.cpp @@ -1,19 +1,107 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps Values class to python + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + #include -#include +#include "gtsam/nonlinear/Values.h" +#include "gtsam/geometry/Point3.h" +#include "gtsam/geometry/Rot3.h" +#include "gtsam/geometry/Pose3.h" using namespace boost::python; using namespace gtsam; +/** The function ValuesAt is a workaround to be able to call the correct templated version + * of Values::at. Without it, python would only try to match the last 'at' metho defined + * below. With this wrapper function we can call 'at' in python passing an extra type, + * which will define the type to be returned. Example: + * + * >>> import gtsam + * >>> v = gtsam.nonlinear.Values() + * >>> v.insert(1,gtsam.geometry.Point3()) + * >>> v.insert(2,gtsam.geometry.Rot3()) + * >>> v.insert(3,gtsam.geometry.Pose3()) + * >>> v.at(1,gtsam.geometry.Point3()) + * >>> v.at(2,gtsam.geometry.Rot3()) + * >>> v.at(3,gtsam.geometry.Pose3()) + * + * A more 'pythonic' way I think would be to not use this function and define different + * 'at' methods below using the name of the type in the function name, like: + * + * .def("point3_at", &Values::at, return_internal_reference<>()) + * .def("rot3_at", &Values::at, return_internal_reference<>()) + * .def("pose3_at", &Values::at, return_internal_reference<>()) + * + * and then they could be accessed from python as + * + * >>> import gtsam + * >>> v = gtsam.nonlinear.Values() + * >>> v.insert(1,gtsam.geometry.Point3()) + * >>> v.insert(2,gtsam.geometry.Rot3()) + * >>> v.insert(3,gtsam.geometry.Pose3()) + * >>> v.point3_at(1) + * >>> v.rot3_at(2) + * >>> v.pose3_at(3) + * + * In fact, I just saw the pythonic way sounds more clear, so I'm sticking with this and + * leaving the comments here for future reference. I'm using the PEP0008 for method naming. + * See: https://www.python.org/dev/peps/pep-0008/#function-and-method-arguments + */ +// template +// const T & ValuesAt( const Values & v, Key j, T /*type*/) +// { +// return v.at(j); +// } + void exportValues(){ - const Value& (Values::*at1)(Key) const = &Values::at; + // NOTE: Apparently the class 'Value'' is deprecated, so the commented lines below + // will compile, but are useless in the python wrapper. We need to use specific + // 'at' and 'insert' methods for each type. + // const Value& (Values::*at1)(Key) const = &Values::at; + // void (Values::*insert1)(Key, const Value&) = &Values::insert; bool (Values::*exists1)(Key) const = &Values::exists; - void (Values::*insert1)(Key, const Value&) = &Values::insert; + void (Values::*insert_point3)(Key, const gtsam::Point3&) = &Values::insert; + void (Values::*insert_rot3) (Key, const gtsam::Rot3&) = &Values::insert; + void (Values::*insert_pose3) (Key, const gtsam::Pose3&) = &Values::insert; class_("Values", init<>()) .def(init()) - .def("at", at1, return_value_policy()) + .def("clear", &Values::clear) + .def("dim", &Values::dim) + .def("empty", &Values::empty) + .def("equals", &Values::equals) + .def("erase", &Values::erase) + .def("insert_fixed", &Values::insertFixed) + .def("print", &Values::print) + .def("size", &Values::size) + .def("swap", &Values::swap) + // NOTE: Following commented lines add useless methods on Values + // .def("insert", insert1) + // .def("at", at1, return_value_policy()) + .def("insert", insert_point3) + .def("insert", insert_rot3) + .def("insert", insert_pose3) + // NOTE: The following commented lines are another way of specializing the return type. + // See long comment above. + // .def("at", &ValuesAt, return_internal_reference<>()) + // .def("at", &ValuesAt, return_internal_reference<>()) + // .def("at", &ValuesAt, return_internal_reference<>()) + .def("point3_at", &Values::at, return_value_policy()) + .def("rot3_at", &Values::at, return_value_policy()) + .def("pose3_at", &Values::at, return_value_policy()) .def("exists", exists1) - .def("insert", insert1, return_value_policy()) ; } \ No newline at end of file diff --git a/python/handwritten/slam/BetweenFactor.cpp b/python/handwritten/slam/BetweenFactor.cpp index 2732e4e7e..428f54e6f 100644 --- a/python/handwritten/slam/BetweenFactor.cpp +++ b/python/handwritten/slam/BetweenFactor.cpp @@ -1,14 +1,53 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps BetweenFactor for several values to python + * @author Andrew Melim + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + #include -#include +#include "gtsam/slam/BetweenFactor.h" +#include "gtsam/geometry/Point2.h" +#include "gtsam/geometry/Rot2.h" +#include "gtsam/geometry/Pose2.h" +#include "gtsam/geometry/Point3.h" +#include "gtsam/geometry/Rot3.h" +#include "gtsam/geometry/Pose3.h" using namespace boost::python; using namespace gtsam; using namespace std; -template -void exportBetweenFactor(const std::string& name){ - class_(name, init<>()) - .def(init()) - ; -} \ No newline at end of file +// template +// void exportBetweenFactor(const std::string& name){ +// class_(name, init<>()) +// .def(init()) +// ; +// } + +#define BETWEENFACTOR(VALUE) \ + class_< BetweenFactor, bases, boost::shared_ptr< BetweenFactor > >("BetweenFactor"#VALUE) \ + .def(init()) \ + .def("measured", &BetweenFactor::measured, return_internal_reference<>()) \ +; + +void exportBetweenFactors() +{ + BETWEENFACTOR(Point2) + BETWEENFACTOR(Rot2) + BETWEENFACTOR(Pose2) + BETWEENFACTOR(Point3) + BETWEENFACTOR(Rot3) + BETWEENFACTOR(Pose3) +} diff --git a/python/handwritten/slam/PriorFactor.cpp b/python/handwritten/slam/PriorFactor.cpp index 1984fe144..6004c9957 100644 --- a/python/handwritten/slam/PriorFactor.cpp +++ b/python/handwritten/slam/PriorFactor.cpp @@ -1,14 +1,53 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps PriorFactor for several values to python + * @author Andrew Melim + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + #include -#include +#include "gtsam/slam/PriorFactor.h" +#include "gtsam/geometry/Point2.h" +#include "gtsam/geometry/Rot2.h" +#include "gtsam/geometry/Pose2.h" +#include "gtsam/geometry/Point3.h" +#include "gtsam/geometry/Rot3.h" +#include "gtsam/geometry/Pose3.h" using namespace boost::python; using namespace gtsam; using namespace std; -template< class FACTOR, class VALUE > -void exportPriorFactor(const std::string& name){ - class_< FACTOR >(name.c_str(), init<>()) - .def(init< Key, VALUE&, SharedNoiseModel >()) - ; +// template< class FACTOR, class VALUE > +// void exportPriorFactor(const std::string& name){ +// class_< FACTOR >(name.c_str(), init<>()) +// .def(init< Key, VALUE&, SharedNoiseModel >()) +// ; +// } + +#define PRIORFACTOR(VALUE) \ + class_< PriorFactor, bases, boost::shared_ptr< PriorFactor > >("PriorFactor"#VALUE) \ + .def(init()) \ + .def("prior", &PriorFactor::prior, return_internal_reference<>()) \ +; + +void exportPriorFactors() +{ + PRIORFACTOR(Point2) + PRIORFACTOR(Rot2) + PRIORFACTOR(Pose2) + PRIORFACTOR(Point3) + PRIORFACTOR(Rot3) + PRIORFACTOR(Pose3) } \ No newline at end of file diff --git a/python/handwritten/utils/NumpyEigen.cpp b/python/handwritten/utils/NumpyEigen.cpp new file mode 100644 index 000000000..82d3b6d5a --- /dev/null +++ b/python/handwritten/utils/NumpyEigen.cpp @@ -0,0 +1,52 @@ + /* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief register conversion matrix between numpy and Eigen + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + + #include + +#include + +#include "gtsam/base/Matrix.h" +#include "gtsam/base/Vector.h" + +using namespace boost::python; +using namespace gtsam; + +void registerNumpyEigenConversions() +{ + import_array(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + +}