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

870 lines
32 KiB
Text
Raw Normal View History

initial commit
2012-09-16 12:33:11 +00:00
/** @file
* @brief GPU-ICP Algorithm
* @author Deyuan Qiu, University of Applied Sciences Bonn-Rhein-Sieg, Sankt Augustin, Germany.
* Fraunhofer IAIS, Sankt Augustin, Germany.
*/
#include "slam6d/cuda/CIcpGpuCuda_kernel.cuh"
#include "slam6d/cuda/CIcpGpuCuda.cuh"
#include "slam6d/cuda/CSystem.h"
void CIcpGpuCuda::init(unsigned unWidth, unsigned unHeight, unsigned max_iter)
{
// Initialize CUTIL
int d;
cudaSetDevice(0); // Since we have only one GPU I didn't initialize anything
// It may cause problems if more GPUs take into account
// The reason of doing that was initialization of it several times
// Now it is not initialized several times and just once.
unMaxIteration = max_iter;
matrices = (Matrix**)malloc(sizeof(Matrix*) * max_iter);
for(int i = 0 ; i < max_iter ; ++i){
matrices[i] = new Matrix(4,4);
Matrix* m = matrices[i];
(*m)(1,1) = 1;(*m)(1,2) = 0;(*m)(1,3) = 0;(*m)(1,4) = 0;
(*m)(2,1) = 0;(*m)(2,2) = 1;(*m)(2,3) = 0;(*m)(2,4) = 0;
(*m)(3,1) = 0;(*m)(3,2) = 0;(*m)(3,3) = 1;(*m)(3,4) = 0;
(*m)(4,1) = 0;(*m)(4,2) = 0;(*m)(4,3) = 0;(*m)(4,4) = 1;
}
// set data size
setResolution(unWidth, unHeight);
// cout<<"unSizeData: "<<unSizeData<<endl;
// cout<<"Tree Size :" <<TREESIZE<<endl;
// Initialize CUBLAS
cublasStatus statusCUBLAS = cublasInit();
if (statusCUBLAS != CUBLAS_STATUS_SUCCESS) {
cout<<"The error status is \n";
cout<<statusCUBLAS<<endl;
fprintf (stderr, "!!!! CUBLAS initialization error\n");
exit(1);
}
// Initialize CUDPP
CUDPPConfiguration config;
config.datatype = CUDPP_FLOAT;
config.algorithm = CUDPP_COMPACT;
config.options = CUDPP_OPTION_FORWARD;
result = cudppPlan(&compactplan, config, unSizeData, 1, 0);
if (CUDPP_SUCCESS != result) printf("Error creating CUDPPPlan\n");
_unSizeTree = (unsigned)TREESIZE;
CUDA_SAFE_CALL(cudaMallocHost((void**)&fSplit, _unSizeTree*sizeof(float)));
CUDA_SAFE_CALL(cudaMallocHost((void**)&unIdx, _unSizeTree*sizeof(unsigned)));
CUDA_SAFE_CALL(cudaMallocHost((void**)&unAxis, _unSizeTree*sizeof(unsigned)));
CUDA_SAFE_CALL(cudaMallocHost((void**)&bIsLeaf, _unSizeTree*sizeof(bool)));
CUDA_SAFE_CALL(cudaMallocHost((void**)&fLoBound, _unSizeTree*sizeof(float)));
CUDA_SAFE_CALL(cudaMallocHost((void**)&fHiBound, _unSizeTree*sizeof(float)));
CUDA_SAFE_CALL(cudaMallocHost((void**)&f4Mdl, unSizeData*sizeof(float4))); // to be downloaded to texture
// Host memory allocation
CUDA_SAFE_CALL(cudaMallocHost((void**)&fHstScnX, unSizeData*sizeof(float)));
CUDA_SAFE_CALL(cudaMallocHost((void**)&fHstScnY, unSizeData *sizeof(float)));
CUDA_SAFE_CALL(cudaMallocHost((void**)&fHstScnZ, unSizeData*sizeof(float))); //scene
fHstScn[0]=fHstScnX;
fHstScn[1]=fHstScnY;
fHstScn[2]=fHstScnZ;
CUDA_SAFE_CALL(cudaMallocHost((void**)&pNoPairs, sizeof(unsigned)));
CSystem<double>::allocate(unSizeData, 3, h_idata); //model
// Device memory allocation
CUDA_SAFE_CALL(cudaMalloc((void**)&fDist, unSizeData*sizeof(float)));
CUDA_SAFE_CALL(cudaMalloc((void**)&fDistCpt, unSizeData*sizeof(float)));
CUDA_SAFE_CALL(cudaMalloc((void**)&unMask, unSizeData*sizeof(unsigned)));
CUDA_SAFE_CALL(cudaMalloc((void**)&fDevMdlPairX, unSizeData*sizeof(float))); //pairs after shrinking
CUDA_SAFE_CALL(cudaMalloc((void**)&fDevMdlPairY, unSizeData*sizeof(float)));
CUDA_SAFE_CALL(cudaMalloc((void**)&fDevMdlPairZ, unSizeData*sizeof(float)));
CUDA_SAFE_CALL(cudaMalloc((void**)&fDevScnPairX, unSizeData*sizeof(float)));
CUDA_SAFE_CALL(cudaMalloc((void**)&fDevScnPairY, unSizeData*sizeof(float)));
CUDA_SAFE_CALL(cudaMalloc((void**)&fDevScnPairZ, unSizeData*sizeof(float)));
/////////////// Added by Shams
CUDA_SAFE_CALL(cudaMalloc((void**)&cngfDevScnX,unSizeData*sizeof(float)));
CUDA_SAFE_CALL(cudaMalloc((void**)&cngfDevScnY,unSizeData*sizeof(float)));
CUDA_SAFE_CALL(cudaMalloc((void**)&cngfDevScnZ,unSizeData*sizeof(float)));
CUDA_SAFE_CALL(cudaMalloc((void**)&cngfDevMdlPairX, unSizeData*sizeof(float))); //pairs after shrinking
CUDA_SAFE_CALL(cudaMalloc((void**)&cngfDevMdlPairY, unSizeData*sizeof(float)));
CUDA_SAFE_CALL(cudaMalloc((void**)&cngfDevMdlPairZ, unSizeData*sizeof(float)));
CUDA_SAFE_CALL(cudaMalloc((void**)&cngfDevScnPairX, unSizeData*sizeof(float)));
CUDA_SAFE_CALL(cudaMalloc((void**)&cngfDevScnPairY, unSizeData*sizeof(float)));
CUDA_SAFE_CALL(cudaMalloc((void**)&cngfDevScnPairZ, unSizeData*sizeof(float)));
///////////////
CUDA_SAFE_CALL(cudaMalloc((void**)&fCenModX, unSizeData*sizeof(float)));
CUDA_SAFE_CALL(cudaMalloc((void**)&fCenModY, unSizeData*sizeof(float)));
CUDA_SAFE_CALL(cudaMalloc((void**)&fCenModZ, unSizeData*sizeof(float)));
CUDA_SAFE_CALL(cudaMalloc((void**)&fCenScnX, unSizeData*sizeof(float)));
CUDA_SAFE_CALL(cudaMalloc((void**)&fCenScnY, unSizeData*sizeof(float)));
CUDA_SAFE_CALL(cudaMalloc((void**)&fCenScnZ, unSizeData*sizeof(float)));
CUDA_SAFE_CALL(cudaMalloc((void**)&unNoPairs, sizeof(unsigned)));
CUDA_SAFE_CALL(cudaMalloc((void**)&fDevScnX, unSizeData*sizeof(float)));
CUDA_SAFE_CALL(cudaMalloc((void**)&fDevScnY, unSizeData*sizeof(float)));
CUDA_SAFE_CALL(cudaMalloc((void**)&fDevScnZ, unSizeData*sizeof(float)));
CUDA_SAFE_CALL(cudaMalloc((void**)&fDevSplit, _unSizeTree*sizeof(float)));
CUDA_SAFE_CALL(cudaMalloc((void**)&unDevIdx, _unSizeTree*sizeof(unsigned)));
CUDA_SAFE_CALL(cudaMalloc((void**)&unDevAxis, _unSizeTree*sizeof(unsigned)));
CUDA_SAFE_CALL(cudaMalloc((void**)&bDevIsLeaf, _unSizeTree*sizeof(bool)));
CUDA_SAFE_CALL(cudaMalloc((void**)&fDevLoBound, _unSizeTree*sizeof(float)));
CUDA_SAFE_CALL(cudaMalloc((void**)&fDevHiBound, _unSizeTree*sizeof(float)));
CUDA_SAFE_CALL(cudaMallocArray(&cuArray, &cuDesc, _unWidth, _unHeight)); //to be bound to texture
// Initialize states
fMaxProcTime = 0.0f;
fMaxDeviation = 0.0f;
_fSearchRadiusMax = 0.0f;
_fSearchRadiusMin = 0.0f;
_fRadiusStep = 0.0f;
_unNoQSizeStep = 0;
_dElapsedTime = 0.0;
/*
* Array of ones to be used instead of abs sum
*/
cudaMallocHost((void**)&temp_ones, unSizeData*sizeof(float));
cudaMalloc((void**)&ones, unSizeData*sizeof(float)); // Array of ones
for(int i = 0; i < unSizeData ; ++i)temp_ones[i] = 1.0f;
cudaMemcpy(ones, temp_ones, unSizeData*sizeof(float), cudaMemcpyHostToDevice);
}
CIcpGpuCuda::~CIcpGpuCuda(){
/////////////
// tidy up
/////////////
CUDA_SAFE_CALL(cudaUnbindTexture(refTex));
CUDA_SAFE_CALL(cudaFreeArray(cuArray));
CUDA_SAFE_CALL(cudaFree(fDevSplit));
CUDA_SAFE_CALL(cudaFree(unDevIdx));
CUDA_SAFE_CALL(cudaFree(unDevAxis));
CUDA_SAFE_CALL(cudaFree(bDevIsLeaf));
CUDA_SAFE_CALL(cudaFree(fDevLoBound));
CUDA_SAFE_CALL(cudaFree(fDevHiBound));
CUDA_SAFE_CALL(cudaFree(fDevScnX));
CUDA_SAFE_CALL(cudaFree(fDevScnY));
CUDA_SAFE_CALL(cudaFree(fDevScnZ));
CUDA_SAFE_CALL(cudaFree(fDist));
CUDA_SAFE_CALL(cudaFree(fDistCpt));
CUDA_SAFE_CALL(cudaFree(fCenModX));
CUDA_SAFE_CALL(cudaFree(fCenModY));
CUDA_SAFE_CALL(cudaFree(fCenModZ));
CUDA_SAFE_CALL(cudaFree(fCenScnX));
CUDA_SAFE_CALL(cudaFree(fCenScnY));
CUDA_SAFE_CALL(cudaFree(fCenScnZ));
CUDA_SAFE_CALL(cudaFree(unMask));
CUDA_SAFE_CALL(cudaFree(fDevMdlPairX));
CUDA_SAFE_CALL(cudaFree(fDevMdlPairY));
CUDA_SAFE_CALL(cudaFree(fDevMdlPairZ));
CUDA_SAFE_CALL(cudaFree(fDevScnPairX));
CUDA_SAFE_CALL(cudaFree(fDevScnPairY));
CUDA_SAFE_CALL(cudaFree(fDevScnPairZ));
CUDA_SAFE_CALL(cudaFree(cngfDevScnX));
CUDA_SAFE_CALL(cudaFree(cngfDevScnY));
CUDA_SAFE_CALL(cudaFree(cngfDevScnZ));
CUDA_SAFE_CALL(cudaFree(cngfDevMdlPairX));
CUDA_SAFE_CALL(cudaFree(cngfDevMdlPairY));
CUDA_SAFE_CALL(cudaFree(cngfDevMdlPairZ));
CUDA_SAFE_CALL(cudaFree(cngfDevScnPairX));
CUDA_SAFE_CALL(cudaFree(cngfDevScnPairY));
CUDA_SAFE_CALL(cudaFree(cngfDevScnPairZ));
CUDA_SAFE_CALL(cudaFree(unNoPairs));
CUDA_SAFE_CALL(cudaFree(ones));
CUDA_SAFE_CALL(cudaFreeHost(fSplit));
CUDA_SAFE_CALL(cudaFreeHost(unIdx));
CUDA_SAFE_CALL(cudaFreeHost(unAxis));
CUDA_SAFE_CALL(cudaFreeHost(bIsLeaf));
CUDA_SAFE_CALL(cudaFreeHost(fHstScnX));
CUDA_SAFE_CALL(cudaFreeHost(fHstScnY));
CUDA_SAFE_CALL(cudaFreeHost(fHstScnZ));
CUDA_SAFE_CALL(cudaFreeHost(fLoBound));
CUDA_SAFE_CALL(cudaFreeHost(fHiBound));
CUDA_SAFE_CALL(cudaFreeHost(pNoPairs));
CUDA_SAFE_CALL(cudaFreeHost(f4Mdl));
CUDA_SAFE_CALL(cudaFreeHost(temp_ones));
free(h_idata);
/////////
// Exit
/////////
bool bShutDownSuccess = true;
// Done with CUDPP
result = cudppDestroyPlan(compactplan);
if (CUDPP_SUCCESS != result){
printf("Error destroying CUDPPPlan\n");
bShutDownSuccess = false;
}
// Done with CUBLAS
cublasStatus statusCUBLAS = cublasShutdown();
if (statusCUBLAS != CUBLAS_STATUS_SUCCESS) {
fprintf (stderr, "!!!! shutdown error (A)\n");
bShutDownSuccess = false;
}
// Done with ANN
annClose();
}
void CIcpGpuCuda::setResolution(unsigned unWidth, unsigned unHeight){
if (unWidth>0 && unHeight>0) {
_unWidth = unWidth;
_unHeight = unHeight;
unSizeData = _unWidth*_unHeight;
//configure block and grid size
unsigned unBlockSize = (unsigned)BLOCKSIZE; //192
if(unSizeData<=64){
unNoThreads = 64;
unNoBlocks = 1;
}
else if(unSizeData<=128){
unNoThreads = 128;
unNoBlocks = 1;
}
else if(unSizeData<=unBlockSize){
unNoThreads = unBlockSize;
unNoBlocks = 1;
}
else{
unNoThreads = unBlockSize;
if(unSizeData%unBlockSize) unNoBlocks = unSizeData/unBlockSize + 1;
else unNoBlocks = unSizeData/unBlockSize;
}
cout<<"unNoThreads: "<<unNoThreads<<'\t'<<"unNoBlocks: "<<unNoBlocks<<endl;
dimBlock.x=unNoThreads;
dimBlock.y=1;
dimBlock.z=1;
dimGrid.x=unNoBlocks;
dimGrid.y=1;
dimGrid.z=1;
}
else{
cout<<"Each aspect of resolution must be bigger than 0."<<endl;
exit(1);
}
}
void CIcpGpuCuda::setMaxIteration(unsigned unTimes){
if(unTimes>0) unMaxIteration = unTimes;
else{
cout<<"Error setting maximum iterations."<<endl;
exit(1);
}
}
void CIcpGpuCuda::setMaxProcTime(double dMilliseconds){
if(dMilliseconds>0) fMaxProcTime = dMilliseconds;
else{
cout<<"Error setting maximum processing time."<<endl;
exit(1);
}
}
void CIcpGpuCuda::setMaxDeviation(double fDeviation){
if(fDeviation>0) fMaxDeviation = fDeviation;
else{
cout<<"Error setting maximum deviation."<<endl;
exit(1);
}
}
void CIcpGpuCuda::setSize(unsigned int width, unsigned int height){
//The memory allocation is once done for all scans
//we require to update the sizes for each scan matching process
//The size which is set in constructor is the maximum possible,
//and this size is associated to each pair.
setResolution(width, height);
cout<<"unSizeData: "<<unSizeData<<endl;
}
void CIcpGpuCuda::setSearchRadius(float fRadiusMax, float fRadiusMin, unsigned unIterations){
if((fRadiusMax>=fRadiusMin)&&(unIterations>0)){
_fSearchRadiusMax = fRadiusMax;
_fSearchRadiusMin = fRadiusMin;
_fRadiusStep = (_fSearchRadiusMax-_fSearchRadiusMin)/(float)unIterations;
_unIterations = unIterations;
_unNoQSizeStep = unIterations/(unsigned)NO_QSIZE;
}
else{
cout<<"Error setting search radius."<<endl;
exit(1);
}
}
float** CIcpGpuCuda::getScenePointer() {
return fHstScn;
}
double** CIcpGpuCuda::getModelPointer(void){
return h_idata;
}
unsigned CIcpGpuCuda::getSize(void){
return unSizeData;
}
void CIcpGpuCuda::setTreePointer(ANNkd_tree *tree){
kdTree = tree;
}
void CIcpGpuCuda::getTreePointer(ANNkd_tree *&tree ){
tree = kdTree;
}
void CIcpGpuCuda::setTree()
{
// preparation
st = new ANNkdStats();
kdTree->getStats(*st);
int nDepth = st->depth;
// cout<<"level of the tree: "<<st->depth<<" (counted from 0)"<<endl;
// decide size of array to be uploaded to GPU
unSizeTree = depth2size(nDepth);
// cout<<"unSizeTree: "<<unSizeTree<<endl;
if (unSizeTree>_unSizeTree) {
cout << "Not enough memory for tree construction. Tree size must be smaller than "
<< unSizeTree <<endl;
exit(1);
}
// rearrange
ANNkd_split* pRoot = (ANNkd_split*)kdTree->getRoot();
if (unSizeData>1) {
rearrange(pRoot, 1);
} else {
cout<<"Not enough points in the tree."<<endl;
exit(1);
}
// download the tree
CUDA_SAFE_CALL(cudaMemcpy(fDevSplit, fSplit, unSizeTree*sizeof(float), cudaMemcpyHostToDevice));
CUDA_SAFE_CALL(cudaMemcpy(unDevIdx, unIdx, unSizeTree*sizeof(unsigned), cudaMemcpyHostToDevice));
CUDA_SAFE_CALL(cudaMemcpy(unDevAxis, unAxis, unSizeTree*sizeof(unsigned), cudaMemcpyHostToDevice));
CUDA_SAFE_CALL(cudaMemcpy(bDevIsLeaf, bIsLeaf, unSizeTree*sizeof(bool), cudaMemcpyHostToDevice));
CUDA_SAFE_CALL(cudaMemcpy(fDevLoBound, fLoBound, unSizeTree*sizeof(float), cudaMemcpyHostToDevice));
CUDA_SAFE_CALL(cudaMemcpy(fDevHiBound, fHiBound, unSizeTree*sizeof(float), cudaMemcpyHostToDevice));
// clean up
// delete kdTree;
delete st;
}
void CIcpGpuCuda::setModel()
{
// using texture memory
for(unsigned i = 0; i < unSizeData; i++) { // type cast
f4Mdl[i].x=(float)h_idata[i][0];
f4Mdl[i].y=(float)h_idata[i][1];
f4Mdl[i].z=(float)h_idata[i][2];
}
cudaMemcpyToArray(cuArray,0,0,f4Mdl,unSizeData*sizeof(float4),cudaMemcpyHostToDevice);
cudaBindTextureToArray(refTex,cuArray);
}
void CIcpGpuCuda::setScene()
{
CUDA_SAFE_CALL(cudaMemcpy(fDevScnX, fHstScnX, unSizeData*sizeof(float), cudaMemcpyHostToDevice));
CUDA_SAFE_CALL(cudaMemcpy(fDevScnY, fHstScnY, unSizeData*sizeof(float), cudaMemcpyHostToDevice));
CUDA_SAFE_CALL(cudaMemcpy(fDevScnZ, fHstScnZ, unSizeData*sizeof(float), cudaMemcpyHostToDevice));
}
inline int CIcpGpuCuda::depth2size(int nDepth)
{
double nSize = 0;
for(int i = 0;i <= nDepth; i++)
{
nSize += pow(2.0,(double)i);
}
return (int)(nSize+0.5);
}
void CIcpGpuCuda::rearrange(ANNkd_ptr root, unsigned unStart)
{
if(root!=NULL && root!=KD_TRIVIAL){
if (root->isLeaf()) {
bIsLeaf[unStart-1] = true;
unIdx[unStart-1] = (unsigned)(((ANNkd_leaf*)root)->getIdxArray())[0];
}
else {
ANNkd_ptr nL = ((ANNkd_split*)root)->getLeftChild();
ANNkd_ptr nR = ((ANNkd_split*)root)->getRightChild();
bIsLeaf[unStart-1] = false;
fSplit[unStart-1] = (float)((ANNkd_split*)root)->getCutVal();
unAxis[unStart-1] = (unsigned)((ANNkd_split*)root)->getCutDim();
fLoBound[unStart-1] = ((ANNkd_split*)root)->getLoBound();
fHiBound[unStart-1] = ((ANNkd_split*)root)->getHiBound();
rearrange(nL, unStart*2);
rearrange(nR, unStart*2+1);
}
}
}
void CIcpGpuCuda::setPointClouds(void){
setTree();
setModel();
setScene();
}
void CIcpGpuCuda::iteration(){
//////////////////////////
// prepare for iteration
//////////////////////////
EnumIcpState icpStat = ICP_PROCESSING;
float fSearchRadius = _fSearchRadiusMax;
unsigned unNoIter = 0;
float fDeviation = 0.0;
unsigned unQStep = 0;
final_matrix = new Matrix(4,4);
(*final_matrix)(1,1) = 1.0;(*final_matrix)(1,2) = 0.0;(*final_matrix)(1,3) = 0.0;(*final_matrix)(1,4) = 0.0;
(*final_matrix)(2,1) = 0.0;(*final_matrix)(2,2) = 1.0;(*final_matrix)(2,3) = 0.0;(*final_matrix)(2,4) = 0.0;
(*final_matrix)(3,1) = 0.0;(*final_matrix)(3,2) = 0.0;(*final_matrix)(3,3) = 1.0;(*final_matrix)(3,4) = 0.0;
(*final_matrix)(4,1) = 0.0;(*final_matrix)(4,2) = 0.0;(*final_matrix)(4,3) = 0.0;(*final_matrix)(4,4) = 1.0;
Matrix matrix(4,4);
init_time = clock();
// The main loop of ICP
while(icpStat == ICP_PROCESSING){
if (unNoIter <= _unIterations)
unQStep = unNoIter/_unNoQSizeStep;
findNearestNeighbors(fSearchRadius, unQStep);
result=cudppCompact(compactplan, fDistCpt, (size_t*)unNoPairs,
fDist, unMask, (size_t)unSizeData);
if (CUDPP_SUCCESS != result)
printf("Error cudppCompact\n");
CUDA_SAFE_CALL(cudaMemcpy(pNoPairs, unNoPairs, sizeof(unsigned), cudaMemcpyDeviceToHost));
unPairs = *pNoPairs;
if (unPairs) {
//////////////////////
// transform estimation
//////////////////////
// Compute centroids (assume all data are non-negative)
float *fCm = new float(3);
float *fCs = new float(3);
computeCentroid(fDevMdlPairX,fDevMdlPairY,fDevMdlPairZ, fCm);
computeCentroid(fDevScnPairX,fDevScnPairY,fDevScnPairZ, fCs);
fDeviation = cublasSdot(unSizeData,fDistCpt,1,ones,1);
fDeviation /= unPairs;
getCublasErr();
// check for termination conditions
unNoIter++;
if(unNoIter<_unIterations)
fSearchRadius-=_fRadiusStep;
if(fDeviation <= fMaxDeviation)
icpStat = ICP_SUCCESS;
else if(unNoIter >= unMaxIteration + 1) // unNoIter starts from 1
icpStat = ICP_MAXITERATIONS;
else if ( (double)(clock() - init_time)/ (double)CLOCKS_PER_SEC * 1000>= fMaxProcTime )
icpStat = ICP_TIMEELAPSED;
else
{
//Calculate centered point pairs
class_centralize(unMask,
fDevMdlPairX,fDevMdlPairY,fDevMdlPairZ,fDevScnPairX,fDevScnPairY,fDevScnPairZ,
fCm[0],fCm[1],fCm[2],fCs[0],fCs[1],fCs[2],
fCenModX,fCenModY,fCenModZ,fCenScnX,fCenScnY,fCenScnZ);
//Fill H matrix
Matrix H(3,3);
H = computeHMatrix();
//SVD
Matrix U(3,3);
DiagonalMatrix Lamda(3);
Matrix V(3,3);
SVD(H,Lamda,U,V);
//Get rotation
Matrix R(3,3);
R = V*(U.t());
// Calculate translation
double dTranslation[3];
ColumnVector col_vec(3);
for(unsigned j = 0; j < 3; j++)
col_vec(j+1) = fCs[j];
ColumnVector r_time_colVec = ColumnVector(R*col_vec);
dTranslation[0] = fCm[0] - r_time_colVec(1);
dTranslation[1] = fCm[1] - r_time_colVec(2);
dTranslation[2] = fCm[2] - r_time_colVec(3);
matrix = fillHomoMatrix(&R,dTranslation);
*final_matrix = matrix * (*final_matrix);
for(int i = 1 ; i < 5 ; ++i)
for(int j = 1; j < 5 ; ++j){
(*matrices[unNoIter - 1])(i,j) = (matrix)(i,j);
}
/////////////
// transform
/////////////
class_transformation(fDevScnX, fDevScnY, fDevScnZ,
(float)matrix(1,1), (float)matrix(1,2), (float)matrix(1,3), (float)matrix(1,4),
(float)matrix(2,1), (float)matrix(2,2), (float)matrix(2,3), (float)matrix(2,4),
(float)matrix(3,1), (float)matrix(3,2), (float)matrix(3,3), (float)matrix(3,4));
///////////////
// termination
///////////////
}
}//if(unPairs)
else icpStat = ICP_NOTMATCHABLE;
}//while(icpStat == ICP_PROCESSING)
_dElapsedTime = (double)(clock() - init_time)/(double)CLOCKS_PER_SEC * 1000.0; //temporary
cout<<"=========="<<endl;
switch(icpStat)
{
case ICP_LIMIT:
cout<<"terminated: convergent limit reached."<<endl;
break;
case ICP_NOTMATCHABLE:
cout<<"terminated: point clounds not matchable."<<endl;
/*
(*final_matrix)(1,1) = 1.0;(*final_matrix)(1,2) = 0.0;(*final_matrix)(1,3) = 0.0;(*final_matrix)(1,4) = 0.0;
(*final_matrix)(2,1) = 0.0;(*final_matrix)(2,2) = 1.0;(*final_matrix)(2,3) = 0.0;(*final_matrix)(2,4) = 0.0;
(*final_matrix)(3,1) = 0.0;(*final_matrix)(3,2) = 0.0;(*final_matrix)(3,3) = 1.0;(*final_matrix)(3,4) = 0.0;
(*final_matrix)(4,1) = 0.0;(*final_matrix)(4,2) = 0.0;(*final_matrix)(4,3) = 0.0;(*final_matrix)(4,4) = 1.0;
*/
break;
case ICP_MAXITERATIONS:
cout<<"terminated: maximum iteration exceeds."<<endl;
break;
case ICP_TIMEELAPSED:
cout<<"terminated: maximum time elapsed."<<endl;
break;
case ICP_SUCCESS:
cout<<"succeeded: maximum deviation reached."<<endl;
break;
}
cout<<"elapsed time:\t"<<_dElapsedTime<<"ms"<<endl;
cout<<"iterations:\t"<<unNoIter - 1<<endl;
cout<<"deviation:\t"<<fDeviation<<endl;
}
double CIcpGpuCuda::getTime(void){
return _dElapsedTime;
}
Matrix* CIcpGpuCuda::getMatrix(void){
return final_matrix;
}
void CIcpGpuCuda::getCublasErr()
{
cublasStatus statusCUBLAS;
statusCUBLAS = cublasGetError();
if (statusCUBLAS != CUBLAS_STATUS_SUCCESS) {
cout<<"CUBLAS error: ";
switch(statusCUBLAS){
case CUBLAS_STATUS_NOT_INITIALIZED: cout<<"CUBLAS library not initialized"<<endl;break;
case CUBLAS_STATUS_ALLOC_FAILED: cout<<"resource allocation failed"<<endl;break;
case CUBLAS_STATUS_INVALID_VALUE: cout<<"unsupported numerical value was passed to function"<<endl;break;
case CUBLAS_STATUS_MAPPING_ERROR: cout<<"access to GPU memory space failed"<<endl;break;
case CUBLAS_STATUS_EXECUTION_FAILED: cout<<"GPU program failed to execute"<<endl;break;
case CUBLAS_STATUS_INTERNAL_ERROR: cout<<"an internal CUBLAS operation failed"<<endl;break;
default: cout<<"undefined error"<<endl;
}
}
// cout<<"error number: "<<statusCUBLAS<<endl;
// fprintf (stderr, "CUBLAS error.\n");};
}
void CIcpGpuCuda::getCudaErr(void){
cudaError_t error=cudaGetLastError();
cout<<cudaGetErrorString(error)<<endl;
}
//kd-tree based nearest neighbor search, using a priority queue: no parameters are needed??
void CIcpGpuCuda::class_nns_priority(
float* fDevScnX, //scene point cloud
float* fDevScnY,
float* fDevScnZ,
float* fDist, //squared distance between pairs, for deviation calculation
float* fDevSplit, //kd-tree: position of splitting plain (inner node)
unsigned* unDevIdx, //kd-tree: index of point (leaf node)
unsigned* unDevAxis, //kd-tree: axis where splitting plain locates (inner node)
bool* bDevIsLeaf, //kd-tree: node type (both nodes)
float* fDevLoBound, //kd-tree: lower bounding box (inner node)
float* fDevHiBound, //kd-tree: higher bounding box (inner node)
unsigned* unMask, //a 0-1 mask of pair and non-pairs.
float* fDevMdlPairX,
float* fDevMdlPairY,
float* fDevMdlPairZ,
float* fDevScnPairX,
float* fDevScnPairY,
float* fDevScnPairZ,
float fSearchRadius,
unsigned unSize,
unsigned unWidth,
unsigned unQStep)
{
wrapper_nns_priority( fDevScnX, fDevScnY, fDevScnZ,
fDist, fDevSplit, unDevIdx, unDevAxis, bDevIsLeaf, fDevLoBound, fDevHiBound,
unMask,
fDevMdlPairX, fDevMdlPairY, fDevMdlPairZ, fDevScnPairX, fDevScnPairY, fDevScnPairZ,
dimGrid, dimBlock, fSearchRadius,
unSize, unWidth,
unQStep );
}
//centralize a pointcloud
void CIcpGpuCuda::class_centralize(unsigned* unMask,
float* fDevMdlPairX,
float* fDevMdlPairY,
float* fDevMdlPairZ,
float* fDevScnPairX,
float* fDevScnPairY,
float* fDevScnPairZ,
float fcm0,
float fcm1,
float fcm2,
float fcs0,
float fcs1,
float fcs2,
float* fCenteredModX, //centered point cloud
float* fCenteredModY,
float* fCenteredModZ,
float* fCenteredScnX,
float* fCenteredScnY,
float* fCenteredScnZ){
wrapper_centralize(unMask,
fDevMdlPairX,fDevMdlPairY,fDevMdlPairZ,fDevScnPairX,fDevScnPairY,fDevScnPairZ,
fcm0,fcm1,fcm2,fcs0,fcs1,fcs2,
fCenteredModX,fCenteredModY,fCenteredModZ,fCenteredScnX,fCenteredScnY,fCenteredScnZ,
dimGrid, dimBlock);
}
// transform point cloud
void CIcpGpuCuda::class_transformation(float* fDevScnX, // point cloud to be transformed
float* fDevScnY,
float* fDevScnZ,
float m00, float m01, float m02, float m03,
float m10, float m11, float m12, float m13,
float m20, float m21, float m22, float m23)
{
wrapper_transformation(fDevScnX, fDevScnY, fDevScnZ,
m00, m01, m02, m03,
m10, m11, m12, m13,
m20, m21, m22, m23,
dimGrid, dimBlock);
}
void CIcpGpuCuda::setMinimums(float x, float y, float z)
{
min_x = x;
min_y = y;
min_z = z;
}
Matrix** CIcpGpuCuda::getMatrices(){
return matrices;
}
void CIcpGpuCuda::setTrans_Trans_inv(const double tr[], const double tr_inv[]){
trans = new Matrix(4,4);
trans_inv = new Matrix(4,4);
(*trans)(1,1) = tr[0]; (*trans)(2,1)=tr[1];(*trans)(3,1)=tr[2];(*trans)(4,1)=tr[3];
(*trans)(1,2) = tr[4]; (*trans)(2,2)=tr[5];(*trans)(3,2)=tr[6];(*trans)(4,2)=tr[7];
(*trans)(1,3) = tr[8]; (*trans)(2,3)=tr[9];(*trans)(3,3)=tr[10];(*trans)(4,3)=tr[11];
(*trans)(1,4) = tr[12]; (*trans)(2,4)=tr[13];(*trans)(3,4)=tr[14];(*trans)(4,4)=tr[15];
(*trans_inv)(1,1) = tr_inv[0]; (*trans_inv)(2,1)=tr_inv[1];(*trans_inv)(3,1)=tr_inv[2];(*trans_inv)(4,1)=tr_inv[3];
(*trans_inv)(1,2) = tr_inv[4]; (*trans_inv)(2,2)=tr_inv[5];(*trans_inv)(3,2)=tr_inv[6];(*trans_inv)(4,2)=tr_inv[7];
(*trans_inv)(1,3) = tr_inv[8]; (*trans_inv)(2,3)=tr_inv[9];(*trans_inv)(3,3)=tr_inv[10];(*trans_inv)(4,3)=tr_inv[11];
(*trans_inv)(1,4) = tr_inv[12]; (*trans_inv)(2,4)=tr_inv[13];(*trans_inv)(3,4)=tr_inv[14];(*trans_inv)(4,4)=tr_inv[15];
}
void CIcpGpuCuda::findNearestNeighbors(float fSearchRadius, unsigned unQStep)
{
// We make a copy of the existing scene point cloud in order to transform it
cudaMemcpy(cngfDevScnX, fDevScnX, unSizeData*sizeof(float), cudaMemcpyDeviceToDevice);
cudaMemcpy(cngfDevScnY, fDevScnY, unSizeData*sizeof(float), cudaMemcpyDeviceToDevice);
cudaMemcpy(cngfDevScnZ, fDevScnZ, unSizeData*sizeof(float), cudaMemcpyDeviceToDevice);
//////
class_transformation(cngfDevScnX, cngfDevScnY, cngfDevScnZ ,
(*trans_inv)(1,1), (*trans_inv)(1,2), (*trans_inv)(1,3), (*trans_inv)(1,4),
(*trans_inv)(2,1), (*trans_inv)(2,2), (*trans_inv)(2,3), (*trans_inv)(2,4),
(*trans_inv)(3,1), (*trans_inv)(3,2), (*trans_inv)(3,3), (*trans_inv)(3,4)
);
class_nns_priority( cngfDevScnX, cngfDevScnY, cngfDevScnZ,
fDist, fDevSplit, unDevIdx, unDevAxis, bDevIsLeaf, fDevLoBound, fDevHiBound,
unMask, fDevMdlPairX, fDevMdlPairY, fDevMdlPairZ, fDevScnPairX, fDevScnPairY, fDevScnPairZ,
fSearchRadius, unSizeData, _unWidth, unQStep);
class_transformation(fDevMdlPairX, fDevMdlPairY, fDevMdlPairZ,
(*trans)(1,1), (*trans)(1,2), (*trans)(1,3), (*trans)(1,4),
(*trans)(2,1), (*trans)(2,2), (*trans)(2,3), (*trans)(2,4),
(*trans)(3,1), (*trans)(3,2), (*trans)(3,3), (*trans)(3,4)
);
class_transformation(fDevScnPairX, fDevScnPairY, fDevScnPairZ,
(*trans)(1,1), (*trans)(1,2), (*trans)(1,3), (*trans)(1,4),
(*trans)(2,1), (*trans)(2,2), (*trans)(2,3), (*trans)(2,4),
(*trans)(3,1), (*trans)(3,2), (*trans)(3,3), (*trans)(3,4)
);
/*fDevScnX
cout<<"Trans Mat is : \n";
printMatrix(trans);
cout<<"Trans Inv Mat is : \n";
printMatrix(trans_inv);
*/
float tmpscn_x[10];
float tmpscn_y[10];
float tmpscn_z[10];
float tmpmdl_x[10];
float tmpmdl_y[10];
float tmpmdl_z[10];
cudaMemcpy(tmpscn_x, fDevScnPairX, 10*sizeof(float), cudaMemcpyDeviceToHost);
cudaMemcpy(tmpscn_y, fDevScnPairY, 10*sizeof(float), cudaMemcpyDeviceToHost);
cudaMemcpy(tmpscn_z, fDevScnPairZ, 10*sizeof(float), cudaMemcpyDeviceToHost);
cudaMemcpy(tmpmdl_x, fDevMdlPairX, 10*sizeof(float), cudaMemcpyDeviceToHost);
cudaMemcpy(tmpmdl_y, fDevMdlPairY, 10*sizeof(float), cudaMemcpyDeviceToHost);
cudaMemcpy(tmpmdl_z, fDevMdlPairZ, 10*sizeof(float), cudaMemcpyDeviceToHost);
}
Matrix CIcpGpuCuda::fillHomoMatrix(Matrix* R, double* dTranslation){
//Fill result
Matrix matrix(4,4);
matrix(1,1) = (*R)(1,1);
matrix(1,2) = (*R)(1,2);
matrix(1,3) = (*R)(1,3);
matrix(1,4) = dTranslation[0];
matrix(2,1) = (*R)(2,1);
matrix(2,2) = (*R)(2,2);
matrix(2,3) = (*R)(2,3);
matrix(2,4) = dTranslation[1];
matrix(3,1) = (*R)(3,1);
matrix(3,2) = (*R)(3,2);
matrix(3,3) = (*R)(3,3);
matrix(3,4) = dTranslation[2];
matrix(4,1) = 0;
matrix(4,2) = 0;
matrix(4,3) = 0;
matrix(4,4) = 1;
return matrix;
}
void CIcpGpuCuda::computeCentroid(float* x, float* y, float* z, float*& center){
/*
cublasSasum function works only with absolute values.
So I created an array of ones and used a dot product
in order to simulate a simple sum function
*/
center[0] = cublasSdot(unSizeData,x,1,ones,1);
center[0] /=unPairs;
center[1] = cublasSdot(unSizeData,y,1,ones,1);
center[1] /=unPairs;
center[2] = cublasSdot(unSizeData,z,1,ones,1);
center[2] /=unPairs;
}
Matrix CIcpGpuCuda::computeHMatrix(){
Matrix H(3,3);
H = 0.0;
unsigned unSizeOfSec = 200000; // need to be tuned for best performance!// +++ Fill by gpu +++
if(unSizeData<=unSizeOfSec){
H(1,1) = (double)cublasSdot(unSizeData,fCenScnX,1,fCenModX,1);
H(1,2) = (double)cublasSdot(unSizeData,fCenScnX,1,fCenModY,1);
H(1,3) = (double)cublasSdot(unSizeData,fCenScnX,1,fCenModZ,1);
H(2,1) = (double)cublasSdot(unSizeData,fCenScnY,1,fCenModX,1);
H(2,2) = (double)cublasSdot(unSizeData,fCenScnY,1,fCenModY,1);
H(2,3) = (double)cublasSdot(unSizeData,fCenScnY,1,fCenModZ,1);
H(3,1) = (double)cublasSdot(unSizeData,fCenScnZ,1,fCenModX,1);
H(3,2) = (double)cublasSdot(unSizeData,fCenScnZ,1,fCenModY,1);
H(3,3) = (double)cublasSdot(unSizeData,fCenScnZ,1,fCenModZ,1);
}
else{
unsigned unSections = (unsigned)(unSizeData/unSizeOfSec);//cout<<"unSections: "<<unSections<<endl;
unsigned unStub = (unsigned)(unSizeData%unSizeOfSec);//cout<<"unStub: "<<unStub<<endl;
for(unsigned i=0;i<unSections;i++){
H(1,1) += (double)cublasSdot(unSizeOfSec,fCenScnX+i*unSizeOfSec,1,fCenModX+i*unSizeOfSec,1);
H(1,2) += (double)cublasSdot(unSizeOfSec,fCenScnX+i*unSizeOfSec,1,fCenModY+i*unSizeOfSec,1);
H(1,3) += (double)cublasSdot(unSizeOfSec,fCenScnX+i*unSizeOfSec,1,fCenModZ+i*unSizeOfSec,1);
H(2,1) += (double)cublasSdot(unSizeOfSec,fCenScnY+i*unSizeOfSec,1,fCenModX+i*unSizeOfSec,1);
H(2,2) += (double)cublasSdot(unSizeOfSec,fCenScnY+i*unSizeOfSec,1,fCenModY+i*unSizeOfSec,1);
H(2,3) += (double)cublasSdot(unSizeOfSec,fCenScnY+i*unSizeOfSec,1,fCenModZ+i*unSizeOfSec,1);
H(3,1) += (double)cublasSdot(unSizeOfSec,fCenScnZ+i*unSizeOfSec,1,fCenModX+i*unSizeOfSec,1);
H(3,2) += (double)cublasSdot(unSizeOfSec,fCenScnZ+i*unSizeOfSec,1,fCenModY+i*unSizeOfSec,1);
H(3,3) += (double)cublasSdot(unSizeOfSec,fCenScnZ+i*unSizeOfSec,1,fCenModZ+i*unSizeOfSec,1);
}
if(unStub){
H(1,1) += (double)cublasSdot(unStub,fCenScnX+unSections*unSizeOfSec,1,fCenModX+unSections*unSizeOfSec,1);
H(1,2) += (double)cublasSdot(unStub,fCenScnX+unSections*unSizeOfSec,1,fCenModY+unSections*unSizeOfSec,1);
H(1,3) += (double)cublasSdot(unStub,fCenScnX+unSections*unSizeOfSec,1,fCenModZ+unSections*unSizeOfSec,1);
H(2,1) += (double)cublasSdot(unStub,fCenScnY+unSections*unSizeOfSec,1,fCenModX+unSections*unSizeOfSec,1);
H(2,2) += (double)cublasSdot(unStub,fCenScnY+unSections*unSizeOfSec,1,fCenModY+unSections*unSizeOfSec,1);
H(2,3) += (double)cublasSdot(unStub,fCenScnY+unSections*unSizeOfSec,1,fCenModZ+unSections*unSizeOfSec,1);
H(3,1) += (double)cublasSdot(unStub,fCenScnZ+unSections*unSizeOfSec,1,fCenModX+unSections*unSizeOfSec,1);
H(3,2) += (double)cublasSdot(unStub,fCenScnZ+unSections*unSizeOfSec,1,fCenModY+unSections*unSizeOfSec,1);
H(3,3) += (double)cublasSdot(unStub,fCenScnZ+unSections*unSizeOfSec,1,fCenModZ+unSections*unSizeOfSec,1);
}
}
getCublasErr();
return H;
}
void CIcpGpuCuda::printMatrix(Matrix* mat){
for(int i = 1 ; i < 5 ; ++i)
cout<<(*mat)(i,1)<< " "<<(*mat)(i,2)<< " "<<(*mat)(i,3)<<
" "<<(*mat)(i,4)<< endl;
}
Reference in a new issue Copy permalink