From 192445fb771f3fe0c2cf93a3f2ffc38dbe510c66 Mon Sep 17 00:00:00 2001 From: yaoshihong Date: Thu, 13 Jul 2023 18:35:39 +0800 Subject: [PATCH 01/12] ADD: start and end vertocal_fov_angle --- .../frustum_models/depth_camera_frustum.hpp | 4 ++-- .../three_dimensional_lidar_frustum.hpp | 8 +++---- .../measurement_buffer.hpp | 5 +++-- .../measurement_reading.h | 10 +++++---- src/frustum_models/depth_camera_frustum.cpp | 12 +++++------ .../three_dimensional_lidar_frustum.cpp | 21 +++++++++++++------ src/measurement_buffer.cpp | 9 +++++--- src/spatio_temporal_voxel_grid.cpp | 6 ++++-- src/spatio_temporal_voxel_layer.cpp | 11 ++++++---- 9 files changed, 53 insertions(+), 33 deletions(-) diff --git a/include/spatio_temporal_voxel_layer/frustum_models/depth_camera_frustum.hpp b/include/spatio_temporal_voxel_layer/frustum_models/depth_camera_frustum.hpp index 661705d5..ebba4bd5 100644 --- a/include/spatio_temporal_voxel_layer/frustum_models/depth_camera_frustum.hpp +++ b/include/spatio_temporal_voxel_layer/frustum_models/depth_camera_frustum.hpp @@ -53,7 +53,7 @@ namespace geometry class DepthCameraFrustum : public Frustum { public: - DepthCameraFrustum(const double& vFOV, const double& hFOV, + DepthCameraFrustum(const double& vSFOV, const double& vEFOV, const double& hFOV, const double& min_dist, const double& max_dist); virtual ~DepthCameraFrustum(void); @@ -73,7 +73,7 @@ class DepthCameraFrustum : public Frustum double Dot(const VectorWithPt3D&, const openvdb::Vec3d&) const; double Dot(const VectorWithPt3D&, const Eigen::Vector3d&) const; - double _vFOV, _hFOV, _min_d, _max_d; + double _vSFOV, _hFOV, _min_d, _max_d; std::vector _plane_normals; Eigen::Vector3d _position; Eigen::Quaterniond _orientation; diff --git a/include/spatio_temporal_voxel_layer/frustum_models/three_dimensional_lidar_frustum.hpp b/include/spatio_temporal_voxel_layer/frustum_models/three_dimensional_lidar_frustum.hpp index 86d2a8c0..c2cbb52d 100644 --- a/include/spatio_temporal_voxel_layer/frustum_models/three_dimensional_lidar_frustum.hpp +++ b/include/spatio_temporal_voxel_layer/frustum_models/three_dimensional_lidar_frustum.hpp @@ -52,7 +52,7 @@ namespace geometry class ThreeDimensionalLidarFrustum : public Frustum { public: - ThreeDimensionalLidarFrustum(const double& vFOV, const double& vFOVPadding, + ThreeDimensionalLidarFrustum(const double& vEFOV,const double& vSFOV, const double& vFOVPadding, const double& hFOV, const double& min_dist, const double& max_dist); virtual ~ThreeDimensionalLidarFrustum(void); @@ -71,11 +71,11 @@ class ThreeDimensionalLidarFrustum : public Frustum double Dot(const VectorWithPt3D&, const openvdb::Vec3d&) const; double Dot(const VectorWithPt3D&, const Eigen::Vector3d&) const; - double _vFOV, _vFOVPadding, _hFOV, _min_d, _max_d; + double _vSFOV, _vEFOV, _vFOVPadding, _hFOV, _min_d, _max_d; double _hFOVhalf; double _min_d_squared, _max_d_squared; - double _tan_vFOVhalf; - double _tan_vFOVhalf_squared; + double _tan_vSFOV, _tan_vEFOV; + double _tan_vSFOV_squared, _tan_vEFOV_squared; Eigen::Vector3d _position; Eigen::Quaterniond _orientation; Eigen::Quaterniond _orientation_conjugate; diff --git a/include/spatio_temporal_voxel_layer/measurement_buffer.hpp b/include/spatio_temporal_voxel_layer/measurement_buffer.hpp index ecd405c9..51bd9021 100644 --- a/include/spatio_temporal_voxel_layer/measurement_buffer.hpp +++ b/include/spatio_temporal_voxel_layer/measurement_buffer.hpp @@ -83,7 +83,8 @@ class MeasurementBuffer const double& tf_tolerance, \ const double& min_d, \ const double& max_d, \ - const double& vFOV, \ + const double& vSFOV, \ + const double& vEFOV, \ const double& vFOVPadding, \ const double& hFOV, \ const double& decay_acceleration, \ @@ -129,7 +130,7 @@ class MeasurementBuffer std::string _global_frame, _topic_name, _sensor_frame; std::list _observation_list; double _min_obstacle_height, _max_obstacle_height, _obstacle_range, _tf_tolerance; - double _min_z, _max_z, _vertical_fov, _vertical_fov_padding, _horizontal_fov; + double _min_z, _max_z, _vertical_start_fov, _vertical_end_fov, _vertical_fov_padding, _horizontal_fov; double _decay_acceleration, _voxel_size; bool _marking, _clearing, _voxel_filter, _clear_buffer_after_reading, _enabled; ModelType _model_type; diff --git a/include/spatio_temporal_voxel_layer/measurement_reading.h b/include/spatio_temporal_voxel_layer/measurement_reading.h index 24e9a756..f59fee58 100644 --- a/include/spatio_temporal_voxel_layer/measurement_reading.h +++ b/include/spatio_temporal_voxel_layer/measurement_reading.h @@ -66,7 +66,7 @@ struct MeasurementReading /*****************************************************************************/ MeasurementReading(geometry_msgs::Point& origin, pcl::PointCloud cloud, \ - double obstacle_range, double min_z, double max_z, double vFOV, + double obstacle_range, double min_z, double max_z, double vSFOV, double vEFOV, double vFOVPadding, double hFOV, double decay_acceleration, bool marking, bool clearing, ModelType model_type) : /*****************************************************************************/ @@ -75,7 +75,8 @@ struct MeasurementReading _obstacle_range_in_m(obstacle_range), \ _min_z_in_m(min_z), \ _max_z_in_m(max_z), \ - _vertical_fov_in_rad(vFOV), \ + _vertical_start_fov_in_rad(vSFOV), \ + _vertical_end_fov_in_rad(vEFOV), \ _vertical_fov_padding_in_m(vFOVPadding), \ _horizontal_fov_in_rad(hFOV), \ _decay_acceleration(decay_acceleration), \ @@ -101,7 +102,8 @@ struct MeasurementReading _obstacle_range_in_m(obs._obstacle_range_in_m), \ _min_z_in_m(obs._min_z_in_m), \ _max_z_in_m(obs._max_z_in_m), \ - _vertical_fov_in_rad(obs._vertical_fov_in_rad), \ + _vertical_start_fov_in_rad(obs._vertical_start_fov_in_rad),\ + _vertical_end_fov_in_rad(obs._vertical_end_fov_in_rad), \ _vertical_fov_padding_in_m(obs._vertical_fov_padding_in_m),\ _horizontal_fov_in_rad(obs._horizontal_fov_in_rad), \ _marking(obs._marking), \ @@ -116,7 +118,7 @@ struct MeasurementReading geometry_msgs::Quaternion _orientation; pcl::PointCloud::Ptr _cloud; double _obstacle_range_in_m, _min_z_in_m, _max_z_in_m; - double _vertical_fov_in_rad, _vertical_fov_padding_in_m, _horizontal_fov_in_rad; + double _vertical_start_fov_in_rad,_vertical_end_fov_in_rad, _vertical_fov_padding_in_m, _horizontal_fov_in_rad; double _marking, _clearing, _decay_acceleration; ModelType _model_type; diff --git a/src/frustum_models/depth_camera_frustum.cpp b/src/frustum_models/depth_camera_frustum.cpp index 3ae5aa01..72032f14 100644 --- a/src/frustum_models/depth_camera_frustum.cpp +++ b/src/frustum_models/depth_camera_frustum.cpp @@ -41,9 +41,9 @@ namespace geometry { /*****************************************************************************/ -DepthCameraFrustum::DepthCameraFrustum(const double& vFOV, const double& hFOV, +DepthCameraFrustum::DepthCameraFrustum(const double& vSFOV, const double& vEFOV, const double& hFOV, const double& min_dist, const double& max_dist) : - _vFOV(vFOV), _hFOV(hFOV), _min_d(min_dist), _max_d(max_dist) + _vSFOV(vSFOV), _hFOV(hFOV), _min_d(min_dist), _max_d(max_dist) /*****************************************************************************/ { _valid_frustum = false; @@ -68,7 +68,7 @@ void DepthCameraFrustum::ComputePlaneNormals(void) /*****************************************************************************/ { // give ability to construct with bogus values - if (_vFOV == 0 && _hFOV == 0) + if (_vSFOV == 0 && _hFOV == 0) { _valid_frustum = false; return; @@ -81,18 +81,18 @@ void DepthCameraFrustum::ComputePlaneNormals(void) // rotate going CCW Eigen::Affine3d rx = - Eigen::Affine3d(Eigen::AngleAxisd(_vFOV/2.,Eigen::Vector3d::UnitX())); + Eigen::Affine3d(Eigen::AngleAxisd(_vSFOV/2.,Eigen::Vector3d::UnitX())); Eigen::Affine3d ry = Eigen::Affine3d(Eigen::AngleAxisd(_hFOV/2.,Eigen::Vector3d::UnitY())); deflected_vecs.push_back(rx * ry * Z); - rx = Eigen::Affine3d(Eigen::AngleAxisd(-_vFOV/2.,Eigen::Vector3d::UnitX())); + rx = Eigen::Affine3d(Eigen::AngleAxisd(-_vSFOV/2.,Eigen::Vector3d::UnitX())); deflected_vecs.push_back(rx * ry * Z); ry = Eigen::Affine3d(Eigen::AngleAxisd(-_hFOV/2.,Eigen::Vector3d::UnitY())); deflected_vecs.push_back(rx * ry * Z); - rx = Eigen::Affine3d(Eigen::AngleAxisd( _vFOV/2.,Eigen::Vector3d::UnitX())); + rx = Eigen::Affine3d(Eigen::AngleAxisd( _vSFOV/2.,Eigen::Vector3d::UnitX())); deflected_vecs.push_back(rx * ry * Z); // get and store CCW 4 corners for each 2 planes at ends diff --git a/src/frustum_models/three_dimensional_lidar_frustum.cpp b/src/frustum_models/three_dimensional_lidar_frustum.cpp index 1095b1ea..74b1b8f4 100644 --- a/src/frustum_models/three_dimensional_lidar_frustum.cpp +++ b/src/frustum_models/three_dimensional_lidar_frustum.cpp @@ -41,12 +41,14 @@ namespace geometry { /*****************************************************************************/ -ThreeDimensionalLidarFrustum::ThreeDimensionalLidarFrustum(const double& vFOV, +ThreeDimensionalLidarFrustum::ThreeDimensionalLidarFrustum(const double& vSFOV, + const double& vEFOV, const double& vFOVPadding, const double& hFOV, const double& min_dist, const double& max_dist) - : _vFOV(vFOV), + : _vSFOV(vSFOV), + _vEFOV(vEFOV), _vFOVPadding(vFOVPadding), _hFOV(hFOV), _min_d(min_dist), @@ -54,8 +56,10 @@ ThreeDimensionalLidarFrustum::ThreeDimensionalLidarFrustum(const double& vFOV, /*****************************************************************************/ { _hFOVhalf = _hFOV / 2.0; - _tan_vFOVhalf = tan(_vFOV / 2.0); - _tan_vFOVhalf_squared = _tan_vFOVhalf * _tan_vFOVhalf; + _tan_vSFOV = tan(_vSFOV); + _tan_vEFOV = tan(_vEFOV); + _tan_vSFOV_squared = _tan_vSFOV * _tan_vSFOV; + _tan_vEFOV_squared = _tan_vEFOV * _tan_vEFOV; _min_d_squared = _min_d * _min_d; _max_d_squared = _max_d * _max_d; _full_hFOV = false; @@ -100,8 +104,13 @@ bool ThreeDimensionalLidarFrustum::IsInside(const openvdb::Vec3d &pt) } // // Check if inside frustum valid vFOV - const double v_padded = fabs(transformed_pt[2]) + _vFOVPadding; - if (( v_padded * v_padded / radial_distance_squared) > _tan_vFOVhalf_squared) + // const double v_padded = fabs(transformed_pt[2]) + _vFOVPadding; + // if (( v_padded * v_padded / radial_distance_squared) > _tan_vSFOV_squared) + // { + // return false; + // } + const double v_padded = transformed_pt[2] + _vFOVPadding; + if ((v_padded * v_padded / radial_distance_squared) > (v_padded < 1e-9 ? _tan_vSFOV_squared : _tan_vEFOV_squared)) { return false; } diff --git a/src/measurement_buffer.cpp b/src/measurement_buffer.cpp index b583f65d..92755e6c 100644 --- a/src/measurement_buffer.cpp +++ b/src/measurement_buffer.cpp @@ -53,7 +53,8 @@ MeasurementBuffer::MeasurementBuffer(const std::string& topic_name, \ const double& tf_tolerance, \ const double& min_d, \ const double& max_d, \ - const double& vFOV, \ + const double& vSFOV, \ + const double& vEFOV, \ const double& vFOVPadding, \ const double& hFOV, \ const double& decay_acceleration, \ @@ -71,7 +72,8 @@ MeasurementBuffer::MeasurementBuffer(const std::string& topic_name, \ _topic_name(topic_name), _min_obstacle_height(min_obstacle_height), _max_obstacle_height(max_obstacle_height), _obstacle_range(obstacle_range), _tf_tolerance(tf_tolerance), _min_z(min_d), _max_z(max_d), - _vertical_fov(vFOV), _vertical_fov_padding(vFOVPadding), + _vertical_start_fov(vSFOV),_vertical_end_fov(vEFOV), + _vertical_fov_padding(vFOVPadding), _horizontal_fov(hFOV), _decay_acceleration(decay_acceleration), _marking(marking), _clearing(clearing), _voxel_size(voxel_size), _voxel_filter(voxel_filter), _enabled(enabled), @@ -140,7 +142,8 @@ void MeasurementBuffer::BufferPCLCloud(const \ _observation_list.front()._obstacle_range_in_m = _obstacle_range; _observation_list.front()._min_z_in_m = _min_z; _observation_list.front()._max_z_in_m = _max_z; - _observation_list.front()._vertical_fov_in_rad = _vertical_fov; + _observation_list.front()._vertical_start_fov_in_rad = _vertical_start_fov; + _observation_list.front()._vertical_end_fov_in_rad = _vertical_end_fov; _observation_list.front()._vertical_fov_padding_in_m = _vertical_fov_padding; _observation_list.front()._horizontal_fov_in_rad = _horizontal_fov; _observation_list.front()._decay_acceleration = _decay_acceleration; diff --git a/src/spatio_temporal_voxel_grid.cpp b/src/spatio_temporal_voxel_grid.cpp index a496504c..9a63659d 100644 --- a/src/spatio_temporal_voxel_grid.cpp +++ b/src/spatio_temporal_voxel_grid.cpp @@ -125,7 +125,8 @@ void SpatioTemporalVoxelGrid::ClearFrustums(const \ geometry::Frustum* frustum; if (it->_model_type == DEPTH_CAMERA) { - frustum = new geometry::DepthCameraFrustum(it->_vertical_fov_in_rad, + frustum = new geometry::DepthCameraFrustum(it->_vertical_start_fov_in_rad, + it->_vertical_end_fov_in_rad, it->_horizontal_fov_in_rad, it->_min_z_in_m, it->_max_z_in_m); @@ -133,7 +134,8 @@ void SpatioTemporalVoxelGrid::ClearFrustums(const \ else if (it->_model_type == THREE_DIMENSIONAL_LIDAR) { frustum = new geometry::ThreeDimensionalLidarFrustum( \ - it->_vertical_fov_in_rad, + it->_vertical_start_fov_in_rad, + it->_vertical_end_fov_in_rad, it->_vertical_fov_padding_in_m, it->_horizontal_fov_in_rad, it->_min_z_in_m, diff --git a/src/spatio_temporal_voxel_layer.cpp b/src/spatio_temporal_voxel_layer.cpp index 09379384..fdce4396 100644 --- a/src/spatio_temporal_voxel_layer.cpp +++ b/src/spatio_temporal_voxel_layer.cpp @@ -145,7 +145,7 @@ void SpatioTemporalVoxelLayer::onInitialize(void) // get the parameters for the specific topic double observation_keep_time, expected_update_rate, min_obstacle_height; - double max_obstacle_height, min_z, max_z, vFOV, vFOVPadding; + double max_obstacle_height, min_z, max_z, vSFOV, vEFOV, vFOVPadding; double hFOV, decay_acceleration; std::string topic, sensor_frame, data_type; bool inf_is_valid, clearing, marking, voxel_filter, clear_after_reading, enabled; @@ -164,8 +164,10 @@ void SpatioTemporalVoxelLayer::onInitialize(void) source_node.param("min_z", min_z, 0.); // maximum distance from camera it can see source_node.param("max_z", max_z, 10.); - // vertical FOV angle in rad - source_node.param("vertical_fov_angle", vFOV, 0.7); + // vertical FOV start angle in rad + source_node.param("vertical_fov_start_angle", vSFOV, -0.35); + // vertical FOV end angle in rad + source_node.param("vertical_fov_end_angle", vEFOV, 0.35); // vertical FOV padding in meters (3D lidar frustum only) source_node.param("vertical_fov_padding", vFOVPadding, 0.0); // horizontal FOV angle in rad @@ -208,7 +210,7 @@ void SpatioTemporalVoxelLayer::onInitialize(void) (new buffer::MeasurementBuffer(topic, observation_keep_time, \ expected_update_rate, min_obstacle_height, max_obstacle_height, \ obstacle_range, *tf_, _global_frame, sensor_frame, \ - transform_tolerance, min_z, max_z, vFOV, vFOVPadding, hFOV, \ + transform_tolerance, min_z, max_z, vSFOV, vEFOV, vFOVPadding, hFOV, \ decay_acceleration, marking, clearing, _voxel_size, \ voxel_filter, enabled, clear_after_reading, model_type))); @@ -489,6 +491,7 @@ bool SpatioTemporalVoxelLayer::updateFootprint(double robot_x, double robot_y, \ touch(_transformed_footprint[i].x, _transformed_footprint[i].y, \ min_x, min_y, max_x, max_y); } + return true; } /*****************************************************************************/ From 4b4f57529230c95ccda71ee317bcd3594a546d45 Mon Sep 17 00:00:00 2001 From: yaoshihong Date: Thu, 13 Jul 2023 18:35:56 +0800 Subject: [PATCH 02/12] ADD: README --- README.md | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/README.md b/README.md index 3cb99b7d..45ab289b 100644 --- a/README.md +++ b/README.md @@ -122,12 +122,13 @@ rgbd_obstacle_layer: topic: camera1/depth/points marking: false clearing: true - min_z: 0.1 #default 0, meters - max_z: 7.0 #default 10, meters - vertical_fov_angle: 0.7 #default 0.7, radians - horizontal_fov_angle: 1.04 #default 1.04, radians - decay_acceleration: 1. #default 0, 1/s^2. If laser scanner MUST be 0 - model_type: 0 #default 0 (depth camera). Use 1 for 3D Lidar + min_z: 0.1 #default 0, meters + max_z: 7.0 #default 10, meters + vertical_fov_start_angle: 0.7 #default 0.7, radians + vertical_fov_end_angle: 0.7 #default 0.7, radians + horizontal_fov_angle: 1.04 #default 1.04, radians + decay_acceleration: 1. #default 0, 1/s^2. If laser scanner MUST be 0 + model_type: 0 #default 0 (depth camera). Use 1 for 3D Lidar ``` More configuration samples are included in the example folder, including a 3D lidar one. From b23bd9861868f7499aef867a20bde00d360813e0 Mon Sep 17 00:00:00 2001 From: yaoshihong Date: Wed, 19 Jul 2023 17:45:27 +0800 Subject: [PATCH 03/12] ADD: fix bug --- README.md | 16 ++++---- .../frustum_models/depth_camera_frustum.hpp | 4 +- .../three_dimensional_lidar_frustum.hpp | 11 ++++-- src/frustum_models/depth_camera_frustum.cpp | 12 +++--- .../three_dimensional_lidar_frustum.cpp | 38 ++++++++++++++++++- 5 files changed, 61 insertions(+), 20 deletions(-) diff --git a/README.md b/README.md index 45ab289b..76ff6e04 100644 --- a/README.md +++ b/README.md @@ -122,13 +122,15 @@ rgbd_obstacle_layer: topic: camera1/depth/points marking: false clearing: true - min_z: 0.1 #default 0, meters - max_z: 7.0 #default 10, meters - vertical_fov_start_angle: 0.7 #default 0.7, radians - vertical_fov_end_angle: 0.7 #default 0.7, radians - horizontal_fov_angle: 1.04 #default 1.04, radians - decay_acceleration: 1. #default 0, 1/s^2. If laser scanner MUST be 0 - model_type: 0 #default 0 (depth camera). Use 1 for 3D Lidar + min_z: 0.1 #default 0, meters + max_z: 7.0 #default 10, meters + vertical_fov_angle: 0.7 #default 0.7, radians + use_start_end_angle: false #default false, if set to true, the following vertical_fov_start_angle end vertical_fov_end_angle is used instead of vertical_fov_ange + vertical_fov_start_angle: -0.12 #default -0.12, radians,fov non-bisecting lidar's negative Angle + vertical_fov_end_angle: 1.0 #default 1.0, radians,fov non-bisecting lidar's positive Angle + horizontal_fov_angle: 1.04 #default 1.04, radians + decay_acceleration: 1. #default 0, 1/s^2. If laser scanner MUST be 0 + model_type: 0 #default 0 (depth camera). Use 1 for 3D Lidar ``` More configuration samples are included in the example folder, including a 3D lidar one. diff --git a/include/spatio_temporal_voxel_layer/frustum_models/depth_camera_frustum.hpp b/include/spatio_temporal_voxel_layer/frustum_models/depth_camera_frustum.hpp index ebba4bd5..661705d5 100644 --- a/include/spatio_temporal_voxel_layer/frustum_models/depth_camera_frustum.hpp +++ b/include/spatio_temporal_voxel_layer/frustum_models/depth_camera_frustum.hpp @@ -53,7 +53,7 @@ namespace geometry class DepthCameraFrustum : public Frustum { public: - DepthCameraFrustum(const double& vSFOV, const double& vEFOV, const double& hFOV, + DepthCameraFrustum(const double& vFOV, const double& hFOV, const double& min_dist, const double& max_dist); virtual ~DepthCameraFrustum(void); @@ -73,7 +73,7 @@ class DepthCameraFrustum : public Frustum double Dot(const VectorWithPt3D&, const openvdb::Vec3d&) const; double Dot(const VectorWithPt3D&, const Eigen::Vector3d&) const; - double _vSFOV, _hFOV, _min_d, _max_d; + double _vFOV, _hFOV, _min_d, _max_d; std::vector _plane_normals; Eigen::Vector3d _position; Eigen::Quaterniond _orientation; diff --git a/include/spatio_temporal_voxel_layer/frustum_models/three_dimensional_lidar_frustum.hpp b/include/spatio_temporal_voxel_layer/frustum_models/three_dimensional_lidar_frustum.hpp index c2cbb52d..f3f2fed2 100644 --- a/include/spatio_temporal_voxel_layer/frustum_models/three_dimensional_lidar_frustum.hpp +++ b/include/spatio_temporal_voxel_layer/frustum_models/three_dimensional_lidar_frustum.hpp @@ -52,7 +52,9 @@ namespace geometry class ThreeDimensionalLidarFrustum : public Frustum { public: - ThreeDimensionalLidarFrustum(const double& vEFOV,const double& vSFOV, const double& vFOVPadding, + ThreeDimensionalLidarFrustum(const double& vFOV, const double& vFOVPadding, + const double& hFOV, const double& min_dist, const double& max_dist); + ThreeDimensionalLidarFrustum(const double& vEFOV,const double& vSFOV, const double& vFOVPadding, const double& hFOV, const double& min_dist, const double& max_dist); virtual ~ThreeDimensionalLidarFrustum(void); @@ -71,11 +73,12 @@ class ThreeDimensionalLidarFrustum : public Frustum double Dot(const VectorWithPt3D&, const openvdb::Vec3d&) const; double Dot(const VectorWithPt3D&, const Eigen::Vector3d&) const; - double _vSFOV, _vEFOV, _vFOVPadding, _hFOV, _min_d, _max_d; + bool _use_start_end_fov; + double _vFOV, _vSFOV, _vEFOV, _vFOVPadding, _hFOV, _min_d, _max_d; double _hFOVhalf; double _min_d_squared, _max_d_squared; - double _tan_vSFOV, _tan_vEFOV; - double _tan_vSFOV_squared, _tan_vEFOV_squared; + double _tan_vFOVhalf, _tan_vSFOV, _tan_vEFOV; + double _tan_vFOVhalf_squared , _tan_vSFOV_squared, _tan_vEFOV_squared; Eigen::Vector3d _position; Eigen::Quaterniond _orientation; Eigen::Quaterniond _orientation_conjugate; diff --git a/src/frustum_models/depth_camera_frustum.cpp b/src/frustum_models/depth_camera_frustum.cpp index 72032f14..3ae5aa01 100644 --- a/src/frustum_models/depth_camera_frustum.cpp +++ b/src/frustum_models/depth_camera_frustum.cpp @@ -41,9 +41,9 @@ namespace geometry { /*****************************************************************************/ -DepthCameraFrustum::DepthCameraFrustum(const double& vSFOV, const double& vEFOV, const double& hFOV, +DepthCameraFrustum::DepthCameraFrustum(const double& vFOV, const double& hFOV, const double& min_dist, const double& max_dist) : - _vSFOV(vSFOV), _hFOV(hFOV), _min_d(min_dist), _max_d(max_dist) + _vFOV(vFOV), _hFOV(hFOV), _min_d(min_dist), _max_d(max_dist) /*****************************************************************************/ { _valid_frustum = false; @@ -68,7 +68,7 @@ void DepthCameraFrustum::ComputePlaneNormals(void) /*****************************************************************************/ { // give ability to construct with bogus values - if (_vSFOV == 0 && _hFOV == 0) + if (_vFOV == 0 && _hFOV == 0) { _valid_frustum = false; return; @@ -81,18 +81,18 @@ void DepthCameraFrustum::ComputePlaneNormals(void) // rotate going CCW Eigen::Affine3d rx = - Eigen::Affine3d(Eigen::AngleAxisd(_vSFOV/2.,Eigen::Vector3d::UnitX())); + Eigen::Affine3d(Eigen::AngleAxisd(_vFOV/2.,Eigen::Vector3d::UnitX())); Eigen::Affine3d ry = Eigen::Affine3d(Eigen::AngleAxisd(_hFOV/2.,Eigen::Vector3d::UnitY())); deflected_vecs.push_back(rx * ry * Z); - rx = Eigen::Affine3d(Eigen::AngleAxisd(-_vSFOV/2.,Eigen::Vector3d::UnitX())); + rx = Eigen::Affine3d(Eigen::AngleAxisd(-_vFOV/2.,Eigen::Vector3d::UnitX())); deflected_vecs.push_back(rx * ry * Z); ry = Eigen::Affine3d(Eigen::AngleAxisd(-_hFOV/2.,Eigen::Vector3d::UnitY())); deflected_vecs.push_back(rx * ry * Z); - rx = Eigen::Affine3d(Eigen::AngleAxisd( _vSFOV/2.,Eigen::Vector3d::UnitX())); + rx = Eigen::Affine3d(Eigen::AngleAxisd( _vFOV/2.,Eigen::Vector3d::UnitX())); deflected_vecs.push_back(rx * ry * Z); // get and store CCW 4 corners for each 2 planes at ends diff --git a/src/frustum_models/three_dimensional_lidar_frustum.cpp b/src/frustum_models/three_dimensional_lidar_frustum.cpp index 74b1b8f4..b3a7a81f 100644 --- a/src/frustum_models/three_dimensional_lidar_frustum.cpp +++ b/src/frustum_models/three_dimensional_lidar_frustum.cpp @@ -40,6 +40,32 @@ namespace geometry { +/*****************************************************************************/ +ThreeDimensionalLidarFrustum::ThreeDimensionalLidarFrustum(const double& vFOV, + const double& vFOVPadding, + const double& hFOV, + const double& min_dist, + const double& max_dist) + : _vFOV(vFOV), + _vFOVPadding(vFOVPadding), + _hFOV(hFOV), + _min_d(min_dist), + _max_d(max_dist) +/*****************************************************************************/ +{ + _use_start_end_fov = false; + _hFOVhalf = _hFOV / 2.0; + _tan_vFOVhalf = tan(_vFOV / 2.0); + _tan_vFOVhalf_squared = _tan_vFOVhalf * _tan_vFOVhalf; + _min_d_squared = _min_d * _min_d; + _max_d_squared = _max_d * _max_d; + _full_hFOV = false; + if (_hFOV > 6.27) + { + _full_hFOV = true; + } +} + /*****************************************************************************/ ThreeDimensionalLidarFrustum::ThreeDimensionalLidarFrustum(const double& vSFOV, const double& vEFOV, @@ -55,6 +81,7 @@ ThreeDimensionalLidarFrustum::ThreeDimensionalLidarFrustum(const double& vSFOV, _max_d(max_dist) /*****************************************************************************/ { + _use_start_end_fov = true; _hFOVhalf = _hFOV / 2.0; _tan_vSFOV = tan(_vSFOV); _tan_vEFOV = tan(_vEFOV); @@ -110,7 +137,16 @@ bool ThreeDimensionalLidarFrustum::IsInside(const openvdb::Vec3d &pt) // return false; // } const double v_padded = transformed_pt[2] + _vFOVPadding; - if ((v_padded * v_padded / radial_distance_squared) > (v_padded < 1e-9 ? _tan_vSFOV_squared : _tan_vEFOV_squared)) + double tan_vFOV_squared; + if (_use_start_end_fov) + { + tan_vFOV_squared = (v_padded < 1e-9 ? _tan_vSFOV_squared : _tan_vEFOV_squared); + } + else + { + tan_vFOV_squared = _tan_vFOVhalf_squared; + } + if ((v_padded * v_padded / radial_distance_squared) > tan_vFOV_squared) { return false; } From e5d8478d4e67a6ea05cd6ec5ed11fd44929a8b2a Mon Sep 17 00:00:00 2001 From: yaoshihong Date: Thu, 20 Jul 2023 15:48:21 +0800 Subject: [PATCH 04/12] ADD: params --- .../measurement_buffer.hpp | 27 +++++++++- .../measurement_reading.h | 31 +++++++++-- src/measurement_buffer.cpp | 53 +++++++++++++++++-- src/spatio_temporal_voxel_layer.cpp | 42 +++++++++++---- 4 files changed, 133 insertions(+), 20 deletions(-) diff --git a/include/spatio_temporal_voxel_layer/measurement_buffer.hpp b/include/spatio_temporal_voxel_layer/measurement_buffer.hpp index 51bd9021..ab49ebc0 100644 --- a/include/spatio_temporal_voxel_layer/measurement_buffer.hpp +++ b/include/spatio_temporal_voxel_layer/measurement_buffer.hpp @@ -71,6 +71,30 @@ typedef pcl::PointCloud::Ptr point_cloud_ptr; class MeasurementBuffer { public: + MeasurementBuffer(const std::string& topic_name, \ + const double& observation_keep_time, \ + const double& expected_update_rate, \ + const double& min_obstacle_height, \ + const double& max_obstacle_height, \ + const double& obstacle_range, \ + tf::TransformListener& tf, \ + const std::string& global_frame, \ + const std::string& sensor_frame, \ + const double& tf_tolerance, \ + const double& min_d, \ + const double& max_d, \ + const double& vFOV, \ + const double& vFOVPadding, \ + const double& hFOV, \ + const double& decay_acceleration, \ + const bool& marking, \ + const bool& clearing, \ + const double& voxel_size, \ + const bool& voxel_filter, \ + const bool& enabled, \ + const bool& clear_buffer_after_reading, \ + const ModelType& model_type); + MeasurementBuffer(const std::string& topic_name, \ const double& observation_keep_time, \ const double& expected_update_rate, \ @@ -130,9 +154,10 @@ class MeasurementBuffer std::string _global_frame, _topic_name, _sensor_frame; std::list _observation_list; double _min_obstacle_height, _max_obstacle_height, _obstacle_range, _tf_tolerance; - double _min_z, _max_z, _vertical_start_fov, _vertical_end_fov, _vertical_fov_padding, _horizontal_fov; + double _min_z, _max_z, _vertical_fov, _vertical_start_fov, _vertical_end_fov, _vertical_fov_padding, _horizontal_fov; double _decay_acceleration, _voxel_size; bool _marking, _clearing, _voxel_filter, _clear_buffer_after_reading, _enabled; + bool _use_start_end_fov; ModelType _model_type; }; diff --git a/include/spatio_temporal_voxel_layer/measurement_reading.h b/include/spatio_temporal_voxel_layer/measurement_reading.h index f59fee58..68592026 100644 --- a/include/spatio_temporal_voxel_layer/measurement_reading.h +++ b/include/spatio_temporal_voxel_layer/measurement_reading.h @@ -66,7 +66,7 @@ struct MeasurementReading /*****************************************************************************/ MeasurementReading(geometry_msgs::Point& origin, pcl::PointCloud cloud, \ - double obstacle_range, double min_z, double max_z, double vSFOV, double vEFOV, + double obstacle_range, double min_z, double max_z, double vFOV, double vFOVPadding, double hFOV, double decay_acceleration, bool marking, bool clearing, ModelType model_type) : /*****************************************************************************/ @@ -75,8 +75,7 @@ struct MeasurementReading _obstacle_range_in_m(obstacle_range), \ _min_z_in_m(min_z), \ _max_z_in_m(max_z), \ - _vertical_start_fov_in_rad(vSFOV), \ - _vertical_end_fov_in_rad(vEFOV), \ + _vertical_fov_in_rad(vFOV), \ _vertical_fov_padding_in_m(vFOVPadding), \ _horizontal_fov_in_rad(hFOV), \ _decay_acceleration(decay_acceleration), \ @@ -102,6 +101,7 @@ struct MeasurementReading _obstacle_range_in_m(obs._obstacle_range_in_m), \ _min_z_in_m(obs._min_z_in_m), \ _max_z_in_m(obs._max_z_in_m), \ + _vertical_fov_in_rad(obs._vertical_fov_in_rad), \ _vertical_start_fov_in_rad(obs._vertical_start_fov_in_rad),\ _vertical_end_fov_in_rad(obs._vertical_end_fov_in_rad), \ _vertical_fov_padding_in_m(obs._vertical_fov_padding_in_m),\ @@ -114,11 +114,34 @@ struct MeasurementReading { } + + /*****************************************************************************/ + MeasurementReading(geometry_msgs::Point& origin, pcl::PointCloud cloud, \ + double obstacle_range, double min_z, double max_z, double vSFOV, double vEFOV, + double vFOVPadding, double hFOV, double decay_acceleration, bool marking, + bool clearing, ModelType model_type) : + /*****************************************************************************/ + _origin(origin), \ + _cloud(new pcl::PointCloud(cloud)), \ + _obstacle_range_in_m(obstacle_range), \ + _min_z_in_m(min_z), \ + _max_z_in_m(max_z), \ + _vertical_start_fov_in_rad(vSFOV), \ + _vertical_end_fov_in_rad(vEFOV), \ + _vertical_fov_padding_in_m(vFOVPadding), \ + _horizontal_fov_in_rad(hFOV), \ + _decay_acceleration(decay_acceleration), \ + _marking(marking), \ + _clearing(clearing), \ + _model_type(model_type) + { + } + geometry_msgs::Point _origin; geometry_msgs::Quaternion _orientation; pcl::PointCloud::Ptr _cloud; double _obstacle_range_in_m, _min_z_in_m, _max_z_in_m; - double _vertical_start_fov_in_rad,_vertical_end_fov_in_rad, _vertical_fov_padding_in_m, _horizontal_fov_in_rad; + double _vertical_fov_in_rad, _vertical_start_fov_in_rad,_vertical_end_fov_in_rad, _vertical_fov_padding_in_m, _horizontal_fov_in_rad; double _marking, _clearing, _decay_acceleration; ModelType _model_type; diff --git a/src/measurement_buffer.cpp b/src/measurement_buffer.cpp index 92755e6c..58c405e9 100644 --- a/src/measurement_buffer.cpp +++ b/src/measurement_buffer.cpp @@ -40,6 +40,45 @@ namespace buffer { +/*****************************************************************************/ +MeasurementBuffer::MeasurementBuffer(const std::string& topic_name, \ + const double& observation_keep_time, \ + const double& expected_update_rate, \ + const double& min_obstacle_height, \ + const double& max_obstacle_height, \ + const double& obstacle_range, \ + tf::TransformListener& tf, \ + const std::string& global_frame, \ + const std::string& sensor_frame, \ + const double& tf_tolerance, \ + const double& min_d, \ + const double& max_d, \ + const double& vFOV, \ + const double& vFOVPadding, \ + const double& hFOV, \ + const double& decay_acceleration, \ + const bool& marking, \ + const bool& clearing, \ + const double& voxel_size, \ + const bool& voxel_filter, \ + const bool& enabled, \ + const bool& clear_buffer_after_reading, \ + const ModelType& model_type) : +/*****************************************************************************/ + _tf(tf), _observation_keep_time(observation_keep_time), + _expected_update_rate(expected_update_rate),_last_updated(ros::Time::now()), + _global_frame(global_frame), _sensor_frame(sensor_frame), + _topic_name(topic_name), _min_obstacle_height(min_obstacle_height), + _max_obstacle_height(max_obstacle_height), _obstacle_range(obstacle_range), + _tf_tolerance(tf_tolerance), _min_z(min_d), _max_z(max_d), + _vertical_fov(vFOV), _vertical_fov_padding(vFOVPadding), _use_start_end_fov(false), + _horizontal_fov(hFOV), _decay_acceleration(decay_acceleration), + _marking(marking), _clearing(clearing), _voxel_size(voxel_size), + _voxel_filter(voxel_filter), _enabled(enabled), + _clear_buffer_after_reading(clear_buffer_after_reading), + _model_type(model_type) +{ +} /*****************************************************************************/ MeasurementBuffer::MeasurementBuffer(const std::string& topic_name, \ const double& observation_keep_time, \ @@ -72,7 +111,7 @@ MeasurementBuffer::MeasurementBuffer(const std::string& topic_name, \ _topic_name(topic_name), _min_obstacle_height(min_obstacle_height), _max_obstacle_height(max_obstacle_height), _obstacle_range(obstacle_range), _tf_tolerance(tf_tolerance), _min_z(min_d), _max_z(max_d), - _vertical_start_fov(vSFOV),_vertical_end_fov(vEFOV), + _vertical_start_fov(vSFOV),_vertical_end_fov(vEFOV), _use_start_end_fov(true), _vertical_fov_padding(vFOVPadding), _horizontal_fov(hFOV), _decay_acceleration(decay_acceleration), _marking(marking), _clearing(clearing), _voxel_size(voxel_size), @@ -142,8 +181,15 @@ void MeasurementBuffer::BufferPCLCloud(const \ _observation_list.front()._obstacle_range_in_m = _obstacle_range; _observation_list.front()._min_z_in_m = _min_z; _observation_list.front()._max_z_in_m = _max_z; - _observation_list.front()._vertical_start_fov_in_rad = _vertical_start_fov; - _observation_list.front()._vertical_end_fov_in_rad = _vertical_end_fov; + if (_use_start_end_fov) + { + _observation_list.front()._vertical_start_fov_in_rad = _vertical_start_fov; + _observation_list.front()._vertical_end_fov_in_rad = _vertical_end_fov; + } + else + { + _observation_list.front()._vertical_fov_in_rad = _vertical_fov; + } _observation_list.front()._vertical_fov_padding_in_m = _vertical_fov_padding; _observation_list.front()._horizontal_fov_in_rad = _horizontal_fov; _observation_list.front()._decay_acceleration = _decay_acceleration; @@ -329,4 +375,3 @@ void MeasurementBuffer::Unlock(void) } } // namespace buffer - diff --git a/src/spatio_temporal_voxel_layer.cpp b/src/spatio_temporal_voxel_layer.cpp index fdce4396..59be7663 100644 --- a/src/spatio_temporal_voxel_layer.cpp +++ b/src/spatio_temporal_voxel_layer.cpp @@ -145,10 +145,10 @@ void SpatioTemporalVoxelLayer::onInitialize(void) // get the parameters for the specific topic double observation_keep_time, expected_update_rate, min_obstacle_height; - double max_obstacle_height, min_z, max_z, vSFOV, vEFOV, vFOVPadding; + double max_obstacle_height, min_z, max_z, vFOV, vSFOV, vEFOV, vFOVPadding; double hFOV, decay_acceleration; std::string topic, sensor_frame, data_type; - bool inf_is_valid, clearing, marking, voxel_filter, clear_after_reading, enabled; + bool inf_is_valid, clearing, marking, voxel_filter, clear_after_reading, enabled, use_start_end_fov; source_node.param("topic", topic, source); source_node.param("sensor_frame", sensor_frame, std::string("")); @@ -164,6 +164,10 @@ void SpatioTemporalVoxelLayer::onInitialize(void) source_node.param("min_z", min_z, 0.); // maximum distance from camera it can see source_node.param("max_z", max_z, 10.); + // vertical FOV angle in rad + source_node.param("vertical_fov_angle", vFOV, 0.7); + // use start and end of vertical FOV instead of center + source_node.param("use_start_end_fov", use_start_end_fov, false); // vertical FOV start angle in rad source_node.param("vertical_fov_start_angle", vSFOV, -0.35); // vertical FOV end angle in rad @@ -204,15 +208,31 @@ void SpatioTemporalVoxelLayer::onInitialize(void) source_node.getParam(obstacle_range_param_name, obstacle_range); } - // create an observation buffer - _observation_buffers.push_back( - boost::shared_ptr - (new buffer::MeasurementBuffer(topic, observation_keep_time, \ - expected_update_rate, min_obstacle_height, max_obstacle_height, \ - obstacle_range, *tf_, _global_frame, sensor_frame, \ - transform_tolerance, min_z, max_z, vSFOV, vEFOV, vFOVPadding, hFOV, \ - decay_acceleration, marking, clearing, _voxel_size, \ - voxel_filter, enabled, clear_after_reading, model_type))); + if (model_type == ModelType::THREE_DIMENSIONAL_LIDAR && use_start_end_fov) + { + // create an observation buffer + _observation_buffers.push_back( + boost::shared_ptr + (new buffer::MeasurementBuffer(topic, observation_keep_time, \ + expected_update_rate, min_obstacle_height, max_obstacle_height, \ + obstacle_range, *tf_, _global_frame, sensor_frame, \ + transform_tolerance, min_z, max_z, vSFOV, vEFOV, vFOVPadding, hFOV, \ + decay_acceleration, marking, clearing, _voxel_size, \ + voxel_filter, enabled, clear_after_reading, model_type))); + } + // use FOV + else if (model_type == ModelType::DEPTH_CAMERA) + { + // create an observation buffer + _observation_buffers.push_back( + boost::shared_ptr + (new buffer::MeasurementBuffer(topic, observation_keep_time, \ + expected_update_rate, min_obstacle_height, max_obstacle_height, \ + obstacle_range, *tf_, _global_frame, sensor_frame, \ + transform_tolerance, min_z, max_z, vFOV, vFOVPadding, hFOV, \ + decay_acceleration, marking, clearing, _voxel_size, \ + voxel_filter, enabled, clear_after_reading, model_type))); + } // Add buffer to marking observation buffers if (marking == true) From c127311159fc32e26e521a945342323da9a49539 Mon Sep 17 00:00:00 2001 From: yaoshihong Date: Thu, 20 Jul 2023 15:52:57 +0800 Subject: [PATCH 05/12] ADD: Change the default values of start and end --- src/spatio_temporal_voxel_layer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/spatio_temporal_voxel_layer.cpp b/src/spatio_temporal_voxel_layer.cpp index 59be7663..b838375f 100644 --- a/src/spatio_temporal_voxel_layer.cpp +++ b/src/spatio_temporal_voxel_layer.cpp @@ -169,9 +169,9 @@ void SpatioTemporalVoxelLayer::onInitialize(void) // use start and end of vertical FOV instead of center source_node.param("use_start_end_fov", use_start_end_fov, false); // vertical FOV start angle in rad - source_node.param("vertical_fov_start_angle", vSFOV, -0.35); + source_node.param("vertical_fov_start_angle", vSFOV, -0.12); // vertical FOV end angle in rad - source_node.param("vertical_fov_end_angle", vEFOV, 0.35); + source_node.param("vertical_fov_end_angle", vEFOV, 1.0); // vertical FOV padding in meters (3D lidar frustum only) source_node.param("vertical_fov_padding", vFOVPadding, 0.0); // horizontal FOV angle in rad From 220dc697dea118e24231b3cdaf1fc053f62e1a94 Mon Sep 17 00:00:00 2001 From: yaoshihong Date: Thu, 20 Jul 2023 15:58:32 +0800 Subject: [PATCH 06/12] ADD: fix depth camera bug --- src/spatio_temporal_voxel_grid.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/spatio_temporal_voxel_grid.cpp b/src/spatio_temporal_voxel_grid.cpp index 9a63659d..1826ae6b 100644 --- a/src/spatio_temporal_voxel_grid.cpp +++ b/src/spatio_temporal_voxel_grid.cpp @@ -125,8 +125,7 @@ void SpatioTemporalVoxelGrid::ClearFrustums(const \ geometry::Frustum* frustum; if (it->_model_type == DEPTH_CAMERA) { - frustum = new geometry::DepthCameraFrustum(it->_vertical_start_fov_in_rad, - it->_vertical_end_fov_in_rad, + frustum = new geometry::DepthCameraFrustum(it->_vertical_fov_in_rad, it->_horizontal_fov_in_rad, it->_min_z_in_m, it->_max_z_in_m); From c5fd3ea91e0a125f04f55e5d7ba9506981d8c2a6 Mon Sep 17 00:00:00 2001 From: yaoshihong Date: Thu, 20 Jul 2023 18:09:59 +0800 Subject: [PATCH 07/12] ADD: fix MeasurementReading and layer bug --- .../measurement_reading.h | 11 +++++--- src/spatio_temporal_voxel_grid.cpp | 26 ++++++++++++++----- src/spatio_temporal_voxel_layer.cpp | 6 ++--- 3 files changed, 29 insertions(+), 14 deletions(-) diff --git a/include/spatio_temporal_voxel_layer/measurement_reading.h b/include/spatio_temporal_voxel_layer/measurement_reading.h index 68592026..06599b9a 100644 --- a/include/spatio_temporal_voxel_layer/measurement_reading.h +++ b/include/spatio_temporal_voxel_layer/measurement_reading.h @@ -81,7 +81,8 @@ struct MeasurementReading _decay_acceleration(decay_acceleration), \ _marking(marking), \ _clearing(clearing), \ - _model_type(model_type) + _model_type(model_type), \ + _use_start_end_fov(false) { } @@ -110,7 +111,8 @@ struct MeasurementReading _clearing(obs._clearing), \ _orientation(obs._orientation), \ _decay_acceleration(obs._decay_acceleration), \ - _model_type(obs._model_type) + _model_type(obs._model_type), \ + _use_start_end_fov(obs._use_start_end_fov) { } @@ -133,7 +135,8 @@ struct MeasurementReading _decay_acceleration(decay_acceleration), \ _marking(marking), \ _clearing(clearing), \ - _model_type(model_type) + _model_type(model_type), \ + _use_start_end_fov(false) { } @@ -144,7 +147,7 @@ struct MeasurementReading double _vertical_fov_in_rad, _vertical_start_fov_in_rad,_vertical_end_fov_in_rad, _vertical_fov_padding_in_m, _horizontal_fov_in_rad; double _marking, _clearing, _decay_acceleration; ModelType _model_type; - + bool _use_start_end_fov; }; } // end namespace diff --git a/src/spatio_temporal_voxel_grid.cpp b/src/spatio_temporal_voxel_grid.cpp index 1826ae6b..bffd3dd1 100644 --- a/src/spatio_temporal_voxel_grid.cpp +++ b/src/spatio_temporal_voxel_grid.cpp @@ -132,13 +132,25 @@ void SpatioTemporalVoxelGrid::ClearFrustums(const \ } else if (it->_model_type == THREE_DIMENSIONAL_LIDAR) { - frustum = new geometry::ThreeDimensionalLidarFrustum( \ - it->_vertical_start_fov_in_rad, - it->_vertical_end_fov_in_rad, - it->_vertical_fov_padding_in_m, - it->_horizontal_fov_in_rad, - it->_min_z_in_m, - it->_max_z_in_m); + if (it->_use_start_end_fov) + { + frustum = new geometry::ThreeDimensionalLidarFrustum( \ + it->_vertical_start_fov_in_rad, + it->_vertical_end_fov_in_rad, + it->_vertical_fov_padding_in_m, + it->_horizontal_fov_in_rad, + it->_min_z_in_m, + it->_max_z_in_m); + } + else + { + frustum = new geometry::ThreeDimensionalLidarFrustum( \ + it->_vertical_fov_in_rad, + it->_vertical_fov_padding_in_m, + it->_horizontal_fov_in_rad, + it->_min_z_in_m, + it->_max_z_in_m); + } } else { diff --git a/src/spatio_temporal_voxel_layer.cpp b/src/spatio_temporal_voxel_layer.cpp index b838375f..51b8549e 100644 --- a/src/spatio_temporal_voxel_layer.cpp +++ b/src/spatio_temporal_voxel_layer.cpp @@ -148,7 +148,7 @@ void SpatioTemporalVoxelLayer::onInitialize(void) double max_obstacle_height, min_z, max_z, vFOV, vSFOV, vEFOV, vFOVPadding; double hFOV, decay_acceleration; std::string topic, sensor_frame, data_type; - bool inf_is_valid, clearing, marking, voxel_filter, clear_after_reading, enabled, use_start_end_fov; + bool inf_is_valid, clearing, marking, voxel_filter, clear_after_reading, enabled, use_start_end_angle; source_node.param("topic", topic, source); source_node.param("sensor_frame", sensor_frame, std::string("")); @@ -167,7 +167,7 @@ void SpatioTemporalVoxelLayer::onInitialize(void) // vertical FOV angle in rad source_node.param("vertical_fov_angle", vFOV, 0.7); // use start and end of vertical FOV instead of center - source_node.param("use_start_end_fov", use_start_end_fov, false); + source_node.param("use_start_end_angle", use_start_end_angle, false); // vertical FOV start angle in rad source_node.param("vertical_fov_start_angle", vSFOV, -0.12); // vertical FOV end angle in rad @@ -208,7 +208,7 @@ void SpatioTemporalVoxelLayer::onInitialize(void) source_node.getParam(obstacle_range_param_name, obstacle_range); } - if (model_type == ModelType::THREE_DIMENSIONAL_LIDAR && use_start_end_fov) + if (model_type == ModelType::THREE_DIMENSIONAL_LIDAR && use_start_end_angle) { // create an observation buffer _observation_buffers.push_back( From 257707c1d7c882f9af02347f2004cfccb8e90abc Mon Sep 17 00:00:00 2001 From: yaoshihong Date: Mon, 24 Jul 2023 11:47:03 +0800 Subject: [PATCH 08/12] ADD: use one contructor --- .../three_dimensional_lidar_frustum.hpp | 8 +-- .../measurement_buffer.hpp | 32 ++-------- .../measurement_reading.h | 38 +++--------- .../three_dimensional_lidar_frustum.cpp | 35 +++-------- src/measurement_buffer.cpp | 59 +++---------------- src/spatio_temporal_voxel_grid.cpp | 14 +---- src/spatio_temporal_voxel_layer.cpp | 34 +++-------- 7 files changed, 43 insertions(+), 177 deletions(-) diff --git a/include/spatio_temporal_voxel_layer/frustum_models/three_dimensional_lidar_frustum.hpp b/include/spatio_temporal_voxel_layer/frustum_models/three_dimensional_lidar_frustum.hpp index f3f2fed2..1ea7b0b4 100644 --- a/include/spatio_temporal_voxel_layer/frustum_models/three_dimensional_lidar_frustum.hpp +++ b/include/spatio_temporal_voxel_layer/frustum_models/three_dimensional_lidar_frustum.hpp @@ -52,10 +52,8 @@ namespace geometry class ThreeDimensionalLidarFrustum : public Frustum { public: - ThreeDimensionalLidarFrustum(const double& vFOV, const double& vFOVPadding, - const double& hFOV, const double& min_dist, const double& max_dist); - ThreeDimensionalLidarFrustum(const double& vEFOV,const double& vSFOV, const double& vFOVPadding, - const double& hFOV, const double& min_dist, const double& max_dist); + ThreeDimensionalLidarFrustum(const double& vFOV, const bool& use_start_end_angle, const double& vEFOV,const double& vSFOV, + const double& vFOVPadding, const double& hFOV, const double& min_dist, const double& max_dist); virtual ~ThreeDimensionalLidarFrustum(void); // Does nothing in 3D lidar model @@ -73,7 +71,7 @@ class ThreeDimensionalLidarFrustum : public Frustum double Dot(const VectorWithPt3D&, const openvdb::Vec3d&) const; double Dot(const VectorWithPt3D&, const Eigen::Vector3d&) const; - bool _use_start_end_fov; + bool _use_start_end_angle; double _vFOV, _vSFOV, _vEFOV, _vFOVPadding, _hFOV, _min_d, _max_d; double _hFOVhalf; double _min_d_squared, _max_d_squared; diff --git a/include/spatio_temporal_voxel_layer/measurement_buffer.hpp b/include/spatio_temporal_voxel_layer/measurement_buffer.hpp index ab49ebc0..1bd13e5d 100644 --- a/include/spatio_temporal_voxel_layer/measurement_buffer.hpp +++ b/include/spatio_temporal_voxel_layer/measurement_buffer.hpp @@ -71,30 +71,6 @@ typedef pcl::PointCloud::Ptr point_cloud_ptr; class MeasurementBuffer { public: - MeasurementBuffer(const std::string& topic_name, \ - const double& observation_keep_time, \ - const double& expected_update_rate, \ - const double& min_obstacle_height, \ - const double& max_obstacle_height, \ - const double& obstacle_range, \ - tf::TransformListener& tf, \ - const std::string& global_frame, \ - const std::string& sensor_frame, \ - const double& tf_tolerance, \ - const double& min_d, \ - const double& max_d, \ - const double& vFOV, \ - const double& vFOVPadding, \ - const double& hFOV, \ - const double& decay_acceleration, \ - const bool& marking, \ - const bool& clearing, \ - const double& voxel_size, \ - const bool& voxel_filter, \ - const bool& enabled, \ - const bool& clear_buffer_after_reading, \ - const ModelType& model_type); - MeasurementBuffer(const std::string& topic_name, \ const double& observation_keep_time, \ const double& expected_update_rate, \ @@ -107,8 +83,10 @@ class MeasurementBuffer const double& tf_tolerance, \ const double& min_d, \ const double& max_d, \ - const double& vSFOV, \ - const double& vEFOV, \ + const double& vFOV, \ + const bool& use_start_end_angle, \ + const double& vSFOV, \ + const double& vEFOV, \ const double& vFOVPadding, \ const double& hFOV, \ const double& decay_acceleration, \ @@ -157,7 +135,7 @@ class MeasurementBuffer double _min_z, _max_z, _vertical_fov, _vertical_start_fov, _vertical_end_fov, _vertical_fov_padding, _horizontal_fov; double _decay_acceleration, _voxel_size; bool _marking, _clearing, _voxel_filter, _clear_buffer_after_reading, _enabled; - bool _use_start_end_fov; + bool _use_start_end_angle; ModelType _model_type; }; diff --git a/include/spatio_temporal_voxel_layer/measurement_reading.h b/include/spatio_temporal_voxel_layer/measurement_reading.h index 06599b9a..675c0bac 100644 --- a/include/spatio_temporal_voxel_layer/measurement_reading.h +++ b/include/spatio_temporal_voxel_layer/measurement_reading.h @@ -66,9 +66,9 @@ struct MeasurementReading /*****************************************************************************/ MeasurementReading(geometry_msgs::Point& origin, pcl::PointCloud cloud, \ - double obstacle_range, double min_z, double max_z, double vFOV, - double vFOVPadding, double hFOV, double decay_acceleration, bool marking, - bool clearing, ModelType model_type) : + double obstacle_range, double min_z, double max_z, double vFOV, double vSFOV, + double vEFOV, double vFOVPadding, double hFOV, double decay_acceleration, + bool marking, bool clearing, ModelType model_type) : /*****************************************************************************/ _origin(origin), \ _cloud(new pcl::PointCloud(cloud)), \ @@ -76,13 +76,14 @@ struct MeasurementReading _min_z_in_m(min_z), \ _max_z_in_m(max_z), \ _vertical_fov_in_rad(vFOV), \ + _vertical_start_fov_in_rad(vSFOV), \ + _vertical_end_fov_in_rad(vEFOV), \ _vertical_fov_padding_in_m(vFOVPadding), \ _horizontal_fov_in_rad(hFOV), \ _decay_acceleration(decay_acceleration), \ _marking(marking), \ _clearing(clearing), \ - _model_type(model_type), \ - _use_start_end_fov(false) + _model_type(model_type) { } @@ -112,34 +113,11 @@ struct MeasurementReading _orientation(obs._orientation), \ _decay_acceleration(obs._decay_acceleration), \ _model_type(obs._model_type), \ - _use_start_end_fov(obs._use_start_end_fov) + _use_start_end_angle(obs._use_start_end_angle) { } - /*****************************************************************************/ - MeasurementReading(geometry_msgs::Point& origin, pcl::PointCloud cloud, \ - double obstacle_range, double min_z, double max_z, double vSFOV, double vEFOV, - double vFOVPadding, double hFOV, double decay_acceleration, bool marking, - bool clearing, ModelType model_type) : - /*****************************************************************************/ - _origin(origin), \ - _cloud(new pcl::PointCloud(cloud)), \ - _obstacle_range_in_m(obstacle_range), \ - _min_z_in_m(min_z), \ - _max_z_in_m(max_z), \ - _vertical_start_fov_in_rad(vSFOV), \ - _vertical_end_fov_in_rad(vEFOV), \ - _vertical_fov_padding_in_m(vFOVPadding), \ - _horizontal_fov_in_rad(hFOV), \ - _decay_acceleration(decay_acceleration), \ - _marking(marking), \ - _clearing(clearing), \ - _model_type(model_type), \ - _use_start_end_fov(false) - { - } - geometry_msgs::Point _origin; geometry_msgs::Quaternion _orientation; pcl::PointCloud::Ptr _cloud; @@ -147,7 +125,7 @@ struct MeasurementReading double _vertical_fov_in_rad, _vertical_start_fov_in_rad,_vertical_end_fov_in_rad, _vertical_fov_padding_in_m, _horizontal_fov_in_rad; double _marking, _clearing, _decay_acceleration; ModelType _model_type; - bool _use_start_end_fov; + bool _use_start_end_angle; }; } // end namespace diff --git a/src/frustum_models/three_dimensional_lidar_frustum.cpp b/src/frustum_models/three_dimensional_lidar_frustum.cpp index b3a7a81f..aff20e58 100644 --- a/src/frustum_models/three_dimensional_lidar_frustum.cpp +++ b/src/frustum_models/three_dimensional_lidar_frustum.cpp @@ -42,47 +42,26 @@ namespace geometry /*****************************************************************************/ ThreeDimensionalLidarFrustum::ThreeDimensionalLidarFrustum(const double& vFOV, + const bool& use_start_end_angle, + const double& vSFOV, + const double& vEFOV, const double& vFOVPadding, const double& hFOV, const double& min_dist, const double& max_dist) : _vFOV(vFOV), + _use_start_end_angle(use_start_end_angle), + _vSFOV(vSFOV), + _vEFOV(vEFOV), _vFOVPadding(vFOVPadding), _hFOV(hFOV), _min_d(min_dist), _max_d(max_dist) /*****************************************************************************/ { - _use_start_end_fov = false; _hFOVhalf = _hFOV / 2.0; _tan_vFOVhalf = tan(_vFOV / 2.0); _tan_vFOVhalf_squared = _tan_vFOVhalf * _tan_vFOVhalf; - _min_d_squared = _min_d * _min_d; - _max_d_squared = _max_d * _max_d; - _full_hFOV = false; - if (_hFOV > 6.27) - { - _full_hFOV = true; - } -} - -/*****************************************************************************/ -ThreeDimensionalLidarFrustum::ThreeDimensionalLidarFrustum(const double& vSFOV, - const double& vEFOV, - const double& vFOVPadding, - const double& hFOV, - const double& min_dist, - const double& max_dist) - : _vSFOV(vSFOV), - _vEFOV(vEFOV), - _vFOVPadding(vFOVPadding), - _hFOV(hFOV), - _min_d(min_dist), - _max_d(max_dist) -/*****************************************************************************/ -{ - _use_start_end_fov = true; - _hFOVhalf = _hFOV / 2.0; _tan_vSFOV = tan(_vSFOV); _tan_vEFOV = tan(_vEFOV); _tan_vSFOV_squared = _tan_vSFOV * _tan_vSFOV; @@ -138,7 +117,7 @@ bool ThreeDimensionalLidarFrustum::IsInside(const openvdb::Vec3d &pt) // } const double v_padded = transformed_pt[2] + _vFOVPadding; double tan_vFOV_squared; - if (_use_start_end_fov) + if (_use_start_end_angle) { tan_vFOV_squared = (v_padded < 1e-9 ? _tan_vSFOV_squared : _tan_vEFOV_squared); } diff --git a/src/measurement_buffer.cpp b/src/measurement_buffer.cpp index 58c405e9..bef3bcce 100644 --- a/src/measurement_buffer.cpp +++ b/src/measurement_buffer.cpp @@ -54,6 +54,9 @@ MeasurementBuffer::MeasurementBuffer(const std::string& topic_name, \ const double& min_d, \ const double& max_d, \ const double& vFOV, \ + const bool& use_start_end_angle, \ + const double& vSFOV, \ + const double& vEFOV, \ const double& vFOVPadding, \ const double& hFOV, \ const double& decay_acceleration, \ @@ -71,47 +74,8 @@ MeasurementBuffer::MeasurementBuffer(const std::string& topic_name, \ _topic_name(topic_name), _min_obstacle_height(min_obstacle_height), _max_obstacle_height(max_obstacle_height), _obstacle_range(obstacle_range), _tf_tolerance(tf_tolerance), _min_z(min_d), _max_z(max_d), - _vertical_fov(vFOV), _vertical_fov_padding(vFOVPadding), _use_start_end_fov(false), - _horizontal_fov(hFOV), _decay_acceleration(decay_acceleration), - _marking(marking), _clearing(clearing), _voxel_size(voxel_size), - _voxel_filter(voxel_filter), _enabled(enabled), - _clear_buffer_after_reading(clear_buffer_after_reading), - _model_type(model_type) -{ -} -/*****************************************************************************/ -MeasurementBuffer::MeasurementBuffer(const std::string& topic_name, \ - const double& observation_keep_time, \ - const double& expected_update_rate, \ - const double& min_obstacle_height, \ - const double& max_obstacle_height, \ - const double& obstacle_range, \ - tf::TransformListener& tf, \ - const std::string& global_frame, \ - const std::string& sensor_frame, \ - const double& tf_tolerance, \ - const double& min_d, \ - const double& max_d, \ - const double& vSFOV, \ - const double& vEFOV, \ - const double& vFOVPadding, \ - const double& hFOV, \ - const double& decay_acceleration, \ - const bool& marking, \ - const bool& clearing, \ - const double& voxel_size, \ - const bool& voxel_filter, \ - const bool& enabled, \ - const bool& clear_buffer_after_reading, \ - const ModelType& model_type) : -/*****************************************************************************/ - _tf(tf), _observation_keep_time(observation_keep_time), - _expected_update_rate(expected_update_rate),_last_updated(ros::Time::now()), - _global_frame(global_frame), _sensor_frame(sensor_frame), - _topic_name(topic_name), _min_obstacle_height(min_obstacle_height), - _max_obstacle_height(max_obstacle_height), _obstacle_range(obstacle_range), - _tf_tolerance(tf_tolerance), _min_z(min_d), _max_z(max_d), - _vertical_start_fov(vSFOV),_vertical_end_fov(vEFOV), _use_start_end_fov(true), + _vertical_fov(vFOV), _use_start_end_angle(use_start_end_angle && model_type == ModelType::THREE_DIMENSIONAL_LIDAR), + _vertical_start_fov(vSFOV),_vertical_end_fov(vEFOV), _vertical_fov_padding(vFOVPadding), _horizontal_fov(hFOV), _decay_acceleration(decay_acceleration), _marking(marking), _clearing(clearing), _voxel_size(voxel_size), @@ -181,15 +145,10 @@ void MeasurementBuffer::BufferPCLCloud(const \ _observation_list.front()._obstacle_range_in_m = _obstacle_range; _observation_list.front()._min_z_in_m = _min_z; _observation_list.front()._max_z_in_m = _max_z; - if (_use_start_end_fov) - { - _observation_list.front()._vertical_start_fov_in_rad = _vertical_start_fov; - _observation_list.front()._vertical_end_fov_in_rad = _vertical_end_fov; - } - else - { - _observation_list.front()._vertical_fov_in_rad = _vertical_fov; - } + _observation_list.front()._vertical_fov_in_rad = _vertical_fov; + _observation_list.front()._use_start_end_angle = _use_start_end_angle; + _observation_list.front()._vertical_start_fov_in_rad = _vertical_start_fov; + _observation_list.front()._vertical_end_fov_in_rad = _vertical_end_fov; _observation_list.front()._vertical_fov_padding_in_m = _vertical_fov_padding; _observation_list.front()._horizontal_fov_in_rad = _horizontal_fov; _observation_list.front()._decay_acceleration = _decay_acceleration; diff --git a/src/spatio_temporal_voxel_grid.cpp b/src/spatio_temporal_voxel_grid.cpp index bffd3dd1..9a17cbf7 100644 --- a/src/spatio_temporal_voxel_grid.cpp +++ b/src/spatio_temporal_voxel_grid.cpp @@ -132,25 +132,15 @@ void SpatioTemporalVoxelGrid::ClearFrustums(const \ } else if (it->_model_type == THREE_DIMENSIONAL_LIDAR) { - if (it->_use_start_end_fov) - { frustum = new geometry::ThreeDimensionalLidarFrustum( \ + it->_vertical_fov_in_rad, + it->_use_start_end_angle, it->_vertical_start_fov_in_rad, it->_vertical_end_fov_in_rad, it->_vertical_fov_padding_in_m, it->_horizontal_fov_in_rad, it->_min_z_in_m, it->_max_z_in_m); - } - else - { - frustum = new geometry::ThreeDimensionalLidarFrustum( \ - it->_vertical_fov_in_rad, - it->_vertical_fov_padding_in_m, - it->_horizontal_fov_in_rad, - it->_min_z_in_m, - it->_max_z_in_m); - } } else { diff --git a/src/spatio_temporal_voxel_layer.cpp b/src/spatio_temporal_voxel_layer.cpp index 51b8549e..439b4abc 100644 --- a/src/spatio_temporal_voxel_layer.cpp +++ b/src/spatio_temporal_voxel_layer.cpp @@ -208,31 +208,15 @@ void SpatioTemporalVoxelLayer::onInitialize(void) source_node.getParam(obstacle_range_param_name, obstacle_range); } - if (model_type == ModelType::THREE_DIMENSIONAL_LIDAR && use_start_end_angle) - { - // create an observation buffer - _observation_buffers.push_back( - boost::shared_ptr - (new buffer::MeasurementBuffer(topic, observation_keep_time, \ - expected_update_rate, min_obstacle_height, max_obstacle_height, \ - obstacle_range, *tf_, _global_frame, sensor_frame, \ - transform_tolerance, min_z, max_z, vSFOV, vEFOV, vFOVPadding, hFOV, \ - decay_acceleration, marking, clearing, _voxel_size, \ - voxel_filter, enabled, clear_after_reading, model_type))); - } - // use FOV - else if (model_type == ModelType::DEPTH_CAMERA) - { - // create an observation buffer - _observation_buffers.push_back( - boost::shared_ptr - (new buffer::MeasurementBuffer(topic, observation_keep_time, \ - expected_update_rate, min_obstacle_height, max_obstacle_height, \ - obstacle_range, *tf_, _global_frame, sensor_frame, \ - transform_tolerance, min_z, max_z, vFOV, vFOVPadding, hFOV, \ - decay_acceleration, marking, clearing, _voxel_size, \ - voxel_filter, enabled, clear_after_reading, model_type))); - } + // create an observation buffer + _observation_buffers.push_back( + boost::shared_ptr + (new buffer::MeasurementBuffer(topic, observation_keep_time, \ + expected_update_rate, min_obstacle_height, max_obstacle_height, \ + obstacle_range, *tf_, _global_frame, sensor_frame, \ + transform_tolerance, min_z, max_z, vFOV, use_start_end_angle, vSFOV, vEFOV, vFOVPadding, hFOV, \ + decay_acceleration, marking, clearing, _voxel_size, \ + voxel_filter, enabled, clear_after_reading, model_type))); // Add buffer to marking observation buffers if (marking == true) From 3b011f3514e2c9b77d55e3be3b5b57f775372074 Mon Sep 17 00:00:00 2001 From: yaoshihong Date: Tue, 1 Aug 2023 10:20:56 +0800 Subject: [PATCH 09/12] ADD: fix bug and optimazed code --- .../three_dimensional_lidar_frustum.hpp | 5 +++-- .../three_dimensional_lidar_frustum.cpp | 11 ++++------- 2 files changed, 7 insertions(+), 9 deletions(-) diff --git a/include/spatio_temporal_voxel_layer/frustum_models/three_dimensional_lidar_frustum.hpp b/include/spatio_temporal_voxel_layer/frustum_models/three_dimensional_lidar_frustum.hpp index 1ea7b0b4..a69fadd6 100644 --- a/include/spatio_temporal_voxel_layer/frustum_models/three_dimensional_lidar_frustum.hpp +++ b/include/spatio_temporal_voxel_layer/frustum_models/three_dimensional_lidar_frustum.hpp @@ -52,8 +52,9 @@ namespace geometry class ThreeDimensionalLidarFrustum : public Frustum { public: - ThreeDimensionalLidarFrustum(const double& vFOV, const bool& use_start_end_angle, const double& vEFOV,const double& vSFOV, - const double& vFOVPadding, const double& hFOV, const double& min_dist, const double& max_dist); + ThreeDimensionalLidarFrustum(const double& vFOV, const bool& use_start_end_angle, + const double& vEFOV, const double& vSFOV, const double& vFOVPadding, + const double& hFOV, const double& min_dist, const double& max_dist); virtual ~ThreeDimensionalLidarFrustum(void); // Does nothing in 3D lidar model diff --git a/src/frustum_models/three_dimensional_lidar_frustum.cpp b/src/frustum_models/three_dimensional_lidar_frustum.cpp index aff20e58..a83e9944 100644 --- a/src/frustum_models/three_dimensional_lidar_frustum.cpp +++ b/src/frustum_models/three_dimensional_lidar_frustum.cpp @@ -115,16 +115,13 @@ bool ThreeDimensionalLidarFrustum::IsInside(const openvdb::Vec3d &pt) // { // return false; // } - const double v_padded = transformed_pt[2] + _vFOVPadding; - double tan_vFOV_squared; + const double v_padded = fabs(transformed_pt[2]) + _vFOVPadding; + double tan_vFOV_squared = _tan_vFOVhalf_squared; if (_use_start_end_angle) { - tan_vFOV_squared = (v_padded < 1e-9 ? _tan_vSFOV_squared : _tan_vEFOV_squared); - } - else - { - tan_vFOV_squared = _tan_vFOVhalf_squared; + tan_vFOV_squared = (transformed_pt[2] < 0 ? _tan_vSFOV_squared : _tan_vEFOV_squared); } + if ((v_padded * v_padded / radial_distance_squared) > tan_vFOV_squared) { return false; From 80741f983605ff93513f681a344749e0fe2a5362 Mon Sep 17 00:00:00 2001 From: yaoshihong Date: Tue, 1 Aug 2023 10:39:19 +0800 Subject: [PATCH 10/12] ADD: fix formating errors --- src/measurement_buffer.cpp | 3 ++- src/spatio_temporal_voxel_grid.cpp | 16 ++++++++-------- 2 files changed, 10 insertions(+), 9 deletions(-) diff --git a/src/measurement_buffer.cpp b/src/measurement_buffer.cpp index bef3bcce..919ad555 100644 --- a/src/measurement_buffer.cpp +++ b/src/measurement_buffer.cpp @@ -74,7 +74,8 @@ MeasurementBuffer::MeasurementBuffer(const std::string& topic_name, \ _topic_name(topic_name), _min_obstacle_height(min_obstacle_height), _max_obstacle_height(max_obstacle_height), _obstacle_range(obstacle_range), _tf_tolerance(tf_tolerance), _min_z(min_d), _max_z(max_d), - _vertical_fov(vFOV), _use_start_end_angle(use_start_end_angle && model_type == ModelType::THREE_DIMENSIONAL_LIDAR), + _vertical_fov(vFOV), _use_start_end_angle( + use_start_end_angle && model_type == ModelType::THREE_DIMENSIONAL_LIDAR), _vertical_start_fov(vSFOV),_vertical_end_fov(vEFOV), _vertical_fov_padding(vFOVPadding), _horizontal_fov(hFOV), _decay_acceleration(decay_acceleration), diff --git a/src/spatio_temporal_voxel_grid.cpp b/src/spatio_temporal_voxel_grid.cpp index 9a17cbf7..5a17f943 100644 --- a/src/spatio_temporal_voxel_grid.cpp +++ b/src/spatio_temporal_voxel_grid.cpp @@ -133,14 +133,14 @@ void SpatioTemporalVoxelGrid::ClearFrustums(const \ else if (it->_model_type == THREE_DIMENSIONAL_LIDAR) { frustum = new geometry::ThreeDimensionalLidarFrustum( \ - it->_vertical_fov_in_rad, - it->_use_start_end_angle, - it->_vertical_start_fov_in_rad, - it->_vertical_end_fov_in_rad, - it->_vertical_fov_padding_in_m, - it->_horizontal_fov_in_rad, - it->_min_z_in_m, - it->_max_z_in_m); + it->_vertical_fov_in_rad, + it->_use_start_end_angle, + it->_vertical_start_fov_in_rad, + it->_vertical_end_fov_in_rad, + it->_vertical_fov_padding_in_m, + it->_horizontal_fov_in_rad, + it->_min_z_in_m, + it->_max_z_in_m); } else { From 83219ff2c8f579ec5d167a8c0d31f036053b43df Mon Sep 17 00:00:00 2001 From: yaoshihong Date: Tue, 1 Aug 2023 10:49:02 +0800 Subject: [PATCH 11/12] ADD: fix formating errors --- include/spatio_temporal_voxel_layer/measurement_buffer.hpp | 3 ++- include/spatio_temporal_voxel_layer/measurement_reading.h | 3 ++- src/frustum_models/three_dimensional_lidar_frustum.cpp | 5 ----- src/spatio_temporal_voxel_grid.cpp | 2 +- src/spatio_temporal_voxel_layer.cpp | 6 ++++-- 5 files changed, 9 insertions(+), 10 deletions(-) diff --git a/include/spatio_temporal_voxel_layer/measurement_buffer.hpp b/include/spatio_temporal_voxel_layer/measurement_buffer.hpp index 1bd13e5d..2ecabfe5 100644 --- a/include/spatio_temporal_voxel_layer/measurement_buffer.hpp +++ b/include/spatio_temporal_voxel_layer/measurement_buffer.hpp @@ -132,7 +132,8 @@ class MeasurementBuffer std::string _global_frame, _topic_name, _sensor_frame; std::list _observation_list; double _min_obstacle_height, _max_obstacle_height, _obstacle_range, _tf_tolerance; - double _min_z, _max_z, _vertical_fov, _vertical_start_fov, _vertical_end_fov, _vertical_fov_padding, _horizontal_fov; + double _min_z, _max_z, _vertical_fov, _vertical_start_fov, _vertical_end_fov; + double _vertical_fov_padding, _horizontal_fov; double _decay_acceleration, _voxel_size; bool _marking, _clearing, _voxel_filter, _clear_buffer_after_reading, _enabled; bool _use_start_end_angle; diff --git a/include/spatio_temporal_voxel_layer/measurement_reading.h b/include/spatio_temporal_voxel_layer/measurement_reading.h index 675c0bac..784dac67 100644 --- a/include/spatio_temporal_voxel_layer/measurement_reading.h +++ b/include/spatio_temporal_voxel_layer/measurement_reading.h @@ -122,7 +122,8 @@ struct MeasurementReading geometry_msgs::Quaternion _orientation; pcl::PointCloud::Ptr _cloud; double _obstacle_range_in_m, _min_z_in_m, _max_z_in_m; - double _vertical_fov_in_rad, _vertical_start_fov_in_rad,_vertical_end_fov_in_rad, _vertical_fov_padding_in_m, _horizontal_fov_in_rad; + double _vertical_fov_in_rad, _vertical_start_fov_in_rad, _vertical_end_fov_in_rad; + double _vertical_fov_padding_in_m, _horizontal_fov_in_rad; double _marking, _clearing, _decay_acceleration; ModelType _model_type; bool _use_start_end_angle; diff --git a/src/frustum_models/three_dimensional_lidar_frustum.cpp b/src/frustum_models/three_dimensional_lidar_frustum.cpp index a83e9944..ac3735f0 100644 --- a/src/frustum_models/three_dimensional_lidar_frustum.cpp +++ b/src/frustum_models/three_dimensional_lidar_frustum.cpp @@ -110,11 +110,6 @@ bool ThreeDimensionalLidarFrustum::IsInside(const openvdb::Vec3d &pt) } // // Check if inside frustum valid vFOV - // const double v_padded = fabs(transformed_pt[2]) + _vFOVPadding; - // if (( v_padded * v_padded / radial_distance_squared) > _tan_vSFOV_squared) - // { - // return false; - // } const double v_padded = fabs(transformed_pt[2]) + _vFOVPadding; double tan_vFOV_squared = _tan_vFOVhalf_squared; if (_use_start_end_angle) diff --git a/src/spatio_temporal_voxel_grid.cpp b/src/spatio_temporal_voxel_grid.cpp index 5a17f943..31284f53 100644 --- a/src/spatio_temporal_voxel_grid.cpp +++ b/src/spatio_temporal_voxel_grid.cpp @@ -132,7 +132,7 @@ void SpatioTemporalVoxelGrid::ClearFrustums(const \ } else if (it->_model_type == THREE_DIMENSIONAL_LIDAR) { - frustum = new geometry::ThreeDimensionalLidarFrustum( \ + frustum = new geometry::ThreeDimensionalLidarFrustum( \ it->_vertical_fov_in_rad, it->_use_start_end_angle, it->_vertical_start_fov_in_rad, diff --git a/src/spatio_temporal_voxel_layer.cpp b/src/spatio_temporal_voxel_layer.cpp index 439b4abc..2cb97607 100644 --- a/src/spatio_temporal_voxel_layer.cpp +++ b/src/spatio_temporal_voxel_layer.cpp @@ -148,7 +148,8 @@ void SpatioTemporalVoxelLayer::onInitialize(void) double max_obstacle_height, min_z, max_z, vFOV, vSFOV, vEFOV, vFOVPadding; double hFOV, decay_acceleration; std::string topic, sensor_frame, data_type; - bool inf_is_valid, clearing, marking, voxel_filter, clear_after_reading, enabled, use_start_end_angle; + bool inf_is_valid, clearing, marking, voxel_filter, clear_after_reading, enabled; + bool use_start_end_angle; source_node.param("topic", topic, source); source_node.param("sensor_frame", sensor_frame, std::string("")); @@ -214,7 +215,8 @@ void SpatioTemporalVoxelLayer::onInitialize(void) (new buffer::MeasurementBuffer(topic, observation_keep_time, \ expected_update_rate, min_obstacle_height, max_obstacle_height, \ obstacle_range, *tf_, _global_frame, sensor_frame, \ - transform_tolerance, min_z, max_z, vFOV, use_start_end_angle, vSFOV, vEFOV, vFOVPadding, hFOV, \ + transform_tolerance, min_z, max_z, vFOV, use_start_end_angle, + vSFOV, vEFOV, vFOVPadding, hFOV, \ decay_acceleration, marking, clearing, _voxel_size, \ voxel_filter, enabled, clear_after_reading, model_type))); From 01f33d98f63fc226199d63918c3f3ed6643db1d2 Mon Sep 17 00:00:00 2001 From: yaoshihong Date: Tue, 1 Aug 2023 10:52:58 +0800 Subject: [PATCH 12/12] ADD: fix formating errors --- src/spatio_temporal_voxel_layer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/spatio_temporal_voxel_layer.cpp b/src/spatio_temporal_voxel_layer.cpp index 2cb97607..c3de3029 100644 --- a/src/spatio_temporal_voxel_layer.cpp +++ b/src/spatio_temporal_voxel_layer.cpp @@ -215,7 +215,7 @@ void SpatioTemporalVoxelLayer::onInitialize(void) (new buffer::MeasurementBuffer(topic, observation_keep_time, \ expected_update_rate, min_obstacle_height, max_obstacle_height, \ obstacle_range, *tf_, _global_frame, sensor_frame, \ - transform_tolerance, min_z, max_z, vFOV, use_start_end_angle, + transform_tolerance, min_z, max_z, vFOV, use_start_end_angle, \ vSFOV, vEFOV, vFOVPadding, hFOV, \ decay_acceleration, marking, clearing, _voxel_size, \ voxel_filter, enabled, clear_after_reading, model_type)));