tracking plot changes

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:

src/module/tracking_module.cc

@@ -94,7 +94,8 @@ void TrackingModule::Update(std::unique_ptr<data::Frame> &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,26 +107,58 @@ void TrackingModule::Update(std::unique_ptr<data::Frame> &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<double>{rg, error});
-                        stats_.frameErrors.emplace_back(vector<double>{(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<double>{rg, error, error - prevError, angularError, bearingError - bearingErrorPrev});
+                        stats_.frameErrors.emplace_back(vector<double>{(double)landmark.GetNumObservations() - 1, (double)i, rg, error, bearingError});
+                        stats_.successRadDists.emplace_back(vector<double>{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]++;
             numGood++;
         }
         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<data::Frame> &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<double>{r, (double)frameNum_});
                 }
                 stats_.trackLengths.push_back(landmark.GetNumObservations() - 1);
             }

src/module/tracking_module.h

@@ -26,7 +26,8 @@ class TrackingModule
         std::vector<std::vector<double>> frameErrors;
         std::vector<std::vector<int>> frameTrackCounts;
         std::vector<int> trackLengths;
-        std::vector<double> failureRadDists;
+        std::vector<std::vector<double>> failureRadDists;
+        std::vector<std::vector<double>> successRadDists;
     };
 
     TrackingModule(std::unique_ptr<feature::Detector> &detector, std::unique_ptr<feature::Tracker> &tracker, std::unique_ptr<odometry::FivePoint> &checker, int minFeaturesRegion = 5, int maxFeaturesRegion = 5000);

src/ros/tracking_eval.cc

@@ -110,7 +110,8 @@ template <bool Stereo>
 void TrackingEval<Stereo>::GetResultsData(std::map<std::string, std::vector<std::vector<double>>> &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<vector<double>>();