You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
132 lines
3.9 KiB
Plaintext
132 lines
3.9 KiB
Plaintext
#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;
|
|
}
|
|
|