Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Kinetic/fix/vertical fov bug #266

Open
wants to merge 12 commits into
base: kinetic-devel
Choose a base branch
from
Open

Kinetic/fix/vertical fov bug #266

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
12 commits
Select commit Hold shift + click to select a range
192445f
ADD: start and end vertocal_fov_angle
yShzZpp Jul 13, 2023
4b4f575
ADD: README
yShzZpp Jul 13, 2023
b23bd98
ADD: fix bug
yShzZpp Jul 19, 2023
e5d8478
ADD: params
yShzZpp Jul 20, 2023
c127311
ADD: Change the default values of start and end
yShzZpp Jul 20, 2023
220dc69
ADD: fix depth camera bug
yShzZpp Jul 20, 2023
c5fd3ea
ADD: fix MeasurementReading and layer bug
yShzZpp Jul 20, 2023
257707c
ADD: use one contructor
yShzZpp Jul 24, 2023
3b011f3
ADD: fix bug and optimazed code
yShzZpp Aug 1, 2023
80741f9
ADD: fix formating errors
yShzZpp Aug 1, 2023
83219ff
ADD: fix formating errors
yShzZpp Aug 1, 2023
01f33d9
ADD: fix formating errors
yShzZpp Aug 1, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
15 changes: 9 additions & 6 deletions README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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.

Expand Down
12 changes: 7 additions & 5 deletions include/spatio_temporal_voxel_layer/frustum_models/three_dimensional_lidar_frustum.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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;
Expand Down
7 changes: 6 additions & 1 deletion include/spatio_temporal_voxel_layer/measurement_buffer.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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, \
Expand Down Expand Up @@ -129,9 +132,11 @@ class MeasurementBuffer
std::string _global_frame, _topic_name, _sensor_frame;
std::list<observation::MeasurementReading> _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;
};

Expand Down
19 changes: 13 additions & 6 deletions include/spatio_temporal_voxel_layer/measurement_reading.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -66,16 +66,18 @@ struct MeasurementReading

/*****************************************************************************/
MeasurementReading(geometry_msgs::Point& origin, pcl::PointCloud<pcl::PointXYZ> 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<pcl::PointXYZ>(cloud)), \
_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), \
Expand All @@ -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<pcl::PointXYZ>::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
Expand Down
18 changes: 17 additions & 1 deletion src/frustum_models/three_dimensional_lidar_frustum.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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),
Expand All @@ -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;
Expand Down Expand Up @@ -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;
Copy link
Owner

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Please make sure to test, in detail, this logic, since we've found now 2 different errors in it that weren't caught before. test the boundaries of the different conditions and make sure this finally now works properly at the correct limit conditions

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'm really sorry. I'm a beginner, and I've tried my best to locate where the problem might be, but with no success so far. I'm not sure if this will inconvenience you, but I still hope you could lend me a hand and point out where the issue lies. I'll do my best to fix it.

Copy link
Owner

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I don't know if there is an issue - I'm simply asking for you to write some tests to make sure this is correct in dealing with your different start/end FOV angles. If its not working, then look back at the geometry :-)

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'm not sure how to write tests, but I can describe our application scenario to you.
Our robot is a cuboid (1.000.50.3) with a front end that has a height of 0.7cm. Previously, we used 2D lidars, which only provided information about a single plane at a height of 0.2 meters. Now, due to new requirements, the robot needs to perceive obstacles below a height of 3 meters in front. Because the robot design is fixed, a 3D lidar can only be installed in the front part of the robot, at a height of 0.47 meters.

The lidar we are using is from Livox, with a FOV of -7° to 53°. Assuming our mounting angle is a roll angle of 90°, the angles it can see are illustrated in the diagram below.

Therefore, in the existing code, if I set the vertical FOV (vfov) to 114 degrees, points with positive values on the z-axis in the lidar's coordinate system will be correctly cleared. However, points with negative z-values or those below -7° will be cleared as well, which is not the desired outcome. On the other hand, if the vfov is set to a value less than 114 degrees, there will be residual points on the positive z-axis that cannot be cleared.
livox

Copy link
Owner

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I can appreciate that - so what's the problem? What aren't you able to test about your own code contribution to make sure it actually does what you want it to do? Previous iterations of this PR clearly didn't work properly so I can tell you didn't test in detail to make sure the behavior you want as working. What are you going to test now with the changes to ensure your changes work as expected?

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;
}
Expand Down
12 changes: 10 additions & 2 deletions src/measurement_buffer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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, \
Expand All @@ -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),
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved
_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),
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -326,4 +335,3 @@ void MeasurementBuffer::Unlock(void)
}

} // namespace buffer

13 changes: 8 additions & 5 deletions src/spatio_temporal_voxel_grid.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand Down
13 changes: 11 additions & 2 deletions src/spatio_temporal_voxel_layer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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(""));
Expand All @@ -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
Expand Down Expand Up @@ -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)));

Expand Down Expand Up @@ -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;
}

/*****************************************************************************/
Expand Down