508 lines
15 KiB
Text
508 lines
15 KiB
Text
|
/*
|
||
|
|
* 2DGridder implementation
|
||
|
|
*
|
||
|
|
* Copyright (C) Uwe Hebbelmann, Sebastian Stock, Andre Schemschat
|
||
|
|
*
|
||
|
|
* Released under the GPL version 3.
|
||
|
|
*
|
||
|
|
*/
|
||
|
|
|
||
|
|
#include "grid/scanGrid.h"
|
||
|
|
#include "grid/scanToGrid.h"
|
||
|
|
#include "slam6d/scan.h"
|
||
|
|
#include "grid/scanmanager.h"
|
||
|
|
#include "grid/parcelmanager.h"
|
||
|
|
#include "grid/gridWriter.h"
|
||
|
|
#include "grid/viewpointinfo.h"
|
||
|
|
#include "grid/gridlines.h"
|
||
|
|
#include "slam6d/globals.icc"
|
||
|
|
|
||
|
|
#include "grid/Debug.h"
|
||
|
|
|
||
|
|
#include <math.h>
|
||
|
|
#include <fstream>
|
||
|
|
#include <iostream>
|
||
|
|
#include <sstream>
|
||
|
|
#include <vector>
|
||
|
|
#include <dirent.h>
|
||
|
|
|
||
|
|
using std::cerr;
|
||
|
|
using std::flush;
|
||
|
|
|
||
|
|
#ifdef _MSC_VER
|
||
|
|
#define _USE_MATH_DEFINES
|
||
|
|
#include <windows.h>
|
||
|
|
#include "GL/glut.h" /* Header File For The glu toolkit */
|
||
|
|
#include "XGetopt.h"
|
||
|
|
#else
|
||
|
|
#include <getopt.h>
|
||
|
|
#include <strings.h>
|
||
|
|
#endif
|
||
|
|
|
||
|
|
/**
|
||
|
|
* 2DGridder
|
||
|
|
*
|
||
|
|
* Main class to convert a 3D scan to a 2D grid
|
||
|
|
*
|
||
|
|
* @author Andre Schemschadt, Uwe Hebbelmann, Sebastian Stock
|
||
|
|
* @date 20.02.2008
|
||
|
|
*/
|
||
|
|
|
||
|
|
/**
|
||
|
|
* Explains the usage of this program's command line parameters
|
||
|
|
*
|
||
|
|
* @param prog name of the program
|
||
|
|
*/
|
||
|
|
void usage(char* prog)
|
||
|
|
{
|
||
|
|
#ifndef _MSC_VER
|
||
|
|
const string bold("\033[1m");
|
||
|
|
const string normal("\033[m");
|
||
|
|
#else
|
||
|
|
const string bold("");
|
||
|
|
const string normal("");
|
||
|
|
#endif
|
||
|
|
|
||
|
|
cout << endl
|
||
|
|
<< "Usage: " << prog << endl
|
||
|
|
<< " [-s NR] [-e NR] [-m NR] [-M NR] [-f F] [-o DIR] [-t] " << endl
|
||
|
|
<< " [-h NR] [-H NR] [-r NR] [-w] [-p NR] [-P Nr] [-y] [-n] " <<endl
|
||
|
|
<< " [-g] [-d] [-l] [-a NR] inputdirectory" << endl << endl;
|
||
|
|
|
||
|
|
cout << " -s NR start at scan NR (i.e., neglects the first NR scans)" << endl
|
||
|
|
<< " [ATTENTION: counting starts with 0]" << endl
|
||
|
|
<< " -e NR end after scan NR" << "" << endl
|
||
|
|
<< " -m NR set the maximal range distance to NR 'units' (unit of scan data, e.g. cm)" << endl
|
||
|
|
<< " (default 2500, unit of scan data, e.g. cm)" << endl
|
||
|
|
<< " -M NR set the minimal range distance to NR 'units' (unit of scan data, e.g. cm)" << endl
|
||
|
|
<< " -f F format = F" << endl
|
||
|
|
<< " using shared library F for input" << endl
|
||
|
|
<< " (chose F from {uos, uos_map, uos_frames, uos_map_frames, old, rts, rts_map, ifp, riegl_bin, riegl_txt, zahn, ply})" << endl
|
||
|
|
<< " -o DIR the output directory (if none is set use the input directory" << endl
|
||
|
|
<< " -t read a file containing a initial transformation matrix" << endl
|
||
|
|
<< " -H NR set the maximal relevant height to NR 'units' (unit of scan data, e.g. cm)" << endl
|
||
|
|
<< " -h NR set the minimal relevant height to NR 'units' (unit of scan data, e.g. cm)" << endl
|
||
|
|
<< " -r NR the width of the gridunit (unit of scan data, e.g. cm per unit)" << endl
|
||
|
|
<< " -w default true, if set false " << endl
|
||
|
|
<< " (if true free points between viewpoint and points will be created)" << endl
|
||
|
|
<< " -p NR set the parcel width (unit of scan data, e.g. cm)" << endl
|
||
|
|
<< " -P NR set the parcel height (unit of scan data, e.g. cm)" << endl
|
||
|
|
<< " -y default false, if set true " <<endl
|
||
|
|
<< " (if true the transformationmatrix of the scans will be corrected (the value for Y)" << endl
|
||
|
|
<< " -n default true, if set false" << endl
|
||
|
|
<< " (if true, neighbours will be weighted) " << endl
|
||
|
|
<< " -a NR the spot diameter of the laser per 3000 units of scan data, e.g. cm " << endl
|
||
|
|
<< " (default 15 cm / 3000 cm) " << endl
|
||
|
|
<< " -g default false, if set true " << endl
|
||
|
|
<< " (if true, the grids will be written) " << endl
|
||
|
|
<< " -d default true, if set false " << endl
|
||
|
|
<< " (if true, the world will be written) " << endl
|
||
|
|
<< " -l default false, if set true " << endl
|
||
|
|
<< " (if true, lines will be created and written) " << endl
|
||
|
|
<< " -i default false, if set true " << endl
|
||
|
|
<< " (if true, a world ppm will be created) " << endl
|
||
|
|
<< " -c default 50. This is the numbers of scans which " << endl
|
||
|
|
<< " will be process at a time" << endl
|
||
|
|
<< " -R default false, if set the programm will resume " << endl
|
||
|
|
<< endl << endl;
|
||
|
|
|
||
|
|
exit(1);
|
||
|
|
}
|
||
|
|
|
||
|
|
/**
|
||
|
|
* Checks if the directory exists
|
||
|
|
* @param path the directory to check
|
||
|
|
* @return true if exists, else false
|
||
|
|
*/
|
||
|
|
bool directoryExists(const string path )
|
||
|
|
{
|
||
|
|
DIR *pDir;
|
||
|
|
bool bExists = false;
|
||
|
|
|
||
|
|
pDir = opendir(path.c_str());
|
||
|
|
|
||
|
|
if (pDir != NULL){
|
||
|
|
bExists = true;
|
||
|
|
closedir (pDir);
|
||
|
|
}
|
||
|
|
|
||
|
|
return bExists;
|
||
|
|
}
|
||
|
|
|
||
|
|
/**
|
||
|
|
* A function that parses the command-line arguments and sets the respective flags.
|
||
|
|
*
|
||
|
|
* @param argc the number of arguments
|
||
|
|
* @param argv the arguments
|
||
|
|
* @param inputdir parsing result - the input directory
|
||
|
|
* @param outputdir parsing result - the output directory
|
||
|
|
* @param start parsing result - starting at scan number 'start'
|
||
|
|
* @param end parsing result - stopping at scan number 'end'
|
||
|
|
* @param maxDist parsing result - maximal distance
|
||
|
|
* @param minDist parsing result - minimal distance
|
||
|
|
* @param readInitial parsing result - read a file containing a initial transformation matrix
|
||
|
|
* @param type parsing result - file format to be read
|
||
|
|
* @param minHeight the minimal relevant height
|
||
|
|
* @param maxHeight the maximal relevant height
|
||
|
|
* @param resolution the width of the gridunit (centimeters per unit)
|
||
|
|
* @param waypoints default true, if set false (if true free points between viewpoint and points will be created)
|
||
|
|
* @param parcel_width the width of the parcel
|
||
|
|
* @param parcel_height the height of the parcel
|
||
|
|
* @param correctY default true, if set false (if true the transformationmatrix of the scans will be corrected (the value for Y)
|
||
|
|
* @return 0, if the parsing was successful, 1 otherwise
|
||
|
|
*/
|
||
|
|
int parseArgs(int argc, char **argv,
|
||
|
|
string &inputdir, string &outputdir,
|
||
|
|
int& start, int& end, int& maxDist, int& minDist,
|
||
|
|
bool &readInitial, IOType &type, bool& correctY,
|
||
|
|
double &minHeight, double &maxHeight, long &resolution,
|
||
|
|
bool &waypoints, bool &neighbours,
|
||
|
|
int &parcelWidth, int &parcelHeight,
|
||
|
|
bool &writeWorld, bool &writeLines, bool &writeGrids,
|
||
|
|
int &spotradius, bool &writeWorldppm, int& count,
|
||
|
|
bool &resume)
|
||
|
|
{
|
||
|
|
int c;
|
||
|
|
|
||
|
|
// from unistd.h
|
||
|
|
extern char *optarg;
|
||
|
|
extern int optind;
|
||
|
|
|
||
|
|
cout << endl;
|
||
|
|
while ((c = getopt (argc, argv, "o:s:a:e:m:ncwgidlRM:h:H:f:r:p:P:yt")) != -1)
|
||
|
|
{
|
||
|
|
switch (c)
|
||
|
|
{
|
||
|
|
case 'o':
|
||
|
|
outputdir = optarg;
|
||
|
|
if (!directoryExists(outputdir)) {
|
||
|
|
cerr << "Error: Directory doesn't exist.\n";
|
||
|
|
exit(1);
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
case 's':
|
||
|
|
start = atoi(optarg);
|
||
|
|
if (start < 0) {
|
||
|
|
cerr << "Error: Cannot start at a negative scan number.\n";
|
||
|
|
exit(1);
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
case 'e':
|
||
|
|
end = atoi(optarg);
|
||
|
|
if (end < 0) {
|
||
|
|
cerr << "Error: Cannot end at a negative scan number.\n";
|
||
|
|
exit(1);
|
||
|
|
}
|
||
|
|
if (end < start) {
|
||
|
|
cerr << "Error: <end> cannot be smaller than <start>.\n";
|
||
|
|
exit(1);
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
case 'c':
|
||
|
|
count = atoi(optarg);
|
||
|
|
if( count < 1){
|
||
|
|
cerr << "Error: <count> must be greater than 1.\n";
|
||
|
|
exit(1);
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
case 'R':
|
||
|
|
resume = true;
|
||
|
|
break;
|
||
|
|
case 'm':
|
||
|
|
maxDist = atoi(optarg);
|
||
|
|
break;
|
||
|
|
case 'M':
|
||
|
|
minDist = atoi(optarg);
|
||
|
|
break;
|
||
|
|
case 't':
|
||
|
|
readInitial = true;
|
||
|
|
break;
|
||
|
|
case 'f':
|
||
|
|
try {
|
||
|
|
type = formatname_to_io_type(optarg);
|
||
|
|
} catch (...) { // runtime_error
|
||
|
|
cerr << "Format " << optarg << " unknown." << endl;
|
||
|
|
abort();
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
case 'H':
|
||
|
|
maxHeight = atoi(optarg);
|
||
|
|
break;
|
||
|
|
case 'h':
|
||
|
|
minHeight = atoi(optarg);
|
||
|
|
break;
|
||
|
|
case 'g':
|
||
|
|
writeGrids = true;
|
||
|
|
break;
|
||
|
|
case 'r':
|
||
|
|
resolution = atol(optarg);
|
||
|
|
if (resolution < 1) {
|
||
|
|
cerr << "Error: <resolution> cannot be smaller than 1.\n";
|
||
|
|
exit(1);
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
case 'w':
|
||
|
|
waypoints = false;
|
||
|
|
break;
|
||
|
|
case 'n':
|
||
|
|
neighbours = false;
|
||
|
|
break;
|
||
|
|
case 'p':
|
||
|
|
parcelWidth = atol(optarg);
|
||
|
|
if (parcelWidth < 1) {
|
||
|
|
cerr << "Error: <parcel_width> cannot be smaller than 1.\n";
|
||
|
|
exit(1);
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
case 'P':
|
||
|
|
parcelHeight = atol(optarg);
|
||
|
|
if (parcelHeight < 1) {
|
||
|
|
cerr << "Error: <parcel_height> cannot be smaller than 1.\n";
|
||
|
|
exit(1);
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
case 'y':
|
||
|
|
correctY = true;
|
||
|
|
break;
|
||
|
|
case 'a':
|
||
|
|
spotradius = atoi(optarg);
|
||
|
|
if (spotradius < 0) {
|
||
|
|
cerr << "Error: <spotradius> cannot be smaller than 0.\n";
|
||
|
|
exit(1);
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
case 'd':
|
||
|
|
writeWorld = false;
|
||
|
|
break;
|
||
|
|
case 'l':
|
||
|
|
writeLines = true;
|
||
|
|
break;
|
||
|
|
case 'i':
|
||
|
|
writeWorldppm = true;
|
||
|
|
break;
|
||
|
|
case '?':
|
||
|
|
usage(argv[0]);
|
||
|
|
return 1;
|
||
|
|
default:
|
||
|
|
abort();
|
||
|
|
}}
|
||
|
|
|
||
|
|
if (optind != argc-1) {
|
||
|
|
cerr << "\n*** Input directory missing ***" << endl;
|
||
|
|
usage(argv[0]);
|
||
|
|
}
|
||
|
|
inputdir = argv[optind];
|
||
|
|
|
||
|
|
//If no output directory is set, set it to the input directory
|
||
|
|
if (outputdir == "") outputdir = inputdir;
|
||
|
|
|
||
|
|
#ifndef _MSC_VER
|
||
|
|
if (inputdir[inputdir.length()-1] != '/') inputdir = inputdir + "/";
|
||
|
|
if (outputdir[outputdir.length()-1] != '/') outputdir = outputdir + "/";
|
||
|
|
#else
|
||
|
|
if (inputdir[inputdir.length()-1] != '\\') inputdir = inputdir + "\\";
|
||
|
|
if (outputdir[outputdir.length()-1] != '\\') outputdir = outputdir + "\\";
|
||
|
|
#endif
|
||
|
|
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
/**
|
||
|
|
* Main function.
|
||
|
|
* Reads the scan (scan000.3d, ...) and frames files (scan000.frames, ...) from the data directory.
|
||
|
|
* The frames are used for animation of the matching process. Converts the 3D scan data to a 2D grid map
|
||
|
|
* and stores it
|
||
|
|
*
|
||
|
|
* @param argc count of the command-line arguments
|
||
|
|
* @param argv command-line arguments
|
||
|
|
*/
|
||
|
|
int main(int argc, char **argv){
|
||
|
|
// Start message
|
||
|
|
cout << "(c) University of Osnabrueck, 2006 - 2008" << endl << endl
|
||
|
|
<< "Restricted Usage" << endl
|
||
|
|
<< "Don't use without permission" << endl;
|
||
|
|
|
||
|
|
|
||
|
|
// Usage
|
||
|
|
if(argc <= 1){
|
||
|
|
usage(argv[0]);
|
||
|
|
}
|
||
|
|
|
||
|
|
///////////////////////////////////////
|
||
|
|
// Defaultvalues
|
||
|
|
string scandir = "";
|
||
|
|
string outputdir = "./parcels/";
|
||
|
|
|
||
|
|
int start = 0;
|
||
|
|
int end = -1;
|
||
|
|
int maxDistance = 2500;
|
||
|
|
int minDistance = -1;
|
||
|
|
bool readInitial = false;
|
||
|
|
IOType scantype = UOS;
|
||
|
|
bool correctY = false;
|
||
|
|
|
||
|
|
double maxRelevantHeight = 50;
|
||
|
|
double minRelevantHeight = 2;
|
||
|
|
long resolution = 10;
|
||
|
|
bool createWaypoints = true;
|
||
|
|
bool createNeighbours = true;
|
||
|
|
|
||
|
|
int parcelWidth = 1000;
|
||
|
|
int parcelHeight = 1000;
|
||
|
|
|
||
|
|
int spotradius = 15;
|
||
|
|
|
||
|
|
bool writeWorld = true;
|
||
|
|
bool writeLines = false;
|
||
|
|
bool writeGrids = false;
|
||
|
|
bool writeWorldppm = false;
|
||
|
|
|
||
|
|
double isSolidPoint = 0.2;
|
||
|
|
int count = 50;
|
||
|
|
bool resume = false;
|
||
|
|
///////////////////////////////////////
|
||
|
|
|
||
|
|
|
||
|
|
// parses the command-line arguments and sets the respective flags
|
||
|
|
parseArgs(argc, argv,
|
||
|
|
scandir, outputdir, start, end, maxDistance, minDistance,
|
||
|
|
readInitial, scantype, correctY,
|
||
|
|
minRelevantHeight, maxRelevantHeight, resolution,
|
||
|
|
createWaypoints, createNeighbours,
|
||
|
|
parcelWidth, parcelHeight,
|
||
|
|
writeWorld, writeLines, writeGrids, spotradius, writeWorldppm,
|
||
|
|
count, resume);
|
||
|
|
|
||
|
|
// calculate parcel width and height
|
||
|
|
parcelWidth /= resolution;
|
||
|
|
parcelHeight /= resolution;
|
||
|
|
|
||
|
|
// create parcelmanager
|
||
|
|
cout << "Create parcelmanager ..." << endl;
|
||
|
|
parcelmanager parcelman(parcelWidth, parcelHeight,
|
||
|
|
outputdir, resolution, resume);
|
||
|
|
|
||
|
|
cout << "Create viewpointlist ... " << endl;
|
||
|
|
viewpointinfo viewpoint(outputdir);
|
||
|
|
|
||
|
|
for (int i = start; i <= end; i+=count)
|
||
|
|
{
|
||
|
|
int endloop = i + count - 1 < end ? i + count - 1 : end;
|
||
|
|
|
||
|
|
// get Scans
|
||
|
|
cout << "Reading scans " << i << " to " << endloop << " ... ";
|
||
|
|
scanmanager scanman;
|
||
|
|
scanman.startscan(scandir, outputdir, scantype, i, endloop,
|
||
|
|
readInitial, maxDistance, minDistance,
|
||
|
|
correctY);
|
||
|
|
|
||
|
|
cout << "Done."<< endl;
|
||
|
|
|
||
|
|
// create grid from scans
|
||
|
|
cout << "Creating grids " << i << " to " << endloop << " ... ";
|
||
|
|
scanToGrid stg(resolution,
|
||
|
|
minRelevantHeight,
|
||
|
|
maxRelevantHeight,
|
||
|
|
maxDistance,
|
||
|
|
spotradius,
|
||
|
|
createWaypoints,
|
||
|
|
createNeighbours);
|
||
|
|
|
||
|
|
cout << "Done."<< endl;
|
||
|
|
|
||
|
|
// convert scans
|
||
|
|
cout << "Converting " << scanman.getScanCount() <<" scans ... ";
|
||
|
|
|
||
|
|
vector<scanGrid*> grids;
|
||
|
|
for(size_t j = 0; j < scanman.getScanCount(); j++)
|
||
|
|
{
|
||
|
|
cout << "." << flush;
|
||
|
|
double* p = scanman.getMatrix(j).back();
|
||
|
|
scanman.getScan(j).transformAll(p);
|
||
|
|
grids.push_back(stg.convert(scanman.getScan(j), p));
|
||
|
|
}
|
||
|
|
cout << "Done." << endl;
|
||
|
|
|
||
|
|
// start writing
|
||
|
|
cout << "Processing from " << i << " to " << endloop << endl;
|
||
|
|
for(size_t j = 0; j < grids.size(); ++j)
|
||
|
|
{
|
||
|
|
cout << "Adding scan " << i << " ... " << flush;
|
||
|
|
parcelman.addGrid(grids[j],
|
||
|
|
grids[j]->getViewpointX(),
|
||
|
|
grids[j]->getViewpointZ());
|
||
|
|
|
||
|
|
viewpoint.addGrid(grids[j]);
|
||
|
|
cout << "Done."<< endl;
|
||
|
|
}
|
||
|
|
|
||
|
|
// print grids for each scan if wished
|
||
|
|
if(writeGrids)
|
||
|
|
{
|
||
|
|
cout << "Writing grids " << i
|
||
|
|
<< " to " << endloop << " ... " << endl;
|
||
|
|
|
||
|
|
string str;
|
||
|
|
stringstream stream;
|
||
|
|
for(size_t j = 0; j < grids.size(); ++j)
|
||
|
|
{
|
||
|
|
cout << "Writing grid " << i << " ... " << flush;
|
||
|
|
stream.clear();
|
||
|
|
stream << outputdir << "grid" << j+i << ".ppm ";
|
||
|
|
stream >> str;
|
||
|
|
|
||
|
|
ppmWriter writer(str);
|
||
|
|
writer.write(*grids[j]);
|
||
|
|
cout << "Done."<<endl;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
cout << "Freeing subdate ... "<< flush;
|
||
|
|
for(size_t j = 0; j < grids.size(); ++j)
|
||
|
|
delete grids[j];
|
||
|
|
grids.clear();
|
||
|
|
cout << "Done."<< endl;
|
||
|
|
}
|
||
|
|
|
||
|
|
// write the world and the viewpoints
|
||
|
|
if(writeWorld)
|
||
|
|
{
|
||
|
|
// write world map
|
||
|
|
cout << "Writing world map ... " << flush;
|
||
|
|
parcelman.writeWorld("world.2dm");
|
||
|
|
cout << "done." << endl;
|
||
|
|
|
||
|
|
// write viewpoints
|
||
|
|
cout << "Writing viewpoints ... " << flush;
|
||
|
|
viewpoint.write("viewpoints.pts");
|
||
|
|
cout << "Done." << endl;
|
||
|
|
}
|
||
|
|
|
||
|
|
// write gridlines
|
||
|
|
if(writeLines || writeWorldppm)
|
||
|
|
{
|
||
|
|
cout << "Creating world grid ... ";
|
||
|
|
grid *g = parcelman.createWorldGrid();
|
||
|
|
cout << "Done." << endl;
|
||
|
|
|
||
|
|
if(writeLines) {
|
||
|
|
cout << "Writing Lines ... ";
|
||
|
|
gridlines glines(g, maxDistance, isSolidPoint);
|
||
|
|
glines.writeLin(outputdir + "/lines.plot");
|
||
|
|
cout << "Done." << endl;
|
||
|
|
}
|
||
|
|
|
||
|
|
if(writeWorldppm) {
|
||
|
|
cout << "Writing world ppm ... ";
|
||
|
|
ppmWriter writer(outputdir + "world.ppm");
|
||
|
|
writer.write(*g);
|
||
|
|
cout << "Done." << endl;
|
||
|
|
}
|
||
|
|
|
||
|
|
delete g;
|
||
|
|
}
|
||
|
|
|
||
|
|
cout << "Freeing data ... " << endl;
|
||
|
|
}
|