diff --git a/algorithms/dense-reconstruction/depth_integration/include/depth-integration/depth-integration-inl.h b/algorithms/dense-reconstruction/depth_integration/include/depth-integration/depth-integration-inl.h index 5740cf9778..c5dad2847d 100644 --- a/algorithms/dense-reconstruction/depth_integration/include/depth-integration/depth-integration-inl.h +++ b/algorithms/dense-reconstruction/depth_integration/include/depth-integration/depth-integration-inl.h @@ -532,7 +532,7 @@ void integrateAllSensorDepthResourcesOfType( // If we have timing information for the points undistort the point // cloud, based on the current poses. if (point_cloud.hasTimes()) { - int32_t min_time_ns, max_time_ns; + int64_t min_time_ns, max_time_ns; point_cloud.getMinMaxTimeNanoseconds(&min_time_ns, &max_time_ns); // The undistortion might take us outside the posegraph range diff --git a/algorithms/online-map-builders/include/online-map-builders/stream-map-builder.h b/algorithms/online-map-builders/include/online-map-builders/stream-map-builder.h index 17f44bda29..dc4463253f 100644 --- a/algorithms/online-map-builders/include/online-map-builders/stream-map-builder.h +++ b/algorithms/online-map-builders/include/online-map-builders/stream-map-builder.h @@ -3,8 +3,12 @@ #include #include +#include +#include #include +#include #include +#include #include #include #include @@ -47,6 +51,8 @@ class StreamMapBuilder { void apply(const vio::MapUpdate& update); void apply(const vio::MapUpdate& update, bool deep_copy_nframe); + void finishMapping(); + vi_map::MissionId getMissionId() const { return mission_id_; } @@ -114,6 +120,10 @@ class StreamMapBuilder { void notifyLoopClosureConstraintBuffer(); void notifyWheelOdometryConstraintBuffer(); void notifyExternalFeaturesMeasurementBuffer(); + // if dump_remaining_points is true, the buffer will be emptied into the best + // vertex available. Otherwise, the buffer will only be used to associate + // points to N-1 vertices. + void notifyLidarMeasurementBuffer(bool dump_remaining_points); void addRootViwlsVertex( const std::shared_ptr& nframe, @@ -198,6 +208,9 @@ class StreamMapBuilder { external_features_outlier_rejection_pipelines_; static constexpr size_t kKeepNMostRecentImages = 10u; + + std::unordered_map + lidar_point_cloud_buffer_; }; template @@ -226,24 +239,29 @@ void StreamMapBuilder::attachLidarMeasurement( if (lidar_sensor.hasPointTimestamps()) { const uint32_t convert_to_ns = lidar_sensor.getTimestampConversionToNanoseconds(); + const int64_t time_offset_ns = lidar_sensor.hasRelativePointTimestamps() - ? lidar_measurement.getTimestampNanoseconds() - : 0; + ? 0 + : -lidar_measurement.getTimestampNanoseconds(); + // The point cloud timestamps are absolute. backend::convertPointCloudType( lidar_measurement.getPointCloud(), &point_cloud, true, convert_to_ns, time_offset_ns); + lidar_point_cloud_buffer_[lidar_sensor_id].append(point_cloud); + return; } else { backend::convertPointCloudType( lidar_measurement.getPointCloud(), &point_cloud, false); - } - backend::ResourceType point_cloud_type = - backend::getResourceTypeForPointCloud(point_cloud); - vi_map::VIMission& mission = map_->getMission(mission_id_); - map_->addSensorResource( - point_cloud_type, lidar_sensor_id, - lidar_measurement.getTimestampNanoseconds(), point_cloud, &mission); + backend::ResourceType point_cloud_type = + backend::getResourceTypeForPointCloud(point_cloud); + vi_map::VIMission& mission = map_->getMission(mission_id_); + map_->addSensorResource( + point_cloud_type, lidar_sensor_id, + lidar_measurement.getTimestampNanoseconds(), point_cloud, &mission); + return; + } } template diff --git a/algorithms/online-map-builders/src/stream-map-builder.cc b/algorithms/online-map-builders/src/stream-map-builder.cc index 5a92812436..7e22676ee0 100644 --- a/algorithms/online-map-builders/src/stream-map-builder.cc +++ b/algorithms/online-map-builders/src/stream-map-builder.cc @@ -164,6 +164,12 @@ void StreamMapBuilder::apply( notifyBuffers(); } +void StreamMapBuilder::finishMapping() { + LOG(INFO) << "[StreamMapBuilder] Mapping finished callbacks..."; + // Dump lidar point cloud buffer + notifyLidarMeasurementBuffer(/*dump_remaining_points=*/true); +} + void StreamMapBuilder::addRootViwlsVertex( const aslam::VisualNFrame::Ptr& nframe, const vio::ViNodeState& vinode_state) { @@ -465,6 +471,7 @@ void StreamMapBuilder::notifyBuffers() { notifyLoopClosureConstraintBuffer(); notifyWheelOdometryConstraintBuffer(); notifyExternalFeaturesMeasurementBuffer(); + notifyLidarMeasurementBuffer(/*dump_remaining_points=*/false); } void StreamMapBuilder::notifyAbsolute6DoFConstraintBuffer() { @@ -1144,4 +1151,110 @@ void StreamMapBuilder::notifyExternalFeaturesMeasurementBuffer() { } } +void StreamMapBuilder::notifyLidarMeasurementBuffer( + const bool dump_remaining_points) { + if (map_->numVertices() < 1u || lidar_point_cloud_buffer_.empty()) { + return; + } + vi_map::VIMission& mission = map_->getMission(mission_id_); + + for (auto& buffer_with_sensor_id : lidar_point_cloud_buffer_) { + const auto& lidar_sensor_id = buffer_with_sensor_id.first; + auto& point_cloud_buffer = buffer_with_sensor_id.second; + + CHECK(std::is_sorted( + point_cloud_buffer.times_ns.begin(), point_cloud_buffer.times_ns.end())) + << "[StreamMapBuilder] The lidar point cloud buffer is not sorted!"; + + const backend::ResourceType point_cloud_type = + backend::getResourceTypeForPointCloud(point_cloud_buffer); + + pose_graph::VertexId current_vertex_id; + pose_graph::VertexId next_vertex_id; + + const auto initial_min_lidar_time = point_cloud_buffer.times_ns.front(); + + // Get the vertex with the timestamp closest to the min lidar time. + uint64_t delta_ns = 0; + // NOTE(gtonetti): We always want to include the lidar measurements, so we + // set the max interpolation time to the maximum possible value + // (tolerance_ns is cast to int64_t) + constexpr uint64_t kMaxInterpolationTimeNs = + static_cast(std::numeric_limits::max()); + + if (!queries_.getClosestVertexIdByTimestamp( + initial_min_lidar_time, kMaxInterpolationTimeNs, ¤t_vertex_id, + &delta_ns)) { + LOG(FATAL) + << "[StreamMapBuilder] Could not find a vertex for the initial " + "lidar measurement!"; + } + + if (!map_->getNextVertex(current_vertex_id, &next_vertex_id)) { + CHECK(current_vertex_id == last_vertex_); + } + while (current_vertex_id != last_vertex_ && point_cloud_buffer.size() > 0) { + const auto current_vertex_ts = + map_->getVertex(current_vertex_id).getMinTimestampNanoseconds(); + const auto next_vertex_ts = + map_->getVertex(next_vertex_id).getMinTimestampNanoseconds(); + const auto midpoint_time = + current_vertex_ts + (next_vertex_ts - current_vertex_ts) / 2; + + const auto min_lidar_time = point_cloud_buffer.times_ns.front(); + const auto max_lidar_time = point_cloud_buffer.times_ns.back(); + + CHECK(midpoint_time >= min_lidar_time) + << "[StreamMapBuilder] Something went wrong! the the min_lidar_time " + "should always be closer to the current vertex than than the next " + "vertex!"; + + if (midpoint_time > max_lidar_time) { + // we should wait for the next lidar measurement to arrive before + // processing this vertex + break; + } + + resources::PointCloud points_for_current_vertex = + point_cloud_buffer.splitAtTime(midpoint_time, /*is_sorted=*/true); + CHECK(points_for_current_vertex.times_ns.back() <= midpoint_time); + CHECK(point_cloud_buffer.times_ns.front() > midpoint_time); + CHECK(point_cloud_buffer.size() > 0) + << "[StreamMapBuilder] Split did not leave any points in the " + "buffer! " + "This is impossible!"; + + const auto num_points_split = points_for_current_vertex.size(); + + // Associate the split points with the current vertex. + CHECK(!map_->hasSensorResource( + mission, point_cloud_type, lidar_sensor_id, current_vertex_ts)) + << "[StreamMapBuilder] There is already a point cloud resource at " + "the current vertex timestamp!"; + + // shift timestamps to the current vertex timestamp + points_for_current_vertex.shiftTimestamps(current_vertex_ts); + map_->addSensorResource( + point_cloud_type, lidar_sensor_id, current_vertex_ts, + points_for_current_vertex, &mission); + VLOG(3) << "[StreamMapBuilder] Associated " << num_points_split + << " points with vertex " << current_vertex_id << " (ts " + << current_vertex_ts << ")"; + current_vertex_id = next_vertex_id; + map_->getNextVertex(current_vertex_id, &next_vertex_id); + } + if (dump_remaining_points) { + VLOG(3) << "[StreamMapBuilder] Dumping remaining points " + << point_cloud_buffer.size() << " into current " + << "vertex " << current_vertex_id; + // Dump the remaining points into the current vertex + map_->addSensorResource( + point_cloud_type, lidar_sensor_id, + map_->getVertex(current_vertex_id).getMinTimestampNanoseconds(), + point_cloud_buffer, &mission); + point_cloud_buffer.clear(); + } + } +} + } // namespace online_map_builders diff --git a/applications/maplab-node/src/map-builder-flow.cc b/applications/maplab-node/src/map-builder-flow.cc index 009a3b0d22..ce083da45e 100644 --- a/applications/maplab-node/src/map-builder-flow.cc +++ b/applications/maplab-node/src/map-builder-flow.cc @@ -277,6 +277,8 @@ bool MapBuilderFlow::saveMapAndOptionallyOptimize( if (stop_mapping) { mapping_terminated_ = true; + // Finish mapping + stream_map_builder_.finishMapping(); } // Early exit if the map is empty. diff --git a/backend/map-resources/include/map-resources/resource-conversion-inl.h b/backend/map-resources/include/map-resources/resource-conversion-inl.h index dcbe4b5c5b..7e29c7fb70 100644 --- a/backend/map-resources/include/map-resources/resource-conversion-inl.h +++ b/backend/map-resources/include/map-resources/resource-conversion-inl.h @@ -1,6 +1,7 @@ #ifndef MAP_RESOURCES_RESOURCE_CONVERSION_INL_H_ #define MAP_RESOURCES_RESOURCE_CONVERSION_INL_H_ +#include #include #include #include @@ -133,17 +134,17 @@ void addPointToPointCloud( template void addTimeToPointCloud( - const int32_t time, const size_t index, PointCloudType* point_cloud) { + const int64_t time, const size_t index, PointCloudType* point_cloud) { LOG(FATAL) << "This point cloud either does not support times" << "or it is not implemented!"; } template <> void addTimeToPointCloud( - const int32_t time, const size_t index, + const int64_t time, const size_t index, resources::PointCloud* point_cloud); template <> void addTimeToPointCloud( - const int32_t time, const size_t index, + const int64_t time, const size_t index, sensor_msgs::PointCloud2* point_cloud); template @@ -364,7 +365,7 @@ void getLabelFromPointCloud( template void getTimeFromPointCloud( const PointCloudType& /*point_cloud*/, const size_t /*index*/, - int32_t* /*time*/, const int32_t /*convert_to_ns*/, + int64_t* /*time*/, const int32_t /*convert_to_ns*/, const int64_t /*time_offset_ns*/) { LOG(FATAL) << "This point cloud either does not support times of the " << "requested type or it is not implemented!"; @@ -372,12 +373,12 @@ void getTimeFromPointCloud( template <> void getTimeFromPointCloud( const resources::PointCloud& point_cloud, const size_t index, - int32_t* time, const int32_t /*convert_to_ns*/, + int64_t* time, const int32_t /*convert_to_ns*/, const int64_t /*time_offset_ns*/); template <> void getTimeFromPointCloud( const sensor_msgs::PointCloud2& point_cloud, const size_t index, - int32_t* time, const int32_t convert_to_ns, const int64_t time_offset_ns); + int64_t* time, const int32_t convert_to_ns, const int64_t time_offset_ns); template bool convertDepthMapToPointCloud( @@ -550,7 +551,7 @@ bool convertPointCloudType( } if (with_timestamps && output_has_times) { - int32_t time; + int64_t time; getTimeFromPointCloud( input_cloud, point_idx, &time, convert_to_ns, time_offset_ns); addTimeToPointCloud(time, point_idx, output_cloud); diff --git a/backend/map-resources/include/map-resources/resource-conversion.h b/backend/map-resources/include/map-resources/resource-conversion.h index d512278fd2..af11915187 100644 --- a/backend/map-resources/include/map-resources/resource-conversion.h +++ b/backend/map-resources/include/map-resources/resource-conversion.h @@ -2,6 +2,7 @@ #define MAP_RESOURCES_RESOURCE_CONVERSION_H_ #include +#include #include #include #include @@ -64,7 +65,7 @@ void addColorToPointCloud( PointCloudType* point_cloud); template void addTimeToPointCloud( - const int32_t time, const size_t index, PointCloudType* point_cloud); + const int64_t time, const size_t index, PointCloudType* point_cloud); template void getPointFromPointCloud( @@ -82,7 +83,7 @@ void getColorFromPointCloud( resources::RgbaColor* color); template void getTimeFromPointCloud( - const PointCloudType& point_cloud, const size_t index, int32_t* time, + const PointCloudType& point_cloud, const size_t index, int64_t* time, const int32_t convert_to_ns, const int64_t time_offset_ns); template diff --git a/backend/map-resources/src/resource-conversion.cc b/backend/map-resources/src/resource-conversion.cc index d24388646c..6bbaeeecf2 100644 --- a/backend/map-resources/src/resource-conversion.cc +++ b/backend/map-resources/src/resource-conversion.cc @@ -3,6 +3,7 @@ #include #include #include +#include #include #include #include @@ -652,7 +653,7 @@ void getLabelFromPointCloud( template <> void addTimeToPointCloud( - const int32_t time, const size_t index, + const int64_t time, const size_t index, resources::PointCloud* point_cloud) { DCHECK_NOTNULL(point_cloud); DCHECK_LT(index, point_cloud->times_ns.size()); @@ -661,9 +662,9 @@ void addTimeToPointCloud( template <> void addTimeToPointCloud( - const int32_t time, const size_t index, + const int64_t time, const size_t index, sensor_msgs::PointCloud2* point_cloud) { - sensor_msgs::PointCloud2Iterator it_time( + sensor_msgs::PointCloud2Iterator it_time( *point_cloud, kPointCloud2TimeV1); it_time += index; @@ -673,7 +674,7 @@ void addTimeToPointCloud( template <> void getTimeFromPointCloud( const resources::PointCloud& point_cloud, const size_t index, - int32_t* time, const int32_t /*convert_to_ns*/, + int64_t* time, const int32_t /*convert_to_ns*/, const int64_t /*time_offset_ns*/) { DCHECK_NOTNULL(time); @@ -684,7 +685,7 @@ void getTimeFromPointCloud( template <> void getTimeFromPointCloud( const sensor_msgs::PointCloud2& point_cloud, const size_t index, - int32_t* time, const int32_t convert_to_ns, + int64_t* time, const int32_t convert_to_ns, const int64_t time_offset_ns) { DCHECK_NOTNULL(time); sensor_msgs::PointField field = getTimeField(point_cloud); @@ -698,14 +699,14 @@ void getTimeFromPointCloud( case sensor_msgs::PointField::FLOAT64: { double time_f = boost::apply_visitor(time_visitor_double.setIndex(index), var); - *time = static_cast(time_f * convert_to_ns - time_offset_ns); + *time = static_cast(time_f * convert_to_ns - time_offset_ns); break; } case sensor_msgs::PointField::INT32: case sensor_msgs::PointField::UINT32: { int64_t time_i = boost::apply_visitor(time_visitor_int64.setIndex(index), var); - *time = static_cast(time_i * convert_to_ns - time_offset_ns); + *time = static_cast(time_i * convert_to_ns - time_offset_ns); break; } default: { diff --git a/common/resources-common/include/resources-common/point-cloud.h b/common/resources-common/include/resources-common/point-cloud.h index 6c4bf34162..58d296bb1e 100644 --- a/common/resources-common/include/resources-common/point-cloud.h +++ b/common/resources-common/include/resources-common/point-cloud.h @@ -149,9 +149,16 @@ class PointCloud { void appendTransformed( const PointCloud& other, const aslam::Transformation& T_A_B); + // Shifts the point cloud by a given offset in time. The offset is in + // nanoseconds. + void shiftTimestamps(int64_t offset_ns) { + for (size_t i = 0; i < times_ns.size(); ++i) { + times_ns[i] -= offset_ns; + } + } // Get the min and max timestamps for all the points. Useful for undistortion. void getMinMaxTimeNanoseconds( - int32_t* min_time_ns, int32_t* max_time_ns) const; + int64_t* min_time_ns, int64_t* max_time_ns) const; // Undistort the point cloud given a set of initial high accuracy poses // from the start to the end of the scan. Linear interpolation will be used @@ -169,6 +176,14 @@ class PointCloud { // Removes points inside a 3D bounding box. void filterBoundingBox3D(BoundingBox3D box_filter); + // Splits the point cloud at the give time. If is_sorted is true, the point + // cloud is assumed to be sorted by timestamp. Otherwise, the point cloud will + // be sorted by timestamp first. + // Returns a new point cloud containing all points with timestamp <= time. + PointCloud splitAtTime(int64_t time, bool is_sorted); + + void orderByTimestamp(); + // Downsample using a voxel grid. The returned point cloud will only // have xyz coordinates, all other information is stripped away. void downsampleVoxelized(double voxel_size, PointCloud* voxelized) const; @@ -191,7 +206,7 @@ class PointCloud { std::vector colors; std::vector scalars; std::vector labels; - std::vector times_ns; + std::vector times_ns; }; } // namespace resources diff --git a/common/resources-common/include/resources-common/tinyply/tinyply.h b/common/resources-common/include/resources-common/tinyply/tinyply.h index 4c555b2256..c3f278d601 100644 --- a/common/resources-common/include/resources-common/tinyply/tinyply.h +++ b/common/resources-common/include/resources-common/tinyply/tinyply.h @@ -43,7 +43,8 @@ class PlyProperty { INT32, UINT32, FLOAT32, - FLOAT64 + FLOAT64, + INT64 }; PlyProperty(std::istream& is); @@ -105,6 +106,7 @@ static std::map PropertyTable{ {PlyProperty::Type::UINT16, {2, "ushort"}}, {PlyProperty::Type::INT32, {4, "int"}}, {PlyProperty::Type::UINT32, {4, "uint"}}, + {PlyProperty::Type::INT64, {8, "long"}}, {PlyProperty::Type::FLOAT32, {4, "float"}}, {PlyProperty::Type::FLOAT64, {8, "double"}}, {PlyProperty::Type::INVALID, {0, "INVALID"}}}; @@ -122,6 +124,8 @@ inline PlyProperty::Type property_type_from_string(const std::string& t) { return PlyProperty::Type::INT32; else if (t == "uint32" || t == "uint") return PlyProperty::Type::UINT32; + else if (t == "int64" || t == "long") + return PlyProperty::Type::INT64; else if (t == "float32" || t == "float") return PlyProperty::Type::FLOAT32; else if (t == "float64" || t == "double") @@ -157,6 +161,8 @@ inline void resize_vector( case PlyProperty::Type::UINT32: ptr = resize(v, newSize); break; + case PlyProperty::Type::INT64: + ptr = resize(v, newSize); case PlyProperty::Type::FLOAT32: ptr = resize(v, newSize); break; @@ -182,6 +188,8 @@ inline PlyProperty::Type property_type_for_type(std::vector& /*theType*/) { return PlyProperty::Type::INT32; else if (std::is_same::value) return PlyProperty::Type::UINT32; + else if (std::is_same::value) + return PlyProperty::Type::INT64; else if (std::is_same::value) return PlyProperty::Type::FLOAT32; else if (std::is_same::value) @@ -241,8 +249,8 @@ class PlyFile { return 0; // count and verify large enough - auto instance_counter = [&]( - const std::string& elementKey, const std::string& propertyKey) { + auto instance_counter = [&](const std::string& elementKey, + const std::string& propertyKey) { for (auto e : get_elements()) { if (e.name != elementKey) continue; @@ -403,6 +411,6 @@ class PlyFile { std::vector requestedElements; }; -} // namesapce tinyply +} // namespace tinyply #endif // RESOURCES_COMMON_TINYPLY_TINYPLY_H_ diff --git a/common/resources-common/src/point-cloud.cc b/common/resources-common/src/point-cloud.cc index 1e928bb4b2..00029a6e17 100644 --- a/common/resources-common/src/point-cloud.cc +++ b/common/resources-common/src/point-cloud.cc @@ -104,13 +104,13 @@ void PointCloud::appendTransformed( } void PointCloud::getMinMaxTimeNanoseconds( - int32_t* min_time_ns, int32_t* max_time_ns) const { + int64_t* min_time_ns, int64_t* max_time_ns) const { CHECK_NOTNULL(min_time_ns); CHECK_NOTNULL(max_time_ns); // Initialize to the opposite ends and iterate over point times. - *min_time_ns = std::numeric_limits::max(); - *max_time_ns = std::numeric_limits::min(); + *min_time_ns = std::numeric_limits::max(); + *max_time_ns = std::numeric_limits::min(); for (size_t i = 0; i < size(); ++i) { *min_time_ns = std::min(*min_time_ns, times_ns[i]); *max_time_ns = std::max(*max_time_ns, times_ns[i]); @@ -149,8 +149,8 @@ void PointCloud::undistort( // Initialize the two poses between which we interpolate. By sorting the // points by timestamp beforehand, we never need to go back afterward. int64_t intermediary_index = 1; - int32_t time_A = timestamps[0]; - int32_t time_B = timestamps[1]; + int64_t time_A = timestamps[0]; + int64_t time_B = timestamps[1]; aslam::Transformation pose_A = poses[0]; aslam::Transformation pose_B = poses[1]; @@ -180,7 +180,7 @@ size_t PointCloud::filterValidMinMaxBox( std::vector colors_new; std::vector scalars_new; std::vector labels_new; - std::vector times_ns_new; + std::vector times_ns_new; // We guess that we are on average not going to remove that many points. xyz_new.reserve(xyz.size()); @@ -269,6 +269,212 @@ size_t PointCloud::filterValidMinMaxBox( return num_removed; } +PointCloud PointCloud::splitAtTime(const int64_t time, const bool is_sorted) { + CHECK(checkConsistency(true)) + << "Point cloud is not consistent at the start of splitAtTime!"; + if (!is_sorted) { + // Sorting is quite expensive, so we only do it if we actually need to. + orderByTimestamp(); + } + + PointCloud result; + if (!hasTimes() || times_ns.empty()) { + return result; + } + + const auto split_iter = + std::upper_bound(times_ns.begin(), times_ns.end(), time); + const size_t split_idx = std::distance(times_ns.begin(), split_iter); + + if (split_idx == 0) { + return result; + } + if (split_idx == times_ns.size()) { + result = *this; + // empty the current PC + clear(); + return result; + } + + // Ensure split_idx is not out of bounds (paranoid check, upper_bound behavior + // should prevent this for non-empty times_ns) + CHECK(split_idx <= times_ns.size()) + << "split_idx calculated incorrectly or times_ns modified unexpectedly."; + + // Create result cloud and reserve space + result.times_ns.reserve(split_idx); + result.xyz.reserve(split_idx * 3); + if (hasNormals()) { // Check based on original 'this' state + result.normals.reserve(split_idx * 3); + } + if (hasColor()) { // Check based on original 'this' state + result.colors.reserve(split_idx); + } + if (hasScalars()) { // Check based on original 'this' state + result.scalars.reserve(split_idx); + } + if (hasLabels()) { // Check based on original 'this' state + result.labels.reserve(split_idx); + } + + // Copy data to result cloud + // These iterators are safe because split_idx <= times_ns.size() (and other + // vectors are consistent initially) + result.times_ns.assign(times_ns.begin(), times_ns.begin() + split_idx); + result.xyz.assign(xyz.begin(), xyz.begin() + (split_idx * 3)); + + if (hasNormals()) { + CHECK(normals.size() >= split_idx * 3) + << "Insufficient normals for assignment."; + result.normals.assign(normals.begin(), normals.begin() + (split_idx * 3)); + } + if (hasColor()) { + CHECK(colors.size() >= split_idx) << "Insufficient colors for assignment."; + result.colors.assign(colors.begin(), colors.begin() + split_idx); + } + if (hasScalars()) { + CHECK(scalars.size() >= split_idx) + << "Insufficient scalars for assignment."; + result.scalars.assign(scalars.begin(), scalars.begin() + split_idx); + } + if (hasLabels()) { + CHECK(labels.size() >= split_idx) << "Insufficient labels for assignment."; + result.labels.assign(labels.begin(), labels.begin() + split_idx); + } + + // modify the optional data first since hasX() methods depend on the size of + // the required vectors + if (hasNormals()) { + CHECK(normals.size() >= split_idx * 3) << "Insufficient normals for erase."; + normals.erase(normals.begin(), normals.begin() + (split_idx * 3)); + } + if (hasColor()) { + CHECK(colors.size() >= split_idx) << "Insufficient colors for erase."; + colors.erase(colors.begin(), colors.begin() + split_idx); + } + if (hasScalars()) { + CHECK(scalars.size() >= split_idx) << "Insufficient scalars for erase."; + scalars.erase(scalars.begin(), scalars.begin() + split_idx); + } + if (hasLabels()) { + CHECK(labels.size() >= split_idx) << "Insufficient labels for erase."; + labels.erase(labels.begin(), labels.begin() + split_idx); + } + + // Now erase primary data + // These iterators are safe because split_idx was determined from + // times_ns.begin() and times_ns hasn't been modified by other erasures yet. + CHECK(times_ns.size() >= split_idx) << "Insufficient times_ns for erase."; + times_ns.erase(times_ns.begin(), times_ns.begin() + split_idx); + + CHECK(xyz.size() >= split_idx * 3) << "Insufficient xyz for erase."; + xyz.erase(xyz.begin(), xyz.begin() + (split_idx * 3)); + + // Final consistency checks + CHECK(checkConsistency(true)) + << "Point cloud ('this') is not consistent after erase!"; + CHECK(result.checkConsistency(true)) + << "Resulting point cloud ('result') is not consistent!"; + + return result; +} + +void PointCloud::orderByTimestamp() { + const size_t num_points = this->size(); + + if (num_points == 0) { + return; + } + if (!hasTimes()) { + LOG(WARNING) << "[PointCloud] Ordering by timestamp failed because " + "timestamps are not available!"; + return; + } + + std::vector p(num_points); + std::iota(p.begin(), p.end(), 0); + + // Sort the index vector based on the timestamps. + std::sort(p.begin(), p.end(), [&](size_t idx1, size_t idx2) { + return times_ns[idx1] < times_ns[idx2]; + }); + + // Create new vectors to store the sorted data. + std::vector xyz_sorted(xyz.size()); + std::vector normals_sorted; + std::vector colors_sorted; + std::vector scalars_sorted; + std::vector labels_sorted; + std::vector times_ns_sorted(num_points); + + // Cache the presence of optional data fields. + const bool current_has_normals = hasNormals(); + const bool current_has_colors = hasColor(); + const bool current_has_scalars = hasScalars(); + const bool current_has_labels = hasLabels(); + + // Resize sorted vectors for optional data if they are present. + if (current_has_normals) { + normals_sorted.resize(normals.size()); + } + if (current_has_colors) { + colors_sorted.resize(colors.size()); + } + if (current_has_scalars) { + scalars_sorted.resize(scalars.size()); + } + if (current_has_labels) { + labels_sorted.resize(labels.size()); + } + + // Populate the sorted vectors using the sorted indices. + for (size_t i = 0; i < num_points; ++i) { + size_t original_idx = p[i]; + + xyz_sorted[i * 3 + 0] = xyz[original_idx * 3 + 0]; + xyz_sorted[i * 3 + 1] = xyz[original_idx * 3 + 1]; + xyz_sorted[i * 3 + 2] = xyz[original_idx * 3 + 2]; + + if (current_has_normals) { + normals_sorted[i * 3 + 0] = normals[original_idx * 3 + 0]; + normals_sorted[i * 3 + 1] = normals[original_idx * 3 + 1]; + normals_sorted[i * 3 + 2] = normals[original_idx * 3 + 2]; + } + if (current_has_colors) { + colors_sorted[i * 3 + 0] = colors[original_idx * 3 + 0]; + colors_sorted[i * 3 + 1] = colors[original_idx * 3 + 1]; + colors_sorted[i * 3 + 2] = colors[original_idx * 3 + 2]; + } + + if (current_has_scalars) { + scalars_sorted[i] = scalars[original_idx]; + } + + if (current_has_labels) { + labels_sorted[i] = labels[original_idx]; + } + + times_ns_sorted[i] = times_ns[original_idx]; + } + + xyz = std::move(xyz_sorted); + times_ns = std::move(times_ns_sorted); + + if (current_has_normals) { + normals = std::move(normals_sorted); + } + if (current_has_colors) { + colors = std::move(colors_sorted); + } + if (current_has_scalars) { + scalars = std::move(scalars_sorted); + } + if (current_has_labels) { + labels = std::move(labels_sorted); + } + CHECK(checkConsistency(true)) << "Point cloud is not consistent!"; +} + void PointCloud::downsampleVoxelized( double voxel_size, PointCloud* voxelized) const { CHECK_GT(voxel_size, 1e-3) << "Voxel size is too small."; @@ -345,7 +551,7 @@ void PointCloud::writeToFile(const std::string& file_path) const { if (!times_ns.empty()) { ply_file.add_properties_to_element( - "vertex", {"time_ns"}, const_cast&>(times_ns)); + "vertex", {"time_ns"}, const_cast&>(times_ns)); } ply_file.comments.push_back("generated by tinyply from maplab"); diff --git a/common/resources-common/src/tinyply/tinyply.cc b/common/resources-common/src/tinyply/tinyply.cc index cb09f3f6ac..0e60069aeb 100644 --- a/common/resources-common/src/tinyply/tinyply.cc +++ b/common/resources-common/src/tinyply/tinyply.cc @@ -5,6 +5,7 @@ // https://github.com/ddiakopoulos/tinyply #include "resources-common/tinyply/tinyply.h" +#include namespace tinyply { @@ -156,6 +157,9 @@ void PlyFile::read_property_binary( case PlyProperty::Type::UINT32: ply_cast(dest, src.data()); break; + case PlyProperty::Type::INT64: + ply_cast(dest, src.data()); + break; case PlyProperty::Type::FLOAT32: ply_cast_float(dest, src.data()); break; @@ -189,6 +193,9 @@ void PlyFile::read_property_ascii( case PlyProperty::Type::UINT32: ply_cast_ascii(dest, is); break; + case PlyProperty::Type::INT64: + ply_cast_ascii(dest, is); + break; case PlyProperty::Type::FLOAT32: ply_cast_ascii(dest, is); break; @@ -222,6 +229,9 @@ void PlyFile::write_property_ascii( case PlyProperty::Type::UINT32: os << *reinterpret_cast(src); break; + case PlyProperty::Type::INT64: + os << *reinterpret_cast(src); + break; case PlyProperty::Type::FLOAT32: os << *reinterpret_cast(src); break;