From 6c6d0e1dd6550496e01bb57373deb12975a1cbf4 Mon Sep 17 00:00:00 2001 From: SakodaShintaro Date: Mon, 11 Mar 2024 17:14:49 +0900 Subject: [PATCH] feat: add a regression test (#33) * Added a regression_test Signed-off-by: Shintaro Sakoda * Fixed CMakeLists.txt Signed-off-by: Shintaro Sakoda * Fixed return Signed-off-by: Shintaro Sakoda * Updated README.md Signed-off-by: Shintaro Sakoda * Added download_data.sh Signed-off-by: Shintaro Sakoda * Added regression_test.yml Signed-off-by: Shintaro Sakoda * Fixed dir name Signed-off-by: Shintaro Sakoda * Added container Signed-off-by: Shintaro Sakoda * Added "Install dependencies" Signed-off-by: Shintaro Sakoda * Added quiet Signed-off-by: Shintaro Sakoda * Added unzip Signed-off-by: Shintaro Sakoda * Added -q to unzip Signed-off-by: Shintaro Sakoda * Added source Signed-off-by: Shintaro Sakoda * Fixed path Signed-off-by: Shintaro Sakoda * Added "shell: bash" Signed-off-by: Shintaro Sakoda * Added libpcl-dev Signed-off-by: Shintaro Sakoda * Added python dependencies Signed-off-by: Shintaro Sakoda * Added reference/result.csv Signed-off-by: Shintaro Sakoda * Added python3-pip Signed-off-by: Shintaro Sakoda * Added Upload output files Signed-off-by: Shintaro Sakoda * Fixed name and execute if Signed-off-by: Shintaro Sakoda * Updated result by GitHub Actions result Signed-off-by: Shintaro Sakoda --------- Signed-off-by: Shintaro Sakoda Signed-off-by: Shintaro Sakoda --- .github/workflows/regression_test.yml | 49 ++ .gitignore | 1 + CMakeLists.txt | 12 + README.md | 50 ++ apps/regression_test.cpp | 133 +++ include/multigrid_pclomp/multigrid_ndt_omp.h | 2 + regression_test_data/.gitignore | 2 + regression_test_data/reference/result.csv | 821 +++++++++++++++++++ script/compare_regression_test_result.py | 68 ++ script/convert_rosbag_to_test_data.py | 135 +++ script/download_data.sh | 19 + script/execute_regression_test.sh | 8 + 12 files changed, 1300 insertions(+) create mode 100644 .github/workflows/regression_test.yml create mode 100644 .gitignore create mode 100644 apps/regression_test.cpp create mode 100644 regression_test_data/.gitignore create mode 100644 regression_test_data/reference/result.csv create mode 100644 script/compare_regression_test_result.py create mode 100644 script/convert_rosbag_to_test_data.py create mode 100755 script/download_data.sh create mode 100755 script/execute_regression_test.sh diff --git a/.github/workflows/regression_test.yml b/.github/workflows/regression_test.yml new file mode 100644 index 00000000..11fa6470 --- /dev/null +++ b/.github/workflows/regression_test.yml @@ -0,0 +1,49 @@ +name: regression_test + +on: + pull_request: + types: + - opened + - synchronize + +jobs: + regression_test: + runs-on: ubuntu-latest + container: + image: ros:humble + steps: + - name: Check out repository + uses: actions/checkout@v4 + with: + ref: ${{ github.event.pull_request.head.ref }} + + - name: Install dependencies + run: | + apt-get update + apt-get install -y wget unzip libpcl-dev python3-pip + pip3 install pandas matplotlib + + - name: Download data + run: | + ./script/download_data.sh + + - name: Build + shell: bash + run: | + source /opt/ros/humble/setup.bash + mkdir -p build + cd build + cmake .. + make -j + cd .. + + - name: Run + run: | + ./script/execute_regression_test.sh + + - name: Upload output files + uses: actions/upload-artifact@v4 + if: always() + with: + name: regression-test-output + path: ./regression_test_data/output/ diff --git a/.gitignore b/.gitignore new file mode 100644 index 00000000..378eac25 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +build diff --git a/CMakeLists.txt b/CMakeLists.txt index 13af01e1..416e44fb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -144,4 +144,16 @@ endif() ament_auto_package() +# regression test +add_executable(regression_test + apps/regression_test.cpp +) +add_dependencies(regression_test + ndt_omp multigrid_ndt_omp +) +target_link_libraries(regression_test + ${PCL_LIBRARIES} + ndt_omp multigrid_ndt_omp +) + endif() diff --git a/README.md b/README.md index d7b093e5..c62f8cc8 100644 --- a/README.md +++ b/README.md @@ -57,3 +57,53 @@ Red: target, Green: source, Blue: aligned ## Related packages - [ndt_omp](https://github.com/koide3/ndt_omp) - [fast_gicp](https://github.com/SMRT-AIST/fast_gicp) + +## Regression Test + +### Preparation + +You can use `script/convert_rosbag_to_test_data.py` to convert a rosbag to regression test data. + +The regression test data should be placed in `./regression_test_data/input` directory. + +A sample data is [here](https://drive.google.com/file/d/1E-_zj2nchmntioSJJgyoDQEYHtrs3o-C/view). + +The sample data is recorded by AWSIM, so the corresponding map is [nishishinjuku_autoware_map](https://github.com/tier4/AWSIM/releases/download/v1.1.0/nishishinjuku_autoware_map.zip). + +The directory structure should be like this: + +```bash +./regression_test_data/ +└── input + ├── kinematic_state.csv + ├── pointcloud_map.pcd # means map.pcd + └── sensor_pcd + ├── pointcloud_00000000.pcd + ├── pointcloud_00000001.pcd + ├── pointcloud_00000002.pcd + ├── ... + ├── pointcloud_00000817.pcd + ├── pointcloud_00000818.pcd + └── pointcloud_00000819.pcd +``` + +### build + +```bash +mkdir build +cd build +cmake .. +make -j +``` + +### Run + +```bash +./regression_test ../regression_test_data/input ../regression_test_data/output +``` + +### Check + +```bash +python3 script/compare_regression_test_result.py ../regression_test_data/output ../regression_test_data/reference_output +``` diff --git a/apps/regression_test.cpp b/apps/regression_test.cpp new file mode 100644 index 00000000..05825dbb --- /dev/null +++ b/apps/regression_test.cpp @@ -0,0 +1,133 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +std::vector glob(const std::string& input_dir) { + glob_t buffer; + std::vector files; + glob((input_dir + "/*").c_str(), 0, NULL, &buffer); + for(size_t i = 0; i < buffer.gl_pathc; i++) { + files.push_back(buffer.gl_pathv[i]); + } + globfree(&buffer); + std::sort(files.begin(), files.end()); + return files; +} + +int main(int argc, char** argv) { + if(argc != 3) { + std::cout << "usage: ./regression_test " << std::endl; + return 0; + } + + const std::string input_dir = argv[1]; + const std::string output_dir = argv[2]; + + // load target pcd + const std::string target_pcd = input_dir + "/pointcloud_map.pcd"; + pcl::PointCloud::Ptr target_cloud(new pcl::PointCloud()); + if(pcl::io::loadPCDFile(target_pcd, *target_cloud)) { + std::cerr << "failed to load " << target_pcd << std::endl; + return 1; + } + + // prepare ndt + pclomp::MultiGridNormalDistributionsTransform::Ptr mg_ndt_omp(new pclomp::MultiGridNormalDistributionsTransform()); + mg_ndt_omp->setResolution(1.0); + mg_ndt_omp->setNumThreads(4); + mg_ndt_omp->setInputTarget(target_cloud); + mg_ndt_omp->setMaximumIterations(30); + mg_ndt_omp->setTransformationEpsilon(0.0); + mg_ndt_omp->createVoxelKdtree(); + + // prepare sensor_pcd + 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 + 63.100010,81377.359702,49916.899866,41.232589,0.953768,0.000494,-0.007336,0.300453,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 + 63.133344,81377.359780,49916.899912,41.232735,0.953769,0.000491,-0.007332,0.300452,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 + ... + */ + std::ifstream ifs(input_dir + "/kinematic_state.csv"); + std::string line; + std::getline(ifs, line); // skip header + std::vector initial_pose_list; + while(std::getline(ifs, line)) { + std::istringstream iss(line); + std::string token; + std::vector tokens; + while(std::getline(iss, token, ',')) { + tokens.push_back(token); + } + const double timestamp = std::stod(tokens[0]); + const double pose_x = std::stod(tokens[1]); + const double pose_y = std::stod(tokens[2]); + const double pose_z = std::stod(tokens[3]); + const double quat_w = std::stod(tokens[4]); + const double quat_x = std::stod(tokens[5]); + const double quat_y = std::stod(tokens[6]); + const double quat_z = std::stod(tokens[7]); + Eigen::Matrix4f initial_pose = Eigen::Matrix4f::Identity(); + initial_pose.block<3, 3>(0, 0) = Eigen::Quaternionf(quat_w, quat_x, quat_y, quat_z).toRotationMatrix(); + initial_pose.block<3, 1>(0, 3) = Eigen::Vector3f(pose_x, pose_y, pose_z); + initial_pose_list.push_back(initial_pose); + } + + if(initial_pose_list.size() != source_pcd_list.size()) { + std::cerr << "initial_pose_list.size() != source_pcd_list.size()" << std::endl; + return 1; + } + const int64_t n_data = initial_pose_list.size(); + + std::cout << std::fixed; + + // execute align + for(int64_t i = 0; i < n_data; i++) { + const Eigen::Matrix4f initial_pose = initial_pose_list[i]; + const std::string& source_pcd = source_pcd_list[i]; + pcl::PointCloud::Ptr source_cloud(new pcl::PointCloud()); + if(pcl::io::loadPCDFile(source_pcd, *source_cloud)) { + std::cerr << "failed to load " << source_pcd << std::endl; + return 1; + } + 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; + } + + // output result + mkdir(output_dir.c_str(), 0777); + std::ofstream ofs(output_dir + "/result.csv"); + ofs << "elapsed_milliseconds,score" << std::endl; + ofs << std::fixed; + for(size_t i = 0; i < elapsed_milliseconds.size(); i++) { + ofs << elapsed_milliseconds[i] << "," << scores[i] << std::endl; + } +} diff --git a/include/multigrid_pclomp/multigrid_ndt_omp.h b/include/multigrid_pclomp/multigrid_ndt_omp.h index d4aa7e87..b436a623 100644 --- a/include/multigrid_pclomp/multigrid_ndt_omp.h +++ b/include/multigrid_pclomp/multigrid_ndt_omp.h @@ -64,6 +64,8 @@ #include namespace pclomp { +enum NeighborSearchMethod { KDTREE, DIRECT26, DIRECT7, DIRECT1 }; + struct NdtResult { Eigen::Matrix4f pose; float transform_probability; diff --git a/regression_test_data/.gitignore b/regression_test_data/.gitignore new file mode 100644 index 00000000..ba64707c --- /dev/null +++ b/regression_test_data/.gitignore @@ -0,0 +1,2 @@ +input/ +output/ diff --git a/regression_test_data/reference/result.csv b/regression_test_data/reference/result.csv new file mode 100644 index 00000000..7c03f1f4 --- /dev/null +++ b/regression_test_data/reference/result.csv @@ -0,0 +1,821 @@ +elapsed_milliseconds,score +849.205000,1.649302 +19.490000,1.645455 +18.785000,1.657175 +18.974000,1.631545 +19.006000,1.656746 +19.045000,1.651334 +19.673000,1.634316 +18.893000,1.634507 +19.010000,1.654363 +19.035000,1.653594 +19.091000,1.656799 +19.079000,1.630988 +19.014000,1.643923 +19.289000,1.653671 +19.071000,1.650060 +19.009000,1.640574 +19.203000,1.642808 +19.110000,1.656991 +18.971000,1.652129 +19.005000,1.653247 +19.010000,1.661678 +19.122000,1.644760 +19.204000,1.649469 +19.247000,1.648430 +18.957000,1.640761 +18.926000,1.659842 +18.895000,1.654068 +18.959000,1.643565 +19.050000,1.647220 +19.102000,1.646623 +19.134000,1.644773 +19.052000,1.662741 +19.469000,1.641208 +19.340000,1.654897 +19.566000,1.655202 +18.841000,1.644679 +19.147000,1.643472 +18.996000,1.649472 +19.136000,1.656925 +19.303000,1.645797 +19.171000,1.649763 +18.986000,1.649941 +19.148000,1.663932 +18.740000,1.643844 +18.950000,1.641626 +19.266000,1.644452 +19.082000,1.635165 +18.865000,1.639557 +18.633000,1.647679 +19.106000,1.643170 +18.918000,1.658048 +18.897000,1.645533 +19.002000,1.654884 +19.772000,1.663906 +19.120000,1.650788 +19.180000,1.646930 +19.101000,1.646234 +19.349000,1.645808 +19.172000,1.640761 +19.069000,1.656329 +18.867000,1.654983 +19.235000,1.657170 +18.719000,1.654927 +19.357000,1.633910 +18.998000,1.654198 +19.083000,1.646206 +19.234000,1.636978 +18.928000,1.658676 +19.168000,1.647176 +18.977000,1.640164 +19.106000,1.658642 +19.063000,1.642015 +19.248000,1.636283 +19.486000,1.650205 +19.175000,1.642693 +19.011000,1.638402 +19.057000,1.653528 +18.765000,1.643166 +19.132000,1.639998 +19.015000,1.635433 +19.038000,1.648461 +18.866000,1.649547 +19.522000,1.651231 +19.199000,1.646498 +21.486000,1.644638 +19.018000,1.652131 +19.255000,1.656560 +18.940000,1.649332 +19.184000,1.649668 +19.030000,1.651472 +19.183000,1.656508 +19.148000,1.641698 +19.040000,1.655081 +19.280000,1.643055 +19.418000,1.649579 +19.136000,1.644870 +19.133000,1.645615 +19.008000,1.638198 +26.365000,1.647325 +19.430000,1.656469 +18.834000,1.645456 +18.746000,1.650329 +18.763000,1.651568 +18.781000,1.647218 +19.211000,1.656735 +19.015000,1.654277 +19.153000,1.648428 +19.596000,1.645899 +18.962000,1.651216 +18.766000,1.661507 +18.954000,1.638370 +19.515000,1.646561 +19.049000,1.640266 +19.143000,1.646165 +19.263000,1.637545 +19.008000,1.646525 +19.551000,1.637713 +19.037000,1.648281 +18.780000,1.629804 +19.069000,1.655485 +19.110000,1.649058 +19.294000,1.639571 +19.180000,1.655161 +19.276000,1.637150 +19.116000,1.643987 +19.265000,1.656992 +19.129000,1.655975 +24.122000,1.664421 +19.157000,1.673310 +19.086000,1.663261 +18.923000,1.665350 +19.139000,1.662681 +19.374000,1.656977 +19.026000,1.649441 +19.615000,1.654070 +19.689000,1.641211 +19.271000,1.655878 +19.159000,1.655797 +19.134000,1.636281 +19.368000,1.643963 +19.206000,1.627477 +19.259000,1.620919 +19.882000,1.627357 +19.985000,1.626483 +19.639000,1.651748 +20.479000,1.633598 +20.040000,1.646211 +19.944000,1.662859 +20.092000,1.653762 +20.267000,1.646281 +20.657000,1.624114 +20.236000,1.632324 +20.268000,1.631177 +19.750000,1.629598 +20.146000,1.615175 +20.359000,1.622337 +20.854000,1.616761 +21.257000,1.617484 +21.312000,1.625670 +21.404000,1.627888 +21.260000,1.625169 +21.152000,1.659083 +21.118000,1.660188 +21.453000,1.650945 +21.489000,1.639722 +21.351000,1.678918 +21.152000,1.666009 +21.115000,1.637550 +21.270000,1.650923 +20.857000,1.656656 +20.668000,1.654936 +20.860000,1.641426 +20.956000,1.657122 +21.501000,1.656997 +21.723000,1.649709 +21.977000,1.637592 +21.581000,1.623207 +21.336000,1.618689 +21.832000,1.596758 +21.986000,1.616799 +21.430000,1.599509 +22.567000,1.586842 +21.862000,1.610540 +21.613000,1.587071 +25.494000,1.594149 +22.178000,1.606783 +21.866000,1.569617 +22.066000,1.559880 +22.512000,1.601503 +22.567000,1.621540 +40.441000,1.588615 +34.793000,1.589233 +22.592000,1.576688 +39.786000,1.599838 +22.863000,1.589079 +23.693000,1.571210 +23.709000,1.577753 +23.388000,1.563797 +23.466000,1.575946 +23.772000,1.566688 +23.272000,1.566468 +24.276000,1.573553 +24.865000,1.590770 +24.787000,1.571187 +25.443000,1.546471 +25.704000,1.572670 +25.613000,1.564887 +25.677000,1.565152 +25.087000,1.583207 +25.539000,1.564644 +25.395000,1.554077 +25.464000,1.560324 +25.506000,1.576829 +26.475000,1.570052 +26.604000,1.583504 +26.636000,1.595998 +27.548000,1.589282 +27.618000,1.555417 +29.018000,1.580051 +34.479000,1.563215 +30.198000,1.569372 +29.313000,1.586108 +30.299000,1.588529 +30.151000,1.566799 +29.234000,1.565353 +29.514000,1.586496 +30.287000,1.603731 +31.264000,1.575287 +30.668000,1.575212 +30.246000,1.563342 +30.230000,1.593126 +44.167000,1.584873 +34.097000,1.596432 +33.472000,1.592819 +33.091000,1.591028 +33.652000,1.585010 +33.148000,1.595584 +34.096000,1.601088 +34.428000,1.575514 +34.204000,1.594988 +35.720000,1.623456 +34.914000,1.608309 +33.681000,1.592747 +33.835000,1.602283 +33.329000,1.589584 +32.429000,1.582557 +31.072000,1.605275 +32.326000,1.617105 +30.623000,1.587287 +28.827000,1.603428 +36.362000,1.589234 +28.837000,1.573462 +26.961000,1.587939 +26.744000,1.568311 +25.815000,1.599494 +25.865000,1.606107 +27.670000,1.594736 +29.396000,1.564509 +30.660000,1.572753 +31.337000,1.599807 +31.167000,1.592354 +32.289000,1.586738 +32.983000,1.579660 +35.119000,1.585163 +36.948000,1.600614 +37.218000,1.589650 +36.620000,1.584868 +36.604000,1.600991 +36.214000,1.579637 +36.539000,1.595261 +36.605000,1.580686 +36.908000,1.608780 +38.392000,1.589642 +38.652000,1.578849 +38.413000,1.586245 +36.351000,1.586884 +36.477000,1.579248 +36.009000,1.562585 +35.892000,1.576895 +42.185000,1.583990 +35.614000,1.574662 +35.026000,1.581846 +33.746000,1.587091 +33.956000,1.576124 +33.812000,1.589197 +34.043000,1.575462 +33.474000,1.564463 +33.125000,1.572360 +31.265000,1.569569 +32.089000,1.566893 +32.879000,1.565414 +32.260000,1.530541 +30.818000,1.562887 +31.504000,1.541495 +31.232000,1.560478 +30.467000,1.550002 +29.716000,1.553956 +29.630000,1.567832 +30.245000,1.576910 +30.895000,1.561405 +29.776000,1.582101 +29.367000,1.577669 +29.552000,1.576371 +30.002000,1.549340 +30.416000,1.531859 +30.340000,1.560020 +31.635000,1.561944 +31.084000,1.538890 +31.247000,1.557965 +30.747000,1.533800 +37.415000,1.545496 +30.204000,1.553476 +29.322000,1.539230 +29.662000,1.541975 +29.311000,1.540792 +29.067000,1.535298 +30.682000,1.526891 +29.735000,1.565890 +29.522000,1.543745 +30.362000,1.542908 +29.321000,1.544178 +29.701000,1.542258 +30.394000,1.562363 +29.906000,1.528866 +28.787000,1.564181 +28.993000,1.551899 +29.078000,1.544127 +28.937000,1.557285 +29.434000,1.548242 +28.964000,1.565185 +29.132000,1.554072 +28.961000,1.566474 +27.573000,1.561154 +28.077000,1.568771 +28.606000,1.564295 +27.770000,1.563081 +26.968000,1.545724 +27.990000,1.569752 +27.052000,1.563678 +26.763000,1.561125 +27.332000,1.579675 +26.815000,1.566947 +27.011000,1.553431 +34.883000,1.587921 +28.433000,1.570063 +27.735000,1.563485 +28.517000,1.565120 +27.107000,1.560681 +26.986000,1.553110 +35.778000,1.587408 +29.934000,1.572467 +28.391000,1.570502 +29.050000,1.541885 +28.472000,1.553451 +28.504000,1.559770 +28.225000,1.549390 +29.377000,1.580880 +29.471000,1.563743 +29.309000,1.590810 +29.711000,1.562337 +30.117000,1.562016 +30.096000,1.576218 +30.113000,1.572881 +28.856000,1.589848 +29.600000,1.559634 +29.344000,1.581025 +29.727000,1.588879 +28.921000,1.582426 +28.047000,1.592276 +29.125000,1.574536 +28.798000,1.611506 +28.608000,1.607696 +27.973000,1.600329 +28.152000,1.593377 +28.338000,1.595611 +26.839000,1.648447 +26.458000,1.613381 +32.986000,1.614883 +29.326000,1.585989 +26.513000,1.601688 +27.373000,1.637751 +26.542000,1.596729 +26.719000,1.570620 +27.343000,1.590756 +27.490000,1.598657 +28.156000,1.609553 +27.708000,1.587323 +27.055000,1.573583 +27.633000,1.570846 +27.018000,1.588184 +26.626000,1.596390 +26.590000,1.569033 +26.726000,1.590759 +26.112000,1.570800 +25.099000,1.594271 +25.548000,1.579705 +24.860000,1.598626 +24.086000,1.619432 +23.716000,1.576285 +23.769000,1.630852 +24.015000,1.634186 +24.339000,1.625691 +24.675000,1.597475 +24.851000,1.606869 +25.287000,1.609303 +25.191000,1.606799 +25.742000,1.610058 +25.088000,1.602403 +25.289000,1.620215 +25.762000,1.611780 +25.113000,1.625445 +26.500000,1.614679 +25.409000,1.613636 +26.009000,1.620382 +32.498000,1.606847 +25.676000,1.600789 +25.728000,1.611271 +26.143000,1.596319 +30.115000,1.581269 +26.336000,1.628960 +26.662000,1.589098 +26.731000,1.603819 +26.645000,1.604426 +27.046000,1.570769 +27.492000,1.599138 +28.234000,1.622746 +27.255000,1.606088 +27.330000,1.605473 +27.105000,1.622846 +28.018000,1.607569 +26.659000,1.612938 +26.123000,1.609178 +26.519000,1.607283 +27.212000,1.612857 +27.303000,1.600844 +27.967000,1.586725 +27.192000,1.607461 +27.558000,1.606522 +26.215000,1.582158 +25.728000,1.566301 +26.212000,1.595514 +25.850000,1.610569 +25.163000,1.608741 +25.148000,1.611503 +25.814000,1.606453 +25.499000,1.604275 +25.782000,1.610855 +25.750000,1.579503 +26.187000,1.572891 +25.629000,1.610596 +26.125000,1.582147 +32.388000,1.595272 +26.041000,1.581731 +25.583000,1.589395 +26.541000,1.583061 +27.118000,1.587607 +27.258000,1.593468 +28.764000,1.573738 +28.709000,1.576627 +29.078000,1.573952 +29.897000,1.567386 +29.542000,1.592496 +29.165000,1.584544 +29.676000,1.551211 +30.655000,1.566675 +30.444000,1.550426 +30.438000,1.554443 +30.999000,1.575421 +30.911000,1.574400 +30.567000,1.542067 +31.071000,1.549480 +31.383000,1.564411 +30.720000,1.545004 +31.305000,1.551885 +30.751000,1.558604 +31.275000,1.554879 +30.523000,1.571850 +30.708000,1.525876 +30.682000,1.532417 +30.520000,1.559089 +30.986000,1.566884 +30.396000,1.571161 +30.654000,1.562736 +37.806000,1.537731 +31.229000,1.549564 +32.166000,1.507371 +32.211000,1.569424 +32.288000,1.557155 +32.081000,1.542070 +32.640000,1.518675 +32.748000,1.541587 +31.854000,1.563031 +32.630000,1.541346 +32.109000,1.561384 +33.737000,1.547208 +34.452000,1.569526 +33.729000,1.564793 +33.091000,1.564228 +33.000000,1.563590 +33.079000,1.544530 +33.127000,1.547373 +33.223000,1.536300 +33.350000,1.530389 +33.433000,1.531272 +33.351000,1.536984 +33.385000,1.525749 +37.060000,1.534667 +34.683000,1.570611 +34.090000,1.572378 +34.841000,1.582434 +34.976000,1.555531 +35.175000,1.564372 +36.903000,1.567409 +42.359000,1.604050 +36.265000,1.592582 +38.331000,1.578525 +37.594000,1.558082 +40.284000,1.543516 +37.235000,1.550095 +38.934000,1.565640 +38.365000,1.571839 +37.818000,1.558382 +39.184000,1.549957 +40.042000,1.567696 +39.593000,1.573192 +39.758000,1.567884 +39.979000,1.579151 +40.568000,1.573891 +40.869000,1.590063 +41.715000,1.563388 +41.309000,1.580269 +40.828000,1.564619 +42.075000,1.564789 +41.968000,1.569529 +42.105000,1.570633 +41.642000,1.566695 +41.002000,1.560571 +47.707000,1.556471 +42.221000,1.553281 +41.564000,1.549264 +41.906000,1.576996 +42.669000,1.550934 +42.090000,1.560972 +42.312000,1.576988 +41.630000,1.554254 +41.353000,1.577913 +42.465000,1.562672 +41.394000,1.557823 +41.779000,1.564379 +41.429000,1.550996 +42.259000,1.574417 +42.393000,1.585521 +41.797000,1.593017 +41.378000,1.587050 +41.906000,1.608653 +41.722000,1.588187 +42.383000,1.582066 +40.988000,1.568606 +40.503000,1.567608 +44.779000,1.591856 +41.099000,1.582025 +49.312000,1.589727 +40.096000,1.584350 +40.745000,1.574009 +40.818000,1.560905 +39.762000,1.542081 +38.783000,1.566856 +38.842000,1.578686 +38.923000,1.570683 +39.488000,1.580161 +39.717000,1.558463 +38.665000,1.560972 +38.554000,1.566090 +39.130000,1.575020 +38.176000,1.558117 +38.347000,1.589411 +37.747000,1.558229 +38.280000,1.564723 +37.756000,1.574309 +37.613000,1.560104 +36.394000,1.585170 +36.699000,1.581355 +36.969000,1.560241 +38.189000,1.572020 +36.949000,1.583262 +37.250000,1.598938 +44.158000,1.569521 +36.696000,1.561752 +36.422000,1.556140 +36.380000,1.572706 +36.953000,1.599642 +38.336000,1.585064 +37.296000,1.597923 +37.101000,1.587891 +36.878000,1.561129 +37.029000,1.589003 +37.144000,1.562079 +36.056000,1.571715 +36.633000,1.568802 +36.786000,1.559794 +35.958000,1.539824 +36.652000,1.554309 +36.978000,1.572261 +36.400000,1.548423 +36.175000,1.564260 +35.349000,1.562605 +36.724000,1.561645 +36.321000,1.574517 +35.510000,1.580435 +35.959000,1.581677 +35.608000,1.595746 +35.178000,1.568573 +35.629000,1.574046 +42.457000,1.574906 +35.441000,1.588519 +34.720000,1.598406 +34.579000,1.592359 +35.295000,1.599725 +35.303000,1.587191 +32.479000,1.602953 +28.624000,1.572381 +27.164000,1.574652 +26.779000,1.568433 +26.683000,1.547872 +27.105000,1.546290 +27.056000,1.555905 +27.094000,1.534778 +27.560000,1.526133 +26.712000,1.564447 +26.655000,1.558645 +26.507000,1.567702 +26.978000,1.565965 +26.882000,1.572264 +27.643000,1.520256 +26.737000,1.529832 +27.294000,1.566885 +27.231000,1.567700 +27.492000,1.546928 +28.573000,1.571650 +28.052000,1.558299 +28.553000,1.544331 +28.155000,1.580268 +27.817000,1.576558 +27.936000,1.576448 +28.542000,1.571499 +28.223000,1.533167 +34.259000,1.565663 +27.790000,1.561353 +27.823000,1.561960 +28.114000,1.547703 +28.430000,1.546268 +28.883000,1.542077 +28.457000,1.559387 +27.769000,1.577464 +28.430000,1.572023 +27.921000,1.562889 +28.495000,1.563105 +27.924000,1.552329 +27.624000,1.545117 +28.326000,1.583571 +27.336000,1.556781 +28.351000,1.544115 +28.338000,1.548101 +28.008000,1.549210 +27.855000,1.545209 +27.949000,1.551119 +28.763000,1.561896 +28.080000,1.568584 +28.551000,1.553223 +27.311000,1.565208 +27.465000,1.546277 +27.903000,1.545537 +27.456000,1.537591 +27.095000,1.556372 +27.952000,1.552044 +28.218000,1.588865 +27.837000,1.557866 +27.055000,1.540362 +32.971000,1.541785 +27.242000,1.562042 +26.829000,1.576978 +33.218000,1.585717 +27.214000,1.576130 +27.064000,1.552591 +26.319000,1.571389 +26.966000,1.567792 +26.779000,1.561164 +26.279000,1.556867 +26.507000,1.565454 +26.153000,1.567802 +27.085000,1.555785 +27.098000,1.584283 +27.178000,1.574372 +27.395000,1.575740 +27.495000,1.595777 +27.649000,1.580945 +27.229000,1.576678 +27.069000,1.576997 +26.360000,1.542078 +27.140000,1.564622 +27.833000,1.542155 +28.066000,1.550460 +28.013000,1.567922 +27.898000,1.557640 +27.889000,1.581486 +27.643000,1.569154 +28.337000,1.545944 +28.148000,1.544876 +28.010000,1.561118 +28.378000,1.561681 +28.060000,1.556409 +27.902000,1.586383 +28.017000,1.549266 +27.478000,1.576767 +27.982000,1.571528 +27.189000,1.573162 +27.957000,1.577914 +34.069000,1.577063 +26.987000,1.580522 +26.591000,1.589471 +26.309000,1.601403 +26.828000,1.603254 +26.354000,1.574236 +26.932000,1.552263 +26.549000,1.586185 +26.567000,1.579650 +27.013000,1.569807 +26.956000,1.568990 +26.641000,1.579772 +26.636000,1.578765 +26.788000,1.592434 +27.346000,1.592694 +26.827000,1.576594 +26.937000,1.597322 +26.698000,1.581952 +26.847000,1.594467 +26.580000,1.583085 +26.500000,1.593096 +26.753000,1.594350 +26.234000,1.575924 +26.640000,1.592358 +27.060000,1.571770 +26.663000,1.564300 +26.786000,1.573390 +27.477000,1.582894 +27.790000,1.599226 +27.965000,1.578155 +29.162000,1.601619 +28.856000,1.600849 +28.754000,1.586192 +28.804000,1.600415 +28.712000,1.600504 +28.566000,1.583543 +34.846000,1.605969 +29.134000,1.586575 +29.151000,1.599379 +29.027000,1.613092 +28.425000,1.607401 +28.977000,1.603530 +28.961000,1.611450 +28.422000,1.597218 +28.231000,1.586995 +28.760000,1.573856 +28.415000,1.593113 +29.291000,1.591995 +28.989000,1.577323 +28.720000,1.590884 +28.679000,1.584160 +28.703000,1.575559 +28.633000,1.577261 +28.622000,1.585512 +28.439000,1.590505 +28.538000,1.597469 +28.823000,1.575082 +29.357000,1.578613 +30.515000,1.581674 +31.035000,1.583917 +30.605000,1.588749 +31.672000,1.576913 +30.964000,1.557431 +31.644000,1.558084 +31.414000,1.569852 +31.446000,1.571797 +30.765000,1.583822 +30.835000,1.582597 +30.753000,1.578006 +36.898000,1.574602 +31.309000,1.573153 +31.027000,1.574423 +30.960000,1.570951 +31.269000,1.576236 +31.013000,1.584972 +31.568000,1.575845 +31.521000,1.561800 +31.562000,1.562828 +31.746000,1.565543 +31.922000,1.559586 +30.945000,1.560715 +31.251000,1.564336 +31.273000,1.579297 +31.488000,1.560669 +31.645000,1.574023 +31.231000,1.579339 +31.196000,1.571131 +31.533000,1.562223 +31.286000,1.569210 +31.743000,1.568779 +31.401000,1.560183 +31.308000,1.565035 +31.077000,1.567427 +31.399000,1.565671 +31.827000,1.575287 +31.335000,1.563523 +31.145000,1.572455 +31.504000,1.562585 +31.275000,1.561461 +30.893000,1.579329 +37.599000,1.555551 +30.893000,1.565113 +31.563000,1.569207 \ No newline at end of file diff --git a/script/compare_regression_test_result.py b/script/compare_regression_test_result.py new file mode 100644 index 00000000..94d458e4 --- /dev/null +++ b/script/compare_regression_test_result.py @@ -0,0 +1,68 @@ +"""This script compares two results of a regression test. +""" + +import argparse +import pathlib +import pandas as pd +import matplotlib.pyplot as plt + + +def parse_args(): + parser = argparse.ArgumentParser() + parser.add_argument("current_output_dir", type=pathlib.Path) + parser.add_argument("reference_output_dir", type=pathlib.Path) + return parser.parse_args() + + +if __name__ == "__main__": + args = parse_args() + current_output_dir = args.current_output_dir + reference_output_dir = args.reference_output_dir + + current_df = pd.read_csv(current_output_dir / "result.csv") + reference_df = pd.read_csv(reference_output_dir / "result.csv") + + """ + elapsed_milliseconds,score + 884.714000,3.959331 + 15.413000,3.725043 + ... + """ + + # The first data point in results is slow, so remove it. + current_df = current_df.iloc[1:] + reference_df = reference_df.iloc[1:] + + # calculate the ratio of elapsed time + elapsed_milliseconds_ratio = ( + current_df["elapsed_milliseconds"] / reference_df["elapsed_milliseconds"] + ) + elapsed_milliseconds_ratio_mean = elapsed_milliseconds_ratio.mean() + print(f"{elapsed_milliseconds_ratio_mean=:.3f} (current / reference)") + assert elapsed_milliseconds_ratio_mean < 1.1, "The elapsed time is too slow." + + # plot + plt.plot(current_df["elapsed_milliseconds"], label="current") + plt.plot(reference_df["elapsed_milliseconds"], label="reference") + plt.legend() + plt.xlabel("index") + plt.ylabel("elapsed_milliseconds") + plt.grid() + plt.savefig( + current_output_dir / "elapsed_milliseconds.png", + bbox_inches="tight", + pad_inches=0.05, + ) + plt.close() + + # calculate difference of score + score_diff = current_df["score"] - reference_df["score"] + all_zero = (score_diff == 0).all() + if all_zero: + print("The scores are perfectly the same.") + print("OK") + exit(0) + + score_diff_abs_mean = score_diff.abs().mean() + print(f"{score_diff_abs_mean=:.3f}") + assert score_diff_abs_mean < 0.1, "The score is too different." diff --git a/script/convert_rosbag_to_test_data.py b/script/convert_rosbag_to_test_data.py new file mode 100644 index 00000000..38bc5398 --- /dev/null +++ b/script/convert_rosbag_to_test_data.py @@ -0,0 +1,135 @@ +"""This script converts a rosbag to regression test input data for NDT apps. +""" + +import rosbag2_py +from rclpy.serialization import deserialize_message +from rosidl_runtime_py.utilities import get_message +import argparse +import pathlib +import pandas as pd +from sensor_msgs_py import point_cloud2 +import open3d as o3d +import numpy as np + + +def parse_args(): + parser = argparse.ArgumentParser() + parser.add_argument("rosbag_path", type=pathlib.Path) + parser.add_argument("output_dir", type=pathlib.Path) + return parser.parse_args() + + +if __name__ == "__main__": + args = parse_args() + rosbag_path = args.rosbag_path + output_dir = args.output_dir + + serialization_format = "cdr" + storage_options = rosbag2_py.StorageOptions( + uri=str(rosbag_path), storage_id="sqlite3" + ) + converter_options = rosbag2_py.ConverterOptions( + input_serialization_format=serialization_format, + output_serialization_format=serialization_format, + ) + + reader = rosbag2_py.SequentialReader() + reader.open(storage_options, converter_options) + + topic_types = reader.get_all_topics_and_types() + type_map = { + topic_types[i].name: topic_types[i].type for i in range(len(topic_types)) + } + + target_topics = [ + "/localization/kinematic_state", + "/localization/util/downsample/pointcloud", + ] + storage_filter = rosbag2_py.StorageFilter(topics=target_topics) + reader.set_filter(storage_filter) + + kinematic_state_list = [] + pointcloud_list = [] + + while reader.has_next(): + (topic, data, timestamp_rosbag) = reader.read_next() + msg_type = get_message(type_map[topic]) + msg = deserialize_message(data, msg_type) + timestamp_header = ( + int(msg.header.stamp.sec) + int(msg.header.stamp.nanosec) * 1e-9 + ) + if topic == "/localization/kinematic_state": + pose = msg.pose.pose + twist = msg.twist.twist + kinematic_state_list.append( + { + "timestamp": timestamp_header, + "pose_x": pose.position.x, + "pose_y": pose.position.y, + "pose_z": pose.position.z, + "quat_w": pose.orientation.w, + "quat_x": pose.orientation.x, + "quat_y": pose.orientation.y, + "quat_z": pose.orientation.z, + "twist_linear_x": twist.linear.x, + "twist_linear_y": twist.linear.y, + "twist_linear_z": twist.linear.z, + "twist_angular_x": twist.angular.x, + "twist_angular_y": twist.angular.y, + "twist_angular_z": twist.angular.z, + } + ) + elif topic == "/localization/util/downsample/pointcloud": + pointcloud_list.append( + { + "timestamp": timestamp_header, + "pointcloud": msg, + } + ) + else: + assert False, f"Unknown topic: {topic}" + + print(f"{len(kinematic_state_list)=}") + print(f"{len(pointcloud_list)=}") + + df_kinematic_state = pd.DataFrame(kinematic_state_list) + + output_dir.mkdir(parents=True, exist_ok=True) + (output_dir / "sensor_pcd").mkdir(parents=True, exist_ok=True) + + index_kinematic_state = 0 + used_flag_kinematic_state = [False] * len(kinematic_state_list) + + for i, data in enumerate(pointcloud_list): + timestamp = data["timestamp"] + pointcloud = data["pointcloud"] # sensor_msgs.msg.PointCloud2 + + # find nearest kinematic_state + while index_kinematic_state + 1 < len(kinematic_state_list): + if kinematic_state_list[index_kinematic_state + 1]["timestamp"] > timestamp: + break + index_kinematic_state += 1 + + if index_kinematic_state >= len(kinematic_state_list): + break + + used_flag_kinematic_state[index_kinematic_state] = True + index_kinematic_state += 1 + + # convert pointcloud to pcd format + pcd = point_cloud2.read_points(pointcloud, field_names=("x", "y", "z")) + pcd = np.column_stack((pcd["x"], pcd["y"], pcd["z"])) + o3d_pointcloud = o3d.geometry.PointCloud() + o3d_pointcloud.points = o3d.utility.Vector3dVector(pcd) + + # save pointcloud + output_path = output_dir / "sensor_pcd" / f"pointcloud_{i:08d}.pcd" + o3d.io.write_point_cloud(str(output_path), o3d_pointcloud) + + # save kinematic_state + df_kinematic_state = df_kinematic_state[used_flag_kinematic_state] + df_kinematic_state.reset_index(drop=True, inplace=True) + df_kinematic_state.to_csv( + output_dir / "kinematic_state.csv", index=False, float_format="%.6f" + ) + print(f"Saved to {output_dir.resolve()}") diff --git a/script/download_data.sh b/script/download_data.sh new file mode 100755 index 00000000..691320e6 --- /dev/null +++ b/script/download_data.sh @@ -0,0 +1,19 @@ +#!/bin/bash +set -eux + +cd $(dirname $0)/.. + +mkdir -p regression_test_data +cd regression_test_data + +# Download the input data +wget 'https://drive.google.com/uc?export=download&id=1E-_zj2nchmntioSJJgyoDQEYHtrs3o-C' -O ndt_omp_regression_test_input.zip --quiet +unzip -q ndt_omp_regression_test_input.zip +rm ndt_omp_regression_test_input.zip + +# Download the map data +wget https://github.com/tier4/AWSIM/releases/download/v1.1.0/nishishinjuku_autoware_map.zip --quiet +unzip -q nishishinjuku_autoware_map.zip +rm nishishinjuku_autoware_map.zip +mv nishishinjuku_autoware_map/pointcloud_map.pcd ./input/ +rm -rf nishishinjuku_autoware_map diff --git a/script/execute_regression_test.sh b/script/execute_regression_test.sh new file mode 100755 index 00000000..184af92f --- /dev/null +++ b/script/execute_regression_test.sh @@ -0,0 +1,8 @@ +#!/bin/bash +set -eux + +cd $(dirname $0)/../build + +make -j8 +./regression_test ../regression_test_data/input/ ../regression_test_data/output +python3 ../script/compare_regression_test_result.py ../regression_test_data/output/ ../regression_test_data/reference/