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

Minor

Browse Source
This commit is contained in:
Michel Breyer 2021-08-26 11:43:03 +02:00
parent 813e9cd6c5
commit 7a53fddc31
1 changed files with 31 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
active_grasp/nbv.py
View File

@ -1,47 +1,34 @@
import itertools import itertools
import numpy as np import numpy as np
import rospy
from .policy import BasePolicy from .policy import MultiViewPolicy
from vgn.utils import look_at, spherical_to_cartesian from vgn.utils import look_at, spherical_to_cartesian
class NextBestView(BasePolicy): class NextBestView(MultiViewPolicy):
def __init__(self, rate, filter_grasps): def __init__(self, rate):
super().__init__(rate, filter_grasps) super().__init__(rate)
self.max_viewpoints = 20 self.max_views = 20
self.min_gain = 10.0 self.min_ig = 10.0
def activate(self, bbox):
super().activate(bbox)
def update(self, img, extrinsic): def update(self, img, extrinsic):
# Integrate latest measurement self.integrate(img, extrinsic)
self.integrate_img(img, extrinsic) views = self.generate_views()
gains = self.compute_expected_information_gains(views)
# Generate viewpoints costs = self.compute_movement_costs(views)
views = self.generate_viewpoints()
# 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) utilities = gains / np.sum(gains) - costs / np.sum(costs)
# Visualize
self.vis.views(self.base_frame, self.intrinsic, views, utilities) self.vis.views(self.base_frame, self.intrinsic, views, utilities)
# Determine next-best-view
i = np.argmax(utilities) i = np.argmax(utilities)
nbv, g = views[i], gains[i] nbv, ig = views[i], gains[i]
if self.check_done(g): if self.check_stopping_criteria(ig):
self.best_grasp = self.compute_best_grasp()
self.done = True self.done = True
return else:
return nbv.inv() # the controller expects T_cam_base return nbv.inv() # the controller expects T_cam_base
def generate_viewpoints(self): def generate_views(self):
r, h = 0.14, 0.2 r, h = 0.14, 0.2
thetas = np.arange(1, 4) * np.deg2rad(30) thetas = np.arange(1, 4) * np.deg2rad(30)
phis = np.arange(1, 6) * np.deg2rad(60) phis = np.arange(1, 6) * np.deg2rad(60)
@ -53,7 +40,21 @@ class NextBestView(BasePolicy):
views.append(look_at(eye, target, up).inv()) views.append(look_at(eye, target, up).inv())
return views return views
def compute_ig(self, view, downsample=20): def compute_expected_information_gains(self, views):
return [self.ig_fn(v) for v in views]
def compute_movement_costs(self, views):
return [self.cost_fn(v) for v in views]
def check_stopping_criteria(self, ig):
if len(self.views) > self.max_views:
return True
if ig < self.min_ig:
return True
# TODO check whether a "good enough" grasp has been found
return False
def ig_fn(self, view, downsample=20):
fx = self.intrinsic.fx / downsample fx = self.intrinsic.fx / downsample
fy = self.intrinsic.fy / downsample fy = self.intrinsic.fy / downsample
cx = self.intrinsic.cx / downsample cx = self.intrinsic.cx / downsample
@ -114,8 +115,5 @@ class NextBestView(BasePolicy):
return ig return ig
def compute_cost(self, view): def cost_fn(self, view):
return 1.0 return 1.0
def check_done(self, gain):
return len(self.viewpoints) > self.max_viewpoints or gain < self.min_gain