Added time

Signed-off-by: Shintaro Sakoda <[email protected]>

apps/check_covariance.cpp

@@ -58,10 +58,6 @@ int main(int argc, char** argv) {
   const std::string source_pcd_dir = input_dir + "/sensor_pcd/";
   std::vector<std::string> source_pcd_list = glob(source_pcd_dir);
 
-  // prepare results
-  std::vector<double> elapsed_milliseconds;
-  std::vector<double> scores;
-
   // load kinematic_state.csv
   /*
   timestamp,pose_x,pose_y,pose_z,quat_w,quat_x,quat_y,quat_z,twist_linear_x,twist_linear_y,twist_linear_z,twist_angular_x,twist_angular_y,twist_angular_z
@@ -109,7 +105,10 @@ int main(int argc, char** argv) {
   mkdir(output_dir.c_str(), 0777);
   std::ofstream ofs(output_dir + "/result.csv");
   ofs << std::fixed;
-  ofs << "index,elapsed_milliseconds,score,x,y,cov_by_la_00,cov_by_la_01,cov_by_la_10,cov_by_la_11,cov_by_mndt_00,cov_by_mndt_01,cov_by_mndt_10,cov_by_mndt_11,cov_by_mndt_score_00,cov_by_mndt_score_01,cov_by_mndt_score_10,cov_by_mndt_score_11" << std::endl;
+  ofs << "index,score,x,y,elapsed_la,cov_by_la_00,cov_by_la_01,cov_by_la_10,cov_by_la_11,elapsed_mndt,cov_by_mndt_00,cov_by_mndt_01,cov_by_mndt_10,cov_by_mndt_11,elapsed_mndt_score,cov_by_mndt_score_00,cov_by_mndt_score_01,cov_by_mndt_score_10,cov_by_mndt_score_11" << std::endl;
+
+  auto t1 = std::chrono::system_clock::now();
+  auto t2 = std::chrono::system_clock::now();
 
   // execute align
   for(int64_t i = 0; i < n_data; i++) {
@@ -122,24 +121,33 @@ int main(int argc, char** argv) {
     }
     mg_ndt_omp->setInputSource(source_cloud);
     pcl::PointCloud<pcl::PointXYZ>::Ptr aligned(new pcl::PointCloud<pcl::PointXYZ>());
-    auto t1 = std::chrono::system_clock::now();
     mg_ndt_omp->align(*aligned, initial_pose);
     const pclomp::NdtResult ndt_result = mg_ndt_omp->getResult();
-    auto t2 = std::chrono::system_clock::now();
-    const auto elapsed = std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1).count() / 1000.0;
     const double score = ndt_result.nearest_voxel_transformation_likelihood;
-    elapsed_milliseconds.push_back(elapsed);
-    scores.push_back(score);
-    std::cout << source_pcd << ", num=" << std::setw(4) << source_cloud->size() << " points, time=" << elapsed << " [msec], score=" << score << std::endl;
+    std::cout << source_pcd << ", num=" << std::setw(4) << source_cloud->size() << " points, score=" << score << std::endl;
 
     // estimate covariance
+    // (1) Laplace approximation
+    t1 = std::chrono::system_clock::now();
     const Eigen::Matrix2d cov_by_la = pclomp::estimate_xy_covariance_by_Laplace_approximation(ndt_result.hessian);
+    t2 = std::chrono::system_clock::now();
+    const auto elapsed_la = std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1).count() / 1000.0;
+
+    // (2) Multi NDT
+    t1 = std::chrono::system_clock::now();
     const Eigen::Matrix2d cov_by_mndt = pclomp::estimate_xy_covariance_by_multi_ndt(ndt_result, mg_ndt_omp, initial_pose, offset_x, offset_y);
+    t2 = std::chrono::system_clock::now();
+    const auto elapsed_mndt = std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1).count() / 1000.0;
+
+    // (3) Multi NDT with score
+    t1 = std::chrono::system_clock::now();
     const Eigen::Matrix2d cov_by_mndt_score = pclomp::estimate_xy_covariance_by_multi_ndt_score(ndt_result, mg_ndt_omp, initial_pose, offset_x, offset_y);
+    t2 = std::chrono::system_clock::now();
+    const auto elapsed_mndt_score = std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1).count() / 1000.0;
 
-    ofs << i << "," << elapsed << "," << score << "," << initial_pose(0, 3) << "," << initial_pose(1, 3);
-    ofs << "," << cov_by_la(0, 0) << "," << cov_by_la(0, 1) << "," << cov_by_la(1, 0) << "," << cov_by_la(1, 1);
-    ofs << "," << cov_by_mndt(0, 0) << "," << cov_by_mndt(0, 1) << "," << cov_by_mndt(1, 0) << "," << cov_by_mndt(1, 1);
-    ofs << "," << cov_by_mndt_score(0, 0) << "," << cov_by_mndt_score(0, 1) << "," << cov_by_mndt_score(1, 0) << "," << cov_by_mndt_score(1, 1) << std::endl;
+    ofs << i << "," << score << "," << initial_pose(0, 3) << "," << initial_pose(1, 3);
+    ofs << "," << elapsed_la << "," << cov_by_la(0, 0) << "," << cov_by_la(0, 1) << "," << cov_by_la(1, 0) << "," << cov_by_la(1, 1);
+    ofs << "," << elapsed_mndt << "," << cov_by_mndt(0, 0) << "," << cov_by_mndt(0, 1) << "," << cov_by_mndt(1, 0) << "," << cov_by_mndt(1, 1);
+    ofs << "," << elapsed_mndt_score << "," << cov_by_mndt_score(0, 0) << "," << cov_by_mndt_score(0, 1) << "," << cov_by_mndt_score(1, 0) << "," << cov_by_mndt_score(1, 1) << std::endl;
   }
 }

script/plot_covariance.py

@@ -42,13 +42,27 @@ def plot_ellipse(mean, cov, color, label, scale):
 
     """
     df_result
-    index,elapsed_milliseconds,score,x,y,cov_by_la_00,cov_by_la_01,cov_by_la_10,cov_by_la_11,cov_by_mndt_00,cov_by_mndt_01,cov_by_mndt_10,cov_by_mndt_11,cov_by_mndt_score_00,cov_by_mndt_score_01,cov_by_mndt_score_10,cov_by_mndt_score_11
-    0,576.501000,3.429885,81377.359375,49916.902344,0.000110,0.000007,0.000007,0.000116,0.000017,-0.000002,-0.000002,0.000002,0.106793,0.080732,0.080732,0.177989
-    1,2.180000,3.412068,81377.359375,49916.902344,0.000111,0.000007,0.000007,0.000117,0.000009,0.000004,0.000004,0.000009,0.108240,0.080859,0.080859,0.179305
-    2,1.885000,3.424125,81377.359375,49916.902344,0.000109,0.000007,0.000007,0.000116,0.000000,0.000000,0.000000,0.000009,0.106697,0.080186,0.080186,0.179408
+    index,score,x,y,elapsed_la,cov_by_la_00,cov_by_la_01,cov_by_la_10,cov_by_la_11,elapsed_mndt,cov_by_mndt_00,cov_by_mndt_01,cov_by_mndt_10,cov_by_mndt_11,elapsed_mndt_score,cov_by_mndt_score_00,cov_by_mndt_score_01,cov_by_mndt_score_10,cov_by_mndt_score_11
+    0,3.429885,81377.359375,49916.902344,0.000000,0.000110,0.000007,0.000007,0.000116,112.322000,0.000017,0.000000,0.000000,0.000003,12.878000,0.002066,0.000650,0.000650,0.002814
+    1,3.412068,81377.359375,49916.902344,0.000000,0.000111,0.000007,0.000007,0.000117,62.720000,0.187296,0.104792,0.104792,0.226846,12.767000,0.002203,0.000831,0.000831,0.003018
+    2,3.424125,81377.359375,49916.902344,0.000000,0.000109,0.000007,0.000007,0.000116,61.792000,0.161584,0.092505,0.092505,0.237047,12.803000,0.002010,0.000674,0.000674,0.002697
     ...
     """
 
+    # plot time
+    plt.figure()
+    plt.plot(df_result["elapsed_la"], label="Laplace Approximation")
+    plt.plot(df_result["elapsed_mndt"], label="Multi NDT")
+    plt.plot(df_result["elapsed_mndt_score"], label="Multi NDT Score")
+    plt.xlabel("Frame")
+    plt.ylabel("Time [msec]")
+    plt.legend()
+    plt.tight_layout()
+    save_path = result_csv.parent / "time.png"
+    plt.savefig(save_path, bbox_inches="tight", pad_inches=0.05)
+    print(f"Saved: {save_path}")
+    plt.close()
+
     cov_by_la = df_result[
         ["cov_by_la_00", "cov_by_la_01", "cov_by_la_10", "cov_by_la_11"]
     ].values