depth_error_calibration.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
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()            # (u, v) -> [z]
00029 eps_z = 0.1                    # 10cm
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     # before classifire outputs meaningful value,
00116     # intercept is a list, so we skip the value
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             # publish frequency image
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     # bgr
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     # parse argument
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         #plt.ion()
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()


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Tue Jul 2 2019 19:41:42