diff --git a/apps/multi_view_tls_registration/multi_view_tls_registration.cpp b/apps/multi_view_tls_registration/multi_view_tls_registration.cpp index a64b0325..a9473ea7 100644 --- a/apps/multi_view_tls_registration/multi_view_tls_registration.cpp +++ b/apps/multi_view_tls_registration/multi_view_tls_registration.cpp @@ -107,10 +107,17 @@ void save_intersection( void save_separately_to_las(const Session& session, fs::path outwd, std::string extension) { - for (auto& p : session.point_clouds_container.point_clouds) + const auto& clouds = session.point_clouds_container.point_clouds; + if (clouds.size() > 65535) + { + std::cerr << "warning: more than 65535 scans, point_source_ID capped at 65535\n"; + } + for (size_t scan_idx = 0; scan_idx < clouds.size(); ++scan_idx) { + const auto& p = clouds[scan_idx]; if (p.visible) { + const unsigned short psid = static_cast(std::min(scan_idx, 65535)); fs::path file_path_in = p.file_name; fs::path file_path_put = outwd; file_path_put /= (file_path_in.stem().string() + "_processed" + extension); @@ -118,7 +125,7 @@ void save_separately_to_las(const Session& session, fs::path outwd, std::string std::cout << "file_out: " << file_path_put << std::endl; std::cout << "start save_processed_pc" << std::endl; bool compressed = (extension == ".laz"); - save_processed_pc(file_path_in, file_path_put, p.m_pose, session.point_clouds_container.offset, compressed); + save_processed_pc(file_path_in, file_path_put, p.m_pose, session.point_clouds_container.offset, compressed, psid); std::cout << "save_processed_pc finished" << std::endl; } } @@ -134,12 +141,21 @@ void save_trajectories_to_laz( std::vector pointcloud; std::vector intensity; std::vector timestamps; + std::vector point_source_ids; + + const auto& clouds = session.point_clouds_container.point_clouds; + if (clouds.size() > 65535) + { + std::cerr << "warning: more than 65535 scans, point_source_ID capped at 65535\n"; + } float consecutive_distance = 0; - for (auto& p : session.point_clouds_container.point_clouds) + for (size_t scan_idx = 0; scan_idx < clouds.size(); ++scan_idx) { + const auto& p = clouds[scan_idx]; if (p.visible) { + const unsigned short psid = static_cast(std::min(scan_idx, 65535)); for (int i = 0; i < p.local_trajectory.size(); i++) { const auto& pp = p.local_trajectory[i].m_pose.translation(); @@ -185,6 +201,7 @@ void save_trajectories_to_laz( pointcloud.push_back(vp); intensity.push_back(0); timestamps.push_back(p.local_trajectory[i].timestamps.first); + point_source_ids.push_back(psid); } else { @@ -194,6 +211,7 @@ void save_trajectories_to_laz( pointcloud.push_back(vp); intensity.push_back(0); timestamps.push_back(p.local_trajectory[i].timestamps.first); + point_source_ids.push_back(psid); } } } @@ -207,7 +225,8 @@ void save_trajectories_to_laz( timestamps, session.point_clouds_container.offset.x(), session.point_clouds_container.offset.y(), - session.point_clouds_container.offset.z())) + session.point_clouds_container.offset.z(), + &point_source_ids)) { std::cout << "problem with saving file: " << output_file_name << std::endl; } diff --git a/core/include/Core/export_laz.h b/core/include/Core/export_laz.h index 9ace206f..81940230 100644 --- a/core/include/Core/export_laz.h +++ b/core/include/Core/export_laz.h @@ -4,6 +4,7 @@ #include +#include #include #include #include @@ -79,12 +80,19 @@ class LazWriter } bool writePoint( - const Eigen::Vector3d& position, unsigned short intensity, double timestamp, double offset_x, double offset_y, double offset_z) + const Eigen::Vector3d& position, + unsigned short intensity, + double timestamp, + double offset_x, + double offset_y, + double offset_z, + unsigned short point_source_id = 0) { point_->intensity = intensity; point_->return_number = 1; point_->number_of_returns = 1; point_->gps_time = timestamp * 1e9; + point_->point_source_ID = point_source_id; laszip_F64 coordinates[3]; coordinates[0] = position.x() + offset_x; @@ -155,7 +163,8 @@ inline bool exportLaz( const std::vector& timestamps, double offset_x = 0.0, double offset_y = 0.0, - double offset_alt = 0.0) + double offset_alt = 0.0, + const std::vector* point_source_ids = nullptr) { LazWriter writer; if (!writer.open(filename, offset_x, offset_y, offset_alt)) @@ -163,7 +172,8 @@ inline bool exportLaz( for (size_t i = 0; i < pointcloud.size(); i++) { - if (!writer.writePoint(pointcloud[i], intensity[i], timestamps[i], offset_x, offset_y, offset_alt)) + const unsigned short psid = (point_source_ids && i < point_source_ids->size()) ? (*point_source_ids)[i] : 0; + if (!writer.writePoint(pointcloud[i], intensity[i], timestamps[i], offset_x, offset_y, offset_alt, psid)) return false; } @@ -175,7 +185,8 @@ inline void save_processed_pc( const fs::path& file_path_put, const Eigen::Affine3d& m_pose, const Eigen::Vector3d& offset, - bool override_compressed = false) + bool override_compressed = false, + unsigned short point_source_id = 0) { std::cout << "processing: " << file_path_in << std::endl; @@ -292,6 +303,7 @@ inline void save_processed_pc( output_point->classification = input_point->classification; output_point->num_extra_bytes = input_point->num_extra_bytes; output_point->gps_time = input_point->gps_time; + output_point->point_source_ID = point_source_id; memcpy(output_point->extra_bytes, input_point->extra_bytes, output_point->num_extra_bytes); if (laszip_write_point(laszip_writer)) @@ -396,6 +408,13 @@ inline void save_all_to_las(const Session& session, std::string output_las_name, bool found_first_pose = false; const auto& offset = session.point_clouds_container.offset; + const auto& clouds = session.point_clouds_container.point_clouds; + + if (clouds.size() > 65535) + { + std::cerr << "warning: more than 65535 scans, point_source_ID capped at 65535\n"; + } + LazWriter writer; if (!writer.open(output_las_name, offset.x(), offset.y(), offset.z())) { @@ -403,8 +422,9 @@ inline void save_all_to_las(const Session& session, std::string output_las_name, return; } - for (const auto& p : session.point_clouds_container.point_clouds) + for (size_t scan_idx = 0; scan_idx < clouds.size(); ++scan_idx) { + const auto& p = clouds[scan_idx]; if (p.visible) { if (!found_first_pose) @@ -413,6 +433,8 @@ inline void save_all_to_las(const Session& session, std::string output_las_name, first_pose = p.m_pose; } + const unsigned short psid = static_cast(std::min(scan_idx, 65535)); + for (size_t i = 0; i < p.points_local.size(); ++i) { if (skip_ts_0) @@ -428,7 +450,7 @@ inline void save_all_to_las(const Session& session, std::string output_las_name, unsigned short inten = (i < p.intensities.size()) ? p.intensities[i] : 0; double ts = (i < p.timestamps.size()) ? p.timestamps[i] : 0.0; - if (!writer.writePoint(vp, inten, ts, offset.x(), offset.y(), offset.z())) + if (!writer.writePoint(vp, inten, ts, offset.x(), offset.y(), offset.z(), psid)) return; } }