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.
153 lines
4.2 KiB
Plaintext
153 lines
4.2 KiB
Plaintext
/*
|
|
* graphToro implementation
|
|
*
|
|
* Copyright (C) Jochen Sprickerhof
|
|
*
|
|
* Released under the GPL version 3.
|
|
*
|
|
*/
|
|
|
|
|
|
/**
|
|
* @file TORO wrapper
|
|
* @author Jochen Sprickerhof. Institute of Computer Science, University of Osnabrueck, Germany.
|
|
*/
|
|
|
|
#include "slam6d/graphToro.h"
|
|
|
|
#include "slam6d/metaScan.h"
|
|
#include "slam6d/lum6Deuler.h"
|
|
#include "slam6d/globals.icc"
|
|
|
|
#include <fstream>
|
|
using std::ofstream;
|
|
using std::ifstream;
|
|
#include <cfloat>
|
|
#include <cstring>
|
|
using namespace NEWMAT;
|
|
/**
|
|
* This function is used to match a set of laser scans with any minimally
|
|
* connected Graph.
|
|
*
|
|
* @param gr Some Graph with no real subgraphs except for itself
|
|
* @param allScans Contains all laser scans
|
|
* @param nrIt The number of iterations the LUM-algorithm will run
|
|
* @return Euclidian distance of all pose shifts
|
|
*/
|
|
double graphToro::doGraphSlam6D(Graph gr, vector <Scan *> allScans, int nrIt)
|
|
{
|
|
Matrix C(6, 6);
|
|
double invers[16], rela[16], rPos[3], rPosTheta[3], rPosQuat[4];
|
|
double Pl0[16];
|
|
|
|
ofstream outFile("toro.graph");
|
|
int n = gr.getNrScans();
|
|
for(int i = 0; i < n; i++) {
|
|
QuatRPYEuler(allScans[i]->get_rPosQuat(), rPosTheta);
|
|
outFile << "VERTEX3" << " " << i <<
|
|
" " << (allScans[i]->get_rPos()[0]/100) <<
|
|
" " << (allScans[i]->get_rPos()[1]/100) <<
|
|
" " << (allScans[i]->get_rPos()[2]/100) <<
|
|
" " << rPosTheta[0] <<
|
|
" " << rPosTheta[1] <<
|
|
" " << rPosTheta[2] << endl;
|
|
}
|
|
|
|
for(int i = 0; i < gr.getNrLinks(); i++){
|
|
int first = gr.getLink(i,0);
|
|
int last = gr.getLink(i,1);
|
|
|
|
if(first != last-1) {
|
|
vector <Scan *> meta_start;
|
|
for(int i = first - 2; i <= first + 2; i++) {
|
|
if(i >= 0) {
|
|
meta_start.push_back(allScans[i]);
|
|
}
|
|
}
|
|
MetaScan *start = new MetaScan(meta_start, false, false);
|
|
|
|
//static size of metascan
|
|
int offset_last_start = 2;
|
|
int offset_last_end = 0;
|
|
|
|
vector <Scan *> meta_end;
|
|
for(int i = last - offset_last_start; i <= last + offset_last_end && i < n; i++) {
|
|
if(i >= 0) {
|
|
meta_end.push_back(allScans[i]);
|
|
}
|
|
}
|
|
MetaScan *end = new MetaScan(meta_end, false, false);
|
|
|
|
memcpy(Pl0, allScans[last]->get_transMat(), 16 * sizeof(double));
|
|
my_icp->match(start, end);
|
|
|
|
delete start;
|
|
delete end;
|
|
}
|
|
|
|
M4inv(allScans[last]->get_transMat(), invers);
|
|
MMult(invers, allScans[first]->get_transMat(), rela);
|
|
Matrix4ToQuat(rela, rPosQuat, rPos);
|
|
QuatRPYEuler(rPosQuat, rPosTheta);
|
|
|
|
lum6DEuler::covarianceEuler(allScans[first], allScans[last], my_icp->get_nns_method(), my_icp->get_rnd(), my_icp->get_max_dist_match2(), &C);
|
|
|
|
outFile << "EDGE3" << " " << last << " " << first << " " <<
|
|
(rPos[0]/100) << " " <<
|
|
(rPos[1]/100) << " " <<
|
|
(rPos[2]/100) << " " <<
|
|
rPosTheta[0] << " " <<
|
|
rPosTheta[1] << " " <<
|
|
rPosTheta[2] << " ";
|
|
for(int i = 1; i < 7; i++)
|
|
for(int j = i; j < 7; j++)
|
|
outFile << C(i, j) << " ";
|
|
outFile << endl;
|
|
|
|
if(first != last-1) {
|
|
allScans[last]->transformToMatrix(Pl0,Scan::INVALID);
|
|
}
|
|
}
|
|
outFile.close();
|
|
|
|
system("sort toro.graph > toro2.graph && mv toro2.graph toro.graph && ./bin/toro3d toro.graph");
|
|
|
|
ifstream inFile("toro-treeopt-final.graph");
|
|
string tag;
|
|
int id;
|
|
double dd;
|
|
double rPosN[3], rPosThetaN[3];
|
|
while(inFile) {
|
|
inFile >> tag;
|
|
if(tag == "VERTEX3") {
|
|
inFile >> id;
|
|
if(id == n-1) {
|
|
inFile >> rPosN[0] >> rPosN[1] >> rPosN[2] >> rPosThetaN[0] >> rPosThetaN[1] >> rPosThetaN[2];
|
|
rPosN[0] *= 100;
|
|
rPosN[1] *= 100;
|
|
rPosN[2] *= 100;
|
|
} else {
|
|
inFile >> rPos[0] >> rPos[1] >> rPos[2] >> rPosTheta[0] >> rPosTheta[1] >> rPosTheta[2];
|
|
rPos[0] *= 100;
|
|
rPos[1] *= 100;
|
|
rPos[2] *= 100;
|
|
RPYEulerQuat(rPosTheta, rPosQuat);
|
|
if(id != 0) {
|
|
allScans[id]->transformToQuat(rPos, rPosQuat, Scan::GRAPHTORO, 1);
|
|
}
|
|
}
|
|
}
|
|
else if(tag == "EDGE3") {
|
|
inFile >> id >> id;
|
|
for(int i=0; i < 22; i++) {
|
|
inFile >> dd;
|
|
}
|
|
}
|
|
}
|
|
RPYEulerQuat(rPosThetaN, rPosQuat);
|
|
allScans[n-1]->transformToQuat(rPosN, rPosQuat, Scan::GRAPHTORO, 2);
|
|
inFile.close();
|
|
|
|
return DBL_MAX;
|
|
}
|