D2D registration

edit

Hi,

I'm trying to get the NDTMatcherD2D to work with my NDT map, but it performed poorly slow and so I looked into the algorithm and saw that it was approximating a normal distribution using a pointcloud. Since I already have a NDT map I thought it might be faster to plug it directly into the algorithm, but I have severe problems with my normal distributions since they have very small values and many of them are also very flat.

I replaced all calls involving the approximated maps using the means and covariances of my map. Then I multiplied the diagonal elements of the covariance with a scaling factor, because of the covariance's small size.

The algorithm is running, but it does not calculate the correct transformation matrix, it always returns something similar to an identity matrix.

These are the code snippets I replaced. They are from ndt_matcher_d2d.

If you need more information/snippets, I will provide them. Thanks for you help.

Replacement:

Temp.setIdentity();

tv_start = clock();
ret = this->matchNew(target, source, Temp);
std::cout << "Matching time" << (clock() - tv_start)/(CLOCKS_PER_SEC/1000) << std::endl;
tv_end = clock();

time_match += (double(tv_end - tv_start))/CLOCKS_PER_SEC;

//transform moving
T = Temp*T; //ORIGINAL

Original:

if(isIrregularGrid)
{

    OctTree<PointTarget> pr1;
    NDTMap<PointTarget> targetNDT( &pr1 );
    targetNDT.loadPointCloud( target );
    targetNDT.computeNDTCells();

    OctTree<PointSource> pr2;
    NDTMap<PointSource> sourceNDT( &pr2 );
    sourceNDT.loadPointCloud( source );
    sourceNDT.computeNDTCells();

    ret = this->match( targetNDT, sourceNDT, T );

}
else
{

    //iterative regular grid
    for(int r_ctr = resolutions.size()-1; r_ctr >=0;  r_ctr--)
    {

        current_resolution = resolutions[r_ctr];

        LazyGrid<PointSource> prototypeSource(current_resolution);
        LazyGrid<PointTarget> prototypeTarget(current_resolution);

        gettimeofday(&tv_start,NULL);
        NDTMap<PointTarget> targetNDT( &prototypeTarget );
        targetNDT.loadPointCloud( target );
        targetNDT.computeNDTCells();

        NDTMap<PointSource> sourceNDT( &prototypeSource );
        sourceNDT.loadPointCloud( sourceCloud );
        sourceNDT.computeNDTCells();
        gettimeofday(&tv_end,NULL);

        time_load += (tv_end.tv_sec-tv_start.tv_sec)*1000.+(tv_end.tv_usec-tv_start.tv_usec)/1000.;
        Temp.setIdentity();

        gettimeofday(&tv_start,NULL);
        ret = this->match( targetNDT, sourceNDT, Temp );
        std::cout << "Matching time" << (clock() - tv_start)/(CLOCKS_PER_SEC/1000) << std::endl;
        lslgeneric::transformPointCloudInPlace(Temp,sourceCloud);
        gettimeofday(&tv_end,NULL);

        time_match += (tv_end.tv_sec-tv_start.tv_sec)*1000.+(tv_end.tv_usec-tv_start.tv_usec)/1000.;

        //transform moving
        T = Temp*T; //ORIGINAL
        //T = T*Temp;

   }
}

And

Replacement:

            meanMoving = source.getMean(index);
        CMoving= source.getScaledCov(index);

        computeDerivativesLocal(meanMoving, CMoving, _Jest, _Hest, _Zest, _ZHest, computeHessian);

        point.x = meanMoving(0);
        point.y = meanMoving(1);
        point.z = meanMoving(2);

        std::vector<int> neighborIndexes;
        neighborIndexes.clear();
        target.getNeighbors(neighborIndexes, meanMoving(0), meanMoving(1), meanMoving(2), 2);

        for(unsigned int j = 0; j < neighborIndexes.size(); j++) {
            if (!target.checkPointExists(j)) continue;
            if (!target.hasGaussian(j)) continue;

            transformed = meanMoving - target.getMean(j);
            CSum = (target.getScaledCov(j) + CMoving);
            CSum.computeInverseAndDetWithCheck(Cinv,det,exists);

Original:

 meanMoving = sourceNDT[i]->getMean();
    CMoving= sourceNDT[i]->getCov();
    point.x = meanMoving(0);
    point.y = meanMoving(1);
    point.z = meanMoving(2);
    std::vector<NDTCell<PointSource>*> cells = targetNDT.getCellsForPoint(point,2); //targetNDT.getAllCells(); //
    for(unsigned int j=0; j<cells.size(); j++)
    {
        cell = cells[j];
        if(cell == NULL)
        {
            continue;
        }
        if(cell->hasGaussian_)
        {
            transformed = meanMoving - cell->getMean();
            CFixed = cell->getCov();
            CSum = (CFixed+CMoving);
            CSum.computeInverseAndDetWithCheck(Cinv,det,exists);