diff --git a/scripts/tracking_eval_plot.py b/scripts/tracking_eval_plot.py index 4c53fc8..caa9173 100644 --- a/scripts/tracking_eval_plot.py +++ b/scripts/tracking_eval_plot.py @@ -15,6 +15,7 @@ if not os.path.isdir(args.results_path) and args.results_path.endswith('.hdf5'): with h5py.File(args.results_path, 'r') as f: failures = f['failures'][:] + successes = f['successes'][:] radial_errors = f['radial_errors'][:] length_errors = f['length_errors'][:] track_counts = f['track_counts'][:] @@ -29,21 +30,25 @@ ax4 = fig.add_subplot(5, 1, 4) ax5 = fig.add_subplot(5, 1, 5) - binned_errors = [[0] for i in range(10)] - for r, e in radial_errors: - if e < 50: - binned_errors[int(min(r, 0.499999) / 0.05)].append(e) - ax1.set_title('Pixel error distribution for various radial distances') - ax1.set_ylabel('Pixel error') - ax1.set_xlabel('Radial distance') - ax1.set_xticks(np.arange(1, 11)) - ax1.set_xticklabels(['{}-{}'.format(i * 0.05, (i + 1) * 0.05) for i in range(10)]) - ax1.set_ylim([0, 20]) - ax1.violinplot(binned_errors) - ax1.plot([i for i in range(1, 11)], [np.array(binned_errors[i]).mean() for i in range(10)], '-o', markersize=4, c='black') + df = pandas.DataFrame() + re_filt = radial_errors[np.where((radial_errors[:, 1] < 50) & (radial_errors[:, 2] > 0))] + df = df.append(pandas.DataFrame({'Radial distance': np.round(re_filt[:, 0] / 0.005) * 0.005, 'Relative endpoint error': re_filt[:, 2]})) + # binned_errors = [[0] for i in range(30)] + # for r, aee, ree, ae, rbe in radial_errors: + # if aee < 50: + # binned_errors[int(r / 0.025)].append(aee) + # ax1.set_title('Pixel error distribution for various radial distances') + # ax1.set_ylabel('Pixel error') + # ax1.set_xlabel('Radial distance') + # ax1.set_xticks(np.arange(1, 22)) + # ax1.set_xticklabels(['{}-{}'.format(i * 0.025, (i + 1) * 0.025) for i in range(21)]) + # ax1.set_ylim([0, 10]) + # ax1.violinplot(binned_errors[:21]) + # ax1.plot([i for i in range(1, 22)], [np.array(binned_errors[i]).mean() for i in range(21)], '-o', markersize=4, c='black') + sns.relplot(x="Radial distance", y="Relative endpoint error", kind="line", data=df, ci="sd", ax=ax1) ax2.set_title('Pixel errors over track lifetime') - ax2.set_ylim([0, 30]) + ax2.set_ylim([0, 0.1]) ax2.set_ylabel('Pixel error') ax2.set_xlabel('Frame') err = dict() @@ -54,7 +59,7 @@ err[row[1]] = [] if row[1] not in rs: rs[row[1]] = [] - err[row[1]].append((row[0], row[3])) + err[row[1]].append((row[0], row[4])) rs[row[1]].append((row[2])) if row[1] > num_tracks: num_tracks = int(row[1]) @@ -71,11 +76,11 @@ for j in range(0, len(e) - 1): ax2.plot([e[j][0], e[j+1][0]], [e[j][1], e[j+1][1]], color=np.squeeze(colors[j])) i += int(num_tracks / 2000) - tl_err = [(tl, e) for tl, _, _, e in length_errors] + tl_err = [(tl, be) for tl, _, _, e, be in length_errors] lim = int(max(tl_err, key=lambda item:item[0])[0]) binned_errors = [[] for i in range(lim + 1)] - for tl, _, _, e in length_errors: - binned_errors[int(tl)].append(e) + for tl, _, _, e, be in length_errors: + binned_errors[int(tl)].append(be) ax2.plot([i for i in range(0, lim)], [np.array(binned_errors[i]).mean() for i in range(0, lim)], '-o', markersize=4, c='black') ax3.set_title('Number of tracks over frames') @@ -93,17 +98,34 @@ ax4.axvline(track_lengths.mean(), color='k', linestyle='dashed', linewidth=1) ax4.axvline(np.median(track_lengths), color='k', linestyle='dotted', linewidth=1) - ax5.set_title('Number of failures at various radial distances') + ax5.set_title('Ratio of inliers over radial distances') ax5.set_xlabel('Radial distance') - ax5.set_ylabel('Count') - ax5.hist([r for r in failures], bins=[0.0125 * a for a in range(0, 41)], color='palegreen') + ax5.set_ylabel('Ratio of inliers') + lim = int(successes.max(axis=0)[1]) + binned_inliers = [[0 for j in range(50)] for i in range(lim)] + binned_outliers = [[0 for j in range(50)] for i in range(lim)] + for r, f in successes: + binned_inliers[int(f) - 1][int(r / 0.0125)] += 1 + for r, f in failures: + binned_outliers[int(f) - 1][int(r / 0.0125)] += 1 + binned_ratio = [0. for i in range(lim)] + for r in range(50): + for f in range(lim): + total = binned_outliers[f][r] + binned_inliers[f][r] + if total > 0: + binned_ratio[r] += binned_inliers[f][r] / float(total) + binned_ratio[r] /= lim + # ax5.hist([r for r, _ in failures], bins=[0.0125 * a for a in range(0, 43)], color='palegreen') + ax5.bar([0.0125 * a for a in range(0, 43)], [binned_ratio[a] for a in range(43)], 0.0125) plt.show() elif os.path.isdir(args.results_path): fig1 = plt.figure(1) fig2 = plt.figure(2) + # fig3 = plt.figure(3) fig1.suptitle('KLT Tracking Accuracy for Various FOVs and Motions') fig2.suptitle('KLT Tracking Lifetime Distributions for Various FOVs and Motions') + # fig3.suptitle('KLT Tracking Error Distribution Over Radial Distance for Various FOVs and Motions') motion_count = 0 fov_dict = dict() last_fov_num = 0 @@ -141,21 +163,24 @@ num_rows = motion_count num_cols = last_fov_num motion_inx = 0 + df_ee = pandas.DataFrame() + df_ae = pandas.DataFrame() + df_be = pandas.DataFrame() for motion in os.listdir(args.results_path): if os.path.isdir(os.path.join(args.results_path, motion)): - df = pandas.DataFrame() + df_lifetime = pandas.DataFrame() bag_dir = os.path.join(args.results_path, motion) motion_exists = False for fovstr in fovs: - failures = np.empty(shape=(1,0)) - radial_errors = np.empty(shape=(0,2)) + failures = np.empty(shape=(0,2)) + radial_errors = np.empty(shape=(0,5)) track_lengths = np.empty(shape=(1,0)) file_exists = False for filename in os.listdir(bag_dir): if filename.endswith(fovstr + '.tracking.hdf5'): results_file = os.path.join(bag_dir, filename) with h5py.File(results_file, 'r') as f: - failures = np.hstack((failures, f['failures'][:])) + failures = np.vstack((failures, f['failures'][:])) radial_errors = np.vstack((radial_errors, f['radial_errors'][:])) tl = f['track_lengths'][:] track_lengths = np.hstack((track_lengths, tl[:, tl[0, :] > 0])) @@ -166,7 +191,7 @@ fov = int(fovstr) if file_exists: ax = fig1.add_subplot(num_rows, num_cols, motion_inx * num_cols + fov_dict[fov] + 1) - ax.hist([[r for r, e in radial_errors if e <= 5], [r for r, e in radial_errors if 5 < e <= 20], [r for r, e in radial_errors if 20 < e <= 50], [r for r in failures]], bins=[i * 0.05 for i in range(11)], alpha=0.5, label=['<5', '5-20', '20-50', 'Failures'], stacked=False) + ax.hist([[r for r, aee, ree, ae, rbe in radial_errors if aee <= 5], [r for r, aee, ree, ae, rbe in radial_errors if 5 < aee <= 20], [r for r, aee, ree, ae, rbe in radial_errors if 20 < aee <= 50], [r for r, _ in failures]], bins=[i * 0.05 for i in range(11)], alpha=0.5, label=['<5', '5-20', '20-50', 'Failures'], stacked=False) ax.legend(loc='best', title='Pixel error', fontsize='x-small') if motion_inx == 0: @@ -179,14 +204,31 @@ ax.set_ylabel('Number of tracks') ax.yaxis.set_label_position("right") - df = df.append(pandas.DataFrame({'FOV': [fov for i in range(len(track_lengths[0]))], 'Track lifetime (frames)': track_lengths[0, :]})) + df_lifetime = df_lifetime.append(pandas.DataFrame({'FOV': [fov for i in range(len(track_lengths[0]))], 'Track lifetime (frames)': track_lengths[0, :]})) + re_filt_ee = radial_errors[np.where((radial_errors[:, 1] < 50) & (radial_errors[:, 2] > 0))] + re_filt_ae = radial_errors[np.where(radial_errors[:, 1] < 50)] + re_filt_be = radial_errors[np.where((radial_errors[:, 1] < 50) & (radial_errors[:, 4] > 0))] + df_ee = df_ee.append(pandas.DataFrame({'Radial distance': np.round(re_filt_ee[:, 0] / 0.005) * 0.005, 'FOV': [fovstr for i in range(len(re_filt_ee))], 'Motion': [motion for i in range(len(re_filt_ee))], 'Relative endpoint error': re_filt_ee[:, 2]})) + df_ae = df_ae.append(pandas.DataFrame({'Radial distance': np.round(re_filt_ae[:, 0] / 0.005) * 0.005, 'FOV': [fovstr for i in range(len(re_filt_ae))], 'Motion': [motion for i in range(len(re_filt_ae))], 'Angular error': re_filt_ae[:, 3]})) + df_be = df_be.append(pandas.DataFrame({'Radial distance': np.round(re_filt_be[:, 0] / 0.005) * 0.005, 'FOV': [fovstr for i in range(len(re_filt_be))], 'Motion': [motion for i in range(len(re_filt_be))], 'Relative bearing error': re_filt_be[:, 4]})) else: print '[WARNING] No results files found in directory {} for FOV {}'.format(os.path.join(bag_dir), fovstr) if motion_exists: ax2 = fig2.add_subplot(num_rows, 1, motion_inx + 1) - sns.violinplot(x='FOV', y='Track lifetime (frames)', data=df, ax=ax2, palette="muted", inner='quart') + sns.violinplot(x='FOV', y='Track lifetime (frames)', data=df_lifetime, ax=ax2, palette="muted", inner='quart') ax2.set_title('Motion {}'.format(motion)) motion_inx += 1 + + # ax_ee = fig3.add_subplot(1, 3, 1) + sns.relplot(x="Radial distance", y="Relative endpoint error", kind="line", data=df_ee, ci="sd", row='Motion', hue='FOV', legend="full", hue_order=fovs, palette=sns.color_palette("muted", n_colors=len(fovs))) + # ax_ee.set_title('Relative endpoint error') + # ax_ae = fig3.add_subplot(1, 3, 2) + sns.relplot(x="Radial distance", y="Angular error", kind="line", data=df_ae, ci="sd", row='Motion', hue='FOV', legend="full", hue_order=fovs, palette=sns.color_palette("muted", n_colors=len(fovs))) + # ax_ae.set_title('Angular error') + # ax_be = fig3.add_subplot(1, 3, 3) + sns.relplot(x="Radial distance", y="Relative bearing error", kind="line", data=df_be, ci="sd", row='Motion', hue='FOV', legend="full", hue_order=fovs, palette=sns.color_palette("muted", n_colors=len(fovs))) + # ax_be.set_title('Relative bearing error') + plt.show() else: diff --git a/src/module/tracking_module.cc b/src/module/tracking_module.cc index 84ae438..649a4d0 100644 --- a/src/module/tracking_module.cc +++ b/src/module/tracking_module.cc @@ -94,7 +94,8 @@ void TrackingModule::Update(std::unique_ptr &frame) regionCount_[{rinx, tinx}]++; regionLandmarks_[{rinx, tinx}].push_back(&landmark); - const data::Feature *obsPrev = landmark.GetObservationByFrameID((*next(frames_.rbegin()))->GetID()); + const data::Feature *obsPrevFrame = landmark.GetObservationByFrameID((*next(frames_.rbegin()))->GetID()); + const data::Feature *obsPrev = landmark.GetObservationByFrameID(lastKeyframe_->GetID()); if (obsPrev != nullptr) { Vector2d pixelGnd; @@ -106,18 +107,50 @@ void TrackingModule::Update(std::unique_ptr &frame) pixel << obs->GetKeypoint().pt.x, obs->GetKeypoint().pt.y; double error = (pixel - pixelGnd).norm(); - visualization_.AddTrack(cv::Point2f(pixelGnd(0), pixelGnd(1)), obsPrev->GetKeypoint().pt, obs->GetKeypoint().pt, error, i); + if (obsPrevFrame != nullptr) + { + visualization_.AddTrack(cv::Point2f(pixelGnd(0), pixelGnd(1)), obsPrevFrame->GetKeypoint().pt, obs->GetKeypoint().pt, error, i); + } double xg = pixelGnd(0) - frames_.back()->GetImage().cols / 2. + 0.5; double yg = pixelGnd(1) - frames_.back()->GetImage().rows / 2. + 0.5; double rg = sqrt(xg * xg + yg * yg) / imsize; - stats_.radialErrors.emplace_back(vector{rg, error}); - stats_.frameErrors.emplace_back(vector{(double)landmark.GetNumObservations() - 1, (double)i, rg, error}); + + Vector2d pixelPrev; + pixelPrev << obsPrev->GetKeypoint().pt.x, obsPrev->GetKeypoint().pt.y; + Vector3d flow; + flow = (pixel - pixelPrev).homogeneous().normalized(); + Vector3d ray; + frames_.back()->GetCameraModel().UnprojectToBearing(pixel, ray); + Vector3d rayGnd; + frames_.back()->GetCameraModel().UnprojectToBearing(pixelGnd, rayGnd); + Vector3d rayPrev; + lastKeyframe_->GetCameraModel().UnprojectToBearing(pixelPrev, rayPrev); + Vector3d rayGndPrev = rayPrev; + double prevError = 0; + Vector3d flowGnd; + flowGnd << 0, 0, 1; + Vector2d pixelGndPrev; + if (lastKeyframe_->GetCameraModel().ProjectToImage(util::TFUtil::WorldFrameToCameraFrame(util::TFUtil::TransformPoint(lastKeyframe_->GetInversePose(), landmark.GetGroundTruth())), pixelGndPrev)) + { + prevError = (pixelPrev - pixelGndPrev).norm(); + flowGnd = (pixelGnd - pixelGndPrev).homogeneous().normalized(); + lastKeyframe_->GetCameraModel().UnprojectToBearing(pixelGndPrev, rayGndPrev); + } + double angularError = acos(flow.dot(flowGnd)); + double bearingError = acos(ray.normalized().dot(rayGnd.normalized())); + double bearingErrorPrev = acos(rayPrev.normalized().dot(rayGndPrev.normalized())); + stats_.radialErrors.emplace_back(vector{rg, error, error - prevError, angularError, bearingError - bearingErrorPrev}); + stats_.frameErrors.emplace_back(vector{(double)landmark.GetNumObservations() - 1, (double)i, rg, error, bearingError}); + stats_.successRadDists.emplace_back(vector{rg, (double)frameNum_}); } } else { - visualization_.AddTrack(obsPrev->GetKeypoint().pt, obs->GetKeypoint().pt, i); + if (obsPrevFrame != nullptr) + { + visualization_.AddTrack(obsPrevFrame->GetKeypoint().pt, obs->GetKeypoint().pt, i); + } } } stats_.trackLengths[i]++; @@ -125,7 +158,7 @@ void TrackingModule::Update(std::unique_ptr &frame) } else { - const data::Feature *obs = landmark.GetObservationByFrameID((*next(frames_.rbegin()))->GetID()); + const data::Feature *obs = landmark.GetObservationByFrameID(lastKeyframe_->GetID()); if (obs != nullptr) // Failed in current frame { @@ -135,7 +168,7 @@ void TrackingModule::Update(std::unique_ptr &frame) double x = pixelGnd(0) - frames_.back()->GetImage().cols / 2. + 0.5; double y = pixelGnd(1) - frames_.back()->GetImage().rows / 2. + 0.5; double r = sqrt(x * x + y * y) / imsize; - stats_.failureRadDists.push_back(r); + stats_.failureRadDists.emplace_back(vector{r, (double)frameNum_}); } stats_.trackLengths.push_back(landmark.GetNumObservations() - 1); } diff --git a/src/module/tracking_module.h b/src/module/tracking_module.h index 22deaf3..c67e5e6 100644 --- a/src/module/tracking_module.h +++ b/src/module/tracking_module.h @@ -26,7 +26,8 @@ class TrackingModule std::vector> frameErrors; std::vector> frameTrackCounts; std::vector trackLengths; - std::vector failureRadDists; + std::vector> failureRadDists; + std::vector> successRadDists; }; TrackingModule(std::unique_ptr &detector, std::unique_ptr &tracker, std::unique_ptr &checker, int minFeaturesRegion = 5, int maxFeaturesRegion = 5000); diff --git a/src/ros/tracking_eval.cc b/src/ros/tracking_eval.cc index 319a16b..9bfab75 100644 --- a/src/ros/tracking_eval.cc +++ b/src/ros/tracking_eval.cc @@ -110,7 +110,8 @@ template void TrackingEval::GetResultsData(std::map>> &data) { module::TrackingModule::Stats &stats = trackingModule_->GetStats(); - data["failures"] = {stats.failureRadDists}; + data["failures"] = stats.failureRadDists; + data["successes"] = stats.successRadDists; data["radial_errors"] = stats.radialErrors; data["length_errors"] = stats.frameErrors; data["track_counts"] = vector>();