dump_depth_error.py

Go to the documentation of this file.

#!/usr/bin/env python

import rospy
from jsk_recognition_msgs.msg import DepthErrorResult, PlotData
import numpy as np
import csv

grid = 0.1                     # 10 cm
bins = {}

data = PlotData()

def callback(msg):
    bin_index = int(msg.true_depth / grid)
    if not bins.has_key(bin_index):
        bins[bin_index] = []
    err = msg.true_depth - msg.observed_depth
    bins[bin_index].append((msg, err))
    for bin_index, msg_and_errs in bins.items():
        if len(msg_and_errs) < 5:
            continue
        print "Bin: {0}m ~ {1}m".format(grid * bin_index, grid * (bin_index + 1))
        print "   Sample:", len(msg_and_errs)
        errs = [err for (msg, err) in msg_and_errs]
        mean = np.mean(errs, axis=0)
        stddev = np.std(errs, axis=0)
        print "   Mean:", mean
        print "   Stddev:", stddev
    data.xs.append(msg.true_depth)
    data.ys.append(msg.observed_depth)
    pub.publish(data)
    writer.writerow([msg.true_depth, msg.observed_depth])
        

if __name__ == "__main__":
    rospy.init_node("plot_depth_error")
    pub = rospy.Publisher("~scatter", PlotData)
    f = open("output.csv", "w")
    writer = csv.writer(f)
    sub = rospy.Subscriber("/depth_image_error/output", DepthErrorResult, callback)
    rospy.spin()