Method cascadeIter_

source/MRMesh/MRMultiwayICP.cpp:1014–1077

}

bool MultiwayICP::cascadeIter_( bool p2pl /*= true */ )
{
    for ( ICPLayer l = 0; l < pairsGridPerLayer_.size(); ++l )
    {
        updateLayerPairs_( l );

        const auto& pairsOnLayer = pairsGridPerLayer_[l];
        auto numHyperGroups = cascadeIndexer_->getNumElements( l + 1 );

        for ( ICPElementId hgI( 0 ); hgI < numHyperGroups; ++hgI )
        {
            auto nodes = cascadeIndexer_->getElementNodes( l + 1, hgI );
            assert( nodes.any() );
            auto numNodes = nodes.count();
            if ( numNodes == 1 )
                continue;

            int indI = 0;
            MultiwayAligningTransform mat;
            mat.reset( int( numNodes ) );
            for ( auto nodeI : nodes )
            {
                int indJ = 0;
                for ( auto nodeJ : nodes )
                {
                    if ( nodeI == nodeJ )
                    {
                        ++indJ;
                        continue;
                    }
                    const auto& pairs = pairsOnLayer[nodeI][nodeJ];
                    for ( auto idx : pairs.active )
                    {
                        const auto& data = pairs.vec[idx];
                        if ( p2pl )
                            mat.add( indI, data.srcPoint, indJ, data.tgtPoint, data.tgtNorm, data.weight );
                        else
                            mat.add( indI, data.srcPoint, indJ, data.tgtPoint, data.weight );
                    }
                    ++indJ;
                }
                ++indI;
            }

            MultiwayAligningTransform::Stabilizer stabilizer;
            stabilizer.rot = samplingSize_;
            stabilizer.shift = 1e-2f;
            auto res = mat.solve( stabilizer );
            indI = 0;
            for ( auto nodeI : nodes )
            {
                auto resI = res[indI].rigidXf();
                if ( std::isnan( resI.b.x ) )
                    return false;
                const auto& leaves = cascadeIndexer_->getElementLeaves( l, nodeI );
                for ( auto objId : leaves )
                    objs_[objId].xf = AffineXf3f( resI * AffineXf3d( objs_[objId].xf ) );