-
Notifications
You must be signed in to change notification settings - Fork 186
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.
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
base: kinetic-devel
Are you sure you want to change the base?
Changes from 8 commits
192445f
4b4f575
b23bd98
e5d8478
c127311
220dc69
c5fd3ea
257707c
3b011f3
80741f9
83219ff
01f33d9
File filter
Filter by extension
Conversations
Jump to
Diff view
Diff view
There are no files selected for viewing
Original file line number | Diff line number | Diff line change | ||||
---|---|---|---|---|---|---|
|
@@ -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; | ||||||
|
@@ -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; | ||||||
There was a problem hiding this comment. Choose a reason for hiding this commentThe 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 |
||||||
double tan_vFOV_squared; | ||||||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more.
Suggested change
Then remove the else statement. |
||||||
if (_use_start_end_angle) | ||||||
{ | ||||||
tan_vFOV_squared = (v_padded < 1e-9 ? _tan_vSFOV_squared : _tan_vEFOV_squared); | ||||||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Why not compare to |
||||||
} | ||||||
else | ||||||
{ | ||||||
tan_vFOV_squared = _tan_vFOVhalf_squared; | ||||||
} | ||||||
if ((v_padded * v_padded / radial_distance_squared) > tan_vFOV_squared) | ||||||
{ | ||||||
return false; | ||||||
} | ||||||
|
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
There was a problem hiding this comment.
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