/* +---------------------------------------------------------------------------+
| The Mobile Robot Programming Toolkit (MRPT) C++ library |
| |
| http://www.mrpt.org/ |
| |
| Copyright (C) 2005-2011 University of Malaga |
| |
| This software was written by the Machine Perception and Intelligent |
| Robotics Lab, University of Malaga (Spain). |
| Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
| |
| This file is part of the MRPT project. |
| |
| MRPT is free software: you can redistribute it and/or modify |
| it under the terms of the GNU General Public License as published by |
| the Free Software Foundation, either version 3 of the License, or |
| (at your option) any later version. |
| |
| MRPT is distributed in the hope that it will be useful, |
| but WITHOUT ANY WARRANTY; without even the implied warranty of |
| MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| GNU General Public License for more details. |
| |
| You should have received a copy of the GNU General Public License |
| along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
| |
+---------------------------------------------------------------------------+ */
/* ------------------------------------------------------
rgbd2rawlog
A small tool to translate RGB+D datasets from TUM:
http://cvpr.in.tum.de/data/datasets/rgbd-dataset/
into MRPT rawlog binary format, ready to be parsed
by RawLogViewer or user programs.
Usage:
rgbd_dataset2rawlog [PATH_TO_RGBD_DATASET] [OUTPUT_NAME]
Where expected input files are:
- PATH_TO_RGBD_DATASET/accelerometer.txt
- PATH_TO_RGBD_DATASET/depth.txt
- PATH_TO_RGBD_DATASET/rgb.txt
- PATH_TO_RGBD_DATASET/rgb/... (Images, 3x8u)
- PATH_TO_RGBD_DATASET/depth/... (Images, 1x16u)
Output files:
- OUTPUT_NAME.rawlog: The output rawlog file.
- OUTPUT_NAME_Images/...: External RGB images.
------------------------------------------------------ */
#include <mrpt/base.h>
#include <mrpt/obs.h>
using namespace std;
using namespace mrpt;
using namespace mrpt::slam;
const double KINECT_FPS = 30.0;
// Based on: http://stackoverflow.com/questions/218488/finding-best-matching-key-for-a-given-key-in-a-sorted-stl-container
template <class CONTAINER>
typename CONTAINER::const_iterator find_closest(
const CONTAINER &data,
const typename CONTAINER::key_type &searchkey )
{
typename CONTAINER::const_iterator upper = data.lower_bound(searchkey);
if (upper == data.begin() || upper->first==searchkey)
return upper;
typename CONTAINER::const_iterator lower = upper;
--lower;
if (upper == data.end() ||
(searchkey - lower->first) < (upper->first - searchkey))
return lower;
return upper;
}
void rgbd2rawlog(const string &src_path, const string &out_name)
{
const string in_fil_acc = src_path+string("/accelerometer.txt");
const string in_fil_depth = src_path+string("/depth.txt");
const string in_fil_rgb = src_path+string("/rgb.txt");
ASSERT_FILE_EXISTS_(in_fil_acc)
ASSERT_FILE_EXISTS_(in_fil_depth)
ASSERT_FILE_EXISTS_(in_fil_rgb)
// make a list with RGB & DEPTH files ------------------------
map<double,string> list_rgb, list_depth;
map<double,vector<double> > list_acc;
std::istringstream line;
{
mrpt::utils::CTextFileLinesParser fparser(in_fil_rgb);
while (fparser.getNextLine(line))
{
double tim;
string f;
if ( line >> tim >> f )
list_rgb[tim]=f;
}
}
{
mrpt::utils::CTextFileLinesParser fparser(in_fil_depth);
while (fparser.getNextLine(line))
{
double tim;
string f;
if ( line >> tim >> f )
list_depth[tim]=f;
}
}
{
mrpt::utils::CTextFileLinesParser fparser(in_fil_acc);
while (fparser.getNextLine(line))
{
double tim;
double ax,ay,az;
if ( line >> tim >> ax >> ay >> az )
{
vector<double> &v = list_acc[tim];
v.resize(3);
v[0] = ax;
v[1] = ay;
v[2] = az;
}
}
}
cout << "Parsed: " << list_depth.size() << " / " << list_rgb.size() << " / " << list_acc.size() << " depth/rgb/acc entries.\n";
// Create output directory for images ------------------------------
const string out_img_dir = out_name + string("_Images");
cout << "Creating images directory: " << out_img_dir << endl;
mrpt::system::createDirectory(out_img_dir);
// Create rawlog file ----------------------------------------------
const string out_rawlog_fil = out_name + string(".rawlog");
cout << "Creating rawlog: " << out_rawlog_fil << endl;
mrpt::utils::CFileGZOutputStream f_out(out_rawlog_fil);
// Fill out the common field to all entries:
CObservation3DRangeScan obs;
obs.sensorLabel = "KINECT";
obs.range_is_depth = true; // Kinect style: ranges are actually depth values, not Euclidean distances.
// Range & RGB images are already registered and warped to remove distortion:
const double FOCAL = 525.0;
obs.cameraParams.nrows = 640; obs.cameraParams.ncols = 480;
obs.cameraParams.fx(FOCAL); obs.cameraParams.fy(FOCAL);
obs.cameraParams.cx(319.5); obs.cameraParams.cy(239.5);
obs.cameraParamsIntensity = obs.cameraParams;
obs.relativePoseIntensityWRTDepth = mrpt::poses::CPose3D(0,0,0,DEG2RAD(-90),0,DEG2RAD(-90)); // No translation between rgb & range cameras, and rotation only due to XYZ axes conventions.
CObservationIMU obs_imu;
obs_imu.sensorLabel = "KINECT_ACC";
obs_imu.dataIsPresent[IMU_X_ACC] = true;
obs_imu.dataIsPresent[IMU_Y_ACC] = true;
obs_imu.dataIsPresent[IMU_Z_ACC] = true;
// Go thru the data:
unsigned int counter = 0;
for (map<double,string>::const_iterator it_list_rgb=list_rgb.begin();it_list_rgb!=list_rgb.end();++it_list_rgb)
{
cout << "Processing image " << counter<< "\r"; cout.flush();
counter++;
// This is not the most efficient solution in the world, but...come on! it's just a rawlog converter tool!
map<double,string>::const_iterator it_list_depth = find_closest( list_depth, it_list_rgb->first );
const double At = std::abs( it_list_rgb->first - it_list_depth->first );
if (At> (1./KINECT_FPS)*.5 )
{
cout << "\nWarning: Discarding observation for too separated RGB/D timestamps: " << At*1e3 << " ms\n";
}
else
{
// OK, we accept this RGB-DEPTH pair:
const double avrg_time = .5* (it_list_rgb->first + it_list_depth->first);
obs.timestamp = mrpt::system::time_tToTimestamp(avrg_time);
// RGB img:
obs.hasIntensityImage = true;
obs.intensityImage.loadFromFile( src_path + string("/") + it_list_rgb->second );
const string sRGBfile = mrpt::format("%.06f_rgb.png", avrg_time );
obs.intensityImage.saveToFile( out_img_dir + string("/") + sRGBfile );
obs.intensityImage.setExternalStorage(sRGBfile);
// Depth:
obs.hasRangeImage = true;
obs.rangeImage_forceResetExternalStorage();
mrpt::utils::CImage depth_img;
if (!depth_img.loadFromFile(src_path + string("/") + it_list_depth->second ))
throw std::runtime_error(string("Error loading depth image!: ") + it_list_depth->second);
const unsigned int w = depth_img.getWidth();
const unsigned int h = depth_img.getHeight();
obs.rangeImage_setSize(h,w);
for (unsigned int row=0;row<h;row++)
{
const uint16_t *ptr = reinterpret_cast<const uint16_t *>( depth_img.get_unsafe(0,row) );
for (unsigned int col=0;col<w;col++)
obs.rangeImage(row,col) = (*ptr++) * (1./5000);
}
const string sDepthfile = mrpt::format("%.06f_depth.bin", avrg_time );
obs.rangeImage_convertToExternalStorage( sDepthfile, out_img_dir + string("/") );
// save:
f_out << obs;
// Search for acc data:
map<double,vector<double> >::const_iterator it_list_acc = find_closest(list_acc ,avrg_time);
const double At_acc = std::abs( it_list_rgb->first - it_list_acc->first );
if (At_acc<1e-2)
{
obs_imu.timestamp = mrpt::system::time_tToTimestamp(avrg_time);
obs_imu.rawMeasurements[IMU_X_ACC] = it_list_acc->second[0];
obs_imu.rawMeasurements[IMU_Y_ACC] = it_list_acc->second[1];
obs_imu.rawMeasurements[IMU_Z_ACC] = it_list_acc->second[2];
f_out << obs_imu;
}
}
}
cout << "\nAll done!\n";
}
// ------------------------------------------------------
// MAIN
// ------------------------------------------------------
int main(int argc, char**argv)
{
try
{
if (argc!=3)
{
cerr << "Usage: " << argv[0] << " [PATH_TO_RGBD_DATASET] [OUTPUT_NAME]\n";
return 1;
}
rgbd2rawlog(argv[1],argv[2]);
return 0;
} catch (std::exception &e)
{
std::cout << "MRPT exception caught: " << e.what() << std::endl;
return -1;
}
catch (...)
{
printf("Untyped exception!!");
return -1;
}
}
