c++ - 使用 Eigen 过滤球体内的点

Question

我有一组 3D 点，我需要计算哪些点最接近给定点p。我想知道在 Eigen 中这可能是正确的方法。到目前为止，我有：

Matrix<double, Dynamic, 3> points; // The set of 3D points
Matrix<double, 1, 3> p;

// Populate the "points" matrix

...

// Fill a matrix with several copies of "p" in order to match the size
of "points"

Matrix<double, Dynamic, 3> pp(points.rows(), 3);

pp = Matrix<double, Dynamic, 1>::Ones(points.rows, 1) * p;

Matrix<double, Dynamic, 1> sq_distances = (points - pp).rowwise.squaredNorm();
Matrix<bool, Dynamic, 1> nearest_points = sq_distances < (dist_threshold * dist_threshold);

然后我可以有一些方法来提取满足“最近点”条件的“点”中的点，如

Matrix<double, Dynamic, 3> nearest = points(nearest_points);

?

Answer 1

对于最近的我建议：

int i;
double sqdist = (points.rowwise()-p).rowwise().squaredNorm().minCoeff(&i);
nearest = points.row(i);

对于给定球中的那些，您目前必须自己编写一个循环：

ArrayXd sqdists = (points.rowwise()-p).rowwise().squaredNorm();
Matrix<double,Dynamic,3> nearests( (sqdists<sqradius).count(), 3 );
int count = 0;
for(int i=0; i<points.rows(); ++i)
  if(sqdists(i)<sqradius)
    nearests.row(count++) = points.row(i);