From 12a2d457dcfb6e24ce56c745b295f967112b113d Mon Sep 17 00:00:00 2001 From: janusz Date: Sat, 18 Jan 2025 19:35:33 +0100 Subject: [PATCH] work on lo --- CMakeLists.txt | 1 + apps/lidar_odometry_step_1/lidar_odometry.cpp | 204 +- .../lidar_odometry_utils.cpp | 76 + .../lidar_odometry_utils.h | 19 + .../lidar_odometry_utils_optimizers.cpp | 969 +++++++-- .../CMakeLists.txt | 52 + .../livox_mid_360_intrinsic_calibration.cpp | 1790 +++++++++++++++++ .../mandeye_mission_recorder_calibration.cpp | 4 +- .../mandeye_raw_data_viewer.cpp | 1157 +++++++++-- 9 files changed, 3683 insertions(+), 589 deletions(-) create mode 100644 apps/livox_mid_360_intrinsic_calibration/CMakeLists.txt create mode 100644 apps/livox_mid_360_intrinsic_calibration/livox_mid_360_intrinsic_calibration.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index f47e324..63d4144 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -105,6 +105,7 @@ add_subdirectory(apps/compare_trajectories) add_subdirectory(apps/quick_start_demo) add_subdirectory(apps/mandeye_mission_recorder_calibration) add_subdirectory(apps/mandeye_single_session_viewer) +add_subdirectory(apps/livox_mid_360_intrinsic_calibration) # CPack configuration set(CPACK_PACKAGE_NAME "hd_mapping") diff --git a/apps/lidar_odometry_step_1/lidar_odometry.cpp b/apps/lidar_odometry_step_1/lidar_odometry.cpp index ec38f61..e3213fb 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry.cpp @@ -994,6 +994,14 @@ void lidar_odometry_gui() } if (!simple_gui) { + ImGui::Text("------- robust and accurate lidar odometry -----------"); + ImGui::Checkbox("use_robust_and_accurate_lidar_odometry", ¶ms.use_robust_and_accurate_lidar_odometry); + ImGui::InputDouble("distance_bucket", ¶ms.distance_bucket); + ImGui::InputDouble("polar_angle_deg", ¶ms.polar_angle_deg); + ImGui::InputDouble("azimutal_angle_deg", ¶ms.azimutal_angle_deg); + ImGui::InputInt("number of iterations", ¶ms.robust_and_accurate_lidar_odometry_iterations); + ImGui::InputDouble("max distance lidar", ¶ms.max_distance_lidar); + ImGui::Text("------------------------------------------------------"); // if (step_1_done && step_2_done) //{ /*ImGui::Text("'Consistency' makes trajectory smooth, point cloud will be more consistent"); @@ -2369,28 +2377,6 @@ void alternative_approach() std::cout << fn << std::endl; } - // for (const auto &fn : laz_files) - //{ - // std::vector points = load_point_cloud(fn); - // std::cout << "points.cloud(): " << points.size() << std::endl; - //} - - /*int current_file_index = 0; - int current_point_index = 0; - - std::vector points; - - bool do_not_stop = true; - while (do_not_stop){ - bool do_not_stop_internal_loop = true; - while (do_not_stop_internal_loop){ - bool collected_all_points = get_next_batch_of_points( - point_count_threshold, laz_files, current_file_index, int ¤t_point_index_offset, - std::vector &points) - } - }*/ - // get_next_batch_of_points(point_count_threshold, laz_files, current_file_index, current_point_index, points); - std::vector prev_points; std::vector> all_points; std::vector> tmp_points = get_batches_of_points(laz_files[0], point_count_threshold, prev_points); @@ -2508,177 +2494,3 @@ Eigen::Vector3d rayIntersection(const LaserBeam &laser_beam, const RegistrationP return out_point; } - -#if 0 -bool exportLaz(const std::string &filename, - const std::vector &pointcloud, - const std::vector &intensity, - const std::vector ×tamps, - double offset_x, double offset_y, double offset_alt) -{ - - constexpr float scale = 0.0001f; // one tenth of milimeter - // find max - Eigen::Vector3d _max(-1000000000.0, -1000000000.0, -1000000000.0); - Eigen::Vector3d _min(1000000000.0, 1000000000.0, 1000000000.0); - - for (auto &p : pointcloud) - { - if (p.x() < _min.x()) - { - _min.x() = p.x(); - } - if (p.y() < _min.y()) - { - _min.y() = p.y(); - } - if (p.z() < _min.z()) - { - _min.z() = p.z(); - } - - if (p.x() > _max.x()) - { - _max.x() = p.x(); - } - if (p.y() > _max.y()) - { - _max.y() = p.y(); - } - if (p.z() > _max.z()) - { - _max.z() = p.z(); - } - } - - // create the writer - laszip_POINTER laszip_writer; - if (laszip_create(&laszip_writer)) - { - fprintf(stderr, "DLL ERROR: creating laszip writer\n"); - return false; - } - - // get a pointer to the header of the writer so we can populate it - - laszip_header *header; - - if (laszip_get_header_pointer(laszip_writer, &header)) - { - fprintf(stderr, "DLL ERROR: getting header pointer from laszip writer\n"); - return false; - } - - // populate the header - - header->file_source_ID = 4711; - header->global_encoding = (1 << 0); // see LAS specification for details - header->version_major = 1; - header->version_minor = 2; - // header->file_creation_day = 120; - // header->file_creation_year = 2013; - header->point_data_format = 1; - header->point_data_record_length = 0; - header->number_of_point_records = pointcloud.size(); - header->number_of_points_by_return[0] = pointcloud.size(); - header->number_of_points_by_return[1] = 0; - header->point_data_record_length = 28; - header->x_scale_factor = scale; - header->y_scale_factor = scale; - header->z_scale_factor = scale; - - header->max_x = _max.x() + offset_x; - header->min_x = _min.x() + offset_x; - header->max_y = _max.y() + offset_y; - header->min_y = _min.y() + offset_y; - header->max_z = _max.z() + offset_alt; - header->min_z = _min.z() + offset_alt; - - header->x_offset = offset_x; - header->y_offset = offset_y; - header->z_offset = offset_alt; - - // optional: use the bounding box and the scale factor to create a "good" offset - // open the writer - laszip_BOOL compress = (strstr(filename.c_str(), ".laz") != 0); - - if (laszip_open_writer(laszip_writer, filename.c_str(), compress)) - { - fprintf(stderr, "DLL ERROR: opening laszip writer for '%s'\n", filename.c_str()); - return false; - } - - fprintf(stderr, "writing file '%s' %scompressed\n", filename.c_str(), (compress ? "" : "un")); - - // get a pointer to the point of the writer that we will populate and write - - laszip_point *point; - if (laszip_get_point_pointer(laszip_writer, &point)) - { - fprintf(stderr, "DLL ERROR: getting point pointer from laszip writer\n"); - return false; - } - - laszip_I64 p_count = 0; - laszip_F64 coordinates[3]; - - for (int i = 0; i < pointcloud.size(); i++) - { - point->intensity = intensity[i]; - - const auto &p = pointcloud[i]; - point->gps_time = timestamps[i] * 1e9; - - p_count++; - coordinates[0] = p.x() + offset_x; - coordinates[1] = p.y() + offset_y; - coordinates[2] = p.z() + offset_alt; - if (laszip_set_coordinates(laszip_writer, coordinates)) - { - fprintf(stderr, "DLL ERROR: setting coordinates for point %I64d\n", p_count); - return false; - } - - // p.SetIntensity(pp.intensity); - - // if (i < intensity.size()) { - // point->intensity = intensity[i]; - // } - // laszip_set_point - - if (laszip_write_point(laszip_writer)) - { - fprintf(stderr, "DLL ERROR: writing point %I64d\n", p_count); - return false; - } - } - - if (laszip_get_point_count(laszip_writer, &p_count)) - { - fprintf(stderr, "DLL ERROR: getting point count\n"); - return false; - } - - fprintf(stderr, "successfully written %I64d points\n", p_count); - - // close the writer - - if (laszip_close_writer(laszip_writer)) - { - fprintf(stderr, "DLL ERROR: closing laszip writer\n"); - return false; - } - - // destroy the writer - - if (laszip_destroy(laszip_writer)) - { - fprintf(stderr, "DLL ERROR: destroying laszip writer\n"); - return false; - } - - std::cout << "exportLaz DONE" << std::endl; - - return true; -} -#endif \ No newline at end of file diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp b/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp index 8e9aa82..085a02f 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp @@ -183,6 +183,82 @@ void update_rgd(NDT::GridParameters &rgd_params, NDTBucketMapType &buckets, } } +void update_rgd_spherical_coordinates(NDT::GridParameters &rgd_params, NDTBucketMapType &buckets, + std::vector &points_global, std::vector &points_global_spherical, Eigen::Vector3d viewport) +{ + Eigen::Vector3d b(rgd_params.resolution_X, rgd_params.resolution_Y, rgd_params.resolution_Z); + + for (int i = 0; i < points_global.size(); i++) + { + auto index_of_bucket = get_rgd_index(points_global_spherical[i], b); + + auto bucket_it = buckets.find(index_of_bucket); + + if (bucket_it != buckets.end()) + { + auto &this_bucket = bucket_it->second; + this_bucket.number_of_points++; + const auto &curr_mean = points_global[i].point; + const auto &mean = this_bucket.mean; + // buckets[index_of_bucket].mean += (mean - curr_mean) / buckets[index_of_bucket].number_of_points; + + auto mean_diff = mean - curr_mean; + Eigen::Matrix3d cov_update; + cov_update.row(0) = mean_diff.x() * mean_diff; + cov_update.row(1) = mean_diff.y() * mean_diff; + cov_update.row(2) = mean_diff.z() * mean_diff; + + // this_bucket.cov = this_bucket.cov * (this_bucket.number_of_points - 1) / this_bucket.number_of_points + + // cov_update * (this_bucket.number_of_points - 1) / (this_bucket.number_of_points * this_bucket.number_of_points); + + if (this_bucket.number_of_points == 2) + { + this_bucket.cov = this_bucket.cov * (this_bucket.number_of_points - 1) / this_bucket.number_of_points + + cov_update * (this_bucket.number_of_points - 1) / (this_bucket.number_of_points * this_bucket.number_of_points); + } + + if (this_bucket.number_of_points == 3) + { + this_bucket.cov = this_bucket.cov * (this_bucket.number_of_points - 1) / this_bucket.number_of_points + + cov_update * (this_bucket.number_of_points - 1) / (this_bucket.number_of_points * this_bucket.number_of_points); + + // calculate normal vector + Eigen::SelfAdjointEigenSolver eigen_solver(this_bucket.cov, Eigen::ComputeEigenvectors); + Eigen::Matrix3d eigenVectorsPCA = eigen_solver.eigenvectors(); + + Eigen::Vector3d nv = eigenVectorsPCA.col(1).cross(eigenVectorsPCA.col(2)); + nv.normalize(); + + // flip towards viewport + if (nv.dot(viewport - this_bucket.mean) < 0.0) + { + nv *= -1.0; + } + this_bucket.normal_vector = nv; + } + + if (this_bucket.number_of_points > 3) + { + Eigen::Vector3d &nv = this_bucket.normal_vector; + + if (nv.dot(viewport - this_bucket.mean) >= 0.0) + { + this_bucket.cov = this_bucket.cov * (this_bucket.number_of_points - 1) / this_bucket.number_of_points + + cov_update * (this_bucket.number_of_points - 1) / (this_bucket.number_of_points * this_bucket.number_of_points); + } + } + } + else + { + NDT::Bucket bucket_to_add; + bucket_to_add.mean = points_global[i].point; + bucket_to_add.cov = Eigen::Matrix3d::Identity() * 0.03 * 0.03; + bucket_to_add.number_of_points = 1; + buckets.emplace(index_of_bucket, bucket_to_add); + } + } +} + bool saveLaz(const std::string &filename, const WorkerData &data, double threshould_output_filter) { constexpr float scale = 0.0001f; // one tenth of milimeter diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils.h b/apps/lidar_odometry_step_1/lidar_odometry_utils.h index dd16ac0..c74125d 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils.h +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils.h @@ -66,6 +66,14 @@ struct LidarOdometryParams double sliding_window_trajectory_length_threshold = 50.0; bool save_calibration_validation = true; int calibration_validation_points = 1000000; + + //rgd_sf + bool use_robust_and_accurate_lidar_odometry = false; + double distance_bucket = 0.5; + double polar_angle_deg = 5.0; + double azimutal_angle_deg = 5.0; + int robust_and_accurate_lidar_odometry_iterations = 20; + double max_distance_lidar = 30.0; }; unsigned long long int get_index(const int16_t x, const int16_t y, const int16_t z); @@ -81,6 +89,9 @@ std::vector decimate(const std::vector &points, double bucke void update_rgd(NDT::GridParameters &rgd_params, NDTBucketMapType &buckets, std::vector &points_global, Eigen::Vector3d viewport = Eigen::Vector3d(0, 0, 0)); +void update_rgd_spherical_coordinates(NDT::GridParameters &rgd_params, NDTBucketMapType &buckets, + std::vector &points_global, std::vector &points_global_spherical, Eigen::Vector3d viewport); + //! This function load inertial measurement unit data. //! This function expects a file with the following format: //! timestamp angular_velocity_x angular_velocity_y angular_velocity_z linear_acceleration_x linear_acceleration_y linear_acceleration_z imu_id @@ -116,6 +127,14 @@ void optimize(std::vector &intermediate_points, std::vector> &imu_roll_pitch*/ ); +void optimize_sf(std::vector &intermediate_points, std::vector &intermediate_trajectory, + std::vector &intermediate_trajectory_motion_model, + NDT::GridParameters &rgd_params, NDTBucketMapType &buckets, bool useMultithread ); + +void optimize_sf2(std::vector &intermediate_points, std::vector &intermediate_points_sf, std::vector &intermediate_trajectory, + std::vector &intermediate_trajectory_motion_model, + NDT::GridParameters &rgd_params, bool useMultithread); + void optimize_icp(std::vector &intermediate_points, std::vector &intermediate_trajectory, std::vector &intermediate_trajectory_motion_model, NDT::GridParameters &rgd_params, /*NDTBucketMapType &buckets*/ std::vector points_global, bool useMultithread /*, diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp index 39ccebd..ddbb322 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp @@ -415,6 +415,686 @@ void optimize_icp(std::vector &intermediate_points, std::vector &intermediate_points, std::vector &intermediate_trajectory, + std::vector &intermediate_trajectory_motion_model, + NDT::GridParameters &rgd_params, NDTBucketMapType &buckets_, bool multithread) +{ + // std::cout << "optimize_sf" << std::endl; + + auto int_tr = intermediate_trajectory; + auto int_tr_tmp = intermediate_trajectory; + auto int_tr_mm = intermediate_trajectory_motion_model; + + // NDT::GridParameters rgd_params; + // rgd_params.resolution_X = 0.3; // distance bucket + // rgd_params.resolution_Y = 0.3; // polar angle deg + // rgd_params.resolution_Z = 0.3; // azimutal angle deg + // rgd_params.resolution_X = 0.5; // distance bucket + // rgd_params.resolution_Y = 5.0; // polar angle deg + // rgd_params.resolution_Z = 5.0; // azimutal angle deg + + std::vector point_cloud_global; + std::vector points_local; + + std::vector point_cloud_global_sc; + // std::vector points_local_sc; + + for (int i = 0; i < intermediate_points.size(); i++) + { + double r_l = intermediate_points[i].point.norm(); + if (r_l > 0.5 && intermediate_points[i].index_pose != -1 && r_l < 30) + { + double polar_angle_deg_l = atan2(intermediate_points[i].point.y(), intermediate_points[i].point.x()) / M_PI * 180.0; + double azimutal_angle_deg_l = acos(intermediate_points[i].point.z() / r_l) / M_PI * 180.0; + + Eigen::Vector3d pp = intermediate_points[i].point; + // pps.x() = r; + // pps.y() = polar_angle_deg; + // pps.z() = azimutal_angle_deg; + // point_cloud_spherical_coordinates.push_back(pps); + + Eigen::Affine3d pose = intermediate_trajectory[intermediate_points[i].index_pose]; + + pp = pose * pp; + + Point3Di pg = intermediate_points[i]; + pg.point = pp; + + point_cloud_global.push_back(pg); + points_local.push_back(intermediate_points[i]); + + /////////////////////////////////////////////////////// + Point3Di p_sl = intermediate_points[i]; + p_sl.point.x() = r_l; + p_sl.point.y() = polar_angle_deg_l; + p_sl.point.z() = azimutal_angle_deg_l; + + // points_local_sc.push_back(p_sl); + // + double r_g = pg.point.norm(); + double polar_angle_deg_g = atan2(pg.point.y(), pg.point.x()) / M_PI * 180.0; + double azimutal_angle_deg_g = acos(pg.point.z() / r_g) / M_PI * 180.0; + + Eigen::Vector3d p_sg = intermediate_points[i].point; + p_sg.x() = r_g; + p_sg.y() = polar_angle_deg_g; + p_sg.z() = azimutal_angle_deg_g; + + point_cloud_global_sc.push_back(p_sg); + } + } + + NDTBucketMapType buckets; + update_rgd_spherical_coordinates(rgd_params, buckets, point_cloud_global, point_cloud_global_sc, {0, 0, 0}); + // update_rgd(rgd_params, buckets, point_cloud_global, {0, 0, 0}); + // std::cout << "buckets.size(): " << buckets.size() << std::endl; + + std::vector> tripletListA; + std::vector> tripletListP; + std::vector> tripletListB; + std::vector my_mutex(1); + + Eigen::Vector3d b(rgd_params.resolution_X, rgd_params.resolution_Y, rgd_params.resolution_Z); + + const auto hessian_fun = [&](const Point3Di &intermediate_points_i) + { + int ir = tripletListB.size(); + double delta_x; + double delta_y; + double delta_z; + + Eigen::Affine3d m_pose = intermediate_trajectory[intermediate_points_i.index_pose]; + Eigen::Vector3d point_local(intermediate_points_i.point.x(), intermediate_points_i.point.y(), intermediate_points_i.point.z()); + Eigen::Vector3d point_global = m_pose * point_local; + + /////////////// + double r = point_global.norm(); + double polar_angle_deg = atan2(point_global.y(), point_global.x()) / M_PI * 180.0; + double azimutal_angle_deg = acos(point_global.z() / r) / M_PI * 180.0; + /////////////// + + auto index_of_bucket = get_rgd_index({r, polar_angle_deg, azimutal_angle_deg}, b); + // auto index_of_bucket = get_rgd_index(point_global, b); + + auto bucket_it = buckets.find(index_of_bucket); + // no bucket found + if (bucket_it == buckets.end()) + { + return; + } + auto &this_bucket = bucket_it->second; + + Eigen::Vector3d mean(this_bucket.mean.x(), this_bucket.mean.y(), this_bucket.mean.z()); + + Eigen::Matrix jacobian; + TaitBryanPose pose_s = pose_tait_bryan_from_affine_matrix(m_pose); + + point_to_point_source_to_target_tait_bryan_wc(delta_x, delta_y, delta_z, + pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, + point_local.x(), point_local.y(), point_local.z(), mean.x(), mean.y(), mean.z()); + + point_to_point_source_to_target_tait_bryan_wc_jacobian(jacobian, + pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, + point_local.x(), point_local.y(), point_local.z()); + std::mutex &m = my_mutex[0]; // mutexes[intermediate_points_i.index_pose]; + std::unique_lock lck(m); + + int c = intermediate_points_i.index_pose * 6; + for (int row = 0; row < 3; row++) + { + for (int col = 0; col < 6; col++) + { + if (jacobian(row, col) != 0.0) + { + tripletListA.emplace_back(ir + row, c + col, -jacobian(row, col)); + } + } + } + + Eigen::Matrix3d infm = this_bucket.cov.inverse(); + + tripletListB.emplace_back(ir, 0, delta_x); + tripletListB.emplace_back(ir + 1, 0, delta_y); + tripletListB.emplace_back(ir + 2, 0, delta_z); + + tripletListP.emplace_back(ir, ir, infm(0, 0)); + tripletListP.emplace_back(ir, ir + 1, infm(0, 1)); + tripletListP.emplace_back(ir, ir + 2, infm(0, 2)); + tripletListP.emplace_back(ir + 1, ir, infm(1, 0)); + tripletListP.emplace_back(ir + 1, ir + 1, infm(1, 1)); + tripletListP.emplace_back(ir + 1, ir + 2, infm(1, 2)); + tripletListP.emplace_back(ir + 2, ir, infm(2, 0)); + tripletListP.emplace_back(ir + 2, ir + 1, infm(2, 1)); + tripletListP.emplace_back(ir + 2, ir + 2, infm(2, 2)); + }; + + if (points_local.size() > 100) + { + // std::cout << "start adding lidar observations" << std::endl; + if (multithread) + { + std::for_each(std::execution::par_unseq, std::begin(points_local), std::end(points_local), hessian_fun); + } + else + { + std::for_each(std::begin(points_local), std::end(points_local), hessian_fun); + } + // std::cout << "adding lidar observations finished" << std::endl; + } + + std::vector> odo_edges; + for (size_t i = 1; i < intermediate_trajectory.size(); i++) + { + odo_edges.emplace_back(i - 1, i); + } + + std::vector poses; + std::vector poses_desired; + + for (size_t i = 0; i < intermediate_trajectory.size(); i++) + { + poses.push_back(pose_tait_bryan_from_affine_matrix(intermediate_trajectory[i])); + } + for (size_t i = 0; i < intermediate_trajectory.size(); i++) + { + poses_desired.push_back(pose_tait_bryan_from_affine_matrix(intermediate_trajectory[i])); + } + + for (size_t i = 0; i < odo_edges.size(); i++) + { + /*Eigen::Matrix relative_pose_measurement_odo; + relative_pose_tait_bryan_wc_case1(relative_pose_measurement_odo, + poses_desired[odo_edges[i].first].px, + poses_desired[odo_edges[i].first].py, + poses_desired[odo_edges[i].first].pz, + poses_desired[odo_edges[i].first].om, + poses_desired[odo_edges[i].first].fi, + poses_desired[odo_edges[i].first].ka, + poses_desired[odo_edges[i].second].px, + poses_desired[odo_edges[i].second].py, + poses_desired[odo_edges[i].second].pz, + poses_desired[odo_edges[i].second].om, + poses_desired[odo_edges[i].second].fi, + poses_desired[odo_edges[i].second].ka);*/ + + Eigen::Matrix delta; + relative_pose_obs_eq_tait_bryan_wc_case1( + delta, + poses[odo_edges[i].first].px, + poses[odo_edges[i].first].py, + poses[odo_edges[i].first].pz, + poses[odo_edges[i].first].om, + poses[odo_edges[i].first].fi, + poses[odo_edges[i].first].ka, + poses[odo_edges[i].second].px, + poses[odo_edges[i].second].py, + poses[odo_edges[i].second].pz, + poses[odo_edges[i].second].om, + poses[odo_edges[i].second].fi, + poses[odo_edges[i].second].ka, + 0, 0, 0, 0, 0, 0); + // relative_pose_measurement_odo(0, 0), + // relative_pose_measurement_odo(1, 0), + // relative_pose_measurement_odo(2, 0), + // relative_pose_measurement_odo(3, 0), + // relative_pose_measurement_odo(4, 0), + // relative_pose_measurement_odo(5, 0)); + + Eigen::Matrix jacobian; + relative_pose_obs_eq_tait_bryan_wc_case1_jacobian(jacobian, + poses[odo_edges[i].first].px, + poses[odo_edges[i].first].py, + poses[odo_edges[i].first].pz, + poses[odo_edges[i].first].om, + poses[odo_edges[i].first].fi, + poses[odo_edges[i].first].ka, + poses[odo_edges[i].second].px, + poses[odo_edges[i].second].py, + poses[odo_edges[i].second].pz, + poses[odo_edges[i].second].om, + poses[odo_edges[i].second].fi, + poses[odo_edges[i].second].ka); + + int ir = tripletListB.size(); + + int ic_1 = odo_edges[i].first * 6; + int ic_2 = odo_edges[i].second * 6; + + for (size_t row = 0; row < 6; row++) + { + tripletListA.emplace_back(ir + row, ic_1, -jacobian(row, 0)); + tripletListA.emplace_back(ir + row, ic_1 + 1, -jacobian(row, 1)); + tripletListA.emplace_back(ir + row, ic_1 + 2, -jacobian(row, 2)); + tripletListA.emplace_back(ir + row, ic_1 + 3, -jacobian(row, 3)); + tripletListA.emplace_back(ir + row, ic_1 + 4, -jacobian(row, 4)); + tripletListA.emplace_back(ir + row, ic_1 + 5, -jacobian(row, 5)); + + tripletListA.emplace_back(ir + row, ic_2, -jacobian(row, 6)); + tripletListA.emplace_back(ir + row, ic_2 + 1, -jacobian(row, 7)); + tripletListA.emplace_back(ir + row, ic_2 + 2, -jacobian(row, 8)); + tripletListA.emplace_back(ir + row, ic_2 + 3, -jacobian(row, 9)); + tripletListA.emplace_back(ir + row, ic_2 + 4, -jacobian(row, 10)); + tripletListA.emplace_back(ir + row, ic_2 + 5, -jacobian(row, 11)); + } + + tripletListB.emplace_back(ir, 0, delta(0, 0)); + tripletListB.emplace_back(ir + 1, 0, delta(1, 0)); + tripletListB.emplace_back(ir + 2, 0, delta(2, 0)); + tripletListB.emplace_back(ir + 3, 0, delta(3, 0)); + tripletListB.emplace_back(ir + 4, 0, delta(4, 0)); + tripletListB.emplace_back(ir + 5, 0, delta(5, 0)); + + tripletListP.emplace_back(ir, ir, 1000000); + tripletListP.emplace_back(ir + 1, ir + 1, 1000000); + tripletListP.emplace_back(ir + 2, ir + 2, 1000000); + tripletListP.emplace_back(ir + 3, ir + 3, 1000000); + tripletListP.emplace_back(ir + 4, ir + 4, 1000000); + tripletListP.emplace_back(ir + 5, ir + 5, 1000000); + } + + int ic = 0; + int ir = tripletListB.size(); + tripletListA.emplace_back(ir, ic * 6 + 0, 1); + tripletListA.emplace_back(ir + 1, ic * 6 + 1, 1); + tripletListA.emplace_back(ir + 2, ic * 6 + 2, 1); + tripletListA.emplace_back(ir + 3, ic * 6 + 3, 1); + tripletListA.emplace_back(ir + 4, ic * 6 + 4, 1); + tripletListA.emplace_back(ir + 5, ic * 6 + 5, 1); + + tripletListP.emplace_back(ir, ir, 1000000); + tripletListP.emplace_back(ir + 1, ir + 1, 1000000); + tripletListP.emplace_back(ir + 2, ir + 2, 1000000); + tripletListP.emplace_back(ir + 3, ir + 3, 1000000); + tripletListP.emplace_back(ir + 4, ir + 4, 1000000); + tripletListP.emplace_back(ir + 5, ir + 5, 1000000); + + tripletListB.emplace_back(ir, 0, 0); + tripletListB.emplace_back(ir + 1, 0, 0); + tripletListB.emplace_back(ir + 2, 0, 0); + tripletListB.emplace_back(ir + 3, 0, 0); + tripletListB.emplace_back(ir + 4, 0, 0); + tripletListB.emplace_back(ir + 5, 0, 0); + + Eigen::SparseMatrix matA(tripletListB.size(), intermediate_trajectory.size() * 6); + Eigen::SparseMatrix matP(tripletListB.size(), tripletListB.size()); + Eigen::SparseMatrix matB(tripletListB.size(), 1); + + matA.setFromTriplets(tripletListA.begin(), tripletListA.end()); + matP.setFromTriplets(tripletListP.begin(), tripletListP.end()); + matB.setFromTriplets(tripletListB.begin(), tripletListB.end()); + + Eigen::SparseMatrix AtPA(intermediate_trajectory.size() * 6, intermediate_trajectory.size() * 6); + Eigen::SparseMatrix AtPB(intermediate_trajectory.size() * 6, 1); + + { + Eigen::SparseMatrix AtP = matA.transpose() * matP; + AtPA = (AtP)*matA; + AtPB = (AtP)*matB; + } + + tripletListA.clear(); + tripletListP.clear(); + tripletListB.clear(); + + // AtPA += AtPAndt.sparseView(); + // AtPB += AtPBndt.sparseView(); + + // Eigen::SparseMatrix AtPA_I(intrinsics.size() * 6, intrinsics.size() * 6); + // AtPA_I.setIdentity(); + // AtPA_I *= 1; + // AtPA += AtPA_I; + + Eigen::SimplicialCholesky> + solver(AtPA); + // std::cout << "start solving" << std::endl; + Eigen::SparseMatrix x = solver.solve(AtPB); + // std::cout << "start finished" << std::endl; + std::vector h_x; + for (int k = 0; k < x.outerSize(); ++k) + { + for (Eigen::SparseMatrix::InnerIterator it(x, k); it; ++it) + { + // std::cout << it.value() << " "; + h_x.push_back(it.value()); + } + // std::cout << std::endl; + } + + if (h_x.size() == 6 * int_tr.size()) + { + int counter = 0; + + for (size_t i = 0; i < int_tr.size(); i++) + { + TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(int_tr[i]); + // auto prev_pose = pose; + pose.px += h_x[counter++]; + pose.py += h_x[counter++]; + pose.pz += h_x[counter++]; + pose.om += h_x[counter++]; + pose.fi += h_x[counter++]; + pose.ka += h_x[counter++]; + + int_tr[i] = affine_matrix_from_pose_tait_bryan(pose); + // all_data[index_rendered_points_local].poses[i] = intermediate_trajectory[i]; + } + + intermediate_trajectory = int_tr; + intermediate_trajectory_motion_model = int_tr; + + // auto int_tr = intermediate_trajectory; + // auto int_tr_mm = intermediate_trajectory_motion_model; + + /*Eigen::Affine3d m = int_tr_tmp[0]; + + std::vector out; + out.push_back(m); + + for (int i = 1; i < int_tr.size(); i++) + { + auto update = int_tr[i - 1] * int_tr[i]; + m = m * update; + out.push_back(m); + } + + intermediate_trajectory = out; + intermediate_trajectory_motion_model = out;*/ + } + else + { + std::cout << "optimization failed" << std::endl; + } +} + +void optimize_sf2(std::vector &intermediate_points, std::vector &intermediate_points_sf, std::vector &intermediate_trajectory, + std::vector &intermediate_trajectory_motion_model, + NDT::GridParameters &rgd_params, bool useMultithread) +{ + std::vector point_cloud_global; + std::vector point_cloud_global_sc; + std::vector indexes; + + for (int i = 0; i < intermediate_points.size(); i++) + { + Point3Di pg = intermediate_points[i]; + pg.point = intermediate_trajectory[intermediate_points[i].index_pose] * pg.point; + point_cloud_global.push_back(pg); + double r_g = pg.point.norm(); + point_cloud_global_sc.emplace_back(r_g, atan2(pg.point.y(), pg.point.x()) / M_PI * 180.0, acos(pg.point.z() / r_g) / M_PI * 180.0); + indexes.push_back(i); + } + + NDTBucketMapType buckets; + update_rgd_spherical_coordinates(rgd_params, buckets, point_cloud_global, point_cloud_global_sc, {0, 0, 0}); + + std::vector mutexes(indexes.size()); + + Eigen::Vector3d b(rgd_params.resolution_X, rgd_params.resolution_Y, rgd_params.resolution_Z); + + Eigen::MatrixXd AtPAndt(intermediate_trajectory.size() * 6, intermediate_trajectory.size() * 6); + AtPAndt.setZero(); + Eigen::MatrixXd AtPBndt(intermediate_trajectory.size() * 6, 1); + AtPBndt.setZero(); + + const auto hessian_fun = [&](const int &indexes_i) + { + auto index_of_bucket = get_rgd_index(point_cloud_global_sc[indexes_i], b); + auto bucket_it = buckets.find(index_of_bucket); + // no bucket found + if (bucket_it == buckets.end()) + { + return; + } + auto &this_bucket = bucket_it->second; + + const Eigen::Matrix3d &infm = this_bucket.cov.inverse(); + const double threshold = 10000.0; + + if ((infm.array() > threshold).any()) + { + return; + } + if ((infm.array() < -threshold).any()) + { + return; + } + + const Eigen::Affine3d &m_pose = intermediate_trajectory[intermediate_points[indexes_i].index_pose]; // intermediate_trajectory[intermediate_points_i.index_pose]; + const Eigen::Vector3d &p_s = intermediate_points[indexes_i].point; + const TaitBryanPose pose_s = pose_tait_bryan_from_affine_matrix(m_pose); + // + + Eigen::Matrix AtPA; + point_to_point_source_to_target_tait_bryan_wc_AtPA_simplified( + AtPA, + pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, + p_s.x(), p_s.y(), p_s.z(), + infm(0, 0), infm(0, 1), infm(0, 2), infm(1, 0), infm(1, 1), infm(1, 2), infm(2, 0), infm(2, 1), infm(2, 2)); + + Eigen::Matrix AtPB; + point_to_point_source_to_target_tait_bryan_wc_AtPB_simplified( + AtPB, + pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, + p_s.x(), p_s.y(), p_s.z(), + infm(0, 0), infm(0, 1), infm(0, 2), infm(1, 0), infm(1, 1), infm(1, 2), infm(2, 0), infm(2, 1), infm(2, 2), + this_bucket.mean.x(), this_bucket.mean.y(), this_bucket.mean.z()); + + int c = intermediate_points[indexes_i].index_pose * 6; + + std::mutex &m = mutexes[intermediate_points[indexes_i].index_pose]; + std::unique_lock lck(m); + AtPAndt.block<6, 6>(c, c) += AtPA; + AtPBndt.block<6, 1>(c, 0) -= AtPB; + }; + + if (useMultithread) + { + std::for_each(std::execution::par_unseq, std::begin(indexes), std::end(indexes), hessian_fun); + } + else + { + std::for_each(std::begin(indexes), std::end(indexes), hessian_fun); + } + + /// + std::vector poses; + std::vector poses_desired; + + for (size_t i = 0; i < intermediate_trajectory.size(); i++) + { + poses.push_back(pose_tait_bryan_from_affine_matrix(intermediate_trajectory[i])); + } + for (size_t i = 0; i < intermediate_trajectory_motion_model.size(); i++) + { + poses_desired.push_back(pose_tait_bryan_from_affine_matrix(intermediate_trajectory_motion_model[i])); + } + + std::vector> odo_edges; + for (size_t i = 1; i < intermediate_trajectory.size(); i++) + { + odo_edges.emplace_back(i - 1, i); + } + + for (size_t i = 0; i < odo_edges.size(); i++) + { + /*Eigen::Matrix relative_pose_measurement_odo; + relative_pose_tait_bryan_wc_case1_simplified_1(relative_pose_measurement_odo, + poses_desired[odo_edges[i].first].px, + poses_desired[odo_edges[i].first].py, + poses_desired[odo_edges[i].first].pz, + poses_desired[odo_edges[i].first].om, + poses_desired[odo_edges[i].first].fi, + poses_desired[odo_edges[i].first].ka, + poses_desired[odo_edges[i].second].px, + poses_desired[odo_edges[i].second].py, + poses_desired[odo_edges[i].second].pz, + poses_desired[odo_edges[i].second].om, + poses_desired[odo_edges[i].second].fi, + poses_desired[odo_edges[i].second].ka);*/ + + Eigen::Matrix AtPAodo; + relative_pose_obs_eq_tait_bryan_wc_case1_AtPA_simplified(AtPAodo, + poses[odo_edges[i].first].px, + poses[odo_edges[i].first].py, + poses[odo_edges[i].first].pz, + poses[odo_edges[i].first].om, + poses[odo_edges[i].first].fi, + poses[odo_edges[i].first].ka, + poses[odo_edges[i].second].px, + poses[odo_edges[i].second].py, + poses[odo_edges[i].second].pz, + poses[odo_edges[i].second].om, + poses[odo_edges[i].second].fi, + poses[odo_edges[i].second].ka, + 1000000, + 1000000, + 1000000, + 1000000, + 1000000, + 1000000); + Eigen::Matrix AtPBodo; + relative_pose_obs_eq_tait_bryan_wc_case1_AtPB_simplified(AtPBodo, + poses[odo_edges[i].first].px, + poses[odo_edges[i].first].py, + poses[odo_edges[i].first].pz, + poses[odo_edges[i].first].om, + poses[odo_edges[i].first].fi, + poses[odo_edges[i].first].ka, + poses[odo_edges[i].second].px, + poses[odo_edges[i].second].py, + poses[odo_edges[i].second].pz, + poses[odo_edges[i].second].om, + poses[odo_edges[i].second].fi, + poses[odo_edges[i].second].ka, + // relative_pose_measurement_odo(0, 0), + // relative_pose_measurement_odo(1, 0), + // relative_pose_measurement_odo(2, 0), + // relative_pose_measurement_odo(3, 0), + // relative_pose_measurement_odo(4, 0), + // relative_pose_measurement_odo(5, 0), + 0, 0, 0, 0, 0, 0, + 1000000, + 1000000, + 1000000, + 1000000, + 1000000, + 1000000); + int ic_1 = odo_edges[i].first * 6; + int ic_2 = odo_edges[i].second * 6; + + for (int row = 0; row < 6; row++) + { + for (int col = 0; col < 6; col++) + { + AtPAndt(ic_1 + row, ic_1 + col) += AtPAodo(row, col); + AtPAndt(ic_1 + row, ic_2 + col) += AtPAodo(row, col + 6); + AtPAndt(ic_2 + row, ic_1 + col) += AtPAodo(row + 6, col); + AtPAndt(ic_2 + row, ic_2 + col) += AtPAodo(row + 6, col + 6); + } + } + + for (int row = 0; row < 6; row++) + { + AtPBndt(ic_1 + row, 0) -= AtPBodo(row, 0); + AtPBndt(ic_2 + row, 0) -= AtPBodo(row + 6, 0); + } + } + + std::vector> tripletListA; + std::vector> tripletListP; + std::vector> tripletListB; + + int ic = 0; + int ir = tripletListB.size(); + tripletListA.emplace_back(ir, ic * 6 + 0, 1); + tripletListA.emplace_back(ir + 1, ic * 6 + 1, 1); + tripletListA.emplace_back(ir + 2, ic * 6 + 2, 1); + tripletListA.emplace_back(ir + 3, ic * 6 + 3, 1); + tripletListA.emplace_back(ir + 4, ic * 6 + 4, 1); + tripletListA.emplace_back(ir + 5, ic * 6 + 5, 1); + + tripletListP.emplace_back(ir, ir, 1000000); + tripletListP.emplace_back(ir + 1, ir + 1, 1000000); + tripletListP.emplace_back(ir + 2, ir + 2, 1000000); + tripletListP.emplace_back(ir + 3, ir + 3, 1000000); + tripletListP.emplace_back(ir + 4, ir + 4, 1000000); + tripletListP.emplace_back(ir + 5, ir + 5, 1000000); + + tripletListB.emplace_back(ir, 0, 0); + tripletListB.emplace_back(ir + 1, 0, 0); + tripletListB.emplace_back(ir + 2, 0, 0); + tripletListB.emplace_back(ir + 3, 0, 0); + tripletListB.emplace_back(ir + 4, 0, 0); + tripletListB.emplace_back(ir + 5, 0, 0); + + Eigen::SparseMatrix matA(tripletListB.size(), intermediate_trajectory.size() * 6); + Eigen::SparseMatrix matP(tripletListB.size(), tripletListB.size()); + Eigen::SparseMatrix matB(tripletListB.size(), 1); + + matA.setFromTriplets(tripletListA.begin(), tripletListA.end()); + matP.setFromTriplets(tripletListP.begin(), tripletListP.end()); + matB.setFromTriplets(tripletListB.begin(), tripletListB.end()); + + Eigen::SparseMatrix AtPA(intermediate_trajectory.size() * 6, intermediate_trajectory.size() * 6); + Eigen::SparseMatrix AtPB(intermediate_trajectory.size() * 6, 1); + + { + Eigen::SparseMatrix AtP = matA.transpose() * matP; + AtPA = (AtP)*matA; + AtPB = (AtP)*matB; + } + + tripletListA.clear(); + tripletListP.clear(); + tripletListB.clear(); + + AtPA += AtPAndt.sparseView(); + AtPB += AtPBndt.sparseView(); + + Eigen::SparseMatrix AtPA_I(intermediate_trajectory.size() * 6, intermediate_trajectory.size() * 6); + AtPA_I.setIdentity(); + AtPA += AtPA_I; + + Eigen::SimplicialCholesky> + solver(AtPA); + Eigen::SparseMatrix x = solver.solve(AtPB); + std::vector h_x; + for (int k = 0; k < x.outerSize(); ++k) + { + for (Eigen::SparseMatrix::InnerIterator it(x, k); it; ++it) + { + h_x.push_back(it.value()); + } + } + + if (h_x.size() == 6 * intermediate_trajectory.size()) + { + int counter = 0; + + for (size_t i = 0; i < intermediate_trajectory.size(); i++) + { + TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(intermediate_trajectory[i]); + auto prev_pose = pose; + pose.px += h_x[counter++]; + pose.py += h_x[counter++]; + pose.pz += h_x[counter++]; + pose.om += h_x[counter++]; + pose.fi += h_x[counter++]; + pose.ka += h_x[counter++]; + + // Eigen::Vector3d p1(prev_pose.px, prev_pose.py, prev_pose.pz); + // Eigen::Vector3d p2(pose.px, pose.py, pose.pz); + + // if ((p1 - p2).norm() < 1.0) + //{ + intermediate_trajectory[i] = affine_matrix_from_pose_tait_bryan(pose); + intermediate_trajectory_motion_model[i] = intermediate_trajectory[i]; + //} + } + } + /// +} + void optimize(std::vector &intermediate_points, std::vector &intermediate_trajectory, std::vector &intermediate_trajectory_motion_model, NDT::GridParameters &rgd_params, NDTBucketMapType &buckets, bool multithread /*, @@ -1370,187 +2050,6 @@ void fix_ptch_roll(std::vector &worker_data) bool compute_step_2(std::vector &worker_data, LidarOdometryParams ¶ms, double &ts_failure) { -#if 0 - bool add_pitch_roll_constraint = false; - - if (worker_data.size() != 0) - { - std::chrono::time_point start, end; - start = std::chrono::system_clock::now(); - - double acc_distance = 0.0; - // std::vector points_global; - - Eigen::Affine3d m_last = params.m_g; - auto tmp = worker_data[0].intermediate_trajectory; - - worker_data[0].intermediate_trajectory[0] = m_last; - for (int k = 1; k < tmp.size(); k++) - { - Eigen::Affine3d m_update = tmp[k - 1].inverse() * tmp[k]; - m_last = m_last * m_update; - worker_data[0].intermediate_trajectory[k] = m_last; - } - worker_data[0].intermediate_trajectory_motion_model = worker_data[0].intermediate_trajectory; - - auto points_global = params.initial_points; - for (int i = 0; i < points_global.size(); i++) - { - points_global[i].point = params.m_g * points_global[i].point; - points_global[i].index_pose = -1; - } - - // - for (int i = 0; i < worker_data.size(); i++) - { - Eigen::Vector3d mean_shift(0.0, 0.0, 0.0); - if (i > 1 && params.use_motion_from_previous_step) - { - mean_shift = worker_data[i - 1].intermediate_trajectory[0].translation() - worker_data[i - 2].intermediate_trajectory[worker_data[i - 2].intermediate_trajectory.size() - 1].translation(); - mean_shift /= ((worker_data[i - 2].intermediate_trajectory.size()) - 2); - - if (mean_shift.norm() > 1.0) - { - std::cout << "!!!mean_shift.norm() > 1.0!!!" << std::endl; - mean_shift = Eigen::Vector3d(0.0, 0.0, 0.0); - } - - std::vector new_trajectory; - Eigen::Affine3d current_node = worker_data[i].intermediate_trajectory[0]; - new_trajectory.push_back(current_node); - - for (int tr = 1; tr < worker_data[i].intermediate_trajectory.size(); tr++) - { - current_node.linear() = worker_data[i].intermediate_trajectory[tr].linear(); - current_node.translation() += mean_shift; - new_trajectory.push_back(current_node); - } - - worker_data[i].intermediate_trajectory = new_trajectory; - //////////////////////////////////////////////////////////////////////// - std::vector new_trajectory_motion_model; - Eigen::Affine3d current_node_motion_model = worker_data[i].intermediate_trajectory_motion_model[0]; - new_trajectory_motion_model.push_back(current_node_motion_model); - - Eigen::Vector3d mean_shift_t = worker_data[i].intermediate_trajectory_motion_model[0].linear() * ((worker_data[i].intermediate_trajectory[0].linear()).inverse() * mean_shift); - - for (int tr = 1; tr < worker_data[i].intermediate_trajectory_motion_model.size(); tr++) - { - current_node_motion_model.linear() = worker_data[i].intermediate_trajectory_motion_model[tr].linear(); - current_node_motion_model.translation() += mean_shift_t; - new_trajectory_motion_model.push_back(current_node_motion_model); - } - - worker_data[i].intermediate_trajectory_motion_model = new_trajectory_motion_model; - } - - std::chrono::time_point start1, end1; - start1 = std::chrono::system_clock::now(); - - auto tmp_worker_data = worker_data[i].intermediate_trajectory; - - for (int iter = 0; iter < /*params.nr_iter*/ 30; iter++) - { - optimize_icp(worker_data[i].intermediate_points, worker_data[i].intermediate_trajectory, worker_data[i].intermediate_trajectory_motion_model, - params.in_out_params, /*params.buckets*/ points_global, params.useMultithread, add_pitch_roll_constraint, worker_data[i].imu_roll_pitch); - } - end1 = std::chrono::system_clock::now(); - std::chrono::duration elapsed_seconds1 = end1 - start1; - std::cout << "optimizing worker_data [" << i + 1 << "] of " << worker_data.size() << " acc_distance: " << acc_distance << " elapsed time: " << elapsed_seconds1.count() << std::endl; - - /*for (int ii = 0; ii < worker_data[i].intermediate_points.size(); ii++) - { - Point3Di p = worker_data[i].intermediate_points[ii]; - p.point = worker_data[i].intermediate_trajectory[p.index_pose] * p.point; - p.index_point = -1; - p.index_pose = -1; - points_global.push_back(p); - // p. - } - points_global = decimate(points_global, 0.03, 0.03, 0.03); - - std::cout << "points_global.size() " << points_global.size() << std::endl; - if (points_global.size() > 10000) - { - std::vector points_global_new; - for (int k = points_global.size() - 1; k > points_global.size() - 10000; k--) - { - points_global_new.push_back(points_global[k]); - } - points_global = points_global_new; - }*/ - - // temp save - if (i % 100 == 0) - { - std::vector global_points_; - for (int k = 0; k < worker_data[i].intermediate_points.size(); k++) - { - Point3Di p = worker_data[i].intermediate_points[k]; - int index_pose = p.index_pose; - p.point = worker_data[i].intermediate_trajectory[index_pose] * p.point; - global_points_.push_back(p); - } - std::string fn = params.working_directory_preview + "/temp_point_cloud_" + std::to_string(i) + ".laz"; - saveLaz(fn.c_str(), global_points_); - } - auto acc_distance_tmp = acc_distance; - acc_distance += ((worker_data[i].intermediate_trajectory[0].inverse()) * - worker_data[i].intermediate_trajectory[worker_data[i].intermediate_trajectory.size() - 1]) - .translation() - .norm(); - - if (!(acc_distance == acc_distance)) - { - worker_data[i].intermediate_trajectory = tmp_worker_data; - std::cout << "CHALLENGING DATA OCCURED!!!" << std::endl; - acc_distance = acc_distance_tmp; - std::cout << "please split data set into subsets" << std::endl; - ts_failure = worker_data[i].intermediate_trajectory_timestamps[0].first; - // std::cout << "calculations canceled for TIMESTAMP: " << (long long int)worker_data[i].intermediate_trajectory_timestamps[0].first << std::endl; - return false; - } - - // update - - for (int j = i + 1; j < worker_data.size(); j++) - { - Eigen::Affine3d m_last = worker_data[j - 1].intermediate_trajectory[worker_data[j - 1].intermediate_trajectory.size() - 1]; - auto tmp = worker_data[j].intermediate_trajectory; - - worker_data[j].intermediate_trajectory[0] = m_last; - - for (int k = 1; k < tmp.size(); k++) - { - Eigen::Affine3d m_update = tmp[k - 1].inverse() * tmp[k]; - m_last = m_last * m_update; - worker_data[j].intermediate_trajectory[k] = m_last; - } - } - - if (i > 1) - { - double translation = (worker_data[i - 1].intermediate_trajectory[0].translation() - - worker_data[i - 2].intermediate_trajectory[0].translation()) - .norm(); - params.consecutive_distance += translation; - } - } - - end = std::chrono::system_clock::now(); - std::chrono::duration elapsed_seconds = end - start; - std::time_t end_time = std::chrono::system_clock::to_time_t(end); - - std::cout << "finished computation at " << std::ctime(&end_time) - << "elapsed time: " << elapsed_seconds.count() << "s\n"; - } -#else - // std::vector mm_poses; - // for (int i = 0; i < worker_data.size(); i++) - //{ - // mm_poses.push_back(worker_data[i].intermediate_trajectory_motion_model[0]); - // } - if (worker_data.size() != 0) { @@ -1580,12 +2079,12 @@ bool compute_step_2(std::vector &worker_data, LidarOdometryParams &p for (int i = 0; i < worker_data.size(); i++) { - //std::cout << "jojo" << std::endl; + // std::cout << "jojo" << std::endl; Eigen::Vector3d mean_shift(0.0, 0.0, 0.0); if (i > 1 && params.use_motion_from_previous_step) { - //mean_shift = worker_data[i - 1].intermediate_trajectory[0].translation() - worker_data[i - 2].intermediate_trajectory[worker_data[i - 2].intermediate_trajectory.size() - 1].translation(); - //mean_shift /= ((worker_data[i - 2].intermediate_trajectory.size()) - 2); + // mean_shift = worker_data[i - 1].intermediate_trajectory[0].translation() - worker_data[i - 2].intermediate_trajectory[worker_data[i - 2].intermediate_trajectory.size() - 1].translation(); + // mean_shift /= ((worker_data[i - 2].intermediate_trajectory.size()) - 2); mean_shift = worker_data[i - 1].intermediate_trajectory[worker_data[i - 1].intermediate_trajectory.size() - 1].translation() - worker_data[i - 2].intermediate_trajectory[worker_data[i - 2].intermediate_trajectory.size() - 1].translation(); @@ -1606,7 +2105,7 @@ bool compute_step_2(std::vector &worker_data, LidarOdometryParams &p auto update = worker_data[i].intermediate_trajectory[tr - 1].inverse() * worker_data[i].intermediate_trajectory[tr]; current_node = current_node * update; // current_node.linear() = //worker_data[i].intermediate_trajectory[tr].linear(); - //current_node.translation() += mean_shift; + // current_node.translation() += mean_shift; new_trajectory.push_back(current_node); } @@ -1661,15 +2160,83 @@ bool compute_step_2(std::vector &worker_data, LidarOdometryParams &p std::chrono::time_point start1, end1; start1 = std::chrono::system_clock::now(); - auto tmp_worker_data = worker_data[i].intermediate_trajectory; + // std::string fn1 = "input_" + std::to_string(i) + ".txt"; + // ofstream file1; + // file1.open(fn1); + // for (int k = 0; k < worker_data[i].intermediate_trajectory.size(); k++){ + // file1 << worker_data[i].intermediate_trajectory[k](0, 3) << " " << worker_data[i].intermediate_trajectory[k](1, 3) << " " << worker_data[i].intermediate_trajectory[k](2, 3) << " 0" << std::endl; + // } + // file1.close(); + if (params.use_robust_and_accurate_lidar_odometry) + { + auto tr = worker_data[i].intermediate_trajectory; + auto trmm = worker_data[i].intermediate_trajectory_motion_model; - //std::string fn1 = "input_" + std::to_string(i) + ".txt"; - //ofstream file1; - //file1.open(fn1); - //for (int k = 0; k < worker_data[i].intermediate_trajectory.size(); k++){ - // file1 << worker_data[i].intermediate_trajectory[k](0, 3) << " " << worker_data[i].intermediate_trajectory[k](1, 3) << " " << worker_data[i].intermediate_trajectory[k](2, 3) << " 0" << std::endl; - //} - //file1.close(); + auto firstm = tr[0]; + + for (auto &t : tr) + { + t.translation() -= firstm.translation(); + } + for (auto &t : trmm) + { + t.translation() -= firstm.translation(); + } + + NDT::GridParameters rgd_params_sc; + + rgd_params_sc.resolution_X = params.distance_bucket; + rgd_params_sc.resolution_Y = params.polar_angle_deg; + rgd_params_sc.resolution_Z = params.azimutal_angle_deg; + + // for (int iter = 0; iter < 10; iter++) + //{ + // optimize_sf(worker_data[i].intermediate_points, tr, trmm, + // rgd_params_sc, params.buckets, /*params.useMultithread*/ false); + // } + std::vector points_local_sf; + std::vector points_local; + + /// + for (int ii = 0; ii < worker_data[i].intermediate_points.size(); ii++) + { + double r_l = worker_data[i].intermediate_points[ii].point.norm(); + if (r_l > 0.5 && worker_data[i].intermediate_points[ii].index_pose != -1 && r_l < params.max_distance_lidar) + { + double polar_angle_deg_l = atan2(worker_data[i].intermediate_points[ii].point.y(), worker_data[i].intermediate_points[ii].point.x()) / M_PI * 180.0; + double azimutal_angle_deg_l = acos(worker_data[i].intermediate_points[ii].point.z() / r_l) / M_PI * 180.0; + + points_local.push_back(worker_data[i].intermediate_points[ii]); + + /////////////////////////////////////////////////////// + Point3Di p_sl = worker_data[i].intermediate_points[ii]; + p_sl.point.x() = r_l; + p_sl.point.y() = polar_angle_deg_l; + p_sl.point.z() = azimutal_angle_deg_l; + + points_local_sf.push_back(p_sl); + } + } + /// + std::cout << "optimize_sf2" << std::endl; + for (int iter = 0; iter < params.robust_and_accurate_lidar_odometry_iterations; iter++) + { + optimize_sf2(points_local, points_local_sf, tr, trmm, rgd_params_sc, params.useMultithread); + } + + for (auto &t : tr) + { + t.translation() += firstm.translation(); + } + + for (auto &t : trmm) + { + t.translation() += firstm.translation(); + } + + worker_data[i].intermediate_trajectory = tr; + worker_data[i].intermediate_trajectory_motion_model = tr; + } for (int iter = 0; iter < params.nr_iter; iter++) { @@ -1677,14 +2244,14 @@ bool compute_step_2(std::vector &worker_data, LidarOdometryParams &p params.in_out_params, params.buckets, params.useMultithread /*, add_pitch_roll_constraint, worker_data[i].imu_roll_pitch*/); } - //std::string fn2 = "output_" + std::to_string(i) + ".txt"; - //ofstream file2; - //file2.open(fn2); - //for (int k = 0; k < worker_data[i].intermediate_trajectory.size(); k++) + // std::string fn2 = "output_" + std::to_string(i) + ".txt"; + // ofstream file2; + // file2.open(fn2); + // for (int k = 0; k < worker_data[i].intermediate_trajectory.size(); k++) //{ - // file2 << worker_data[i].intermediate_trajectory[k](0, 3) << " " << worker_data[i].intermediate_trajectory[k](1, 3) << " " << worker_data[i].intermediate_trajectory[k](2, 3) << " 1" << std::endl; - //} - //file2.close(); + // file2 << worker_data[i].intermediate_trajectory[k](0, 3) << " " << worker_data[i].intermediate_trajectory[k](1, 3) << " " << worker_data[i].intermediate_trajectory[k](2, 3) << " 1" << std::endl; + // } + // file2.close(); end1 = std::chrono::system_clock::now(); std::chrono::duration elapsed_seconds1 = end1 - start1; @@ -1874,7 +2441,7 @@ bool compute_step_2(std::vector &worker_data, LidarOdometryParams &p } std::cout << "length_of_trajectory: " << length_of_trajectory << " [m]" << std::endl; } -#endif + return true; } diff --git a/apps/livox_mid_360_intrinsic_calibration/CMakeLists.txt b/apps/livox_mid_360_intrinsic_calibration/CMakeLists.txt new file mode 100644 index 0000000..1ee9ec0 --- /dev/null +++ b/apps/livox_mid_360_intrinsic_calibration/CMakeLists.txt @@ -0,0 +1,52 @@ +cmake_minimum_required(VERSION 3.15.0) + +project(livox_mid_360_intrinsic_calibration) + +add_executable( + livox_mid_360_intrinsic_calibration + livox_mid_360_intrinsic_calibration.cpp + ../lidar_odometry_step_1/lidar_odometry_utils.cpp + ../lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp +) + +target_include_directories( + livox_mid_360_intrinsic_calibration + PRIVATE include + ${REPOSITORY_DIRECTORY}/core/include + ${REPOSITORY_DIRECTORY}/core_hd_mapping/include + ${EXTERNAL_LIBRARIES_DIRECTORY} + ${EXTERNAL_LIBRARIES_DIRECTORY}/glm + ${EIGEN3_INCLUDE_DIR} + ${EXTERNAL_LIBRARIES_DIRECTORY}/imgui + ${EXTERNAL_LIBRARIES_DIRECTORY}/imgui/backends + ${EXTERNAL_LIBRARIES_DIRECTORY}/ImGuizmo + ${EXTERNAL_LIBRARIES_DIRECTORY}/glew-2.2.0/include + ${FREEGLUT_INCLUDE_DIR} + ${EXTERNAL_LIBRARIES_DIRECTORY}/json/include + ${EXTERNAL_LIBRARIES_DIRECTORY}/portable-file-dialogs-master + ${LASZIP_INCLUDE_DIR}/LASzip/include + ${EXTERNAL_LIBRARIES_DIRECTORY}/observation_equations/codes + ${EXTERNAL_LIBRARIES_DIRECTORY}/Fusion/Fusion) + +target_link_libraries( + livox_mid_360_intrinsic_calibration + PRIVATE Fusion + ${FREEGLUT_LIBRARY} + ${OPENGL_gl_LIBRARY} + OpenGL::GLU + ${PLATFORM_LASZIP_LIB} + ${CORE_LIBRARIES} + ${GUI_LIBRARIES} + ${PLATFORM_MISCELLANEOUS_LIBS}) + +if(WIN32) + add_custom_command( + TARGET livox_mid_360_intrinsic_calibration + POST_BUILD + COMMAND + ${CMAKE_COMMAND} -E copy $ + $ + COMMAND_EXPAND_LISTS) +endif() + +install(TARGETS livox_mid_360_intrinsic_calibration DESTINATION bin) \ No newline at end of file diff --git a/apps/livox_mid_360_intrinsic_calibration/livox_mid_360_intrinsic_calibration.cpp b/apps/livox_mid_360_intrinsic_calibration/livox_mid_360_intrinsic_calibration.cpp new file mode 100644 index 0000000..f88b4af --- /dev/null +++ b/apps/livox_mid_360_intrinsic_calibration/livox_mid_360_intrinsic_calibration.cpp @@ -0,0 +1,1790 @@ +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include + +#include + +#include +#include "../lidar_odometry_step_1/lidar_odometry_utils.h" + +#include + +#include + +#include + +#include + +#define SAMPLE_PERIOD (1.0 / 200.0) +namespace fs = std::filesystem; + +const unsigned int window_width = 800; +const unsigned int window_height = 600; +double camera_ortho_xy_view_zoom = 10; +double camera_ortho_xy_view_shift_x = 0.0; +double camera_ortho_xy_view_shift_y = 0.0; +double camera_mode_ortho_z_center_h = 0.0; +double camera_ortho_xy_view_rotation_angle_deg = 0; +bool is_ortho = false; +bool show_axes = true; +ImVec4 clear_color = ImVec4(0.8f, 0.8f, 0.8f, 1.0f); +ImVec4 pc_color = ImVec4(1.0f, 0.0f, 0.0f, 1.0f); +ImVec4 pc_color_point = ImVec4(1.0f, 1.0f, 0.0f, 1.0f); +ImVec4 pc_color_ray = ImVec4(1.0f, 0.0f, 1.0f, 1.0f); +ImVec4 pc_color_point_cloud_path = ImVec4(0.0f, 0.0f, 0.0f, 1.0f); +ImVec4 intrinsics_color = ImVec4(0.4f, 0.0f, 0.4f, 1.0f); +ImVec4 intrinsic_path_color = ImVec4(0.8f, 0.0f, 0.4f, 1.0f); + +// ImVec4 pc_color2 = ImVec4(0.0f, 0.0f, 1.0f, 1.00f); + +int point_size = 1; +Eigen::Vector3f rotation_center = Eigen::Vector3f::Zero(); +float translate_x, translate_y = 0.0; +float translate_z = -50.0; +float rotate_x = 0.0, rotate_y = 0.0; +int mouse_old_x, mouse_old_y; +int mouse_buttons = 0; +bool gui_mouse_down{false}; +float mouse_sensitivity = 1.0; + +float m_ortho_projection[] = {1, 0, 0, 0, + 0, 1, 0, 0, + 0, 0, 1, 0, + 0, 0, 0, 1}; + +float m_ortho_gizmo_view[] = {1, 0, 0, 0, + 0, 1, 0, 0, + 0, 0, 1, 0, + 0, 0, 0, 1}; + +bool show_pc = true; +bool show_single_point_and_ray = false; +int single_point_size = 4; + +std::vector point_cloud_path; +std::vector intrinsic_path; +bool show_point_cloud_path = true; + +bool show_intrinsics = false; +bool apply_intrinsics_in_render = false; +bool show_intrinsic_path = false; + +const std::vector LAS_LAZ_filter = {"LAS file (*.laz)", "*.laz", "LASzip file (*.las)", "*.las", "All files", "*"}; +std::vector point_cloud; +std::vector intrinsics; + +int index_rendered_point_and_ray = -1; + +std::vector> point_intrinsic_correspondances; + +// std::unordered_map idToSn; +// std::unordered_map calibrations; + +// struct Checked +//{ +// bool check; +// }; +// std::vector calibrated_lidar; +// std::vector imu_lidar; + +// double search_radious = 0.1; + +// bool show_grid = true; +// bool manual_calibration = false; + +void calibrate_intrinsics(); + +void reshape(int w, int h) +{ + glViewport(0, 0, (GLsizei)w, (GLsizei)h); + glMatrixMode(GL_PROJECTION); + glLoadIdentity(); + if (!is_ortho) + { + gluPerspective(60.0, (GLfloat)w / (GLfloat)h, 0.01, 10000.0); + } + else + { + ImGuiIO &io = ImGui::GetIO(); + float ratio = float(io.DisplaySize.x) / float(io.DisplaySize.y); + + glOrtho(-camera_ortho_xy_view_zoom, camera_ortho_xy_view_zoom, + -camera_ortho_xy_view_zoom / ratio, + camera_ortho_xy_view_zoom / ratio, -100000, 100000); + // glOrtho(-translate_z, translate_z, -translate_z * (float)h / float(w), translate_z * float(h) / float(w), -10000, 10000); + } + glMatrixMode(GL_MODELVIEW); + glLoadIdentity(); +} + +void motion(int x, int y) +{ + ImGuiIO &io = ImGui::GetIO(); + io.MousePos = ImVec2((float)x, (float)y); + + if (!io.WantCaptureMouse) + { + float dx, dy; + dx = (float)(x - mouse_old_x); + dy = (float)(y - mouse_old_y); + + if (is_ortho) + { + if (mouse_buttons & 1) + { + float ratio = float(io.DisplaySize.x) / float(io.DisplaySize.y); + Eigen::Vector3d v(dx * (camera_ortho_xy_view_zoom / (GLsizei)io.DisplaySize.x * 2), + dy * (camera_ortho_xy_view_zoom / (GLsizei)io.DisplaySize.y * 2 / ratio), 0); + TaitBryanPose pose_tb; + pose_tb.px = 0.0; + pose_tb.py = 0.0; + pose_tb.pz = 0.0; + pose_tb.om = 0.0; + pose_tb.fi = 0.0; + pose_tb.ka = camera_ortho_xy_view_rotation_angle_deg * M_PI / 180.0; + auto m = affine_matrix_from_pose_tait_bryan(pose_tb); + Eigen::Vector3d v_t = m * v; + camera_ortho_xy_view_shift_x += v_t.x(); + camera_ortho_xy_view_shift_y += v_t.y(); + } + } + else + { + gui_mouse_down = mouse_buttons > 0; + if (mouse_buttons & 1) + { + rotate_x += dy * 0.2f; // * mouse_sensitivity; + rotate_y += dx * 0.2f; // * mouse_sensitivity; + } + if (mouse_buttons & 4) + { + translate_x += dx * 0.05f * mouse_sensitivity; + translate_y -= dy * 0.05f * mouse_sensitivity; + } + } + + mouse_old_x = x; + mouse_old_y = y; + } + glutPostRedisplay(); +} + +std::vector load_pc(const std::string &lazFile) +{ + std::vector points; + laszip_POINTER laszip_reader; + if (laszip_create(&laszip_reader)) + { + fprintf(stderr, "DLL ERROR: creating laszip reader\n"); + std::abort(); + } + + laszip_BOOL is_compressed = 0; + if (laszip_open_reader(laszip_reader, lazFile.c_str(), &is_compressed)) + { + fprintf(stderr, "DLL ERROR: opening laszip reader for '%s'\n", lazFile.c_str()); + std::abort(); + } + std::cout << "compressed : " << is_compressed << std::endl; + laszip_header *header; + + if (laszip_get_header_pointer(laszip_reader, &header)) + { + fprintf(stderr, "DLL ERROR: getting header pointer from laszip reader\n"); + std::abort(); + } + fprintf(stderr, "file '%s' contains %u points\n", lazFile.c_str(), header->number_of_point_records); + laszip_point *point; + if (laszip_get_point_pointer(laszip_reader, &point)) + { + fprintf(stderr, "DLL ERROR: getting point pointer from laszip reader\n"); + std::abort(); + } + + int counter_ts0 = 0; + int counter_filtered_points = 0; + + std::cout << "header->number_of_point_records: " << header->number_of_point_records << std::endl; + + for (laszip_U32 j = 0; j < header->number_of_point_records; j++) + { + + if (laszip_read_point(laszip_reader)) + { + fprintf(stderr, "DLL ERROR: reading point %u\n", j); + laszip_close_reader(laszip_reader); + return points; + // std::abort(); + } + + Point3Di p; + int id = point->user_data; + + // Eigen::Affine3d calibration = calibrations.empty() ? Eigen::Affine3d::Identity() : calibrations.at(id); + const Eigen::Vector3d pf(header->x_offset + header->x_scale_factor * static_cast(point->X), header->y_offset + header->y_scale_factor * static_cast(point->Y), header->z_offset + header->z_scale_factor * static_cast(point->Z)); + + p.point = pf; + p.lidarid = id; + p.timestamp = point->gps_time; + p.intensity = point->intensity; + + points.emplace_back(p); + } + + std::cout << "total number points: " << points.size() << std::endl; + laszip_close_reader(laszip_reader); + return points; +} + +void project_gui() +{ + if (ImGui::Begin("main gui window")) + { + ImGui::ColorEdit3("clear color", (float *)&clear_color); + + if (show_pc) + { + ImGui::ColorEdit3("pc_color", (float *)&pc_color); + } + + if (ImGui::Button("Load pointcloud (lidar****.laz) (step 1)")) + { + static std::shared_ptr open_file; + std::vector input_file_names; + ImGui::PushItemFlag(ImGuiItemFlags_Disabled, (bool)open_file); + const auto t = [&]() + { + auto sel = pfd::open_file("Point cloud files", "C:\\", LAS_LAZ_filter, true).result(); + for (int i = 0; i < sel.size(); i++) + { + input_file_names.push_back(sel[i]); + } + }; + std::thread t1(t); + t1.join(); + + if (input_file_names.size() > 0) + { + for (int i = 0; i < input_file_names.size(); i++) + { + auto pc = load_pc(input_file_names[i].c_str()); + + for (const auto &p : pc) + { + point_cloud.emplace_back(p); + } + } + } + + for (int i = 0; i < point_cloud.size(); i++) + { + point_cloud[i].index_pose = i; + intrinsics.push_back(Eigen::Affine3d::Identity()); + } + } + + ImGui::Checkbox("show_pc", &show_pc); + ImGui::Checkbox("show_single_point_and_ray", &show_single_point_and_ray); + + if (show_single_point_and_ray) + { + ImGui::ColorEdit3("pc_color_point", (float *)&pc_color_point); + ImGui::ColorEdit3("pc_color_ray", (float *)&pc_color_ray); + + ImGui::InputInt("single_point_size", &single_point_size); + if (single_point_size < 1) + { + single_point_size = 1; + } + + ImGui::InputInt("index_rendered_point_and_ray", &index_rendered_point_and_ray, 1, 100); + + if (index_rendered_point_and_ray < 0) + { + index_rendered_point_and_ray = 0; + } + + if (index_rendered_point_and_ray > point_cloud.size() - 1) + { + index_rendered_point_and_ray = point_cloud.size() - 1; + } + + point_cloud_path.push_back(point_cloud[index_rendered_point_and_ray].point); + intrinsic_path.push_back(intrinsics[index_rendered_point_and_ray].translation()); + } + + ImGui::ColorEdit3("pc_color_point_cloud_path", (float *)&pc_color_point_cloud_path); + ImGui::ColorEdit3("intrinsic_path_color", (float *)&intrinsic_path_color); + + if (ImGui::Button("clear point_cloud_path and intrinsic path")) + { + point_cloud_path.clear(); + intrinsic_path.clear(); + } + + ImGui::Checkbox("show_point_cloud_path", &show_point_cloud_path); + + if (ImGui::Button("calibrate_intrinsics")) + { + calibrate_intrinsics(); + } + if (ImGui::Button("calibrate_intrinsics x 10")) + { + for(int i = 0; i < 10; i++){ + std::cout << "iteration: " << i + 1 << " of 10" << std::endl; + calibrate_intrinsics(); + } + + } + if (ImGui::Button("calibrate_intrinsics x 100")) + { + for (int i = 0; i < 100; i++) + { + std::cout << "iteration: " << i + 1 << " of 100" << std::endl; + calibrate_intrinsics(); + } + } + + ImGui::ColorEdit3("intrinsics_color", (float *)&intrinsics_color); + + ImGui::Checkbox("show_intrinsics", &show_intrinsics); + ImGui::Checkbox("apply_intrinsics_in_render", &apply_intrinsics_in_render); + ImGui::Checkbox("show_intrinsic_path", &show_intrinsic_path); + + ////////////////////////// + + if (ImGui::Button("save point cloud")) + { + std::shared_ptr save_file; + std::string output_file_name = ""; + ImGui::PushItemFlag(ImGuiItemFlags_Disabled, (bool)save_file); + const auto t = [&]() + { + auto sel = pfd::save_file("Save las or laz file", "C:\\", LAS_LAZ_filter).result(); + output_file_name = sel; + std::cout << "las or laz file to save: '" << output_file_name << "'" << std::endl; + }; + std::thread t1(t); + t1.join(); + + if (output_file_name.size() > 0) + { + std::vector pointcloud; + std::vector intensity; + std::vector timestamps; + + for (int i = 0; i < point_cloud.size(); i++) + { + pointcloud.push_back(intrinsics[point_cloud[i].index_pose] * point_cloud[i].point); + intensity.push_back(point_cloud[i].intensity); + timestamps.push_back(point_cloud[i].timestamp); + } + + if (!exportLaz(output_file_name, pointcloud, intensity, timestamps, 0, 0, 0)) + { + std::cout << "problem with saving file: " << output_file_name << std::endl; + } + } + } + + if (ImGui::Button("calculate current point_intrinsic_correspondances cloud")) + { + point_intrinsic_correspondances.clear(); + + Eigen::Vector3d current_point = intrinsics[point_cloud[index_rendered_point_and_ray].index_pose] * point_cloud[index_rendered_point_and_ray].point; + + for (int i = 0; i < point_cloud.size(); i++) + { + Eigen::Vector3d point = intrinsics[point_cloud[i].index_pose] * point_cloud[i].point; + + if ((current_point - point).norm() < 0.03) + { + point_intrinsic_correspondances.emplace_back(current_point, point); + point_intrinsic_correspondances.emplace_back(intrinsics[point_cloud[i].index_pose].translation(), point); + } + // index_rendered_point_and_ray + } + + // index_rendered_point_and_ray + } + + ImGui::End(); + } + +#if 0 + if (ImGui::Begin("main gui window")) + { + + + + if (idToSn.size() == 2) + { + ImGui::ColorEdit3(idToSn.at(0).c_str(), (float *)&pc_color); + ImGui::ColorEdit3(idToSn.at(1).c_str(), (float *)&pc_color2); + } + else + { + ImGui::ColorEdit3("pc_color_lidar_1", (float *)&pc_color); + ImGui::ColorEdit3("pc_color_lidar_2", (float *)&pc_color2); + } + + ImGui::Checkbox("show_grid", &show_grid); + ImGui::Checkbox("show_axes", &show_axes); + + if (calibrated_lidar.size() == 2) + { + ImGui::Text(".......... Check calibrated lidar .............."); + + int chosen_lidar = -1; + for (int i = 0; i < calibrated_lidar.size(); i++) + { + std::string name = idToSn.at(i); + bool before = calibrated_lidar[i].check; + ImGui::Checkbox(name.c_str(), &calibrated_lidar[i].check); + bool after = calibrated_lidar[i].check; + + if (!before && after) + { + chosen_lidar = i; + } + } + + bool is_all_false = true; + for (int i = 0; i < calibrated_lidar.size(); i++) + { + if (calibrated_lidar[i].check) + { + is_all_false = false; + } + } + + if (is_all_false) + { + chosen_lidar = 0; + } + + if (chosen_lidar != -1) + { + for (int i = 0; i < calibrated_lidar.size(); i++) + { + calibrated_lidar[i].check = false; + } + calibrated_lidar[chosen_lidar].check = true; + } + + ImGui::Text("------------------------------------------------"); + ImGui::Checkbox("manual_calibration", &manual_calibration); + + if (manual_calibration) + { + int index_calibrated_lidar = -1; + for (int i = 0; i < calibrated_lidar.size(); i++) + { + if (calibrated_lidar[i].check) + { + index_calibrated_lidar = i; + } + } + + if (index_calibrated_lidar != -1) + { + TaitBryanPose tb_pose = pose_tait_bryan_from_affine_matrix(calibrations.at(index_calibrated_lidar)); + tb_pose.om = tb_pose.om * 180.0 / M_PI; + tb_pose.fi = tb_pose.fi * 180.0 / M_PI; + tb_pose.ka = tb_pose.ka * 180.0 / M_PI; + + auto tmp = tb_pose; + + ImGui::InputDouble(std::string(idToSn.at(index_calibrated_lidar) + "_x [m] (offset in X-red axis)").c_str(), &tb_pose.px, 0.01, 0.1); + ImGui::InputDouble(std::string(idToSn.at(index_calibrated_lidar) + "_y [m] (offset in Y-green axis)").c_str(), &tb_pose.py, 0.01, 0.1); + ImGui::InputDouble(std::string(idToSn.at(index_calibrated_lidar) + "_z [m] (offset in Z-blue axis)").c_str(), &tb_pose.pz, 0.01, 0.1); + ImGui::InputDouble(std::string(idToSn.at(index_calibrated_lidar) + "_om [deg] (angle around X-red axis)").c_str(), &tb_pose.om, 0.1, 1.0); + ImGui::InputDouble(std::string(idToSn.at(index_calibrated_lidar) + "_fi [deg] (angle around Y-green axis)").c_str(), &tb_pose.fi, 0.1, 1.0); + ImGui::InputDouble(std::string(idToSn.at(index_calibrated_lidar) + "_ka [deg] (angle around Z-blue axis)").c_str(), &tb_pose.ka, 0.1, 1.0); + + if (tmp.px != tb_pose.px || tmp.py != tb_pose.py || tmp.pz != tb_pose.pz || + tmp.om != tb_pose.om || tmp.fi != tb_pose.fi || tmp.ka != tb_pose.ka) + { + tb_pose.om = tb_pose.om * M_PI / 180.0; + tb_pose.fi = tb_pose.fi * M_PI / 180.0; + tb_pose.ka = tb_pose.ka * M_PI / 180.0; + + Eigen::Affine3d m_pose = affine_matrix_from_pose_tait_bryan(tb_pose); + calibrations.at(index_calibrated_lidar) = m_pose; + } + } + + // calibrations.at(0) = m0; + } + ImGui::Text("................................................"); + } + + if (idToSn.size() == 0) + { + if (ImGui::Button("Load 'LiDAR serial number to index' file (lidar****.sn) (step 1)")) + { + static std::shared_ptr open_file; + std::vector input_file_names; + ImGui::PushItemFlag(ImGuiItemFlags_Disabled, (bool)open_file); + const auto t = [&]() + { + auto sel = pfd::open_file("Calibration files", "C:\\", sn_filter, true).result(); + for (int i = 0; i < sel.size(); i++) + { + input_file_names.push_back(sel[i]); + // std::cout << "las file: '" << input_file_name << "'" << std::endl; + } + }; + std::thread t1(t); + t1.join(); + + idToSn = MLvxCalib::GetIdToSnMapping(input_file_names[0]); + } + } + + if (idToSn.size() == 2 && calibrations.size() == 0) + { + if (ImGui::Button("Load calibration (*.json) (step 2)")) + { + static std::shared_ptr open_file; + std::vector input_file_names; + ImGui::PushItemFlag(ImGuiItemFlags_Disabled, (bool)open_file); + const auto t = [&]() + { + auto sel = pfd::open_file("Calibration files", "C:\\", json_filter, true).result(); + for (int i = 0; i < sel.size(); i++) + { + input_file_names.push_back(sel[i]); + } + }; + std::thread t1(t); + t1.join(); + + if (input_file_names.size() > 0) + { + std::cout << "loading file: " << input_file_names[0] << std::endl; + + calibration = MLvxCalib::GetCalibrationFromFile(input_file_names[0]); + imuSnToUse = MLvxCalib::GetImuSnToUse(input_file_names[0]); + + calibrations = MLvxCalib::CombineIntoCalibration(idToSn, calibration); + + if (!calibration.empty()) + { + std::cout << "Loaded calibration for: \n"; + for (const auto &[sn, _] : calibration) + { + std::cout << " -> " << sn << std::endl; + } + std::cout << "imuSnToUse: " << imuSnToUse << std::endl; + } + } + } + + ImGui::SameLine(); + ImGui::Text(" if You dont have any calibration file --> "); + ImGui::SameLine(); + if (ImGui::Button("Save default calibration (optional before step 2)")) + { + std::shared_ptr save_file; + std::string output_file_name = ""; + ImGui::PushItemFlag(ImGuiItemFlags_Disabled, (bool)save_file); + const auto t = [&]() + { + auto sel = pfd::save_file("Save *.json file", "", json_filter).result(); + output_file_name = sel; + std::cout << "Calibration file to save: '" << output_file_name << "'" << std::endl; + }; + std::thread t1(t); + t1.join(); + + if (output_file_name.size() > 0) + { + std::string l1 = idToSn.at(0); + std::string l2 = idToSn.at(1); + + std::cout + << "output_file_name: " << output_file_name << std::endl; + nlohmann::json j; + + j["calibration"][l1.c_str()]["identity"] = "true"; + j["calibration"][l2.c_str()]["order"] = "ROW"; + j["calibration"][l2.c_str()]["inverted"] = "FALSE"; + j["calibration"][l2.c_str()]["data"][0] = 1; + j["calibration"][l2.c_str()]["data"][1] = 0; + j["calibration"][l2.c_str()]["data"][2] = 0; + j["calibration"][l2.c_str()]["data"][3] = 0; + j["calibration"][l2.c_str()]["data"][4] = 0; + j["calibration"][l2.c_str()]["data"][5] = 1; + j["calibration"][l2.c_str()]["data"][6] = 0; + j["calibration"][l2.c_str()]["data"][7] = 0; + j["calibration"][l2.c_str()]["data"][8] = 0; + j["calibration"][l2.c_str()]["data"][9] = 0; + j["calibration"][l2.c_str()]["data"][10] = 1; + j["calibration"][l2.c_str()]["data"][11] = 0; + j["calibration"][l2.c_str()]["data"][12] = 0; + j["calibration"][l2.c_str()]["data"][13] = 0; + j["calibration"][l2.c_str()]["data"][14] = 0; + j["calibration"][l2.c_str()]["data"][15] = 1; + j["imuToUse"] = l1.c_str(); + + std::ofstream fs(output_file_name); + if (!fs.good()) + return; + fs << j.dump(2); + fs.close(); + } + } + } + + if (calibrations.size() > 0 && point_cloud.size() == 0) + { + if (ImGui::Button("Load pointcloud (lidar****.laz) (step 3)")) + { + static std::shared_ptr open_file; + std::vector input_file_names; + ImGui::PushItemFlag(ImGuiItemFlags_Disabled, (bool)open_file); + const auto t = [&]() + { + auto sel = pfd::open_file("Point cloud files", "C:\\", LAS_LAZ_filter, true).result(); + for (int i = 0; i < sel.size(); i++) + { + input_file_names.push_back(sel[i]); + } + }; + std::thread t1(t); + t1.join(); + + if (input_file_names.size() > 0) + { + for (int i = 0; i < input_file_names.size(); i++) + { + auto pc = load_pc(input_file_names[i].c_str(), true, filter_threshold_xy); + + for (const auto &p : pc) + { + point_cloud.emplace_back(p); + } + } + } + + Checked check; + check.check = true; + calibrated_lidar.push_back(check); + imu_lidar.push_back(check); + check.check = false; + calibrated_lidar.push_back(check); + imu_lidar.push_back(check); + } + } + + if (point_cloud.size() > 0) + { + std::string calibrated_lidar_name = "calibrate["; + + for (int i = 0; i < calibrated_lidar.size(); i++) + { + if (calibrated_lidar[i].check) + { + calibrated_lidar_name += idToSn.at(i); + } + } + + calibrated_lidar_name += "] (step 4)"; + + if (ImGui::Button(calibrated_lidar_name.c_str())) + { + int number_of_iterations = 10; + PairWiseICP icp; + + Eigen::Affine3d m0 = calibrations.at(0); + Eigen::Affine3d m1 = calibrations.at(1); + + std::vector lidar0; + std::vector lidar1; + + for (const auto &p : point_cloud) + { + if (p.lidarid == 0) + { + lidar0.emplace_back(p); + } + else + { + lidar1.emplace_back(p); + } + } + + std::cout << "decimation: " << params.decimation << std::endl; + std::cout << "point cloud size before" << std::endl; + std::cout << "lidar0.size(): " << lidar0.size() << std::endl; + std::cout << "lidar1.size(): " << lidar1.size() << std::endl; + + lidar0 = decimate(lidar0, params.decimation, params.decimation, params.decimation); + lidar1 = decimate(lidar1, params.decimation, params.decimation, params.decimation); + + std::cout << "point cloud size after" << std::endl; + std::cout << "lidar0.size(): " << lidar0.size() << std::endl; + std::cout << "lidar1.size(): " << lidar1.size() << std::endl; + + std::vector pc0; + std::vector pc1; + + if (calibrated_lidar[0].check) + { + for (const auto &s : lidar0) + { + pc0.emplace_back(s.point.x(), s.point.y(), s.point.z()); + } + for (const auto &t : lidar1) + { + auto pp = m1 * t.point; + pc1.emplace_back(pp.x(), pp.y(), pp.z()); + } + + if (icp.compute(pc0, pc1, search_radious, number_of_iterations, m0)) + { + calibrations.at(0) = m0; + } + } + else + { + for (const auto &s : lidar0) + { + auto pp = m0 * s.point; + pc0.emplace_back(pp.x(), pp.y(), pp.z()); + } + for (const auto &t : lidar1) + { + pc1.emplace_back(t.point.x(), t.point.y(), t.point.z()); + } + if (icp.compute(pc1, pc0, search_radious, number_of_iterations, m1)) + { + calibrations.at(1) = m1; + } + } + } + + ImGui::SameLine(); + + ImGui::InputDouble("search_radious: ", &search_radious); + if (search_radious < 0.02) + { + search_radious = 0.02; + } + + /**/ + + // + ImGui::Text("========================================================================="); + + if (imu_lidar.size() == 2) + { + ImGui::Text(".......... Check imu for lidar odometry in calibration file .........."); + + int chosen_imu = -1; + for (int i = 0; i < imu_lidar.size(); i++) + { + std::string name = idToSn.at(i); + bool before = imu_lidar[i].check; + ImGui::Checkbox(std::string(name + "_imu").c_str(), &imu_lidar[i].check); + bool after = imu_lidar[i].check; + + if (!before && after) + { + chosen_imu = i; + } + } + + bool is_all_imu_false = true; + for (int i = 0; i < imu_lidar.size(); i++) + { + if (imu_lidar[i].check) + { + is_all_imu_false = false; + } + } + + if (is_all_imu_false) + { + chosen_imu = 0; + } + + if (chosen_imu != -1) + { + for (int i = 0; i < imu_lidar.size(); i++) + { + imu_lidar[i].check = false; + } + imu_lidar[chosen_imu].check = true; + } + } + + if (ImGui::Button("Save result calibration as 'calibration.json' (step 5)")) + { + std::shared_ptr save_file; + std::string output_file_name = ""; + ImGui::PushItemFlag(ImGuiItemFlags_Disabled, (bool)save_file); + const auto t = [&]() + { + auto sel = pfd::save_file("Save las or laz file", "C:\\", json_filter).result(); + output_file_name = sel; + std::cout << "las or laz file to save: '" << output_file_name << "'" << std::endl; + }; + std::thread t1(t); + t1.join(); + + if (output_file_name.size() > 0) + { + std::cout << "output_file_name: " << output_file_name << std::endl; + nlohmann::json j; + + j["calibration"][idToSn.at(0)]["order"] = "ROW"; + j["calibration"][idToSn.at(0)]["inverted"] = "FALSE"; + j["calibration"][idToSn.at(0)]["data"][0] = calibrations.at(0)(0, 0); + j["calibration"][idToSn.at(0)]["data"][1] = calibrations.at(0)(0, 1); + j["calibration"][idToSn.at(0)]["data"][2] = calibrations.at(0)(0, 2); + j["calibration"][idToSn.at(0)]["data"][3] = calibrations.at(0)(0, 3); + j["calibration"][idToSn.at(0)]["data"][4] = calibrations.at(0)(1, 0); + j["calibration"][idToSn.at(0)]["data"][5] = calibrations.at(0)(1, 1); + j["calibration"][idToSn.at(0)]["data"][6] = calibrations.at(0)(1, 2); + j["calibration"][idToSn.at(0)]["data"][7] = calibrations.at(0)(1, 3); + j["calibration"][idToSn.at(0)]["data"][8] = calibrations.at(0)(2, 0); + j["calibration"][idToSn.at(0)]["data"][9] = calibrations.at(0)(2, 1); + j["calibration"][idToSn.at(0)]["data"][10] = calibrations.at(0)(2, 2); + j["calibration"][idToSn.at(0)]["data"][11] = calibrations.at(0)(2, 3); + j["calibration"][idToSn.at(0)]["data"][12] = 0; + j["calibration"][idToSn.at(0)]["data"][13] = 0; + j["calibration"][idToSn.at(0)]["data"][14] = 0; + j["calibration"][idToSn.at(0)]["data"][15] = 1; + + j["calibration"][idToSn.at(1)]["order"] = "ROW"; + j["calibration"][idToSn.at(1)]["inverted"] = "FALSE"; + j["calibration"][idToSn.at(1)]["data"][0] = calibrations.at(1)(0, 0); + j["calibration"][idToSn.at(1)]["data"][1] = calibrations.at(1)(0, 1); + j["calibration"][idToSn.at(1)]["data"][2] = calibrations.at(1)(0, 2); + j["calibration"][idToSn.at(1)]["data"][3] = calibrations.at(1)(0, 3); + j["calibration"][idToSn.at(1)]["data"][4] = calibrations.at(1)(1, 0); + j["calibration"][idToSn.at(1)]["data"][5] = calibrations.at(1)(1, 1); + j["calibration"][idToSn.at(1)]["data"][6] = calibrations.at(1)(1, 2); + j["calibration"][idToSn.at(1)]["data"][7] = calibrations.at(1)(1, 3); + j["calibration"][idToSn.at(1)]["data"][8] = calibrations.at(1)(2, 0); + j["calibration"][idToSn.at(1)]["data"][9] = calibrations.at(1)(2, 1); + j["calibration"][idToSn.at(1)]["data"][10] = calibrations.at(1)(2, 2); + j["calibration"][idToSn.at(1)]["data"][11] = calibrations.at(1)(2, 3); + j["calibration"][idToSn.at(1)]["data"][12] = 0; + j["calibration"][idToSn.at(1)]["data"][13] = 0; + j["calibration"][idToSn.at(1)]["data"][14] = 0; + j["calibration"][idToSn.at(1)]["data"][15] = 1; + + if (imu_lidar[0].check) + { + j["imuToUse"] = idToSn.at(0); + } + else + { + j["imuToUse"] = idToSn.at(1); + } + std::ofstream fs(output_file_name); + if (!fs.good()) + return; + fs << j.dump(2); + fs.close(); + } + } + ImGui::SameLine(); + ImGui::Text(" imprtant notice! 'lidar_odometry_step_1.exe' program requires file name 'calibration.json' in folder with data"); + ImGui::Text("========================================================================="); + } + ImGui::End(); + } +#endif + return; +} + +void display() +{ + ImGuiIO &io = ImGui::GetIO(); + glViewport(0, 0, (GLsizei)io.DisplaySize.x, (GLsizei)io.DisplaySize.y); + glMatrixMode(GL_PROJECTION); + glLoadIdentity(); + float ratio = float(io.DisplaySize.x) / float(io.DisplaySize.y); + + if (is_ortho) + { + glOrtho(-camera_ortho_xy_view_zoom, camera_ortho_xy_view_zoom, + -camera_ortho_xy_view_zoom / ratio, + camera_ortho_xy_view_zoom / ratio, -100000, 100000); + + glm::mat4 proj = glm::orthoLH_ZO(-camera_ortho_xy_view_zoom, camera_ortho_xy_view_zoom, + -camera_ortho_xy_view_zoom / ratio, + camera_ortho_xy_view_zoom / ratio, -100, 100); + + std::copy(&proj[0][0], &proj[3][3], m_ortho_projection); + + Eigen::Vector3d v_eye_t(-camera_ortho_xy_view_shift_x, camera_ortho_xy_view_shift_y, camera_mode_ortho_z_center_h + 10); + Eigen::Vector3d v_center_t(-camera_ortho_xy_view_shift_x, camera_ortho_xy_view_shift_y, camera_mode_ortho_z_center_h); + Eigen::Vector3d v(0, 1, 0); + + TaitBryanPose pose_tb; + pose_tb.px = 0.0; + pose_tb.py = 0.0; + pose_tb.pz = 0.0; + pose_tb.om = 0.0; + pose_tb.fi = 0.0; + pose_tb.ka = -camera_ortho_xy_view_rotation_angle_deg * M_PI / 180.0; + auto m = affine_matrix_from_pose_tait_bryan(pose_tb); + + Eigen::Vector3d v_t = m * v; + + gluLookAt(v_eye_t.x(), v_eye_t.y(), v_eye_t.z(), + v_center_t.x(), v_center_t.y(), v_center_t.z(), + v_t.x(), v_t.y(), v_t.z()); + glm::mat4 lookat = glm::lookAt(glm::vec3(v_eye_t.x(), v_eye_t.y(), v_eye_t.z()), + glm::vec3(v_center_t.x(), v_center_t.y(), v_center_t.z()), + glm::vec3(v_t.x(), v_t.y(), v_t.z())); + std::copy(&lookat[0][0], &lookat[3][3], m_ortho_gizmo_view); + } + + glClearColor(clear_color.x * clear_color.w, clear_color.y * clear_color.w, clear_color.z * clear_color.w, clear_color.w); + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + glEnable(GL_DEPTH_TEST); + + if (!is_ortho) + { + reshape((GLsizei)io.DisplaySize.x, (GLsizei)io.DisplaySize.y); + + Eigen::Affine3f viewTranslation = Eigen::Affine3f::Identity(); + viewTranslation.translate(rotation_center); + Eigen::Affine3f viewLocal = Eigen::Affine3f::Identity(); + viewLocal.translate(Eigen::Vector3f(translate_x, translate_y, translate_z)); + viewLocal.rotate(Eigen::AngleAxisf(M_PI * rotate_x / 180.f, Eigen::Vector3f::UnitX())); + viewLocal.rotate(Eigen::AngleAxisf(M_PI * rotate_y / 180.f, Eigen::Vector3f::UnitZ())); + + Eigen::Affine3f viewTranslation2 = Eigen::Affine3f::Identity(); + viewTranslation2.translate(-rotation_center); + + Eigen::Affine3f result = viewTranslation * viewLocal * viewTranslation2; + + glLoadMatrixf(result.matrix().data()); + /* glTranslatef(translate_x, translate_y, translate_z); + glRotatef(rotate_x, 1.0, 0.0, 0.0); + glRotatef(rotate_y, 0.0, 0.0, 1.0);*/ + } + else + { + glMatrixMode(GL_MODELVIEW); + glLoadIdentity(); + } + + if (ImGui::GetIO().KeyCtrl) + { + glBegin(GL_LINES); + glColor3f(1.f, 1.f, 1.f); + glVertex3fv(rotation_center.data()); + glVertex3f(rotation_center.x() + 1.f, rotation_center.y(), rotation_center.z()); + glVertex3fv(rotation_center.data()); + glVertex3f(rotation_center.x() - 1.f, rotation_center.y(), rotation_center.z()); + glVertex3fv(rotation_center.data()); + glVertex3f(rotation_center.x(), rotation_center.y() - 1.f, rotation_center.z()); + glVertex3fv(rotation_center.data()); + glVertex3f(rotation_center.x(), rotation_center.y() + 1.f, rotation_center.z()); + glVertex3fv(rotation_center.data()); + glVertex3f(rotation_center.x(), rotation_center.y(), rotation_center.z() - 1.f); + glVertex3fv(rotation_center.data()); + glVertex3f(rotation_center.x(), rotation_center.y(), rotation_center.z() + 1.f); + glEnd(); + } + + /*if (show_axes) + { + glLineWidth(2); + glBegin(GL_LINES); + glColor3f(1.0f, 0.0f, 0.0f); + glVertex3f(0.0f, 0.0f, 0.0f); + glVertex3f(1, 0.0f, 0.0f); + + glColor3f(0.0f, 1.0f, 0.0f); + glVertex3f(0.0f, 0.0f, 0.0f); + glVertex3f(0.0f, 1, 0.0f); + + glColor3f(0.0f, 0.0f, 1.0f); + glVertex3f(0.0f, 0.0f, 0.0f); + glVertex3f(0.0f, 0.0f, 1); + glEnd(); + glLineWidth(1); + }*/ + + if (show_pc) + { + glBegin(GL_POINTS); + for (int i = 0; i < point_cloud.size(); i++) + { + glColor3f(pc_color.x, pc_color.y, pc_color.z); + if (apply_intrinsics_in_render) + { + auto p = intrinsics[i] * point_cloud[i].point; + glVertex3f(p.x(), p.y(), p.z()); + } + else + { + glVertex3f(point_cloud[i].point.x(), point_cloud[i].point.y(), point_cloud[i].point.z()); + } + } + glEnd(); + } + + // ImVec4 pc_color_point = ImVec4(1.0f, 1.0f, 0.0f, 1.00f); + // ImVec4 pc_color_ray = ImVec4(1.0f, 0.0f, 1.0f, 1.00f); + if (show_single_point_and_ray) + { + glPointSize(single_point_size); + glColor3f(pc_color_point.x, pc_color_point.y, pc_color_point.z); + glBegin(GL_POINTS); + glVertex3f(point_cloud[index_rendered_point_and_ray].point.x(), point_cloud[index_rendered_point_and_ray].point.y(), point_cloud[index_rendered_point_and_ray].point.z()); + glEnd(); + glPointSize(1); + + glColor3f(pc_color_ray.x, pc_color_ray.y, pc_color_ray.z); + glBegin(GL_LINES); + glVertex3f(intrinsics[index_rendered_point_and_ray](0, 3), intrinsics[index_rendered_point_and_ray](1, 3), intrinsics[index_rendered_point_and_ray](2, 3)); + glVertex3f(point_cloud[index_rendered_point_and_ray].point.x(), point_cloud[index_rendered_point_and_ray].point.y(), point_cloud[index_rendered_point_and_ray].point.z()); + glEnd(); + } + + if (show_point_cloud_path) + { + glColor3f(pc_color_point_cloud_path.x, pc_color_point_cloud_path.y, pc_color_point_cloud_path.z); + + glBegin(GL_LINE_STRIP); + for (int i = 0; i < point_cloud_path.size(); i++) + { + bool is_to_render = true; + + if (i > 0) + { + if ((point_cloud_path[i - 1] - point_cloud_path[i]).norm() > 1.0) + { + is_to_render = false; + } + } + + if (point_cloud_path[i].norm() < 0.1) + { + is_to_render = false; + } + + if (!is_to_render) + { + glEnd(); + glBegin(GL_LINE_STRIP); + } + else + { + glVertex3f(point_cloud_path[i].x(), point_cloud_path[i].y(), point_cloud_path[i].z()); + } + } + glEnd(); + } + + if (show_intrinsic_path) + { + glColor3f(intrinsic_path_color.x, intrinsic_path_color.y, intrinsic_path_color.z); + + glBegin(GL_LINE_STRIP); + for (int i = 0; i < intrinsic_path.size(); i++) + { + glVertex3f(intrinsic_path[i].x(), intrinsic_path[i].y(), intrinsic_path[i].z()); + } + glEnd(); + // ImGui::ColorEdit3("intrinsic_path_color", (float *)&intrinsic_path_color); + + // if (ImGui::Button("clear point_cloud_path and intrinsic path")) + //{ + // point_cloud_path.clear(); + // intrinsic_path.clear(); + // } + } + + if (show_intrinsics) + { + glColor3f(intrinsics_color.x, intrinsics_color.y, intrinsics_color.z); + + glBegin(GL_LINE_STRIP); + for (int i = 0; i < intrinsics.size(); i++) + { + glVertex3f(intrinsics[i](0, 3), intrinsics[i](1, 3), intrinsics[i](2, 3)); + } + glEnd(); + } + + glColor3f(0, 0, 0); + glBegin(GL_LINES); + for (const auto &p : point_intrinsic_correspondances) + { + glVertex3f(p.first.x(), p.first.y(), p.first.z()); + glVertex3f(p.second.x(), p.second.y(), p.second.z()); + } + glEnd(); + // point_intrinsic_correspondances + + // ImGui::Checkbox("show_intrinsics", &show_intrinsics); + // ImGui::Checkbox("apply_intrinsics_in_render", &apply_intrinsics_in_render); + + // if (index_rendered_point_and_ray > point_cloud.size() - 1) + //{ + // index_rendered_point_and_ray = point_cloud.size() - 1; + // } + +#if 0 + if (calibration.size() > 0) + { + for (const auto &c : calibration) + { + Eigen::Affine3d m = c.second; + glBegin(GL_LINES); + glColor3f(1.0f, 0.0f, 0.0f); + glVertex3f(m(0, 3), m(1, 3), m(2, 3)); + glVertex3f(m(0, 3) + m(0, 0), m(1, 3) + m(1, 0), m(2, 3) + m(2, 0)); + + glColor3f(0.0f, 1.0f, 0.0f); + glVertex3f(m(0, 3), m(1, 3), m(2, 3)); + glVertex3f(m(0, 3) + m(0, 1), m(1, 3) + m(1, 1), m(2, 3) + m(2, 1)); + + glColor3f(0.0f, 0.0f, 1.0f); + glVertex3f(m(0, 3), m(1, 3), m(2, 3)); + glVertex3f(m(0, 3) + m(0, 2), m(1, 3) + m(1, 2), m(2, 3) + m(2, 2)); + glEnd(); + } + } + + // point_cloud + // + if (manual_calibration) + { + int index_calibrated_lidar = -1; + for (int i = 0; i < calibrated_lidar.size(); i++) + { + if (calibrated_lidar[i].check) + { + index_calibrated_lidar = i; + } + } + + glBegin(GL_POINTS); + for (const auto &p : point_cloud) + { + Eigen::Affine3d cal = calibrations.empty() ? Eigen::Affine3d::Identity() : calibrations.at(p.lidarid); + + if (p.lidarid == 0) + { + if (p.lidarid != index_calibrated_lidar) + { + glColor3f(0.5, 0.5, 0.5); + } + else + { + glColor3f(pc_color.x, pc_color.y, pc_color.z); + } + } + else + { + if (p.lidarid != index_calibrated_lidar) + { + glColor3f(0.5, 0.5, 0.5); + } + else + { + glColor3f(pc_color2.x, pc_color2.y, pc_color2.z); + } + } + + auto pp = cal * p.point; + + glVertex3f(pp.x(), pp.y(), pp.z()); + } + glEnd(); + } + else + { + glBegin(GL_POINTS); + for (const auto &p : point_cloud) + { + Eigen::Affine3d cal = calibrations.empty() ? Eigen::Affine3d::Identity() : calibrations.at(p.lidarid); + + if (p.lidarid == 0) + { + glColor3f(pc_color.x, pc_color.y, pc_color.z); + } + else + { + glColor3f(pc_color2.x, pc_color2.y, pc_color2.z); + } + + auto pp = cal * p.point; + + glVertex3f(pp.x(), pp.y(), pp.z()); + } + glEnd(); + } + // + + if (show_grid) + { + glColor3f(0.4, 0.4, 0.4); + glBegin(GL_LINES); + for (float x = -10.0f; x <= 10.0f; x += 1.0f) + { + glVertex3f(x, -10.0, 0.0); + glVertex3f(x, 10.0, 0.0); + } + for (float y = -10.0f; y <= 10.0f; y += 1.0f) + { + glVertex3f(-10.0, y, 0.0); + glVertex3f(10.0, y, 0.0); + } + glEnd(); + } +#endif + ImGui_ImplOpenGL2_NewFrame(); + ImGui_ImplGLUT_NewFrame(); + + project_gui(); + + ImGui::Render(); + ImGui_ImplOpenGL2_RenderDrawData(ImGui::GetDrawData()); + + glutSwapBuffers(); + glutPostRedisplay(); +} + +bool initGL(int *argc, char **argv) +{ + glutInit(argc, argv); + glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE); + glutInitWindowSize(window_width, window_height); + glutCreateWindow("livox_mid_360_intrinsic_calibration " HDMAPPING_VERSION_STRING); + glutDisplayFunc(display); + glutMotionFunc(motion); + + // default initialization + glClearColor(0.0, 0.0, 0.0, 1.0); + glEnable(GL_DEPTH_TEST); + + // viewport + glViewport(0, 0, window_width, window_height); + + // projection + glMatrixMode(GL_PROJECTION); + glLoadIdentity(); + gluPerspective(60.0, (GLfloat)window_width / (GLfloat)window_height, 0.01, 10000.0); + glutReshapeFunc(reshape); + ImGui::CreateContext(); + ImGuiIO &io = ImGui::GetIO(); + (void)io; + // io.ConfigFlags |= ImGuiConfigFlags_NavEnableKeyboard; // Enable Keyboard Controls + + ImGui::StyleColorsDark(); + ImGui_ImplGLUT_Init(); + ImGui_ImplGLUT_InstallFuncs(); + ImGui_ImplOpenGL2_Init(); + return true; +} + +void wheel(int button, int dir, int x, int y); + +void mouse(int glut_button, int state, int x, int y) +{ + ImGuiIO &io = ImGui::GetIO(); + io.MousePos = ImVec2((float)x, (float)y); + int button = -1; + if (glut_button == GLUT_LEFT_BUTTON) + button = 0; + if (glut_button == GLUT_RIGHT_BUTTON) + button = 1; + if (glut_button == GLUT_MIDDLE_BUTTON) + button = 2; + if (button != -1 && state == GLUT_DOWN) + io.MouseDown[button] = true; + if (button != -1 && state == GLUT_UP) + io.MouseDown[button] = false; + + static int glutMajorVersion = glutGet(GLUT_VERSION) / 10000; + if (state == GLUT_DOWN && (glut_button == 3 || glut_button == 4) && + glutMajorVersion < 3) + { + wheel(glut_button, glut_button == 3 ? 1 : -1, x, y); + } + + if (!io.WantCaptureMouse) + { + if (glut_button == GLUT_MIDDLE_BUTTON && state == GLUT_DOWN && io.KeyCtrl) + { + } + + if (state == GLUT_DOWN) + { + mouse_buttons |= 1 << glut_button; + } + else if (state == GLUT_UP) + { + mouse_buttons = 0; + } + mouse_old_x = x; + mouse_old_y = y; + } +} + +void wheel(int button, int dir, int x, int y) +{ + if (dir > 0) + { + if (is_ortho) + { + camera_ortho_xy_view_zoom -= 0.1f * camera_ortho_xy_view_zoom; + + if (camera_ortho_xy_view_zoom < 0.1) + { + camera_ortho_xy_view_zoom = 0.1; + } + } + else + { + translate_z -= 0.05f * translate_z; + } + } + else + { + if (is_ortho) + { + camera_ortho_xy_view_zoom += 0.1 * camera_ortho_xy_view_zoom; + } + else + { + translate_z += 0.05f * translate_z; + } + } + + return; +} + +int main(int argc, char *argv[]) +{ + // params.decimation = 0.03; + + initGL(&argc, argv); + glutDisplayFunc(display); + glutMouseFunc(mouse); + glutMotionFunc(motion); + glutMouseWheelFunc(wheel); + + glutMainLoop(); + + ImGui_ImplOpenGL2_Shutdown(); + ImGui_ImplGLUT_Shutdown(); + + ImGui::DestroyContext(); + return 0; +} + +void calibrate_intrinsics() +{ + bool multithread = false; + + std::cout + << "calibrate_intrinsics" << std::endl; + + // step 1 build rgd + std::cout << "build rgd" << std::endl; + + // for (int i = 0; i < pp.size(); i++) + //{ + // pp[i].point = params.m_g * pp[i].point; + // } + + std::vector point_cloud_global; + // std::vector intrinsics; + + for (int i = 0; i < point_cloud.size(); i++) + { + Point3Di p = point_cloud[i]; + p.point = intrinsics[i] * point_cloud[i].point; + point_cloud_global.push_back(p); + } + + NDT::GridParameters rgd_params; + rgd_params.resolution_X = 0.2; + rgd_params.resolution_Y = 0.2; + rgd_params.resolution_Z = 0.2; + + NDTBucketMapType buckets; + + update_rgd(rgd_params, buckets, point_cloud_global, {0, 0, 0}); + + std::cout << "buckets.size(): " << buckets.size() << std::endl; + + ///////////////////////////////////////////////////////////////////////// + std::vector> tripletListA; + std::vector> tripletListP; + std::vector> tripletListB; + + // Eigen::MatrixXd AtPAndt(intrinsics.size() * 6, intrinsics.size() * 6); + // AtPAndt.setZero(); + // Eigen::MatrixXd AtPBndt(intrinsics.size() * 6, 1); + // AtPBndt.setZero(); + Eigen::Vector3d b(rgd_params.resolution_X, rgd_params.resolution_Y, rgd_params.resolution_Z); + + // std::vector mutexes(intrinsics.size()); + + // std::cout << "jojo" << std::endl; + const auto hessian_fun = [&](const Point3Di &intermediate_points_i) + { + int ir = tripletListB.size(); + double delta_x; + double delta_y; + double delta_z; + + Eigen::Affine3d m_pose = intrinsics[intermediate_points_i.index_pose]; + Eigen::Vector3d point_local(intermediate_points_i.point.x(), intermediate_points_i.point.y(), intermediate_points_i.point.z()); + Eigen::Vector3d point_global = m_pose * point_local; + + auto index_of_bucket = get_rgd_index(point_global, b); + + auto bucket_it = buckets.find(index_of_bucket); + // no bucket found + if (bucket_it == buckets.end()) + { + return; + } + auto &this_bucket = bucket_it->second; + + Eigen::Vector3d mean(this_bucket.mean.x(), this_bucket.mean.y(), this_bucket.mean.z()); + + Eigen::Matrix jacobian; + TaitBryanPose pose_s = pose_tait_bryan_from_affine_matrix(m_pose); + + point_to_point_source_to_target_tait_bryan_wc(delta_x, delta_y, delta_z, + pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, + point_local.x(), point_local.y(), point_local.z(), mean.x(), mean.y(), mean.z()); + + point_to_point_source_to_target_tait_bryan_wc_jacobian(jacobian, + pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, + point_local.x(), point_local.y(), point_local.z()); + + int c = intermediate_points_i.index_pose * 6; + for (int row = 0; row < 3; row++) + { + for (int col = 0; col < 6; col++) + { + if (jacobian(row, col) != 0.0) + { + tripletListA.emplace_back(ir + row, c + col, -jacobian(row, col)); + } + } + } + + Eigen::Matrix3d infm = this_bucket.cov.inverse(); + + tripletListB.emplace_back(ir, 0, delta_x); + tripletListB.emplace_back(ir + 1, 0, delta_y); + tripletListB.emplace_back(ir + 2, 0, delta_z); + + tripletListP.emplace_back(ir, ir, infm(0, 0)); + tripletListP.emplace_back(ir, ir + 1, infm(0, 1)); + tripletListP.emplace_back(ir, ir + 2, infm(0, 2)); + tripletListP.emplace_back(ir + 1, ir, infm(1, 0)); + tripletListP.emplace_back(ir + 1, ir + 1, infm(1, 1)); + tripletListP.emplace_back(ir + 1, ir + 2, infm(1, 2)); + tripletListP.emplace_back(ir + 2, ir, infm(2, 0)); + tripletListP.emplace_back(ir + 2, ir + 1, infm(2, 1)); + tripletListP.emplace_back(ir + 2, ir + 2, infm(2, 2)); + }; + + std::cout << "start adding lidar observations" << std::endl; + if (multithread) + { + std::for_each(std::execution::par_unseq, std::begin(point_cloud), std::end(point_cloud), hessian_fun); + } + else + { + std::for_each(std::begin(point_cloud), std::end(point_cloud), hessian_fun); + } + std::cout << "adding lidar observations finished" << std::endl; + + std::vector> odo_edges; + for (size_t i = 1; i < intrinsics.size(); i++) + { + odo_edges.emplace_back(i - 1, i); + } + + std::vector poses; + std::vector poses_desired; + + for (size_t i = 0; i < intrinsics.size(); i++) + { + poses.push_back(pose_tait_bryan_from_affine_matrix(intrinsics[i])); + } + for (size_t i = 0; i < intrinsics.size(); i++) + { + poses_desired.push_back(pose_tait_bryan_from_affine_matrix(intrinsics[i])); + } + + for (size_t i = 0; i < odo_edges.size(); i++) + { + Eigen::Matrix delta; + relative_pose_obs_eq_tait_bryan_wc_case1( + delta, + poses[odo_edges[i].first].px, + poses[odo_edges[i].first].py, + poses[odo_edges[i].first].pz, + poses[odo_edges[i].first].om, + poses[odo_edges[i].first].fi, + poses[odo_edges[i].first].ka, + poses[odo_edges[i].second].px, + poses[odo_edges[i].second].py, + poses[odo_edges[i].second].pz, + poses[odo_edges[i].second].om, + poses[odo_edges[i].second].fi, + poses[odo_edges[i].second].ka, + 0, + 0, + 0, + 0, + 0, + 0); + + Eigen::Matrix jacobian; + relative_pose_obs_eq_tait_bryan_wc_case1_jacobian(jacobian, + poses[odo_edges[i].first].px, + poses[odo_edges[i].first].py, + poses[odo_edges[i].first].pz, + poses[odo_edges[i].first].om, + poses[odo_edges[i].first].fi, + poses[odo_edges[i].first].ka, + poses[odo_edges[i].second].px, + poses[odo_edges[i].second].py, + poses[odo_edges[i].second].pz, + poses[odo_edges[i].second].om, + poses[odo_edges[i].second].fi, + poses[odo_edges[i].second].ka); + + int ir = tripletListB.size(); + + int ic_1 = odo_edges[i].first * 6; + int ic_2 = odo_edges[i].second * 6; + + for (size_t row = 0; row < 6; row++) + { + tripletListA.emplace_back(ir + row, ic_1, -jacobian(row, 0)); + tripletListA.emplace_back(ir + row, ic_1 + 1, -jacobian(row, 1)); + tripletListA.emplace_back(ir + row, ic_1 + 2, -jacobian(row, 2)); + tripletListA.emplace_back(ir + row, ic_1 + 3, -jacobian(row, 3)); + tripletListA.emplace_back(ir + row, ic_1 + 4, -jacobian(row, 4)); + tripletListA.emplace_back(ir + row, ic_1 + 5, -jacobian(row, 5)); + + tripletListA.emplace_back(ir + row, ic_2, -jacobian(row, 6)); + tripletListA.emplace_back(ir + row, ic_2 + 1, -jacobian(row, 7)); + tripletListA.emplace_back(ir + row, ic_2 + 2, -jacobian(row, 8)); + tripletListA.emplace_back(ir + row, ic_2 + 3, -jacobian(row, 9)); + tripletListA.emplace_back(ir + row, ic_2 + 4, -jacobian(row, 10)); + tripletListA.emplace_back(ir + row, ic_2 + 5, -jacobian(row, 11)); + } + + tripletListB.emplace_back(ir, 0, delta(0, 0)); + tripletListB.emplace_back(ir + 1, 0, delta(1, 0)); + tripletListB.emplace_back(ir + 2, 0, delta(2, 0)); + tripletListB.emplace_back(ir + 3, 0, delta(3, 0)); + tripletListB.emplace_back(ir + 4, 0, delta(4, 0)); + tripletListB.emplace_back(ir + 5, 0, delta(5, 0)); + + tripletListP.emplace_back(ir, ir, 100000000); + tripletListP.emplace_back(ir + 1, ir + 1, 100000000); + tripletListP.emplace_back(ir + 2, ir + 2, 100000000); + tripletListP.emplace_back(ir + 3, ir + 3, 10000000000); + tripletListP.emplace_back(ir + 4, ir + 4, 10000000000); + tripletListP.emplace_back(ir + 5, ir + 5, 10000000000); + } + + std::vector> odo_edges_to_0; + for (size_t i = 0; i < intrinsics.size(); i++) + { + odo_edges_to_0.emplace_back(i, i); + } + + for (size_t i = 0; i < odo_edges_to_0.size(); i++) + { + Eigen::Matrix delta; + relative_pose_obs_eq_tait_bryan_wc_case1( + delta, + poses[odo_edges_to_0[i].first].px, + poses[odo_edges_to_0[i].first].py, + poses[odo_edges_to_0[i].first].pz, + poses[odo_edges_to_0[i].first].om, + poses[odo_edges_to_0[i].first].fi, + poses[odo_edges_to_0[i].first].ka, + poses[odo_edges_to_0[i].second].px, + poses[odo_edges_to_0[i].second].py, + poses[odo_edges_to_0[i].second].pz, + poses[odo_edges_to_0[i].second].om, + poses[odo_edges_to_0[i].second].fi, + poses[odo_edges_to_0[i].second].ka, + 0, + 0, + 0, + 0, + 0, + 0); + + Eigen::Matrix jacobian; + relative_pose_obs_eq_tait_bryan_wc_case1_jacobian(jacobian, + poses[odo_edges_to_0[i].first].px, + poses[odo_edges_to_0[i].first].py, + poses[odo_edges_to_0[i].first].pz, + poses[odo_edges_to_0[i].first].om, + poses[odo_edges_to_0[i].first].fi, + poses[odo_edges_to_0[i].first].ka, + poses[odo_edges_to_0[i].second].px, + poses[odo_edges_to_0[i].second].py, + poses[odo_edges_to_0[i].second].pz, + poses[odo_edges_to_0[i].second].om, + poses[odo_edges_to_0[i].second].fi, + poses[odo_edges_to_0[i].second].ka); + + int ir = tripletListB.size(); + + int ic_1 = odo_edges_to_0[i].first * 6; + int ic_2 = odo_edges_to_0[i].second * 6; + + for (size_t row = 0; row < 6; row++) + { + tripletListA.emplace_back(ir + row, ic_1, -jacobian(row, 0)); + tripletListA.emplace_back(ir + row, ic_1 + 1, -jacobian(row, 1)); + tripletListA.emplace_back(ir + row, ic_1 + 2, -jacobian(row, 2)); + tripletListA.emplace_back(ir + row, ic_1 + 3, -jacobian(row, 3)); + tripletListA.emplace_back(ir + row, ic_1 + 4, -jacobian(row, 4)); + tripletListA.emplace_back(ir + row, ic_1 + 5, -jacobian(row, 5)); + + tripletListA.emplace_back(ir + row, ic_2, -jacobian(row, 6)); + tripletListA.emplace_back(ir + row, ic_2 + 1, -jacobian(row, 7)); + tripletListA.emplace_back(ir + row, ic_2 + 2, -jacobian(row, 8)); + tripletListA.emplace_back(ir + row, ic_2 + 3, -jacobian(row, 9)); + tripletListA.emplace_back(ir + row, ic_2 + 4, -jacobian(row, 10)); + tripletListA.emplace_back(ir + row, ic_2 + 5, -jacobian(row, 11)); + } + + tripletListB.emplace_back(ir, 0, delta(0, 0)); + tripletListB.emplace_back(ir + 1, 0, delta(1, 0)); + tripletListB.emplace_back(ir + 2, 0, delta(2, 0)); + tripletListB.emplace_back(ir + 3, 0, delta(3, 0)); + tripletListB.emplace_back(ir + 4, 0, delta(4, 0)); + tripletListB.emplace_back(ir + 5, 0, delta(5, 0)); + + tripletListP.emplace_back(ir, ir, 1000000); + tripletListP.emplace_back(ir + 1, ir + 1, 1000000); + tripletListP.emplace_back(ir + 2, ir + 2, 1000000); + tripletListP.emplace_back(ir + 3, ir + 3, 10000000000); + tripletListP.emplace_back(ir + 4, ir + 4, 10000000000); + tripletListP.emplace_back(ir + 5, ir + 5, 10000000000); + } + + int ic = 0; + int ir = tripletListB.size(); + tripletListA.emplace_back(ir, ic * 6 + 0, 1); + tripletListA.emplace_back(ir + 1, ic * 6 + 1, 1); + tripletListA.emplace_back(ir + 2, ic * 6 + 2, 1); + tripletListA.emplace_back(ir + 3, ic * 6 + 3, 1); + tripletListA.emplace_back(ir + 4, ic * 6 + 4, 1); + tripletListA.emplace_back(ir + 5, ic * 6 + 5, 1); + + tripletListP.emplace_back(ir, ir, 1); + tripletListP.emplace_back(ir + 1, ir + 1, 1); + tripletListP.emplace_back(ir + 2, ir + 2, 1); + tripletListP.emplace_back(ir + 3, ir + 3, 1); + tripletListP.emplace_back(ir + 4, ir + 4, 1); + tripletListP.emplace_back(ir + 5, ir + 5, 1); + + tripletListB.emplace_back(ir, 0, 0); + tripletListB.emplace_back(ir + 1, 0, 0); + tripletListB.emplace_back(ir + 2, 0, 0); + tripletListB.emplace_back(ir + 3, 0, 0); + tripletListB.emplace_back(ir + 4, 0, 0); + tripletListB.emplace_back(ir + 5, 0, 0); + + Eigen::SparseMatrix matA(tripletListB.size(), intrinsics.size() * 6); + Eigen::SparseMatrix matP(tripletListB.size(), tripletListB.size()); + Eigen::SparseMatrix matB(tripletListB.size(), 1); + + matA.setFromTriplets(tripletListA.begin(), tripletListA.end()); + matP.setFromTriplets(tripletListP.begin(), tripletListP.end()); + matB.setFromTriplets(tripletListB.begin(), tripletListB.end()); + + Eigen::SparseMatrix AtPA(intrinsics.size() * 6, intrinsics.size() * 6); + Eigen::SparseMatrix AtPB(intrinsics.size() * 6, 1); + + { + Eigen::SparseMatrix AtP = matA.transpose() * matP; + AtPA = (AtP)*matA; + AtPB = (AtP)*matB; + } + + tripletListA.clear(); + tripletListP.clear(); + tripletListB.clear(); + + // AtPA += AtPAndt.sparseView(); + // AtPB += AtPBndt.sparseView(); + + // Eigen::SparseMatrix AtPA_I(intrinsics.size() * 6, intrinsics.size() * 6); + // AtPA_I.setIdentity(); + // AtPA_I *= 1; + // AtPA += AtPA_I; + + Eigen::SimplicialCholesky> + solver(AtPA); + std::cout << "start solving" << std::endl; + Eigen::SparseMatrix x = solver.solve(AtPB); + std::cout << "start finished" << std::endl; + std::vector h_x; + for (int k = 0; k < x.outerSize(); ++k) + { + for (Eigen::SparseMatrix::InnerIterator it(x, k); it; ++it) + { + // std::cout << it.value() << " "; + h_x.push_back(it.value()); + } + } + + if (h_x.size() == 6 * intrinsics.size()) + { + int counter = 0; + + for (size_t i = 0; i < intrinsics.size(); i++) + { + TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(intrinsics[i]); + auto prev_pose = pose; + pose.px += h_x[counter++]; + pose.py += h_x[counter++]; + pose.pz += h_x[counter++]; + pose.om += h_x[counter++]; + pose.fi += h_x[counter++]; + pose.ka += h_x[counter++] * 0; + intrinsics[i] = affine_matrix_from_pose_tait_bryan(pose); + } + } + else + { + std::cout << "intrinsic calibration failed" << std::endl; + } + return; +} \ No newline at end of file diff --git a/apps/mandeye_mission_recorder_calibration/mandeye_mission_recorder_calibration.cpp b/apps/mandeye_mission_recorder_calibration/mandeye_mission_recorder_calibration.cpp index 8e412ac..4a46cd6 100644 --- a/apps/mandeye_mission_recorder_calibration/mandeye_mission_recorder_calibration.cpp +++ b/apps/mandeye_mission_recorder_calibration/mandeye_mission_recorder_calibration.cpp @@ -655,9 +655,9 @@ void project_gui() ImGui::SameLine(); ImGui::InputDouble("search_radious: ", &search_radious); - if (search_radious < 0.05) + if (search_radious < 0.02) { - search_radious = 0.05; + search_radious = 0.02; } /**/ diff --git a/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp b/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp index 102a542..5410c4a 100644 --- a/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp +++ b/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp @@ -22,6 +22,8 @@ #include +#include + #define SAMPLE_PERIOD (1.0 / 200.0) namespace fs = std::filesystem; @@ -58,6 +60,16 @@ float m_ortho_gizmo_view[] = {1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1}; +struct AllData +{ + std::vector> timestamps; + std::vector poses; + std::vector points_local; + std::vector lidar_ids; +}; + +std::vector all_data; + std::vector laz_files; std::vector csv_files; std::vector sn_files; @@ -71,10 +83,29 @@ bool fusionConventionNed = false; int number_of_points_threshold = 20000; bool is_init = true; int index_rendered_points_local = -1; -std::vector> all_points_local; -std::vector> all_lidar_ids; +// std::vector> all_points_local; +// std::vector> all_points_local; +// std::vector> all_lidar_ids; std::vector indexes_to_filename; std::vector all_file_names; +double ahrs_gain = 0.5; +// bool is_slerp = false; + +const std::vector LAS_LAZ_filter = {"LAS file (*.laz)", "*.laz", "LASzip file (*.las)", "*.las", "All files", "*"}; + +double rgd_x = 0.5; +double rgd_y = 5; +double rgd_z = 5; + +std::vector> rgd_nn; +std::vector> mean_cov; +bool show_mean_cov = false; +bool show_rgd_nn = false; + +void optimize(); +std::vector> get_nn(); +void draw_ellipse(const Eigen::Matrix3d &covar, Eigen::Vector3d &mean, Eigen::Vector3f color, float nstd); +std::vector> get_mean_cov(); void reshape(int w, int h) { @@ -174,14 +205,17 @@ void project_gui() } } + ImGui::InputDouble("ahrs_gain", &ahrs_gain); + // ImGui::Checkbox("is_slerp", &is_slerp); + ImGui::InputInt("index_rendered_points_local", &index_rendered_points_local); if (index_rendered_points_local < 0) { index_rendered_points_local = 0; } - if (index_rendered_points_local >= all_points_local.size()) + if (index_rendered_points_local >= all_data.size()) { - index_rendered_points_local = all_points_local.size() - 1; + index_rendered_points_local = all_data.size() - 1; } if (is_init) @@ -348,6 +382,7 @@ void project_gui() { ahrs.settings.convention = FusionConventionNed; } + ahrs.settings.gain = ahrs_gain; std::map> trajectory; @@ -391,11 +426,28 @@ void project_gui() } std::cout << "number of points: " << number_of_points << std::endl; - std::vector points_local; + std::cout << "start indexing points" << std::endl; + + // std::vector points_global; + std::vector points_local; std::vector lidar_ids; + Eigen::Affine3d m_prev; + Eigen::Affine3d m_next; + + Eigen::Quaterniond q_prev; + Eigen::Quaterniond q_next; + Eigen::Quaterniond q; + + double time_prev; + double time_next; + double curr_time; + + double t; + for (int i = 0; i < pointsPerFile.size(); i++) { + std::cout << "indexed: " << i + 1 << " of " << pointsPerFile.size() << " files" << std::endl; for (const auto &pp : pointsPerFile[i]) { auto lower = std::lower_bound(timestamps.begin(), timestamps.end(), pp.timestamp, @@ -407,195 +459,72 @@ void project_gui() if (index_pose >= 0 && index_pose < poses.size()) { auto ppp = pp; - ppp.point = poses[index_pose] * ppp.point; - // int lidar_id = pp.lidarid; - lidar_ids.push_back(pp.lidarid); - points_local.push_back(ppp.point); - } - - if (points_local.size() > number_of_points_threshold) - { - all_points_local.push_back(points_local); - indexes_to_filename.push_back(i); - points_local.clear(); - - all_lidar_ids.push_back(lidar_ids); - lidar_ids.clear(); - } - } - } + // Eigen::Affine3d m = poses[index_pose]; + /*if (is_slerp) + { + if (index_pose > 0) + { + m_prev = poses[index_pose - 1]; + m_next = poses[index_pose]; - if (all_points_local.size() > 0) - { - is_init = false; - index_rendered_points_local = 0; - } + q_prev = Eigen::Quaterniond(m_prev.rotation()); + q_next = Eigen::Quaterniond(m_next.rotation()); -#if 0 - std::cout << "start transforming points" << std::endl; + time_prev = timestamps[index_pose - 1].first; + time_next = timestamps[index_pose].first; + curr_time = ppp.timestamp; - int number_of_initial_points = 0; - double timestamp_begin; - for (const auto &pp : pointsPerFile) - { - // number_of_points += pp.size(); - for (const auto &p : pp) - { - number_of_initial_points++; - params.initial_points.push_back(p); - if (number_of_initial_points > threshold_initial_points) - { - timestamp_begin = p.timestamp; - break; - } - } - if (number_of_initial_points > threshold_initial_points) - { - break; - } - } + t = (curr_time - time_prev) / (time_next - time_prev); + q = q_prev.slerp(t, q_next); - std::cout << "timestamp_begin: " << timestamp_begin << std::endl; - - std::vector> timestamps; - std::vector poses; - for (const auto &t : trajectory) - { - if (t.first >= timestamp_begin) - { - timestamps.emplace_back(t.first, t.second.second); - Eigen::Affine3d m; - m.matrix() = t.second.first; - poses.push_back(m); - } - } - - std::cout << "poses.size(): " << poses.size() << std::endl; - - if (poses.empty()) - { - std::cerr << "Loading poses went wrong! Could not load poses!" << std::endl; - return; - } - - int thershold = 20; - WorkerData wd; - // std::vector temp_ts; - // temp_ts.reserve(1000000); + m.linear() = q.toRotationMatrix(); + } + }*/ - // int last_point = 0; - int index_begin = 0; + points_local.push_back(ppp); - for (size_t i = 0; i < poses.size(); i++) - { - if (i % 1000 == 0) - { - std::cout << "preparing data " << i + 1 << " of " << poses.size() << std::endl; - } - wd.intermediate_trajectory.emplace_back(poses[i]); - wd.intermediate_trajectory_motion_model.emplace_back(poses[i]); - wd.intermediate_trajectory_timestamps.emplace_back(timestamps[i]); + // ppp.point = m * ppp.point; + lidar_ids.push_back(pp.lidarid); + // points_global.push_back(ppp); + } - // - TaitBryanPose tb = pose_tait_bryan_from_affine_matrix(poses[i]); - wd.imu_roll_pitch.emplace_back(tb.om, tb.fi); + if (points_local.size() > number_of_points_threshold) + { + // all_points_local.push_back(points_global); + indexes_to_filename.push_back(i); + // points_global.clear(); + // all_lidar_ids.push_back(lidar_ids); - // temp_ts.emplace_back(timestamps[i]); + /////////////////////////////////////// + AllData data; + data.points_local = points_local; + data.lidar_ids = lidar_ids; - if (wd.intermediate_trajectory.size() >= thershold) - { - /*auto index_lower = std::lower_bound(points.begin(), points.end(), wd.intermediate_trajectory_timestamps[0], - [](Point3Di lhs, double rhs) -> bool - { return lhs.timestamp < rhs; }); - unsigned long long int i_begin = std::distance(points.begin(), index_lower); - - auto index_upper = std::lower_bound(points.begin(), points.end(), wd.intermediate_trajectory_timestamps[wd.intermediate_trajectory_timestamps.size() - 1], - [](Point3Di lhs, double rhs) -> bool - { return lhs.timestamp < rhs; }); - unsigned long long int i_end = std::distance(points.begin(), index_upper);*/ - - std::vector points; - // for (const auto &pp : pointsPerFile) - //{ - // for (const auto &p : pp) - // { - // if (p.timestamp >= wd.intermediate_trajectory_timestamps[0] && p.timestamp <= wd.intermediate_trajectory_timestamps[wd.intermediate_trajectory_timestamps.size() - 1]){ - // points.push_back(p); - // } - // } - // } - bool found = false; - - for (int index = index_begin; index < pointsPerFile.size(); index++) - { - for (const auto &p : pointsPerFile[index]) - { - if (p.timestamp >= wd.intermediate_trajectory_timestamps[0].first && p.timestamp <= wd.intermediate_trajectory_timestamps[wd.intermediate_trajectory_timestamps.size() - 1].first) - { - points.push_back(p); - } - if (p.timestamp >= wd.intermediate_trajectory_timestamps[0].first && !found) + for (int i = 0; i < timestamps.size(); i++) { - index_begin = index; - found = true; - } - if (p.timestamp > wd.intermediate_trajectory_timestamps[wd.intermediate_trajectory_timestamps.size() - 1].first) - { - break; + if (timestamps[i].first >= points_local[0].timestamp && timestamps[i].first <= points_local[points_local.size() - 1].timestamp) + { + data.timestamps.push_back(timestamps[i]); + data.poses.push_back(poses[i]); + } } + all_data.push_back(data); + + points_local.clear(); + lidar_ids.clear(); + ////////////////////////////////////// } } + } - // for (unsigned long long int k = i_begin; k < i_end; k++) - // if (i % 1000 == 0) - //{ - // std::cout << "points.size() " << points.size() << std::endl; - //} - - for (unsigned long long int k = 0; k < points.size(); k++) - { - // if (points[k].timestamp > wd.intermediate_trajectory_timestamps[0] && points[k].timestamp < wd.intermediate_trajectory_timestamps[wd.intermediate_trajectory_timestamps.size() - 1]) - //{ - auto p = points[k]; - auto lower = std::lower_bound(wd.intermediate_trajectory_timestamps.begin(), wd.intermediate_trajectory_timestamps.end(), p.timestamp, - [](std::pair lhs, double rhs) -> bool - { return lhs.first < rhs; }); - - p.index_pose = std::distance(wd.intermediate_trajectory_timestamps.begin(), lower); - wd.intermediate_points.emplace_back(p); - wd.original_points.emplace_back(p); - //} - } + std::cout << "indexing points finished" << std::endl; - //if (params.decimation > 0.0) - //{ - // wd.intermediate_points = decimate(wd.intermediate_points, params.decimation, params.decimation, params.decimation); - //} - - /*worker_data.push_back(wd); - wd.intermediate_points.clear(); - wd.original_points.clear(); - wd.intermediate_trajectory.clear(); - wd.intermediate_trajectory_motion_model.clear(); - wd.intermediate_trajectory_timestamps.clear(); - wd.imu_roll_pitch.clear(); - - wd.intermediate_points.reserve(1000000); - wd.original_points.reserve(1000000); - wd.intermediate_trajectory.reserve(1000); - wd.intermediate_trajectory_motion_model.reserve(1000); - wd.intermediate_trajectory_timestamps.reserve(1000); - wd.imu_roll_pitch.reserve(1000);*/ - - // temp_ts.clear(); + if (all_data.size() > 0) + { + is_init = false; + index_rendered_points_local = 0; } } - - //params.m_g = worker_data[0].intermediate_trajectory[0]; - //step_1_done = true; - //std::cout << "step_1_done please click 'compute_all (step 2)' to continue calculations" << std::endl; -#endif - } else { std::cout << "please select files correctly" << std::endl; @@ -616,6 +545,66 @@ void project_gui() ImGui::Text(fn.c_str()); } } + + if (ImGui::Button("save point cloud")) + { + std::shared_ptr save_file; + std::string output_file_name = ""; + ImGui::PushItemFlag(ImGuiItemFlags_Disabled, (bool)save_file); + const auto t = [&]() + { + auto sel = pfd::save_file("Save las or laz file", "C:\\", LAS_LAZ_filter).result(); + output_file_name = sel; + std::cout << "las or laz file to save: '" << output_file_name << "'" << std::endl; + }; + std::thread t1(t); + t1.join(); + + if (output_file_name.size() > 0) + { + std::vector pointcloud; + std::vector intensity; + std::vector timestamps; + + /*if (index_rendered_points_local >= 0 && index_rendered_points_local < all_points_local.size()) + { + for (int i = 0; i < all_points_local[index_rendered_points_local].size(); i++) + { + pointcloud.emplace_back(all_points_local[index_rendered_points_local][i].point.x(), all_points_local[index_rendered_points_local][i].point.y(), all_points_local[index_rendered_points_local][i].point.z()); + intensity.push_back(all_points_local[index_rendered_points_local][i].intensity); + timestamps.push_back(all_points_local[index_rendered_points_local][i].timestamp); + } + }*/ + + if (!exportLaz(output_file_name, pointcloud, intensity, timestamps, 0, 0, 0)) + { + std::cout << "problem with saving file: " << output_file_name << std::endl; + } + } + } + + if (ImGui::Button("optimize")) + { + optimize(); + } + + ImGui::InputDouble("rgd_x", &rgd_x); + ImGui::InputDouble("rgd_y", &rgd_y); + ImGui::InputDouble("rgd_z", &rgd_z); + ImGui::Checkbox("show show_rgd_nn", &show_rgd_nn); + + if (ImGui::Button("get_nn")) + { + rgd_nn = get_nn(); + } + + ImGui::Checkbox("show_mean_cov", &show_mean_cov); + + if (ImGui::Button("get_mean_cov")) + { + mean_cov = get_mean_cov(); + } + ImGui::End(); } return; @@ -734,28 +723,78 @@ void display() } // + /* glPointSize(point_size); + glBegin(GL_POINTS); + if (index_rendered_points_local >= 0 && index_rendered_points_local < all_points_local.size()) + { + for (int i = 0; i < all_points_local[index_rendered_points_local].size(); i++) + { + if (all_lidar_ids[index_rendered_points_local][i] == 0) + { + glColor3f(pc_color.x, pc_color.y, pc_color.z); + } + else + { + glColor3f(pc_color2.x, pc_color2.y, pc_color2.z); + } + glVertex3f(all_points_local[index_rendered_points_local][i].point.x(), all_points_local[index_rendered_points_local][i].point.y(), all_points_local[index_rendered_points_local][i].point.z()); + } + } + glEnd(); + glPointSize(1);*/ + glPointSize(point_size); glBegin(GL_POINTS); - if (index_rendered_points_local >= 0 && index_rendered_points_local < all_points_local.size()) + if (index_rendered_points_local >= 0 && index_rendered_points_local < all_data.size()) { - //for (const auto &p : all_points_local[index_rendered_points_local]) - //{ - // - // glVertex3f(p.x(), p.y(), p.z()); - //} - - for (int i = 0; i < all_points_local[index_rendered_points_local].size(); i++){ - if (all_lidar_ids[index_rendered_points_local][i] == 0){ - glColor3f(pc_color.x, pc_color.y, pc_color.z); - }else{ - glColor3f(pc_color2.x, pc_color2.y, pc_color2.z); + for (int i = 0; i < all_data[index_rendered_points_local].points_local.size(); i++) + { + auto lower = std::lower_bound(all_data[index_rendered_points_local].timestamps.begin(), all_data[index_rendered_points_local].timestamps.end(), all_data[index_rendered_points_local].points_local[i].timestamp, + [](std::pair lhs, double rhs) -> bool + { return lhs.first < rhs; }); + + int index_pose = std::distance(all_data[index_rendered_points_local].timestamps.begin(), lower); + + if (index_pose >= 0 && index_pose < all_data[index_rendered_points_local].poses.size()) + { + Eigen::Affine3d m = all_data[index_rendered_points_local].poses[index_pose]; + Eigen::Vector3d p = m * all_data[index_rendered_points_local].points_local[i].point; + + if (all_data[index_rendered_points_local].lidar_ids[i] == 0) + { + glColor3f(pc_color.x, pc_color.y, pc_color.z); + } + else + { + glColor3f(pc_color2.x, pc_color2.y, pc_color2.z); + } + glVertex3f(p.x(), p.y(), p.z()); } - glVertex3f(all_points_local[index_rendered_points_local][i].x(), all_points_local[index_rendered_points_local][i].y(), all_points_local[index_rendered_points_local][i].z()); } } glEnd(); glPointSize(1); + if (show_rgd_nn) + { + glColor3f(0, 0, 0); + glBegin(GL_LINES); + for (const auto &nn : rgd_nn) + { + glVertex3f(nn.first.x(), nn.first.y(), nn.first.z()); + glVertex3f(nn.second.x(), nn.second.y(), nn.second.z()); + } + glEnd(); + } + + if (show_mean_cov) + { + for (const auto &mc : mean_cov) + { + draw_ellipse(mc.second, mc.first, Eigen::Vector3f(1, 0, 0), 1); + } + } + ImGui_ImplOpenGL2_NewFrame(); ImGui_ImplGLUT_NewFrame(); @@ -892,4 +931,742 @@ int main(int argc, char *argv[]) ImGui::DestroyContext(); return 0; +} + +void optimize() +{ + if (index_rendered_points_local >= 0 && index_rendered_points_local < all_data.size()) + { + auto worker_data = all_data[index_rendered_points_local]; + + auto tr = worker_data.intermediate_trajectory; + auto trmm = worker_data.intermediate_trajectory_motion_model; + } + +#if 0 + if (params.use_robust_and_accurate_lidar_odometry) + { + + + auto firstm = tr[0]; + + for (auto &t : tr) + { + t.translation() -= firstm.translation(); + } + for (auto &t : trmm) + { + t.translation() -= firstm.translation(); + } + + NDT::GridParameters rgd_params_sc; + + rgd_params_sc.resolution_X = params.distance_bucket; + rgd_params_sc.resolution_Y = params.polar_angle_deg; + rgd_params_sc.resolution_Z = params.azimutal_angle_deg; + + // for (int iter = 0; iter < 10; iter++) + //{ + // optimize_sf(worker_data[i].intermediate_points, tr, trmm, + // rgd_params_sc, params.buckets, /*params.useMultithread*/ false); + // } + std::vector points_local_sf; + std::vector points_local; + + /// + for (int ii = 0; ii < worker_data[i].intermediate_points.size(); ii++) + { + double r_l = worker_data[i].intermediate_points[ii].point.norm(); + if (r_l > 0.5 && worker_data[i].intermediate_points[ii].index_pose != -1 && r_l < params.max_distance_lidar) + { + double polar_angle_deg_l = atan2(worker_data[i].intermediate_points[ii].point.y(), worker_data[i].intermediate_points[ii].point.x()) / M_PI * 180.0; + double azimutal_angle_deg_l = acos(worker_data[i].intermediate_points[ii].point.z() / r_l) / M_PI * 180.0; + + points_local.push_back(worker_data[i].intermediate_points[ii]); + + /////////////////////////////////////////////////////// + Point3Di p_sl = worker_data[i].intermediate_points[ii]; + p_sl.point.x() = r_l; + p_sl.point.y() = polar_angle_deg_l; + p_sl.point.z() = azimutal_angle_deg_l; + + points_local_sf.push_back(p_sl); + } + } + /// + std::cout << "optimize_sf2" << std::endl; + for (int iter = 0; iter < params.robust_and_accurate_lidar_odometry_iterations; iter++) + { + optimize_sf2(points_local, points_local_sf, tr, trmm, rgd_params_sc, params.useMultithread); + } + + for (auto &t : tr) + { + t.translation() += firstm.translation(); + } + + for (auto &t : trmm) + { + t.translation() += firstm.translation(); + } + + worker_data[i].intermediate_trajectory = tr; + worker_data[i].intermediate_trajectory_motion_model = tr; + } +#endif +#if 0 + bool multithread = false; + std::cout << "optimize" << std::endl; + if (index_rendered_points_local >= 0 && index_rendered_points_local < all_data.size()) + { + // for (int i = 0; i < all_data[index_rendered_points_local].poses.size(); i++){ + // all_data[index_rendered_points_local].poses[i](0, 3) += 1.0; + // } + + + + // index data + for (int i = 0; i < worker_data.points_local.size(); i++) + { + auto lower = std::lower_bound(worker_data.timestamps.begin(), worker_data.timestamps.end(), worker_data.points_local[i].timestamp, + [](std::pair lhs, double rhs) -> bool + { return lhs.first < rhs; }); + + int index_pose = std::distance(worker_data.timestamps.begin(), lower); + + if (index_pose >= 0 && index_pose < worker_data.poses.size()) + { + worker_data.points_local[i].index_pose = index_pose; + } + else + { + worker_data.points_local[i].index_pose = -1; + } + } + + NDT::GridParameters rgd_params; + // rgd_params.resolution_X = 0.3; // distance bucket + // rgd_params.resolution_Y = 0.3; // polar angle deg + // rgd_params.resolution_Z = 0.3; // azimutal angle deg + + rgd_params.resolution_X = rgd_x; // distance bucket + rgd_params.resolution_Y = rgd_y; // polar angle deg + rgd_params.resolution_Z = rgd_z; // azimutal angle deg + + std::vector point_cloud_global; + std::vector points_local; + + std::vector point_cloud_global_sc; + std::vector points_local_sc; + + for (int i = 0; i < worker_data.points_local.size(); i++) + { + double r_l = worker_data.points_local[i].point.norm(); + if (r_l > 0.5 && worker_data.points_local[i].index_pose != -1 && r_l < 10) + { + double polar_angle_deg_l = atan2(worker_data.points_local[i].point.y(), worker_data.points_local[i].point.x()) / M_PI * 180.0; + double azimutal_angle_deg_l = acos(worker_data.points_local[i].point.z() / r_l) / M_PI * 180.0; + + Eigen::Vector3d pp = worker_data.points_local[i].point; + // pps.x() = r; + // pps.y() = polar_angle_deg; + // pps.z() = azimutal_angle_deg; + // point_cloud_spherical_coordinates.push_back(pps); + + Eigen::Affine3d pose = worker_data.poses[worker_data.points_local[i].index_pose]; + + pp = pose * pp; + + Point3Di pg = worker_data.points_local[i]; + pg.point = pp; + + point_cloud_global.push_back(pg); + points_local.push_back(worker_data.points_local[i]); + + /////////////////////////////////////////////////////// + Point3Di p_sl = worker_data.points_local[i]; + p_sl.point.x() = r_l; + p_sl.point.y() = polar_angle_deg_l; + p_sl.point.z() = azimutal_angle_deg_l; + + points_local_sc.push_back(p_sl); + // + double r_g = pg.point.norm(); + double polar_angle_deg_g = atan2(pg.point.y(), pg.point.x()) / M_PI * 180.0; + double azimutal_angle_deg_g = acos(pg.point.z() / r_l) / M_PI * 180.0; + + Eigen::Vector3d p_sg = worker_data.points_local[i].point; + p_sg.x() = r_g; + p_sg.y() = polar_angle_deg_g; + p_sg.z() = azimutal_angle_deg_g; + + point_cloud_global_sc.push_back(p_sg); + } + } + + NDTBucketMapType buckets; + update_rgd_spherical_coordinates(rgd_params, buckets, point_cloud_global, point_cloud_global_sc, {0, 0, 0}); + // update_rgd(rgd_params, buckets, point_cloud_global, {0, 0, 0}); + std::cout << "buckets.size(): " << buckets.size() << std::endl; + + std::vector> tripletListA; + std::vector> tripletListP; + std::vector> tripletListB; + + Eigen::Vector3d b(rgd_params.resolution_X, rgd_params.resolution_Y, rgd_params.resolution_Z); + + const auto hessian_fun = [&](const Point3Di &intermediate_points_i) + { + int ir = tripletListB.size(); + double delta_x; + double delta_y; + double delta_z; + + Eigen::Affine3d m_pose = worker_data.poses[intermediate_points_i.index_pose]; + Eigen::Vector3d point_local(intermediate_points_i.point.x(), intermediate_points_i.point.y(), intermediate_points_i.point.z()); + Eigen::Vector3d point_global = m_pose * point_local; + + /////////////// + double r = point_global.norm(); + double polar_angle_deg = atan2(point_global.y(), point_global.x()) / M_PI * 180.0; + double azimutal_angle_deg = acos(point_global.z() / r) / M_PI * 180.0; + /////////////// + + auto index_of_bucket = get_rgd_index({r, polar_angle_deg, azimutal_angle_deg}, b); + // auto index_of_bucket = get_rgd_index(point_global, b); + + auto bucket_it = buckets.find(index_of_bucket); + // no bucket found + if (bucket_it == buckets.end()) + { + return; + } + auto &this_bucket = bucket_it->second; + + Eigen::Vector3d mean(this_bucket.mean.x(), this_bucket.mean.y(), this_bucket.mean.z()); + + Eigen::Matrix jacobian; + TaitBryanPose pose_s = pose_tait_bryan_from_affine_matrix(m_pose); + + point_to_point_source_to_target_tait_bryan_wc(delta_x, delta_y, delta_z, + pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, + point_local.x(), point_local.y(), point_local.z(), mean.x(), mean.y(), mean.z()); + + point_to_point_source_to_target_tait_bryan_wc_jacobian(jacobian, + pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, + point_local.x(), point_local.y(), point_local.z()); + + int c = intermediate_points_i.index_pose * 6; + for (int row = 0; row < 3; row++) + { + for (int col = 0; col < 6; col++) + { + if (jacobian(row, col) != 0.0) + { + tripletListA.emplace_back(ir + row, c + col, -jacobian(row, col)); + } + } + } + + Eigen::Matrix3d infm = this_bucket.cov.inverse(); + + tripletListB.emplace_back(ir, 0, delta_x); + tripletListB.emplace_back(ir + 1, 0, delta_y); + tripletListB.emplace_back(ir + 2, 0, delta_z); + + tripletListP.emplace_back(ir, ir, infm(0, 0)); + tripletListP.emplace_back(ir, ir + 1, infm(0, 1)); + tripletListP.emplace_back(ir, ir + 2, infm(0, 2)); + tripletListP.emplace_back(ir + 1, ir, infm(1, 0)); + tripletListP.emplace_back(ir + 1, ir + 1, infm(1, 1)); + tripletListP.emplace_back(ir + 1, ir + 2, infm(1, 2)); + tripletListP.emplace_back(ir + 2, ir, infm(2, 0)); + tripletListP.emplace_back(ir + 2, ir + 1, infm(2, 1)); + tripletListP.emplace_back(ir + 2, ir + 2, infm(2, 2)); + }; + + if (points_local.size() > 100) + { + std::cout << "start adding lidar observations" << std::endl; + if (multithread) + { + std::for_each(std::execution::par_unseq, std::begin(points_local), std::end(points_local), hessian_fun); + } + else + { + std::for_each(std::begin(points_local), std::end(points_local), hessian_fun); + } + std::cout << "adding lidar observations finished" << std::endl; + } + + std::vector> odo_edges; + for (size_t i = 1; i < worker_data.poses.size(); i++) + { + odo_edges.emplace_back(i - 1, i); + } + + std::vector poses; + std::vector poses_desired; + + for (size_t i = 0; i < worker_data.poses.size(); i++) + { + poses.push_back(pose_tait_bryan_from_affine_matrix(worker_data.poses[i])); + } + for (size_t i = 0; i < worker_data.poses.size(); i++) + { + poses_desired.push_back(pose_tait_bryan_from_affine_matrix(worker_data.poses[i])); + } + + for (size_t i = 0; i < odo_edges.size(); i++) + { + /*Eigen::Matrix relative_pose_measurement_odo; + relative_pose_tait_bryan_wc_case1(relative_pose_measurement_odo, + poses_desired[odo_edges[i].first].px, + poses_desired[odo_edges[i].first].py, + poses_desired[odo_edges[i].first].pz, + poses_desired[odo_edges[i].first].om, + poses_desired[odo_edges[i].first].fi, + poses_desired[odo_edges[i].first].ka, + poses_desired[odo_edges[i].second].px, + poses_desired[odo_edges[i].second].py, + poses_desired[odo_edges[i].second].pz, + poses_desired[odo_edges[i].second].om, + poses_desired[odo_edges[i].second].fi, + poses_desired[odo_edges[i].second].ka);*/ + + Eigen::Matrix delta; + relative_pose_obs_eq_tait_bryan_wc_case1( + delta, + poses[odo_edges[i].first].px, + poses[odo_edges[i].first].py, + poses[odo_edges[i].first].pz, + poses[odo_edges[i].first].om, + poses[odo_edges[i].first].fi, + poses[odo_edges[i].first].ka, + poses[odo_edges[i].second].px, + poses[odo_edges[i].second].py, + poses[odo_edges[i].second].pz, + poses[odo_edges[i].second].om, + poses[odo_edges[i].second].fi, + poses[odo_edges[i].second].ka, + 0, 0, 0, 0, 0, 0); + // relative_pose_measurement_odo(0, 0), + // relative_pose_measurement_odo(1, 0), + // relative_pose_measurement_odo(2, 0), + // relative_pose_measurement_odo(3, 0), + // relative_pose_measurement_odo(4, 0), + // relative_pose_measurement_odo(5, 0)); + + Eigen::Matrix jacobian; + relative_pose_obs_eq_tait_bryan_wc_case1_jacobian(jacobian, + poses[odo_edges[i].first].px, + poses[odo_edges[i].first].py, + poses[odo_edges[i].first].pz, + poses[odo_edges[i].first].om, + poses[odo_edges[i].first].fi, + poses[odo_edges[i].first].ka, + poses[odo_edges[i].second].px, + poses[odo_edges[i].second].py, + poses[odo_edges[i].second].pz, + poses[odo_edges[i].second].om, + poses[odo_edges[i].second].fi, + poses[odo_edges[i].second].ka); + + int ir = tripletListB.size(); + + int ic_1 = odo_edges[i].first * 6; + int ic_2 = odo_edges[i].second * 6; + + for (size_t row = 0; row < 6; row++) + { + tripletListA.emplace_back(ir + row, ic_1, -jacobian(row, 0)); + tripletListA.emplace_back(ir + row, ic_1 + 1, -jacobian(row, 1)); + tripletListA.emplace_back(ir + row, ic_1 + 2, -jacobian(row, 2)); + tripletListA.emplace_back(ir + row, ic_1 + 3, -jacobian(row, 3)); + tripletListA.emplace_back(ir + row, ic_1 + 4, -jacobian(row, 4)); + tripletListA.emplace_back(ir + row, ic_1 + 5, -jacobian(row, 5)); + + tripletListA.emplace_back(ir + row, ic_2, -jacobian(row, 6)); + tripletListA.emplace_back(ir + row, ic_2 + 1, -jacobian(row, 7)); + tripletListA.emplace_back(ir + row, ic_2 + 2, -jacobian(row, 8)); + tripletListA.emplace_back(ir + row, ic_2 + 3, -jacobian(row, 9)); + tripletListA.emplace_back(ir + row, ic_2 + 4, -jacobian(row, 10)); + tripletListA.emplace_back(ir + row, ic_2 + 5, -jacobian(row, 11)); + } + + tripletListB.emplace_back(ir, 0, delta(0, 0)); + tripletListB.emplace_back(ir + 1, 0, delta(1, 0)); + tripletListB.emplace_back(ir + 2, 0, delta(2, 0)); + tripletListB.emplace_back(ir + 3, 0, delta(3, 0)); + tripletListB.emplace_back(ir + 4, 0, delta(4, 0)); + tripletListB.emplace_back(ir + 5, 0, delta(5, 0)); + + tripletListP.emplace_back(ir, ir, 1000000); + tripletListP.emplace_back(ir + 1, ir + 1, 1000000); + tripletListP.emplace_back(ir + 2, ir + 2, 1000000); + tripletListP.emplace_back(ir + 3, ir + 3, 1000000); + tripletListP.emplace_back(ir + 4, ir + 4, 1000000); + tripletListP.emplace_back(ir + 5, ir + 5, 1000000); + } + + int ic = 0; + int ir = tripletListB.size(); + tripletListA.emplace_back(ir, ic * 6 + 0, 1); + tripletListA.emplace_back(ir + 1, ic * 6 + 1, 1); + tripletListA.emplace_back(ir + 2, ic * 6 + 2, 1); + tripletListA.emplace_back(ir + 3, ic * 6 + 3, 1); + tripletListA.emplace_back(ir + 4, ic * 6 + 4, 1); + tripletListA.emplace_back(ir + 5, ic * 6 + 5, 1); + + tripletListP.emplace_back(ir, ir, 1); + tripletListP.emplace_back(ir + 1, ir + 1, 1); + tripletListP.emplace_back(ir + 2, ir + 2, 1); + tripletListP.emplace_back(ir + 3, ir + 3, 1); + tripletListP.emplace_back(ir + 4, ir + 4, 1); + tripletListP.emplace_back(ir + 5, ir + 5, 1); + + tripletListB.emplace_back(ir, 0, 0); + tripletListB.emplace_back(ir + 1, 0, 0); + tripletListB.emplace_back(ir + 2, 0, 0); + tripletListB.emplace_back(ir + 3, 0, 0); + tripletListB.emplace_back(ir + 4, 0, 0); + tripletListB.emplace_back(ir + 5, 0, 0); + + Eigen::SparseMatrix matA(tripletListB.size(), worker_data.poses.size() * 6); + Eigen::SparseMatrix matP(tripletListB.size(), tripletListB.size()); + Eigen::SparseMatrix matB(tripletListB.size(), 1); + + matA.setFromTriplets(tripletListA.begin(), tripletListA.end()); + matP.setFromTriplets(tripletListP.begin(), tripletListP.end()); + matB.setFromTriplets(tripletListB.begin(), tripletListB.end()); + + Eigen::SparseMatrix AtPA(worker_data.poses.size() * 6, worker_data.poses.size() * 6); + Eigen::SparseMatrix AtPB(worker_data.poses.size() * 6, 1); + + { + Eigen::SparseMatrix AtP = matA.transpose() * matP; + AtPA = (AtP)*matA; + AtPB = (AtP)*matB; + } + + tripletListA.clear(); + tripletListP.clear(); + tripletListB.clear(); + + // AtPA += AtPAndt.sparseView(); + // AtPB += AtPBndt.sparseView(); + + // Eigen::SparseMatrix AtPA_I(intrinsics.size() * 6, intrinsics.size() * 6); + // AtPA_I.setIdentity(); + // AtPA_I *= 1; + // AtPA += AtPA_I; + + Eigen::SimplicialCholesky> + solver(AtPA); + std::cout << "start solving" << std::endl; + Eigen::SparseMatrix x = solver.solve(AtPB); + std::cout << "start finished" << std::endl; + std::vector h_x; + for (int k = 0; k < x.outerSize(); ++k) + { + for (Eigen::SparseMatrix::InnerIterator it(x, k); it; ++it) + { + // std::cout << it.value() << " "; + h_x.push_back(it.value()); + } + // std::cout << std::endl; + } + + if (h_x.size() == 6 * worker_data.poses.size()) + { + int counter = 0; + + for (size_t i = 0; i < worker_data.poses.size(); i++) + { + TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(worker_data.poses[i]); + auto prev_pose = pose; + pose.px += h_x[counter++]; + pose.py += h_x[counter++]; + pose.pz += h_x[counter++]; + pose.om += h_x[counter++]; + pose.fi += h_x[counter++]; + pose.ka += h_x[counter++]; + + worker_data.poses[i] = affine_matrix_from_pose_tait_bryan(pose); + all_data[index_rendered_points_local].poses[i] = worker_data.poses[i]; + } + } + else + { + std::cout << "optimization failed" << std::endl; + } + } + +#endif + return; +} + +std::vector> get_nn() +{ + std::vector> nn; + if (index_rendered_points_local >= 0 && index_rendered_points_local < all_data.size()) + { + // for (int i = 0; i < all_data[index_rendered_points_local].poses.size(); i++){ + // all_data[index_rendered_points_local].poses[i](0, 3) += 1.0; + // } + + auto worker_data = all_data[index_rendered_points_local]; + + // index data + for (int i = 0; i < worker_data.points_local.size(); i++) + { + auto lower = std::lower_bound(worker_data.timestamps.begin(), worker_data.timestamps.end(), worker_data.points_local[i].timestamp, + [](std::pair lhs, double rhs) -> bool + { return lhs.first < rhs; }); + + int index_pose = std::distance(worker_data.timestamps.begin(), lower); + + if (index_pose >= 0 && index_pose < worker_data.poses.size()) + { + worker_data.points_local[i].index_pose = index_pose; + } + else + { + worker_data.points_local[i].index_pose = -1; + } + } + + NDT::GridParameters rgd_params; + // rgd_params.resolution_X = 0.3; // distance bucket + // rgd_params.resolution_Y = 0.3; // polar angle deg + // rgd_params.resolution_Z = 0.3; // azimutal angle deg + + rgd_params.resolution_X = rgd_x; // distance bucket + rgd_params.resolution_Y = rgd_y; // polar angle deg + rgd_params.resolution_Z = rgd_z; // azimutal angle deg + + std::vector point_cloud_global; + std::vector points_local; + + std::vector point_cloud_global_sc; + std::vector points_local_sc; + + for (int i = 0; i < worker_data.points_local.size(); i++) + { + double r_l = worker_data.points_local[i].point.norm(); + if (r_l > 0.5 && worker_data.points_local[i].index_pose != -1) + { + double polar_angle_deg_l = atan2(worker_data.points_local[i].point.y(), worker_data.points_local[i].point.x()) / M_PI * 180.0; + double azimutal_angle_deg_l = acos(worker_data.points_local[i].point.z() / r_l) / M_PI * 180.0; + + Eigen::Vector3d pp = worker_data.points_local[i].point; + // pps.x() = r; + // pps.y() = polar_angle_deg; + // pps.z() = azimutal_angle_deg; + // point_cloud_spherical_coordinates.push_back(pps); + + Eigen::Affine3d pose = worker_data.poses[worker_data.points_local[i].index_pose]; + + pp = pose * pp; + + Point3Di pg = worker_data.points_local[i]; + pg.point = pp; + + point_cloud_global.push_back(pg); + points_local.push_back(worker_data.points_local[i]); + + /////////////////////////////////////////////////////// + Point3Di p_sl = worker_data.points_local[i]; + p_sl.point.x() = r_l; + p_sl.point.y() = polar_angle_deg_l; + p_sl.point.z() = azimutal_angle_deg_l; + + points_local_sc.push_back(p_sl); + // + double r_g = pg.point.norm(); + double polar_angle_deg_g = atan2(pg.point.y(), pg.point.x()) / M_PI * 180.0; + double azimutal_angle_deg_g = acos(pg.point.z() / r_l) / M_PI * 180.0; + + Eigen::Vector3d p_sg = worker_data.points_local[i].point; + p_sg.x() = r_g; + p_sg.y() = polar_angle_deg_g; + p_sg.z() = azimutal_angle_deg_g; + + point_cloud_global_sc.push_back(p_sg); + } + } + + NDTBucketMapType buckets; + update_rgd_spherical_coordinates(rgd_params, buckets, point_cloud_global, point_cloud_global_sc, {0, 0, 0}); + + ///////////// + // std::vector> nn; + Eigen::Vector3d b(rgd_params.resolution_X, rgd_params.resolution_Y, rgd_params.resolution_Z); + + for (int i = 0; i < point_cloud_global_sc.size(); i++) + { + auto index_of_bucket = get_rgd_index(point_cloud_global_sc[i], b); + + auto bucket_it = buckets.find(index_of_bucket); + + if (bucket_it != buckets.end()) + { + auto &this_bucket = bucket_it->second; + this_bucket.number_of_points++; + // const auto &curr_mean = points_global[i].point; + const auto &mean = this_bucket.mean; + + nn.emplace_back(point_cloud_global[i].point, mean); + + ////////////// + } + } + } + return nn; +} + +void draw_ellipse(const Eigen::Matrix3d &covar, Eigen::Vector3d &mean, Eigen::Vector3f color, float nstd = 3) +{ + + Eigen::LLT> cholSolver(covar); + Eigen::Matrix3d transform = cholSolver.matrixL(); + + const double pi = 3.141592; + const double di = 0.02; + const double dj = 0.04; + const double du = di * 2 * pi; + const double dv = dj * pi; + glColor3f(color.x(), color.y(), color.z()); + + for (double i = 0; i < 1.0; i += di) // horizonal + { + for (double j = 0; j < 1.0; j += dj) // vertical + { + double u = i * 2 * pi; // 0 to 2pi + double v = (j - 0.5) * pi; //-pi/2 to pi/2 + + const Eigen::Vector3d pp0(cos(v) * cos(u), cos(v) * sin(u), sin(v)); + const Eigen::Vector3d pp1(cos(v) * cos(u + du), cos(v) * sin(u + du), sin(v)); + const Eigen::Vector3d pp2(cos(v + dv) * cos(u + du), cos(v + dv) * sin(u + du), sin(v + dv)); + const Eigen::Vector3d pp3(cos(v + dv) * cos(u), cos(v + dv) * sin(u), sin(v + dv)); + Eigen::Vector3d tp0 = transform * (nstd * pp0) + mean; + Eigen::Vector3d tp1 = transform * (nstd * pp1) + mean; + Eigen::Vector3d tp2 = transform * (nstd * pp2) + mean; + Eigen::Vector3d tp3 = transform * (nstd * pp3) + mean; + + glBegin(GL_LINE_LOOP); + glVertex3dv(tp0.data()); + glVertex3dv(tp1.data()); + glVertex3dv(tp2.data()); + glVertex3dv(tp3.data()); + glEnd(); + } + } +} + +std::vector> get_mean_cov() +{ + std::vector> mc; + if (index_rendered_points_local >= 0 && index_rendered_points_local < all_data.size()) + { + // for (int i = 0; i < all_data[index_rendered_points_local].poses.size(); i++){ + // all_data[index_rendered_points_local].poses[i](0, 3) += 1.0; + // } + + auto worker_data = all_data[index_rendered_points_local]; + + // index data + for (int i = 0; i < worker_data.points_local.size(); i++) + { + auto lower = std::lower_bound(worker_data.timestamps.begin(), worker_data.timestamps.end(), worker_data.points_local[i].timestamp, + [](std::pair lhs, double rhs) -> bool + { return lhs.first < rhs; }); + + int index_pose = std::distance(worker_data.timestamps.begin(), lower); + + if (index_pose >= 0 && index_pose < worker_data.poses.size()) + { + worker_data.points_local[i].index_pose = index_pose; + } + else + { + worker_data.points_local[i].index_pose = -1; + } + } + + NDT::GridParameters rgd_params; + // rgd_params.resolution_X = 0.3; // distance bucket + // rgd_params.resolution_Y = 0.3; // polar angle deg + // rgd_params.resolution_Z = 0.3; // azimutal angle deg + + rgd_params.resolution_X = rgd_x; // distance bucket + rgd_params.resolution_Y = rgd_y; // polar angle deg + rgd_params.resolution_Z = rgd_z; // azimutal angle deg + + std::vector point_cloud_global; + std::vector points_local; + + std::vector point_cloud_global_sc; + std::vector points_local_sc; + + for (int i = 0; i < worker_data.points_local.size(); i++) + { + double r_l = worker_data.points_local[i].point.norm(); + if (r_l > 0.5 && worker_data.points_local[i].index_pose != -1) + { + double polar_angle_deg_l = atan2(worker_data.points_local[i].point.y(), worker_data.points_local[i].point.x()) / M_PI * 180.0; + double azimutal_angle_deg_l = acos(worker_data.points_local[i].point.z() / r_l) / M_PI * 180.0; + + Eigen::Vector3d pp = worker_data.points_local[i].point; + // pps.x() = r; + // pps.y() = polar_angle_deg; + // pps.z() = azimutal_angle_deg; + // point_cloud_spherical_coordinates.push_back(pps); + + Eigen::Affine3d pose = worker_data.poses[worker_data.points_local[i].index_pose]; + + pp = pose * pp; + + Point3Di pg = worker_data.points_local[i]; + pg.point = pp; + + point_cloud_global.push_back(pg); + points_local.push_back(worker_data.points_local[i]); + + /////////////////////////////////////////////////////// + Point3Di p_sl = worker_data.points_local[i]; + p_sl.point.x() = r_l; + p_sl.point.y() = polar_angle_deg_l; + p_sl.point.z() = azimutal_angle_deg_l; + + points_local_sc.push_back(p_sl); + // + double r_g = pg.point.norm(); + double polar_angle_deg_g = atan2(pg.point.y(), pg.point.x()) / M_PI * 180.0; + double azimutal_angle_deg_g = acos(pg.point.z() / r_l) / M_PI * 180.0; + + Eigen::Vector3d p_sg = worker_data.points_local[i].point; + p_sg.x() = r_g; + p_sg.y() = polar_angle_deg_g; + p_sg.z() = azimutal_angle_deg_g; + + point_cloud_global_sc.push_back(p_sg); + } + } + + NDTBucketMapType buckets; + update_rgd_spherical_coordinates(rgd_params, buckets, point_cloud_global, point_cloud_global_sc, {0, 0, 0}); + + ///////////// + Eigen::Vector3d b(rgd_params.resolution_X, rgd_params.resolution_Y, rgd_params.resolution_Z); + + for (const auto &b : buckets) + { + auto &this_bucket = b.second; + + mc.emplace_back(this_bucket.mean, this_bucket.cov); + } + } + + return mc; } \ No newline at end of file