diff --git a/README.md b/README.md index 3cb99b7d..76ff6e04 100644 --- a/README.md +++ b/README.md @@ -122,12 +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_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/three_dimensional_lidar_frustum.hpp b/include/spatio_temporal_voxel_layer/frustum_models/three_dimensional_lidar_frustum.hpp index 86d2a8c0..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 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 @@ -71,11 +72,12 @@ 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; + bool _use_start_end_angle; + double _vFOV, _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_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/include/spatio_temporal_voxel_layer/measurement_buffer.hpp b/include/spatio_temporal_voxel_layer/measurement_buffer.hpp index ecd405c9..2ecabfe5 100644 --- a/include/spatio_temporal_voxel_layer/measurement_buffer.hpp +++ b/include/spatio_temporal_voxel_layer/measurement_buffer.hpp @@ -84,6 +84,9 @@ class MeasurementBuffer 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, \ @@ -129,9 +132,11 @@ 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_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; 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..784dac67 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,6 +76,8 @@ 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), \ @@ -102,24 +104,29 @@ struct MeasurementReading _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), \ _clearing(obs._clearing), \ _orientation(obs._orientation), \ _decay_acceleration(obs._decay_acceleration), \ - _model_type(obs._model_type) + _model_type(obs._model_type), \ + _use_start_end_angle(obs._use_start_end_angle) { } + 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_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; }; } // end namespace diff --git a/src/frustum_models/three_dimensional_lidar_frustum.cpp b/src/frustum_models/three_dimensional_lidar_frustum.cpp index 1095b1ea..ac3735f0 100644 --- a/src/frustum_models/three_dimensional_lidar_frustum.cpp +++ b/src/frustum_models/three_dimensional_lidar_frustum.cpp @@ -42,11 +42,17 @@ 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), @@ -56,6 +62,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; @@ -101,7 +111,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) + double tan_vFOV_squared = _tan_vFOVhalf_squared; + if (_use_start_end_angle) + { + 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; } diff --git a/src/measurement_buffer.cpp b/src/measurement_buffer.cpp index b583f65d..919ad555 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,7 +74,10 @@ 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_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), _voxel_filter(voxel_filter), _enabled(enabled), @@ -141,6 +147,9 @@ void MeasurementBuffer::BufferPCLCloud(const \ _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()._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; @@ -326,4 +335,3 @@ void MeasurementBuffer::Unlock(void) } } // namespace buffer - diff --git a/src/spatio_temporal_voxel_grid.cpp b/src/spatio_temporal_voxel_grid.cpp index a496504c..31284f53 100644 --- a/src/spatio_temporal_voxel_grid.cpp +++ b/src/spatio_temporal_voxel_grid.cpp @@ -133,11 +133,14 @@ void SpatioTemporalVoxelGrid::ClearFrustums(const \ else if (it->_model_type == THREE_DIMENSIONAL_LIDAR) { 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); + 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 { diff --git a/src/spatio_temporal_voxel_layer.cpp b/src/spatio_temporal_voxel_layer.cpp index 09379384..c3de3029 100644 --- a/src/spatio_temporal_voxel_layer.cpp +++ b/src/spatio_temporal_voxel_layer.cpp @@ -145,10 +145,11 @@ 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, 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 use_start_end_angle; source_node.param("topic", topic, source); source_node.param("sensor_frame", sensor_frame, std::string("")); @@ -166,6 +167,12 @@ void SpatioTemporalVoxelLayer::onInitialize(void) 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_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 + 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 @@ -208,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, 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))); @@ -489,6 +497,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; } /*****************************************************************************/