3dpcp/.svn/pristine/f2/f201e6a61a676209aa2e9a2a38e842f63733d361.svn-base
2012-09-16 14:33:11 +02:00

285 lines
7 KiB
Text

/**
* @file
* @brief Implementation of reading 3D scans
* @author Andreas Nuechter. Jacobs University Bremen gGmbH
* @author Jan Elseberg. Smart Systems Group, Jacobs University Bremen gGmbH, Germany.
*/
/*
#include <libxml/tree.h>
#include <libxml/parser.h>
#include <libxml/xpath.h>
#include <libxml/xpathInternals.h>
*/
#include <libxml/tree.h>
#include <libxml/parser.h>
#include <libxml/xpath.h>
#include <libxml/xpathInternals.h>
#include "slam6d/scan_io_rxp.h"
#include "slam6d/scan_io_riegl_project.h"
#include "riegl/scanlib.hpp"
#include "slam6d/globals.icc"
#include <fstream>
using std::ifstream;
#include <iostream>
using std::cerr;
using std::endl;
#include <string.h>
#include <algorithm>
using std::swap;
#ifdef _MSC_VER
#include <windows.h>
#endif
using namespace scanlib;
using namespace std;
using namespace std::tr1;
static vector<string> filenames;
static vector<double *> eulers;
static xmlNodePtr srSeekChildNodeNamed(xmlNodePtr p, const char * name){
if(p == NULL || name == NULL) return NULL;
for(p=p->children; p!= NULL; p=p->next){
if(p->name && (strcmp((char*)p->name,name) == 0)){
return p;
}
}
return NULL;
}
static void matrixToPos(const char* matrix, double *rPos, double *rPosTheta) {
stringstream ss( stringstream::in | stringstream::out);
ss << matrix;
double inMatrix[16], tMatrix[16];
for (int n=0; n<16; n++)
{
ss >> inMatrix[n];
}
tMatrix[0] = inMatrix[5];
tMatrix[1] = -inMatrix[9];
tMatrix[2] = -inMatrix[1];
tMatrix[3] = -inMatrix[13];
tMatrix[4] = -inMatrix[6];
tMatrix[5] = inMatrix[10];
tMatrix[6] = inMatrix[2];
tMatrix[7] = inMatrix[14];
tMatrix[8] = -inMatrix[4];
tMatrix[9] = inMatrix[8];
tMatrix[10] = inMatrix[0];
tMatrix[11] = inMatrix[12];
tMatrix[12] = -inMatrix[7];
tMatrix[13] = inMatrix[11];
tMatrix[14] = inMatrix[3];
tMatrix[15] = inMatrix[15];
Matrix4ToEuler(tMatrix, rPosTheta, rPos);
rPos[0] *= 100;
rPos[1] *= 100;
rPos[2] *= 100;
}
static int getPaths(const char* filename) {
double rPos[3];
double rPosTheta[3];
xmlDocPtr doc;
xmlXPathContextPtr xpathCtx;
xmlXPathObjectPtr xpathObj;
const xmlChar* names = BAD_CAST "/project/scanpositions/scanposition";
/* Load XML document */
doc = xmlParseFile(filename);
if (doc == NULL) {
fprintf(stderr, "Error: unable to parse file \"%s\"\n", filename);
return(-1);
}
/* Create xpath evaluation context */
xpathCtx = xmlXPathNewContext(doc);
if(xpathCtx == NULL) {
fprintf(stderr,"Error: unable to create new XPath context\n");
xmlFreeDoc(doc);
return(-1);
}
/* Evaluate xpath expression */
xpathObj = xmlXPathEvalExpression(names, xpathCtx);
// parse the scanpositions
xmlNodeSetPtr nodes = xpathObj->nodesetval;
int size;
int iter;
size = (nodes) ? nodes->nodeNr : 0;
for(iter = 0; iter < size; iter++) {
xmlNodePtr position = nodes->nodeTab[iter];
xmlNodePtr name = srSeekChildNodeNamed(position, "name");
xmlNodePtr singlescans = srSeekChildNodeNamed(position, "singlescans");
xmlNodePtr scan = srSeekChildNodeNamed(singlescans, "scan");
xmlNodePtr file = srSeekChildNodeNamed(scan, "file");
xmlNodePtr sop = srSeekChildNodeNamed(position, "sop");
xmlNodePtr matrix = srSeekChildNodeNamed(sop, "matrix");
char FILEN[1024];
char POSITIONN[1024];
char MATRIX[1024];
if (file != 0 && name != 0 && matrix != 0) {
sprintf(POSITIONN, "%s", xmlNodeGetContent(name ));
sprintf(FILEN, "%s", xmlNodeGetContent(file ));
sprintf(MATRIX, "%s", xmlNodeGetContent(matrix));
// compute pose
double *euler = new double[6];
matrixToPos(MATRIX, rPos, rPosTheta);
for (unsigned int i = 0; i < 3; i++) euler[i] = rPos[i];
for (unsigned int i = 0; i < 3; i++) euler[i+3] = rPosTheta[i];
eulers.push_back(euler);
// put together filename
string filename = "";
filename = "SCANS/" + (string)POSITIONN + "/SINGLESCANS/" + (string)FILEN + "";
//cout << rPos[0] << " " << rPos[1] << " " << rPos[2] << " " << filename << endl;
cout << "Project Scan " << filename << " is mapped to index " << iter << endl;
filenames.push_back(filename);
}
}
/* Cleanup of XPath data */
xmlXPathFreeObject(xpathObj);
xmlXPathFreeContext(xpathCtx);
/* free the document */
xmlFreeDoc(doc);
return(0);
}
/**
* Reads specified scans from given directory in
* the file format Riegl Laser Measurement GmbH
* uses. It will be compiled as shared lib.
*
* Scan poses will NOT be initialized after a call
* to this function. Initial pose estimation works
* only with the -p switch, i.e., trusting the initial
* estimations by Riegl.
*
* @param start Starts to read with this scan
* @param end Stops with this scan
* @param dir The directory from which to read
* @param maxDist Reads only Points up to this Distance
* @param minDist Reads only Points from this Distance
* @param euler Initital pose estimates (will not be applied to the points
* @param ptss Vector containing the read points
*/
int ScanIO_riegl_project::readScans(int start, int end, string &dir, int maxDist, int minDist,
double *euler, vector<Point> &ptss)
{
static int fileCounter = start;
// very first call to readScans...
if (fileCounter == start) {
// parse riegl projects xml file
string xmlFileName = dir + "project.rsp";
getPaths(xmlFileName.c_str());
}
string scanFileName;
string poseFileName;
if (end > -1 && fileCounter > end) return -1; // 'nuf read
scanFileName = "file://" + dir + filenames[fileCounter];
// read 3D scan
cout << "Processing Scan " << scanFileName;
cout.flush();
for (unsigned int i = 0; i <= 5; i++) euler[i] = eulers[fileCounter][i];
cout << " @ pose (" << euler[0] << "," << euler[1] << "," << euler[2]
<< "," << deg(euler[3]) << "," << deg(euler[4]) << "," << deg(euler[5]) << ")" << endl;
// open scanfile
std::tr1::shared_ptr<basic_rconnection> rc;
rc = basic_rconnection::create(scanFileName);
rc->open();
// decoder splits the binary file into readable chunks
decoder_rxpmarker dec(rc);
// importer interprets the chunks
importer imp(&ptss, maxDist, minDist);
// iterate over chunks
buffer buf;
for ( dec.get(buf); !dec.eoi(); dec.get(buf) ) {
imp.dispatch(buf.begin(), buf.end());
}
//done
rc->close();
fileCounter++;
return fileCounter-1;
}
/**
* 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_riegl_project;
}
/**
* 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