Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Stack Of Tasks
eigenpy
Commits
cf46bbfe
Commit
cf46bbfe
authored
Jul 24, 2014
by
Nicolas Mansard
Committed by
nmansard
Jul 24, 2014
Browse files
Added the geometry module.
parent
92f87dd4
Changes
17
Hide whitespace changes
Inline
Side-by-side
CMakeLists.txt
View file @
cf46bbfe
...
...
@@ -36,8 +36,15 @@ INCLUDE_DIRECTORIES(${PYTHON_INCLUDE_DIRS})
# ----------------------------------------------------
SET
(
${
PROJECT_NAME
}
_HEADERS
src/eigenpy.hpp
src/exception.hpp
src/map.hpp
src/simple.hpp
src/geometry.hpp
src/angle-axis.hpp
src/quaternion.hpp
)
MAKE_DIRECTORY
(
"
${${
PROJECT_NAME
}
_BINARY_DIR
}
/include/eigenpy"
)
INCLUDE_DIRECTORIES
(
${${
PROJECT_NAME
}
_BINARY_DIR
}
/include/eigenpy
)
FOREACH
(
header
${${
PROJECT_NAME
}
_HEADERS
}
)
GET_FILENAME_COMPONENT
(
headerName
${
header
}
NAME
)
...
...
@@ -52,17 +59,42 @@ FOREACH(header ${${PROJECT_NAME}_HEADERS})
ENDIF
(
WIN32
)
INSTALL
(
FILES
${${
PROJECT_NAME
}
_SOURCE_DIR
}
/
${
header
}
DESTINATION
${
CMAKE_INSTALL_PREFIX
}
/include/
${
PROJECT_NAME
}
PERMISSIONS OWNER_READ GROUP_READ WORLD_READ
OWNER_WRITE
)
PERMISSIONS OWNER_READ GROUP_READ WORLD_READ
)
ENDFOREACH
(
header
)
# ----------------------------------------------------
# --- TARGETS ----------------------------------------
# ----------------------------------------------------
ADD_LIBRARY
(
eigenpy SHARED unittest/libeigenpy.cpp
)
ADD_LIBRARY
(
eigenpy SHARED
src/exception.cpp
src/simple.cpp
src/angle-axis.cpp
src/quaternion.cpp
)
TARGET_LINK_LIBRARIES
(
eigenpy
${
Boost_LIBRARIES
}
)
INSTALL
(
TARGETS eigenpy DESTINATION
${
CMAKE_INSTALL_PREFIX
}
/lib
)
# ----------------------------------------------------
# --- UNIT TEST --------------------------------------
# ----------------------------------------------------
ADD_LIBRARY
(
matrix SHARED unittest/matrix.cpp
)
TARGET_LINK_LIBRARIES
(
matrix
${
Boost_LIBRARIES
}
eigenpy
)
SET_TARGET_PROPERTIES
(
matrix PROPERTIES PREFIX
""
)
ADD_LIBRARY
(
geometry SHARED unittest/geometry.cpp
)
TARGET_LINK_LIBRARIES
(
geometry
${
Boost_LIBRARIES
}
eigenpy
)
SET_TARGET_PROPERTIES
(
geometry PROPERTIES PREFIX
""
)
ADD_LIBRARY
(
se3 SHARED unittest/se3.cpp
)
TARGET_LINK_LIBRARIES
(
se3
${
Boost_LIBRARIES
}
eigenpy
)
SET_TARGET_PROPERTIES
(
se3 PROPERTIES PREFIX
""
)
#ADD_LIBRARY(align SHARED unittest/libalign.cpp)
#TARGET_LINK_LIBRARIES(align ${Boost_LIBRARIES} eigenpy)
ADD_EXECUTABLE
(
se3 EXCLUDE_FROM_ALL unittest/se3.cpp
)
#TARGET_INCLUDE_DIRECTORIES(se3 unittest/)
ADD_EXECUTABLE
(
tse3 EXCLUDE_FROM_ALL unittest/tse3.cpp
)
PKG_CONFIG_APPEND_CFLAGS
(
${
_Eigen_CFLAGS
}
)
PKG_CONFIG_APPEND_CFLAGS
(
"-I
${
PYTHON_INCLUDE_DIRS
}
"
)
...
...
python/test_unit.py
View file @
cf46bbfe
import
numpy
as
np
import
lib
eigenpy
import
matrix
as
eigenpy
verbose
=
False
if
verbose
:
print
"===> From MatrixXd to Py"
M
=
lib
eigenpy
.
naturals
(
3
,
3
,
verbose
)
M
=
eigenpy
.
naturals
(
3
,
3
,
verbose
)
Mcheck
=
np
.
reshape
(
np
.
array
(
range
(
9
),
np
.
double
),[
3
,
3
])
assert
np
.
array_equal
(
Mcheck
,
M
)
if
verbose
:
print
"===> From Matrix3d to Py"
M33
=
lib
eigenpy
.
naturals33
(
verbose
)
M33
=
eigenpy
.
naturals33
(
verbose
)
assert
np
.
array_equal
(
Mcheck
,
M33
)
if
verbose
:
print
"===> From VectorXd to Py"
v
=
lib
eigenpy
.
naturalsX
(
3
,
verbose
)
v
=
eigenpy
.
naturalsX
(
3
,
verbose
)
vcheck
=
np
.
array
([
range
(
3
),],
np
.
double
).
T
assert
np
.
array_equal
(
vcheck
,
v
)
...
...
@@ -24,43 +24,43 @@ Mref = np.reshape(np.array(range(64),np.double),[8,8])
if
verbose
:
print
"===> Matrix 8x8"
M
=
Mref
assert
(
np
.
array_equal
(
M
,
lib
eigenpy
.
reflex
(
M
,
verbose
))
);
assert
(
np
.
array_equal
(
M
,
eigenpy
.
reflex
(
M
,
verbose
))
);
if
verbose
:
print
"===> Block 0:3x0:3"
M
=
Mref
[
0
:
3
,
0
:
3
]
assert
(
np
.
array_equal
(
M
,
lib
eigenpy
.
reflex
(
M
,
verbose
))
);
assert
(
np
.
array_equal
(
M
,
eigenpy
.
reflex
(
M
,
verbose
))
);
if
verbose
:
print
"===> Block 1:3x1:3"
M
=
Mref
[
1
:
3
,
1
:
3
]
assert
(
np
.
array_equal
(
M
,
lib
eigenpy
.
reflex
(
M
,
verbose
))
);
assert
(
np
.
array_equal
(
M
,
eigenpy
.
reflex
(
M
,
verbose
))
);
if
verbose
:
print
"===> Block 1:5:2x1:5:2"
M
=
Mref
[
1
:
5
:
2
,
1
:
5
:
2
]
assert
(
np
.
array_equal
(
M
,
lib
eigenpy
.
reflex
(
M
,
verbose
))
);
assert
(
np
.
array_equal
(
M
,
eigenpy
.
reflex
(
M
,
verbose
))
);
if
verbose
:
print
"===> Block 1:8:3x1:5"
M
=
Mref
[
1
:
8
:
3
,
1
:
5
]
assert
(
np
.
array_equal
(
M
,
lib
eigenpy
.
reflex
(
M
,
verbose
))
);
assert
(
np
.
array_equal
(
M
,
eigenpy
.
reflex
(
M
,
verbose
))
);
if
verbose
:
print
"===> Block transpose 1:8:3x1:6:2"
M
=
Mref
[
1
:
8
:
3
,
0
:
6
:
2
].
T
assert
(
np
.
array_equal
(
M
,
lib
eigenpy
.
reflex
(
M
,
verbose
))
);
assert
(
np
.
array_equal
(
M
,
eigenpy
.
reflex
(
M
,
verbose
))
);
if
verbose
:
print
"===> Block Vector 1x0:6:2"
M
=
Mref
[
1
:
2
,
0
:
6
:
2
]
assert
(
np
.
array_equal
(
M
,
lib
eigenpy
.
reflex
(
M
,
verbose
))
);
assert
(
np
.
array_equal
(
M
,
eigenpy
.
reflex
(
M
,
verbose
))
);
if
verbose
:
print
"===> Block Vector 1x0:6:2 tanspose"
M
=
Mref
[
1
:
2
,
0
:
6
:
2
].
T
assert
(
np
.
array_equal
(
M
,
lib
eigenpy
.
reflex
(
M
,
verbose
))
);
assert
(
np
.
array_equal
(
M
,
eigenpy
.
reflex
(
M
,
verbose
))
);
if
verbose
:
print
"===> Block Vector 0:6:2x1"
M
=
Mref
[
0
:
6
:
2
,
1
:
2
]
assert
(
np
.
array_equal
(
M
,
lib
eigenpy
.
reflex
(
M
,
verbose
))
);
assert
(
np
.
array_equal
(
M
,
eigenpy
.
reflex
(
M
,
verbose
))
);
if
verbose
:
print
"===> Block Vector 0:6:2x1 tanspose"
M
=
Mref
[
0
:
6
:
2
,
1
:
2
].
T
assert
(
np
.
array_equal
(
M
,
lib
eigenpy
.
reflex
(
M
,
verbose
))
);
assert
(
np
.
array_equal
(
M
,
eigenpy
.
reflex
(
M
,
verbose
))
);
if
verbose
:
print
"===> From Py to Eigen::VectorXd"
if
verbose
:
print
"===> From Py to Eigen::VectorXd"
...
...
@@ -68,15 +68,15 @@ if verbose: print "===> From Py to Eigen::VectorXd"
if
verbose
:
print
"===> Block Vector 0:6:2x1 1 dim"
M
=
Mref
[
0
:
6
:
2
,
1
].
T
assert
(
np
.
array_equal
(
np
.
array
([
M
,]).
T
,
lib
eigenpy
.
reflexV
(
M
,
verbose
))
);
assert
(
np
.
array_equal
(
np
.
array
([
M
,]).
T
,
eigenpy
.
reflexV
(
M
,
verbose
))
);
if
verbose
:
print
"===> Block Vector 0:6:2x1"
M
=
Mref
[
0
:
6
:
2
,
1
:
2
]
assert
(
np
.
array_equal
(
M
,
lib
eigenpy
.
reflexV
(
M
,
verbose
))
);
assert
(
np
.
array_equal
(
M
,
eigenpy
.
reflexV
(
M
,
verbose
))
);
if
verbose
:
print
"===> Block Vector 0:6:2x1 transpose"
M
=
Mref
[
0
:
6
:
2
,
1
:
2
].
T
assert
(
np
.
array_equal
(
M
.
T
,
lib
eigenpy
.
reflexV
(
M
,
verbose
))
);
assert
(
np
.
array_equal
(
M
.
T
,
eigenpy
.
reflexV
(
M
,
verbose
))
);
if
verbose
:
print
"===> From Py to Eigen::Matrix3d"
if
verbose
:
print
"===> From Py to Eigen::Matrix3d"
...
...
@@ -84,13 +84,13 @@ if verbose: print "===> From Py to Eigen::Matrix3d"
if
verbose
:
print
"===> Block Vector 0:3x0:6:2 "
M
=
Mref
[
0
:
3
,
0
:
6
:
2
]
assert
(
np
.
array_equal
(
M
,
lib
eigenpy
.
reflex33
(
M
,
verbose
))
);
assert
(
np
.
array_equal
(
M
,
eigenpy
.
reflex33
(
M
,
verbose
))
);
if
verbose
:
print
"===> Block Vector 0:3x0:6:2 T"
M
=
Mref
[
0
:
3
,
0
:
6
].
T
try
:
assert
(
np
.
array_equal
(
M
,
lib
eigenpy
.
reflex33
(
M
,
verbose
))
);
except
lib
eigenpy
.
e
xception
,
e
:
assert
(
np
.
array_equal
(
M
,
eigenpy
.
reflex33
(
M
,
verbose
))
);
except
eigenpy
.
E
xception
,
e
:
if
verbose
:
print
"As expected, got the following /ROW/ error:"
,
e
.
message
if
verbose
:
print
"===> From Py to Eigen::Vector3d"
...
...
@@ -98,6 +98,6 @@ if verbose: print "===> From Py to Eigen::Vector3d"
if
verbose
:
print
"===> From Py to Eigen::Vector3d"
M
=
Mref
[
0
:
3
,
1
:
2
]
assert
(
np
.
array_equal
(
M
,
lib
eigenpy
.
reflex3
(
M
,
verbose
))
);
assert
(
np
.
array_equal
(
M
,
eigenpy
.
reflex3
(
M
,
verbose
))
);
python/tgeometry.py
0 → 100644
View file @
cf46bbfe
from
geometry
import
*
import
numpy
as
np
from
numpy
import
cos
,
sin
verbose
=
True
def
isapprox
(
a
,
b
,
epsilon
=
1e-6
):
if
a
.
__class__
==
b
.
__class__
==
np
.
ndarray
:
return
np
.
allclose
(
a
,
b
,
epsilon
)
else
:
return
abs
(
a
-
b
)
<
epsilon
# --- Quaternion ---------------------------------------------------------------
q
=
Quaternion
(
1
,
2
,
3
,
4
)
q
.
normalize
()
assert
(
isapprox
(
np
.
linalg
.
norm
(
q
.
coeffs
()),
q
.
norm
()))
assert
(
isapprox
(
np
.
linalg
.
norm
(
q
.
coeffs
()),
1
))
r
=
AngleAxis
(
q
)
q2
=
Quaternion
(
r
)
assert
(
q
==
q2
)
assert
(
isapprox
(
q
.
coeffs
(),
q2
.
coeffs
()))
Rq
=
q
.
matrix
()
Rr
=
r
.
matrix
()
assert
(
isapprox
(
np
.
dot
(
Rq
,
Rq
.
T
),
np
.
eye
(
3
)))
assert
(
isapprox
(
Rr
,
Rq
))
qR
=
Quaternion
(
Rr
)
assert
(
q
==
qR
)
assert
(
isapprox
(
q
.
coeffs
(),
qR
.
coeffs
()))
assert
(
isapprox
(
qR
[
3
],
1.
/
np
.
sqrt
(
30
)))
try
:
qR
[
5
]
print
"Error, this message should not appear."
except
Exception
,
e
:
if
verbose
:
print
"As expected, catched exception: "
,
e
.
message
# --- Angle Vector ------------------------------------------------
r
=
AngleAxis
(.
1
,
np
.
array
([
1
,
0
,
0
],
np
.
double
))
if
verbose
:
print
"Rx(.1) =
\n\n
"
,
r
.
matrix
(),
"
\n
"
assert
(
isapprox
(
r
.
matrix
()[
2
,
2
],
cos
(
r
.
angle
)))
assert
(
isapprox
(
r
.
axis
,
np
.
array
([[
1
,
0
,
0
],],
np
.
double
).
T
)
)
assert
(
isapprox
(
r
.
angle
,
0.1
)
)
r
.
axis
=
np
.
array
([
0
,
1
,
0
],
np
.
double
)
assert
(
isapprox
(
r
.
matrix
()[
0
,
0
],
cos
(
r
.
angle
)))
ri
=
r
.
inverse
()
assert
(
isapprox
(
ri
.
angle
,
-
.
1
)
)
R
=
r
.
matrix
()
r2
=
AngleAxis
(
np
.
dot
(
R
,
R
))
assert
(
isapprox
(
r2
.
angle
,
r
.
angle
*
2
)
)
# --- USER FUNCTIONS -----------------------------------------------------------
ro
=
testOutAngleAxis
()
assert
(
ro
.
__class__
==
AngleAxis
)
res
=
testInAngleAxis
(
r
)
assert
(
res
==
r
.
angle
)
qo
=
testOutQuaternion
()
assert
(
qo
.
__class__
==
Quaternion
)
res
=
testInQuaternion_fx
(
q
)
assert
(
q
.
norm
()
==
res
)
try
:
testInQuaternion
(
q
)
print
"Error, this message should not appear."
except
:
if
verbose
:
print
"As expected, catch a Boost::python::ArgError exception."
src/angle-axis.cpp
0 → 100644
View file @
cf46bbfe
#include
"eigenpy/angle-axis.hpp"
#include
"eigenpy/geometry.hpp"
namespace
eigenpy
{
void
exposeAngleAxis
()
{
AngleAxisVisitor
<
Eigen
::
AngleAxisd
>::
expose
();
}
}
// namespace eigenpy
src/angle-axis.hpp
0 → 100644
View file @
cf46bbfe
#ifndef __eigenpy_Angle_Axis_hpp__
#define __eigenpy_Angle_Axis_hpp__
#include
<Eigen/Core>
#include
<Eigen/Geometry>
#include
<boost/python.hpp>
#include
"eigenpy/simple.hpp"
namespace
eigenpy
{
template
<
>
struct
UnalignedEquivalent
<
Eigen
::
AngleAxisd
>
{
typedef
Eigen
::
AngleAxis
<
double
>
type
;
};
namespace
bp
=
boost
::
python
;
template
<
typename
D
>
class
AngleAxisVisitor
:
public
boost
::
python
::
def_visitor
<
AngleAxisVisitor
<
D
>
>
{
typedef
D
AngleAxis
;
typedef
typename
eigenpy
::
UnalignedEquivalent
<
D
>::
type
AngleAxisUnaligned
;
typedef
typename
AngleAxisUnaligned
::
Scalar
Scalar
;
typedef
typename
eigenpy
::
UnalignedEquivalent
<
typename
AngleAxisUnaligned
::
Vector3
>::
type
Vector3
;
typedef
typename
eigenpy
::
UnalignedEquivalent
<
typename
AngleAxisUnaligned
::
Matrix3
>::
type
Matrix3
;
typedef
eigenpy
::
UnalignedEquivalent
<
Eigen
::
Quaternion
<
Scalar
>
>
Quaternion
;
public:
template
<
class
PyClass
>
void
visit
(
PyClass
&
cl
)
const
{
cl
.
def
(
bp
::
init
<
Scalar
,
Vector3
>
((
bp
::
arg
(
"angle"
),
bp
::
arg
(
"axis"
)),
"Initialize from angle and axis"
))
.
def
(
bp
::
init
<
Matrix3
>
((
bp
::
arg
(
"rotationMatrix"
)),
"Initialize from a rotation matrix"
))
.
def
(
"__init__"
,
bp
::
make_constructor
(
&
AngleAxisVisitor
::
constructFromQuaternion
,
bp
::
default_call_policies
(),
(
bp
::
arg
(
"quaternion"
))),
"Initialize from quaternion"
)
.
def
(
bp
::
init
<
AngleAxisUnaligned
>
((
bp
::
arg
(
"clone"
))))
.
def
(
"matrix"
,
&
AngleAxisUnaligned
::
toRotationMatrix
,
"Return the corresponding rotation matrix 3x3."
)
.
def
(
"vector"
,
&
AngleAxisVisitor
::
toVector3
,
"Return the correspond angle*axis vector3"
)
.
add_property
(
"axis"
,
&
AngleAxisVisitor
::
getAxis
,
&
AngleAxisVisitor
::
setAxis
)
.
add_property
(
"angle"
,
&
AngleAxisVisitor
::
getAngle
,
&
AngleAxisVisitor
::
setAngle
)
/* --- Methods --- */
.
def
(
"normalize"
,
&
AngleAxisVisitor
::
normalize
,
"Normalize the axis vector (without changing the angle)."
)
.
def
(
"inverse"
,
&
AngleAxisUnaligned
::
inverse
,
"Return the inverse rotation."
)
.
def
(
"apply"
,
&
AngleAxisVisitor
::
apply
,(
bp
::
arg
(
"vector3"
)),
"Apply the rotation map to the vector"
)
/* --- Operators --- */
.
def
(
bp
::
self
*
bp
::
other
<
Vector3
>
())
.
def
(
"__eq__"
,
&
AngleAxisVisitor
::
__eq__
)
.
def
(
"__ne__"
,
&
AngleAxisVisitor
::
__ne__
)
.
def
(
"__abs__"
,
&
AngleAxisVisitor
::
getAngleAbs
)
;
}
private:
static
AngleAxisUnaligned
*
constructFromQuaternion
(
const
Eigen
::
Quaternion
<
Scalar
,
Eigen
::
DontAlign
>
&
qu
)
{
Eigen
::
Quaternion
<
Scalar
>
q
=
qu
;
return
new
AngleAxisUnaligned
(
q
);
}
static
Vector3
apply
(
const
AngleAxisUnaligned
&
r
,
const
Vector3
&
v
)
{
return
r
*
v
;
}
static
Vector3
getAxis
(
const
AngleAxisUnaligned
&
self
)
{
return
self
.
axis
();
}
static
void
setAxis
(
AngleAxisUnaligned
&
self
,
const
Vector3
&
r
)
{
self
=
AngleAxisUnaligned
(
self
.
angle
(),
r
);
}
static
double
getAngle
(
const
AngleAxisUnaligned
&
self
)
{
return
self
.
angle
();
}
static
void
setAngle
(
AngleAxisUnaligned
&
self
,
const
double
&
th
)
{
self
=
AngleAxisUnaligned
(
th
,
self
.
axis
()
);
}
static
double
getAngleAbs
(
const
AngleAxisUnaligned
&
self
)
{
return
std
::
abs
(
self
.
angle
());
}
static
bool
__eq__
(
const
AngleAxisUnaligned
&
u
,
const
AngleAxisUnaligned
&
v
)
{
return
u
.
isApprox
(
v
);
}
static
bool
__ne__
(
const
AngleAxisUnaligned
&
u
,
const
AngleAxisUnaligned
&
v
)
{
return
!
__eq__
(
u
,
v
);
}
static
Vector3
toVector3
(
const
AngleAxisUnaligned
&
self
)
{
return
self
.
axis
()
*
self
.
angle
();
}
static
void
normalize
(
AngleAxisUnaligned
&
self
)
{
setAxis
(
self
,
self
.
axis
()
/
self
.
axis
().
norm
());
}
private:
static
PyObject
*
convert
(
AngleAxis
const
&
q
)
{
AngleAxisUnaligned
qx
=
q
;
return
boost
::
python
::
incref
(
boost
::
python
::
object
(
qx
).
ptr
());
}
public:
static
void
expose
()
{
bp
::
class_
<
AngleAxisUnaligned
>
(
"AngleAxis"
,
"AngleAxis representation of rotations.
\n\n
"
,
bp
::
init
<>
())
.
def
(
AngleAxisVisitor
<
D
>
());
// TODO: check the problem of fix-size Angle Axis.
//bp::to_python_converter< AngleAxis,AngleAxisVisitor<D> >();
}
};
}
// namespace eigenpy
#endif //ifndef __eigenpy_Angle_Axis_hpp__
src/eigenpy.hpp
View file @
cf46bbfe
...
...
@@ -18,60 +18,17 @@
#include
<boost/python.hpp>
#include
<numpy/arrayobject.h>
#include
<iostream>
#include
<eigenpy/exception.hpp>
#include
<eigenpy/simple.hpp>
#include
<eigenpy/map.hpp>
namespace
eigenpy
{
template
<
typename
MatType
,
int
IsVector
>
struct
MapNumpyTraits
{};
/* Wrap a numpy::array with an Eigen::Map. No memory copy. */
template
<
typename
MatType
>
struct
MapNumpy
{
typedef
MapNumpyTraits
<
MatType
,
MatType
::
IsVectorAtCompileTime
>
Impl
;
typedef
typename
Impl
::
EigenMap
EigenMap
;
static
inline
EigenMap
map
(
PyArrayObject
*
pyArray
);
};
/* Eigenpy exception. They can be catch with python (equivalent eigenpy.exception class). */
class
exception
:
public
std
::
exception
{
public:
exception
(
std
::
string
msg
)
:
message
(
msg
)
{}
const
char
*
what
()
const
throw
()
{
return
this
->
message
.
c_str
();
}
~
exception
()
throw
()
{}
std
::
string
getMessage
()
{
return
message
;
}
static
void
registerException
();
private:
static
void
translateException
(
exception
const
&
e
);
static
PyObject
*
pyType
;
std
::
string
message
;
};
/* Enable the Eigen--Numpy serialization for the templated MatrixBase class. */
template
<
typename
MatType
,
typename
EigenEquivalentType
>
void
enableEigenPySpecific
();
/* Enable Eigen-Numpy serialization for a set of standard MatrixBase instance. */
void
enableEigenPy
()
{
exception
::
registerException
();
enableEigenPySpecific
<
Eigen
::
MatrixXd
,
Eigen
::
MatrixXd
>
();
enableEigenPySpecific
<
Eigen
::
Matrix2d
,
Eigen
::
Matrix2d
>
();
enableEigenPySpecific
<
Eigen
::
Matrix3d
,
Eigen
::
Matrix3d
>
();
enableEigenPySpecific
<
Eigen
::
Matrix4d
,
Eigen
::
Matrix4d
>
();
enableEigenPySpecific
<
Eigen
::
VectorXd
,
Eigen
::
VectorXd
>
();
enableEigenPySpecific
<
Eigen
::
Vector2d
,
Eigen
::
Vector2d
>
();
enableEigenPySpecific
<
Eigen
::
Vector3d
,
Eigen
::
Vector3d
>
();
enableEigenPySpecific
<
Eigen
::
Vector4d
,
Eigen
::
Vector4d
>
();
}
}
/* --- DETAILS ------------------------------------------------------------------ */
...
...
@@ -85,69 +42,6 @@ namespace eigenpy
template
<
>
struct
NumpyEquivalentType
<
int
>
{
enum
{
type_code
=
NPY_INT
};};
template
<
>
struct
NumpyEquivalentType
<
float
>
{
enum
{
type_code
=
NPY_FLOAT
};};
/* --- MAP ON NUMPY ----------------------------------------------------------- */
template
<
typename
MatType
>
struct
MapNumpyTraits
<
MatType
,
0
>
{
typedef
Eigen
::
Stride
<
Eigen
::
Dynamic
,
Eigen
::
Dynamic
>
Stride
;
typedef
Eigen
::
Map
<
MatType
,
0
,
Stride
>
EigenMap
;
typedef
typename
MatType
::
Scalar
T
;
static
EigenMap
mapImpl
(
PyArrayObject
*
pyArray
)
{
assert
(
PyArray_NDIM
(
pyArray
)
==
2
);
const
int
R
=
PyArray_DIMS
(
pyArray
)[
0
];
const
int
C
=
PyArray_DIMS
(
pyArray
)[
1
];
const
int
itemsize
=
PyArray_ITEMSIZE
(
pyArray
);
const
int
stride1
=
PyArray_STRIDE
(
pyArray
,
0
)
/
itemsize
;
const
int
stride2
=
PyArray_STRIDE
(
pyArray
,
1
)
/
itemsize
;
if
(
(
MatType
::
RowsAtCompileTime
!=
R
)
&&
(
MatType
::
RowsAtCompileTime
!=
Eigen
::
Dynamic
)
)
{
throw
eigenpy
::
exception
(
"The number of rows does not fit with the matrix type."
);
}
if
(
(
MatType
::
ColsAtCompileTime
!=
C
)
&&
(
MatType
::
ColsAtCompileTime
!=
Eigen
::
Dynamic
)
)
{
throw
eigenpy
::
exception
(
"The number of columns does not fit with the matrix type."
);
}
T
*
pyData
=
reinterpret_cast
<
T
*>
(
PyArray_DATA
(
pyArray
));
return
EigenMap
(
pyData
,
R
,
C
,
Stride
(
stride2
,
stride1
)
);
}
};
template
<
typename
MatType
>
struct
MapNumpyTraits
<
MatType
,
1
>
{
typedef
Eigen
::
InnerStride
<
Eigen
::
Dynamic
>
Stride
;
typedef
Eigen
::
Map
<
MatType
,
0
,
Stride
>
EigenMap
;
typedef
typename
MatType
::
Scalar
T
;
static
EigenMap
mapImpl
(
PyArrayObject
*
pyArray
)
{
assert
(
PyArray_NDIM
(
pyArray
)
<=
2
);
int
rowMajor
;
if
(
PyArray_NDIM
(
pyArray
)
==
1
)
rowMajor
=
0
;
else
rowMajor
=
(
PyArray_DIMS
(
pyArray
)[
0
]
>
PyArray_DIMS
(
pyArray
)[
1
])
?
0
:
1
;
const
int
R
=
PyArray_DIMS
(
pyArray
)[
rowMajor
];
const
int
itemsize
=
PyArray_ITEMSIZE
(
pyArray
);
const
int
stride
=
PyArray_STRIDE
(
pyArray
,
rowMajor
)
/
itemsize
;;
if
(
(
MatType
::
MaxSizeAtCompileTime
!=
R
)
&&
(
MatType
::
MaxSizeAtCompileTime
!=
Eigen
::
Dynamic
)
)
{
throw
eigenpy
::
exception
(
"The number of elements does not fit with the vector type."
);
}
T
*
pyData
=
reinterpret_cast
<
T
*>
(
PyArray_DATA
(
pyArray
));
return
EigenMap
(
pyData
,
R
,
1
,
Stride
(
stride
)
);
}
};
template
<
typename
MatType
>
typename
MapNumpy
<
MatType
>::
EigenMap
MapNumpy
<
MatType
>::
map
(
PyArrayObject
*
pyArray
)
{
return
Impl
::
mapImpl
(
pyArray
);
}
/* --- TO PYTHON -------------------------------------------------------------- */
template
<
typename
MatType
,
typename
EquivalentEigenType
>
...
...
@@ -174,7 +68,7 @@ namespace eigenpy
template
<
typename
MatType
,
int
ROWS
,
int
COLS
>
struct
TraitsMatrixConstructor
{
static
MatType
&
construct
(
void
*
storage
,
int
r
,
int
c
)
static
MatType
&
construct
(
void
*
storage
,
int
/*r*/
,
int
/*c*/
)
{
return
*
new
(
storage
)
MatType
();
}
...
...
@@ -192,7 +86,7 @@ namespace eigenpy
template
<
typename
MatType
,
int
R
>
struct
TraitsMatrixConstructor
<
MatType
,
R
,
Eigen
::
Dynamic
>
{
static
MatType
&
construct
(
void
*
storage
,
int
r
,
int
c
)
static
MatType
&
construct
(
void
*
storage
,
int
/*r*/
,
int
c
)
{
return
*
new
(
storage
)
MatType
(
c
);
}
...
...
@@ -201,7 +95,7 @@ namespace eigenpy
template
<
typename
MatType
,
int
C
>
struct
TraitsMatrixConstructor
<
MatType
,
Eigen
::
Dynamic
,
C
>
{
static
MatType
&
construct
(
void
*
storage
,
int
r
,
int
c
)
static
MatType
&
construct
(
void
*
storage
,
int
r
,
int
/*c*/
)
{
return
*
new
(
storage
)
MatType
(
r
);
}
...
...
@@ -235,7 +129,7 @@ namespace eigenpy
return
0
;
}
if
(
PyArray_ObjectType
(
obj_ptr
,
0
)
!=
NumpyEquivalentType
<
T
>::
type_code
)
if
(
(
PyArray_ObjectType
(
obj_ptr
,
0
)
)
!=
NumpyEquivalentType
<
T
>::
type_code
)
{
std
::
cerr
<<
"The internal type as no Eigen equivalent."
<<
std
::
endl
;
return
0
;
...
...
@@ -275,30 +169,22 @@ namespace eigenpy
void
enableEigenPySpecific
()
{
import_array
();
boost
::
python
::
to_python_converter
<
MatType
,
eigenpy
::
EigenToPy
<
MatType
,
EigenEquivalentType
>
>
();
eigenpy
::
EigenFromPy
<
MatType
,
EigenEquivalentType
>
();
}
/* --- EXCEPTION ----------------------------------------------------------------- */
PyObject
*
exception
::
pyType
;
#ifdef EIGEN_DONT_VECTORIZE
boost
::
python
::
to_python_converter
<
MatType
,
eigenpy
::
EigenToPy
<
MatType
,
MatType
>
>
();
eigenpy
::
EigenFromPy
<
MatType
,
MatType
>
();
#else
typedef
typename
eigenpy
::
UnalignedEquivalent
<
MatType
>::
type
MatTypeDontAlign
;
boost
::
python
::
to_python_converter
<
MatType
,
eigenpy
::
EigenToPy
<
MatType
,
MatType
>
>
();
boost
::
python
::
to_python_converter
<
MatTypeDontAlign
,
eigenpy
::
EigenToPy
<
MatTypeDontAlign
,
MatTypeDontAlign
>
>
();
eigenpy
::
EigenFromPy
<
MatTypeDontAlign
,
MatTypeDontAlign
>
();
#endif
void
exception
::
translateException
(
exception
const
&
e
)
{
assert
(
NULL
!=
pyType
);
// Return an exception object of type pyType and value object(e).
PyErr_SetObject
(
pyType
,
boost
::
python
::
object
(
e
).
ptr
());
}
void
exception
::
registerException
()
{
pyType
=
boost
::
python
::
class_
<
eigenpy
::
exception
>
(
"exception"
,
boost
::
python
::
init
<
std
::
string
>
())
.
add_property
(
"message"
,
&
eigenpy
::
exception
::
getMessage
)
.
ptr
();
boost
::
python
::
register_exception_translator
<
eigenpy
::
exception
>