josch
/
3dpcp
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
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.
main
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from '8b4640b056'
${ noResults }
3dpcp/src/slam6d/scan_diff2d.cc

587 lines
17 KiB
C++
Raw Blame History

/*
* scan_diff2d implementation
*
* Copyright (C) Dorit Borrmann
*
* Released under the GPL version 3.
*
*/
/**
* @file
* @brief Main program calculating difference of 3D scans.
*
* Program for calculating the 2d-difference between scans after matching with slam6d
* Usage: bin/scan_diff2d -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 an image showing the color coded difference of the two scans
* ATTENTION: All scans between START and END will be loaded!
* @author Dorit Borrmann. Automation Group, Jacobs University Bremen gGmbH, Germany.
*/
#ifdef _MSC_VER
#ifdef 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 "slam6d/scan.h"
#include "slam6d/scan_io.h"
#include "slam6d/globals.icc"
#ifdef _OPENMP
#include <omp.h>
#endif
#ifndef _MSC_VER
#include <getopt.h>
#else
#include "XGetopt.h"
#endif
#include "show/colormanager.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 "slam6d/kd.h"
#include "slam6d/kdc.h"
#include <limits.h>
#include <algorithm>
using namespace std;
int WIDTH=0;
int HEIGHT=0;
double MINTHETA;
double MAXTHETA;
double MINPHI;
double MAXPHI;
double minphi,maxphi;
double mintheta,maxtheta;
bool **stencilImage1;
bool **stencilImage;
double **rangeImage;
double **rangeImage2;
Point **pointImage;
void writeRI(const char *filename, double max, double shift)
{
SHSVMap cmap;
float color[3];
unsigned int buckets = 4096;
ofstream fp; // The PPM File
unsigned char *ibuffer; // The PPM Output Buffer
// Allocate memory for the the frame buffer and output buffer
ibuffer = new unsigned char[WIDTH * HEIGHT * 3];
// Open the output file
fp.open(filename, ios::out);
// Write a proper P6 PPM header
fp << "P6" << endl << "# CREATOR: 3D_Viewer by Andreas Nuechter, University of Osnabrueck"
<< endl << WIDTH << " " << HEIGHT << " " << UCHAR_MAX << endl;
// Loop through the frame buffer data, writing to the PPM file. Be careful
// to account for the frame buffer having 4 bytes per pixel while the
// output file has 3 bytes per pixel
int l = 0;
for (int i = 0; i < HEIGHT; i++) { // For each row
for (int jj = 0; jj < WIDTH; jj++) { // For each column
int j = (jj + (int)(WIDTH*shift))%WIDTH;
if(!stencilImage[j][i]) {
ibuffer[l++] = UCHAR_MAX;
ibuffer[l++] = UCHAR_MAX;
ibuffer[l++] = UCHAR_MAX;
} else {
cmap.calcColor(color, (max + rangeImage[j][i])/(max*2.0) * buckets, buckets);
ibuffer[l++] = (unsigned char)min( (int)((color[0])*UCHAR_MAX), UCHAR_MAX);
ibuffer[l++] = (unsigned char)min( (int)((color[1])*UCHAR_MAX), UCHAR_MAX);
ibuffer[l++] = (unsigned char)min( (int)((color[2])*UCHAR_MAX), UCHAR_MAX);
}
} // end column
} // end row
// Write output buffer to the file */
fp.write((const char*)ibuffer, sizeof(unsigned char) * (3 * WIDTH * HEIGHT));
fp.close();
fp.clear();
delete [] ibuffer;
}
void toRI(double _x, double _y, double _z, int &X, int &Y, double &range) {
double x = _z/100.0;
double y = _x/(-100.0);
double z = _y/100.0;
double phi = 0.0;
double theta = 0.0;
range = std::sqrt(x*x + y*y + z*z);
if (range > 0.00000001) {
phi = atan2(y,x);
phi = ((phi<0.0)?(phi+2.0*M_PI):phi);
theta = std::acos(z/range);
if (phi > maxphi) maxphi = phi;
if (phi < minphi) minphi = phi;
if (theta > maxtheta) maxtheta = theta;
if (theta < mintheta) mintheta = theta;
}
X = WIDTH * (phi/( MAXPHI - MINPHI ));
Y = HEIGHT * ((theta - MINTHETA)/( MAXTHETA - MINTHETA ));
}
void toPoint(int X, int Y, double range, double &x, double &y, double &z) {
x = y = z = 0.0;
if (range > 0.000000001) {
double _z = cos(((double)Y/(double)HEIGHT * (double)(MAXTHETA - MINTHETA)) + MINTHETA) * range;
double _y = sin((double)X/(double)WIDTH * (double)(MAXPHI - MINPHI)) * range;
double _x = cos((double)X/(double)WIDTH * (double)(MAXPHI - MINPHI)) * range;
x = _y*-100.0;
y = _z*100.0;
z = _x*100.0;
}
}
/**
* 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
<< 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, double &res, double &shift, int &mode)
{
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' },
{ "end", required_argument, 0, 'e' },
{ "dist", required_argument, 0, 'd' },
{ "resolution", required_argument, 0, 'r' },
{ "shift", required_argument, 0, 'h' },
{ "mode", required_argument, 0, '0' },
{ 0, 0, 0, 0} // needed, cf. getopt.h
};
cout << endl;
while ((c = getopt_long(argc, argv, "h:r:f:d:s:e:m:M:", longopts, NULL)) != -1)
switch (c)
{
case '0':
mode = atoi(optarg);
break;
case 'h':
shift = atof(optarg);
break;
case 'r':
res = atof(optarg);
break;
case 'd':
dist = atof(optarg);
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); }
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;
default:
abort ();
}
if(start < 0 || end < 0 ) {
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;
}
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, 2010" << 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;
double resolution = 0.25;
double shift = 0.5;
int mode = 0;
parseArgs(argc, argv, dir, start, end, maxDist, minDist, dist, type, desc, resolution, shift, mode);
resolution = rad(resolution);
// Get Scans (all scans between start and end)
Scan::dir = dir;
string scanFileName;
string framesFileName;
ifstream frames_in;
double inMatrix0[17];
double inMatrix1[17];
cout << "Reading Scan No. " << start;
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();
cout << "Reading Scan No. " << end;
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();
Scan::readScans(type, start, end, dir, maxDist, minDist, 0);
int endIndex = Scan::allScans.size() - 1;
Scan *rangescan = Scan::allScans[0];
Scan *targetscan = Scan::allScans[endIndex];
Scan::allScans[endIndex]->calcReducedPoints(-1, 0);
double tinv[16]; M4inv(inMatrix0, tinv);
double tmat[16];
MMult(tinv, inMatrix1, tmat);
Scan::allScans[endIndex]->transform(tmat, Scan::INVALID);
string name;
int thread_num = 0;
vector<double*> diff;
double transMat[16];
cout << "set up range image" << endl;
double x,y,z,r;
int X,Y;
// first subsample to RI
const vector<Point> *points = rangescan->get_points();
maxphi = maxtheta = -DBL_MAX;
mintheta = minphi = DBL_MAX;
cout << "establish max and mins..." << endl;
double MINDIST = 40.0;
for (unsigned int i = 0; i < points->size(); i++) {
Point p = (*points)[i];
////////////////////////////
if ( sqrt(p.x*p.x + p.z*p.z ) < MINDIST ) continue;
///////////////////////////
toRI(p.x, p.y, p.z, X, Y, r);
}
MAXTHETA = maxtheta;
MINTHETA = mintheta;
MAXPHI = maxphi;
MINPHI = minphi;
HEIGHT = (MAXTHETA - MINTHETA)/resolution;
WIDTH = (MAXPHI - MINPHI)/resolution;
switch (mode) {
case 0:
{
cout << "creating data structures " << WIDTH << " " << HEIGHT << endl;
stencilImage = new bool*[WIDTH+1];
rangeImage = new double*[WIDTH+1];
pointImage = new Point*[WIDTH+1];
for (int i = 0; i <= WIDTH; i++) {
stencilImage[i] = new bool[HEIGHT+1];
rangeImage[i] = new double[HEIGHT+1];
pointImage[i] = new Point[HEIGHT+1];
for (int j = 0; j <= HEIGHT; j++) {
rangeImage[i][j] = DBL_MAX;
stencilImage[i][j] = false;
}
}
cout << "thin out points " << points->size() << endl;
for (unsigned int i = 0; i < points->size(); i++) {
Point p = (*points)[i];
////////////////////////////
if ( sqrt(p.x*p.x + p.z*p.z ) < MINDIST ) continue;
///////////////////////////
toRI(p.x, p.y, p.z, X, Y, r);
if (X >= 0 && X < WIDTH && Y >= 0 && Y < HEIGHT ) {
if (rangeImage[X][Y] > r) {
stencilImage[X][Y] = true;
rangeImage[X][Y] = r;
pointImage[X][Y] = p;
}
}
}
cout << "done setting up range image" << endl;
targetscan->createTree(false, false);
const SearchTree *kd = targetscan->get_tree();
cout << " looking for closest points... " << endl;
double mdm2 = dist*dist;
for (int X = 0; X < WIDTH; X++) {
for (int Y = 0; Y < HEIGHT; Y++) {
if (!stencilImage[X][Y]) continue;
double p[3];
p[0] = pointImage[X][Y].x;
p[1] = pointImage[X][Y].y;
p[2] = pointImage[X][Y].z;
double *closest = kd->FindClosest(p, mdm2, 0);
if (closest) {
rangeImage[X][Y] = sqrt(Dist2(closest, p));
double v[3];
for (int i = 0; i < 3; i++)
v[i] = closest[i] - p[i];
double d = v[0]*p[0] + v[1]*p[1] + v[2]*p[2];
if (d <= 0) {
rangeImage[X][Y] *= -1.0;
}
} else {
rangeImage[X][Y] = dist;
}
}
}
}
break;
case 1:
{
cout << "creating data structures " << WIDTH << " " << HEIGHT << endl;
stencilImage = new bool*[WIDTH+1];
stencilImage1 = new bool*[WIDTH+1];
rangeImage = new double*[WIDTH+1];
rangeImage2 = new double*[WIDTH+1];
for (int i = 0; i <= WIDTH; i++) {
stencilImage[i] = new bool[HEIGHT+1];
stencilImage1[i] = new bool[HEIGHT+1];
rangeImage[i] = new double[HEIGHT+1];
rangeImage2[i] = new double[HEIGHT+1];
for (int j = 0; j <= HEIGHT; j++) {
rangeImage[i][j] = DBL_MAX;
rangeImage2[i][j] = DBL_MAX;
stencilImage[i][j] = false;
stencilImage1[i][j] = false;
}
}
cout << "thin out points " << points->size() << endl;
for (unsigned int i = 0; i < points->size(); i++) {
Point p = (*points)[i];
////////////////////////////
if ( sqrt(p.x*p.x + p.z*p.z ) < MINDIST ) continue;
///////////////////////////
toRI(p.x, p.y, p.z, X, Y, r);
if (X >= 0 && X < WIDTH && Y >= 0 && Y < HEIGHT ) {
if (rangeImage[X][Y] > r) {
stencilImage1[X][Y] = true;
rangeImage[X][Y] = fabs(r);
}
}
}
for (unsigned int i = 0; i < Scan::allScans[endIndex]->get_points_red_size(); i++) {
Point p;
p.x = Scan::allScans[endIndex]->get_points_red()[i][0];
p.y = Scan::allScans[endIndex]->get_points_red()[i][1];
p.z = Scan::allScans[endIndex]->get_points_red()[i][2];
////////////////////////////
if ( sqrt(p.x*p.x + p.z*p.z ) < MINDIST ) continue;
///////////////////////////
toRI(p.x, p.y, p.z, X, Y, r);
if (X >= 0 && X < WIDTH && Y >= 0 && Y < HEIGHT ) {
if (rangeImage2[X][Y] > r && stencilImage1[X][Y]) {
stencilImage[X][Y] = true;
rangeImage2[X][Y] = fabs(r);
}
}
}
cout << "done setting up range image" << endl;
for (int X = 0; X < WIDTH; X++) {
for (int Y = 0; Y < HEIGHT; Y++) {
if (!stencilImage[X][Y]) continue;
if (rangeImage[X][Y] == DBL_MAX) continue;
rangeImage[X][Y] = 100.0 * (rangeImage[X][Y] - rangeImage2[X][Y]) ; // to meters
}
}
}
break;
default:
break;
}
name = dir + "diff_" + to_string(start, 3) + "_" + to_string(end,3) + ".ppm";
writeRI(name.c_str(), dist, shift);
delete Scan::allScans[0];
Scan::allScans.clear();
cout << endl << endl;
cout << "Normal program end." << endl << endl;
}
Reference in New Issue View Git Blame Copy Permalink