patchworkpp_ros::utils::EigenMatToPointCloud2() should use points.rows(), NOT points.size() when calling CreatePointCloud2Msg()

[ewak]

Using points.size() results in too much memory space being allocated for the pointcloud. This results in a pointcloud that is 3 times bigger than needed where 2/3s of the points are a (0,0,0)

Here is a test showing the problem and solution.

#include <iostream>

// Patchwork++-ROS
#include "Utils.hpp"

inline std::unique_ptr<sensor_msgs::msg::PointCloud2> EigenMatToPointCloud2Good(
    const Eigen::MatrixX3f &points, const std_msgs::msg::Header &header) {
    auto msg = patchworkpp_ros::utils::CreatePointCloud2Msg(points.rows(), header);
    patchworkpp_ros::utils::FillPointCloud2XYZ(points, *msg);
    return msg;
}

inline std::unique_ptr<sensor_msgs::msg::PointCloud2> EigenMatToPointCloud2Bad(
    const Eigen::MatrixX3f &points, const std_msgs::msg::Header &header) {
    auto msg = patchworkpp_ros::utils::CreatePointCloud2Msg(points.size(), header);
    patchworkpp_ros::utils::FillPointCloud2XYZ(points, *msg);
    return msg;
}

int main() {
    std::cout << "e_rows" << std::endl;

    Eigen::MatrixX3f pointsIn(5, 3);
    pointsIn << 1.0f, 2.0f, 3.0f, 4.0f, 5.0f, 6.0f, 7.0f, 8.0f, 9.0f, 10.0f,
        11.0f, 12.0f, 13.0f, 14.0f, 15.0f;

    std::cout << "pointsIn = " << std::endl << pointsIn << std::endl;
    std::cout << "pointsIn.size() = " << pointsIn.size() << std::endl;
    std::cout << "pointsIn.rows() = " << pointsIn.rows() << std::endl;

    std::cout << "*** Good allocate pointcloud using points.rows()" << std::endl;
    std_msgs::msg::Header header;
    auto pcMsg = EigenMatToPointCloud2Good(pointsIn, header);
    std::cout << "pcMsg->height =" << pcMsg->height << std::endl;
    std::cout << "pcMsg->width =" << pcMsg->width << std::endl;

    std::shared_ptr<sensor_msgs::msg::PointCloud2> pcMsgPtr(std::move(pcMsg));
    auto pointsOut = patchworkpp_ros::utils::PointCloud2ToEigenMat(pcMsgPtr);
    std::cout << "pointsOut = " << std::endl << pointsOut << std::endl;
    std::cout << "pointsOut.size() = " << pointsOut.size() << std::endl;
    std::cout << "pointsOut.rows() = " << pointsOut.rows() << std::endl;


    std::cout << "*** Bad allocate pointcloud using points.size()" << std::endl;
    auto pcMsgBad = EigenMatToPointCloud2Bad(pointsIn, header);
    std::cout << "pcMsgBad->height =" << pcMsgBad->height << std::endl;
    std::cout << "pcMsgBad2->width =" << pcMsgBad->width << std::endl;

    std::shared_ptr<sensor_msgs::msg::PointCloud2> pcMsgBadPtr(std::move(pcMsgBad));

    auto pointsOutBad = patchworkpp_ros::utils::PointCloud2ToEigenMat(pcMsgBadPtr);
    std::cout << "pointsOutBad = " << std::endl << pointsOutBad << std::endl;
    std::cout << "pointsOutBad.size() = " << pointsOutBad.size() << std::endl;
    std::cout << "pointsOutBad.rows() = " << pointsOutBad.rows() << std::endl;
    return 0;
}

Here is the output

e_rows
pointsIn = 
 1  2  3
 4  5  6
 7  8  9
10 11 12
13 14 15
pointsIn.size() = 15
pointsIn.rows() = 5
*** Good allocate pointcloud using points.rows()
pcMsg->height =1
pcMsg->width =5
pointsOut = 
 1  2  3
 4  5  6
 7  8  9
10 11 12
13 14 15
pointsOut.size() = 15
pointsOut.rows() = 5
*** Bad allocate pointcloud using points.size()
pcMsgBad->height =1
pcMsgBad2->width =15
pointsOutBad = 
 1  2  3
 4  5  6
 7  8  9
10 11 12
13 14 15
 0  0  0
 0  0  0
 0  0  0
 0  0  0
 0  0  0
 0  0  0
 0  0  0
 0  0  0
 0  0  0
 0  0  0
pointsOutBad.size() = 45
pointsOutBad.rows() = 15

[ewak]

See PR #68