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
sot-core
Commits
e6a1c65d
Commit
e6a1c65d
authored
Nov 13, 2020
by
Maximilien Naveau
Browse files
[operators][fir_filter] fix some bug on the python bindings side
parent
7f5a93e5
Pipeline
#12159
passed with stage
in 57 minutes and 47 seconds
Changes
4
Pipelines
1
Hide whitespace changes
Inline
Side-by-side
src/matrix/fir-filter.cpp
View file @
e6a1c65d
...
...
@@ -23,7 +23,7 @@ using dynamicgraph::Vector;
\
template <> \
const std::string sotClassType<sotSigType, sotCoefType>::CLASS_NAME = \
std::string(className) + "_" + #sotSigType + "
,
" + #sotCoefType
+ "_";
\
std::string(className) + "_" + #sotSigType + "
_
" + #sotCoefType
;
\
\
template <> \
const std::string &sotClassType<sotSigType, sotCoefType>::getClassName(void) \
...
...
src/matrix/operator-python-module-py.cc
View file @
e6a1c65d
...
...
@@ -102,6 +102,10 @@ BOOST_PYTHON_MODULE(wrap)
exposeUnaryOp
<
HomogeneousMatrixToVector
>
();
exposeUnaryOp
<
SkewSymToVector
>
();
exposeUnaryOp
<
PoseUThetaToMatrixHomo
>
();
exposeUnaryOp
<
HomogeneousMatrixToVector
>
();
exposeUnaryOp
<
HomogeneousMatrixToSE3Vector
>
();
exposeUnaryOp
<
SE3VectorToHomogeneousMatrix
>
();
exposeUnaryOp
<
PoseQuaternionToMatrixHomo
>
();
exposeUnaryOp
<
MatrixHomoToPoseQuaternion
>
();
exposeUnaryOp
<
MatrixHomoToPoseRollPitchYaw
>
();
exposeUnaryOp
<
PoseRollPitchYawToMatrixHomo
>
();
...
...
src/matrix/operator.cpp
View file @
e6a1c65d
...
...
@@ -58,40 +58,11 @@ REGISTER_UNARY_OP(InverserQuaternion, Inverse_of_unitquat);
/* ----------------------------------------------------------------------- */
REGISTER_UNARY_OP
(
HomogeneousMatrixToVector
,
MatrixHomoToPoseUTheta
);
struct
HomogeneousMatrixToSE3Vector
:
public
UnaryOpHeader
<
MatrixHomogeneous
,
dg
::
Vector
>
{
void
operator
()(
const
MatrixHomogeneous
&
M
,
dg
::
Vector
&
res
)
{
res
.
resize
(
12
);
res
.
head
<
3
>
()
=
M
.
translation
();
res
.
segment
(
3
,
3
)
=
M
.
linear
().
row
(
0
);
res
.
segment
(
6
,
3
)
=
M
.
linear
().
row
(
1
);
res
.
segment
(
9
,
3
)
=
M
.
linear
().
row
(
2
);
}
};
REGISTER_UNARY_OP
(
HomogeneousMatrixToSE3Vector
,
MatrixHomoToSE3Vector
);
struct
SE3VectorToHomogeneousMatrix
:
public
UnaryOpHeader
<
dg
::
Vector
,
MatrixHomogeneous
>
{
void
operator
()(
const
dg
::
Vector
&
vect
,
MatrixHomogeneous
&
Mres
)
{
Mres
.
translation
()
=
vect
.
head
<
3
>
();
Mres
.
linear
().
row
(
0
)
=
vect
.
segment
(
3
,
3
);
Mres
.
linear
().
row
(
1
)
=
vect
.
segment
(
6
,
3
);
Mres
.
linear
().
row
(
2
)
=
vect
.
segment
(
9
,
3
);
}
};
REGISTER_UNARY_OP
(
SE3VectorToHomogeneousMatrix
,
SE3VectorToMatrixHomo
);
REGISTER_UNARY_OP
(
SkewSymToVector
,
SkewSymToVector
);
REGISTER_UNARY_OP
(
PoseUThetaToMatrixHomo
,
PoseUThetaToMatrixHomo
);
REGISTER_UNARY_OP
(
MatrixHomoToPoseQuaternion
,
MatrixHomoToPoseQuaternion
);
struct
PoseQuaternionToMatrixHomo
:
public
UnaryOpHeader
<
Vector
,
MatrixHomogeneous
>
{
void
operator
()(
const
dg
::
Vector
&
vect
,
MatrixHomogeneous
&
Mres
)
{
Mres
.
translation
()
=
vect
.
head
<
3
>
();
Mres
.
linear
()
=
VectorQuaternion
(
vect
.
tail
<
4
>
()).
toRotationMatrix
();
}
};
REGISTER_UNARY_OP
(
PoseQuaternionToMatrixHomo
,
PoseQuatToMatrixHomo
);
REGISTER_UNARY_OP
(
MatrixHomoToPoseRollPitchYaw
,
MatrixHomoToPoseRollPitchYaw
);
REGISTER_UNARY_OP
(
PoseRollPitchYawToMatrixHomo
,
PoseRollPitchYawToMatrixHomo
);
...
...
src/matrix/operator.hh
View file @
e6a1c65d
...
...
@@ -310,6 +310,35 @@ struct PoseUThetaToMatrixHomo
}
};
struct
SE3VectorToHomogeneousMatrix
:
public
UnaryOpHeader
<
dg
::
Vector
,
MatrixHomogeneous
>
{
void
operator
()(
const
dg
::
Vector
&
vect
,
MatrixHomogeneous
&
Mres
)
{
Mres
.
translation
()
=
vect
.
head
<
3
>
();
Mres
.
linear
().
row
(
0
)
=
vect
.
segment
(
3
,
3
);
Mres
.
linear
().
row
(
1
)
=
vect
.
segment
(
6
,
3
);
Mres
.
linear
().
row
(
2
)
=
vect
.
segment
(
9
,
3
);
}
};
struct
HomogeneousMatrixToSE3Vector
:
public
UnaryOpHeader
<
MatrixHomogeneous
,
dg
::
Vector
>
{
void
operator
()(
const
MatrixHomogeneous
&
M
,
dg
::
Vector
&
res
)
{
res
.
resize
(
12
);
res
.
head
<
3
>
()
=
M
.
translation
();
res
.
segment
(
3
,
3
)
=
M
.
linear
().
row
(
0
);
res
.
segment
(
6
,
3
)
=
M
.
linear
().
row
(
1
);
res
.
segment
(
9
,
3
)
=
M
.
linear
().
row
(
2
);
}
};
struct
PoseQuaternionToMatrixHomo
:
public
UnaryOpHeader
<
Vector
,
MatrixHomogeneous
>
{
void
operator
()(
const
dg
::
Vector
&
vect
,
MatrixHomogeneous
&
Mres
)
{
Mres
.
translation
()
=
vect
.
head
<
3
>
();
Mres
.
linear
()
=
VectorQuaternion
(
vect
.
tail
<
4
>
()).
toRotationMatrix
();
}
};
struct
MatrixHomoToPoseQuaternion
:
public
UnaryOpHeader
<
MatrixHomogeneous
,
Vector
>
{
inline
void
operator
()(
const
MatrixHomogeneous
&
M
,
Vector
&
res
)
{
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment