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 2 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
13 changes: 7 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,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.

Expand Down
4 changes: 2 additions & 2 deletions include/spatio_temporal_voxel_layer/frustum_models/depth_camera_frustum.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand All @@ -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<VectorWithPt3D> _plane_normals;
Eigen::Vector3d _position;
Eigen::Quaterniond _orientation;
Expand Down
8 changes: 4 additions & 4 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,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);

Expand All @@ -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;
Expand Down
5 changes: 3 additions & 2 deletions 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 @@ -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, \
Expand Down Expand Up @@ -129,7 +130,7 @@ 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_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;
Expand Down
10 changes: 6 additions & 4 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,7 +66,7 @@ struct MeasurementReading

/*****************************************************************************/
MeasurementReading(geometry_msgs::Point& origin, pcl::PointCloud<pcl::PointXYZ> 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) :
/*****************************************************************************/
Expand All @@ -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), \
Expand All @@ -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), \
Expand All @@ -116,7 +118,7 @@ struct MeasurementReading
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_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;

Expand Down
12 changes: 6 additions & 6 deletions src/frustum_models/depth_camera_frustum.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;
Expand All @@ -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
Expand Down
21 changes: 15 additions & 6 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 @@ -41,21 +41,25 @@ 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),
_max_d(max_dist)
/*****************************************************************************/
{
_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 +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;
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.

if ((v_padded * v_padded / radial_distance_squared) > (v_padded < 1e-9 ? _tan_vSFOV_squared : _tan_vEFOV_squared))
{
return false;
}
Expand Down
9 changes: 6 additions & 3 deletions src/measurement_buffer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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, \
Expand All @@ -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),
Expand Down Expand Up @@ -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;
Expand Down
6 changes: 4 additions & 2 deletions src/spatio_temporal_voxel_grid.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -125,15 +125,17 @@ 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);
}
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,
Expand Down
11 changes: 7 additions & 4 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,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;
Expand All @@ -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
Expand Down Expand Up @@ -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)));

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

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