# Unable to transform PointCloud2 with TF StampedTransform

I have seen many questions along these lines, but this is a unique case. We have a solid-state LiDAR that is publishing PointCloud2 messages at 100Hz and a RealSense T265 tracking camera that publishes odom transforms at 200Hz. I have tried literally every hack in the book to integrate the T265 poses with the LiDAR points, but nothing is working.

Now, I have a launch file that contains the following static transform publisher:

<node pkg="tf" type="static_transform_publisher" name="t265_to_lidar" args="0 0 0 0 0 0 /camera_odom_frame /lidar_frame 1000"/>


If I do not publish this static transform, ROS will not see either the camera_odom_frame or the lidar_frame on the callback. Here is the code being used for the ROS node:

#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>
#include <sensor_msgs/PointCloud2.h>
#include <opencv2/opencv.hpp>

#include <geometry_msgs/PointStamped.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/message_filter.h>
#include <message_filters/subscriber.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

using namespace Eigen;
using namespace std;

tf::TransformListener *tf_listener;
ros::Publisher publisher;

void orient_cloud(const sensor_msgs::PointCloud2 &cloud_msg) {

tf::StampedTransform transform;

try{
tf_listener->lookupTransform("camera_odom_frame", "lidar_frame",
}
catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
}

Matrix4f m;
pcl_ros::transformAsMatrix(transform, m);

sensor_msgs::PointCloud2 output;
pcl_ros::transformPointCloud(m, cloud_msg, output);

publisher.publish (output);

}

int main(int argc, char **argv) {

ros::init(argc, argv, "captureLiDARIMU");
ros::NodeHandle nh;

tf_listener = new tf::TransformListener();

ros::Subscriber subscriber = nh.subscribe("/lidar/points", 100, orient_cloud);

ros::spin();

return 0;

}


First thing you are probably wondering is why I implemented an Eigen 4x4 matrix. This was to troubleshoot the StampedTransform, because it never changes. It stays at the initial "zero" pose, regardless of moving the T265 around. In other words, the tf_listener->lookupTransform is never giving anything other than the initial pose.

What I don't understand is why ROS doesn't pick up on the T265's camera_odom_frame transforms. If I separately invoke "rostopic echo /camera/odom/sample" which has the odom_frame as its parent, it will show the changing poses as I move around. I originally attempted this without the static transform publisher, but then, as I mentioned above, I receive continual "lidar_frame passed to lookupTransform argument source_frame does not exist" messages on the callback. This also is confusing, because the sensor_msg from the LiDAR is being handed over directly to the callback. Why wouldn't it be able to see its parent frame?

We also attempted to convert this to a message_filter model, but there are issues in Noetic which prevent using PointCloud2 as data types. I haven't figured out that problem yet, so trying to keep things clean and simple. I do hope someone has an idea what the problem is here, because functionally, this should be very straightforward.

edit retag close merge delete

I'm confused: you're publishing a static identity transform yourself, then do lookup_transform(..) and then ask why it always returns an identity transform?

If I do not publish this static transform, ROS will not see either the camera_odom_frame or the lidar_frame on the callback.

Could you perhaps clarify who is publishing what transforms? What does rqt_tf_tree show with all your nodes running?

I originally attempted this without the static transform publisher, but then, as I mentioned above, I receive continual "lidar_frame passed to lookupTransform argument source_frame does not exist" messages on the callback. This also is confusing, because the sensor_msg from the LiDAR is being handed over directly to the callback. Why wouldn't it be able to see its parent frame?

Drivers setting frame_id in PointCloud messages does not result in TF frames getting broadcast. Something will have to publish those transforms.

So that error seems like it ...(more)

( 2021-04-30 03:33:14 -0500 )edit

Thank you for the feedback. Our team is fairly new to ROS and we are grappling with some of the concepts. Perhaps it would be easier if I first stated our goal: we have a solid-state LiDAR that is not publishing any transforms, only PointCloud2 messages. It has an IMU, but it is very inaccurate, so we have turned it off and installed a RealSense T265 (mounted together with the LiDAR) to give it orientation.

So the problem is that we have a stream of non-oriented points with no transforms. And we simply want to integrate those points, using the message timestamps, with the T265 transforms at the same time. This is why I originally thought message_filter was the answer, but it was not playing nicely, so we turned to a callback.

I had presumed that the static transform publisher was doing nothing more than placing the two sensors in ...(more)

( 2021-04-30 09:43:57 -0500 )edit

I used this repo as a starting point. More or less the same principle: subscribe to PointCloud2 messages then push them to a callback where they are transformed by an external source (IMU). There is a critical piece of the puzzle I am missing here, as no one ever talks about the transforms. If I understand you correctly, it seems that what we have to do to make this work is publish TF messages along with the PointCloud2 with a static "zero-state transform". Then there will be something to measure against. But it brings up the question --- isn't there a more elegant way to do it? Why can't I simply use a TimeSynchronizer to pipe the TF and PointCloud2 messages through a method that does nothing more than read its quaternion / translation, transform the cloud, and then re-publish it? That's our goal.

Here is the TF Tree ...(more)

( 2021-04-30 10:05:21 -0500 )edit