def validatePose(self, mesh : Mesh, pose, error):
''' Given a mesh, pose, and representation of the scene (self.SceneKd), figure out how
good the pose is at describing the scene.
Then return True if its good enough, and False otherwise.
'''
R, o = pose
if R is None or o is None:
return False, None
return error < self.MaxDistanceError, -error
nearbyDistances, nearbyPoints_ind = self.SceneKd.query(o.reshape((1,3)), k = 10000, distance_upper_bound = mesh.Radius)
nearbyPoints_ind = np.array(nearbyPoints_ind)
nearbyPoints = self.Scene[nearbyPoints_ind[nearbyPoints_ind < len(self.Scene)]]
maxPoints = DENSITY * mesh.SurfaceArea
if len(nearbyPoints) < 0.3 * maxPoints:
return False, None
nearbyPoints = (nearbyPoints - o) @ R
distanceToMesh = mesh.distanceQuery(nearbyPoints)
outliers = np.sum(distanceToMesh > error)
inliers = np.sum(np.abs(distanceToMesh) <= error)
# print(outliers, inliers)
return inliers / maxPoints > 0.2, inliers / maxPoints
if outliers > 0:
return False, None
if inliers < 60:
return False, None
return True
