def __init__(self, _): super(CalibrateVision, self).__init__() self.exportLog = ExportLog("CalibrateVision.log") self.execCount = 0 return
def __dbscanCalcAndNeuralNetFilter(self, data): """ Start dbscan calculation to filter raw data, initialize the neural net to filter/calibrate data :param data: BitBots data object """ ##################### ### initilization ### ##################### post1u_filtered = None post1v_filtered = None post2u_filtered = None post2v_filtered = None goalcenteru = None goalcenterv = None goalcenterunfilteredu = None goalcenterunfilteredv = None ######################### ### DBSCAN parameters ### ######################### minPoints = 5 # minimum number of points to build a cluster epsilon = 75.0 # range to search for neighboring points ############## ### Post 1 ### ############## if len(self.dbscanBufferPost1) > 0: # Post 1 - raw u, v values post1rawx = numpy.array([elem["u"] for elem in self.dbscanBufferPost1]) post1rawv = numpy.array([elem["v"] for elem in self.dbscanBufferPost1]) # Post 1 - create a DBScan object and start calculation post1scan = DBScan(post1rawx, post1rawv, minPoints, epsilon) # Post 1 - extract filtered data out of the DBScan object post1u_values, post1v_values = post1scan.getFilteredData() # Post 1 - calculate the mean out of the filtered data post1u_filtered = numpy.mean(post1u_values) post1v_filtered = numpy.mean(post1v_values) ############## ### Post 2 ### ############## if len(self.dbscanBufferPost2) > 0: # Post 2 - raw u, v values post2rawx = numpy.array([elem["u"] for elem in self.dbscanBufferPost2]) post2rawv = numpy.array([elem["v"] for elem in self.dbscanBufferPost2]) # Post 2 - create a DBScan object and start calculation post2scan = DBScan(post2rawx, post2rawv, minPoints, epsilon) # Post 2 - extract filtered data out of the DBScan object post2u_values, post2v_values = post2scan.getFilteredData() # Post 2 - calculate the mean out of the filtered data #todo hier kommen irgendwo warnings wegen aufruf von mean auf was leerem post2u_filtered = numpy.mean(post2u_values) post2v_filtered = numpy.mean(post2v_values) ################### ### Goal center ### ################### if (post1u_filtered is not None) and\ (post1v_filtered is not None) and\ (post2u_filtered is not None) and\ (post2v_filtered is not None): goalcenteru = (post1u_filtered + post2u_filtered) / 2.0 goalcenterv = (post1v_filtered + post2v_filtered) / 2.0 ####################### ### Log/debug stuff ### ####################### if self.pipeDebugIntoFile: exportLog = ExportLog("GoalDBScan.log") exportData = OrderedDict() x1, y1, u1, v1 = data[DATA_KEY_GOAL_INFO][0] # collect current raw values for debugging/logging - post 1 x2, y2, u2, v2 = data[DATA_KEY_GOAL_INFO][1] # collect current raw values for debugging/logging - post 2 goalcenterunfilteredu = (u1 + u2) / 2.0 goalcenterunfilteredv = (v1 + v2) / 2.0 exportData["post1_u_raw"] = str(u1) exportData["post1_v_raw"] = str(v1) exportData["post2_u_raw"] = str(u2) exportData["post2_v_raw"] = str(v2) exportData["post1_u_filtered"] = str(post1u_filtered) exportData["post1_v_filtered"] = str(post1v_filtered) exportData["post2_u_filtered"] = str(post2u_filtered) exportData["post2_v_filtered"] = str(post2v_filtered) exportData["goalcenter_u_filtered"] = str(goalcenteru) exportData["goalcenter_v_filtered"] = str(goalcenterv) exportData["goalcenter_u_unfiltered"] = str(goalcenterunfilteredu) exportData["goalcenter_v_unfiltered"] = str(goalcenterunfilteredv) exportLog.addDataRecord(exportData) exportLog.writeLog() ###################################### ### Goal information - named tuple ### ###################################### # prepare an object for filtered information goalinfo = namedtuple("goalinfo", ["u_center", "v_center", "u_post1", "v_post1", "u_post2", "v_post2", "time"]) ########################################## ### Neural net - calibration/filtering ### ########################################## #inputLength = 2 # input length of input layer #outputLength = 2 # output length of output layer #hiddenLayer = [10, 5] # hidden layer configuration (amount of hidden layers & neurons for each layer) # Create the neural net (MLP) #net = NeuralNetworkClass.NeuralNetwork(inputLength, hiddenLayer, outputLength) # Load saved configuration (weights & bias) #net.loadWeightsAndBias("/home/darwin/logfiles/") # filter/calibrate the goal center information via the neural net #goalcenteru, goalcenterv = net.calculate([goalcenteru/10000.0, goalcenterv/10000.0])["output"] # normalize the goal information again #goalcenteru *= 10000 #goalcenterv *= 10000 ######################### ### Set filtered data ### ######################### data[DATA_KEY_GOAL_INFO_FILTERED] = goalinfo(u_center=goalcenteru, v_center=goalcenterv, u_post1=post1u_filtered, v_post1=post1v_filtered, u_post2=post2u_filtered, v_post2=post2v_filtered, time=time.time()) debug_m(3, "goal_center_u", str(goalcenteru)) debug_m(3, "goal_center_v", str(goalcenterv)) debug_m(3, "post1_u", str(post1u_filtered)) debug_m(3, "post1_v", str(post1v_filtered)) debug_m(3, "post2_u", str(post2u_filtered)) debug_m(3, "post2_v", str(post2v_filtered))
class CalibrateVision(AbstractDecisionModule): """ Records data for vision calibration purposes """ def __init__(self, _): super(CalibrateVision, self).__init__() self.exportLog = ExportLog("CalibrateVision.log") self.execCount = 0 return def perform(self, connector, reevaluate=False): connector.walking_capsule().stop_walking() # If interrupt from throw comes here we want to wait until its over if connector.animation_capsule().is_animation_busy(): return # record data goal_info_filtered = connector.filtered_vision_capsule().get_local_goal_filtered() goal_info_unfiltered = connector.raw_vision_capsule().get_goal_infos() # stop if no data is recorded if goal_info_filtered is None or goal_info_unfiltered is None: return # return if only one post is seen if len(goal_info_unfiltered) < 2: return # build log data set export_data = OrderedDict() self.execCount += 1 export_data["execCount"] = self.execCount export_data["post1_u_raw"] = goal_info_unfiltered[0].u export_data["post1_v_raw"] = goal_info_unfiltered[0].v export_data["post2_u_raw"] = goal_info_unfiltered[1].u export_data["post2_v_raw"] = goal_info_unfiltered[1].v goalcenter_unfiltered_u = (goal_info_unfiltered[0].u + goal_info_unfiltered[1].u) / 2.0 goalcenter_unfiltered_v = (goal_info_unfiltered[0].v + goal_info_unfiltered[0].v) / 2.0 export_data["center_u_raw"] = goalcenter_unfiltered_u export_data["center_v_raw"] = goalcenter_unfiltered_v export_data["post1_u_filtered"] = goal_info_filtered.u_post1 export_data["post1_v_filtered"] = goal_info_filtered.v_post1 export_data["post2_u_filtered"] = goal_info_filtered.u_post2 export_data["post2_v_filtered"] = goal_info_filtered.v_post2 export_data["center_u_filtered"] = goal_info_filtered.u_center export_data["center_v_filtered"] = goal_info_filtered.v_center # add data set to buffer self.exportLog.addDataRecord(export_data) # write data self.exportLog.writeLog() return self.push(Wait)