yorsh87 / nicp

Normal Iterative Closest Point (NICP) Algorithm C++ Library
GNU General Public License v3.0
396 stars 115 forks source link

Check for _minDistance <= d <= _maxDistance once #14

Closed bmegli closed 3 years ago

bmegli commented 6 years ago

When projecting in:

test for _minDistance <= d <= _maxDistance is made twice for every point:

Example from pinhole point projector:

 inline bool _project(int &x, int &y, float &d, const Point &p) const {
      Eigen::Vector4f ip = _KRt * p;
      d = ip.coeff(2);
      if(d < _minDistance || d > _maxDistance) //this already tests and returns false
    return false;
      ip *= (1.0f / d);
      x = (int)round(ip.coeff(0));
      y = (int)round(ip.coeff(1)); 
      return true;
}
void PinholePointProjector::project(IntImage &indexImage,
                      DepthImage &depthImage, 
                      const PointVector &points) const {
    assert(_imageRows && _imageCols && "PinholePointProjector: _imageRows and _imageCols are zero");

    //... 

    for(size_t i = 0; i < points.size(); i++, point++) {
      int x, y;
      float d;
      if(!_project(x, y, d, *point) ||
     d < _minDistance || d > _maxDistance || //this test for every point was already made
     x < 0 || x >= indexImage.cols ||
     y < 0 || y >= indexImage.rows)
    continue;

      // ...

      }
    }
}