3dpcp/.svn/pristine/59/59e7b58ae231a710825fa8db7e87125c4dec1291.svn-base

169 lines
4.4 KiB
Text
Raw Normal View History

2012-09-16 12:33:11 +00:00
/*
* 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];
}
}