Sanity check failed when query set and point set are different

[yuhao]

Hi! I wrote a small sample code, where I added two point sets, and used one as the query set and the other as the point set. I then did a sanity check, but it failed. The relevant code is below. Am I doing anything wrong?

  // read_pc_data is my unity function to read point cloud files
  read_pc_data(data_file, &points); 
  read_pc_data(query_file, &queries);

  unsigned int numPoints = static_cast<int>(points.size());
  printf("Number of points: %d \n", numPoints);

  unsigned int numQueries = static_cast<int>(queries.size());
  printf("Number of queries: %d \n", numQueries);
  
  //Create neighborhood search instance
  NeighborhoodSearch nsearch(radius);
  printf("Radius: %lf \n", radius);
  
  //Add point sets
  auto pointIndex = nsearch.add_point_set(points.front().data(), points.size(), true, true);
  auto queryIndex = nsearch.add_point_set(queries.front().data(), queries.size(), true, true);
  nsearch.set_active(pointIndex, pointIndex, false);
  nsearch.set_active(pointIndex, queryIndex, false);
  nsearch.set_active(queryIndex, queryIndex, false);
  
  nsearch.find_neighbors();
  
  auto ps = nsearch.point_set(queryIndex);
  auto r_queries = ps.GetPoints();
  std::cout << ps.n_points() << std::endl;
  for (int i = 0; i < ps.n_points(); ++i)
  {
    Real3 query = ((Real3*)r_queries)[i];
    for (int j = 0; j < ps.n_neighbors(pointIndex, i); ++j)
    {
      auto neighbor = ps.neighbor(pointIndex, i, j);
      auto diff = query - ((Real3*)r_queries)[neighbor];

      float squaredLength = diff[0] * diff[0] + diff[1] * diff[1] + diff[2] * diff[2];
      float distance = sqrt(squaredLength);

      if (distance > radius)
      {
        throw std::runtime_error("Not a neighbor");  <<<---- this fails for certain queries
      }
    }
  }

The point file (contains the points in the search space) is

0.0,0.0,0.0
9.72500038147,2.72699999809,73.8320007324
5.0,0.0,0.0
9.95300006866,2.6970000267,72.9100036621
6.0,0.0,0.0
9.91199970245,2.71600008011,73.466003418
4.99,4.99,0
9.56000041962,2.54600000381,68.4359970093

And the query file is

0.0,0.0,0.0
5.0,0.0,0.0
6.0,0.0,0.0
4.99,4.99,0

I was wondering if I was doing anything wrong, and could you reproduce the (incorrect) result?

[PerspectivesLab]

let me know if this works better for you

	auto ps = nsearch.point_set(queryIndex);
	auto r_queries = ps.GetPoints();

	auto psPoints = nsearch.point_set(pointIndex); // reference
	auto r_reference = psPoints.GetPoints();

	std::cout << "query points: " << ps.n_points() << std::endl;
	for (int i = 0; i < ps.n_points(); ++i)
	{
		Real3 query = ((Real3*)r_queries)[i];
		std::cout << "query point neighbors: " << ps.n_neighbors(pointIndex, i) << std::endl;
		for (int j = 0; j < ps.n_neighbors(pointIndex, i); ++j)
		{
			auto neighbor = ps.neighbor(pointIndex, i, j);
			//Real3 posNeigh = ((Real3*)r_queries)[neighbor]; <<== wrong ?
			Real3 posNeigh = ((Real3*)r_reference)[neighbor];
			auto diff = query - posNeigh;
			float squaredLength = diff[0] * diff[0] + diff[1] * diff[1] + diff[2] * diff[2];
			float distance = sqrt(squaredLength);

			if (distance <= radius)
			{
				std::cout << "distance: " << distance << std::endl;
			}
		}
	}