Line data    Source code

       1             : #ifndef __ROSEE_UTILS_EIGEN__
       2             : #define __ROSEE_UTILS_EIGEN__
       3             : 
       4             : #include <Eigen/Dense>
       5             : #include <std_msgs/Float32MultiArray.h>
       6             : 
       7             : namespace ROSEE
       8             : {
       9             : 
      10             : namespace Utils
      11             : {
      12             : 
      13             : /**
      14             :  * @brief Utility to fill the Float32MultiArray ROS message from an eigen matrix.
      15             :  * The Float32MultiArray matrix will be stored in column major, independently from the
      16             :  * eigen matrix that can be row major (even by default eigen matrix is column major,
      17             :  * it can be templatizate specifing the row major)
      18             :  */
      19           0 : static std_msgs::Float32MultiArray eigenMatrixToFloat32MultiArray (Eigen::MatrixXd eigenMatrix) {
      20             :     
      21           0 :     std_msgs::Float32MultiArray rosMatrix;
      22             :     
      23           0 :     unsigned int nRow = eigenMatrix.rows();
      24           0 :     unsigned int nCol = eigenMatrix.cols();
      25           0 :     unsigned int size = nRow*nCol;
      26             :     
      27           0 :     if (size == 0){ //no reason to continue
      28           0 :         std::cerr << "[Utils::eigenMatrixToFloat32MultiArray] eigenMatrix passed has size 0" << std::endl;
      29           0 :         return rosMatrix;
      30             :     }
      31             :     
      32           0 :     rosMatrix.layout.dim.push_back(std_msgs::MultiArrayDimension());
      33           0 :     rosMatrix.layout.dim.push_back(std_msgs::MultiArrayDimension());
      34           0 :     rosMatrix.layout.dim[0].label = "column";
      35           0 :     rosMatrix.layout.dim[0].size = nCol;
      36           0 :     rosMatrix.layout.dim[0].stride = nCol*nRow;
      37             :     
      38           0 :     rosMatrix.layout.dim[1].label = "row";
      39           0 :     rosMatrix.layout.dim[1].size = nRow;
      40           0 :     rosMatrix.layout.dim[1].stride = nRow;
      41             :     
      42           0 :     rosMatrix.layout.data_offset = 0;
      43             :   
      44           0 :     rosMatrix.data.resize(size);
      45           0 :     int posRow = 0;
      46           0 :     for (int iCol=0; iCol<nCol; iCol++){
      47           0 :         for (int iRow=0; iRow<nRow; iRow++){
      48           0 :             posRow = iCol*nRow;
      49           0 :             rosMatrix.data.at(posRow + iRow) = eigenMatrix(iRow,iCol);
      50             :         }
      51             :     }
      52             :     
      53           0 :     return rosMatrix;
      54             : 
      55             :     
      56             : }
      57             : 
      58           0 : static std::vector<float> eigenVectorToStdVector (Eigen::VectorXd eigenVector) {
      59             :     
      60           0 :     std::vector<float> stdVect(eigenVector.data(), eigenVector.data() + eigenVector.size());
      61             :     
      62           0 :     return stdVect;
      63             : }
      64             : 
      65             : }} //namespaces
      66             : 
      67             : #endif // __ROSEE_UTILS_EIGEN__