As to answering my own question:
I made a small .ply recorder that listens to a topic and writes out any received PointClouds to a .ply file.
Usage: pcl_plyrecord <topic name>
common usage for rgbdslam would be:
./pcl_plyrecord /rgbdslam/batch_clouds
Now you will typically start rgbdslam and record something with it (CTRL+P).
Now, to get it to record a usable .ply file, "send" the data with the rgbdslam tool (CTRL+S).
This will send the acquired point cloud to the previously mentioned topic and will make it record to a .ply file.
It will output to output.ply
CTRL+C to stop the program (try to stop it when no new data is coming in, or your output file might miss some)
WARNING: It will overwrite output.ply every time you start the program!
The program records xyzrgb type pointclouds, like the ones rgbdslam outputs. If you import the .ply in meshlab you will have a pointmap with color. Just what we need! :)
You can find the source package here.
It contains the source and a buildscript and readme for quick compiling. (read the readme!!)
Or for those who just want to have a look at the source:
pcl_plyrecord.cpp
#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <boost/foreach.hpp>
#include <stdio.h>
#include "pcl_plyrecord.h"
#include <math.h>
//#include "rply.h"
typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud;
int count = 0;
FILE *file;
long int HEADER_COUNT_POS = 0;
/**
* Add one point to output .ply file
* Point will be omitted if it contains NaN coordinates
**/
void addPoint(const float x, const float y, const float z, const float rgb)
{
// Filter out points with NaN coordinates
if(isnan(x) || isnan(y) || isnan(z) || isnan(rgb))
return; // one of the values is NaN
RGBValue color = RGBValue();
color.float_value = rgb;
unsigned char red = color.Red;
unsigned char green = color.Green;
unsigned char blue = color.Blue;
fprintf(file, "%f %f %f %u %u %u\n", x, y, z, red, green, blue);
count++;
}
/**
* Write initial header of .ply file
* Vertices count will be set to 0
**/
void writeheader(void)
{
fprintf (file, "ply\n");
fprintf (file, "format ascii 1.0\n");
fprintf (file, "comment Created with pcl_plyrecord\n");
HEADER_COUNT_POS = ftell(file); // store location of vertices count
fprintf (file, "element vertex %-10i\n", 0); // with extra padding for easy overwrite later on
fprintf (file, "property float x\n");
fprintf (file, "property float y\n");
fprintf (file, "property float z\n");
fprintf (file, "property uchar red\n");
fprintf (file, "property uchar green\n");
fprintf (file, "property uchar blue\n");
fprintf (file, "end_header\n");
}
/**
* Overwrite the vertices count in the header at the top of the .ply file
* Because of extra padding to the number, larger numbers can always be written in-place
* File pointer is set to the end of the file again afterwards
**/
void correctHeader(void)
{
fseek(file, HEADER_COUNT_POS, SEEK_SET);
fprintf (file, "element vertex %-10i\n", count);
fseek(file, 0, SEEK_END);
}
/**
* Process a new incoming pointcloud
* After adding all points of the cloud the vertices count in the header is altered
* Interrupting the program before the header is altered will only make available
* the vertices up to the previous fully processed pointcloud.
**/
void callback(const PointCloud::ConstPtr& msg)
{
// printf ("Cloud: width = %d, height = %d\n", msg->width, msg->height);
BOOST_FOREACH (const pcl::PointXYZRGB& pt, msg->points)
addPoint(pt.x, pt.y, pt.z, pt.rgb);
correctHeader();
}
int main(int argc, char** argv)
{
if(argc < 2) {
printf ("Specify topic to record as parameter.\n");
exit(0);
}
ros::init(argc, argv, "pcl_plyrecord");
file = fopen("output.ply", "w");
writeheader();
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe<PointCloud>(argv[1], 1, callback);
ros::spin();
}
pcl_plyrecord.h
typedef union
{
struct /*anonymous*/
{
unsigned char Blue;
unsigned char Green;
unsigned char Red;
unsigned char Alpha;
};
float float_value;
long long_value;
} RGBValue;
The most 'twiddly' part was to update the header of the .ply file, since the number of vertices has to be known at the start of the file. The header is updated after every full pointcloud that is processed. This means that if you CTRL + C before a pointcloud is fully processed, the header won't be updated and meshlab will only import vertices till the last full pointcloud that got processed.
Originally posted by duststorm with karma: 91 on 2011-03-22
This answer was ACCEPTED on the original site
Post score: 2
Original comments
Comment by Shark on 2011-05-05:
What you proposed above about also recording the transform is correct. However, this discussion is kind of moot now with the new version of rgbdslam, which let's to directly save the accumulated point cloud to .pcd or .ply
Comment by duststorm on 2011-04-21:
If someone knows the correct workings of the transform and could explain, I could fix the code to account for transformations.
Comment by duststorm on 2011-04-21:
Maybe the fix would be to also record the /transform topic, extract the proper transformation information for an incoming pointcloud and multiply that pointcloud (or each point in the pointcloud) with the proper transformation matrix. I don't know ROS that well to be able to answer for sure.
Comment by duststorm on 2011-04-21:
Do you think so? I didn't notice that yet in my resulting files, but I must admit I haven't really been experimenting with it a lot. I do know however that when trying bag to pcd conversion there might be compaints about missing /transform data or such.
Comment by Shark on 2011-04-14:
I've implemented your code, but I think there is a slight problem. You do not account for the transform anywhere, so the pointclouds are not mapped correctly in your combined ply file.
Comment by duststorm on 2011-03-24:
As a sidenote: the output file could be made more compact if the program would output a binary instead of an ascii .ply file. Of course that makes it a little harder to read and parse afterwards. Meshlab supports both.