def forward(self, x, coords2d=None):
bx_sm = F.softmax(self.beta * (-x).view(1, -1), dim=1)
if coords2d is None:
coords2d = generate_2dgrid(x.size(2), x.size(3), False)
coords2d_flat = coords2d.view(2, -1)
return (bx_sm.expand_as(coords2d_flat) *
coords2d_flat).sum(dim=1) / bx_sm.sum(dim=1)
def reset(self):
super(ORBSLAM2Agent, self).reset()
self.offset_to_goal = None
self.tracking_is_OK = False
self.waypointPose6D = None
self.unseen_obstacle = False
self.action_history = []
self.planned_waypoints = []
self.map2DObstacles = self.init_map2d()
n, ch, height, width = self.map2DObstacles.size()
self.coordinatesGrid = generate_2dgrid(height, width, False).to(
self.device
)
self.pose6D = self.init_pose6d()
self.action_history = []
self.pose6D_history = []
self.position_history = []
self.planned2Dpath = torch.zeros((0))
self.slam.reset()
self.cur_time = 0
self.toDoList = []
self.waypoint_id = 0
if self.device != torch.device("cpu"):
torch.cuda.empty_cache()
return
def reset(self):
self.tracking_is_OK = False
self.unseen_obstacle = False
self.map2DObstacles = self.init_map2d()
n, ch, height, width = self.map2DObstacles.size()
self.coordinatesGrid = generate_2dgrid(height, width,
False).to(self.device)
self.pose6D = self.init_pose6d()
self.pose6D_history = []
self.position_history = []
self.slam.reset()
self.cur_time = 0
self.toDoList = []
if self.device != torch.device("cpu"):
torch.cuda.empty_cache()
return
def forward(self, x, coords2d=None):
val, idx = x.view(-1).min(dim=0)
if coords2d is None:
coords2d = generate_2dgrid(x.size(2), x.size(3), False)
coords2d_flat = coords2d.view(2, -1)
return coords2d_flat[:, idx].view(2)
