void findVertexNeighbors ( Vertex target , List < ChessboardCorner > corners ) { // if( target.index == 18 ) { // System.out.println("Vertex Neighbors "+target.index); // } ChessboardCorner targetCorner = corners . get ( target . index ) ; // distance is Euclidean squared double maxDist = Double . MAX_VALUE == maxNeighborDistance ? maxNeighborDistance : maxNeighborDistance * maxNeighborDistance ; nnSearch . findNearest ( corners . get ( target . index ) , maxDist , maxNeighbors , nnResults ) ; // storage distances here to find median distance of closest neighbors distanceTmp . reset ( ) ; for ( int i = 0 ; i < nnResults . size ; i ++ ) { NnData < ChessboardCorner > r = nnResults . get ( i ) ; if ( r . index == target . index ) continue ; distanceTmp . add ( r . distance ) ; double oriDiff = UtilAngle . distHalf ( targetCorner . orientation , r . point . orientation ) ; Edge edge = edges . grow ( ) ; boolean parallel ; if ( oriDiff <= orientationTol ) { // see if it's parallel parallel = true ; } else if ( Math . abs ( oriDiff - Math . PI / 2.0 ) <= orientationTol ) { // see if it's perpendicular parallel = false ; } else { edges . removeTail ( ) ; continue ; } // Use the relative angles of orientation and direction to prune more obviously bad matches double dx = r . point . x - targetCorner . x ; double dy = r . point . y - targetCorner . y ; edge . distance = Math . sqrt ( r . distance ) ; edge . dst = vertexes . get ( r . index ) ; edge . direction = Math . atan2 ( dy , dx ) ; double direction180 = UtilAngle . boundHalf ( edge . direction ) ; double directionDiff = UtilAngle . distHalf ( direction180 , r . point . orientation ) ; boolean remove ; EdgeSet edgeSet ; if ( parallel ) { // test to see if direction and orientation are aligned or off by 90 degrees remove = directionDiff > 2 * directionTol && Math . abs ( directionDiff - Math . PI / 2.0 ) > 2 * directionTol ; edgeSet = target . parallel ; } else { // should be at 45 degree angle remove = Math . abs ( directionDiff - Math . PI / 4.0 ) > 2 * directionTol ; edgeSet = target . perpendicular ; } if ( remove ) { edges . removeTail ( ) ; continue ; } edgeSet . add ( edge ) ; } // Compute the distance of the closest neighbors. This is used later on to identify ambiguous corners. // If it's a graph corner there should be at least 3 right next to the node. if ( distanceTmp . size == 0 ) { target . neighborDistance = 0 ; } else { sorter . sort ( distanceTmp . data , distanceTmp . size ) ; int idx = Math . min ( 3 , distanceTmp . size - 1 ) ; target . neighborDistance = Math . sqrt ( distanceTmp . data [ idx ] ) ; // NN distance is Euclidean squared } }
Use nearest neighbor search to find closest corners . Split those into two groups parallel and perpendicular .