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

Move on a half sphere

Browse Source
This commit is contained in:
Michel Breyer
2021-09-11 20:49:55 +02:00
parent 4ebd587553
commit 65bdb5422d
7 changed files with 101 additions and 126 deletions
Download Patch File Download Diff File Expand all files Collapse all files

66
active_grasp/nbv.py
View File

@@ -2,59 +2,40 @@ import itertools
import numpy as np
import rospy
from .policy import MultiViewPolicy, compute_error
from vgn.utils import look_at
from .policy import MultiViewPolicy
class NextBestView(MultiViewPolicy):
def __init__(self, rate):
super().__init__(rate)
self.max_views = 20
self.min_ig = 10.0
self.cost_factor = 10.0
def __init__(self):
super().__init__()
self.max_views = 40
def activate(self, bbox):
super().activate(bbox)
def activate(self, bbox, view_sphere):
super().activate(bbox, view_sphere)
self.generate_view_candidates()
def update(self, img, x):
if len(self.views) > self.max_views:
self.done = True
return np.zeros(6)
else:
self.integrate(img, x)
views = self.view_candidates
gains = self.compute_expected_information_gains(views)
costs = self.compute_movement_costs(views)
utilities = gains / np.sum(gains) - costs / np.sum(costs)
self.vis.views(self.base_frame, self.intrinsic, views, utilities)
i = np.argmax(utilities)
nbv, ig = views[i], gains[i]
cmd = self.compute_velocity_cmd(*compute_error(nbv, x))
if self.best_grasp:
R, t = self.best_grasp.pose.rotation, self.best_grasp.pose.translation
if np.linalg.norm(t - x.translation) < self.min_z_dist:
self.done = True
return np.zeros(6)
center = t
eye = R.apply([0.0, 0.0, -0.2]) + t
up = np.r_[1.0, 0.0, 0.0]
x_d = look_at(eye, center, up)
cmd = self.compute_velocity_cmd(*compute_error(x_d, x))
return cmd
def generate_view_candidates(self):
thetas = np.arange(1, 4) * np.deg2rad(30)
phis = np.arange(1, 6) * np.deg2rad(60)
self.view_candidates = []
for theta, phi in itertools.product(thetas, phis):
view = self.view_sphere.get_view(theta, phi)
if self.is_view_feasible(view):
if self.view_sphere.is_feasible(view):
self.view_candidates.append(view)
def compute_expected_information_gains(self, views):
return [self.ig_fn(v) for v in views]
def update(self, img, x):
if len(self.views) > self.max_views:
self.done = True
else:
self.integrate(img, x)
views = self.view_candidates
gains = [self.ig_fn(v) for v in views]
costs = [self.cost_fn(v) for v in views]
utilities = gains / np.sum(gains) - costs / np.sum(costs)
self.vis.views(self.base_frame, self.intrinsic, views, utilities)
i = np.argmax(utilities)
nbv, _ = views[i], gains[i]
self.x_d = nbv
def ig_fn(self, view, downsample=20):
fx = self.intrinsic.fx / downsample
@@ -88,18 +69,14 @@ class NextBestView(MultiViewPolicy):
origin = view.translation
direction = np.r_[(u - cx) / fx, (v - cy) / fy, 1.0]
direction = view.rotation.apply(direction / np.linalg.norm(direction))
# self.vis.rays(self.task_frame, origin, [direction])
# rospy.sleep(0.01)
t, tsdf_prev = t_min, -1.0
while t < t_max:
p = origin + t * direction
t += t_step
# self.vis.point(self.task_frame, p)
# rospy.sleep(0.01)
index = get_voxel_at(p)
if index is not None:
i, j, k = index
@@ -117,8 +94,5 @@ class NextBestView(MultiViewPolicy):
return ig
def compute_movement_costs(self, views):
return [self.cost_fn(v) for v in views]
def cost_fn(self, view):
return 1.0