Merged in feature/faster_cython (pull request #300)
Faster and more concise cython Approved-by: Duy-Nguyen Ta <thduynguyen@gmail.com>release/4.3a0
commit
03c939fafe
|
@ -52,7 +52,7 @@ function(pyx_to_cpp target pyx_file generated_cpp include_dirs)
|
|||
add_custom_command(
|
||||
OUTPUT ${generated_cpp}
|
||||
COMMAND
|
||||
${CYTHON_EXECUTABLE} -a -v --cplus ${includes_for_cython} ${pyx_file} -o ${generated_cpp}
|
||||
${CYTHON_EXECUTABLE} -X boundscheck=False -a -v --fast-fail --cplus ${includes_for_cython} ${pyx_file} -o ${generated_cpp}
|
||||
VERBATIM)
|
||||
add_custom_target(${target} ALL DEPENDS ${generated_cpp})
|
||||
endfunction()
|
||||
|
@ -68,7 +68,7 @@ endfunction()
|
|||
# - output_dir: The output directory
|
||||
function(build_cythonized_cpp target cpp_file output_lib_we output_dir)
|
||||
add_library(${target} MODULE ${cpp_file})
|
||||
set_target_properties(${target} PROPERTIES LINK_FLAGS "-undefined dynamic_lookup"
|
||||
set_target_properties(${target} PROPERTIES COMPILE_FLAGS "-w" LINK_FLAGS "-undefined dynamic_lookup"
|
||||
OUTPUT_NAME ${output_lib_we} PREFIX "" LIBRARY_OUTPUT_DIRECTORY ${output_dir})
|
||||
endfunction()
|
||||
|
||||
|
|
|
@ -26,9 +26,11 @@ if (GTSAM_INSTALL_CYTHON_TOOLBOX)
|
|||
gtsam_unstable # library to link with
|
||||
"gtsam_unstable;gtsam_unstable_header;gtsam_cython" # dependencies to be built before wrapping
|
||||
)
|
||||
# for some reasons cython gtsam_unstable can't find gtsam/gtsam.pxd without doing this
|
||||
file(WRITE ${PROJECT_BINARY_DIR}/cython/gtsam_unstable/__init__.py "")
|
||||
endif()
|
||||
|
||||
# Install the custom-generated __init__.py with gtsam_unstable disabled
|
||||
# Install the custom-generated __init__.py
|
||||
# This is to make the build/cython/gtsam folder a python package, so gtsam can be found while wrapping gtsam_unstable
|
||||
configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam/__init__.py.in ${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py)
|
||||
install_cython_files("${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py" "${GTSAM_CYTHON_INSTALL_PATH}/gtsam")
|
||||
|
|
|
@ -1,6 +1,5 @@
|
|||
import unittest
|
||||
import gtsam
|
||||
from math import *
|
||||
import numpy as np
|
||||
|
||||
|
||||
|
@ -11,5 +10,13 @@ class TestCal3Unified(unittest.TestCase):
|
|||
self.assertEqual(K.fx(), 1.)
|
||||
self.assertEqual(K.fx(), 1.)
|
||||
|
||||
def test_retract(self):
|
||||
expected = gtsam.Cal3Unified(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, 1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10, 0.1 + 1)
|
||||
K = gtsam.Cal3Unified(100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0.1)
|
||||
d = np.array([2, 3, 4, 5, 6, 7, 8, 9, 10, 1], order='F')
|
||||
actual = K.retract(d)
|
||||
self.assertTrue(actual.equals(expected, 1e-9))
|
||||
np.testing.assert_allclose(d, K.localCoordinates(actual))
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
|
@ -0,0 +1,44 @@
|
|||
import math
|
||||
import unittest
|
||||
|
||||
from gtsam import Point3, Rot3, Pose3
|
||||
|
||||
|
||||
class TestPose3(unittest.TestCase):
|
||||
|
||||
def test__between(self):
|
||||
T2 = Pose3(Rot3.Rodrigues(0.3,0.2,0.1),Point3(3.5,-8.2,4.2))
|
||||
T3 = Pose3(Rot3.Rodrigues(-90, 0, 0), Point3(1, 2, 3))
|
||||
expected = T2.inverse().compose(T3)
|
||||
actual = T2.between(T3)
|
||||
self.assertTrue(actual.equals(expected, 1e-6))
|
||||
|
||||
def test_transform_to(self):
|
||||
transform = Pose3(Rot3.Rodrigues(0,0,-1.570796), Point3(2,4, 0))
|
||||
actual = transform.transform_to(Point3(3,2,10))
|
||||
expected = Point3 (2,1,10)
|
||||
self.assertTrue(actual.equals(expected, 1e-6))
|
||||
|
||||
def test_range(self):
|
||||
l1 = Point3(1, 0, 0)
|
||||
l2 = Point3(1, 1, 0)
|
||||
x1 = Pose3()
|
||||
|
||||
xl1 = Pose3(Rot3.Ypr(0.0, 0.0, 0.0), Point3(1, 0, 0))
|
||||
xl2 = Pose3(Rot3.Ypr(0.0, 1.0, 0.0), Point3(1, 1, 0))
|
||||
|
||||
# establish range is indeed zero
|
||||
self.assertEqual(1,x1.range(point=l1))
|
||||
|
||||
# establish range is indeed sqrt2
|
||||
self.assertEqual(math.sqrt(2.0),x1.range(point=l2))
|
||||
|
||||
# establish range is indeed zero
|
||||
self.assertEqual(1,x1.range(pose=xl1))
|
||||
|
||||
# establish range is indeed sqrt2
|
||||
self.assertEqual(math.sqrt(2.0),x1.range(pose=xl2))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
|
@ -0,0 +1,32 @@
|
|||
import math
|
||||
import numpy as np
|
||||
import unittest
|
||||
|
||||
from gtsam import Pose2, Point3, Rot3, Pose3, Cal3_S2, SimpleCamera
|
||||
|
||||
K = Cal3_S2(625, 625, 0, 0, 0)
|
||||
|
||||
class TestSimpleCamera(unittest.TestCase):
|
||||
|
||||
def test_constructor(self):
|
||||
pose1 = Pose3(Rot3(np.diag([1, -1, -1])), Point3(0, 0, 0.5))
|
||||
camera = SimpleCamera(pose1, K)
|
||||
self.assertTrue(camera.calibration().equals(K, 1e-9))
|
||||
self.assertTrue(camera.pose().equals(pose1, 1e-9))
|
||||
|
||||
def test_level2(self):
|
||||
# Create a level camera, looking in Y-direction
|
||||
pose2 = Pose2(0.4,0.3,math.pi/2.0)
|
||||
camera = SimpleCamera.Level(K, pose2, 0.1)
|
||||
|
||||
# expected
|
||||
x = Point3(1,0,0)
|
||||
y = Point3(0,0,-1)
|
||||
z = Point3(0,1,0)
|
||||
wRc = Rot3(x,y,z)
|
||||
expected = Pose3(wRc,Point3(0.4,0.3,0.1))
|
||||
self.assertTrue(camera.pose().equals(expected, 1e-9))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
|
@ -1,25 +1,28 @@
|
|||
import unittest
|
||||
import gtsam
|
||||
import numpy as np
|
||||
|
||||
from gtsam import Point2, Point3, Unit3, Rot2, Pose2, Rot3, Pose3
|
||||
from gtsam import Values, Cal3_S2, Cal3DS2, Cal3Bundler, EssentialMatrix, imuBias_ConstantBias
|
||||
|
||||
|
||||
class TestValues(unittest.TestCase):
|
||||
|
||||
def test_values(self):
|
||||
values = gtsam.Values()
|
||||
E = gtsam.EssentialMatrix(gtsam.Rot3(), gtsam.Unit3())
|
||||
values = Values()
|
||||
E = EssentialMatrix(Rot3(), Unit3())
|
||||
tol = 1e-9
|
||||
|
||||
values.insert(0, gtsam.Point2())
|
||||
values.insert(1, gtsam.Point3())
|
||||
values.insert(2, gtsam.Rot2())
|
||||
values.insert(3, gtsam.Pose2())
|
||||
values.insert(4, gtsam.Rot3())
|
||||
values.insert(5, gtsam.Pose3())
|
||||
values.insert(6, gtsam.Cal3_S2())
|
||||
values.insert(7, gtsam.Cal3DS2())
|
||||
values.insert(8, gtsam.Cal3Bundler())
|
||||
values.insert(0, Point2(0,0))
|
||||
values.insert(1, Point3(0,0,0))
|
||||
values.insert(2, Rot2())
|
||||
values.insert(3, Pose2())
|
||||
values.insert(4, Rot3())
|
||||
values.insert(5, Pose3())
|
||||
values.insert(6, Cal3_S2())
|
||||
values.insert(7, Cal3DS2())
|
||||
values.insert(8, Cal3Bundler())
|
||||
values.insert(9, E)
|
||||
values.insert(10, gtsam.imuBias_ConstantBias())
|
||||
values.insert(10, imuBias_ConstantBias())
|
||||
|
||||
# Special cases for Vectors and Matrices
|
||||
# Note that gtsam's Eigen Vectors and Matrices requires double-precision
|
||||
|
@ -29,7 +32,7 @@ class TestValues(unittest.TestCase):
|
|||
# will have 'int' type.
|
||||
#
|
||||
# The wrapper will automatically fix the type and storage order for you,
|
||||
# but for performace reasons, it's recommended to specify the correct
|
||||
# but for performance reasons, it's recommended to specify the correct
|
||||
# type and storage order.
|
||||
vec = np.array([1., 2., 3.]) # for vectors, the order is not important, but dtype still is
|
||||
values.insert(11, vec)
|
||||
|
@ -41,18 +44,18 @@ class TestValues(unittest.TestCase):
|
|||
mat2 = np.array([[1,2,],[3,5]])
|
||||
values.insert(13, mat2)
|
||||
|
||||
self.assertTrue(values.atPoint2(0).equals(gtsam.Point2(), tol))
|
||||
self.assertTrue(values.atPoint3(1).equals(gtsam.Point3(), tol))
|
||||
self.assertTrue(values.atRot2(2).equals(gtsam.Rot2(), tol))
|
||||
self.assertTrue(values.atPose2(3).equals(gtsam.Pose2(), tol))
|
||||
self.assertTrue(values.atRot3(4).equals(gtsam.Rot3(), tol))
|
||||
self.assertTrue(values.atPose3(5).equals(gtsam.Pose3(), tol))
|
||||
self.assertTrue(values.atCal3_S2(6).equals(gtsam.Cal3_S2(), tol))
|
||||
self.assertTrue(values.atCal3DS2(7).equals(gtsam.Cal3DS2(), tol))
|
||||
self.assertTrue(values.atCal3Bundler(8).equals(gtsam.Cal3Bundler(), tol))
|
||||
self.assertTrue(values.atPoint2(0).equals(Point2(), tol))
|
||||
self.assertTrue(values.atPoint3(1).equals(Point3(), tol))
|
||||
self.assertTrue(values.atRot2(2).equals(Rot2(), tol))
|
||||
self.assertTrue(values.atPose2(3).equals(Pose2(), tol))
|
||||
self.assertTrue(values.atRot3(4).equals(Rot3(), tol))
|
||||
self.assertTrue(values.atPose3(5).equals(Pose3(), tol))
|
||||
self.assertTrue(values.atCal3_S2(6).equals(Cal3_S2(), tol))
|
||||
self.assertTrue(values.atCal3DS2(7).equals(Cal3DS2(), tol))
|
||||
self.assertTrue(values.atCal3Bundler(8).equals(Cal3Bundler(), tol))
|
||||
self.assertTrue(values.atEssentialMatrix(9).equals(E, tol))
|
||||
self.assertTrue(values.atimuBias_ConstantBias(
|
||||
10).equals(gtsam.imuBias_ConstantBias(), tol))
|
||||
10).equals(imuBias_ConstantBias(), tol))
|
||||
|
||||
# special cases for Vector and Matrix:
|
||||
actualVector = values.atVector(11)
|
||||
|
|
|
@ -33,5 +33,6 @@ install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
|
|||
PATTERN "__init__.py.in" EXCLUDE)
|
||||
install(TARGETS cythonize_eigency_core cythonize_eigency_conversions
|
||||
DESTINATION "${GTSAM_CYTHON_INSTALL_PATH}/gtsam_eigency")
|
||||
install(FILES "${OUTPUT_DIR}/conversions_api.h" DESTINATION ${GTSAM_CYTHON_INSTALL_PATH}/gtsam_eigency)
|
||||
configure_file(__init__.py.in ${GTSAM_CYTHON_INSTALL_PATH}/gtsam_eigency/__init__.py)
|
||||
install(FILES ${OUTPUT_DIR}/conversions_api.h DESTINATION ${GTSAM_CYTHON_INSTALL_PATH}/gtsam_eigency)
|
||||
configure_file(__init__.py.in ${OUTPUT_DIR}/__init__.py)
|
||||
install(FILES ${OUTPUT_DIR}/__init__.py DESTINATION ${GTSAM_CYTHON_INSTALL_PATH}/gtsam_eigency)
|
||||
|
|
132
gtsam.h
132
gtsam.h
|
@ -2,9 +2,13 @@
|
|||
|
||||
* GTSAM Wrap Module Definition
|
||||
*
|
||||
* These are the current classes available through the matlab toolbox interface,
|
||||
* These are the current classes available through the matlab and python wrappers,
|
||||
* add more functions/classes as they are available.
|
||||
*
|
||||
* IMPORTANT: the python wrapper supports keyword arguments for functions/methods. Hence, the
|
||||
* argument names matter. An implementation restriction is that in overloaded methods
|
||||
* or functions, arguments of different types *have* to have different names.
|
||||
*
|
||||
* Requirements:
|
||||
* Classes must start with an uppercase letter
|
||||
* - Can wrap a typedef
|
||||
|
@ -16,7 +20,7 @@
|
|||
* - Any class with which be copied with boost::make_shared()
|
||||
* - boost::shared_ptr of any object type
|
||||
* Constructors
|
||||
* - Overloads are supported
|
||||
* - Overloads are supported, but arguments of different types *have* to have different names
|
||||
* - A class with no constructors can be returned from other functions but not allocated directly in MATLAB
|
||||
* Methods
|
||||
* - Constness has no effect
|
||||
|
@ -26,7 +30,7 @@
|
|||
* Static methods
|
||||
* - Must start with a letter (upper or lowercase) and use the "static" keyword
|
||||
* - The first letter will be made uppercase in the generated MATLAB interface
|
||||
* - Overloads are supported
|
||||
* - Overloads are supported, but arguments of different types *have* to have different names
|
||||
* Arguments to functions any of
|
||||
* - Eigen types: Matrix, Vector
|
||||
* - Eigen types and classes as an optionally const reference
|
||||
|
@ -145,9 +149,9 @@ class KeyList {
|
|||
// Actually a FastSet<Key>
|
||||
class KeySet {
|
||||
KeySet();
|
||||
KeySet(const gtsam::KeySet& other);
|
||||
KeySet(const gtsam::KeyVector& other);
|
||||
KeySet(const gtsam::KeyList& other);
|
||||
KeySet(const gtsam::KeySet& set);
|
||||
KeySet(const gtsam::KeyVector& vector);
|
||||
KeySet(const gtsam::KeyList& list);
|
||||
|
||||
// Testable
|
||||
void print(string s) const;
|
||||
|
@ -480,6 +484,11 @@ class Rot3 {
|
|||
// Standard Constructors and Named Constructors
|
||||
Rot3();
|
||||
Rot3(Matrix R);
|
||||
Rot3(const gtsam::Point3& col1, const gtsam::Point3& col2, const gtsam::Point3& col3);
|
||||
Rot3(double R11, double R12, double R13,
|
||||
double R21, double R22, double R23,
|
||||
double R31, double R32, double R33);
|
||||
|
||||
static gtsam::Rot3 Rx(double t);
|
||||
static gtsam::Rot3 Ry(double t);
|
||||
static gtsam::Rot3 Rz(double t);
|
||||
|
@ -491,6 +500,7 @@ class Rot3 {
|
|||
static gtsam::Rot3 Ypr(double y, double p, double r);
|
||||
static gtsam::Rot3 Quaternion(double w, double x, double y, double z);
|
||||
static gtsam::Rot3 Rodrigues(Vector v);
|
||||
static gtsam::Rot3 Rodrigues(double wx, double wy, double wz);
|
||||
|
||||
// Testable
|
||||
void print(string s) const;
|
||||
|
@ -586,7 +596,7 @@ class Pose3 {
|
|||
Pose3(const gtsam::Pose3& other);
|
||||
Pose3(const gtsam::Rot3& r, const gtsam::Point3& t);
|
||||
Pose3(const gtsam::Pose2& pose2); // FIXME: shadows Pose3(Pose3 pose)
|
||||
Pose3(Matrix t);
|
||||
Pose3(Matrix mat);
|
||||
|
||||
// Testable
|
||||
void print(string s) const;
|
||||
|
@ -595,23 +605,23 @@ class Pose3 {
|
|||
// Group
|
||||
static gtsam::Pose3 identity();
|
||||
gtsam::Pose3 inverse() const;
|
||||
gtsam::Pose3 compose(const gtsam::Pose3& p2) const;
|
||||
gtsam::Pose3 between(const gtsam::Pose3& p2) const;
|
||||
gtsam::Pose3 compose(const gtsam::Pose3& pose) const;
|
||||
gtsam::Pose3 between(const gtsam::Pose3& pose) const;
|
||||
|
||||
// Manifold
|
||||
gtsam::Pose3 retract(Vector v) const;
|
||||
Vector localCoordinates(const gtsam::Pose3& T2) const;
|
||||
Vector localCoordinates(const gtsam::Pose3& pose) const;
|
||||
|
||||
// Lie Group
|
||||
static gtsam::Pose3 Expmap(Vector v);
|
||||
static Vector Logmap(const gtsam::Pose3& p);
|
||||
static Vector Logmap(const gtsam::Pose3& pose);
|
||||
Matrix AdjointMap() const;
|
||||
Vector Adjoint(Vector xi) const;
|
||||
static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz);
|
||||
|
||||
// Group Action on Point3
|
||||
gtsam::Point3 transform_from(const gtsam::Point3& p) const;
|
||||
gtsam::Point3 transform_to(const gtsam::Point3& p) const;
|
||||
gtsam::Point3 transform_from(const gtsam::Point3& point) const;
|
||||
gtsam::Point3 transform_to(const gtsam::Point3& point) const;
|
||||
|
||||
// Standard Interface
|
||||
gtsam::Rot3 rotation() const;
|
||||
|
@ -636,7 +646,7 @@ class Pose3Vector
|
|||
size_t size() const;
|
||||
bool empty() const;
|
||||
gtsam::Pose3 at(size_t n) const;
|
||||
void push_back(const gtsam::Pose3& x);
|
||||
void push_back(const gtsam::Pose3& pose);
|
||||
};
|
||||
|
||||
#include <gtsam/geometry/Unit3.h>
|
||||
|
@ -909,7 +919,7 @@ class PinholeCamera {
|
|||
gtsam::Point2 project(const gtsam::Point3& point);
|
||||
gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const;
|
||||
double range(const gtsam::Point3& point);
|
||||
double range(const gtsam::Pose3& point);
|
||||
double range(const gtsam::Pose3& pose);
|
||||
|
||||
// enabling serialization functionality
|
||||
void serialize() const;
|
||||
|
@ -946,7 +956,7 @@ virtual class SimpleCamera {
|
|||
gtsam::Point2 project(const gtsam::Point3& point);
|
||||
gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const;
|
||||
double range(const gtsam::Point3& point);
|
||||
double range(const gtsam::Pose3& point);
|
||||
double range(const gtsam::Pose3& pose);
|
||||
|
||||
// enabling serialization functionality
|
||||
void serialize() const;
|
||||
|
@ -1060,13 +1070,13 @@ virtual class SymbolicFactorGraph {
|
|||
const gtsam::Ordering& ordering);
|
||||
pair<gtsam::SymbolicBayesTree*, gtsam::SymbolicFactorGraph*> eliminatePartialMultifrontal(
|
||||
const gtsam::KeyVector& keys);
|
||||
gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& variables);
|
||||
gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& variables);
|
||||
gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& variables,
|
||||
gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering);
|
||||
gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector);
|
||||
gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering,
|
||||
const gtsam::Ordering& marginalizedVariableOrdering);
|
||||
gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& variables,
|
||||
gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector,
|
||||
const gtsam::Ordering& marginalizedVariableOrdering);
|
||||
gtsam::SymbolicFactorGraph* marginal(const gtsam::KeyVector& variables);
|
||||
gtsam::SymbolicFactorGraph* marginal(const gtsam::KeyVector& key_vector);
|
||||
};
|
||||
|
||||
#include <gtsam/symbolic/SymbolicConditional.h>
|
||||
|
@ -1164,9 +1174,9 @@ class VariableIndex {
|
|||
//template<T = {gtsam::FactorGraph}>
|
||||
//VariableIndex(const T& factorGraph, size_t nVariables);
|
||||
//VariableIndex(const T& factorGraph);
|
||||
VariableIndex(const gtsam::SymbolicFactorGraph& factorGraph);
|
||||
VariableIndex(const gtsam::GaussianFactorGraph& factorGraph);
|
||||
VariableIndex(const gtsam::NonlinearFactorGraph& factorGraph);
|
||||
VariableIndex(const gtsam::SymbolicFactorGraph& sfg);
|
||||
VariableIndex(const gtsam::GaussianFactorGraph& gfg);
|
||||
VariableIndex(const gtsam::NonlinearFactorGraph& fg);
|
||||
VariableIndex(const gtsam::VariableIndex& other);
|
||||
|
||||
// Testable
|
||||
|
@ -1460,7 +1470,7 @@ class GaussianFactorGraph {
|
|||
|
||||
// Building the graph
|
||||
void push_back(const gtsam::GaussianFactor* factor);
|
||||
void push_back(const gtsam::GaussianConditional* factor);
|
||||
void push_back(const gtsam::GaussianConditional* conditional);
|
||||
void push_back(const gtsam::GaussianFactorGraph& graph);
|
||||
void push_back(const gtsam::GaussianBayesNet& bayesNet);
|
||||
void push_back(const gtsam::GaussianBayesTree& bayesTree);
|
||||
|
@ -1499,13 +1509,13 @@ class GaussianFactorGraph {
|
|||
const gtsam::Ordering& ordering);
|
||||
pair<gtsam::GaussianBayesTree*, gtsam::GaussianFactorGraph*> eliminatePartialMultifrontal(
|
||||
const gtsam::KeyVector& keys);
|
||||
gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& variables);
|
||||
gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& variables);
|
||||
gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& variables,
|
||||
gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering);
|
||||
gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector);
|
||||
gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering,
|
||||
const gtsam::Ordering& marginalizedVariableOrdering);
|
||||
gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& variables,
|
||||
gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector,
|
||||
const gtsam::Ordering& marginalizedVariableOrdering);
|
||||
gtsam::GaussianFactorGraph* marginal(const gtsam::KeyVector& variables);
|
||||
gtsam::GaussianFactorGraph* marginal(const gtsam::KeyVector& key_vector);
|
||||
|
||||
// Conversion to matrices
|
||||
Matrix sparseJacobian_() const;
|
||||
|
@ -1859,34 +1869,34 @@ class Values {
|
|||
// void update(size_t j, const gtsam::Value& val);
|
||||
// gtsam::Value at(size_t j) const;
|
||||
|
||||
void insert(size_t j, const gtsam::Point2& t);
|
||||
void insert(size_t j, const gtsam::Point3& t);
|
||||
void insert(size_t j, const gtsam::Rot2& t);
|
||||
void insert(size_t j, const gtsam::Pose2& t);
|
||||
void insert(size_t j, const gtsam::Rot3& t);
|
||||
void insert(size_t j, const gtsam::Pose3& t);
|
||||
void insert(size_t j, const gtsam::Cal3_S2& t);
|
||||
void insert(size_t j, const gtsam::Cal3DS2& t);
|
||||
void insert(size_t j, const gtsam::Cal3Bundler& t);
|
||||
void insert(size_t j, const gtsam::EssentialMatrix& t);
|
||||
void insert(size_t j, const gtsam::SimpleCamera& t);
|
||||
void insert(size_t j, const gtsam::imuBias::ConstantBias& t);
|
||||
void insert(size_t j, Vector t);
|
||||
void insert(size_t j, Matrix t);
|
||||
void insert(size_t j, const gtsam::Point2& point2);
|
||||
void insert(size_t j, const gtsam::Point3& point3);
|
||||
void insert(size_t j, const gtsam::Rot2& rot2);
|
||||
void insert(size_t j, const gtsam::Pose2& pose2);
|
||||
void insert(size_t j, const gtsam::Rot3& rot3);
|
||||
void insert(size_t j, const gtsam::Pose3& pose3);
|
||||
void insert(size_t j, const gtsam::Cal3_S2& cal3_s2);
|
||||
void insert(size_t j, const gtsam::Cal3DS2& cal3ds2);
|
||||
void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler);
|
||||
void insert(size_t j, const gtsam::EssentialMatrix& essential_matrix);
|
||||
void insert(size_t j, const gtsam::SimpleCamera& simpel_camera);
|
||||
void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
|
||||
void insert(size_t j, Vector vector);
|
||||
void insert(size_t j, Matrix matrix);
|
||||
|
||||
void update(size_t j, const gtsam::Point2& t);
|
||||
void update(size_t j, const gtsam::Point3& t);
|
||||
void update(size_t j, const gtsam::Rot2& t);
|
||||
void update(size_t j, const gtsam::Pose2& t);
|
||||
void update(size_t j, const gtsam::Rot3& t);
|
||||
void update(size_t j, const gtsam::Pose3& t);
|
||||
void update(size_t j, const gtsam::Cal3_S2& t);
|
||||
void update(size_t j, const gtsam::Cal3DS2& t);
|
||||
void update(size_t j, const gtsam::Cal3Bundler& t);
|
||||
void update(size_t j, const gtsam::EssentialMatrix& t);
|
||||
void update(size_t j, const gtsam::imuBias::ConstantBias& t);
|
||||
void update(size_t j, Vector t);
|
||||
void update(size_t j, Matrix t);
|
||||
void update(size_t j, const gtsam::Point2& point2);
|
||||
void update(size_t j, const gtsam::Point3& point3);
|
||||
void update(size_t j, const gtsam::Rot2& rot2);
|
||||
void update(size_t j, const gtsam::Pose2& pose2);
|
||||
void update(size_t j, const gtsam::Rot3& rot3);
|
||||
void update(size_t j, const gtsam::Pose3& pose3);
|
||||
void update(size_t j, const gtsam::Cal3_S2& cal3_s2);
|
||||
void update(size_t j, const gtsam::Cal3DS2& cal3ds2);
|
||||
void update(size_t j, const gtsam::Cal3Bundler& cal3bundler);
|
||||
void update(size_t j, const gtsam::EssentialMatrix& essential_matrix);
|
||||
void update(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
|
||||
void update(size_t j, Vector vector);
|
||||
void update(size_t j, Matrix matrix);
|
||||
|
||||
template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Pose2, gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::imuBias::ConstantBias, Vector, Matrix}>
|
||||
T at(size_t j);
|
||||
|
@ -2100,10 +2110,10 @@ class ISAM2Params {
|
|||
void print(string str) const;
|
||||
|
||||
/** Getters and Setters for all properties */
|
||||
void setOptimizationParams(const gtsam::ISAM2GaussNewtonParams& params);
|
||||
void setOptimizationParams(const gtsam::ISAM2DoglegParams& params);
|
||||
void setRelinearizeThreshold(double relinearizeThreshold);
|
||||
void setRelinearizeThreshold(const gtsam::ISAM2ThresholdMap& relinearizeThreshold);
|
||||
void setOptimizationParams(const gtsam::ISAM2GaussNewtonParams& gauss_newton__params);
|
||||
void setOptimizationParams(const gtsam::ISAM2DoglegParams& dogleg_params);
|
||||
void setRelinearizeThreshold(double threshold);
|
||||
void setRelinearizeThreshold(const gtsam::ISAM2ThresholdMap& threshold_map);
|
||||
int getRelinearizeSkip() const;
|
||||
void setRelinearizeSkip(int relinearizeSkip);
|
||||
bool isEnableRelinearization() const;
|
||||
|
|
|
@ -103,7 +103,7 @@ class PoseRTV {
|
|||
#include <gtsam_unstable/geometry/Pose3Upright.h>
|
||||
class Pose3Upright {
|
||||
Pose3Upright();
|
||||
Pose3Upright(const gtsam::Pose3Upright& x);
|
||||
Pose3Upright(const gtsam::Pose3Upright& other);
|
||||
Pose3Upright(const gtsam::Rot2& bearing, const gtsam::Point3& t);
|
||||
Pose3Upright(double x, double y, double z, double theta);
|
||||
Pose3Upright(const gtsam::Pose2& pose, double z);
|
||||
|
@ -141,7 +141,7 @@ class Pose3Upright {
|
|||
#include <gtsam_unstable/geometry/BearingS2.h>
|
||||
class BearingS2 {
|
||||
BearingS2();
|
||||
BearingS2(double azimuth, double elevation);
|
||||
BearingS2(double azimuth_double, double elevation_double);
|
||||
BearingS2(const gtsam::Rot2& azimuth, const gtsam::Rot2& elevation);
|
||||
|
||||
gtsam::Rot2 azimuth() const;
|
||||
|
|
|
@ -281,15 +281,16 @@ std::string ArgumentList::pyx_paramsList() const {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::string ArgumentList::pyx_castParamsToPythonType() const {
|
||||
std::string ArgumentList::pyx_castParamsToPythonType(
|
||||
const std::string& indent) const {
|
||||
if (size() == 0)
|
||||
return " pass\n";
|
||||
return "";
|
||||
|
||||
// cast params to their correct python argument type to pass in the function call later
|
||||
string s;
|
||||
for (size_t j = 0; j < size(); ++j)
|
||||
s += " " + at(j).name + " = <" + at(j).type.pyxArgumentType()
|
||||
+ ">(__params['" + at(j).name + "'])\n";
|
||||
s += indent + at(j).name + " = <" + at(j).type.pyxArgumentType()
|
||||
+ ">(__params[" + std::to_string(j) + "])\n";
|
||||
return s;
|
||||
}
|
||||
|
||||
|
|
|
@ -131,7 +131,7 @@ struct ArgumentList: public std::vector<Argument> {
|
|||
void emit_cython_pyx(FileWriter& file) const;
|
||||
std::string pyx_asParams() const;
|
||||
std::string pyx_paramsList() const;
|
||||
std::string pyx_castParamsToPythonType() const;
|
||||
std::string pyx_castParamsToPythonType(const std::string& indent) const;
|
||||
std::string pyx_convertEigenTypeAndStorageOrder(const std::string& indent) const;
|
||||
|
||||
/**
|
||||
|
|
|
@ -764,6 +764,22 @@ void Class::emit_cython_pxd(FileWriter& pxdFile) const {
|
|||
if (numMethods == 0)
|
||||
pxdFile.oss << " pass\n";
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
void Class::emit_cython_wrapper_pxd(FileWriter& pxdFile) const {
|
||||
pxdFile.oss << "\ncdef class " << pyxClassName();
|
||||
if (getParent())
|
||||
pxdFile.oss << "(" << getParent()->pyxClassName() << ")";
|
||||
pxdFile.oss << ":\n";
|
||||
pxdFile.oss << " cdef " << shared_pxd_class_in_pyx() << " "
|
||||
<< shared_pxd_obj_in_pyx() << "\n";
|
||||
// cyCreateFromShared
|
||||
pxdFile.oss << " @staticmethod\n";
|
||||
pxdFile.oss << " cdef " << pyxClassName() << " cyCreateFromShared(const "
|
||||
<< shared_pxd_class_in_pyx() << "& other)\n";
|
||||
for(const StaticMethod& m: static_methods | boost::adaptors::map_values)
|
||||
m.emit_cython_wrapper_pxd(pxdFile, *this);
|
||||
if (static_methods.size()>0) pxdFile.oss << "\n";
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Class::pyxInitParentObj(FileWriter& pyxFile, const std::string& pyObj,
|
||||
|
@ -825,29 +841,19 @@ void Class::emit_cython_pyx(FileWriter& pyxFile, const std::vector<Class>& allCl
|
|||
if (parentClass) pyxFile.oss << "(" << parentClass->pyxClassName() << ")";
|
||||
pyxFile.oss << ":\n";
|
||||
|
||||
// shared variable of the corresponding cython object
|
||||
// pyxFile.oss << " cdef " << shared_pxd_class_in_pyx() << " " << shared_pxd_obj_in_pyx() << "\n";
|
||||
|
||||
// __cinit___
|
||||
pyxFile.oss << " def __init__(self, *args, **kwargs):\n"
|
||||
" self." << shared_pxd_obj_in_pyx() << " = "
|
||||
<< shared_pxd_class_in_pyx() << "()\n";
|
||||
|
||||
// __init___
|
||||
pyxFile.oss << " def __init__(self, *args, **kwargs):\n";
|
||||
pyxFile.oss << " cdef list __params\n";
|
||||
pyxFile.oss << " self." << shared_pxd_obj_in_pyx() << " = " << shared_pxd_class_in_pyx() << "()\n";
|
||||
pyxFile.oss << " if len(args)==0 and len(kwargs)==1 and kwargs.has_key('cyCreateFromShared'):\n return\n";
|
||||
for (size_t i = 0; i<constructor.nrOverloads(); ++i) {
|
||||
pyxFile.oss << " " << "elif" << " self."
|
||||
<< pyxClassName() << "_" << i
|
||||
<< "(*args, **kwargs):\n pass\n";
|
||||
}
|
||||
pyxFile.oss << " else:\n raise TypeError('" << pyxClassName()
|
||||
<< " construction failed!')\n";
|
||||
|
||||
pyxInitParentObj(pyxFile, " self", "self." + shared_pxd_obj_in_pyx(), allClasses);
|
||||
pyxFile.oss << "\n";
|
||||
|
||||
// Constructors
|
||||
constructor.emit_cython_pyx(pyxFile, *this);
|
||||
if (constructor.nrOverloads()>0) pyxFile.oss << "\n";
|
||||
pyxFile.oss << " if (self." << shared_pxd_obj_in_pyx() << ".use_count()==0):\n";
|
||||
pyxFile.oss << " raise TypeError('" << pyxClassName()
|
||||
<< " construction failed!')\n";
|
||||
pyxInitParentObj(pyxFile, " self", "self." + shared_pxd_obj_in_pyx(), allClasses);
|
||||
pyxFile.oss << "\n";
|
||||
|
||||
// cyCreateFromShared
|
||||
pyxFile.oss << " @staticmethod\n";
|
||||
|
@ -855,10 +861,10 @@ void Class::emit_cython_pyx(FileWriter& pyxFile, const std::vector<Class>& allCl
|
|||
<< shared_pxd_class_in_pyx() << "& other):\n"
|
||||
<< " if other.get() == NULL:\n"
|
||||
<< " raise RuntimeError('Cannot create object from a nullptr!')\n"
|
||||
<< " cdef " << pyxClassName() << " ret = " << pyxClassName() << "(cyCreateFromShared=True)\n"
|
||||
<< " ret." << shared_pxd_obj_in_pyx() << " = other\n";
|
||||
pyxInitParentObj(pyxFile, " ret", "other", allClasses);
|
||||
pyxFile.oss << " return ret" << "\n";
|
||||
<< " cdef " << pyxClassName() << " return_value = " << pyxClassName() << "(cyCreateFromShared=True)\n"
|
||||
<< " return_value." << shared_pxd_obj_in_pyx() << " = other\n";
|
||||
pyxInitParentObj(pyxFile, " return_value", "other", allClasses);
|
||||
pyxFile.oss << " return return_value" << "\n\n";
|
||||
|
||||
for(const StaticMethod& m: static_methods | boost::adaptors::map_values)
|
||||
m.emit_cython_pyx(pyxFile, *this);
|
||||
|
|
|
@ -165,6 +165,7 @@ public:
|
|||
|
||||
// emit cython wrapper
|
||||
void emit_cython_pxd(FileWriter& pxdFile) const;
|
||||
void emit_cython_wrapper_pxd(FileWriter& pxdFile) const;
|
||||
void emit_cython_pyx(FileWriter& pyxFile,
|
||||
const std::vector<Class>& allClasses) const;
|
||||
void pyxInitParentObj(FileWriter& pyxFile, const std::string& pyObj,
|
||||
|
|
|
@ -136,8 +136,7 @@ void Constructor::emit_cython_pxd(FileWriter& pxdFile, const Class& cls) const {
|
|||
// generate the constructor
|
||||
pxdFile.oss << " " << cls.pxdClassName() << "(";
|
||||
args.emit_cython_pxd(pxdFile, cls.pxdClassName(), cls.templateArgs);
|
||||
pxdFile.oss << ") "
|
||||
<< "except +\n";
|
||||
pxdFile.oss << ") " << "except +\n";
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -145,15 +144,16 @@ void Constructor::emit_cython_pxd(FileWriter& pxdFile, const Class& cls) const {
|
|||
void Constructor::emit_cython_pyx(FileWriter& pyxFile, const Class& cls) const {
|
||||
for (size_t i = 0; i < nrOverloads(); i++) {
|
||||
ArgumentList args = argumentList(i);
|
||||
pyxFile.oss << " def " + cls.pyxClassName() + "_" + to_string(i) +
|
||||
"(self, *args, **kwargs):\n";
|
||||
pyxFile.oss << pyx_resolveOverloadParams(args, true);
|
||||
pyxFile.oss << argumentList(i).pyx_convertEigenTypeAndStorageOrder(" ");
|
||||
pyxFile.oss << " try:\n";
|
||||
pyxFile.oss << pyx_resolveOverloadParams(args, true, 3);
|
||||
pyxFile.oss
|
||||
<< argumentList(i).pyx_convertEigenTypeAndStorageOrder(" ");
|
||||
|
||||
pyxFile.oss << " self." << cls.shared_pxd_obj_in_pyx() << " = "
|
||||
<< cls.shared_pxd_class_in_pyx() << "(new " << cls.pxd_class_in_pyx()
|
||||
<< "(" << args.pyx_asParams() << "))\n";
|
||||
pyxFile.oss << " return True\n\n";
|
||||
pyxFile.oss << " self." << cls.shared_pxd_obj_in_pyx() << " = "
|
||||
<< cls.shared_pxd_class_in_pyx() << "(new " << cls.pxd_class_in_pyx()
|
||||
<< "(" << args.pyx_asParams() << "))\n";
|
||||
pyxFile.oss << " except:\n";
|
||||
pyxFile.oss << " pass\n";
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -155,9 +155,11 @@ void GlobalFunction::emit_cython_pyx_no_overload(FileWriter& file) const {
|
|||
|
||||
// Function definition
|
||||
file.oss << "def " << funcName;
|
||||
|
||||
// modify name of function instantiation as python doesn't allow overloads
|
||||
// e.g. template<T={A,B,C}> funcName(...) --> funcNameA, funcNameB, funcNameC
|
||||
if (templateArgValue_) file.oss << templateArgValue_->pyxClassName();
|
||||
|
||||
// funtion arguments
|
||||
file.oss << "(";
|
||||
argumentList(0).emit_cython_pyx(file);
|
||||
|
@ -189,28 +191,33 @@ void GlobalFunction::emit_cython_pyx(FileWriter& file) const {
|
|||
file.oss << "def " << funcName << "(*args, **kwargs):\n";
|
||||
for (size_t i = 0; i < N; ++i) {
|
||||
file.oss << " success, results = " << funcName << "_" << i
|
||||
<< "(*args, **kwargs)\n";
|
||||
<< "(args, kwargs)\n";
|
||||
file.oss << " if success:\n return results\n";
|
||||
}
|
||||
file.oss << " raise TypeError('Could not find the correct overload')\n";
|
||||
|
||||
for (size_t i = 0; i < N; ++i) {
|
||||
ArgumentList args = argumentList(i);
|
||||
file.oss << "def " + funcName + "_" + to_string(i) +
|
||||
"(*args, **kwargs):\n";
|
||||
file.oss << pyx_resolveOverloadParams(args, false, 1); // lazy: always return None even if it's a void function
|
||||
|
||||
/// Call cython corresponding function
|
||||
file.oss << argumentList(i).pyx_convertEigenTypeAndStorageOrder(" ");
|
||||
string ret = pyx_functionCall("", pxdName(), i);
|
||||
file.oss << "def " + funcName + "_" + to_string(i) + "(args, kwargs):\n";
|
||||
file.oss << " cdef list __params\n";
|
||||
if (!returnVals_[i].isVoid()) {
|
||||
file.oss << " cdef " << returnVals_[i].pyx_returnType()
|
||||
<< " ret = " << ret << "\n";
|
||||
file.oss << " return True, " << returnVals_[i].pyx_casting("ret") << "\n";
|
||||
} else {
|
||||
file.oss << " " << ret << "\n";
|
||||
file.oss << " return True, None\n";
|
||||
file.oss << " cdef " << returnVals_[i].pyx_returnType() << " return_value\n";
|
||||
}
|
||||
file.oss << " try:\n";
|
||||
file.oss << pyx_resolveOverloadParams(args, false, 2); // lazy: always return None even if it's a void function
|
||||
|
||||
/// Call corresponding cython function
|
||||
file.oss << argumentList(i).pyx_convertEigenTypeAndStorageOrder(" ");
|
||||
string call = pyx_functionCall("", pxdName(), i);
|
||||
if (!returnVals_[i].isVoid()) {
|
||||
file.oss << " return_value = " << call << "\n";
|
||||
file.oss << " return True, " << returnVals_[i].pyx_casting("return_value") << "\n";
|
||||
} else {
|
||||
file.oss << " " << call << "\n";
|
||||
file.oss << " return True, None\n";
|
||||
}
|
||||
file.oss << " except:\n";
|
||||
file.oss << " return False, None\n\n";
|
||||
}
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
|
@ -85,8 +85,12 @@ void Method::emit_cython_pxd(FileWriter& file, const Class& cls) const {
|
|||
for (size_t i = 0; i < nrOverloads(); ++i) {
|
||||
file.oss << " ";
|
||||
returnVals_[i].emit_cython_pxd(file, cls.pxdClassName(), cls.templateArgs);
|
||||
file.oss << pyRename(name_) + " \"" + name_ + "\""
|
||||
<< "(";
|
||||
const string renamed = pyRename(name_);
|
||||
if (renamed != name_) {
|
||||
file.oss << pyRename(name_) + " \"" + name_ + "\"" << "(";
|
||||
} else {
|
||||
file.oss << name_ << "(";
|
||||
}
|
||||
argumentList(i).emit_cython_pxd(file, cls.pxdClassName(), cls.templateArgs);
|
||||
file.oss << ")";
|
||||
// if (is_const_) file.oss << " const";
|
||||
|
@ -99,20 +103,23 @@ void Method::emit_cython_pxd(FileWriter& file, const Class& cls) const {
|
|||
void Method::emit_cython_pyx_no_overload(FileWriter& file,
|
||||
const Class& cls) const {
|
||||
string funcName = pyRename(name_);
|
||||
|
||||
// leverage python's special treatment for print
|
||||
if (funcName == "print_") {
|
||||
//file.oss << " def __str__(self):\n self.print_('')\n return ''\n";
|
||||
file.oss << " def __str__(self):\n";
|
||||
file.oss << " strBuf = RedirectCout()\n";
|
||||
file.oss << " self.print_('')\n";
|
||||
file.oss << " return strBuf.str()\n";
|
||||
}
|
||||
|
||||
// Function definition
|
||||
file.oss << " def " << funcName;
|
||||
|
||||
// modify name of function instantiation as python doesn't allow overloads
|
||||
// e.g. template<T={A,B,C}> funcName(...) --> funcNameA, funcNameB, funcNameC
|
||||
if (templateArgValue_) file.oss << templateArgValue_->pyxClassName();
|
||||
// funtion arguments
|
||||
|
||||
// function arguments
|
||||
file.oss << "(self";
|
||||
if (argumentList(0).size() > 0) file.oss << ", ";
|
||||
argumentList(0).emit_cython_pyx(file);
|
||||
|
@ -138,7 +145,8 @@ void Method::emit_cython_pyx(FileWriter& file, const Class& cls) const {
|
|||
// doesn't allow overloads
|
||||
// e.g. template<T={A,B,C}> funcName(...) --> funcNameA, funcNameB, funcNameC
|
||||
string instantiatedName =
|
||||
(templateArgValue_) ? funcName + templateArgValue_->pyxClassName() : funcName;
|
||||
(templateArgValue_) ? funcName + templateArgValue_->pyxClassName() :
|
||||
funcName;
|
||||
|
||||
size_t N = nrOverloads();
|
||||
// It's easy if there's no overload
|
||||
|
@ -149,32 +157,49 @@ void Method::emit_cython_pyx(FileWriter& file, const Class& cls) const {
|
|||
|
||||
// Dealing with overloads..
|
||||
file.oss << " def " << instantiatedName << "(self, *args, **kwargs):\n";
|
||||
for (size_t i = 0; i < N; ++i) {
|
||||
file.oss << " success, results = self." << instantiatedName << "_" << i
|
||||
<< "(*args, **kwargs)\n";
|
||||
file.oss << " if success:\n return results\n";
|
||||
}
|
||||
file.oss << " raise TypeError('Could not find the correct overload')\n";
|
||||
file.oss << " cdef list __params\n";
|
||||
|
||||
for (size_t i = 0; i < N; ++i) {
|
||||
ArgumentList args = argumentList(i);
|
||||
file.oss << " def " + instantiatedName + "_" + to_string(i) +
|
||||
"(self, *args, **kwargs):\n";
|
||||
file.oss << pyx_resolveOverloadParams(args, false); // lazy: always return None even if it's a void function
|
||||
|
||||
/// Call cython corresponding function
|
||||
file.oss << argumentList(i).pyx_convertEigenTypeAndStorageOrder(" ");
|
||||
string caller = "self." + cls.shared_pxd_obj_in_pyx() + ".get()";
|
||||
|
||||
string ret = pyx_functionCall(caller, funcName, i);
|
||||
if (!returnVals_[i].isVoid()) {
|
||||
file.oss << " cdef " << returnVals_[i].pyx_returnType()
|
||||
<< " ret = " << ret << "\n";
|
||||
file.oss << " return True, " << returnVals_[i].pyx_casting("ret") << "\n";
|
||||
// Define return values for all possible overloads
|
||||
vector<string> return_type; // every overload has a return type, possibly void
|
||||
map<string, string> return_value; // we only define one return value for every distinct type
|
||||
size_t j = 1;
|
||||
for (size_t i = 0; i < nrOverloads(); ++i) {
|
||||
if (returnVals_[i].isVoid()) {
|
||||
return_type.push_back("void");
|
||||
} else {
|
||||
file.oss << " " << ret << "\n";
|
||||
file.oss << " return True, None\n";
|
||||
const string type = returnVals_[i].pyx_returnType();
|
||||
return_type.push_back(type);
|
||||
if (return_value.count(type) == 0) {
|
||||
const string value = "return_value_" + to_string(j++);
|
||||
return_value[type] = value;
|
||||
file.oss << " cdef " << type << " " << value << "\n";
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (size_t i = 0; i < nrOverloads(); ++i) {
|
||||
ArgumentList args = argumentList(i);
|
||||
file.oss << " try:\n";
|
||||
file.oss << pyx_resolveOverloadParams(args, false, 3); // lazy: always return None even if it's a void function
|
||||
|
||||
/// Call corresponding cython function
|
||||
file.oss << args.pyx_convertEigenTypeAndStorageOrder(" ");
|
||||
string caller = "self." + cls.shared_pxd_obj_in_pyx() + ".get()";
|
||||
string call = pyx_functionCall(caller, funcName, i);
|
||||
if (!returnVals_[i].isVoid()) {
|
||||
const string type = return_type[i];
|
||||
const string value = return_value[type];
|
||||
file.oss << " " << value << " = " << call << "\n";
|
||||
file.oss << " return " << returnVals_[i].pyx_casting(value)
|
||||
<< "\n";
|
||||
} else {
|
||||
file.oss << " " << call << "\n";
|
||||
file.oss << " return\n";
|
||||
}
|
||||
file.oss << " except:\n";
|
||||
file.oss << " pass\n";
|
||||
}
|
||||
file.oss
|
||||
<< " raise TypeError('Incorrect arguments for method call.')\n\n";
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
|
@ -348,50 +348,41 @@ void Module::emit_cython_pxd(FileWriter& pxdFile) const {
|
|||
" shared_ptr()\n"
|
||||
" shared_ptr(T*)\n"
|
||||
" T* get()\n"
|
||||
" long use_count() const\n"
|
||||
" T& operator*()\n\n"
|
||||
" cdef shared_ptr[T] dynamic_pointer_cast[T,U](const shared_ptr[U]& r)\n"
|
||||
" cdef shared_ptr[T] make_shared[T](const T& r)\n";
|
||||
" cdef shared_ptr[T] make_shared[T](const T& r)\n\n";
|
||||
|
||||
for(const TypedefPair& types: typedefs)
|
||||
types.emit_cython_pxd(pxdFile);
|
||||
|
||||
//... wrap all classes
|
||||
for (const Class& cls : uninstantiatedClasses) {
|
||||
cls.emit_cython_pxd(pxdFile);
|
||||
cls.emit_cython_pxd(pxdFile);
|
||||
|
||||
for (const Class& expCls : expandedClasses) {
|
||||
//... ctypedef for template instantiations
|
||||
if (expCls.templateClass &&
|
||||
expCls.templateClass->pxdClassName() == cls.pxdClassName()) {
|
||||
pxdFile.oss << "ctypedef " << expCls.templateClass->pxdClassName()
|
||||
<< "[";
|
||||
for (size_t i = 0; i < expCls.templateInstTypeList.size(); ++i)
|
||||
pxdFile.oss << expCls.templateInstTypeList[i].pxdClassName()
|
||||
<< ((i == expCls.templateInstTypeList.size() - 1)
|
||||
? ""
|
||||
: ", ");
|
||||
pxdFile.oss << "] " << expCls.pxdClassName() << "\n";
|
||||
}
|
||||
for (const Class& expCls : expandedClasses) {
|
||||
bool matchingNonTemplated = !expCls.templateClass
|
||||
&& expCls.pxdClassName() == cls.pxdClassName();
|
||||
bool isTemplatedFromCls = expCls.templateClass
|
||||
&& expCls.templateClass->pxdClassName() == cls.pxdClassName();
|
||||
|
||||
if ((!expCls.templateClass &&
|
||||
expCls.pxdClassName() == cls.pxdClassName()) ||
|
||||
(expCls.templateClass &&
|
||||
expCls.templateClass->pxdClassName() == cls.pxdClassName())) {
|
||||
pxdFile.oss << "\n";
|
||||
pxdFile.oss << "cdef class " << expCls.pyxClassName();
|
||||
if (expCls.getParent())
|
||||
pxdFile.oss << "(" << expCls.getParent()->pyxClassName() << ")";
|
||||
pxdFile.oss << ":\n";
|
||||
pxdFile.oss << " cdef " << expCls.shared_pxd_class_in_pyx()
|
||||
<< " " << expCls.shared_pxd_obj_in_pyx() << "\n";
|
||||
// cyCreateFromShared
|
||||
pxdFile.oss << " @staticmethod\n";
|
||||
pxdFile.oss << " cdef " << expCls.pyxClassName()
|
||||
<< " cyCreateFromShared(const "
|
||||
<< expCls.shared_pxd_class_in_pyx() << "& other)\n";
|
||||
}
|
||||
// ctypedef for template instantiations
|
||||
if (isTemplatedFromCls) {
|
||||
pxdFile.oss << "\n";
|
||||
pxdFile.oss << "ctypedef " << expCls.templateClass->pxdClassName()
|
||||
<< "[";
|
||||
for (size_t i = 0; i < expCls.templateInstTypeList.size(); ++i)
|
||||
pxdFile.oss << expCls.templateInstTypeList[i].pxdClassName()
|
||||
<< ((i == expCls.templateInstTypeList.size() - 1) ? "" : ", ");
|
||||
pxdFile.oss << "] " << expCls.pxdClassName() << "\n";
|
||||
}
|
||||
pxdFile.oss << "\n\n";
|
||||
|
||||
// Python wrapper class
|
||||
if (isTemplatedFromCls || matchingNonTemplated) {
|
||||
expCls.emit_cython_wrapper_pxd(pxdFile);
|
||||
}
|
||||
}
|
||||
pxdFile.oss << "\n\n";
|
||||
}
|
||||
|
||||
//... wrap global functions
|
||||
|
@ -412,6 +403,17 @@ void Module::emit_cython_pyx(FileWriter& pyxFile) const {
|
|||
"from "<< pxdHeader << " cimport dynamic_pointer_cast\n"
|
||||
"from "<< pxdHeader << " cimport make_shared\n";
|
||||
|
||||
pyxFile.oss << "# C helper function that copies all arguments into a positional list.\n"
|
||||
"cdef list process_args(list keywords, tuple args, dict kwargs):\n"
|
||||
" cdef str keyword\n"
|
||||
" cdef int n = len(args), m = len(keywords)\n"
|
||||
" cdef list params = list(args)\n"
|
||||
" assert len(args)+len(kwargs) == m, 'Expected {} arguments'.format(m)\n"
|
||||
" try:\n"
|
||||
" return params + [kwargs[keyword] for keyword in keywords[n:]]\n"
|
||||
" except:\n"
|
||||
" raise ValueError('Epected arguments ' + str(keywords))\n";
|
||||
|
||||
// import all typedefs, e.g. from gtsam_wrapper cimport Key, so we don't need to say gtsam.Key
|
||||
for(const Qualified& q: Qualified::BasicTypedefs) {
|
||||
pyxFile.oss << "from " << pxdHeader << " cimport " << q.pxdClassName() << "\n";
|
||||
|
|
|
@ -71,35 +71,29 @@ public:
|
|||
return os;
|
||||
}
|
||||
|
||||
std::string pyx_resolveOverloadParams(const ArgumentList& args, bool isVoid, size_t indentLevel = 2) const {
|
||||
std::string pyx_resolveOverloadParams(const ArgumentList& args, bool isVoid,
|
||||
size_t indentLevel = 2) const {
|
||||
std::string indent;
|
||||
for (size_t i = 0; i<indentLevel; ++i) indent += " ";
|
||||
for (size_t i = 0; i < indentLevel; ++i)
|
||||
indent += " ";
|
||||
std::string s;
|
||||
s += indent + "if len(args)+len(kwargs) !=" + std::to_string(args.size()) + ":\n";
|
||||
s += indent + " return False";
|
||||
s += (!isVoid) ? ", None\n" : "\n";
|
||||
s += indent + "__params = process_args([" + args.pyx_paramsList()
|
||||
+ "], args, kwargs)\n";
|
||||
s += args.pyx_castParamsToPythonType(indent);
|
||||
if (args.size() > 0) {
|
||||
s += indent + "__params = kwargs.copy()\n";
|
||||
s += indent + "__names = [" + args.pyx_paramsList() + "]\n";
|
||||
s += indent + "for i in range(len(args)):\n";
|
||||
s += indent + " __params[__names[i]] = args[i]\n";
|
||||
for (size_t i = 0; i<args.size(); ++i) {
|
||||
for (size_t i = 0; i < args.size(); ++i) {
|
||||
// For python types we can do the assert after the assignment and save list accesses
|
||||
if (args[i].type.isNonBasicType() || args[i].type.isEigen()) {
|
||||
std::string param = "__params[__names[" + std::to_string(i) + "]]";
|
||||
s += indent + "if not isinstance(" + param + ", " +
|
||||
args[i].type.pyxArgumentType() + ")";
|
||||
if (args[i].type.isEigen())
|
||||
s += " or not " + param + ".ndim == " +
|
||||
((args[i].type.pyxClassName() == "Vector") ? "1" : "2");
|
||||
s += ":\n";
|
||||
s += indent + " return False" + ((isVoid) ? "" : ", None") + "\n";
|
||||
std::string param = args[i].name;
|
||||
s += indent + "assert isinstance(" + param + ", "
|
||||
+ args[i].type.pyxArgumentType() + ")";
|
||||
if (args[i].type.isEigen()) {
|
||||
s += " and " + param + ".ndim == "
|
||||
+ ((args[i].type.pyxClassName() == "Vector") ? "1" : "2");
|
||||
}
|
||||
s += "\n";
|
||||
}
|
||||
}
|
||||
s += indent + "try:\n";
|
||||
s += args.pyx_castParamsToPythonType();
|
||||
s += indent + "except:\n";
|
||||
s += indent + " return False";
|
||||
s += (!isVoid) ? ", None\n" : "\n";
|
||||
}
|
||||
return s;
|
||||
}
|
||||
|
|
|
@ -237,7 +237,7 @@ public:
|
|||
|
||||
/// the internal Cython shared obj in a Python class wrappper
|
||||
std::string shared_pxd_obj_in_pyx() const {
|
||||
return "shared_" + pxdClassName() + "_";
|
||||
return pxdClassName() + "_";
|
||||
}
|
||||
|
||||
std::string make_shared_pxd_class_in_pyx() const {
|
||||
|
|
|
@ -58,7 +58,6 @@ string StaticMethod::wrapper_call(FileWriter& wrapperFile, Str cppClassName,
|
|||
|
||||
/* ************************************************************************* */
|
||||
void StaticMethod::emit_cython_pxd(FileWriter& file, const Class& cls) const {
|
||||
// don't support overloads for static method :-(
|
||||
for(size_t i = 0; i < nrOverloads(); ++i) {
|
||||
file.oss << " @staticmethod\n";
|
||||
file.oss << " ";
|
||||
|
@ -69,6 +68,18 @@ void StaticMethod::emit_cython_pxd(FileWriter& file, const Class& cls) const {
|
|||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void StaticMethod::emit_cython_wrapper_pxd(FileWriter& file,
|
||||
const Class& cls) const {
|
||||
if (nrOverloads() > 1) {
|
||||
for (size_t i = 0; i < nrOverloads(); ++i) {
|
||||
string funcName = name_ + "_" + to_string(i);
|
||||
file.oss << " @staticmethod\n";
|
||||
file.oss << " cdef tuple " + funcName + "(tuple args, dict kwargs)\n";
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void StaticMethod::emit_cython_pyx_no_overload(FileWriter& file,
|
||||
const Class& cls) const {
|
||||
|
@ -80,12 +91,12 @@ void StaticMethod::emit_cython_pyx_no_overload(FileWriter& file,
|
|||
|
||||
/// Call cython corresponding function and return
|
||||
file.oss << argumentList(0).pyx_convertEigenTypeAndStorageOrder(" ");
|
||||
string ret = pyx_functionCall(cls.pxd_class_in_pyx(), name_, 0);
|
||||
string call = pyx_functionCall(cls.pxd_class_in_pyx(), name_, 0);
|
||||
file.oss << " ";
|
||||
if (!returnVals_[0].isVoid()) {
|
||||
file.oss << "return " << returnVals_[0].pyx_casting(ret) << "\n";
|
||||
file.oss << "return " << returnVals_[0].pyx_casting(call) << "\n";
|
||||
} else
|
||||
file.oss << ret << "\n";
|
||||
file.oss << call << "\n";
|
||||
file.oss << "\n";
|
||||
}
|
||||
|
||||
|
@ -98,38 +109,42 @@ void StaticMethod::emit_cython_pyx(FileWriter& file, const Class& cls) const {
|
|||
}
|
||||
|
||||
// Dealing with overloads..
|
||||
file.oss << " @staticmethod\n";
|
||||
file.oss << " @staticmethod # overloaded\n";
|
||||
file.oss << " def " << name_ << "(*args, **kwargs):\n";
|
||||
for (size_t i = 0; i < N; ++i) {
|
||||
string funcName = name_ + "_" + to_string(i);
|
||||
file.oss << " success, results = " << cls.pyxClassName() << "."
|
||||
<< funcName << "(*args, **kwargs)\n";
|
||||
<< funcName << "(args, kwargs)\n";
|
||||
file.oss << " if success:\n return results\n";
|
||||
}
|
||||
file.oss << " raise TypeError('Could not find the correct overload')\n";
|
||||
file.oss << " raise TypeError('Could not find the correct overload')\n\n";
|
||||
|
||||
// Create cdef methods for all overloaded methods
|
||||
for(size_t i = 0; i < N; ++i) {
|
||||
file.oss << " @staticmethod\n";
|
||||
|
||||
string funcName = name_ + "_" + to_string(i);
|
||||
string pxdFuncName = name_ + ((i>0)?"_" + to_string(i):"");
|
||||
file.oss << " @staticmethod\n";
|
||||
file.oss << " cdef tuple " + funcName + "(tuple args, dict kwargs):\n";
|
||||
file.oss << " cdef list __params\n";
|
||||
if (!returnVals_[i].isVoid()) {
|
||||
file.oss << " cdef " << returnVals_[i].pyx_returnType() << " return_value\n";
|
||||
}
|
||||
file.oss << " try:\n";
|
||||
ArgumentList args = argumentList(i);
|
||||
file.oss << " def " + funcName + "(*args, **kwargs):\n";
|
||||
file.oss << pyx_resolveOverloadParams(args, false); // lazy: always return None even if it's a void function
|
||||
file.oss << pyx_resolveOverloadParams(args, false, 3);
|
||||
|
||||
/// Call cython corresponding function and return
|
||||
file.oss << argumentList(i).pyx_convertEigenTypeAndStorageOrder(" ");
|
||||
string ret = pyx_functionCall(cls.pxd_class_in_pyx(), pxdFuncName, i);
|
||||
file.oss << args.pyx_convertEigenTypeAndStorageOrder(" ");
|
||||
string pxdFuncName = name_ + ((i>0)?"_" + to_string(i):"");
|
||||
string call = pyx_functionCall(cls.pxd_class_in_pyx(), pxdFuncName, i);
|
||||
if (!returnVals_[i].isVoid()) {
|
||||
file.oss << " cdef " << returnVals_[i].pyx_returnType()
|
||||
<< " ret = " << ret << "\n";
|
||||
file.oss << " return True, " << returnVals_[i].pyx_casting("ret") << "\n";
|
||||
file.oss << " return_value = " << call << "\n";
|
||||
file.oss << " return True, " << returnVals_[i].pyx_casting("return_value") << "\n";
|
||||
} else {
|
||||
file.oss << " " << call << "\n";
|
||||
file.oss << " return True, None\n";
|
||||
}
|
||||
else {
|
||||
file.oss << " " << ret << "\n";
|
||||
file.oss << " return True, None\n";
|
||||
}
|
||||
file.oss << "\n";
|
||||
file.oss << " except:\n";
|
||||
file.oss << " return False, None\n\n";
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -35,6 +35,7 @@ struct StaticMethod: public MethodBase {
|
|||
}
|
||||
|
||||
void emit_cython_pxd(FileWriter& file, const Class& cls) const;
|
||||
void emit_cython_wrapper_pxd(FileWriter& file, const Class& cls) const;
|
||||
void emit_cython_pyx(FileWriter& file, const Class& cls) const;
|
||||
void emit_cython_pyx_no_overload(FileWriter& file, const Class& cls) const;
|
||||
|
||||
|
|
|
@ -12,26 +12,28 @@ cdef extern from "boost/shared_ptr.hpp" namespace "boost":
|
|||
shared_ptr()
|
||||
shared_ptr(T*)
|
||||
T* get()
|
||||
long use_count() const
|
||||
T& operator*()
|
||||
|
||||
cdef shared_ptr[T] dynamic_pointer_cast[T,U](const shared_ptr[U]& r)
|
||||
cdef shared_ptr[T] make_shared[T](const T& r)
|
||||
|
||||
cdef extern from "gtsam/geometry/Point2.h" namespace "gtsam":
|
||||
cdef cppclass CPoint2 "gtsam::Point2":
|
||||
CPoint2() except +
|
||||
CPoint2(double x, double y) except +
|
||||
|
||||
void argChar "argChar"(char a) except +
|
||||
void argUChar "argUChar"(unsigned char a) except +
|
||||
int dim "dim"() except +
|
||||
void eigenArguments "eigenArguments"(const VectorXd& v, const MatrixXd& m) except +
|
||||
char returnChar "returnChar"() except +
|
||||
CVectorNotEigen vectorConfusion "vectorConfusion"() except +
|
||||
double x "x"() except +
|
||||
double y "y"() except +
|
||||
void argChar(char a) except +
|
||||
void argUChar(unsigned char a) except +
|
||||
int dim() except +
|
||||
void eigenArguments(const VectorXd& v, const MatrixXd& m) except +
|
||||
char returnChar() except +
|
||||
CVectorNotEigen vectorConfusion() except +
|
||||
double x() except +
|
||||
double y() except +
|
||||
|
||||
cdef class Point2:
|
||||
cdef shared_ptr[CPoint2] shared_CPoint2_
|
||||
cdef shared_ptr[CPoint2] CPoint2_
|
||||
@staticmethod
|
||||
cdef Point2 cyCreateFromShared(const shared_ptr[CPoint2]& other)
|
||||
|
||||
|
@ -45,41 +47,42 @@ cdef extern from "gtsam/geometry/Point3.h" namespace "gtsam":
|
|||
@staticmethod
|
||||
double staticFunction "staticFunction"() except +
|
||||
|
||||
double norm "norm"() except +
|
||||
double norm() except +
|
||||
|
||||
cdef class Point3:
|
||||
cdef shared_ptr[CPoint3] shared_CPoint3_
|
||||
cdef shared_ptr[CPoint3] CPoint3_
|
||||
@staticmethod
|
||||
cdef Point3 cyCreateFromShared(const shared_ptr[CPoint3]& other)
|
||||
|
||||
|
||||
|
||||
cdef extern from "folder/path/to/Test.h":
|
||||
cdef cppclass CTest "Test":
|
||||
CTest() except +
|
||||
CTest(double a, const MatrixXd& b) except +
|
||||
|
||||
void arg_EigenConstRef "arg_EigenConstRef"(const MatrixXd& value) except +
|
||||
pair[CTest,shared_ptr[CTest]] create_MixedPtrs "create_MixedPtrs"() except +
|
||||
pair[shared_ptr[CTest],shared_ptr[CTest]] create_ptrs "create_ptrs"() except +
|
||||
void arg_EigenConstRef(const MatrixXd& value) except +
|
||||
pair[CTest,shared_ptr[CTest]] create_MixedPtrs() except +
|
||||
pair[shared_ptr[CTest],shared_ptr[CTest]] create_ptrs() except +
|
||||
void print_ "print"() except +
|
||||
shared_ptr[CPoint2] return_Point2Ptr "return_Point2Ptr"(bool value) except +
|
||||
CTest return_Test "return_Test"(shared_ptr[CTest]& value) except +
|
||||
shared_ptr[CTest] return_TestPtr "return_TestPtr"(shared_ptr[CTest]& value) except +
|
||||
bool return_bool "return_bool"(bool value) except +
|
||||
double return_double "return_double"(double value) except +
|
||||
bool return_field "return_field"(const CTest& t) except +
|
||||
int return_int "return_int"(int value) except +
|
||||
MatrixXd return_matrix1 "return_matrix1"(const MatrixXd& value) except +
|
||||
MatrixXd return_matrix2 "return_matrix2"(const MatrixXd& value) except +
|
||||
pair[VectorXd,MatrixXd] return_pair "return_pair"(const VectorXd& v, const MatrixXd& A) except +
|
||||
pair[shared_ptr[CTest],shared_ptr[CTest]] return_ptrs "return_ptrs"(shared_ptr[CTest]& p1, shared_ptr[CTest]& p2) except +
|
||||
size_t return_size_t "return_size_t"(size_t value) except +
|
||||
string return_string "return_string"(string value) except +
|
||||
VectorXd return_vector1 "return_vector1"(const VectorXd& value) except +
|
||||
VectorXd return_vector2 "return_vector2"(const VectorXd& value) except +
|
||||
shared_ptr[CPoint2] return_Point2Ptr(bool value) except +
|
||||
CTest return_Test(shared_ptr[CTest]& value) except +
|
||||
shared_ptr[CTest] return_TestPtr(shared_ptr[CTest]& value) except +
|
||||
bool return_bool(bool value) except +
|
||||
double return_double(double value) except +
|
||||
bool return_field(const CTest& t) except +
|
||||
int return_int(int value) except +
|
||||
MatrixXd return_matrix1(const MatrixXd& value) except +
|
||||
MatrixXd return_matrix2(const MatrixXd& value) except +
|
||||
pair[VectorXd,MatrixXd] return_pair(const VectorXd& v, const MatrixXd& A) except +
|
||||
pair[shared_ptr[CTest],shared_ptr[CTest]] return_ptrs(shared_ptr[CTest]& p1, shared_ptr[CTest]& p2) except +
|
||||
size_t return_size_t(size_t value) except +
|
||||
string return_string(string value) except +
|
||||
VectorXd return_vector1(const VectorXd& value) except +
|
||||
VectorXd return_vector2(const VectorXd& value) except +
|
||||
|
||||
cdef class Test:
|
||||
cdef shared_ptr[CTest] shared_CTest_
|
||||
cdef shared_ptr[CTest] CTest_
|
||||
@staticmethod
|
||||
cdef Test cyCreateFromShared(const shared_ptr[CTest]& other)
|
||||
|
||||
|
@ -89,7 +92,7 @@ cdef extern from "folder/path/to/Test.h":
|
|||
pass
|
||||
|
||||
cdef class MyBase:
|
||||
cdef shared_ptr[CMyBase] shared_CMyBase_
|
||||
cdef shared_ptr[CMyBase] CMyBase_
|
||||
@staticmethod
|
||||
cdef MyBase cyCreateFromShared(const shared_ptr[CMyBase]& other)
|
||||
|
||||
|
@ -98,24 +101,26 @@ cdef extern from "folder/path/to/Test.h":
|
|||
cdef cppclass CMyTemplate "MyTemplate"[T](CMyBase):
|
||||
CMyTemplate() except +
|
||||
|
||||
void accept_T "accept_T"(const T& value) except +
|
||||
void accept_Tptr "accept_Tptr"(shared_ptr[T]& value) except +
|
||||
pair[T,shared_ptr[T]] create_MixedPtrs "create_MixedPtrs"() except +
|
||||
pair[shared_ptr[T],shared_ptr[T]] create_ptrs "create_ptrs"() except +
|
||||
T return_T "return_T"(shared_ptr[T]& value) except +
|
||||
shared_ptr[T] return_Tptr "return_Tptr"(shared_ptr[T]& value) except +
|
||||
pair[shared_ptr[T],shared_ptr[T]] return_ptrs "return_ptrs"(shared_ptr[T]& p1, shared_ptr[T]& p2) except +
|
||||
void accept_T(const T& value) except +
|
||||
void accept_Tptr(shared_ptr[T]& value) except +
|
||||
pair[T,shared_ptr[T]] create_MixedPtrs() except +
|
||||
pair[shared_ptr[T],shared_ptr[T]] create_ptrs() except +
|
||||
T return_T(shared_ptr[T]& value) except +
|
||||
shared_ptr[T] return_Tptr(shared_ptr[T]& value) except +
|
||||
pair[shared_ptr[T],shared_ptr[T]] return_ptrs(shared_ptr[T]& p1, shared_ptr[T]& p2) except +
|
||||
ARG templatedMethod[ARG](const ARG& t) except +
|
||||
|
||||
ctypedef CMyTemplate[CPoint2] CMyTemplatePoint2
|
||||
|
||||
cdef class MyTemplatePoint2(MyBase):
|
||||
cdef shared_ptr[CMyTemplatePoint2] shared_CMyTemplatePoint2_
|
||||
cdef shared_ptr[CMyTemplatePoint2] CMyTemplatePoint2_
|
||||
@staticmethod
|
||||
cdef MyTemplatePoint2 cyCreateFromShared(const shared_ptr[CMyTemplatePoint2]& other)
|
||||
|
||||
ctypedef CMyTemplate[MatrixXd] CMyTemplateMatrix
|
||||
|
||||
cdef class MyTemplateMatrix(MyBase):
|
||||
cdef shared_ptr[CMyTemplateMatrix] shared_CMyTemplateMatrix_
|
||||
cdef shared_ptr[CMyTemplateMatrix] CMyTemplateMatrix_
|
||||
@staticmethod
|
||||
cdef MyTemplateMatrix cyCreateFromShared(const shared_ptr[CMyTemplateMatrix]& other)
|
||||
|
||||
|
@ -124,10 +129,11 @@ cdef extern from "folder/path/to/Test.h":
|
|||
cdef cppclass CMyFactor "MyFactor"[POSE,POINT]:
|
||||
CMyFactor(size_t key1, size_t key2, double measured, const shared_ptr[CnoiseModel_Base]& noiseModel) except +
|
||||
|
||||
|
||||
ctypedef CMyFactor[CPose2, MatrixXd] CMyFactorPosePoint2
|
||||
|
||||
cdef class MyFactorPosePoint2:
|
||||
cdef shared_ptr[CMyFactorPosePoint2] shared_CMyFactorPosePoint2_
|
||||
cdef shared_ptr[CMyFactorPosePoint2] CMyFactorPosePoint2_
|
||||
@staticmethod
|
||||
cdef MyFactorPosePoint2 cyCreateFromShared(const shared_ptr[CMyFactorPosePoint2]& other)
|
||||
|
||||
|
|
|
@ -4,6 +4,16 @@ cimport geometry
|
|||
from geometry cimport shared_ptr
|
||||
from geometry cimport dynamic_pointer_cast
|
||||
from geometry cimport make_shared
|
||||
# C helper function that copies all arguments into a positional list.
|
||||
cdef list process_args(list keywords, tuple args, dict kwargs):
|
||||
cdef str keyword
|
||||
cdef int n = len(args), m = len(keywords)
|
||||
cdef list params = list(args)
|
||||
assert len(args)+len(kwargs) == m, 'Expected {} arguments'.format(m)
|
||||
try:
|
||||
return params + [kwargs[keyword] for keyword in keywords[n:]]
|
||||
except:
|
||||
raise ValueError('Epected arguments ' + str(keywords))
|
||||
from gtsam_eigency.core cimport *
|
||||
from libcpp cimport bool
|
||||
|
||||
|
@ -14,104 +24,83 @@ from cython.operator cimport dereference as deref
|
|||
|
||||
cdef class Point2:
|
||||
def __init__(self, *args, **kwargs):
|
||||
self.shared_CPoint2_ = shared_ptr[CPoint2]()
|
||||
cdef list __params
|
||||
self.CPoint2_ = shared_ptr[CPoint2]()
|
||||
if len(args)==0 and len(kwargs)==1 and kwargs.has_key('cyCreateFromShared'):
|
||||
return
|
||||
elif self.Point2_0(*args, **kwargs):
|
||||
pass
|
||||
elif self.Point2_1(*args, **kwargs):
|
||||
pass
|
||||
else:
|
||||
raise TypeError('Point2 construction failed!')
|
||||
|
||||
def Point2_0(self, *args, **kwargs):
|
||||
if len(args)+len(kwargs) !=0:
|
||||
return False
|
||||
self.shared_CPoint2_ = shared_ptr[CPoint2](new CPoint2())
|
||||
return True
|
||||
|
||||
def Point2_1(self, *args, **kwargs):
|
||||
if len(args)+len(kwargs) !=2:
|
||||
return False
|
||||
__params = kwargs.copy()
|
||||
__names = ['x', 'y']
|
||||
for i in range(len(args)):
|
||||
__params[__names[i]] = args[i]
|
||||
try:
|
||||
x = <double>(__params['x'])
|
||||
y = <double>(__params['y'])
|
||||
__params = process_args([], args, kwargs)
|
||||
self.CPoint2_ = shared_ptr[CPoint2](new CPoint2())
|
||||
except:
|
||||
return False
|
||||
self.shared_CPoint2_ = shared_ptr[CPoint2](new CPoint2(x, y))
|
||||
return True
|
||||
|
||||
pass
|
||||
try:
|
||||
__params = process_args(['x', 'y'], args, kwargs)
|
||||
x = <double>(__params[0])
|
||||
y = <double>(__params[1])
|
||||
self.CPoint2_ = shared_ptr[CPoint2](new CPoint2(x, y))
|
||||
except:
|
||||
pass
|
||||
if (self.CPoint2_.use_count()==0):
|
||||
raise TypeError('Point2 construction failed!')
|
||||
|
||||
@staticmethod
|
||||
cdef Point2 cyCreateFromShared(const shared_ptr[CPoint2]& other):
|
||||
if other.get() == NULL:
|
||||
raise RuntimeError('Cannot create object from a nullptr!')
|
||||
cdef Point2 ret = Point2(cyCreateFromShared=True)
|
||||
ret.shared_CPoint2_ = other
|
||||
return ret
|
||||
cdef Point2 return_value = Point2(cyCreateFromShared=True)
|
||||
return_value.CPoint2_ = other
|
||||
return return_value
|
||||
|
||||
def argChar(self, char a):
|
||||
self.shared_CPoint2_.get().argChar(a)
|
||||
self.CPoint2_.get().argChar(a)
|
||||
def argUChar(self, unsigned char a):
|
||||
self.shared_CPoint2_.get().argUChar(a)
|
||||
self.CPoint2_.get().argUChar(a)
|
||||
def dim(self):
|
||||
cdef int ret = self.shared_CPoint2_.get().dim()
|
||||
cdef int ret = self.CPoint2_.get().dim()
|
||||
return ret
|
||||
def eigenArguments(self, np.ndarray v, np.ndarray m):
|
||||
v = v.astype(float, order='F', copy=False)
|
||||
m = m.astype(float, order='F', copy=False)
|
||||
self.shared_CPoint2_.get().eigenArguments(<VectorXd>(Map[VectorXd](v)), <MatrixXd>(Map[MatrixXd](m)))
|
||||
self.CPoint2_.get().eigenArguments(<VectorXd>(Map[VectorXd](v)), <MatrixXd>(Map[MatrixXd](m)))
|
||||
def returnChar(self):
|
||||
cdef char ret = self.shared_CPoint2_.get().returnChar()
|
||||
cdef char ret = self.CPoint2_.get().returnChar()
|
||||
return ret
|
||||
def vectorConfusion(self):
|
||||
cdef shared_ptr[CVectorNotEigen] ret = make_shared[CVectorNotEigen](self.shared_CPoint2_.get().vectorConfusion())
|
||||
cdef shared_ptr[CVectorNotEigen] ret = make_shared[CVectorNotEigen](self.CPoint2_.get().vectorConfusion())
|
||||
return VectorNotEigen.cyCreateFromShared(ret)
|
||||
def x(self):
|
||||
cdef double ret = self.shared_CPoint2_.get().x()
|
||||
cdef double ret = self.CPoint2_.get().x()
|
||||
return ret
|
||||
def y(self):
|
||||
cdef double ret = self.shared_CPoint2_.get().y()
|
||||
cdef double ret = self.CPoint2_.get().y()
|
||||
return ret
|
||||
|
||||
|
||||
cdef class Point3:
|
||||
def __init__(self, *args, **kwargs):
|
||||
self.shared_CPoint3_ = shared_ptr[CPoint3]()
|
||||
cdef list __params
|
||||
self.CPoint3_ = shared_ptr[CPoint3]()
|
||||
if len(args)==0 and len(kwargs)==1 and kwargs.has_key('cyCreateFromShared'):
|
||||
return
|
||||
elif self.Point3_0(*args, **kwargs):
|
||||
pass
|
||||
else:
|
||||
raise TypeError('Point3 construction failed!')
|
||||
|
||||
def Point3_0(self, *args, **kwargs):
|
||||
if len(args)+len(kwargs) !=3:
|
||||
return False
|
||||
__params = kwargs.copy()
|
||||
__names = ['x', 'y', 'z']
|
||||
for i in range(len(args)):
|
||||
__params[__names[i]] = args[i]
|
||||
try:
|
||||
x = <double>(__params['x'])
|
||||
y = <double>(__params['y'])
|
||||
z = <double>(__params['z'])
|
||||
__params = process_args(['x', 'y', 'z'], args, kwargs)
|
||||
x = <double>(__params[0])
|
||||
y = <double>(__params[1])
|
||||
z = <double>(__params[2])
|
||||
self.CPoint3_ = shared_ptr[CPoint3](new CPoint3(x, y, z))
|
||||
except:
|
||||
return False
|
||||
self.shared_CPoint3_ = shared_ptr[CPoint3](new CPoint3(x, y, z))
|
||||
return True
|
||||
|
||||
pass
|
||||
if (self.CPoint3_.use_count()==0):
|
||||
raise TypeError('Point3 construction failed!')
|
||||
|
||||
@staticmethod
|
||||
cdef Point3 cyCreateFromShared(const shared_ptr[CPoint3]& other):
|
||||
if other.get() == NULL:
|
||||
raise RuntimeError('Cannot create object from a nullptr!')
|
||||
cdef Point3 ret = Point3(cyCreateFromShared=True)
|
||||
ret.shared_CPoint3_ = other
|
||||
return ret
|
||||
cdef Point3 return_value = Point3(cyCreateFromShared=True)
|
||||
return_value.CPoint3_ = other
|
||||
return return_value
|
||||
|
||||
@staticmethod
|
||||
def StaticFunctionRet(double z):
|
||||
return Point3.cyCreateFromShared(make_shared[CPoint3](CPoint3.StaticFunctionRet(z)))
|
||||
|
@ -122,366 +111,337 @@ cdef class Point3:
|
|||
|
||||
|
||||
def norm(self):
|
||||
cdef double ret = self.shared_CPoint3_.get().norm()
|
||||
cdef double ret = self.CPoint3_.get().norm()
|
||||
return ret
|
||||
|
||||
|
||||
cdef class Test:
|
||||
def __init__(self, *args, **kwargs):
|
||||
self.shared_CTest_ = shared_ptr[CTest]()
|
||||
cdef list __params
|
||||
self.CTest_ = shared_ptr[CTest]()
|
||||
if len(args)==0 and len(kwargs)==1 and kwargs.has_key('cyCreateFromShared'):
|
||||
return
|
||||
elif self.Test_0(*args, **kwargs):
|
||||
pass
|
||||
elif self.Test_1(*args, **kwargs):
|
||||
pass
|
||||
else:
|
||||
raise TypeError('Test construction failed!')
|
||||
|
||||
def Test_0(self, *args, **kwargs):
|
||||
if len(args)+len(kwargs) !=0:
|
||||
return False
|
||||
self.shared_CTest_ = shared_ptr[CTest](new CTest())
|
||||
return True
|
||||
|
||||
def Test_1(self, *args, **kwargs):
|
||||
if len(args)+len(kwargs) !=2:
|
||||
return False
|
||||
__params = kwargs.copy()
|
||||
__names = ['a', 'b']
|
||||
for i in range(len(args)):
|
||||
__params[__names[i]] = args[i]
|
||||
if not isinstance(__params[__names[1]], np.ndarray) or not __params[__names[1]].ndim == 2:
|
||||
return False
|
||||
try:
|
||||
a = <double>(__params['a'])
|
||||
b = <np.ndarray>(__params['b'])
|
||||
__params = process_args([], args, kwargs)
|
||||
self.CTest_ = shared_ptr[CTest](new CTest())
|
||||
except:
|
||||
return False
|
||||
b = b.astype(float, order='F', copy=False)
|
||||
self.shared_CTest_ = shared_ptr[CTest](new CTest(a, <MatrixXd>(Map[MatrixXd](b))))
|
||||
return True
|
||||
|
||||
pass
|
||||
try:
|
||||
__params = process_args(['a', 'b'], args, kwargs)
|
||||
a = <double>(__params[0])
|
||||
b = <np.ndarray>(__params[1])
|
||||
assert isinstance(b, np.ndarray) and b.ndim == 2
|
||||
b = b.astype(float, order='F', copy=False)
|
||||
self.CTest_ = shared_ptr[CTest](new CTest(a, <MatrixXd>(Map[MatrixXd](b))))
|
||||
except:
|
||||
pass
|
||||
if (self.CTest_.use_count()==0):
|
||||
raise TypeError('Test construction failed!')
|
||||
|
||||
@staticmethod
|
||||
cdef Test cyCreateFromShared(const shared_ptr[CTest]& other):
|
||||
if other.get() == NULL:
|
||||
raise RuntimeError('Cannot create object from a nullptr!')
|
||||
cdef Test ret = Test(cyCreateFromShared=True)
|
||||
ret.shared_CTest_ = other
|
||||
return ret
|
||||
cdef Test return_value = Test(cyCreateFromShared=True)
|
||||
return_value.CTest_ = other
|
||||
return return_value
|
||||
|
||||
def arg_EigenConstRef(self, np.ndarray value):
|
||||
value = value.astype(float, order='F', copy=False)
|
||||
self.shared_CTest_.get().arg_EigenConstRef(<MatrixXd>(Map[MatrixXd](value)))
|
||||
self.CTest_.get().arg_EigenConstRef(<MatrixXd>(Map[MatrixXd](value)))
|
||||
def create_MixedPtrs(self):
|
||||
cdef pair [CTest,shared_ptr[CTest]] ret = self.shared_CTest_.get().create_MixedPtrs()
|
||||
cdef pair [CTest,shared_ptr[CTest]] ret = self.CTest_.get().create_MixedPtrs()
|
||||
return (Test.cyCreateFromShared(make_shared[CTest](ret.first)),Test.cyCreateFromShared(ret.second))
|
||||
def create_ptrs(self):
|
||||
cdef pair [shared_ptr[CTest],shared_ptr[CTest]] ret = self.shared_CTest_.get().create_ptrs()
|
||||
cdef pair [shared_ptr[CTest],shared_ptr[CTest]] ret = self.CTest_.get().create_ptrs()
|
||||
return (Test.cyCreateFromShared(ret.first),Test.cyCreateFromShared(ret.second))
|
||||
def __str__(self):
|
||||
strBuf = RedirectCout()
|
||||
self.print_('')
|
||||
return strBuf.str()
|
||||
def print_(self):
|
||||
self.shared_CTest_.get().print_()
|
||||
self.CTest_.get().print_()
|
||||
def return_Point2Ptr(self, bool value):
|
||||
cdef shared_ptr[CPoint2] ret = self.shared_CTest_.get().return_Point2Ptr(value)
|
||||
cdef shared_ptr[CPoint2] ret = self.CTest_.get().return_Point2Ptr(value)
|
||||
return Point2.cyCreateFromShared(ret)
|
||||
def return_Test(self, Test value):
|
||||
cdef shared_ptr[CTest] ret = make_shared[CTest](self.shared_CTest_.get().return_Test(value.shared_CTest_))
|
||||
cdef shared_ptr[CTest] ret = make_shared[CTest](self.CTest_.get().return_Test(value.CTest_))
|
||||
return Test.cyCreateFromShared(ret)
|
||||
def return_TestPtr(self, Test value):
|
||||
cdef shared_ptr[CTest] ret = self.shared_CTest_.get().return_TestPtr(value.shared_CTest_)
|
||||
cdef shared_ptr[CTest] ret = self.CTest_.get().return_TestPtr(value.CTest_)
|
||||
return Test.cyCreateFromShared(ret)
|
||||
def return_bool(self, bool value):
|
||||
cdef bool ret = self.shared_CTest_.get().return_bool(value)
|
||||
cdef bool ret = self.CTest_.get().return_bool(value)
|
||||
return ret
|
||||
def return_double(self, double value):
|
||||
cdef double ret = self.shared_CTest_.get().return_double(value)
|
||||
cdef double ret = self.CTest_.get().return_double(value)
|
||||
return ret
|
||||
def return_field(self, Test t):
|
||||
cdef bool ret = self.shared_CTest_.get().return_field(deref(t.shared_CTest_))
|
||||
cdef bool ret = self.CTest_.get().return_field(deref(t.CTest_))
|
||||
return ret
|
||||
def return_int(self, int value):
|
||||
cdef int ret = self.shared_CTest_.get().return_int(value)
|
||||
cdef int ret = self.CTest_.get().return_int(value)
|
||||
return ret
|
||||
def return_matrix1(self, np.ndarray value):
|
||||
value = value.astype(float, order='F', copy=False)
|
||||
cdef MatrixXd ret = self.shared_CTest_.get().return_matrix1(<MatrixXd>(Map[MatrixXd](value)))
|
||||
cdef MatrixXd ret = self.CTest_.get().return_matrix1(<MatrixXd>(Map[MatrixXd](value)))
|
||||
return ndarray_copy(ret)
|
||||
def return_matrix2(self, np.ndarray value):
|
||||
value = value.astype(float, order='F', copy=False)
|
||||
cdef MatrixXd ret = self.shared_CTest_.get().return_matrix2(<MatrixXd>(Map[MatrixXd](value)))
|
||||
cdef MatrixXd ret = self.CTest_.get().return_matrix2(<MatrixXd>(Map[MatrixXd](value)))
|
||||
return ndarray_copy(ret)
|
||||
def return_pair(self, np.ndarray v, np.ndarray A):
|
||||
v = v.astype(float, order='F', copy=False)
|
||||
A = A.astype(float, order='F', copy=False)
|
||||
cdef pair [VectorXd,MatrixXd] ret = self.shared_CTest_.get().return_pair(<VectorXd>(Map[VectorXd](v)), <MatrixXd>(Map[MatrixXd](A)))
|
||||
cdef pair [VectorXd,MatrixXd] ret = self.CTest_.get().return_pair(<VectorXd>(Map[VectorXd](v)), <MatrixXd>(Map[MatrixXd](A)))
|
||||
return (ndarray_copy(ret.first).squeeze(),ndarray_copy(ret.second))
|
||||
def return_ptrs(self, Test p1, Test p2):
|
||||
cdef pair [shared_ptr[CTest],shared_ptr[CTest]] ret = self.shared_CTest_.get().return_ptrs(p1.shared_CTest_, p2.shared_CTest_)
|
||||
cdef pair [shared_ptr[CTest],shared_ptr[CTest]] ret = self.CTest_.get().return_ptrs(p1.CTest_, p2.CTest_)
|
||||
return (Test.cyCreateFromShared(ret.first),Test.cyCreateFromShared(ret.second))
|
||||
def return_size_t(self, size_t value):
|
||||
cdef size_t ret = self.shared_CTest_.get().return_size_t(value)
|
||||
cdef size_t ret = self.CTest_.get().return_size_t(value)
|
||||
return ret
|
||||
def return_string(self, string value):
|
||||
cdef string ret = self.shared_CTest_.get().return_string(value)
|
||||
cdef string ret = self.CTest_.get().return_string(value)
|
||||
return ret
|
||||
def return_vector1(self, np.ndarray value):
|
||||
value = value.astype(float, order='F', copy=False)
|
||||
cdef VectorXd ret = self.shared_CTest_.get().return_vector1(<VectorXd>(Map[VectorXd](value)))
|
||||
cdef VectorXd ret = self.CTest_.get().return_vector1(<VectorXd>(Map[VectorXd](value)))
|
||||
return ndarray_copy(ret).squeeze()
|
||||
def return_vector2(self, np.ndarray value):
|
||||
value = value.astype(float, order='F', copy=False)
|
||||
cdef VectorXd ret = self.shared_CTest_.get().return_vector2(<VectorXd>(Map[VectorXd](value)))
|
||||
cdef VectorXd ret = self.CTest_.get().return_vector2(<VectorXd>(Map[VectorXd](value)))
|
||||
return ndarray_copy(ret).squeeze()
|
||||
|
||||
|
||||
cdef class MyBase:
|
||||
def __init__(self, *args, **kwargs):
|
||||
self.shared_CMyBase_ = shared_ptr[CMyBase]()
|
||||
cdef list __params
|
||||
self.CMyBase_ = shared_ptr[CMyBase]()
|
||||
if len(args)==0 and len(kwargs)==1 and kwargs.has_key('cyCreateFromShared'):
|
||||
return
|
||||
else:
|
||||
if (self.CMyBase_.use_count()==0):
|
||||
raise TypeError('MyBase construction failed!')
|
||||
|
||||
@staticmethod
|
||||
cdef MyBase cyCreateFromShared(const shared_ptr[CMyBase]& other):
|
||||
if other.get() == NULL:
|
||||
raise RuntimeError('Cannot create object from a nullptr!')
|
||||
cdef MyBase ret = MyBase(cyCreateFromShared=True)
|
||||
ret.shared_CMyBase_ = other
|
||||
return ret
|
||||
cdef MyBase return_value = MyBase(cyCreateFromShared=True)
|
||||
return_value.CMyBase_ = other
|
||||
return return_value
|
||||
|
||||
|
||||
|
||||
cdef class MyTemplatePoint2(MyBase):
|
||||
def __init__(self, *args, **kwargs):
|
||||
self.shared_CMyTemplatePoint2_ = shared_ptr[CMyTemplatePoint2]()
|
||||
cdef list __params
|
||||
self.CMyTemplatePoint2_ = shared_ptr[CMyTemplatePoint2]()
|
||||
if len(args)==0 and len(kwargs)==1 and kwargs.has_key('cyCreateFromShared'):
|
||||
return
|
||||
elif self.MyTemplatePoint2_0(*args, **kwargs):
|
||||
try:
|
||||
__params = process_args([], args, kwargs)
|
||||
self.CMyTemplatePoint2_ = shared_ptr[CMyTemplatePoint2](new CMyTemplatePoint2())
|
||||
except:
|
||||
pass
|
||||
else:
|
||||
if (self.CMyTemplatePoint2_.use_count()==0):
|
||||
raise TypeError('MyTemplatePoint2 construction failed!')
|
||||
self.shared_CMyBase_ = <shared_ptr[CMyBase]>(self.shared_CMyTemplatePoint2_)
|
||||
|
||||
def MyTemplatePoint2_0(self, *args, **kwargs):
|
||||
if len(args)+len(kwargs) !=0:
|
||||
return False
|
||||
self.shared_CMyTemplatePoint2_ = shared_ptr[CMyTemplatePoint2](new CMyTemplatePoint2())
|
||||
return True
|
||||
|
||||
self.CMyBase_ = <shared_ptr[CMyBase]>(self.CMyTemplatePoint2_)
|
||||
|
||||
@staticmethod
|
||||
cdef MyTemplatePoint2 cyCreateFromShared(const shared_ptr[CMyTemplatePoint2]& other):
|
||||
if other.get() == NULL:
|
||||
raise RuntimeError('Cannot create object from a nullptr!')
|
||||
cdef MyTemplatePoint2 ret = MyTemplatePoint2(cyCreateFromShared=True)
|
||||
ret.shared_CMyTemplatePoint2_ = other
|
||||
ret.shared_CMyBase_ = <shared_ptr[CMyBase]>(other)
|
||||
return ret
|
||||
cdef MyTemplatePoint2 return_value = MyTemplatePoint2(cyCreateFromShared=True)
|
||||
return_value.CMyTemplatePoint2_ = other
|
||||
return_value.CMyBase_ = <shared_ptr[CMyBase]>(other)
|
||||
return return_value
|
||||
|
||||
def accept_T(self, Point2 value):
|
||||
self.shared_CMyTemplatePoint2_.get().accept_T(deref(value.shared_CPoint2_))
|
||||
self.CMyTemplatePoint2_.get().accept_T(deref(value.CPoint2_))
|
||||
def accept_Tptr(self, Point2 value):
|
||||
self.shared_CMyTemplatePoint2_.get().accept_Tptr(value.shared_CPoint2_)
|
||||
self.CMyTemplatePoint2_.get().accept_Tptr(value.CPoint2_)
|
||||
def create_MixedPtrs(self):
|
||||
cdef pair [CPoint2,shared_ptr[CPoint2]] ret = self.shared_CMyTemplatePoint2_.get().create_MixedPtrs()
|
||||
cdef pair [CPoint2,shared_ptr[CPoint2]] ret = self.CMyTemplatePoint2_.get().create_MixedPtrs()
|
||||
return (Point2.cyCreateFromShared(make_shared[CPoint2](ret.first)),Point2.cyCreateFromShared(ret.second))
|
||||
def create_ptrs(self):
|
||||
cdef pair [shared_ptr[CPoint2],shared_ptr[CPoint2]] ret = self.shared_CMyTemplatePoint2_.get().create_ptrs()
|
||||
cdef pair [shared_ptr[CPoint2],shared_ptr[CPoint2]] ret = self.CMyTemplatePoint2_.get().create_ptrs()
|
||||
return (Point2.cyCreateFromShared(ret.first),Point2.cyCreateFromShared(ret.second))
|
||||
def return_T(self, Point2 value):
|
||||
cdef shared_ptr[CPoint2] ret = make_shared[CPoint2](self.shared_CMyTemplatePoint2_.get().return_T(value.shared_CPoint2_))
|
||||
cdef shared_ptr[CPoint2] ret = make_shared[CPoint2](self.CMyTemplatePoint2_.get().return_T(value.CPoint2_))
|
||||
return Point2.cyCreateFromShared(ret)
|
||||
def return_Tptr(self, Point2 value):
|
||||
cdef shared_ptr[CPoint2] ret = self.shared_CMyTemplatePoint2_.get().return_Tptr(value.shared_CPoint2_)
|
||||
cdef shared_ptr[CPoint2] ret = self.CMyTemplatePoint2_.get().return_Tptr(value.CPoint2_)
|
||||
return Point2.cyCreateFromShared(ret)
|
||||
def return_ptrs(self, Point2 p1, Point2 p2):
|
||||
cdef pair [shared_ptr[CPoint2],shared_ptr[CPoint2]] ret = self.shared_CMyTemplatePoint2_.get().return_ptrs(p1.shared_CPoint2_, p2.shared_CPoint2_)
|
||||
cdef pair [shared_ptr[CPoint2],shared_ptr[CPoint2]] ret = self.CMyTemplatePoint2_.get().return_ptrs(p1.CPoint2_, p2.CPoint2_)
|
||||
return (Point2.cyCreateFromShared(ret.first),Point2.cyCreateFromShared(ret.second))
|
||||
def templatedMethodMatrix(self, np.ndarray t):
|
||||
t = t.astype(float, order='F', copy=False)
|
||||
cdef MatrixXd ret = self.shared_CMyTemplatePoint2_.get().templatedMethod[MatrixXd](<MatrixXd>(Map[MatrixXd](t)))
|
||||
cdef MatrixXd ret = self.CMyTemplatePoint2_.get().templatedMethod[MatrixXd](<MatrixXd>(Map[MatrixXd](t)))
|
||||
return ndarray_copy(ret)
|
||||
def templatedMethodPoint2(self, Point2 t):
|
||||
cdef shared_ptr[CPoint2] ret = make_shared[CPoint2](self.shared_CMyTemplatePoint2_.get().templatedMethod[CPoint2](deref(t.shared_CPoint2_)))
|
||||
cdef shared_ptr[CPoint2] ret = make_shared[CPoint2](self.CMyTemplatePoint2_.get().templatedMethod[CPoint2](deref(t.CPoint2_)))
|
||||
return Point2.cyCreateFromShared(ret)
|
||||
def templatedMethodPoint3(self, Point3 t):
|
||||
cdef shared_ptr[CPoint3] ret = make_shared[CPoint3](self.shared_CMyTemplatePoint2_.get().templatedMethod[CPoint3](deref(t.shared_CPoint3_)))
|
||||
cdef shared_ptr[CPoint3] ret = make_shared[CPoint3](self.CMyTemplatePoint2_.get().templatedMethod[CPoint3](deref(t.CPoint3_)))
|
||||
return Point3.cyCreateFromShared(ret)
|
||||
def templatedMethodVector(self, np.ndarray t):
|
||||
t = t.astype(float, order='F', copy=False)
|
||||
cdef VectorXd ret = self.shared_CMyTemplatePoint2_.get().templatedMethod[VectorXd](<VectorXd>(Map[VectorXd](t)))
|
||||
cdef VectorXd ret = self.CMyTemplatePoint2_.get().templatedMethod[VectorXd](<VectorXd>(Map[VectorXd](t)))
|
||||
return ndarray_copy(ret).squeeze()
|
||||
def dynamic_cast_MyTemplatePoint2_MyBase(MyBase parent):
|
||||
try:
|
||||
return MyTemplatePoint2.cyCreateFromShared(<shared_ptr[CMyTemplatePoint2]>dynamic_pointer_cast[CMyTemplatePoint2,CMyBase](parent.shared_CMyBase_))
|
||||
return MyTemplatePoint2.cyCreateFromShared(<shared_ptr[CMyTemplatePoint2]>dynamic_pointer_cast[CMyTemplatePoint2,CMyBase](parent.CMyBase_))
|
||||
except:
|
||||
raise TypeError('dynamic cast failed!')
|
||||
|
||||
|
||||
cdef class MyTemplateMatrix(MyBase):
|
||||
def __init__(self, *args, **kwargs):
|
||||
self.shared_CMyTemplateMatrix_ = shared_ptr[CMyTemplateMatrix]()
|
||||
cdef list __params
|
||||
self.CMyTemplateMatrix_ = shared_ptr[CMyTemplateMatrix]()
|
||||
if len(args)==0 and len(kwargs)==1 and kwargs.has_key('cyCreateFromShared'):
|
||||
return
|
||||
elif self.MyTemplateMatrix_0(*args, **kwargs):
|
||||
try:
|
||||
__params = process_args([], args, kwargs)
|
||||
self.CMyTemplateMatrix_ = shared_ptr[CMyTemplateMatrix](new CMyTemplateMatrix())
|
||||
except:
|
||||
pass
|
||||
else:
|
||||
if (self.CMyTemplateMatrix_.use_count()==0):
|
||||
raise TypeError('MyTemplateMatrix construction failed!')
|
||||
self.shared_CMyBase_ = <shared_ptr[CMyBase]>(self.shared_CMyTemplateMatrix_)
|
||||
|
||||
def MyTemplateMatrix_0(self, *args, **kwargs):
|
||||
if len(args)+len(kwargs) !=0:
|
||||
return False
|
||||
self.shared_CMyTemplateMatrix_ = shared_ptr[CMyTemplateMatrix](new CMyTemplateMatrix())
|
||||
return True
|
||||
|
||||
self.CMyBase_ = <shared_ptr[CMyBase]>(self.CMyTemplateMatrix_)
|
||||
|
||||
@staticmethod
|
||||
cdef MyTemplateMatrix cyCreateFromShared(const shared_ptr[CMyTemplateMatrix]& other):
|
||||
if other.get() == NULL:
|
||||
raise RuntimeError('Cannot create object from a nullptr!')
|
||||
cdef MyTemplateMatrix ret = MyTemplateMatrix(cyCreateFromShared=True)
|
||||
ret.shared_CMyTemplateMatrix_ = other
|
||||
ret.shared_CMyBase_ = <shared_ptr[CMyBase]>(other)
|
||||
return ret
|
||||
cdef MyTemplateMatrix return_value = MyTemplateMatrix(cyCreateFromShared=True)
|
||||
return_value.CMyTemplateMatrix_ = other
|
||||
return_value.CMyBase_ = <shared_ptr[CMyBase]>(other)
|
||||
return return_value
|
||||
|
||||
def accept_T(self, np.ndarray value):
|
||||
value = value.astype(float, order='F', copy=False)
|
||||
self.shared_CMyTemplateMatrix_.get().accept_T(<MatrixXd>(Map[MatrixXd](value)))
|
||||
self.CMyTemplateMatrix_.get().accept_T(<MatrixXd>(Map[MatrixXd](value)))
|
||||
def accept_Tptr(self, np.ndarray value):
|
||||
value = value.astype(float, order='F', copy=False)
|
||||
self.shared_CMyTemplateMatrix_.get().accept_Tptr(<MatrixXd>(Map[MatrixXd](value)))
|
||||
self.CMyTemplateMatrix_.get().accept_Tptr(<MatrixXd>(Map[MatrixXd](value)))
|
||||
def create_MixedPtrs(self):
|
||||
cdef pair [MatrixXd,shared_ptr[MatrixXd]] ret = self.shared_CMyTemplateMatrix_.get().create_MixedPtrs()
|
||||
cdef pair [MatrixXd,shared_ptr[MatrixXd]] ret = self.CMyTemplateMatrix_.get().create_MixedPtrs()
|
||||
return (ndarray_copy(ret.first),ndarray_copy(ret.second))
|
||||
def create_ptrs(self):
|
||||
cdef pair [shared_ptr[MatrixXd],shared_ptr[MatrixXd]] ret = self.shared_CMyTemplateMatrix_.get().create_ptrs()
|
||||
cdef pair [shared_ptr[MatrixXd],shared_ptr[MatrixXd]] ret = self.CMyTemplateMatrix_.get().create_ptrs()
|
||||
return (ndarray_copy(ret.first),ndarray_copy(ret.second))
|
||||
def return_T(self, np.ndarray value):
|
||||
value = value.astype(float, order='F', copy=False)
|
||||
cdef MatrixXd ret = self.shared_CMyTemplateMatrix_.get().return_T(<MatrixXd>(Map[MatrixXd](value)))
|
||||
cdef MatrixXd ret = self.CMyTemplateMatrix_.get().return_T(<MatrixXd>(Map[MatrixXd](value)))
|
||||
return ndarray_copy(ret)
|
||||
def return_Tptr(self, np.ndarray value):
|
||||
value = value.astype(float, order='F', copy=False)
|
||||
cdef shared_ptr[MatrixXd] ret = self.shared_CMyTemplateMatrix_.get().return_Tptr(<MatrixXd>(Map[MatrixXd](value)))
|
||||
cdef shared_ptr[MatrixXd] ret = self.CMyTemplateMatrix_.get().return_Tptr(<MatrixXd>(Map[MatrixXd](value)))
|
||||
return ndarray_copy(ret)
|
||||
def return_ptrs(self, np.ndarray p1, np.ndarray p2):
|
||||
p1 = p1.astype(float, order='F', copy=False)
|
||||
p2 = p2.astype(float, order='F', copy=False)
|
||||
cdef pair [shared_ptr[MatrixXd],shared_ptr[MatrixXd]] ret = self.shared_CMyTemplateMatrix_.get().return_ptrs(<MatrixXd>(Map[MatrixXd](p1)), <MatrixXd>(Map[MatrixXd](p2)))
|
||||
cdef pair [shared_ptr[MatrixXd],shared_ptr[MatrixXd]] ret = self.CMyTemplateMatrix_.get().return_ptrs(<MatrixXd>(Map[MatrixXd](p1)), <MatrixXd>(Map[MatrixXd](p2)))
|
||||
return (ndarray_copy(ret.first),ndarray_copy(ret.second))
|
||||
def templatedMethodMatrix(self, np.ndarray t):
|
||||
t = t.astype(float, order='F', copy=False)
|
||||
cdef MatrixXd ret = self.shared_CMyTemplateMatrix_.get().templatedMethod[MatrixXd](<MatrixXd>(Map[MatrixXd](t)))
|
||||
cdef MatrixXd ret = self.CMyTemplateMatrix_.get().templatedMethod[MatrixXd](<MatrixXd>(Map[MatrixXd](t)))
|
||||
return ndarray_copy(ret)
|
||||
def templatedMethodPoint2(self, Point2 t):
|
||||
cdef shared_ptr[CPoint2] ret = make_shared[CPoint2](self.shared_CMyTemplateMatrix_.get().templatedMethod[CPoint2](deref(t.shared_CPoint2_)))
|
||||
cdef shared_ptr[CPoint2] ret = make_shared[CPoint2](self.CMyTemplateMatrix_.get().templatedMethod[CPoint2](deref(t.CPoint2_)))
|
||||
return Point2.cyCreateFromShared(ret)
|
||||
def templatedMethodPoint3(self, Point3 t):
|
||||
cdef shared_ptr[CPoint3] ret = make_shared[CPoint3](self.shared_CMyTemplateMatrix_.get().templatedMethod[CPoint3](deref(t.shared_CPoint3_)))
|
||||
cdef shared_ptr[CPoint3] ret = make_shared[CPoint3](self.CMyTemplateMatrix_.get().templatedMethod[CPoint3](deref(t.CPoint3_)))
|
||||
return Point3.cyCreateFromShared(ret)
|
||||
def templatedMethodVector(self, np.ndarray t):
|
||||
t = t.astype(float, order='F', copy=False)
|
||||
cdef VectorXd ret = self.shared_CMyTemplateMatrix_.get().templatedMethod[VectorXd](<VectorXd>(Map[VectorXd](t)))
|
||||
cdef VectorXd ret = self.CMyTemplateMatrix_.get().templatedMethod[VectorXd](<VectorXd>(Map[VectorXd](t)))
|
||||
return ndarray_copy(ret).squeeze()
|
||||
def dynamic_cast_MyTemplateMatrix_MyBase(MyBase parent):
|
||||
try:
|
||||
return MyTemplateMatrix.cyCreateFromShared(<shared_ptr[CMyTemplateMatrix]>dynamic_pointer_cast[CMyTemplateMatrix,CMyBase](parent.shared_CMyBase_))
|
||||
return MyTemplateMatrix.cyCreateFromShared(<shared_ptr[CMyTemplateMatrix]>dynamic_pointer_cast[CMyTemplateMatrix,CMyBase](parent.CMyBase_))
|
||||
except:
|
||||
raise TypeError('dynamic cast failed!')
|
||||
|
||||
|
||||
cdef class MyVector3:
|
||||
def __init__(self, *args, **kwargs):
|
||||
self.shared_CMyVector3_ = shared_ptr[CMyVector3]()
|
||||
cdef list __params
|
||||
self.CMyVector3_ = shared_ptr[CMyVector3]()
|
||||
if len(args)==0 and len(kwargs)==1 and kwargs.has_key('cyCreateFromShared'):
|
||||
return
|
||||
elif self.MyVector3_0(*args, **kwargs):
|
||||
try:
|
||||
__params = process_args([], args, kwargs)
|
||||
self.CMyVector3_ = shared_ptr[CMyVector3](new CMyVector3())
|
||||
except:
|
||||
pass
|
||||
else:
|
||||
if (self.CMyVector3_.use_count()==0):
|
||||
raise TypeError('MyVector3 construction failed!')
|
||||
|
||||
def MyVector3_0(self, *args, **kwargs):
|
||||
if len(args)+len(kwargs) !=0:
|
||||
return False
|
||||
self.shared_CMyVector3_ = shared_ptr[CMyVector3](new CMyVector3())
|
||||
return True
|
||||
|
||||
|
||||
@staticmethod
|
||||
cdef MyVector3 cyCreateFromShared(const shared_ptr[CMyVector3]& other):
|
||||
if other.get() == NULL:
|
||||
raise RuntimeError('Cannot create object from a nullptr!')
|
||||
cdef MyVector3 ret = MyVector3(cyCreateFromShared=True)
|
||||
ret.shared_CMyVector3_ = other
|
||||
return ret
|
||||
cdef MyVector3 return_value = MyVector3(cyCreateFromShared=True)
|
||||
return_value.CMyVector3_ = other
|
||||
return return_value
|
||||
|
||||
|
||||
|
||||
cdef class MyVector12:
|
||||
def __init__(self, *args, **kwargs):
|
||||
self.shared_CMyVector12_ = shared_ptr[CMyVector12]()
|
||||
cdef list __params
|
||||
self.CMyVector12_ = shared_ptr[CMyVector12]()
|
||||
if len(args)==0 and len(kwargs)==1 and kwargs.has_key('cyCreateFromShared'):
|
||||
return
|
||||
elif self.MyVector12_0(*args, **kwargs):
|
||||
try:
|
||||
__params = process_args([], args, kwargs)
|
||||
self.CMyVector12_ = shared_ptr[CMyVector12](new CMyVector12())
|
||||
except:
|
||||
pass
|
||||
else:
|
||||
if (self.CMyVector12_.use_count()==0):
|
||||
raise TypeError('MyVector12 construction failed!')
|
||||
|
||||
def MyVector12_0(self, *args, **kwargs):
|
||||
if len(args)+len(kwargs) !=0:
|
||||
return False
|
||||
self.shared_CMyVector12_ = shared_ptr[CMyVector12](new CMyVector12())
|
||||
return True
|
||||
|
||||
|
||||
@staticmethod
|
||||
cdef MyVector12 cyCreateFromShared(const shared_ptr[CMyVector12]& other):
|
||||
if other.get() == NULL:
|
||||
raise RuntimeError('Cannot create object from a nullptr!')
|
||||
cdef MyVector12 ret = MyVector12(cyCreateFromShared=True)
|
||||
ret.shared_CMyVector12_ = other
|
||||
return ret
|
||||
cdef MyVector12 return_value = MyVector12(cyCreateFromShared=True)
|
||||
return_value.CMyVector12_ = other
|
||||
return return_value
|
||||
|
||||
|
||||
|
||||
cdef class MyFactorPosePoint2:
|
||||
def __init__(self, *args, **kwargs):
|
||||
self.shared_CMyFactorPosePoint2_ = shared_ptr[CMyFactorPosePoint2]()
|
||||
cdef list __params
|
||||
self.CMyFactorPosePoint2_ = shared_ptr[CMyFactorPosePoint2]()
|
||||
if len(args)==0 and len(kwargs)==1 and kwargs.has_key('cyCreateFromShared'):
|
||||
return
|
||||
elif self.MyFactorPosePoint2_0(*args, **kwargs):
|
||||
pass
|
||||
else:
|
||||
raise TypeError('MyFactorPosePoint2 construction failed!')
|
||||
|
||||
def MyFactorPosePoint2_0(self, *args, **kwargs):
|
||||
if len(args)+len(kwargs) !=4:
|
||||
return False
|
||||
__params = kwargs.copy()
|
||||
__names = ['key1', 'key2', 'measured', 'noiseModel']
|
||||
for i in range(len(args)):
|
||||
__params[__names[i]] = args[i]
|
||||
if not isinstance(__params[__names[3]], noiseModel_Base):
|
||||
return False
|
||||
try:
|
||||
key1 = <size_t>(__params['key1'])
|
||||
key2 = <size_t>(__params['key2'])
|
||||
measured = <double>(__params['measured'])
|
||||
noiseModel = <noiseModel_Base>(__params['noiseModel'])
|
||||
__params = process_args(['key1', 'key2', 'measured', 'noiseModel'], args, kwargs)
|
||||
key1 = <size_t>(__params[0])
|
||||
key2 = <size_t>(__params[1])
|
||||
measured = <double>(__params[2])
|
||||
noiseModel = <noiseModel_Base>(__params[3])
|
||||
assert isinstance(noiseModel, noiseModel_Base)
|
||||
self.CMyFactorPosePoint2_ = shared_ptr[CMyFactorPosePoint2](new CMyFactorPosePoint2(key1, key2, measured, noiseModel.CnoiseModel_Base_))
|
||||
except:
|
||||
return False
|
||||
self.shared_CMyFactorPosePoint2_ = shared_ptr[CMyFactorPosePoint2](new CMyFactorPosePoint2(key1, key2, measured, noiseModel.shared_CnoiseModel_Base_))
|
||||
return True
|
||||
|
||||
pass
|
||||
if (self.CMyFactorPosePoint2_.use_count()==0):
|
||||
raise TypeError('MyFactorPosePoint2 construction failed!')
|
||||
|
||||
@staticmethod
|
||||
cdef MyFactorPosePoint2 cyCreateFromShared(const shared_ptr[CMyFactorPosePoint2]& other):
|
||||
if other.get() == NULL:
|
||||
raise RuntimeError('Cannot create object from a nullptr!')
|
||||
cdef MyFactorPosePoint2 ret = MyFactorPosePoint2(cyCreateFromShared=True)
|
||||
ret.shared_CMyFactorPosePoint2_ = other
|
||||
return ret
|
||||
cdef MyFactorPosePoint2 return_value = MyFactorPosePoint2(cyCreateFromShared=True)
|
||||
return_value.CMyFactorPosePoint2_ = other
|
||||
return return_value
|
||||
|
||||
|
||||
|
||||
|
||||
|
@ -489,37 +449,33 @@ def aGlobalFunction():
|
|||
cdef VectorXd ret = pxd_aGlobalFunction()
|
||||
return ndarray_copy(ret).squeeze()
|
||||
def overloadedGlobalFunction(*args, **kwargs):
|
||||
success, results = overloadedGlobalFunction_0(*args, **kwargs)
|
||||
success, results = overloadedGlobalFunction_0(args, kwargs)
|
||||
if success:
|
||||
return results
|
||||
success, results = overloadedGlobalFunction_1(*args, **kwargs)
|
||||
success, results = overloadedGlobalFunction_1(args, kwargs)
|
||||
if success:
|
||||
return results
|
||||
raise TypeError('Could not find the correct overload')
|
||||
def overloadedGlobalFunction_0(*args, **kwargs):
|
||||
if len(args)+len(kwargs) !=1:
|
||||
return False, None
|
||||
__params = kwargs.copy()
|
||||
__names = ['a']
|
||||
for i in range(len(args)):
|
||||
__params[__names[i]] = args[i]
|
||||
def overloadedGlobalFunction_0(args, kwargs):
|
||||
cdef list __params
|
||||
cdef VectorXd return_value
|
||||
try:
|
||||
a = <int>(__params['a'])
|
||||
__params = process_args(['a'], args, kwargs)
|
||||
a = <int>(__params[0])
|
||||
return_value = pxd_overloadedGlobalFunction(a)
|
||||
return True, ndarray_copy(return_value).squeeze()
|
||||
except:
|
||||
return False, None
|
||||
cdef VectorXd ret = pxd_overloadedGlobalFunction(a)
|
||||
return True, ndarray_copy(ret).squeeze()
|
||||
def overloadedGlobalFunction_1(*args, **kwargs):
|
||||
if len(args)+len(kwargs) !=2:
|
||||
return False, None
|
||||
__params = kwargs.copy()
|
||||
__names = ['a', 'b']
|
||||
for i in range(len(args)):
|
||||
__params[__names[i]] = args[i]
|
||||
|
||||
def overloadedGlobalFunction_1(args, kwargs):
|
||||
cdef list __params
|
||||
cdef VectorXd return_value
|
||||
try:
|
||||
a = <int>(__params['a'])
|
||||
b = <double>(__params['b'])
|
||||
__params = process_args(['a', 'b'], args, kwargs)
|
||||
a = <int>(__params[0])
|
||||
b = <double>(__params[1])
|
||||
return_value = pxd_overloadedGlobalFunction(a, b)
|
||||
return True, ndarray_copy(return_value).squeeze()
|
||||
except:
|
||||
return False, None
|
||||
cdef VectorXd ret = pxd_overloadedGlobalFunction(a, b)
|
||||
return True, ndarray_copy(ret).squeeze()
|
||||
|
||||
|
|
Loading…
Reference in New Issue