3dpcp/.svn/pristine/70/706c417cb83dd655174ab106e6cf522421f3379d.svn-base
2012-09-16 14:33:11 +02:00

131 lines
3.9 KiB
Text

#include <dlfcn.h>
using namespace std;
#include "slam6d/cuda/icp6Dcuda.h"
#include "slam6d/cuda/CIcpGpuCuda.cuh"
#include "newmat/newmat.h"
void icp6Dcuda::initGPUicp(float max_rad, float min_rad, int iter, int max_iter,
int max_proctime, float max_dev, const double trans[], const double trans_inv[])
{
#ifndef WITH_SCANSERVER
unsigned int max_size = Scan::max_points_red_size;
#else //WITH_SCANSERVER
unsigned int max_size = Scan::getMaxCountReduced();
#endif //WITH_SCANSERVER
if (max_scnSize < max_size) {
if (icp != 0) {
delete icp;
icp = 0;
}
}
if (icp == 0) {
max_scnSize = max_size;
icp = new CIcpGpuCuda((unsigned)10, ceil((float)max_size/10.f) ,
(unsigned)max_num_iterations);
icp->setMaxProcTime((double) max_proctime);
icp->setMaxDeviation((double) max_dev);
icp->setSearchRadius((float) max_rad, (float) min_rad, (unsigned)max_num_iterations);
}
}
icp6Dcuda::icp6Dcuda(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)
: icp6D(my_icp6Dminimizer, max_dist_match,
max_num_iterations, quiet, meta, rnd, eP,
anim, epsilonICP, nns_method, cuda_enabled)
{
icp = 0;
max_scnSize = 0;
}
/**
* 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 icp6Dcuda::match(Scan* PreviousScan, Scan* CurrentScan)
{
#ifndef WITH_SCANSERVER
int mdlSize = PreviousScan->get_points_red_size(), scnSize = CurrentScan->get_points_red_size();
#else //WITH_SCANSERVER
int mdlSize = PreviousScan->getCountReduced(), scnSize = CurrentScan->getCountReduced();
#endif //WITH_SCANSERVER
double** mdl;
float** scn;
const double *trans;
double trans_inv[16];
trans = PreviousScan->get_transMat();
M4inv(trans, trans_inv);
initGPUicp(sqrt(max_dist_match2), sqrt(max_dist_match2), max_num_iterations,
max_num_iterations, INT_MAX, epsilonICP, trans, trans_inv);
h_idata = icp->getModelPointer();
fHstScn = icp->getScenePointer();
icp->setTrans_Trans_inv(trans, trans_inv);
icp->setSize(10, ceil((float)max(mdlSize, scnSize)/10.0f));
#ifndef WITH_SCANSERVER
double **mod_dat = PreviousScan->get_org_points_red();
double *const*scn_dat = CurrentScan->get_points_red();
#else //WITH_SCANSERVER
DataXYZ scn_dat(CurrentScan->getXYZReduced());
DataXYZ mod_dat(PreviousScan->getXYZReducedOriginal());
#endif //WITH_SCANSERVER
mdl = h_idata;
scn = fHstScn;
cout << "model point cloud size is " << mdlSize << "\n";
for (unsigned int i = 0; i < mdlSize; ++i) {
mdl[i][0] = (float)mod_dat[i][0];
mdl[i][1] = (float)mod_dat[i][1];
mdl[i][2] = (float)mod_dat[i][2];
}
cout << "scene point cloud size is " << scnSize << "\n";
for (unsigned int i = 0; i < scnSize; ++i) {
scn[0][i] = (float)scn_dat[i][0];
scn[1][i] = (float)scn_dat[i][1];
scn[2][i] = (float)scn_dat[i][2];
}
icp->setTreePointer(const_cast<ANNkd_tree *>(PreviousScan->getANNTree()));
float result[4][4];
// doICP
icp->setPointClouds();
icp->iteration();
Matrix* M = icp->getMatrix();
for (int i = 0 ; i < 4 ; ++i){
for(int j = 0 ; j < 4 ; ++j) {
result[i][j] = (*M)(i+1,j+1);
}
}
if(anim > 0){
Matrix** mats = icp->getMatrices();
for(int ci = 0 ; ci < max_num_iterations ; ci += anim){
Matrix* cur_mat = mats[ci];
double xf[16];
for(int i = 1; i < 5 ; ++i)
for(int j = 1; j < 5; ++j)
xf[(i-1)+(j-1)*4] = (*cur_mat)(i,j);
CurrentScan->transform(xf, Scan::ICP, 0);
}
}
else {
double alignxf[16];
for (int i = 0; i < 4; ++i)
for (int j = 0; j < 4; ++j){
alignxf[i + j * 4] = result[i][j];
}
CurrentScan->transform(alignxf, Scan::ICP, 0); // write end pose
}
return EXIT_SUCCESS;
}