3dpcp/.svn/pristine/41/417f60bd08cbdf9dd3e55ddbfb2d155f195e384b.svn-base

800 lines
23 KiB
Text
Raw Normal View History

2012-09-16 12:33:11 +00:00
/*
* toImage implementation
*
* Copyright (C) Dorit Borrmann
*
* Released under the GPL version 3.
*
*/
/**
* @file
* @author Dorit Borrmann. Institute of Computer Science, University of Osnabrueck, Germany.
*/
#include <cfloat>
#include <fstream>
#ifndef _MSC_VER
#include <getopt.h>
#else
#include "XGetopt.h"
#endif
#include <iostream>
using std::ofstream;
using std::flush;
using std::cout;
using std::string;
using std::cerr;
using std::endl;
#include <errno.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 "shapes/image.h"
#include "shapes/hough.h"
#include "shapes/ppair.h"
#include "shapes/quadtree.h"
#include <map>
#include "newmat/newmatap.h"
using namespace NEWMAT;
//#include "shapes/convexplane.h"
#define MAX_PARALLEL_ANGLE 10.0
#define MAX_EQUAL_DIST 10.0
#define MIN_PARALLEL_DIST 50.0
#define MIN_RIGHT_ANGLE 80.0
#define NUM_ITERS 1
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 << " [options] directory" << endl << endl;
cout << bold << "OPTIONS" << normal << 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, 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 << " -p" << normal << " P, " << bold << "--plane=" << normal << "P" << endl
<< " using algorithm P for plane detection" << endl
<< " (chose P from {rht, sht, pht, ppht, apht})" << endl
<< endl
<< bold << " -r" << normal << " NR, " << bold << "--reduce=" << normal << "NR" << endl
<< " turns on octree based point reduction (voxel size=<NR>)" << 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
<< endl << endl;
cout << bold << "EXAMPLES " << normal << endl
<< " " << prog << " -m 500 -r 5 dat" << endl
<< " " << prog << " --max=5000 -r 10.2 dat" << endl
<< " " << prog << " -s 2 -e 10 -r dat" << endl << endl;
exit(1);
}
int parseArgs(int argc, char **argv, string &dir, double &red, int &start, int
&maxDist, int&minDist, int &octree, IOType &type, bool &quiet) {
bool reduced = false;
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' },
{ "reduce", required_argument, 0, 'r' },
{ "quit", no_argument, 0, 'q' },
{ "octree", optional_argument, 0, 'O' },
{ 0, 0, 0, 0} // needed, cf. getopt.h
};
cout << endl;
while ((c = getopt_long(argc, argv, "f:r:s:e:m:M:p:Oq", longopts, NULL)) != -1)
switch (c)
{
case 'r':
red = atof(optarg);
reduced = true;
break;
case 's':
start = atoi(optarg);
if (start < 0) { cerr << "Error: Cannot start 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 'q':
quiet = true;
break;
case 'm':
maxDist = atoi(optarg);
break;
case 'O':
if (optarg) {
octree = atoi(optarg);
} else {
octree = 1;
}
break;
case 'M':
minDist = atoi(optarg);
break;
case '?':
usage(argv[0]);
return 1;
default:
abort ();
}
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
return 0;
}
void getPPairs(vector<ConvexPlane*> &allPlanes, PPair **ppairs) {
for (unsigned int i = 0; i < allPlanes.size(); i++) {
for (unsigned int j = i+1; j < allPlanes.size(); j++) {
PPair pair(allPlanes[i], allPlanes[j]);
// parallel or equal planes
if (deg(pair.angle) < MAX_PARALLEL_ANGLE) {
// planes are equal
if (pair.plane_distance < MAX_EQUAL_DIST) {
pair.type = PPair::EQUAL;
// planes are parallel
} else {//if (pair.plane_distance > MIN_PARALLEL_DIST) {
pair.type = PPair::PARALLEL;
// planes are nothing
}
// plane is perpendicular
} else if (deg(pair.angle) > MIN_RIGHT_ANGLE) {
pair.type = PPair::PERPENDICULAR;
// planes are nothing in particular
}
ppairs[i][j] = pair;
ppairs[j][i] = pair;
}
}
}
void getZHK(unsigned int length, bool **graph, vector<set<int> > &zhks ) {
set<int > unused;
for (unsigned int i = 0; i < length; i++) unused.insert(i);
// as long as we have nodes cluster
while(!unused.empty()) {
// start new cluster
int index = *unused.begin();
unused.erase(index);
set<int> zhk;
zhk.insert(index);
// now look for all connected nodes
vector<int> unvisited;
unvisited.push_back(index);
while (!unvisited.empty()) {
int index = *unvisited.begin();
unvisited.erase(unvisited.begin());
for (unsigned int i = 0; i < length; i++) {
if (i != index && graph[index][i] && zhk.count(i) == 0 ) {
zhk.insert(i);
unvisited.push_back(i);
unused.erase(i);
}
}
}
zhks.push_back(zhk);
}
}
void flip( const double *normal , double *n ) {
// project to the other side
double k[3];
n[0] = normal[0];
n[1] = normal[1];
n[2] = 1;
toKartesian(n, k);
k[0] *= -1;
k[1] *= -1;
k[2] *= -1;
toPolar(k, n);
}
bool parallel(set<double*> &p1, set<double*> &p2, double min_angle) {
for (set<double *>::iterator it = p1.begin();
it != p1.end(); it++) {
for (set<double *>::iterator jt = p2.begin();
jt != p2.end(); jt++) {
double *a = *it;
double *b = *jt;
//if ( polardist(a,b) > ( rad(180) - min_angle ) )
double flipped[3];
flip(a, flipped);
if (polardist(a,b) < min_angle || polardist(flipped, b) < min_angle)
{
return true;
}
}
}
return false;
}
// TODO no idea what this does
/*
ColumnVector calcRotRhos(vector<ConvexPlane*> &planes) {
unsigned int n = planes.size();
double xbar[3];
double *normal;
double nx;
ColumnVector A(n+3);
Matrix B(n+3,n+3);
B = 0;
A = 0;
// fill B and A
// for all planes
for (unsigned int i = 0; i < n; i++) {
vector<Point> points = planes[i]->points;
normal = planes[i]->n;
// for all x_j in plane i
for (unsigned int j = 0; j < points.size(); j++) {
Point p = points[j];
xbar[0] = normal[1] * p.z - normal[2]*p.y;
xbar[1] = normal[2] * p.x - normal[0]*p.z;
xbar[2] = normal[0] * p.y - normal[1]*p.x;
nx = normal[0]*p.x + normal[1]*p.y + normal[2]*p.z;
A(1) += nx * xbar[0];
A(2) += nx * xbar[1];
A(3) += nx * xbar[2];
A(i+1 + 3) -= nx;
// upper left 3x3 matrix of B
B(1, 1) += xbar[0]*xbar[0];
B(2, 2) += xbar[1]*xbar[1];
B(3, 3) += xbar[2]*xbar[2];
B(1,2) += xbar[0]*xbar[1];
B(2,1) += xbar[0]*xbar[1];
B(1,3) += xbar[0]*xbar[2];
B(3,1) += xbar[0]*xbar[2];
B(2,3) += xbar[1]*xbar[2];
B(3,2) += xbar[1]*xbar[2];
B(1, i+1 +3) -= xbar[0];
B(i+1 +3, 1) -= xbar[0];
B(2, i+1 +3) -= xbar[1];
B(i+1 +3, 2) -= xbar[1];
B(3, i+1 +3) -= xbar[2];
B(i+1 +3, 3) -= xbar[2];
}
B(i+1 +3, i+1 +3) = points.size();
}
// calc result
// cout << "A: " << endl << A << endl;
// cout << "B: " << endl << B << endl;
ColumnVector X = B.i() * A;
// cout << "X: " << endl << X << endl;
return X;
}*/
void join(vector<set<double*> > &cps, double min_angle, vector<set<double*> > &n_cps) {
bool **par = new bool*[cps.size()];
for (unsigned int i = 0; i< cps.size(); i++) {
par[i] = new bool[cps.size()];
}
for (unsigned int i = 0; i < cps.size(); i++) {
par[i][i] = false;
for (unsigned int j = i+1; j < cps.size(); j++) {
if (parallel(cps[i], cps[j], min_angle) ) {
par[i][j] = true;
par[j][i] = true;
}
else {
par[i][j] = false;
par[j][i] = false;
}
}
}
vector<set<int> > zhks;
getZHK(cps.size(), par, zhks);
for (unsigned int i = 0; i < zhks.size(); i++) {
set<double*> zhk;
n_cps.push_back(zhk);
for (set<int>::iterator it = zhks[i].begin(); it != zhks[i].end(); it++) {
n_cps[i].insert( cps[*it].begin(), cps[*it].end() );
}
}
for (unsigned int i = 0; i< cps.size(); i++) {
delete[] par[i];
}
delete[] par;
}
void getModel(vector<ConvexPlane*> &model, vector<ConvexPlane*> &allPlanes) {
int planes = allPlanes.size();//588;
double min_angle = rad(MAX_PARALLEL_ANGLE);
double perp_angle = rad(MIN_RIGHT_ANGLE);
int min_size = 0;
// polar coordinates of plane
vector<double *> norms;
for (unsigned int i = 0; i < allPlanes.size(); i ++) {;
double *norm = new double[5];
toPolar(allPlanes[i]->n, norm); // 0: phi, 1:theta
norm[2] = allPlanes[i]->pointsize; // number of points
norm[3] = i; // index
norm[4] = allPlanes[i]->rho; // rho
norms.push_back(norm);
}
// copy of polar coordinate representation
double **normals = new double*[planes];//[planes][4];
for (int j = 0; j < planes; j++) {
normals[j] = norms[j];
}
// all normals in Quadtree and cluster
QuadTree tree(normals, planes , 0.40, min_angle);
vector<set<double *> > cpsold;
cpsold = tree.getClusters();
//join similar normals
vector<set<double *> > cps;
join(cpsold, min_angle, cps);
// Filtering
for (unsigned int i = 0; i < cps.size(); i++) {
cout << "Cluster " << i << ": " << cps[i].size();
set<double *>::iterator it = cps[i].begin();
double *rep = *it;
// for each normal in this cluster
while ( it != cps[i].end()) {
double *normal = (*it);
double flipped[3];
flip(normal, flipped);
// check wether the flipped one is closer
if (polardist(rep, flipped) < polardist(rep, normal) ) {
normal[0] = flipped[0];
normal[1] = flipped[1];
}
cout << " " << normal[3] << " normal: " << normal[0] << " " << normal[1] << endl;
it++;
}
}
double **cnorms = new double*[cps.size()];
bool **cnrels = new bool*[cps.size()];
int *normpts = new int[cps.size()];
cout << "Center normals.." << endl;
// Calculate the centerd normals
for (unsigned int i = 0; i < cps.size(); i++) {
cout << "Cluster " << i << ":" << endl;
cnorms[i] = new double[3];
cnrels[i] = new bool[cps.size()];
cnorms[i][0] = 0;
cnorms[i][1] = 0;
cnorms[i][2] = 0;
int sumpoints = 0;
for (set<double *>::iterator it = cps[i].begin();
it != cps[i].end(); it++) {
double *normal = (*it);
double k[3], n[3];
n[0] = normal[0];
n[1] = normal[1];
n[2] = 1;
toKartesian(n, k);
cnorms[i][0] += k[0] * normal[2];
cnorms[i][1] += k[1] * normal[2];
cnorms[i][2] += k[2] * normal[2];
sumpoints += normal[2];
}
normpts[i] = sumpoints;
cnorms[i][0] /= sumpoints;
cnorms[i][1] /= sumpoints;
cnorms[i][2] /= sumpoints;
Normalize3(cnorms[i]);
double p[3];
toPolar(cnorms[i], p);
cnorms[i][0] = p[0];
cnorms[i][1] = p[1];
cout << " cnormal: " << cnorms[i][0] << " " << cnorms[i][1] << endl;
}
// Calculate relations between clusters
for (unsigned int i = 0; i < cps.size(); i++) {
cnrels[i][i] = false;
for (unsigned int j = i+1; j < cps.size(); j++) {
double angle = polardist(cnorms[i], cnorms[j]);
cnrels[i][j] = angle > perp_angle && angle < (rad(180.0) - perp_angle);
cnrels[j][i] = cnrels[i][j];
}
}
// calculate clusters that are big enough
vector<int> big_clusters;
for (unsigned int i = 0; i < cps.size(); i++) {
if (normpts[i] > min_size) {
big_clusters.push_back(i);
}
}
cout << "look for biggest cliques..." << endl;
// Calculate biggest 3-cliques
int ci, cj, ck, cpoints;
ci = cj = ck = -1;
cpoints = 0;
for (vector<int>::iterator it = big_clusters.begin();
it != big_clusters.end(); it++) {
unsigned int i = *it;
for (vector<int>::iterator jt = it+1;
jt != big_clusters.end(); jt++) {
unsigned int j = *jt;
if (!cnrels[i][j]) continue;
for (vector<int>::iterator kt = jt+1;
kt != big_clusters.end(); kt++) {
unsigned int k = *kt;
if (cnrels[i][k] && cnrels[j][k]) {
// candidate clique
int sum = normpts[i] + normpts[j] + normpts[k];
if (sum > cpoints) {
cpoints = sum;
ci = i;
cj = j;
ck = k;
cout << i << " " << j << " " << k << " " << sum << endl;
}
}
}
}
}
if (ci == -1 || cj == -1 || ck == -1) {
cout << "No clique could be found!" << endl;
return;
}
// Estimate new normals
double n[3];
ConvexPlane p1(cnorms[ci]);
p1.project(cnorms[cj], n);
ConvexPlane p2(n);
Cross(p1.n, p2.n, n);
ConvexPlane p3(n);
// map from allPlane index to ci, cj or ck
map<int, int> idxTotype;
int idx = 0;
cout << "hmm " << endl;
vector<ConvexPlane*> numerical_planes;
for (set<double *>::iterator it = cps[ci].begin(); it != cps[ci].end(); it++) {
double *normal = (*it);
int index = (int)normal[3];
ConvexPlane *plane = allPlanes[index];
/*
plane->n[0] = p1.n[0];
plane->n[1] = p1.n[1];
plane->n[2] = p1.n[2];
*/
model.push_back(plane);
cout << "for ci = " << ci << " plane index " << idx << " ap index " << index << endl;
idxTotype[idx++] = ci;
}
for (set<double *>::iterator it = cps[cj].begin(); it != cps[cj].end(); it++) {
double *normal = (*it);
int index = (int)normal[3];
ConvexPlane *plane = allPlanes[index];
/*
plane->n[0] = p2.n[0];
plane->n[1] = p2.n[1];
plane->n[2] = p2.n[2];
*/
model.push_back(plane);
cout << "for cj = " << cj << " plane index " << idx << " ap index " << index << endl;
idxTotype[idx++] = cj;
}
for (set<double *>::iterator it = cps[ck].begin(); it != cps[ck].end(); it++) {
double *normal = (*it);
int index = (int)normal[3];
ConvexPlane *plane = allPlanes[index];
/*
plane->n[0] = p3.n[0];
plane->n[1] = p3.n[1];
plane->n[2] = p3.n[2];
*/
model.push_back(plane);
cout << "for ck = " << ck << " plane index " << idx << " ap index " << index << endl;
idxTotype[idx++] = ck;
}
/*
int MAX_IT = 100;
// int MAX_IT = 10000;
int it_nr = 0;
ColumnVector X;
double MIN_ERR = 0.0005;
// double MIN_ERR = 0.0000000000000000000001;
double error = 0.0;
do {
//cout << it_nr << endl;
it_nr++;
// calc rotation and rho's
X = calcRotRhos(numerical_planes);
// apply rotation to normals
double rPos[3] = {0.0, 0.0, 0.0};
double rPosTheta[3];
rPosTheta[0] = -X(1);
rPosTheta[1] = -X(2);
rPosTheta[2] = -X(3);
error = fabs (X(1)) + fabs(X(2)) + fabs(X(3)) ;
// cout << rPosTheta[0] << " " << rPosTheta[1] << " " <<rPosTheta[2] << " " << endl;
double alignxf[16];
EulerToMatrix4(rPos, rPosTheta, alignxf);
// cout << alignxf << endl;
for(unsigned int j = 0; j < numerical_planes.size(); j++) {
Point normal;
normal.x = numerical_planes[j]->n[0];
normal.y = numerical_planes[j]->n[1];
normal.z = numerical_planes[j]->n[2];
// cout << "NP " << j << " " << normal.x << " " << normal.y << " " << normal.z << endl;
normal.transform(alignxf);
// cout <<"to" << endl;
// cout << normal.x << " " << normal.y << " " << normal.z << endl;
numerical_planes[j]->n[0] = normal.x;
numerical_planes[j]->n[1] = normal.y;
numerical_planes[j]->n[2] = normal.z;
// cout << "NP " << j << " " << normal.x << " " << normal.y << " " << normal.z << endl;
}
// repeat
} while (error > MIN_ERR || it_nr > MAX_IT);
// cout << "ERRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRR" << endl;
// apply rho's to planes
for(unsigned int j = 0; j < numerical_planes.size(); j++) {
// cout << j << " s rho is " << numerical_planes[j]->rho << endl;
numerical_planes[j]->rho = -X(j+1 +3);
// cout << j << " s rho is " << numerical_planes[j]->rho << endl;
}
bool **graph = new bool*[numerical_planes.size()];
for (unsigned int i = 0; i < numerical_planes.size(); i++) {
graph[i] = new bool[numerical_planes.size()];
}
for (unsigned int i = 0; i < numerical_planes.size(); i++) {
graph[i][i] = false;
for (unsigned int j = i+1; j < numerical_planes.size(); j++) {
if( fabs(numerical_planes[i]->rho - numerical_planes[j]->rho) < MAX_EQUAL_DIST
&& idxTotype[i] == idxTotype[j] ) { // check wether planes belong to the same type
graph[i][j] = true;
graph[j][i] = true;
} else {
graph[i][j] = false;
graph[j][i] = false;
}
}
}
vector<set<int> > zhks;
getZHK(numerical_planes.size(), graph, zhks);
for (unsigned int i = 0; i < zhks.size(); i++) {
vector<ConvexPlane*> pplane;
for (set<int>::iterator it = zhks[i].begin(); it != zhks[i].end(); it++) {
pplane.push_back(numerical_planes[*it]);
cout << "zhk " << i << " p " << (*it) << endl;
}
ConvexPlane *plane = new ConvexPlane(pplane);
model.push_back(plane);
}
*/
//exit(0);
}
/**
* Main function. The Hough Transform is called for the scan indicated as
* argument.
*
*/
int main(int argc, char **argv)
{
cout << "(c) Jacobs University Bremen, gGmbH, 2011" << endl << endl;
if (argc <= 1) {
usage(argv[0]);
}
// parsing the command line parameters
// init, default values if not specified
string dir;
double red = -1.0;
int start = 0;
int maxDist = -1;
int minDist = -1;
int octree = 0;
bool quiet = false;
IOType type = UOS;
cout << "Parse args" << endl;
parseArgs(argc, argv, dir, red, start, maxDist, minDist, octree, type, quiet);
Scan::dir = dir;
int fileNr = start;
string planedir = dir + "planes";
#ifdef _MSC_VER
int success = mkdir(planedir.c_str());
#else
int success = mkdir(planedir.c_str(), S_IRWXU|S_IRWXG|S_IRWXO);
#endif
if(success == 0) {
if(!quiet) {
cout << "Writing planes to " << planedir << endl;
}
} else if(errno == EEXIST) {
cout << "Directory " << planedir << " exists already. CONTINUE" << endl;
} else {
cerr << "Creating directory " << planedir << " failed" << endl;
exit(1);
}
Scan::readScans(type, fileNr, fileNr, dir, maxDist, minDist, 0);
// reduction filter for current scan!
//Scan::allScans[0]->calcReducedPoints(red, octree);
Scan::allScans[0]->toGlobal(red, octree);
double id[16];
M4identity(id);
for(int i = 0; i < 10; i++) {
Scan::allScans[0]->transform(id, Scan::ICP, 0); // write end pose
}
//Hough hough(Scan::allScans[0], quiet);
//hough.RHT();
cout << "Hough done" << endl;
// calculate relations between planes
//vector<ConvexPlane*> * allPlanes = &hough.planes;
/*
PPair **p_graph;
p_graph = new PPair*[allPlanes->size()];
for (unsigned int i = 0; i < allPlanes->size(); i++) {
p_graph[i] = new PPair[allPlanes->size()];
}
getPPairs(*allPlanes, p_graph);
*/
// optimize relations
//vector<ConvexPlane*> model;
//getModel(model, *allPlanes);
//cout << "GetModel done" << endl;
//Hough houghdelete(Scan::allScans[0], quiet);
cout << "start range image calculation" << endl;
long starttime = GetCurrentTimeInMilliSec();
int size;
//double * const* points_red = houghdelete.deletePoints(*allPlanes, size);
//double * const* points_red = hough.getPoints(size);
cout << "Size " << size << endl;
double * const* points_red = Scan::allScans[0]->get_points_reduced();
// Image myImage(0.0, 360.0, 0.0, 180.0, 0.04, points_red, Scan::allScans[0]->get_points_red_size());
Image myImage(0.0, 360.0, 0.0, 180.0, 0.08, Scan::allScans[0]->get_points());
//Image myImage(90.0, 270.0, 40.0, 90.0, 1.0, points_red, size);
starttime = (GetCurrentTimeInMilliSec() - starttime);
cout << "Time for Constructor call: " << starttime << endl;
starttime = GetCurrentTimeInMilliSec();
int** regdat;
int width = myImage.getWidth();
regdat = new int*[width];
for(int i = 0; i < width; i++) {
regdat[i] = new int[myImage.getHeight()];
}
int** regdat2;
regdat2 = new int*[width];
for(int i = 0; i < width; i++) {
regdat2[i] = new int[myImage.getHeight()];
}
//myImage.blobColor(05.0, regdat);
//cout << "blobColor done" << endl;
//myImage.printImage("blub.ppm", true);
myImage.printImage("blub.ppm", false);
float min = myImage.getMin();
float max = myImage.getMax();
float tenPercent = (max - min)*0.45;
cout << "MinMax " << min << " " << max << " " << tenPercent << " " << (max - min) << " "<< (max - tenPercent) << endl;
myImage.calcMarker(min + tenPercent, regdat);
cout << "Marker done" << endl;
int regions = myImage.cluster(5, regdat, regdat2);
cout << "Nu kommen die Punkte" << endl;
myImage.writeCenters(regions, regdat2, Scan::allScans[0]->get_points());
cout << "dat woarn se" << endl;
//myImage.blobColor(5, regdat2);
//cout << "blobColor done" << endl;
//myImage.printScans(regdat, points_red, Scan::allScans[0]->get_points_red_size());
cout << "print Scans done" << endl;
starttime = (GetCurrentTimeInMilliSec() - starttime);
cout << "Time for whatever " << starttime << endl;
delete Scan::allScans[0];
Scan::allScans.clear();
}