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)
