def Cast_Votes(self, results, distances, patch_real_centers):
self.votes = {
'Weigth': [],
'RealPosition': [],
'ProjectedPoint': [],
'Orientation': [],
'Obj_id': []
}
for patch, neighboor in zip(*np.where(
distances <= self.Neighboors_distance_threshold)):
reading = self.CodeBookData[results[patch, neighboor]]
self.votes['RealPosition'].extend(reading['Translation'] +
patch_real_centers[patch])
self.votes['Orientation'].extend(reading['euler_angles'])
self.votes['Obj_id'].append(reading['obj_ids'])
#weight = np.exp(-distances[patch,neighboor]) #from original papper
#Modificacao
ratio = self.Neighboors_distance_threshold / np.abs(
np.log(0.0001)) #Votos no threshold tem weight 0.001
weight = np.exp(
-distances[patch, neighboor] /
ratio) #tentativa de dar mais peso a distancia entre features
self.votes['Weigth'].append(weight)
#Project points
if len(self.votes['RealPosition']) > 0:
self.votes['ProjectedPoint'] = pat.from_3D_to_image(
points=self.votes['RealPosition'], Intrins=self.Intrins)
def Cast_Votes(self, Positions, Orientations):
self.votes_regist = {
'Position': [],
'Orientation': [],
'Projection': []
}
self.votes_regist['Position'] = Positions
self.votes_regist['Orientation'] = Orientations
self.votes_regist['Projection'] = pat.from_3D_to_image(
points=Positions, Intrins=self.Intrins)[:, :2]
tmp_depth = copy.deepcopy(Kinect.imgDepth) #copy
tmp_rgbd = Kinect.get_RGBD()
#Estimate Objects in image------------------------------------
start = time.time()
position_predictions, rotation_predictions = Detector.Estimate_Objects_6DOF(
RGBDimage=tmp_rgbd)
if (not np.any(position_predictions)) or (
not np.any(rotation_predictions)):
#In case there are no predictions
print 'Bad sample!'
continue
end = time.time()
#project positions to image:
projected = pat.from_3D_to_image(position_predictions)
#Transform to Baxter referencial:
Baxter_pred = pat.from_CAM_to_WS(points_on_CAM=position_predictions,
CAM_on_WS=Detector.Extrins)
#===============================================================
#----------------------- Visualize -----------------------------
#===============================================================
#AXES 1-----------------------------
axes1.set_title('RGB image')
axes1.axis('off')
axes1.imshow(tmp_rgb)
for proj in projected:
sct1 = axes1.scatter(proj[0], proj[1])
#AXES 2-----------------------------