413 lines
12 KiB
Text
413 lines
12 KiB
Text
#ifndef SCAN_H
|
|
#define SCAN_H
|
|
|
|
#include "io_types.h"
|
|
#include "data_types.h"
|
|
#include "point_type.h"
|
|
#include "ptpair.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, LOOPTORO, LOOPHOGMAN, GRAPHTORO,
|
|
GRAPHHOGMAN
|
|
};
|
|
|
|
// 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;
|
|
|
|
//! 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);
|
|
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]);
|
|
|
|
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;
|
|
|
|
//! 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 caldReducedOnDemandPrivate.
|
|
*
|
|
* The intention is to reduce points, transforme them to the initial pose and
|
|
* then copy them to original for the SearchTree.
|
|
*/
|
|
void calcReducedOnDemand();
|
|
|
|
//! 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;
|
|
|
|
//! 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]);
|
|
|
|
//! Creating reduced points
|
|
void calcReducedPoints();
|
|
|
|
//! 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;
|
|
|
|
//! Mutex for safely reducing points and creating the search tree just once in a multithreaded environment
|
|
// it can not be compiled in wein32 use boost 1.48, therefore we remeove it temporarily
|
|
// boost::mutex m_mutex_reduction, m_mutex_create_tree;
|
|
};
|
|
|
|
#include "scan.icc"
|
|
|
|
#endif //SCAN_H
|