893 lines
27 KiB
Text
893 lines
27 KiB
Text
/*
|
|
* 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"
|
|
|
|
#include "normals/normals.h"
|
|
|
|
#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::calcNormalsOnDemand()
|
|
{
|
|
// 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_normals);
|
|
if(m_has_normals) return;
|
|
calcNormalsOnDemandPrivate();
|
|
m_has_normals = true;
|
|
}
|
|
|
|
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 normals for all points
|
|
*/
|
|
void Scan::calcNormals()
|
|
{
|
|
cout << "calcNormals" << endl;
|
|
DataXYZ xyz(get("xyz"));
|
|
DataNormal xyz_normals(create("normal", sizeof(double)*3*xyz.size()));
|
|
if(xyz.size() == 0)
|
|
throw runtime_error("Could not calculate reduced points, XYZ data is empty");
|
|
|
|
vector<Point> points;
|
|
points.reserve(xyz.size());
|
|
vector<Point> normals;
|
|
normals.reserve(xyz.size());
|
|
for(unsigned int j = 0; j < xyz.size(); j++) {
|
|
points.push_back(Point(xyz[j][0], xyz[j][1], xyz[j][2]));
|
|
}
|
|
const int K_NEIGHBOURS = 10;
|
|
calculateNormalsAKNN(normals, points, K_NEIGHBOURS, get_rPos());
|
|
for (unsigned int i = 0; i < normals.size(); ++i) {
|
|
xyz_normals[i][0] = normals[i].x;
|
|
xyz_normals[i][1] = normals[i].y;
|
|
xyz_normals[i][2] = normals[i].z;
|
|
}
|
|
}
|
|
|
|
/**
|
|
* 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"));
|
|
DataXYZ xyz_normals(get(""));
|
|
if (reduction_pointtype.hasNormal()) {
|
|
DataXYZ my_xyz_normals(get("normal"));
|
|
xyz_normals = my_xyz_normals;
|
|
}
|
|
DataReflectance reflectance(get(""));
|
|
if (reduction_pointtype.hasReflectance()) {
|
|
DataReflectance my_reflectance(get("reflectance"));
|
|
reflectance = my_reflectance;
|
|
}
|
|
|
|
#ifdef WITH_METRICS
|
|
ClientMetric::scan_load_time.end(t);
|
|
Timer tl = ClientMetric::calc_reduced_points_time.start();
|
|
#endif //WITH_METRICS
|
|
|
|
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];
|
|
}
|
|
}
|
|
if (reduction_pointtype.hasReflectance()) {
|
|
DataReflectance reflectance_reduced(create("reflectance reduced", sizeof(float)*reflectance.size()));
|
|
for(unsigned int i = 0; i < xyz.size(); ++i) {
|
|
reflectance_reduced[i] = reflectance[i];
|
|
}
|
|
}
|
|
if (reduction_pointtype.hasNormal()) {
|
|
DataNormal normal_reduced(create("normal reduced", sizeof(double)*3*xyz.size()));
|
|
for(unsigned int i = 0; i < xyz.size(); ++i) {
|
|
for(unsigned int j = 0; j < 3; ++j) {
|
|
normal_reduced[i][j] = xyz_normals[i][j];
|
|
}
|
|
}
|
|
}
|
|
|
|
} else {
|
|
|
|
double **xyz_in = new double*[xyz.size()];
|
|
for (unsigned int i = 0; i < xyz.size(); ++i) {
|
|
xyz_in[i] = new double[reduction_pointtype.getPointDim()];
|
|
unsigned int j = 0;
|
|
for (; j < 3; ++j)
|
|
xyz_in[i][j] = xyz[i][j];
|
|
if (reduction_pointtype.hasReflectance())
|
|
xyz_in[i][j++] = reflectance[i];
|
|
if (reduction_pointtype.hasNormal())
|
|
for (unsigned int l = 0; l < 3; ++l)
|
|
xyz_in[i][j] = xyz_normals[i][l];
|
|
}
|
|
|
|
// start reduction
|
|
// build octree-tree from CurrentScan
|
|
// put full data into the octtree
|
|
BOctTree<double> *oct = new BOctTree<double>(xyz_in,
|
|
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));
|
|
DataReflectance reflectance_reduced(get(""));
|
|
DataNormal normal_reduced(get(""));
|
|
if (reduction_pointtype.hasReflectance()) {
|
|
DataReflectance my_reflectance_reduced(create("reflectance reduced",
|
|
sizeof(float)*size));
|
|
reflectance_reduced = my_reflectance_reduced;
|
|
}
|
|
if (reduction_pointtype.hasNormal()) {
|
|
DataNormal my_normal_reduced(create("normal reduced", sizeof(double)*3*size));
|
|
normal_reduced = my_normal_reduced;
|
|
}
|
|
for(unsigned int i = 0; i < size; ++i) {
|
|
unsigned int j = 0;
|
|
for (; j < 3; ++j)
|
|
xyz_reduced[i][j] = center[i][j];
|
|
if (reduction_pointtype.hasReflectance())
|
|
reflectance_reduced[i] = center[i][j++];
|
|
if (reduction_pointtype.hasNormal())
|
|
for (unsigned int l = 0; l < 3; ++l)
|
|
normal_reduced[i][l] = center[i][j++];
|
|
}
|
|
}
|
|
|
|
#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]);
|
|
}
|
|
|
|
DataNormal normal_reduced(get("normal reduced"));
|
|
for (unsigned int i = 0; i < normal_reduced.size(); ++i) {
|
|
transform3normal(alignxf, normal_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, PairingMode pairing_mode)
|
|
{
|
|
// 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"));
|
|
DataNormal normal_reduced(Target->get("normal reduced"));
|
|
Source->getSearchTree()->getPtPairs(pairs, Source->dalignxf,
|
|
xyz_reduced, normal_reduced, 0, xyz_reduced.size(),
|
|
thread_num,
|
|
rnd, max_dist_match2, sum, centroid_m, centroid_d,
|
|
pairing_mode);
|
|
|
|
// 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],
|
|
PairingMode pairing_mode)
|
|
{
|
|
// 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"));
|
|
DataNormal normal_reduced(Target->get("normal 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, normal_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], pairing_mode);
|
|
}
|
|
} else {
|
|
DataXYZ xyz_reduced(Target->get("xyz reduced"));
|
|
DataNormal normal_reduced(Target->get("normal reduced"));
|
|
search->getPtPairs(&pairs[thread_num], Source->dalignxf,
|
|
xyz_reduced, normal_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], pairing_mode);
|
|
}
|
|
|
|
// 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;
|
|
}
|