~estraw/visual-odometry

2cf6be51661a8451b57b31e75f6b330321f946e8 — Evan Straw 3 months ago 2deb962 master
Add code to graph error histograms
1 files changed, 65 insertions(+), 3 deletions(-)

M src/make_graphs.py
M src/make_graphs.py => src/make_graphs.py +65 -3
@@ 11,7 11,8 @@ import time

DATASET_DIR: str = "/tmp/KITTI/"
SEQ = 0
N = 500
N = 1000
N2 = 2000
T_STEP = 0.025
T_MIN = 0
T_MAX = 1


@@ 53,8 54,40 @@ def avgErrorForNFrames(seq: int, n: int, thresh: float):
            n_failed)


if __name__ == "__main__":
    start_time = time.time()
def errorForNFrames(seq: int, n: int, thresh: float):
    frame_nos = np.arange(0, n, 1)
    seq_str: str = "%02d" % seq
    cap: vi.VideoInput = vi.KITTIDatasetInput(DATASET_DIR, seq_str)
    if not cap.hasGroundTruth:
        raise ValueError("Sequence %s does not have ground truth data"
                         % seq_str)
    vo: VisualOdometry = VisualOdometry(cap.P1, cap.P2)
    first: bool = False
    errs = ([], [])
    for i in trange(n, desc="thresh=%.2f" % thresh, leave=False,
                    mininterval=0.5):
        if not cap.grab():
            print("Error: could not open sequence %s under %s"
                  % (seq_str, DATASET_DIR), file=sys.stderr)
            sys.exit(1)
        frame = cap.retrieve()
        if not first:
            vo.start(frame)
            first = True
        else:
            rvec, tvec = vo.compute(frame, thresh)
            if rvec is None:
                frame_nos = np.delete(frame_nos, i)
                continue
            rvec_real, tvec_real = cap.poseDelta
            r_err, t_err = vo.computeError(rvec, tvec, rvec_real, tvec_real)
            errs[0].append(r_err)
            errs[1].append(t_err)

    return frame_nos, (np.array(errs[0]), np.array(errs[1]))


def threshGraphs():
    r_errs = np.array([])
    t_errs = np.array([])
    threshes = np.arange(T_MIN, T_MAX+T_STEP, T_STEP)


@@ 105,5 138,34 @@ if __name__ == "__main__":
    fail_vs_thresh.savefig('fail_vs_thresh.png',
                           dpi=300, bbox_inches='tight')


def errorGraphs():
    frame_nos, (r_errs, t_errs) = errorForNFrames(SEQ, N2, 0.5)
    r_errs *= (180 / math.pi)
    r_error_hist = plt.figure()
    r_ax = r_error_hist.add_subplot(111)
    r_ax.set_xlabel("Rotation error (degrees)")
    r_ax.set_ylabel("Number of frames")
    r_ax.set_title("Rotation error for %d frames" % N2)
    r_ax.hist(r_errs, bins=30)

    t_error_hist = plt.figure()
    t_ax = t_error_hist.add_subplot(111)
    t_ax.set_xlabel("Translation error (meters)")
    t_ax.set_ylabel("Number of frames")
    t_ax.set_title("Translation error for %d frames" % N2)
    t_ax.hist(t_errs, bins=30)

    r_error_hist.savefig('r_error_dist.png',
                         dpi=300, bbox_inches='tight')
    t_error_hist.savefig('t_error_dist.png',
                         dpi=300, bbox_inches='tight')


if __name__ == "__main__":
    start_time = time.time()
    threshGraphs()
    errorGraphs()

    secs = int(time.time() - start_time)
    print(" Done! Took %d seconds" % secs)