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.
499 lines
16 KiB
C++
499 lines
16 KiB
C++
/*
|
|
* matchMarker implementation
|
|
*
|
|
* Copyright (C) Dorit Borrmann
|
|
*
|
|
* Released under the GPL version 3.
|
|
*
|
|
*/
|
|
|
|
#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 <time.h>
|
|
|
|
#include "slam6d/scan.h"
|
|
|
|
#include "slam6d/scan_io.h"
|
|
#include "slam6d/globals.icc"
|
|
#include "slam6d/icp6Dminimizer.h"
|
|
#include "slam6d/icp6Dsvd.h"
|
|
#include "slam6d/icp6D.h"
|
|
|
|
#ifndef _MSC_VER
|
|
#include <getopt.h>
|
|
#else
|
|
#include "XGetopt.h"
|
|
#endif
|
|
|
|
struct match {
|
|
double* s_a;
|
|
double* s_b;
|
|
double* s_c;
|
|
double* t_a;
|
|
double* t_b;
|
|
double* t_c;
|
|
int n_o_m;
|
|
double variance;
|
|
double alignxf[16];
|
|
};
|
|
|
|
void refine(int source, int target, match &tmp) {
|
|
// transform scan according to matching triple
|
|
Scan::allScans[target]->transformToMatrix(tmp.alignxf,Scan::INVALID);
|
|
// search for point pairs
|
|
//TODO
|
|
bool quiet = true;
|
|
icp6Dminimizer *my_icp6Dminimizer = 0;
|
|
my_icp6Dminimizer = new icp6D_SVD(quiet);
|
|
|
|
icp6D *my_icp = 0;
|
|
double mdm = 25;
|
|
int mni = 100;
|
|
my_icp = new icp6D(my_icp6Dminimizer, mdm, mni, quiet, false, -1, false, 1, 0.0000, false, false);
|
|
|
|
my_icp->match(Scan::allScans[source], Scan::allScans[target]);
|
|
|
|
////cout << "Point pairs: "<< tmp.n_o_m << endl;
|
|
const double * transMat = Scan::allScans[target]->get_transMat();
|
|
for(int i = 0; i < 16; i++) {
|
|
//cout << tmp.alignxf[i] << " " ;
|
|
tmp.alignxf[i] = transMat[i];
|
|
}
|
|
//cout << endl;
|
|
}
|
|
|
|
void evaluate(int source, int target, match &tmp) {
|
|
//cout << "S1 " << tmp.s_a[0] << " " << tmp.s_a[1] << " " << tmp.s_a[2] << endl;
|
|
//cout << "T1 " << tmp.t_a[0] << " " << tmp.t_a[1] << " " << tmp.t_a[2] << endl;
|
|
//cout << "S2 " << tmp.s_b[0] << " " << tmp.s_b[1] << " " << tmp.s_b[2] << endl;
|
|
//cout << "T2 " << tmp.t_b[0] << " " << tmp.t_b[1] << " " << tmp.t_b[2] << endl;
|
|
//cout << "S3 " << tmp.s_c[0] << " " << tmp.s_c[1] << " " << tmp.s_c[2] << endl;
|
|
//cout << "T3 " << tmp.t_c[0] << " " << tmp.t_c[1] << " " << tmp.t_c[2] << endl;
|
|
|
|
// Point pairs
|
|
vector<PtPair> pairs_in;
|
|
pairs_in.push_back(PtPair(tmp.s_a, tmp.t_a));
|
|
pairs_in.push_back(PtPair(tmp.s_b, tmp.t_b));
|
|
pairs_in.push_back(PtPair(tmp.s_c, tmp.t_c));
|
|
|
|
// Calculate centroids
|
|
double centroid_s[3];
|
|
double centroid_t[3];
|
|
centroid_s[0] = ((tmp.s_a[0] + tmp.s_b[0] + tmp.s_c[0])/3.0);
|
|
centroid_s[1] = ((tmp.s_a[1] + tmp.s_b[1] + tmp.s_c[1])/3.0);
|
|
centroid_s[2] = ((tmp.s_a[2] + tmp.s_b[2] + tmp.s_c[2])/3.0);
|
|
centroid_t[0] = ((tmp.t_a[0] + tmp.t_b[0] + tmp.t_c[0])/3.0);
|
|
centroid_t[1] = ((tmp.t_a[1] + tmp.t_b[1] + tmp.t_c[1])/3.0);
|
|
centroid_t[2] = ((tmp.t_a[2] + tmp.t_b[2] + tmp.t_c[2])/3.0);
|
|
|
|
// transform scan according to matching triple
|
|
icp6Dminimizer *my_icp6Dminimizer = 0;
|
|
my_icp6Dminimizer = new icp6D_SVD(true);
|
|
my_icp6Dminimizer->Point_Point_Align(pairs_in, tmp.alignxf, centroid_s, centroid_t);
|
|
Scan::allScans[target]->transformToMatrix(tmp.alignxf,Scan::INVALID);
|
|
|
|
vector<PtPair> pairs_out;
|
|
// search for point pairs
|
|
Scan::getPtPairs(&pairs_out, Scan::allScans[source], Scan::allScans[target], 1, 0, 25.0, centroid_s, centroid_t);
|
|
|
|
tmp.n_o_m = pairs_out.size();
|
|
//cout << tmp.n_o_m << endl;
|
|
|
|
if(tmp.n_o_m > Scan::allScans[target]->get_points_red_size()) {
|
|
//cout << "Point pairs: "<< tmp.n_o_m << endl;
|
|
|
|
/*
|
|
for(int i = 0; i < 16; i++) {
|
|
cout << tmp.alignxf[i] << " ";
|
|
}
|
|
for(int i = 0; i < pairs_out.size(); i++) {
|
|
cout << pairs_out[i].p1.x << " " << pairs_out[i].p1.y << " " << pairs_out[i].p1.z <<
|
|
" " << pairs_out[i].p2.x << " " << pairs_out[i].p2.y << " " << pairs_out[i].p2.z << endl;
|
|
}
|
|
*/
|
|
}
|
|
|
|
//cout << "S1 " << tmp.s_a[0] << " " << tmp.s_a[1] << " " << tmp.s_a[2] << endl;
|
|
//cout << "T1 " << tmp.t_a[0] << " " << tmp.t_a[1] << " " << tmp.t_a[2] << endl;
|
|
//cout << "S2 " << tmp.s_b[0] << " " << tmp.s_b[1] << " " << tmp.s_b[2] << endl;
|
|
//cout << "T2 " << tmp.t_b[0] << " " << tmp.t_b[1] << " " << tmp.t_b[2] << endl;
|
|
//cout << "S3 " << tmp.s_c[0] << " " << tmp.s_c[1] << " " << tmp.s_c[2] << endl;
|
|
//cout << "T3 " << tmp.t_c[0] << " " << tmp.t_c[1] << " " << tmp.t_c[2] << endl;
|
|
//cout << endl;
|
|
Scan::allScans[target]->resetPose();
|
|
|
|
delete my_icp6Dminimizer;
|
|
}
|
|
|
|
void matchBruteForce(int start, int end) { // BOF
|
|
double* const* source = Scan::allScans[start]->get_points_red();
|
|
double* const* target = Scan::allScans[end]->get_points_red();
|
|
int nr_m_s = Scan::allScans[start]->get_points_red_size();
|
|
int nr_m_t = Scan::allScans[end]->get_points_red_size();
|
|
double dist_s[nr_m_s][nr_m_s];
|
|
double dist_t[nr_m_t][nr_m_t];
|
|
double eps = 5.0;
|
|
vector<match> matching_results;
|
|
|
|
// calculate distance graphs
|
|
for(int i = 0; i < nr_m_s; i++) {
|
|
for(int j = i; j < nr_m_s; j++) {
|
|
double dist = sqrt(Dist2(source[i], source[j]));
|
|
dist_s[i][j] = dist;
|
|
dist_s[j][i] = dist;
|
|
//cout << dist << " ";
|
|
}
|
|
//cout << endl;
|
|
}
|
|
//cout << endl;
|
|
for(int i = 0; i < nr_m_t; i++) {
|
|
for(int j = i; j < nr_m_t; j++) {
|
|
double dist = sqrt(Dist2(target[i], target[j]));
|
|
dist_t[i][j] = dist;
|
|
dist_t[j][i] = dist;
|
|
//cout << dist << " ";
|
|
}
|
|
//cout << endl;
|
|
}
|
|
//cout << endl;
|
|
//cout << "Done calculating distances" << endl;
|
|
|
|
// do matching
|
|
for(int i = 0; i < nr_m_s; i++) { // 1
|
|
for(int j = i + 1; j < nr_m_s; j++) { // 2
|
|
double dist_a = dist_s[i][j];
|
|
for(int k = 0; k < nr_m_t; k++) { // 3
|
|
for(int l = 0; l < nr_m_t; l++) { // 4
|
|
if(fabs(dist_t[k][l] - dist_a) < eps) { // 5
|
|
for(int m = j + 1; m < nr_m_s; m++) { // 6
|
|
double dist_3_i = dist_s[i][m];
|
|
double dist_3_j = dist_s[j][m];
|
|
for(int n = 0; n < nr_m_t; n++) { // 7
|
|
if(fabs(dist_t[k][n] - dist_3_i) < eps) { // 8
|
|
if(fabs(dist_t[l][n] - dist_3_j) < eps) { // 9
|
|
//match found calculate error function
|
|
match tmp;
|
|
tmp.s_a = source[i];
|
|
tmp.s_b = source[j];
|
|
tmp.s_c = source[m];
|
|
tmp.t_a = target[k];
|
|
tmp.t_b = target[l];
|
|
tmp.t_c = target[n];
|
|
evaluate(start, end, tmp);
|
|
matching_results.push_back(tmp);
|
|
} // 9
|
|
} // 8
|
|
} // 7
|
|
} // 6
|
|
} // 5
|
|
} // 4
|
|
} // 3
|
|
} // 2
|
|
} // 1
|
|
int max = 0;
|
|
int max_i = 0;
|
|
for(int i = 0; i < matching_results.size(); i++) {
|
|
if(matching_results[i].n_o_m > max) {
|
|
max_i = i;
|
|
max = matching_results[i].n_o_m;
|
|
}
|
|
//cout << i << ": " << matching_results[i].n_o_m << ", " << matching_results[i].variance << endl;
|
|
for(int j = 0; j < 16; j++) {
|
|
//cout << matching_results[i].alignxf[j] << " ";
|
|
}
|
|
}
|
|
//cout << "Max index: " << max_i << ", points: " << max << endl;
|
|
for(int i = 0; i < 16; i++) {
|
|
//cout << matching_results[max_i].alignxf[i] << " ";
|
|
}
|
|
refine(start, end, matching_results[max_i]);
|
|
//cout << endl << "Refined: " << endl;
|
|
for(int i = 0; i < 16; i++) {
|
|
cout << matching_results[max_i].alignxf[i] << " ";
|
|
}
|
|
|
|
cout << " 2" << endl;
|
|
} // EOF
|
|
|
|
void matchMarker(int start, int end) { // BOF
|
|
cout << start << " " << end << endl;
|
|
double* const* source = Scan::allScans[start]->get_points_red();
|
|
double* const* target = Scan::allScans[end]->get_points_red();
|
|
int nr_m_s = Scan::allScans[start]->get_points_red_size();
|
|
int nr_m_t = Scan::allScans[end]->get_points_red_size();
|
|
cout << "Points: " << nr_m_s << " " << nr_m_t << endl;
|
|
double dist_s[nr_m_s][nr_m_s];
|
|
double dist_t[nr_m_t][nr_m_t];
|
|
double eps = 5.0;
|
|
vector<match> matching_results;
|
|
|
|
// calculate distance graphs
|
|
for(int i = 0; i < nr_m_s; i++) {
|
|
for(int j = i; j < nr_m_s; j++) {
|
|
double dist = sqrt(Dist2(source[i], source[j]));
|
|
dist_s[i][j] = dist;
|
|
dist_s[j][i] = dist;
|
|
//cout << dist << " ";
|
|
}
|
|
//cout << endl;
|
|
}
|
|
//cout << endl;
|
|
for(int i = 0; i < nr_m_t; i++) {
|
|
for(int j = i; j < nr_m_t; j++) {
|
|
double dist = sqrt(Dist2(target[i], target[j]));
|
|
dist_t[i][j] = dist;
|
|
dist_t[j][i] = dist;
|
|
//cout << dist << " ";
|
|
}
|
|
//cout << endl;
|
|
}
|
|
//cout << endl;
|
|
//cout << "Done calculating distances" << endl;
|
|
for(int runs = 0; runs < 100; runs++) {
|
|
int i = (int) (nr_m_s*(rand()/(RAND_MAX+1.0)));
|
|
int j = (int) (nr_m_s*(rand()/(RAND_MAX+1.0)));
|
|
int m = (int) (nr_m_s*(rand()/(RAND_MAX+1.0)));
|
|
double dist_a = dist_s[i][j];
|
|
double dist_3_i = dist_s[i][m];
|
|
double dist_3_j = dist_s[j][m];
|
|
|
|
|
|
// do matching
|
|
for(int k = 0; k < nr_m_t; k++) { // 3
|
|
for(int l = 0; l < nr_m_t; l++) { // 4
|
|
if(fabs(dist_t[k][l] - dist_a) < eps) { // 5
|
|
|
|
for(int n = 0; n < nr_m_t; n++) { // 7
|
|
if(fabs(dist_t[k][n] - dist_3_i) < eps) { // 8
|
|
if(fabs(dist_t[l][n] - dist_3_j) < eps) { // 9
|
|
//match found calculate error function
|
|
match tmp;
|
|
tmp.s_a = source[i];
|
|
tmp.s_b = source[j];
|
|
tmp.s_c = source[m];
|
|
tmp.t_a = target[k];
|
|
tmp.t_b = target[l];
|
|
tmp.t_c = target[n];
|
|
evaluate(start, end, tmp);
|
|
matching_results.push_back(tmp);
|
|
} // 9
|
|
} // 8
|
|
} // 7
|
|
} // 6
|
|
// 5
|
|
} // 4
|
|
} // 3
|
|
} // 1
|
|
int max = 0;
|
|
int max_i = 0;
|
|
for(int i = 0; i < matching_results.size(); i++) {
|
|
if(matching_results[i].n_o_m > max) {
|
|
max_i = i;
|
|
max = matching_results[i].n_o_m;
|
|
//cout << i << " " << max << endl;
|
|
}
|
|
//cout << i << ": " << matching_results[i].n_o_m << ", " << matching_results[i].variance << endl;
|
|
for(int j = 0; j < 16; j++) {
|
|
//cout << matching_results[i].alignxf[j] << " ";
|
|
}
|
|
}
|
|
cout << "Really?" << endl;
|
|
//cout << "Max index: " << max_i << ", points: " << max << endl;
|
|
for(int i = 0; i < 16; i++) {
|
|
//cout << matching_results[max_i].alignxf[i] << " ";
|
|
}
|
|
refine(start, end, matching_results[max_i]);
|
|
//cout << endl << "Refined: " << endl;
|
|
for(int i = 0; i < 16; i++) {
|
|
cout << matching_results[max_i].alignxf[i] << " ";
|
|
}
|
|
|
|
cout << " 2" << endl;
|
|
} // EOF
|
|
|
|
/**
|
|
* 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 << " -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 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, double &dist,
|
|
IOType &type, bool &desc)
|
|
{
|
|
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' },
|
|
{ "start", required_argument, 0, 's' },
|
|
{ "end", required_argument, 0, 'e' },
|
|
{ "dist", required_argument, 0, 'd' },
|
|
{ 0, 0, 0, 0} // needed, cf. getopt.h
|
|
};
|
|
|
|
//cout << endl;
|
|
while ((c = getopt_long(argc, argv, "f:d:s:e:", longopts, NULL)) != -1)
|
|
switch (c)
|
|
{
|
|
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 '?':
|
|
usage(argv[0]);
|
|
return 1;
|
|
default:
|
|
abort ();
|
|
}
|
|
|
|
if(start < 0 || end < 0 || start == end) {
|
|
cerr << "\n*** You need two different scans for marker based position calculation ***" << 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 = UOS;
|
|
bool desc = false;
|
|
|
|
parseArgs(argc, argv, dir, start, end, dist, type, desc);
|
|
|
|
Scan::readScansRedSearch(type, start, end, dir, maxDist, minDist, -1, 1, false, false, false);
|
|
cout << "Start match marker ..." << endl;
|
|
srand(time(0));
|
|
srand(0);
|
|
long starttime = GetCurrentTimeInMilliSec();
|
|
// matchBruteForce(start, end-start);
|
|
matchMarker(0, end - start);
|
|
starttime = (GetCurrentTimeInMilliSec() - starttime);
|
|
cout << " done in " << starttime << " ms" << endl;
|
|
|
|
vector <Scan*>::iterator Iter = Scan::allScans.begin();
|
|
for( ; Iter != Scan::allScans.end(); ) {
|
|
Iter = Scan::allScans.begin();
|
|
delete (*Iter);
|
|
cout << ".";
|
|
cout.flush();
|
|
}
|
|
Scan::allScans.clear();
|
|
|
|
cout << endl << endl;
|
|
cout << "Normal program end." << endl << endl;
|
|
}
|