Draw colored camera views in rviz

2021-08-16 17:38:17 +02:00
parent 4b4d54240b
commit ac5c490ae8
3 changed files with 68 additions and 7 deletions
--- a/active_grasp/nbv.py
+++ b/active_grasp/nbv.py
@@ -30,13 +30,15 @@ class NextBestView(BasePolicy):

        # Generate viewpoints
        views = self.generate_viewpoints()
-        self.visualizer.views(views)

        # Evaluate viewpoints
        gains = [self.compute_ig(v) for v in views]
        costs = [self.compute_cost(v) for v in views]
        utilities = gains / np.sum(gains) - costs / np.sum(costs)

+        # Visualize
+        self.visualizer.views(self.intrinsic, views, utilities)
+
        # Determine next-best-view
        nbv = views[np.argmax(utilities)]