ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

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

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.

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.

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="">

name>

common usage for rgbdslam would be: ./pcl_plyrecord /rgbdslam/batch_clouds

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.

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.

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.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.