josch/3dpcp
1
0
Fork 0
Code Issues Pull requests Projects Releases Wiki Activity
3dpcp/.svn/pristine/f4/f48c644c7b43095857378c88f8a3ce6b029e13f7.svn-base

863 lines
25 KiB
Text
Raw Normal View History

update svn to r758
2012-11-07 10:27:39 +00:00
/*
* 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;
}
Reference in a new issue Copy permalink