Currently, the way the initial closest face of the robot is found is suboptimal, as we search with a kd-tree over the vertices. This leads to problems in the situation shown in the attached image, as it would find the tip of the "stalactite" as the closest vertex to the robot. The solution proposed in #103 improves the locally wrong assignments but still does not cover the case from the attached image.
Proposed solution: Embree can perform an actual closest-point-to-surface search, accelerated by the underlying BVH. For reference, I used this in RMCL for the optimal closest-point correspondences. Using the same approach here would solve the problem sketched here right away, while staying backwards compatible in all other situations. We currently use the raycasting interface of lvr2. To solve this properly, we would have to extend this interface with a closest-point query, which involves implementing closest-point queries for both the Embree and the fallback implementation.
Idea to start quickly: for the fallback-raycaster implementation, use the same vertex-based search as the initial implementation. From there, we can optimize.
Currently, the way the initial closest face of the robot is found is suboptimal, as we search with a kd-tree over the vertices. This leads to problems in the situation shown in the attached image, as it would find the tip of the "stalactite" as the closest vertex to the robot. The solution proposed in #103 improves the locally wrong assignments but still does not cover the case from the attached image.
Proposed solution: Embree can perform an actual closest-point-to-surface search, accelerated by the underlying BVH. For reference, I used this in RMCL for the optimal closest-point correspondences. Using the same approach here would solve the problem sketched here right away, while staying backwards compatible in all other situations. We currently use the raycasting interface of lvr2. To solve this properly, we would have to extend this interface with a closest-point query, which involves implementing closest-point queries for both the Embree and the fallback implementation.
Idea to start quickly: for the fallback-raycaster implementation, use the same vertex-based search as the initial implementation. From there, we can optimize.