168 lines
4.4 KiB
Text
168 lines
4.4 KiB
Text
/*
|
|
* elch6Dslerp implementation
|
|
*
|
|
* Copyright (C) Jochen Sprickerhof
|
|
*
|
|
* Released under the GPL version 3.
|
|
*
|
|
*/
|
|
|
|
/**
|
|
* @file ELCH implementation using SLERP
|
|
* @author Jochen Sprickerhof. Institute of Computer Science, University of Osnabrueck, Germany.
|
|
*/
|
|
|
|
#include "slam6d/elch6Dslerp.h"
|
|
|
|
#include "slam6d/metaScan.h"
|
|
#include "slam6d/lum6Dquat.h"
|
|
#include "slam6d/globals.icc"
|
|
|
|
#include <iostream>
|
|
using std::cout;
|
|
using std::cerr;
|
|
using std::endl;
|
|
|
|
#include <fstream>
|
|
using std::ofstream;
|
|
|
|
#include <cstring>
|
|
|
|
#include <boost/graph/graph_traits.hpp>
|
|
using boost::graph_traits;
|
|
using namespace NEWMAT;
|
|
/**
|
|
* ELCH loop closing function using SLERP
|
|
* matches first and last scan of a loop with ICP
|
|
* distributes the error
|
|
*
|
|
* @param allScans all laser scans
|
|
* @param first index of first laser scan in the loop
|
|
* @param last indes of last laser scan in the loop
|
|
* @param g graph for loop optimization
|
|
*/
|
|
void elch6Dslerp::close_loop(const vector <Scan *> &allScans, int first, int last, graph_t &g)
|
|
{
|
|
int n = num_vertices(g);
|
|
graph_t grb[4];
|
|
graph_traits <graph_t>::edge_iterator ei = edges(g).first;
|
|
int num_arcs = num_edges(g);
|
|
int li = 0;
|
|
#ifdef _OPENMP
|
|
#pragma omp parallel for firstprivate(li, ei)
|
|
#endif
|
|
for(int i = 0; i < num_arcs; i++) {
|
|
for(;i > li; li++, ei++) ;
|
|
for(;i < li; li--, ei--) ;
|
|
Matrix C(7, 7);
|
|
int from = source(*ei, g);
|
|
int to = target(*ei, g);
|
|
lum6DQuat::covarianceQuat(allScans[from], allScans[to], my_icp6D->get_nns_method(), my_icp6D->get_rnd(), my_icp6D->get_max_dist_match2(), &C);
|
|
C = C.i();
|
|
for(int j = 0; j < 3; j++) {
|
|
#ifdef _OPENMP
|
|
#pragma omp critical
|
|
#endif
|
|
add_edge(from, to, fabs(C(j + 1, j + 1)), grb[j]);
|
|
}
|
|
#ifdef _OPENMP
|
|
#pragma omp critical
|
|
#endif
|
|
add_edge(from, to, fabs(C(4, 4)) + fabs(C(5, 5)) + fabs(C(6, 6)) + fabs(C(7, 7)), grb[3]);
|
|
li++;
|
|
ei++;
|
|
}
|
|
|
|
double *weights[4];
|
|
for(int i = 0; i < 4; i++) {
|
|
weights[i] = new double[n];
|
|
graph_balancer(grb[i], first, last, weights[i]);
|
|
}
|
|
|
|
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++) {
|
|
meta_end.push_back(allScans[i]);
|
|
}
|
|
MetaScan *end = new MetaScan(meta_end, false, false);
|
|
|
|
double Pl0[16];
|
|
memcpy(Pl0, allScans[last]->get_transMat(), 16 * sizeof(double));
|
|
|
|
my_icp6D->match(start, end);
|
|
|
|
delete start;
|
|
delete end;
|
|
|
|
// store ICP
|
|
double Pp0[16];
|
|
memcpy(Pp0, allScans[last]->get_transMat(), 16 * sizeof(double));
|
|
|
|
//compute Delta
|
|
double Pf0[16], Pf0_inv[16], tmp1[16], tmp2[16], deltaf[16];
|
|
memcpy(Pf0, allScans[first]->get_transMat(), 16 * sizeof(double));
|
|
M4inv(Pf0, Pf0_inv);
|
|
|
|
MMult(Pf0_inv, Pl0, tmp1);
|
|
M4inv(tmp1, tmp2);
|
|
MMult(Pp0, tmp2, tmp1);
|
|
MMult(Pf0_inv, tmp1, deltaf);
|
|
|
|
double deltaT[3], deltaQ[4];
|
|
Matrix4ToQuat(deltaf, deltaQ, deltaT);
|
|
|
|
if(!quiet) {
|
|
double axisangle[4];
|
|
axisangle[0] = deltaQ[0];
|
|
axisangle[1] = deltaQ[1];
|
|
axisangle[2] = deltaQ[2];
|
|
axisangle[3] = deltaQ[3];
|
|
QuatToAA(axisangle);
|
|
cout << "Delta: " << deltaT[0] << " " << deltaT[1] << " " << deltaT[2] << " " << axisangle[0] << " " << axisangle[1] << " " << axisangle[2] << " " << axisangle[3] << endl;
|
|
}
|
|
|
|
//transform scans
|
|
double idQ[4] = {1, 0, 0, 0}, rPos[3], rPosQuat[4], delta0[16];
|
|
|
|
rPos[0] = deltaT[0] * weights[0][0];
|
|
rPos[1] = deltaT[1] * weights[1][0];
|
|
rPos[2] = deltaT[2] * weights[2][0];
|
|
slerp(idQ, deltaQ, weights[3][0], rPosQuat);
|
|
QuatToMatrix4(rPosQuat, rPos, tmp1);
|
|
M4inv(tmp1, tmp2);
|
|
MMult(Pf0, tmp2, delta0);
|
|
|
|
#ifdef _OPENMP
|
|
#pragma omp parallel for private(rPos, rPosQuat, tmp1, tmp2)
|
|
#endif
|
|
for(int i = 1; i < n; i++) {
|
|
if(i >= last - offset_last_start && i <= last + offset_last_end) {
|
|
MMult(delta0, Pf0_inv, tmp1);
|
|
} else {
|
|
rPos[0] = deltaT[0] * weights[0][i];
|
|
rPos[1] = deltaT[1] * weights[1][i];
|
|
rPos[2] = deltaT[2] * weights[2][i];
|
|
slerp(idQ, deltaQ, weights[3][i], rPosQuat);
|
|
QuatToMatrix4(rPosQuat, rPos, tmp1);
|
|
MMult(delta0, tmp1, tmp2);
|
|
MMult(tmp2, Pf0_inv, tmp1);
|
|
}
|
|
allScans[i]->transform(tmp1, Scan::ELCH, i == n-1 ? 2 : 1);
|
|
}
|
|
|
|
for(int i = 0; i < 4; i++) {
|
|
delete [] weights[i];
|
|
}
|
|
|
|
}
|