PCL‐OpenCV Wiki - ArcturusNavigation/all_seaing_vehicle GitHub Wiki

Our sensor fusion algorithm (bbox_project_pcloud, description in all_seaing_perception wiki) combines the bounding box detections in the camera image with the LiDAR point cloud, and computes the point cluster that corresponds to each detection.

To do easier processing of point clouds and images (and not reinvent the wheel) we are using the PCL (Point Cloud Library) and OpenCV libraries in C++ (which is why the perception nodes are C++, there is no PCL support for Python). We are also using the pcl_conversions and cv_bridge packages to convert between the ROS2 message types and the respective PCL/OpenCV objects types, and image_geometry for projecting 3D points to the image plane (and can also use to project 2D image points to 3D rays).

Point clouds are represented in ROS2 with the sensor_msgs/msg/PointCloud2 message type. The header field is a std_msgs/msg/Header message containing the frame id the point cloud corresponds to and the time stamp of the message. The rest of the fields are not really intuitive, and we are not going to use them directly, but rather convert the message to respective PCL type, which we'll talk about in the next section.

Images are represented with the sensor_msgs/msg/Image message. The header field contains the same information as in the point cloud message (but with the camera optical frame instead), the height and width fields are the number of rows and columns of the image. Be aware that in the image frame, the indexing starts from the top left, and x is right, y is down, z is forwards. The rest of the fields we are going to access after converting the image to the respective OpenCV type, since it's easier.

A sensor_msgs/msg/CameraInfo message contains a header and information about the way the camera image corresponds to the real world (intrinsic matrix and distortion information).

Our internal representation for the local and global map is the all_seaing_interfaces/msg/ObstacleMap message type, which contains a global and local header (respective world and robot frame ids and the time stamp), as well as a list of all_seaing_interfaces/msg/Obstacle objects, which contain the centroid, 2D (projected) convex hull, 2D area, and bounding box of the detected object, both in the robot's frame and the global one.

All the types in PCL are in the pcl namespace.

In PCL, simple 3D points are represented with the PointXYZ type, containing its x, y, and z coordinates as the respective fields. We can add an intensity value, in the intensity field, using the PointXYZI type, or the point's color, with the PointXYZRGB and PointXYZHSV types and the r,g,b and h,s,v fields respectively.

Point clouds are represented with the PointCloud type (e.g., for RGB points, it's pcl::PointCloud<pcl::PointXYZRGB>). It contains the header field of type PCLHeader, similar to the ROS header (although we usually don't use it), and the points vector of points with the respective type.

There is also a way to store indices of a point cloud, using the PointIndices type; the indices field is the vector of the point indices.

When working with point clouds, we are usually using pointers, so we are usually working with the pcl::PointCloud<PointXYZHSV>::Ptr type, and accesing the respective fields using -> (e.g. point_cloud->points).

To perform euclidean clustering in a set of points of a point cloud, we create a EuclideanClusterExtraction object and a search::KdTree one (which is used by the clustering algorithm to search for nearby points), and set the required parameters, to get a std::vector<pcl::PointIndices>& pointer with the extracted clusters' point indices (wrt the source point cloud, which is a pointer of type pcl::PointCloud<pcl::PointXYZHSV>::Ptr). Here is a piece of code (from perception_utilities.cpp) that showcases how to do euclidean clustering:

// pcl::PointCloud<pcl::PointXYZHSV>::Ptr pcloud_ptr, std::vector<pcl::PointIndices>& clusters_indices
pcl::search::KdTree<pcl::PointXYZHSV>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZHSV>);
if (!pcloud_ptr->points.empty())
    tree->setInputCloud(pcloud_ptr);

pcl::EuclideanClusterExtraction<pcl::PointXYZHSV> ec;
ec.setClusterTolerance(clustering_distance);
ec.setMinClusterSize(obstacle_sz_min);
ec.setMaxClusterSize(obstacle_sz_max);
ec.setSearchMethod(tree);
ec.setInputCloud(pcloud_ptr);
ec.extract(clusters_indices);

We can copy a point cloud by using the pcl::copyPointCloud function, with the option to only copy a subset of the points:

// pcl::PointCloud<pcl::PointXYZI>::Ptr &in_cloud_ptr
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_2d(new pcl::PointCloud<pcl::PointXYZI>);
pcl::copyPointCloud(*in_cloud_ptr, *cloud_2d);

// pcl::PointIndices indices
pcl::copyPointCloud(*in_cloud_ptr, it->indices, *cloud_lidar_ptr);

Given a point cloud, we can find its mean/centroid (with the option to do that on a subset of indices):

// pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud_ptr
pcl::PointXYZI avg;
pcl::computeCentroid(*in_cloud_ptr, avg);

, covariance matrix (or normalized):

// pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud_ptr
Eigen::Matrix3f cov_mat;
pcl::computeCovarianceMatrix(*in_cloud_ptr, cov_mat);

, or both:

// pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud_ptr
Eigen::Matrix3d cov_mat;
Eigen::Vector4d centr;
pcl::computeMeanAndCovarianceMatrix(pcl_in, cov_mat, centr);

We can also get the minimum and maximum XYZ (and also I or RGB) values of the point cloud:

// pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud_ptr
pcl::PointXYZI minPoint, maxPoint;
pcl::getMinMax3D(*in_cloud_ptr, minPoint, maxPoint);

To get the minimum oriented bounding box (from perception_utilities.cpp):

// pcl::PointCloud<pcl::PointXYZI> pcl_in
Eigen::Matrix3d cov_mat;
Eigen::Vector4d centr;
pcl::computeMeanAndCovarianceMatrix(pcl_in, cov_mat, centr);
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver(cov_mat, Eigen::ComputeEigenvectors);
Eigen::Matrix3d eigenvecs = eigen_solver.eigenvectors();
Eigen::Vector3d eigenvals = eigen_solver.eigenvalues();
// from https://codextechnicanum.blogspot.com/2015/04/find-minimum-oriented-bounding-box-of.html
// Transform the original cloud to the origin where the principal components correspond to the axes.
Eigen::Matrix4d projectionTransform(Eigen::Matrix4d::Identity());
projectionTransform.block<3,3>(0,0) = eigenvecs.transpose();
projectionTransform.block<3,1>(0,3) = -1.f * (projectionTransform.block<3,3>(0,0) * centr.head<3>());
pcl::PointCloud<pcl::PointXYZI>::Ptr cloudPointsProjected (new pcl::PointCloud<pcl::PointXYZI>);
pcl::transformPointCloud(pcl_in, *cloudPointsProjected, projectionTransform);
// Get the minimum and maximum points of the transformed cloud.
pcl::PointXYZI minPoint, maxPoint;
pcl::getMinMax3D(*cloudPointsProjected, minPoint, maxPoint);
Eigen::Vector3d axes_length = Eigen::Vector3d(maxPoint.x - minPoint.x, maxPoint.y - minPoint.y, maxPoint.z - minPoint.z);

All the types in OpenCV are in the cv namespace.

In OpenCV, images are represented as Mat objects. The rows, cols fields provide the size of the image.

The Point type, that has different variants like Point2d, Point3d etc., can represent 2D or 3D points with values of type int (i), float (f), double (d), or int64 (l) (for example, Point2d holds two values of type double).

Similar to the Point type, there are the Vec types (Vec3d etc.), which also include Vec#b corresponding to uchar, Vec#s for short, and Vec#w for ushort, and no Vec#l (for int64). These usually correspond to image values or other vectors of numbers.

A Scalar is a 4-element vector, also used for pixel values (usually in thresholds, like for color segmentation), and the 4th value might be 0 if we are only working with 3 channels.

In order to access the values of the pixels of the image, we can use the at function, as follows:

// cv::Point image_pt
cv::Vec3b image_pt_hsv_vec3b = cv_hsv.at<cv::Vec3b>(row,col);
int r = image_pt_hsv_vec3b[0];
// ...

The at function also takes Point as an input for the pixel position:

// cv::Point image_pt
cv::Vec3b image_pt_hsv_vec3b = cv_hsv.at<cv::Vec3b>(image_pt);
int r = image_pt_hsv_vec3b[0];
// ...

We can convert between color encodings using the cvtColor function, setting the appropriate color conversion code:

// cv::Mat cv_bgr
cv::Mat cv_hsv;
cv::cvtColor(cv_bgr, cv_hsv, cv::COLOR_BGR2HSV);

To perform color segmentation on an image, there are a couple of steps we need to follow.

First, we need to convert the image to HSV (as shown above), set some HSV limits, and create a mask (Mat with single channel) that has value of 255 when the point satisfies the HSV condition and 0 otherwise, using the inRange function:

// cv::Mat img // in HSV
int h_min=lims[0], h_max=lims[1], s_min=lims[2], s_max=lims[3], v_min=lims[4], v_max=lims[5];
cv::Mat mask;
cv::inRange(img, cv::Scalar(h_min, s_min, v_min), cv::Scalar(h_max, s_max, v_max), mask);

Afterwards, we are using the erode and dilate functions, that take a Mat kernel typically created with the getStructuringElement function, to remove outlying points (erode) and make the resulting contours continuous (dilate), in the mask from the previous step:

cv::erode(mask, mask, cv::getStructuringElement(cv::MORPH_RECT, cv::Size(erode_sz, erode_sz)));
cv::dilate(mask, mask, cv::getStructuringElement(cv::MORPH_RECT, cv::Size(dilate_sz, dilate_sz)));

The final step is to split the mask into contours (represented by a vector of std::vector<cv::Point> lists of the points in the perimeter of the image segment), using the findContours function:

cv::findContours(mask, contours, cv::RETR_TREE, cv::CHAIN_APPROX_SIMPLE);

We can convert a sensor_msgs/msg/PointCloud2 message to the respective pcl::PointCloud PCL type using the fromROSMsg function:

// sensor_msgs::msg::PointCloud2 pcl_msg
pcl::PointCloud<PointXYZI>::Ptr pcl_out;
pcl::fromROSMsg(pcl_msg, *pcl_out);

and vice-versa, using toROSMsg:

// pcl::PointCloud<PointXYZI>::Ptr pcl_in
sensor_msgs::msg::PointCloud2 pcl_msg;
pcl::toROSMsg(*pcl_in, pcl_msg);

We can use the doTransform function from tf2_sensor_msgs, along with the PCL-ROS2 conversions shown above, to transform a point cloud between different coordinate frames:

// geometry_msgs::msg::TransformStamped tf
tf2::doTransform<sensor_msgs::msg::PointCloud2>(pcl_in_msg, pcl_out_msg, tf);

We can use the toCvCopy function to convert a sensor_msgs/msg/Image ROS2 message to an intermediate CvImagePtr object, and then to an OpenCV Mat object:

// sensor_msgs::msg::Image(::ConstSharedPtr if ptr) in_img_msg
cv_bridge::CvImagePtr cv_ptr;
try {
    cv_ptr = cv_bridge::toCvCopy(in_img_msg, sensor_msgs::image_encodings::BGR8);
} catch (cv_bridge::Exception &e) {
    RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what());
    return;
}
cv::Mat img = cv_ptr->image;

This package allows us to use the PinholeCameraModel that we get from the respective sensor_msgs/msg/CameraInfo ROS2 message, using:

// sensor_msgs::msg::CameraInfo info_msg
m_cam_model.fromCameraInfo(info_msg);

to project 3D points to the camera plane using the project3dToPixel function:

// image_geometry::PinholeCameraModel cmodel, cv::Point3d xyz
cv::Point2d proj_pt = cmodel.project3dToPixel(xyz);

To make it easier to create Obstacle messages that will be incorporated in an ObstacleMap, we are using an intermediary Obstacle<PointT> (where PointT can be pcl::PointXYZ, pcl::PointXYZI, pcl::PointXYZRGB, or pcl::PointHSV) class, with the following methods:

• to_ros_msg(all_seaing_interfaces::msg::Obstacle &out_obstacle_msg): creates an Obstacle message from the internal representation of the obstacle within the class instance.
• Obstacle(std_msgs::msg::Header local_header, std_msgs::msg::Header global_header, all_seaing_interfaces::msg::Obstacle in_obstacle_msg): class constructor using the local and global headers, and an existing Obstacle message.
• Obstacle(std_msgs::msg::Header local_header, std_msgs::msg::Header global_header, const typename pcl::PointCloud<PointT>::Ptr in_cloud_ptr, const std::vector<int> &in_cluster_indices, int in_id,geometry_msgs::msg::TransformStamped lidar_map_tf): class constructor given the local and global headers, input point cloud, indices to be selected, id for the obstacle, and the transform from the LiDAR frame to the global map frame.
• Obstacle(std_msgs::msg::Header header, const typename pcl::PointCloud<PointT>::Ptr pcloud, int in_id, bool global): class constructor using a header, point cloud, and id, and whether it's in the global frame or not (to set the respective fields, the rest are left empty).
• Getters and setters for most of the message fields.
• transform_pcl_pt(PointT pt_in, PointT& pt_tf, geometry_msgs::msg::TransformStamped tf): transforms a PCL point from one frame to another given the required transform.
• local_to_global(std_msgs::msg::Header global_header, geometry_msgs::msg::TransformStamped lidar_map_tf): fill out the respective global fields in the obstacle class given the global header and the respective local->global transformation.
• local_to_global(std_msgs::msg::Header global_header, double dx, double dy, double dtheta): same as above but instead of a TF transformation it accepts the respective dx, dy, dtheta values.
• global_to_local similar to global_to_local but inverse.
• global_transform(geometry_msgs::msg::TransformStamped tf): transform the global points' position by the given TF transform.
• global_transform(double dx, double dy, double dtheta): same as above but with dx, dy, dtheta.

A set of various functions that are commonly used in our obstacle detection/sensor fusion nodes, with the respective descriptions in the comments above the function declarations.