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

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 7 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
9 changes: 6 additions & 3 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 @@ -54,6 +54,8 @@ 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);
virtual ~ThreeDimensionalLidarFrustum(void);

// Does nothing in 3D lidar model
Expand All @@ -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 _vFOV, _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_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
30 changes: 28 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 @@ -71,7 +71,7 @@ typedef pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud_ptr;
class MeasurementBuffer
{
public:
MeasurementBuffer(const std::string& topic_name, \
MeasurementBuffer(const std::string& topic_name, \
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved
const double& observation_keep_time, \
const double& expected_update_rate, \
const double& min_obstacle_height, \
Expand All @@ -95,6 +95,31 @@ class MeasurementBuffer
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, \
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);

~MeasurementBuffer(void);

// Buffers for different types of pointclouds
Expand Down Expand Up @@ -129,9 +154,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_fov;
ModelType _model_type;
};

Expand Down
36 changes: 32 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 @@ -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)
{
}

Expand All @@ -102,24 +103,51 @@ 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_fov(obs._use_start_end_fov)
{
}


/*****************************************************************************/
MeasurementReading(geometry_msgs::Point& origin, pcl::PointCloud<pcl::PointXYZ> 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<pcl::PointXYZ>(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<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_fov;
};

} // end namespace
Expand Down
49 changes: 47 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 @@ -53,6 +53,7 @@ ThreeDimensionalLidarFrustum::ThreeDimensionalLidarFrustum(const double& vFOV,
_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;
Expand All @@ -65,6 +66,36 @@ ThreeDimensionalLidarFrustum::ThreeDimensionalLidarFrustum(const double& vFOV,
}
}

/*****************************************************************************/
ThreeDimensionalLidarFrustum::ThreeDimensionalLidarFrustum(const double& vSFOV,
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved
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;
_tan_vEFOV_squared = _tan_vEFOV * _tan_vEFOV;
_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(void)
/*****************************************************************************/
Expand Down Expand Up @@ -100,8 +131,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

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

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_fov)
{
tan_vFOV_squared = (v_padded < 1e-9 ? _tan_vSFOV_squared : _tan_vEFOV_squared);
Copy link
Owner

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
54 changes: 51 additions & 3 deletions src/measurement_buffer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -71,7 +71,48 @@ 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), _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_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 +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_fov_in_rad = _vertical_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;
Expand Down Expand Up @@ -326,4 +375,3 @@ void MeasurementBuffer::Unlock(void)
}

} // namespace buffer

25 changes: 19 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,25 @@ 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);
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
{
Expand Down
Loading