869 lines
32 KiB
Text
869 lines
32 KiB
Text
/** @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;
|
|
}
|