00001
00002 import cv2
00003 import numpy as np
00004 import matplotlib.pyplot as plt
00005 import matplotlib.animation as animation
00006 from jsk_recognition_msgs.msg import DepthErrorResult
00007 from sklearn import linear_model
00008 import time
00009 import threading
00010 import sys
00011 import rospy
00012 import argparse
00013 import csv
00014 import math
00015 import datetime
00016 from jsk_recognition_msgs.srv import SetDepthCalibrationParameter
00017 from jsk_recognition_msgs.msg import DepthCalibrationParameter
00018 from sensor_msgs.msg import Image
00019 from cv_bridge import CvBridge, CvBridgeError
00020
00021 xs = []
00022 raw_xs = []
00023 ys = []
00024 us = []
00025 vs = []
00026 c_us = []
00027 c_vs = []
00028 value_cache = dict()
00029 eps_z = 0.1
00030 lock = threading.Lock()
00031 u_min = None
00032 u_max = None
00033 v_min = None
00034 v_max = None
00035 model = None
00036 set_param = None
00037 MODELS = ["linear", "quadratic", "quadratic-uv", "quadratic-uv-abs", "quadratic-uv-quadratic", "quadratic-uv-quadratic-abs"]
00038
00039 def query_yes_no(question, default=None):
00040 """Ask a yes/no question via raw_input() and return their answer.
00041
00042 "question" is a string that is presented to the user.
00043 "default" is the presumed answer if the user just hits <Enter>.
00044 It must be "yes" (the default), "no" or None (meaning
00045 an answer is required of the user).
00046
00047 The "answer" return value is one of "yes" or "no".
00048 """
00049 valid = {"yes": True, "y": True, "ye": True,
00050 "no": False, "n": False}
00051 if default is None:
00052 prompt = " [y/n] "
00053 elif default == "yes":
00054 prompt = " [Y/n] "
00055 elif default == "no":
00056 prompt = " [y/N] "
00057 else:
00058 raise ValueError("invalid default answer: '%s'" % default)
00059
00060 while True:
00061 sys.stdout.write(question + prompt)
00062 choice = raw_input().lower()
00063 if default is not None and choice == '':
00064 return valid[default]
00065 elif choice in valid:
00066 return valid[choice]
00067 else:
00068 sys.stdout.write("Please respond with 'yes' or 'no' "
00069 "(or 'y' or 'n').\n")
00070
00071
00072 def getXFromFeatureVector(v):
00073 if model == "linear":
00074 return v[0]
00075 elif model == "quadratic":
00076 return v[1]
00077 elif model == "quadratic-uv" or model == "quadratic-uv-abs":
00078 return v[-3]
00079 elif model == "quadratic-uv-quadratic" or model == "quadratic-uv-quadratic-abs":
00080 return v[-5]
00081
00082 def genFeatureVector(x, u, v, cu, cv):
00083 global model
00084 x2 = x * x
00085 u2 = u * u
00086 v2 = v * v
00087 if model == "linear":
00088 return [x]
00089 elif model == "quadratic":
00090 return [x2, x]
00091 elif model == "quadratic-uv":
00092 return [u * x2, v * x2, x2,
00093 u * x, v * x, x,
00094 u, v]
00095 elif model == "quadratic-uv-abs":
00096 u = abs(u - cu)
00097 v = abs(v - cv)
00098 return [u * x2, v * x2, x2,
00099 u * x, v * x, x,
00100 u, v]
00101 elif model == "quadratic-uv-quadratic":
00102 return [u2 * x2, u * x2, v2 * x2, v * x2, x2,
00103 u2 * x, u * x, v2 * x, v * x, x,
00104 u2, u, v2, v]
00105 elif model == "quadratic-uv-quadratic-abs":
00106 u = abs(u - cu)
00107 v = abs(v - cv)
00108 u2 = u * u
00109 v2 = v * v
00110 return [u2 * x2, u * x2, v2 * x2, v * x2, x2,
00111 u2 * x, u * x, v2 * x, v * x, x,
00112 u2, u, v2, v]
00113
00114 def isValidClassifier(classifier):
00115
00116
00117 c0 = classifier.intercept_
00118 return not hasattr(c0, "__len__");
00119
00120 def setParameter(classifier):
00121 global set_param
00122 c = classifier.coef_
00123 c0 = classifier.intercept_
00124 param = DepthCalibrationParameter()
00125 if not isValidClassifier(classifier):
00126 print "parameters are list"
00127 return
00128 if model == "linear":
00129 param.coefficients2 = [0, 0, 0, 0, 0]
00130 param.coefficients1 = [0, 0, 0, 0, c[0]]
00131 param.coefficients0 = [0, 0, 0, 0, c0]
00132 param.use_abs = False
00133 elif model == "quadratic":
00134 param.coefficients2 = [0, 0, 0, 0, c[0]]
00135 param.coefficients1 = [0, 0, 0, 0, c[1]]
00136 param.coefficients0 = [0, 0, 0, 0, c0]
00137 param.use_abs = False
00138 elif model == "quadratic-uv":
00139 param.coefficients2 = [0, c[0], 0, c[1], c[2]]
00140 param.coefficients1 = [0, c[3], 0, c[4], c[5]]
00141 param.coefficients0 = [0, c[6], 0, c[7], c0]
00142 param.use_abs = False
00143 elif model == "quadratic-uv-abs":
00144 param.coefficients2 = [0, c[0], 0, c[1], c[2]]
00145 param.coefficients1 = [0, c[3], 0, c[4], c[5]]
00146 param.coefficients0 = [0, c[6], 0, c[7], c0]
00147 param.use_abs = True
00148 elif model == "quadratic-uv-quadratic":
00149 param.coefficients2 = c[0:5]
00150 param.coefficients1 = c[5:10]
00151 param.coefficients0 = [c[10], c[11], c[12], c[13], c0]
00152 param.use_abs = False
00153 elif model == "quadratic-uv-quadratic-abs":
00154 param.coefficients2 = c[0:5]
00155 param.coefficients1 = c[5:10]
00156 param.coefficients0 = [c[10], c[11], c[12], c[13], c0]
00157 param.use_abs = True
00158 set_param(param)
00159
00160 def processData(x, y, u, v, cu, cv, fit = True):
00161 global xs, ys, classifier, u_min, u_max, v_min, v_max, raw_xs
00162 uu = int(u/10)
00163 vv = int(v/10)
00164 with lock:
00165 if value_cache.has_key((uu, vv)):
00166 zs = value_cache[(uu, vv)]
00167 for z in zs:
00168 if abs(z - y) < eps_z:
00169 print "duplicated value"
00170 return
00171 else:
00172 value_cache[(uu, vv)].append(y)
00173 else:
00174 value_cache[(uu, vv)] = [y]
00175 raw_xs.append(x)
00176 us.append(u)
00177 vs.append(v)
00178 c_us.append(cu)
00179 c_vs.append(cv)
00180 if u > u_min and u < u_max and v < v_max and v > v_min:
00181 print (x, y)
00182 xs.append(genFeatureVector(x, u, v, cu, cv))
00183 ys.append(y)
00184 if fit:
00185 classifier.fit(xs, ys)
00186 try:
00187 setParameter(classifier)
00188 except rospy.service.ServiceException, e:
00189 rospy.logfatal("failed to call service: %s" % (e.message))
00190 try:
00191 print modelEquationString(classifier)
00192 except Exception, e:
00193 rospy.logwarn("failed to print model: %s" % e.message)
00194
00195 else:
00196 print "(%d, %d) is out of range" % (u, v)
00197
00198 def callback(msg):
00199 global xs, ys, classifier, u_min, u_max, v_min, v_max, raw_xs
00200
00201 x = msg.observed_depth
00202 y = msg.true_depth
00203 u = msg.u
00204 v = msg.v
00205 if math.isnan(x) or math.isnan(y):
00206 return
00207 processData(x, y, u, v, msg.center_u, msg.center_v)
00208
00209 def uvCoefString(c, absolute=False):
00210 if absolute:
00211 return "%f|u| + %f|v| + %f" % (c[0], c[1], c[2])
00212 else:
00213 return "%fu + %fv + %f" % (c[0], c[1], c[2])
00214 def uvQuadraticCoefString(c, absolute=False):
00215 if absolute:
00216 return "%f|u|^2 + %f|u| + %f|v|^2 + %f|v| + %f" % (c[0], c[1], c[2], c[3], c[4])
00217 else:
00218 return "%fu^2 + %fu + %fv^2 + %fv + %f" % (c[0], c[1], c[2], c[3], c[4])
00219
00220 def modelEquationString(classifier):
00221 global model, xs, ys
00222 c = classifier.coef_
00223 i = classifier.intercept_
00224 if model == "linear":
00225 return "%fz + %f\n(score: %f)" % (
00226 c[0], i,
00227 classifier.score(xs, ys))
00228 elif model == "quadratic":
00229 return "%fz^2 + %fz + %f\n(score: %f)" % (
00230 c[0],
00231 c[1],
00232 i,
00233 classifier.score(xs, ys))
00234 elif model == "quadratic-uv":
00235 return "(%s)z^2 +\n(%s)z +\n%s\n(score: %f)" % (
00236 uvCoefString(c[0:3]),
00237 uvCoefString(c[3:6]),
00238 uvCoefString([c[6], c[7], i]),
00239 classifier.score(xs, ys))
00240 elif model == "quadratic-uv-abs":
00241 return "(%s)z^2 +\n(%s)z +\n%s\n(score: %f)" % (
00242 uvCoefString(c[0:3], True),
00243 uvCoefString(c[3:6], True),
00244 uvCoefString([c[6], c[7], i], True),
00245 classifier.score(xs, ys))
00246 elif model == "quadratic-uv-quadratic":
00247 return "(%s)z^2 +\n(%s)z +\n%s\n(score: %f)" % (
00248 uvQuadraticCoefString(c[0:5]),
00249 uvQuadraticCoefString(c[5:10]),
00250 uvQuadraticCoefString([c[10], c[11], c[12], c[13], i]),
00251 classifier.score(xs, ys))
00252 elif model == "quadratic-uv-quadratic-abs":
00253 return "(%s)z^2 +\n(%s)z +\n%s\n(score: %f)" % (
00254 uvQuadraticCoefString(c[0:5], True),
00255 uvQuadraticCoefString(c[5:10], True),
00256 uvQuadraticCoefString([c[10], c[11], c[12], c[13], i], True),
00257 classifier.score(xs, ys))
00258
00259 def applyModel(x, u, v, cu, cv, clssifier):
00260 global model
00261 c = classifier.coef_
00262 i = classifier.intercept_
00263 if model == "linear":
00264 return c[0] * x + i
00265 elif model == "quadratic":
00266 return c[0] * x * x + c[1] * x + i
00267 elif model == "quadratic-uv":
00268 return ((c[0] * u + c[1] * v + c[2]) * x * x +
00269 (c[3] * u + c[4] * v + c[5]) * x +
00270 c[6] * u + c[7] * v + i)
00271 elif model == "quadratic-uv-abs":
00272 u = abs(u - cu)
00273 v = abs(v - cv)
00274 return ((c[0] * u + c[1] * v + c[2]) * x * x +
00275 (c[3] * u + c[4] * v + c[5]) * x +
00276 c[6] * u + c[7] * v + i)
00277 elif model == "quadratic-uv-quadratic":
00278 u2 = u * u
00279 v2 = v * v
00280 return ((c[0] * u2 + c[1] * u + c[2] * v2 + c[3] * v + c[4]) * x * x +
00281 (c[5] * u2 + c[6] * u + c[7] * v2 + c[8] * v + c[9]) * x +
00282 (c[10] * u2 + c[11] * u + c[12] * v2 + c[13] * v + i))
00283 elif model == "quadratic-uv-quadratic-abs":
00284 u = abs(u - cu)
00285 v = abs(v - cv)
00286 u2 = u * u
00287 v2 = v * v
00288 return ((c[0] * u2 + c[1] * u + c[2] * v2 + c[3] * v + c[4]) * x * x +
00289 (c[5] * u2 + c[6] * u + c[7] * v2 + c[8] * v + c[9]) * x +
00290 (c[10] * u2 + c[11] * u + c[12] * v2 + c[13] * v + i))
00291
00292
00293 def updatePlot(num):
00294 global xs, ax, width, height
00295 if rospy.is_shutdown():
00296 plt.close()
00297 return
00298 with lock:
00299 if len(xs) == 0:
00300 return
00301 try:
00302 plt.cla()
00303 plt.xlabel('Z from depth image')
00304 plt.ylabel('Z from checker board')
00305 plt.grid(True)
00306 plt.title(model)
00307 plt.scatter([getXFromFeatureVector(x) for x in xs[:-1]],
00308 ys[:-1], s=10, c='b', zorder=10, alpha=0.1)
00309 plt.scatter([getXFromFeatureVector(xs[-1])],
00310 [ys[-1]], s=10, c='r', zorder=10, alpha=1.0)
00311 xmin = np.amin([getXFromFeatureVector(x) for x in xs])
00312 xmax = np.amax([getXFromFeatureVector(x) for x in xs])
00313 X_test = [[xmin * xmin, xmin], [xmax * xmax, xmax]]
00314 X_range = np.linspace(xmin, xmax, 100)
00315 ax.plot(X_range, X_range, linewidth=2, color='green', alpha=0.5)
00316 ax.plot(X_range,
00317 applyModel(X_range,
00318 width / 2, height / 2,
00319 width / 2, height / 2,
00320 classifier),
00321 linewidth=2, color='red', alpha=0.5)
00322 plt.text(xmin, xmax - 0.1,
00323 modelEquationString(classifier),
00324 fontsize=12)
00325
00326 bridge = CvBridge()
00327 img = generateFrequencyMap()
00328 pub_image.publish(bridge.cv2_to_imgmsg(img, "bgr8"))
00329 except Exception, e:
00330 rospy.logerr(e.message)
00331
00332 def generateFrequencyMap():
00333 global width, height
00334
00335 img = np.tile(np.uint8([0,0,0]), (height / 10, width / 10, 1))
00336 frequency = dict()
00337 for (u, v) in value_cache.keys():
00338 min_color = np.uint8([255, 0, 0])
00339 max_color = np.uint8([0, 0, 255])
00340 uu = u
00341 vv = v
00342 if frequency.has_key((uu, vv)):
00343 frequency[(uu, vv)] = frequency[(uu, vv)] + len(value_cache[(u, v)])
00344 else:
00345 frequency[(uu, vv)] = len(value_cache[(u, v)])
00346 for (u, v) in frequency.keys():
00347 r = min(frequency[(u, v)] / 10.0, 1)
00348 img[v, u] = min_color * (1 - r) + max_color * r
00349 return img
00350
00351 def main():
00352 global ax, xs, ys, classifier, u_min, u_max, v_min, v_max, model, set_param
00353 global width, height, pub_image
00354 pub_image = rospy.Publisher("~frequency_image", Image)
00355 set_param = rospy.ServiceProxy("/camera_remote/depth_calibration/set_calibration_parameter",
00356 SetDepthCalibrationParameter)
00357
00358 parser = argparse.ArgumentParser()
00359 parser.add_argument('--csv')
00360 parser.add_argument('--model', default="linear")
00361 parser.add_argument('--models', action="store_true", default=False)
00362 parser.add_argument("--width", default=640)
00363 parser.add_argument("--height", default=480)
00364 args = parser.parse_args(rospy.myargv()[1:])
00365 width = args.width
00366 height = args.height
00367 if args.models:
00368 for m in MODELS:
00369 print m
00370 return
00371 model = args.model
00372 if model not in MODELS:
00373 raise Exception("Unknown Model: %s" % (model))
00374
00375 if not args.csv:
00376 sub = rospy.Subscriber("depth_image_error/output",
00377 DepthErrorResult, callback)
00378
00379 fig = plt.figure()
00380 ax = plt.axes([.12, .12, .8, .8])
00381 anim = animation.FuncAnimation(fig, updatePlot)
00382 classifier = linear_model.LinearRegression()
00383 u_min = rospy.get_param("~u_min", 0)
00384 u_max = rospy.get_param("~u_max", 4096)
00385 v_min = rospy.get_param("~v_min", 0)
00386 v_max = rospy.get_param("~v_max", 4096)
00387
00388 if args.csv:
00389 for row in csv.reader(open(args.csv, "rb")):
00390 x = float(row[0])
00391 y = float(row[1])
00392 u = float(row[2])
00393 v = float(row[3])
00394 cu = float(row[4])
00395 cv = float(row[5])
00396 processData(x, y, u, v, cu, cv, fit = False)
00397 classifier.fit(xs, ys)
00398 try:
00399 setParameter(classifier)
00400 except rospy.service.ServiceException, e:
00401 rospy.logfatal("failed to call service: %s" % (e.message))
00402 try:
00403 plt.show()
00404 finally:
00405 if not args.csv:
00406 csv_filename = "calibration-%s.csv" % datetime.datetime.now().strftime('%Y-%m-%d-%H-%M-%S')
00407 print "Save calibration parameters to %s" % (csv_filename)
00408 with open(csv_filename, "w") as f:
00409 for x, y, u, v, cu, cv in zip(raw_xs, ys, us, vs, c_us, c_vs):
00410 f.write("%f,%f,%d,%d,%f,%f\n" % (x, y, u, v, cu, cv))
00411 if query_yes_no("Dump result into yaml file?"):
00412 yaml_filename = "calibration_parameter_%s.yaml" % datetime.datetime.now().strftime('%Y_%m_%d_%H_%M_%S')
00413 print "writing to %s" % yaml_filename
00414 c = classifier.coef_
00415 if model == "quadratic-uv-abs" or model == "quadratic-uv-quadratic-abs":
00416 use_abs = "True"
00417 else:
00418 use_abs = "False"
00419
00420 with open(yaml_filename, "w") as f:
00421 f.write("""
00422 coefficients2: [%s, %s, %s, %s, %s]
00423 coefficients1: [%s, %s, %s, %s, %s]
00424 coefficients0: [%s, %s, %s, %s, %s]
00425 use_abs: %s
00426 """ % (
00427 repr(c[0]), repr(c[1]), repr(c[2]), repr(c[3]), repr(c[4]),
00428 repr(c[5]), repr(c[6]), repr(c[7]), repr(c[8]), repr(c[9]),
00429 repr(c[10]), repr(c[11]), repr(c[12]), repr(c[13]), repr(classifier.intercept_),
00430 use_abs))
00431
00432 if __name__ == "__main__":
00433 rospy.init_node("depth_error_logistic_regression")
00434 main()