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.
392 lines
11 KiB
C++
392 lines
11 KiB
C++
/*
|
|
* scan_diff implementation
|
|
*
|
|
* Copyright (C) Dorit Borrmann
|
|
*
|
|
* Released under the GPL version 3.
|
|
*
|
|
*/
|
|
|
|
|
|
/**
|
|
* @file
|
|
* @brief Main program calculating difference of 3D scans.
|
|
*
|
|
* Program for calculating the difference between scans after matching with slam6d
|
|
* Usage: bin/scan_diff -s <START> -e <END> -d <NR> 'dir',
|
|
* Use -s for the first scan, -e for the second scan
|
|
* 'dir' the directory of a set of scans
|
|
* The result is a scan in the UOS format (.3d) that contains all points from
|
|
* the first scan that do NOT have a corresponding point in the second scan
|
|
* within a distance of less than NR units.
|
|
* Difference scans will be written to 'dir/diff'
|
|
* ATTENTION: All scans between START and END will be loaded!
|
|
* @author Dorit Borrmann. Automation Group, Jacobs University Bremen gGmbH, Germany.
|
|
*/
|
|
#ifdef _MSC_VER
|
|
#if !defined _OPENMP && defined OPENMP
|
|
#define _OPENMP
|
|
#endif
|
|
#endif
|
|
|
|
#define WANT_STREAM ///< define the WANT stream :)
|
|
#include <string>
|
|
using std::string;
|
|
#include <iostream>
|
|
using std::cout;
|
|
using std::cerr;
|
|
using std::endl;
|
|
#include <fstream>
|
|
using std::ofstream;
|
|
using std::ifstream;
|
|
#include <errno.h>
|
|
#include <vector>
|
|
|
|
#include "slam6d/globals.icc"
|
|
#include "slam6d/io_utils.h"
|
|
#include "slam6d/scan.h"
|
|
|
|
#include "scanserver/clientInterface.h"
|
|
|
|
#ifdef _OPENMP
|
|
#include <omp.h>
|
|
#endif
|
|
|
|
#ifndef _MSC_VER
|
|
#include <getopt.h>
|
|
#else
|
|
#include "XGetopt.h"
|
|
#endif
|
|
|
|
#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
|
|
|
|
|
|
/**
|
|
* Explains the usage of this program's command line parameters
|
|
*/
|
|
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 << "-s <START> -e <END> [options] directory" << endl << endl;
|
|
cout << bold << "OPTIONS" << normal << 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 << " -e" << normal << " NR, " << bold << "--end=" << normal << "NR" << endl
|
|
<< " end after scan NR" << endl
|
|
<< 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, rxp,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 << " -d" << normal << " NR, " << bold << "--dist=" << normal << "NR" << endl
|
|
<< " write all points that have no corresponding point closer than NR 'units'" << endl
|
|
<< bold << " -S, --scanserver" << normal << endl
|
|
<< " Use the scanserver as an input method and handling of scan data" << endl
|
|
<< endl << endl;
|
|
|
|
cout << bold << "EXAMPLES " << normal << endl
|
|
<< " " << prog << " -m 500 -d 5 dat" << endl
|
|
<< " " << prog << " --max=5000 -d 10.2 dat" << endl
|
|
<< endl;
|
|
exit(1);
|
|
}
|
|
|
|
/** A function that parses the command-line arguments and sets the respective flags.
|
|
* @param argc the number of arguments
|
|
* @param argv the arguments
|
|
* @param dir the directory
|
|
* @param start first scan number 'start'
|
|
* @param end last scan number 'end'
|
|
* @param maxDist - maximal distance of points being loaded
|
|
* @param minDist - minimal distance of points being loaded
|
|
* @param dist the maximal distance for a point pair
|
|
* @param type the scan format
|
|
* @param desc true if start is greater than end
|
|
* @return 0, if the parsing was successful. 1 otherwise
|
|
*/
|
|
int parseArgs(int argc, char **argv, string &dir,
|
|
int &start, int &end, int &maxDist, int &minDist, double &dist,
|
|
IOType &type, bool &desc, bool scanserver)
|
|
{
|
|
int c;
|
|
// from unistd.h:
|
|
extern char *optarg;
|
|
extern int optind;
|
|
|
|
WriteOnce<IOType> w_type(type);
|
|
WriteOnce<int> w_start(start), w_end(end);
|
|
|
|
/* 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' },
|
|
{ "end", required_argument, 0, 'e' },
|
|
{ "dist", required_argument, 0, 'd' },
|
|
{ "scanserver", no_argument, 0, 'S' },
|
|
{ 0, 0, 0, 0} // needed, cf. getopt.h
|
|
};
|
|
|
|
cout << endl;
|
|
while ((c = getopt_long(argc, argv, "f:d:s:e:m:M:", longopts, NULL)) != -1)
|
|
switch (c)
|
|
{
|
|
case 'd':
|
|
dist = atof(optarg);
|
|
break;
|
|
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); }
|
|
break;
|
|
case 'f':
|
|
try {
|
|
type = formatname_to_io_type(optarg);
|
|
} catch (...) { // runtime_error
|
|
cerr << "Format " << optarg << " unknown." << endl;
|
|
abort();
|
|
}
|
|
break;
|
|
case 'm':
|
|
maxDist = atoi(optarg);
|
|
break;
|
|
case 'M':
|
|
minDist = atoi(optarg);
|
|
break;
|
|
case '?':
|
|
usage(argv[0]);
|
|
return 1;
|
|
case 'S':
|
|
scanserver = true;
|
|
break;
|
|
default:
|
|
abort ();
|
|
}
|
|
|
|
if(start < 0 || end < 0 || start == end) {
|
|
cerr << "\n*** You need two different scans for difference calculations ***" << endl;
|
|
usage(argv[0]);
|
|
}
|
|
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
|
|
if(start > end) {
|
|
double tmp = start;
|
|
start = end;
|
|
end = tmp;
|
|
desc = true;
|
|
}
|
|
|
|
parseFormatFile(dir, w_type, w_start, w_end);
|
|
|
|
return 0;
|
|
}
|
|
|
|
|
|
/**
|
|
* Main program for calculating the difference of two scans.
|
|
* Usage: bin/scan_diff -d <NR> -s <NR> -e <NR> 'dir',
|
|
* Use -s and -e for the two scans,
|
|
* -d
|
|
* and 'dir' the directory of a set of scans
|
|
* Difference scans will be written to 'dir/diff/scan[00]s.3d'
|
|
*
|
|
*/
|
|
int main(int argc, char **argv)
|
|
{
|
|
|
|
cout << "(c) Jacobs University Bremen, gGmbH, 2012" << endl << endl;
|
|
|
|
if (argc <= 1) {
|
|
usage(argv[0]);
|
|
}
|
|
|
|
// parsing the command line parameters
|
|
// init, default values if not specified
|
|
string dir;
|
|
double dist = 0;
|
|
int start = -1, end = -1;
|
|
int maxDist = -1;
|
|
int minDist = -1;
|
|
IOType type = RIEGL_TXT;
|
|
bool desc = false;
|
|
bool scanserver = false;
|
|
|
|
parseArgs(argc, argv, dir, start, end, maxDist, minDist, dist, type, desc, scanserver);
|
|
|
|
if (scanserver) {
|
|
try {
|
|
ClientInterface::create();
|
|
} catch(std::runtime_error& e) {
|
|
cerr << "ClientInterface could not be created: " << e.what() << endl;
|
|
cerr << "Start the scanserver first." << endl;
|
|
exit(-1);
|
|
}
|
|
}
|
|
|
|
string diffdir = dir + "diff";
|
|
|
|
#ifdef _MSC_VER
|
|
int success = mkdir(diffdir.c_str());
|
|
#else
|
|
int success = mkdir(diffdir.c_str(), S_IRWXU|S_IRWXG|S_IRWXO);
|
|
#endif
|
|
if(success == 0) {
|
|
cout << "Writing scans to " << diffdir << endl;
|
|
} else if(errno == EEXIST) {
|
|
cout << "Directory " << diffdir << " exists already. CONTINUE" << endl;
|
|
} else {
|
|
cerr << "Creating directory " << diffdir << " failed" << endl;
|
|
exit(1);
|
|
}
|
|
string scanFileName;
|
|
string framesFileName;
|
|
ifstream frames_in;
|
|
double inMatrix0[17];
|
|
double inMatrix1[17];
|
|
|
|
cout << "Reading Scan No. " << start << endl;
|
|
framesFileName = dir + "scan" + to_string(start,3) + ".frames";
|
|
frames_in.open(framesFileName.c_str());
|
|
|
|
if(!frames_in.good()) {
|
|
cerr << "Couldn't read frames " << end << endl;
|
|
exit(1);
|
|
}
|
|
while(frames_in.good()) {
|
|
for (unsigned int i = 0; i < 17; frames_in >> inMatrix0[i++]);
|
|
}
|
|
frames_in.close();
|
|
frames_in.clear();
|
|
for(int i = 0; i < 16; i++) {
|
|
cout << inMatrix0[i] << " ";
|
|
}
|
|
cout << endl;
|
|
|
|
cout << "Reading Scan No. " << end << endl;
|
|
framesFileName = dir + "scan" + to_string(end,3) + ".frames";
|
|
frames_in.open(framesFileName.c_str());
|
|
|
|
if(!frames_in.good()) {
|
|
cerr << "Couldn't read frames " << end << endl;
|
|
exit(1);
|
|
}
|
|
while(frames_in.good()) {
|
|
for (unsigned int i = 0; i < 17; frames_in >> inMatrix1[i++]);
|
|
}
|
|
frames_in.close();
|
|
frames_in.clear();
|
|
for(int i = 0; i < 16; i++) {
|
|
cout << inMatrix1[i] << " ";
|
|
}
|
|
cout << endl;
|
|
|
|
Scan::openDirectory(scanserver, dir, type, start, end);
|
|
|
|
if(Scan::allScans.size() == 0) {
|
|
cerr << "No scans found. Did you use the correct format?" << endl;
|
|
exit(-1);
|
|
}
|
|
|
|
Scan* scan_first = *(Scan::allScans.begin());
|
|
Scan* scan_second = *(Scan::allScans.end()-1);
|
|
|
|
scan_first->transform(inMatrix0, Scan::INVALID);
|
|
scan_second->transform(inMatrix1, Scan::INVALID);
|
|
|
|
DataXYZ xyz_first(scan_first->get("xyz"));
|
|
DataXYZ xyz_second(scan_second->get("xyz"));
|
|
|
|
cout << "Scan " << scan_first->getIdentifier() << " with " << xyz_first.size() << " points" << endl;
|
|
cout << "Scan " << scan_second->getIdentifier() << " with " << xyz_second.size() << " points" << endl;
|
|
|
|
int thread_num = 0;
|
|
std::vector<double*> diff;
|
|
double transMat[16];
|
|
|
|
if(desc) {
|
|
Scan::getNoPairsSimple(diff, scan_second, scan_first, thread_num, dist);
|
|
M4inv(inMatrix1, transMat);
|
|
scanFileName = dir + "diff/scan" + to_string(end,3) + ".3d";
|
|
} else {
|
|
Scan::getNoPairsSimple(diff, scan_first, scan_second, thread_num, dist);
|
|
M4inv(inMatrix0, transMat);
|
|
scanFileName = dir + "diff/scan" + to_string(start,3) + ".3d";
|
|
}
|
|
|
|
cout << endl;
|
|
for(int i = 0; i < 16; i++) {
|
|
cout << transMat[i] << " ";
|
|
}
|
|
cout << endl;
|
|
for(int i = 0; i < 16; i++) {
|
|
cout << inMatrix0[i] << " ";
|
|
}
|
|
cout << endl;
|
|
|
|
ofstream ptsout(scanFileName.c_str());
|
|
|
|
for(unsigned int i = 0; i < diff.size(); i++) {
|
|
Point p(diff[i]);
|
|
p.transform(transMat);
|
|
ptsout << p.x << " " << p.y << " " << p.z << " " << endl;
|
|
}
|
|
|
|
ptsout.close();
|
|
ptsout.clear();
|
|
|
|
if (scanserver) {
|
|
scan_first->clear("xyz");
|
|
scan_second->clear("xyz");
|
|
}
|
|
|
|
cout << endl << endl;
|
|
cout << "Normal program end." << endl << endl;
|
|
|
|
if (scanserver) {
|
|
Scan::closeDirectory();
|
|
}
|
|
|
|
return 0;
|
|
}
|