3dpcp/.svn/pristine/28/285085933c29eb9ada8d86d00a14a0ce0932294b.svn-base

323 lines
9.1 KiB
Text
Raw Normal View History

2012-09-16 12:33:11 +00:00
/*
* plane implementation
*
* Copyright (C) Dorit Borrmann
*
* Released under the GPL version 3.
*
*/
/**
* @file
* @author Dorit Borrmann. Institute of Computer Science, University of Osnabrueck, Germany.
*/
#include <cfloat>
#include <fstream>
#ifndef _MSC_VER
#include <getopt.h>
#else
#include "XGetopt.h"
#endif
#include <iostream>
using std::ofstream;
using std::flush;
using std::cout;
using std::string;
using std::cerr;
using std::endl;
#include <errno.h>
#ifdef _MSC_VER
#define strcasecmp _stricmp
#define strncasecmp _strnicmp
#include <windows.h>
#include <direct.h>
#else
#include <sys/stat.h>
#include <sys/types.h>
#include <strings.h>
#include <dlfcn.h>
#endif
#include "shapes/hough.h"
#include "shapes/shape.h"
#include "shapes/ransac.h"
enum plane_alg {
RHT, SHT, PHT, PPHT, APHT, RANSAC
};
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
<< bold << "USAGE " << normal << endl
<< " " << prog << " [options] directory" << endl << endl;
cout << bold << "OPTIONS" << normal << endl
<< bold << " -f" << normal << " F, " << bold << "--format=" << normal << "F" << endl
<< " using shared library F for input" << endl
<< " (chose F from {uos, uos_map, uos_rgb, uos_frames, uos_map_frames, old, rts, rts_map, ifp, riegl_txt, riegl_rgb, riegl_bin, zahn, ply})" << endl
<< endl
<< bold << " -m" << normal << " NR, " << bold << "--max=" << normal << "NR" << endl
<< " neglegt all data points with a distance larger than NR 'units'" << endl
<< endl
<< bold << " -M" << normal << " NR, " << bold << "--min=" << normal << "NR" << endl
<< " neglegt all data points with a distance smaller than NR 'units'" << endl
<< endl
<< bold << " -p" << normal << " P, " << bold << "--plane=" << normal << "P" << endl
<< " using algorithm P for plane detection" << endl
<< " (chose P from {rht, sht, pht, ppht, apht, ran})" << endl
<< endl
<< bold << " -r" << normal << " NR, " << bold << "--reduce=" << normal << "NR" << endl
<< " turns on octree based point reduction (voxel size=<NR>)" << endl
<< endl
<< bold << " -O" << normal << " NR (optional), " << bold << "--octree=" << normal << "NR (optional)" << endl
<< " use randomized octree based point reduction (pts per voxel=<NR>)" << endl
<< " requires " << bold << "-r" << normal <<" or " << bold << "--reduce" << endl
<< endl
<< bold << " -s" << normal << " NR, " << bold << "--start=" << normal << "NR" << endl
<< " start at scan NR (i.e., neglects the first NR scans)" << endl
<< " [ATTENTION: counting naturally starts with 0]" << endl
<< endl
<< bold << " -S, --scanserver" << normal << endl
<< " Use the scanserver as an input method and handling of scan data" << endl
<< endl
<< endl << endl;
cout << bold << "EXAMPLES " << normal << endl
<< " " << prog << " -m 500 -r 5 dat" << endl
<< " " << prog << " --max=5000 -r 10.2 dat" << endl
<< " " << prog << " -s 2 -e 10 -r dat" << endl << endl;
exit(1);
}
/**
* Parses command line arguments needed for plane detection. For details about
* the argument see usage().
*/
int parseArgs(int argc, char **argv, string &dir, double &red, int &start, int
&maxDist, int&minDist, int &octree, IOType &type, plane_alg &alg, bool
&quiet, bool& scanserver)
{
bool reduced = false;
int c;
// from unistd.h:
extern char *optarg;
extern int optind;
/* options descriptor */
// 0: no arguments, 1: required argument, 2: optional argument
static struct option longopts[] = {
{ "format", required_argument, 0, 'f' },
{ "max", required_argument, 0, 'm' },
{ "min", required_argument, 0, 'M' },
{ "start", required_argument, 0, 's' },
{ "reduce", required_argument, 0, 'r' },
{ "plane", required_argument, 0, 'p' },
{ "quiet", no_argument, 0, 'q' },
{ "octree", optional_argument, 0, 'O' },
{ "scanserver", no_argument, 0, 'S' },
{ 0, 0, 0, 0} // needed, cf. getopt.h
};
cout << endl;
while ((c = getopt_long(argc, argv, "f:r:s:e:m:M:p:O:q", longopts, NULL)) != -1)
switch (c)
{
case 'r':
red = atof(optarg);
reduced = true;
break;
case 's':
start = atoi(optarg);
if (start < 0) { cerr << "Error: Cannot start at a negative scan number.\n"; exit(1); }
break;
case 'f':
try {
type = formatname_to_io_type(optarg);
} catch (...) { // runtime_error
cerr << "Format " << optarg << " unknown." << endl;
abort();
}
break;
case 'p':
if(strcasecmp(optarg, "rht") == 0) alg = RHT;
else if(strcasecmp(optarg, "sht") == 0) alg = SHT;
else if(strcasecmp(optarg, "pht") == 0) alg = PHT;
else if(strcasecmp(optarg, "ppht") == 0) alg = PPHT;
else if(strcasecmp(optarg, "apht") == 0) alg = APHT;
else if(strcasecmp(optarg, "ran") == 0) alg = RANSAC;
else abort();
break;
case 'q':
quiet = true;
break;
case 'm':
maxDist = atoi(optarg);
break;
case 'O':
if (optarg) {
octree = atoi(optarg);
} else {
octree = 1;
}
break;
case 'M':
minDist = atoi(optarg);
break;
case 'S':
scanserver = true;
break;
case '?':
usage(argv[0]);
return 1;
default:
abort ();
}
if (optind != argc-1) {
cerr << "\n*** Directory missing ***" << endl;
usage(argv[0]);
}
dir = argv[optind];
#ifndef _MSC_VER
if (dir[dir.length()-1] != '/') dir = dir + "/";
#else
if (dir[dir.length()-1] != '\\') dir = dir + "\\";
#endif
return 0;
}
/**
* Main function. The Hough Transform or RANSAC are called for the scan indicated as
* argument.
*
*/
int main(int argc, char **argv)
{
cout << "(c) Jacobs University Bremen, gGmbH, 2010" << endl << endl;
if (argc <= 1) {
usage(argv[0]);
}
// parsing the command line parameters
// init, default values if not specified
string dir;
double red = -1.0;
int start = 0;
int maxDist = -1;
int minDist = -1;
int octree = 0;
bool quiet = false;
IOType type = UOS;
plane_alg alg = RHT;
bool scanserver = false;
cout << "Parse args" << endl;
parseArgs(argc, argv, dir, red, start, maxDist, minDist, octree, type, alg, quiet, scanserver);
int fileNr = start;
string planedir = dir + "planes";
#ifdef _MSC_VER
int success = mkdir(planedir.c_str());
#else
int success = mkdir(planedir.c_str(), S_IRWXU|S_IRWXG|S_IRWXO);
#endif
if(success == 0) {
if(!quiet) {
cout << "Writing planes to " << planedir << endl;
}
} else if(errno == EEXIST) {
cout << "Directory " << planedir << " exists already. CONTINUE" << endl;
} else {
cerr << "Creating directory " << planedir << " failed" << endl;
exit(1);
}
Scan::openDirectory(scanserver, dir, type, fileNr, fileNr);
Scan* scan = Scan::allScans.front();
scan->setRangeFilter(maxDist, minDist);
scan->setReductionParameter(red, octree);
// scan->setSearchTreeParameter(nns_method, use_cuda);
scan->toGlobal();
double id[16];
M4identity(id);
for(int i = 0; i < 10; i++) {
scan->transform(id, Scan::ICP, 0); // write end pose
}
if (!quiet) cout << "start plane detection" << endl;
long starttime = GetCurrentTimeInMilliSec();
if(alg >= RANSAC) {
vector<double *> points;
CollisionPlane<double> * plane;
plane = new CollisionPlane<double>(1.0); // 1.0 cm maxdist
Ransac(*plane, Scan::allScans[0], &points);
starttime = (GetCurrentTimeInMilliSec() - starttime);
cout << "nr points " << points.size() << endl;
double nx,ny,nz,d;
plane->getPlane(nx,ny,nz,d);
if(!quiet) cout << "DONE " << endl;
if(!quiet) cout << nx << " " << ny << " " << nz << " " << d << endl;
/**
for (unsigned int i = 0; i < points.size(); i++) {
cerr << points[i][0] << " " << points[i][1] << " " << points[i][2] << endl;
}
*/
for(int i = points.size() - 1; i > -1; i--) {
delete[] points[i];
}
delete plane;
} else {
Hough hough(Scan::allScans[0], quiet);
starttime = (GetCurrentTimeInMilliSec() - starttime);
cout << "Time for Constructor call: " << starttime << endl;
starttime = GetCurrentTimeInMilliSec();
if (!quiet) cout << "algorithm: " << alg << endl;
// choose Hough method here
switch(alg) {
case RHT: hough.RHT();
break;
case SHT: hough.SHT();
break;
case PHT: hough.PHT();
break;
case PPHT: hough.PPHT();
break;
case APHT: hough.APHT();
break;
default: usage(argv[0]);
exit(1);
break;
}
hough.writePlanes(0);
cout << "Write Planes done" << endl;
starttime = (GetCurrentTimeInMilliSec() - starttime);
}
cout << "Time for Plane Detection " << starttime << endl;
delete Scan::allScans[0];
Scan::allScans.clear();
}