From 486e7fce9be93b4fe8cd4d4d92ed54196ecd64e4 Mon Sep 17 00:00:00 2001
From: Michel Breyer <10465414+mbreyer@users.noreply.github.com>
Date: Mon, 6 Dec 2021 14:03:36 +0100
Subject: [PATCH] Load downsample from rosparam server

---
 src/active_grasp/nbv.py | 8 +++++++-
 1 file changed, 7 insertions(+), 1 deletion(-)

diff --git a/src/active_grasp/nbv.py b/src/active_grasp/nbv.py
index 443db27..2b61b0c 100644
--- a/src/active_grasp/nbv.py
+++ b/src/active_grasp/nbv.py
@@ -56,10 +56,14 @@ def raycast(
 class NextBestView(MultiViewPolicy):
     def __init__(self):
         super().__init__()
+        self.load_parameters()
         self.compile()
+
+    def load_parameters(self):
         self.min_z_dist = rospy.get_param("~camera/min_z_dist")
         self.max_views = rospy.get_param("nbv_grasp/max_views")
         self.min_gain = rospy.get_param("nbv_grasp/min_gain")
+        self.downsample = rospy.get_param("nbv/downsample")
 
     def compile(self):
         # Trigger the JIT compilation
@@ -93,7 +97,7 @@ class NextBestView(MultiViewPolicy):
             with Timer("view_generation"):
                 views = self.generate_views(q)
             with Timer("ig_computation"):
-                gains = [self.ig_fn(v, 10) for v in views]
+                gains = [self.ig_fn(v, self.downsample) for v in views]
             with Timer("cost_computation"):
                 costs = [self.cost_fn(v) for v in views]
             utilities = gains / np.sum(gains) - costs / np.sum(costs)
@@ -135,6 +139,8 @@ class NextBestView(MultiViewPolicy):
         cx = self.intrinsic.cx / downsample
         cy = self.intrinsic.cy / downsample
 
+        print(fx)
+
         # Project bbox onto the image plane to get better bounds
         T_cam_base = view.inv()
         corners = np.array([T_cam_base.apply(p) for p in self.bbox.corners]).T