Commit d4b13427 authored by Maximilien Naveau's avatar Maximilien Naveau
Browse files

cleaning and renaming a bunch of operator in order to get consistent naming.

parent e0849421
......@@ -11,10 +11,10 @@ from .operator import (
HomoToTwist, Inverse_of_matrix, Inverse_of_matrixHomo,
Inverse_of_matrixrotation, Inverse_of_matrixtwist, Inverse_of_unitquat,
MatrixDiagonal, MatrixHomoToPose, MatrixHomoToPoseQuaternion,
PoseQuatToMatrixHomo, MatrixHomoToPoseRollPitchYaw, MatrixHomoToPoseUTheta,
MatrixToHomo, MatrixToQuaternion, MatrixToRPY, MatrixToUTheta,
MatrixHomoToSE3Vector, SE3VectorToMatrixHomo, MatrixTranspose,
Multiply_double_vector, Multiply_matrix_vector,
PoseQuaternionToMatrixHomo, MatrixHomoToPoseRollPitchYaw,
MatrixHomoToPoseUTheta, MatrixToHomo, MatrixToQuaternion, MatrixToRPY,
MatrixToUTheta, MatrixHomoToSE3Vector, SE3VectorToMatrixHomo,
MatrixTranspose, Multiply_double_vector, Multiply_matrix_vector,
Multiply_matrixTwist_vector, Multiply_matrixHomo_vector,
Multiply_of_double, Multiply_of_matrix, Multiply_of_matrixHomo,
Multiply_of_matrixrotation, Multiply_of_matrixtwist,
......
......@@ -4,7 +4,7 @@ from dynamic_graph.sot.core.gain_adaptive import GainAdaptive
from dynamic_graph.sot.core.matrix_util import matrixToTuple, vectorToTuple
from dynamic_graph.sot.core.meta_task_6d import toFlags # kept for backward compatibility
from dynamic_graph.sot.core.meta_tasks import setGain
from dynamic_graph.sot.core.sot import Task
from dynamic_graph.sot.core.task import Task
from numpy import identity, matrix, zeros
......
......@@ -3,7 +3,7 @@ from dynamic_graph.sot.core.feature_visual_point import FeatureVisualPoint
from dynamic_graph.sot.core.gain_adaptive import GainAdaptive
from dynamic_graph.sot.core.meta_tasks import setGain
from dynamic_graph.sot.core.op_point_modifier import OpPointModifier
from dynamic_graph.sot.core.sot import Task
from dynamic_graph.sot.core.task import Task
from dynamic_graph.sot.core.visual_point_projecter import VisualPointProjecter
......
......@@ -99,12 +99,11 @@ BOOST_PYTHON_MODULE(wrap)
/* --- SE3/SO3 conversions ----------------------------------------------- */
/* ----------------------------------------------------------------------- */
exposeUnaryOp < HomogeneousMatrixToVector> ();
exposeUnaryOp < SkewSymToVector> ();
exposeUnaryOp < PoseUThetaToMatrixHomo> ();
exposeUnaryOp < HomogeneousMatrixToVector> ();
exposeUnaryOp < HomogeneousMatrixToSE3Vector> ();
exposeUnaryOp < SE3VectorToHomogeneousMatrix> ();
exposeUnaryOp < MatrixHomoToPoseUTheta> ();
exposeUnaryOp < MatrixHomoToSE3Vector> ();
exposeUnaryOp < SE3VectorToMatrixHomo> ();
exposeUnaryOp < PoseQuaternionToMatrixHomo> ();
exposeUnaryOp < MatrixHomoToPoseQuaternion> ();
exposeUnaryOp < MatrixHomoToPoseRollPitchYaw> ();
......
......@@ -57,13 +57,13 @@ REGISTER_UNARY_OP(InverserQuaternion, Inverse_of_unitquat);
/* --- SE3/SO3 conversions ----------------------------------------------- */
/* ----------------------------------------------------------------------- */
REGISTER_UNARY_OP(HomogeneousMatrixToVector, MatrixHomoToPoseUTheta);
REGISTER_UNARY_OP(HomogeneousMatrixToSE3Vector, MatrixHomoToSE3Vector);
REGISTER_UNARY_OP(SE3VectorToHomogeneousMatrix, SE3VectorToMatrixHomo);
REGISTER_UNARY_OP(MatrixHomoToPoseUTheta, MatrixHomoToPoseUTheta);
REGISTER_UNARY_OP(MatrixHomoToSE3Vector, MatrixHomoToSE3Vector);
REGISTER_UNARY_OP(SE3VectorToMatrixHomo, SE3VectorToMatrixHomo);
REGISTER_UNARY_OP(SkewSymToVector, SkewSymToVector);
REGISTER_UNARY_OP(PoseUThetaToMatrixHomo, PoseUThetaToMatrixHomo);
REGISTER_UNARY_OP(MatrixHomoToPoseQuaternion, MatrixHomoToPoseQuaternion);
REGISTER_UNARY_OP(PoseQuaternionToMatrixHomo, PoseQuatToMatrixHomo);
REGISTER_UNARY_OP(PoseQuaternionToMatrixHomo, PoseQuaternionToMatrixHomo);
REGISTER_UNARY_OP(MatrixHomoToPoseRollPitchYaw, MatrixHomoToPoseRollPitchYaw);
REGISTER_UNARY_OP(PoseRollPitchYawToMatrixHomo, PoseRollPitchYawToMatrixHomo);
REGISTER_UNARY_OP(PoseRollPitchYawToPoseUTheta, PoseRollPitchYawToPoseUTheta);
......
......@@ -278,7 +278,7 @@ struct InverserQuaternion
/* --- SE3/SO3 conversions ----------------------------------------------- */
/* ----------------------------------------------------------------------- */
struct HomogeneousMatrixToVector
struct MatrixHomoToPoseUTheta
: public UnaryOpHeader<MatrixHomogeneous, dg::Vector> {
inline void operator()(const MatrixHomogeneous &M, dg::Vector &res) {
res.resize(6);
......@@ -310,7 +310,7 @@ struct PoseUThetaToMatrixHomo
}
};
struct SE3VectorToHomogeneousMatrix
struct SE3VectorToMatrixHomo
: public UnaryOpHeader<dg::Vector, MatrixHomogeneous> {
void operator()(const dg::Vector &vect, MatrixHomogeneous &Mres) {
Mres.translation() = vect.head<3>();
......@@ -320,7 +320,7 @@ struct SE3VectorToHomogeneousMatrix
}
};
struct HomogeneousMatrixToSE3Vector
struct MatrixHomoToSE3Vector
: public UnaryOpHeader<MatrixHomogeneous, dg::Vector> {
void operator()(const MatrixHomogeneous &M, dg::Vector &res) {
res.resize(12);
......
......@@ -199,7 +199,7 @@ BOOST_AUTO_TEST_CASE(matrixHomo_poseQuaternion) {
test_impl<MatrixHomoToPoseQuaternion, PoseQuaternionToMatrixHomo>();
}
BOOST_AUTO_TEST_CASE(matrixHomo_se3Vector) {
test_impl<HomogeneousMatrixToSE3Vector, SE3VectorToHomogeneousMatrix>();
test_impl<MatrixHomoToSE3Vector, SE3VectorToMatrixHomo>();
}
BOOST_AUTO_TEST_SUITE_END()
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