587 lines
17 KiB
Text
587 lines
17 KiB
Text
|
/*
|
||
|
|
* 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;
|
||
|
|
}
|