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.
863 lines
25 KiB
Plaintext
863 lines
25 KiB
Plaintext
/*
|
|
* scan implementation
|
|
*
|
|
* Copyright (C) Andreas Nuechter, Kai Lingemann, Dorit Borrmann, Jan Elseberg, Thomas Escher
|
|
*
|
|
* Released under the GPL version 3.
|
|
*
|
|
*/
|
|
|
|
#include "slam6d/scan.h"
|
|
|
|
#include "slam6d/basicScan.h"
|
|
#include "slam6d/managedScan.h"
|
|
#include "slam6d/metaScan.h"
|
|
#include "slam6d/searchTree.h"
|
|
#include "slam6d/kd.h"
|
|
#include "slam6d/Boctree.h"
|
|
#include "slam6d/globals.icc"
|
|
|
|
#ifdef WITH_METRICS
|
|
#include "slam6d/metrics.h"
|
|
#endif
|
|
|
|
#ifdef _MSC_VER
|
|
#define _NO_PARALLEL_READ
|
|
#endif
|
|
|
|
#ifdef __APPLE__
|
|
#define _NO_PARALLEL_READ
|
|
#endif
|
|
|
|
using std::vector;
|
|
|
|
|
|
vector<Scan*> Scan::allScans;
|
|
bool Scan::scanserver = false;
|
|
|
|
|
|
void Scan::openDirectory(bool scanserver, const std::string& path, IOType type,
|
|
int start, int end)
|
|
{
|
|
Scan::scanserver = scanserver;
|
|
if(scanserver)
|
|
ManagedScan::openDirectory(path, type, start, end);
|
|
else
|
|
BasicScan::openDirectory(path, type, start, end);
|
|
}
|
|
|
|
void Scan::closeDirectory()
|
|
{
|
|
if(scanserver)
|
|
ManagedScan::closeDirectory();
|
|
else
|
|
BasicScan::closeDirectory();
|
|
}
|
|
|
|
Scan::Scan()
|
|
{
|
|
unsigned int i;
|
|
|
|
// pose and transformations
|
|
for(i = 0; i < 3; ++i) rPos[i] = 0;
|
|
for(i = 0; i < 3; ++i) rPosTheta[i] = 0;
|
|
for(i = 0; i < 4; ++i) rQuat[i] = 0;
|
|
M4identity(transMat);
|
|
M4identity(transMatOrg);
|
|
M4identity(dalignxf);
|
|
|
|
// trees and reduction methods
|
|
cuda_enabled = false;
|
|
nns_method = -1;
|
|
kd = 0;
|
|
ann_kd_tree = 0;
|
|
|
|
// reduction on-demand
|
|
reduction_voxelSize = 0.0;
|
|
reduction_nrpts = 0;
|
|
reduction_pointtype = PointType();
|
|
|
|
// flags
|
|
m_has_reduced = false;
|
|
|
|
// octtree
|
|
octtree_reduction_voxelSize = 0.0;
|
|
octtree_voxelSize = 0.0;
|
|
octtree_pointtype = PointType();
|
|
octtree_loadOct = false;
|
|
octtree_saveOct = false;
|
|
}
|
|
|
|
Scan::~Scan()
|
|
{
|
|
if(kd) delete kd;
|
|
}
|
|
|
|
void Scan::setReductionParameter(double voxelSize, int nrpts, PointType pointtype)
|
|
{
|
|
reduction_voxelSize = voxelSize;
|
|
reduction_nrpts = nrpts;
|
|
reduction_pointtype = pointtype;
|
|
}
|
|
|
|
void Scan::setSearchTreeParameter(int nns_method, bool cuda_enabled)
|
|
{
|
|
searchtree_nnstype = nns_method;
|
|
searchtree_cuda_enabled = cuda_enabled;
|
|
}
|
|
|
|
void Scan::setOcttreeParameter(double reduction_voxelSize, double voxelSize, PointType pointtype, bool loadOct, bool saveOct)
|
|
{
|
|
octtree_reduction_voxelSize = reduction_voxelSize;
|
|
octtree_voxelSize = voxelSize;
|
|
octtree_pointtype = pointtype;
|
|
octtree_loadOct = loadOct;
|
|
octtree_saveOct = saveOct;
|
|
}
|
|
|
|
void Scan::clear(unsigned int types)
|
|
{
|
|
if(types & DATA_XYZ) clear("xyz");
|
|
if(types & DATA_RGB) clear("rgb");
|
|
if(types & DATA_REFLECTANCE) clear("reflectance");
|
|
if(types & DATA_TEMPERATURE) clear("temperature");
|
|
if(types & DATA_AMPLITUDE) clear("amplitude");
|
|
if(types & DATA_TYPE) clear("type");
|
|
if(types & DATA_DEVIATION) clear("deviation");
|
|
}
|
|
|
|
SearchTree* Scan::getSearchTree()
|
|
{
|
|
// if the search tree hasn't been created yet, calculate everything
|
|
if(kd == 0) {
|
|
createSearchTree();
|
|
}
|
|
return kd;
|
|
}
|
|
|
|
void Scan::toGlobal() {
|
|
calcReducedPoints();
|
|
transform(transMatOrg, INVALID);
|
|
}
|
|
|
|
/**
|
|
* Computes a search tree depending on the type.
|
|
*/
|
|
void Scan::createSearchTree()
|
|
{
|
|
// multiple threads will call this function at the same time because they all work on one pair of Scans, just let the first one (who sees a nullpointer) do the creation
|
|
boost::lock_guard<boost::mutex> lock(m_mutex_create_tree);
|
|
if(kd != 0) return;
|
|
|
|
// make sure the original points are created before starting the measurement
|
|
DataXYZ xyz_orig(get("xyz reduced original"));
|
|
|
|
#ifdef WITH_METRICS
|
|
Timer tc = ClientMetric::create_tree_time.start();
|
|
#endif //WITH_METRICS
|
|
|
|
createSearchTreePrivate();
|
|
|
|
#ifdef WITH_METRICS
|
|
ClientMetric::create_tree_time.end(tc);
|
|
#endif //WITH_METRICS
|
|
}
|
|
|
|
void Scan::calcReducedOnDemand()
|
|
{
|
|
// multiple threads will call this function at the same time because they all work on one pair of Scans, just let the first one (who sees count as zero) do the reduction
|
|
boost::lock_guard<boost::mutex> lock(m_mutex_reduction);
|
|
if(m_has_reduced) return;
|
|
|
|
#ifdef WITH_METRICS
|
|
Timer t = ClientMetric::on_demand_reduction_time.start();
|
|
#endif //WITH_METRICS
|
|
|
|
calcReducedOnDemandPrivate();
|
|
|
|
m_has_reduced = true;
|
|
|
|
#ifdef WITH_METRICS
|
|
ClientMetric::on_demand_reduction_time.end(t);
|
|
#endif //WITH_METRICS
|
|
}
|
|
|
|
void Scan::copyReducedToOriginal()
|
|
{
|
|
#ifdef WITH_METRICS
|
|
Timer t = ClientMetric::copy_original_time.start();
|
|
#endif //WITH_METRICS
|
|
|
|
DataXYZ xyz_reduced(get("xyz reduced"));
|
|
unsigned int size = xyz_reduced.size();
|
|
DataXYZ xyz_reduced_orig(create("xyz reduced original", sizeof(double)*3*size));
|
|
for(unsigned int i = 0; i < size; ++i) {
|
|
for(unsigned int j = 0; j < 3; ++j) {
|
|
xyz_reduced_orig[i][j] = xyz_reduced[i][j];
|
|
}
|
|
}
|
|
|
|
#ifdef WITH_METRICS
|
|
ClientMetric::copy_original_time.end(t);
|
|
#endif //WITH_METRICS
|
|
}
|
|
|
|
void Scan::copyOriginalToReduced()
|
|
{
|
|
#ifdef WITH_METRICS
|
|
Timer t = ClientMetric::copy_original_time.start();
|
|
#endif //WITH_METRICS
|
|
|
|
DataXYZ xyz_reduced_orig(get("xyz reduced original"));
|
|
unsigned int size = xyz_reduced_orig.size();
|
|
DataXYZ xyz_reduced(create("xyz reduced", sizeof(double)*3*size));
|
|
for(unsigned int i = 0; i < size; ++i) {
|
|
for(unsigned int j = 0; j < 3; ++j) {
|
|
xyz_reduced[i][j] = xyz_reduced_orig[i][j];
|
|
}
|
|
}
|
|
|
|
#ifdef WITH_METRICS
|
|
ClientMetric::copy_original_time.end(t);
|
|
#endif //WITH_METRICS
|
|
}
|
|
|
|
|
|
|
|
/**
|
|
* Computes an octtree of the current scan, then getting the
|
|
* reduced points as the centers of the octree voxels.
|
|
*/
|
|
void Scan::calcReducedPoints()
|
|
{
|
|
#ifdef WITH_METRICS
|
|
Timer t = ClientMetric::scan_load_time.start();
|
|
#endif //WITH_METRICS
|
|
|
|
|
|
// get xyz to start the scan load, separated here for time measurement
|
|
|
|
DataXYZ xyz(get("xyz"));
|
|
DataReflectance reflectance(get("reflectance"));
|
|
if(xyz.size() == 0)
|
|
throw runtime_error("Could not calculate reduced points, XYZ data is empty");
|
|
|
|
if (reflectance.size()==0) {
|
|
|
|
// no reduction needed
|
|
// copy vector of points to array of points to avoid
|
|
// further copying
|
|
if(reduction_voxelSize <= 0.0) {
|
|
// copy the points
|
|
DataXYZ xyz_reduced(create("xyz reduced", sizeof(double)*3*xyz.size()));
|
|
for(unsigned int i = 0; i < xyz.size(); ++i) {
|
|
for(unsigned int j = 0; j < 3; ++j) {
|
|
xyz_reduced[i][j] = xyz[i][j];
|
|
}
|
|
}
|
|
} else {
|
|
// start reduction
|
|
// build octree-tree from CurrentScan
|
|
// put full data into the octtree
|
|
BOctTree<double> *oct = new BOctTree<double>(PointerArray<double>(xyz).get(),
|
|
xyz.size(), reduction_voxelSize, reduction_pointtype);
|
|
|
|
vector<double*> center;
|
|
center.clear();
|
|
if (reduction_nrpts > 0) {
|
|
if (reduction_nrpts == 1) {
|
|
oct->GetOctTreeRandom(center);
|
|
} else {
|
|
oct->GetOctTreeRandom(center, reduction_nrpts);
|
|
}
|
|
} else {
|
|
oct->GetOctTreeCenter(center);
|
|
}
|
|
|
|
// storing it as reduced scan
|
|
unsigned int size = center.size();
|
|
DataXYZ xyz_reduced(create("xyz reduced", sizeof(double)*3*size));
|
|
for(unsigned int i = 0; i < size; ++i) {
|
|
for(unsigned int j = 0; j < 3; ++j) {
|
|
xyz_reduced[i][j] = center[i][j];
|
|
}
|
|
}
|
|
delete oct;
|
|
}
|
|
} else {
|
|
if(xyz.size() != reflectance.size())
|
|
throw runtime_error("Could not calculate reduced reflectance, reflectance size is different from points size");
|
|
double **xyz_reflectance = new double*[xyz.size()];
|
|
for (unsigned int i = 0; i < xyz.size(); ++i) {
|
|
xyz_reflectance[i] = new double[4];
|
|
for (unsigned int j = 0; j < 3; ++j)
|
|
xyz_reflectance[i][j] = xyz[i][j];
|
|
xyz_reflectance[i][3] = reflectance[i];
|
|
}
|
|
|
|
#ifdef WITH_METRICS
|
|
ClientMetric::scan_load_time.end(t);
|
|
Timer tl = ClientMetric::calc_reduced_points_time.start();
|
|
#endif //WITH_METRICS
|
|
|
|
// no reduction needed
|
|
// copy vector of points to array of points to avoid
|
|
// further copying
|
|
if(reduction_voxelSize <= 0.0) {
|
|
// copy the points
|
|
if (reduction_pointtype.hasReflectance()) {
|
|
DataXYZ xyz_reduced(create("xyz reduced", sizeof(double)*3*xyz.size()));
|
|
DataReflectance reflectance_reduced(create("reflectance reduced", sizeof(double)*reflectance.size()));
|
|
for(unsigned int i = 0; i < xyz.size(); ++i) {
|
|
for(unsigned int j = 0; j < 3; ++j)
|
|
xyz_reduced[i][j] = xyz[i][j];
|
|
reflectance_reduced[i] = reflectance[i];
|
|
}
|
|
} else {
|
|
DataXYZ xyz_reduced(create("xyz reduced", sizeof(double)*3*xyz.size()));
|
|
for(unsigned int i = 0; i < xyz.size(); ++i) {
|
|
for(unsigned int j = 0; j < 3; ++j) {
|
|
xyz_reduced[i][j] = xyz[i][j];
|
|
}
|
|
}
|
|
}
|
|
} else {
|
|
// start reduction
|
|
// build octree-tree from CurrentScan
|
|
// put full data into the octtree
|
|
BOctTree<double> *oct = new BOctTree<double>(xyz_reflectance, xyz.size(), reduction_voxelSize, reduction_pointtype);
|
|
vector<double*> reduced;
|
|
reduced.clear();
|
|
|
|
if (reduction_nrpts > 0) {
|
|
if (reduction_nrpts == 1) {
|
|
oct->GetOctTreeRandom(reduced);
|
|
} else {
|
|
oct->GetOctTreeRandom(reduced, reduction_nrpts);
|
|
}
|
|
} else {
|
|
oct->GetOctTreeCenter(reduced);
|
|
}
|
|
|
|
// storing it as reduced scan
|
|
unsigned int size = reduced.size();
|
|
if (reduction_pointtype.hasReflectance()) {
|
|
DataXYZ xyz_reduced(create("xyz reduced", sizeof(double)*3*size));
|
|
for(unsigned int i = 0; i < size; ++i) {
|
|
for(unsigned int j = 0; j < 3; ++j) {
|
|
xyz_reduced[i][j] = reduced[i][j];
|
|
}
|
|
}
|
|
DataReflectance reflectance_reduced(create("reflectance reduced", sizeof(float)*size));
|
|
for(unsigned int i = 0; i < size; ++i)
|
|
reflectance_reduced[i] = reduced[i][3];
|
|
} else {
|
|
DataXYZ xyz_reduced(create("xyz reduced", sizeof(double)*3*size));
|
|
for(unsigned int i = 0; i < size; ++i)
|
|
for(unsigned int j = 0; j < 3; ++j)
|
|
xyz_reduced[i][j] = reduced[i][j];
|
|
}
|
|
delete oct;
|
|
}
|
|
for (unsigned int i = 0; i < xyz.size(); ++i) {
|
|
delete[] xyz_reflectance[i];
|
|
}
|
|
delete[] xyz_reflectance;
|
|
#ifdef WITH_METRICS
|
|
ClientMetric::calc_reduced_points_time.end(tl);
|
|
#endif //WITH_METRICS
|
|
}
|
|
}
|
|
|
|
|
|
/**
|
|
* Merges the scan's intrinsic coordinates with the robot position.
|
|
* @param prevScan The scan that's transformation is extrapolated,
|
|
* i.e., odometry extrapolation
|
|
*
|
|
* For additional information see the following paper (jfr2007.pdf):
|
|
*
|
|
* Andreas Nüchter, Kai Lingemann, Joachim Hertzberg, and Hartmut Surmann,
|
|
* 6D SLAM - 3D Mapping Outdoor Environments Journal of Field Robotics (JFR),
|
|
* Special Issue on Quantitative Performance Evaluation of Robotic and Intelligent
|
|
* Systems, Wiley & Son, ISSN 1556-4959, Volume 24, Issue 8-9, pages 699 - 722,
|
|
* August/September, 2007
|
|
*
|
|
*/
|
|
void Scan::mergeCoordinatesWithRoboterPosition(Scan* prevScan)
|
|
{
|
|
double tempMat[16], deltaMat[16];
|
|
M4inv(prevScan->get_transMatOrg(), tempMat);
|
|
MMult(prevScan->get_transMat(), tempMat, deltaMat);
|
|
transform(deltaMat, INVALID); //apply delta transformation of the previous scan
|
|
}
|
|
|
|
/**
|
|
* The method transforms all points with the given transformation matrix.
|
|
*/
|
|
void Scan::transformAll(const double alignxf[16])
|
|
{
|
|
DataXYZ xyz(get("xyz"));
|
|
unsigned int i=0 ;
|
|
// #pragma omp parallel for
|
|
for(; i < xyz.size(); ++i) {
|
|
transform3(alignxf, xyz[i]);
|
|
}
|
|
// TODO: test for ManagedScan compability, may need a touch("xyz") to mark saving the new values
|
|
}
|
|
|
|
//! Internal function of transform which alters the reduced points
|
|
void Scan::transformReduced(const double alignxf[16])
|
|
{
|
|
#ifdef WITH_METRICS
|
|
Timer t = ClientMetric::transform_time.start();
|
|
#endif //WITH_METRICS
|
|
|
|
DataXYZ xyz_reduced(get("xyz reduced"));
|
|
unsigned int i=0;
|
|
// #pragma omp parallel for
|
|
for( ; i < xyz_reduced.size(); ++i) {
|
|
transform3(alignxf, xyz_reduced[i]);
|
|
}
|
|
|
|
#ifdef WITH_METRICS
|
|
ClientMetric::transform_time.end(t);
|
|
#endif //WITH_METRICS
|
|
}
|
|
|
|
//! Internal function of transform which handles the matrices
|
|
void Scan::transformMatrix(const double alignxf[16])
|
|
{
|
|
double tempxf[16];
|
|
|
|
// apply alignxf to transMat and update pose vectors
|
|
MMult(alignxf, transMat, tempxf);
|
|
memcpy(transMat, tempxf, sizeof(transMat));
|
|
Matrix4ToEuler(transMat, rPosTheta, rPos);
|
|
Matrix4ToQuat(transMat, rQuat);
|
|
|
|
#ifdef DEBUG
|
|
cerr << "(" << rPos[0] << ", " << rPos[1] << ", " << rPos[2] << ", "
|
|
<< rPosTheta[0] << ", " << rPosTheta[1] << ", " << rPosTheta[2] << ")" << endl;
|
|
|
|
cerr << transMat << endl;
|
|
#endif
|
|
|
|
// apply alignxf to dalignxf
|
|
MMult(alignxf, dalignxf, tempxf);
|
|
memcpy(dalignxf, tempxf, sizeof(transMat));
|
|
}
|
|
|
|
/**
|
|
* Transforms the scan by a given transformation and writes a new frame. The idea
|
|
* is to write for every transformation in all files, such that the show program
|
|
* is able to determine, whcih scans have to be drawn in which color. Hidden scans
|
|
* (or later processed scans) are written with INVALID.
|
|
*
|
|
* @param alignxf Transformation matrix
|
|
* @param colour Specifies which colour should the written to the frames file
|
|
* @param islum Is the transformtion part of LUM, i.e., all scans are transformed?
|
|
* In this case only LUM transformation is stored, otherwise all scans are processed
|
|
* -1 no transformation is stored
|
|
* 0 ICP transformation
|
|
* 1 LUM transformation, all scans except last scan
|
|
* 2 LUM transformation, last scan only
|
|
*/
|
|
void Scan::transform(const double alignxf[16], const AlgoType type, int islum)
|
|
{
|
|
MetaScan* meta = dynamic_cast<MetaScan*>(this);
|
|
|
|
if(meta) {
|
|
for(unsigned int i = 0; i < meta->size(); ++i) {
|
|
meta->getScan(i)->transform(alignxf, type, -1);
|
|
}
|
|
}
|
|
|
|
#ifdef TRANSFORM_ALL_POINTS
|
|
transformAll(alignxf);
|
|
#endif //TRANSFORM_ALL_POINTS
|
|
|
|
#ifdef DEBUG
|
|
cerr << alignxf << endl;
|
|
cerr << "(" << rPos[0] << ", " << rPos[1] << ", " << rPos[2] << ", "
|
|
<< rPosTheta[0] << ", " << rPosTheta[1] << ", " << rPosTheta[2] << ") ---> ";
|
|
#endif
|
|
|
|
// transform points
|
|
transformReduced(alignxf);
|
|
|
|
// update matrices
|
|
transformMatrix(alignxf);
|
|
|
|
// store transformation in frames
|
|
if(type != INVALID) {
|
|
#ifdef WITH_METRICS
|
|
Timer t = ClientMetric::add_frames_time.start();
|
|
#endif //WITH_METRICS
|
|
bool in_meta;
|
|
MetaScan* meta = dynamic_cast<MetaScan*>(this);
|
|
int found = 0;
|
|
unsigned int scans_size = allScans.size();
|
|
|
|
switch (islum) {
|
|
case -1:
|
|
// write no tranformation
|
|
break;
|
|
case 0:
|
|
for(unsigned int i = 0; i < scans_size; ++i) {
|
|
Scan* scan = allScans[i];
|
|
in_meta = false;
|
|
if(meta) {
|
|
for(unsigned int j = 0; j < meta->size(); ++j) {
|
|
if(meta->getScan(j) == scan) {
|
|
found = i;
|
|
in_meta = true;
|
|
}
|
|
}
|
|
}
|
|
|
|
if(scan == this || in_meta) {
|
|
found = i;
|
|
scan->addFrame(type);
|
|
} else {
|
|
if(found == 0) {
|
|
scan->addFrame(ICPINACTIVE);
|
|
} else {
|
|
scan->addFrame(INVALID);
|
|
}
|
|
}
|
|
}
|
|
break;
|
|
case 1:
|
|
addFrame(type);
|
|
break;
|
|
case 2:
|
|
for(unsigned int i = 0; i < scans_size; ++i) {
|
|
Scan* scan = allScans[i];
|
|
if(scan == this) {
|
|
found = i;
|
|
addFrame(type);
|
|
allScans[0]->addFrame(type);
|
|
continue;
|
|
}
|
|
if (found != 0) {
|
|
scan->addFrame(INVALID);
|
|
}
|
|
}
|
|
break;
|
|
default:
|
|
cerr << "invalid point transformation mode" << endl;
|
|
}
|
|
|
|
#ifdef WITH_METRICS
|
|
ClientMetric::add_frames_time.end(t);
|
|
#endif //WITH_METRICS
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Transforms the scan by a given transformation and writes a new frame. The idea
|
|
* is to write for every transformation in all files, such that the show program
|
|
* is able to determine, whcih scans have to be drawn in which color. Hidden scans
|
|
* (or later processed scans) are written with INVALID.
|
|
*
|
|
* @param alignQuat Quaternion for the rotation
|
|
* @param alignt Translation vector
|
|
* @param colour Specifies which colour should the written to the frames file
|
|
* @param islum Is the transformtion part of LUM, i.e., all scans are transformed?
|
|
* In this case only LUM transformation is stored, otherwise all scans are processed
|
|
* -1 no transformation is stored
|
|
* 0 ICP transformation
|
|
* 1 LUM transformation, all scans except last scan
|
|
* 2 LUM transformation, last scan only
|
|
*/
|
|
void Scan::transform(const double alignQuat[4], const double alignt[3],
|
|
const AlgoType type, int islum)
|
|
{
|
|
double alignxf[16];
|
|
QuatToMatrix4(alignQuat, alignt, alignxf);
|
|
transform(alignxf, type, islum);
|
|
}
|
|
|
|
/**
|
|
* Transforms the scan, so that the given Matrix
|
|
* prepresent the next pose.
|
|
*
|
|
* @param alignxf Transformation matrix to which this scan will be set to
|
|
* @param islum Is the transformation part of LUM?
|
|
*/
|
|
void Scan::transformToMatrix(double alignxf[16], const AlgoType type, int islum)
|
|
{
|
|
double tinv[16];
|
|
M4inv(transMat, tinv);
|
|
transform(tinv, INVALID);
|
|
transform(alignxf, type, islum);
|
|
}
|
|
|
|
/**
|
|
* Transforms the scan, so that the given Euler angles
|
|
* prepresent the next pose.
|
|
*
|
|
* @param rP Translation to which this scan will be set to
|
|
* @param rPT Orientation as Euler angle to which this scan will be set
|
|
* @param islum Is the transformation part of LUM?
|
|
*/
|
|
void Scan::transformToEuler(double rP[3], double rPT[3], const AlgoType type, int islum)
|
|
{
|
|
#ifdef WITH_METRICS
|
|
// called in openmp context in lum6Deuler.cc:422
|
|
ClientMetric::transform_time.set_threadsafety(true);
|
|
ClientMetric::add_frames_time.set_threadsafety(true);
|
|
#endif //WITH_METRICS
|
|
|
|
double tinv[16];
|
|
double alignxf[16];
|
|
M4inv(transMat, tinv);
|
|
transform(tinv, INVALID);
|
|
EulerToMatrix4(rP, rPT, alignxf);
|
|
transform(alignxf, type, islum);
|
|
|
|
#ifdef WITH_METRICS
|
|
ClientMetric::transform_time.set_threadsafety(false);
|
|
ClientMetric::add_frames_time.set_threadsafety(false);
|
|
#endif //WITH_METRICS
|
|
}
|
|
|
|
/**
|
|
* Transforms the scan, so that the given Euler angles
|
|
* prepresent the next pose.
|
|
*
|
|
* @param rP Translation to which this scan will be set to
|
|
* @param rPQ Orientation as Quaternion to which this scan will be set
|
|
* @param islum Is the transformation part of LUM?
|
|
*/
|
|
void Scan::transformToQuat(double rP[3], double rPQ[4], const AlgoType type, int islum)
|
|
{
|
|
double tinv[16];
|
|
double alignxf[16];
|
|
M4inv(transMat, tinv);
|
|
transform(tinv, INVALID);
|
|
QuatToMatrix4(rPQ, rP, alignxf);
|
|
transform(alignxf, type, islum);
|
|
}
|
|
|
|
/**
|
|
* Calculates Source\Target
|
|
* Calculates a set of corresponding point pairs and returns them. It
|
|
* computes the k-d trees and deletes them after the pairs have been
|
|
* found. This slow function should be used only for testing
|
|
*
|
|
* @param pairs The resulting point pairs (vector will be filled)
|
|
* @param Target The scan to whiche the points are matched
|
|
* @param thread_num number of the thread (for parallelization)
|
|
* @param rnd randomized point selection
|
|
* @param max_dist_match2 maximal allowed distance for matching
|
|
*/
|
|
|
|
void Scan::getNoPairsSimple(vector <double*> &diff,
|
|
Scan* Source, Scan* Target,
|
|
int thread_num,
|
|
double max_dist_match2)
|
|
{
|
|
DataXYZ xyz_reduced(Source->get("xyz reduced"));
|
|
KDtree* kd = new KDtree(PointerArray<double>(Target->get("xyz reduced")).get(), Target->size<DataXYZ>("xyz reduced"));
|
|
|
|
cout << "Max: " << max_dist_match2 << endl;
|
|
for (unsigned int i = 0; i < xyz_reduced.size(); i++) {
|
|
|
|
double p[3];
|
|
p[0] = xyz_reduced[i][0];
|
|
p[1] = xyz_reduced[i][1];
|
|
p[2] = xyz_reduced[i][2];
|
|
|
|
|
|
double *closest = kd->FindClosest(p, max_dist_match2, thread_num);
|
|
if (!closest) {
|
|
diff.push_back(xyz_reduced[i]);
|
|
//diff.push_back(closest);
|
|
}
|
|
}
|
|
|
|
delete kd;
|
|
}
|
|
|
|
/**
|
|
* Calculates a set of corresponding point pairs and returns them. It
|
|
* computes the k-d trees and deletes them after the pairs have been
|
|
* found. This slow function should be used only for testing
|
|
*
|
|
* @param pairs The resulting point pairs (vector will be filled)
|
|
* @param Source The scan whose points are matched to Targets' points
|
|
* @param Target The scan to whiche the points are matched
|
|
* @param thread_num number of the thread (for parallelization)
|
|
* @param rnd randomized point selection
|
|
* @param max_dist_match2 maximal allowed distance for matching
|
|
*/
|
|
void Scan::getPtPairsSimple(vector <PtPair> *pairs,
|
|
Scan* Source, Scan* Target,
|
|
int thread_num,
|
|
int rnd, double max_dist_match2,
|
|
double *centroid_m, double *centroid_d)
|
|
{
|
|
KDtree* kd = new KDtree(PointerArray<double>(Source->get("xyz reduced")).get(), Source->size<DataXYZ>("xyz reduced"));
|
|
DataXYZ xyz_reduced(Target->get("xyz reduced"));
|
|
|
|
for (unsigned int i = 0; i < xyz_reduced.size(); i++) {
|
|
if (rnd > 1 && rand(rnd) != 0) continue; // take about 1/rnd-th of the numbers only
|
|
|
|
double p[3];
|
|
p[0] = xyz_reduced[i][0];
|
|
p[1] = xyz_reduced[i][1];
|
|
p[2] = xyz_reduced[i][2];
|
|
|
|
double *closest = kd->FindClosest(p, max_dist_match2, thread_num);
|
|
if (closest) {
|
|
centroid_m[0] += closest[0];
|
|
centroid_m[1] += closest[1];
|
|
centroid_m[2] += closest[2];
|
|
centroid_d[0] += p[0];
|
|
centroid_d[1] += p[1];
|
|
centroid_d[2] += p[2];
|
|
PtPair myPair(closest, p);
|
|
pairs->push_back(myPair);
|
|
}
|
|
}
|
|
centroid_m[0] /= pairs[thread_num].size();
|
|
centroid_m[1] /= pairs[thread_num].size();
|
|
centroid_m[2] /= pairs[thread_num].size();
|
|
centroid_d[0] /= pairs[thread_num].size();
|
|
centroid_d[1] /= pairs[thread_num].size();
|
|
centroid_d[2] /= pairs[thread_num].size();
|
|
|
|
delete kd;
|
|
}
|
|
|
|
|
|
/**
|
|
* Calculates a set of corresponding point pairs and returns them.
|
|
* The function uses the k-d trees stored the the scan class, thus
|
|
* the function createTrees and deletTrees have to be called before
|
|
* resp. afterwards.
|
|
* Here we implement the so called "fast corresponding points"; k-d
|
|
* trees are not recomputed, instead the apply the inverse transformation
|
|
* to to the given point set.
|
|
*
|
|
* @param pairs The resulting point pairs (vector will be filled)
|
|
* @param Source The scan whose points are matched to Targets' points
|
|
* @param Target The scan to whiche the points are matched
|
|
* @param thread_num number of the thread (for parallelization)
|
|
* @param rnd randomized point selection
|
|
* @param max_dist_match2 maximal allowed distance for matching
|
|
* @return a set of corresponding point pairs
|
|
*/
|
|
void Scan::getPtPairs(vector <PtPair> *pairs,
|
|
Scan* Source, Scan* Target,
|
|
int thread_num,
|
|
int rnd, double max_dist_match2, double &sum,
|
|
double *centroid_m, double *centroid_d)
|
|
{
|
|
// initialize centroids
|
|
for(unsigned int i = 0; i < 3; ++i) {
|
|
centroid_m[i] = 0;
|
|
centroid_d[i] = 0;
|
|
}
|
|
|
|
// get point pairs
|
|
DataXYZ xyz_reduced(Target->get("xyz reduced"));
|
|
Source->getSearchTree()->getPtPairs(pairs, Source->dalignxf,
|
|
xyz_reduced, 0, xyz_reduced.size(),
|
|
thread_num,
|
|
rnd, max_dist_match2, sum, centroid_m, centroid_d);
|
|
|
|
// normalize centroids
|
|
unsigned int size = pairs->size();
|
|
if(size != 0) {
|
|
for(unsigned int i = 0; i < 3; ++i) {
|
|
centroid_m[i] /= size;
|
|
centroid_d[i] /= size;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
/**
|
|
* Calculates a set of corresponding point pairs and returns them.
|
|
* The function uses the k-d trees stored the the scan class, thus
|
|
* the function createTrees and delteTrees have to be called before
|
|
* resp. afterwards.
|
|
*
|
|
* @param pairs The resulting point pairs (vector will be filled)
|
|
* @param Source The scan whose points are matched to Targets' points
|
|
* @param Target The scan to whiche the points are matched
|
|
* @param thread_num The number of the thread that is computing ptPairs in parallel
|
|
* @param step The number of steps for parallelization
|
|
* @param rnd randomized point selection
|
|
* @param max_dist_match2 maximal allowed distance for matching
|
|
* @param sum The sum of distances of the points
|
|
*
|
|
* These intermediate values are for the parallel ICP algorithm
|
|
* introduced in the paper
|
|
* "The Parallel Iterative Closest Point Algorithm"
|
|
* by Langis / Greenspan / Godin, IEEE 3DIM 2001
|
|
*
|
|
*/
|
|
void Scan::getPtPairsParallel(vector <PtPair> *pairs, Scan* Source, Scan* Target,
|
|
int thread_num, int step,
|
|
int rnd, double max_dist_match2,
|
|
double *sum,
|
|
double centroid_m[OPENMP_NUM_THREADS][3], double centroid_d[OPENMP_NUM_THREADS][3])
|
|
{
|
|
// initialize centroids
|
|
for(unsigned int i = 0; i < 3; ++i) {
|
|
centroid_m[thread_num][i] = 0;
|
|
centroid_d[thread_num][i] = 0;
|
|
}
|
|
|
|
// get point pairs
|
|
SearchTree* search = Source->getSearchTree();
|
|
// differentiate between a meta scan (which has no reduced points) and a normal scan
|
|
// if Source is also a meta scan it already has a special meta-kd-tree
|
|
MetaScan* meta = dynamic_cast<MetaScan*>(Target);
|
|
if(meta) {
|
|
for(unsigned int i = 0; i < meta->size(); ++i) {
|
|
// determine step for each scan individually
|
|
DataXYZ xyz_reduced(meta->getScan(i)->get("xyz reduced"));
|
|
unsigned int max = xyz_reduced.size();
|
|
unsigned int step = max / OPENMP_NUM_THREADS;
|
|
// call ptpairs for each scan and accumulate ptpairs, centroids and sum
|
|
search->getPtPairs(&pairs[thread_num], Source->dalignxf,
|
|
xyz_reduced, step * thread_num, step * thread_num + step,
|
|
thread_num,
|
|
rnd, max_dist_match2, sum[thread_num],
|
|
centroid_m[thread_num], centroid_d[thread_num]);
|
|
}
|
|
} else {
|
|
DataXYZ xyz_reduced(Target->get("xyz reduced"));
|
|
search->getPtPairs(&pairs[thread_num], Source->dalignxf,
|
|
xyz_reduced, thread_num * step, thread_num * step + step,
|
|
thread_num,
|
|
rnd, max_dist_match2, sum[thread_num],
|
|
centroid_m[thread_num], centroid_d[thread_num]);
|
|
}
|
|
|
|
// normalize centroids
|
|
unsigned int size = pairs[thread_num].size();
|
|
if(size != 0) {
|
|
for(unsigned int i = 0; i < 3; ++i) {
|
|
centroid_m[thread_num][i] /= size;
|
|
centroid_d[thread_num][i] /= size;
|
|
}
|
|
}
|
|
}
|
|
|
|
unsigned int Scan::getMaxCountReduced(ScanVector& scans)
|
|
{
|
|
unsigned int max = 0;
|
|
for(std::vector<Scan*>::iterator it = scans.begin(); it != scans.end(); ++it) {
|
|
unsigned int count = (*it)->size<DataXYZ>("xyz reduced");
|
|
if(count > max)
|
|
max = count;
|
|
}
|
|
return max;
|
|
}
|