update to svn r762

This commit is contained in:
josch 2012-11-13 09:12:22 +01:00
parent 8b4640b056
commit c559c1eec9
79 changed files with 13274 additions and 2270 deletions

View file

@ -0,0 +1,33 @@
SET(SHOW_LIBS glui scan ANN ${OPENGL_LIBRARIES} normals newmat)
IF(WIN32)
IF( CMAKE_SIZEOF_VOID_P EQUAL 8 )
SET(SHOW_LIBS ${SHOW_LIBS} ${CMAKE_SOURCE_DIR}/3rdparty/windows/x64/freeglut.lib XGetopt)
ELSE( CMAKE_SIZEOF_VOID_P EQUAL 8 )
SET(SHOW_LIBS ${SHOW_LIBS} ${CMAKE_SOURCE_DIR}/3rdparty/windows/freeglut.lib XGetopt)
ENDIF(CMAKE_SIZEOF_VOID_P EQUAL 8 )
ENDIF(WIN32)
IF (UNIX)
SET(SHOW_LIBS newmat dl ${SHOW_LIBS} ${GLUT_LIBRARIES})
ENDIF(UNIX)
IF(WITH_GLEE)
SET(SHOW_LIBS ${SHOW_LIBS} glee)
ENDIF(WITH_GLEE)
SET(SHOW_SRCS NurbsPath.cc PathGraph.cc vertexarray.cc viewcull.cc colormanager.cc compacttree.cc scancolormanager.cc display.cc)
IF (WITH_SHOW)
add_executable(show show.cc ${SHOW_SRCS})
target_link_libraries(show ${SHOW_LIBS})
ENDIF(WITH_SHOW)
IF(WITH_WXSHOW)
add_executable(wxshow wxshow.cc selectionframe.cc ${SHOW_SRCS})
target_link_libraries(wxshow ${wxWidgets_LIBRARIES} wxthings ${SHOW_LIBS})
ENDIF(WITH_WXSHOW)
### EXPORT SHARED LIBS
IF(EXPORT_SHARED_LIBS)
add_library(show_s SHARED ${SHOW_SRCS})
target_link_libraries(show_s newmat_s)
ENDIF(EXPORT_SHARED_LIBS)

View file

@ -0,0 +1,136 @@
/** @file
* @brief Representation of 3D scan matching with ICP
* @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany.
*/
#ifndef __ICP6D_H__
#define __ICP6D_H__
#include <vector>
using std::vector;
#include "newmat/newmat.h"
//using namespace NEWMAT;
#include "slam6d/scan.h"
#include "slam6d/icp6Dminimizer.h"
#include "slam6d/pairingMode.h"
/**
* @brief Representation of 3D scan matching with ICP.
*
* Manages the matching of 3D scans.
* Important values, such as maximal matching distance,
* maximal number of iterations, etc.
* are specified in the constructor.
*/
class icp6D {
public:
icp6D(icp6Dminimizer *my_icp6Dminimizer,
double max_dist_match = 25.0,
int max_num_iterations = 50,
bool quiet = false,
bool meta = false,
int rnd = 1,
bool eP = true,
int anim = -1,
double epsilonICP = 0.0000001,
int nns_method = simpleKD,
bool cuda_enabled = false,
bool cad_matching = false);
/**
* Destructor (empty, but needed, because virtual)
*/
virtual ~icp6D() {};
void doICP(vector <Scan *> allScans, PairingMode pairing_mode = CLOSEST_POINT);
virtual int match(Scan* PreviousScan, Scan* CurrentScan, PairingMode pairing_mode = CLOSEST_POINT);
void covarianceEuler(Scan *scan1, Scan *scan2, NEWMAT::Matrix *C);
void covarianceQuat(Scan *scan1, Scan *scan2, NEWMAT::Matrix *C);
double Point_Point_Error(Scan* PreviousScan, Scan* CurrentScan, double max_dist_match, unsigned int *nrp=0);
inline int get_rnd();
inline bool get_meta();
inline int get_anim();
inline int get_nns_method();
inline void set_anim(int anim);
inline double get_max_dist_match2();
inline void set_max_dist_match2(double max_dist_match2);
inline void set_max_num_iterations(int max_num_iterations);
inline void set_cad_matching (bool cad_matching);
inline bool get_cad_matching (void);
protected:
/**
* suppress output to cout
*/
bool quiet;
/**
* take every rnd point for matching
*/
int rnd;
/**
* extrapolate odometry
*/
bool eP;
/**
* match against all scans (= meta scan), or against the last scan only
*/
bool meta;
/**
* specifies which NNS method should be used
*/
int nns_method;
/**
* specifies if the ANN trees have to be built
*/
bool cuda_enabled;
/**
* the maximal distance (^2 !!!) for matching
*/
double max_dist_match2;
/**
* the maximal number of iterations
*/
int max_num_iterations;
/**
* write anim'th animation frame
*/
int anim;
/**
* epsilon for stopping ICP algorithm ( convergence criterium )
*/
double epsilonICP;
/**
* ptr to ICP error function minimizer functor
*/
icp6Dminimizer *my_icp6Dminimizer;
/**
* Maximum number of points in all scans
*/
unsigned int max_scn_size; //FIXME -> update with metascan
/**
* determines if CAD models are matched against one scan
*/
bool cad_matching;
};
#include "icp6D.icc"
#endif

View file

@ -0,0 +1,176 @@
/*
* show_animate implementation
*
* Copyright (C) Jan Elseberg, Dorit Borrmann
*
* Released under the GPL version 3.
*
*/
#ifdef _MSC_VER
#define _USE_MATH_DEFINES
#include <windows.h>
#endif
#ifdef __APPLE__
#include <GLUT/glut.h>
#else
#include <GL/glut.h>
#endif
#include "slam6d/point.h"
#include "slam6d/scan.h"
#include "show/colordisplay.h"
#include "show/colormanager.h"
#include "show/scancolormanager.h"
#include <vector>
#include <float.h>
#include "slam6d/point_type.h"
using std::vector;
ScanColorManager::ScanColorManager(unsigned int _buckets, PointType type, bool animation_color) : pointtype(type), animationColor(animation_color) {
valid = false;
inverted = false;
buckets = _buckets;
// types = _types;
pointtype = type;
//pointdim = PointType::PointDim(types);
//
mins = new float[pointtype.getPointDim()];
maxs = new float[pointtype.getPointDim()];
for (unsigned int i = 0; i < pointtype.getPointDim(); i++) {
mins[i] = FLT_MAX;
maxs[i] = FLT_MIN;
}
currenttype = PointType::USE_HEIGHT;
currentdim = 0;
}
void ScanColorManager::registerTree(colordisplay *b) { allScans.push_back(b); }
void ScanColorManager::setColorMap(ColorMap &cm) {
makeValid();
for (unsigned int i = 0; i < allManager.size(); i++) {
allManager[i]->setColorMap(cm);
}
}
void ScanColorManager::setColorMap(ColorMap::CM &cm) {
ColorMap cmap = ColorMap::getColorMap(cm);
setColorMap(cmap);
}
void ScanColorManager::setCurrentType(unsigned int type) {
makeValid();
if (type == PointType::USE_NONE) {
for (unsigned int i = 0; i < allScans.size(); i++) {
allScans[i]->setColorManager(0);
}
}
currentdim = pointtype.getType(type);
if(type != PointType::USE_NONE) {
for (unsigned int i = 0; i < allManager.size(); i++) {
allManager[i]->setCurrentDim(currentdim);
}
currenttype = type;
}
}
void ScanColorManager::setMinMax(float min, float max) {
makeValid();
for (unsigned int i = 0; i < allManager.size(); i++) {
allManager[i]->setMinMax(min, max);
}
}
void ScanColorManager::setMode(const unsigned int &mode) {
if (mode == ScanColorManager::MODE_STATIC) {
for (unsigned int i = 0; i < allScans.size(); i++) {
allScans[i]->setColorManager(staticManager[i]);
}
} else if (mode == ScanColorManager::MODE_COLOR_SCAN) {
for (unsigned int i = 0; i < allScans.size(); i++) {
allScans[i]->setColorManager(scanManager[i]);
}
} else if (mode == ScanColorManager::MODE_ANIMATION) {
if (animationColor) {
for (unsigned int i = 0; i < allScans.size(); i++) {
allScans[i]->setColorManager(0);
}
}
} else if (mode == ScanColorManager::MODE_POINT_COLOR) {
for (unsigned int i = 0; i < allScans.size(); i++) {
allScans[i]->setColorManager(colorsManager[i]);
}
}
}
float ScanColorManager::getMin() { return mins[currentdim];};
float ScanColorManager::getMax() { return maxs[currentdim];};
float ScanColorManager::getMin(unsigned int dim) { return mins[dim];};
float ScanColorManager::getMax(unsigned int dim) { return maxs[dim];};
unsigned int ScanColorManager::getPointDim() { return pointtype.getPointDim(); };
void ScanColorManager::makeValid() {
if (!valid) {
for (unsigned int i = 0; i < allScans.size(); i++) {
colordisplay *scan = allScans[i];
ColorManager *cm = new ColorManager(buckets, pointtype.getPointDim(), mins, maxs);
cm->setCurrentDim(currentdim);
scan->setColorManager(cm);
staticManager.push_back(cm);
// new colormanager for scan index influenced colorscheme
DiffMap m;
// JetMap m;
float c[3] = {0,0,0};
m.calcColor(c, i, allScans.size() );
ColorManager *cmc = new ColorManager(buckets, pointtype.getPointDim(), mins, maxs, c);
scanManager.push_back(cmc);
// new colormanager for the color based on the color of the points
CColorManager *ccm = new CColorManager(buckets, pointtype.getPointDim(), mins, maxs, pointtype.getType(PointType::USE_COLOR));
colorsManager.push_back(ccm);
allManager.push_back(cm);
allManager.push_back(cmc);
allManager.push_back(ccm);
}
valid = true;
}
}
void ScanColorManager::selectColors(Scan::AlgoType type) {
switch(type) {
case Scan::ICP:
glColor4d(0.85, 0.30,0.023, 1.0);
//glColor4d(1.0, 0.30,0.30, 1.0);
//glColor4d(1.0, 0.00,0.00, 1.0);
break;
case Scan::ICPINACTIVE:
glColor4d(0.78, 0.63,0.57, 1.0);
//glColor4d(1.00, 1.00,1.00, 1.0);
break;
case Scan::LUM:
glColor4d(1.0, 0.0,0.0, 1.0);
break;
case Scan::ELCH:
glColor4d(0.0, 1.0,0.0, 1.0);
break;
default:
glColor4d(1.0, 1.0, 1.0, 1.0);
break;
}
}
const unsigned int ScanColorManager::MODE_STATIC = 0;
const unsigned int ScanColorManager::MODE_COLOR_SCAN = 1;
const unsigned int ScanColorManager::MODE_ANIMATION = 2;
const unsigned int ScanColorManager::MODE_POINT_COLOR = 3;

View file

@ -0,0 +1,893 @@
/*
* 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;
}

View file

@ -0,0 +1,996 @@
/*
* slam6D implementation
*
* Copyright (C) Andreas Nuechter, Kai Lingemann, Jochen Sprickerhof
*
* Released under the GPL version 3.
*
*/
/**
* @file
* @brief Main programm for matching 3D scans (6D SLAM)
*
* Main programm to match 3D scans with ICP and the globally
* consistent matching approach.
* Use -i from the command line to match with ICP,
* and -I to match 3D Scans using the global algorithm.
*
* @author Andreas Nuechter. Jacobs University Bremen gGmbH, Germany
* @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Jochen Sprickerhof. Institute of Computer Science, University of Osnabrueck, Germany.
*/
#include "slam6d/scan.h"
#include "slam6d/metaScan.h"
#include "slam6d/io_utils.h"
#include "slam6d/icp6Dapx.h"
#include "slam6d/icp6Dsvd.h"
#include "slam6d/icp6Dquat.h"
#include "slam6d/icp6Dortho.h"
#include "slam6d/icp6Dhelix.h"
#include "slam6d/icp6Ddual.h"
#include "slam6d/icp6Dlumeuler.h"
#include "slam6d/icp6Dlumquat.h"
#include "slam6d/icp6Dquatscale.h"
#include "slam6d/icp6D.h"
#ifdef WITH_CUDA
#include "slam6d/cuda/icp6Dcuda.h"
#endif
#include "slam6d/lum6Deuler.h"
#include "slam6d/lum6Dquat.h"
#include "slam6d/ghelix6DQ2.h"
#include "slam6d/elch6Deuler.h"
#include "slam6d/elch6Dquat.h"
#include "slam6d/elch6DunitQuat.h"
#include "slam6d/elch6Dslerp.h"
#include "slam6d/graphSlam6D.h"
#include "slam6d/gapx6D.h"
#include "slam6d/graph.h"
#include "slam6d/globals.icc"
#ifndef _MSC_VER
#include <getopt.h>
#else
#include "XGetopt.h"
#endif
#include <csignal>
#ifdef _MSC_VER
#define strcasecmp _stricmp
#define strncasecmp _strnicmp
#else
#include <strings.h>
#endif
#ifdef WITH_METRICS
#include "slam6d/metrics.h"
#endif //WITH_METRICS
#ifdef _MSC_VER
#if !defined _OPENMP && defined OPENMP
#define _OPENMP
#endif
#endif
#ifdef _OPENMP
#include <omp.h>
#endif
#define WANT_STREAM ///< define the WANT stream :)
#include <string>
using std::string;
#include <iostream>
using std::cout;
using std::cerr;
using std::endl;
#include <fstream>
using std::ifstream;
// Handling Segmentation faults and CTRL-C
void sigSEGVhandler (int v)
{
static bool segfault = false;
if(!segfault) {
segfault = true;
cout << endl
<< "# **************************** #" << endl
<< " Segmentation fault or Ctrl-C" << endl
<< "# **************************** #" << endl
<< endl;
// save frames and close scans
for(ScanVector::iterator it = Scan::allScans.begin(); it != Scan::allScans.end(); ++it) {
(*it)->saveFrames();
}
cout << "Frames saved." << endl;
Scan::closeDirectory();
}
exit(-1);
}
/**
* Explains the usage of this program's command line parameters
*/
void usage(char* prog)
{
#ifndef _MSC_VER
const string bold("\033[1m");
const string normal("\033[m");
#else
const string bold("");
const string normal("");
#endif
cout << endl
<< bold << "USAGE " << normal << endl
<< " " << prog << " [options] directory" << endl << endl;
cout << bold << "OPTIONS" << normal << endl
<< bold << " -a" << normal << " NR, " << bold << "--algo=" << normal << "NR [default: 1]" << endl
<< " selects the minimizazion method for the ICP matching algorithm" << endl
<< " 1 = unit quaternion based method by Horn" << endl
<< " 2 = singular value decomposition by Arun et al. " << endl
<< " 3 = orthonormal matrices by Horn et al." << endl
<< " 4 = dual quaternion method by Walker et al." << endl
<< " 5 = helix approximation by Hofer & Potmann" << endl
<< " 6 = small angle approximation" << endl
<< " 7 = Lu & Milios style, i.e., uncertainty based, with Euler angles" << endl
<< " 8 = Lu & Milios style, i.e., uncertainty based, with Quaternion" << endl
<< " 9 = unit quaternion with scale method by Horn" << endl
<< endl
<< bold << " -A" << normal << " NR, " << bold << "--anim=" << normal << "NR [default: first and last frame only]" << endl
<< " if specified, use only every NR-th frame for animation" << endl
<< endl
<< bold << " -c" << normal << " NR, " << bold << "--cldist=" << normal << "NR [default: 500]" << endl
<< " specifies the maximal distance for closed loops" << endl
<< endl
<< bold << " -C" << normal << " NR, " << bold << "--clpairs=" << normal << "NR [default: 6]" << endl
<< " specifies the minimal number of points for an overlap. If not specified" << endl
<< " cldist is used instead" << endl
<< endl
<< bold << " --cache" << normal << endl
<< " turns on cached k-d tree search" << endl
<< endl
<< bold << " -d" << normal << " NR, " << bold << "--dist=" << normal << "NR [default: 25]" << endl
<< " sets the maximal point-to-point distance for matching with ICP to <NR> 'units'" << endl
<< " (unit of scan data, e.g. cm)" << endl
<< endl
<< bold << " -D" << normal << " NR, " << bold << "--distSLAM="
<< normal << "NR [default: same value as -d option]" << endl
<< " sets the maximal point-to-point distance for matching with SLAM to <NR> 'units'" << endl
<< " (unit of scan data, e.g. cm)" << endl
<< endl
<< bold << " --DlastSLAM" << normal << " NR [default not set]" << endl
<< " sets the maximal point-to-point distance for the final SLAM correction," << endl
<< " if final SLAM is not required don't set it." << endl
<< endl
<< bold << " -e" << normal << " NR, " << bold << "--end=" << normal << "NR" << endl
<< " end after scan NR" << endl
<< endl
<< bold << " --exportAllPoints" << normal << endl
<< " writes all registered reduced points to the file points.pts before" << endl
<< " slam6D terminated" << endl
<< endl
<< bold << " --epsICP=" << normal << "NR [default: 0.00001]" << endl
<< " stop ICP iteration if difference is smaller than NR" << endl
<< endl
<< bold << " --epsSLAM=" << normal << " NR [default: 0.5]" << endl
<< " stop SLAM iteration if average difference is smaller than NR" << endl
<< endl
<< bold << " -f" << normal << " F, " << bold << "--format=" << normal << "F" << endl
<< " using shared library F for input" << endl
<< " (chose F from {uos, uos_map, uos_rgb, uos_frames, uos_map_frames, old, rts, rts_map, ifp, riegl_txt, riegl_rgb, riegl_bin, zahn, ply, wrl, xyz, zuf, iais, front, x3d, rxp, ais })" << endl
<< endl
<< bold << " -G" << normal << " NR, " << bold << "--graphSlam6DAlgo=" << normal << "NR [default: 0]" << endl
<< " selects the minimizazion method for the SLAM matching algorithm" << endl
<< " 0 = no global relaxation technique" << endl
<< " 1 = Lu & Milios extension using euler angles due to Borrmann et al." << endl
<< " 2 = Lu & Milios extension using using unit quaternions" << endl
<< " 3 = HELIX approximation by Hofer and Pottmann" << endl
<< " 4 = small angle approximation" << endl
<< bold << " -i" << normal << " NR, " << bold << "--iter=" << normal << "NR [default: 50]" << endl
<< " sets the maximal number of ICP iterations to <NR>" << endl
<< endl
<< bold << " -I" << normal << " NR, " << bold << "--iterSLAM=" << normal << "NR [default: 0]" << endl
<< " sets the maximal number of iterations for SLAM to <NR>" << endl
<< " (if not set, graphSLAM is not executed)" << endl
<< endl
<< bold << " -l" << normal << " NR, " << bold << "--loopsize=" << normal << "NR [default: 20]" << endl
<< " sets the size of a loop, i.e., a loop must exceed <NR> of scans" << endl
<< endl
<< bold << " -L" << normal << " NR, " << bold << "--loop6DAlgo=" << normal << "NR [default: 0]" << endl
<< " selects the method for closing the loop explicitly" << endl
<< " 0 = no loop closing technique" << endl
<< " 1 = euler angles" << endl
<< " 2 = quaternions " << endl
<< " 3 = unit quaternions" << endl
<< " 4 = SLERP (recommended)" << endl
<< endl
<< bold << " --metascan" << normal << endl
<< " Match current scan against a meta scan of all previous scans (default match against the last scan only)" << endl
<< endl
<< bold << " -m" << normal << " NR, " << bold << "--max=" << normal << "NR" << endl
<< " neglegt all data points with a distance larger than NR 'units'" << endl
<< endl
<< bold << " -M" << normal << " NR, " << bold << "--min=" << normal << "NR" << endl
<< " neglegt all data points with a distance smaller than NR 'units'" << endl
<< endl
<< bold << " -n" << normal << " FILE, " << bold << "--net=" << normal << "FILE" << endl
<< " specifies the file that includes the net structure for SLAM" << endl
<< endl
<< bold << " -O" << normal << "NR (optional), " << bold << "--octree=" << normal << "NR (optional)" << endl
<< " use randomized octree based point reduction (pts per voxel=<NR>)" << endl
<< " requires " << bold << "-r" << normal <<" or " << bold << "--reduce" << endl
<< endl
<< bold << " -p, --trustpose" << normal << endl
<< " Trust the pose file, do not extrapolate the last transformation." << endl
<< " (just for testing purposes, or gps input.)" << endl
<< endl
<< bold << " -q, --quiet" << normal << endl
<< " Quiet mode. Suppress (most) messages" << endl
<< endl
<< bold << " -Q, --veryquiet" << normal << endl
<< " Very quiet mode. Suppress all messages, except in case of error." << endl
<< endl
<< bold << " -S, --scanserver" << normal << endl
<< " Use the scanserver as an input method and handling of scan data" << endl
<< endl
<< bold << " -r" << normal << " NR, " << bold << "--reduce=" << normal << "NR" << endl
<< " turns on octree based point reduction (voxel size=<NR>)" << endl
<< endl
<< bold << " -R" << normal << " NR, " << bold << "--random=" << normal << "NR" << endl
<< " turns on randomized reduction, using about every <NR>-th point only" << endl
<< endl
<< bold << " -s" << normal << " NR, " << bold << "--start=" << normal << "NR" << endl
<< " start at scan NR (i.e., neglects the first NR scans)" << endl
<< " [ATTENTION: counting naturally starts with 0]" << endl
<< endl
<< bold << " -t" << normal << " NR, " << bold << "--nns_method=" << normal << "NR [default: 1]" << endl
<< " selects the Nearest Neighbor Search Algorithm" << endl
<< " 0 = simple k-d tree " << endl
<< " 1 = cached k-d tree " << endl
<< " 2 = ANNTree " << endl
<< " 3 = BOCTree " << endl
<< endl
<< bold << " -u" << normal <<", "<< bold<<"--cuda" << normal << endl
<< " this option activates icp running on GPU instead of CPU"<<endl
<< endl << endl;
cout << bold << "EXAMPLES " << normal << endl
<< " " << prog << " dat" << endl
<< " " << prog << " --max=500 -r 10.2 -i 20 dat" << endl
<< " " << prog << " -s 2 -e 10 dat" << endl << endl;
exit(1);
}
/** A function that parses the command-line arguments and sets the respective flags.
* @param argc the number of arguments
* @param argv the arguments
* @param dir the directory
* @param red using point reduction?
* @param rand use randomized point reduction?
* @param mdm maximal distance match
* @param mdml maximal distance match for SLAM
* @param mni maximal number of iterations
* @param start starting at scan number 'start'
* @param end stopping at scan number 'end'
* @param maxDist - maximal distance of points being loaded
* @param minDist - minimal distance of points being loaded
* @param quiet switches on/off the quiet mode
* @param veryQuiet switches on/off the 'very quiet' mode
* @param extrapolate_pose - i.e., extrapolating the odometry by the last transformation
* (vs. taking the pose file as <b>exact</b>)
* @param meta match against all scans (= meta scan), or against the last scan only???
* @param anim selects the rotation representation for the matching algorithm
* @param mni_lum sets the maximal number of iterations for SLAM
* @param net specifies the file that includes the net structure for SLAM
* @param cldist specifies the maximal distance for closed loops
* @param epsilonICP stop ICP iteration if difference is smaller than this value
* @param epsilonSLAM stop SLAM iteration if average difference is smaller than this value
* @param algo specfies the used algorithm for rotation computation
* @param lum6DAlgo specifies the used algorithm for global SLAM correction
* @param loopsize defines the minimal loop size
* @return 0, if the parsing was successful. 1 otherwise
*/
int parseArgs(int argc, char **argv, string &dir, double &red, int &rand,
double &mdm, double &mdml, double &mdmll,
int &mni, int &start, int &end, int &maxDist, int &minDist, bool &quiet, bool &veryQuiet,
bool &extrapolate_pose, bool &meta, int &algo, int &loopSlam6DAlgo, int &lum6DAlgo, int &anim,
int &mni_lum, string &net, double &cldist, int &clpairs, int &loopsize,
double &epsilonICP, double &epsilonSLAM, int &nns_method, bool &exportPts, double &distLoop,
int &iterLoop, double &graphDist, int &octree, bool &cuda_enabled, IOType &type,
bool& scanserver, PairingMode& pairing_mode)
{
int c;
// from unistd.h:
extern char *optarg;
extern int optind;
WriteOnce<IOType> w_type(type);
WriteOnce<int> w_start(start), w_end(end);
/* options descriptor */
// 0: no arguments, 1: required argument, 2: optional argument
static struct option longopts[] = {
{ "format", required_argument, 0, 'f' },
{ "algo", required_argument, 0, 'a' },
{ "nns_method", required_argument, 0, 't' },
{ "loop6DAlgo", required_argument, 0, 'L' },
{ "graphSlam6DAlgo", required_argument, 0, 'G' },
{ "net", required_argument, 0, 'n' },
{ "iter", required_argument, 0, 'i' },
{ "iterSLAM", required_argument, 0, 'I' },
{ "max", required_argument, 0, 'm' },
{ "loopsize", required_argument, 0, 'l' },
{ "cldist", required_argument, 0, 'c' },
{ "clpairs", required_argument, 0, 'C' },
{ "min", required_argument, 0, 'M' },
{ "dist", required_argument, 0, 'd' },
{ "distSLAM", required_argument, 0, 'D' },
{ "start", required_argument, 0, 's' },
{ "end", required_argument, 0, 'e' },
{ "reduce", required_argument, 0, 'r' },
{ "octree", optional_argument, 0, 'O' },
{ "random", required_argument, 0, 'R' },
{ "quiet", no_argument, 0, 'q' },
{ "veryquiet", no_argument, 0, 'Q' },
{ "trustpose", no_argument, 0, 'p' },
{ "anim", required_argument, 0, 'A' },
{ "metascan", no_argument, 0, '2' }, // use the long format only
{ "DlastSLAM", required_argument, 0, '4' }, // use the long format only
{ "epsICP", required_argument, 0, '5' }, // use the long format only
{ "epsSLAM", required_argument, 0, '6' }, // use the long format only
{ "normalshoot", no_argument, 0, '7' }, // use the long format only
{ "point-to-plane", no_argument, 0, 'z' }, // use the long format only
{ "exportAllPoints", no_argument, 0, '8' },
{ "distLoop", required_argument, 0, '9' }, // use the long format only
{ "iterLoop", required_argument, 0, '1' }, // use the long format only
{ "graphDist", required_argument, 0, '3' }, // use the long format only
{ "cuda", no_argument, 0, 'u' }, // cuda will be enabled
{ "scanserver", no_argument, 0, 'S' },
{ 0, 0, 0, 0} // needed, cf. getopt.h
};
cout << endl;
while ((c = getopt_long(argc, argv, "O:f:A:G:L:a:t:r:R:d:D:i:l:I:c:C:n:s:e:m:M:uqQpS", longopts, NULL)) != -1) {
switch (c) {
case 'a':
algo = atoi(optarg);
if ((algo < 0) || (algo > 9)) {
cerr << "Error: ICP Algorithm not available." << endl;
exit(1);
}
break;
case 't':
nns_method = atoi(optarg);
if ((nns_method < 0) || (nns_method > 3)) {
cerr << "Error: NNS Method not available." << endl;
exit(1);
}
break;
case 'L':
loopSlam6DAlgo = atoi(optarg);
if (loopSlam6DAlgo < 0 || loopSlam6DAlgo > 6) {
cerr << "Error: global loop closing algorithm not available." << endl;
exit(1);
}
break;
case 'G':
lum6DAlgo = atoi(optarg);
if ((lum6DAlgo < 0) || (lum6DAlgo > 6)) {
cerr << "Error: global relaxation algorithm not available." << endl;
exit(1);
}
break;
case 'c':
cldist = atof(optarg);
break;
case 'C':
clpairs = atoi(optarg);
break;
case 'l':
loopsize = atoi(optarg);
break;
case 'r':
red = atof(optarg);
break;
case 'O':
if (optarg) {
octree = atoi(optarg);
} else {
octree = 1;
}
break;
case 'R':
rand = atoi(optarg);
break;
case 'd':
mdm = atof(optarg);
break;
case 'D':
mdml = atof(optarg);
break;
case 'i':
mni = atoi(optarg);
break;
case 'I':
mni_lum = atoi(optarg);
break;
case 'n':
net = optarg;
break;
case 's':
w_start = atoi(optarg);
if (start < 0) { cerr << "Error: Cannot start at a negative scan number.\n"; exit(1); }
break;
case 'e':
w_end = atoi(optarg);
if (end < 0) { cerr << "Error: Cannot end at a negative scan number.\n"; exit(1); }
if (end < start) { cerr << "Error: <end> cannot be smaller than <start>.\n"; exit(1); }
break;
case 'm':
maxDist = atoi(optarg);
break;
case 'M':
minDist = atoi(optarg);
break;
case 'q':
quiet = true;
break;
case 'Q':
quiet = veryQuiet = true;
break;
case 'p':
extrapolate_pose = false;
break;
case 'A':
anim = atoi(optarg);
break;
case '2': // = --metascan
meta = true;
break;
case '4': // = --DlastSLAM
mdmll = atof(optarg);
break;
case '5': // = --epsICP
epsilonICP = atof(optarg);
break;
case '6': // = --epsSLAM
epsilonSLAM = atof(optarg);
break;
case '8': // not used
exportPts = true;
break;
case '9': // = --distLoop
distLoop = atof(optarg);
break;
case '1': // = --iterLoop
iterLoop = atoi(optarg);
break;
case '3': // = --graphDist
graphDist = atof(optarg);
break;
case '7': // = --normalshoot
pairing_mode = CLOSEST_POINT_ALONG_NORMAL;
break;
case 'z': // = --point-to-plane
pairing_mode = CLOSEST_PLANE;
break;
case 'f':
try {
w_type = formatname_to_io_type(optarg);
} catch (...) { // runtime_error
cerr << "Format " << optarg << " unknown." << endl;
abort();
}
break;
case 'u':
cuda_enabled = true;
break;
case 'S':
scanserver = true;
break;
case '?':
usage(argv[0]);
return 1;
default:
abort ();
}
}
if (optind != argc-1) {
cerr << "\n*** Directory missing ***" << endl;
usage(argv[0]);
}
dir = argv[optind];
#ifndef _MSC_VER
if (dir[dir.length()-1] != '/') dir = dir + "/";
#else
if (dir[dir.length()-1] != '\\') dir = dir + "\\";
#endif
parseFormatFile(dir, w_type, w_start, w_end);
return 0;
}
/**
* This function is does all the matching stuff
* it iterates over all scans using the algorithm objects to calculate new poses
* objects could be NULL if algorithm should not be used
*
* @param cldist maximal distance for closing loops
* @param loopsize minimal loop size
* @param allScans Contains all laser scans
* @param my_icp6D the ICP implementation
* @param meta_icp math ICP against a metascan
* @param nns_method Indicates the nearest neigbor search method to be used
* @param my_loopSlam6D used loopoptimizer
* @param my_graphSlam6D used global optimization
* @param nrIt The number of iterations the global SLAM-algorithm will run
* @param epsilonSLAM epsilon for global SLAM iteration
* @param mdml maximal distance match for global SLAM
* @param mdmll maximal distance match for global SLAM after all scans ar matched
*/
void matchGraph6Dautomatic(double cldist, int loopsize, vector <Scan *> allScans, icp6D *my_icp6D,
bool meta_icp, int nns_method, bool cuda_enabled,
loopSlam6D *my_loopSlam6D, graphSlam6D *my_graphSlam6D, int nrIt,
double epsilonSLAM, double mdml, double mdmll, double graphDist,
bool &eP, IOType type)
{
double cldist2 = sqr(cldist);
// list of scan for metascan
vector < Scan* > metas;
// graph for loop optimization
graph_t g;
int n = allScans.size();
int loop_detection = 0;
double dist, min_dist = -1;
int first = 0, last = 0;
for(int i = 1; i < n; i++) {
cout << i << "/" << n << endl;
add_edge(i-1, i, g);
if(eP) {
allScans[i]->mergeCoordinatesWithRoboterPosition(allScans[i-1]);
}
//Hack to get all icp transformations into the .frames Files
if(i == n-1 && my_icp6D != NULL && my_icp6D->get_anim() == -2) {
my_icp6D->set_anim(-1);
}
/*if(i == 85 || i == 321 || i == 533) {
my_icp6D->set_anim(1);
}*/
if(my_icp6D != NULL){
cout << "ICP" << endl;
// Matching strongly linked scans with ICPs
if(meta_icp) {
metas.push_back(allScans[i - 1]);
MetaScan* meta_scan = new MetaScan(metas);
my_icp6D->match(meta_scan, allScans[i]);
delete meta_scan;
} else {
switch(type) {
case UOS_MAP:
case UOS_MAP_FRAMES:
my_icp6D->match(allScans[0], allScans[i]);
break;
case RTS_MAP:
//untested (and could not work)
//if(i < 220-22 && i > 250-22) match(allScans[0], CurrentScan);
my_icp6D->match(allScans[0], allScans[i]);
break;
default:
my_icp6D->match(allScans[i - 1], allScans[i]);
break;
}
}
} else {
double id[16];
M4identity(id);
allScans[i]->transform(id, Scan::ICP, 0);
}
/*if(i == 85 || i == 321 || i == 533) {
my_icp6D->set_anim(-2);
}*/
if(loop_detection == 1) {
loop_detection = 2;
}
for(int j = 0; j < i - loopsize; j++) {
dist = Dist2(allScans[j]->get_rPos(), allScans[i]->get_rPos());
if(dist < cldist2) {
loop_detection = 1;
if(min_dist < 0 || dist < min_dist) {
min_dist = dist;
first = j;
last = i;
}
}
}
if(loop_detection == 2) {
loop_detection = 0;
min_dist = -1;
if(my_loopSlam6D != NULL) {
cout << "Loop close: " << first << " " << last << endl;
my_loopSlam6D->close_loop(allScans, first, last, g);
add_edge(first, last, g);
}
if(my_graphSlam6D != NULL && mdml > 0) {
int j = 0;
double ret;
do {
// recalculate graph
Graph *gr = new Graph(i + 1, cldist2, loopsize);
cout << "Global: " << j << endl;
ret = my_graphSlam6D->doGraphSlam6D(*gr, allScans, 1);
delete gr;
j++;
} while (j < nrIt && ret > epsilonSLAM);
}
}
}
if(loop_detection == 1 && my_loopSlam6D != NULL) {
cout << "Loop close: " << first << " " << last << endl;
my_loopSlam6D->close_loop(allScans, first, last, g);
add_edge(first, last, g);
}
if(my_graphSlam6D != NULL && mdml > 0.0) {
int j = 0;
double ret;
do {
// recalculate graph
Graph *gr = new Graph(n, cldist2, loopsize);
cout << "Global: " << j << endl;
ret = my_graphSlam6D->doGraphSlam6D(*gr, allScans, 1);
delete gr;
j++;
} while (j < nrIt && ret > epsilonSLAM);
}
if(my_graphSlam6D != NULL && mdmll > 0.0) {
my_graphSlam6D->set_mdmll(mdmll);
int j = 0;
double ret;
do {
// recalculate graph
Graph *gr = new Graph(n, sqr(graphDist), loopsize);
cout << "Global: " << j << endl;
ret = my_graphSlam6D->doGraphSlam6D(*gr, allScans, 1);
delete gr;
j++;
} while (j < nrIt && ret > epsilonSLAM);
}
}
/**
* Main program for 6D SLAM.
* Usage: bin/slam6D 'dir',
* with 'dir' the directory of a set of scans
* ...
*/
int main(int argc, char **argv)
{
signal (SIGSEGV, sigSEGVhandler);
signal (SIGINT, sigSEGVhandler);
cout << "slam6D - A highly efficient SLAM implementation based on scan matching" << endl
<< " with 6 degrees of freedom" << endl
<< "(c) Jacobs University Bremen gGmbH, Germany, since 2009" << endl
<< " University of Osnabrueck, Germany, 2006 - 2009" << endl << endl;
if (argc <= 1) {
usage(argv[0]);
}
// parsing the command line parameters
// init, default values if not specified
string dir;
double red = -1.0, mdmll = -1.0, mdml = 25.0, mdm = 25.0;
int rand = -1, mni = 50;
int start = 0, end = -1;
bool quiet = false;
bool veryQuiet = false;
int maxDist = -1;
int minDist = -1;
bool eP = true; // should we extrapolate the pose??
bool meta = false; // match against meta scan, or against LAST scan only?
int algo = 1;
int mni_lum = -1;
double cldist = 500;
int clpairs = -1;
int loopsize = 20;
string net = "none";
int anim = -1;
double epsilonICP = 0.00001;
double epsilonSLAM = 0.5;
int nns_method = simpleKD;
bool exportPts = false;
int loopSlam6DAlgo = 0;
int lum6DAlgo = 0;
double distLoop = 700.0;
int iterLoop = 100;
double graphDist = cldist;
int octree = 0; // employ randomized octree reduction?
bool cuda_enabled = false;
IOType type = UOS;
bool scanserver = false;
PairingMode pairing_mode = CLOSEST_POINT;
parseArgs(argc, argv, dir, red, rand, mdm, mdml, mdmll, mni, start, end,
maxDist, minDist, quiet, veryQuiet, eP, meta, algo, loopSlam6DAlgo, lum6DAlgo, anim,
mni_lum, net, cldist, clpairs, loopsize, epsilonICP, epsilonSLAM,
nns_method, exportPts, distLoop, iterLoop, graphDist, octree, cuda_enabled, type,
scanserver, pairing_mode);
cout << "slam6D will proceed with the following parameters:" << endl;
//@@@ to do :-)
// TODO: writer a proper TODO ^
Scan::openDirectory(scanserver, dir, type, start, end);
if(Scan::allScans.size() == 0) {
cerr << "No scans found. Did you use the correct format?" << endl;
exit(-1);
}
for(ScanVector::iterator it = Scan::allScans.begin(); it != Scan::allScans.end(); ++it) {
Scan* scan = *it;
scan->setRangeFilter(maxDist, minDist);
unsigned int types;
if ((pairing_mode == CLOSEST_POINT_ALONG_NORMAL) ||
(pairing_mode == CLOSEST_PLANE)) {
types = PointType::USE_NORMAL;
}
scan->setReductionParameter(red, octree, PointType(types));
scan->setSearchTreeParameter(nns_method, cuda_enabled);
}
icp6Dminimizer *my_icp6Dminimizer = 0;
switch (algo) {
case 1 :
my_icp6Dminimizer = new icp6D_QUAT(quiet);
break;
case 2 :
my_icp6Dminimizer = new icp6D_SVD(quiet);
break;
case 3 :
my_icp6Dminimizer = new icp6D_ORTHO(quiet);
break;
case 4 :
my_icp6Dminimizer = new icp6D_DUAL(quiet);
break;
case 5 :
my_icp6Dminimizer = new icp6D_HELIX(quiet);
break;
case 6 :
my_icp6Dminimizer = new icp6D_APX(quiet);
break;
case 7 :
my_icp6Dminimizer = new icp6D_LUMEULER(quiet);
break;
case 8 :
my_icp6Dminimizer = new icp6D_LUMQUAT(quiet);
break;
case 9 :
my_icp6Dminimizer = new icp6D_QUAT_SCALE(quiet);
break;
}
// match the scans and print the time used
long starttime = GetCurrentTimeInMilliSec();
#ifdef WITH_METRICS
Timer t = ClientMetric::matching_time.start();
#endif //WITH_METRICS
if (mni_lum == -1 && loopSlam6DAlgo == 0) {
icp6D *my_icp = 0;
if (cuda_enabled) {
#ifdef WITH_CUDA
my_icp = new icp6Dcuda(my_icp6Dminimizer, mdm, mni, quiet, meta, rand, eP,
anim, epsilonICP, nns_method, cuda_enabled);
#else
cout << "slam6d was not compiled for excuting CUDA code" << endl;
#endif
} else {
my_icp = new icp6D(my_icp6Dminimizer, mdm, mni, quiet, meta, rand, eP,
anim, epsilonICP, nns_method, cuda_enabled);
}
// check if CAD matching was selected as type
if (type == UOS_CAD)
{
my_icp->set_cad_matching (true);
}
if (my_icp) my_icp->doICP(Scan::allScans, pairing_mode);
delete my_icp;
} else if (clpairs > -1) {
//!!!!!!!!!!!!!!!!!!!!!!!!
icp6D *my_icp = 0;
if (cuda_enabled) {
#ifdef WITH_CUDA
my_icp = new icp6Dcuda(my_icp6Dminimizer, mdm, mni, quiet, meta, rand, eP,
anim, epsilonICP, nns_method, cuda_enabled);
#else
cout << "slam6d was not compiled for excuting CUDA code" << endl;
#endif
} else {
my_icp = new icp6D(my_icp6Dminimizer, mdm, mni, quiet, meta, rand, eP,
anim, epsilonICP, nns_method, cuda_enabled);
}
my_icp->doICP(Scan::allScans, pairing_mode);
graphSlam6D *my_graphSlam6D = new lum6DEuler(my_icp6Dminimizer, mdm, mdml, mni, quiet, meta,
rand, eP, anim, epsilonICP, nns_method, epsilonSLAM);
my_graphSlam6D->matchGraph6Dautomatic(Scan::allScans, mni_lum, clpairs, loopsize);
//!!!!!!!!!!!!!!!!!!!!!!!!
} else {
graphSlam6D *my_graphSlam6D = 0;
switch (lum6DAlgo) {
case 1 :
my_graphSlam6D = new lum6DEuler(my_icp6Dminimizer, mdm, mdml, mni, quiet, meta, rand, eP,
anim, epsilonICP, nns_method, epsilonSLAM);
break;
case 2 :
my_graphSlam6D = new lum6DQuat(my_icp6Dminimizer, mdm, mdml, mni, quiet, meta, rand, eP,
anim, epsilonICP, nns_method, epsilonSLAM);
break;
case 3 :
my_graphSlam6D = new ghelix6DQ2(my_icp6Dminimizer, mdm, mdml, mni, quiet, meta, rand, eP,
anim, epsilonICP, nns_method, epsilonSLAM);
break;
case 4 :
my_graphSlam6D = new gapx6D(my_icp6Dminimizer, mdm, mdml, mni, quiet, meta, rand, eP,
anim, epsilonICP, nns_method, epsilonSLAM);
break;
}
// Construct Network
if (net != "none") {
icp6D *my_icp = 0;
if (cuda_enabled) {
#ifdef WITH_CUDA
my_icp = new icp6Dcuda(my_icp6Dminimizer, mdm, mni, quiet, meta, rand, eP,
anim, epsilonICP, nns_method);
#else
cout << "slam6d was not compiled for excuting CUDA code" << endl;
#endif
} else {
my_icp = new icp6D(my_icp6Dminimizer, mdm, mni, quiet, meta, rand, eP,
anim, epsilonICP, nns_method);
}
my_icp->doICP(Scan::allScans, pairing_mode);
Graph* structure;
structure = new Graph(net);
my_graphSlam6D->doGraphSlam6D(*structure, Scan::allScans, mni_lum);
if(mdmll > 0.0) {
my_graphSlam6D->set_mdmll(mdmll);
my_graphSlam6D->doGraphSlam6D(*structure, Scan::allScans, mni_lum);
}
} else {
icp6D *my_icp = 0;
if(algo > 0) {
if (cuda_enabled) {
#ifdef WITH_CUDA
my_icp = new icp6Dcuda(my_icp6Dminimizer, mdm, mni, quiet, meta, rand, eP,
anim, epsilonICP, nns_method);
#else
cout << "slam6d was not compiled for excuting CUDA code" << endl;
#endif
} else {
my_icp = new icp6D(my_icp6Dminimizer, mdm, mni, quiet, meta, rand, eP,
anim, epsilonICP, nns_method);
}
loopSlam6D *my_loopSlam6D = 0;
switch(loopSlam6DAlgo) {
case 1:
my_loopSlam6D = new elch6Deuler(veryQuiet, my_icp6Dminimizer, distLoop, iterLoop,
rand, eP, 10, epsilonICP, nns_method);
break;
case 2:
my_loopSlam6D = new elch6Dquat(veryQuiet, my_icp6Dminimizer, distLoop, iterLoop,
rand, eP, 10, epsilonICP, nns_method);
break;
case 3:
my_loopSlam6D = new elch6DunitQuat(veryQuiet, my_icp6Dminimizer, distLoop, iterLoop,
rand, eP, 10, epsilonICP, nns_method);
break;
case 4:
my_loopSlam6D = new elch6Dslerp(veryQuiet, my_icp6Dminimizer, distLoop, iterLoop,
rand, eP, 10, epsilonICP, nns_method);
break;
}
matchGraph6Dautomatic(cldist, loopsize, Scan::allScans, my_icp, meta,
nns_method, cuda_enabled, my_loopSlam6D, my_graphSlam6D,
mni_lum, epsilonSLAM, mdml, mdmll, graphDist, eP, type);
delete my_icp;
if(loopSlam6DAlgo > 0) {
delete my_loopSlam6D;
}
}
if(my_graphSlam6D > 0) {
delete my_graphSlam6D;
}
}
}
#ifdef WITH_METRICS
ClientMetric::matching_time.end(t);
#endif //WITH_METRICS
long endtime = GetCurrentTimeInMilliSec() - starttime;
cout << "Matching done in " << endtime << " milliseconds!!!" << endl;
if (exportPts) {
cout << "Export all 3D Points to file \"points.pts\"" << endl;
ofstream redptsout("points.pts");
for(unsigned int i = 0; i < Scan::allScans.size(); i++) {
DataXYZ xyz_r(Scan::allScans[i]->get("xyz reduced"));
DataNormal normal_r(Scan::allScans[i]->get("normal reduced"));
for(unsigned int i = 0; i < xyz_r.size(); ++i) {
int r,g,b;
r = (int)(normal_r[i][0] * (127.5) + 127.5);
g = (int)(normal_r[i][1] * (127.5) + 127.5);
b = (int)(fabs(normal_r[i][2]) * (255.0));
redptsout << xyz_r[i][0] << ' ' << xyz_r[i][1] << ' ' << xyz_r[i][2]
<< ' ' << r << ' ' << g << ' ' << b << '\n';
}
redptsout << std::flush;
}
redptsout.close();
redptsout.clear();
}
const double* p;
ofstream redptsout("loopclose.pts");
for(ScanVector::iterator it = Scan::allScans.begin(); it != Scan::allScans.end(); ++it)
{
Scan* scan = *it;
p = scan->get_rPos();
Point x(p[0], p[1], p[2]);
redptsout << x << endl;
scan->saveFrames();
}
redptsout.close();
Scan::closeDirectory();
delete my_icp6Dminimizer;
cout << endl << endl;
cout << "Normal program end." << endl
<< (red < 0 && rand < 0 ? "(-> HINT: For a significant speedup, please use the '-r' or '-R' parameter <-)\n"
: "")
<< endl;
// print metric information
#ifdef WITH_METRICS
ClientMetric::print(scanserver);
#endif //WITH_METRICS
}

View file

@ -0,0 +1,46 @@
/**
* @file
* @brief Representation of the parameter of a k-d tree
* @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany.
*/
#ifndef __KDPARAMS_H__
#define __KDPARAMS_H__
/**
* @brief Contains the intermediate (static) values of a k-d tree or a cached k-d tree
*
* A parameter class for the latter k-d tree.
* Includes the padding for parallelizetion
* to avoid cache conflicts.
**/
class KDParams
{
public:
/**
* pointer to the closest point. size = 4 bytes of 32 bit machines
*/
double *closest;
/**
* distance to the closest point. size = 8 bytes
*/
double closest_d2;
/**
* pointer to the point, size = 4 bytes of 32 bit machines
*/
double *p;
/**
* pointer to direction vector, if we're using FindClosestAlongDir
*/
double *dir;
/**
* expand to 128 bytes to avoid false-sharing, 16 bytes from above + 28*4 bytes = 128 bytes
*/
int padding[28];
};
#endif

View file

@ -0,0 +1,450 @@
/*
* basicScan implementation
*
* Copyright (C) Thomas Escher, Kai Lingemann
*
* Released under the GPL version 3.
*
*/
#include "slam6d/basicScan.h"
#include "scanio/scan_io.h"
#include "slam6d/kd.h"
#include "slam6d/Boctree.h"
#include "slam6d/ann_kd.h"
#ifdef WITH_METRICS
#include "slam6d/metrics.h"
#endif //WITH_METRICS
#include <list>
#include <utility>
#include <fstream>
using std::ifstream;
using std::ofstream;
using std::flush;
using std::string;
using std::map;
using std::pair;
using std::vector;
#include <boost/filesystem/operations.hpp>
using namespace boost::filesystem;
void BasicScan::openDirectory(const std::string& path, IOType type, int start, int end)
{
#ifdef WITH_METRICS
Timer t = ClientMetric::read_scan_time.start();
#endif //WITH_METRICS
// create an instance of ScanIO
ScanIO* sio = ScanIO::getScanIO(type);
// query available scans in the directory from the ScanIO
std::list<std::string> identifiers(sio->readDirectory(path.c_str(), start, end));
Scan::allScans.reserve(identifiers.size());
// for each identifier, create a scan
for(std::list<std::string>::iterator it = identifiers.begin(); it != identifiers.end(); ++it) {
Scan::allScans.push_back(new BasicScan(path, *it, type));
}
#ifdef WITH_METRICS
ClientMetric::read_scan_time.end(t);
#endif //WITH_METRICS
}
void BasicScan::closeDirectory()
{
// clean up the scan vector
for(ScanVector::iterator it = Scan::allScans.begin(); it != Scan::allScans.end(); ++it)
delete *it;
Scan::allScans.clear();
}
BasicScan::BasicScan(double *_rPos, double *_rPosTheta, vector<double*> points) {
init();
for(int i = 0; i < 3; i++) {
rPos[i] = _rPos[i];
rPosTheta[i] = _rPosTheta[i];
}
// write original pose matrix
EulerToMatrix4(rPos, rPosTheta, transMatOrg);
// initialize transform matrices from the original one, could just copy transMatOrg to transMat instead
transformMatrix(transMatOrg);
// reset the delta align matrix to represent only the transformations after local-to-global (transMatOrg) one
M4identity(dalignxf);
PointFilter filter;
if(m_filter_range_set)
filter.setRange(m_filter_max, m_filter_min);
if(m_filter_height_set)
filter.setHeight(m_filter_top, m_filter_bottom);
if(m_range_mutation_set)
filter.setRangeMutator(m_range_mutation);
double* data = reinterpret_cast<double*>(create("xyz", sizeof(double) * 3 * points.size()).get_raw_pointer());
int tmp = 0;
for(unsigned int i = 0; i < points.size(); ++i) {
for(unsigned int j = 0; j < 3; j++) {
data[tmp++] = points[i][j];
}
}
}
BasicScan::BasicScan(const std::string& path, const std::string& identifier, IOType type) :
m_path(path), m_identifier(identifier), m_type(type)
{
init();
// request pose from file
double euler[6];
ScanIO* sio = ScanIO::getScanIO(m_type);
sio->readPose(m_path.c_str(), m_identifier.c_str(), euler);
rPos[0] = euler[0];
rPos[1] = euler[1];
rPos[2] = euler[2];
rPosTheta[0] = euler[3];
rPosTheta[1] = euler[4];
rPosTheta[2] = euler[5];
// write original pose matrix
EulerToMatrix4(euler, &euler[3], transMatOrg);
// initialize transform matrices from the original one, could just copy transMatOrg to transMat instead
transformMatrix(transMatOrg);
// reset the delta align matrix to represent only the transformations after local-to-global (transMatOrg) one
M4identity(dalignxf);
}
BasicScan::~BasicScan()
{
for (map<string, pair<unsigned char*, unsigned int>>::iterator it = m_data.begin(); it != m_data.end(); it++) {
delete it->second.first;
}
}
void BasicScan::init()
{
m_filter_max = 0.0;
m_filter_min = 0.0;
m_filter_top = 0.0;
m_filter_bottom = 0.0;
m_range_mutation = 0.0;
m_filter_range_set = false;
m_filter_height_set = false;
m_range_mutation_set = false;
}
void BasicScan::setRangeFilter(double max, double min)
{
m_filter_max = max;
m_filter_min = min;
m_filter_range_set = true;
}
void BasicScan::setHeightFilter(double top, double bottom)
{
m_filter_top = top;
m_filter_bottom = bottom;
m_filter_height_set = true;
}
void BasicScan::setRangeMutation(double range)
{
m_range_mutation_set = true;
m_range_mutation = range;
}
void BasicScan::get(unsigned int types)
{
ScanIO* sio = ScanIO::getScanIO(m_type);
vector<double> xyz;
vector<unsigned char> rgb;
vector<float> reflectance;
vector<float> temperature;
vector<float> amplitude;
vector<int> type;
vector<float> deviation;
PointFilter filter;
if(m_filter_range_set)
filter.setRange(m_filter_max, m_filter_min);
if(m_filter_height_set)
filter.setHeight(m_filter_top, m_filter_bottom);
if(m_range_mutation_set)
filter.setRangeMutator(m_range_mutation);
sio->readScan(m_path.c_str(),
m_identifier.c_str(),
filter,
&xyz,
&rgb,
&reflectance,
&temperature,
&amplitude,
&type,
&deviation);
// for each requested and filled data vector, allocate and write contents to their new data fields
if(types & DATA_XYZ && !xyz.empty()) {
double* data = reinterpret_cast<double*>(create("xyz", sizeof(double) * xyz.size()).get_raw_pointer());
for(unsigned int i = 0; i < xyz.size(); ++i) data[i] = xyz[i];
}
if(types & DATA_RGB && !rgb.empty()) {
unsigned char* data = reinterpret_cast<unsigned char*>(create("rgb", sizeof(unsigned char) * rgb.size()).get_raw_pointer());
for(unsigned int i = 0; i < rgb.size(); ++i) data[i] = rgb[i];
}
if(types & DATA_REFLECTANCE && !reflectance.empty()) {
float* data = reinterpret_cast<float*>(create("reflectance", sizeof(float) * reflectance.size()).get_raw_pointer());
for(unsigned int i = 0; i < reflectance.size(); ++i) data[i] = reflectance[i];
}
if(types & DATA_TEMPERATURE && !temperature.empty()) {
float* data = reinterpret_cast<float*>(create("temperature", sizeof(float) * temperature.size()).get_raw_pointer());
for(unsigned int i = 0; i < temperature.size(); ++i) data[i] = temperature[i];
}
if(types & DATA_AMPLITUDE && !amplitude.empty()) {
int* data = reinterpret_cast<int*>(create("amplitude", sizeof(int) * amplitude.size()).get_raw_pointer());
for(unsigned int i = 0; i < amplitude.size(); ++i) data[i] = amplitude[i];
}
if(types & DATA_TYPE && !type.empty()) {
float* data = reinterpret_cast<float*>(create("type", sizeof(double) * type.size()).get_raw_pointer());
for(unsigned int i = 0; i < type.size(); ++i) data[i] = type[i];
}
if(types & DATA_DEVIATION && !deviation.empty()) {
float* data = reinterpret_cast<float*>(create("deviation", sizeof(float) * deviation.size()).get_raw_pointer());
for(unsigned int i = 0; i < deviation.size(); ++i) data[i] = deviation[i];
}
}
DataPointer BasicScan::get(const std::string& identifier)
{
// try to get data
map<string, pair<unsigned char*, unsigned int>>::iterator it = m_data.find(identifier);
// create data fields
if(it == m_data.end()) {
// load from file
if(identifier == "xyz") get(DATA_XYZ); else
if(identifier == "rgb") get(DATA_RGB); else
if(identifier == "reflectance") get(DATA_REFLECTANCE); else
if(identifier == "temperature") get(DATA_TEMPERATURE); else
if(identifier == "amplitude") get(DATA_AMPLITUDE); else
if(identifier == "type") get(DATA_TYPE); else
if(identifier == "deviation") get(DATA_DEVIATION); else
// normals on demand
if(identifier == "normal") calcNormalsOnDemand(); else
// reduce on demand
if(identifier == "xyz reduced") calcReducedOnDemand(); else
if(identifier == "xyz reduced original") calcReducedOnDemand(); else
// show requests reduced points, manipulate in showing the same entry
if(identifier == "xyz reduced show") {
calcReducedOnDemand();
m_data["xyz reduced show"] = m_data["xyz reduced"];
} else
if(identifier == "octtree") {
createOcttree();
}
it = m_data.find(identifier);
}
// if nothing can be loaded, return an empty pointer
if(it == m_data.end())
return DataPointer(0, 0);
else
return DataPointer(it->second.first, it->second.second);
}
DataPointer BasicScan::create(const std::string& identifier, unsigned int size)
{
map<string, pair<unsigned char*, unsigned int>>::iterator it = m_data.find(identifier);
if(it != m_data.end()) {
// try to reuse, otherwise reallocate
if(it->second.second != size) {
delete it->second.first;
it->second.first = new unsigned char[size];
it->second.second = size;
}
} else {
// create a new block of data
it = m_data.insert(
std::make_pair(
identifier,
std::make_pair(
new unsigned char[size],
size
)
)
).first;
}
return DataPointer(it->second.first, it->second.second);
}
void BasicScan::clear(const std::string& identifier)
{
map<string, pair<unsigned char*, unsigned int>>::iterator it = m_data.find(identifier);
if(it != m_data.end()) {
delete it->second.first;
m_data.erase(it);
}
}
void BasicScan::createSearchTreePrivate()
{
DataXYZ xyz_orig(get("xyz reduced original"));
PointerArray<double> ar(xyz_orig);
switch(searchtree_nnstype)
{
case simpleKD:
kd = new KDtree(ar.get(), xyz_orig.size());
break;
case ANNTree:
kd = new ANNtree(ar, xyz_orig.size());
break;
case BOCTree:
kd = new BOctTree<double>(ar.get(), xyz_orig.size(), 10.0, PointType(), true);
break;
case -1:
throw runtime_error("Cannot create a SearchTree without setting a type.");
default:
throw runtime_error("SearchTree type not implemented");
}
// TODO: make the switch cases above work with CUDA
if (searchtree_cuda_enabled) createANNTree();
}
void BasicScan::calcReducedOnDemandPrivate()
{
// create reduced points and transform to initial position,
// save a copy of this for SearchTree
calcReducedPoints();
transformReduced(transMatOrg);
copyReducedToOriginal();
}
void BasicScan::calcNormalsOnDemandPrivate()
{
// create normals
calcNormals();
}
void BasicScan::createANNTree()
{
// TODO: metrics
#ifdef WITH_CUDA
if(!ann_kd_tree) {
DataXYZ xyz_orig(get("xyz reduced original"));
ann_kd_tree = new ANNkd_tree(PointArray<double>(xyz_orig).get(), xyz_orig.size(), 3, 1, ANN_KD_STD);
cout << "Cuda tree was generated with " << xyz_orig.size() << " points" << endl;
} else {
cout << "Cuda tree exists. No need for another creation" << endl;
}
#endif
}
void BasicScan::createOcttree()
{
string scanFileName = m_path + "scan" + m_identifier + ".oct";
BOctTree<float>* btree = 0;
// try to load from file, if successful return
if(octtree_loadOct && exists(scanFileName)) {
btree = new BOctTree<float>(scanFileName);
m_data.insert(
std::make_pair(
"octtree",
std::make_pair(
reinterpret_cast<unsigned char*>(btree),
0 // or memorySize()?
)
)
);
return;
}
// create octtree from scan
if(octtree_reduction_voxelSize > 0) { // with reduction, only xyz points
DataXYZ xyz_r(get("xyz reduced show"));
btree = new BOctTree<float>(PointerArray<double>(xyz_r).get(), xyz_r.size(), octtree_voxelSize, octtree_pointtype, true);
} else { // without reduction, xyz + attribute points
float** pts = octtree_pointtype.createPointArray<float>(this);
unsigned int nrpts = size<DataXYZ>("xyz");
btree = new BOctTree<float>(pts, nrpts, octtree_voxelSize, octtree_pointtype, true);
for(unsigned int i = 0; i < nrpts; ++i) delete[] pts[i]; delete[] pts;
}
// save created octtree
if(octtree_saveOct) {
cout << "Saving octree " << scanFileName << endl;
btree->serialize(scanFileName);
}
m_data.insert(
std::make_pair(
"octtree",
std::make_pair(
reinterpret_cast<unsigned char*>(btree),
0 // or memorySize()?
)
)
);
}
unsigned int BasicScan::readFrames()
{
string filename = m_path + "scan" + m_identifier + ".frames";
ifstream file(filename.c_str());
file.exceptions(ifstream::eofbit|ifstream::failbit|ifstream::badbit);
try {
double transformation[16];
unsigned int type;
do {
file >> transformation >> type;
m_frames.push_back(Frame(transformation, type));
} while(file.good());
} catch(...) {}
return m_frames.size();
}
void BasicScan::saveFrames()
{
string filename = m_path + "scan" + m_identifier + ".frames";
ofstream file(filename.c_str());
for(vector<Frame>::iterator it = m_frames.begin(); it != m_frames.end(); ++it) {
file << it->transformation << it->type << '\n';
}
file << flush;
file.close();
}
unsigned int BasicScan::getFrameCount()
{
return m_frames.size();
}
void BasicScan::getFrame(unsigned int i, const double*& pose_matrix, AlgoType& type)
{
const Frame& frame(m_frames.at(i));
pose_matrix = frame.transformation;
type = static_cast<AlgoType>(frame.type);
}
void BasicScan::addFrame(AlgoType type)
{
m_frames.push_back(Frame(transMat, type));
}

View file

@ -0,0 +1,101 @@
/**
* @file
* @brief Representation of a general search trees
* @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany.
*/
#ifndef __SEARCHTREE_H__
#define __SEARCHTREE_H__
#include <vector>
using std::vector;
#include "ptpair.h"
#include "data_types.h"
#include "pairingMode.h"
/**
* @brief The tree structure
*
* A tree holds the pointer to the data
**/
class Tree {
public:
/**
* Destructor - deletes the tree
* pure virtual, i.e., must be implented by a derived class
*/
virtual inline ~Tree() {};
};
class Scan;
/**
* @brief The search tree structure
*
* A search tree holds the pointer to the data.
* Furthermore, search functionality must be privided
**/
class SearchTree : public Tree {
friend class Scan;
public:
/**
* Constructor (default)
*/
inline SearchTree() {};
/**
* Constructor - Constructs a tree from the input.
* must be implented by a derived class
* @param pts 3D array of points
* @param n number of points
*/
SearchTree(double **pts, int n);
/**
* Destructor - deletes the tree
* virtual, i.e., must be implented by a derived class
*/
virtual inline ~SearchTree() {};
/**
* Aquire a lock on this tree, signaling that its resources are in use. Neccessary for cached data in the scanserver.
*/
virtual inline void lock() {};
/**
* Release the lock on this tree, signaling that its resources are aren't in use anymore. Neccessary for cached data in the scanserver.
*/
virtual inline void unlock() {};
/**
* This Search function returns a pointer to the closest point
* of the query point within maxdist2. If there if no such point
* a 0-pointer might be returned.
*
* @param _p Pointer to query point
* @param maxdist2 Maximal distance for closest points
* @param threadNum If parallel threads share the search tree the thread num must be given
* @return Pointer to closest point
*/
virtual double *FindClosest(double *_p, double maxdist2, int threadNum = 0) const = 0;
virtual double *FindClosestAlongDir(double *_p, double *_dir, double maxdist2, int threadNum) const;
virtual void getPtPairs(vector <PtPair> *pairs,
double *source_alignxf,
double * const *q_points, unsigned int startindex, unsigned int endindex,
int thread_num,
int rnd, double max_dist_match2, double &sum,
double *centroid_m, double *centroid_d);
virtual void getPtPairs(vector <PtPair> *pairs,
double *source_alignxf,
const DataXYZ& xyz_r, const DataNormal& normal_r, unsigned int startindex, unsigned int endindex,
int thread_num,
int rnd, double max_dist_match2, double &sum,
double *centroid_m, double *centroid_d, PairingMode pairing_mode = CLOSEST_POINT);
};
#endif

File diff suppressed because it is too large Load diff

View file

@ -0,0 +1,52 @@
/** @file
* @brief Representation of the optimized k-d tree.
* @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Thomas Escher
*/
#ifndef __KD_H__
#define __KD_H__
#include "slam6d/kdparams.h"
#include "slam6d/searchTree.h"
#include "slam6d/kdTreeImpl.h"
#ifdef _MSC_VER
#if !defined _OPENMP && defined OPENMP
#define _OPENMP
#endif
#endif
#ifdef _OPENMP
#include <omp.h>
#endif
struct Void { };
struct PtrAccessor {
inline double *operator() (Void, double* indices) {
return indices;
}
};
/**
* @brief The optimized k-d tree.
*
* A kD tree for points, with limited
* capabilities (find nearest point to
* a given point, or to a ray).
**/
class KDtree : public SearchTree, private KDTreeImpl<Void, double*, PtrAccessor> {
public:
KDtree(double **pts, int n);
virtual ~KDtree();
virtual double *FindClosest(double *_p, double maxdist2, int threadNum = 0) const;
virtual double *FindClosestAlongDir(double *_p, double *_dir, double maxdist2, int threadNum = 0) const;
};
#endif

View file

@ -0,0 +1,9 @@
IF(WITH_NORMALS)
FIND_PACKAGE(OpenCV REQUIRED)
add_library(normals normals.cc)
target_link_libraries(normals newmat scan ANN fbr_cv_io fbr_panorama ${OpenCV_LIBS})
add_executable(calc_normals calc_normals.cc)
target_link_libraries(calc_normals normals ${Boost_LIBRARIES})
ENDIF(WITH_NORMALS)

File diff suppressed because it is too large Load diff

View file

@ -0,0 +1,235 @@
/*
* point_type implementation
*
* Copyright (C) Jan Elseberg, Dorit Borrmann.
*
* Released under the GPL version 3.
*
*/
/**
* @file
* @brief Representation of a 3D point type
* @author Jan Elsberg. Automation Group, Jacobs University Bremen gGmbH, Germany.
*/
#include "slam6d/point_type.h"
#include "slam6d/scan.h"
#include <string>
using std::string;
#include <iostream>
using std::cout;
using std::cerr;
using std::endl;
#include <fstream>
#include <string.h>
#include <stdexcept>
using std::runtime_error;
PointType::PointType() {
types = USE_NONE;
pointdim = 3;
dimensionmap[1] = dimensionmap[2] = dimensionmap[3] = dimensionmap[4] = dimensionmap[5] = dimensionmap[6] = dimensionmap[7] = 1; // choose height per default
dimensionmap[8] = 1;
dimensionmap[0] = 1; // height
}
PointType::PointType(unsigned int _types) : types(_types) {
dimensionmap[1] = dimensionmap[2] = dimensionmap[3] = dimensionmap[4] = dimensionmap[5] = dimensionmap[6] = dimensionmap[7] = dimensionmap[8] = 1; // choose height per default
dimensionmap[0] = 1; // height
pointdim = 3;
if (types & PointType::USE_REFLECTANCE) dimensionmap[1] = pointdim++;
if (types & PointType::USE_NORMAL) {
pointdim += 3;
dimensionmap[2] = pointdim;
}
if (types & PointType::USE_TEMPERATURE) dimensionmap[3] = pointdim++;
if (types & PointType::USE_AMPLITUDE) dimensionmap[4] = pointdim++;
if (types & PointType::USE_DEVIATION) dimensionmap[5] = pointdim++;
if (types & PointType::USE_TYPE) dimensionmap[6] = pointdim++;
if (types & PointType::USE_COLOR) dimensionmap[7] = pointdim++;
if (types & PointType::USE_TIME) dimensionmap[8] = pointdim++;
if (types & PointType::USE_INDEX) dimensionmap[9] = pointdim++;
}
bool PointType::hasReflectance() {
return hasType(USE_REFLECTANCE);
}
bool PointType::hasNormal() {
return hasType(USE_NORMAL);
}
bool PointType::hasTemperature() {
return hasType(USE_TEMPERATURE);
}
bool PointType::hasAmplitude() {
return hasType(USE_AMPLITUDE);
}
bool PointType::hasDeviation() {
return hasType(USE_DEVIATION);
}
bool PointType::hasType() {
return hasType(USE_TYPE);
}
bool PointType::hasColor() {
return hasType(USE_COLOR);
}
bool PointType::hasTime() {
return hasType(USE_TIME);
}
bool PointType::hasIndex() {
return hasType(USE_INDEX);
}
unsigned int PointType::getReflectance() {
return dimensionmap[1];
}
unsigned int PointType::getTemperature() {
return dimensionmap[2];
}
unsigned int PointType::getAmplitude() {
return dimensionmap[3];
}
unsigned int PointType::getDeviation() {
return dimensionmap[4];
}
unsigned int PointType::getTime() {
return dimensionmap[7];
}
unsigned int PointType::getIndex() {
return dimensionmap[8];
}
unsigned int PointType::getType() {
return dimensionmap[5];
}
unsigned int PointType::getType(unsigned int type) {
if (type == USE_NONE ) {
return dimensionmap[0];
} else if (type == USE_HEIGHT) {
return dimensionmap[0];
} else if (type == USE_REFLECTANCE) {
return dimensionmap[1];
} else if (type == USE_NORMAL) {
return dimensionmap[2];
} else if (type == USE_TEMPERATURE) {
return dimensionmap[3];
} else if (type == USE_AMPLITUDE) {
return dimensionmap[4];
} else if (type == USE_DEVIATION) {
return dimensionmap[5];
} else if (type == USE_TYPE) {
return dimensionmap[6];
} else if (type == USE_COLOR) {
return dimensionmap[7];
} else if (type == USE_TIME) {
return dimensionmap[8];
} else {
return 0;
}
}
unsigned int PointType::getPointDim() { return pointdim; }
PointType PointType::deserialize(std::ifstream &f) {
unsigned int types;
f.read(reinterpret_cast<char*>(&types), sizeof(unsigned int));
return PointType(types);
}
void PointType::serialize(std::ofstream &f) {
f.write(reinterpret_cast<char*>(&types), sizeof(unsigned int));
}
unsigned int PointType::toFlags() const { return types; }
bool PointType::hasType(unsigned int type) {
return types & type;
}
const unsigned int PointType::USE_NONE = 0;
const unsigned int PointType::USE_REFLECTANCE = 1;
const unsigned int PointType::USE_NORMAL = 2;
const unsigned int PointType::USE_TEMPERATURE = 4;
const unsigned int PointType::USE_AMPLITUDE = 8;
const unsigned int PointType::USE_DEVIATION = 16;
const unsigned int PointType::USE_HEIGHT = 32;
const unsigned int PointType::USE_TYPE = 64;
const unsigned int PointType::USE_COLOR = 128;
const unsigned int PointType::USE_TIME = 256;
const unsigned int PointType::USE_INDEX = 512;
void PointType::useScan(Scan* scan)
{
// clear pointers first
m_xyz = 0; m_rgb = 0; m_reflectance = 0; m_temperature = 0; m_amplitude = 0; m_type = 0; m_deviation = 0;
// collectively load data to avoid unneccessary loading times due to split get("") calls
unsigned int types = DATA_XYZ;
if(hasColor()) types |= DATA_RGB;
if(hasReflectance()) types |= DATA_REFLECTANCE;
if(hasTemperature()) types |= DATA_TEMPERATURE;
if(hasAmplitude()) types |= DATA_AMPLITUDE;
if(hasType()) types |= DATA_TYPE;
if(hasDeviation()) types |= DATA_DEVIATION;
scan->get(types);
// access data
try {
m_xyz = new DataXYZ(scan->get("xyz"));
if(hasColor()) m_rgb = new DataRGB(scan->get("rgb"));
if(hasReflectance()) m_reflectance = new DataReflectance(scan->get("reflectance"));
if(hasTemperature()) m_temperature = new DataTemperature(scan->get("temperature"));
if(hasAmplitude()) m_amplitude = new DataAmplitude(scan->get("amplitude"));
if(hasType()) m_type = new DataType(scan->get("type"));
if(hasDeviation()) m_deviation = new DataDeviation(scan->get("deviation"));
// check if data is available, otherwise reset pointer to indicate that the scan doesn't prove this value
if(m_rgb && !m_rgb->valid()) { delete m_rgb; m_rgb = 0; }
if(m_reflectance && !m_reflectance->valid()) { delete m_reflectance; m_reflectance = 0; }
if(m_temperature && !m_temperature->valid()) { delete m_temperature; m_temperature = 0; }
if(m_amplitude && !m_amplitude->valid()) { delete m_amplitude; m_amplitude = 0; }
if(m_type && !m_type->valid()) { delete m_type; m_type = 0; }
if(m_deviation && !m_deviation->valid()) { delete m_deviation; m_deviation = 0; }
} catch(runtime_error& e) {
// unlock everything again
clearScan();
throw e;
}
}
void PointType::clearScan()
{
// unlock data access
if(m_xyz) delete m_xyz;
if(hasColor() && m_rgb) delete m_rgb;
if(hasReflectance() && m_reflectance) delete m_reflectance;
if(hasTemperature() && m_temperature) delete m_temperature;
if(hasAmplitude() && m_amplitude) delete m_amplitude;
if(hasType() && m_type) delete m_type;
if(hasDeviation() && m_deviation) delete m_deviation;
// TODO: scan->clear() on all of these types
}
unsigned int PointType::getScanSize(Scan* scan)
{
return scan->size<DataXYZ>("xyz");
}

View file

@ -0,0 +1,370 @@
/*
* icp6D implementation
*
* Copyright (C) Andreas Nuechter, Kai Lingemann
*
* Released under the GPL version 3.
*
*/
/**
* @file
* @brief Implementation of 3D scan matching with ICP
* @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany.
*/
#include "slam6d/icp6D.h"
#include "slam6d/metaScan.h"
#include "slam6d/globals.icc"
#include <iomanip>
using std::cerr;
#include <string.h>
#ifdef _MSC_VER
#if !defined _OPENMP && defined OPENMP
#define _OPENMP
#endif
#endif
#ifdef _OPENMP
#include <omp.h>
#endif
/**
* Constructor
*
* @param my_icp6Dminimizer Pointer to the ICP-minimizer
* @param max_dist_match Maximum distance to which point pairs are collected
* @param max_num_iterations Maximum number of iterations
* @param quiet Whether to print to the standard output
* @param meta Match against a meta scan?
* @param rnd Randomized point selection
* @param eP Extrapolate odometry?
* @param anim Animate which frames?
* @param epsilonICP Termination criterion
* @param nns_method Selects NNS method to be used
*/
icp6D::icp6D(icp6Dminimizer *my_icp6Dminimizer, double max_dist_match,
int max_num_iterations, bool quiet, bool meta, int rnd, bool eP,
int anim, double epsilonICP, int nns_method, bool cuda_enabled,
bool cad_matching)
{
this->my_icp6Dminimizer = my_icp6Dminimizer;
this->anim = anim;
this->cuda_enabled = cuda_enabled;
this->nns_method = nns_method;
if (!quiet) {
cout << "Maximal distance match : " << max_dist_match << endl
<< "Maximal number of iterations: " << max_num_iterations << endl << endl;
}
// checks
if (max_dist_match < 0.0) {
cerr << "ERROR [ICP6D]: first parameter (max_dist_match) has to be >= 0," << endl;
exit(1);
}
if (max_num_iterations < 0) {
cerr << "ERROR [ICP6D]: second parameter (max_num_iterations) has to be >= 0." << endl;
exit(1);
}
this->max_dist_match2 = sqr(max_dist_match);
this->max_num_iterations = max_num_iterations;
this->quiet = quiet;
this->meta = meta;
this->rnd = rnd;
this->eP = eP;
this->epsilonICP = epsilonICP;
// Set initial seed (for "real" random numbers)
// srand( (unsigned)time( NULL ) );
this->cad_matching = cad_matching;
}
/**
* Matches a 3D Scan against a 3D Scan
* @param PreviousScan The scan or metascan forming the model
* @param CurrentScan The current scan thas is to be matched
* @return The number of iterations done in this matching run
*/
int icp6D::match(Scan* PreviousScan, Scan* CurrentScan,
PairingMode pairing_mode)
{
// If ICP shall not be applied, then just write
// the identity matrix
if (max_num_iterations == 0) {
double id[16];
M4identity(id);
CurrentScan->transform(id, Scan::ICP, 0); // write end pose
return 0;
}
// icp main loop
double ret = 0.0, prev_ret = 0.0, prev_prev_ret = 0.0;
int iter = 0;
double alignxf[16];
for (iter = 0; iter < max_num_iterations; iter++) {
prev_prev_ret = prev_ret;
prev_ret = ret;
#ifdef _OPENMP
// Implementation according to the paper
// "The Parallel Iterative Closest Point Algorithm"
// by Langis / Greenspan / Godin, IEEE 3DIM 2001
//
// The same information are given in (ecrm2007.pdf)
// Andreas Nüchter. Parallelization of Scan Matching
// for Robotic 3D Mapping. In Proceedings of the 3rd
// European Conference on Mobile Robots (ECMR '07),
// Freiburg, Germany, September 2007
omp_set_num_threads(OPENMP_NUM_THREADS);
int max = (int)CurrentScan->size<DataXYZ>("xyz reduced");
int step = max / OPENMP_NUM_THREADS;
vector<PtPair> pairs[OPENMP_NUM_THREADS];
double sum[OPENMP_NUM_THREADS];
double centroid_m[OPENMP_NUM_THREADS][3];
double centroid_d[OPENMP_NUM_THREADS][3];
double Si[OPENMP_NUM_THREADS][9];
unsigned int n[OPENMP_NUM_THREADS];
for (int i = 0; i < OPENMP_NUM_THREADS; i++) {
sum[i] = centroid_m[i][0] = centroid_m[i][1] = centroid_m[i][2] = 0.0;
centroid_d[i][0] = centroid_d[i][1] = centroid_d[i][2] = 0.0;
Si[i][0] = Si[i][1] = Si[i][2] = Si[i][3] = Si[i][4] = Si[i][5] = Si[i][6] = Si[i][7] = Si[i][8] = 0.0;
n[i] = 0;
}
#pragma omp parallel
{
int thread_num = omp_get_thread_num();
Scan::getPtPairsParallel(pairs, PreviousScan, CurrentScan,
thread_num, step,
rnd, max_dist_match2,
sum, centroid_m, centroid_d, pairing_mode);
n[thread_num] = (unsigned int)pairs[thread_num].size();
if ((my_icp6Dminimizer->getAlgorithmID() == 1) ||
(my_icp6Dminimizer->getAlgorithmID() == 2)) {
for (unsigned int i = 0; i < n[thread_num]; i++) {
double pp[3] = {pairs[thread_num][i].p1.x - centroid_m[thread_num][0],
pairs[thread_num][i].p1.y - centroid_m[thread_num][1],
pairs[thread_num][i].p1.z - centroid_m[thread_num][2]};
double qq[3] = {pairs[thread_num][i].p2.x - centroid_d[thread_num][0],
pairs[thread_num][i].p2.y - centroid_d[thread_num][1],
pairs[thread_num][i].p2.z - centroid_d[thread_num][2]};
/*
double pp[3] = {pairs[thread_num][i].p1.x - centroid_d[thread_num][0],
pairs[thread_num][i].p1.y - centroid_d[thread_num][1],
pairs[thread_num][i].p1.z - centroid_d[thread_num][2]};
double qq[3] = {pairs[thread_num][i].p2.x - centroid_m[thread_num][0],
pairs[thread_num][i].p2.y - centroid_m[thread_num][1],
pairs[thread_num][i].p2.z - centroid_m[thread_num][2]};
*/
// formula (6)
Si[thread_num][0] += pp[0] * qq[0];
Si[thread_num][1] += pp[0] * qq[1];
Si[thread_num][2] += pp[0] * qq[2];
Si[thread_num][3] += pp[1] * qq[0];
Si[thread_num][4] += pp[1] * qq[1];
Si[thread_num][5] += pp[1] * qq[2];
Si[thread_num][6] += pp[2] * qq[0];
Si[thread_num][7] += pp[2] * qq[1];
Si[thread_num][8] += pp[2] * qq[2];
}
}
} // end parallel
// do we have enough point pairs?
unsigned int pairssize = 0;
for (int i = 0; i < OPENMP_NUM_THREADS; i++) {
pairssize += n[i];
}
if (pairssize > 3) {
if ((my_icp6Dminimizer->getAlgorithmID() == 1) ||
(my_icp6Dminimizer->getAlgorithmID() == 2) ) {
ret = my_icp6Dminimizer->Point_Point_Align_Parallel(OPENMP_NUM_THREADS,
n, sum, centroid_m, centroid_d, Si,
alignxf);
} else if (my_icp6Dminimizer->getAlgorithmID() == 6) {
ret = my_icp6Dminimizer->Point_Point_Align_Parallel(OPENMP_NUM_THREADS,
n, sum, centroid_m, centroid_d,
pairs,
alignxf);
} else {
cout << "This parallel minimization algorithm is not implemented !!!" << endl;
exit(-1);
}
} else {
//break;
}
#else
double centroid_m[3] = {0.0, 0.0, 0.0};
double centroid_d[3] = {0.0, 0.0, 0.0};
vector<PtPair> pairs;
Scan::getPtPairs(&pairs, PreviousScan, CurrentScan, 0, rnd,
max_dist_match2, ret, centroid_m, centroid_d, pairing_mode);
// do we have enough point pairs?
if (pairs.size() > 3) {
if (my_icp6Dminimizer->getAlgorithmID() == 3 || my_icp6Dminimizer->getAlgorithmID() == 8 ) {
memcpy(alignxf, CurrentScan->get_transMat(), sizeof(alignxf));
}
ret = my_icp6Dminimizer->Point_Point_Align(pairs, alignxf, centroid_m, centroid_d);
} else {
break;
}
#endif
if ((iter == 0 && anim != -2) || ((anim > 0) && (iter % anim == 0))) {
CurrentScan->transform(alignxf, Scan::ICP, 0); // transform the current scan
} else {
CurrentScan->transform(alignxf, Scan::ICP, -1); // transform the current scan
}
if ((fabs(ret - prev_ret) < epsilonICP) && (fabs(ret - prev_prev_ret) < epsilonICP)) {
double id[16];
M4identity(id);
if(anim == -2) {
CurrentScan->transform(id, Scan::ICP, -1); // write end pose
} else {
CurrentScan->transform(id, Scan::ICP, 0); // write end pose
}
break;
}
}
return iter;
}
/**
* Computes the point to point error between two scans
*
*
*/
double icp6D::Point_Point_Error(Scan* PreviousScan, Scan* CurrentScan, double max_dist_match, unsigned int *np) {
double error = 0;
unsigned int nr_ppairs = 0;
#ifdef _OPENMP
omp_set_num_threads(OPENMP_NUM_THREADS);
int max = (int)CurrentScan->size<DataXYZ>("xyz reduced");
int step = max / OPENMP_NUM_THREADS;
vector<PtPair> pairs[OPENMP_NUM_THREADS];
double sum[OPENMP_NUM_THREADS];
double centroid_m[OPENMP_NUM_THREADS][3];
double centroid_d[OPENMP_NUM_THREADS][3];
for (int i = 0; i < OPENMP_NUM_THREADS; i++) {
sum[i] = centroid_m[i][0] = centroid_m[i][1] = centroid_m[i][2] = 0.0;
centroid_d[i][0] = centroid_d[i][1] = centroid_d[i][2] = 0.0;
}
#pragma omp parallel
{
int thread_num = omp_get_thread_num();
Scan::getPtPairsParallel(pairs, PreviousScan, CurrentScan,
thread_num, step,
rnd, sqr(max_dist_match),
sum, centroid_m, centroid_d, CLOSEST_POINT);
}
for (unsigned int thread_num = 0; thread_num < OPENMP_NUM_THREADS; thread_num++) {
for (unsigned int i = 0; i < (unsigned int)pairs[thread_num].size(); i++) {
error += sqr(pairs[thread_num][i].p1.x - pairs[thread_num][i].p2.x)
+ sqr(pairs[thread_num][i].p1.y - pairs[thread_num][i].p2.y)
+ sqr(pairs[thread_num][i].p1.z - pairs[thread_num][i].p2.z);
}
nr_ppairs += (unsigned int)pairs[thread_num].size();
}
#else
double centroid_m[3] = {0.0, 0.0, 0.0};
double centroid_d[3] = {0.0, 0.0, 0.0};
vector<PtPair> pairs;
Scan::getPtPairs(&pairs, PreviousScan, CurrentScan, 0, rnd, sqr(max_dist_match),
error, centroid_m, centroid_d, CLOSEST_POINT);
// getPtPairs computes error as sum of squared distances
error = 0;
for (unsigned int i = 0; i < pairs.size(); i++) {
error += sqrt(
sqr(pairs[i].p1.x - pairs[i].p2.x)
+ sqr(pairs[i].p1.y - pairs[i].p2.y)
+ sqr(pairs[i].p1.z - pairs[i].p2.z) );
}
nr_ppairs = pairs.size();
#endif
if (np) *np = nr_ppairs;
// return sqrt(error/nr_ppairs);
return error/nr_ppairs;
}
/**
* This function matches the scans only with ICP
*
* @param allScans Contains all necessary scans.
*/
void icp6D::doICP(vector <Scan *> allScans, PairingMode pairing_mode)
{
double id[16];
M4identity(id);
vector < Scan* > meta_scans;
Scan* my_MetaScan = 0;
for(unsigned int i = 0; i < allScans.size(); i++) {
cout << i << "*" << endl;
Scan *CurrentScan = allScans[i];
Scan *PreviousScan = 0;
if (i > 0) {
PreviousScan = allScans[i-1];
if (eP) { // extrapolate odometry
CurrentScan->mergeCoordinatesWithRoboterPosition(PreviousScan);
}
}
if (i > 0) {
if (meta) {
match(my_MetaScan, CurrentScan, pairing_mode);
} else
if (cad_matching) {
match(allScans[0], CurrentScan, pairing_mode);
} else {
match(PreviousScan, CurrentScan, pairing_mode);
}
}
// push processed scan
if ( meta && i != allScans.size()-1 ) {
meta_scans.push_back(CurrentScan);
if (my_MetaScan) {
delete my_MetaScan;
}
my_MetaScan = new MetaScan(meta_scans, nns_method, cuda_enabled);
}
}
}

View file

@ -0,0 +1,183 @@
/*
* searchTree implementation
*
* Copyright (C) Jan Elseberg, Andreas Nuechter
*
* Released under the GPL version 3.
*
*/
/**
* @file
* @brief Representation of a general search trees
* @author Jan Elseberg. Jacobs University Bremen gGmbH, Germany
* @author Andreas Nuechter. Jacobs University Bremen gGmbH, Germany
*/
#include "slam6d/searchTree.h"
#include "slam6d/scan.h"
#include "slam6d/globals.icc"
#include <stdexcept>
double *SearchTree::FindClosestAlongDir(double *_p, double *_dir, double maxdist2, int threadNum) const
{
throw std::runtime_error("Method FindClosestAlongDir is not implemented");
}
void SearchTree::getPtPairs(vector <PtPair> *pairs,
double *source_alignxf, // source
double * const *q_points, unsigned int startindex, unsigned int endindex, // target
int thread_num,
int rnd, double max_dist_match2, double &sum,
double *centroid_m, double *centroid_d)
{
// prepare this tree for resource access in FindClosest
lock();
double local_alignxf_inv[16];
M4inv(source_alignxf, local_alignxf_inv);
// t is the original point from target, s is the (inverted) query point from target and then
// the closest point in source
double t[3], s[3];
for (unsigned int i = startindex; i < endindex; i++) {
if (rnd > 1 && rand(rnd) != 0) continue; // take about 1/rnd-th of the numbers only
t[0] = q_points[i][0];
t[1] = q_points[i][1];
t[2] = q_points[i][2];
transform3(local_alignxf_inv, t, s);
double *closest = this->FindClosest(s, max_dist_match2, thread_num);
if (closest) {
transform3(source_alignxf, closest, s);
// This should be right, model=Source=First=not moving
centroid_m[0] += s[0];
centroid_m[1] += s[1];
centroid_m[2] += s[2];
centroid_d[0] += t[0];
centroid_d[1] += t[1];
centroid_d[2] += t[2];
PtPair myPair(s, t);
double p12[3] = {
myPair.p1.x - myPair.p2.x,
myPair.p1.y - myPair.p2.y,
myPair.p1.z - myPair.p2.z };
sum += Len2(p12);
pairs->push_back(myPair);
/*cout << "PTPAIR" << i << " "
<< p[0] << " "
<< p[1] << " "
<< p[2] << " - "
<< q_points[i][0] << " "
<< q_points[i][1] << " "
<< q_points[i][2] << " " << Len2(p12) << endl; */
}
}
// release resource access lock
unlock();
return;
}
void SearchTree::getPtPairs(vector <PtPair> *pairs,
double *source_alignxf, // source
const DataXYZ& xyz_r, const DataNormal& normal_r, unsigned int startindex, unsigned int endindex, // target
int thread_num,
int rnd, double max_dist_match2, double &sum,
double *centroid_m, double *centroid_d, PairingMode pairing_mode)
{
// prepare this tree for resource access in FindClosest
lock();
double local_alignxf_inv[16];
M4inv(source_alignxf, local_alignxf_inv);
// t is the original point from target, s is the (inverted) query point from target and then
// the closest point in source
double t[3], s[3], normal[3];
for (unsigned int i = startindex; i < endindex; i++) {
if (rnd > 1 && rand(rnd) != 0) continue; // take about 1/rnd-th of the numbers only
t[0] = xyz_r[i][0];
t[1] = xyz_r[i][1];
t[2] = xyz_r[i][2];
transform3(local_alignxf_inv, t, s);
double *closest;
if (pairing_mode != CLOSEST_POINT) {
normal[0] = normal_r[i][0];
normal[1] = normal_r[i][1];
normal[2] = normal_r[i][2];
Normalize3(normal);
}
if (pairing_mode == CLOSEST_POINT_ALONG_NORMAL) {
transform3normal(local_alignxf_inv, normal);
closest = this->FindClosestAlongDir(s, normal, max_dist_match2, thread_num);
// discard points farther than 20 cm
if (closest && sqrt(Dist2(closest, s)) > 20) closest = NULL;
} else {
closest = this->FindClosest(s, max_dist_match2, thread_num);
}
if (closest) {
transform3(source_alignxf, closest, s);
if (pairing_mode == CLOSEST_PLANE) {
// need to mutate s if we are looking for closest point-to-plane
// s_ = (n,s-t)*n + t
// to find the projection of s onto plane formed by normal n and point t
double tmp[3], s_[3];
double dot;
sub3(s, t, tmp);
dot = Dot(normal, tmp);
scal_mul3(normal, dot, tmp);
add3(tmp, t, s_);
s[0] = s_[0];
s[1] = s_[1];
s[2] = s_[2];
}
// This should be right, model=Source=First=not moving
centroid_m[0] += s[0];
centroid_m[1] += s[1];
centroid_m[2] += s[2];
centroid_d[0] += t[0];
centroid_d[1] += t[1];
centroid_d[2] += t[2];
PtPair myPair(s, t);
double p12[3] = {
myPair.p1.x - myPair.p2.x,
myPair.p1.y - myPair.p2.y,
myPair.p1.z - myPair.p2.z };
sum += Len2(p12);
pairs->push_back(myPair);
/*cout << "PTPAIR" << i << " "
<< p[0] << " "
<< p[1] << " "
<< p[2] << " - "
<< q_points[i][0] << " "
<< q_points[i][1] << " "
<< q_points[i][2] << " " << Len2(p12) << endl; */
}
}
// release resource access lock
unlock();
return;
}

View file

@ -0,0 +1,118 @@
### TOOLS
IF(WITH_TOOLS)
FIND_PACKAGE(OpenCV REQUIRED)
### SCAN_RED
add_executable(scan_red scan_red.cc fbr/fbr_global.cc fbr/panorama.cc fbr/scan_cv.cc)
IF(UNIX)
target_link_libraries(scan_red scan dl ANN fbr_cv_io fbr_panorama ${OpenCV_LIBS} ${Boost_LIBRARIES})
ENDIF(UNIX)
IF (WIN32)
target_link_libraries(scan_red scan ANN XGetopt)
ENDIF(WIN32)
### SCAN_DIFF
add_executable(scan_diff scan_diff.cc)
# add_executable(scan_diff2d scan_diff2d.cc ../show/colormanager.cc)
IF(UNIX)
target_link_libraries(scan_diff scan dl ANN)
# target_link_libraries(scan_diff2d scan dl ANN)
ENDIF(UNIX)
IF (WIN32)
target_link_libraries(scan_diff scan ANN XGetopt)
# target_link_libraries(scan_diff2d scan ANN XGetopt)
ENDIF(WIN32)
add_executable(frame_to_graph frame_to_graph.cc)
add_executable(convergence convergence.cc)
add_executable(graph_balancer graph_balancer.cc)
add_executable(exportPoints exportPoints.cc)
add_executable(frames2riegl frames2riegl.cc)
add_executable(frames2pose frames2pose.cc)
add_executable(pose2frames pose2frames.cc)
add_executable(riegl2frames riegl2frames.cc)
add_executable(toGlobal toGlobal.cc)
IF(UNIX)
target_link_libraries(graph_balancer scan ${Boost_GRAPH_LIBRARY} ${Boost_SERIALIZATION_LIBRARY} ${Boost_REGEX_LIBRARY})
target_link_libraries(exportPoints scan dl ANN)
target_link_libraries(toGlobal scan)
ENDIF(UNIX)
IF (WIN32)
target_link_libraries(frame_to_graph XGetopt ${Boost_LIBRARIES})
target_link_libraries(convergence XGetopt ${Boost_LIBRARIES})
target_link_libraries(graph_balancer scan XGetopt ${Boost_LIBRARIES})
target_link_libraries(exportPoints scan ANN XGetopt ${Boost_LIBRARIES})
target_link_libraries(frames2pose XGetopt ${Boost_LIBRARIES})
target_link_libraries(pose2frames XGetopt ${Boost_LIBRARIES})
target_link_libraries(frames2riegl XGetopt ${Boost_LIBRARIES})
target_link_libraries(riegl2frames XGetopt ${Boost_LIBRARIES})
target_link_libraries(toGlobal XGetopt ${Boost_LIBRARIES})
ENDIF(WIN32)
ENDIF(WITH_TOOLS)
### SCANLIB
SET(SCANLIB_SRCS
kd.cc kdManaged.cc kdMeta.cc graphSlam6D.cc
graph.cc icp6D.cc icp6Dapx.cc icp6Dsvd.cc
icp6Dortho.cc icp6Dquat.cc icp6Dhelix.cc icp6Dlumeuler.cc
icp6Dlumquat.cc icp6Ddual.cc lum6Deuler.cc lum6Dquat.cc
ghelix6DQ2.cc gapx6D.cc ann_kd.cc elch6D.cc
elch6Dquat.cc elch6DunitQuat.cc elch6Dslerp.cc elch6Deuler.cc
point_type.cc icp6Dquatscale.cc searchTree.cc Boctree.cc
scan.cc basicScan.cc managedScan.cc metaScan.cc
io_types.cc io_utils.cc pointfilter.cc allocator.cc
)
if(WITH_METRICS)
set(SCANLIB_SRCS ${SCANLIB_SRCS} metrics.cc)
endif(WITH_METRICS)
add_library(scan STATIC ${SCANLIB_SRCS})
FIND_PACKAGE(OpenCV REQUIRED)
target_link_libraries(scan scanclient scanio normals)
IF(UNIX)
target_link_libraries(scan dl)
ENDIF(UNIX)
### EXPORT SHARED LIBS
IF(EXPORT_SHARED_LIBS)
add_library(scan_s SHARED ${SCANLIB_SRCS})
#target_link_libraries(scan_s ${Boost_LIBRARIES} newmat)
target_link_libraries(scan_s newmat_s sparse_s ANN_s )
ENDIF(EXPORT_SHARED_LIBS)
### SLAM6D
IF(WITH_CUDA)
CUDA_COMPILE(CUDA_FILES cuda/CIcpGpuCuda.cu )
add_executable(slam6D slam6D.cc cuda/icp6Dcuda.cc ${CUDA_FILES})
target_link_libraries(slam6D ${CUDA_LIBRARIES} ANN cudpp64)
CUDA_ADD_CUBLAS_TO_TARGET(slam6D)
CUDA_ADD_CUTIL_TO_TARGET(slam6D)
ELSE(WITH_CUDA)
add_executable(slam6D slam6D.cc)
ENDIF(WITH_CUDA)
IF(UNIX)
target_link_libraries(slam6D scan newmat sparse ANN)
ENDIF(UNIX)
IF(WIN32)
target_link_libraries(slam6D scan newmat sparse ANN XGetopt ${Boost_LIBRARIES})
ENDIF(WIN32)
#IF(MSVC)
# INSTALL(TARGETS slam6D RUNTIME DESTINATION ${CMAKE_SOURCE_DIR}/windows)
#ENDIF(MSVC)

View file

@ -0,0 +1,390 @@
/**
*
* Copyright (C) Jacobs University Bremen
*
* @author Vaibhav Kumar Mehta
* @author Corneliu Claudiu Prodescu
* @file normals.cc
*/
#include <vector>
#include <ANN/ANN.h>
#include <slam6d/io_types.h>
#include <slam6d/globals.icc>
#include <slam6d/scan.h>
#include "slam6d/fbr/panorama.h"
#include <scanserver/clientInterface.h>
#include "newmat/newmat.h"
#include "newmat/newmatap.h"
#include "normals/normals.h"
using namespace NEWMAT;
using namespace std;
///////////////////////////////////////////////////////
/////////////NORMALS USING AKNN METHOD ////////////////
///////////////////////////////////////////////////////
void calculateNormalsAKNN(vector<Point> &normals,vector<Point> &points, int k, const double _rPos[3] )
{
cout<<"Total number of points: "<<points.size()<<endl;
int nr_neighbors = k;
ColumnVector rPos(3);
for (int i = 0; i < 3; ++i)
rPos(i+1) = _rPos[i];
ANNpointArray pa = annAllocPts(points.size(), 3);
for (size_t i=0; i<points.size(); ++i)
{
pa[i][0] = points[i].x;
pa[i][1] = points[i].y;
pa[i][2] = points[i].z;
}
ANNkd_tree t(pa, points.size(), 3);
ANNidxArray nidx = new ANNidx[nr_neighbors];
ANNdistArray d = new ANNdist[nr_neighbors];
for (size_t i=0; i<points.size(); ++i)
{
ANNpoint p = pa[i];
//ANN search for k nearest neighbors
//indexes of the neighbors along with the query point
//stored in the array n
t.annkSearch(p, nr_neighbors, nidx, d, 0.0);
Point mean(0.0,0.0,0.0);
Matrix X(nr_neighbors,3);
SymmetricMatrix A(3);
Matrix U(3,3);
DiagonalMatrix D(3);
//calculate mean for all the points
for (int j=0; j<nr_neighbors; ++j)
{
mean.x += points[nidx[j]].x;
mean.y += points[nidx[j]].y;
mean.z += points[nidx[j]].z;
}
mean.x /= nr_neighbors;
mean.y /= nr_neighbors;
mean.z /= nr_neighbors;
//calculate covariance = A for all the points
for (int i = 0; i < nr_neighbors; ++i) {
X(i+1, 1) = points[nidx[i]].x - mean.x;
X(i+1, 2) = points[nidx[i]].y - mean.y;
X(i+1, 3) = points[nidx[i]].z - mean.z;
}
A << 1.0/nr_neighbors * X.t() * X;
EigenValues(A, D, U);
//normal = eigenvector corresponding to lowest
//eigen value that is the 1st column of matrix U
ColumnVector n(3);
n(1) = U(1,1);
n(2) = U(2,1);
n(3) = U(3,1);
ColumnVector point_vector(3);
point_vector(1) = p[0] - rPos(1);
point_vector(2) = p[1] - rPos(2);
point_vector(3) = p[2] - rPos(3);
point_vector = point_vector / point_vector.NormFrobenius();
Real angle = (n.t() * point_vector).AsScalar();
if (angle < 0) {
n *= -1.0;
}
n = n / n.NormFrobenius();
normals.push_back(Point(n(1), n(2), n(3)));
}
delete[] nidx;
delete[] d;
annDeallocPts(pa);
}
////////////////////////////////////////////////////////////////
/////////////NORMALS USING ADAPTIVE AKNN METHOD ////////////////
////////////////////////////////////////////////////////////////
void calculateNormalsAdaptiveAKNN(vector<Point> &normals,vector<Point> &points,
int kmin, int kmax, const double _rPos[3])
{
ColumnVector rPos(3);
for (int i = 0; i < 3; ++i)
rPos(i+1) = _rPos[i];
cout<<"Total number of points: "<<points.size()<<endl;
int nr_neighbors;
ANNpointArray pa = annAllocPts(points.size(), 3);
for (size_t i=0; i<points.size(); ++i)
{
pa[i][0] = points[i].x;
pa[i][1] = points[i].y;
pa[i][2] = points[i].z;
}
ANNkd_tree t(pa, points.size(), 3);
Point mean(0.0,0.0,0.0);
double e1,e2,e3;
for (size_t i=0; i<points.size(); ++i)
{
Matrix U(3,3);
ANNpoint p = pa[i];
for(int kidx = kmin; kidx < kmax; kidx++)
{
nr_neighbors=kidx+1;
ANNidxArray nidx = new ANNidx[nr_neighbors];
ANNdistArray d = new ANNdist[nr_neighbors];
//ANN search for k nearest neighbors
//indexes of the neighbors along with the query point
//stored in the array n
t.annkSearch(p, nr_neighbors, nidx, d, 0.0);
mean.x=0,mean.y=0,mean.z=0;
//calculate mean for all the points
for (int j=0; j<nr_neighbors; ++j)
{
mean.x += points[nidx[j]].x;
mean.y += points[nidx[j]].y;
mean.z += points[nidx[j]].z;
}
mean.x /= nr_neighbors;
mean.y /= nr_neighbors;
mean.z /= nr_neighbors;
Matrix X(nr_neighbors,3);
SymmetricMatrix A(3);
DiagonalMatrix D(3);
//calculate covariance = A for all the points
for (int j = 0; j < nr_neighbors; ++j) {
X(j+1, 1) = points[nidx[j]].x - mean.x;
X(j+1, 2) = points[nidx[j]].y - mean.y;
X(j+1, 3) = points[nidx[j]].z - mean.z;
}
A << 1.0/nr_neighbors * X.t() * X;
EigenValues(A, D, U);
e1 = D(1);
e2 = D(2);
e3 = D(3);
delete[] nidx;
delete[] d;
//We take the particular k if the second maximum eigen value
//is at least 25 percent of the maximum eigen value
if ((e1 > 0.25 * e2) && (fabs(1.0 - (double)e2/(double)e3) < 0.25))
break;
}
//normal = eigenvector corresponding to lowest
//eigen value that is the 1rd column of matrix U
ColumnVector n(3);
n(1) = U(1,1);
n(2) = U(2,1);
n(3) = U(3,1);
ColumnVector point_vector(3);
point_vector(1) = p[0] - rPos(1);
point_vector(2) = p[1] - rPos(2);
point_vector(3) = p[2] - rPos(3);
point_vector = point_vector / point_vector.NormFrobenius();
Real angle = (n.t() * point_vector).AsScalar();
if (angle < 0) {
n *= -1.0;
}
n = n / n.NormFrobenius();
normals.push_back(Point(n(1), n(2), n(3)));
}
annDeallocPts(pa);
}
///////////////////////////////////////////////////////
/////////////NORMALS USING IMAGE NEIGHBORS ////////////
///////////////////////////////////////////////////////
void calculateNormalsPANORAMA(vector<Point> &normals,
vector<Point> &points,
vector< vector< vector< cv::Vec3f > > > extendedMap,
const double _rPos[3])
{
ColumnVector rPos(3);
for (int i = 0; i < 3; ++i)
rPos(i+1) = _rPos[i];
cout<<"Total number of points: "<<points.size()<<endl;
points.clear();
int nr_neighbors = 0;
cout << "height of Image: "<<extendedMap.size()<<endl;
cout << "width of Image: "<<extendedMap[0].size()<<endl;
// as the nearest neighbors and then the same PCA method as done in AKNN
//temporary dynamic array for all the neighbors of a given point
vector<cv::Vec3f> neighbors;
for (size_t i=0; i< extendedMap.size(); i++)
{
for (size_t j=0; j<extendedMap[i].size(); j++)
{
if (extendedMap[i][j].size() == 0) continue;
neighbors.clear();
Point mean(0.0,0.0,0.0);
// Offset for neighbor computation
int offset[2][5] = {{-1,0,1,0,0},{0,-1,0,1,0}};
// Traversing all the cells in the extended map
for (int n = 0; n < 5; ++n) {
int x = i + offset[0][n];
int y = j + offset[1][n];
// Copy the neighboring buckets into the vector
if (x >= 0 && x < (int)extendedMap.size() &&
y >= 0 && y < (int)extendedMap[x].size()) {
for (unsigned int k = 0; k < extendedMap[x][y].size(); k++) {
neighbors.push_back(extendedMap[x][y][k]);
}
}
}
nr_neighbors = neighbors.size();
cv::Vec3f p = extendedMap[i][j][0];
//if no neighbor point is found in the 4-neighboring pixels then normal is set to zero
if (nr_neighbors < 3)
{
points.push_back(Point(p[0], p[1], p[2]));
normals.push_back(Point(0.0,0.0,0.0));
continue;
}
//calculate mean for all the points
Matrix X(nr_neighbors,3);
SymmetricMatrix A(3);
Matrix U(3,3);
DiagonalMatrix D(3);
//calculate mean for all the points
for(int k = 0; k < nr_neighbors; k++)
{
cv::Vec3f pp = neighbors[k];
mean.x += pp[0];
mean.y += pp[1];
mean.z += pp[2];
}
mean.x /= nr_neighbors;
mean.y /= nr_neighbors;
mean.z /= nr_neighbors;
//calculate covariance = A for all the points
for (int n = 0; n < nr_neighbors; ++n) {
cv::Vec3f pp = neighbors[n];
X(n+1, 1) = pp[0] - mean.x;
X(n+1, 2) = pp[1] - mean.y;
X(n+1, 3) = pp[2] - mean.z;
}
A << 1.0/nr_neighbors * X.t() * X;
EigenValues(A, D, U);
//normal = eigenvector corresponding to lowest
//eigen value that is the 1st column of matrix U
ColumnVector n(3);
n(1) = U(1,1);
n(2) = U(2,1);
n(3) = U(3,1);
ColumnVector point_vector(3);
point_vector(1) = p[0] - rPos(1);
point_vector(2) = p[1] - rPos(2);
point_vector(3) = p[2] - rPos(3);
point_vector = point_vector / point_vector.NormFrobenius();
Real angle = (n.t() * point_vector).AsScalar();
if (angle < 0) {
n *= -1.0;
}
n = n / n.NormFrobenius();
for (unsigned int k = 0; k < extendedMap[i][j].size(); k++) {
cv::Vec3f p = extendedMap[i][j][k];
points.push_back(Point(p[0], p[1], p[2]));
normals.push_back(Point(n(1), n(2), n(3)));
}
}
}
}
//////////////////////////////////////////////////////////////////////////////////////////////
///////////FAST NORMALS USING PANORAMA EQUIRECTANGULAR RANGE IMAGE //////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////
/*
void calculateNormalsFAST(vector<Point> &normals,vector<Point> &points,cv::Mat &img,vector<vector<vector<cv::Vec3f>>> extendedMap)
{
cout<<"Total number of points: "<<points.size()<<endl;
points.clear();
int nr_points = 0;
//int nr_neighbors = 0,nr_neighbors_center = 0;
cout << "height of Image: "<<extendedMap.size()<<endl;
cout << "width of Image: "<<extendedMap[0].size()<<endl;
for (size_t i=0; i< extendedMap.size(); ++i)
{
for (size_t j=0; j<extendedMap[0].size(); j++)
{
double theta,phi,rho;
double x,y,z;
double dRdTheta,dRdPhi;
double n[3],m;
nr_points = extendedMap[i][j].size();
if (nr_points == 0 ) continue;
for (int k = 0; k< nr_points; k++)
{
cv::Vec3f p = extendedMap[i][j][k];
x = p[0];
y = p[1];
z = p[2];
rho = sqrt(x*x + y*y + z*z);
theta = atan(y/x);
phi = atan(z/x);
//Sobel Filter for the derivative
dRdTheta = dRdPhi = 0.0;
if (i == 0 || i == extendedMap.size()-1 || j == 0 || j == extendedMap[0].size()-1)
{
points.push_back(Point(x, y, z));
normals.push_back(Point(0.0,0.0,0.0));
continue;
}
dRdPhi += 10*img.at<uchar>(i-1,j);
dRdPhi += 3 *img.at<uchar>(i-1,j-1);
dRdPhi += 3 *img.at<uchar>(i-1,j+1);
dRdPhi -= 10*img.at<uchar>(i+1,j);
dRdPhi -= 3 *img.at<uchar>(i+1,j-1);
dRdPhi -= 3 *img.at<uchar>(i+1,j+1);
dRdTheta += 10*img.at<uchar>(i,j-1);
dRdTheta += 3 *img.at<uchar>(i-1,j-1);
dRdTheta += 3 *img.at<uchar>(i+1,j-1);
dRdTheta -= 10*img.at<uchar>(i,j+1);
dRdTheta -= 3 *img.at<uchar>(i-1,j+1);
dRdTheta -= 3 *img.at<uchar>(i+1,j+1);
n[0] = cos(theta) * sin(phi) - sin(theta) * dRdTheta / rho / sin(phi) +
cos(theta) * cos(phi) * dRdPhi / rho;
n[1] = sin(theta) * sin(phi) + cos(theta) * dRdTheta / rho / sin(phi) +
sin(theta) * cos(phi) * dRdPhi / rho;
n[2] = cos(phi) - sin(phi) * dRdPhi / rho;
//n[2] = -n[2];
m = sqrt(n[0]*n[0]+n[1]*n[1]+n[2]*n[2]);
n[0] /= m; n[1] /= m; n[2] /= m;
points.push_back(Point(x, y, z));
normals.push_back(Point(n[0],n[1],n[2]));
}
}
}
}
*/

View file

@ -0,0 +1,82 @@
/** @file
* @brief Representation of the optimized k-d tree. MetaScan variant.
* @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Thomas Escher
*/
#ifndef __KD_META_H__
#define __KD_META_H__
#include "kdparams.h"
#include "searchTree.h"
#include "data_types.h"
#include "kdTreeImpl.h"
#include <boost/thread/mutex.hpp>
#include <boost/thread/locks.hpp>
#ifdef _MSC_VER
#if !defined _OPENMP && defined OPENMP
#define _OPENMP
#endif
#endif
#ifdef _OPENMP
#include <omp.h>
#endif
class Scan;
struct Index {
unsigned int s, i;
inline void set(unsigned int _s, unsigned int _i) { s = _s; i = _i; }
};
struct IndexAccessor {
inline double* operator() (const DataXYZ* const* pts, const Index& i) const {
return (*pts[i.s])[i.i];
}
};
/**
* @brief The optimized k-d tree.
*
* A kD tree for points, with limited
* capabilities (find nearest point to
* a given point, or to a ray).
**/
class KDtreeMetaManaged :
public SearchTree,
private KDTreeImpl<const DataXYZ* const*, Index, IndexAccessor>
{
public:
KDtreeMetaManaged(const vector<Scan*>& scans);
virtual ~KDtreeMetaManaged();
virtual void lock();
virtual void unlock();
//! Aquires cached data first to pass on to the usual KDtree to process
virtual double* FindClosest(double *_p, double maxdist2, int threadNum = 0) const;
virtual double *FindClosestAlongDir(double *_p, double *_dir, double maxdist2, int threadNum = 0) const;
private:
Scan** m_scans;
DataXYZ** m_data;
unsigned int m_size;
//! Mutex for safely reducing points just once in a multithreaded environment
boost::mutex m_mutex_locking;
volatile unsigned int m_count_locking;
// constructor initializer list hacks
Index* m_temp_indices;
Index* prepareTempIndices(const vector<Scan*>& scans);
unsigned int getPointsSize(const vector<Scan*>& scans);
};
#endif

View file

@ -0,0 +1,91 @@
/*
* kdManaged implementation
*
* Copyright (C) Andreas Nuechter, Kai Lingemann, Thomas Escher
*
* Released under the GPL version 3.
*
*/
/** @file
* @brief An optimized k-d tree implementation
* @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Thomas Escher. Institute of Computer Science, University of Osnabrueck, Germany.
*/
#ifdef _MSC_VER
#define _USE_MATH_DEFINES
#endif
#include "slam6d/kdManaged.h"
#include "slam6d/scan.h"
#include "slam6d/globals.icc"
#include <iostream>
using std::cout;
using std::cerr;
using std::endl;
#include <algorithm>
using std::swap;
#include <cmath>
#include <cstring>
// KDtree class static variables
template<class PointData, class AccessorData, class AccessorFunc>
KDParams KDTreeImpl<PointData, AccessorData, AccessorFunc>::params[MAX_OPENMP_NUM_THREADS];
KDtreeManaged::KDtreeManaged(Scan* scan) :
m_scan(scan), m_data(0), m_count_locking(0)
{
create(scan->get("xyz reduced original"), prepareTempIndices(scan->size<DataXYZ>("xyz reduced original")), scan->size<DataXYZ>("xyz reduced original"));
// allocate in prepareTempIndices, deleted here
delete[] m_temp_indices;
}
unsigned int* KDtreeManaged::prepareTempIndices(unsigned int n)
{
m_temp_indices = new unsigned int[n];
for(unsigned int i = 0; i < n; ++i)
m_temp_indices[i] = i;
return m_temp_indices;
}
double* KDtreeManaged::FindClosest(double *_p, double maxdist2, int threadNum) const
{
params[threadNum].closest = 0;
params[threadNum].closest_d2 = maxdist2;
params[threadNum].p = _p;
_FindClosest(*m_data, threadNum);
return params[threadNum].closest;
}
double* KDtreeManaged::FindClosestAlongDir(double *_p, double *_dir, double maxdist2, int threadNum) const
{
params[threadNum].closest = NULL;
params[threadNum].closest_d2 = maxdist2;
params[threadNum].p = _p;
params[threadNum].dir = _dir;
_FindClosestAlongDir(*m_data, threadNum);
return params[threadNum].closest;
}
void KDtreeManaged::lock()
{
boost::lock_guard<boost::mutex> lock(m_mutex_locking);
++m_count_locking;
if(m_data == 0) {
// aquire an array lock from the scan and hold it while the tree is in use
m_data = new DataXYZ(m_scan->get("xyz reduced original"));
}
}
void KDtreeManaged::unlock()
{
boost::lock_guard<boost::mutex> lock(m_mutex_locking);
--m_count_locking;
if(m_count_locking == 0 && m_data != 0) {
delete m_data;
m_data = 0;
}
}

View file

@ -0,0 +1,143 @@
/**
* @file
* @brief Representation of a 3D point
* @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany.
*/
#ifndef __POINT_H__
#define __POINT_H__
#include <cmath>
#include <iostream>
using std::ostream;
using std::istream;
#include <stdexcept>
using std::runtime_error;
/**
* @brief Representation of a point in 3D space
*/
class Point {
public:
/**
* Default constructor
*/
inline Point() { x = y = z = 0.0; point_id = 0; type = 0; reflectance = 0.0; temperature = 0.0; amplitude = 0.0; deviation = 0.0; rgb[0] = 255; rgb[1] = 255; rgb[2] = 255;};
/**
* Copy constructor
*/
inline Point(const Point& p) { x = p.x; y = p.y; z = p.z; type = p.type; point_id = p.point_id;
reflectance = p.reflectance; temperature = p.temperature; amplitude = p.amplitude; deviation = p.deviation; rgb[0] = p.rgb[0]; rgb[1] = p.rgb[1]; rgb[2] = p.rgb[2];};
/**
* Constructor with an array, i.e., vecctor of coordinates
*/
inline Point(const double *p) { x = p[0]; y = p[1]; z = p[2]; type = 0; reflectance = 0.0; temperature = 0.0; amplitude = 0.0; deviation = 0.0; rgb[0] = 255; rgb[1] = 255; rgb[2] = 255;};
inline Point(const double *p, const char *c) { x = p[0]; y = p[1]; z = p[2]; type = 0; reflectance = 0.0; temperature = 0.0; amplitude = 0.0; deviation = 0.0; rgb[0] = c[0]; rgb[1] = c[1]; rgb[2] = c[2];};
/**
* Constructor with three double values
*/
inline Point(const double _x, const double _y, const double _z) { x = _x; y = _y; z = _z; type = 0; reflectance = 0.0; temperature = 0.0; amplitude = 0.0; deviation = 0.0; rgb[0] = 255; rgb[1] = 255; rgb[2] = 255;};
inline Point(const double _x, const double _y, const double _z, const char _r,
const char _g, const char _b) { x = _x; y = _y; z = _z; type = 0; reflectance = 0.0; temperature = 0.0; amplitude = 0.0; deviation = 0.0; rgb[0] = 255; rgb[1] = 255; rgb[2] = 255; rgb[0] = _r; rgb[1] = _g; rgb[2] = _b;};
static inline Point cross(const Point &X, const Point &Y) {
Point res;
res.x = X.y * Y.z - X.z * Y.y;
res.y = X.z * Y.x - X.x * Y.z;
res.z = X.x * Y.y - X.y * Y.x;
return res;
};
static inline Point norm(const Point &p) {
double l = sqrt(p.x*p.x + p.y*p.y + p.z*p.z);
Point res(p.x/l, p.y/l, p.z/l);
return res;
};
inline Point operator+(const Point &p) const {
Point res;
res.x = x + p.x;
res.y = y + p.y;
res.z = z + p.z;
return res;
};
inline Point operator-(const Point &p) const {
Point res;
res.x = x - p.x;
res.y = y - p.y;
res.z = z - p.z;
return res;
};
inline Point& operator-=(const Point &p) {
x -= p.x;
y -= p.y;
z -= p.z;
return *this;
};
inline Point& operator+=(const Point &p) {
x += p.x;
y += p.y;
z += p.z;
return *this;
};
inline void transform(const double alignxf[16]);
inline double distance(const Point& p);
inline friend ostream& operator<<(ostream& os, const Point& p);
inline friend istream& operator>>(istream& is, Point& p);
// also public; set/get functions not necessary here
/// x coordinate in 3D space
double x;
/// y coordinate in 3D space
double y;
/// z coordinate in 3D space
double z;
/// normal x direction in 3D space
double nx;
/// normal x direction in 3D space
double ny;
/// normal x direction in 3D space
double nz;
/// additional information about the point, e.g., semantic
/// also used in veloscan for distiuguish moving or static
int type;
/////////////////////////for veloslam/////////////////////////////
double rad;
/// tang in cylindrical coordinates for veloscan
double tan_theta;
// point id in points for veloscan , you can use it find point.
long point_id;
/////////////////////////for veloslam/////////////////////////////
// color information of the point between 0 and 255
// rgb
unsigned char rgb[3];
float reflectance;
float temperature;
float amplitude;
float deviation;
};
inline Point operator*(const double &v, const Point &p) {
Point res;
res.x = v * p.x;
res.y = v * p.y;
res.z = v * p.z;
return res;
}
#include "point.icc"
#endif

View file

@ -0,0 +1,43 @@
#ifndef META_SCAN_H
#define META_SCAN_H
#include "scan.h"
#include <vector>
class MetaScan : public Scan {
public:
MetaScan(std::vector<Scan*> scans, int nns_method = -1, bool cuda_enabled = false);
virtual ~MetaScan();
//! How many scans this meta scan contains
unsigned int size() const;
//! Return the contained scan
Scan* getScan(unsigned int i) const;
virtual void setRangeFilter(double max, double min) {}
virtual void setHeightFilter(double top, double bottom) {}
virtual const char* getIdentifier() const { return "metascan"; }
virtual DataPointer get(const std::string& identifier) { return DataPointer(0, 0); }
virtual void get(unsigned int types) {}
virtual DataPointer create(const std::string& identifier, unsigned int size) { return DataPointer(0, 0); }
virtual void clear(const std::string& identifier) {}
virtual unsigned int readFrames() { return 0; }
virtual void saveFrames() {}
virtual unsigned int getFrameCount() { return 0; }
virtual void getFrame(unsigned int i, const double*& pose_matrix, AlgoType& type) {}
protected:
virtual void createSearchTreePrivate();
virtual void calcReducedOnDemandPrivate() {}
virtual void calcNormalsOnDemandPrivate() {}
virtual void addFrame(AlgoType type) {}
private:
std::vector<Scan*> m_scans;
};
#endif //META_SCAN_H

View file

@ -0,0 +1,862 @@
/*
* 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;
}

View file

@ -0,0 +1,73 @@
/** @file
* @brief Representation of the optimized k-d tree.
* @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Thomas Escher
*/
#ifndef __KD_MANAGED_H__
#define __KD_MANAGED_H__
#include "slam6d/kdparams.h"
#include "slam6d/searchTree.h"
#include "slam6d/data_types.h"
#include "slam6d/kdTreeImpl.h"
#ifdef _MSC_VER
#if !defined _OPENMP && defined OPENMP
#define _OPENMP
#endif
#endif
#ifdef _OPENMP
#include <omp.h>
#endif
#include <boost/thread/mutex.hpp>
#include <boost/thread/locks.hpp>
class Scan;
struct ArrayAccessor {
inline double *operator() (const DataXYZ& pts, unsigned int i) {
return pts[i];
}
};
/**
* @brief The optimized k-d tree.
*
* A kD tree for points, with limited
* capabilities (find nearest point to
* a given point, or to a ray).
**/
class KDtreeManaged :
public SearchTree,
private KDTreeImpl<const DataXYZ&, unsigned int, ArrayAccessor>
{
public:
KDtreeManaged(Scan* scan);
virtual ~KDtreeManaged() {}
virtual void lock();
virtual void unlock();
//! Aquires cached data first to pass on to the usual KDtree to process
virtual double* FindClosest(double *_p, double maxdist2, int threadNum = 0) const;
virtual double *FindClosestAlongDir(double *_p, double *_dir, double maxdist2, int threadNum = 0) const;
private:
Scan* m_scan;
DataXYZ* m_data;
//! Mutex for safely reducing points just once in a multithreaded environment
boost::mutex m_mutex_locking;
volatile unsigned int m_count_locking;
// constructor initializer list hacks
unsigned int* m_temp_indices;
unsigned int* prepareTempIndices(unsigned int n);
};
#endif

View file

@ -0,0 +1,439 @@
cmake_minimum_required (VERSION 2.8.2)
SET(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/3rdparty/CMakeModules" ${CMAKE_MODULE_PATH})
project (3DTK)
#include_directories(OPENGL_INCLUDE_DIR)
IF(WIN32)
set(Boost_USE_STATIC_LIBS TRUE)
ELSE(WIN32)
set(Boost_USE_STATIC_LIBS FALSE)
ENDIF(WIN32)
SET(Boost_ADDITIONAL_VERSIONS "1.42" "1.42.0" "1.44" "1.44.0" "1.45.0" "1.45" "1.46" "1.46.1" "1.47.0" "1.47" "1.48" "1.49")
IF(WIN32)
# for some unknown reason no one variant works on all windows platforms
find_package( Boost COMPONENTS serialization graph regex filesystem system thread date_time REQUIRED)
ELSE(WIN32)
find_package( Boost COMPONENTS serialization graph regex filesystem system thread date_time REQUIRED)
ENDIF(WIN32)
if(Boost_FOUND)
link_directories(${BOOST_LIBRARY_DIRS})
include_directories(${Boost_INCLUDE_DIRS})
add_definitions(${Boost_LIB_DIAGNOSTIC_DEFINITIONS})
endif()
#################################################
# Declare Options and modify build accordingly ##
#################################################
FUNCTION(ENFORCE_OPTION_DEP_3DTK option VALUE)
SET (${option} "${VALUE}" CACHE BOOL "${${option}_DESCRIPTION}" FORCE) # this option set to VALUE as advised
#now make sure other dependencies are also true
FOREACH(d ${${option}_DEPENDENCIES}) # look through all my dependencies
STRING(REGEX REPLACE " +" ";" CMAKE_DEPENDENT_OPTION_DEP "${d}")
# check for a not in front
STRING(STRIP "${CMAKE_DEPENDENT_OPTION_DEP}" CMAKE_DEPENDENT_OPTION_DEP)
STRING(SUBSTRING "${CMAKE_DEPENDENT_OPTION_DEP}" 0 3 CMAKE_DEPENDENT_OPTION_DEP_3)
STRING(TOUPPER "${CMAKE_DEPENDENT_OPTION_DEP_3}" CMAKE_DEPENDENT_OPTION_DEP_3)
STRING(COMPARE EQUAL "${CMAKE_DEPENDENT_OPTION_DEP_3}" "NOT" CMAKE_DEPENDENT_OPTION_DEP_NOT)
#STRING(REPLACE "NOT " "" CMAKE_DEPENDENT_OPTION_DEP "${d}")
IF(CMAKE_DEPENDENT_OPTION_DEP_NOT) # we found a NOT
STRING(REPLACE "NOT;" "" CMAKE_DEPENDENT_OPTION_DEP "${CMAKE_DEPENDENT_OPTION_DEP}")
IF(${CMAKE_DEPENDENT_OPTION_DEP}) # not met, make sure it is
ENFORCE_OPTION_DEP_3DTK(${CMAKE_DEPENDENT_OPTION_DEP} OFF)
ELSE(${CMAKE_DEPENDENT_OPTION_DEP}) # dependency is met
ENDIF(${CMAKE_DEPENDENT_OPTION_DEP})
ELSE(CMAKE_DEPENDENT_OPTION_DEP_NOT)
IF(${CMAKE_DEPENDENT_OPTION_DEP}) # dependency is met
ELSE(${CMAKE_DEPENDENT_OPTION_DEP}) # not met, make sure it is
ENFORCE_OPTION_DEP_3DTK(${CMAKE_DEPENDENT_OPTION_DEP} ON)
ENDIF(${CMAKE_DEPENDENT_OPTION_DEP})
ENDIF(CMAKE_DEPENDENT_OPTION_DEP_NOT)
ENDFOREACH(d)
ENDFUNCTION(ENFORCE_OPTION_DEP_3DTK)
MACRO(OPT_DEP option doc default depends)
OPTION(${option} "${doc}" "${default}")
SET(${option} "${${option}}" CACHE BOOL "${doc}" FORCE)
SET(${option}_DEPENDENCIES "${depends}" CACHE INTERNAL "" FORCE)
SET(${option}_DESCRIPTION "${doc}" CACHE INTERNAL "" FORCE)
IF (${option})
#MESSAGE(STATUS "Yes ${option} is true")
# MESSAGE("FOREACH d in ${depends}")
FOREACH(d ${depends})
STRING(REGEX REPLACE " +" ";" CMAKE_DEPENDENT_OPTION_DEP "${d}")
# check for a not in front
STRING(STRIP "${CMAKE_DEPENDENT_OPTION_DEP}" CMAKE_DEPENDENT_OPTION_DEP)
STRING(SUBSTRING "${CMAKE_DEPENDENT_OPTION_DEP}" 0 3 CMAKE_DEPENDENT_OPTION_DEP_3)
STRING(TOUPPER "${CMAKE_DEPENDENT_OPTION_DEP_3}" CMAKE_DEPENDENT_OPTION_DEP_3)
STRING(COMPARE EQUAL "${CMAKE_DEPENDENT_OPTION_DEP_3}" "NOT" CMAKE_DEPENDENT_OPTION_DEP_NOT)
IF(CMAKE_DEPENDENT_OPTION_DEP_NOT) # we found a NOT
STRING(REPLACE "NOT;" "" CMAKE_DEPENDENT_OPTION_DEP "${CMAKE_DEPENDENT_OPTION_DEP}")
IF(${CMAKE_DEPENDENT_OPTION_DEP}) # not met, make sure it is
ENFORCE_OPTION_DEP_3DTK(${CMAKE_DEPENDENT_OPTION_DEP} OFF)
ELSE(${CMAKE_DEPENDENT_OPTION_DEP}) # dependency is met
ENDIF(${CMAKE_DEPENDENT_OPTION_DEP})
ELSE(CMAKE_DEPENDENT_OPTION_DEP_NOT)
IF(${CMAKE_DEPENDENT_OPTION_DEP}) # dependency is met
ELSE(${CMAKE_DEPENDENT_OPTION_DEP}) # not met, make sure it is
ENFORCE_OPTION_DEP_3DTK(${CMAKE_DEPENDENT_OPTION_DEP} ON)
ENDIF(${CMAKE_DEPENDENT_OPTION_DEP})
ENDIF(CMAKE_DEPENDENT_OPTION_DEP_NOT)
ENDFOREACH(d)
ENDIF(${option})
ENDMACRO(OPT_DEP)
## FreeGLUT
OPT_DEP(WITH_FREEGLUT "Whether freeglut is available. This enables iterative drawing in show. ON/OFF" ON "")
IF(WITH_FREEGLUT)
MESSAGE(STATUS "With freeglut")
SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DWITH_FREEGLUT")
ELSE(WITH_FREEGLUT)
MESSAGE(STATUS "Without freeglut")
ENDIF(WITH_FREEGLUT)
## Show
OPT_DEP(WITH_SHOW "Whether to build Show. This is the Visualization program of slam6d. ON/OFF" ON "" )
IF(WITH_SHOW)
FIND_PACKAGE(OpenGL REQUIRED)
FIND_PACKAGE(GLUT REQUIRED)
MESSAGE(STATUS "With show")
ELSE(WITH_SHOW)
# SET (WITH_OCTREE_DISPLAY "ON" CACHE INTERNAL "" FORCE)
MESSAGE(STATUS "Without show")
ENDIF(WITH_SHOW)
## WXShow
OPT_DEP(WITH_WXSHOW "Whether to build WXShow. This is the wxwidgets variant of Show. ON/OFF" OFF "")
IF(WITH_WXSHOW)
FIND_PACKAGE(OpenGL REQUIRED)
FIND_PACKAGE(GLUT REQUIRED)
find_package(wxWidgets COMPONENTS core base gl REQUIRED)
# set wxWidgets_wxrc_EXECUTABLE to be ignored in the configuration
SET (wxWidgets_wxrc_EXECUTABLE " " CACHE INTERNAL "" FORCE)
# wxWidgets include (this will do all the magic to configure everything)
include( ${wxWidgets_USE_FILE})
MESSAGE(STATUS "With wxshow")
ELSE(WITH_WXSHOW)
MESSAGE(STATUS "Without wxshow")
ENDIF(WITH_WXSHOW)
## Shapes
OPT_DEP(WITH_SHAPE_DETECTION "Whether to build shapes and planes executable for detecting planes. ON/OFF" OFF "")
IF(WITH_SHAPE_DETECTION)
MESSAGE(STATUS "With shape detection")
ELSE(WITH_SHAPE_DETECTION)
MESSAGE(STATUS "Without shape detection")
ENDIF(WITH_SHAPE_DETECTION)
## Interior reconstruction
OPT_DEP(WITH_MODEL "Whether to build model executable for modelling interior environments. ON/OFF" OFF "")
if(WITH_MODEL)
message(STATUS "With interior reconstruction")
else(WITH_MODEL)
message(STATUS "Without interior reconstruction")
endif(WITH_MODEL)
## Thermo
OPT_DEP(WITH_THERMO "Whether to build executables for mutual calibration of laser scanner and camera. ON/OFF" OFF "WITH_SHAPE_DETECTION")
IF(WITH_THERMO)
#for OpenCV 2.1
FIND_PACKAGE(OpenCV REQUIRED)
include("3rdparty/CMakeModules/OpenCV.cmake")
SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${ADDITIONAL_OPENCV_FLAGS}")
add_subdirectory(3rdparty/cvblob)
include_directories(${CMAKE_SOURCE_DIR}/3rdparty/cvblob)
link_directories(${CMAKE_SOURCE_DIR}/3rdparty/cvblob)
MESSAGE(STATUS "With thermo")
ELSE(WITH_THERMO)
MESSAGE(STATUS "Without thermo")
ENDIF(WITH_THERMO)
## Octree
OPT_DEP(WITH_COMPACT_OCTREE "Whether to use the compact octree display ON/OFF" OFF "")
IF(WITH_COMPACT_OCTREE)
MESSAGE(STATUS "Using compact octrees")
SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DUSE_COMPACT_TREE")
ELSE(WITH_COMPACT_OCTREE)
MESSAGE(STATUS "Not using compact octreees: Warning uses more memory")
ENDIF(WITH_COMPACT_OCTREE)
## Glee?
OPT_DEP(WITH_GLEE "Whether to use OpenGL extensions, requires glee. ON/OFF" OFF "")
IF(WITH_GLEE)
MESSAGE(STATUS "Using opengl extensions")
SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DWITH_GLEE")
ELSE(WITH_GLEE)
MESSAGE(STATUS "Not using opengl extensions")
ENDIF(WITH_GLEE)
## Gridder
OPT_DEP(WITH_GRIDDER "Whether to build the 2DGridder binary ON/OFF" OFF "")
IF(WITH_GRIDDER)
MESSAGE(STATUS "With 2DGridder")
ELSE(WITH_GRIDDER)
MESSAGE(STATUS "Without 2DGridder")
ENDIF(WITH_GRIDDER)
## Dynamic VELOSLAM
OPT_DEP(WITH_VELOSLAM "Whether to build the Velodyne data processing (veloslam/veloshow) ON/OFF" OFF "WITH_SHOW")
IF(WITH_VELOSLAM)
MESSAGE(STATUS "With VELOSLAM")
ELSE(WITH_VELOSLAM)
MESSAGE(STATUS "Without VELOSLAM")
ENDIF(WITH_VELOSLAM)
## Home-made Laserscanner
OPT_DEP(WITH_DAVID_3D_SCANNER "Whether to build the David scanner app for homemade laser scanners binary ON/OFF" OFF "")
IF(WITH_DAVID_3D_SCANNER)
MESSAGE(STATUS "With David scanner")
ELSE(WITH_DAVID_3D_SCANNER)
MESSAGE(STATUS "Without David scanner")
ENDIF(WITH_DAVID_3D_SCANNER)
## Tools
OPT_DEP(WITH_TOOLS "Whether to build additional tools like convergence frame_to_graph etc. ON/OFF" OFF "WITH_FBR")
IF(WITH_TOOLS)
MESSAGE(STATUS "With Tools")
find_package (Boost COMPONENTS program_options REQUIRED)
ELSE(WITH_TOOLS)
MESSAGE(STATUS "Without Tools")
ENDIF(WITH_TOOLS)
## Segmentation
OPT_DEP(WITH_SEGMENTATION "Whether to build scan segmantion program ON/OFF" OFF "WITH_FBR")
IF(WITH_SEGMENTATION)
MESSAGE(STATUS "With segmentation")
find_package (Boost COMPONENTS program_options REQUIRED)
ELSE(WITH_SEGMENTATION)
MESSAGE(STATUS "Without segmentation")
ENDIF(WITH_SEGMENTATION)
## Normals
OPT_DEP(WITH_NORMALS "Whether to build separate program for normal computation ON/OFF" OFF "WITH_FBR")
IF(WITH_NORMALS)
MESSAGE(STATUS "With normals")
ELSE(WITH_NORMALS)
MESSAGE(STATUS "Without normals")
ENDIF(WITH_NORMALS)
## RivLib
OPT_DEP(WITH_RIVLIB "Whether the RIEGL rivlib is present ON/OFF" OFF "")
IF(WITH_RIVLIB)
MESSAGE(STATUS "Compiling a scan IO for RXP files")
include_directories(${CMAKE_SOURCE_DIR}/3rdparty)
link_directories(${CMAKE_SOURCE_DIR}/3rdparty)
SET(RIEGL_DIR ${CMAKE_SOURCE_DIR}/3rdparty/riegl/)
IF(WIN32)
SET(RIVLIB ${RIEGL_DIR}libscanlib-mt.lib ${RIEGL_DIR}libctrllib-mt.lib ${RIEGL_DIR}libboost_system-mt-1_43_0-vns.lib)
ELSE(WIN32)
SET(RIVLIB ${RIEGL_DIR}libscanlib-mt-s.a ${RIEGL_DIR}libctrllib-mt-s.a ${RIEGL_DIR}libboost_system-mt-s-1_43_0-vns.a pthread)
ENDIF(WIN32)
FIND_PACKAGE(LibXml2 )
ELSE(WITH_RIVLIB)
MESSAGE(STATUS "Do NOT compile a scan IO for RXP")
ENDIF(WITH_RIVLIB)
## CUDA support, TODO depend on CUDA_FIND
OPT_DEP(WITH_CUDA "Compile with CUDA support" OFF "")
IF(WITH_CUDA)
MESSAGE(STATUS "Compiling WITH CUDA support")
FIND_PACKAGE(CUDA)
SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DWITH_CUDA")
ELSE(WITH_CUDA)
MESSAGE(STATUS "Compiling WITHOUT CUDA support")
ENDIF(WITH_CUDA)
## PMD
OPT_DEP(WITH_PMD "Whether to build the PMD tools like grabVideoAnd3D calibrate etc. ON/OFF" OFF "")
IF(WITH_PMD)
FIND_PACKAGE(OpenGL REQUIRED)
MESSAGE(STATUS "With Tools")
ELSE(WITH_PMD)
MESSAGE(STATUS "Without Tools")
ENDIF(WITH_PMD)
## FBR
OPT_DEP(WITH_FBR "Whether to compile feature based registration. ON/OFF" OFF "")
IF(WITH_FBR)
FIND_PACKAGE(OpenCV REQUIRED)
include("3rdparty/CMakeModules/OpenCV.cmake")
SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${ADDITIONAL_OPENCV_FLAGS}")
MESSAGE(STATUS "With FBR ")
ELSE(WITH_FBR)
MESSAGE(STATUS "Without FBR")
ENDIF(WITH_FBR)
# OPEN
FIND_PACKAGE(OpenMP)
IF(OPENMP_FOUND)
OPT_DEP(WITH_OPENMP "Whether to use parallel processing capabilities of OPENMP. ON/OFF" ON "")
ENDIF(OPENMP_FOUND)
IF(OPENMP_FOUND AND WITH_OPENMP)
MESSAGE(STATUS "With OpenMP ")
SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DMAX_OPENMP_NUM_THREADS=${NUMBER_OF_CPUS} -DOPENMP_NUM_THREADS=${NUMBER_OF_CPUS} ${OpenMP_CXX_FLAGS} -DOPENMP")
ELSE(OPENMP_FOUND AND WITH_OPENMP)
MESSAGE(STATUS "Without OpenMP")
SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DMAX_OPENMP_NUM_THREADS=1 -DOPENMP_NUM_THREADS=1")
ENDIF(OPENMP_FOUND AND WITH_OPENMP)
OPT_DEP(EXPORT_SHARED_LIBS "Whether to build additional shared libraries for use in other projects. ON/OFF" OFF "WITH_SHOW;WITH_FBR")
IF(EXPORT_SHARED_LIBS)
## Compile a single shared library containing all of 3DTK
add_library(slam SHARED src/slam6d/icp6D.cc)
target_link_libraries(slam scan_s ANN_s sparse_s newmat_s show_s fbr_s)
MESSAGE(STATUS "exporting additional libraries")
ELSE(EXPORT_SHARED_LIBS)
MESSAGE(STATUS "not exporting libraries")
ENDIF(EXPORT_SHARED_LIBS)
OPT_DEP(WITH_METRICS "Whether to use metrics in slam6d. ON/OFF" OFF "")
IF(WITH_METRICS)
MESSAGE(STATUS "With metrics in slam6d.")
SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DWITH_METRICS")
ELSE(WITH_METRICS)
MESSAGE(STATUS "Without metrics in slam6d.")
ENDIF(WITH_METRICS)
#################################################
# OPERATING SYSTEM SPECIFIC BEHAVIOUR ##
#################################################
## Special treatment for system specifics
IF(APPLE)
add_definitions(-Dfopen64=fopen)
ENDIF(APPLE)
## Multiple Cores
IF(APPLE)
SET(PROCESSOR_COUNT 2)
ELSE(APPLE)
INCLUDE(CountProcessors)
SET(NUMBER_OF_CPUS "${PROCESSOR_COUNT}" CACHE STRING "The number of processors to use (default: ${PROCESSOR_COUNT})" )
ENDIF(APPLE)
IF(WIN32)
SET(ADDITIONAL_CFLAGS "-O2" CACHE STRING "Additional flags given to the compiler ( -O2)" )
include_directories(${CMAKE_SOURCE_DIR}/3rdparty/windows/)
link_directories(${CMAKE_SOURCE_DIR}/3rdparty/windows)
link_directories(${CMAKE_SOURCE_DIR}/3rdparty/windows/x64)
add_library(XGetopt STATIC ${CMAKE_SOURCE_DIR}/3rdparty/windows/XGetopt.cpp)
SET(CMAKE_STATIC_LIBRARY_SUFFIX "32.lib")
ELSE(WIN32)
SET(ADDITIONAL_CFLAGS "-O3 -std=c++0x -msse3 -Wall -finline-functions -Wno-unused-but-set-variable -Wno-write-strings -Wno-char-subscripts -Wno-unused-result" CACHE STRING"Additional flags given to the compiler (-O3 -Wall -finline-functions -Wno-write-strings)" )
# Add include path for OpenGL without GL/-prefix
# to avoid the include incompatibility between MACOS
# and linux
FIND_PATH(OPENGL_INC gl.h /usr/include/GL)
include_directories(${OPENGL_INC})
ENDIF(WIN32)
# Add OpenGL includes for MACOS if needed
# The OSX OpenGL frameworks natively supports freeglut extensions
IF(APPLE)
include_directories(/System/Library/Frameworks/GLUT.framework/Headers)
include_directories(/System/Library/Frameworks/OpenGL.framework/Headers)
ENDIF(APPLE)
# hack to "circumvent" Debug and Release folders that are created under visual studio
# this is why the INSTALL target has to be used in visual studio
IF(MSVC)
INSTALL(DIRECTORY ${CMAKE_SOURCE_DIR}/bin/Release/ CONFIGURATIONS Release DESTINATION ${CMAKE_SOURCE_DIR}/windows FILES_MATCHING PATTERN "*.dll" PATTERN "*.exe")
IF( CMAKE_SIZEOF_VOID_P EQUAL 8 )
INSTALL(DIRECTORY ${CMAKE_SOURCE_DIR}/3rdparty/windows/x64/ CONFIGURATIONS Release DESTINATION ${CMAKE_SOURCE_DIR}/windows FILES_MATCHING PATTERN "*.dll" PATTERN "*.exe")
ELSE( CMAKE_SIZEOF_VOID_P EQUAL 8 )
INSTALL(DIRECTORY ${CMAKE_SOURCE_DIR}/3rdparty/windows/ CONFIGURATIONS Release DESTINATION ${CMAKE_SOURCE_DIR}/windows FILES_MATCHING PATTERN "*.dll" PATTERN "*.exe")
ENDIF(CMAKE_SIZEOF_VOID_P EQUAL 8 )
INSTALL(DIRECTORY ${CMAKE_SOURCE_DIR}/bin/Debug/ CONFIGURATIONS Debug DESTINATION ${CMAKE_SOURCE_DIR}/windows FILES_MATCHING PATTERN "*.dll" PATTERN "*.exe")
IF( CMAKE_SIZEOF_VOID_P EQUAL 8 )
INSTALL(DIRECTORY ${CMAKE_SOURCE_DIR}/3rdparty/windows/x64/ CONFIGURATIONS Debug DESTINATION ${CMAKE_SOURCE_DIR}/windows FILES_MATCHING PATTERN "*.dll" PATTERN "*.exe")
ELSE( CMAKE_SIZEOF_VOID_P EQUAL 8 )
INSTALL(DIRECTORY ${CMAKE_SOURCE_DIR}/3rdparty/windows/ CONFIGURATIONS Debug DESTINATION ${CMAKE_SOURCE_DIR}/windows FILES_MATCHING PATTERN "*.dll" PATTERN "*.exe")
ENDIF(CMAKE_SIZEOF_VOID_P EQUAL 8 )
ENDIF(MSVC)
#################################################
# GENERAL PROJECT SETTINGS ##
#################################################
SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${ADDITIONAL_CFLAGS}")
# Hide CMake variables
SET (CMAKE_INSTALL_PREFIX "/usr/local" CACHE INTERNAL "" FORCE)
SET (CMAKE_BUILD_TYPE "" CACHE INTERNAL "" FORCE)
# Set output directories for libraries and executables
SET( CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/lib )
SET( CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/obj )
SET( CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/bin )
# Set include and link dirs ...
include_directories(${CMAKE_SOURCE_DIR}/include)
include_directories(${CMAKE_SOURCE_DIR}/3rdparty/)
include_directories(${CMAKE_SOURCE_DIR}/3rdparty/glui)
include_directories(${CMAKE_SOURCE_DIR}/3rdparty/wxthings/include/)
include_directories(${CMAKE_SOURCE_DIR}/3rdparty/ann_1.1.1_modified/include)
include_directories(${CMAKE_SOURCE_DIR}/3rdparty/ann_1.1.1_modified/src)
link_directories(${CMAKE_SOURCE_DIR}/obj)
link_directories(${CMAKE_SOURCE_DIR}/lib)
add_subdirectory(3rdparty)
add_subdirectory(src/slam6d)
add_subdirectory(src/scanio)
add_subdirectory(src/scanserver)
add_subdirectory(src/segmentation)
add_subdirectory(src/normals)
add_subdirectory(src/veloslam)
add_subdirectory(src/show)
add_subdirectory(src/grid)
add_subdirectory(src/pmd)
add_subdirectory(src/shapes)
add_subdirectory(src/thermo)
add_subdirectory(src/slam6d/fbr)
add_subdirectory(src/scanner)
add_subdirectory(src/model)
MESSAGE (STATUS "Build environment is set up!")

File diff suppressed because it is too large Load diff

View file

@ -0,0 +1,219 @@
/*
* scan_io_riegl_txt implementation
*
* Copyright (C) Thomas Escher, Kai Lingemann, Andreas Nuechter
*
* Released under the GPL version 3.
*
*/
/**
* @file
* @brief Implementation of reading 3D scans
* @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Thomas Escher
*/
#include "scanio/scan_io_riegl_txt.h"
#include <iostream>
using std::cout;
using std::cerr;
using std::endl;
#include <vector>
#ifdef _MSC_VER
#include <windows.h>
#endif
#include <boost/filesystem/operations.hpp>
#include <boost/filesystem/fstream.hpp>
using namespace boost::filesystem;
#include "slam6d/globals.icc"
#define DATA_PATH_PREFIX "scan"
#define DATA_PATH_SUFFIX ".txt"
#define POSE_PATH_PREFIX "scan"
#define POSE_PATH_SUFFIX ".dat"
std::list<std::string> ScanIO_riegl_txt::readDirectory(const char* dir_path, unsigned int start, unsigned int end)
{
std::list<std::string> identifiers;
for(unsigned int i = start; i <= end; ++i) {
// identifier is /d/d/d (000-999)
std::string identifier(to_string(i,3));
// scan consists of data (.3d) and pose (.pose) files
path data(dir_path);
data /= path(std::string(DATA_PATH_PREFIX) + identifier + DATA_PATH_SUFFIX);
path pose(dir_path);
pose /= path(std::string(POSE_PATH_PREFIX) + identifier + POSE_PATH_SUFFIX);
// stop if part of a scan is missing or end by absence is detected
if(!exists(data) || !exists(pose))
break;
identifiers.push_back(identifier);
}
return identifiers;
}
void ScanIO_riegl_txt::readPose(const char* dir_path, const char* identifier, double* pose)
{
unsigned int i;
path pose_path(dir_path);
pose_path /= path(std::string(POSE_PATH_PREFIX) + identifier + POSE_PATH_SUFFIX);
if(!exists(pose_path))
throw std::runtime_error(std::string("There is no pose file for [") + identifier + "] in [" + dir_path + "]");
// open pose file
ifstream pose_file(pose_path);
// if the file is open, read contents
if(pose_file.good()) {
double rPos[3], rPosTheta[16];
double inMatrix[16], tMatrix[16];
for (i = 0; i < 16; ++i)
pose_file >> inMatrix[i];
pose_file.close();
// transform input pose
tMatrix[0] = inMatrix[5];
tMatrix[1] = -inMatrix[9];
tMatrix[2] = -inMatrix[1];
tMatrix[3] = -inMatrix[13];
tMatrix[4] = -inMatrix[6];
tMatrix[5] = inMatrix[10];
tMatrix[6] = inMatrix[2];
tMatrix[7] = inMatrix[14];
tMatrix[8] = -inMatrix[4];
tMatrix[9] = inMatrix[8];
tMatrix[10] = inMatrix[0];
tMatrix[11] = inMatrix[12];
tMatrix[12] = -inMatrix[7];
tMatrix[13] = inMatrix[11];
tMatrix[14] = inMatrix[3];
tMatrix[15] = inMatrix[15];
Matrix4ToEuler(tMatrix, rPosTheta, rPos);
pose[0] = 100*rPos[0];
pose[1] = 100*rPos[1];
pose[2] = 100*rPos[2];
pose[3] = rPosTheta[0];
pose[4] = rPosTheta[1];
pose[5] = rPosTheta[2];
} else {
throw std::runtime_error(std::string("Pose file could not be opened for [") + identifier + "] in [" + dir_path + "]");
}
}
bool ScanIO_riegl_txt::supports(IODataType type)
{
return !!(type & (DATA_XYZ | DATA_REFLECTANCE));
}
void ScanIO_riegl_txt::readScan(const char* dir_path, const char* identifier, PointFilter& filter, std::vector<double>* xyz, std::vector<unsigned char>* rgb, std::vector<float>* reflectance, std::vector<float>* temperature, std::vector<float>* amplitude, std::vector<int>* type, std::vector<float>* deviation)
{
unsigned int i;
// error handling
path data_path(dir_path);
data_path /= path(std::string(DATA_PATH_PREFIX) + identifier + DATA_PATH_SUFFIX);
if(!exists(data_path))
throw std::runtime_error(std::string("There is no scan file for [") + identifier + "] in [" + dir_path + "]");
if(xyz != 0 || reflectance != 0) {
// open data file
ifstream data_file(data_path);
data_file.exceptions(ifstream::eofbit|ifstream::failbit|ifstream::badbit);
// read the point count
unsigned int count;
data_file >> count;
// reserve enough space for faster reading
xyz->reserve(3*count);
// read points
// z x y range theta phi reflectance
double point[7];
double tmp;
while(data_file.good()) {
try {
for(i = 0; i < 7; ++i) data_file >> point[i];
} catch(std::ios_base::failure& e) {
break;
}
// the enemy's x/y/z is mapped to slam's z/x/y, shuffle time!
// invert x axis
// convert coordinate to cm
tmp = point[2];
point[2] = 100.0 * point[0];
point[0] = -100.0 * point[1];
point[1] = 100.0 * tmp;
// apply filter and insert point
if(filter.check(point)) {
if(xyz != 0) {
for(i = 0; i < 3; ++i) xyz->push_back(point[i]);
}
if(reflectance != 0) {
double r = point[6];
r += 32;
r /= 64;
r -= 0.2;
r /= 0.3;
if (r < 0) r = 0;
if (r > 1) r = 1;
reflectance->push_back(r);
}
}
}
data_file.close();
}
}
/**
* class factory for object construction
*
* @return Pointer to new object
*/
#ifdef _MSC_VER
extern "C" __declspec(dllexport) ScanIO* create()
#else
extern "C" ScanIO* create()
#endif
{
return new ScanIO_riegl_txt;
}
/**
* class factory for object construction
*
* @return Pointer to new object
*/
#ifdef _MSC_VER
extern "C" __declspec(dllexport) void destroy(ScanIO *sio)
#else
extern "C" void destroy(ScanIO *sio)
#endif
{
delete sio;
}
#ifdef _MSC_VER
BOOL APIENTRY DllMain(HANDLE hModule, DWORD dwReason, LPVOID lpReserved)
{
return TRUE;
}
#endif

View file

@ -0,0 +1,99 @@
#ifndef __SCANCOLORMANAGER_H__
#define __SCANCOLORMANAGER_H__
#ifdef _MSC_VER
#define _USE_MATH_DEFINES
#include <windows.h>
#endif
#ifdef __APPLE__
#include <GLUT/glut.h>
#else
#include <GL/glut.h>
#endif
#include "slam6d/point.h"
#include "slam6d/scan.h"
#include "show/colordisplay.h"
//#include "show/show_Boctree.h"
#include "show/colormanager.h"
#include <vector>
#include <float.h>
#include "slam6d/point_type.h"
using std::vector;
//template <class T> class Show_BOctTree;
/**
* This class is a special ColorManager that handles a set of Colormanagers.
* This manager is capable of mapping managers to scans in dependence of the state of the
* show program.
*/
class ScanColorManager {
public:
static const unsigned int MODE_STATIC;
static const unsigned int MODE_COLOR_SCAN;
static const unsigned int MODE_ANIMATION;
static const unsigned int MODE_POINT_COLOR;
ScanColorManager(unsigned int _buckets, PointType type, bool animation_color = true);
void registerTree(colordisplay *b);
void setColorMap(ColorMap &cm);
void setColorMap(ColorMap::CM &cm);
void setCurrentType(unsigned int type);
void setMinMax(float min, float max);
void setMode(const unsigned int &mode);
void setInvert(bool invert);
float getMin();
float getMax();
float getMin(unsigned int dim);
float getMax(unsigned int dim);
unsigned int getPointDim();
void makeValid();
void selectColors(Scan::AlgoType type);
template<class P>
void updateRanges(P *point);
protected:
unsigned int currentdim;
vector<colordisplay *> allScans;
vector<ColorManager *> allManager;
vector<ColorManager *> staticManager;
vector<ColorManager *> scanManager;
vector<CColorManager *> colorsManager;
unsigned int currenttype;
unsigned int buckets;
/** stores minima and maxima for each point dimension */
float *mins;
float *maxs;
/** maps valuetypes to point dimension for easier access */
PointType pointtype;
bool animationColor; /**< Alter colors when animating */
bool valid;
bool colorScans;
bool inverted;
};
template<class P>
void ScanColorManager::updateRanges(P *point) {
for (unsigned int i = 0; i < pointtype.getPointDim(); i++) {
if (point[i] < mins[i]) mins[i] = point[i];
if (point[i] > maxs[i]) maxs[i] = point[i];
}
}
#endif

View file

@ -0,0 +1,227 @@
/**
* @file
* @brief Basic DataPointer class and its derivates SingleArray and TripleArray
*
* This file contains several classes for array-like access. The SingleArray
* and TripleArray classes and their typedefs to DataXYZ/... overload
* the operator[] and have a size function to act as their native arrays.
* Similar to the array classes, SingleObject represents a whole object with
* all its members in that allocated space.
*
* If an array of pointers to the elements of a TripleArray is required it can
* create a temporary class PointerArray which holds creates and deletes a
* native pointer array and follows the RAII-pattern.
*/
#ifndef DATA_TYPES_H
#define DATA_TYPES_H
/**
* Representation of a pointer to a data field with no access methods.
*
* Type-specialized access is gained by deriving from this class and
* implementing access functions like operator[] and size().
*
* The PrivateImplementation feature enables RAII-type locking mechanisms
* used in the scanserver for holding CacheObject-locks. It is protected so
* that scanserver-unaware code can only construct this class with a pointer
* and size. Initialization of a derived class with these locking mechanisms
* creates this class with the private implementation value, which will be
* deleted in this dtor when it completely falls out of scope.
*/
class DataPointer {
protected:
//! Subclass for storing further members and attaching an overloadable dtor
class PrivateImplementation {
public:
virtual ~PrivateImplementation() {}
};
public:
// DataPointer& operator=(const DataPointer&) = delete;
// DataPointer(const DataPointer&) = delete;
/**
* Ctor for the initial creation
*
* @param pointer base pointer to the data
* @param size of the pointed data in bytes
*/
DataPointer(unsigned char* pointer, unsigned int size,
PrivateImplementation* private_impl = 0) :
m_pointer(pointer), m_size(size), m_private_impl(private_impl) {
}
/**
* Copy-Ctor for passing along via return by value
*
* The type-specialized classes (B) will be called with their
* B(DataPointer&&) temporary ctor and call this constructor, so the private
* imlementation has to be taken away. The temporary inside that constructor
* isn't seen as temporary anymore, so we need a simple reference-ctor.
*/
DataPointer(DataPointer& other) {
m_pointer = other.m_pointer;
m_size = other.m_size;
// take ownership of this value, other is a temporary and will deconstruct
m_private_impl = other.m_private_impl;
other.m_private_impl = 0;
};
/**
* Same as DataPointer(DataPointer&), except this is for functions returning
* DataPointer instead of derived classes, so the temporary-ctor is used.
*/
DataPointer(DataPointer&& other) {
m_pointer = other.m_pointer;
m_size = other.m_size;
// take ownership of this value, other is a temporary and will deconstruct
m_private_impl = other.m_private_impl;
other.m_private_impl = 0;
}
//! Delete the private implementation with its derived dtor
~DataPointer() {
if(m_private_impl != 0)
delete m_private_impl;
}
//! Indicator for nullpointer / no data contained if false
inline bool valid() {
return m_size != 0;
}
inline unsigned char* get_raw_pointer() const { return m_pointer; }
protected:
unsigned char* m_pointer;
unsigned int m_size;
private:
PrivateImplementation* m_private_impl;
};
template<typename T>
class SingleArray : public DataPointer {
public:
//! Cast return-by-value temporary DataPointer to this type of array
SingleArray(DataPointer&& temp) :
DataPointer(temp)
{
}
SingleArray(SingleArray&& temp) :
DataPointer(temp)
{
}
//! Represent the pointer as an array of T
inline T& operator[](unsigned int i) const
{
return *(reinterpret_cast<T*>(m_pointer) + i);
}
//! The number of T instances in this array
unsigned int size() {
return m_size / sizeof(T);
}
};
template<typename T>
class TripleArray : public DataPointer {
public:
//! Cast return-by-value temporary DataPointer to this type of array
TripleArray(DataPointer&& temp) :
DataPointer(temp)
{
}
TripleArray(TripleArray&& temp) :
DataPointer(temp)
{
}
//! Represent the pointer as an array of T[3]
inline T* operator[](unsigned int i) const
{
return reinterpret_cast<T*>(m_pointer) + (i*3);
}
//! The number of T[3] instances in this array
unsigned int size() const {
return m_size / (3 * sizeof(T));
}
};
template<typename T>
class SingleObject : public DataPointer {
public:
//! Cast return-by-value temporary DataPointer to this type of object
SingleObject(DataPointer&& temp) :
DataPointer(temp)
{
}
SingleObject(SingleObject&& temp) :
DataPointer(temp)
{
}
//! Type-cast
inline T& get() const
{
return *reinterpret_cast<T*>(m_pointer);
}
//! There is only one object in here
unsigned int size() const {
return 1;
}
};
/**
* To simplify T** access patterns for an array of T[3] (points), this RAII-
* type class helps creating and managing this pointer array on the stack.
*/
template<typename T>
class PointerArray {
public:
//! Create a temporary array and fill it sequentially with pointers to points
PointerArray(const TripleArray<T>& data) {
unsigned int size = data.size();
m_array = new T*[size];
for(unsigned int i = 0; i < size; ++i)
m_array[i] = data[i];
}
//! Removes the temporary array on destruction (RAII)
~PointerArray() {
delete[] m_array;
}
//! Conversion operator to interface the TripleArray to a T** array
inline T** get() const { return m_array; }
private:
T** m_array;
};
// TODO: naming, see scan.h
typedef TripleArray<double> DataXYZ;
typedef TripleArray<float> DataXYZFloat;
typedef TripleArray<unsigned char> DataRGB;
typedef TripleArray<double> DataNormal;
typedef SingleArray<float> DataReflectance;
typedef SingleArray<float> DataTemperature;
typedef SingleArray<float> DataAmplitude;
typedef SingleArray<int> DataType;
typedef SingleArray<float> DataDeviation;
#endif //DATA_TYPES_H

View file

@ -0,0 +1,118 @@
### TOOLS
IF(WITH_TOOLS)
FIND_PACKAGE(OpenCV REQUIRED)
### SCAN_RED
add_executable(scan_red scan_red.cc fbr/fbr_global.cc fbr/panorama.cc fbr/scan_cv.cc)
IF(UNIX)
target_link_libraries(scan_red scan dl ANN fbr_cv_io fbr_panorama ${OpenCV_LIBS} ${Boost_LIBRARIES})
ENDIF(UNIX)
IF (WIN32)
target_link_libraries(scan_red scan ANN XGetopt)
ENDIF(WIN32)
### SCAN_DIFF
add_executable(scan_diff scan_diff.cc)
# add_executable(scan_diff2d scan_diff2d.cc ../show/colormanager.cc)
IF(UNIX)
target_link_libraries(scan_diff scan dl ANN)
# target_link_libraries(scan_diff2d scan dl ANN)
ENDIF(UNIX)
IF (WIN32)
target_link_libraries(scan_diff scan ANN XGetopt)
# target_link_libraries(scan_diff2d scan ANN XGetopt)
ENDIF(WIN32)
add_executable(frame_to_graph frame_to_graph.cc)
add_executable(convergence convergence.cc)
add_executable(graph_balancer graph_balancer.cc)
add_executable(exportPoints exportPoints.cc)
add_executable(frames2riegl frames2riegl.cc)
add_executable(frames2pose frames2pose.cc)
add_executable(pose2frames pose2frames.cc)
add_executable(riegl2frames riegl2frames.cc)
add_executable(toGlobal toGlobal.cc)
IF(UNIX)
target_link_libraries(graph_balancer scan ${Boost_GRAPH_LIBRARY} ${Boost_SERIALIZATION_LIBRARY} ${Boost_REGEX_LIBRARY})
target_link_libraries(exportPoints scan dl ANN)
target_link_libraries(toGlobal scan)
ENDIF(UNIX)
IF (WIN32)
target_link_libraries(frame_to_graph XGetopt ${Boost_LIBRARIES})
target_link_libraries(convergence XGetopt ${Boost_LIBRARIES})
target_link_libraries(graph_balancer scan XGetopt ${Boost_LIBRARIES})
target_link_libraries(exportPoints scan ANN XGetopt ${Boost_LIBRARIES})
target_link_libraries(frames2pose XGetopt ${Boost_LIBRARIES})
target_link_libraries(pose2frames XGetopt ${Boost_LIBRARIES})
target_link_libraries(frames2riegl XGetopt ${Boost_LIBRARIES})
target_link_libraries(riegl2frames XGetopt ${Boost_LIBRARIES})
target_link_libraries(toGlobal XGetopt ${Boost_LIBRARIES})
ENDIF(WIN32)
ENDIF(WITH_TOOLS)
### SCANLIB
SET(SCANLIB_SRCS
kd.cc kdManaged.cc kdMeta.cc graphSlam6D.cc
graph.cc icp6Dapx.cc icp6D.cc icp6Dsvd.cc
icp6Dortho.cc icp6Dquat.cc icp6Dhelix.cc icp6Dlumeuler.cc
icp6Dlumquat.cc icp6Ddual.cc lum6Deuler.cc lum6Dquat.cc
ghelix6DQ2.cc gapx6D.cc graphToro.cc ann_kd.cc
graphHOG-Man.cc elch6D.cc elch6Dquat.cc elch6DunitQuat.cc
elch6Dslerp.cc elch6Deuler.cc loopToro.cc loopHOG-Man.cc
point_type.cc icp6Dquatscale.cc searchTree.cc Boctree.cc
allocator.cc
scan.cc basicScan.cc managedScan.cc metaScan.cc
io_types.cc io_utils.cc pointfilter.cc
)
if(WITH_METRICS)
set(SCANLIB_SRCS ${SCANLIB_SRCS} metrics.cc)
endif(WITH_METRICS)
add_library(scan STATIC ${SCANLIB_SRCS})
target_link_libraries(scan scanclient scanio)
IF(UNIX)
target_link_libraries(scan dl)
ENDIF(UNIX)
### EXPORT SHARED LIBS
IF(EXPORT_SHARED_LIBS)
add_library(scan_s SHARED ${SCANLIB_SRCS})
#target_link_libraries(scan_s ${Boost_LIBRARIES} newmat)
target_link_libraries(scan_s newmat sparse ANN scanclient pointfilter scanio)
ENDIF(EXPORT_SHARED_LIBS)
### SLAM6D
IF(WITH_CUDA)
CUDA_COMPILE(CUDA_FILES cuda/CIcpGpuCuda.cu )
add_executable(slam6D slam6D.cc cuda/icp6Dcuda.cc ${CUDA_FILES})
target_link_libraries(slam6D ${CUDA_LIBRARIES} ANN cudpp64)
CUDA_ADD_CUBLAS_TO_TARGET(slam6D)
CUDA_ADD_CUTIL_TO_TARGET(slam6D)
ELSE(WITH_CUDA)
add_executable(slam6D slam6D.cc)
ENDIF(WITH_CUDA)
IF(UNIX)
target_link_libraries(slam6D scan newmat sparse ANN)
ENDIF(UNIX)
IF(WIN32)
target_link_libraries(slam6D scan newmat sparse ANN XGetopt ${Boost_LIBRARIES})
ENDIF(WIN32)
#IF(MSVC)
# INSTALL(TARGETS slam6D RUNTIME DESTINATION ${CMAKE_SOURCE_DIR}/windows)
#ENDIF(MSVC)

View file

@ -0,0 +1,73 @@
#ifndef BASIC_SCAN_H
#define BASIC_SCAN_H
#include "scan.h"
#include "frame.h"
#include <vector>
#include <map>
class BasicScan : public Scan {
public:
BasicScan() {};
static void openDirectory(const std::string& path, IOType type, int start, int end);
static void closeDirectory();
/*
Scan(const double *euler, int maxDist = -1);
Scan(const double rPos[3], const double rPosTheta[3], int maxDist = -1);
Scan(const double _rPos[3], const double _rPosTheta[3], vector<double *> &pts);
*/
virtual ~BasicScan();
virtual void setRangeFilter(double max, double min);
virtual void setHeightFilter(double top, double bottom);
virtual void setRangeMutation(double range);
virtual const char* getIdentifier() const { return m_identifier.c_str(); }
virtual DataPointer get(const std::string& identifier);
virtual void get(unsigned int types);
virtual DataPointer create(const std::string& identifier, unsigned int size);
virtual void clear(const std::string& identifier);
virtual unsigned int readFrames();
virtual void saveFrames();
virtual unsigned int getFrameCount();
virtual void getFrame(unsigned int i, const double*& pose_matrix, AlgoType& type);
//! Constructor for creation of Scans without openDirectory
BasicScan(double * rPos, double * rPosTheta, std::vector<double*> points);
protected:
virtual void createSearchTreePrivate();
virtual void calcReducedOnDemandPrivate();
virtual void calcNormalsOnDemandPrivate();
virtual void addFrame(AlgoType type);
private:
//! Path and identifier of where the scan is located
std::string m_path, m_identifier;
IOType m_type;
double m_filter_max, m_filter_min, m_filter_top, m_filter_bottom, m_range_mutation;
bool m_filter_range_set, m_filter_height_set, m_range_mutation_set;
std::map<std::string, std::pair<unsigned char*, unsigned int>> m_data;
std::vector<Frame> m_frames;
//! Constructor for openDirectory
BasicScan(const std::string& path, const std::string& identifier, IOType type);
//! Initialization function
void init();
void createANNTree();
void createOcttree();
};
#endif //BASIC_SCAN_H

View file

@ -0,0 +1,273 @@
/**
* @file
* @brief Representation of a 3D point type
* @author Jan Elsberg. Automation Group, Jacobs University Bremen gGmbH, Germany.
*/
#ifndef __POINT_TYPE_H__
#define __POINT_TYPE_H__
#include "point.h"
#include "data_types.h"
#include <string>
using std::string;
#include <iostream>
using std::cout;
using std::cerr;
using std::endl;
#include <iostream>
#include <fstream>
#include <string.h>
class Scan;
class PointType {
public:
static const unsigned int USE_NONE;
static const unsigned int USE_REFLECTANCE;
static const unsigned int USE_NORMAL;
static const unsigned int USE_TEMPERATURE;
static const unsigned int USE_AMPLITUDE;
static const unsigned int USE_DEVIATION;
static const unsigned int USE_HEIGHT;
static const unsigned int USE_TYPE;
static const unsigned int USE_COLOR;
static const unsigned int USE_TIME;
static const unsigned int USE_INDEX;
PointType();
PointType(unsigned int _types);
bool hasReflectance();
bool hasNormal();
bool hasTemperature();
bool hasAmplitude();
bool hasDeviation();
bool hasType();
bool hasColor();
bool hasTime();
bool hasIndex();
unsigned int getReflectance();
unsigned int getTemperature();
unsigned int getAmplitude();
unsigned int getDeviation();
unsigned int getTime();
unsigned int getIndex();
unsigned int getType();
unsigned int getType(unsigned int type);
unsigned int getPointDim();
static PointType deserialize(std::ifstream &f);
void serialize(std::ofstream &f);
unsigned int toFlags() const;
template <class T>
T *createPoint(const Point &P, unsigned int index=0);
template <class T>
Point createPoint(T *p);
//! Aquire DataPointer objects from \a scan, determined by its types
void useScan(Scan* scan);
//! Release the DataPointer objects
void clearScan();
//! Create a point with attributes via the DataPointers from the scan
template<typename T>
T* createPoint(unsigned int i, unsigned int index = 0);
//! Create an array with coordinate+attribute array per point with transfer of ownership
template<typename T>
T** createPointArray(Scan* scan);
private:
/**
* collection of flags
*/
unsigned int types;
/**
* Derived from types: 3 spatial dimensions + 1 for each flag set
**/
unsigned int pointdim;
/**
* Stores the size of each point in bytes
**/
unsigned int pointbytes;
/**
* Derived from types, to map type to the array index for each point
**/
int dimensionmap[9];
bool hasType(unsigned int type);
unsigned int getScanSize(Scan* scan);
DataXYZ* m_xyz;
DataXYZ* m_normal;
DataRGB* m_rgb;
DataReflectance* m_reflectance;
DataTemperature* m_temperature;
DataAmplitude* m_amplitude;
DataType* m_type;
DataDeviation* m_deviation;
};
template <class T>
T *PointType::createPoint(const Point &P, unsigned int index ) {
unsigned int counter = 0;
T *p = new T[pointdim];
p[counter++] = P.x;
p[counter++] = P.y;
p[counter++] = P.z;
if (types & USE_REFLECTANCE) {
p[counter++] = P.reflectance;
}
if (types & USE_NORMAL) {
p[counter++] = P.nx;
p[counter++] = P.ny;
p[counter++] = P.nz;
}
if (types & USE_TEMPERATURE) {
p[counter++] = P.temperature;
}
if (types & USE_AMPLITUDE) {
p[counter++] = P.amplitude;
}
if (types & USE_DEVIATION) {
p[counter++] = P.deviation;
}
if (types & USE_TYPE) {
p[counter++] = P.type;
}
if (types & USE_COLOR) {
memcpy(&p[counter], P.rgb, 3);
counter++;
}
if (types & USE_TIME) {
// p[counter++] = P.timestamp;
}
if (types & USE_INDEX) {
p[counter++] = index;
}
return p;
}
template <class T>
Point PointType::createPoint(T *p) {
Point P;
unsigned int counter = 0;
P.x = p[counter++];
P.y = p[counter++];
P.z = p[counter++];
if (types & USE_REFLECTANCE) {
P.reflectance = p[counter++];
}
if (types & USE_NORMAL) {
p[counter++] = P.nx;
p[counter++] = P.ny;
p[counter++] = P.nz;
}
if (types & USE_TEMPERATURE) {
P.temperature = p[counter++];
}
if (types & USE_AMPLITUDE) {
P.amplitude = p[counter++];
}
if (types & USE_DEVIATION) {
P.deviation = p[counter++];
}
if (types & USE_TYPE) {
P.type = p[counter++];
}
if (types & USE_COLOR) {
memcpy(P.rgb, &p[counter], 3);
counter++;
}
if (types & USE_TIME) {
// P.timestamp = p[counter++];
}
return P;
}
template <class T>
T *PointType::createPoint(unsigned int i, unsigned int index) {
unsigned int counter = 0;
T* p = new T[pointdim];
for(unsigned int j = 0; j < 3; ++j)
p[counter++] = (*m_xyz)[i][j];
// if a type is requested try to write the value if the scan provided one
if (types & USE_REFLECTANCE) {
p[counter++] = (m_reflectance? (*m_reflectance)[i]: 0);
}
if (types & USE_NORMAL) {
for(unsigned int j = 0; j < 3; ++j)
p[counter++] = (*m_normal)[i][j];
}
if (types & USE_TEMPERATURE) {
p[counter++] = (m_temperature? (*m_temperature)[i]: 0);
}
if (types & USE_AMPLITUDE) {
p[counter++] = (m_amplitude? (*m_amplitude)[i]: 0);
}
if (types & USE_DEVIATION) {
p[counter++] = (m_deviation? (*m_deviation)[i]: 0);
}
if (types & USE_TYPE) {
p[counter++] = (m_type? (*m_type)[i]: 0);
}
if (types & USE_COLOR) {
if(m_rgb)
memcpy(&p[counter], (*m_rgb)[i], 3);
else
p[counter] = 0;
counter++;
}
if (types & USE_TIME) {
}
if (types & USE_INDEX) {
p[counter++] = index;
}
return p;
}
template<typename T>
T** PointType::createPointArray(Scan* scan)
{
// access data with prefetching
useScan(scan);
// create a float array with requested attributes by pointtype via createPoint
unsigned int nrpts = getScanSize(scan);
T** pts = new T*[nrpts];
for(unsigned int i = 0; i < nrpts; i++) {
pts[i] = createPoint<T>(i);
}
// unlock access to data, remove unneccessary data fields
clearScan();
return pts;
}
#endif

View file

@ -0,0 +1,433 @@
cmake_minimum_required (VERSION 2.8.2)
SET(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/3rdparty/CMakeModules" ${CMAKE_MODULE_PATH})
project (Slam6D)
#include_directories(OPENGL_INCLUDE_DIR)
IF(WIN32)
set(Boost_USE_STATIC_LIBS TRUE)
ELSE(WIN32)
set(Boost_USE_STATIC_LIBS FALSE)
ENDIF(WIN32)
SET(Boost_ADDITIONAL_VERSIONS "1.42" "1.42.0" "1.44" "1.44.0" "1.45.0" "1.45" "1.46" "1.46.1" "1.47.0" "1.47" "1.48" "1.49")
IF(WIN32)
# for some unknown reason no one variant works on all windows platforms
find_package( Boost COMPONENTS serialization graph regex filesystem system thread date_time REQUIRED)
ELSE(WIN32)
find_package( Boost COMPONENTS serialization graph regex filesystem system thread date_time REQUIRED)
ENDIF(WIN32)
if(Boost_FOUND)
link_directories(${BOOST_LIBRARY_DIRS})
include_directories(${Boost_INCLUDE_DIRS})
add_definitions(${Boost_LIB_DIAGNOSTIC_DEFINITIONS})
endif()
#################################################
# Declare Options and modify build accordingly ##
#################################################
## FreeGLUT
OPTION(WITH_FREEGLUT "Whether freeglut is available. This enables iterative drawing in show. ON/OFF" ON)
IF(WITH_FREEGLUT)
MESSAGE(STATUS "With freeglut")
SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DWITH_FREEGLUT")
ELSE(WITH_FREEGLUT)
MESSAGE(STATUS "Without freeglut")
ENDIF(WITH_FREEGLUT)
## Show
OPTION(WITH_SHOW "Whether to build Show. This is the Visualization program of slam6d. ON/OFF" ON)
IF(WITH_SHOW)
FIND_PACKAGE(OpenGL REQUIRED)
FIND_PACKAGE(GLUT REQUIRED)
MESSAGE(STATUS "With show")
ELSE(WITH_SHOW)
# SET (WITH_OCTREE_DISPLAY "ON" CACHE INTERNAL "" FORCE)
MESSAGE(STATUS "Without show")
ENDIF(WITH_SHOW)
## WXShow
OPTION(WITH_WXSHOW "Whether to build WXShow. This is the wxwidgets variant of Show. ON/OFF" OFF)
IF(WITH_WXSHOW)
FIND_PACKAGE(OpenGL REQUIRED)
FIND_PACKAGE(GLUT REQUIRED)
find_package(wxWidgets COMPONENTS core base gl REQUIRED)
# set wxWidgets_wxrc_EXECUTABLE to be ignored in the configuration
SET (wxWidgets_wxrc_EXECUTABLE " " CACHE INTERNAL "" FORCE)
# wxWidgets include (this will do all the magic to configure everything)
include( ${wxWidgets_USE_FILE})
MESSAGE(STATUS "With wxshow")
ELSE(WITH_XWSHOW)
MESSAGE(STATUS "Without wxshow")
ENDIF(WITH_WXSHOW)
## Shapes
OPTION(WITH_SHAPE_DETECTION "Whether to build shapes and planes executable for detecting planes. ON/OFF" OFF)
IF(WITH_SHAPE_DETECTION)
MESSAGE(STATUS "With shape detection")
ELSE(WITH_SHAPE_DETECTION)
MESSAGE(STATUS "Without shape detection")
ENDIF(WITH_SHAPE_DETECTION)
## Interior reconstruction
option(WITH_MODEL "Whether to build model executable for modelling interior environments. ON/OFF" OFF)
if(WITH_MODEL)
message(STATUS "With interior reconstruction")
else(WITH_MODEL)
message(STATUS "Without interior reconstruction")
endif(WITH_MODEL)
## Thermo
OPTION(WITH_THERMO "Whether to build executables for mutual calibration of laser scanner and camera. ON/OFF" OFF)
IF(WITH_THERMO)
FIND_PACKAGE(OpenCV REQUIRED)
add_subdirectory(3rdparty/cvblob)
include_directories(${CMAKE_SOURCE_DIR}/3rdparty/cvblob)
link_directories(${CMAKE_SOURCE_DIR}/3rdparty/cvblob)
MESSAGE(STATUS "With thermo")
ELSE(WITH_THERMO)
MESSAGE(STATUS "Without thermo")
ENDIF(WITH_THERMO)
## Octree
OPTION(WITH_OCTREE_DISPLAY "Whether to use octree display for efficiently culling scans ON/OFF" ON)
IF(WITH_OCTREE_DISPLAY)
MESSAGE(STATUS "Using octree display")
SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DUSE_GL_POINTS")
ELSE(WITH_OCTREE_DISPLAY)
MESSAGE(STATUS "Using displaylists: Warning potentially much slower")
ENDIF(WITH_OCTREE_DISPLAY)
#SET (WITH_OCTREE_DISPLAY ${WITH_OCTREE_DISPLAY} CACHE BOOL
#"Whether to use octree display for efficiently culling scans ON/OFF" FORCE)
## Octree
OPTION(WITH_COMPACT_OCTREE "Whether to use the compact octree display ON/OFF" OFF)
IF(WITH_COMPACT_OCTREE)
MESSAGE(STATUS "Using compact octrees")
SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DUSE_COMPACT_TREE")
ELSE(WITH_COMPACT_OCTREE)
MESSAGE(STATUS "Not using compact octreees: Warning uses more memory")
ENDIF(WITH_COMPACT_OCTREE)
## Glee?
OPTION(WITH_GLEE "Whether to use OpenGL extensions, requires glee. ON/OFF" OFF)
IF(WITH_GLEE)
MESSAGE(STATUS "Using opengl extensions")
SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DWITH_GLEE")
ELSE(WITH_GLEE)
MESSAGE(STATUS "Not using opengl extensions")
ENDIF(WITH_GLEE)
## Gridder
OPTION(WITH_GRIDDER "Whether to build the 2DGridder binary ON/OFF" OFF)
IF(WITH_GRIDDER)
MESSAGE(STATUS "With 2DGridder")
ELSE(WITH_GRIDDER)
MESSAGE(STATUS "Without 2DGridder")
ENDIF(WITH_GRIDDER)
## Dynamic VELOSLAM
OPTION(WITH_VELOSLAM "Whether to build the Velodyne data processing (veloslam/veloshow) ON/OFF" OFF)
IF(WITH_VELOSLAM)
MESSAGE(STATUS "With VELOSLAM")
ELSE(WITH_VELOSLAM)
MESSAGE(STATUS "Without VELOSLAM")
ENDIF(WITH_VELOSLAM)
## Home-made Laserscanner
OPTION(WITH_DAVID_3D_SCANNER "Whether to build the David scanner app for homemade laser scanners binary ON/OFF" OFF)
IF(WITH_DAVID_3D_SCANNER)
MESSAGE(STATUS "With David scanner")
ELSE(WITH_DAVID_3D_SCANNER)
MESSAGE(STATUS "Without David scanner")
ENDIF(WITH_DAVID_3D_SCANNER)
## Tools
OPTION(WITH_TOOLS "Whether to build additional tools like convergence frame_to_graph etc. ON/OFF" OFF)
IF(WITH_TOOLS)
MESSAGE(STATUS "With Tools")
find_package (Boost COMPONENTS program_options REQUIRED)
ELSE(WITH_TOOLS)
MESSAGE(STATUS "Without Tools")
ENDIF(WITH_TOOLS)
## Segmentation
OPTION(WITH_SEGMENTATION "Whether to build scan segmantion program ON/OFF" OFF)
IF(WITH_SEGMENTATION)
MESSAGE(STATUS "With segmentation")
find_package (Boost COMPONENTS program_options REQUIRED)
ELSE(WITH_SEGMENTATION)
MESSAGE(STATUS "Without segmentation")
ENDIF(WITH_SEGMENTATION)
## Normals
OPTION(WITH_NORMALS "Whether to build program for normal computation ON/OFF" OFF)
IF(WITH_NORMALS)
MESSAGE(STATUS "With normals")
ELSE(WITH_NORMALS)
MESSAGE(STATUS "Without normals")
ENDIF(WITH_NORMALS)
## CAD matching
OPTION (WITH_CAD "Wether to build with CAD import lib ON/OFF" OFF)
IF (WITH_CAD)
MESSAGE (STATUS "With CAD import")
find_package (Boost COMPONENTS program_options filesystem REQUIRED)
ELSE (WITH_CAD)
MESSAGE (STATUS "Without CAD import")
ENDIF (WITH_CAD)
## RivLib
OPTION(WITH_RIVLIB "Whether the RIEGL rivlib is present ON/OFF" OFF)
IF(WITH_RIVLIB)
MESSAGE(STATUS "Compiling a scan IO for RXP files")
include_directories(${CMAKE_SOURCE_DIR}/3rdparty)
link_directories(${CMAKE_SOURCE_DIR}/3rdparty)
SET(RIEGL_DIR ${CMAKE_SOURCE_DIR}/3rdparty/riegl/)
IF(WIN32)
SET(RIVLIB ${RIEGL_DIR}libscanlib-mt.lib ${RIEGL_DIR}libctrllib-mt.lib ${RIEGL_DIR}libboost_system-mt-1_43_0-vns.lib)
ELSE(WIN32)
SET(RIVLIB ${RIEGL_DIR}libscanlib-mt-s.a ${RIEGL_DIR}libctrllib-mt-s.a ${RIEGL_DIR}libboost_system-mt-s-1_43_0-vns.a pthread)
ENDIF(WIN32)
FIND_PACKAGE(LibXml2 )
ELSE(WITH_RIVLIB)
MESSAGE(STATUS "Do NOT compile a scan IO for RXP")
ENDIF(WITH_RIVLIB)
## CUDA support, TODO depend on CUDA_FIND
OPTION(WITH_CUDA "Compile with CUDA support" OFF)
IF(WITH_CUDA)
MESSAGE(STATUS "Compiling WITH CUDA support")
FIND_PACKAGE(CUDA)
SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DWITH_CUDA")
ELSE(WITH_CUDA)
MESSAGE(STATUS "Compiling WITHOUT CUDA support")
ENDIF(WITH_CUDA)
## PMD
OPTION(WITH_PMD "Whether to build the PMD tools like grabVideoAnd3D calibrate etc. ON/OFF" OFF)
IF(WITH_PMD)
FIND_PACKAGE(OpenGL REQUIRED)
MESSAGE(STATUS "With Tools")
ELSE(WITH_PMD)
MESSAGE(STATUS "Without Tools")
ENDIF(WITH_PMD)
## FBR
OPTION(WITH_FBR "Whether to compile feature based registration. ON/OFF" OFF)
IF(WITH_FBR)
MESSAGE(STATUS "With FBR ")
ELSE(WITH_FBR)
MESSAGE(STATUS "Without FBR")
ENDIF(WITH_FBR)
## Special treatment for system specifics
IF(APPLE)
add_definitions(-Dfopen64=fopen)
ENDIF(APPLE)
## Multiple Cores
IF(APPLE)
SET(PROCESSOR_COUNT 2)
ELSE(APPLE)
INCLUDE(CountProcessors)
SET(NUMBER_OF_CPUS "${PROCESSOR_COUNT}" CACHE STRING "The number of processors to use (default: ${PROCESSOR_COUNT})" )
ENDIF(APPLE)
# OPEN
FIND_PACKAGE(OpenMP)
IF(OPENMP_FOUND)
OPTION(WITH_OPENMP "Whether to use parallel processing capabilities of OPENMP. ON/OFF" ON)
ENDIF(OPENMP_FOUND)
IF(OPENMP_FOUND AND WITH_OPENMP)
MESSAGE(STATUS "With OpenMP ")
SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DMAX_OPENMP_NUM_THREADS=${NUMBER_OF_CPUS} -DOPENMP_NUM_THREADS=${NUMBER_OF_CPUS} ${OpenMP_CXX_FLAGS} -DOPENMP")
ELSE(OPENMP_FOUND AND WITH_OPENMP)
MESSAGE(STATUS "Without OpenMP")
SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DMAX_OPENMP_NUM_THREADS=1 -DOPENMP_NUM_THREADS=1")
ENDIF(OPENMP_FOUND AND WITH_OPENMP)
## TORO
OPTION(WITH_TORO "Whether to use TORO. ON/OFF" OFF)
IF(WITH_TORO)
IF(WIN32)
SET(Subversion_SVN_EXECUTABLE "svn.exe")
ENDIF(WIN32)
cmake_minimum_required (VERSION 2.8)
include(ExternalProject)
ExternalProject_Add(toro3d
SVN_REPOSITORY https://www.openslam.org/data/svn/toro/trunk
SOURCE_DIR "${CMAKE_SOURCE_DIR}/3rdparty/toro"
CONFIGURE_COMMAND ""
BUILD_COMMAND make
BUILD_IN_SOURCE 1
INSTALL_COMMAND cp ${CMAKE_SOURCE_DIR}/3rdparty/toro/toro3d ${CMAKE_SOURCE_DIR}/bin/
)
MESSAGE(STATUS "With TORO ")
ELSE(WITH_TORO)
MESSAGE(STATUS "Without TORO")
ENDIF(WITH_TORO)
## HOGMAN
OPTION(WITH_HOGMAN "Whether to use HOGMAN. ON/OFF" OFF)
IF(WITH_HOGMAN)
# dependant on libqt4-devi
find_package( Qt4 REQUIRED)
# CMake of earlier versions do not have external project capabilities
cmake_minimum_required (VERSION 2.8)
include(ExternalProject)
ExternalProject_Add(hogman
SVN_REPOSITORY https://svn.openslam.org/data/svn/hog-man/trunk
SOURCE_DIR "${CMAKE_SOURCE_DIR}/3rdparty/hogman"
CONFIGURE_COMMAND <SOURCE_DIR>/configure --prefix=<INSTALL_DIR>
BUILD_COMMAND LD_LIBRARY_PATH=${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib make
BUILD_IN_SOURCE 1
INSTALL_COMMAND cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/bin/hogman3d ${CMAKE_SOURCE_DIR}/bin/ &&
cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_csparse.so ${CMAKE_SOURCE_DIR}/lib/ &&
cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_graph_optimizer_hogman.so ${CMAKE_SOURCE_DIR}/lib/ &&
cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_graph_optimizer.so ${CMAKE_SOURCE_DIR}/lib/ &&
cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_graph.so ${CMAKE_SOURCE_DIR}/lib/ &&
cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_graph_viewer.so ${CMAKE_SOURCE_DIR}/lib/ &&
cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_math.so ${CMAKE_SOURCE_DIR}/lib/ &&
cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_qglviewer.so ${CMAKE_SOURCE_DIR}/lib/ &&
cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_stuff.so ${CMAKE_SOURCE_DIR}/lib/
)
MESSAGE(STATUS "With HOGMAN: Currently hogman needs to be compiled manually, please make sure hogman3d is somewhere in your PATH")
ELSE(WITH_HOGMAN)
MESSAGE(STATUS "Without HOGMAN")
ENDIF(WITH_HOGMAN)
OPTION(EXPORT_SHARED_LIBS "Whether to build additional shared libraries for use in other projects. ON/OFF" OFF)
IF(EXPORT_SHARED_LIBS)
MESSAGE(STATUS "exporting additional libraries")
ELSE(EXPORT_SHARED_LIBS)
MESSAGE(STATUS "not exporting libraries")
ENDIF(EXPORT_SHARED_LIBS)
OPTION(WITH_METRICS "Whether to use metrics in slam6d. ON/OFF" OFF)
IF(WITH_METRICS)
MESSAGE(STATUS "With metrics in slam6d.")
SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DWITH_METRICS")
ELSE(WITH_METRICS)
MESSAGE(STATUS "Without metrics in slam6d.")
ENDIF(WITH_METRICS)
IF(WIN32)
SET(ADDITIONAL_CFLAGS "-O2" CACHE STRING"Additional flags given to the compiler ()" )
include_directories(${CMAKE_SOURCE_DIR}/3rdparty/windows/)
link_directories(${CMAKE_SOURCE_DIR}/3rdparty/windows)
link_directories(${CMAKE_SOURCE_DIR}/3rdparty/windows/x64)
add_library(XGetopt STATIC ${CMAKE_SOURCE_DIR}/3rdparty/windows/XGetopt.cpp)
SET(CMAKE_STATIC_LIBRARY_SUFFIX "32.lib")
ELSE(WIN32)
SET(ADDITIONAL_CFLAGS "-O3 -std=c++0x -msse3 -Wall -finline-functions -Wno-unused-but-set-variable -Wno-write-strings -Wno-char-subscripts -Wno-unused-result" CACHE STRING"Additional flags given to the compiler (-O3 -Wall -finline-functions -Wno-write-strings)" )
# Add include path for OpenGL without GL/-prefix
# to avoid the include incompatibility between MACOS
# and linux
FIND_PATH(OPENGL_INC gl.h /usr/include/GL)
include_directories(${OPENGL_INC})
ENDIF(WIN32)
# Add OpenGL includes for MACOS if needed
# The OSX OpenGL frameworks natively supports freeglut extensions
IF(APPLE)
include_directories(/System/Library/Frameworks/GLUT.framework/Headers)
include_directories(/System/Library/Frameworks/OpenGL.framework/Headers)
ENDIF(APPLE)
SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${ADDITIONAL_CFLAGS}")
# Hide CMake variables
SET (CMAKE_INSTALL_PREFIX "/usr/local" CACHE INTERNAL "" FORCE)
SET (CMAKE_BUILD_TYPE "" CACHE INTERNAL "" FORCE)
# Set output directories for libraries and executables
SET( CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/lib )
SET( CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/obj )
SET( CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/bin )
# Set include and link dirs ...
include_directories(${CMAKE_SOURCE_DIR}/include)
include_directories(${CMAKE_SOURCE_DIR}/3rdparty/)
include_directories(${CMAKE_SOURCE_DIR}/3rdparty/glui)
include_directories(${CMAKE_SOURCE_DIR}/3rdparty/wxthings/include/)
include_directories(${CMAKE_SOURCE_DIR}/3rdparty/ann_1.1.1_modified/include)
include_directories(${CMAKE_SOURCE_DIR}/3rdparty/ann_1.1.1_modified/src)
link_directories(${CMAKE_SOURCE_DIR}/obj)
link_directories(${CMAKE_SOURCE_DIR}/lib)
add_subdirectory(3rdparty)
add_subdirectory(src/slam6d)
add_subdirectory(src/scanio)
add_subdirectory(src/scanserver)
add_subdirectory(src/segmentation)
add_subdirectory(src/normals)
add_subdirectory(src/veloslam)
add_subdirectory(src/show)
add_subdirectory(src/grid)
add_subdirectory(src/pmd)
add_subdirectory(src/shapes)
add_subdirectory(src/thermo)
IF(WITH_FBR)
add_subdirectory(src/slam6d/fbr)
ENDIF(WITH_FBR)
add_subdirectory(src/scanner)
add_subdirectory(src/model)
IF(EXPORT_SHARED_LIBS)
## Compiling a shared library containing all of the project
add_library(slam SHARED src/slam6d/icp6D.cc)
target_link_libraries(slam scan_s ANN_s sparse_s newmat_s show_s fbr_s)
ENDIF(EXPORT_SHARED_LIBS)
MESSAGE (STATUS "Build environment is set up!")
# hack to "circumvent" Debug and Release folders that are created under visual studio
# this is why the INSTALL target has to be used in visual studio
IF(MSVC)
INSTALL(DIRECTORY ${CMAKE_SOURCE_DIR}/bin/Release/ CONFIGURATIONS Release DESTINATION ${CMAKE_SOURCE_DIR}/windows FILES_MATCHING PATTERN "*.dll" PATTERN "*.exe")
IF( CMAKE_SIZEOF_VOID_P EQUAL 8 )
INSTALL(DIRECTORY ${CMAKE_SOURCE_DIR}/3rdparty/windows/x64/ CONFIGURATIONS Release DESTINATION ${CMAKE_SOURCE_DIR}/windows FILES_MATCHING PATTERN "*.dll" PATTERN "*.exe")
ELSE( CMAKE_SIZEOF_VOID_P EQUAL 8 )
INSTALL(DIRECTORY ${CMAKE_SOURCE_DIR}/3rdparty/windows/ CONFIGURATIONS Release DESTINATION ${CMAKE_SOURCE_DIR}/windows FILES_MATCHING PATTERN "*.dll" PATTERN "*.exe")
ENDIF(CMAKE_SIZEOF_VOID_P EQUAL 8 )
INSTALL(DIRECTORY ${CMAKE_SOURCE_DIR}/bin/Debug/ CONFIGURATIONS Debug DESTINATION ${CMAKE_SOURCE_DIR}/windows FILES_MATCHING PATTERN "*.dll" PATTERN "*.exe")
IF( CMAKE_SIZEOF_VOID_P EQUAL 8 )
INSTALL(DIRECTORY ${CMAKE_SOURCE_DIR}/3rdparty/windows/x64/ CONFIGURATIONS Debug DESTINATION ${CMAKE_SOURCE_DIR}/windows FILES_MATCHING PATTERN "*.dll" PATTERN "*.exe")
ELSE( CMAKE_SIZEOF_VOID_P EQUAL 8 )
INSTALL(DIRECTORY ${CMAKE_SOURCE_DIR}/3rdparty/windows/ CONFIGURATIONS Debug DESTINATION ${CMAKE_SOURCE_DIR}/windows FILES_MATCHING PATTERN "*.dll" PATTERN "*.exe")
ENDIF(CMAKE_SIZEOF_VOID_P EQUAL 8 )
ENDIF(MSVC)

View file

@ -0,0 +1,433 @@
#ifndef SCAN_H
#define SCAN_H
#include "io_types.h"
#include "data_types.h"
#include "point_type.h"
#include "ptpair.h"
#include "pairingMode.h"
#include <string>
#include <vector>
#include <boost/thread/mutex.hpp>
#include <boost/thread/locks.hpp>
//! SearchTree types
enum nns_type {
simpleKD, ANNTree, BOCTree
};
class Scan;
typedef std::vector<Scan*> ScanVector;
class SearchTree;
class ANNkd_tree;
/** HOWTO scan
First: Load scans (if you want to use the scanmanager, use ManagedScan)
BasicScan/ManagedScan::openDirectory(path, type, start, end);
Pass it to functions (by reference to link it to the same instance) or store it in a global variable
After loading you might want to set parameters
for(ScanVector::iterator it = Scan::allScans.begin(); it != Scan::allScans.end(); ++it) {
Scan* scan = *it;
scan->setRangeFilter(maxDist, minDist);
scan->setHeightFilter(top, bottom); // thermo
scan->setReductionParameter(voxelSize, nrpts[, pointtype]);
scan->setSearchTreeParameter(nns_method, use_cuda);
}
Access the contained data, will be loaded and calculated on demand
DataXYZ xyz = scan->get("xyz");
DataXYZ reduced = scan->get("xyz reduced");
DataRGB rgb = scan->get("rgb");
xyz[i][0..2]
reflectance[i]
unsigned int size = scan->size("xyz reduced");
In order to use the prefetching of all requested data field in the scanserver, mark them for use. This is relevant for efficiency, which would otherwise cause loading the files each time another data field is requested.
scan->get(DATA_XYZ | DATA_RGB | ...);
Under circumstances the data fields are not available (e.g. no color in uos-type scans)
DataRGB rgb = scan->get("rgb");
if(rgb.valid()) { ok, do something }
If backward-compability to pointer arrays is needed, the PointerArray class can adapt
BOctTree(PointerArray(scan->get("xyz")).get(), scan->size("xyz"), ...);
If data isn't needed anymore, flag it for removal
scan->clear("xyz");
scan->clear(DATA_XYZ | DATA_RGB | ...);
Creating data fields with the correct byte size
scan->create("xyz somethingelse", sizeof(double)*3*N);
Reading frames in show:
unsigned int size = scan->readFrames();
const double* pose;
AlgoType type;
scan->getFrame(i, pose, type);
Last, if program ends, clean up
Scan::closeDirectory(scans);
**/
/**
* This class bundles common features of different scan implementations by
* abstraction. It handles the algorithmic parts and leaves IO and other
* features to the deriving classes.
*/
class Scan {
//friend class SearchTree; // TODO: is this neccessary?
public:
enum AlgoType { INVALID, ICP, ICPINACTIVE, LUM, ELCH };
// delete copy-ctor and assignment, scans shouldn't be copied by basic class
Scan(const Scan& other) = delete;
Scan& operator=(const Scan& other) = delete;
virtual ~Scan();
//! Holder of all scans
// also used in transform for adding frames for each scan at the same time
static std::vector<Scan*> allScans;
/**
* Attempt to read a directory under \a path and return its read scans.
* No scans are loaded at this point, only checked if all exist.
*
* @param scanserver whether to use managed scans in the scanserver or not
* @param path to the directory containing the scans
* @param type determining which ScanIO to use
* @param start first scan to use
* @param end last scan to use, -1 means from start to last available
*/
static void openDirectory(bool scanserver, const std::string& path, IOType type,
int start, int end = -1);
/**
* "Close" a directory by deleting all its scans and emptying the
* Scan::allScans vector.
*/
static void closeDirectory();
/* Input filtering and parameter functions */
//! Input filtering for all points based on their euclidean length
virtual void setRangeFilter(double max, double min) = 0;
//! Input filtering for all points based on their height
virtual void setHeightFilter(double top, double bottom) = 0;
//! Input mutation to set range of all points to a constant value;
virtual void setRangeMutation(double range) { }
//! Set reduction parameters, but don't reduce yet
virtual void setReductionParameter(double voxelSize, int nrpts = 0,
PointType pointtype = PointType());
//! Set SearchTree type, but don't create it yet
void setSearchTreeParameter(int nns_method, bool cuda_enabled);
/**
* Set octtree parameters for show
* @param loadOct will load the serialized octtree from disk regardless
* @param saveOct serialize octtree if not loaded by loadOct after creation
*/
virtual void setOcttreeParameter(double reduction_voxelSize,
double octtree_voxelSize, PointType pointtype,
bool loadOct, bool saveOct);
/* Basic getter functions */
inline const double* get_rPos() const;
inline const double* get_rPosTheta() const;
inline const double* get_rPosQuat() const;
//! Pose matrix after initial and match transformations (org+dalign)
inline const double* get_transMat() const;
//! Original pose matrix after initial transform
inline const double* get_transMatOrg() const;
//! Accumulated delta transformation matrix
inline const double* getDAlign() const;
inline SearchTree* getSearchTree();
inline ANNkd_tree* getANNTree() const;
virtual const char* getIdentifier() const = 0;
//! Determine the maximum number of reduced points in \a scans
static unsigned int getMaxCountReduced(ScanVector& scans);
/* Functions for altering data fields, implementation specific */
/**
* Get the data field \a identifier, calculate it on demand if neccessary.
*
* If "xyz reduced" or "xyz reduced original" is requested, the reduction is
* started with "xyz" as input.
*/
virtual DataPointer get(const std::string& identifier) = 0;
/**
* Load the requested IODataTypes, joined by |, from the scan file.
*
* This feature is neccessary to load multiple data fields at once, not all
* one by one with each get("...") access.
*/
virtual void get(unsigned int types) = 0;
/**
* Creates a data field \a identifier with \a size bytes.
*/
virtual DataPointer create(const std::string& identifier, unsigned int size) = 0;
/**
* Clear the data field \a identifier, removing its allocated memory if
* possible or marking it for prioritized removal.
*/
virtual void clear(const std::string& identifier) = 0;
//! Extension to clear for more than one identifier, e.g. clear(DATA_XYZ | DATA_RGB);
void clear(unsigned int types);
/**
* Get the size of \a identifier as if it were requested and size() called
* upon its type specialized DataPointer class.
* e.g size<DataXYZ>("xyz reduced")
*/
template<typename T>
unsigned int size(const std::string& identifier) {
return (T(get(identifier))).size();
}
/* Frame handling functions */
/**
* Open the .frames-file and read its contents. If not read, the frame list
* will be empty.
* @return count of frames if file has been read, zero otherwise
*/
virtual unsigned int readFrames() = 0;
/**
* Write the accumulated frames into a .frames-file.
*/
virtual void saveFrames() = 0;
//! Count of frames
virtual unsigned int getFrameCount() = 0;
//! Get contents of a frame, pass matrix pointer and type by reference
virtual void getFrame(unsigned int i, const double*& pose_matrix, AlgoType& type) = 0;
protected:
/**
* Called from transform, this will add its current transMat pose with
* the given type as a frame into the list of frames
*/
virtual void addFrame(AlgoType type) = 0;
public:
/* Direct creation of reduced points and search tree */
//! Apply reduction and initial transMatOrg transformation
void toGlobal();
//! Copy reduced points to original and create search tree on it
void createSearchTree();
/* Common transformation and matching functions */
void mergeCoordinatesWithRoboterPosition(Scan* prevScan);
void transformAll(const double alignxf[16]);
void transformAll(const double alignQuat[4], const double alignt[3]);
void transform(const double alignxf[16],
const AlgoType type, int islum = 0);
void transform(const double alignQuat[4],
const double alignt[3], const AlgoType type, int islum = 0);
void transformToMatrix(double alignxf[16],
const AlgoType type, int islum = 0);
void transformToEuler(double rP[3], double rPT[3],
const AlgoType type, int islum = 0);
void transformToQuat(double rP[3], double rPQ[4],
const AlgoType type, int islum = 0);
// Scan matching functions
static void getPtPairs(std::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 = CLOSEST_POINT);
static void getNoPairsSimple(std::vector<double*> &diff,
Scan* Source, Scan* Target,
int thread_num,
double max_dist_match2);
static void getPtPairsSimple(std::vector<PtPair> *pairs,
Scan* Source, Scan* Target,
int thread_num,
int rnd, double max_dist_match2,
double *centroid_m, double *centroid_d);
static void getPtPairsParallel(std::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);
protected:
/**
* The pose of the scan
* Note: rPos/rPosTheta and transMat _should_
* always represent the same pose!!!
*/
double rPos[3], //!< 3D position
rPosTheta[3], //!< 3D rotation in Euler representation
rQuat[4], //!< 3D rotation in Quaternion representation
transMat[16], //!< (4x4) transformation matrix
transMatOrg[16]; //!< The original pose of the scan, e.g., from odometry
/**
* The dalignxf transformation represents the delta transformation virtually applied
* to the tree and is used to compute are actual corresponding points.
*/
double dalignxf[16];
//! Run ICP on GPU instead of CPU
bool cuda_enabled;
//! Defines the method used for nearest neighbor search and which tree to use
int nns_method;
//! SearchTree for point pair matching, works on the search points
SearchTree* kd;
//! This KD tree is created only for the CUDA usages
ANNkd_tree* ann_kd_tree;
//! Voxelsize of the octtree used for reduction
double reduction_voxelSize;
//! Which point to take out of the reduction octtree, 0 for center
int reduction_nrpts;
//! Pointtype used for the reduction octtree
PointType reduction_pointtype;
//! Type of the searchtree to be created
int searchtree_nnstype;
//! Use CUDA for searching
bool searchtree_cuda_enabled;
//! Flag whether "xyz reduced" has been initialized for this Scan yet
bool m_has_reduced;
//! Flag whether "normals" has been initialized for this Scan yet
bool m_has_normals;
//! Reduction value used for octtree input
double octtree_reduction_voxelSize;
//! Voxelsize used in the octtree itself
double octtree_voxelSize;
//! Pointtype for the Octtree
PointType octtree_pointtype;
//! Flags to load or save the octtrees from/to storage
bool octtree_loadOct, octtree_saveOct;
/**
* Basic initializing constructor calling the initalization function.
* Can only be called from deriving classes.
*/
Scan();
/**
* This function handles the reduction of points. It builds a lock for
* multithread-safety and calls calcReducedOnDemandPrivate.
*
* The intention is to reduce points, transforme them to the initial pose and
* then copy them to original for the SearchTree.
*/
void calcReducedOnDemand();
/**
* This function handles the computation of the normals. It builds a lock for
* multithread-safety and calls caldNormalsOnDemandPrivate.
*/
void calcNormalsOnDemand();
//! Create specific SearchTree variants matching the capability of the Scan
virtual void createSearchTreePrivate() = 0;
//! Create reduced points in a multithread-safe environment matching the capability of the Scan
virtual void calcReducedOnDemandPrivate() = 0;
//! Create normals in a multithread-safe environment matching the capability of the Scan
virtual void calcNormalsOnDemandPrivate() = 0;
//! Creating normals
void calcNormals();
//! Internal function of transform which alters the reduced points
void transformReduced(const double alignxf[16]);
//! Internal function of transform which handles the matrices
void transformMatrix(const double alignxf[16]);
//@FIXME
public:
//! Creating reduced points
void calcReducedPoints();
protected:
//! Copies reduced points to original points without any transformation.
void copyReducedToOriginal();
//! Inverse functionality of copyReducedToOriginal.
void copyOriginalToReduced();
private:
//! flag for openDirectory and closeDirectory to distinguish the scans
static bool scanserver;
public:
//! Mutex for safely reducing points and creating the search tree just once in a multithreaded environment
// it can not be compiled in win32 use boost 1.48, therefore we remeove it temporarily
boost::mutex m_mutex_reduction, m_mutex_create_tree, m_mutex_normals;
};
#include "scan.icc"
#endif //SCAN_H

View file

@ -0,0 +1,276 @@
/** @file
* @brief Representation of the optimized k-d tree.
* @author Remus Dumitru. Jacobs University Bremen, Germany
* @author Corneliu-Claudiu Prodescu. Jacobs University Bremen, Germany
* @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Thomas Escher. Institute of Computer Science, University of Osnabrueck, Germany.
*/
#ifndef __KD_TREE_IMPL_H__
#define __KD_TREE_IMPL_H__
#include "slam6d/kdparams.h"
#include "globals.icc"
#ifdef _MSC_VER
#if !defined _OPENMP && defined OPENMP
#define _OPENMP
#endif
#endif
#ifdef _OPENMP
#include <omp.h>
#endif
/**
* @brief The optimized k-d tree.
*
* A kD tree for points, with limited
* capabilities (find nearest point to
* a given point, or to a ray).
**/
template<class PointData, class AccessorData, class AccessorFunc>
class KDTreeImpl {
public:
inline KDTreeImpl() { }
virtual inline ~KDTreeImpl() {
if (!npts) {
#ifdef WITH_OPENMP_KD
omp_set_num_threads(OPENMP_NUM_THREADS);
#pragma omp parallel for schedule(dynamic)
#endif
for (int i = 0; i < 2; i++) {
if (i == 0 && node.child1) delete node.child1;
if (i == 1 && node.child2) delete node.child2;
}
} else {
if (leaf.p) delete [] leaf.p;
}
}
virtual void create(PointData pts, AccessorData *indices, size_t n) {
AccessorFunc point;
// Find bbox
double xmin = point(pts, indices[0])[0], xmax = point(pts, indices[0])[0];
double ymin = point(pts, indices[0])[1], ymax = point(pts, indices[0])[1];
double zmin = point(pts, indices[0])[2], zmax = point(pts, indices[0])[2];
for(unsigned int i = 1; i < n; i++) {
xmin = min(xmin, point(pts, indices[i])[0]);
xmax = max(xmax, point(pts, indices[i])[0]);
ymin = min(ymin, point(pts, indices[i])[1]);
ymax = max(ymax, point(pts, indices[i])[1]);
zmin = min(zmin, point(pts, indices[i])[2]);
zmax = max(zmax, point(pts, indices[i])[2]);
}
// Leaf nodes
if ((n > 0) && (n <= 10)) {
npts = n;
leaf.p = new AccessorData[n];
// fill leaf index array with indices
for(unsigned int i = 0; i < n; ++i) {
leaf.p[i] = indices[i];
}
return;
}
// Else, interior nodes
npts = 0;
node.center[0] = 0.5 * (xmin+xmax);
node.center[1] = 0.5 * (ymin+ymax);
node.center[2] = 0.5 * (zmin+zmax);
node.dx = 0.5 * (xmax-xmin);
node.dy = 0.5 * (ymax-ymin);
node.dz = 0.5 * (zmax-zmin);
node.r2 = sqr(node.dx) + sqr(node.dy) + sqr(node.dz);
// Find longest axis
if (node.dx > node.dy) {
if (node.dx > node.dz) {
node.splitaxis = 0;
} else {
node.splitaxis = 2;
}
} else {
if (node.dy > node.dz) {
node.splitaxis = 1;
} else {
node.splitaxis = 2;
}
}
// Partition
double splitval = node.center[node.splitaxis];
if ( fabs(max(max(node.dx,node.dy),node.dz)) < 0.01 ) {
npts = n;
leaf.p = new AccessorData[n];
// fill leaf index array with indices
for(unsigned int i = 0; i < n; ++i) {
leaf.p[i] = indices[i];
}
return;
}
AccessorData* left = indices, * right = indices + n - 1;
while(true) {
while(point(pts, *left)[node.splitaxis] < splitval)
left++;
while(point(pts, *right)[node.splitaxis] >= splitval)
right--;
if(right < left)
break;
std::swap(*left, *right);
}
// Build subtrees
int i;
#ifdef WITH_OPENMP_KD // does anybody know the reason why this is slower ?? --Andreas
omp_set_num_threads(OPENMP_NUM_THREADS);
#pragma omp parallel for schedule(dynamic)
#endif
for (i = 0; i < 2; i++) {
if (i == 0) {
node.child1 = new KDTreeImpl();
node.child1->create(pts, indices, left - indices);
}
if (i == 1) {
node.child2 = new KDTreeImpl();
node.child2->create(pts, left, n - (left - indices));
}
}
}
protected:
/**
* storing the parameters of the k-d tree, i.e., the current closest point,
* the distance to the current closest point and the point itself.
* These global variable are needed in this search.
*
* Padded in the parallel case.
*/
#ifdef _OPENMP
#ifdef __INTEL_COMPILER
__declspec (align(16)) static KDParams params[MAX_OPENMP_NUM_THREADS];
#else
static KDParams params[MAX_OPENMP_NUM_THREADS];
#endif //__INTEL_COMPILER
#else
static KDParams params[MAX_OPENMP_NUM_THREADS];
#endif
/**
* number of points. If this is 0: intermediate node. If nonzero: leaf.
*/
int npts;
/**
* Cue the standard rant about anon unions but not structs in C++
*/
union {
/**
* in case of internal node...
*/
struct {
double center[3]; ///< storing the center of the voxel (R^3)
double dx, ///< defining the voxel itself
dy, ///< defining the voxel itself
dz, ///< defining the voxel itself
r2; ///< defining the voxel itself
int splitaxis; ///< defining the kind of splitaxis
KDTreeImpl *child1; ///< pointers to the childs
KDTreeImpl *child2; ///< pointers to the childs
} node;
/**
* in case of leaf node ...
*/
struct {
/**
* store the value itself
* Here we store just a pointer to the data
*/
AccessorData* p;
} leaf;
};
void _FindClosest(const PointData& pts, int threadNum) const {
AccessorFunc point;
// Leaf nodes
if (npts) {
for (int i = 0; i < npts; i++) {
double myd2 = Dist2(params[threadNum].p, point(pts, leaf.p[i]));
if (myd2 < params[threadNum].closest_d2) {
params[threadNum].closest_d2 = myd2;
params[threadNum].closest = point(pts, leaf.p[i]);
}
}
return;
}
// Quick check of whether to abort
double approx_dist_bbox = max(max(fabs(params[threadNum].p[0]-node.center[0])-node.dx,
fabs(params[threadNum].p[1]-node.center[1])-node.dy),
fabs(params[threadNum].p[2]-node.center[2])-node.dz);
if (approx_dist_bbox >= 0 && sqr(approx_dist_bbox) >= params[threadNum].closest_d2)
return;
// Recursive case
double myd = node.center[node.splitaxis] - params[threadNum].p[node.splitaxis];
if (myd >= 0.0) {
node.child1->_FindClosest(pts, threadNum);
if (sqr(myd) < params[threadNum].closest_d2) {
node.child2->_FindClosest(pts, threadNum);
}
} else {
node.child2->_FindClosest(pts, threadNum);
if (sqr(myd) < params[threadNum].closest_d2) {
node.child1->_FindClosest(pts, threadNum);
}
}
}
void _FindClosestAlongDir(const PointData& pts, int threadNum) const {
AccessorFunc point;
// Leaf nodes
if (npts) {
for (int i=0; i < npts; i++) {
double p2p[] = { params[threadNum].p[0] - point(pts, leaf.p[i])[0],
params[threadNum].p[1] - point(pts, leaf.p[i])[1],
params[threadNum].p[2] - point(pts, leaf.p[i])[2] };
double myd2 = Len2(p2p) - sqr(Dot(p2p, params[threadNum].dir));
if ((myd2 < params[threadNum].closest_d2)) {
params[threadNum].closest_d2 = myd2;
params[threadNum].closest = point(pts, leaf.p[i]);
}
}
return;
}
// Quick check of whether to abort
double p2c[] = { params[threadNum].p[0] - node.center[0],
params[threadNum].p[1] - node.center[1],
params[threadNum].p[2] - node.center[2] };
double myd2center = Len2(p2c) - sqr(Dot(p2c, params[threadNum].dir));
if (myd2center > node.r2 + params[threadNum].closest_d2 + 2.0f * max(node.r2, params[threadNum].closest_d2))
return;
// Recursive case
if (params[threadNum].p[node.splitaxis] < node.center[node.splitaxis] ) {
node.child1->_FindClosestAlongDir(pts, threadNum);
node.child2->_FindClosestAlongDir(pts, threadNum);
} else {
node.child2->_FindClosestAlongDir(pts, threadNum);
node.child1->_FindClosestAlongDir(pts, threadNum);
}
}
};
#endif

View file

@ -0,0 +1,132 @@
/*
* kdMeta implementation
*
* Copyright (C) Andreas Nuechter, Kai Lingemann, Thomas Escher
*
* Released under the GPL version 3.
*
*/
/** @file
* @brief An optimized k-d tree implementation. MetaScan variant.
* @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Thomas Escher. Institute of Computer Science, University of Osnabrueck, Germany.
*/
#ifdef _MSC_VER
#define _USE_MATH_DEFINES
#endif
#include "slam6d/kdMeta.h"
#include "slam6d/globals.icc"
#include "slam6d/scan.h"
#include <iostream>
using std::cout;
using std::cerr;
using std::endl;
#include <algorithm>
using std::swap;
#include <cmath>
#include <cstring>
// KDtree class static variables
template<class PointData, class AccessorData, class AccessorFunc>
KDParams KDTreeImpl<PointData, AccessorData, AccessorFunc>::params[MAX_OPENMP_NUM_THREADS];
KDtreeMetaManaged::KDtreeMetaManaged(const vector<Scan*>& scans) :
m_count_locking(0)
{
// create scan pointer and data pointer arrays
m_size = scans.size();
m_scans = new Scan*[m_size];
for(unsigned int i = 0; i < m_size; ++i)
m_scans[i] = scans[i];
m_data = new DataXYZ*[m_size];
lock();
create(m_data, prepareTempIndices(scans), getPointsSize(scans));
unlock();
// allocate in prepareTempIndices, deleted here
delete[] m_temp_indices;
}
KDtreeMetaManaged::~KDtreeMetaManaged()
{
delete[] m_scans;
delete[] m_data;
}
Index* KDtreeMetaManaged::prepareTempIndices(const vector<Scan*>& scans)
{
unsigned int n = getPointsSize(scans);
m_temp_indices = new Index[n];
unsigned int s = 0, j = 0;
unsigned int scansize = scans[s]->size<DataXYZ>("xyz reduced");
for(unsigned int i = 0; i < n; ++i) {
m_temp_indices[i].set(s, j++);
// switch to next scan
if(j >= scansize) {
++s;
j = 0;
if(s < scans.size())
scansize = scans[s]->size<DataXYZ>("xyz reduced");
}
}
return m_temp_indices;
}
unsigned int KDtreeMetaManaged::getPointsSize(const vector<Scan*>& scans)
{
unsigned int n = 0;
for(vector<Scan*>::const_iterator it = scans.begin(); it != scans.end(); ++it) {
n += (*it)->size<DataXYZ>("xyz reduced");
}
return n;
}
double* KDtreeMetaManaged::FindClosest(double *_p, double maxdist2, int threadNum) const
{
params[threadNum].closest = 0;
params[threadNum].closest_d2 = maxdist2;
params[threadNum].p = _p;
_FindClosest(m_data, threadNum);
return params[threadNum].closest;
}
double* KDtreeMetaManaged::FindClosestAlongDir(double *_p, double *_dir, double maxdist2, int threadNum) const
{
params[threadNum].closest = NULL;
params[threadNum].closest_d2 = maxdist2;
params[threadNum].p = _p;
params[threadNum].dir = _dir;
_FindClosestAlongDir(m_data, threadNum);
return params[threadNum].closest;
}
void KDtreeMetaManaged::lock()
{
boost::lock_guard<boost::mutex> lock(m_mutex_locking);
if(m_count_locking == 0) {
// lock all the contained scans, metascan uses the transformed points
for(unsigned int i = 0; i < m_size; ++i) {
m_data[i] = new DataXYZ(m_scans[i]->get("xyz reduced"));
}
}
++m_count_locking;
}
void KDtreeMetaManaged::unlock()
{
boost::lock_guard<boost::mutex> lock(m_mutex_locking);
--m_count_locking;
if(m_count_locking == 0) {
// delete each locking object
for(unsigned int i = 0; i < m_size; ++i) {
delete m_data[i];
}
}
}

View file

@ -0,0 +1,81 @@
#ifndef MANAGED_SCAN_H
#define MANAGED_SCAN_H
#include "scan.h"
#include "scanserver/sharedScan.h"
class ManagedScan : public Scan {
public:
static void openDirectory(const std::string& path, IOType type,
int start, int end = -1);
static void closeDirectory();
static std::size_t getMemorySize();
virtual ~ManagedScan();
virtual void setRangeFilter(double max, double min);
virtual void setHeightFilter(double top, double bottom);
virtual void setRangeMutation(double range);
virtual void setReductionParameter(double voxelSize, int nrpts = 0,
PointType pointtype = PointType());
void setShowReductionParameter(double voxelSize, int nrpts = 0,
PointType pointtype = PointType());
virtual void setOcttreeParameter(double reduction_voxelSize,
double octtree_voxelSize, PointType pointtype,
bool loadOct, bool saveOct);
virtual const char* getIdentifier() const { return m_shared_scan->getIdentifier(); }
virtual DataPointer get(const std::string& identifier);
virtual void get(unsigned int types);
virtual DataPointer create(const std::string& identifier, unsigned int size);
virtual void clear(const std::string& identifier);
virtual unsigned int readFrames();
virtual void saveFrames();
virtual unsigned int getFrameCount();
virtual void getFrame(unsigned int i, const double*& pose_matrix, AlgoType& type);
protected:
virtual void createSearchTreePrivate();
virtual void calcReducedOnDemandPrivate();
virtual void calcNormalsOnDemandPrivate() {};
virtual void addFrame(AlgoType type);
private:
//! Reference to the shared scan
SharedScan* m_shared_scan;
//! SharedScan vector to be deleted on closeDirectory
static SharedScanVector* shared_scans;
//! Flag to keep track of whether we have to update/create reduced points at the start
bool m_reduced_ready;
//! Flag to reset the persistent frames on write actions in slam to avoid a clear call
bool m_reset_frames_on_write;
//! Voxelsize of the octtree used for reduction
double show_reduction_voxelSize;
//! Which point to take out of the reduction octtree, 0 for center
int show_reduction_nrpts;
//! Pointtype used for the reduction octtree
PointType show_reduction_pointtype;
ManagedScan(SharedScan* shared_scan);
//! Create reduced points for show
void calcReducedShow();
//! Create Octtree for show
void createOcttree();
};
#endif //MANAGED_SCAN_H

View file

@ -0,0 +1,79 @@
/*
* kd implementation
*
* Copyright (C) Andreas Nuechter, Kai Lingemann, Thomas Escher
*
* Released under the GPL version 3.
*
*/
/** @file
* @brief An optimized k-d tree implementation
* @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Thomas Escher Institute of Computer Science, University of Osnabrueck, Germany.
*/
#ifdef _MSC_VER
#define _USE_MATH_DEFINES
#endif
#include "slam6d/kd.h"
#include "slam6d/globals.icc"
#include <iostream>
using std::cout;
using std::cerr;
using std::endl;
#include <algorithm>
using std::swap;
#include <cmath>
#include <cstring>
// KDtree class static variables
template<class PointData, class AccessorData, class AccessorFunc>
KDParams KDTreeImpl<PointData, AccessorData, AccessorFunc>::params[MAX_OPENMP_NUM_THREADS];
/**
* Constructor
*
* Create a KD tree from the points pointed to by the array pts
*
* @param pts 3D array of points
* @param n number of points
*/
KDtree::KDtree(double **pts, int n)
{
create(Void(), pts, n);
}
KDtree::~KDtree()
{
}
/**
* Finds the closest point within the tree,
* wrt. the point given as first parameter.
* @param _p point
* @param maxdist2 maximal search distance.
* @param threadNum Thread number, for parallelization
* @return Pointer to the closest point
*/
double *KDtree::FindClosest(double *_p, double maxdist2, int threadNum) const
{
params[threadNum].closest = 0;
params[threadNum].closest_d2 = maxdist2;
params[threadNum].p = _p;
_FindClosest(Void(), threadNum);
return params[threadNum].closest;
}
double *KDtree::FindClosestAlongDir(double *_p, double *_dir, double maxdist2, int threadNum) const
{
params[threadNum].closest = NULL;
params[threadNum].closest_d2 = maxdist2;
params[threadNum].p = _p;
params[threadNum].dir = _dir;
_FindClosestAlongDir(Void(), threadNum);
return params[threadNum].closest;
}

Binary file not shown.

View file

@ -239,7 +239,7 @@ ENDIF(WITH_SEGMENTATION)
## Normals ## Normals
OPT_DEP(WITH_NORMALS "Whether to build program for normal computation ON/OFF" OFF "WITH_FBR") OPT_DEP(WITH_NORMALS "Whether to build separate program for normal computation ON/OFF" OFF "WITH_FBR")
IF(WITH_NORMALS) IF(WITH_NORMALS)
MESSAGE(STATUS "With normals") MESSAGE(STATUS "With normals")
@ -312,58 +312,6 @@ ELSE(OPENMP_FOUND AND WITH_OPENMP)
SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DMAX_OPENMP_NUM_THREADS=1 -DOPENMP_NUM_THREADS=1") SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DMAX_OPENMP_NUM_THREADS=1 -DOPENMP_NUM_THREADS=1")
ENDIF(OPENMP_FOUND AND WITH_OPENMP) ENDIF(OPENMP_FOUND AND WITH_OPENMP)
## TORO
OPT_DEP(WITH_TORO "Whether to use TORO. ON/OFF" OFF "")
IF(WITH_TORO)
IF(WIN32)
SET(Subversion_SVN_EXECUTABLE "svn.exe")
ENDIF(WIN32)
cmake_minimum_required (VERSION 2.8)
include(ExternalProject)
ExternalProject_Add(toro3d
SVN_REPOSITORY https://www.openslam.org/data/svn/toro/trunk
SOURCE_DIR "${CMAKE_SOURCE_DIR}/3rdparty/toro"
CONFIGURE_COMMAND ""
BUILD_COMMAND make
BUILD_IN_SOURCE 1
INSTALL_COMMAND cp ${CMAKE_SOURCE_DIR}/3rdparty/toro/toro3d ${CMAKE_SOURCE_DIR}/bin/
)
MESSAGE(STATUS "With TORO ")
ELSE(WITH_TORO)
MESSAGE(STATUS "Without TORO")
ENDIF(WITH_TORO)
## HOGMAN
OPT_DEP(WITH_HOGMAN "Whether to use HOGMAN. ON/OFF" OFF "")
IF(WITH_HOGMAN)
# dependant on libqt4-devi
find_package( Qt4 REQUIRED)
# CMake of earlier versions do not have external project capabilities
cmake_minimum_required (VERSION 2.8)
include(ExternalProject)
ExternalProject_Add(hogman
SVN_REPOSITORY https://svn.openslam.org/data/svn/hog-man/trunk
SOURCE_DIR "${CMAKE_SOURCE_DIR}/3rdparty/hogman"
CONFIGURE_COMMAND <SOURCE_DIR>/configure --prefix=<INSTALL_DIR>
BUILD_COMMAND LD_LIBRARY_PATH=${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib make
BUILD_IN_SOURCE 1
INSTALL_COMMAND cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/bin/hogman3d ${CMAKE_SOURCE_DIR}/bin/ &&
cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_csparse.so ${CMAKE_SOURCE_DIR}/lib/ &&
cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_graph_optimizer_hogman.so ${CMAKE_SOURCE_DIR}/lib/ &&
cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_graph_optimizer.so ${CMAKE_SOURCE_DIR}/lib/ &&
cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_graph.so ${CMAKE_SOURCE_DIR}/lib/ &&
cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_graph_viewer.so ${CMAKE_SOURCE_DIR}/lib/ &&
cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_math.so ${CMAKE_SOURCE_DIR}/lib/ &&
cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_qglviewer.so ${CMAKE_SOURCE_DIR}/lib/ &&
cp ${CMAKE_SOURCE_DIR}/3rdparty/hogman/lib/libhogman_stuff.so ${CMAKE_SOURCE_DIR}/lib/
)
MESSAGE(STATUS "With HOGMAN: Currently hogman needs to be compiled manually, please make sure hogman3d is somewhere in your PATH")
ELSE(WITH_HOGMAN)
MESSAGE(STATUS "Without HOGMAN")
ENDIF(WITH_HOGMAN)
OPT_DEP(EXPORT_SHARED_LIBS "Whether to build additional shared libraries for use in other projects. ON/OFF" OFF "WITH_SHOW;WITH_FBR") OPT_DEP(EXPORT_SHARED_LIBS "Whether to build additional shared libraries for use in other projects. ON/OFF" OFF "WITH_SHOW;WITH_FBR")

View file

@ -36,7 +36,7 @@ class ScanColorManager {
static const unsigned int MODE_ANIMATION; static const unsigned int MODE_ANIMATION;
static const unsigned int MODE_POINT_COLOR; static const unsigned int MODE_POINT_COLOR;
ScanColorManager(unsigned int _buckets, PointType type); ScanColorManager(unsigned int _buckets, PointType type, bool animation_color = true);
void registerTree(colordisplay *b); void registerTree(colordisplay *b);
@ -80,6 +80,8 @@ class ScanColorManager {
/** maps valuetypes to point dimension for easier access */ /** maps valuetypes to point dimension for easier access */
PointType pointtype; PointType pointtype;
bool animationColor; /**< Alter colors when animating */
bool valid; bool valid;
bool colorScans; bool colorScans;
bool inverted; bool inverted;

View file

@ -42,6 +42,7 @@ public:
protected: protected:
virtual void createSearchTreePrivate(); virtual void createSearchTreePrivate();
virtual void calcReducedOnDemandPrivate(); virtual void calcReducedOnDemandPrivate();
virtual void calcNormalsOnDemandPrivate();
virtual void addFrame(AlgoType type); virtual void addFrame(AlgoType type);
private: private:

View file

@ -217,6 +217,7 @@ private:
typedef TripleArray<double> DataXYZ; typedef TripleArray<double> DataXYZ;
typedef TripleArray<float> DataXYZFloat; typedef TripleArray<float> DataXYZFloat;
typedef TripleArray<unsigned char> DataRGB; typedef TripleArray<unsigned char> DataRGB;
typedef TripleArray<double> DataNormal;
typedef SingleArray<float> DataReflectance; typedef SingleArray<float> DataReflectance;
typedef SingleArray<float> DataTemperature; typedef SingleArray<float> DataTemperature;
typedef SingleArray<float> DataAmplitude; typedef SingleArray<float> DataAmplitude;

View file

@ -273,8 +273,8 @@ inline void M4identity( T *M )
*/ */
template <class T> template <class T>
inline void MMult(const T *M1, inline void MMult(const T *M1,
const T *M2, const T *M2,
T *Mout) T *Mout)
{ {
Mout[ 0] = M1[ 0]*M2[ 0]+M1[ 4]*M2[ 1]+M1[ 8]*M2[ 2]+M1[12]*M2[ 3]; Mout[ 0] = M1[ 0]*M2[ 0]+M1[ 4]*M2[ 1]+M1[ 8]*M2[ 2]+M1[12]*M2[ 3];
Mout[ 1] = M1[ 1]*M2[ 0]+M1[ 5]*M2[ 1]+M1[ 9]*M2[ 2]+M1[13]*M2[ 3]; Mout[ 1] = M1[ 1]*M2[ 0]+M1[ 5]*M2[ 1]+M1[ 9]*M2[ 2]+M1[13]*M2[ 3];
@ -296,8 +296,8 @@ inline void MMult(const T *M1,
template <class T> template <class T>
inline void MMult(const T *M1, inline void MMult(const T *M1,
const T *M2, const T *M2,
float *Mout) float *Mout)
{ {
Mout[ 0] = M1[ 0]*M2[ 0]+M1[ 4]*M2[ 1]+M1[ 8]*M2[ 2]+M1[12]*M2[ 3]; Mout[ 0] = M1[ 0]*M2[ 0]+M1[ 4]*M2[ 1]+M1[ 8]*M2[ 2]+M1[12]*M2[ 3];
Mout[ 1] = M1[ 1]*M2[ 0]+M1[ 5]*M2[ 1]+M1[ 9]*M2[ 2]+M1[13]*M2[ 3]; Mout[ 1] = M1[ 1]*M2[ 0]+M1[ 5]*M2[ 1]+M1[ 9]*M2[ 2]+M1[13]*M2[ 3];
@ -326,9 +326,9 @@ inline void MMult(const T *M1,
template <class T> template <class T>
inline void VTrans(const T *M, const T *V, T *P) inline void VTrans(const T *M, const T *V, T *P)
{ {
P[0] = M[0] * V[0] + M[4] * V[1] + M[8] * V[2] + M[12]; P[0] = M[0] * V[0] + M[4] * V[1] + M[8] * V[2] + M[12];
P[1] = M[1] * V[0] + M[5] * V[1] + M[9] * V[2] + M[13]; P[1] = M[1] * V[0] + M[5] * V[1] + M[9] * V[2] + M[13];
P[2] = M[2] * V[0] + M[6] * V[1] + M[10] * V[2] + M[14]; P[2] = M[2] * V[0] + M[6] * V[1] + M[10] * V[2] + M[14];
} }
/** /**
@ -368,8 +368,8 @@ inline double M3det( const T *M )
{ {
double det; double det;
det = (double)(M[0] * ( M[4]*M[8] - M[7]*M[5] ) det = (double)(M[0] * ( M[4]*M[8] - M[7]*M[5] )
- M[1] * ( M[3]*M[8] - M[6]*M[5] ) - M[1] * ( M[3]*M[8] - M[6]*M[5] )
+ M[2] * ( M[3]*M[7] - M[6]*M[4] )); + M[2] * ( M[3]*M[7] - M[6]*M[4] ));
return ( det ); return ( det );
} }
@ -545,8 +545,8 @@ inline unsigned char randUC(unsigned char rnd)
inline double polardist(double* p, double *p2) { inline double polardist(double* p, double *p2) {
double stheta = sin(p[0]) * sin(p2[0]); double stheta = sin(p[0]) * sin(p2[0]);
double myd2 = acos( stheta * cos(p[1]) * cos(p2[1]) double myd2 = acos( stheta * cos(p[1]) * cos(p2[1])
+ stheta * sin(p[1]) * sin(p2[1]) + stheta * sin(p[1]) * sin(p2[1])
+ cos(p[0]) * cos(p2[0])); + cos(p[0]) * cos(p2[0]));
return myd2; return myd2;
} }
@ -569,8 +569,8 @@ inline void toPolar(double *n, double *polar) {
rho = Len(n); rho = Len(n);
Normalize3(n); Normalize3(n);
// if(fabs(1 - fabs(n[1])) < 0.001) { // if(fabs(1 - fabs(n[1])) < 0.001) {
// cout << "Y " << n[0] << " " << n[1] << " " << n[2] << endl; // cout << "Y " << n[0] << " " << n[1] << " " << n[2] << endl;
phi = acos(n[2]); phi = acos(n[2]);
//if ( fabs(phi) < 0.0001) phi = 0.0001; //if ( fabs(phi) < 0.0001) phi = 0.0001;
@ -609,7 +609,7 @@ inline void toPolar(double *n, double *polar) {
} }
} }
/* } else { /* } else {
theta = 0.0; theta = 0.0;
phi = 0.0; phi = 0.0;
}*/ }*/
@ -634,11 +634,11 @@ static inline void M4_submat(const T *Min, T *Mout, int i, int j ) {
// loop through 3x3 submatrix // loop through 3x3 submatrix
for( di = 0; di < 3; di ++ ) { for( di = 0; di < 3; di ++ ) {
for( dj = 0; dj < 3; dj ++ ) { for( dj = 0; dj < 3; dj ++ ) {
// map 3x3 element (destination) to 4x4 element (source) // map 3x3 element (destination) to 4x4 element (source)
si = di + ( ( di >= i ) ? 1 : 0 ); si = di + ( ( di >= i ) ? 1 : 0 );
sj = dj + ( ( dj >= j ) ? 1 : 0 ); sj = dj + ( ( dj >= j ) ? 1 : 0 );
// copy element // copy element
Mout[di * 3 + dj] = Min[si * 4 + sj]; Mout[di * 3 + dj] = Min[si * 4 + sj];
} }
} }
} }
@ -684,9 +684,9 @@ static inline int M4inv(const T *Min, T *Mout )
int i, j, sign; int i, j, sign;
for ( i = 0; i < 4; i++ ) { for ( i = 0; i < 4; i++ ) {
for ( j = 0; j < 4; j++ ) { for ( j = 0; j < 4; j++ ) {
sign = 1 - ( (i +j) % 2 ) * 2; sign = 1 - ( (i +j) % 2 ) * 2;
M4_submat( Min, mtemp, i, j ); M4_submat( Min, mtemp, i, j );
Mout[i+j*4] = ( M3det( mtemp ) * sign ) / mdet; Mout[i+j*4] = ( M3det( mtemp ) * sign ) / mdet;
} }
} }
return( 1 ); return( 1 );
@ -787,9 +787,9 @@ static inline bool choldc(unsigned int n, double **A, double *diag)
* Solve Ax=B after choldc * Solve Ax=B after choldc
* +++++++++-------------++++++++++++ */ * +++++++++-------------++++++++++++ */
static inline void cholsl(double A[3][3], static inline void cholsl(double A[3][3],
double diag[3], double diag[3],
double B[3], double B[3],
double x[3]) double x[3])
{ {
for (int i=0; i < 3; i++) { for (int i=0; i < 3; i++) {
double sum = B[i]; double sum = B[i];
@ -813,10 +813,10 @@ static inline void cholsl(double A[3][3],
* Solve Ax=B after choldc * Solve Ax=B after choldc
* +++++++++-------------++++++++++++ */ * +++++++++-------------++++++++++++ */
static inline void cholsl(unsigned int n, static inline void cholsl(unsigned int n,
double **A, double **A,
double *diag, double *diag,
double *B, double *B,
double *x) double *x)
{ {
for (unsigned int i=0; i < n; i++) { for (unsigned int i=0; i < n; i++) {
double sum = B[i]; double sum = B[i];
@ -1090,63 +1090,63 @@ inline int LU_factor( double A[4][4], int indx[4])
int minMN = 4; int minMN = 4;
for (j = 0; j < minMN; j++) for (j = 0; j < minMN; j++)
{
// find pivot in column j and test for singularity.
jp = j;
t = fabs(A[j][j]);
for (i = j+1; i < M; i++)
if ( fabs(A[i][j]) > t)
{
jp = i;
t = fabs(A[i][j]);
}
indx[j] = jp;
// jp now has the index of maximum element
// of column j, below the diagonal
if ( A[jp][j] == 0 )
return 1; // factorization failed because of zero pivot
if (jp != j) // swap rows j and jp
for (k = 0; k < N; k++)
{
t = A[j][k];
A[j][k] = A[jp][k];
A[jp][k] =t;
}
if (j < M) // compute elements j+1:M of jth column
{ {
// note A(j,j), was A(jp,p) previously which was
// guarranteed not to be zero (Label #1)
//
double recp = 1.0 / A[j][j];
// find pivot in column j and test for singularity. for (k = j+1; k < M; k++)
A[k][j] *= recp;
jp = j;
t = fabs(A[j][j]);
for (i = j+1; i < M; i++)
if ( fabs(A[i][j]) > t)
{
jp = i;
t = fabs(A[i][j]);
}
indx[j] = jp;
// jp now has the index of maximum element
// of column j, below the diagonal
if ( A[jp][j] == 0 )
return 1; // factorization failed because of zero pivot
if (jp != j) // swap rows j and jp
for (k = 0; k < N; k++)
{
t = A[j][k];
A[j][k] = A[jp][k];
A[jp][k] =t;
}
if (j < M) // compute elements j+1:M of jth column
{
// note A(j,j), was A(jp,p) previously which was
// guarranteed not to be zero (Label #1)
//
double recp = 1.0 / A[j][j];
for (k = j+1; k < M; k++)
A[k][j] *= recp;
}
if (j < minMN)
{
// rank-1 update to trailing submatrix: E = E - x*y;
//
// E is the region A(j+1:M, j+1:N)
// x is the column vector A(j+1:M,j)
// y is row vector A(j,j+1:N)
int ii,jj;
for (ii = j+1; ii < M; ii++)
for (jj = j+1; jj < N; jj++)
A[ii][jj] -= A[ii][j]*A[j][jj];
}
} }
if (j < minMN)
{
// rank-1 update to trailing submatrix: E = E - x*y;
//
// E is the region A(j+1:M, j+1:N)
// x is the column vector A(j+1:M,j)
// y is row vector A(j,j+1:N)
int ii,jj;
for (ii = j+1; ii < M; ii++)
for (jj = j+1; jj < N; jj++)
A[ii][jj] -= A[ii][j]*A[j][jj];
}
}
return 0; return 0;
} }
@ -1167,23 +1167,23 @@ inline int LU_solve(const double A[4][4], const int indx[4], double b[4])
double sum = 0.0; double sum = 0.0;
for (i = 0; i < n; i++) for (i = 0; i < n; i++)
{ {
ip=indx[i]; ip=indx[i];
sum=b[ip]; sum=b[ip];
b[ip]=b[i]; b[ip]=b[i];
if (ii) if (ii)
for (j = ii;j <= i-1; j++) for (j = ii;j <= i-1; j++)
sum -= A[i][j]*b[j]; sum -= A[i][j]*b[j];
else if (sum) ii=i; else if (sum) ii=i;
b[i]=sum; b[i]=sum;
} }
for (i = n-1; i >= 0; i--) for (i = n-1; i >= 0; i--)
{ {
sum=b[i]; sum=b[i];
for (j = i+1; j < n; j++) for (j = i+1; j < n; j++)
sum -= A[i][j]*b[j]; sum -= A[i][j]*b[j];
b[i]=sum/A[i][i]; b[i]=sum/A[i][i];
} }
return 0; return 0;
} }
@ -1273,10 +1273,10 @@ static inline void RPYEulerQuat(const double *euler, double *quat)
double cpsi = cos(euler[2]); double cpsi = cos(euler[2]);
double _r[3][3] = { //create rotational Matrix double _r[3][3] = { //create rotational Matrix
{cpsi*ctheta, cpsi*stheta*sphi - spsi*cphi, cpsi*stheta*cphi + spsi*sphi}, {cpsi*ctheta, cpsi*stheta*sphi - spsi*cphi, cpsi*stheta*cphi + spsi*sphi},
{spsi*ctheta, spsi*stheta*sphi + cpsi*cphi, spsi*stheta*cphi - cpsi*sphi}, {spsi*ctheta, spsi*stheta*sphi + cpsi*cphi, spsi*stheta*cphi - cpsi*sphi},
{ -stheta, ctheta*sphi, ctheta*cphi} { -stheta, ctheta*sphi, ctheta*cphi}
}; };
#define MY_MAX(a,b) (((a)>(b))?(a):(b)) #define MY_MAX(a,b) (((a)>(b))?(a):(b))
double _w = sqrt(MY_MAX(0, 1 + _r[0][0] + _r[1][1] + _r[2][2]))/2.0; double _w = sqrt(MY_MAX(0, 1 + _r[0][0] + _r[1][1] + _r[2][2]))/2.0;
@ -1301,6 +1301,18 @@ inline void transform3(const double *alignxf, double *point)
point[2] = z_neu + alignxf[14]; point[2] = z_neu + alignxf[14];
} }
inline void transform3normal(const double *alignxf, double *normal)
{
double x, y, z;
x = normal[0] * alignxf[0] + normal[1] * alignxf[1] + normal[2] * alignxf[2];
y = normal[0] * alignxf[4] + normal[1] * alignxf[5] + normal[2] * alignxf[6];
z = normal[0] * alignxf[8] + normal[1] * alignxf[9] + normal[2] * alignxf[10];
normal[0] = x;
normal[1] = y;
normal[2] = z;
}
inline void transform3(const double *alignxf, const double *point, double *tpoint) inline void transform3(const double *alignxf, const double *point, double *tpoint)
{ {
tpoint[0] = point[0] * alignxf[0] + point[1] * alignxf[4] + point[2] * alignxf[8] + alignxf[12]; tpoint[0] = point[0] * alignxf[0] + point[1] * alignxf[4] + point[2] * alignxf[8] + alignxf[12];
@ -1308,6 +1320,28 @@ inline void transform3(const double *alignxf, const double *point, double *tpoin
tpoint[2] = point[0] * alignxf[2] + point[1] * alignxf[6] + point[2] * alignxf[10] + alignxf[14]; tpoint[2] = point[0] * alignxf[2] + point[1] * alignxf[6] + point[2] * alignxf[10] + alignxf[14];
} }
inline void scal_mul3(const double *vec_in, const double scalar, double *vec_out)
{
vec_out[0] = vec_in[0] * scalar;
vec_out[1] = vec_in[1] * scalar;
vec_out[2] = vec_in[2] * scalar;
}
inline void sub3(const double *vec1_in, const double *vec2_in, double *vec_out)
{
vec_out[0] = vec1_in[0] - vec2_in[0];
vec_out[1] = vec1_in[1] - vec2_in[1];
vec_out[2] = vec1_in[2] - vec2_in[2];
}
inline void add3(const double *vec1_in, const double *vec2_in, double *vec_out)
{
vec_out[0] = vec1_in[0] + vec2_in[0];
vec_out[1] = vec1_in[1] + vec2_in[1];
vec_out[2] = vec1_in[2] + vec2_in[2];
}
inline std::string trim(const std::string& source) inline std::string trim(const std::string& source)
{ {
unsigned int start = 0, end = source.size() - 1; unsigned int start = 0, end = source.size() - 1;

View file

@ -1,61 +0,0 @@
/**
* @file HOG-Man wrapper
* @author Jochen Sprickerhof. Institute of Computer Science, University of Osnabrueck, Germany.
*/
#ifndef __GRAPH_HOGMAN_H__
#define __GRAPH_HOGMAN_H__
#include "graphSlam6D.h"
class graphHOGMan : public graphSlam6D {
public:
/**
* Constructor (default)
*/
graphHOGMan() {};
/**
* Constructor
*
* @param my_icp6Dminimizer Pointer to ICP minimization functor
* @param mdm Maximum PtoP distance to which point pairs are collected for ICP
* @param max_dist_match Maximum PtoP distance to which point pairs are collected for LUM
* @param max_num_iterations Maximal number of iterations for ICP
* @param quiet Suspesses all output to std out
* @param meta Indicates if metascan matching has to be used
* @param rnd Indicates if randomization has to be used
* @param eP Extrapolate odometry?
* @param anim Animate which frames?
* @param epsilonICP Termination criterion for ICP
* @param nns_method Which nearest neigbor search method shall we use
* @param epsilonLUM Termination criterion for LUM
*/
graphHOGMan(icp6Dminimizer *my_icp6Dminimizer,
double mdm = 25.0,
double max_dist_match = 25.0,
int max_num_iterations = 50,
bool quiet = false,
bool meta = false,
int rnd = 1,
bool eP = true,
int anim = -1,
double epsilonICP = 0.0000001,
int nns_method = simpleKD,
double epsilonLUM = 0.5)
: graphSlam6D(my_icp6Dminimizer,
mdm, max_dist_match,
max_num_iterations, quiet, meta, rnd,
eP, anim, epsilonICP, nns_method, epsilonLUM)
{ }
virtual ~graphHOGMan()
{
delete my_icp;
}
double doGraphSlam6D(Graph gr, vector <Scan*> MetaScan, int nrIt);
};
#endif

View file

@ -1,61 +0,0 @@
/**
* @file TORO wrapper
* @author Jochen Sprickerhof. Institute of Computer Science, University of Osnabrueck, Germany.
*/
#ifndef __GRAPH_TORO_H__
#define __GRAPH_TORO_H__
#include "graphSlam6D.h"
class graphToro : public graphSlam6D {
public:
/**
* Constructor (default)
*/
graphToro() {};
/**
* Constructor
*
* @param my_icp6Dminimizer Pointer to ICP minimization functor
* @param mdm Maximum PtoP distance to which point pairs are collected for ICP
* @param max_dist_match Maximum PtoP distance to which point pairs are collected for LUM
* @param max_num_iterations Maximal number of iterations for ICP
* @param quiet Suspesses all output to std out
* @param meta Indicates if metascan matching has to be used
* @param rnd Indicates if randomization has to be used
* @param eP Extrapolate odometry?
* @param anim Animate which frames?
* @param epsilonICP Termination criterion for ICP
* @param nns_method Which nearest neigbor search method shall we use
* @param epsilonLUM Termination criterion for LUM
*/
graphToro(icp6Dminimizer *my_icp6Dminimizer,
double mdm = 25.0,
double max_dist_match = 25.0,
int max_num_iterations = 50,
bool quiet = false,
bool meta = false,
int rnd = 1,
bool eP = true,
int anim = -1,
double epsilonICP = 0.0000001,
int nns_method = simpleKD,
double epsilonLUM = 0.5)
: graphSlam6D(my_icp6Dminimizer,
mdm, max_dist_match,
max_num_iterations, quiet, meta, rnd,
eP, anim, epsilonICP, nns_method, epsilonLUM)
{ }
virtual ~graphToro()
{
delete my_icp;
}
double doGraphSlam6D(Graph gr, vector <Scan*> MetaScan, int nrIt);
};
#endif

View file

@ -15,6 +15,7 @@ using std::vector;
#include "slam6d/scan.h" #include "slam6d/scan.h"
#include "slam6d/icp6Dminimizer.h" #include "slam6d/icp6Dminimizer.h"
#include "slam6d/pairingMode.h"
/** /**
* @brief Representation of 3D scan matching with ICP. * @brief Representation of 3D scan matching with ICP.
@ -38,15 +39,15 @@ public:
double epsilonICP = 0.0000001, double epsilonICP = 0.0000001,
int nns_method = simpleKD, int nns_method = simpleKD,
bool cuda_enabled = false, bool cuda_enabled = false,
bool cad_matching = false); bool cad_matching = false);
/** /**
* Destructor (empty, but needed, because virtual) * Destructor (empty, but needed, because virtual)
*/ */
virtual ~icp6D() {}; virtual ~icp6D() {};
void doICP(vector <Scan *> allScans); void doICP(vector <Scan *> allScans, PairingMode pairing_mode = CLOSEST_POINT);
virtual int match(Scan* PreviousScan, Scan* CurrentScan); virtual int match(Scan* PreviousScan, Scan* CurrentScan, PairingMode pairing_mode = CLOSEST_POINT);
void covarianceEuler(Scan *scan1, Scan *scan2, NEWMAT::Matrix *C); void covarianceEuler(Scan *scan1, Scan *scan2, NEWMAT::Matrix *C);
void covarianceQuat(Scan *scan1, Scan *scan2, NEWMAT::Matrix *C); void covarianceQuat(Scan *scan1, Scan *scan2, NEWMAT::Matrix *C);
double Point_Point_Error(Scan* PreviousScan, Scan* CurrentScan, double max_dist_match, unsigned int *nrp=0); double Point_Point_Error(Scan* PreviousScan, Scan* CurrentScan, double max_dist_match, unsigned int *nrp=0);

View file

@ -45,6 +45,8 @@ public:
virtual ~KDtree(); virtual ~KDtree();
virtual double *FindClosest(double *_p, double maxdist2, int threadNum = 0) const; virtual double *FindClosest(double *_p, double maxdist2, int threadNum = 0) const;
virtual double *FindClosestAlongDir(double *_p, double *_dir, double maxdist2, int threadNum = 0) const;
}; };
#endif #endif

View file

@ -55,6 +55,8 @@ public:
//! Aquires cached data first to pass on to the usual KDtree to process //! Aquires cached data first to pass on to the usual KDtree to process
virtual double* FindClosest(double *_p, double maxdist2, int threadNum = 0) const; virtual double* FindClosest(double *_p, double maxdist2, int threadNum = 0) const;
virtual double *FindClosestAlongDir(double *_p, double *_dir, double maxdist2, int threadNum = 0) const;
private: private:
Scan* m_scan; Scan* m_scan;
DataXYZ* m_data; DataXYZ* m_data;

View file

@ -62,6 +62,8 @@ public:
//! Aquires cached data first to pass on to the usual KDtree to process //! Aquires cached data first to pass on to the usual KDtree to process
virtual double* FindClosest(double *_p, double maxdist2, int threadNum = 0) const; virtual double* FindClosest(double *_p, double maxdist2, int threadNum = 0) const;
virtual double *FindClosestAlongDir(double *_p, double *_dir, double maxdist2, int threadNum = 0) const;
private: private:
Scan** m_scans; Scan** m_scans;
DataXYZ** m_data; DataXYZ** m_data;

View file

@ -178,9 +178,9 @@ protected:
struct { struct {
double center[3]; ///< storing the center of the voxel (R^3) double center[3]; ///< storing the center of the voxel (R^3)
double dx, ///< defining the voxel itself double dx, ///< defining the voxel itself
dy, ///< defining the voxel itself dy, ///< defining the voxel itself
dz, ///< defining the voxel itself dz, ///< defining the voxel itself
r2; ///< defining the voxel itself r2; ///< defining the voxel itself
int splitaxis; ///< defining the kind of splitaxis int splitaxis; ///< defining the kind of splitaxis
KDTreeImpl *child1; ///< pointers to the childs KDTreeImpl *child1; ///< pointers to the childs
KDTreeImpl *child2; ///< pointers to the childs KDTreeImpl *child2; ///< pointers to the childs
@ -214,8 +214,8 @@ protected:
// Quick check of whether to abort // Quick check of whether to abort
double approx_dist_bbox = max(max(fabs(params[threadNum].p[0]-node.center[0])-node.dx, double approx_dist_bbox = max(max(fabs(params[threadNum].p[0]-node.center[0])-node.dx,
fabs(params[threadNum].p[1]-node.center[1])-node.dy), fabs(params[threadNum].p[1]-node.center[1])-node.dy),
fabs(params[threadNum].p[2]-node.center[2])-node.dz); fabs(params[threadNum].p[2]-node.center[2])-node.dz);
if (approx_dist_bbox >= 0 && sqr(approx_dist_bbox) >= params[threadNum].closest_d2) if (approx_dist_bbox >= 0 && sqr(approx_dist_bbox) >= params[threadNum].closest_d2)
return; return;
@ -233,6 +233,44 @@ protected:
} }
} }
} }
void _FindClosestAlongDir(const PointData& pts, int threadNum) const {
AccessorFunc point;
// Leaf nodes
if (npts) {
for (int i=0; i < npts; i++) {
double p2p[] = { params[threadNum].p[0] - point(pts, leaf.p[i])[0],
params[threadNum].p[1] - point(pts, leaf.p[i])[1],
params[threadNum].p[2] - point(pts, leaf.p[i])[2] };
double myd2 = Len2(p2p) - sqr(Dot(p2p, params[threadNum].dir));
if ((myd2 < params[threadNum].closest_d2)) {
params[threadNum].closest_d2 = myd2;
params[threadNum].closest = point(pts, leaf.p[i]);
}
}
return;
}
// Quick check of whether to abort
double p2c[] = { params[threadNum].p[0] - node.center[0],
params[threadNum].p[1] - node.center[1],
params[threadNum].p[2] - node.center[2] };
double myd2center = Len2(p2c) - sqr(Dot(p2c, params[threadNum].dir));
if (myd2center > node.r2 + params[threadNum].closest_d2 + 2.0f * max(node.r2, params[threadNum].closest_d2))
return;
// Recursive case
if (params[threadNum].p[node.splitaxis] < node.center[node.splitaxis] ) {
node.child1->_FindClosestAlongDir(pts, threadNum);
node.child2->_FindClosestAlongDir(pts, threadNum);
} else {
node.child2->_FindClosestAlongDir(pts, threadNum);
node.child1->_FindClosestAlongDir(pts, threadNum);
}
}
}; };
#endif #endif

View file

@ -32,6 +32,11 @@ public:
*/ */
double *p; double *p;
/**
* pointer to direction vector, if we're using FindClosestAlongDir
*/
double *dir;
/** /**
* expand to 128 bytes to avoid false-sharing, 16 bytes from above + 28*4 bytes = 128 bytes * expand to 128 bytes to avoid false-sharing, 16 bytes from above + 28*4 bytes = 128 bytes
*/ */

View file

@ -1,21 +0,0 @@
/**
* @file HOG-Man wrapper
* @author Jochen Sprickerhof. Institute of Computer Science, University of Osnabrueck, Germany.
*/
#ifndef __LOOP_HOG_MAN_H__
#define __LOOP_HOG_MAN_H__
#include "loopSlam6D.h"
class loopHOGMan : public loopSlam6D {
public:
loopHOGMan(bool _quiet, icp6Dminimizer *my_icp6Dminimizer, double mdm, int max_num_iterations,
int rnd, bool eP, int anim, double epsilonICP, int nns_method)
: loopSlam6D(_quiet, my_icp6Dminimizer, mdm, max_num_iterations, rnd, eP, anim, epsilonICP, nns_method) {}
virtual void close_loop(const vector <Scan *> &allScans, int first, int last, graph_t &g);
};
#endif

View file

@ -1,21 +0,0 @@
/**
* @file TORO wrapper
* @author Jochen Sprickerhof. Institute of Computer Science, University of Osnabrueck, Germany.
*/
#ifndef __LOOP_TORO_H__
#define __LOOP_TORO_H__
#include "loopSlam6D.h"
class loopToro : public loopSlam6D {
public:
loopToro(bool _quiet, icp6Dminimizer *my_icp6Dminimizer, double mdm, int max_num_iterations,
int rnd, bool eP, int anim, double epsilonICP, int nns_method)
: loopSlam6D(_quiet, my_icp6Dminimizer, mdm, max_num_iterations, rnd, eP, anim, epsilonICP, nns_method) {}
virtual void close_loop(const vector <Scan *> &allScans, int first, int last, graph_t &g);
};
#endif

View file

@ -43,6 +43,7 @@ public:
protected: protected:
virtual void createSearchTreePrivate(); virtual void createSearchTreePrivate();
virtual void calcReducedOnDemandPrivate(); virtual void calcReducedOnDemandPrivate();
virtual void calcNormalsOnDemandPrivate() {};
virtual void addFrame(AlgoType type); virtual void addFrame(AlgoType type);
private: private:

View file

@ -34,6 +34,7 @@ public:
protected: protected:
virtual void createSearchTreePrivate(); virtual void createSearchTreePrivate();
virtual void calcReducedOnDemandPrivate() {} virtual void calcReducedOnDemandPrivate() {}
virtual void calcNormalsOnDemandPrivate() {}
virtual void addFrame(AlgoType type) {} virtual void addFrame(AlgoType type) {}
private: private:
std::vector<Scan*> m_scans; std::vector<Scan*> m_scans;

View file

@ -101,6 +101,12 @@ public:
double y; double y;
/// z coordinate in 3D space /// z coordinate in 3D space
double z; double z;
/// normal x direction in 3D space
double nx;
/// normal x direction in 3D space
double ny;
/// normal x direction in 3D space
double nz;
/// additional information about the point, e.g., semantic /// additional information about the point, e.g., semantic
/// also used in veloscan for distiuguish moving or static /// also used in veloscan for distiuguish moving or static
int type; int type;

View file

@ -30,6 +30,7 @@ public:
static const unsigned int USE_NONE; static const unsigned int USE_NONE;
static const unsigned int USE_REFLECTANCE; static const unsigned int USE_REFLECTANCE;
static const unsigned int USE_NORMAL;
static const unsigned int USE_TEMPERATURE; static const unsigned int USE_TEMPERATURE;
static const unsigned int USE_AMPLITUDE; static const unsigned int USE_AMPLITUDE;
static const unsigned int USE_DEVIATION; static const unsigned int USE_DEVIATION;
@ -44,6 +45,7 @@ public:
PointType(unsigned int _types); PointType(unsigned int _types);
bool hasReflectance(); bool hasReflectance();
bool hasNormal();
bool hasTemperature(); bool hasTemperature();
bool hasAmplitude(); bool hasAmplitude();
bool hasDeviation(); bool hasDeviation();
@ -114,6 +116,7 @@ private:
unsigned int getScanSize(Scan* scan); unsigned int getScanSize(Scan* scan);
DataXYZ* m_xyz; DataXYZ* m_xyz;
DataXYZ* m_normal;
DataRGB* m_rgb; DataRGB* m_rgb;
DataReflectance* m_reflectance; DataReflectance* m_reflectance;
DataTemperature* m_temperature; DataTemperature* m_temperature;
@ -134,6 +137,11 @@ T *PointType::createPoint(const Point &P, unsigned int index ) {
if (types & USE_REFLECTANCE) { if (types & USE_REFLECTANCE) {
p[counter++] = P.reflectance; p[counter++] = P.reflectance;
} }
if (types & USE_NORMAL) {
p[counter++] = P.nx;
p[counter++] = P.ny;
p[counter++] = P.nz;
}
if (types & USE_TEMPERATURE) { if (types & USE_TEMPERATURE) {
p[counter++] = P.temperature; p[counter++] = P.temperature;
} }
@ -171,6 +179,11 @@ Point PointType::createPoint(T *p) {
if (types & USE_REFLECTANCE) { if (types & USE_REFLECTANCE) {
P.reflectance = p[counter++]; P.reflectance = p[counter++];
} }
if (types & USE_NORMAL) {
p[counter++] = P.nx;
p[counter++] = P.ny;
p[counter++] = P.nz;
}
if (types & USE_TEMPERATURE) { if (types & USE_TEMPERATURE) {
P.temperature = p[counter++]; P.temperature = p[counter++];
} }
@ -206,6 +219,10 @@ T *PointType::createPoint(unsigned int i, unsigned int index) {
if (types & USE_REFLECTANCE) { if (types & USE_REFLECTANCE) {
p[counter++] = (m_reflectance? (*m_reflectance)[i]: 0); p[counter++] = (m_reflectance? (*m_reflectance)[i]: 0);
} }
if (types & USE_NORMAL) {
for(unsigned int j = 0; j < 3; ++j)
p[counter++] = (*m_normal)[i][j];
}
if (types & USE_TEMPERATURE) { if (types & USE_TEMPERATURE) {
p[counter++] = (m_temperature? (*m_temperature)[i]: 0); p[counter++] = (m_temperature? (*m_temperature)[i]: 0);
} }

View file

@ -5,6 +5,7 @@
#include "data_types.h" #include "data_types.h"
#include "point_type.h" #include "point_type.h"
#include "ptpair.h" #include "ptpair.h"
#include "pairingMode.h"
#include <string> #include <string>
#include <vector> #include <vector>
@ -97,10 +98,7 @@ Last, if program ends, clean up
class Scan { class Scan {
//friend class SearchTree; // TODO: is this neccessary? //friend class SearchTree; // TODO: is this neccessary?
public: public:
enum AlgoType { enum AlgoType { INVALID, ICP, ICPINACTIVE, LUM, ELCH };
INVALID, ICP, ICPINACTIVE, LUM, ELCH, LOOPTORO, LOOPHOGMAN, GRAPHTORO,
GRAPHHOGMAN
};
// delete copy-ctor and assignment, scans shouldn't be copied by basic class // delete copy-ctor and assignment, scans shouldn't be copied by basic class
Scan(const Scan& other) = delete; Scan(const Scan& other) = delete;
@ -108,7 +106,8 @@ public:
virtual ~Scan(); virtual ~Scan();
//! Holder of all scans - also used in transform for adding frames for each scan at the same time //! Holder of all scans
// also used in transform for adding frames for each scan at the same time
static std::vector<Scan*> allScans; static std::vector<Scan*> allScans;
/** /**
@ -286,7 +285,7 @@ public:
Scan* Source, Scan* Target, Scan* Source, Scan* Target,
int thread_num, int thread_num,
int rnd, double max_dist_match2, double &sum, int rnd, double max_dist_match2, double &sum,
double *centroid_m, double *centroid_d); double *centroid_m, double *centroid_d, PairingMode pairing_mode = CLOSEST_POINT);
static void getNoPairsSimple(std::vector<double*> &diff, static void getNoPairsSimple(std::vector<double*> &diff,
Scan* Source, Scan* Target, Scan* Source, Scan* Target,
int thread_num, int thread_num,
@ -302,7 +301,8 @@ public:
int rnd, double max_dist_match2, int rnd, double max_dist_match2,
double *sum, double *sum,
double centroid_m[OPENMP_NUM_THREADS][3], double centroid_m[OPENMP_NUM_THREADS][3],
double centroid_d[OPENMP_NUM_THREADS][3]); double centroid_d[OPENMP_NUM_THREADS][3],
PairingMode pairing_mode);
protected: protected:
/** /**
@ -352,6 +352,9 @@ protected:
//! Flag whether "xyz reduced" has been initialized for this Scan yet //! Flag whether "xyz reduced" has been initialized for this Scan yet
bool m_has_reduced; bool m_has_reduced;
//! Flag whether "normals" has been initialized for this Scan yet
bool m_has_normals;
//! Reduction value used for octtree input //! Reduction value used for octtree input
double octtree_reduction_voxelSize; double octtree_reduction_voxelSize;
@ -372,19 +375,31 @@ protected:
/** /**
* This function handles the reduction of points. It builds a lock for * This function handles the reduction of points. It builds a lock for
* multithread-safety and calls caldReducedOnDemandPrivate. * multithread-safety and calls calcReducedOnDemandPrivate.
* *
* The intention is to reduce points, transforme them to the initial pose and * The intention is to reduce points, transforme them to the initial pose and
* then copy them to original for the SearchTree. * then copy them to original for the SearchTree.
*/ */
void calcReducedOnDemand(); void calcReducedOnDemand();
/**
* This function handles the computation of the normals. It builds a lock for
* multithread-safety and calls caldNormalsOnDemandPrivate.
*/
void calcNormalsOnDemand();
//! Create specific SearchTree variants matching the capability of the Scan //! Create specific SearchTree variants matching the capability of the Scan
virtual void createSearchTreePrivate() = 0; virtual void createSearchTreePrivate() = 0;
//! Create reduced points in a multithread-safe environment matching the capability of the Scan //! Create reduced points in a multithread-safe environment matching the capability of the Scan
virtual void calcReducedOnDemandPrivate() = 0; virtual void calcReducedOnDemandPrivate() = 0;
//! Create normals in a multithread-safe environment matching the capability of the Scan
virtual void calcNormalsOnDemandPrivate() = 0;
//! Creating normals
void calcNormals();
//! Internal function of transform which alters the reduced points //! Internal function of transform which alters the reduced points
void transformReduced(const double alignxf[16]); void transformReduced(const double alignxf[16]);
@ -392,11 +407,11 @@ protected:
void transformMatrix(const double alignxf[16]); void transformMatrix(const double alignxf[16]);
//@FIXME //@FIXME
public: public:
//! Creating reduced points //! Creating reduced points
void calcReducedPoints(); void calcReducedPoints();
protected: protected:
//! Copies reduced points to original points without any transformation. //! Copies reduced points to original points without any transformation.
void copyReducedToOriginal(); void copyReducedToOriginal();
@ -410,7 +425,7 @@ private:
public: public:
//! Mutex for safely reducing points and creating the search tree just once in a multithreaded environment //! Mutex for safely reducing points and creating the search tree just once in a multithreaded environment
// it can not be compiled in win32 use boost 1.48, therefore we remeove it temporarily // it can not be compiled in win32 use boost 1.48, therefore we remeove it temporarily
boost::mutex m_mutex_reduction, m_mutex_create_tree; boost::mutex m_mutex_reduction, m_mutex_create_tree, m_mutex_normals;
}; };
#include "scan.icc" #include "scan.icc"

View file

@ -12,6 +12,7 @@ using std::vector;
#include "ptpair.h" #include "ptpair.h"
#include "data_types.h" #include "data_types.h"
#include "pairingMode.h"
/** /**
* @brief The tree structure * @brief The tree structure
@ -38,6 +39,7 @@ class Scan;
class SearchTree : public Tree { class SearchTree : public Tree {
friend class Scan; friend class Scan;
public: public:
/** /**
* Constructor (default) * Constructor (default)
*/ */
@ -77,7 +79,9 @@ public:
* @param threadNum If parallel threads share the search tree the thread num must be given * @param threadNum If parallel threads share the search tree the thread num must be given
* @return Pointer to closest point * @return Pointer to closest point
*/ */
virtual double *FindClosest(double *_p, double maxdist2, int threadNum = 0) const = 0 ; virtual double *FindClosest(double *_p, double maxdist2, int threadNum = 0) const = 0;
virtual double *FindClosestAlongDir(double *_p, double *_dir, double maxdist2, int threadNum) const;
virtual void getPtPairs(vector <PtPair> *pairs, virtual void getPtPairs(vector <PtPair> *pairs,
double *source_alignxf, double *source_alignxf,
@ -88,10 +92,10 @@ public:
virtual void getPtPairs(vector <PtPair> *pairs, virtual void getPtPairs(vector <PtPair> *pairs,
double *source_alignxf, double *source_alignxf,
const DataXYZ& xyz_r, unsigned int startindex, unsigned int endindex, const DataXYZ& xyz_r, const DataNormal& normal_r, unsigned int startindex, unsigned int endindex,
int thread_num, int thread_num,
int rnd, double max_dist_match2, double &sum, int rnd, double max_dist_match2, double &sum,
double *centroid_m, double *centroid_d); double *centroid_m, double *centroid_d, PairingMode pairing_mode = CLOSEST_POINT);
}; };
#endif #endif

View file

@ -1,7 +1,9 @@
IF(WITH_NORMALS) IF(WITH_NORMALS)
add_executable(normals normals.cc)
FIND_PACKAGE(OpenCV REQUIRED) FIND_PACKAGE(OpenCV REQUIRED)
target_link_libraries(normals newmat scan ANN fbr_cv_io fbr_panorama ${Boost_LIBRARIES} ${OpenCV_LIBS}) add_library(normals normals.cc)
target_link_libraries(normals newmat scan ANN fbr_cv_io fbr_panorama ${OpenCV_LIBS})
add_executable(calc_normals calc_normals.cc)
target_link_libraries(calc_normals normals ${Boost_LIBRARIES})
ENDIF(WITH_NORMALS) ENDIF(WITH_NORMALS)

View file

@ -3,123 +3,27 @@
* Copyright (C) Jacobs University Bremen * Copyright (C) Jacobs University Bremen
* *
* @author Vaibhav Kumar Mehta * @author Vaibhav Kumar Mehta
* @author Corneliu Claudiu Prodescu
* @file normals.cc * @file normals.cc
*/ */
#include <iostream> #include <vector>
#include <string>
#include <fstream>
#include <errno.h>
#include <boost/program_options.hpp>
#include <ANN/ANN.h>
#include <slam6d/io_types.h> #include <slam6d/io_types.h>
#include <slam6d/globals.icc> #include <slam6d/globals.icc>
#include <slam6d/scan.h> #include <slam6d/scan.h>
#include "slam6d/fbr/panorama.h" #include "slam6d/fbr/panorama.h"
#include <scanserver/clientInterface.h> #include <scanserver/clientInterface.h>
#include <ANN/ANN.h>
#include "newmat/newmat.h" #include "newmat/newmat.h"
#include "newmat/newmatap.h" #include "newmat/newmatap.h"
#include "normals/normals.h"
using namespace NEWMAT; using namespace NEWMAT;
#ifdef _MSC_VER
#define strcasecmp _stricmp
#define strncasecmp _strnicmp
#else
#include <strings.h>
#endif
namespace po = boost::program_options;
using namespace std; using namespace std;
enum normal_method {AKNN, ADAPTIVE_AKNN, PANORAMA, PANORAMA_FAST};
/*
* validates normal calculation method specification
*/
void validate(boost::any& v, const std::vector<std::string>& values,
normal_method*, int) {
if (values.size() == 0)
throw std::runtime_error("Invalid model specification");
string arg = values.at(0);
if(strcasecmp(arg.c_str(), "AKNN") == 0) v = AKNN;
else if(strcasecmp(arg.c_str(), "ADAPTIVE_AKNN") == 0) v = ADAPTIVE_AKNN;
else if(strcasecmp(arg.c_str(), "PANORAMA") == 0) v = PANORAMA;
else if(strcasecmp(arg.c_str(), "PANORAMA_FAST") == 0) v = PANORAMA_FAST;
else throw std::runtime_error(std::string("normal calculation method ") + arg + std::string(" is unknown"));
}
/// validate IO types
void validate(boost::any& v, const std::vector<std::string>& values,
IOType*, int) {
if (values.size() == 0)
throw std::runtime_error("Invalid model specification");
string arg = values.at(0);
try {
v = formatname_to_io_type(arg.c_str());
} catch (...) { // runtime_error
throw std::runtime_error("Format " + arg + " unknown.");
}
}
/// Parse commandline options
void parse_options(int argc, char **argv, int &start, int &end, bool &scanserver, int &max_dist, int &min_dist, string &dir,
IOType &iotype, int &k1, int &k2, normal_method &ntype,int &width,int &height)
{
/// ----------------------------------
/// set up program commandline options
/// ----------------------------------
po::options_description cmd_options("Usage: calculateNormals <options> where options are (default values in brackets)");
cmd_options.add_options()
("help,?", "Display this help message")
("start,s", po::value<int>(&start)->default_value(0), "Start at scan number <arg>")
("end,e", po::value<int>(&end)->default_value(-1), "Stop at scan number <arg>")
("scanserver,S", po::value<bool>(&scanserver)->default_value(false), "Use the scanserver as an input method")
("format,f", po::value<IOType>(&iotype)->default_value(UOS),
"using shared library <arg> for input. (chose format from [uos|uosr|uos_map|"
"uos_rgb|uos_frames|uos_map_frames|old|rts|rts_map|ifp|"
"riegl_txt|riegl_rgb|riegl_bin|zahn|ply])")
("max,M", po::value<int>(&max_dist)->default_value(-1),"neglegt all data points with a distance larger than <arg> 'units")
("min,m", po::value<int>(&min_dist)->default_value(-1),"neglegt all data points with a distance smaller than <arg> 'units")
("normal,g", po::value<normal_method>(&ntype)->default_value(AKNN), "normal calculation method "
"(AKNN, ADAPTIVE_AKNN, PANORAMA, PANORAMA_FAST)")
("K1,k", po::value<int>(&k1)->default_value(20), "<arg> value of K value used in the nearest neighbor search of ANN or" "kmin for k-adaptation")
("K2,K", po::value<int>(&k2)->default_value(20), "<arg> value of Kmax for k-adaptation")
("width,w", po::value<int>(&width)->default_value(1280),"width of panorama image")
("height,h", po::value<int>(&height)->default_value(960),"height of panorama image")
;
po::options_description hidden("Hidden options");
hidden.add_options()
("input-dir", po::value<string>(&dir), "input dir");
po::positional_options_description pd;
pd.add("input-dir", 1);
po::options_description all;
all.add(cmd_options).add(hidden);
po::variables_map vmap;
po::store(po::command_line_parser(argc, argv).options(all).positional(pd).run(), vmap);
po::notify(vmap);
if (vmap.count("help")) {
cout << cmd_options << endl << endl;
cout << "SAMPLE COMMAND FOR CALCULATING NORMALS" << endl;
cout << " bin/normals -s 0 -e 0 -f UOS -g AKNN -k 20 dat/" <<endl;
cout << endl << endl;
cout << "SAMPLE COMMAND FOR VIEWING CALCULATING NORMALS IN RGB SPACE" << endl;
cout << " bin/show -c -f UOS_RGB dat/normals/" << endl;
exit(-1);
}
// read scan path
if (dir[dir.length()-1] != '/') dir = dir + "/";
}
/////////////////////////////////////////////////////// ///////////////////////////////////////////////////////
/////////////NORMALS USING AKNN METHOD //////////////// /////////////NORMALS USING AKNN METHOD ////////////////
/////////////////////////////////////////////////////// ///////////////////////////////////////////////////////
@ -134,66 +38,66 @@ void calculateNormalsAKNN(vector<Point> &normals,vector<Point> &points, int k, c
ANNpointArray pa = annAllocPts(points.size(), 3); ANNpointArray pa = annAllocPts(points.size(), 3);
for (size_t i=0; i<points.size(); ++i) for (size_t i=0; i<points.size(); ++i)
{ {
pa[i][0] = points[i].x; pa[i][0] = points[i].x;
pa[i][1] = points[i].y; pa[i][1] = points[i].y;
pa[i][2] = points[i].z; pa[i][2] = points[i].z;
} }
ANNkd_tree t(pa, points.size(), 3); ANNkd_tree t(pa, points.size(), 3);
ANNidxArray nidx = new ANNidx[nr_neighbors]; ANNidxArray nidx = new ANNidx[nr_neighbors];
ANNdistArray d = new ANNdist[nr_neighbors]; ANNdistArray d = new ANNdist[nr_neighbors];
for (size_t i=0; i<points.size(); ++i) for (size_t i=0; i<points.size(); ++i)
{
ANNpoint p = pa[i];
//ANN search for k nearest neighbors
//indexes of the neighbors along with the query point
//stored in the array n
t.annkSearch(p, nr_neighbors, nidx, d, 0.0);
Point mean(0.0,0.0,0.0);
Matrix X(nr_neighbors,3);
SymmetricMatrix A(3);
Matrix U(3,3);
DiagonalMatrix D(3);
//calculate mean for all the points
for (int j=0; j<nr_neighbors; ++j)
{ {
ANNpoint p = pa[i]; mean.x += points[nidx[j]].x;
//ANN search for k nearest neighbors mean.y += points[nidx[j]].y;
//indexes of the neighbors along with the query point mean.z += points[nidx[j]].z;
//stored in the array n
t.annkSearch(p, nr_neighbors, nidx, d, 0.0);
Point mean(0.0,0.0,0.0);
Matrix X(nr_neighbors,3);
SymmetricMatrix A(3);
Matrix U(3,3);
DiagonalMatrix D(3);
//calculate mean for all the points
for (int j=0; j<nr_neighbors; ++j)
{
mean.x += points[nidx[j]].x;
mean.y += points[nidx[j]].y;
mean.z += points[nidx[j]].z;
}
mean.x /= nr_neighbors;
mean.y /= nr_neighbors;
mean.z /= nr_neighbors;
//calculate covariance = A for all the points
for (int i = 0; i < nr_neighbors; ++i) {
X(i+1, 1) = points[nidx[i]].x - mean.x;
X(i+1, 2) = points[nidx[i]].y - mean.y;
X(i+1, 3) = points[nidx[i]].z - mean.z;
}
A << 1.0/nr_neighbors * X.t() * X;
EigenValues(A, D, U);
//normal = eigenvector corresponding to lowest
//eigen value that is the 1st column of matrix U
ColumnVector n(3);
n(1) = U(1,1);
n(2) = U(2,1);
n(3) = U(3,1);
ColumnVector point_vector(3);
point_vector(1) = p[0] - rPos(1);
point_vector(2) = p[1] - rPos(2);
point_vector(3) = p[2] - rPos(3);
point_vector = point_vector / point_vector.NormFrobenius();
Real angle = (n.t() * point_vector).AsScalar();
if (angle < 0) {
n *= -1.0;
}
n = n / n.NormFrobenius();
normals.push_back(Point(n(1), n(2), n(3)));
} }
mean.x /= nr_neighbors;
mean.y /= nr_neighbors;
mean.z /= nr_neighbors;
//calculate covariance = A for all the points
for (int i = 0; i < nr_neighbors; ++i) {
X(i+1, 1) = points[nidx[i]].x - mean.x;
X(i+1, 2) = points[nidx[i]].y - mean.y;
X(i+1, 3) = points[nidx[i]].z - mean.z;
}
A << 1.0/nr_neighbors * X.t() * X;
EigenValues(A, D, U);
//normal = eigenvector corresponding to lowest
//eigen value that is the 1st column of matrix U
ColumnVector n(3);
n(1) = U(1,1);
n(2) = U(2,1);
n(3) = U(3,1);
ColumnVector point_vector(3);
point_vector(1) = p[0] - rPos(1);
point_vector(2) = p[1] - rPos(2);
point_vector(3) = p[2] - rPos(3);
point_vector = point_vector / point_vector.NormFrobenius();
Real angle = (n.t() * point_vector).AsScalar();
if (angle < 0) {
n *= -1.0;
}
n = n / n.NormFrobenius();
normals.push_back(Point(n(1), n(2), n(3)));
}
delete[] nidx; delete[] nidx;
delete[] d; delete[] d;
@ -203,7 +107,7 @@ void calculateNormalsAKNN(vector<Point> &normals,vector<Point> &points, int k, c
/////////////NORMALS USING ADAPTIVE AKNN METHOD //////////////// /////////////NORMALS USING ADAPTIVE AKNN METHOD ////////////////
//////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////
void calculateNormalsAdaptiveAKNN(vector<Point> &normals,vector<Point> &points, void calculateNormalsAdaptiveAKNN(vector<Point> &normals,vector<Point> &points,
int kmin, int kmax, const double _rPos[3]) int kmin, int kmax, const double _rPos[3])
{ {
ColumnVector rPos(3); ColumnVector rPos(3);
for (int i = 0; i < 3; ++i) for (int i = 0; i < 3; ++i)
@ -213,88 +117,87 @@ void calculateNormalsAdaptiveAKNN(vector<Point> &normals,vector<Point> &points,
int nr_neighbors; int nr_neighbors;
ANNpointArray pa = annAllocPts(points.size(), 3); ANNpointArray pa = annAllocPts(points.size(), 3);
for (size_t i=0; i<points.size(); ++i) for (size_t i=0; i<points.size(); ++i)
{ {
pa[i][0] = points[i].x; pa[i][0] = points[i].x;
pa[i][1] = points[i].y; pa[i][1] = points[i].y;
pa[i][2] = points[i].z; pa[i][2] = points[i].z;
} }
ANNkd_tree t(pa, points.size(), 3); ANNkd_tree t(pa, points.size(), 3);
Point mean(0.0,0.0,0.0); Point mean(0.0,0.0,0.0);
double temp_n[3],norm_n = 0.0;
double e1,e2,e3; double e1,e2,e3;
for (size_t i=0; i<points.size(); ++i) for (size_t i=0; i<points.size(); ++i)
{
Matrix U(3,3);
ANNpoint p = pa[i];
for(int kidx = kmin; kidx < kmax; kidx++)
{ {
Matrix U(3,3); nr_neighbors=kidx+1;
ANNpoint p = pa[i]; ANNidxArray nidx = new ANNidx[nr_neighbors];
for(int kidx = kmin; kidx < kmax; kidx++) ANNdistArray d = new ANNdist[nr_neighbors];
{ //ANN search for k nearest neighbors
nr_neighbors=kidx+1; //indexes of the neighbors along with the query point
ANNidxArray nidx = new ANNidx[nr_neighbors]; //stored in the array n
ANNdistArray d = new ANNdist[nr_neighbors]; t.annkSearch(p, nr_neighbors, nidx, d, 0.0);
//ANN search for k nearest neighbors mean.x=0,mean.y=0,mean.z=0;
//indexes of the neighbors along with the query point //calculate mean for all the points
//stored in the array n for (int j=0; j<nr_neighbors; ++j)
t.annkSearch(p, nr_neighbors, nidx, d, 0.0); {
mean.x=0,mean.y=0,mean.z=0; mean.x += points[nidx[j]].x;
//calculate mean for all the points mean.y += points[nidx[j]].y;
for (int j=0; j<nr_neighbors; ++j) mean.z += points[nidx[j]].z;
{ }
mean.x += points[nidx[j]].x; mean.x /= nr_neighbors;
mean.y += points[nidx[j]].y; mean.y /= nr_neighbors;
mean.z += points[nidx[j]].z; mean.z /= nr_neighbors;
}
mean.x /= nr_neighbors;
mean.y /= nr_neighbors;
mean.z /= nr_neighbors;
Matrix X(nr_neighbors,3); Matrix X(nr_neighbors,3);
SymmetricMatrix A(3); SymmetricMatrix A(3);
DiagonalMatrix D(3); DiagonalMatrix D(3);
//calculate covariance = A for all the points //calculate covariance = A for all the points
for (int j = 0; j < nr_neighbors; ++j) { for (int j = 0; j < nr_neighbors; ++j) {
X(j+1, 1) = points[nidx[j]].x - mean.x; X(j+1, 1) = points[nidx[j]].x - mean.x;
X(j+1, 2) = points[nidx[j]].y - mean.y; X(j+1, 2) = points[nidx[j]].y - mean.y;
X(j+1, 3) = points[nidx[j]].z - mean.z; X(j+1, 3) = points[nidx[j]].z - mean.z;
} }
A << 1.0/nr_neighbors * X.t() * X; A << 1.0/nr_neighbors * X.t() * X;
EigenValues(A, D, U); EigenValues(A, D, U);
e1 = D(1); e1 = D(1);
e2 = D(2); e2 = D(2);
e3 = D(3); e3 = D(3);
delete[] nidx; delete[] nidx;
delete[] d; delete[] d;
//We take the particular k if the second maximum eigen value //We take the particular k if the second maximum eigen value
//is at least 25 percent of the maximum eigen value //is at least 25 percent of the maximum eigen value
if ((e1 > 0.25 * e2) && (fabs(1.0 - (double)e2/(double)e3) < 0.25)) if ((e1 > 0.25 * e2) && (fabs(1.0 - (double)e2/(double)e3) < 0.25))
break; break;
}
//normal = eigenvector corresponding to lowest
//eigen value that is the 1rd column of matrix U
ColumnVector n(3);
n(1) = U(1,1);
n(2) = U(2,1);
n(3) = U(3,1);
ColumnVector point_vector(3);
point_vector(1) = p[0] - rPos(1);
point_vector(2) = p[1] - rPos(2);
point_vector(3) = p[2] - rPos(3);
point_vector = point_vector / point_vector.NormFrobenius();
Real angle = (n.t() * point_vector).AsScalar();
if (angle < 0) {
n *= -1.0;
}
n = n / n.NormFrobenius();
normals.push_back(Point(n(1), n(2), n(3)));
} }
//normal = eigenvector corresponding to lowest
//eigen value that is the 1rd column of matrix U
ColumnVector n(3);
n(1) = U(1,1);
n(2) = U(2,1);
n(3) = U(3,1);
ColumnVector point_vector(3);
point_vector(1) = p[0] - rPos(1);
point_vector(2) = p[1] - rPos(2);
point_vector(3) = p[2] - rPos(3);
point_vector = point_vector / point_vector.NormFrobenius();
Real angle = (n.t() * point_vector).AsScalar();
if (angle < 0) {
n *= -1.0;
}
n = n / n.NormFrobenius();
normals.push_back(Point(n(1), n(2), n(3)));
}
annDeallocPts(pa); annDeallocPts(pa);
} }
@ -302,9 +205,9 @@ void calculateNormalsAdaptiveAKNN(vector<Point> &normals,vector<Point> &points,
/////////////NORMALS USING IMAGE NEIGHBORS //////////// /////////////NORMALS USING IMAGE NEIGHBORS ////////////
/////////////////////////////////////////////////////// ///////////////////////////////////////////////////////
void calculateNormalsPANORAMA(vector<Point> &normals, void calculateNormalsPANORAMA(vector<Point> &normals,
vector<Point> &points, vector<Point> &points,
vector< vector< vector< cv::Vec3f > > > extendedMap, vector< vector< vector< cv::Vec3f > > > extendedMap,
const double _rPos[3]) const double _rPos[3])
{ {
ColumnVector rPos(3); ColumnVector rPos(3);
for (int i = 0; i < 3; ++i) for (int i = 0; i < 3; ++i)
@ -320,95 +223,94 @@ void calculateNormalsPANORAMA(vector<Point> &normals,
//temporary dynamic array for all the neighbors of a given point //temporary dynamic array for all the neighbors of a given point
vector<cv::Vec3f> neighbors; vector<cv::Vec3f> neighbors;
for (size_t i=0; i< extendedMap.size(); i++) for (size_t i=0; i< extendedMap.size(); i++)
{
for (size_t j=0; j<extendedMap[i].size(); j++)
{ {
for (size_t j=0; j<extendedMap[i].size(); j++) if (extendedMap[i][j].size() == 0) continue;
{ neighbors.clear();
if (extendedMap[i][j].size() == 0) continue; Point mean(0.0,0.0,0.0);
neighbors.clear();
Point mean(0.0,0.0,0.0);
double temp_n[3],norm_n = 0.0;
// Offset for neighbor computation // Offset for neighbor computation
int offset[2][5] = {{-1,0,1,0,0},{0,-1,0,1,0}}; int offset[2][5] = {{-1,0,1,0,0},{0,-1,0,1,0}};
// Traversing all the cells in the extended map // Traversing all the cells in the extended map
for (int n = 0; n < 5; ++n) { for (int n = 0; n < 5; ++n) {
int x = i + offset[0][n]; int x = i + offset[0][n];
int y = j + offset[1][n]; int y = j + offset[1][n];
// Copy the neighboring buckets into the vector // Copy the neighboring buckets into the vector
if (x >= 0 && x < (int)extendedMap.size() && if (x >= 0 && x < (int)extendedMap.size() &&
y >= 0 && y < (int)extendedMap[x].size()) { y >= 0 && y < (int)extendedMap[x].size()) {
for (unsigned int k = 0; k < extendedMap[x][y].size(); k++) { for (unsigned int k = 0; k < extendedMap[x][y].size(); k++) {
neighbors.push_back(extendedMap[x][y][k]); neighbors.push_back(extendedMap[x][y][k]);
} }
} }
} }
nr_neighbors = neighbors.size(); nr_neighbors = neighbors.size();
cv::Vec3f p = extendedMap[i][j][0]; cv::Vec3f p = extendedMap[i][j][0];
//if no neighbor point is found in the 4-neighboring pixels then normal is set to zero //if no neighbor point is found in the 4-neighboring pixels then normal is set to zero
if (nr_neighbors < 3) if (nr_neighbors < 3)
{ {
points.push_back(Point(p[0], p[1], p[2])); points.push_back(Point(p[0], p[1], p[2]));
normals.push_back(Point(0.0,0.0,0.0)); normals.push_back(Point(0.0,0.0,0.0));
continue; continue;
} }
//calculate mean for all the points //calculate mean for all the points
Matrix X(nr_neighbors,3); Matrix X(nr_neighbors,3);
SymmetricMatrix A(3); SymmetricMatrix A(3);
Matrix U(3,3); Matrix U(3,3);
DiagonalMatrix D(3); DiagonalMatrix D(3);
//calculate mean for all the points //calculate mean for all the points
for(int k = 0; k < nr_neighbors; k++) for(int k = 0; k < nr_neighbors; k++)
{ {
cv::Vec3f pp = neighbors[k]; cv::Vec3f pp = neighbors[k];
mean.x += pp[0]; mean.x += pp[0];
mean.y += pp[1]; mean.y += pp[1];
mean.z += pp[2]; mean.z += pp[2];
} }
mean.x /= nr_neighbors; mean.x /= nr_neighbors;
mean.y /= nr_neighbors; mean.y /= nr_neighbors;
mean.z /= nr_neighbors; mean.z /= nr_neighbors;
//calculate covariance = A for all the points //calculate covariance = A for all the points
for (int i = 0; i < nr_neighbors; ++i) { for (int n = 0; n < nr_neighbors; ++n) {
cv::Vec3f pp = neighbors[i]; cv::Vec3f pp = neighbors[n];
X(i+1, 1) = pp[0] - mean.x; X(n+1, 1) = pp[0] - mean.x;
X(i+1, 2) = pp[1] - mean.y; X(n+1, 2) = pp[1] - mean.y;
X(i+1, 3) = pp[2] - mean.z; X(n+1, 3) = pp[2] - mean.z;
} }
A << 1.0/nr_neighbors * X.t() * X; A << 1.0/nr_neighbors * X.t() * X;
EigenValues(A, D, U); EigenValues(A, D, U);
//normal = eigenvector corresponding to lowest //normal = eigenvector corresponding to lowest
//eigen value that is the 1st column of matrix U //eigen value that is the 1st column of matrix U
ColumnVector n(3); ColumnVector n(3);
n(1) = U(1,1); n(1) = U(1,1);
n(2) = U(2,1); n(2) = U(2,1);
n(3) = U(3,1); n(3) = U(3,1);
ColumnVector point_vector(3); ColumnVector point_vector(3);
point_vector(1) = p[0] - rPos(1); point_vector(1) = p[0] - rPos(1);
point_vector(2) = p[1] - rPos(2); point_vector(2) = p[1] - rPos(2);
point_vector(3) = p[2] - rPos(3); point_vector(3) = p[2] - rPos(3);
point_vector = point_vector / point_vector.NormFrobenius(); point_vector = point_vector / point_vector.NormFrobenius();
Real angle = (n.t() * point_vector).AsScalar(); Real angle = (n.t() * point_vector).AsScalar();
if (angle < 0) { if (angle < 0) {
n *= -1.0; n *= -1.0;
} }
n = n / n.NormFrobenius(); n = n / n.NormFrobenius();
for (unsigned int k = 0; k < extendedMap[i][j].size(); k++) { for (unsigned int k = 0; k < extendedMap[i][j].size(); k++) {
cv::Vec3f p = extendedMap[i][j][k]; cv::Vec3f p = extendedMap[i][j][k];
points.push_back(Point(p[0], p[1], p[2])); points.push_back(Point(p[0], p[1], p[2]));
normals.push_back(Point(n(1), n(2), n(3))); normals.push_back(Point(n(1), n(2), n(3)));
} }
}
} }
}
} }
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
///////////FAST NORMALS USING PANORAMA EQUIRECTANGULAR RANGE IMAGE ////////////////////////// ///////////FAST NORMALS USING PANORAMA EQUIRECTANGULAR RANGE IMAGE //////////////////////////
@ -486,268 +388,3 @@ void calculateNormalsPANORAMA(vector<Point> &normals,
} }
} }
*/ */
/*
* retrieve a cv::Mat with x,y,z,r from a scan object
* functionality borrowed from scan_cv::convertScanToMat but this function
* does not allow a scanserver to be used, prints to stdout and can only
* handle a single scan
*/
cv::Mat scan2mat(Scan *source)
{
DataXYZ xyz = source->get("xyz");
DataReflectance xyz_reflectance = source->get("reflectance");
unsigned int nPoints = xyz.size();
cv::Mat scan(nPoints,1,CV_32FC(4));
scan = cv::Scalar::all(0);
cv::MatIterator_<cv::Vec4f> it;
it = scan.begin<cv::Vec4f>();
for(unsigned int i = 0; i < nPoints; i++){
float x, y, z, reflectance;
x = xyz[i][0];
y = xyz[i][1];
z = xyz[i][2];
if(xyz_reflectance.size() != 0)
{
reflectance = xyz_reflectance[i];
//normalize the reflectance
reflectance += 32;
reflectance /= 64;
reflectance -= 0.2;
reflectance /= 0.3;
if (reflectance < 0) reflectance = 0;
if (reflectance > 1) reflectance = 1;
}
(*it)[0] = x;
(*it)[1] = y;
(*it)[2] = z;
if(xyz_reflectance.size() != 0)
(*it)[3] = reflectance;
else
(*it)[3] = 0;
++it;
}
return scan;
}
/*
* convert a matrix of float values (range image) to a matrix of unsigned
* eight bit characters using different techniques
*/
cv::Mat float2uchar(cv::Mat &source, bool logarithm, float cutoff)
{
cv::Mat result(source.size(), CV_8U, cv::Scalar::all(0));
float max = 0;
// find maximum value
if (cutoff == 0.0) {
// without cutoff, just iterate through all values to find the largest
for (cv::MatIterator_<float> it = source.begin<float>();
it != source.end<float>(); ++it) {
float val = *it;
if (val > max) {
max = val;
}
}
} else {
// when a cutoff is specified, sort all the points by value and then
// specify the max so that <cutoff> values are larger than it
vector<float> sorted(source.cols*source.rows);
int i = 0;
for (cv::MatIterator_<float> it = source.begin<float>();
it != source.end<float>(); ++it, ++i) {
sorted[i] = *it;
}
std::sort(sorted.begin(), sorted.end());
max = sorted[(int)(source.cols*source.rows*(1.0-cutoff))];
cout << "A cutoff of " << cutoff << " resulted in a max value of " << max << endl;
}
cv::MatIterator_<float> src = source.begin<float>();
cv::MatIterator_<uchar> dst = result.begin<uchar>();
cv::MatIterator_<float> end = source.end<float>();
if (logarithm) {
// stretch values from 0 to max logarithmically over 0 to 255
// using the logarithm allows to represent smaller values with more
// precision and larger values with less
max = log(max+1);
for (; src != end; ++src, ++dst) {
float val = (log(*src+1)*255.0)/max;
if (val > 255)
*dst = 255;
else
*dst = (uchar)val;
}
} else {
// stretch values from 0 to max linearly over 0 to 255
for (; src != end; ++src, ++dst) {
float val = (*src*255.0)/max;
if (val > 255)
*dst = 255;
else
*dst = (uchar)val;
}
}
return result;
}
/// Write a pose file with the specofied name
void writePoseFiles(string dir, const double* rPos, const double* rPosTheta,int scanNumber)
{
string poseFileName = dir + "/scan" + to_string(scanNumber, 3) + ".pose";
ofstream posout(poseFileName.c_str());
posout << rPos[0] << " "
<< rPos[1] << " "
<< rPos[2] << endl
<< deg(rPosTheta[0]) << " "
<< deg(rPosTheta[1]) << " "
<< deg(rPosTheta[2]) << endl;
posout.clear();
posout.close();
}
/// write scan files for all segments
void writeScanFiles(string dir, vector<Point> &points, vector<Point> &normals, int scanNumber)
{
string ofilename = dir + "/scan" + to_string(scanNumber, 3) + ".3d";
ofstream normptsout(ofilename.c_str());
for (size_t i=0; i<points.size(); ++i)
{
int r,g,b;
r = (int)(normals[i].x * (127.5) + 127.5);
g = (int)(normals[i].y * (127.5) + 127.5);
b = (int)(fabs(normals[i].z) * (255.0));
normptsout <<points[i].x<<" "<<points[i].y<<" "<<points[i].z<<" "<<r<<" "<<g<<" "<<b<<" "<<endl;
}
normptsout.clear();
normptsout.close();
}
/// =============================================
/// Main
/// =============================================
int main(int argc, char** argv)
{
int start, end;
bool scanserver;
int max_dist, min_dist;
string dir;
IOType iotype;
int k1, k2;
normal_method ntype;
int width, height;
parse_options(argc, argv, start, end, scanserver, max_dist, min_dist,
dir, iotype, k1, k2, ntype, width, height);
/// ----------------------------------
/// Prepare and read scans
/// ----------------------------------
if (scanserver) {
try {
ClientInterface::create();
} catch(std::runtime_error& e) {
cerr << "ClientInterface could not be created: " << e.what() << endl;
cerr << "Start the scanserver first." << endl;
exit(-1);
}
}
/// Make directory for saving the scan segments
string normdir = dir + "normals";
#ifdef _MSC_VER
int success = mkdir(normdir.c_str());
#else
int success = mkdir(normdir.c_str(), S_IRWXU|S_IRWXG|S_IRWXO);
#endif
if(success == 0) {
cout << "Writing segments to " << normdir << endl;
} else if(errno == EEXIST) {
cout << "WARN: Directory " << normdir << " exists already. Contents will be overwriten" << endl;
} else {
cerr << "Creating directory " << normdir << " failed" << endl;
exit(1);
}
/// Read the scans
Scan::openDirectory(scanserver, dir, iotype, start, end);
if(Scan::allScans.size() == 0) {
cerr << "No scans found. Did you use the correct format?" << endl;
exit(-1);
}
cv::Mat img;
/// --------------------------------------------
/// Initialize and perform segmentation
/// --------------------------------------------
std::vector<Scan*>::iterator it = Scan::allScans.begin();
int scanNumber = 0;
for( ; it != Scan::allScans.end(); ++it) {
Scan* scan = *it;
// apply optional filtering
scan->setRangeFilter(max_dist, min_dist);
const double* rPos = scan->get_rPos();
const double* rPosTheta = scan->get_rPosTheta();
/// read scan into points
DataXYZ xyz(scan->get("xyz"));
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]));
}
if(ntype == AKNN)
calculateNormalsAKNN(normals,points, k1, rPos);
else if(ntype == ADAPTIVE_AKNN)
calculateNormalsAdaptiveAKNN(normals,points, k1, k2, rPos);
else
{
// create panorama
fbr::panorama fPanorama(width, height, fbr::EQUIRECTANGULAR, 1, 0, fbr::EXTENDED);
fPanorama.createPanorama(scan2mat(scan));
// the range image has to be converted from float to uchar
img = fPanorama.getRangeImage();
img = float2uchar(img, 0, 0.0);
if(ntype == PANORAMA)
calculateNormalsPANORAMA(normals,points,fPanorama.getExtendedMap(), rPos);
else if(ntype == PANORAMA_FAST)
cout << "PANORAMA_FAST is not working yet" << endl;
// calculateNormalsFAST(normals,points,img,fPanorama.getExtendedMap());
}
// pose file (repeated for the number of segments
writePoseFiles(normdir, rPos, rPosTheta, scanNumber);
// scan files for all segments
writeScanFiles(normdir, points,normals,scanNumber);
scanNumber++;
}
// shutdown everything
if (scanserver)
ClientInterface::destroy();
else
Scan::closeDirectory();
cout << "Normal program end" << endl;
return 0;
}

View file

@ -1,4 +1,4 @@
SET(SHOW_LIBS glui scan ANN ${OPENGL_LIBRARIES}) SET(SHOW_LIBS glui scan ANN ${OPENGL_LIBRARIES} normals newmat)
IF(WIN32) IF(WIN32)
IF( CMAKE_SIZEOF_VOID_P EQUAL 8 ) IF( CMAKE_SIZEOF_VOID_P EQUAL 8 )
SET(SHOW_LIBS ${SHOW_LIBS} ${CMAKE_SOURCE_DIR}/3rdparty/windows/x64/freeglut.lib XGetopt) SET(SHOW_LIBS ${SHOW_LIBS} ${CMAKE_SOURCE_DIR}/3rdparty/windows/x64/freeglut.lib XGetopt)

View file

@ -27,7 +27,7 @@
#include "slam6d/point_type.h" #include "slam6d/point_type.h"
using std::vector; using std::vector;
ScanColorManager::ScanColorManager(unsigned int _buckets, PointType type) : pointtype(type) { ScanColorManager::ScanColorManager(unsigned int _buckets, PointType type, bool animation_color) : pointtype(type), animationColor(animation_color) {
valid = false; valid = false;
inverted = false; inverted = false;
buckets = _buckets; buckets = _buckets;
@ -96,8 +96,10 @@ ScanColorManager::ScanColorManager(unsigned int _buckets, PointType type) : poin
allScans[i]->setColorManager(scanManager[i]); allScans[i]->setColorManager(scanManager[i]);
} }
} else if (mode == ScanColorManager::MODE_ANIMATION) { } else if (mode == ScanColorManager::MODE_ANIMATION) {
for (unsigned int i = 0; i < allScans.size(); i++) { if (animationColor) {
allScans[i]->setColorManager(0); for (unsigned int i = 0; i < allScans.size(); i++) {
allScans[i]->setColorManager(0);
}
} }
} else if (mode == ScanColorManager::MODE_POINT_COLOR) { } else if (mode == ScanColorManager::MODE_POINT_COLOR) {
for (unsigned int i = 0; i < allScans.size(); i++) { for (unsigned int i = 0; i < allScans.size(); i++) {
@ -158,18 +160,6 @@ ScanColorManager::ScanColorManager(unsigned int _buckets, PointType type) : poin
case Scan::ELCH: case Scan::ELCH:
glColor4d(0.0, 1.0,0.0, 1.0); glColor4d(0.0, 1.0,0.0, 1.0);
break; break;
case Scan::LOOPTORO:
glColor4d(0.0, 0.0, 1.0, 1.0);
break;
case Scan::LOOPHOGMAN:
glColor4d(0.0, 1.0, 1.0, 1.0);
break;
case Scan::GRAPHTORO:
glColor4d(1.0, 0.0, 1.0, 1.0);
break;
case Scan::GRAPHHOGMAN:
glColor4d(1.0, 1.0, 0.0, 1.0);
break;
default: default:
glColor4d(1.0, 1.0, 1.0, 1.0); glColor4d(1.0, 1.0, 1.0, 1.0);
break; break;

View file

@ -626,9 +626,7 @@ void setResetView(int origin) {
// set origin to the center of mass of all scans // set origin to the center of mass of all scans
for (size_t i = 0; i < octpts.size(); ++i) { for (size_t i = 0; i < octpts.size(); ++i) {
vector <sfloat*> points; vector <sfloat*> points;
#ifdef USE_COMPACT_TREE #ifndef USE_COMPACT_TREE
((compactTree*)octpts[i])->AllPoints( points );
#else
BOctTree<sfloat>* cur_tree = ((Show_BOctTree<sfloat>*)octpts[i])->getTree(); BOctTree<sfloat>* cur_tree = ((Show_BOctTree<sfloat>*)octpts[i])->getTree();
cur_tree->AllPoints( points ); cur_tree->AllPoints( points );
#endif #endif
@ -881,7 +879,11 @@ void initShow(int argc, char **argv){
} }
} }
} }
cm = new ScanColorManager(4096, pointtype); if (sphereMode > 0.0) {
cm = new ScanColorManager(4096, pointtype, /* animation_color = */ false);
} else {
cm = new ScanColorManager(4096, pointtype, /* animation_color = */ true);
}
#ifdef USE_COMPACT_TREE #ifdef USE_COMPACT_TREE
cout << "Creating compact display octrees.." << endl; cout << "Creating compact display octrees.." << endl;

View file

@ -61,16 +61,14 @@ ENDIF(WITH_TOOLS)
SET(SCANLIB_SRCS SET(SCANLIB_SRCS
kd.cc kdManaged.cc kdMeta.cc graphSlam6D.cc kd.cc kdManaged.cc kdMeta.cc graphSlam6D.cc
graph.cc icp6Dapx.cc icp6D.cc icp6Dsvd.cc graph.cc icp6D.cc icp6Dapx.cc icp6Dsvd.cc
icp6Dortho.cc icp6Dquat.cc icp6Dhelix.cc icp6Dlumeuler.cc icp6Dortho.cc icp6Dquat.cc icp6Dhelix.cc icp6Dlumeuler.cc
icp6Dlumquat.cc icp6Ddual.cc lum6Deuler.cc lum6Dquat.cc icp6Dlumquat.cc icp6Ddual.cc lum6Deuler.cc lum6Dquat.cc
ghelix6DQ2.cc gapx6D.cc graphToro.cc ann_kd.cc ghelix6DQ2.cc gapx6D.cc ann_kd.cc elch6D.cc
graphHOG-Man.cc elch6D.cc elch6Dquat.cc elch6DunitQuat.cc elch6Dquat.cc elch6DunitQuat.cc elch6Dslerp.cc elch6Deuler.cc
elch6Dslerp.cc elch6Deuler.cc loopToro.cc loopHOG-Man.cc
point_type.cc icp6Dquatscale.cc searchTree.cc Boctree.cc point_type.cc icp6Dquatscale.cc searchTree.cc Boctree.cc
allocator.cc
scan.cc basicScan.cc managedScan.cc metaScan.cc scan.cc basicScan.cc managedScan.cc metaScan.cc
io_types.cc io_utils.cc pointfilter.cc io_types.cc io_utils.cc pointfilter.cc allocator.cc
) )
if(WITH_METRICS) if(WITH_METRICS)
@ -79,7 +77,9 @@ endif(WITH_METRICS)
add_library(scan STATIC ${SCANLIB_SRCS}) add_library(scan STATIC ${SCANLIB_SRCS})
target_link_libraries(scan scanclient scanio) FIND_PACKAGE(OpenCV REQUIRED)
target_link_libraries(scan scanclient scanio normals)
IF(UNIX) IF(UNIX)
target_link_libraries(scan dl) target_link_libraries(scan dl)

View file

@ -242,6 +242,8 @@ DataPointer BasicScan::get(const std::string& identifier)
if(identifier == "amplitude") get(DATA_AMPLITUDE); else if(identifier == "amplitude") get(DATA_AMPLITUDE); else
if(identifier == "type") get(DATA_TYPE); else if(identifier == "type") get(DATA_TYPE); else
if(identifier == "deviation") get(DATA_DEVIATION); else if(identifier == "deviation") get(DATA_DEVIATION); else
// normals on demand
if(identifier == "normal") calcNormalsOnDemand(); else
// reduce on demand // reduce on demand
if(identifier == "xyz reduced") calcReducedOnDemand(); else if(identifier == "xyz reduced") calcReducedOnDemand(); else
if(identifier == "xyz reduced original") calcReducedOnDemand(); else if(identifier == "xyz reduced original") calcReducedOnDemand(); else
@ -326,12 +328,20 @@ void BasicScan::createSearchTreePrivate()
void BasicScan::calcReducedOnDemandPrivate() void BasicScan::calcReducedOnDemandPrivate()
{ {
// create reduced points and transform to initial position, save a copy of this for SearchTree // create reduced points and transform to initial position,
// save a copy of this for SearchTree
calcReducedPoints(); calcReducedPoints();
transformReduced(transMatOrg); transformReduced(transMatOrg);
copyReducedToOriginal(); copyReducedToOriginal();
} }
void BasicScan::calcNormalsOnDemandPrivate()
{
// create normals
calcNormals();
}
void BasicScan::createANNTree() void BasicScan::createANNTree()
{ {
// TODO: metrics // TODO: metrics

View file

@ -1,151 +0,0 @@
/*
* graphHOG-Max implementation
*
* Copyright (C) Jochen Sprickerhof
*
* Released under the GPL version 3.
*
*/
/**
* @file HOG-Man wrapper
* @author Jochen Sprickerhof. Institute of Computer Science, University of Osnabrueck, Germany.
*/
#include "slam6d/graphHOG-Man.h"
#include "slam6d/metaScan.h"
#include "slam6d/lum6Deuler.h"
#include "slam6d/globals.icc"
#include <fstream>
using std::ofstream;
using std::ifstream;
#include <cfloat>
#include <cstring>
using namespace NEWMAT;
/**
* This function is used to match a set of laser scans with any minimally
* connected Graph.
*
* @param gr Some Graph with no real subgraphs except for itself
* @param allScans Contains all laser scans
* @param nrIt The number of iterations the LUM-algorithm will run
* @return Euclidian distance of all pose shifts
*/
double graphHOGMan::doGraphSlam6D(Graph gr, vector <Scan *> allScans, int nrIt)
{
Matrix C(6, 6);
double invers[16], rela[16], rPos[3], rPosTheta[3], rPosQuat[4];
double Pl0[16];
ofstream outFile("hogman.graph");
int n = gr.getNrScans();
for(int i = 0; i < n; i++) {
QuatRPYEuler(allScans[i]->get_rPosQuat(), rPosTheta);
outFile << "VERTEX3" << " " << i <<
" " << (allScans[i]->get_rPos()[0]/100) <<
" " << (allScans[i]->get_rPos()[1]/100) <<
" " << (allScans[i]->get_rPos()[2]/100) <<
" " << rPosTheta[0] <<
" " << rPosTheta[1] <<
" " << rPosTheta[2] << endl;
}
for(int i = 0; i < gr.getNrLinks(); i++){
int first = gr.getLink(i,0);
int last = gr.getLink(i,1);
if(first != last-1) {
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++) {
if(i >= 0) {
meta_end.push_back(allScans[i]);
}
}
MetaScan *end = new MetaScan(meta_end, false, false);
memcpy(Pl0, allScans[last]->get_transMat(), 16 * sizeof(double));
my_icp->match(start, end);
delete start;
delete end;
}
M4inv(allScans[last]->get_transMat(), invers);
MMult(invers, allScans[first]->get_transMat(), rela);
Matrix4ToQuat(rela, rPosQuat, rPos);
QuatRPYEuler(rPosQuat, rPosTheta);
lum6DEuler::covarianceEuler(allScans[first], allScans[last], my_icp->get_nns_method(), my_icp->get_rnd(), my_icp->get_max_dist_match2(), &C);
outFile << "EDGE3" << " " << last << " " << first << " " <<
(rPos[0]/100) << " " <<
(rPos[1]/100) << " " <<
(rPos[2]/100) << " " <<
rPosTheta[0] << " " <<
rPosTheta[1] << " " <<
rPosTheta[2] << " ";
for(int i = 1; i < 7; i++)
for(int j = i; j < 7; j++)
outFile << C(i, j) << " ";
outFile << endl;
if(first != last-1) {
allScans[last]->transformToMatrix(Pl0,Scan::INVALID);
}
}
outFile.close();
system("LD_LIBRARY_PATH=./bin/ ./bin/hogman3d -update 1 -oc -o hogman-final.graph hogman.graph");
ifstream inFile("hogman-final.graph");
string tag;
int id;
double dd;
double rPosN[3], rPosThetaN[3];
while(inFile) {
inFile >> tag;
if(tag == "VERTEX3") {
inFile >> id;
if(id == n-1) {
inFile >> rPosN[0] >> rPosN[1] >> rPosN[2] >> rPosThetaN[0] >> rPosThetaN[1] >> rPosThetaN[2];
rPosN[0] *= 100;
rPosN[1] *= 100;
rPosN[2] *= 100;
} else {
inFile >> rPos[0] >> rPos[1] >> rPos[2] >> rPosTheta[0] >> rPosTheta[1] >> rPosTheta[2];
rPos[0] *= 100;
rPos[1] *= 100;
rPos[2] *= 100;
RPYEulerQuat(rPosTheta, rPosQuat);
if(id != 0) {
allScans[id]->transformToQuat(rPos, rPosQuat, Scan::GRAPHHOGMAN, 1);
}
}
}
else if(tag == "EDGE3") {
inFile >> id >> id;
for(int i=0; i < 22; i++) {
inFile >> dd;
}
}
}
RPYEulerQuat(rPosThetaN, rPosQuat);
allScans[n-1]->transformToQuat(rPosN, rPosQuat, Scan::GRAPHHOGMAN, 2);
inFile.close();
return DBL_MAX;
}

View file

@ -1,152 +0,0 @@
/*
* graphToro implementation
*
* Copyright (C) Jochen Sprickerhof
*
* Released under the GPL version 3.
*
*/
/**
* @file TORO wrapper
* @author Jochen Sprickerhof. Institute of Computer Science, University of Osnabrueck, Germany.
*/
#include "slam6d/graphToro.h"
#include "slam6d/metaScan.h"
#include "slam6d/lum6Deuler.h"
#include "slam6d/globals.icc"
#include <fstream>
using std::ofstream;
using std::ifstream;
#include <cfloat>
#include <cstring>
using namespace NEWMAT;
/**
* This function is used to match a set of laser scans with any minimally
* connected Graph.
*
* @param gr Some Graph with no real subgraphs except for itself
* @param allScans Contains all laser scans
* @param nrIt The number of iterations the LUM-algorithm will run
* @return Euclidian distance of all pose shifts
*/
double graphToro::doGraphSlam6D(Graph gr, vector <Scan *> allScans, int nrIt)
{
Matrix C(6, 6);
double invers[16], rela[16], rPos[3], rPosTheta[3], rPosQuat[4];
double Pl0[16];
ofstream outFile("toro.graph");
int n = gr.getNrScans();
for(int i = 0; i < n; i++) {
QuatRPYEuler(allScans[i]->get_rPosQuat(), rPosTheta);
outFile << "VERTEX3" << " " << i <<
" " << (allScans[i]->get_rPos()[0]/100) <<
" " << (allScans[i]->get_rPos()[1]/100) <<
" " << (allScans[i]->get_rPos()[2]/100) <<
" " << rPosTheta[0] <<
" " << rPosTheta[1] <<
" " << rPosTheta[2] << endl;
}
for(int i = 0; i < gr.getNrLinks(); i++){
int first = gr.getLink(i,0);
int last = gr.getLink(i,1);
if(first != last-1) {
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++) {
if(i >= 0) {
meta_end.push_back(allScans[i]);
}
}
MetaScan *end = new MetaScan(meta_end, false, false);
memcpy(Pl0, allScans[last]->get_transMat(), 16 * sizeof(double));
my_icp->match(start, end);
delete start;
delete end;
}
M4inv(allScans[last]->get_transMat(), invers);
MMult(invers, allScans[first]->get_transMat(), rela);
Matrix4ToQuat(rela, rPosQuat, rPos);
QuatRPYEuler(rPosQuat, rPosTheta);
lum6DEuler::covarianceEuler(allScans[first], allScans[last], my_icp->get_nns_method(), my_icp->get_rnd(), my_icp->get_max_dist_match2(), &C);
outFile << "EDGE3" << " " << last << " " << first << " " <<
(rPos[0]/100) << " " <<
(rPos[1]/100) << " " <<
(rPos[2]/100) << " " <<
rPosTheta[0] << " " <<
rPosTheta[1] << " " <<
rPosTheta[2] << " ";
for(int i = 1; i < 7; i++)
for(int j = i; j < 7; j++)
outFile << C(i, j) << " ";
outFile << endl;
if(first != last-1) {
allScans[last]->transformToMatrix(Pl0,Scan::INVALID);
}
}
outFile.close();
system("sort toro.graph > toro2.graph && mv toro2.graph toro.graph && ./bin/toro3d toro.graph");
ifstream inFile("toro-treeopt-final.graph");
string tag;
int id;
double dd;
double rPosN[3], rPosThetaN[3];
while(inFile) {
inFile >> tag;
if(tag == "VERTEX3") {
inFile >> id;
if(id == n-1) {
inFile >> rPosN[0] >> rPosN[1] >> rPosN[2] >> rPosThetaN[0] >> rPosThetaN[1] >> rPosThetaN[2];
rPosN[0] *= 100;
rPosN[1] *= 100;
rPosN[2] *= 100;
} else {
inFile >> rPos[0] >> rPos[1] >> rPos[2] >> rPosTheta[0] >> rPosTheta[1] >> rPosTheta[2];
rPos[0] *= 100;
rPos[1] *= 100;
rPos[2] *= 100;
RPYEulerQuat(rPosTheta, rPosQuat);
if(id != 0) {
allScans[id]->transformToQuat(rPos, rPosQuat, Scan::GRAPHTORO, 1);
}
}
}
else if(tag == "EDGE3") {
inFile >> id >> id;
for(int i=0; i < 22; i++) {
inFile >> dd;
}
}
}
RPYEulerQuat(rPosThetaN, rPosQuat);
allScans[n-1]->transformToQuat(rPosN, rPosQuat, Scan::GRAPHTORO, 2);
inFile.close();
return DBL_MAX;
}

View file

@ -92,7 +92,8 @@ icp6D::icp6D(icp6Dminimizer *my_icp6Dminimizer, double max_dist_match,
* @param CurrentScan The current scan thas is to be matched * @param CurrentScan The current scan thas is to be matched
* @return The number of iterations done in this matching run * @return The number of iterations done in this matching run
*/ */
int icp6D::match(Scan* PreviousScan, Scan* CurrentScan) int icp6D::match(Scan* PreviousScan, Scan* CurrentScan,
PairingMode pairing_mode)
{ {
// If ICP shall not be applied, then just write // If ICP shall not be applied, then just write
// the identity matrix // the identity matrix
@ -148,7 +149,7 @@ int icp6D::match(Scan* PreviousScan, Scan* CurrentScan)
Scan::getPtPairsParallel(pairs, PreviousScan, CurrentScan, Scan::getPtPairsParallel(pairs, PreviousScan, CurrentScan,
thread_num, step, thread_num, step,
rnd, max_dist_match2, rnd, max_dist_match2,
sum, centroid_m, centroid_d); sum, centroid_m, centroid_d, pairing_mode);
n[thread_num] = (unsigned int)pairs[thread_num].size(); n[thread_num] = (unsigned int)pairs[thread_num].size();
@ -214,7 +215,7 @@ int icp6D::match(Scan* PreviousScan, Scan* CurrentScan)
vector<PtPair> pairs; vector<PtPair> pairs;
Scan::getPtPairs(&pairs, PreviousScan, CurrentScan, 0, rnd, Scan::getPtPairs(&pairs, PreviousScan, CurrentScan, 0, rnd,
max_dist_match2, ret, centroid_m, centroid_d); max_dist_match2, ret, centroid_m, centroid_d, pairing_mode);
// do we have enough point pairs? // do we have enough point pairs?
if (pairs.size() > 3) { if (pairs.size() > 3) {
@ -281,7 +282,7 @@ double icp6D::Point_Point_Error(Scan* PreviousScan, Scan* CurrentScan, double ma
Scan::getPtPairsParallel(pairs, PreviousScan, CurrentScan, Scan::getPtPairsParallel(pairs, PreviousScan, CurrentScan,
thread_num, step, thread_num, step,
rnd, sqr(max_dist_match), rnd, sqr(max_dist_match),
sum, centroid_m, centroid_d); sum, centroid_m, centroid_d, CLOSEST_POINT);
} }
@ -299,7 +300,8 @@ double icp6D::Point_Point_Error(Scan* PreviousScan, Scan* CurrentScan, double ma
double centroid_d[3] = {0.0, 0.0, 0.0}; double centroid_d[3] = {0.0, 0.0, 0.0};
vector<PtPair> pairs; vector<PtPair> pairs;
Scan::getPtPairs(&pairs, PreviousScan, CurrentScan, 0, rnd, sqr(max_dist_match),error, centroid_m, centroid_d); Scan::getPtPairs(&pairs, PreviousScan, CurrentScan, 0, rnd, sqr(max_dist_match),
error, centroid_m, centroid_d, CLOSEST_POINT);
// getPtPairs computes error as sum of squared distances // getPtPairs computes error as sum of squared distances
error = 0; error = 0;
@ -322,7 +324,7 @@ double icp6D::Point_Point_Error(Scan* PreviousScan, Scan* CurrentScan, double ma
* *
* @param allScans Contains all necessary scans. * @param allScans Contains all necessary scans.
*/ */
void icp6D::doICP(vector <Scan *> allScans) void icp6D::doICP(vector <Scan *> allScans, PairingMode pairing_mode)
{ {
double id[16]; double id[16];
M4identity(id); M4identity(id);
@ -346,12 +348,12 @@ void icp6D::doICP(vector <Scan *> allScans)
if (i > 0) { if (i > 0) {
if (meta) { if (meta) {
match(my_MetaScan, CurrentScan); match(my_MetaScan, CurrentScan, pairing_mode);
} else } else
if (cad_matching) { if (cad_matching) {
match(allScans[0], CurrentScan); match(allScans[0], CurrentScan, pairing_mode);
} else { } else {
match(PreviousScan, CurrentScan); match(PreviousScan, CurrentScan, pairing_mode);
} }
} }

View file

@ -67,3 +67,13 @@ double *KDtree::FindClosest(double *_p, double maxdist2, int threadNum) const
_FindClosest(Void(), threadNum); _FindClosest(Void(), threadNum);
return params[threadNum].closest; return params[threadNum].closest;
} }
double *KDtree::FindClosestAlongDir(double *_p, double *_dir, double maxdist2, int threadNum) const
{
params[threadNum].closest = NULL;
params[threadNum].closest_d2 = maxdist2;
params[threadNum].p = _p;
params[threadNum].dir = _dir;
_FindClosestAlongDir(Void(), threadNum);
return params[threadNum].closest;
}

View file

@ -60,6 +60,16 @@ double* KDtreeManaged::FindClosest(double *_p, double maxdist2, int threadNum) c
return params[threadNum].closest; return params[threadNum].closest;
} }
double* KDtreeManaged::FindClosestAlongDir(double *_p, double *_dir, double maxdist2, int threadNum) const
{
params[threadNum].closest = NULL;
params[threadNum].closest_d2 = maxdist2;
params[threadNum].p = _p;
params[threadNum].dir = _dir;
_FindClosestAlongDir(*m_data, threadNum);
return params[threadNum].closest;
}
void KDtreeManaged::lock() void KDtreeManaged::lock()
{ {
boost::lock_guard<boost::mutex> lock(m_mutex_locking); boost::lock_guard<boost::mutex> lock(m_mutex_locking);

View file

@ -97,6 +97,16 @@ double* KDtreeMetaManaged::FindClosest(double *_p, double maxdist2, int threadNu
return params[threadNum].closest; return params[threadNum].closest;
} }
double* KDtreeMetaManaged::FindClosestAlongDir(double *_p, double *_dir, double maxdist2, int threadNum) const
{
params[threadNum].closest = NULL;
params[threadNum].closest_d2 = maxdist2;
params[threadNum].p = _p;
params[threadNum].dir = _dir;
_FindClosestAlongDir(m_data, threadNum);
return params[threadNum].closest;
}
void KDtreeMetaManaged::lock() void KDtreeMetaManaged::lock()
{ {
boost::lock_guard<boost::mutex> lock(m_mutex_locking); boost::lock_guard<boost::mutex> lock(m_mutex_locking);

View file

@ -1,171 +0,0 @@
/*
* loopHOG-Man implementation
*
* Copyright (C) Jochen Sprickerhof
*
* Released under the GPL version 3.
*
*/
/**
* @file HOG-Man wrapper
* @author Jochen Sprickerhof. Institute of Computer Science, University of Osnabrueck, Germany.
*/
#include "slam6d/loopHOG-Man.h"
#include "slam6d/metaScan.h"
#include "slam6d/lum6Deuler.h"
#include "slam6d/globals.icc"
#include <fstream>
using std::ofstream;
using std::ifstream;
#include <boost/graph/graph_traits.hpp>
using boost::graph_traits;
using namespace NEWMAT;
/**
* @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 loopHOGMan::close_loop(const vector <Scan *> &allScans, int first, int last, graph_t &g)
{
int n = num_vertices(g);
Matrix C(6, 6);
double invers[16], rela[16], rPos[3], rPosTheta[3], rPosQuat[4];
ofstream outFile("hogman.graph");
for(int i = 0; i < n; i++) {
QuatRPYEuler(allScans[i]->get_rPosQuat(), rPosTheta);
outFile << "VERTEX3" << " " << i <<
" " << (allScans[i]->get_rPos()[0]/100) <<
" " << (allScans[i]->get_rPos()[1]/100) <<
" " << (allScans[i]->get_rPos()[2]/100) <<
" " << rPosTheta[0] <<
" " << rPosTheta[1] <<
" " << rPosTheta[2] << "\n";
}
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) private(invers, rela, rPos, rPosTheta, rPosQuat, C)
#endif
for(int i = 0; i < num_arcs; i++) {
for(;i > li; li++, ei++) ;
for(;i < li; li--, ei--) ;
int from = source(*ei, g);
int to = target(*ei, g);
M4inv(allScans[from]->get_transMat(), invers);
MMult(invers, allScans[to]->get_transMat(), rela);
Matrix4ToQuat(rela, rPosQuat, rPos);
QuatRPYEuler(rPosQuat, rPosTheta);
lum6DEuler::covarianceEuler(allScans[from], allScans[to], my_icp6D->get_nns_method(), my_icp6D->get_rnd(), my_icp6D->get_max_dist_match2(), &C);
#ifdef _OPENMP
#pragma omp critical
#endif
{
outFile << "EDGE3" << " " << from << " " << to << " " <<
(rPos[0]/100) << " " <<
(rPos[1]/100) << " " <<
(rPos[2]/100) << " " <<
rPosTheta[0] << " " <<
rPosTheta[1] << " " <<
rPosTheta[2] << " ";
for(int i = 1; i < 7; i++)
for(int j = i; j < 7; j++)
outFile << C(i, j) << " ";
outFile << endl;
}
}
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);
my_icp6D->match(start, end);
delete start;
delete end;
M4inv(allScans[last]->get_transMat(), invers);
MMult(invers, allScans[first]->get_transMat(), rela);
Matrix4ToQuat(rela, rPosQuat, rPos);
QuatRPYEuler(rPosQuat, rPosTheta);
lum6DEuler::covarianceEuler(allScans[first], allScans[last], my_icp6D->get_nns_method(), my_icp6D->get_rnd(), my_icp6D->get_max_dist_match2(), &C);
outFile << "EDGE3" << " " << last << " " << first << " " <<
(rPos[0]/100) << " " <<
(rPos[1]/100) << " " <<
(rPos[2]/100) << " " <<
rPosTheta[0] << " " <<
rPosTheta[1] << " " <<
rPosTheta[2] << " " <<
C(1, 1) << " " << C(1, 2) << " " << C(1, 3) << " " << C(1, 4) << " " << C(1, 5) << " " << C(1, 6) << " " <<
C(1, 1) << " " << C(1, 2) << " " << C(1, 3) << " " << C(1, 4) << " " << C(1, 5) << " " <<
C(1, 1) << " " << C(1, 2) << " " << C(1, 3) << " " << C(1, 4) << " " <<
C(1, 1) << " " << C(1, 2) << " " << C(1, 3) << " " <<
C(1, 1) << " " << C(1, 2) << " " <<
C(1, 1) << " " << "\n";
outFile.close();
system("LD_LIBRARY_PATH=./bin/ ./bin/hogman3d -update 1 -oc -o hogman-final.graph hogman.graph");
ifstream inFile("hogman-final.graph");
string tag;
int id;
double dd;
double rPosN[3], rPosThetaN[3];
while(inFile) {
inFile >> tag;
if(tag == "VERTEX3") {
inFile >> id;
if(id == n-1) {
inFile >> rPosN[0] >> rPosN[1] >> rPosN[2] >> rPosThetaN[0] >> rPosThetaN[1] >> rPosThetaN[2];
rPosN[0] *= 100;
rPosN[1] *= 100;
rPosN[2] *= 100;
} else {
inFile >> rPos[0] >> rPos[1] >> rPos[2] >> rPosTheta[0] >> rPosTheta[1] >> rPosTheta[2];
rPos[0] *= 100;
rPos[1] *= 100;
rPos[2] *= 100;
RPYEulerQuat(rPosTheta, rPosQuat);
if(id != 0) {
allScans[id]->transformToQuat(rPos, rPosQuat, Scan::LOOPHOGMAN, 1);
}
}
}
else if(tag == "EDGE3") {
inFile >> id >> id;
for(int i=0; i < 22; i++) {
inFile >> dd;
}
}
}
RPYEulerQuat(rPosThetaN, rPosQuat);
allScans[n-1]->transformToQuat(rPosN, rPosQuat, Scan::LOOPHOGMAN, 2);
inFile.close();
}

View file

@ -1,172 +0,0 @@
/*
* loopToro implementation
*
* Copyright (C) Jochen Sprickerhof
*
* Released under the GPL version 3.
*
*/
/**
* @file TORO wrapper
* @author Jochen Sprickerhof. Institute of Computer Science, University of Osnabrueck, Germany.
*/
#include "slam6d/loopToro.h"
#include "slam6d/metaScan.h"
#include "slam6d/lum6Deuler.h"
#include "slam6d/globals.icc"
#include <fstream>
using std::ofstream;
using std::ifstream;
#include <boost/graph/graph_traits.hpp>
using boost::graph_traits;
using namespace NEWMAT;
/**
* @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 loopToro::close_loop(const vector <Scan *> &allScans, int first, int last, graph_t &g)
{
int n = num_vertices(g);
Matrix C(6, 6);
double invers[16], rela[16], rPos[3], rPosTheta[3], rPosQuat[4];
ofstream outFile("toro.graph");
for(int i = 0; i < n; i++) {
QuatRPYEuler(allScans[i]->get_rPosQuat(), rPosTheta);
outFile << "VERTEX3" << " " << i <<
" " << (allScans[i]->get_rPos()[0]/100) <<
" " << (allScans[i]->get_rPos()[1]/100) <<
" " << (allScans[i]->get_rPos()[2]/100) <<
" " << rPosTheta[0] <<
" " << rPosTheta[1] <<
" " << rPosTheta[2] << "\n";
}
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) private(invers, rela, rPos, rPosTheta, rPosQuat, C)
#endif
for(int i = 0; i < num_arcs; i++) {
for(;i > li; li++, ei++) ;
for(;i < li; li--, ei--) ;
int from = source(*ei, g);
int to = target(*ei, g);
M4inv(allScans[from]->get_transMat(), invers);
MMult(invers, allScans[to]->get_transMat(), rela);
Matrix4ToQuat(rela, rPosQuat, rPos);
QuatRPYEuler(rPosQuat, rPosTheta);
lum6DEuler::covarianceEuler(allScans[from], allScans[to], my_icp6D->get_nns_method(), my_icp6D->get_rnd(), my_icp6D->get_max_dist_match2(), &C);
#ifdef _OPENMP
#pragma omp critical
#endif
{
outFile << "EDGE3" << " " << from << " " << to << " " <<
(rPos[0]/100) << " " <<
(rPos[1]/100) << " " <<
(rPos[2]/100) << " " <<
rPosTheta[0] << " " <<
rPosTheta[1] << " " <<
rPosTheta[2] << " ";
for(int i = 1; i < 7; i++)
for(int j = i; j < 7; j++)
outFile << C(i, j) << " ";
outFile << endl;
}
}
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);
my_icp6D->match(start, end);
delete start;
delete end;
M4inv(allScans[last]->get_transMat(), invers);
MMult(invers, allScans[first]->get_transMat(), rela);
Matrix4ToQuat(rela, rPosQuat, rPos);
QuatRPYEuler(rPosQuat, rPosTheta);
lum6DEuler::covarianceEuler(allScans[first], allScans[last], my_icp6D->get_nns_method(), my_icp6D->get_rnd(), my_icp6D->get_max_dist_match2(), &C);
outFile << "EDGE3" << " " << last << " " << first << " " <<
(rPos[0]/100) << " " <<
(rPos[1]/100) << " " <<
(rPos[2]/100) << " " <<
rPosTheta[0] << " " <<
rPosTheta[1] << " " <<
rPosTheta[2] << " " <<
C(1, 1) << " " << C(1, 2) << " " << C(1, 3) << " " << C(1, 4) << " " << C(1, 5) << " " << C(1, 6) << " " <<
C(1, 1) << " " << C(1, 2) << " " << C(1, 3) << " " << C(1, 4) << " " << C(1, 5) << " " <<
C(1, 1) << " " << C(1, 2) << " " << C(1, 3) << " " << C(1, 4) << " " <<
C(1, 1) << " " << C(1, 2) << " " << C(1, 3) << " " <<
C(1, 1) << " " << C(1, 2) << " " <<
C(1, 1) << " " << "\n";
outFile.close();
system("sort toro.graph > toro2.graph && mv toro2.graph toro.graph && ./bin/toro3d -i 300 toro.graph");
ifstream inFile("toro-treeopt-final.graph");
string tag;
int id;
double dd;
double rPosN[3], rPosThetaN[3];
while(inFile) {
inFile >> tag;
if(tag == "VERTEX3") {
inFile >> id;
if(id == n-1) {
inFile >> rPosN[0] >> rPosN[1] >> rPosN[2] >> rPosThetaN[0] >> rPosThetaN[1] >> rPosThetaN[2];
rPosN[0] *= 100;
rPosN[1] *= 100;
rPosN[2] *= 100;
} else {
inFile >> rPos[0] >> rPos[1] >> rPos[2] >> rPosTheta[0] >> rPosTheta[1] >> rPosTheta[2];
rPos[0] *= 100;
rPos[1] *= 100;
rPos[2] *= 100;
RPYEulerQuat(rPosTheta, rPosQuat);
if(id != 0) {
allScans[id]->transformToQuat(rPos, rPosQuat, Scan::LOOPTORO, 1);
}
}
}
else if(tag == "EDGE3") {
inFile >> id >> id;
for(int i=0; i < 22; i++) {
inFile >> dd;
}
}
}
RPYEulerQuat(rPosThetaN, rPosQuat);
allScans[n-1]->transformToQuat(rPosN, rPosQuat, Scan::LOOPTORO, 2);
inFile.close();
}

View file

@ -46,18 +46,25 @@ PointType::PointType(unsigned int _types) : types(_types) {
pointdim = 3; pointdim = 3;
if (types & PointType::USE_REFLECTANCE) dimensionmap[1] = pointdim++; if (types & PointType::USE_REFLECTANCE) dimensionmap[1] = pointdim++;
if (types & PointType::USE_TEMPERATURE) dimensionmap[2] = pointdim++; if (types & PointType::USE_NORMAL) {
if (types & PointType::USE_AMPLITUDE) dimensionmap[3] = pointdim++; pointdim += 3;
if (types & PointType::USE_DEVIATION) dimensionmap[4] = pointdim++; dimensionmap[2] = pointdim;
if (types & PointType::USE_TYPE) dimensionmap[5] = pointdim++; }
if (types & PointType::USE_COLOR) dimensionmap[6] = pointdim++; if (types & PointType::USE_TEMPERATURE) dimensionmap[3] = pointdim++;
if (types & PointType::USE_TIME) dimensionmap[7] = pointdim++; if (types & PointType::USE_AMPLITUDE) dimensionmap[4] = pointdim++;
if (types & PointType::USE_INDEX) dimensionmap[8] = pointdim++; if (types & PointType::USE_DEVIATION) dimensionmap[5] = pointdim++;
if (types & PointType::USE_TYPE) dimensionmap[6] = pointdim++;
if (types & PointType::USE_COLOR) dimensionmap[7] = pointdim++;
if (types & PointType::USE_TIME) dimensionmap[8] = pointdim++;
if (types & PointType::USE_INDEX) dimensionmap[9] = pointdim++;
} }
bool PointType::hasReflectance() { bool PointType::hasReflectance() {
return hasType(USE_REFLECTANCE); return hasType(USE_REFLECTANCE);
} }
bool PointType::hasNormal() {
return hasType(USE_NORMAL);
}
bool PointType::hasTemperature() { bool PointType::hasTemperature() {
return hasType(USE_TEMPERATURE); return hasType(USE_TEMPERATURE);
} }
@ -116,18 +123,20 @@ unsigned int PointType::getType(unsigned int type) {
return dimensionmap[0]; return dimensionmap[0];
} else if (type == USE_REFLECTANCE) { } else if (type == USE_REFLECTANCE) {
return dimensionmap[1]; return dimensionmap[1];
} else if (type == USE_TEMPERATURE) { } else if (type == USE_NORMAL) {
return dimensionmap[2]; return dimensionmap[2];
} else if (type == USE_AMPLITUDE) { } else if (type == USE_TEMPERATURE) {
return dimensionmap[3]; return dimensionmap[3];
} else if (type == USE_DEVIATION) { } else if (type == USE_AMPLITUDE) {
return dimensionmap[4]; return dimensionmap[4];
} else if (type == USE_TYPE) { } else if (type == USE_DEVIATION) {
return dimensionmap[5]; return dimensionmap[5];
} else if (type == USE_COLOR) { } else if (type == USE_TYPE) {
return dimensionmap[6]; return dimensionmap[6];
} else if (type == USE_TIME) { } else if (type == USE_COLOR) {
return dimensionmap[7]; return dimensionmap[7];
} else if (type == USE_TIME) {
return dimensionmap[8];
} else { } else {
return 0; return 0;
} }
@ -155,14 +164,15 @@ bool PointType::hasType(unsigned int type) {
const unsigned int PointType::USE_NONE = 0; const unsigned int PointType::USE_NONE = 0;
const unsigned int PointType::USE_REFLECTANCE = 1; const unsigned int PointType::USE_REFLECTANCE = 1;
const unsigned int PointType::USE_TEMPERATURE = 2; const unsigned int PointType::USE_NORMAL = 2;
const unsigned int PointType::USE_AMPLITUDE = 4; const unsigned int PointType::USE_TEMPERATURE = 4;
const unsigned int PointType::USE_DEVIATION = 8; const unsigned int PointType::USE_AMPLITUDE = 8;
const unsigned int PointType::USE_HEIGHT = 16; const unsigned int PointType::USE_DEVIATION = 16;
const unsigned int PointType::USE_TYPE = 32; const unsigned int PointType::USE_HEIGHT = 32;
const unsigned int PointType::USE_COLOR = 64; const unsigned int PointType::USE_TYPE = 64;
const unsigned int PointType::USE_TIME = 128; const unsigned int PointType::USE_COLOR = 128;
const unsigned int PointType::USE_INDEX = 256; const unsigned int PointType::USE_TIME = 256;
const unsigned int PointType::USE_INDEX = 512;
void PointType::useScan(Scan* scan) void PointType::useScan(Scan* scan)

View file

@ -17,6 +17,8 @@
#include "slam6d/Boctree.h" #include "slam6d/Boctree.h"
#include "slam6d/globals.icc" #include "slam6d/globals.icc"
#include "normals/normals.h"
#ifdef WITH_METRICS #ifdef WITH_METRICS
#include "slam6d/metrics.h" #include "slam6d/metrics.h"
#endif #endif
@ -37,7 +39,7 @@ bool Scan::scanserver = false;
void Scan::openDirectory(bool scanserver, const std::string& path, IOType type, void Scan::openDirectory(bool scanserver, const std::string& path, IOType type,
int start, int end) int start, int end)
{ {
Scan::scanserver = scanserver; Scan::scanserver = scanserver;
if(scanserver) if(scanserver)
@ -145,7 +147,9 @@ void Scan::toGlobal() {
*/ */
void Scan::createSearchTree() 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 // 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); boost::lock_guard<boost::mutex> lock(m_mutex_create_tree);
if(kd != 0) return; if(kd != 0) return;
@ -165,7 +169,9 @@ void Scan::createSearchTree()
void Scan::calcReducedOnDemand() 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 // 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); boost::lock_guard<boost::mutex> lock(m_mutex_reduction);
if(m_has_reduced) return; if(m_has_reduced) return;
@ -182,6 +188,17 @@ void Scan::calcReducedOnDemand()
#endif //WITH_METRICS #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() void Scan::copyReducedToOriginal()
{ {
#ifdef WITH_METRICS #ifdef WITH_METRICS
@ -223,6 +240,32 @@ void Scan::copyOriginalToReduced()
} }
/**
* 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 * Computes an octtree of the current scan, then getting the
@ -236,137 +279,110 @@ void Scan::calcReducedPoints()
// get xyz to start the scan load, separated here for time measurement // get xyz to start the scan load, separated here for time measurement
DataXYZ xyz(get("xyz")); DataXYZ xyz(get("xyz"));
DataReflectance reflectance(get("reflectance")); DataXYZ xyz_normals(get(""));
if(xyz.size() == 0) if (reduction_pointtype.hasNormal()) {
throw runtime_error("Could not calculate reduced points, XYZ data is empty"); DataXYZ my_xyz_normals(get("normal"));
xyz_normals = my_xyz_normals;
if (reflectance.size()==0) { }
DataReflectance reflectance(get(""));
// no reduction needed if (reduction_pointtype.hasReflectance()) {
// copy vector of points to array of points to avoid DataReflectance my_reflectance(get("reflectance"));
// further copying reflectance = my_reflectance;
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 #ifdef WITH_METRICS
ClientMetric::scan_load_time.end(t); ClientMetric::scan_load_time.end(t);
Timer tl = ClientMetric::calc_reduced_points_time.start(); Timer tl = ClientMetric::calc_reduced_points_time.start();
#endif //WITH_METRICS #endif //WITH_METRICS
// no reduction needed
// copy vector of points to array of points to avoid
// further copying
if(reduction_voxelSize <= 0.0) { if(reduction_voxelSize <= 0.0) {
// copy the points // copy the points
if (reduction_pointtype.hasReflectance()) { DataXYZ xyz_reduced(create("xyz reduced", sizeof(double)*3*xyz.size()));
DataXYZ xyz_reduced(create("xyz reduced", sizeof(double)*3*xyz.size())); for(unsigned int i = 0; i < xyz.size(); ++i) {
DataReflectance reflectance_reduced(create("reflectance reduced", sizeof(double)*reflectance.size())); for(unsigned int j = 0; j < 3; ++j) {
for(unsigned int i = 0; i < xyz.size(); ++i) { xyz_reduced[i][j] = xyz[i][j];
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];
}
}
} }
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 { } 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 // start reduction
// build octree-tree from CurrentScan // build octree-tree from CurrentScan
// put full data into the octtree // put full data into the octtree
BOctTree<double> *oct = new BOctTree<double>(xyz_reflectance, xyz.size(), reduction_voxelSize, reduction_pointtype); BOctTree<double> *oct = new BOctTree<double>(xyz_in,
vector<double*> reduced; xyz.size(),
reduced.clear(); reduction_voxelSize,
reduction_pointtype);
vector<double*> center;
center.clear();
if (reduction_nrpts > 0) { if (reduction_nrpts > 0) {
if (reduction_nrpts == 1) { if (reduction_nrpts == 1) {
oct->GetOctTreeRandom(reduced); oct->GetOctTreeRandom(center);
} else { } else {
oct->GetOctTreeRandom(reduced, reduction_nrpts); oct->GetOctTreeRandom(center, reduction_nrpts);
} }
} else { } else {
oct->GetOctTreeCenter(reduced); oct->GetOctTreeCenter(center);
} }
// storing it as reduced scan // storing it as reduced scan
unsigned int size = reduced.size(); 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()) { if (reduction_pointtype.hasReflectance()) {
DataXYZ xyz_reduced(create("xyz reduced", sizeof(double)*3*size)); DataReflectance my_reflectance_reduced(create("reflectance reduced",
for(unsigned int i = 0; i < size; ++i) { sizeof(float)*size));
for(unsigned int j = 0; j < 3; ++j) { reflectance_reduced = my_reflectance_reduced;
xyz_reduced[i][j] = reduced[i][j]; }
} if (reduction_pointtype.hasNormal()) {
} DataNormal my_normal_reduced(create("normal reduced", sizeof(double)*3*size));
DataReflectance reflectance_reduced(create("reflectance reduced", sizeof(float)*size)); normal_reduced = my_normal_reduced;
for(unsigned int i = 0; i < size; ++i) }
reflectance_reduced[i] = reduced[i][3]; for(unsigned int i = 0; i < size; ++i) {
} else { unsigned int j = 0;
DataXYZ xyz_reduced(create("xyz reduced", sizeof(double)*3*size)); for (; j < 3; ++j)
for(unsigned int i = 0; i < size; ++i) xyz_reduced[i][j] = center[i][j];
for(unsigned int j = 0; j < 3; ++j) if (reduction_pointtype.hasReflectance())
xyz_reduced[i][j] = reduced[i][j]; 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++];
} }
delete oct;
} }
for (unsigned int i = 0; i < xyz.size(); ++i) {
delete[] xyz_reflectance[i];
}
delete[] xyz_reflectance;
#ifdef WITH_METRICS #ifdef WITH_METRICS
ClientMetric::calc_reduced_points_time.end(tl); ClientMetric::calc_reduced_points_time.end(tl);
#endif //WITH_METRICS #endif //WITH_METRICS
}
} }
@ -399,7 +415,7 @@ void Scan::transformAll(const double alignxf[16])
{ {
DataXYZ xyz(get("xyz")); DataXYZ xyz(get("xyz"));
unsigned int i=0 ; unsigned int i=0 ;
// #pragma omp parallel for // #pragma omp parallel for
for(; i < xyz.size(); ++i) { for(; i < xyz.size(); ++i) {
transform3(alignxf, xyz[i]); transform3(alignxf, xyz[i]);
} }
@ -415,11 +431,17 @@ void Scan::transformReduced(const double alignxf[16])
DataXYZ xyz_reduced(get("xyz reduced")); DataXYZ xyz_reduced(get("xyz reduced"));
unsigned int i=0; unsigned int i=0;
// #pragma omp parallel for // #pragma omp parallel for
for( ; i < xyz_reduced.size(); ++i) { for( ; i < xyz_reduced.size(); ++i) {
transform3(alignxf, xyz_reduced[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 #ifdef WITH_METRICS
ClientMetric::transform_time.end(t); ClientMetric::transform_time.end(t);
#endif //WITH_METRICS #endif //WITH_METRICS
@ -438,7 +460,7 @@ void Scan::transformMatrix(const double alignxf[16])
#ifdef DEBUG #ifdef DEBUG
cerr << "(" << rPos[0] << ", " << rPos[1] << ", " << rPos[2] << ", " cerr << "(" << rPos[0] << ", " << rPos[1] << ", " << rPos[2] << ", "
<< rPosTheta[0] << ", " << rPosTheta[1] << ", " << rPosTheta[2] << ")" << endl; << rPosTheta[0] << ", " << rPosTheta[1] << ", " << rPosTheta[2] << ")" << endl;
cerr << transMat << endl; cerr << transMat << endl;
#endif #endif
@ -480,7 +502,7 @@ void Scan::transform(const double alignxf[16], const AlgoType type, int islum)
#ifdef DEBUG #ifdef DEBUG
cerr << alignxf << endl; cerr << alignxf << endl;
cerr << "(" << rPos[0] << ", " << rPos[1] << ", " << rPos[2] << ", " cerr << "(" << rPos[0] << ", " << rPos[1] << ", " << rPos[2] << ", "
<< rPosTheta[0] << ", " << rPosTheta[1] << ", " << rPosTheta[2] << ") ---> "; << rPosTheta[0] << ", " << rPosTheta[1] << ", " << rPosTheta[2] << ") ---> ";
#endif #endif
// transform points // transform points
@ -572,7 +594,7 @@ void Scan::transform(const double alignxf[16], const AlgoType type, int islum)
* 2 LUM transformation, last scan only * 2 LUM transformation, last scan only
*/ */
void Scan::transform(const double alignQuat[4], const double alignt[3], void Scan::transform(const double alignQuat[4], const double alignt[3],
const AlgoType type, int islum) const AlgoType type, int islum)
{ {
double alignxf[16]; double alignxf[16];
QuatToMatrix4(alignQuat, alignt, alignxf); QuatToMatrix4(alignQuat, alignt, alignxf);
@ -655,9 +677,9 @@ void Scan::transformToQuat(double rP[3], double rPQ[4], const AlgoType type, int
*/ */
void Scan::getNoPairsSimple(vector <double*> &diff, void Scan::getNoPairsSimple(vector <double*> &diff,
Scan* Source, Scan* Target, Scan* Source, Scan* Target,
int thread_num, int thread_num,
double max_dist_match2) double max_dist_match2)
{ {
DataXYZ xyz_reduced(Source->get("xyz reduced")); DataXYZ xyz_reduced(Source->get("xyz reduced"));
KDtree* kd = new KDtree(PointerArray<double>(Target->get("xyz reduced")).get(), Target->size<DataXYZ>("xyz reduced")); KDtree* kd = new KDtree(PointerArray<double>(Target->get("xyz reduced")).get(), Target->size<DataXYZ>("xyz reduced"));
@ -694,10 +716,10 @@ void Scan::getNoPairsSimple(vector <double*> &diff,
* @param max_dist_match2 maximal allowed distance for matching * @param max_dist_match2 maximal allowed distance for matching
*/ */
void Scan::getPtPairsSimple(vector <PtPair> *pairs, void Scan::getPtPairsSimple(vector <PtPair> *pairs,
Scan* Source, Scan* Target, Scan* Source, Scan* Target,
int thread_num, int thread_num,
int rnd, double max_dist_match2, int rnd, double max_dist_match2,
double *centroid_m, double *centroid_d) double *centroid_m, double *centroid_d)
{ {
KDtree* kd = new KDtree(PointerArray<double>(Source->get("xyz reduced")).get(), Source->size<DataXYZ>("xyz reduced")); KDtree* kd = new KDtree(PointerArray<double>(Source->get("xyz reduced")).get(), Source->size<DataXYZ>("xyz reduced"));
DataXYZ xyz_reduced(Target->get("xyz reduced")); DataXYZ xyz_reduced(Target->get("xyz reduced"));
@ -751,10 +773,10 @@ void Scan::getPtPairsSimple(vector <PtPair> *pairs,
* @return a set of corresponding point pairs * @return a set of corresponding point pairs
*/ */
void Scan::getPtPairs(vector <PtPair> *pairs, void Scan::getPtPairs(vector <PtPair> *pairs,
Scan* Source, Scan* Target, Scan* Source, Scan* Target,
int thread_num, int thread_num,
int rnd, double max_dist_match2, double &sum, int rnd, double max_dist_match2, double &sum,
double *centroid_m, double *centroid_d) double *centroid_m, double *centroid_d, PairingMode pairing_mode)
{ {
// initialize centroids // initialize centroids
for(unsigned int i = 0; i < 3; ++i) { for(unsigned int i = 0; i < 3; ++i) {
@ -764,10 +786,12 @@ void Scan::getPtPairs(vector <PtPair> *pairs,
// get point pairs // get point pairs
DataXYZ xyz_reduced(Target->get("xyz reduced")); DataXYZ xyz_reduced(Target->get("xyz reduced"));
DataNormal normal_reduced(Target->get("normal reduced"));
Source->getSearchTree()->getPtPairs(pairs, Source->dalignxf, Source->getSearchTree()->getPtPairs(pairs, Source->dalignxf,
xyz_reduced, 0, xyz_reduced.size(), xyz_reduced, normal_reduced, 0, xyz_reduced.size(),
thread_num, thread_num,
rnd, max_dist_match2, sum, centroid_m, centroid_d); rnd, max_dist_match2, sum, centroid_m, centroid_d,
pairing_mode);
// normalize centroids // normalize centroids
unsigned int size = pairs->size(); unsigned int size = pairs->size();
@ -801,11 +825,14 @@ void Scan::getPtPairs(vector <PtPair> *pairs,
* by Langis / Greenspan / Godin, IEEE 3DIM 2001 * by Langis / Greenspan / Godin, IEEE 3DIM 2001
* *
*/ */
void Scan::getPtPairsParallel(vector <PtPair> *pairs, Scan* Source, Scan* Target, void Scan::getPtPairsParallel(vector <PtPair> *pairs,
int thread_num, int step, Scan* Source, Scan* Target,
int rnd, double max_dist_match2, int thread_num, int step,
double *sum, int rnd, double max_dist_match2,
double centroid_m[OPENMP_NUM_THREADS][3], double centroid_d[OPENMP_NUM_THREADS][3]) double *sum,
double centroid_m[OPENMP_NUM_THREADS][3],
double centroid_d[OPENMP_NUM_THREADS][3],
PairingMode pairing_mode)
{ {
// initialize centroids // initialize centroids
for(unsigned int i = 0; i < 3; ++i) { for(unsigned int i = 0; i < 3; ++i) {
@ -822,22 +849,26 @@ void Scan::getPtPairsParallel(vector <PtPair> *pairs, Scan* Source, Scan* Target
for(unsigned int i = 0; i < meta->size(); ++i) { for(unsigned int i = 0; i < meta->size(); ++i) {
// determine step for each scan individually // determine step for each scan individually
DataXYZ xyz_reduced(meta->getScan(i)->get("xyz reduced")); DataXYZ xyz_reduced(meta->getScan(i)->get("xyz reduced"));
DataNormal normal_reduced(Target->get("normal reduced"));
unsigned int max = xyz_reduced.size(); unsigned int max = xyz_reduced.size();
unsigned int step = max / OPENMP_NUM_THREADS; unsigned int step = max / OPENMP_NUM_THREADS;
// call ptpairs for each scan and accumulate ptpairs, centroids and sum // call ptpairs for each scan and accumulate ptpairs, centroids and sum
search->getPtPairs(&pairs[thread_num], Source->dalignxf, search->getPtPairs(&pairs[thread_num], Source->dalignxf,
xyz_reduced, step * thread_num, step * thread_num + step, xyz_reduced, normal_reduced,
thread_num, step * thread_num, step * thread_num + step,
rnd, max_dist_match2, sum[thread_num], thread_num,
centroid_m[thread_num], centroid_d[thread_num]); rnd, max_dist_match2, sum[thread_num],
centroid_m[thread_num], centroid_d[thread_num], pairing_mode);
} }
} else { } else {
DataXYZ xyz_reduced(Target->get("xyz reduced")); DataXYZ xyz_reduced(Target->get("xyz reduced"));
DataNormal normal_reduced(Target->get("normal reduced"));
search->getPtPairs(&pairs[thread_num], Source->dalignxf, search->getPtPairs(&pairs[thread_num], Source->dalignxf,
xyz_reduced, thread_num * step, thread_num * step + step, xyz_reduced, normal_reduced,
thread_num, thread_num * step, thread_num * step + step,
rnd, max_dist_match2, sum[thread_num], thread_num,
centroid_m[thread_num], centroid_d[thread_num]); rnd, max_dist_match2, sum[thread_num],
centroid_m[thread_num], centroid_d[thread_num], pairing_mode);
} }
// normalize centroids // normalize centroids

View file

@ -18,12 +18,19 @@
#include "slam6d/scan.h" #include "slam6d/scan.h"
#include "slam6d/globals.icc" #include "slam6d/globals.icc"
#include <stdexcept>
double *SearchTree::FindClosestAlongDir(double *_p, double *_dir, double maxdist2, int threadNum) const
{
throw std::runtime_error("Method FindClosestAlongDir is not implemented");
}
void SearchTree::getPtPairs(vector <PtPair> *pairs, void SearchTree::getPtPairs(vector <PtPair> *pairs,
double *source_alignxf, // source double *source_alignxf, // source
double * const *q_points, unsigned int startindex, unsigned int endindex, // target double * const *q_points, unsigned int startindex, unsigned int endindex, // target
int thread_num, int thread_num,
int rnd, double max_dist_match2, double &sum, int rnd, double max_dist_match2, double &sum,
double *centroid_m, double *centroid_d) double *centroid_m, double *centroid_d)
{ {
// prepare this tree for resource access in FindClosest // prepare this tree for resource access in FindClosest
lock(); lock();
@ -63,7 +70,7 @@ void SearchTree::getPtPairs(vector <PtPair> *pairs,
sum += Len2(p12); sum += Len2(p12);
pairs->push_back(myPair); pairs->push_back(myPair);
/*cout << "PTPAIR" << i << " " /*cout << "PTPAIR" << i << " "
<< p[0] << " " << p[0] << " "
<< p[1] << " " << p[1] << " "
<< p[2] << " - " << p[2] << " - "
@ -81,11 +88,11 @@ void SearchTree::getPtPairs(vector <PtPair> *pairs,
} }
void SearchTree::getPtPairs(vector <PtPair> *pairs, void SearchTree::getPtPairs(vector <PtPair> *pairs,
double *source_alignxf, // source double *source_alignxf, // source
const DataXYZ& xyz_r, unsigned int startindex, unsigned int endindex, // target const DataXYZ& xyz_r, const DataNormal& normal_r, unsigned int startindex, unsigned int endindex, // target
int thread_num, int thread_num,
int rnd, double max_dist_match2, double &sum, int rnd, double max_dist_match2, double &sum,
double *centroid_m, double *centroid_d) double *centroid_m, double *centroid_d, PairingMode pairing_mode)
{ {
// prepare this tree for resource access in FindClosest // prepare this tree for resource access in FindClosest
lock(); lock();
@ -95,7 +102,7 @@ void SearchTree::getPtPairs(vector <PtPair> *pairs,
// t is the original point from target, s is the (inverted) query point from target and then // t is the original point from target, s is the (inverted) query point from target and then
// the closest point in source // the closest point in source
double t[3], s[3]; double t[3], s[3], normal[3];
for (unsigned int i = startindex; i < endindex; i++) { for (unsigned int i = startindex; i < endindex; i++) {
if (rnd > 1 && rand(rnd) != 0) continue; // take about 1/rnd-th of the numbers only if (rnd > 1 && rand(rnd) != 0) continue; // take about 1/rnd-th of the numbers only
@ -105,10 +112,43 @@ void SearchTree::getPtPairs(vector <PtPair> *pairs,
transform3(local_alignxf_inv, t, s); transform3(local_alignxf_inv, t, s);
double *closest = this->FindClosest(s, max_dist_match2, thread_num); double *closest;
if (pairing_mode != CLOSEST_POINT) {
normal[0] = normal_r[i][0];
normal[1] = normal_r[i][1];
normal[2] = normal_r[i][2];
Normalize3(normal);
}
if (pairing_mode == CLOSEST_POINT_ALONG_NORMAL) {
transform3normal(local_alignxf_inv, normal);
closest = this->FindClosestAlongDir(s, normal, max_dist_match2, thread_num);
// discard points farther than 20 cm
if (closest && sqrt(Dist2(closest, s)) > 20) closest = NULL;
} else {
closest = this->FindClosest(s, max_dist_match2, thread_num);
}
if (closest) { if (closest) {
transform3(source_alignxf, closest, s); transform3(source_alignxf, closest, s);
if (pairing_mode == CLOSEST_PLANE) {
// need to mutate s if we are looking for closest point-to-plane
// s_ = (n,s-t)*n + t
// to find the projection of s onto plane formed by normal n and point t
double tmp[3], s_[3];
double dot;
sub3(s, t, tmp);
dot = Dot(normal, tmp);
scal_mul3(normal, dot, tmp);
add3(tmp, t, s_);
s[0] = s_[0];
s[1] = s_[1];
s[2] = s_[2];
}
// This should be right, model=Source=First=not moving // This should be right, model=Source=First=not moving
centroid_m[0] += s[0]; centroid_m[0] += s[0];
centroid_m[1] += s[1]; centroid_m[1] += s[1];
@ -125,7 +165,7 @@ void SearchTree::getPtPairs(vector <PtPair> *pairs,
sum += Len2(p12); sum += Len2(p12);
pairs->push_back(myPair); pairs->push_back(myPair);
/*cout << "PTPAIR" << i << " " /*cout << "PTPAIR" << i << " "
<< p[0] << " " << p[0] << " "
<< p[1] << " " << p[1] << " "
<< p[2] << " - " << p[2] << " - "

View file

@ -41,14 +41,10 @@
#include "slam6d/lum6Deuler.h" #include "slam6d/lum6Deuler.h"
#include "slam6d/lum6Dquat.h" #include "slam6d/lum6Dquat.h"
#include "slam6d/ghelix6DQ2.h" #include "slam6d/ghelix6DQ2.h"
#include "slam6d/graphToro.h"
#include "slam6d/graphHOG-Man.h"
#include "slam6d/elch6Deuler.h" #include "slam6d/elch6Deuler.h"
#include "slam6d/elch6Dquat.h" #include "slam6d/elch6Dquat.h"
#include "slam6d/elch6DunitQuat.h" #include "slam6d/elch6DunitQuat.h"
#include "slam6d/elch6Dslerp.h" #include "slam6d/elch6Dslerp.h"
#include "slam6d/loopToro.h"
#include "slam6d/loopHOG-Man.h"
#include "slam6d/graphSlam6D.h" #include "slam6d/graphSlam6D.h"
#include "slam6d/gapx6D.h" #include "slam6d/gapx6D.h"
#include "slam6d/graph.h" #include "slam6d/graph.h"
@ -103,17 +99,17 @@ void sigSEGVhandler (int v)
if(!segfault) { if(!segfault) {
segfault = true; segfault = true;
cout << endl cout << endl
<< "# **************************** #" << endl << "# **************************** #" << endl
<< " Segmentation fault or Ctrl-C" << endl << " Segmentation fault or Ctrl-C" << endl
<< "# **************************** #" << endl << "# **************************** #" << endl
<< endl; << endl;
// save frames and close scans // save frames and close scans
for(ScanVector::iterator it = Scan::allScans.begin(); it != Scan::allScans.end(); ++it) { for(ScanVector::iterator it = Scan::allScans.begin(); it != Scan::allScans.end(); ++it) {
(*it)->saveFrames(); (*it)->saveFrames();
} }
cout << "Frames saved." << endl; cout << "Frames saved." << endl;
Scan::closeDirectory(); Scan::closeDirectory();
} }
exit(-1); exit(-1);
} }
@ -132,149 +128,144 @@ void usage(char* prog)
const string normal(""); const string normal("");
#endif #endif
cout << endl cout << endl
<< bold << "USAGE " << normal << endl << bold << "USAGE " << normal << endl
<< " " << prog << " [options] directory" << endl << endl; << " " << prog << " [options] directory" << endl << endl;
cout << bold << "OPTIONS" << normal << endl cout << bold << "OPTIONS" << normal << endl
<< bold << " -a" << normal << " NR, " << bold << "--algo=" << normal << "NR [default: 1]" << endl << bold << " -a" << normal << " NR, " << bold << "--algo=" << normal << "NR [default: 1]" << endl
<< " selects the minimizazion method for the ICP matching algorithm" << endl << " selects the minimizazion method for the ICP matching algorithm" << endl
<< " 1 = unit quaternion based method by Horn" << endl << " 1 = unit quaternion based method by Horn" << endl
<< " 2 = singular value decomposition by Arun et al. " << endl << " 2 = singular value decomposition by Arun et al. " << endl
<< " 3 = orthonormal matrices by Horn et al." << endl << " 3 = orthonormal matrices by Horn et al." << endl
<< " 4 = dual quaternion method by Walker et al." << endl << " 4 = dual quaternion method by Walker et al." << endl
<< " 5 = helix approximation by Hofer & Potmann" << endl << " 5 = helix approximation by Hofer & Potmann" << endl
<< " 6 = small angle approximation" << endl << " 6 = small angle approximation" << endl
<< " 7 = Lu & Milios style, i.e., uncertainty based, with Euler angles" << endl << " 7 = Lu & Milios style, i.e., uncertainty based, with Euler angles" << endl
<< " 8 = Lu & Milios style, i.e., uncertainty based, with Quaternion" << endl << " 8 = Lu & Milios style, i.e., uncertainty based, with Quaternion" << endl
<< " 9 = unit quaternion with scale method by Horn" << endl << " 9 = unit quaternion with scale method by Horn" << endl
<< endl << endl
<< bold << " -A" << normal << " NR, " << bold << "--anim=" << normal << "NR [default: first and last frame only]" << endl << bold << " -A" << normal << " NR, " << bold << "--anim=" << normal << "NR [default: first and last frame only]" << endl
<< " if specified, use only every NR-th frame for animation" << endl << " if specified, use only every NR-th frame for animation" << endl
<< endl << endl
<< bold << " -c" << normal << " NR, " << bold << "--cldist=" << normal << "NR [default: 500]" << endl << bold << " -c" << normal << " NR, " << bold << "--cldist=" << normal << "NR [default: 500]" << endl
<< " specifies the maximal distance for closed loops" << endl << " specifies the maximal distance for closed loops" << endl
<< endl << endl
<< bold << " -C" << normal << " NR, " << bold << "--clpairs=" << normal << "NR [default: 6]" << endl << bold << " -C" << normal << " NR, " << bold << "--clpairs=" << normal << "NR [default: 6]" << endl
<< " specifies the minimal number of points for an overlap. If not specified" << endl << " specifies the minimal number of points for an overlap. If not specified" << endl
<< " cldist is used instead" << endl << " cldist is used instead" << endl
<< endl << endl
<< bold << " --cache" << normal << endl << bold << " --cache" << normal << endl
<< " turns on cached k-d tree search" << endl << " turns on cached k-d tree search" << endl
<< endl << endl
<< bold << " -d" << normal << " NR, " << bold << "--dist=" << normal << "NR [default: 25]" << endl << bold << " -d" << normal << " NR, " << bold << "--dist=" << normal << "NR [default: 25]" << endl
<< " sets the maximal point-to-point distance for matching with ICP to <NR> 'units'" << endl << " sets the maximal point-to-point distance for matching with ICP to <NR> 'units'" << endl
<< " (unit of scan data, e.g. cm)" << endl << " (unit of scan data, e.g. cm)" << endl
<< endl << endl
<< bold << " -D" << normal << " NR, " << bold << "--distSLAM=" << bold << " -D" << normal << " NR, " << bold << "--distSLAM="
<< normal << "NR [default: same value as -d option]" << endl << normal << "NR [default: same value as -d option]" << endl
<< " sets the maximal point-to-point distance for matching with SLAM to <NR> 'units'" << endl << " sets the maximal point-to-point distance for matching with SLAM to <NR> 'units'" << endl
<< " (unit of scan data, e.g. cm)" << endl << " (unit of scan data, e.g. cm)" << endl
<< endl << endl
<< bold << " --DlastSLAM" << normal << " NR [default not set]" << endl << bold << " --DlastSLAM" << normal << " NR [default not set]" << endl
<< " sets the maximal point-to-point distance for the final SLAM correction," << endl << " sets the maximal point-to-point distance for the final SLAM correction," << endl
<< " if final SLAM is not required don't set it." << endl << " if final SLAM is not required don't set it." << endl
<< endl << endl
<< bold << " -e" << normal << " NR, " << bold << "--end=" << normal << "NR" << endl << bold << " -e" << normal << " NR, " << bold << "--end=" << normal << "NR" << endl
<< " end after scan NR" << endl << " end after scan NR" << endl
<< endl << endl
<< bold << " --exportAllPoints" << normal << endl << bold << " --exportAllPoints" << normal << endl
<< " writes all registered reduced points to the file points.pts before" << endl << " writes all registered reduced points to the file points.pts before" << endl
<< " slam6D terminated" << endl << " slam6D terminated" << endl
<< endl << endl
<< bold << " --epsICP=" << normal << "NR [default: 0.00001]" << endl << bold << " --epsICP=" << normal << "NR [default: 0.00001]" << endl
<< " stop ICP iteration if difference is smaller than NR" << endl << " stop ICP iteration if difference is smaller than NR" << endl
<< endl << endl
<< bold << " --epsSLAM=" << normal << " NR [default: 0.5]" << endl << bold << " --epsSLAM=" << normal << " NR [default: 0.5]" << endl
<< " stop SLAM iteration if average difference is smaller than NR" << endl << " stop SLAM iteration if average difference is smaller than NR" << endl
<< endl << endl
<< bold << " -f" << normal << " F, " << bold << "--format=" << normal << "F" << endl << bold << " -f" << normal << " F, " << bold << "--format=" << normal << "F" << endl
<< " using shared library F for input" << endl << " using shared library F for input" << endl
<< " (chose F from {uos, uos_map, uos_rgb, uos_frames, uos_map_frames, old, rts, rts_map, ifp, riegl_txt, riegl_rgb, riegl_bin, zahn, ply, wrl, xyz, zuf, iais, front, x3d, rxp, ais })" << endl << " (chose F from {uos, uos_map, uos_rgb, uos_frames, uos_map_frames, old, rts, rts_map, ifp, riegl_txt, riegl_rgb, riegl_bin, zahn, ply, wrl, xyz, zuf, iais, front, x3d, rxp, ais })" << endl
<< endl << endl
<< bold << " -G" << normal << " NR, " << bold << "--graphSlam6DAlgo=" << normal << "NR [default: 0]" << endl << bold << " -G" << normal << " NR, " << bold << "--graphSlam6DAlgo=" << normal << "NR [default: 0]" << endl
<< " selects the minimizazion method for the SLAM matching algorithm" << endl << " selects the minimizazion method for the SLAM matching algorithm" << endl
<< " 0 = no global relaxation technique" << endl << " 0 = no global relaxation technique" << endl
<< " 1 = Lu & Milios extension using euler angles due to Borrmann et al." << endl << " 1 = Lu & Milios extension using euler angles due to Borrmann et al." << endl
<< " 2 = Lu & Milios extension using using unit quaternions" << endl << " 2 = Lu & Milios extension using using unit quaternions" << endl
<< " 3 = HELIX approximation by Hofer and Pottmann" << endl << " 3 = HELIX approximation by Hofer and Pottmann" << endl
<< " 4 = small angle approximation" << endl << " 4 = small angle approximation" << endl
<< " 5 = TORO" << endl << bold << " -i" << normal << " NR, " << bold << "--iter=" << normal << "NR [default: 50]" << endl
<< " 6 = HOG-Man" << endl << " sets the maximal number of ICP iterations to <NR>" << endl
<< endl << endl
<< bold << " -i" << normal << " NR, " << bold << "--iter=" << normal << "NR [default: 50]" << endl << bold << " -I" << normal << " NR, " << bold << "--iterSLAM=" << normal << "NR [default: 0]" << endl
<< " sets the maximal number of ICP iterations to <NR>" << endl << " sets the maximal number of iterations for SLAM to <NR>" << endl
<< endl << " (if not set, graphSLAM is not executed)" << endl
<< bold << " -I" << normal << " NR, " << bold << "--iterSLAM=" << normal << "NR [default: 0]" << endl << endl
<< " sets the maximal number of iterations for SLAM to <NR>" << endl << bold << " -l" << normal << " NR, " << bold << "--loopsize=" << normal << "NR [default: 20]" << endl
<< " (if not set, graphSLAM is not executed)" << endl << " sets the size of a loop, i.e., a loop must exceed <NR> of scans" << endl
<< endl << endl
<< bold << " -l" << normal << " NR, " << bold << "--loopsize=" << normal << "NR [default: 20]" << endl << bold << " -L" << normal << " NR, " << bold << "--loop6DAlgo=" << normal << "NR [default: 0]" << endl
<< " sets the size of a loop, i.e., a loop must exceed <NR> of scans" << endl << " selects the method for closing the loop explicitly" << endl
<< endl << " 0 = no loop closing technique" << endl
<< bold << " -L" << normal << " NR, " << bold << "--loop6DAlgo=" << normal << "NR [default: 0]" << endl << " 1 = euler angles" << endl
<< " selects the method for closing the loop explicitly" << endl << " 2 = quaternions " << endl
<< " 0 = no loop closing technique" << endl << " 3 = unit quaternions" << endl
<< " 1 = euler angles" << endl << " 4 = SLERP (recommended)" << endl
<< " 2 = quaternions " << endl << endl
<< " 3 = unit quaternions" << endl << bold << " --metascan" << normal << endl
<< " 4 = SLERP (recommended)" << endl << " Match current scan against a meta scan of all previous scans (default match against the last scan only)" << endl
<< " 5 = TORO" << endl << endl
<< " 6 = HOG-Man" << endl << bold << " -m" << normal << " NR, " << bold << "--max=" << normal << "NR" << endl
<< endl << " neglegt all data points with a distance larger than NR 'units'" << endl
<< bold << " --metascan" << normal << endl << endl
<< " Match current scan against a meta scan of all previous scans (default match against the last scan only)" << endl << bold << " -M" << normal << " NR, " << bold << "--min=" << normal << "NR" << endl
<< endl << " neglegt all data points with a distance smaller than NR 'units'" << endl
<< bold << " -m" << normal << " NR, " << bold << "--max=" << normal << "NR" << endl << endl
<< " neglegt all data points with a distance larger than NR 'units'" << endl << bold << " -n" << normal << " FILE, " << bold << "--net=" << normal << "FILE" << endl
<< endl << " specifies the file that includes the net structure for SLAM" << endl
<< bold << " -M" << normal << " NR, " << bold << "--min=" << normal << "NR" << endl << endl
<< " neglegt all data points with a distance smaller than NR 'units'" << endl << bold << " -O" << normal << "NR (optional), " << bold << "--octree=" << normal << "NR (optional)" << endl
<< endl << " use randomized octree based point reduction (pts per voxel=<NR>)" << endl
<< bold << " -n" << normal << " FILE, " << bold << "--net=" << normal << "FILE" << endl << " requires " << bold << "-r" << normal <<" or " << bold << "--reduce" << endl
<< " specifies the file that includes the net structure for SLAM" << endl << endl
<< endl << bold << " -p, --trustpose" << normal << endl
<< bold << " -O" << normal << "NR (optional), " << bold << "--octree=" << normal << "NR (optional)" << endl << " Trust the pose file, do not extrapolate the last transformation." << endl
<< " use randomized octree based point reduction (pts per voxel=<NR>)" << endl << " (just for testing purposes, or gps input.)" << endl
<< " requires " << bold << "-r" << normal <<" or " << bold << "--reduce" << endl << endl
<< endl << bold << " -q, --quiet" << normal << endl
<< bold << " -p, --trustpose" << normal << endl << " Quiet mode. Suppress (most) messages" << endl
<< " Trust the pose file, do not extrapolate the last transformation." << endl << endl
<< " (just for testing purposes, or gps input.)" << endl << bold << " -Q, --veryquiet" << normal << endl
<< endl << " Very quiet mode. Suppress all messages, except in case of error." << endl
<< bold << " -q, --quiet" << normal << endl << endl
<< " Quiet mode. Suppress (most) messages" << endl << bold << " -S, --scanserver" << normal << endl
<< endl << " Use the scanserver as an input method and handling of scan data" << endl
<< bold << " -Q, --veryquiet" << normal << endl << endl
<< " Very quiet mode. Suppress all messages, except in case of error." << endl << bold << " -r" << normal << " NR, " << bold << "--reduce=" << normal << "NR" << endl
<< endl << " turns on octree based point reduction (voxel size=<NR>)" << endl
<< bold << " -S, --scanserver" << normal << endl << endl
<< " Use the scanserver as an input method and handling of scan data" << endl << bold << " -R" << normal << " NR, " << bold << "--random=" << normal << "NR" << endl
<< endl << " turns on randomized reduction, using about every <NR>-th point only" << endl
<< bold << " -r" << normal << " NR, " << bold << "--reduce=" << normal << "NR" << endl << endl
<< " turns on octree based point reduction (voxel size=<NR>)" << endl << bold << " -s" << normal << " NR, " << bold << "--start=" << normal << "NR" << endl
<< endl << " start at scan NR (i.e., neglects the first NR scans)" << endl
<< bold << " -R" << normal << " NR, " << bold << "--random=" << normal << "NR" << endl << " [ATTENTION: counting naturally starts with 0]" << endl
<< " turns on randomized reduction, using about every <NR>-th point only" << endl << endl
<< endl << bold << " -t" << normal << " NR, " << bold << "--nns_method=" << normal << "NR [default: 1]" << endl
<< bold << " -s" << normal << " NR, " << bold << "--start=" << normal << "NR" << endl << " selects the Nearest Neighbor Search Algorithm" << endl
<< " start at scan NR (i.e., neglects the first NR scans)" << endl << " 0 = simple k-d tree " << endl
<< " [ATTENTION: counting naturally starts with 0]" << endl << " 1 = cached k-d tree " << endl
<< endl << " 2 = ANNTree " << endl
<< bold << " -t" << normal << " NR, " << bold << "--nns_method=" << normal << "NR [default: 1]" << endl << " 3 = BOCTree " << endl
<< " selects the Nearest Neighbor Search Algorithm" << endl << endl
<< " 0 = simple k-d tree " << endl << bold << " -u" << normal <<", "<< bold<<"--cuda" << normal << endl
<< " 1 = cached k-d tree " << endl << " this option activates icp running on GPU instead of CPU"<<endl
<< " 2 = ANNTree " << endl << endl << endl;
<< " 3 = BOCTree " << endl
<< endl
<< bold << " -u" << normal <<", "<< bold<<"--cuda" << normal << endl
<< " this option activates icp running on GPU instead of CPU"<<endl
<< endl << endl;
cout << bold << "EXAMPLES " << normal << endl cout << bold << "EXAMPLES " << normal << endl
<< " " << prog << " dat" << endl << " " << prog << " dat" << endl
<< " " << prog << " --max=500 -r 10.2 -i 20 dat" << endl << " " << prog << " --max=500 -r 10.2 -i 20 dat" << endl
<< " " << prog << " -s 2 -e 10 dat" << endl << endl; << " " << prog << " -s 2 -e 10 dat" << endl << endl;
exit(1); exit(1);
} }
@ -308,13 +299,13 @@ void usage(char* prog)
* @return 0, if the parsing was successful. 1 otherwise * @return 0, if the parsing was successful. 1 otherwise
*/ */
int parseArgs(int argc, char **argv, string &dir, double &red, int &rand, int parseArgs(int argc, char **argv, string &dir, double &red, int &rand,
double &mdm, double &mdml, double &mdmll, double &mdm, double &mdml, double &mdmll,
int &mni, int &start, int &end, int &maxDist, int &minDist, bool &quiet, bool &veryQuiet, int &mni, int &start, int &end, int &maxDist, int &minDist, bool &quiet, bool &veryQuiet,
bool &extrapolate_pose, bool &meta, int &algo, int &loopSlam6DAlgo, int &lum6DAlgo, int &anim, bool &extrapolate_pose, bool &meta, int &algo, int &loopSlam6DAlgo, int &lum6DAlgo, int &anim,
int &mni_lum, string &net, double &cldist, int &clpairs, int &loopsize, int &mni_lum, string &net, double &cldist, int &clpairs, int &loopsize,
double &epsilonICP, double &epsilonSLAM, int &nns_method, bool &exportPts, double &distLoop, double &epsilonICP, double &epsilonSLAM, int &nns_method, bool &exportPts, double &distLoop,
int &iterLoop, double &graphDist, int &octree, bool &cuda_enabled, IOType &type, int &iterLoop, double &graphDist, int &octree, bool &cuda_enabled, IOType &type,
bool& scanserver) bool& scanserver, PairingMode& pairing_mode)
{ {
int c; int c;
// from unistd.h: // from unistd.h:
@ -355,6 +346,8 @@ int parseArgs(int argc, char **argv, string &dir, double &red, int &rand,
{ "DlastSLAM", required_argument, 0, '4' }, // use the long format only { "DlastSLAM", required_argument, 0, '4' }, // use the long format only
{ "epsICP", required_argument, 0, '5' }, // use the long format only { "epsICP", required_argument, 0, '5' }, // use the long format only
{ "epsSLAM", required_argument, 0, '6' }, // use the long format only { "epsSLAM", required_argument, 0, '6' }, // use the long format only
{ "normalshoot", no_argument, 0, '7' }, // use the long format only
{ "point-to-plane", no_argument, 0, 'z' }, // use the long format only
{ "exportAllPoints", no_argument, 0, '8' }, { "exportAllPoints", no_argument, 0, '8' },
{ "distLoop", required_argument, 0, '9' }, // use the long format only { "distLoop", required_argument, 0, '9' }, // use the long format only
{ "iterLoop", required_argument, 0, '1' }, // use the long format only { "iterLoop", required_argument, 0, '1' }, // use the long format only
@ -367,141 +360,147 @@ int parseArgs(int argc, char **argv, string &dir, double &red, int &rand,
cout << endl; cout << endl;
while ((c = getopt_long(argc, argv, "O:f:A:G:L:a:t:r:R:d:D:i:l:I:c:C:n:s:e:m:M:uqQpS", longopts, NULL)) != -1) { while ((c = getopt_long(argc, argv, "O:f:A:G:L:a:t:r:R:d:D:i:l:I:c:C:n:s:e:m:M:uqQpS", longopts, NULL)) != -1) {
switch (c) { switch (c) {
case 'a': case 'a':
algo = atoi(optarg); algo = atoi(optarg);
if ((algo < 0) || (algo > 9)) { if ((algo < 0) || (algo > 9)) {
cerr << "Error: ICP Algorithm not available." << endl; cerr << "Error: ICP Algorithm not available." << endl;
exit(1); exit(1);
} }
break; break;
case 't': case 't':
nns_method = atoi(optarg); nns_method = atoi(optarg);
if ((nns_method < 0) || (nns_method > 3)) { if ((nns_method < 0) || (nns_method > 3)) {
cerr << "Error: NNS Method not available." << endl; cerr << "Error: NNS Method not available." << endl;
exit(1); exit(1);
} }
break; break;
case 'L': case 'L':
loopSlam6DAlgo = atoi(optarg); loopSlam6DAlgo = atoi(optarg);
if (loopSlam6DAlgo < 0 || loopSlam6DAlgo > 6) { if (loopSlam6DAlgo < 0 || loopSlam6DAlgo > 6) {
cerr << "Error: global loop closing algorithm not available." << endl; cerr << "Error: global loop closing algorithm not available." << endl;
exit(1); exit(1);
} }
break; break;
case 'G': case 'G':
lum6DAlgo = atoi(optarg); lum6DAlgo = atoi(optarg);
if ((lum6DAlgo < 0) || (lum6DAlgo > 6)) { if ((lum6DAlgo < 0) || (lum6DAlgo > 6)) {
cerr << "Error: global relaxation algorithm not available." << endl; cerr << "Error: global relaxation algorithm not available." << endl;
exit(1); exit(1);
} }
break; break;
case 'c': case 'c':
cldist = atof(optarg); cldist = atof(optarg);
break; break;
case 'C': case 'C':
clpairs = atoi(optarg); clpairs = atoi(optarg);
break; break;
case 'l': case 'l':
loopsize = atoi(optarg); loopsize = atoi(optarg);
break; break;
case 'r': case 'r':
red = atof(optarg); red = atof(optarg);
break; break;
case 'O': case 'O':
if (optarg) { if (optarg) {
octree = atoi(optarg); octree = atoi(optarg);
} else { } else {
octree = 1; octree = 1;
} }
break; break;
case 'R': case 'R':
rand = atoi(optarg); rand = atoi(optarg);
break; break;
case 'd': case 'd':
mdm = atof(optarg); mdm = atof(optarg);
break; break;
case 'D': case 'D':
mdml = atof(optarg); mdml = atof(optarg);
break; break;
case 'i': case 'i':
mni = atoi(optarg); mni = atoi(optarg);
break; break;
case 'I': case 'I':
mni_lum = atoi(optarg); mni_lum = atoi(optarg);
break; break;
case 'n': case 'n':
net = optarg; net = optarg;
break; break;
case 's': case 's':
w_start = atoi(optarg); w_start = atoi(optarg);
if (start < 0) { cerr << "Error: Cannot start at a negative scan number.\n"; exit(1); } if (start < 0) { cerr << "Error: Cannot start at a negative scan number.\n"; exit(1); }
break; break;
case 'e': case 'e':
w_end = atoi(optarg); w_end = atoi(optarg);
if (end < 0) { cerr << "Error: Cannot end at a negative scan number.\n"; exit(1); } if (end < 0) { cerr << "Error: Cannot end at a negative scan number.\n"; exit(1); }
if (end < start) { cerr << "Error: <end> cannot be smaller than <start>.\n"; exit(1); } if (end < start) { cerr << "Error: <end> cannot be smaller than <start>.\n"; exit(1); }
break; break;
case 'm': case 'm':
maxDist = atoi(optarg); maxDist = atoi(optarg);
break; break;
case 'M': case 'M':
minDist = atoi(optarg); minDist = atoi(optarg);
break; break;
case 'q': case 'q':
quiet = true; quiet = true;
break; break;
case 'Q': case 'Q':
quiet = veryQuiet = true; quiet = veryQuiet = true;
break; break;
case 'p': case 'p':
extrapolate_pose = false; extrapolate_pose = false;
break; break;
case 'A': case 'A':
anim = atoi(optarg); anim = atoi(optarg);
break; break;
case '2': // = --metascan case '2': // = --metascan
meta = true; meta = true;
break; break;
case '4': // = --DlastSLAM case '4': // = --DlastSLAM
mdmll = atof(optarg); mdmll = atof(optarg);
break; break;
case '5': // = --epsICP case '5': // = --epsICP
epsilonICP = atof(optarg); epsilonICP = atof(optarg);
break; break;
case '6': // = --epsSLAM case '6': // = --epsSLAM
epsilonSLAM = atof(optarg); epsilonSLAM = atof(optarg);
break; break;
case '8': // not used case '8': // not used
exportPts = true; exportPts = true;
break; break;
case '9': // = --distLoop case '9': // = --distLoop
distLoop = atof(optarg); distLoop = atof(optarg);
break; break;
case '1': // = --iterLoop case '1': // = --iterLoop
iterLoop = atoi(optarg); iterLoop = atoi(optarg);
break; break;
case '3': // = --graphDist case '3': // = --graphDist
graphDist = atof(optarg); graphDist = atof(optarg);
break; break;
case 'f': case '7': // = --normalshoot
try { pairing_mode = CLOSEST_POINT_ALONG_NORMAL;
w_type = formatname_to_io_type(optarg); break;
} catch (...) { // runtime_error case 'z': // = --point-to-plane
cerr << "Format " << optarg << " unknown." << endl; pairing_mode = CLOSEST_PLANE;
abort(); break;
} case 'f':
break; try {
case 'u': w_type = formatname_to_io_type(optarg);
cuda_enabled = true; } catch (...) { // runtime_error
break; cerr << "Format " << optarg << " unknown." << endl;
case 'S': abort();
scanserver = true; }
break; break;
case '?': case 'u':
usage(argv[0]); cuda_enabled = true;
return 1; break;
default: case 'S':
abort (); scanserver = true;
break;
case '?':
usage(argv[0]);
return 1;
default:
abort ();
} }
} }
@ -541,10 +540,10 @@ int parseArgs(int argc, char **argv, string &dir, double &red, int &rand,
* @param mdmll maximal distance match for global SLAM after all scans ar matched * @param mdmll maximal distance match for global SLAM after all scans ar matched
*/ */
void matchGraph6Dautomatic(double cldist, int loopsize, vector <Scan *> allScans, icp6D *my_icp6D, void matchGraph6Dautomatic(double cldist, int loopsize, vector <Scan *> allScans, icp6D *my_icp6D,
bool meta_icp, int nns_method, bool cuda_enabled, bool meta_icp, int nns_method, bool cuda_enabled,
loopSlam6D *my_loopSlam6D, graphSlam6D *my_graphSlam6D, int nrIt, loopSlam6D *my_loopSlam6D, graphSlam6D *my_graphSlam6D, int nrIt,
double epsilonSLAM, double mdml, double mdmll, double graphDist, double epsilonSLAM, double mdml, double mdmll, double graphDist,
bool &eP, IOType type) bool &eP, IOType type)
{ {
double cldist2 = sqr(cldist); double cldist2 = sqr(cldist);
@ -588,18 +587,18 @@ void matchGraph6Dautomatic(double cldist, int loopsize, vector <Scan *> allScans
delete meta_scan; delete meta_scan;
} else { } else {
switch(type) { switch(type) {
case UOS_MAP: case UOS_MAP:
case UOS_MAP_FRAMES: case UOS_MAP_FRAMES:
my_icp6D->match(allScans[0], allScans[i]); my_icp6D->match(allScans[0], allScans[i]);
break; break;
case RTS_MAP: case RTS_MAP:
//untested (and could not work) //untested (and could not work)
//if(i < 220-22 && i > 250-22) match(allScans[0], CurrentScan); //if(i < 220-22 && i > 250-22) match(allScans[0], CurrentScan);
my_icp6D->match(allScans[0], allScans[i]); my_icp6D->match(allScans[0], allScans[i]);
break; break;
default: default:
my_icp6D->match(allScans[i - 1], allScans[i]); my_icp6D->match(allScans[i - 1], allScans[i]);
break; break;
} }
} }
} else { } else {
@ -701,7 +700,7 @@ int main(int argc, char **argv)
cout << "slam6D - A highly efficient SLAM implementation based on scan matching" << endl cout << "slam6D - A highly efficient SLAM implementation based on scan matching" << endl
<< " with 6 degrees of freedom" << endl << " with 6 degrees of freedom" << endl
<< "(c) Jacobs University Bremen gGmbH, Germany, since 2009" << endl << "(c) Jacobs University Bremen gGmbH, Germany, since 2009" << endl
<< " University of Osnabrueck, Germany, 2006 - 2009" << endl << endl; << " University of Osnabrueck, Germany, 2006 - 2009" << endl << endl;
if (argc <= 1) { if (argc <= 1) {
usage(argv[0]); usage(argv[0]);
@ -739,12 +738,13 @@ int main(int argc, char **argv)
bool cuda_enabled = false; bool cuda_enabled = false;
IOType type = UOS; IOType type = UOS;
bool scanserver = false; bool scanserver = false;
PairingMode pairing_mode = CLOSEST_POINT;
parseArgs(argc, argv, dir, red, rand, mdm, mdml, mdmll, mni, start, end, parseArgs(argc, argv, dir, red, rand, mdm, mdml, mdmll, mni, start, end,
maxDist, minDist, quiet, veryQuiet, eP, meta, algo, loopSlam6DAlgo, lum6DAlgo, anim, maxDist, minDist, quiet, veryQuiet, eP, meta, algo, loopSlam6DAlgo, lum6DAlgo, anim,
mni_lum, net, cldist, clpairs, loopsize, epsilonICP, epsilonSLAM, mni_lum, net, cldist, clpairs, loopsize, epsilonICP, epsilonSLAM,
nns_method, exportPts, distLoop, iterLoop, graphDist, octree, cuda_enabled, type, nns_method, exportPts, distLoop, iterLoop, graphDist, octree, cuda_enabled, type,
scanserver); scanserver, pairing_mode);
cout << "slam6D will proceed with the following parameters:" << endl; cout << "slam6D will proceed with the following parameters:" << endl;
//@@@ to do :-) //@@@ to do :-)
@ -760,39 +760,44 @@ int main(int argc, char **argv)
for(ScanVector::iterator it = Scan::allScans.begin(); it != Scan::allScans.end(); ++it) { for(ScanVector::iterator it = Scan::allScans.begin(); it != Scan::allScans.end(); ++it) {
Scan* scan = *it; Scan* scan = *it;
scan->setRangeFilter(maxDist, minDist); scan->setRangeFilter(maxDist, minDist);
scan->setReductionParameter(red, octree); unsigned int types;
if ((pairing_mode == CLOSEST_POINT_ALONG_NORMAL) ||
(pairing_mode == CLOSEST_PLANE)) {
types = PointType::USE_NORMAL;
}
scan->setReductionParameter(red, octree, PointType(types));
scan->setSearchTreeParameter(nns_method, cuda_enabled); scan->setSearchTreeParameter(nns_method, cuda_enabled);
} }
icp6Dminimizer *my_icp6Dminimizer = 0; icp6Dminimizer *my_icp6Dminimizer = 0;
switch (algo) { switch (algo) {
case 1 : case 1 :
my_icp6Dminimizer = new icp6D_QUAT(quiet); my_icp6Dminimizer = new icp6D_QUAT(quiet);
break; break;
case 2 : case 2 :
my_icp6Dminimizer = new icp6D_SVD(quiet); my_icp6Dminimizer = new icp6D_SVD(quiet);
break; break;
case 3 : case 3 :
my_icp6Dminimizer = new icp6D_ORTHO(quiet); my_icp6Dminimizer = new icp6D_ORTHO(quiet);
break; break;
case 4 : case 4 :
my_icp6Dminimizer = new icp6D_DUAL(quiet); my_icp6Dminimizer = new icp6D_DUAL(quiet);
break; break;
case 5 : case 5 :
my_icp6Dminimizer = new icp6D_HELIX(quiet); my_icp6Dminimizer = new icp6D_HELIX(quiet);
break; break;
case 6 : case 6 :
my_icp6Dminimizer = new icp6D_APX(quiet); my_icp6Dminimizer = new icp6D_APX(quiet);
break; break;
case 7 : case 7 :
my_icp6Dminimizer = new icp6D_LUMEULER(quiet); my_icp6Dminimizer = new icp6D_LUMEULER(quiet);
break; break;
case 8 : case 8 :
my_icp6Dminimizer = new icp6D_LUMQUAT(quiet); my_icp6Dminimizer = new icp6D_LUMQUAT(quiet);
break; break;
case 9 : case 9 :
my_icp6Dminimizer = new icp6D_QUAT_SCALE(quiet); my_icp6Dminimizer = new icp6D_QUAT_SCALE(quiet);
break; break;
} }
// match the scans and print the time used // match the scans and print the time used
@ -807,13 +812,13 @@ int main(int argc, char **argv)
if (cuda_enabled) { if (cuda_enabled) {
#ifdef WITH_CUDA #ifdef WITH_CUDA
my_icp = new icp6Dcuda(my_icp6Dminimizer, mdm, mni, quiet, meta, rand, eP, my_icp = new icp6Dcuda(my_icp6Dminimizer, mdm, mni, quiet, meta, rand, eP,
anim, epsilonICP, nns_method, cuda_enabled); anim, epsilonICP, nns_method, cuda_enabled);
#else #else
cout << "slam6d was not compiled for excuting CUDA code" << endl; cout << "slam6d was not compiled for excuting CUDA code" << endl;
#endif #endif
} else { } else {
my_icp = new icp6D(my_icp6Dminimizer, mdm, mni, quiet, meta, rand, eP, my_icp = new icp6D(my_icp6Dminimizer, mdm, mni, quiet, meta, rand, eP,
anim, epsilonICP, nns_method, cuda_enabled); anim, epsilonICP, nns_method, cuda_enabled);
} }
// check if CAD matching was selected as type // check if CAD matching was selected as type
@ -822,7 +827,7 @@ int main(int argc, char **argv)
my_icp->set_cad_matching (true); my_icp->set_cad_matching (true);
} }
if (my_icp) my_icp->doICP(Scan::allScans); if (my_icp) my_icp->doICP(Scan::allScans, pairing_mode);
delete my_icp; delete my_icp;
} else if (clpairs > -1) { } else if (clpairs > -1) {
//!!!!!!!!!!!!!!!!!!!!!!!! //!!!!!!!!!!!!!!!!!!!!!!!!
@ -830,46 +835,38 @@ int main(int argc, char **argv)
if (cuda_enabled) { if (cuda_enabled) {
#ifdef WITH_CUDA #ifdef WITH_CUDA
my_icp = new icp6Dcuda(my_icp6Dminimizer, mdm, mni, quiet, meta, rand, eP, my_icp = new icp6Dcuda(my_icp6Dminimizer, mdm, mni, quiet, meta, rand, eP,
anim, epsilonICP, nns_method, cuda_enabled); anim, epsilonICP, nns_method, cuda_enabled);
#else #else
cout << "slam6d was not compiled for excuting CUDA code" << endl; cout << "slam6d was not compiled for excuting CUDA code" << endl;
#endif #endif
} else { } else {
my_icp = new icp6D(my_icp6Dminimizer, mdm, mni, quiet, meta, rand, eP, my_icp = new icp6D(my_icp6Dminimizer, mdm, mni, quiet, meta, rand, eP,
anim, epsilonICP, nns_method, cuda_enabled); anim, epsilonICP, nns_method, cuda_enabled);
} }
my_icp->doICP(Scan::allScans); my_icp->doICP(Scan::allScans, pairing_mode);
graphSlam6D *my_graphSlam6D = new lum6DEuler(my_icp6Dminimizer, mdm, mdml, mni, quiet, meta, graphSlam6D *my_graphSlam6D = new lum6DEuler(my_icp6Dminimizer, mdm, mdml, mni, quiet, meta,
rand, eP, anim, epsilonICP, nns_method, epsilonSLAM); rand, eP, anim, epsilonICP, nns_method, epsilonSLAM);
my_graphSlam6D->matchGraph6Dautomatic(Scan::allScans, mni_lum, clpairs, loopsize); my_graphSlam6D->matchGraph6Dautomatic(Scan::allScans, mni_lum, clpairs, loopsize);
//!!!!!!!!!!!!!!!!!!!!!!!! //!!!!!!!!!!!!!!!!!!!!!!!!
} else { } else {
graphSlam6D *my_graphSlam6D = 0; graphSlam6D *my_graphSlam6D = 0;
switch (lum6DAlgo) { switch (lum6DAlgo) {
case 1 : case 1 :
my_graphSlam6D = new lum6DEuler(my_icp6Dminimizer, mdm, mdml, mni, quiet, meta, rand, eP, my_graphSlam6D = new lum6DEuler(my_icp6Dminimizer, mdm, mdml, mni, quiet, meta, rand, eP,
anim, epsilonICP, nns_method, epsilonSLAM); anim, epsilonICP, nns_method, epsilonSLAM);
break; break;
case 2 : case 2 :
my_graphSlam6D = new lum6DQuat(my_icp6Dminimizer, mdm, mdml, mni, quiet, meta, rand, eP, my_graphSlam6D = new lum6DQuat(my_icp6Dminimizer, mdm, mdml, mni, quiet, meta, rand, eP,
anim, epsilonICP, nns_method, epsilonSLAM); anim, epsilonICP, nns_method, epsilonSLAM);
break; break;
case 3 : case 3 :
my_graphSlam6D = new ghelix6DQ2(my_icp6Dminimizer, mdm, mdml, mni, quiet, meta, rand, eP, my_graphSlam6D = new ghelix6DQ2(my_icp6Dminimizer, mdm, mdml, mni, quiet, meta, rand, eP,
anim, epsilonICP, nns_method, epsilonSLAM); anim, epsilonICP, nns_method, epsilonSLAM);
break; break;
case 4 : case 4 :
my_graphSlam6D = new gapx6D(my_icp6Dminimizer, mdm, mdml, mni, quiet, meta, rand, eP, my_graphSlam6D = new gapx6D(my_icp6Dminimizer, mdm, mdml, mni, quiet, meta, rand, eP,
anim, epsilonICP, nns_method, epsilonSLAM); anim, epsilonICP, nns_method, epsilonSLAM);
break; break;
case 5 :
my_graphSlam6D = new graphToro(my_icp6Dminimizer, mdm, mdml, mni, quiet, meta, rand, eP,
-2, epsilonICP, nns_method, epsilonSLAM);
break;
case 6 :
my_graphSlam6D = new graphHOGMan(my_icp6Dminimizer, mdm, mdml, mni, quiet, meta, rand, eP,
-2, epsilonICP, nns_method, epsilonSLAM);
break;
} }
// Construct Network // Construct Network
if (net != "none") { if (net != "none") {
@ -877,15 +874,15 @@ int main(int argc, char **argv)
if (cuda_enabled) { if (cuda_enabled) {
#ifdef WITH_CUDA #ifdef WITH_CUDA
my_icp = new icp6Dcuda(my_icp6Dminimizer, mdm, mni, quiet, meta, rand, eP, my_icp = new icp6Dcuda(my_icp6Dminimizer, mdm, mni, quiet, meta, rand, eP,
anim, epsilonICP, nns_method); anim, epsilonICP, nns_method);
#else #else
cout << "slam6d was not compiled for excuting CUDA code" << endl; cout << "slam6d was not compiled for excuting CUDA code" << endl;
#endif #endif
} else { } else {
my_icp = new icp6D(my_icp6Dminimizer, mdm, mni, quiet, meta, rand, eP, my_icp = new icp6D(my_icp6Dminimizer, mdm, mni, quiet, meta, rand, eP,
anim, epsilonICP, nns_method); anim, epsilonICP, nns_method);
} }
my_icp->doICP(Scan::allScans); my_icp->doICP(Scan::allScans, pairing_mode);
Graph* structure; Graph* structure;
structure = new Graph(net); structure = new Graph(net);
@ -901,46 +898,38 @@ int main(int argc, char **argv)
if (cuda_enabled) { if (cuda_enabled) {
#ifdef WITH_CUDA #ifdef WITH_CUDA
my_icp = new icp6Dcuda(my_icp6Dminimizer, mdm, mni, quiet, meta, rand, eP, my_icp = new icp6Dcuda(my_icp6Dminimizer, mdm, mni, quiet, meta, rand, eP,
anim, epsilonICP, nns_method); anim, epsilonICP, nns_method);
#else #else
cout << "slam6d was not compiled for excuting CUDA code" << endl; cout << "slam6d was not compiled for excuting CUDA code" << endl;
#endif #endif
} else { } else {
my_icp = new icp6D(my_icp6Dminimizer, mdm, mni, quiet, meta, rand, eP, my_icp = new icp6D(my_icp6Dminimizer, mdm, mni, quiet, meta, rand, eP,
anim, epsilonICP, nns_method); anim, epsilonICP, nns_method);
} }
loopSlam6D *my_loopSlam6D = 0; loopSlam6D *my_loopSlam6D = 0;
switch(loopSlam6DAlgo) { switch(loopSlam6DAlgo) {
case 1: case 1:
my_loopSlam6D = new elch6Deuler(veryQuiet, my_icp6Dminimizer, distLoop, iterLoop, my_loopSlam6D = new elch6Deuler(veryQuiet, my_icp6Dminimizer, distLoop, iterLoop,
rand, eP, 10, epsilonICP, nns_method); rand, eP, 10, epsilonICP, nns_method);
break; break;
case 2: case 2:
my_loopSlam6D = new elch6Dquat(veryQuiet, my_icp6Dminimizer, distLoop, iterLoop, my_loopSlam6D = new elch6Dquat(veryQuiet, my_icp6Dminimizer, distLoop, iterLoop,
rand, eP, 10, epsilonICP, nns_method); rand, eP, 10, epsilonICP, nns_method);
break; break;
case 3: case 3:
my_loopSlam6D = new elch6DunitQuat(veryQuiet, my_icp6Dminimizer, distLoop, iterLoop, my_loopSlam6D = new elch6DunitQuat(veryQuiet, my_icp6Dminimizer, distLoop, iterLoop,
rand, eP, 10, epsilonICP, nns_method); rand, eP, 10, epsilonICP, nns_method);
break; break;
case 4: case 4:
my_loopSlam6D = new elch6Dslerp(veryQuiet, my_icp6Dminimizer, distLoop, iterLoop, my_loopSlam6D = new elch6Dslerp(veryQuiet, my_icp6Dminimizer, distLoop, iterLoop,
rand, eP, 10, epsilonICP, nns_method); rand, eP, 10, epsilonICP, nns_method);
break; break;
case 5:
my_loopSlam6D = new loopToro(veryQuiet, my_icp6Dminimizer, distLoop, iterLoop,
rand, eP, 10, epsilonICP, nns_method);
break;
case 6:
my_loopSlam6D = new loopHOGMan(veryQuiet, my_icp6Dminimizer, distLoop, iterLoop,
rand, eP, 10, epsilonICP, nns_method);
break;
} }
matchGraph6Dautomatic(cldist, loopsize, Scan::allScans, my_icp, meta, matchGraph6Dautomatic(cldist, loopsize, Scan::allScans, my_icp, meta,
nns_method, cuda_enabled, my_loopSlam6D, my_graphSlam6D, nns_method, cuda_enabled, my_loopSlam6D, my_graphSlam6D,
mni_lum, epsilonSLAM, mdml, mdmll, graphDist, eP, type); mni_lum, epsilonSLAM, mdml, mdmll, graphDist, eP, type);
delete my_icp; delete my_icp;
if(loopSlam6DAlgo > 0) { if(loopSlam6DAlgo > 0) {
delete my_loopSlam6D; delete my_loopSlam6D;
@ -964,11 +953,16 @@ int main(int argc, char **argv)
ofstream redptsout("points.pts"); ofstream redptsout("points.pts");
for(unsigned int i = 0; i < Scan::allScans.size(); i++) { for(unsigned int i = 0; i < Scan::allScans.size(); i++) {
DataXYZ xyz_r(Scan::allScans[i]->get("xyz reduced")); DataXYZ xyz_r(Scan::allScans[i]->get("xyz reduced"));
DataNormal normal_r(Scan::allScans[i]->get("normal reduced"));
for(unsigned int i = 0; i < xyz_r.size(); ++i) { for(unsigned int i = 0; i < xyz_r.size(); ++i) {
redptsout << xyz_r[i][0] << ' ' << xyz_r[i][1] << ' ' << xyz_r[i][2] << '\n'; int r,g,b;
r = (int)(normal_r[i][0] * (127.5) + 127.5);
g = (int)(normal_r[i][1] * (127.5) + 127.5);
b = (int)(fabs(normal_r[i][2]) * (255.0));
redptsout << xyz_r[i][0] << ' ' << xyz_r[i][1] << ' ' << xyz_r[i][2]
<< ' ' << r << ' ' << g << ' ' << b << '\n';
} }
redptsout << std::flush; redptsout << std::flush;
} }
redptsout.close(); redptsout.close();
redptsout.clear(); redptsout.clear();
@ -979,8 +973,8 @@ int main(int argc, char **argv)
for(ScanVector::iterator it = Scan::allScans.begin(); it != Scan::allScans.end(); ++it) for(ScanVector::iterator it = Scan::allScans.begin(); it != Scan::allScans.end(); ++it)
{ {
Scan* scan = *it; Scan* scan = *it;
p = scan->get_rPos(); p = scan->get_rPos();
Point x(p[0], p[1], p[2]); Point x(p[0], p[1], p[2]);
redptsout << x << endl; redptsout << x << endl;
scan->saveFrames(); scan->saveFrames();
} }
@ -991,9 +985,9 @@ int main(int argc, char **argv)
cout << endl << endl; cout << endl << endl;
cout << "Normal program end." << endl cout << "Normal program end." << endl
<< (red < 0 && rand < 0 ? "(-> HINT: For a significant speedup, please use the '-r' or '-R' parameter <-)\n" << (red < 0 && rand < 0 ? "(-> HINT: For a significant speedup, please use the '-r' or '-R' parameter <-)\n"
: "") : "")
<< endl; << endl;
// print metric information // print metric information
#ifdef WITH_METRICS #ifdef WITH_METRICS

View file

@ -48,14 +48,10 @@
#include "slam6d/lum6Deuler.h" #include "slam6d/lum6Deuler.h"
#include "slam6d/lum6Dquat.h" #include "slam6d/lum6Dquat.h"
#include "slam6d/ghelix6DQ2.h" #include "slam6d/ghelix6DQ2.h"
#include "slam6d/graphToro.h"
#include "slam6d/graphHOG-Man.h"
#include "slam6d/elch6Deuler.h" #include "slam6d/elch6Deuler.h"
#include "slam6d/elch6Dquat.h" #include "slam6d/elch6Dquat.h"
#include "slam6d/elch6DunitQuat.h" #include "slam6d/elch6DunitQuat.h"
#include "slam6d/elch6Dslerp.h" #include "slam6d/elch6Dslerp.h"
#include "slam6d/loopToro.h"
#include "slam6d/loopHOG-Man.h"
#include "slam6d/graphSlam6D.h" #include "slam6d/graphSlam6D.h"
#include "slam6d/gapx6D.h" #include "slam6d/gapx6D.h"
#include "slam6d/graph.h" #include "slam6d/graph.h"
@ -242,8 +238,6 @@ void usage(char* prog)
<< " 2 = Lu & Milios extension using using unit quaternions" << endl << " 2 = Lu & Milios extension using using unit quaternions" << endl
<< " 3 = HELIX approximation by Hofer and Pottmann" << endl << " 3 = HELIX approximation by Hofer and Pottmann" << endl
<< " 4 = small angle approximation" << endl << " 4 = small angle approximation" << endl
<< " 5 = TORO" << endl
<< " 6 = HOG-Man" << endl
<< endl << endl
<< bold << " -i" << normal << " NR, " << bold << "--iter=" << normal << "NR [default: 50]" << endl << bold << " -i" << normal << " NR, " << bold << "--iter=" << normal << "NR [default: 50]" << endl
<< " sets the maximal number of ICP iterations to <NR>" << endl << " sets the maximal number of ICP iterations to <NR>" << endl
@ -262,8 +256,6 @@ void usage(char* prog)
<< " 2 = quaternions " << endl << " 2 = quaternions " << endl
<< " 3 = unit quaternions" << endl << " 3 = unit quaternions" << endl
<< " 4 = SLERP (recommended)" << endl << " 4 = SLERP (recommended)" << endl
<< " 5 = TORO" << endl
<< " 6 = HOG-Man" << endl
<< endl << endl
<< bold << " --metascan" << normal << endl << bold << " --metascan" << normal << endl
<< " Match current scan against a meta scan of all previous scans (default match against the last scan only)" << endl << " Match current scan against a meta scan of all previous scans (default match against the last scan only)" << endl
@ -919,14 +911,6 @@ int FinalSLAM( double &red, int &rand,
my_graphSlam6D = new gapx6D(my_icp6Dminimizer, mdm, mdml, mni, quiet, meta, rand, eP, my_graphSlam6D = new gapx6D(my_icp6Dminimizer, mdm, mdml, mni, quiet, meta, rand, eP,
anim, epsilonICP, nns_method, epsilonSLAM); anim, epsilonICP, nns_method, epsilonSLAM);
break; break;
case 5 :
my_graphSlam6D = new graphToro(my_icp6Dminimizer, mdm, mdml, mni, quiet, meta, rand, eP,
-2, epsilonICP, nns_method, epsilonSLAM);
break;
case 6 :
my_graphSlam6D = new graphHOGMan(my_icp6Dminimizer, mdm, mdml, mni, quiet, meta, rand, eP,
-2, epsilonICP, nns_method, epsilonSLAM);
break;
} }
// Construct Network // Construct Network
if (net != "none") { if (net != "none") {
@ -985,14 +969,6 @@ int FinalSLAM( double &red, int &rand,
my_loopSlam6D = new elch6Dslerp(veryQuiet, my_icp6Dminimizer, distLoop, iterLoop, my_loopSlam6D = new elch6Dslerp(veryQuiet, my_icp6Dminimizer, distLoop, iterLoop,
rand, eP, 10, epsilonICP, nns_method); rand, eP, 10, epsilonICP, nns_method);
break; break;
case 5:
my_loopSlam6D = new loopToro(veryQuiet, my_icp6Dminimizer, distLoop, iterLoop,
rand, eP, 10, epsilonICP, nns_method);
break;
case 6:
my_loopSlam6D = new loopHOGMan(veryQuiet, my_icp6Dminimizer, distLoop, iterLoop,
rand, eP, 10, epsilonICP, nns_method);
break;
} }
matchGraph6Dautomatic(cldist, loopsize, Scan::allScans, my_icp, meta, matchGraph6Dautomatic(cldist, loopsize, Scan::allScans, my_icp, meta,