From 94b1081e74413838d8a1f7c86eac556dc44daa72 Mon Sep 17 00:00:00 2001 From: "Jinyu@BUAA" <38695019+jinyummiao@users.noreply.github.com> Date: Fri, 10 Dec 2021 16:29:19 +0800 Subject: [PATCH 1/3] udpate hdmap loader add a fast researcher based on KD-tree in HDMap loader. update hdmap loader, now it can load all information from HDMap files --- CMakeLists.txt | 12 ++- gnss_odom_optimizer.cc | 29 +++---- hdmap_loader.cc | 179 +++++++++++++++++++++++++++++++++++++++++ hdmap_loader.h | 69 ++++++++++++++++ test_loader.cc | 38 +++++++++ 5 files changed, 309 insertions(+), 18 deletions(-) create mode 100644 hdmap_loader.cc create mode 100644 hdmap_loader.h create mode 100644 test_loader.cc diff --git a/CMakeLists.txt b/CMakeLists.txt index 12371ed..c792814 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,15 +2,19 @@ cmake_minimum_required( VERSION 2.8 ) project( pose_graph ) set( CMAKE_BUILD_TYPE Release ) -set( CMAKE_CXX_FLAGS "-std=c++11 -g -O3" ) +#set( CMAKE_CXX_FLAGS "-std=c++11 -g -O3" ) +set( CMAKE_CXX_STANDARD 14 ) include_directories( "/usr/include/eigen3" ) -find_package(OpenCV) +find_package(OpenCV 4.3.0 REQUIRED) include_directories(${OpenCV_INCLUDE_DIRS}) find_package(Ceres) include_directories(${CERES_INCLUDE_DIRS}) -add_executable(gnss_odom_optimizer gnss_odom_optimizer.cc pose_local_parameterization.cc io.cc regress_plane.cc) -target_link_libraries(gnss_odom_optimizer ${CERES_LIBRARIES}) \ No newline at end of file +add_executable(gnss_odom_optimizer gnss_odom_optimizer.cc pose_local_parameterization.cc io.cc hdmap_loader.cc) +target_link_libraries(gnss_odom_optimizer ${CERES_LIBRARIES} ${OpenCV_LIBS}) + +#add_executable(test_loader test_loader.cc hdmap_loader.cc) +#target_link_libraries(test_loader ${OpenCV_LIBS}) diff --git a/gnss_odom_optimizer.cc b/gnss_odom_optimizer.cc index 14b971b..dfa8294 100644 --- a/gnss_odom_optimizer.cc +++ b/gnss_odom_optimizer.cc @@ -6,7 +6,7 @@ #include "relative_pose_factor.h" #include "gnss_data_factor.h" #include "height_factor.h" -#include "regress_plane.h" +#include "hdmap_loader.h" int main() { @@ -48,20 +48,21 @@ int main() { } // Uncomment to enable the plane constraint. - // HDMapDataBase hdmap_database("../data/hdmap"); - - // double sensor_plane_to_body = 1.7; - // std::vector > pose_timestamps = pose_graph.pose_timestamps(); - // for (const auto & pose_timestamp : pose_timestamps) { - // int id = pose_timestamp.first; - // Eigen::Matrix pose = pose_graph.get_pose(id); - // Eigen::Vector3d xyz = pose.block<3,1>(0,3); - // double height; - // hdmap_database.construct_plane_height_constraint(xyz, 50., height); - // HeightFactor * factor = new HeightFactor(height, sensor_plane_to_body); - // pose_graph.add_observation_factor(id, factor); - // } + HDMapDataBase hdmap_database("/home/jinyu/Documents/pose_graph/data/100101-bxdp_line.utm.txt"); + double sensor_plane_to_body = 1.7; + std::vector > pose_timestamps = pose_graph.pose_timestamps(); + for (const auto & pose_timestamp : pose_timestamps) { + int id = pose_timestamp.first; + Eigen::Matrix pose = pose_graph.get_pose(id); + Eigen::Vector3d xyz = pose.block<3,1>(0,3); + double height; + hdmap_database.construct_plane_height_constraint(xyz, 50, height, "lane"); + std::cout << height << std::endl; + HeightFactor * factor = new HeightFactor(height, sensor_plane_to_body); + pose_graph.add_observation_factor(id, factor); + } + std::cout << "done." << std::endl; // Solve the pose graph. pose_graph.solve(); diff --git a/hdmap_loader.cc b/hdmap_loader.cc new file mode 100644 index 0000000..1985ad2 --- /dev/null +++ b/hdmap_loader.cc @@ -0,0 +1,179 @@ +#include "hdmap_loader.h" + +std::vector string_split(const std::string &str, const char *delim) +{ + std::vector strlist; + int size = str.size(); + char *input = new char[size+1]; + strcpy(input, str.c_str()); + char *token = std::strtok(input, delim); + while (token != NULL) { + strlist.push_back(token); + token = std::strtok(NULL, delim); + } + delete []input; + return strlist; +} + +cv::Mat convert_to_mat(std::vector element_dict) +{ + std::vector element_xy; + for (int i=0; i txt_path) { + for (int i=0; i keys; + std::vector> element_data_; + while (getline(inFile, x)) + { + std::vector line = string_split(x, ","); + if (line[0] == "fid") + { + keys = line; + continue; // skip head + } + std::map data_; + for (int i=0; i tmp = string_split(txt_path, "/."); + std::string elem_type = tmp[tmp.size() - 3]; + hdmap_[elem_type] = element_data_; +} + +void HDMapDataBase::get_element_from_hdmap(std::string element_key) +{ + std::vector> element_dict_str_ = hdmap_[element_key_map_[element_key]]; + for (int i=0; i xyz_str = string_split(element_dict_str_[i]["xyz"], " "); + for (int j=0; j HDMapDataBase::find_neighbors(Eigen::Vector3d camera_xyz, double radius) { + std::vector near_elem; + for (int i=0; i HDMapDataBase::find_neighbors(Eigen::Vector3d camera_xyz, int query_topk) { + std::vector query {static_cast(camera_xyz[0]), static_cast(camera_xyz[1])}; + std::vector retrieved_idx(query_topk); + std::vector retrieved_dst(query_topk); + cv::flann::SearchParams params(32); + pkdtree_->knnSearch(query, retrieved_idx, retrieved_dst, query_topk, params); + std::vector near_elem; + for (auto &r: retrieved_idx) + near_elem.push_back(element_dict_[element_idx_[r]]); + return near_elem; +} + +double calc_dst_point_to_plane(Eigen::Vector3d camera_xyz, Eigen::Vector4d coeff) { + double d = fabs((camera_xyz.dot(coeff.topRows(3))+ coeff[3]) / coeff.topRows(3).norm()); + return d; +} + +// https://www.licc.tech/article?id=72 +// ax+by+cz+d=0 +Eigen::Vector4d plane_fitting(const std::vector & plane_pts) { + Eigen::Vector3d center = Eigen::Vector3d::Zero(); + for (const auto & pt : plane_pts) center += pt; + center /= plane_pts.size(); + + Eigen::MatrixXd A(plane_pts.size(), 3); + for (int i = 0; i < plane_pts.size(); i++) { + A(i, 0) = plane_pts[i][0] - center[0]; + A(i, 1) = plane_pts[i][1] - center[1]; + A(i, 2) = plane_pts[i][2] - center[2]; + } + + Eigen::JacobiSVD svd(A, Eigen::ComputeThinV); + const float a = svd.matrixV()(0, 2); + const float b = svd.matrixV()(1, 2); + const float c = svd.matrixV()(2, 2); + const float d = -(a * center[0] + b * center[1] + c * center[2]); + return Eigen::Vector4d(a, b, c, d); +} + +bool HDMapDataBase::construct_plane_height_constraint(Eigen::Vector3d camera_xyz, double radius, + double & camera_height, std::string element_key) { + get_element_from_hdmap(element_key); + std::vector near_elem = find_neighbors(camera_xyz, radius); + + std::vector near_xyz; + for (auto & elem: near_elem) + for (int i=0; i near_elem = find_neighbors(camera_xyz, query_topk); + + std::vector near_xyz; + for (auto & elem: near_elem) + for (int i=0; i HDMapDataBase::get_element_dict() { + return element_dict_; +} \ No newline at end of file diff --git a/hdmap_loader.h b/hdmap_loader.h new file mode 100644 index 0000000..e3a9c1d --- /dev/null +++ b/hdmap_loader.h @@ -0,0 +1,69 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +typedef struct +{ + int fid; + std::vector xyz; +} Element; + +typedef struct +{ + int node_id; + std::vector inlier_elem; + Eigen::Vector3d center; + int parent_id; + std::vector subnodes_id; +} Node; + +class HDMapDataBase { + + public: + + HDMapDataBase() = delete; + + HDMapDataBase(std::string hdmap_path); + + HDMapDataBase(std::vector hdmap_paths); + + void get_element_from_hdmap(std::string element_key); + + std::vector get_element_dict(); + + std::vector find_neighbors(Eigen::Vector3d camera_xyz, double radius); + + std::vector find_neighbors(Eigen::Vector3d camera_xyz, int query_topk); + + bool construct_plane_height_constraint(Eigen::Vector3d camera_xyz, double radius, double & height, std::string element_key); + + bool construct_plane_height_constraint(Eigen::Vector3d camera_xyz, int query_topk, double & camera_height, std::string element_key); + private: + std::map>> hdmap_; + std::vector element_dict_; + std::vector element_idx_; + cv::flann::Index* pkdtree_; + std::map element_key_map_ = { + {"lane", "100101-bxdp_line"}, + {"arrow", "100109-bxdp_jt"}, + {"lg", "200102-hldp_lg"}, + {"lightpole", "400104-zmlddp"}, + {"xsbz", "400107-jcjdp_xsbz"}, + {"roadside", "600110-lysdp"}, + {"md", "600111-md"}, + {"camera", "800101-sxtdp"} + }; + + void parser_hdmap(std::string txt_path); + + void build_kdtree(); + +}; diff --git a/test_loader.cc b/test_loader.cc new file mode 100644 index 0000000..3e0bcff --- /dev/null +++ b/test_loader.cc @@ -0,0 +1,38 @@ +#include +#include + +#include "chrono" +#include "hdmap_loader.h" + +int main() { + // Uncomment to enable the plane constraint. + std::vector hdmap_paths { + "/home/jinyu/Documents/pose_graph/data/100101-bxdp_line.utm.txt", + "/home/jinyu/Documents/pose_graph/data/100109-bxdp_jt.utm.txt" + }; + HDMapDataBase hdmap_database(hdmap_paths); + hdmap_database.get_element_from_hdmap("lane"); + auto element_dict = hdmap_database.get_element_dict(); + Element test_sample = element_dict[701]; + Eigen::Vector3d test_xyz(3); + test_xyz << test_sample.xyz[0], test_sample.xyz[1], test_sample.xyz[2]; + std::cout << "TEST: " << test_xyz.transpose() << " " << test_sample.fid << std::endl; + auto t1 = std::chrono::steady_clock::now(); + auto neighbors = hdmap_database.find_neighbors(test_xyz, 5); + auto t2 = std::chrono::steady_clock::now(); + for (auto &n: neighbors) + std::cout << "FIND: " << n.xyz[0].transpose() << " " << n.fid << std::endl; + double totaltime = std::chrono::duration(t2 - t1).count(); + std::cout << "Time: " << totaltime << std::endl; + std::cout << std::endl; + + std::cout << "TEST: " << test_xyz.transpose() << " " << test_sample.fid << std::endl; + auto t3 = std::chrono::steady_clock::now(); + auto neighbors2 = hdmap_database.find_neighbors(test_xyz, 2.5); + auto t4 = std::chrono::steady_clock::now(); + for (auto &n: neighbors2) + std::cout << "FIND: " << n.xyz[0].transpose() << " " << n.fid << std::endl; + double totaltime2 = std::chrono::duration(t4 - t3).count(); + std::cout << "Time: " << totaltime2 << std::endl; + return 0; +} \ No newline at end of file From d9874f8c221e483145ec67d5f1f8b38a8bbfb7db Mon Sep 17 00:00:00 2001 From: "Jinyu@BUAA" <38695019+jinyummiao@users.noreply.github.com> Date: Fri, 10 Dec 2021 16:29:38 +0800 Subject: [PATCH 2/3] Delete regress_plane.cc --- regress_plane.cc | 103 ----------------------------------------------- 1 file changed, 103 deletions(-) delete mode 100644 regress_plane.cc diff --git a/regress_plane.cc b/regress_plane.cc deleted file mode 100644 index 37bd2a1..0000000 --- a/regress_plane.cc +++ /dev/null @@ -1,103 +0,0 @@ -#include "regress_plane.h" - -std::vector string_split(const std::string &str, const char *delim) -{ - std::vector strlist; - int size = str.size(); - char *input = new char[size+1]; - strcpy(input, str.c_str()); - char *token = std::strtok(input, delim); - while (token != NULL) { - strlist.push_back(token); - token = std::strtok(NULL, delim); - } - delete []input; - return strlist; -} - -HDMapDataBase::HDMapDataBase(std::string txt_path) { - load_lane_from_hdmap(txt_path); -} - -void HDMapDataBase::load_lane_from_hdmap(std::string txt_path) -{ - std::ifstream inFile(txt_path, std::ios::in); - std::string x; - while (getline(inFile, x)) - { - Lane lane; - std::vector line = string_split(x, ","); - if (line[0] == "fid(T)") - continue; // skip head - lane.id = stoi(line[0]); - std::vector xyz_str = string_split(line[1], " "); - for (int i=0; i & xyz) -{ - xyz.clear(); - for (int i=0; i HDMapDataBase::find_neighbors(Eigen::Vector3d camera_xyz, double radius) -{ - std::vector near_xyz; - for (int i=0; i & plane_pts) -{ - Eigen::Vector3d center = Eigen::Vector3d::Zero(); - for (const auto & pt : plane_pts) center += pt; - center /= plane_pts.size(); - - Eigen::MatrixXd A(plane_pts.size(), 3); - for (int i = 0; i < plane_pts.size(); i++) { - A(i, 0) = plane_pts[i][0] - center[0]; - A(i, 1) = plane_pts[i][1] - center[1]; - A(i, 2) = plane_pts[i][2] - center[2]; - } - - Eigen::JacobiSVD svd(A, Eigen::ComputeThinV); - const float a = svd.matrixV()(0, 2); - const float b = svd.matrixV()(1, 2); - const float c = svd.matrixV()(2, 2); - const float d = -(a * center[0] + b * center[1] + c * center[2]); - return Eigen::Vector4d(a, b, c, d); -} - -bool HDMapDataBase::construct_plane_height_constraint(Eigen::Vector3d camera_xyz, double radius, double & camera_height) { - std::vector near_xyz = find_neighbors(camera_xyz, radius); - Eigen::Vector4d coeff = plane_fitting(near_xyz); - camera_height = calc_dst_point_to_plane(camera_xyz, coeff); - return true; -} \ No newline at end of file From 1f1c6b02da72fd1963ed468971874d1d21c91df3 Mon Sep 17 00:00:00 2001 From: "Jinyu@BUAA" <38695019+jinyummiao@users.noreply.github.com> Date: Fri, 10 Dec 2021 16:29:48 +0800 Subject: [PATCH 3/3] Delete regress_plane.h --- regress_plane.h | 43 ------------------------------------------- 1 file changed, 43 deletions(-) delete mode 100644 regress_plane.h diff --git a/regress_plane.h b/regress_plane.h deleted file mode 100644 index 01e9eb5..0000000 --- a/regress_plane.h +++ /dev/null @@ -1,43 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -typedef struct -{ - int id; - std::vector xyz; -} Lane; - -typedef struct -{ - double timestamp; - Eigen::Vector3d xyz; - Eigen::Matrix3d rot; -} Pose; - -class HDMapDataBase { - - public: - - HDMapDataBase() = delete; - - HDMapDataBase(std::string hdmap_path); - - void load_lane_from_hdmap(std::string txt_path); - - void get_xyz_from_lane(std::vector & xyz); - - std::vector find_neighbors(Eigen::Vector3d camera_xyz, double radius); - - bool construct_plane_height_constraint(Eigen::Vector3d camera_xyz, double radius, double & height); - - private: - std::vector lane_dict_; -};