diff --git a/projected_point_cloud_transport/include/projected_point_cloud_transport/projected_publisher.hpp b/projected_point_cloud_transport/include/projected_point_cloud_transport/projected_publisher.hpp index b3e813f..7fdf4fd 100644 --- a/projected_point_cloud_transport/include/projected_point_cloud_transport/projected_publisher.hpp +++ b/projected_point_cloud_transport/include/projected_point_cloud_transport/projected_publisher.hpp @@ -70,9 +70,23 @@ class ProjectedPublisher void projectCloudOntoSphere(const sensor_msgs::msg::PointCloud2& cloud, cv::Mat& projected_pointcloud_image) const; + // bookkeeping variables + geometry_msgs::msg::Pose view_point_; // (Maybe users can set this somehow?) + + // general parameters uint8_t projection_type_; - geometry_msgs::msg::Pose view_point_; + // params for planar projection + int view_height_; + int view_width_; + float ppx_; + float ppy_; + float fx_; + float fy_; + + // params for spherical projection + double phi_resolution_; + double theta_resolution_; }; } // namespace projected_point_cloud_transport diff --git a/projected_point_cloud_transport/src/projected_publisher.cpp b/projected_point_cloud_transport/src/projected_publisher.cpp index cc59542..6143583 100644 --- a/projected_point_cloud_transport/src/projected_publisher.cpp +++ b/projected_point_cloud_transport/src/projected_publisher.cpp @@ -43,6 +43,17 @@ namespace projected_point_cloud_transport void ProjectedPublisher::declareParameters(const std::string & base_topic) { + // params for planar projection + view_height_ = 720; + view_width_ = 1080; + ppx_ = static_cast(view_width_)/2.0; + ppy_ = static_cast(view_height_)/2.0; + fx_ = view_width_; + fy_ = view_width_; + + // params for spherical projection + phi_resolution_ = 0.034; // radians + theta_resolution_ = 0.034; // radians } std::string ProjectedPublisher::getTransportName() const @@ -93,16 +104,8 @@ void ProjectedPublisher::projectCloudOntoPlane(const sensor_msgs::msg::PointClou // TODO (john.dangelo@tailos.com): if the pointcloud is already organized, just point the cv::Mat at the data projected_pointcloud_image = cv::Mat((int)cloud.height, (int)cloud.width, CV_32FC3, (void*)cloud.data.data()); }else{ - // TODO (john.dangelo@tailos.com): Make these parameters - const int view_height = 720; - const int view_width = 1080; - projected_pointcloud_image = cv::Mat(cv::Size(view_width, view_height), CV_16UC1, cv::Scalar(0)); - const float ppx = view_width/2; - const float ppy = view_height/2; - const float fx = view_width; - const float fy = view_width; - // if the pointcloud is NOT already organized, we need to apply the projection + projected_pointcloud_image = cv::Mat(cv::Size(view_width_, view_height_), CV_16UC1, cv::Scalar(0)); // iterate over the pointcloud sensor_msgs::PointCloud2ConstIterator iter_x(cloud, "x"); @@ -121,10 +124,10 @@ void ProjectedPublisher::projectCloudOntoPlane(const sensor_msgs::msg::PointClou // TODO (john-maidbot): rotate the d x/y/z so that z is along the view point orientation // TODO: can that be made more efficient by transforming the view point instead? - const int col = x/z * fx + ppx; - const int row = y/z * fy + ppy; + const int col = x/z * fx_ + ppx_; + const int row = y/z * fy_ + ppy_; - if(z == 0 || col < 0 || row < 0 || col >= view_width || row >= view_height){ + if(z == 0 || col < 0 || row < 0 || col >= view_width_ || row >= view_height_){ continue; } @@ -135,11 +138,9 @@ void ProjectedPublisher::projectCloudOntoPlane(const sensor_msgs::msg::PointClou } void ProjectedPublisher::projectCloudOntoSphere(const sensor_msgs::msg::PointCloud2& cloud, cv::Mat& projected_pointcloud_image) const{ - // TODO (john.dangelo@tailos.com): Make these parameters - const double phi_resolution = 0.034; // radians - const double theta_resolution = 0.034; // radians - const int phi_bins = 2 * M_PI / phi_resolution; - const int theta_bins = 2 * M_PI / theta_resolution; + // compute image size based on resolution + const int phi_bins = 2 * M_PI / phi_resolution_; + const int theta_bins = 2 * M_PI / theta_resolution_; cv::Mat spherical_image{phi_bins, theta_bins, CV_16UC1, cv::Scalar(0)}; @@ -169,8 +170,8 @@ void ProjectedPublisher::projectCloudOntoSphere(const sensor_msgs::msg::PointClo theta += 2*M_PI; } - const int phi_index = std::min(static_cast(phi / phi_resolution), phi_bins); - const int theta_index = std::min(static_cast(theta / theta_resolution), theta_bins); + const int phi_index = std::min(static_cast(phi / phi_resolution_), phi_bins); + const int theta_index = std::min(static_cast(theta / theta_resolution_), theta_bins); auto& cell = spherical_image.at(phi_index, theta_index); cell = std::min(cell, static_cast(rho * 1000)); // mm resolution diff --git a/projected_point_cloud_transport/src/projected_subscriber.cpp b/projected_point_cloud_transport/src/projected_subscriber.cpp index a96d67c..5304ff5 100644 --- a/projected_point_cloud_transport/src/projected_subscriber.cpp +++ b/projected_point_cloud_transport/src/projected_subscriber.cpp @@ -164,7 +164,8 @@ void ProjectedSubscriber::deprojectSphereToCloud(const cv::Mat& spherical_image, // convert the spherical image pixel to a point cloud point const double rho = static_cast(cell) / 1000.0; // meters const double phi = row * phi_resolution; - const double theta = col * phi_resolution; + const double theta = col * theta_resolution; + // TODO (john-maidbot): This could be a lookup table if performance is a huge concern pcl_itr[0] = rho * std::sin(phi) * std::cos(theta); pcl_itr[1] = rho * std::sin(phi) * std::sin(theta); pcl_itr[2] = rho * std::cos(phi);