Skip to content

Kinetic/fix/vertical fov bug #266

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

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 8 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
11 changes: 6 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,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& vFOV, const bool& use_start_end_angle, const double& vEFOV,const double& vSFOV,
Copy link
Owner

@SteveMacenski SteveMacenski Jul 31, 2023

Choose a reason for hiding this comment

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

Suggested change
ThreeDimensionalLidarFrustum(const double& vFOV, const bool& use_start_end_angle, const double& vEFOV,const double& vSFOV,
ThreeDimensionalLidarFrustum(const double& vFOV, const bool& use_start_end_angle, const double& vEFOV, const double& vSFOV,

Copy link
Owner

@SteveMacenski SteveMacenski Jul 31, 2023

Choose a reason for hiding this comment

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

I also doubt this line is < 100 lines

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 +71,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
6 changes: 5 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,10 @@ 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, _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
18 changes: 12 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,28 @@ 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, _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
28 changes: 26 additions & 2 deletions 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 @@ -100,8 +110,22 @@ 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;
Copy link
Owner

@SteveMacenski SteveMacenski Jul 31, 2023

Choose a reason for hiding this comment

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

I don't think this necessarily works for the negative case, because the padding is always positive even when the transformed point coordinates are negative, that's why we have fabs above.

double tan_vFOV_squared;
Copy link
Owner

@SteveMacenski SteveMacenski Jul 31, 2023

Choose a reason for hiding this comment

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

Suggested change
double tan_vFOV_squared;
double tan_vFOV_squared = _tan_vFOVhalf_squared;

Then remove the else statement.

if (_use_start_end_angle)
{
tan_vFOV_squared = (v_padded < 1e-9 ? _tan_vSFOV_squared : _tan_vEFOV_squared);
Copy link
Owner

@SteveMacenski SteveMacenski Jul 31, 2023

Choose a reason for hiding this comment

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

Why not compare to 0?

}
else
{
tan_vFOV_squared = _tan_vFOVhalf_squared;
}
if ((v_padded * v_padded / radial_distance_squared) > tan_vFOV_squared)
{
return false;
}
Expand Down
11 changes: 9 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,9 @@ 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),
Expand Down Expand Up @@ -141,6 +146,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 +334,3 @@ void MeasurementBuffer::Unlock(void)
}

} // namespace buffer

15 changes: 9 additions & 6 deletions src/spatio_temporal_voxel_grid.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -132,12 +132,15 @@ 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);
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
{
Expand Down
13 changes: 10 additions & 3 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,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, 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 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(""));
Expand All @@ -166,6 +166,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 +214,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, 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 +495,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