Commit e6a1c65d authored by Maximilien Naveau's avatar 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
......@@ -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) \
......
......@@ -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> ();
......
......@@ -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);
......
......@@ -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) {
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment