hofee/nbv_sim
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Load downsample from rosparam server

Browse Source
This commit is contained in:
Michel Breyer 2021-12-06 14:03:36 +01:00
parent 484483aa4c
commit 486e7fce9b
1 changed files with 7 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/active_grasp/nbv.py
View File

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