You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
273 lines
8.5 KiB
C++
273 lines
8.5 KiB
C++
/**
|
|
* @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;
|
|
}
|