luigifreda / plvs

PLVS is a real-time SLAM system with points, lines, volumetric mapping and 3D unsupervised incremental segmentation.
GNU General Public License v3.0
453 stars 66 forks source link

Line matching #17

Closed HJMGARMIN closed 9 months ago

HJMGARMIN commented 9 months ago

Hello, I found that you compute for each line segment its normal orientation θ and signed distance h in the paper. Then, the 2D parameter manifold (θ, h) is partitioned into tiles. Could you explain the specific code location of this part? I didn't find the corresponding code content in ComputeStereoLineMatches() and ExtractLSD(). image

luigifreda commented 9 months ago

Hi, check this block https://github.com/luigifreda/plvs/blob/master/src/Frame.cc#L728 and this function https://github.com/luigifreda/plvs/blob/master/src/Frame.cc#L1416

HJMGARMIN commented 9 months ago

Thanks for your reply. I met a new question as below. Where is the code corresponding to formula 35? image

Is this the part? However, the calculation process of this part of the code looks a little different from Equation 35. For example, there is no K1 and K2, etc.

   ```

Eigen::Vector3d startL(kll.startPointX, kll.startPointY, 1.0); Eigen::Vector3d endL(kll.endPointX, kll.endPointY, 1.0); Eigen::Vector3d ll = startL.cross(endL); ll = ll / sqrt( ll(0)ll(0) + ll(1)ll(1) ); // now ll = [nx ny -d] with (nx^2 + ny^2) = 1

    Eigen::Vector3d startR(klr.startPointX, klr.startPointY, 1.0);
    Eigen::Vector3d endR(klr.endPointX,   klr.endPointY, 1.0);
    Eigen::Vector3d lr = startR.cross(endR);     
    lr = lr / sqrt( lr(0)*lr(0) + lr(1)*lr(1) ); // now lr = [nx ny -d] with (nx^2 + ny^2) = 1       

    const float dotProductThreshold = kLineNormalsDotProdThreshold * sigma; // this a percentage over unitary modulus ( we modulate threshold by sigma)
    const float distThreshold = 2.f * sigma;  // 1.f * sigma // 1 pixel (we modulate threshold by sigma)

    // an alternative way to check the degeneracy 
    if( fabs( ll(0) ) < dotProductThreshold ) continue; // ll = [nx ny -d] with (nx^2 + ny^2) = 1         
    if( fabs( lr(0) ) < dotProductThreshold ) continue; // lr = [nx ny -d] with (nx^2 + ny^2) = 1 

    // detect if lines are equal => if yes the backprojected planes are parallel and we must discard the matched lines 
    if( Geom2DUtils::areLinesEqual(ll, lr, dotProductThreshold, distThreshold)) 
    {
        //std::cout << "detected equal lines[" << ii <<"] - ll " << ll << ", lr: " << lr <<  std::endl;
        continue;
    }

if 0

    if( fabs( lr(0) ) < 0.01 ) continue;

    startR << - (lr(2)+lr(1)*kll.startPointY )/lr(0) , kll.startPointY ,  1.0;
    endR << - (lr(2)+lr(1)*kll.endPointY   )/lr(0) , kll.endPointY ,    1.0;
    double disparity_s = kll.startPointX - startR(0);
    double disparity_e = kll.endPointX   - endR(0);     

    if( disparity_s>=minD && disparity_s<=maxD && disparity_e>=minD && disparity_e<=maxD )
    {
        if(disparity_s<=0)
        {
            disparity_s=0.01;
        }
        if(disparity_e<=0)
        {
            disparity_e=0.01;
        }            

        mvuRightLineStart[idxL] = startR(0);
        mvDepthLineStart[idxL]  = mbf/disparity_s;
        mvuRightLineEnd[idxL] = endR(0);
        mvDepthLineEnd[idxL]  = mbf/disparity_e;
    }
    else
    {
        continue; 
    }

else

    // < TODO: compute covariance and use chi square check ?

    const double disparity_s = lr.dot(startL)/lr(0);        
    const double disparity_e = lr.dot(endL)/lr(0);            

    if( disparity_s>=minD && disparity_s<=maxD && disparity_e>=minD && disparity_e<=maxD )
    {                     
        const double depth_s = mbf / disparity_s;           
        const double depth_e = mbf / disparity_e;               

        mvuRightLineStart[idxL] = startL(0) - disparity_s;
        mvDepthLineStart[idxL]  = depth_s;
        mvuRightLineEnd[idxL] = endL(0) - disparity_e;
        mvDepthLineEnd[idxL]  = depth_e;
    }
    else
    {
        continue;
    }