3dpcp/.svn/pristine/32/32250353bae1cc0945b11ff22f9065c0f50af294.svn-base
2012-09-16 14:33:11 +02:00

287 lines
8.3 KiB
Text

/*
* scan_io_rxp implementation
*
* Copyright (C) Thomas Escher, Kai Lingemann, Andreas Nuechter
*
* Released under the GPL version 3.
*
*/
/**
* @file
* @brief Implementation of reading 3D scans
* @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Thomas Escher
*/
#include "scanio/scan_io_rxp.h"
#include "riegl/scanlib.hpp"
#include <iostream>
using std::cout;
using std::cerr;
using std::endl;
#include <vector>
#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"
using namespace scanlib;
using namespace std;
using namespace std::tr1;
#define DATA_PATH_PREFIX "scan"
#define DATA_PATH_SUFFIX ".rxp"
#define POSE_PATH_PREFIX "scan"
#define POSE_PATH_SUFFIX ".pose"
/*
TODO: this file is still work in progress for change to the new scanserver workflow
this ScanIO has to distinguish a multi scan file and a directory of single scan files and is currently very messy handling these with the importer class
*/
std::list<std::string> ScanIO_rxp::readDirectory(const char* dir_path, unsigned int start, unsigned int end)
{
std::list<std::string> identifiers;
path pose(dir_path);
if(is_regular_file(pose)) {
// TODO: create identifiers for this case
// a) from start to end, requires sanity checks and goodwill of user
// b) iterate through the file and get last index
// c) check last index by other means through the riegl api?
// TEMP: implementing a) without sanity checks :)
for(unsigned int i = start; i < end; ++i) {
identifiers.push_back(to_string(i,3));
}
} else {
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);
path pose(dir_path);
pose /= path(std::string(POSE_PATH_PREFIX) + identifier + POSE_PATH_SUFFIX);
// stop if part of a scan is missing or end by absence is detected
if(!exists(data) || !exists(pose))
break;
identifiers.push_back(identifier);
}
}
return identifiers;
}
void ScanIO_rxp::readPose(const char* dir_path, const char* identifier, double* pose)
{
unsigned int i;
path pose_path(dir_path);
// if the directory actually marks a (multi scan) file, return zero pose
// TODO: test if pose_path gets constructed correctly, see removal of trailing / in the old code
if(is_regular_file(pose_path.string())) {
for(i = 0; i < 6; ++i) pose[i] = 0.0;
return;
}
pose_path /= path(std::string(POSE_PATH_PREFIX) + identifier + POSE_PATH_SUFFIX);
if(!exists(pose_path))
throw std::runtime_error(std::string("There is no pose file for [") + identifier + "] in [" + dir_path + "]");
// open pose file
boost::filesystem::ifstream pose_file(pose_path);
// if the file is open, read contents
if(pose_file.good()) {
// read 6 plain doubles
for(i = 0; i < 6; ++i) pose_file >> pose[i];
pose_file.close();
// convert angles from deg to rad
for(i = 3; i < 6; ++i) pose[i] = rad(pose[i]);
} else {
throw std::runtime_error(std::string("Pose file could not be opened for [") + identifier + "] in [" + dir_path + "]");
}
}
bool ScanIO_rxp::supports(IODataType type)
{
return !!(type & (DATA_XYZ | DATA_REFLECTANCE | DATA_AMPLITUDE | DATA_DEVIATION | DATA_TYPE));
}
void ScanIO_rxp::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>* amplitude, std::vector<int>* type, std::vector<float>* deviation)
{
path data_path(dir_path);
// distinguish file and directory
if(is_regular_file(data_path)) {
stringstream str(identifier);
int scan_index;
str >> scan_index;
// first clean up if they "directory" has changed
// second case of resetting: rewinding index in case of non-sequential index access
if(old_path != dir_path || (imp != 0 && imp->getCurrentScan() > scan_index)) {
// TODO: I'm assuming here that I can simply do this
if(rc != 0) { rc->close(); }
if(dec != 0) { delete dec; dec = 0; }
if(imp != 0) { delete imp; imp = 0; }
// TODO ^
}
// create directory-persistent decoder
if (!dec) {
// remember path
old_path = dir_path;
// create new connection
rc = basic_rconnection::create(data_path.string());
rc->open();
// decoder splits the binary file into readable chunks
dec = new decoder_rxpmarker(rc);
// importer interprets the chunks
// TODO: this won't really work because filter is gone and xyz too and everything breaks
// probably set new filter and xyz in the importer
imp = new importer(filter, scan_index, xyz, reflectance, amplitude, type, deviation);
}
// set new filter and vectors
imp->set(filter, xyz, reflectance, amplitude, type, deviation);
buffer buf;
// skip the first scans
if(imp->getCurrentScan() < scan_index) {
for(dec->get(buf); !dec->eoi(); dec->get(buf)) {
imp->dispatch(buf.begin(), buf.end());
if(imp->getCurrentScan() >= scan_index) break;
}
}
if(dec->eoi()) return;
int cscan = imp->getCurrentScan();
// iterate over chunks, until the next scan is reached
for(dec->get(buf); !dec->eoi(); dec->get(buf)) {
imp->dispatch(buf.begin(), buf.end());
if(imp->getCurrentScan() != cscan) break;
}
return;
}
// error handling
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 + "]");
if(xyz != 0) {
string data_path_str;
data_path_str = "file://" + data_path.string();
rc = basic_rconnection::create(data_path_str);
rc->open();
// decoder splits the binary file into readable chunks
dec = new decoder_rxpmarker(rc);
// importer interprets the chunks
imp = new importer(filter, 0, xyz, reflectance, amplitude, type, deviation);
// iterate over chunks
buffer buf;
for(dec->get(buf); !dec->eoi(); dec->get(buf)) {
imp->dispatch(buf.begin(), buf.end());
}
//done
rc->close();
// TODO: clean up all these pointers, aren't they dangling?
}
}
void importer::on_echo_transformed(echo_type echo)
{
// for multi scan files, ignore those before start
if (currentscan < start) return;
// targets is a member std::vector that contains all
// echoes seen so far, i.e. the current echo is always
// indexed by target_count-1.
target& t(targets[target_count - 1]);
double point[3];
point[0] = t.vertex[1]*-100.0;
point[1] = t.vertex[2]*100.0;
point[2] = t.vertex[0]*100.0;
if(filter->check(point)) {
if(xyz) {
for(unsigned int i = 0; i < 3; ++i) xyz->push_back(point[i]);
}
if(reflectance) reflectance->push_back(t.reflectance);
if(amplitude) amplitude->push_back(t.amplitude);
if(deviation) deviation->push_back(t.deviation);
if(type) {
if(pointcloud::first == echo) type->push_back(0);
else if(pointcloud::interior == echo) type->push_back(1);
else if(pointcloud::last == echo) type->push_back(10);
else if(pointcloud::single == echo) type->push_back(9);
}
}
/*
// target.reflectance
// target.amplitude
// target.deviation
// target.time
// target.vertex point coordinates
*/
}
/**
* 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_rxp;
}
/**
* 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