src/core/cloud.cpp
//! clips to a maxRange around a pose
clips to a maxRange around a pose
clips to a maxRange around a pose
void Cloud::clip(float range, const Eigen::Isometry3f& pose) {
    Eigen::Isometry3f T = pose.inverse();
    range *=range;
    int k = 0;
    for (size_t i=0; i<size(); i++){
      const RichPoint& p = at(i);
      Eigen::Vector3f other_p = T*p.point();
      if (other_p.squaredNorm()<range) {
	at(k) = p;
	k++;
      }
    }
    resize(k);
  }
void Cloud :: clip ( float range , const Eigen :: Isometry3f & pose ) { Eigen :: Isometry3f T = pose . inverse ( ) ; range *= range ; int k = 0 ; for ( size_t i = 0 ; i < size ( ) ; i ++ ) { const RichPoint & p = at ( i ) ; Eigen :: Vector3f other_p = T * p . point ( ) ; if ( other_p . squaredNorm ( ) < range ) { at ( k ) = p ; k ++ ; } } resize ( k ) ; }
clips to a maxRange around a pose
clips to a maxRange around a pose