From 9731d397df490bb6e688b0a4c8bcffe197fbb9d2 Mon Sep 17 00:00:00 2001 From: mwlasiuk Date: Sat, 28 Feb 2026 15:01:17 +0100 Subject: [PATCH 1/4] Remove commented out cout and unused code blocks --- .../mandeye_compare_trajectories.cpp | 15 --- ...ply_timestamps_session_point_cloud_laz.cpp | 2 - ..._data_and_drop_here-precision_forestry.cpp | 38 ------ apps/lidar_odometry_step_1/lidar_odometry.cpp | 50 -------- .../lidar_odometry_gui.cpp | 8 -- .../lidar_odometry_utils.cpp | 40 +----- .../lidar_odometry_utils_optimizers.cpp | 4 - .../livox_mid_360_intrinsic_calibration.cpp | 3 - .../mandeye_raw_data_viewer.cpp | 6 - apps/manual_color/manual_color.cpp | 30 +---- .../multi_session_factor_graph.cpp | 88 +------------ .../multi_session_registration.cpp | 10 +- .../precision_forestry_tools.cpp | 13 -- apps/quick_start_demo/quick_start_demo.cpp | 16 --- core/include/export_laz.h | 14 --- core/src/color_las_loader.cpp | 32 ----- core/src/gnss.cpp | 6 - core/src/manual_pose_graph_loop_closure.cpp | 19 --- core/src/ndt.cpp | 116 +----------------- core/src/nmea.cpp | 1 - ...zation_point_to_point_source_to_target.cpp | 58 +-------- ...stance_point_to_plane_source_to_target.cpp | 2 - ...timize_plane_to_plane_source_to_target.cpp | 2 - ...timize_point_to_plane_source_to_target.cpp | 2 - ...projection_onto_plane_source_to_target.cpp | 6 - .../src/pair_wise_iterative_closest_point.cpp | 10 -- core/src/pfd_wrapper.cpp | 1 - core/src/point_cloud.cpp | 41 ------- core/src/point_clouds.cpp | 54 +------- core/src/pose_graph_loop_closure.cpp | 4 - core/src/pose_graph_slam.cpp | 11 -- core/src/session.cpp | 20 --- core/src/surface.cpp | 55 --------- core/src/utils.cpp | 7 -- core_hd_mapping/src/odo_with_gnss_fusion.cpp | 9 -- core_hd_mapping/src/project_settings.cpp | 15 --- .../src/single_trajectory_viewer.cpp | 1 - 37 files changed, 18 insertions(+), 791 deletions(-) diff --git a/apps/compare_trajectories/mandeye_compare_trajectories.cpp b/apps/compare_trajectories/mandeye_compare_trajectories.cpp index 81c2a4b6..83d43314 100644 --- a/apps/compare_trajectories/mandeye_compare_trajectories.cpp +++ b/apps/compare_trajectories/mandeye_compare_trajectories.cpp @@ -496,32 +496,17 @@ void project_gui() } if (ImGui::Button("(Step 3) Calculate ATE (Absolute Trajectory Error)")) { - // std::cout << std::setprecision(20); - // for (const auto &p : Data::trajectory_est) - //{ - // std::cout << p.first << std::endl; - // } - // std::cout << " ---------------------------" << std::endl; - // for (const auto &p : Data::trajectory_gt) - //{ - // std::cout << p.first << std::endl; - // } - double ATE = 0.0; double max_ATE = -1000000.0; double min_ATE = 1000000.0; int count = 0; for (const auto& p : Data::trajectory_est) { - // std::cout << p.second.matrix() << std::endl; - auto it = getInterpolatedPose(Data::trajectory_gt, p.first); if (!it.isZero()) { auto tp = Data::trajectory_offset * p.second; - // std::cout << tp.matrix() << " " << std::endl; - // std::cout << "ATE " << ATE << std::endl; double ate = (tp - it).norm(); if (ate > max_ATE) diff --git a/apps/console_tools/multiply_timestamps_session_point_cloud_laz.cpp b/apps/console_tools/multiply_timestamps_session_point_cloud_laz.cpp index 3880cc48..ce31ff6d 100644 --- a/apps/console_tools/multiply_timestamps_session_point_cloud_laz.cpp +++ b/apps/console_tools/multiply_timestamps_session_point_cloud_laz.cpp @@ -148,8 +148,6 @@ bool load_pc(PointCloud& pc, const std::string& input_file_name) static_cast(point->rgb[1]) / 256.0, static_cast(point->rgb[2]) / 256.0); - // std::cout << point->rgb[0] << " " << point->rgb[1] << " " << point->rgb[2] << std::endl; - pc.colors.push_back(color); p_count++; diff --git a/apps/lidar_odometry_step_1/drag_folder_with_mandeye_data_and_drop_here-precision_forestry.cpp b/apps/lidar_odometry_step_1/drag_folder_with_mandeye_data_and_drop_here-precision_forestry.cpp index 3a127c63..cde46174 100644 --- a/apps/lidar_odometry_step_1/drag_folder_with_mandeye_data_and_drop_here-precision_forestry.cpp +++ b/apps/lidar_odometry_step_1/drag_folder_with_mandeye_data_and_drop_here-precision_forestry.cpp @@ -294,7 +294,6 @@ void find_best_stretch( { auto lower = std::lower_bound(timestamps.begin(), timestamps.end(), points[i].timestamp); points[i].index_pose = std::distance(timestamps.begin(), lower); - // std::cout << "points[i].timestamp " << points[i].timestamp << " timestamps " << timestamps[points[i].index_pose] << std::endl; } std::set indexes; @@ -1530,8 +1529,6 @@ void display() glBegin(GL_POINTS); for (const auto &p : worker_data[i].intermediate_points) { - // std::cout << "kk"; - // std::cout << p.index_pose; Eigen::Vector3d pt = worker_data[i].intermediate_trajectory[p.index_pose] * p.point; glVertex3d(pt.x(), pt.y(), pt.z()); } @@ -2110,11 +2107,6 @@ void mouse(int glut_button, int state, int x, int y) int main(int argc, char* argv[]) { - // std::cout << "argc " << argc << std::endl; - // if (argc == 2){ - // std::cout << "argv[1]: '" << argv[1] << "'" << std::endl; - // } - spdlog::cfg::load_env_levels(); spdlog::flush_on(spdlog::level::warn); set_lidar_odometry_default_params(params); @@ -2135,40 +2127,10 @@ int main(int argc, char* argv[]) return 0; } - // if (argc == 2) // runnning from command line - //{ - - /*// Load parameters from file using original TomlIO class - TomlIO toml_io; - toml_io.LoadParametersFromTomlFile(argv[2], params); - std::cout << "Parameters loaded OK from: " << argv[2] << std::endl; - - std::string working_directory; - std::vector worker_data; - - std::chrono::time_point start, end; - start = std::chrono::system_clock::now(); - - std::atomic loPause{ false }; - step1(argv[1], params, pointsPerFile, imu_data, working_directory, trajectory, worker_data, loPause); - - step2(worker_data, params, loPause); - - 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 << "calculations finished computation at " << std::ctime(&end_time) - << "Elapsed time: " << formatTime(elapsed_seconds.count()).c_str() << "s\n"; - - save_results(false, elapsed_seconds.count(), working_directory, worker_data, params, argv[3]);*/ - - //} - // else // full GUI mode if (argc == 2) { initGL(&argc, argv, winTitle, display, mouse); glutCloseFunc(on_exit); - // std::cout << argv[0] << " input_folder parameters(*.toml) output_folder" << std::endl; std::cout << "processed folder with mandeye data: '" << argv[1] << "'" << std::endl; lastPar = 4; diff --git a/apps/lidar_odometry_step_1/lidar_odometry.cpp b/apps/lidar_odometry_step_1/lidar_odometry.cpp index 8df75630..e5b65745 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry.cpp @@ -391,22 +391,6 @@ void calculate_trajectory( static_cast(gyr.axis.z * RAD_TO_DEG) - static_cast(bias_gyr_z) }; const FusionVector accelerometer = { acc.axis.x, acc.axis.y, acc.axis.z }; - /*if (provious_time_stamp == 0.0) - { - double SAMPLE_PERIOD = (1.0 / 200.0); - FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, SAMPLE_PERIOD); - } - else - { - double sp = timestamp_pair.first - provious_time_stamp; - // std::cout << "sp: " << sp << std::endl; - if (sp > 0.1) - { - sp = 0.1; - } - FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, sp); - }*/ - if (first) { FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, 1.0 / 200.0); @@ -420,25 +404,6 @@ void calculate_trajectory( double ts_diff = curr_ts - last_ts; FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, ts_diff); - - /*if (ts_diff < 0) - { - std::cout << "WARNING!!!!" << std::endl; - std::cout << "WARNING!!!!" << std::endl; - std::cout << "WARNING!!!!" << std::endl; - std::cout << "WARNING!!!!" << std::endl; - std::cout << "WARNING!!!!" << std::endl; - std::cout << "WARNING!!!!" << std::endl; - } - - if (ts_diff < 0.01) - { - FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, ts_diff); - } - else - { - FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, 1.0/200.0); - }*/ } last_ts = timestamp_pair.first; @@ -500,8 +465,6 @@ bool compute_step_1( break; } - // std::cout << "timestamp_begin: " << timestamp_begin << std::endl; - std::vector> timestamps; std::vector poses; std::vector raw_imu_data; @@ -599,8 +562,6 @@ bool compute_step_1( points.insert(points.end(), std::make_move_iterator(lower), std::make_move_iterator(upper)); } - // wd.original_points = points; - // correct points timestamps if (wd.intermediate_trajectory_timestamps.size() > 2) { @@ -609,15 +570,9 @@ bool compute_step_1( wd.intermediate_trajectory_timestamps[0].first) / points.size(); - // std::cout << "ts_begin " << ts_begin << std::endl; - // std::cout << "ts_step " << ts_step << std::endl; - // std::cout << "ts_end " << wd.intermediate_trajectory_timestamps[wd.intermediate_trajectory_timestamps.size() - 1].first << - // std::endl; - for (size_t pp = 0; pp < points.size(); pp++) points[pp].timestamp = ts_begin + pp * ts_step; } - ////////////////////////////////////////// for (size_t k = 0; k < points.size(); k++) { @@ -996,11 +951,6 @@ void save_result(std::vector& worker_data, LidarOdometryParams& para // Save processing results and complex data to JSON file save_processing_results_json(params, outwd, elapsed_time_s); - - // remove cache - // std::cout << "remove cache: '" << params.working_directory_cache << "' START" << std::endl; - // std::filesystem::remove_all(params.working_directory_cache); - // std::cout << "remove cache: '" << params.working_directory_cache << "' FINISHED" << std::endl; } void filter_reference_buckets(LidarOdometryParams& params) diff --git a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp index 77831d42..10dd3f89 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp @@ -323,7 +323,6 @@ void find_best_stretch( { auto lower = std::lower_bound(timestamps.begin(), timestamps.end(), points[i].timestamp); points[i].index_pose = std::distance(timestamps.begin(), lower); - // std::cout << "points[i].timestamp " << points[i].timestamp << " timestamps " << timestamps[points[i].index_pose] << std::endl; } std::set indexes; @@ -1724,8 +1723,6 @@ void display() glBegin(GL_POINTS); for (const auto &p : worker_data[i].intermediate_points) { - // std::cout << "kk"; - // std::cout << p.index_pose; Eigen::Vector3d pt = worker_data[i].intermediate_trajectory[p.index_pose] * p.point; glVertex3d(pt.x(), pt.y(), pt.z()); } @@ -2324,11 +2321,6 @@ void mouse(int glut_button, int state, int x, int y) int main(int argc, char* argv[]) { - // std::cout << "argc " << argc << std::endl; - // if (argc == 2){ - // std::cout << "argv[1]: '" << argv[1] << "'" << std::endl; - // } - spdlog::cfg::load_env_levels(); spdlog::flush_on(spdlog::level::warn); set_lidar_odometry_default_params(params); diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp b/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp index 712c7d37..01fcd033 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp @@ -7,13 +7,10 @@ #include #include -// #include - namespace fs = std::filesystem; std::vector decimate(const std::vector& points, double bucket_x, double bucket_y, double bucket_z) { - // std::cout << "points.size before decimation: " << points.size() << std::endl; Eigen::Vector3d b(bucket_x, bucket_y, bucket_z); std::vector out; @@ -41,7 +38,6 @@ std::vector decimate(const std::vector& points, double bucke if (ip[i - 1].index_of_bucket != ip[i].index_of_bucket) out.emplace_back(points[ip[i].index_of_point]); - // std::cout << "points.size after decimation: " << out.size() << std::endl; return out; } @@ -53,26 +49,20 @@ Eigen::Matrix4d getInterpolatedPose(const std::map& tra if (it_lower == trajectory.begin()) { - // std::cout << "1" << std::endl; return ret; } if (it_lower->first > query_time) { - // std::cout << "2" << std::endl; it_lower = std::prev(it_lower); } if (it_lower == trajectory.begin()) { - // std::cout << "3" << std::endl; return ret; } if (it_lower == trajectory.end()) { - // std::cout << "4" << std::endl; return ret; } - // std::cout << std::setprecision(10); - // std::cout << it_lower->first << " " << query_time << " " << it_next->first << " " << std::next(it_lower)->first << std::endl; double t1 = it_lower->first; double t2 = it_next->first; @@ -80,17 +70,14 @@ Eigen::Matrix4d getInterpolatedPose(const std::map& tra double difft2 = t2 - query_time; if (t1 == t2 && std::fabs(difft1) < 0.1) { - // std::cout << "5" << std::endl; ret = Eigen::Matrix4d::Identity(); ret.col(3).head<3>() = it_next->second.col(3).head<3>(); ret.topLeftCorner(3, 3) = it_lower->second.topLeftCorner(3, 3); return ret; } - // std::cout << std::fabs(difft1) << " " << std::fabs(difft2) << std::endl; // if (std::fabs(difft1) < 0.15 && std::fabs(difft2) < 0.15) { - // std::cout << "6" << std::endl; assert(t2 > t1); assert(query_time > t1); assert(query_time < t2); @@ -106,15 +93,13 @@ Eigen::Matrix4d getInterpolatedPose(const std::map& tra ret.topLeftCorner(3, 3) = qt.toRotationMatrix(); return ret; } - // std::cout << "Problem with : " << difft1 << " " << difft2 << " q : " << query_time << " t1 :" << t1 << " t2: " << t2 << std::endl; + return ret; } void limit_covariance(Eigen::Matrix3d& io_cov) { return; - // std::cout << "------io_cov in ------------" << std::endl; - // std::cout << io_cov << std::endl; Eigen::EigenSolver eigensolver; eigensolver.compute(io_cov); @@ -122,19 +107,14 @@ void limit_covariance(Eigen::Matrix3d& io_cov) Eigen::Vector3d eigenValues = eigensolver.eigenvalues().real(); Eigen::Matrix3d eigenVectors = eigensolver.eigenvectors().real(); - // modify eigen values for (int k = 0; k < 3; ++k) { eigenValues(k) = std::max(eigenValues(k), 0.0001); } - // create diagonal matrix Eigen::DiagonalMatrix diagonal_matrix(eigenValues(0), eigenValues(1), eigenValues(2)); - // update covariance io_cov = eigenVectors * diagonal_matrix * eigenVectors.inverse(); - // std::cout << "------io_cov out ------------" << std::endl; - // std::cout << io_cov << std::endl; } void update_rgd( @@ -422,8 +402,6 @@ std::vector, FusionVector, FusionVector>> l acc.axis.y = row["accY"].get(); acc.axis.z = row["accZ"].get(); all_data.emplace_back(std::pair(timestamp / 1e9, timestampUnix / 1e9), gyr, acc); - // std::cout << "acc.axis.x: " << acc.axis.x << " acc.axis.y: " << acc.axis.y << " acc.axis.z " << acc.axis.z << " - // imu_id: " << imu_id << std::endl; } } } @@ -449,8 +427,7 @@ std::vector, FusionVector, FusionVector>> l { iss >> timestampUnix; } - // std::cout << data[0] << " " << data[1] << " " << data[2] << " " << data[3] << " " << data[4] << " " << data[5] << " " - // << data[6] << std::endl; + if (data[0] > 0 && imuId == imuToUse) { FusionVector gyr; @@ -472,7 +449,6 @@ std::vector, FusionVector, FusionVector>> l } catch (...) { std::cout << "load_imu error for file: '" << imu_file << "'" << std::endl; - // return all_data; } return all_data; @@ -499,7 +475,7 @@ std::vector load_point_cloud( 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)) @@ -507,7 +483,7 @@ std::vector load_point_cloud( 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)) { @@ -694,7 +670,6 @@ std::unordered_map MLvxCalib::GetCalibrationFromFi { const std::string& lidarSn = calibrationEntry.key(); Eigen::Matrix4d value; - // std::cout << "lidarSn : " << lidarSn << std::endl; bool identity = JsonGetBool(calibrationEntry.value(), "identity", false); if (identity) @@ -718,7 +693,7 @@ std::unordered_map MLvxCalib::GetCalibrationFromFi if (calibrationEntry.value().contains("order")) { std::string order = calibrationEntry.value()["order"].get(); - // std::cout << "order : " << order << std::endl; + std::transform(order.begin(), order.end(), order.begin(), ::toupper); if (order == "COLUMN") value = value.transpose(); @@ -730,9 +705,6 @@ std::unordered_map MLvxCalib::GetCalibrationFromFi Eigen::IOFormat HeavyFmt(Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]"); - // std::cout << "Calibration for " << lidarSn << std::endl; - // std::cout << value.format(HeavyFmt) << std::endl; - // Insert into the map dataMap[lidarSn] = value; } @@ -791,7 +763,6 @@ std::unordered_map MLvxCalib::CombineIntoCalibration( std::unordered_map dataMap; for (const auto& [id, sn] : idToSn) { - // std::cout << "XXX: "<< id << " " << sn << std::endl; const auto& affine = calibration.at(sn); dataMap[id] = affine; } @@ -810,7 +781,6 @@ int MLvxCalib::GetImuIdToUse(const std::unordered_map& idToSn, } for (const auto& [id, sn] : idToSn) { - // std::cout << "snToUse " << snToUse << " sn " << sn << std::endl; if (snToUse == sn) { return id; 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 f514a6e5..5d33b71e 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp @@ -1722,7 +1722,6 @@ static void compute_hessian( if (convergence_result > convergence_delta_threshold_outer_rgd) { check_threshold_outdoor_rgd = false; - // std::cout << "do not use out " << std::endl; } } @@ -2462,12 +2461,10 @@ bool process_worker_step_2( std::vector points_local_sf; std::vector points_local; - /// for (int ii = 0; ii < intermediate_points.size(); ii++) { double r_l = intermediate_points[ii].point.norm(); - // std::cout << worker_data[i].intermediate_points[ii].index_pose << " "; if (r_l > 0.5 && intermediate_points[ii].index_pose != -1 && r_l < params.max_distance_lidar_rigid_sf) { double polar_angle_deg_l = atan2(intermediate_points[ii].point.y(), intermediate_points[ii].point.x()) * RAD_TO_DEG; @@ -2475,7 +2472,6 @@ bool process_worker_step_2( points_local.push_back(intermediate_points[ii]); - /////////////////////////////////////////////////////// Point3Di p_sl = intermediate_points[ii]; p_sl.point.x() = r_l; p_sl.point.y() = polar_angle_deg_l; 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 index 7a24469f..d4f2d1f3 100644 --- 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 @@ -531,7 +531,6 @@ void project_gui() 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); @@ -1450,7 +1449,6 @@ void calibrate_intrinsics() // std::vector mutexes(intrinsics.size()); - // std::cout << "jojo" << std::endl; const auto hessian_fun = [&](const Point3Di& intermediate_points_i) { int ir = tripletListB.size(); @@ -1776,7 +1774,6 @@ void calibrate_intrinsics() { for (Eigen::SparseMatrix::InnerIterator it(x, k); it; ++it) { - // std::cout << it.value() << " "; h_x.push_back(it.value()); } } 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 2f7cc6a1..2d20aa42 100644 --- a/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp +++ b/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp @@ -311,8 +311,6 @@ void optimize() continue; } - // std::cout << infm << std::endl; - const Eigen::Affine3d& m_pose = tr[points_local[i].index_pose]; //spdlog::info("points_local[i].index_pose {}", points_local[i].index_pose); const Eigen::Vector3d& p_s = points_local[i].point; @@ -1585,8 +1583,6 @@ void display() all_data[index_rendered_points_local].points_local[i].timestamp - all_data[index_rendered_points_local].timestamps[index_pose].first) > max_diff) { - // std::cout << index_pose << " " << all_data[index_rendered_points_local].points_local[i].timestamp - - // all_data[index_rendered_points_local].timestamps[index_pose].first << std::endl; max_diff = fabs( all_data[index_rendered_points_local].points_local[i].timestamp - all_data[index_rendered_points_local].timestamps[index_pose].first); @@ -1607,8 +1603,6 @@ void display() int index_pose = std::distance(all_data[index_rendered_points_local].timestamps.begin(), lower) - 1; - // std::cout << index_pose << std::endl; - if (max_diff < 0.1) { if (index_pose >= 0 && index_pose < all_data[index_rendered_points_local].poses.size()) diff --git a/apps/manual_color/manual_color.cpp b/apps/manual_color/manual_color.cpp index 0c24a1c2..0cc51e54 100644 --- a/apps/manual_color/manual_color.cpp +++ b/apps/manual_color/manual_color.cpp @@ -649,13 +649,10 @@ void ImGuiLoadSaveButtons() const auto input_file_names = mandeye::fd::OpenFileDialog("Choose Image", mandeye::fd::ImageFilter, false); if (input_file_names.size()) { - // std::cout << "1" << std::endl; tex1 = make_tex(input_file_names.front()); - // std::cout << "2" << std::endl; SD::imageData = stbi_load(input_file_names.front().c_str(), &SD::imageWidth, &SD::imageHeight, &SD::imageNrChannels, 0); - // std::cout << "3" << std::endl; } - // std::cout << "4" << std::endl; + SystemData::points = ApplyColorToPointcloud( SystemData::points, SystemData::imageData, @@ -663,7 +660,6 @@ void ImGuiLoadSaveButtons() SystemData::imageHeight, SystemData::imageNrChannels, SystemData::camera_pose); - // std::cout << "5" << std::endl; } ImGui::SameLine(); if (ImGui::Button("Load Poincloud")) @@ -791,13 +787,8 @@ void optimize() tripletListP.clear(); tripletListB.clear(); - // std::cout << "AtPA.size: " << AtPA.size() << std::endl; - // std::cout << "AtPB.size: " << AtPB.size() << std::endl; - - // std::cout << "start solving AtPA=AtPB" << std::endl; Eigen::SimplicialCholesky> solver(AtPA); - // std::cout << "x = solver.solve(AtPB)" << std::endl; Eigen::SparseMatrix x = solver.solve(AtPB); std::vector h_x; @@ -812,13 +803,6 @@ void optimize() if (h_x.size() == 6) { - // for (size_t i = 0; i < h_x.size(); i++) - //{ - // std::cout << h_x[i] << std::endl; - // } - // std::cout << "AtPA=AtPB SOLVED" << std::endl; - // std::cout << "update" << std::endl; - int counter = 0; pose.px += h_x[counter++] * 0.1; pose.py += h_x[counter++] * 0.1; @@ -960,13 +944,8 @@ void optimize_fish_eye() tripletListP.clear(); tripletListB.clear(); - // std::cout << "AtPA.size: " << AtPA.size() << std::endl; - // std::cout << "AtPB.size: " << AtPB.size() << std::endl; - - // std::cout << "start solving AtPA=AtPB" << std::endl; Eigen::SimplicialCholesky> solver(AtPA); - // std::cout << "x = solver.solve(AtPB)" << std::endl; Eigen::SparseMatrix x = solver.solve(AtPB); std::vector h_x; @@ -981,13 +960,6 @@ void optimize_fish_eye() if (h_x.size() == 6) { - // for (size_t i = 0; i < h_x.size(); i++) - //{ - // std::cout << h_x[i] << std::endl; - // } - // std::cout << "AtPA=AtPB SOLVED" << std::endl; - // std::cout << "update" << std::endl; - int counter = 0; pose.px += h_x[counter++] * 0.1; pose.py += h_x[counter++] * 0.1; diff --git a/apps/multi_session_registration/multi_session_factor_graph.cpp b/apps/multi_session_registration/multi_session_factor_graph.cpp index 6b736c0e..6b0ceb27 100644 --- a/apps/multi_session_registration/multi_session_factor_graph.cpp +++ b/apps/multi_session_registration/multi_session_factor_graph.cpp @@ -13,7 +13,6 @@ bool optimize(std::vector& sessions, const std::vector& edges) { for (auto& session : sessions) { - // std::cout << session.point_clouds_container.point_clouds.size() << std::endl; for (auto& pc : session.point_clouds_container.point_clouds) pc.m_pose_temp = pc.m_pose; } @@ -45,8 +44,6 @@ bool optimize(std::vector& sessions, const std::vector& edges) } } - /////////////////////////////////////////////////////////////////// - // graph slam bool is_ok = true; std::vector m_poses; std::vector poses_motion_model; @@ -143,7 +140,6 @@ bool optimize(std::vector& sessions, const std::vector& edges) for (int iter = 0; iter < iterations; iter++) { - // std::cout << "iteration " << iter + 1 << " of " << iterations << std::endl; std::vector> tripletListA; std::vector> tripletListP; std::vector> tripletListB; @@ -257,53 +253,14 @@ bool optimize(std::vector& sessions, const std::vector& edges) tripletListB.emplace_back(ir + 4, 0, normalize_angle(delta(4, 0))); tripletListB.emplace_back(ir + 5, 0, normalize_angle(delta(5, 0))); - // std::cout << "delta(0, 0): " << delta(0, 0) << std::endl; - // std::cout << "delta(1, 0): " << delta(0, 0) << std::endl; - // std::cout << "delta(2, 0): " << delta(0, 0) << std::endl; - // std::cout << "normalize_angle(delta(3, 0)): " << normalize_angle(delta(3, 0)) << std::endl; - // std::cout << "normalize_angle(delta(4, 0)): " << normalize_angle(delta(4, 0)) << std::endl; - // std::cout << "normalize_angle(delta(5, 0)): " << normalize_angle(delta(5, 0)) << std::endl; - - // for (int r = 0; r < 6; r++) { - // for (int c = 0; c < 6; c++) { - // tripletListP.emplace_back(ir + r, ir + c, edges[i].information_matrix.coeffRef(r, c)); - // } - // } - - // tripletListP.emplace_back(ir , ir , get_cauchy_w(delta(0, 0), 10)); - // tripletListP.emplace_back(ir + 1, ir + 1, get_cauchy_w(delta(1, 0), 10)); - // tripletListP.emplace_back(ir + 2, ir + 2, get_cauchy_w(delta(2, 0), 10)); - // tripletListP.emplace_back(ir + 3, ir + 3, get_cauchy_w(delta(3, 0), 10)); - // tripletListP.emplace_back(ir + 4, ir + 4, get_cauchy_w(delta(4, 0), 10)); - // tripletListP.emplace_back(ir + 5, ir + 5, get_cauchy_w(delta(5, 0), 10)); - - // if (sessions[all_edges[i].index_session_from].is_ground_truth || sessions[all_edges[i].index_session_to].is_ground_truth) - //{ - // tripletListP.emplace_back(ir, ir, all_edges[i].relative_pose_tb_weights.px); - // tripletListP.emplace_back(ir + 1, ir + 1, all_edges[i].relative_pose_tb_weights.py); - // tripletListP.emplace_back(ir + 2, ir + 2, all_edges[i].relative_pose_tb_weights.pz); - // tripletListP.emplace_back(ir + 3, ir + 3, all_edges[i].relative_pose_tb_weights.om); - // tripletListP.emplace_back(ir + 4, ir + 4, all_edges[i].relative_pose_tb_weights.fi); - // tripletListP.emplace_back(ir + 5, ir + 5, all_edges[i].relative_pose_tb_weights.ka); - // } - // else - //{ - tripletListP.emplace_back(ir, ir, all_edges[i].relative_pose_tb_weights.px); tripletListP.emplace_back(ir + 1, ir + 1, all_edges[i].relative_pose_tb_weights.py); tripletListP.emplace_back(ir + 2, ir + 2, all_edges[i].relative_pose_tb_weights.pz); tripletListP.emplace_back(ir + 3, ir + 3, all_edges[i].relative_pose_tb_weights.om); tripletListP.emplace_back(ir + 4, ir + 4, all_edges[i].relative_pose_tb_weights.fi); tripletListP.emplace_back(ir + 5, ir + 5, all_edges[i].relative_pose_tb_weights.ka); - - // tripletListP.emplace_back(ir, ir, all_edges[i].relative_pose_tb_weights.px * get_cauchy_w(delta(0, 0), 1)); - // tripletListP.emplace_back(ir + 1, ir + 1, all_edges[i].relative_pose_tb_weights.py * get_cauchy_w(delta(1, 0), 1)); - // tripletListP.emplace_back(ir + 2, ir + 2, all_edges[i].relative_pose_tb_weights.pz * get_cauchy_w(delta(2, 0), 1)); - // tripletListP.emplace_back(ir + 3, ir + 3, all_edges[i].relative_pose_tb_weights.om * get_cauchy_w(delta(3, 0), 1)); - // tripletListP.emplace_back(ir + 4, ir + 4, all_edges[i].relative_pose_tb_weights.fi * get_cauchy_w(delta(4, 0), 1)); - // tripletListP.emplace_back(ir + 5, ir + 5, all_edges[i].relative_pose_tb_weights.ka * get_cauchy_w(delta(5, 0), 1)); - // } } + if (is_fix_first_node) { int ir = tripletListB.size(); @@ -590,20 +547,9 @@ bool optimize(std::vector& sessions, const std::vector& edges) tripletListP.emplace_back(ir, ir, 1); tripletListB.emplace_back(ir, 0, 0); } - - // std::cout << "add manual_pose_graph_loop_closure.edge" << std::endl; - // Edge edge; - // edge.index_from = sessions[i].pose_graph_loop_closure.edges[j].index_from + sums[i]; - // edge.index_to = sessions[i].pose_graph_loop_closure.edges[j].index_to + sums[i]; - // edge.relative_pose_tb = sessions[i].pose_graph_loop_closure.edges[j].relative_pose_tb; - // edge.relative_pose_tb_weights = sessions[i].pose_graph_loop_closure.edges[j].relative_pose_tb_weights; - // edge.is_fixed_fi = ToDo - // all_edges.push_back(edge); } } - // fuse_inclination_from_imu - double error_imu = 0; double error_imu_sum = 0; @@ -888,13 +834,8 @@ bool optimize(std::vector& sessions, const std::vector& edges) tripletListP.clear(); tripletListB.clear(); - // std::cout << "AtPA.size: " << AtPA.size() << std::endl; - // std::cout << "AtPB.size: " << AtPB.size() << std::endl; - - // std::cout << "start solving AtPA=AtPB" << std::endl; Eigen::SimplicialCholesky> solver(AtPA); - // std::cout << "x = solver.solve(AtPB)" << std::endl; Eigen::SparseMatrix x = solver.solve(AtPB); std::vector h_x; @@ -907,16 +848,6 @@ bool optimize(std::vector& sessions, const std::vector& edges) } } - // std::cout << "h_x.size(): " << h_x.size() << std::endl; - - // std::cout << "AtPA=AtPB SOLVED" << std::endl; - // std::cout << "updates:" << std::endl; - // for (size_t i = 0; i < h_x.size(); i += 6) - //{ - // std::cout << h_x[i] << "," << h_x[i + 1] << "," << h_x[i + 2] << "," << h_x[i + 3] << "," << h_x[i + 4] << "," << h_x[i + 5] - // << std::endl; - // } - if (h_x.size() == 6 * poses.size()) { int counter = 0; @@ -931,30 +862,13 @@ bool optimize(std::vector& sessions, const std::vector& edges) pose.fi += h_x[counter++] * 0.1; pose.ka += h_x[counter++] * 0.1; - /*if (!vfixed_x[i]) - poses[i].px = pose.px; - if (!vfixed_y[i]) - poses[i].py = pose.py; - if (!vfixed_z[i]) - poses[i].pz = pose.pz; - if (!vfixed_om[i]) - poses[i].om = pose.om; - if (!vfixed_fi[i]) - poses[i].fi = pose.fi; - if (!vfixed_ka[i]) - poses[i].ka = pose.ka;*/ - poses[i].px = pose.px; poses[i].py = pose.py; poses[i].pz = pose.pz; poses[i].om = pose.om; poses[i].fi = pose.fi; poses[i].ka = pose.ka; - - // if (i == 0 && is_fix_first_node) - // poses[i] = pose; } - // std::cout << "optimizing with tait bryan finished" << std::endl; } else { diff --git a/apps/multi_session_registration/multi_session_registration.cpp b/apps/multi_session_registration/multi_session_registration.cpp index 74014c31..417791e6 100644 --- a/apps/multi_session_registration/multi_session_registration.cpp +++ b/apps/multi_session_registration/multi_session_registration.cpp @@ -218,8 +218,7 @@ void ndt_gui() double rms_initial = 0.0; double rms_final = 0.0; double mui = 0.0; - // ndt.optimize(point_clouds_container.point_clouds, rms_initial, rms_final, mui); - // std::cout << "mui: " << mui << " rms_initial: " << rms_initial << " rms_final: " << rms_final << std::endl; + ndt.optimize(sessions, false, compute_mean_and_cov_for_bucket); } ImGui::End(); @@ -281,8 +280,6 @@ void ndt_gui() double rms_initial = 0.0; double rms_final = 0.0; double mui = 0.0; - // ndt.optimize(point_clouds_container.point_clouds, rms_initial, rms_final, mui); - // std::cout << "mui: " << mui << " rms_initial: " << rms_initial << " rms_final: " << rms_final << std::endl; ndt.optimize(session.point_clouds_container.point_clouds, true, compute_mean_and_cov_for_bucket); } @@ -2743,15 +2740,12 @@ void display() for (size_t j = 0; j < sessions[i].point_clouds_container.point_clouds.size(); j++) all_m_poses.push_back(sessions[i].point_clouds_container.point_clouds[j].m_pose); - // if (all_m_poses.size() > 1) - //{ ImGuiIO& io = ImGui::GetIO(); - // ImGuizmo ----------------------------------------------- + ImGuizmo::BeginFrame(); ImGuizmo::Enable(true); ImGuizmo::SetRect(0, 0, io.DisplaySize.x, io.DisplaySize.y); - // std::cout << "3" << std::endl; if (!is_ortho) { GLfloat projection[16]; diff --git a/apps/precision_forestry_tools/precision_forestry_tools.cpp b/apps/precision_forestry_tools/precision_forestry_tools.cpp index cd8a7358..b4f80450 100644 --- a/apps/precision_forestry_tools/precision_forestry_tools.cpp +++ b/apps/precision_forestry_tools/precision_forestry_tools.cpp @@ -219,7 +219,6 @@ void project_gui() if (input_file_names.size() == 1) { - // std::cout << "Las/Laz file (only 1):" << std::endl; for (size_t i = 0; i < input_file_names.size(); i++) { std::cout << input_file_names[i] << std::endl; @@ -346,13 +345,6 @@ void project_gui() LocalShapeFeatures lsf; lsf.calculate_local_shape_features(points_with_lsf, params); - - // for (const auto &p : points_with_lsf) - //{ - // if (p.valid) - // std::cout << p.planarity << "," << p.cylindrical_likeness << "," << p.plane_likeness << "," << p.sphericity << "," << - // p.change_of_curvature << "," << p.omnivariance << "," << p.eigen_entropy << std::endl; - // } } else { @@ -528,15 +520,10 @@ void display() session.point_clouds_container.point_clouds[0].points_local[lowest_points_indexes[i]].x(), session.point_clouds_container.point_clouds[0].points_local[lowest_points_indexes[i]].y(), session.point_clouds_container.point_clouds[0].points_local[lowest_points_indexes[i]].z()); - - // std::cout << session.point_clouds_container.point_clouds[0].points_local[lowest_points_indexes[i]].x() << " " << - // session.point_clouds_container.point_clouds[0].points_local[lowest_points_indexes[i]].y() << " " << - // session.point_clouds_container.point_clouds[0].points_local[lowest_points_indexes[i]].z() << std::endl; } glEnd(); glPointSize(1); } - // void render(bool show_with_initial_pose, const ObservationPicking &observation_picking, int viewer_decmiate_point_cloud); if (show_normal_vectors) { diff --git a/apps/quick_start_demo/quick_start_demo.cpp b/apps/quick_start_demo/quick_start_demo.cpp index d3e782b0..0ab13b2f 100644 --- a/apps/quick_start_demo/quick_start_demo.cpp +++ b/apps/quick_start_demo/quick_start_demo.cpp @@ -599,7 +599,6 @@ bool compute_step_2_demo(std::vector &worker_data, LidarOdometryPara 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: " << (int64_t)worker_data[i].intermediate_trajectory_timestamps[0].first << std::endl; return false; } @@ -763,9 +762,6 @@ int main(int argc, char *argv[]) { working_directory = "."; // fs::path(input_file_names[0]).parent_path().string(); - // std::cout << "0" << std::endl; - - // check if folder exists! if (!fs::exists(working_directory)) { std::cout << "folder '" << working_directory << "' does not exist" << std::endl; @@ -893,8 +889,6 @@ int main(int argc, char *argv[]) std::cout << calib.matrix() << std::endl; } return data; - // std::cout << fn << std::endl; - // }); std::cout << "std::transform finished" << std::endl; @@ -922,8 +916,6 @@ int main(int argc, char *argv[]) const FusionVector gyroscope = {static_cast(gyr.axis.x * 180.0 / M_PI), static_cast(gyr.axis.y * 180.0 / M_PI), static_cast(gyr.axis.z * 180.0 / M_PI)}; const FusionVector accelerometer = {acc.axis.x, acc.axis.y, acc.axis.z}; - // std::cout << "acc.axis.x: " << acc.axis.x << " acc.axis.y: " << acc.axis.y << " acc.axis.z: " << acc.axis.z << std::endl; - FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, SAMPLE_PERIOD); FusionQuaternion quat = FusionAhrsGetQuaternion(&ahrs); @@ -932,14 +924,6 @@ int main(int argc, char *argv[]) Eigen::Affine3d t{Eigen::Matrix4d::Identity()}; t.rotate(d); - // - // TaitBryanPose rot_y; - // rot_y.px = rot_y.py = rot_y.pz = rot_y.px = rot_y.py = rot_y.pz; - // rot_y.fi = -5 * M_PI / 180.0; - // Eigen::Affine3d m_rot_y = affine_matrix_from_pose_tait_bryan(rot_y); - // t = t * m_rot_y; - // - // std::map trajectory; trajectory[timestamp_pair.first] = std::pair(t.matrix(), timestamp_pair.second); const FusionEuler euler = FusionQuaternionToEuler(FusionAhrsGetQuaternion(&ahrs)); counter++; diff --git a/core/include/export_laz.h b/core/include/export_laz.h index f8c80efe..e06e58d5 100644 --- a/core/include/export_laz.h +++ b/core/include/export_laz.h @@ -167,20 +167,6 @@ inline bool exportLaz( return writer.close(); } -// inline Eigen::Vector3d adjustPoint(laszip_F64 input_coordinates[3], const Eigen::Affine3d &m_pose) -//{ -// Eigen::Vector3d i(input_coordinates[0], input_coordinates[1], input_coordinates[2]); -// i -= offset; -// Eigen::Vector3d o = m_pose * i; - -// std::cout << i.x() << " " << i.y() << " " << i.z() << " " << o.x() << " " << o.y() << " " << o.z() << std::endl; -// o += offset; -// output_coordinates[0] = o.x(); -// output_coordinates[1] = o.y(); -// output_coordinates[2] = o.z(); -// return o; -//} - inline void save_processed_pc( const fs::path& file_path_in, const fs::path& file_path_put, diff --git a/core/src/color_las_loader.cpp b/core/src/color_las_loader.cpp index e9c0424e..12440961 100644 --- a/core/src/color_las_loader.cpp +++ b/core/src/color_las_loader.cpp @@ -26,41 +26,9 @@ std::vector mandeye::load(const std::string& lazFile) 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(); - // } - - /*std::cout << "header->number_of_point_records: " << header->number_of_point_records << std::endl; - std::cout << "header->extended_number_of_point_records: " << header->extended_number_of_point_records << std::endl; - - //for (int j = 0; j < header->number_of_point_records; j++) - for (int j = 0; j < header->extended_number_of_point_records; j++) - { - if (laszip_read_point(laszip_reader)) - { - fprintf(stderr, "DLL ERROR: reading point %u\n", j); - std::abort(); - } - - Point p; - p.point.x() = header->x_offset + header->x_scale_factor * static_cast(point->X); - p.point.y() = header->y_offset + header->y_scale_factor * static_cast(point->Y); - p.point.z() = header->z_offset + header->z_scale_factor * static_cast(point->Z); - p.timestamp = point->gps_time; - p.intensity = point->intensity; - - points.emplace_back(p); - }*/ laszip_I64 npoints = (header->number_of_point_records ? header->number_of_point_records : header->extended_number_of_point_records); - // report how many points the file has - fprintf(stderr, "file '%s' contains %I64d points\n", lazFile.c_str(), npoints); laszip_point* point; diff --git a/core/src/gnss.cpp b/core/src/gnss.cpp index 84755318..a23e8d71 100644 --- a/core/src/gnss.cpp +++ b/core/src/gnss.cpp @@ -127,7 +127,6 @@ bool GNSS::load(const std::vector& input_file_names, Eigen::Vector3 pose.x = pose.x - firstGNSS.x; pose.y = pose.y - firstGNSS.y; pose.alt = pose.alt - firstGNSS.alt; - // std::cout << "pose.x: " << pose.x << " pose.y: " << pose.y << " pose.alt: " << pose.alt << std::endl; } } @@ -188,8 +187,6 @@ bool GNSS::load_mercator_projection(const std::vector& input_file_n if (gp.lon != 0) { gnss_poses.push_back(gp); - // std::cout << std::setprecision(20); - // std::cout << "gp.lat " << gp.lat << " gp.lon " << gp.lon << " gp.alt " << gp.alt << std::endl; } } } @@ -312,7 +309,6 @@ bool GNSS::load_nmea_mercator_projection(const std::vector& input_f // register if there is not nans if (gp.lat == gp.lat && gp.lon == gp.lon && gp.alt == gp.alt) { - // std::cout << "gp.lat " << gp.lat << " gp.lon " << gp.lon << std::endl; gnss_poses.push_back(gp); } } @@ -354,8 +350,6 @@ bool GNSS::load_nmea_mercator_projection(const std::vector& input_f std::array result{ wgs84::toCartesian(WGS84Reference, WGS84Position) }; gnss_poses[i].x = result[0]; gnss_poses[i].y = result[1]; - - // std::cout << "gnss_poses[i].x " << gnss_poses[i].x << " gnss_poses[i].y " << gnss_poses[i].y << std::endl; } return true; diff --git a/core/src/manual_pose_graph_loop_closure.cpp b/core/src/manual_pose_graph_loop_closure.cpp index a7d17bd9..912e77f5 100644 --- a/core/src/manual_pose_graph_loop_closure.cpp +++ b/core/src/manual_pose_graph_loop_closure.cpp @@ -354,8 +354,6 @@ void ManualPoseGraphLoopClosure::Gui( if (ImGui::Button(("Reload cache##" + std::to_string(i)).c_str())) { - // std::cout << edges[index_active_edge].index_from - std::cout << point_clouds_container.point_clouds[edges[index_active_edge].index_from].file_name << std::endl; std::cout << point_clouds_container.point_clouds[edges[index_active_edge].index_to].file_name << std::endl; @@ -368,23 +366,6 @@ void ManualPoseGraphLoopClosure::Gui( point_clouds_container.point_clouds[edges[index_active_edge].index_to].file_name, true); point_clouds_container.point_clouds[edges[index_active_edge].index_to].decimate( downsampling_voxel_size, downsampling_voxel_size, downsampling_voxel_size); - - // gizmo = false; - - // std::vector new_edges; - // for (int ni = 0; ni < edges.size(); ni++) - //{ - // if (i != ni) - // new_edges.push_back(edges[ni]); - // } - // edges = new_edges; - - // index_active_edge = std::min(index_active_edge, static_cast(edges.size() - 1)); - // manipulate_active_edge = (edges.size() > 0); - - // for (int i = 0; i < point_clouds_container.point_clouds.size(); i++) - // { - // } } ImGui::SameLine(); diff --git a/core/src/ndt.cpp b/core/src/ndt.cpp index f8d1e2e9..36d360a7 100644 --- a/core/src/ndt.cpp +++ b/core/src/ndt.cpp @@ -679,13 +679,6 @@ void ndt_job( Eigen::Matrix cov_xyz = j * cov_r_alpha_theta * j.transpose(); std::vector points = get_points_normal_distribution(cov_xyz, point_local, num_extended_points); - // for (const auto &ppp : points) - //{ - // std::cout << "---" << std::endl; - // std::cout << ppp << std::endl; - // } - // exit(1); - for (const auto& pp : points) { auto ppg = m * pp; @@ -743,10 +736,7 @@ void ndt_job( if (min_eigen_value > 3 * sigma_r) { continue; - } // else{ - // std::cout << min_eigen_value << " "; - //} - // std::cout << min_eigen_value << " "; + } Eigen::Matrix3d eigenVectorsPCA = eigen_solver.eigenvectors(); Eigen::Vector3d nv = eigenVectorsPCA.col(1).cross(eigenVectorsPCA.col(2)); @@ -1321,8 +1311,6 @@ void ndt_job( (*md_out) = md; (*md_count_out) = md_count; - // std::cout << md << " "; - if (tripletListB.size() > 0) { Eigen::SparseMatrix matA(tripletListB.size(), trajectory_size * number_of_unknowns); @@ -1354,7 +1342,6 @@ void ndt_job( (*AtPA) = AtPAt_tmp; (*AtPB) = AtPBt_tmp; } - // std::cout << AtPAt << std::endl; } } else @@ -1404,7 +1391,6 @@ void ndt_jobi( for (size_t ii = job->index_begin_inclusive; ii < job->index_end_exclusive; ii++) { - // std::cout << ii << " "; NDT::Bucket& b = (*buckets)[ii]; if (b.number_of_points < 5) { @@ -1463,7 +1449,6 @@ void ndt_jobi( const auto& p = (*pp)[(*index_pair_internal)[index].index_of_point]; mean += p.point; //::Vector3d(p.x, p.y, p.z); counter++; - // std::cout << counter << " "; } } @@ -1540,7 +1525,6 @@ void ndt_jobi( (*buckets)[ii].mean = mean; (*buckets)[ii].cov = cov; - // std::cout << mean << " "; #if 0 if(compute_only_mean_and_cov){ continue; @@ -1961,7 +1945,6 @@ void ndt_jobi( (*md_out) = md; (*md_count_out) = md_count; - // std::cout << md << " "; if (tripletListB.size() > 0) { @@ -1994,7 +1977,6 @@ void ndt_jobi( (*AtPA) = AtPAt_tmp; (*AtPB) = AtPBt_tmp; } - // std::cout << AtPAt << std::endl; } } else @@ -2157,7 +2139,6 @@ bool NDT::optimize(std::vector& point_clouds, bool compute_only_maha for (const auto& b : buckets_external) { - // std::cout << "number of points inside bucket: " << b.number_of_points << std::endl; if (b.number_of_points > 1000) { buckets_external_reduced.push_back(b); @@ -2965,7 +2946,6 @@ bool NDT::optimize(std::vector& sessions, bool compute_only_mahalanobis for (const auto& b : buckets_external) { - // std::cout << "number of points inside bucket: " << b.number_of_points << std::endl; if (b.number_of_points > 1000) { buckets_external_reduced.push_back(b); @@ -3632,12 +3612,9 @@ std::vector> NDT::compute_covariance_matrices_and_rm std::vector mposes_inv; for (size_t i = 0; i < point_clouds.size(); i++) { - // poses.push_back(pose_tait_bryan_from_affine_matrix(point_clouds[i].m_pose)); mposes.push_back(point_clouds[i].m_pose); mposes_inv.push_back(point_clouds[i].m_pose.inverse()); } - // std::cout << "XXX" << std::endl; - // std::cout << (int)pose_convention << " " << (int)rotation_matrix_parametrization << " " << (int)number_of_unknowns << std::endl; for (size_t k = 0; k < jobs.size(); k++) { @@ -3685,8 +3662,6 @@ std::vector> NDT::compute_covariance_matrices_and_rm double sq = ssr / ((double)num_obs - point_clouds.size() * number_of_unknowns); rms = ssr / (double)num_obs; - // std::cout << "sq " << sq << " num_obs " << num_obs << std::endl; - bool init = false; Eigen::SparseMatrix AtPA_ndt(point_clouds.size() * number_of_unknowns, point_clouds.size() * number_of_unknowns); @@ -3732,8 +3707,6 @@ std::vector> NDT::compute_covariance_matrices_and_rm } } covariance_matrices.push_back(cm); - // std::cout << "cm" << std::endl; - // std::cout << cm << std::endl; } return covariance_matrices; } @@ -3880,11 +3853,6 @@ bool NDT::compute_cov_mean( } std::cout << "computing cov mean finished" << std::endl; - // for(int i = 0; i < buckets.size(); i++){ - // if(buckets[i].number_of_points > 5){ - // std::cout << i << " " << buckets[i].cov << std::endl; - // } - // } buckets[0].number_of_points = 0; for (auto& b : buckets) { @@ -3907,12 +3875,9 @@ bool NDT::compute_cov_mean( std::cout << "grid_calculate_params start" << std::endl; grid_calculate_params(points, rgd_params); - // for(auto &p:points){ - // std::cout << p.point; - // } + std::cout << "grid_calculate_params finished" << std::endl; - // grid_calculate_params(rgd_params, min_x, max_x, min_y, max_y, min_z, max_z); std::cout << "build_rgd start" << std::endl; build_rgd(points, index_pair, buckets, rgd_params); std::cout << "build_rgd finished" << std::endl; @@ -4001,11 +3966,6 @@ bool NDT::compute_cov_mean( } std::cout << "computing cov mean ndt_jobi finished" << std::endl; - // for(int i = 0; i < buckets.size(); i++){ - // if(buckets[i].number_of_points > 5){ - // std::cout << i << " " << buckets[i].cov << std::endl; - // } - // } buckets[0].number_of_points = 0; // int counter_active_buckets = 0; @@ -4020,7 +3980,7 @@ bool NDT::compute_cov_mean( // counter_active_buckets ++; } } - // std::cout << "counter_active_buckets: " << counter_active_buckets << std::endl; + return true; } @@ -4036,9 +3996,7 @@ bool NDT::compute_cov_mean( std::cout << "grid_calculate_params start" << std::endl; grid_calculate_params(points, rgd_params); - //for(auto &p:points){ - // std::cout << p.point; - //} + std::cout << "grid_calculate_params finished" << std::endl; //grid_calculate_params(rgd_params, min_x, max_x, min_y, max_y, min_z, max_z); @@ -4115,29 +4073,14 @@ bool NDT::compute_cov_mean( } std::cout << "computing cov mean ndt_jobi finished" << std::endl; - //for(int i = 0; i < buckets.size(); i++){ - // if(buckets[i].number_of_points > 5){ - // std::cout << i << " " << buckets[i].cov << std::endl; - // } - //} buckets[0].number_of_points = 0; - //int counter_active_buckets = 0; - /*for(auto &b:buckets){ - if(b.number_of_points < 5){ - b.number_of_points = 0; - }else{ - //counter_active_buckets ++; - } - }*/ - for (auto& [key, value] : buckets){ if(value.number_of_points < 5){ value.number_of_points = 0; } } - //std::cout << "counter_active_buckets: " << counter_active_buckets << std::endl; #endif return true; } @@ -4156,55 +4099,4 @@ void NDT::build_rgd( std::cout << "reindex start" << std::endl; reindex(points, index_pair, rgd_params, num_threads); std::cout << "reindex finished" << std::endl; - - // buckets.resize(rgd_params.number_of_buckets); - - // std::vector jobs = get_jobs(buckets.size(), num_threads); - // std::vector threads; - - // std::cout << "build_rgd_init_jobs start" << std::endl; - // for (size_t i = 0; i < jobs.size(); i++) - //{ - // threads.push_back(std::thread(build_rgd_init_job, i, &jobs[i], &buckets)); - // } - - // for (size_t j = 0; j < threads.size(); j++) - //{ - // threads[j].join(); - // } - // threads.clear(); - // std::cout << "build_rgd_init_jobs finished" << std::endl; - - // jobs = get_jobs(points.size(), num_threads); - - // std::cout << "build_rgd_jobs start jobs.size():" << jobs.size() << std::endl; - // std::cout << "points.size() " << points.size() << std::endl; - // std::cout << "index_pair.size() " << index_pair.size() << std::endl; - // std::cout << "buckets.size() " << buckets.size() << std::endl; - - // for (size_t i = 0; i < jobs.size(); i++) - //{ - // threads.push_back(std::thread(build_rgd_job, i, &jobs[i], &index_pair, &buckets)); - // } - // for (size_t j = 0; j < threads.size(); j++) - //{ - // threads[j].join(); - // } - // threads.clear(); - // std::cout << "build_rgd_jobs finished" << std::endl; - - // jobs = get_jobs(buckets.size(), num_threads); - - // std::cout << "build_rgd_final_jobs start" << std::endl; - // for (size_t i = 0; i < jobs.size(); i++) - //{ - // threads.push_back(std::thread(build_rgd_final_job, i, &jobs[i], &buckets)); - // } - - // for (size_t j = 0; j < threads.size(); j++) - //{ - // threads[j].join(); - // } - // threads.clear(); - // std::cout << "build_rgd_final_jobs finished" << std::endl; } \ No newline at end of file diff --git a/core/src/nmea.cpp b/core/src/nmea.cpp index 350965ff..4f10ae3e 100644 --- a/core/src/nmea.cpp +++ b/core/src/nmea.cpp @@ -116,7 +116,6 @@ namespace hd_mapping::nmea data.longitude = dm_to_dd(lonStr, lonDir, false); data.lon_dir = lonDir; data.altitude = fields[9].empty() ? 0.0 : std::stod(fields[9]); - // std::cout << "data.altitude " << data.altitude << std::endl; data.hdop = fields[8].empty() ? 0.0 : std::stod(fields[8]); data.fix_quality = fields[6].empty() ? 0 : std::stoi(fields[6]); data.satellites_tracked = fields[7].empty() ? 0 : std::stoi(fields[7]); diff --git a/core/src/optimization_point_to_point_source_to_target.cpp b/core/src/optimization_point_to_point_source_to_target.cpp index 533c6003..4b536c9e 100644 --- a/core/src/optimization_point_to_point_source_to_target.cpp +++ b/core/src/optimization_point_to_point_source_to_target.cpp @@ -708,8 +708,6 @@ bool ICP::optimization_point_to_point_source_to_target( for (auto& pc : point_clouds_container.point_clouds) { pc.points_type.resize(pc.points_local.size()); - // std::cout << "computing nv [" << counter_computed_nv++ << "] of " << point_clouds_container.point_clouds.size() << std::endl; - // pc.compute_normal_vectors(0.5); } } @@ -988,11 +986,6 @@ bool ICP::optimization_point_to_point_source_to_target( rms += delta(0, 0) * delta(0, 0) + delta(1, 0) * delta(1, 0) + delta(2, 0) * delta(2, 0); sum_obs += 3.0; - - // if(sqrt(delta(0, 0) * delta(0, 0) + delta(1, 0) * delta(1, 0) + delta(2, 0) * delta(2, 0)) > 0.1){ - // std::cout << sqrt(delta(0, 0) * delta(0, 0) + delta(1, 0) * delta(1, 0) + delta(2, 0) * delta(2, 0)) << - // std::endl; - // } } double curr_rms = sqrt(rms / sum_obs); @@ -1000,12 +993,6 @@ bool ICP::optimization_point_to_point_source_to_target( std::cout << "number of observations: " << sum_obs << std::endl; } } - - // if (!precompute_rgd) - //{ - // point_clouds_container.point_clouds[i].buckets.clear(); - // point_clouds_container.point_clouds[i].index_pairs.clear(); - // } } rms = sqrt(rms / sum_obs); out_rms = rms; @@ -1266,13 +1253,11 @@ bool ICP::optimization_point_to_point_source_to_target( Eigen::SparseMatrix x = solver.solve(AtPB); std::vector h_x; - // std::cout << "Solution: " << std::endl; for (int k = 0; k < x.outerSize(); ++k) { for (Eigen::SparseMatrix::InnerIterator it(x, k); it; ++it) { h_x.push_back(it.value()); - // std::cout << "col: " << it.col() << " row: " << it.row() << " value: " << it.value() << std::endl; } } @@ -1333,12 +1318,6 @@ bool ICP::optimization_point_to_point_source_to_target( { continue; } - // if (point_clouds_container.point_clouds[i].fixed) //ToDo - //{ - // std::cout << "point cloud " << i << " is fixed, continue" << std::endl; - // continue; - // } - if (pose_convention == PoseConvention::wc) { point_clouds_container.point_clouds[i].m_pose = affine_matrix_from_pose_tait_bryan(pose); @@ -1371,11 +1350,6 @@ bool ICP::optimization_point_to_point_source_to_target( { continue; } - // if (point_clouds_container.point_clouds[i].fixed) //ToDo - //{ - // std::cout << "PC: " << point_clouds_container.point_clouds[i].file_name << " is fixed" << std::endl; - // continue; - // } if (pose_convention == PoseConvention::wc) { @@ -1410,12 +1384,6 @@ bool ICP::optimization_point_to_point_source_to_target( { continue; } - // if (point_clouds_container.point_clouds[i].fixed)//ToDo - //{ - // std::cout << "PC: " << point_clouds_container.point_clouds[i].file_name << " is fixed" << std::endl; - // continue; - // } - if (pose_convention == PoseConvention::wc) { point_clouds_container.point_clouds[i].m_pose = affine_matrix_from_pose_quaternion(pose); @@ -1809,19 +1777,8 @@ std::vector> ICP::compute_covarianc bool ICP::optimize_source_to_target_lie_algebra_left_jacobian(PointClouds& point_clouds_container) { - // std::vector> covariance_matrices_before6x6; - // std::vector> covariance_matrices_after6x6; - - // covariance_matrices_before6x6 = - // compute_covariance_matrices_point_to_point_source_to_target_source_to_target_lie_algebra_left_jacobian(point_clouds_container); - + // TODO(mwlasiuk) : return ??? optimize_source_to_target_lie_algebra_left_jacobian(point_clouds_container, is_fix_first_node); - - // covariance_matrices_after6x6 = - // compute_covariance_matrices_point_to_point_source_to_target_source_to_target_lie_algebra_left_jacobian(point_clouds_container); - - // double mui = get_mean_uncertainty_xyz_impact6x6(covariance_matrices_before6x6, covariance_matrices_after6x6); - // std::cout << "mean uncertainty_xyz impact: " << mui << std::endl; return true; } @@ -1950,19 +1907,8 @@ std::vector> ICP:: bool ICP::optimize_source_to_target_lie_algebra_right_jacobian(PointClouds& point_clouds_container) { - // std::vector> covariance_matrices_before6x6; - // std::vector> covariance_matrices_after6x6; - - // covariance_matrices_before6x6 = - // compute_covariance_matrices_point_to_point_source_to_target_source_to_target_lie_algebra_right_jacobian(point_clouds_container); - + // TODO(mwlasiuk) : return ??? optimize_source_to_target_lie_algebra_right_jacobian(point_clouds_container, is_fix_first_node); - - // covariance_matrices_after6x6 = - // compute_covariance_matrices_point_to_point_source_to_target_source_to_target_lie_algebra_right_jacobian(point_clouds_container); - - // double mui = get_mean_uncertainty_xyz_impact6x6(covariance_matrices_before6x6, covariance_matrices_after6x6); - // std::cout << "mean uncertainty_xyz impact: " << mui << std::endl; return true; } diff --git a/core/src/optimize_distance_point_to_plane_source_to_target.cpp b/core/src/optimize_distance_point_to_plane_source_to_target.cpp index 6ca31960..4b8664fd 100644 --- a/core/src/optimize_distance_point_to_plane_source_to_target.cpp +++ b/core/src/optimize_distance_point_to_plane_source_to_target.cpp @@ -873,13 +873,11 @@ bool RegistrationPlaneFeature::optimize_distance_point_to_plane_source_to_target Eigen::SparseMatrix x = solver.solve(AtPB); std::vector h_x; - // std::cout << "Solution: " << std::endl; for (int k = 0; k < x.outerSize(); ++k) { for (Eigen::SparseMatrix::InnerIterator it(x, k); it; ++it) { h_x.push_back(it.value()); - // std::cout << "col: " << it.col() << " row: " << it.row() << " value: " << it.value() << std::endl; } } diff --git a/core/src/optimize_plane_to_plane_source_to_target.cpp b/core/src/optimize_plane_to_plane_source_to_target.cpp index c7317f25..f89b0bec 100644 --- a/core/src/optimize_plane_to_plane_source_to_target.cpp +++ b/core/src/optimize_plane_to_plane_source_to_target.cpp @@ -1120,13 +1120,11 @@ bool RegistrationPlaneFeature::optimize_plane_to_plane_source_to_target( Eigen::SparseMatrix x = solver.solve(AtPB); std::vector h_x; - // std::cout << "Solution: " << std::endl; for (int k = 0; k < x.outerSize(); ++k) { for (Eigen::SparseMatrix::InnerIterator it(x, k); it; ++it) { h_x.push_back(it.value()); - // std::cout << "col: " << it.col() << " row: " << it.row() << " value: " << it.value() << std::endl; } } diff --git a/core/src/optimize_point_to_plane_source_to_target.cpp b/core/src/optimize_point_to_plane_source_to_target.cpp index fd65faad..1232bf8d 100644 --- a/core/src/optimize_point_to_plane_source_to_target.cpp +++ b/core/src/optimize_point_to_plane_source_to_target.cpp @@ -1025,13 +1025,11 @@ bool RegistrationPlaneFeature::optimize_point_to_plane_source_to_target( Eigen::SparseMatrix x = solver.solve(AtPB); std::vector h_x; - // std::cout << "Solution: " << std::endl; for (int k = 0; k < x.outerSize(); ++k) { for (Eigen::SparseMatrix::InnerIterator it(x, k); it; ++it) { h_x.push_back(it.value()); - // std::cout << "col: " << it.col() << " row: " << it.row() << " value: " << it.value() << std::endl; } } diff --git a/core/src/optimize_point_to_projection_onto_plane_source_to_target.cpp b/core/src/optimize_point_to_projection_onto_plane_source_to_target.cpp index 2af18372..91933852 100644 --- a/core/src/optimize_point_to_projection_onto_plane_source_to_target.cpp +++ b/core/src/optimize_point_to_projection_onto_plane_source_to_target.cpp @@ -1070,13 +1070,11 @@ bool RegistrationPlaneFeature::optimize_point_to_projection_onto_plane_source_to Eigen::SparseMatrix x = solver.solve(AtPB); std::vector h_x; - // std::cout << "Solution: " << std::endl; for (int k = 0; k < x.outerSize(); ++k) { for (Eigen::SparseMatrix::InnerIterator it(x, k); it; ++it) { h_x.push_back(it.value()); - // std::cout << "col: " << it.col() << " row: " << it.row() << " value: " << it.value() << std::endl; } } @@ -1611,13 +1609,11 @@ bool RegistrationPlaneFeature::optimize_point_to_projection_onto_plane_source_to Eigen::SparseMatrix x = solver.solve(AtPB); std::vector h_x; - // std::cout << "Solution: " << std::endl; for (int k = 0; k < x.outerSize(); ++k) { for (Eigen::SparseMatrix::InnerIterator it(x, k); it; ++it) { h_x.push_back(it.value()); - // std::cout << "col: " << it.col() << " row: " << it.row() << " value: " << it.value() << std::endl; } } @@ -2049,13 +2045,11 @@ bool RegistrationPlaneFeature::optimize_point_to_projection_onto_plane_source_to Eigen::SparseMatrix x = solver.solve(AtPB); std::vector h_x; - // std::cout << "Solution: " << std::endl; for (int k = 0; k < x.outerSize(); ++k) { for (Eigen::SparseMatrix::InnerIterator it(x, k); it; ++it) { h_x.push_back(it.value()); - // std::cout << "col: " << it.col() << " row: " << it.row() << " value: " << it.value() << std::endl; } } diff --git a/core/src/pair_wise_iterative_closest_point.cpp b/core/src/pair_wise_iterative_closest_point.cpp index c949d9a1..f1494d66 100644 --- a/core/src/pair_wise_iterative_closest_point.cpp +++ b/core/src/pair_wise_iterative_closest_point.cpp @@ -378,7 +378,6 @@ bool PairWiseICP::compute_fast( Eigen::Affine3d& m_pose_result, int dec) { - // std::cout << "PairWiseICP::compute" << std::endl; bool multithread = true; std::vector> indexes; @@ -415,7 +414,6 @@ bool PairWiseICP::compute_fast( for (int iter = 0; iter < number_of_iterations; iter++) { - // std::cout << "iteration: " << iter + 1 << " of: " << number_of_iterations << std::endl; Eigen::MatrixXd AtPA(6, 6); AtPA.setZero(); Eigen::MatrixXd AtPB(6, 1); @@ -517,20 +515,14 @@ bool PairWiseICP::compute_fast( AtPAc = AtPA.sparseView(); AtPBc = AtPB.sparseView(); - // Eigen::SparseMatrix AtPA_I(6, 6); - // AtPA_I.setIdentity(); - // AtPA += AtPA_I; - Eigen::SimplicialCholesky> solver(AtPAc); Eigen::SparseMatrix x = solver.solve(AtPBc); std::vector h_x; for (int k = 0; k < x.outerSize(); ++k) { - // std::cout << "result pose updates" << std::endl; for (Eigen::SparseMatrix::InnerIterator it(x, k); it; ++it) { h_x.push_back(it.value()); - // std::cout << it.row() << " " << it.col() << " " << it.value() << std::endl; } } @@ -547,12 +539,10 @@ bool PairWiseICP::compute_fast( pose.ka += h_x[counter++]; m_pose_result = affine_matrix_from_pose_tait_bryan(pose); - // std::cout << "PairWiseICP::compute SUCCESS" << std::endl; // return true; } else { - // std::cout << "PairWiseICP::compute FAILED" << std::endl; return false; } } diff --git a/core/src/pfd_wrapper.cpp b/core/src/pfd_wrapper.cpp index 16231d83..0b0da44c 100644 --- a/core/src/pfd_wrapper.cpp +++ b/core/src/pfd_wrapper.cpp @@ -73,7 +73,6 @@ namespace mandeye::fd std::string output_folder_name = ""; output_folder_name = pfd::select_folder(title, internal::lastLocationHint).result(); - // std::cout << "folder: '" << output_folder_name << "'" << std::endl; return output_folder_name; } diff --git a/core/src/point_cloud.cpp b/core/src/point_cloud.cpp index 4214ed54..bbd34e35 100644 --- a/core/src/point_cloud.cpp +++ b/core/src/point_cloud.cpp @@ -37,8 +37,6 @@ bool PointCloud::load(const std::string& file_name) // Example of direct access { auto xData = data["vertex"]->properties["x"]; - // std::cout << "x value of the first vertex element:\n" << xData->at(0) << std::endl; - // std::cout << "\n"; auto yData = data["vertex"]->properties["y"]; auto zData = data["vertex"]->properties["z"]; for (size_t i = 0; i < xData->size(); i++) @@ -56,11 +54,6 @@ bool PointCloud::load(const std::string& file_name) } } } - //} - // catch (const plycpp::Exception& e) - //{ - // std::cout << "An exception happened:\n" << e.what() << std::endl; - //} #if 0 std::ifstream infile(file_name); @@ -242,26 +235,16 @@ bool PointCloud::load_pc(const std::string& input_file_name, bool load_cache_mod number_of_points_with_timestamp_eq_0++; } - // std::cout << "p.timestamp: " << p.timestamp << std::endl; - Eigen::Vector3d pp(p.x, p.y, p.z); this->points_local.emplace_back(pp); this->intensities.emplace_back(point->intensity); this->timestamps.emplace_back(p.timestamp); - // Eigen::Vector3d color( - // static_cast(0xFFU * ((point->rgb[0] > 0) ? static_cast(point->rgb[0]) / static_cast(0xFFFFU) : 1.0f)) - /// 256.0, static_cast(0xFFU * ((point->rgb[1] > 0) ? static_cast(point->rgb[1]) / static_cast(0xFFFFU) - //: 1.0f)) / 256.0, static_cast(0xFFU * ((point->rgb[2] > 0) ? static_cast(point->rgb[2]) / - // static_cast(0xFFFFU) : 1.0f)) / 256.0); - Eigen::Vector3d color( static_cast(point->rgb[0]) / 256.0, static_cast(point->rgb[1]) / 256.0, static_cast(point->rgb[2]) / 256.0); - // std::cout << point->rgb[0] << " " << point->rgb[1] << " " << point->rgb[2] << std::endl; - this->colors.push_back(color); p_count++; @@ -325,8 +308,6 @@ void build_rgd_init_job(int i, PointCloud::Job* job, std::vector* index_pair, std::vector* buckets) { - // std::cout << "build_rgd_job:[" << i << "]" << std::endl; - for (uint64_t ii = job->index_begin_inclusive; ii < job->index_end_exclusive; ii++) { uint64_t ind = ii; @@ -365,12 +346,8 @@ void build_rgd_job( void build_rgd_final_job(int i, PointCloud::Job* job, std::vector* buckets) { - // std::cout << "build_rgd_init_job:[" << i << "]" << std::endl; - for (size_t ii = job->index_begin_inclusive; ii < job->index_end_exclusive; ii++) { - // std::cout << job->index_begin_inclusive << " " << job->index_end_exclusive << " " << (*buckets).size() << std::endl; - uint64_t index_begin = (*buckets)[ii].index_begin; uint64_t index_end = (*buckets)[ii].index_end; if (index_begin != -1 && index_end != -1) @@ -382,19 +359,13 @@ void build_rgd_final_job(int i, PointCloud::Job* job, std::vectorpoints_local, this->rgd_params); cout_rgd(); - // std::cout << "grid_calculate_params done" << std::endl; - reindex(this->index_pairs, this->points_local, this->rgd_params); - // std::cout << "reindex done" << std::endl; - - // std::cout << "rgd_params.number_of_buckets: " << rgd_params.number_of_buckets << std::endl; buckets.resize(rgd_params.number_of_buckets); @@ -503,8 +474,6 @@ void reindex_job( std::vector* pairs, PointCloud::GridParameters rgd_params) { - // std::cout << "reindex_job:[" << i << "]" << std::endl; - for (size_t ii = job->index_begin_inclusive; ii < job->index_end_exclusive; ii++) { Eigen::Vector3d& p = (*points)[ii]; @@ -576,11 +545,6 @@ std::vector PointCloud::get_jobs(uint64_t size, int num_threads jobs.push_back(j); } - // std::cout << jobs.size() << " jobs; chunks: "; - // for(size_t i = 0; i < jobs.size(); i++){ - // std::cout << "("<local_trajectory[i].imu_diff_angle_om_fi_ka_deg.z() * 10); - - // std::cout << this->local_trajectory[i].imu_diff_angle_om_fi_ka_deg.x() << " " << - // this->local_trajectory[i].imu_diff_angle_om_fi_ka_deg.y() << " " << this->local_trajectory[i].imu_diff_angle_om_fi_ka_deg.z() - // << std::endl; } glEnd(); } diff --git a/core/src/point_clouds.cpp b/core/src/point_clouds.cpp index 97e5d811..c10c4bc0 100644 --- a/core/src/point_clouds.cpp +++ b/core/src/point_clouds.cpp @@ -181,10 +181,6 @@ bool PointClouds::update_poses_from_RESSO(const std::string& folder_with_point_c pc.m_pose(0, 3) = t14; pc.m_pose(1, 3) = t24; pc.m_pose(2, 3) = t34; - // pc.m_initial_pose = pc.m_pose; - - // std::cout << "update pose: " << std::endl; - // std::cout << pc.m_pose.matrix() << std::endl; pcs.push_back(pc); } @@ -198,13 +194,6 @@ bool PointClouds::update_poses_from_RESSO(const std::string& folder_with_point_c { if (std::filesystem::path(point_clouds[i].file_name).filename().string() == pcs[j].file_name) { - // std::cout << "-------------------------" << std::endl; - // std::cout << "update pose: " << i << std::endl; - // std::cout << "previous pose: " << std::endl - // << point_clouds[i].m_pose.matrix() << std::endl; - // std::cout << "current pose: " << std::endl - // << pcs[j].m_pose.matrix() << std::endl; - point_clouds[i].m_pose = pcs[j].m_pose; point_clouds[i].pose = pose_tait_bryan_from_affine_matrix(point_clouds[i].m_pose); point_clouds[i].gui_translation[0] = point_clouds[i].pose.px; @@ -213,19 +202,8 @@ bool PointClouds::update_poses_from_RESSO(const std::string& folder_with_point_c point_clouds[i].gui_rotation[0] = rad2deg(point_clouds[i].pose.om); point_clouds[i].gui_rotation[1] = rad2deg(point_clouds[i].pose.fi); point_clouds[i].gui_rotation[2] = rad2deg(point_clouds[i].pose.ka); - } // else{ - // std::cout << "std::filesystem::path(point_clouds[i].file_name).filename().string() != pcs[j].file_name" << - // std::endl; std::cout << "std::filesystem::path(point_clouds[i].file_name).filename().string(): "<< - // std::filesystem::path(point_clouds[i].file_name).filename().string() << std::endl; std::cout << - // "pcs[j].file_name: - // "<< pcs[j].file_name << std::endl; std::cout << "j: " << j << std::endl; - // return false; - //} + } } - - /**/ - - // point_clouds[i].m_initial_pose = point_clouds[i].m_pose; } } else @@ -314,13 +292,6 @@ bool PointClouds::update_poses_from_RESSO_inverse(const std::string& folder_with { if (std::filesystem::path(point_clouds[i].file_name).filename().string() == pcs[j].file_name) { - // std::cout << "-------------------------" << std::endl; - // std::cout << "update pose: " << i << std::endl; - // std::cout << "previous pose: " << std::endl - // << point_clouds[i].m_pose.matrix() << std::endl; - // std::cout << "current pose: " << std::endl - // << pcs[j].m_pose.matrix() << std::endl; - point_clouds[i].m_pose = pcs[j].m_pose; point_clouds[i].pose = pose_tait_bryan_from_affine_matrix(point_clouds[i].m_pose); point_clouds[i].gui_translation[0] = point_clouds[i].pose.px; @@ -329,19 +300,8 @@ bool PointClouds::update_poses_from_RESSO_inverse(const std::string& folder_with point_clouds[i].gui_rotation[0] = rad2deg(point_clouds[i].pose.om); point_clouds[i].gui_rotation[1] = rad2deg(point_clouds[i].pose.fi); point_clouds[i].gui_rotation[2] = rad2deg(point_clouds[i].pose.ka); - } // else{ - // std::cout << "std::filesystem::path(point_clouds[i].file_name).filename().string() != pcs[j].file_name" << - // std::endl; std::cout << "std::filesystem::path(point_clouds[i].file_name).filename().string(): "<< - // std::filesystem::path(point_clouds[i].file_name).filename().string() << std::endl; std::cout << - // "pcs[j].file_name: - // "<< pcs[j].file_name << std::endl; std::cout << "j: " << j << std::endl; - // return false; - //} + } } - - /**/ - - // point_clouds[i].m_initial_pose = point_clouds[i].m_pose; } } else @@ -425,13 +385,6 @@ bool PointClouds::update_initial_poses_from_RESSO(const std::string& folder_with { if (std::filesystem::path(point_clouds[i].file_name).filename().string() == pcs[j].file_name) { - // std::cout << "-------------------------" << std::endl; - // std::cout << "update pose: " << i << std::endl; - // std::cout << "previous pose: " << std::endl - // << point_clouds[i].m_initial_pose.matrix() << std::endl; - // std::cout << "current pose: " << std::endl - // << pcs[j].m_initial_pose.matrix() << std::endl; - point_clouds[i].m_initial_pose = pcs[j].m_initial_pose; } } @@ -1203,8 +1156,7 @@ bool PointClouds::load_pc(PointCloud& pc, std::string input_file_name, bool load static_cast(point->rgb[0]) / 256.0, static_cast(point->rgb[1]) / 256.0, static_cast(point->rgb[2]) / 256.0); - - // std::cout << point->rgb[0] << " " << point->rgb[1] << " " << point->rgb[2] << std::endl; + pc.colors.push_back(color); diff --git a/core/src/pose_graph_loop_closure.cpp b/core/src/pose_graph_loop_closure.cpp index aa015bf2..a9ad98e8 100644 --- a/core/src/pose_graph_loop_closure.cpp +++ b/core/src/pose_graph_loop_closure.cpp @@ -330,8 +330,6 @@ void PoseGraphLoopClosure::graph_slam(PointClouds& point_clouds_container, GNSS& p_t.y(), p_t.z()); - // std::cout << " delta_x " << delta_x << " delta_y " << delta_y << " delta_z " << delta_z << std::endl; - int ir = tripletListB.size(); int ic = index_pose * 6; for (int row = 0; row < 3; row++) @@ -1021,8 +1019,6 @@ void PoseGraphLoopClosure::FuseTrajectoryWithGNSS(PointClouds& point_clouds_cont p_t.y(), p_t.z()); - // std::cout << " delta_x " << delta_x << " delta_y " << delta_y << " delta_z " << delta_z << std::endl; - int ir = tripletListB.size(); int ic = 0; for (int row = 0; row < 3; row++) diff --git a/core/src/pose_graph_slam.cpp b/core/src/pose_graph_slam.cpp index 48b7fb79..12361548 100644 --- a/core/src/pose_graph_slam.cpp +++ b/core/src/pose_graph_slam.cpp @@ -990,11 +990,6 @@ bool PoseGraphSLAM::optimize(PointClouds& point_clouds_container) std::cout << "h_x.size(): " << h_x.size() << std::endl; std::cout << "AtPA=AtPB SOLVED" << std::endl; - // std::cout << "updates:" << std::endl; - // for (size_t i = 0; i < h_x.size(); i += 7) { - // std::cout << h_x[i] << "," << h_x[i + 1] << "," << h_x[i + 2] << "," << h_x[i + 3] << "," << h_x[i + 4] << "," << h_x[i + - // 5] << "," << h_x[i + 6] << std::endl; - //} if (h_x.size() == 7 * point_clouds_container.point_clouds.size()) { @@ -1063,12 +1058,6 @@ bool PoseGraphSLAM::optimize(PointClouds& point_clouds_container) } edges.clear(); - //- - // covariance_matrices_after6x6 = compute_covariance_matrices_wc(point_clouds_container); - - // double mui = get_mean_uncertainty_xyz_impact6x6(covariance_matrices_before6x6, covariance_matrices_after6x6); - // std::cout << "mean uncertainty_xyz impact: " << mui << std::endl; - //-- return true; } diff --git a/core/src/session.cpp b/core/src/session.cpp index f36688e3..025282af 100644 --- a/core/src/session.cpp +++ b/core/src/session.cpp @@ -140,11 +140,6 @@ bool Session::load(const std::string& file_name, bool is_decimate, double bucket } } - // std::cout << "------laz file names-----" << std::endl; - // for (const auto &fn : laz_file_names) - //{ - // std::cout << "'" << fn << "'" << std::endl; - // } #if WITH_GUI == 1 for (const auto& gcp_json : data["ground_control_points"]) { @@ -289,22 +284,7 @@ bool Session::load(const std::string& file_name, bool is_decimate, double bucket Eigen::Vector3d diff(fabs(tb_pose_mm.om - tb_pose.om), fabs(tb_pose_mm.fi - tb_pose.fi), fabs(tb_pose_mm.ka - tb_pose.ka)); point_clouds_container.point_clouds[i].local_trajectory[j].imu_diff_angle_om_fi_ka_deg = diff; - - // TaitBryanPose tb_pose2; - // tb_pose2.om = point_clouds_container.point_clouds[i].local_trajectory[j-1].imu_om_fi_ka.x() - - // std::cout << tb_pose.om << " " << tb_pose.fi << " " << tb_pose.ka << " " - // << point_clouds_container.point_clouds[i].local_trajectory[j].imu_om_fi_ka.x() << " " << - // point_clouds_container.point_clouds[i].local_trajectory[j].imu_om_fi_ka.y() << " " - // << point_clouds_container.point_clouds[i].local_trajectory[j].imu_om_fi_ka.z() << std::endl; } - // struct LocalTrajectoryNode{ - // std::pair timestamps; - // Eigen::Affine3d m_pose; - // Eigen::Vector3d imu_om_fi_ka; - // Eigen::Vector3d imu_diff_angle_om_fi_ka_deg; - //}; - // pc.local_trajectory } return true; diff --git a/core/src/surface.cpp b/core/src/surface.cpp index f6a1df2b..3a12ee02 100644 --- a/core/src/surface.cpp +++ b/core/src/surface.cpp @@ -90,22 +90,14 @@ void Surface::generate_initial_surface(const std::vector& point { for (double y = min_y; y <= max_y; y += surface_resolution) { - // if (number_rows == 0) - //{ - // number_cols++; - // } Eigen::Affine3d pose = Eigen::Affine3d::Identity(); pose(0, 3) = x; pose(1, 3) = y; pose(2, 3) = min_z; vertices.push_back(pose); - // std::cout << pose(0, 3) << " " << pose(1, 3) << " " << pose(2, 3) << std::endl; } - // number_rows++; } - // number_rows = floor(double(max_x - min_x) / surface_resolution); - // number_cols = floor(double(max_y - min_y) / surface_resolution); number_rows = 0; for (double x = min_x; x <= max_x; x += surface_resolution) { @@ -375,10 +367,6 @@ void Surface::align_surface_to_ground_points(const std::vector& std::cout << "AtPA=AtPB SOLVED" << std::endl; - // for(size_t i = 0 ; i < h_x.size(); i++){ - // std::cout << h_x[i] << std::endl; - // } - if (h_x.size() == 6 * vertices.size()) { int counter = 0; @@ -536,35 +524,10 @@ std::vector Surface::get_filtered_indexes( cov /= batch_of_points.size(); - // std::cout << if (sqrt(cov(2, 2)) < z_sigma_threshold) { lowest_points[index_element_source].second = true; } - - // std::cout << "cov " << cov << std::endl; - // Eigen::SelfAdjointEigenSolver eigen_solver(cov, Eigen::ComputeEigenvectors); - // points[index_element_source].eigen_values = eigen_solver.eigenvalues(); - // points[index_element_source].eigen_vectors = eigen_solver.eigenvectors(); - - // points[index_element_source].normal_vector = Eigen::Vector3d(points[index_element_source].eigen_vectors(0, 0), - // points[index_element_source].eigen_vectors(1, 0), points[index_element_source].eigen_vectors(2, 0)); - - // double ev1 = points[index_element_source].eigen_values.x(); - // double ev2 = points[index_element_source].eigen_values.y(); - // double ev3 = points[index_element_source].eigen_values.z(); - - // double sum_ev = ev1 + ev2 + ev3; - - // points[index_element_source].planarity = 1 - ((3 * ev1 / sum_ev) * (3 * ev2 / sum_ev) * (3 * ev3 / sum_ev)); - // points[index_element_source].cylindrical_likeness = (ev3 - ev2) / sum_ev; - // points[index_element_source].plane_likeness = 2 * (ev2 - ev1) / (sum_ev); - - // points[index_element_source].sphericity = ev1 / ev3; - // points[index_element_source].change_of_curvature = ev3 / (sum_ev); - // points[index_element_source].omnivariance = std::cbrt(ev1 * ev2 * ev3); - - // points[index_element_source].eigen_entropy = -ev1 * log(ev1) - ev2 * log(ev2) - ev3 * log(ev3); } else { @@ -589,20 +552,6 @@ std::vector Surface::get_filtered_indexes( } } - // - - // for (const auto &p : lowest_points){ - // std::cout << (int)p.second; - //} - - /*std::vector> indexes; - - for (int i = 0; i < lowest_points.size(); i++) - { - uint64_t index = get_rgd_index_2d(lowest_points[i], bucket_dim_xy); - indexes.emplace_back(index, i); - }*/ - return out_indexes; } @@ -672,10 +621,6 @@ std::vector Surface::get_points_without_surface( points[i].z() > z_ground + distance_to_ground_threshold_up) { to_remove[i] = true; - // std::cout << vertices[index_element_target].translation().x() << " " << vertices[index_element_target].translation().y() - // << " " - // << vertices[index_element_target].translation().z() << " " << points[i].x() << " " << points[i].y() << " " << - // points[i].z() << std::endl; } } } diff --git a/core/src/utils.cpp b/core/src/utils.cpp index 410431fb..bbc6aaf4 100644 --- a/core/src/utils.cpp +++ b/core/src/utils.cpp @@ -364,8 +364,6 @@ void keyboardDown(unsigned char key, int x, int y) // forward to ImGui GLUT backend ImGui_ImplGLUT_KeyboardFunc(key, x, y); - - // std::cout << "Down key: " << key << ", mod: " << mods << std::endl; } void keyboardUp(unsigned char key, int x, int y) @@ -385,8 +383,6 @@ void keyboardUp(unsigned char key, int x, int y) io.AddKeyEvent(keyToImGuiKey(key), false); ImGui_ImplGLUT_KeyboardUpFunc(key, x, y); - - // std::cout << "Up key: " << key << ", mod: " << mods << std::endl; } static bool first_time = true; @@ -1401,7 +1397,6 @@ void getClosestTrajectoriesPoint( if (number_visible_sessions == 1) { - // std::cout << "first_session_index " << first_session_index << std::endl; for (size_t i = 0; i < sessions[first_session_index].point_clouds_container.point_clouds.size(); i++) { for (size_t j = 0; j < sessions[first_session_index].point_clouds_container.point_clouds[i].local_trajectory.size(); j++) @@ -1450,7 +1445,6 @@ void getClosestTrajectoriesPoint( { index = second_session_index; } - // std::cout << "first_session_index " << first_session_index << std::endl; for (size_t i = 0; i < sessions[index].point_clouds_container.point_clouds.size(); i++) { for (size_t j = 0; j < sessions[index].point_clouds_container.point_clouds[i].local_trajectory.size(); j++) @@ -1484,7 +1478,6 @@ void getClosestTrajectoriesPoint( } else // (number_visible_sessions > 2) { - // std::cout << "first_session_index " << first_session_index << std::endl; for (size_t s = 0; s < sessions.size(); s++) { if (sessions[s].visible) diff --git a/core_hd_mapping/src/odo_with_gnss_fusion.cpp b/core_hd_mapping/src/odo_with_gnss_fusion.cpp index 123c5579..127c7c4b 100644 --- a/core_hd_mapping/src/odo_with_gnss_fusion.cpp +++ b/core_hd_mapping/src/odo_with_gnss_fusion.cpp @@ -253,7 +253,6 @@ std::vector OdoWithGnssFusion::load_trajectory(const std::string& file_nam break; } Node n; - // std::cout<<"size: "<> OdoWithGnssFusion::find_correspondences_from_lo bool OdoWithGnssFusion::rigid_registration(bool adaptive_robust_kernel) { - // std::cout << "rigid_registration()" << std::endl; - - // barron double min_sum_x = 1000000000000; // std::numeric_limits::max(); double min_sum_y = 1000000000000; // std::numeric_limits::max(); double min_sum_z = 1000000000000; // std::numeric_limits::max(); @@ -414,8 +410,6 @@ bool OdoWithGnssFusion::rigid_registration(bool adaptive_robust_kernel) sum_y += get_truncated_robust_kernel(delta_y, alpha, barron_c, Z_tilde); sum_z += get_truncated_robust_kernel(delta_z, alpha, barron_c, Z_tilde); - // std::cout << "sum_x: " << sum_x << " sum_y: " << sum_y << " sum_z: " << sum_z << std::endl; - if (sum_x < min_sum_x) { min_sum_x = sum_x; @@ -604,8 +598,6 @@ std::vector OdoWithGnssFusion::find_between_nodes() dist_along += dist_increment; dist_along_gnss += dist_increment; - // std::cout << "dist_increment " << dist_increment << std::endl; - if (dist_along_gnss > 10) { auto it = std::lower_bound( @@ -954,7 +946,6 @@ bool OdoWithGnssFusion::semi_rigid_registration() { for (Eigen::SparseMatrix::InnerIterator it(x, k); it; ++it) { - // std::cout << it.row() << " " << it.col() << " " << it.value() << std::endl; h_x.push_back(it.value()); } } diff --git a/core_hd_mapping/src/project_settings.cpp b/core_hd_mapping/src/project_settings.cpp index 3a8ecf08..70121caf 100644 --- a/core_hd_mapping/src/project_settings.cpp +++ b/core_hd_mapping/src/project_settings.cpp @@ -472,18 +472,7 @@ void ProjectSettings::pose_graph_slam(std::vector& rois_with { std::vector bn = find_between_nodes(trj.fused_trajectory); fuse_with_georeference(bn, constraints, trj.fused_trajectory, trj.fused_trajectory_motion_model); - // for (auto& b : bn) { - // std::cout << b.node_outer.index_to_gnss << std::endl; - //} } - /*for (auto& trj : trajectories) { - for (auto& node : trj.fused_trajectory) { - node.m_pose(2, 3) += 10; - std::cout << node.index_to_gnss << " "; - if(node.index_to_gnss != -1) - return; - } - }*/ } std::vector ProjectSettings::find_between_nodes(std::vector& fused_trajectory) @@ -508,9 +497,6 @@ std::vector ProjectSettings::find_between_nodes(std::vector& double dist_increment = (fused_trajectory[i].m_pose.translation() - fused_trajectory[i - 1].m_pose.translation()).norm(); dist_along += dist_increment; - // dist_along_gnss += dist_increment; - - // std::cout << "dist_increment " << dist_increment << std::endl; #if 0 if (dist_along_gnss > 10) { auto it = std::lower_bound(gnss_trajectory_shifted.begin(), gnss_trajectory_shifted.end(), fused_trajectory[i], @@ -805,7 +791,6 @@ void ProjectSettings::fuse_with_georeference( { for (Eigen::SparseMatrix::InnerIterator it(x, k); it; ++it) { - // std::cout << it.row() << " " << it.col() << " " << it.value() << std::endl; h_x.push_back(it.value()); } } diff --git a/core_hd_mapping/src/single_trajectory_viewer.cpp b/core_hd_mapping/src/single_trajectory_viewer.cpp index 132a2296..9468e6b3 100644 --- a/core_hd_mapping/src/single_trajectory_viewer.cpp +++ b/core_hd_mapping/src/single_trajectory_viewer.cpp @@ -108,7 +108,6 @@ std::vector SingleTrajectoryViewer::load_points_and_transform_to_global(d .norm() < 0.1) { ii += 10000; - // std::cout << "pp " << ii << " "; } } } From 51b4b512144eacbe1badddf287d7b3e816cb9da7 Mon Sep 17 00:00:00 2001 From: mwlasiuk Date: Sat, 28 Feb 2026 15:24:54 +0100 Subject: [PATCH 2/4] Converters - printf -> spdlog --- apps/console_tools/CMakeLists.txt | 14 +++--- apps/console_tools/laz_to_pcd.cpp | 16 +++--- apps/console_tools/laz_to_ply.cpp | 14 +++--- apps/console_tools/laz_to_txt.cpp | 14 +++--- ...ply_timestamps_session_point_cloud_laz.cpp | 36 -------------- apps/console_tools/pcd_to_laz.cpp | 49 +++++++++---------- 6 files changed, 55 insertions(+), 88 deletions(-) diff --git a/apps/console_tools/CMakeLists.txt b/apps/console_tools/CMakeLists.txt index 84b50a8e..0db11a6e 100644 --- a/apps/console_tools/CMakeLists.txt +++ b/apps/console_tools/CMakeLists.txt @@ -8,7 +8,7 @@ target_include_directories( matrix_mul PRIVATE include ${REPOSITORY_DIRECTORY}/core/include ${EIGEN3_INCLUDE_DIR}) -target_link_libraries(matrix_mul PRIVATE ${PLATFORM_MISCELLANEOUS_LIBS}) +target_link_libraries(matrix_mul PRIVATE ${PLATFORM_MISCELLANEOUS_LIBS} spdlog::spdlog) add_executable(multiply_timestamps_session_point_cloud_laz multiply_timestamps_session_point_cloud_laz.cpp) @@ -18,7 +18,7 @@ target_include_directories( ${EIGEN3_INCLUDE_DIR} ${LASZIP_INCLUDE_DIR}/LASzip/include) -target_link_libraries(multiply_timestamps_session_point_cloud_laz PRIVATE ${PLATFORM_MISCELLANEOUS_LIBS} ${PLATFORM_LASZIP_LIB}) +target_link_libraries(multiply_timestamps_session_point_cloud_laz PRIVATE ${PLATFORM_MISCELLANEOUS_LIBS} ${PLATFORM_LASZIP_LIB} spdlog::spdlog) add_executable(multiply_timestamps_session_trajectory_csv multiply_timestamps_session_trajectory_csv.cpp) @@ -26,20 +26,20 @@ target_include_directories( multiply_timestamps_session_trajectory_csv PRIVATE include ${REPOSITORY_DIRECTORY}/core/include ${EIGEN3_INCLUDE_DIR}) -target_link_libraries(multiply_timestamps_session_trajectory_csv PRIVATE ${PLATFORM_MISCELLANEOUS_LIBS}) +target_link_libraries(multiply_timestamps_session_trajectory_csv PRIVATE ${PLATFORM_MISCELLANEOUS_LIBS} spdlog::spdlog) add_executable(laz_to_ply laz_to_ply.cpp) -target_link_libraries(laz_to_ply PRIVATE ${PLATFORM_LASZIP_LIB}) +target_link_libraries(laz_to_ply PRIVATE ${PLATFORM_LASZIP_LIB} spdlog::spdlog) target_include_directories(laz_to_ply PRIVATE ${LASZIP_INCLUDE_DIR}/LASzip/include ${PROJECT_BINARY_DIR}/include) add_executable(laz_to_pcd laz_to_pcd.cpp) -target_link_libraries(laz_to_pcd PRIVATE ${PLATFORM_LASZIP_LIB}) +target_link_libraries(laz_to_pcd PRIVATE ${PLATFORM_LASZIP_LIB} spdlog::spdlog) target_include_directories(laz_to_pcd PRIVATE ${LASZIP_INCLUDE_DIR}/LASzip/include ${PROJECT_BINARY_DIR}/include) add_executable(pcd_to_laz pcd_to_laz.cpp) -target_link_libraries(pcd_to_laz PRIVATE ${PLATFORM_LASZIP_LIB}) +target_link_libraries(pcd_to_laz PRIVATE ${PLATFORM_LASZIP_LIB} spdlog::spdlog) target_include_directories(pcd_to_laz PRIVATE ${LASZIP_INCLUDE_DIR}/LASzip/include ${PROJECT_BINARY_DIR}/include) add_executable(laz_to_txt laz_to_txt.cpp) -target_link_libraries(laz_to_txt PRIVATE ${PLATFORM_LASZIP_LIB}) +target_link_libraries(laz_to_txt PRIVATE ${PLATFORM_LASZIP_LIB} spdlog::spdlog) target_include_directories(laz_to_txt PRIVATE ${LASZIP_INCLUDE_DIR}/LASzip/include ${PROJECT_BINARY_DIR}/include) \ No newline at end of file diff --git a/apps/console_tools/laz_to_pcd.cpp b/apps/console_tools/laz_to_pcd.cpp index ad5c0e5f..5674401b 100644 --- a/apps/console_tools/laz_to_pcd.cpp +++ b/apps/console_tools/laz_to_pcd.cpp @@ -5,11 +5,13 @@ #include #include #include -#include #include #include #include #include + +#include + namespace fs = std::filesystem; bool check_path_ext(const char* path, const char* ext) @@ -104,8 +106,8 @@ int main(const int argc, const char** argv) if (argc != expected_argc) { - std::fprintf(stderr, "Invalid argument count. Got %d expected %d.\n", argc, expected_argc); - std::fprintf(stderr, "Usage : %s \n", argv[0]); + spdlog::error("Invalid argument count. Got {} expected {}", argc, expected_argc); + spdlog::error("Usage : {} ", argv[0]); return EXIT_FAILURE; } @@ -115,28 +117,28 @@ int main(const int argc, const char** argv) if (!check_path_ext(from, expected_laz_extension)) { - std::fprintf(stderr, "Invalid extension for input file %s - expected %s\n", from, expected_laz_extension); + spdlog::error("Invalid extension for input file {} - expected {}", from, expected_laz_extension); return EXIT_FAILURE; } if (!check_path_ext(to, expected_ply_extension)) { - std::fprintf(stderr, "Invalid extension for output file %s - expected %s\n", from, expected_ply_extension); + spdlog::error("Invalid extension for output file {} - expected {}", from, expected_ply_extension); return EXIT_FAILURE; } if (!std::filesystem::exists(from)) { - std::fprintf(stderr, "Input file %s - does not exist\n", from); + spdlog::error("Input file {} - does not exist", from); return EXIT_FAILURE; } if (!convert_and_save(from, to)) { - std::fprintf(stderr, "Conversion from %s to %s failed\n", from, to); + spdlog::error("Conversion from {} to {} failed\n", from, to); return EXIT_FAILURE; } diff --git a/apps/console_tools/laz_to_ply.cpp b/apps/console_tools/laz_to_ply.cpp index c494820f..251b66a7 100644 --- a/apps/console_tools/laz_to_ply.cpp +++ b/apps/console_tools/laz_to_ply.cpp @@ -6,6 +6,8 @@ #include #include +#include + struct Point { float x = 0.0f; @@ -100,8 +102,8 @@ int main(const int argc, const char** argv) if (argc != expected_argc) { - std::fprintf(stderr, "Invalid argument count. Got %d expected %d.\n", argc, expected_argc); - std::fprintf(stderr, "Usage : %s \n", argv[0]); + spdlog::error("Invalid argument count. Got {} expected {}", argc, expected_argc); + spdlog::error("Usage : {} ", argv[0]); return EXIT_FAILURE; } @@ -111,28 +113,28 @@ int main(const int argc, const char** argv) if (!check_path_ext(from, expected_laz_extension)) { - std::fprintf(stderr, "Invalid extension for input file %s - expected %s\n", from, expected_laz_extension); + spdlog::error("Invalid extension for input file {} - expected {}", from, expected_laz_extension); return EXIT_FAILURE; } if (!check_path_ext(to, expected_ply_extension)) { - std::fprintf(stderr, "Invalid extension for output file %s - expected %s\n", from, expected_ply_extension); + spdlog::error("Invalid extension for output file {} - expected {}", from, expected_ply_extension); return EXIT_FAILURE; } if (!std::filesystem::exists(from)) { - std::fprintf(stderr, "Input file %s - does not exist\n", from); + spdlog::error("Input file {} - does not exist", from); return EXIT_FAILURE; } if (!convert_and_save(from, to)) { - std::fprintf(stderr, "Conversion from %s to %s failed\n", from, to); + spdlog::error("Conversion from {} to {} failed\n", from, to); return EXIT_FAILURE; } diff --git a/apps/console_tools/laz_to_txt.cpp b/apps/console_tools/laz_to_txt.cpp index 03406821..2de628e0 100644 --- a/apps/console_tools/laz_to_txt.cpp +++ b/apps/console_tools/laz_to_txt.cpp @@ -6,6 +6,8 @@ #include #include +#include + struct Point { float x = 0.0f; @@ -95,8 +97,8 @@ int main(const int argc, const char** argv) if (argc != expected_argc) { - std::fprintf(stderr, "Invalid argument count. Got %d expected %d.\n", argc, expected_argc); - std::fprintf(stderr, "Usage : %s \n", argv[0]); + spdlog::error("Invalid argument count. Got {} expected {}", argc, expected_argc); + spdlog::error("Usage : {} ", argv[0]); return EXIT_FAILURE; } @@ -106,28 +108,28 @@ int main(const int argc, const char** argv) if (!check_path_ext(from, expected_laz_extension)) { - std::fprintf(stderr, "Invalid extension for input file %s - expected %s\n", from, expected_laz_extension); + spdlog::error("Invalid extension for input file {} - expected {}", from, expected_laz_extension); return EXIT_FAILURE; } if (!check_path_ext(to, expected_ply_extension)) { - std::fprintf(stderr, "Invalid extension for output file %s - expected %s\n", from, expected_ply_extension); + spdlog::error("Invalid extension for output file {} - expected {}", from, expected_ply_extension); return EXIT_FAILURE; } if (!std::filesystem::exists(from)) { - std::fprintf(stderr, "Input file %s - does not exist\n", from); + spdlog::error("Input file {} - does not exist", from); return EXIT_FAILURE; } if (!convert_and_save(from, to)) { - std::fprintf(stderr, "Conversion from %s to %s failed\n", from, to); + spdlog::error("Conversion from {} to {} failed", from, to); return EXIT_FAILURE; } diff --git a/apps/console_tools/multiply_timestamps_session_point_cloud_laz.cpp b/apps/console_tools/multiply_timestamps_session_point_cloud_laz.cpp index ce31ff6d..fe25a39d 100644 --- a/apps/console_tools/multiply_timestamps_session_point_cloud_laz.cpp +++ b/apps/console_tools/multiply_timestamps_session_point_cloud_laz.cpp @@ -16,18 +16,6 @@ bool load_pc(PointCloud& pc, const std::string& input_file_name) if (laszip_create(&laszip_reader)) { fprintf(stderr, ":DLL ERROR: creating laszip reader\n"); - /*PointCloud pc; - pc.m_pose = Eigen::Affine3d::Identity(); - pc.m_initial_pose = pc.m_pose; - pc.pose = pose_tait_bryan_from_affine_matrix(pc.m_pose); - pc.gui_translation[0] = pc.pose.px; - pc.gui_translation[1] = pc.pose.py; - pc.gui_translation[2] = pc.pose.pz; - pc.gui_rotation[0] = rad2deg(pc.pose.om); - pc.gui_rotation[1] = rad2deg(pc.pose.fi); - pc.gui_rotation[2] = rad2deg(pc.pose.ka); - pc.file_name = input_file_names[i]; - point_clouds.push_back(pc);*/ return false; } @@ -35,18 +23,6 @@ bool load_pc(PointCloud& pc, const std::string& input_file_name) if (laszip_open_reader(laszip_reader, input_file_name.c_str(), &is_compressed)) { fprintf(stderr, ":DLL ERROR: opening laszip reader for '%s'\n", input_file_name.c_str()); - /*PointCloud pc; - pc.m_pose = Eigen::Affine3d::Identity(); - pc.m_initial_pose = pc.m_pose; - pc.pose = pose_tait_bryan_from_affine_matrix(pc.m_pose); - pc.gui_translation[0] = pc.pose.px; - pc.gui_translation[1] = pc.pose.py; - pc.gui_translation[2] = pc.pose.pz; - pc.gui_rotation[0] = rad2deg(pc.pose.om); - pc.gui_rotation[1] = rad2deg(pc.pose.fi); - pc.gui_rotation[2] = rad2deg(pc.pose.ka); - pc.file_name = input_file_names[i]; - point_clouds.push_back(pc);*/ return false; } laszip_header* header; @@ -54,18 +30,6 @@ bool load_pc(PointCloud& pc, const std::string& input_file_name) if (laszip_get_header_pointer(laszip_reader, &header)) { fprintf(stderr, ":DLL ERROR: getting header pointer from laszip reader\n"); - /*PointCloud pc; - pc.m_pose = Eigen::Affine3d::Identity(); - pc.m_initial_pose = pc.m_pose; - pc.pose = pose_tait_bryan_from_affine_matrix(pc.m_pose); - pc.gui_translation[0] = pc.pose.px; - pc.gui_translation[1] = pc.pose.py; - pc.gui_translation[2] = pc.pose.pz; - pc.gui_rotation[0] = rad2deg(pc.pose.om); - pc.gui_rotation[1] = rad2deg(pc.pose.fi); - pc.gui_rotation[2] = rad2deg(pc.pose.ka); - pc.file_name = input_file_names[i]; - point_clouds.push_back(pc);*/ return false; } diff --git a/apps/console_tools/pcd_to_laz.cpp b/apps/console_tools/pcd_to_laz.cpp index 1a4a3677..e95a2d74 100644 --- a/apps/console_tools/pcd_to_laz.cpp +++ b/apps/console_tools/pcd_to_laz.cpp @@ -5,12 +5,13 @@ #include #include #include -#include #include #include #include #include +#include + namespace fs = std::filesystem; bool check_path_ext(const char* path, const char* ext) @@ -103,7 +104,7 @@ bool convert_and_save(const char* from, const char* to) std::ifstream pcd_file(from, std::ios::binary); if (!pcd_file.is_open()) { - std::fprintf(stderr, "Failed to open input file %s\n", from); + spdlog::error("Failed to open input file {}", from); return false; } @@ -111,7 +112,7 @@ bool convert_and_save(const char* from, const char* to) size_t data_offset = 0; if (!read_pcd_header(pcd_file, pcd_header, data_offset)) { - std::fprintf(stderr, "Failed to read PCD header from file %s\n", from); + spdlog::error("Failed to read PCD header from file {}", from); return false; } @@ -132,21 +133,21 @@ bool convert_and_save(const char* from, const char* to) if (x_offset < 0 || y_offset < 0 || z_offset < 0) { - std::fprintf(stderr, "PCD file must contain x, y, z fields\n"); + spdlog::error("PCD file must contain x, y, z fields"); return false; } - std::fprintf(stderr, "PCD Header: %d points, point record length: %d bytes\n", pcd_header.points, pcd_header.point_record_length); - std::fprintf( - stderr, "Fields: x_offset=%d, y_offset=%d, z_offset=%d, intensity_offset=%d\n", x_offset, y_offset, z_offset, intensity_offset); + spdlog::error("PCD Header: {} points, point record length: {} bytes", pcd_header.points, pcd_header.point_record_length); + spdlog::error("Fields: x_offset={}, y_offset={}, z_offset={}, intensity_offset={}", x_offset, y_offset, z_offset, intensity_offset); // Read all point data std::vector pointsBinary(pcd_header.points * pcd_header.point_record_length); pcd_file.read((char*)pointsBinary.data(), pointsBinary.size()); if (pcd_file.gcount() != static_cast(pointsBinary.size())) { - std::fprintf(stderr, "Failed to read point data from file %s\n", from); - std::fprintf(stderr, "Expected %zu bytes, got %zd bytes\n", pointsBinary.size(), pcd_file.gcount()); + spdlog::error("Failed to read point data from file {}", from); + spdlog::error("Expected {} bytes, got {} bytes", pointsBinary.size(), pcd_file.gcount()); + return false; } @@ -154,7 +155,7 @@ bool convert_and_save(const char* from, const char* to) laszip_POINTER laszip_writer = nullptr; if (laszip_create(&laszip_writer)) { - std::fprintf(stderr, "Failed to create LAZ writer\n"); + spdlog::error("Failed to create LAZ writer"); return false; } @@ -206,14 +207,14 @@ bool convert_and_save(const char* from, const char* to) if (laszip_set_header(laszip_writer, &laszip_hdr)) { laszip_destroy(laszip_writer); - std::fprintf(stderr, "Failed to set LAZ header\n"); + spdlog::error("Failed to set LAZ header"); return false; } if (laszip_open_writer(laszip_writer, to, 1)) { laszip_destroy(laszip_writer); - std::fprintf(stderr, "Failed to open LAZ writer for file %s\n", to); + spdlog::error("Failed to open LAZ writer for file {}", to); return false; } @@ -223,7 +224,7 @@ bool convert_and_save(const char* from, const char* to) { laszip_close_writer(laszip_writer); laszip_destroy(laszip_writer); - std::fprintf(stderr, "Failed to get LAZ point pointer\n"); + spdlog::error("Failed to get LAZ point pointer"); return false; } @@ -271,7 +272,7 @@ bool convert_and_save(const char* from, const char* to) { laszip_close_writer(laszip_writer); laszip_destroy(laszip_writer); - std::fprintf(stderr, "Failed to write point %d to LAZ file %s\n", i, to); + spdlog::error("Failed to write point {} to LAZ file {}", i, to); return false; } } @@ -279,12 +280,12 @@ bool convert_and_save(const char* from, const char* to) if (laszip_close_writer(laszip_writer)) { laszip_destroy(laszip_writer); - std::fprintf(stderr, "Failed to close LAZ writer for file %s\n", to); + spdlog::error("Failed to close LAZ writer for file {}", to); return false; } laszip_destroy(laszip_writer); - std::fprintf(stderr, "Successfully converted %d points\n", pcd_header.points); + spdlog::error("Successfully converted {} points\n", pcd_header.points); return true; } @@ -297,8 +298,8 @@ int main(const int argc, const char** argv) if (argc != expected_argc) { - std::fprintf(stderr, "Invalid argument count. Got %d expected %d.\n", argc, expected_argc); - std::fprintf(stderr, "Usage : %s \n", argv[0]); + spdlog::error("Invalid argument count. Got {} expected {}", argc, expected_argc); + spdlog::error("Usage : {} ", argv[0]); return EXIT_FAILURE; } @@ -308,29 +309,25 @@ int main(const int argc, const char** argv) if (!check_path_ext(from, expected_pcd_extension)) { - std::fprintf(stderr, "Invalid extension for input file %s - expected %s\n", from, expected_pcd_extension); - + spdlog::error("Invalid extension for input file {} - expected {}", from, expected_pcd_extension); return EXIT_FAILURE; } if (!check_path_ext(to, expected_laz_extension)) { - std::fprintf(stderr, "Invalid extension for output file %s - expected %s\n", to, expected_laz_extension); - + spdlog::error("Invalid extension for output file {} - expected {}", to, expected_laz_extension); return EXIT_FAILURE; } if (!std::filesystem::exists(from)) { - std::fprintf(stderr, "Input file %s - does not exist\n", from); - + spdlog::error("Input file {} - does not exist", from); return EXIT_FAILURE; } if (!convert_and_save(from, to)) { - std::fprintf(stderr, "Conversion from %s to %s failed\n", from, to); - + spdlog::error("Conversion from {} to {} failed\n", from, to); return EXIT_FAILURE; } From 43644067019fa1a1209a8bd8f420cd1214ffe3df Mon Sep 17 00:00:00 2001 From: mwlasiuk Date: Sat, 28 Feb 2026 16:42:06 +0100 Subject: [PATCH 3/4] Core + apps : printf -> spdlog --- ...ply_timestamps_session_point_cloud_laz.cpp | 44 ++-------- .../lidar_odometry_utils.cpp | 44 ++-------- .../livox_mid_360_intrinsic_calibration.cpp | 17 ++-- .../mandeye_mission_recorder_calibration.cpp | 11 ++- .../perform_experiment.cpp | 24 +++--- apps/split_multi_livox/CMakeLists.txt | 2 +- apps/split_multi_livox/split_multi_livox.cpp | 17 ++-- core/CMakeLists.txt | 2 +- core/include/point_clouds.h | 2 +- core/src/color_las_loader.cpp | 37 ++++---- core/src/gnss.cpp | 36 +++----- core/src/point_cloud.cpp | 72 ++-------------- core/src/point_clouds.cpp | 86 +++---------------- core_hd_mapping/CMakeLists.txt | 1 + core_hd_mapping/src/laz_wrapper.cpp | 18 ++-- 15 files changed, 120 insertions(+), 293 deletions(-) diff --git a/apps/console_tools/multiply_timestamps_session_point_cloud_laz.cpp b/apps/console_tools/multiply_timestamps_session_point_cloud_laz.cpp index fe25a39d..e37de472 100644 --- a/apps/console_tools/multiply_timestamps_session_point_cloud_laz.cpp +++ b/apps/console_tools/multiply_timestamps_session_point_cloud_laz.cpp @@ -10,35 +10,35 @@ #include +#include + bool load_pc(PointCloud& pc, const std::string& input_file_name) { laszip_POINTER laszip_reader; if (laszip_create(&laszip_reader)) { - fprintf(stderr, ":DLL ERROR: creating laszip reader\n"); + spdlog::error(":DLL ERROR: creating laszip reader"); return false; } laszip_BOOL is_compressed = 0; if (laszip_open_reader(laszip_reader, input_file_name.c_str(), &is_compressed)) { - fprintf(stderr, ":DLL ERROR: opening laszip reader for '%s'\n", input_file_name.c_str()); + spdlog::error(":DLL ERROR: opening laszip reader for '{}'", input_file_name); return false; } laszip_header* header; if (laszip_get_header_pointer(laszip_reader, &header)) { - fprintf(stderr, ":DLL ERROR: getting header pointer from laszip reader\n"); + spdlog::error(":DLL ERROR: getting header pointer from laszip reader"); return false; } - // fprintf(stderr, "file '%s' contains %u points\n", input_file_name.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"); + spdlog::error(":DLL ERROR: getting point pointer from laszip reader"); return false; } @@ -52,28 +52,6 @@ bool load_pc(PointCloud& pc, const std::string& input_file_name) pc.gui_rotation[1] = rad2deg(pc.pose.fi); pc.gui_rotation[2] = rad2deg(pc.pose.ka); - /*for (int 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 true; - // continue; - } - - LAZPoint p; - p.x = header->x_offset + header->x_scale_factor * static_cast(point->X); - p.y = header->y_offset + header->y_scale_factor * static_cast(point->Y); - p.z = header->z_offset + header->z_scale_factor * static_cast(point->Z); - p.timestamp = point->gps_time; - - Eigen::Vector3d pp(p.x, p.y, p.z); - pc.points_local.push_back(pp); - pc.intensities.push_back(point->intensity); - pc.timestamps.push_back(p.timestamp); - }*/ - laszip_I64 npoints = (header->number_of_point_records ? header->number_of_point_records : header->extended_number_of_point_records); std::cout << (is_compressed ? "" : "un") << "compressed file '" << (std::filesystem::path(input_file_name).filename().string()) @@ -85,7 +63,7 @@ bool load_pc(PointCloud& pc, const std::string& input_file_name) { if (laszip_read_point(laszip_reader)) { - fprintf(stderr, "DLL ERROR: reading point %I64d\n", p_count); + spdlog::error("DLL ERROR: reading point {}", p_count); laszip_close_reader(laszip_reader); return false; } @@ -101,12 +79,6 @@ bool load_pc(PointCloud& pc, const std::string& input_file_name) pc.intensities.push_back(point->intensity); pc.timestamps.push_back(p.timestamp); - // Eigen::Vector3d color( - // static_cast(0xFFU * ((point->rgb[0] > 0) ? static_cast(point->rgb[0]) / static_cast(0xFFFFU) : 1.0f)) - /// 256.0, static_cast(0xFFU * ((point->rgb[1] > 0) ? static_cast(point->rgb[1]) / static_cast(0xFFFFU) - //: 1.0f)) / 256.0, static_cast(0xFFU * ((point->rgb[2] > 0) ? static_cast(point->rgb[2]) / - // static_cast(0xFFFFU) : 1.0f)) / 256.0); - Eigen::Vector3d color( static_cast(point->rgb[0]) / 256.0, static_cast(point->rgb[1]) / 256.0, @@ -118,8 +90,6 @@ bool load_pc(PointCloud& pc, const std::string& input_file_name) } laszip_close_reader(laszip_reader); - // laszip_clean(laszip_reader); - // laszip_destroy(laszip_reader); return true; } diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp b/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp index 01fcd033..44512477 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp @@ -7,6 +7,8 @@ #include #include +#include + namespace fs = std::filesystem; std::vector decimate(const std::vector& points, double bucket_x, double bucket_y, double bucket_z) @@ -465,14 +467,14 @@ std::vector load_point_cloud( laszip_POINTER laszip_reader; if (laszip_create(&laszip_reader)) { - fprintf(stderr, "DLL ERROR: creating laszip reader\n"); + spdlog::error("DLL ERROR: creating laszip reader"); 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()); + spdlog::error("DLL ERROR: opening laszip reader for '{}'\n", lazFile); std::abort(); } @@ -480,14 +482,14 @@ std::vector load_point_cloud( if (laszip_get_header_pointer(laszip_reader, &header)) { - fprintf(stderr, "DLL ERROR: getting header pointer from laszip reader\n"); + spdlog::error("DLL ERROR: getting header pointer from laszip reader"); std::abort(); } laszip_point* point; if (laszip_get_point_pointer(laszip_reader, &point)) { - fprintf(stderr, "DLL ERROR: getting point pointer from laszip reader\n"); + spdlog::error("DLL ERROR: getting point pointer from laszip reader"); std::abort(); } @@ -498,7 +500,7 @@ std::vector load_point_cloud( { if (laszip_read_point(laszip_reader)) { - fprintf(stderr, "DLL ERROR: reading point %u\n", j); + spdlog::error("DLL ERROR: reading point {}", j); laszip_close_reader(laszip_reader); return points; // std::abort(); @@ -527,44 +529,12 @@ std::vector load_point_cloud( p.timestamp = point->gps_time; p.intensity = point->intensity; - // add z correction - // if (p.point.z() > 0) - //{ - // double dist = sqrt(p.point.x() * p.point.x() + p.point.y() * p.point.y()); - // double correction = dist * asin(0.08 / 10.0); - - // p.point.z() += correction; - //} - /*if (p.point.z() > 0) - { - double dist = sqrt(p.point.x() * p.point.x() + p.point.y() * p.point.y()); - double correction = 0;//dist * asin(0.08 / 10.0); - - if (dist < 11.0){ - correction = 0.005; - }else{ - correction = -0.015; - } - - p.point.z() += correction; - }*/ - if (p.timestamp == 0 && ommit_points_with_timestamp_equals_zero) { counter_ts0++; } else { - /* underground mining - if (sqrt(pf.x() * pf.x()) < 4.5 && sqrt(pf.y() * pf.y()) < 2){ - counter_filtered_points++; - }else{ - - - points.emplace_back(p); - } - */ - if (sqrt(pf.x() * pf.x() + pf.y() * pf.y()) > filter_threshold_xy_inner && sqrt(pf.x() * pf.x() + pf.y() * pf.y()) < filter_threshold_xy_outer) { 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 index d4f2d1f3..2a9f677d 100644 --- 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 @@ -32,6 +32,8 @@ #include #include +#include + #define SAMPLE_PERIOD (1.0 / 200.0) namespace fs = std::filesystem; @@ -191,14 +193,14 @@ std::vector load_pc(const std::string& lazFile) laszip_POINTER laszip_reader; if (laszip_create(&laszip_reader)) { - fprintf(stderr, "DLL ERROR: creating laszip reader\n"); + spdlog::error("DLL ERROR: creating laszip reader"); 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()); + spdlog::error("DLL ERROR: opening laszip reader for '{}'", lazFile.c_str()); std::abort(); } std::cout << "compressed : " << is_compressed << std::endl; @@ -206,14 +208,16 @@ std::vector load_pc(const std::string& lazFile) if (laszip_get_header_pointer(laszip_reader, &header)) { - fprintf(stderr, "DLL ERROR: getting header pointer from laszip reader\n"); + spdlog::error("DLL ERROR: getting header pointer from laszip reader"); std::abort(); } - fprintf(stderr, "file '%s' contains %u points\n", lazFile.c_str(), header->number_of_point_records); + + spdlog::info("file '{}' contains {} points", 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"); + spdlog::error("DLL ERROR: getting point pointer from laszip reader"); std::abort(); } @@ -226,7 +230,7 @@ std::vector load_pc(const std::string& lazFile) { if (laszip_read_point(laszip_reader)) { - fprintf(stderr, "DLL ERROR: reading point %u\n", j); + spdlog::error("DLL ERROR: reading point {}", j); laszip_close_reader(laszip_reader); return points; // std::abort(); @@ -235,7 +239,6 @@ std::vector load_pc(const std::string& lazFile) 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), 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 f809d404..c3880bb9 100644 --- a/apps/mandeye_mission_recorder_calibration/mandeye_mission_recorder_calibration.cpp +++ b/apps/mandeye_mission_recorder_calibration/mandeye_mission_recorder_calibration.cpp @@ -34,9 +34,10 @@ #include "resource.h" #include // <-- Required for ShellExecuteA #include - #endif +#include + /////////////////////////////////////////////////////////////////////////////////// std::string winTitle = std::string("MR calibration ") + HDMAPPING_VERSION_STRING; @@ -105,14 +106,14 @@ void load_pc( laszip_POINTER laszip_reader; if (laszip_create(&laszip_reader)) { - fprintf(stderr, "DLL ERROR: creating laszip reader\n"); + spdlog::error("DLL ERROR: creating laszip reader"); 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()); + spdlog::error("DLL ERROR: opening laszip reader for '{}'", lazFile); std::abort(); } spdlog::info("Compressed : {}", is_compressed); @@ -123,7 +124,9 @@ void load_pc( spdlog::error("DLL ERROR: getting header pointer from laszip reader"); std::abort(); } - fprintf(stderr, "File '%s' contains %u points\n", lazFile.c_str(), header->number_of_point_records); + + spdlog::info("File '{}' contains {} points", lazFile, header->number_of_point_records); + laszip_point* point; if (laszip_get_point_pointer(laszip_reader, &point)) { diff --git a/apps/multi_view_tls_registration/perform_experiment.cpp b/apps/multi_view_tls_registration/perform_experiment.cpp index c97751a1..f2b4601f 100644 --- a/apps/multi_view_tls_registration/perform_experiment.cpp +++ b/apps/multi_view_tls_registration/perform_experiment.cpp @@ -37,6 +37,8 @@ #include #include +#include + namespace fs = std::filesystem; void export_result_to_folder(std::string output_folder_name, ObservationPicking& observation_picking, Session& session) @@ -2162,7 +2164,7 @@ bool exportLaz(const std::string &filename, laszip_POINTER laszip_writer; if (laszip_create(&laszip_writer)) { - fprintf(stderr, "DLL ERROR: creating laszip writer\n"); + spdlog::error("DLL ERROR: creating laszip writer"); return false; } @@ -2172,7 +2174,7 @@ bool exportLaz(const std::string &filename, if (laszip_get_header_pointer(laszip_writer, &header)) { - fprintf(stderr, "DLL ERROR: getting header pointer from laszip writer\n"); + spdlog::error("DLL ERROR: getting header pointer from laszip writer"); return false; } @@ -2211,18 +2213,18 @@ bool exportLaz(const std::string &filename, if (laszip_open_writer(laszip_writer, filename.c_str(), compress)) { - fprintf(stderr, "DLL ERROR: opening laszip writer for '%s'\n", filename.c_str()); + spdlog::error("DLL ERROR: opening laszip writer for '{}'", filename ); return false; } - fprintf(stderr, "writing file '%s' %scompressed\n", filename.c_str(), (compress ? "" : "un")); + spdlog::info("writing file '{}' {}compressed", filename, (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"); + spdlog::error("DLL ERROR: getting point pointer from laszip writer"); return false; } @@ -2240,7 +2242,7 @@ bool exportLaz(const std::string &filename, 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); + spdlog::error("DLL ERROR: setting coordinates for point {}", p_count); return false; } @@ -2253,24 +2255,24 @@ bool exportLaz(const std::string &filename, if (laszip_write_point(laszip_writer)) { - fprintf(stderr, "DLL ERROR: writing point %I64d\n", p_count); + spdlog::error("DLL ERROR: writing point {}", p_count); return false; } } if (laszip_get_point_count(laszip_writer, &p_count)) { - fprintf(stderr, "DLL ERROR: getting point count\n"); + spdlog::error("DLL ERROR: getting point count"); return false; } - fprintf(stderr, "successfully written %I64d points\n", p_count); + spdlog::info("successfully written {} points", p_count); // close the writer if (laszip_close_writer(laszip_writer)) { - fprintf(stderr, "DLL ERROR: closing laszip writer\n"); + spdlog::error("DLL ERROR: closing laszip writer"); return false; } @@ -2278,7 +2280,7 @@ bool exportLaz(const std::string &filename, if (laszip_destroy(laszip_writer)) { - fprintf(stderr, "DLL ERROR: destroying laszip writer\n"); + spdlog::error("DLL ERROR: destroying laszip writer"); return false; } diff --git a/apps/split_multi_livox/CMakeLists.txt b/apps/split_multi_livox/CMakeLists.txt index 6712d274..4d9853a8 100644 --- a/apps/split_multi_livox/CMakeLists.txt +++ b/apps/split_multi_livox/CMakeLists.txt @@ -23,7 +23,7 @@ target_include_directories( ${FREEGLUT_INCLUDE_DIR}) target_link_libraries(split_multi_livox PRIVATE core ${PLATFORM_LASZIP_LIB} - ${PLATFORM_MISCELLANEOUS_LIBS}) + ${PLATFORM_MISCELLANEOUS_LIBS} spdlog::spdlog) if(WIN32) add_custom_command( diff --git a/apps/split_multi_livox/split_multi_livox.cpp b/apps/split_multi_livox/split_multi_livox.cpp index a8f50406..406b35e3 100644 --- a/apps/split_multi_livox/split_multi_livox.cpp +++ b/apps/split_multi_livox/split_multi_livox.cpp @@ -7,6 +7,8 @@ #include #include +#include + struct Point3Dis { Eigen::Vector3d point; @@ -28,14 +30,14 @@ std::vector load_point_cloud(const std::string& lazFile, bool ommit_p laszip_POINTER laszip_reader; if (laszip_create(&laszip_reader)) { - fprintf(stderr, "DLL ERROR: creating laszip reader\n"); + spdlog::error("DLL ERROR: creating laszip reader"); 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()); + spdlog::error("DLL ERROR: opening laszip reader for '{}'", lazFile); std::abort(); } std::cout << "compressed : " << is_compressed << std::endl; @@ -43,14 +45,16 @@ std::vector load_point_cloud(const std::string& lazFile, bool ommit_p if (laszip_get_header_pointer(laszip_reader, &header)) { - fprintf(stderr, "DLL ERROR: getting header pointer from laszip reader\n"); + spdlog::error("DLL ERROR: getting header pointer from laszip reader"); std::abort(); } - fprintf(stderr, "file '%s' contains %u points\n", lazFile.c_str(), header->number_of_point_records); + + spdlog::info("file '{}' contains {} points", lazFile, 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"); + spdlog::error("DLL ERROR: getting point pointer from laszip reader"); std::abort(); } @@ -60,10 +64,9 @@ std::vector load_point_cloud(const std::string& lazFile, bool ommit_p { if (laszip_read_point(laszip_reader)) { - fprintf(stderr, "DLL ERROR: reading point %u\n", j); + spdlog::error("DLL ERROR: reading point {}", j); laszip_close_reader(laszip_reader); return points; - // std::abort(); } Point3Dil p; diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index c41a0887..a503e255 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -44,7 +44,7 @@ function(add_core_target target_name with_gui) add_library(${target_name} STATIC ${SOURCES}) target_compile_definitions(${target_name} PRIVATE ${DEFINES}) - target_link_libraries(${target_name} PRIVATE ${PLATFORM_LASZIP_LIB} ${PLATFORM_MISCELLANEOUS_LIBS}) + target_link_libraries(${target_name} PRIVATE ${PLATFORM_LASZIP_LIB} ${PLATFORM_MISCELLANEOUS_LIBS} spdlog::spdlog) target_include_directories(${target_name} PRIVATE include ${EIGEN3_INCLUDE_DIR} diff --git a/core/include/point_clouds.h b/core/include/point_clouds.h index aa94c2b5..f8c1ab78 100644 --- a/core/include/point_clouds.h +++ b/core/include/point_clouds.h @@ -102,7 +102,7 @@ class PointClouds void print_point_cloud_dimension(); bool load_3DTK_tls(std::vector input_file_names, bool is_decimate, double bucket_x, double bucket_y, double bucket_z); - bool load_pc(PointCloud& pc, std::string input_file_name, bool load_cache_mode); + bool load_pc(PointCloud& pc, const std::string& input_file_name, bool load_cache_mode); }; double get_mean_uncertainty_xyz_impact6x6( diff --git a/core/src/color_las_loader.cpp b/core/src/color_las_loader.cpp index 12440961..3edcd8ba 100644 --- a/core/src/color_las_loader.cpp +++ b/core/src/color_las_loader.cpp @@ -2,20 +2,22 @@ #include "color_las_loader.h" +#include + std::vector mandeye::load(const std::string& lazFile) { std::vector points; laszip_POINTER laszip_reader; if (laszip_create(&laszip_reader)) { - fprintf(stderr, "DLL ERROR: creating laszip reader\n"); + spdlog::error("DLL ERROR: creating laszip reader"); 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()); + spdlog::error("DLL ERROR: opening laszip reader for '{}'", lazFile); std::abort(); } std::cout << "compressed : " << is_compressed << std::endl; @@ -23,19 +25,19 @@ std::vector mandeye::load(const std::string& lazFile) if (laszip_get_header_pointer(laszip_reader, &header)) { - fprintf(stderr, "DLL ERROR: getting header pointer from laszip reader\n"); + spdlog::error("DLL ERROR: getting header pointer from laszip reader"); std::abort(); } laszip_I64 npoints = (header->number_of_point_records ? header->number_of_point_records : header->extended_number_of_point_records); - fprintf(stderr, "file '%s' contains %I64d points\n", lazFile.c_str(), npoints); + spdlog::info("file '{}' contains {} points", lazFile, npoints); laszip_point* point; if (laszip_get_point_pointer(laszip_reader, &point)) { - fprintf(stderr, "DLL ERROR: getting point pointer from laszip reader\n"); + spdlog::error("DLL ERROR: getting point pointer from laszip reader"); } laszip_I64 p_count = 0; @@ -44,8 +46,9 @@ std::vector mandeye::load(const std::string& lazFile) { if (laszip_read_point(laszip_reader)) { - fprintf(stderr, "DLL ERROR: reading point %I64d\n", p_count); + spdlog::error("DLL ERROR: reading point {}", p_count); } + Point p; p.point.x() = header->x_offset + header->x_scale_factor * static_cast(point->X); p.point.y() = header->y_offset + header->y_scale_factor * static_cast(point->Y); @@ -93,7 +96,7 @@ bool mandeye::saveLaz(const std::string& filename, const std::vector +#include + inline void split(std::string& str, char delim, std::vector& out) { size_t start; @@ -433,22 +435,18 @@ bool GNSS::save_to_laz(const std::string& output_file_names, double offset_x, do laszip_POINTER laszip_writer; if (laszip_create(&laszip_writer)) { - fprintf(stderr, "DLL ERROR: creating laszip writer\n"); + spdlog::error("DLL ERROR: creating laszip writer"); 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"); + spdlog::error("DLL ERROR: getting header pointer from laszip writer"); return false; } - // populate the header - header->file_source_ID = 4711; header->global_encoding = (1 << 0); // see LAS specification for details header->version_major = 1; @@ -476,24 +474,20 @@ bool GNSS::save_to_laz(const std::string& output_file_names, double offset_x, do 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(output_file_names.c_str(), ".laz") != 0); if (laszip_open_writer(laszip_writer, output_file_names.c_str(), compress)) { - fprintf(stderr, "DLL ERROR: opening laszip writer for '%s'\n", output_file_names.c_str()); + spdlog::error("DLL ERROR: opening laszip writer for '{}'", output_file_names); return false; } - fprintf(stderr, "writing file '%s' %scompressed\n", output_file_names.c_str(), (compress ? "" : "un")); - - // get a pointer to the point of the writer that we will populate and write + spdlog::info("writing file '{}' {}compressed\n", output_file_names, (compress ? "" : "un")); laszip_point* point; if (laszip_get_point_pointer(laszip_writer, &point)) { - fprintf(stderr, "DLL ERROR: getting point pointer from laszip writer\n"); + spdlog::error("DLL ERROR: getting point pointer from laszip writer"); return false; } @@ -511,38 +505,34 @@ bool GNSS::save_to_laz(const std::string& output_file_names, double offset_x, do coordinates[2] = p.alt; if (laszip_set_coordinates(laszip_writer, coordinates)) { - fprintf(stderr, "DLL ERROR: setting coordinates for point %I64d\n", p_count); + spdlog::error("DLL ERROR: setting coordinates for point {}", p_count); return false; } if (laszip_write_point(laszip_writer)) { - fprintf(stderr, "DLL ERROR: writing point %I64d\n", p_count); + spdlog::error("DLL ERROR: writing point {}", p_count); return false; } } if (laszip_get_point_count(laszip_writer, &p_count)) { - fprintf(stderr, "DLL ERROR: getting point count\n"); + spdlog::error("DLL ERROR: getting point count"); return false; } - fprintf(stderr, "successfully written %I64d points\n", p_count); - - // close the writer + spdlog::info("successfully written {} points", p_count); if (laszip_close_writer(laszip_writer)) { - fprintf(stderr, "DLL ERROR: closing laszip writer\n"); + spdlog::error("DLL ERROR: closing laszip writer"); return false; } - // destroy the writer - if (laszip_destroy(laszip_writer)) { - fprintf(stderr, "DLL ERROR: destroying laszip writer\n"); + spdlog::error("DLL ERROR: destroying laszip writer"); return false; } diff --git a/core/src/point_cloud.cpp b/core/src/point_cloud.cpp index bbd34e35..da5be209 100644 --- a/core/src/point_cloud.cpp +++ b/core/src/point_cloud.cpp @@ -7,6 +7,8 @@ #include +#include + bool PointCloud::load(const std::string& file_name) { points_local.clear(); @@ -92,66 +94,30 @@ bool PointCloud::load_pc(const std::string& input_file_name, bool load_cache_mod laszip_POINTER laszip_reader; if (laszip_create(&laszip_reader)) { - fprintf(stderr, ":DLL ERROR: creating laszip reader\n"); - /*PointCloud pc; - pc.m_pose = Eigen::Affine3d::Identity(); - pc.m_initial_pose = pc.m_pose; - pc.pose = pose_tait_bryan_from_affine_matrix(pc.m_pose); - pc.gui_translation[0] = pc.pose.px; - pc.gui_translation[1] = pc.pose.py; - pc.gui_translation[2] = pc.pose.pz; - pc.gui_rotation[0] = rad2deg(pc.pose.om); - pc.gui_rotation[1] = rad2deg(pc.pose.fi); - pc.gui_rotation[2] = rad2deg(pc.pose.ka); - pc.file_name = input_file_names[i]; - point_clouds.push_back(pc);*/ + spdlog::error(":DLL ERROR: creating laszip reader"); return false; } laszip_BOOL is_compressed = 0; if (laszip_open_reader(laszip_reader, input_file_name.c_str(), &is_compressed)) { - fprintf(stderr, ":DLL ERROR: opening laszip reader for '%s'\n", input_file_name.c_str()); - /*PointCloud pc; - pc.m_pose = Eigen::Affine3d::Identity(); - pc.m_initial_pose = pc.m_pose; - pc.pose = pose_tait_bryan_from_affine_matrix(pc.m_pose); - pc.gui_translation[0] = pc.pose.px; - pc.gui_translation[1] = pc.pose.py; - pc.gui_translation[2] = pc.pose.pz; - pc.gui_rotation[0] = rad2deg(pc.pose.om); - pc.gui_rotation[1] = rad2deg(pc.pose.fi); - pc.gui_rotation[2] = rad2deg(pc.pose.ka); - pc.file_name = input_file_names[i]; - point_clouds.push_back(pc);*/ + spdlog::error(":DLL ERROR: opening laszip reader for '{}'", input_file_name); return false; } laszip_header* header; if (laszip_get_header_pointer(laszip_reader, &header)) { - fprintf(stderr, ":DLL ERROR: getting header pointer from laszip reader\n"); - /*PointCloud pc; - pc.m_pose = Eigen::Affine3d::Identity(); - pc.m_initial_pose = pc.m_pose; - pc.pose = pose_tait_bryan_from_affine_matrix(pc.m_pose); - pc.gui_translation[0] = pc.pose.px; - pc.gui_translation[1] = pc.pose.py; - pc.gui_translation[2] = pc.pose.pz; - pc.gui_rotation[0] = rad2deg(pc.pose.om); - pc.gui_rotation[1] = rad2deg(pc.pose.fi); - pc.gui_rotation[2] = rad2deg(pc.pose.ka); - pc.file_name = input_file_names[i]; - point_clouds.push_back(pc);*/ + spdlog::error(":DLL ERROR: getting header pointer from laszip reader"); return false; } - // fprintf(stderr, "file '%s' contains %u points\n", input_file_name.c_str(), header->number_of_point_records); + spdlog::info("file '{}' contains {} points", input_file_name, 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"); + spdlog::error(":DLL ERROR: getting point pointer from laszip reader"); return false; } @@ -168,28 +134,6 @@ bool PointCloud::load_pc(const std::string& input_file_name, bool load_cache_mod this->gui_rotation[2] = rad2deg(this->pose.ka); } - /*for (int 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 true; - // continue; - } - - LAZPoint p; - p.x = header->x_offset + header->x_scale_factor * static_cast(point->X); - p.y = header->y_offset + header->y_scale_factor * static_cast(point->Y); - p.z = header->z_offset + header->z_scale_factor * static_cast(point->Z); - p.timestamp = point->gps_time; - - Eigen::Vector3d pp(p.x, p.y, p.z); - pc.points_local.push_back(pp); - pc.intensities.push_back(point->intensity); - pc.timestamps.push_back(p.timestamp); - }*/ - laszip_I64 npoints = (header->number_of_point_records ? header->number_of_point_records : header->extended_number_of_point_records); std::cout << (is_compressed ? "" : "un") << "compressed file '" << (std::filesystem::path(input_file_name).filename().string()) @@ -209,7 +153,7 @@ bool PointCloud::load_pc(const std::string& input_file_name, bool load_cache_mod { if (laszip_read_point(laszip_reader)) { - fprintf(stderr, "DLL ERROR: reading point %I64d\n", p_count); + spdlog::error("DLL ERROR: reading point {}", p_count); laszip_close_reader(laszip_reader); return false; } diff --git a/core/src/point_clouds.cpp b/core/src/point_clouds.cpp index c10c4bc0..ab48009a 100644 --- a/core/src/point_clouds.cpp +++ b/core/src/point_clouds.cpp @@ -4,8 +4,8 @@ #include #include -// #include -// #include +#include + inline void split(std::string& str, char delim, std::vector& out) { size_t start; @@ -1019,73 +1019,36 @@ bool PointClouds::load_pose_ETH(const std::string& fn, Eigen::Affine3d& m_increm return true; } -bool PointClouds::load_pc(PointCloud& pc, std::string input_file_name, bool load_cache_mode) +bool PointClouds::load_pc(PointCloud& pc, const std::string& input_file_name, bool load_cache_mode) { return pc.load_pc(input_file_name, load_cache_mode); #if 0 laszip_POINTER laszip_reader; if (laszip_create(&laszip_reader)) { - fprintf(stderr, ":DLL ERROR: creating laszip reader\n"); - /*PointCloud pc; - pc.m_pose = Eigen::Affine3d::Identity(); - pc.m_initial_pose = pc.m_pose; - pc.pose = pose_tait_bryan_from_affine_matrix(pc.m_pose); - pc.gui_translation[0] = pc.pose.px; - pc.gui_translation[1] = pc.pose.py; - pc.gui_translation[2] = pc.pose.pz; - pc.gui_rotation[0] = rad2deg(pc.pose.om); - pc.gui_rotation[1] = rad2deg(pc.pose.fi); - pc.gui_rotation[2] = rad2deg(pc.pose.ka); - pc.file_name = input_file_names[i]; - point_clouds.push_back(pc);*/ + spdlog::error(":DLL ERROR: creating laszip reader"); return false; } laszip_BOOL is_compressed = 0; if (laszip_open_reader(laszip_reader, input_file_name.c_str(), &is_compressed)) { - fprintf(stderr, ":DLL ERROR: opening laszip reader for '%s'\n", input_file_name.c_str()); - /*PointCloud pc; - pc.m_pose = Eigen::Affine3d::Identity(); - pc.m_initial_pose = pc.m_pose; - pc.pose = pose_tait_bryan_from_affine_matrix(pc.m_pose); - pc.gui_translation[0] = pc.pose.px; - pc.gui_translation[1] = pc.pose.py; - pc.gui_translation[2] = pc.pose.pz; - pc.gui_rotation[0] = rad2deg(pc.pose.om); - pc.gui_rotation[1] = rad2deg(pc.pose.fi); - pc.gui_rotation[2] = rad2deg(pc.pose.ka); - pc.file_name = input_file_names[i]; - point_clouds.push_back(pc);*/ + spdlog::error(":DLL ERROR: opening laszip reader for '{}'", input_file_name); return false; } laszip_header *header; if (laszip_get_header_pointer(laszip_reader, &header)) { - fprintf(stderr, ":DLL ERROR: getting header pointer from laszip reader\n"); - /*PointCloud pc; - pc.m_pose = Eigen::Affine3d::Identity(); - pc.m_initial_pose = pc.m_pose; - pc.pose = pose_tait_bryan_from_affine_matrix(pc.m_pose); - pc.gui_translation[0] = pc.pose.px; - pc.gui_translation[1] = pc.pose.py; - pc.gui_translation[2] = pc.pose.pz; - pc.gui_rotation[0] = rad2deg(pc.pose.om); - pc.gui_rotation[1] = rad2deg(pc.pose.fi); - pc.gui_rotation[2] = rad2deg(pc.pose.ka); - pc.file_name = input_file_names[i]; - point_clouds.push_back(pc);*/ + spdlog::error(":DLL ERROR: getting header pointer from laszip reader"); return false; } - // fprintf(stderr, "file '%s' contains %u points\n", input_file_name.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"); + spdlog::error(":DLL ERROR: getting point pointer from laszip reader"); return false; } @@ -1099,28 +1062,6 @@ bool PointClouds::load_pc(PointCloud& pc, std::string input_file_name, bool load pc.gui_rotation[1] = rad2deg(pc.pose.fi); pc.gui_rotation[2] = rad2deg(pc.pose.ka); - /*for (int 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 true; - // continue; - } - - LAZPoint p; - p.x = header->x_offset + header->x_scale_factor * static_cast(point->X); - p.y = header->y_offset + header->y_scale_factor * static_cast(point->Y); - p.z = header->z_offset + header->z_scale_factor * static_cast(point->Z); - p.timestamp = point->gps_time; - - Eigen::Vector3d pp(p.x, p.y, p.z); - pc.points_local.push_back(pp); - pc.intensities.push_back(point->intensity); - pc.timestamps.push_back(p.timestamp); - }*/ - laszip_I64 npoints = (header->number_of_point_records ? header->number_of_point_records : header->extended_number_of_point_records); std::cout << (is_compressed ? "" : "un") << "compressed file '" << (std::filesystem::path(input_file_name).filename().string()) << "' contains " << npoints << " points" << std::endl; @@ -1131,7 +1072,7 @@ bool PointClouds::load_pc(PointCloud& pc, std::string input_file_name, bool load { if (laszip_read_point(laszip_reader)) { - fprintf(stderr, "DLL ERROR: reading point %I64d\n", p_count); + spdlog::error("DLL ERROR: reading point {}", p_count); laszip_close_reader(laszip_reader); return false; } @@ -1147,11 +1088,6 @@ bool PointClouds::load_pc(PointCloud& pc, std::string input_file_name, bool load pc.intensities.push_back(point->intensity); pc.timestamps.push_back(p.timestamp); - // Eigen::Vector3d color( - // static_cast(0xFFU * ((point->rgb[0] > 0) ? static_cast(point->rgb[0]) / static_cast(0xFFFFU) : 1.0f)) / 256.0, - // static_cast(0xFFU * ((point->rgb[1] > 0) ? static_cast(point->rgb[1]) / static_cast(0xFFFFU) : 1.0f)) / 256.0, - // static_cast(0xFFU * ((point->rgb[2] > 0) ? static_cast(point->rgb[2]) / static_cast(0xFFFFU) : 1.0f)) / 256.0); - Eigen::Vector3d color( static_cast(point->rgb[0]) / 256.0, static_cast(point->rgb[1]) / 256.0, @@ -1164,9 +1100,7 @@ bool PointClouds::load_pc(PointCloud& pc, std::string input_file_name, bool load } laszip_close_reader(laszip_reader); - // laszip_clean(laszip_reader); - // laszip_destroy(laszip_reader); - + return true; #endif diff --git a/core_hd_mapping/CMakeLists.txt b/core_hd_mapping/CMakeLists.txt index 87fc4555..75048e1a 100644 --- a/core_hd_mapping/CMakeLists.txt +++ b/core_hd_mapping/CMakeLists.txt @@ -35,4 +35,5 @@ target_link_libraries(core-hd-mapping core ${PLATFORM_MISCELLANEOUS_LIBS} Threads::Threads + spdlog::spdlog ) diff --git a/core_hd_mapping/src/laz_wrapper.cpp b/core_hd_mapping/src/laz_wrapper.cpp index 312880a7..cf7d65cd 100644 --- a/core_hd_mapping/src/laz_wrapper.cpp +++ b/core_hd_mapping/src/laz_wrapper.cpp @@ -12,6 +12,8 @@ #include +#include + void LazWrapper::imgui(const CommonData& common_data, const ProjectSettings& project_setings) { ImGui::Begin("LazWrapper"); @@ -152,14 +154,14 @@ LAZSector LazWrapper::load_sector(const std::string& filename, const double shif laszip_POINTER laszip_reader; if (laszip_create(&laszip_reader)) { - fprintf(stderr, "DLL ERROR: creating laszip reader\n"); + spdlog::error("DLL ERROR: creating laszip reader"); std::abort(); } laszip_BOOL is_compressed = 0; if (laszip_open_reader(laszip_reader, filename.c_str(), &is_compressed)) { - fprintf(stderr, "DLL ERROR: opening laszip reader for '%s'\n", filename.c_str()); + spdlog::error("DLL ERROR: opening laszip reader for '{}'", filename.c_str()); std::abort(); } std::cout << "compressed : " << is_compressed << std::endl; @@ -167,17 +169,18 @@ LAZSector LazWrapper::load_sector(const std::string& filename, const double shif if (laszip_get_header_pointer(laszip_reader, &header)) { - fprintf(stderr, "DLL ERROR: getting header pointer from laszip reader\n"); + spdlog::error("DLL ERROR: getting header pointer from laszip reader"); std::abort(); } - fprintf(stderr, "file '%s' contains %u points\n", filename.c_str(), header->number_of_point_records); + + spdlog::info("file '{}' contains {} points", filename, 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"); + spdlog::error("DLL ERROR: getting point pointer from laszip reader"); std::abort(); } - // int64_t point_count; sector.point_cloud.reserve(header->number_of_point_records); @@ -190,9 +193,10 @@ LAZSector LazWrapper::load_sector(const std::string& filename, const double shif { if (laszip_read_point(laszip_reader)) { - fprintf(stderr, "DLL ERROR: reading point %u\n", i); + spdlog::error("DLL ERROR: reading point {}", i); std::abort(); } + LAZPoint p; p.x = header->x_offset + header->x_scale_factor * static_cast(point->X) + shift_x; p.y = header->y_offset + header->y_scale_factor * static_cast(point->Y) + shift_y; From 9b83f85f342295677c99163ccd733ebcb1857935 Mon Sep 17 00:00:00 2001 From: mwlasiuk Date: Sat, 28 Feb 2026 19:26:44 +0100 Subject: [PATCH 4/4] Add export_laz.h note --- core/include/export_laz.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/core/include/export_laz.h b/core/include/export_laz.h index e06e58d5..f35f9478 100644 --- a/core/include/export_laz.h +++ b/core/include/export_laz.h @@ -1,5 +1,7 @@ #pragma once +// TODO(mwlasiuk) : do this at the end (print -> spdlog) as it is .h and not cpp + hpp and that breaks some subprojects ... + #include #include #include