Exporting RGBD-6D-SLAM scanned data to meshlab

edit

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