You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
228 lines
5.5 KiB
C++
228 lines
5.5 KiB
C++
/*
|
|
* scan_io_ply implementation
|
|
*
|
|
* Copyright (C) Dorit Borrmann, Thomas Escher, Kai Lingemann, Andreas Nuechter
|
|
*
|
|
* Released under the GPL version 3.
|
|
*
|
|
*/
|
|
|
|
|
|
/**
|
|
* @file
|
|
* @brief Implementation of reading 3D scans
|
|
* @author Dorit Borrmann. Jacobs University Bremen, Germany.
|
|
* @author Kai Lingemann. Inst. of CS, University of Osnabrueck, Germany.
|
|
* @author Andreas Nuechter. Jacobs University Bremen, Germany.
|
|
* @author Thomas Escher. Inst. of CS, University of Osnabrueck, Germany.
|
|
*/
|
|
|
|
#include "scanio/scan_io_ply.h"
|
|
#include "slam6d/point.h"
|
|
|
|
#include <iostream>
|
|
using std::cout;
|
|
using std::cerr;
|
|
using std::endl;
|
|
#include <vector>
|
|
#include <string.h>
|
|
|
|
#ifdef _MSC_VER
|
|
#include <windows.h>
|
|
#endif
|
|
|
|
#include <boost/filesystem/operations.hpp>
|
|
#include <boost/filesystem/fstream.hpp>
|
|
using namespace boost::filesystem;
|
|
|
|
#include "slam6d/globals.icc"
|
|
|
|
#define DATA_PATH_PREFIX "scan"
|
|
#define DATA_PATH_SUFFIX ".ply"
|
|
|
|
std::list<std::string> ScanIO_ply::readDirectory(const char* dir_path,
|
|
unsigned int start,
|
|
unsigned int end)
|
|
{
|
|
std::list<std::string> identifiers;
|
|
for (unsigned int i = start; i <= end; ++i) {
|
|
// identifier is /d/d/d (000-999)
|
|
std::string identifier(to_string(i,3));
|
|
// scan consists of data (.3d) and pose (.pose) files
|
|
path data(dir_path);
|
|
data /= path(std::string(DATA_PATH_PREFIX) + identifier + DATA_PATH_SUFFIX);
|
|
// stop if part of a scan is missing or end by absence is detected
|
|
if (!exists(data))
|
|
break;
|
|
identifiers.push_back(identifier);
|
|
}
|
|
return identifiers;
|
|
}
|
|
|
|
void ScanIO_ply::readPose(const char* dir_path,
|
|
const char* identifier,
|
|
double* pose)
|
|
{
|
|
for (unsigned int i = 0; i < 6; ++i) pose[i] = 0.0;
|
|
}
|
|
|
|
bool ScanIO_ply::supports(IODataType type)
|
|
{
|
|
return !!(type & (DATA_XYZ | DATA_REFLECTANCE | DATA_RGB));
|
|
}
|
|
|
|
void ScanIO_ply::readScan(const char* dir_path,
|
|
const char* identifier,
|
|
PointFilter& filter,
|
|
std::vector<double>* xyz,
|
|
std::vector<unsigned char>* rgb,
|
|
std::vector<float>* reflectance,
|
|
std::vector<float>* temperature,
|
|
std::vector<float>* amplitude,
|
|
std::vector<int>* type,
|
|
std::vector<float>* deviation)
|
|
{
|
|
unsigned int i;
|
|
|
|
// error handling
|
|
path data_path(dir_path);
|
|
data_path /= path(std::string(DATA_PATH_PREFIX) +
|
|
identifier + DATA_PATH_SUFFIX);
|
|
if(!exists(data_path))
|
|
throw std::runtime_error(std::string("There is no scan file for [")
|
|
+ identifier + "] in [" + dir_path + "]");
|
|
|
|
// open data file
|
|
ifstream data_file;
|
|
data_file.open(data_path);
|
|
data_file.exceptions(ifstream::eofbit|ifstream::failbit|ifstream::badbit);
|
|
|
|
if(xyz != 0 && rgb != 0 && reflectance != 0) {
|
|
|
|
// read ply file
|
|
bool binary = false;
|
|
char dummy[256];
|
|
char str[20]; // whatever size
|
|
double matrix[16];
|
|
int matrixPos = 0;
|
|
int nr;
|
|
float d1,d2,d3,d4;
|
|
|
|
// header
|
|
int counter = -2;
|
|
do {
|
|
if (counter > -2) counter++;
|
|
if (data_file.good()) {
|
|
data_file.getline(dummy, 255);
|
|
}
|
|
if (strncmp(dummy, "format", 6) == 0) {
|
|
if (dummy[7] == 'a') binary = false;
|
|
else if (dummy[7] == 'b') binary = true;
|
|
else { cerr << "Don't recognize the format!" << endl; exit(1); }
|
|
}
|
|
else if (strncmp(dummy, "element vertex", 14) == 0) {
|
|
sscanf(dummy,"%s %*s %d",str,&nr);
|
|
counter++;
|
|
}
|
|
else if (strncmp(dummy, "matrix", 6) == 0) {
|
|
sscanf(dummy,"%s %f %f %f %f", str, &d1, &d2, &d3, &d4);
|
|
matrix[matrixPos++] = d1;
|
|
matrix[matrixPos++] = d2;
|
|
matrix[matrixPos++] = d3;
|
|
matrix[matrixPos++] = d4;
|
|
}
|
|
} while (!(strncmp(dummy, "end_header",10) == 0 || !data_file.good()));
|
|
|
|
if (matrixPos > 0) {
|
|
double rPosTheta[3];
|
|
double rPos[3];
|
|
Matrix4ToEuler(matrix, rPosTheta, rPos);
|
|
}
|
|
|
|
for (int i=0; i < nr; i++) {
|
|
Point p;
|
|
float data, confidence, intensity;
|
|
float dummy;
|
|
int r, g, b;
|
|
if (!binary) {
|
|
switch(counter) {
|
|
case 6:
|
|
case 12:
|
|
data_file >> p.z >> p.y >> p.x >> r >> g >> b;
|
|
break;
|
|
case 9:
|
|
data_file >> p.z >> p.y >> p.x
|
|
>> dummy >> dummy >> dummy
|
|
>> r >> g >> b;
|
|
break;
|
|
default:
|
|
data_file >> p.z >> p.x >> p.y >> confidence >> intensity;
|
|
break;
|
|
}
|
|
if(counter == 6 || counter == 9 || counter == 12) {
|
|
p.rgb[0] = (char)r;
|
|
p.rgb[1] = (char)g;
|
|
p.rgb[2] = (char)b;
|
|
} else {
|
|
p.reflectance = intensity;
|
|
}
|
|
} else {
|
|
data_file.read((char*)&data, sizeof(float));
|
|
p.z = (double)data;
|
|
data_file.read((char*)&data, sizeof(float));
|
|
p.x = (double)data;
|
|
data_file.read((char*)&data, sizeof(float));
|
|
p.y = (double)data;
|
|
data_file.read((char*)&confidence, sizeof(float));
|
|
data_file.read((char*)&intensity, sizeof(float));
|
|
}
|
|
|
|
reflectance->push_back(p.reflectance);
|
|
xyz->push_back(p.x * 100);
|
|
xyz->push_back(p.y * 100);
|
|
xyz->push_back(p.z * 100);
|
|
rgb->push_back(static_cast<unsigned char>(p.rgb[0]));
|
|
rgb->push_back(static_cast<unsigned char>(p.rgb[1]));
|
|
rgb->push_back(static_cast<unsigned char>(p.rgb[2]));
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|
|
/**
|
|
* class factory for object construction
|
|
*
|
|
* @return Pointer to new object
|
|
*/
|
|
#ifdef _MSC_VER
|
|
extern "C" __declspec(dllexport) ScanIO* create()
|
|
#else
|
|
extern "C" ScanIO* create()
|
|
#endif
|
|
{
|
|
return new ScanIO_ply;
|
|
}
|
|
|
|
|
|
/**
|
|
* class factory for object construction
|
|
*
|
|
* @return Pointer to new object
|
|
*/
|
|
#ifdef _MSC_VER
|
|
extern "C" __declspec(dllexport) void destroy(ScanIO *sio)
|
|
#else
|
|
extern "C" void destroy(ScanIO *sio)
|
|
#endif
|
|
{
|
|
delete sio;
|
|
}
|
|
|
|
#ifdef _MSC_VER
|
|
BOOL APIENTRY DllMain(HANDLE hModule, DWORD dwReason, LPVOID lpReserved)
|
|
{
|
|
return TRUE;
|
|
}
|
|
#endif
|