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

272 lines
8.5 KiB
Text

/**
* @file model.cc
*
* @auhtor Remus Claudiu Dumitru <r.dumitru@jacobs-university.de>
* @date 12 Feb 2012
*
*/
//==============================================================================
// Defines
//==============================================================================
//==============================================================================
// Includes
//==============================================================================
#include "model/scene.h"
#include "model/util.h"
#include "model/graphicsAlg.h"
#include "slam6d/io_utils.h"
#include "slam6d/io_types.h"
#include <stdio.h>
#include <stdlib.h>
#include <ctype.h>
#ifdef _MSC_VER
#define strcasecmp _stricmp
#define strncasecmp _strnicmp
#else
#include <strings.h>
#endif
#ifndef _MSC_VER
#include <getopt.h>
#else
#include "XGetopt.h"
#endif
#include <string>
#include <iostream>
using namespace std;
//==============================================================================
// Global Variables
//==============================================================================
model::Scene* scene = NULL; // the scene containing all data
bool quiet = false; // no output will be shown
//==============================================================================
// Functions
//==============================================================================
/**
* Prints the usage of the program to stdout.
* @param programName the name of the current program
*/
void printUsage(const string& programName) {
#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
<< "\t" << programName << " [options] directory" << endl << endl;
cout << bold << "OPTIONS" << normal << endl
<< bold << "\t -f" << normal << " F, " << bold << "--format=" << normal << "F" << endl
<< "\t\t using shared library F for input" << endl
<< "\t\t (chose F from {uos, uos_map, uos_rgb, uos_frames, uos_map_frames, old, rxp, rts, rts_map, ifp, riegl_txt, riegl_rgb, riegl_bin, zahn, ply})" << endl
<< endl
// TODO enable the usage of all algos, use rht for now
<< bold << "\t -p" << normal << " P, " << bold << "--plane=" << normal << "P" << endl
<< "\t\t using algorithm P for plane detection" << endl
<< "\t\t (chose P from {rht, sht, pht, ppht, apht, ran})" << endl
<< endl
<< bold << "\t -s" << normal << " NR, " << bold << "--start=" << normal << "NR" << endl
<< "\t\t start at scan NR (i.e., neglects the first NR scans)" << endl
<< "\t\t [ATTENTION: counting naturally starts with 0]" << endl
<< endl
<< bold << "\t -S, --scanserver" << normal << endl
<< "\t\t Use the scanserver as an input method and handling of scan data" << endl
<< endl
<< bold << "\t -e" << normal << " NR, " << bold << "--end=" << normal << "NR" << endl
<< "\t\t end at scan NR (i.e., neglects the scans following NR)" << endl
<< endl;
exit(EXIT_SUCCESS);
}
/**
* Parses program arguments.
*/
void parseArgs(int argc, char* argv[],
string& dir, int& start, int& end, IOType &type, PlaneAlgorithm& alg,
bool& scanserver)
{
char c;
int index;
// from unistd.h:
extern char *optarg;
extern int optind;
WriteOnce<IOType> w_type(type);
WriteOnce<int> w_start(start), w_end(end);
// define the known flags in an option structure
static struct option long_options[] = {
{"start", required_argument, 0, 's'},
{"end", required_argument, 0, 'e'},
{"format", required_argument, 0, 'f'},
{"plane", required_argument, 0, 'p'},
{"help", no_argument, 0, 'h'},
{"scanserver", no_argument, 0, 'S'},
{"quiet", no_argument, 0, 'q'}
};
// loop over the arguments
while ((c = getopt_long(argc, argv, "s:e:f:p:hq", long_options, &index)) != -1) {
switch (c) {
case 's':
w_start = atoi(optarg);
if (start < 0) { cerr << "Error: Cannot start at a negative scan number.\n"; exit(1); }
break;
case 'e':
w_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 'f':
try {
w_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 printUsage(argv[0]);
break;
case 'q':
quiet = true;
break;
case 'S':
scanserver = true;
break;
case 'h':
case '?':
printUsage(argv[0]);
break;
default:
throw runtime_error("getopt_long detected invalid flag");
break;
}
}
if (optind != argc-1) {
cerr << "\n*** Directory missing ***" << endl;
printUsage(argv[0]);
}
dir = argv[optind];
#ifndef _MSC_VER
if (dir[dir.length()-1] != '/') dir = dir + "/";
#else
if (dir[dir.length()-1] != '\\') dir = dir + "\\";
#endif
parseFormatFile(dir, w_type, w_start, w_end);
}
/**
* Cleanup function that deallocates memory and cleans up other stuff...
*/
void cleanup() {
if (!quiet) cout << endl << "== Cleaning up..." << endl;
if (scene != NULL) {
delete scene;
}
if (!quiet) cout << endl << "== Exiting..." << endl;
}
//==============================================================================
// Main
//==============================================================================
int main(int argc, char* argv[]) {
// seed the RNG
srand(time(NULL));
// default values for possible parse parameters
IOType type = UOS;
bool scanserver = false;
// beginning and ending scan number
int start = 0;
int end = 0;
// path to directory containing the scans
string dir;
// minimum and maximum scan distances
int maxDist = -1;
int minDist = -1;
int octree = 1;
double red = -1.0;
// which plane algorithm to perform
PlaneAlgorithm alg = RHT;
// parse the command line arguments before registering and signals and atexit callbacks
parseArgs(argc, argv,
dir, start, end, type, alg,
scanserver);
// register cleanup function to deallocate memory at exit
atexit(cleanup);
if (!model::fileExists(dir)) {
throw runtime_error("directory " + dir + " does not exist");
}
// Prepare to create the scene.
model::Point3d pt(+78.4556, +43.0196, -181.107);
// model::Point3d pt(0.0, 0.0, 0.0);
model::Rotation3d rot(0.0, 0.0, 0.0);
model::Pose6d pose(pt, rot);
vector<model::Pose6d> poses;
poses.push_back(pose);
// Allocate memory for scene object.
scene = new model::Scene(type, start, end, dir, scanserver,
maxDist, minDist, alg, octree, red, poses);
scene->detectWalls();
scene->applyLabels(scene->walls[0]);
scene->applyLabels(scene->walls[1]);
scene->applyLabels(scene->walls[2]);
scene->applyLabels(scene->walls[3]);
scene->applyLabels(scene->ceiling);
scene->applyLabels(scene->floor);
vector<model::CandidateOpening> openings;
scene->addFinalOpenings(scene->walls[0], openings);
scene->correct(scene->walls[0], openings);
scene->addFinalOpenings(scene->walls[1], openings);
scene->correct(scene->walls[1], openings);
scene->addFinalOpenings(scene->walls[2], openings);
scene->correct(scene->walls[2], openings);
scene->addFinalOpenings(scene->walls[3], openings);
scene->correct(scene->walls[3], openings);
scene->addFinalOpenings(scene->ceiling, openings);
scene->correct(scene->ceiling, openings);
scene->addFinalOpenings(scene->floor, openings);
scene->correct(scene->floor, openings);
scene->writeCorrectedWalls(dir);
scene->writeModel(dir);
return 0;
}