From 3eacb2e133c0f192d60b85cd16a6026ab5c8dcb9 Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Sat, 16 Mar 2024 07:54:43 +0900 Subject: [PATCH] Added time Signed-off-by: Shintaro Sakoda --- apps/check_covariance.cpp | 38 +++++++++++++++++++++++--------------- script/plot_covariance.py | 22 ++++++++++++++++++---- 2 files changed, 41 insertions(+), 19 deletions(-) diff --git a/apps/check_covariance.cpp b/apps/check_covariance.cpp index 1c254f3e..07d1d41b 100644 --- a/apps/check_covariance.cpp +++ b/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 source_pcd_list = glob(source_pcd_dir); - // prepare results - std::vector elapsed_milliseconds; - std::vector 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::Ptr aligned(new pcl::PointCloud()); - 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(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(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(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(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; } } diff --git a/script/plot_covariance.py b/script/plot_covariance.py index 7cfb5d8e..c43114e2 100644 --- a/script/plot_covariance.py +++ b/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