dump_depth_error.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import rospy
00004 from jsk_recognition_msgs.msg import DepthErrorResult, PlotData
00005 import numpy as np
00006 import csv
00007 
00008 grid = 0.1                     # 10 cm
00009 bins = {}
00010 
00011 data = PlotData()
00012 
00013 def callback(msg):
00014     bin_index = int(msg.true_depth / grid)
00015     if not bins.has_key(bin_index):
00016         bins[bin_index] = []
00017     err = msg.true_depth - msg.observed_depth
00018     bins[bin_index].append((msg, err))
00019     for bin_index, msg_and_errs in bins.items():
00020         if len(msg_and_errs) < 5:
00021             continue
00022         print "Bin: {0}m ~ {1}m".format(grid * bin_index, grid * (bin_index + 1))
00023         print "   Sample:", len(msg_and_errs)
00024         errs = [err for (msg, err) in msg_and_errs]
00025         mean = np.mean(errs, axis=0)
00026         stddev = np.std(errs, axis=0)
00027         print "   Mean:", mean
00028         print "   Stddev:", stddev
00029     data.xs.append(msg.true_depth)
00030     data.ys.append(msg.observed_depth)
00031     pub.publish(data)
00032     writer.writerow([msg.true_depth, msg.observed_depth])
00033         
00034 
00035 if __name__ == "__main__":
00036     rospy.init_node("plot_depth_error")
00037     pub = rospy.Publisher("~scatter", PlotData)
00038     f = open("output.csv", "w")
00039     writer = csv.writer(f)
00040     sub = rospy.Subscriber("/depth_image_error/output", DepthErrorResult, callback)
00041     rospy.spin()
00042     


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