Skip to content

Commit

Permalink
Update python wrapper and examples
Browse files Browse the repository at this point in the history
  • Loading branch information
@jlblancoc
jlblancoc committed Jun 22, 2023
1 parent 8ad25ee commit e8c375e
Show file tree
Hide file tree
Showing 9 changed files with 190 additions and 10 deletions.
32 changes: 32 additions & 0 deletions python-examples/ros-poses-convert.out
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,32 @@
mrpt p1 : (1.000,2.000,90.00deg)
ros p1 : geometry_msgs.msg.Pose(position=geometry_msgs.msg.Point(x=1.0, y=2.0, z=0.0), orientation=geometry_msgs.msg.Quaternion(x=0.0, y=0.0, z=0.7071067811865475, w=0.7071067811865476))
mrpt p2 : (x,y,z,yaw,pitch,roll)=(1.0000,2.0000,0.0000,90.00deg,-0.00deg,0.00deg)

ros r2 : geometry_msgs.msg.Pose(position=geometry_msgs.msg.Point(x=10.0, y=5.0, z=0.0), orientation=geometry_msgs.msg.Quaternion(x=0.0, y=0.0, z=0.0, w=1.0))
mrpt mr2 : (x,y,z,yaw,pitch,roll)=(10.0000,5.0000,0.0000,0.00deg,-0.00deg,0.00deg)
mrpt mr2b : (x,y,z,qr,qx,qy,qz)=(10.0000,5.0000,0.0000,1.0000,0.0000,0.0000,0.0000)

ros PDF r3 : geometry_msgs.msg.PoseWithCovariance(pose=geometry_msgs.msg.Pose(position=geometry_msgs.msg.Point(x=10.0, y=5.0, z=0.0), orientation=geometry_msgs.msg.Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)), covariance=array([1., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 1., 0., 0.,
0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0.,
0., 1.]))
mrpt PDF mr3 : Mean: (x,y,z,yaw,pitch,roll)=(11.0000,5.0000,0.0000,0.00deg,-0.00deg,0.00deg)
Covariance:
1 0 0 0 0 0
0 1 0 0 0 0
0 0 1 0 0 0
0 0 0 1 0 0
0 0 0 0 1 0
0 0 0 0 0 1

mrpt PDF mr3b : Mean: (x,y,z,yaw,pitch,roll)=(11.0000,5.0000,0.0000,0.00deg,-0.00deg,0.00deg)
Covariance:
1 0 0 0 0 0
0 1 0 0 0 0
0 0 1 0 0 0
0 0 0 1 0 0
0 0 0 0 1 0
0 0 0 0 0 1

ros PDF r3b : geometry_msgs.msg.PoseWithCovariance(pose=geometry_msgs.msg.Pose(position=geometry_msgs.msg.Point(x=11.0, y=5.0, z=0.0), orientation=geometry_msgs.msg.Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)), covariance=array([1., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 1., 0., 0.,
0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0.,
0., 1.]))
17 changes: 17 additions & 0 deletions python/ros_bridge.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -97,3 +97,20 @@ def ROS_PoseWithCovariance_msg_to_CPose3DPDFGaussian(p: PoseWithCovariance) -> _
ret.cov[i, j] = p.covariance[ind_map[i] * 6 + ind_map[j]]

return ret


def CPose3DPDFGaussian_to_ROS_PoseWithCovariance_msg(p: _mrpt.poses.CPose3DPDFGaussian) -> PoseWithCovariance:
"""
Converts from mrpt::poses::CPose3DPDFGaussian to ROS geometry_msgs.PoseWithCovariance
"""
# rearrange covariance (see notes above)
ind_map = [0, 1, 2, 5, 4, 3] # X,Y,Z,YAW,PITCH,ROLL

ret = PoseWithCovariance()
ret.pose = CPose3D_to_ROS_Pose_msg(p.mean)

for i in range(0, 6):
for j in range(0, 6):
ret.covariance[ind_map[i] * 6 + ind_map[j]] = p.cov[i, j]

return ret
14 changes: 13 additions & 1 deletion python/src/mrpt/math/CMatrixDynamic.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,11 +43,23 @@ void bind_mrpt_math_CMatrixDynamic(std::function< pybind11::module &(std::string
cl.def("conservativeResize", (void (mrpt::math::CMatrixDynamic<float>::*)(size_t, size_t)) &mrpt::math::CMatrixDynamic<float>::conservativeResize, "C++: mrpt::math::CMatrixDynamic<float>::conservativeResize(size_t, size_t) --> void", pybind11::arg("row"), pybind11::arg("col"));
cl.def("data", (float * (mrpt::math::CMatrixDynamic<float>::*)()) &mrpt::math::CMatrixDynamic<float>::data, "C++: mrpt::math::CMatrixDynamic<float>::data() --> float *", pybind11::return_value_policy::automatic);
cl.def("__call__", (float & (mrpt::math::CMatrixDynamic<float>::*)(size_t, size_t)) &mrpt::math::CMatrixDynamic<float>::operator(), "C++: mrpt::math::CMatrixDynamic<float>::operator()(size_t, size_t) --> float &", pybind11::return_value_policy::reference, pybind11::arg("row"), pybind11::arg("col"));
cl.def("__getitem__", (float & (mrpt::math::CMatrixDynamic<float>::*)(size_t)) &mrpt::math::CMatrixDynamic<float>::operator[], "C++: mrpt::math::CMatrixDynamic<float>::operator[](size_t) --> float &", pybind11::return_value_policy::reference, pybind11::arg("ith"));
cl.def("cast_float", (class mrpt::math::CMatrixDynamic<float> (mrpt::math::CMatrixDynamic<float>::*)() const) &mrpt::math::CMatrixDynamic<float>::cast_float, "C++: mrpt::math::CMatrixDynamic<float>::cast_float() const --> class mrpt::math::CMatrixDynamic<float>");
cl.def("cast_double", (class mrpt::math::CMatrixDynamic<double> (mrpt::math::CMatrixDynamic<float>::*)() const) &mrpt::math::CMatrixDynamic<float>::cast_double, "C++: mrpt::math::CMatrixDynamic<float>::cast_double() const --> class mrpt::math::CMatrixDynamic<double>");
cl.def("llt_solve", (class mrpt::math::CVectorDynamic<float> (mrpt::math::CMatrixDynamic<float>::*)(const class mrpt::math::CVectorDynamic<float> &) const) &mrpt::math::CMatrixDynamic<float>::llt_solve, "C++: mrpt::math::CMatrixDynamic<float>::llt_solve(const class mrpt::math::CVectorDynamic<float> &) const --> class mrpt::math::CVectorDynamic<float>", pybind11::arg("b"));
cl.def("lu_solve", (class mrpt::math::CVectorDynamic<float> (mrpt::math::CMatrixDynamic<float>::*)(const class mrpt::math::CVectorDynamic<float> &) const) &mrpt::math::CMatrixDynamic<float>::lu_solve, "C++: mrpt::math::CMatrixDynamic<float>::lu_solve(const class mrpt::math::CVectorDynamic<float> &) const --> class mrpt::math::CVectorDynamic<float>", pybind11::arg("b"));

// Manually-added matrix methods:
using dat_t = float;
using mat_t = mrpt::math::CMatrixDynamic<dat_t>;
cl.def("__getitem__", [](const mat_t&self, pybind11::tuple coord) -> dat_t { if (coord.size()==2) return self.coeff(coord[0].cast<size_t>(), coord[1].cast<size_t>()); else if (coord.size()==1) return self[coord[0].cast<size_t>()]; else throw std::invalid_argument("Access with [idx] or [row,col]"); });
cl.def("__setitem__", [](mat_t&self, pybind11::tuple coord, dat_t val) { if (coord.size()==2) self.coeffRef(coord[0].cast<size_t>(), coord[1].cast<size_t>())=val; else if (coord.size()==1) self[coord[0].cast<size_t>()]=val; else throw std::invalid_argument("Access with [idx] or [row,col]"); });
cl.def("__str__", [](const mat_t& o) -> std::string { return o.asString(); } );
cl.def("inMatlabFormat", [](const mat_t& o) -> std::string { return o.inMatlabFormat(); } );
cl.def("size", [](const mat_t&self) -> pybind11::tuple { return pybind11::make_tuple(self.cols(),self.rows()); });
cl.def_static("Identity", [](const size_t N) -> mat_t { return mat_t::Identity(N); }, "Returns the NxN identity matrix");
cl.def_static("Zero", [](const size_t nRows, const size_t nCols) -> mat_t { return mat_t::Zero(nRows,nCols); }, "Returns a matrix with zeroes");
cl.def(pybind11::init( [](pybind11::list vals){ auto m = new mat_t(); const auto nR = vals.size(); if (!nR) return m; const auto nC = vals[0].cast<pybind11::list>().size(); m->setSize(nR,nC); for (size_t r=0;r<nR;r++) { const auto row = vals[r].cast<pybind11::list>(); for (size_t c=0;c<nC;c++) m->coeffRef(r,c) = row[c].cast<dat_t>(); } return m; }));
cl.def("to_list", [](const mat_t&self) -> pybind11::list { auto l = pybind11::list(); const auto nR = self.rows(), nC = self.cols(); for (size_t r=0;r<nR;r++) { auto row = pybind11::list(); l.append(row); for (size_t c=0;c<nC;c++) row.append(self.coeff(r,c)); } return l; });
}
{ // mrpt::math::CMatrixDynamic file:mrpt/math/CMatrixDynamic.h line:41
pybind11::class_<mrpt::math::CMatrixDynamic<unsigned char>, std::shared_ptr<mrpt::math::CMatrixDynamic<unsigned char>>> cl(M("mrpt::math"), "CMatrixDynamic_unsigned_char_t", "");
Expand Down
14 changes: 13 additions & 1 deletion python/src/mrpt/math/CMatrixDynamic_1.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -128,11 +128,23 @@ void bind_mrpt_math_CMatrixDynamic_1(std::function< pybind11::module &(std::stri
cl.def("conservativeResize", (void (mrpt::math::CMatrixDynamic<double>::*)(size_t, size_t)) &mrpt::math::CMatrixDynamic<double>::conservativeResize, "C++: mrpt::math::CMatrixDynamic<double>::conservativeResize(size_t, size_t) --> void", pybind11::arg("row"), pybind11::arg("col"));
cl.def("data", (double * (mrpt::math::CMatrixDynamic<double>::*)()) &mrpt::math::CMatrixDynamic<double>::data, "C++: mrpt::math::CMatrixDynamic<double>::data() --> double *", pybind11::return_value_policy::automatic);
cl.def("__call__", (double & (mrpt::math::CMatrixDynamic<double>::*)(size_t, size_t)) &mrpt::math::CMatrixDynamic<double>::operator(), "C++: mrpt::math::CMatrixDynamic<double>::operator()(size_t, size_t) --> double &", pybind11::return_value_policy::reference, pybind11::arg("row"), pybind11::arg("col"));
cl.def("__getitem__", (double & (mrpt::math::CMatrixDynamic<double>::*)(size_t)) &mrpt::math::CMatrixDynamic<double>::operator[], "C++: mrpt::math::CMatrixDynamic<double>::operator[](size_t) --> double &", pybind11::return_value_policy::reference, pybind11::arg("ith"));
cl.def("cast_float", (class mrpt::math::CMatrixDynamic<float> (mrpt::math::CMatrixDynamic<double>::*)() const) &mrpt::math::CMatrixDynamic<double>::cast_float, "C++: mrpt::math::CMatrixDynamic<double>::cast_float() const --> class mrpt::math::CMatrixDynamic<float>");
cl.def("cast_double", (class mrpt::math::CMatrixDynamic<double> (mrpt::math::CMatrixDynamic<double>::*)() const) &mrpt::math::CMatrixDynamic<double>::cast_double, "C++: mrpt::math::CMatrixDynamic<double>::cast_double() const --> class mrpt::math::CMatrixDynamic<double>");
cl.def("llt_solve", (class mrpt::math::CVectorDynamic<double> (mrpt::math::CMatrixDynamic<double>::*)(const class mrpt::math::CVectorDynamic<double> &) const) &mrpt::math::CMatrixDynamic<double>::llt_solve, "C++: mrpt::math::CMatrixDynamic<double>::llt_solve(const class mrpt::math::CVectorDynamic<double> &) const --> class mrpt::math::CVectorDynamic<double>", pybind11::arg("b"));
cl.def("lu_solve", (class mrpt::math::CVectorDynamic<double> (mrpt::math::CMatrixDynamic<double>::*)(const class mrpt::math::CVectorDynamic<double> &) const) &mrpt::math::CMatrixDynamic<double>::lu_solve, "C++: mrpt::math::CMatrixDynamic<double>::lu_solve(const class mrpt::math::CVectorDynamic<double> &) const --> class mrpt::math::CVectorDynamic<double>", pybind11::arg("b"));

// Manually-added matrix methods:
using dat_t = double;
using mat_t = mrpt::math::CMatrixDynamic<dat_t>;
cl.def("__getitem__", [](const mat_t&self, pybind11::tuple coord) -> dat_t { if (coord.size()==2) return self.coeff(coord[0].cast<size_t>(), coord[1].cast<size_t>()); else if (coord.size()==1) return self[coord[0].cast<size_t>()]; else throw std::invalid_argument("Access with [idx] or [row,col]"); });
cl.def("__setitem__", [](mat_t&self, pybind11::tuple coord, dat_t val) { if (coord.size()==2) self.coeffRef(coord[0].cast<size_t>(), coord[1].cast<size_t>())=val; else if (coord.size()==1) self[coord[0].cast<size_t>()]=val; else throw std::invalid_argument("Access with [idx] or [row,col]"); });
cl.def("__str__", [](const mat_t& o) -> std::string { return o.asString(); } );
cl.def("inMatlabFormat", [](const mat_t& o) -> std::string { return o.inMatlabFormat(); } );
cl.def("size", [](const mat_t&self) -> pybind11::tuple { return pybind11::make_tuple(self.cols(),self.rows()); });
cl.def_static("Identity", [](const size_t N) -> mat_t { return mat_t::Identity(N); }, "Returns the NxN identity matrix");
cl.def_static("Zero", [](const size_t nRows, const size_t nCols) -> mat_t { return mat_t::Zero(nRows,nCols); }, "Returns a matrix with zeroes");
cl.def(pybind11::init( [](pybind11::list vals){ auto m = new mat_t(); const auto nR = vals.size(); if (!nR) return m; const auto nC = vals[0].cast<pybind11::list>().size(); m->setSize(nR,nC); for (size_t r=0;r<nR;r++) { const auto row = vals[r].cast<pybind11::list>(); for (size_t c=0;c<nC;c++) m->coeffRef(r,c) = row[c].cast<dat_t>(); } return m; }));
cl.def("to_list", [](const mat_t&self) -> pybind11::list { auto l = pybind11::list(); const auto nR = self.rows(), nC = self.cols(); for (size_t r=0;r<nR;r++) { auto row = pybind11::list(); l.append(row); for (size_t c=0;c<nC;c++) row.append(self.coeff(r,c)); } return l; });
}
{ // mrpt::math::CMatrixD file:mrpt/math/CMatrixD.h line:23
pybind11::class_<mrpt::math::CMatrixD, std::shared_ptr<mrpt::math::CMatrixD>, PyCallBack_mrpt_math_CMatrixD, mrpt::serialization::CSerializable, mrpt::math::CMatrixDynamic<double>> cl(M("mrpt::math"), "CMatrixD", "This class is a \"CSerializable\" wrapper for\n \"CMatrixDynamic<double>\".\n \n\n For a complete introduction to Matrices and vectors in MRPT, see:\n https://www.mrpt.org/Matrices_vectors_arrays_and_Linear_Algebra_MRPT_and_Eigen_classes\n \n\n\n ");
Expand Down
Loading

0 comments on commit e8c375e

Please sign in to comment.