Kinetic/fix/vertical fov bug #266

yShzZpp · 2023-07-13T10:38:32Z

Subject: Pull Request: Modification for spatioTemporalVoxelLayer

Dear Steve,

I hope this message finds you well. I am submitting my first Pull Request for your repository, [Repository Name], and I would greatly appreciate your review and consideration. I have made modifications to the code related to the usage of the spatioTemporalVoxelLayer, as I encountered compatibility issues with the kinetic version of the library when using a Livox radar.

The Livox radar has a field of view (FOV) ranging from -7° to 53°. However, the existing code simply divides the FOV by two as a parameter, resulting in an incomplete clearance of the costmap in the upper half, while the lower half is cleared properly.

To address this issue, I have made the following modifications to the code:

Replaced the "vertical_fov_angle" parameter with two parameters: "vertical_fov_start_angle" and "vertical_fov_end_angle." This allows for accurate definition of the vertical FOV range and resolves the clearance issue.
I have thoroughly tested these modifications on the Livox radar. However, I must note that I do not have access to a depth camera for comprehensive testing. Therefore, I have only submitted the modifications for the 3D radar functionality.

I genuinely value your expertise and would greatly appreciate your feedback, suggestions, or any optimizations you may have. This Pull Request holds significant meaning for my professional career, and I am committed to making any necessary changes to ensure its successful integration into the project.

Thank you for taking the time to review my code, despite your busy schedule. I look forward to your response.

Best regards,
zzpp

SteveMacenski

I think it would be wise to have this as an 'in addition to' feature instead of 'in place of'. The vast majority of users don't have asymmetric frustrums so having the option to continue to use the current API is preferable.

include/spatio_temporal_voxel_layer/frustum_models/depth_camera_frustum.hpp

src/frustum_models/depth_camera_frustum.cpp

yShzZpp · 2023-07-21T05:49:02Z

Dear Steve,
In this latest revision, I have made changes to incorporate FOV segmentation for 3D LiDAR scans. The segmentation will only be enabled when "model_type = 1" and "use_start_end_angle = true". For cases involving a depth camera, this feature will remain inactive.
Best regards,
zzpp

SteveMacenski

Much better!

include/spatio_temporal_voxel_layer/measurement_buffer.hpp

src/frustum_models/three_dimensional_lidar_frustum.cpp

yShzZpp · 2023-07-24T03:52:58Z

Dear Steve,
I wanted to express my gratitude for your patient guidance and support during the review process. I have made further adjustments to the code.
In this revised version, I have implemented a method that utilizes only a single constructor. I carefully considered your feedback and worked diligently to ensure that the code adheres to the best practices and standards of the project.
I kindly request you to review this latest iteration, and if there are any lingering issues or concerns, please do not hesitate to point them out. I am fully committed to patiently continue refining the code until it meets your expectations.

Once again, I extend my heartfelt appreciation for your valuable input and guidance. Your support is pivotal in my journey of growth and improvement as a developer.

Thank you for taking the time to review my work. I look forward to your feedback.

Best regards,
zzpp

SteveMacenski · 2023-07-31T18:59:44Z

include/spatio_temporal_voxel_layer/frustum_models/three_dimensional_lidar_frustum.hpp

@@ -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,


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,

I also doubt this line is < 100 lines

SteveMacenski · 2023-07-31T19:02:42Z

src/frustum_models/three_dimensional_lidar_frustum.cpp

+  double tan_vFOV_squared;
+  if (_use_start_end_angle)
+  {
+    tan_vFOV_squared = (v_padded < 1e-9 ? _tan_vSFOV_squared : _tan_vEFOV_squared);


Why not compare to 0?

SteveMacenski · 2023-07-31T19:03:24Z

src/frustum_models/three_dimensional_lidar_frustum.cpp

+  // {
+  //   return false;
+  // }
+  const double v_padded = transformed_pt[2] + _vFOVPadding;


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.

SteveMacenski · 2023-07-31T19:03:57Z

src/frustum_models/three_dimensional_lidar_frustum.cpp

+  //   return false;
+  // }
+  const double v_padded = transformed_pt[2] + _vFOVPadding;
+  double tan_vFOV_squared;


Suggested change

double tan_vFOV_squared;

double tan_vFOV_squared = _tan_vFOVhalf_squared;

Then remove the else statement.

src/measurement_buffer.cpp

src/spatio_temporal_voxel_grid.cpp

yShzZpp · 2023-08-01T03:01:15Z

Dear Steve,
In this latest revision, I have diligently addressed the areas you highlighted and made the necessary modifications.
Additionally, I would like to extend my sincerest apologies for not paying attention to the formatting issues in my previous submissions. I have now taken the time to rectify all the formatting problems throughout the code.
Best regards,
zzpp

SteveMacenski · 2023-08-01T20:20:22Z

src/frustum_models/three_dimensional_lidar_frustum.cpp

@@ -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;


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

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.

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 :-)

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.

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?

yShzZpp added 2 commits July 13, 2023 18:35

ADD: start and end vertocal_fov_angle

192445f

ADD: README

4b4f575

SteveMacenski requested changes Jul 14, 2023

View reviewed changes

include/spatio_temporal_voxel_layer/frustum_models/depth_camera_frustum.hpp Outdated Show resolved Hide resolved

src/frustum_models/depth_camera_frustum.cpp Outdated Show resolved Hide resolved

src/frustum_models/depth_camera_frustum.cpp Outdated Show resolved Hide resolved

yShzZpp added 5 commits July 19, 2023 17:45

ADD: fix bug

b23bd98

ADD: params

e5d8478

ADD: Change the default values of start and end

c127311

ADD: fix depth camera bug

220dc69

ADD: fix MeasurementReading and layer bug

c5fd3ea

SteveMacenski requested changes Jul 22, 2023

View reviewed changes

include/spatio_temporal_voxel_layer/measurement_buffer.hpp Outdated Show resolved Hide resolved

src/frustum_models/three_dimensional_lidar_frustum.cpp Outdated Show resolved Hide resolved

ADD: use one contructor

257707c

SteveMacenski requested changes Jul 31, 2023

View reviewed changes

yShzZpp added 4 commits August 1, 2023 10:20

ADD: fix bug and optimazed code

3b011f3

ADD: fix formating errors

80741f9

ADD: fix formating errors

83219ff

ADD: fix formating errors

01f33d9

SteveMacenski reviewed Aug 1, 2023

View reviewed changes

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Kinetic/fix/vertical fov bug #266

Kinetic/fix/vertical fov bug #266

yShzZpp commented Jul 13, 2023

SteveMacenski left a comment

yShzZpp commented Jul 21, 2023

SteveMacenski left a comment

yShzZpp commented Jul 24, 2023

SteveMacenski Jul 31, 2023

SteveMacenski Jul 31, 2023

SteveMacenski Jul 31, 2023

SteveMacenski Jul 31, 2023

SteveMacenski Jul 31, 2023

yShzZpp commented Aug 1, 2023

SteveMacenski Aug 1, 2023

yShzZpp Aug 7, 2023

SteveMacenski Aug 7, 2023

yShzZpp Aug 8, 2023

SteveMacenski Aug 8, 2023

	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,

	double tan_vFOV_squared;
	double tan_vFOV_squared = _tan_vFOVhalf_squared;

		@@ -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;

Kinetic/fix/vertical fov bug #266

Are you sure you want to change the base?

Kinetic/fix/vertical fov bug #266

Conversation

yShzZpp commented Jul 13, 2023

SteveMacenski left a comment

Choose a reason for hiding this comment

yShzZpp commented Jul 21, 2023

SteveMacenski left a comment

Choose a reason for hiding this comment

yShzZpp commented Jul 24, 2023

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

yShzZpp commented Aug 1, 2023

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment