Issue #946 new
Ian Chen
created an issue

I have working code in the gpu_vertical branch but currently integration tests don't pass as the range readings seem to get less accurate as it approaches min/max vertical angles.

The problem is likely to be the maths in the initial gpu camera setup in GpuRaySensor::Init() or the depth image texture mapping code in GpuLaser::CreateMesh

Comments (42)

  1. Jacob Seibert

    @Ian Chen I'm currently trying to work on this issue. Is there any place where I could find a summary of how this plugin is supposed to work so that I could understand the math more easily? Like what exactly first and second pass compute and how undistortion using a mesh works.

    Regards, Jacob

  2. Jacob Seibert

    @Javier Iván Choclin Your commit messages imply that you were able to reduce the error, but when I tried your code the resulting point cloud is still very distorted. Either something of my setup is wrong or I think there is still a bug in the un-distortion mesh.

    What's the current status of your work on this?

    FYI: I'm using the ROS velodyne_gazebo_plugins package which I slightly modified to have a GPU laser version of the sensor.

  3. Jacob Seibert

    I managed to fix the vertical distortion that was still in the point cloud. Now I think the result looks correct, I tested a couple of different settings for vertical/horizontal vield of view and samples and the results looked always good. Would be very nice if either @Ian Chen or @Javier Iván Choclin could test it as well (repository linked above).

    One thing I ignored for now is the interpolation and the resolution parameter (set it to 1). So this won't work yet I think. Wouldn't it generally make sense to get rid of the manual interpolation done in GpuRaySensor.cc and just increase the resolution of the first pass rendering accordingly?

  4. Ian Chen reporter

    that sounds promising, thanks for sharing the code.

    Ignoring the resolution param for now sounds good. The interpolation logic and the resolution param originated from cpu version of ray sensor. For the gpu version, we could just decide to increase the resolution of the first pass. The only thing I am concerned about is that it may introduce more inconsistency between the results of the two versions.

  5. Javier Iván Choclin

    Definitely looks much better, thanks! This is what I see:

    Screenshot from 2018-05-03 10-36-27.png

    I don't know if the inconsistency between the plugins should be a problem, at the end of the day, the way that the data is generated in both cases is completely different, interpolated or not...

  6. Jacob Seibert

    I can think of two ways the resolution param could define the first pass image size: Based on the number of samples (size = resolution * samples) or based on the field of view (size = resolution * fov), e.g. 5 pixels per degree or so. The first option is probably closer to the semantics of that param in the CPU version. On the other hand the second option makes it easier to control the size of the area (pixel in first pass image) that is averaged to get the range for one ray which in theory is only a point.

    One thing that also needs to be fixed: Currently the case of only 1 vertical ray does not seem to work. I will take a look at it the next days.

  7. Ian Chen reporter

    For the CPU case, increasing the resolution are more like generating fake rays that may not really help give better results (for small number of samples):

    samples: 30, resolution 10

    ray_30_10.jpg

    samples: 300, resolution 1

    ray_300_1.jpg

    If I understand correctly, I think in both options, increasing the resolution will result in a larger image size which may produce data similar to the second image?

  8. Jacob Seibert

    I implemented the resolution based on sample count because for now I think it's a bit closer to the CPU case.

    Check out the following images of willow garage environment view from top down with 16 vert. and 1875 horiz. samples:

    vert/horz resolution = 1:

    resolution_1.png

    vert/horz resolution=2:

    resolution_2.png

    vert/horz resolution=4:

    resolution_4.png

    For this implementation of the resolution I made small changes to the API especially because I had to sort out range count and ray count. How critical is that regarding an availability in existing Gazebo 7/8/9 versions?

    In general: What is still to be done in order to prepare a pull request? Probably adjust the tests, I haven't looked at them at all until now... For me it would be very nice if this feature could be released into one of the recent Gazebo releases soon.

    Regards, Jacob

  9. Ian Chen reporter

    The changes are made on top of gazebo7so we need to preserve ABI and API compatibility.

    Looking at the changes, I think you just added the new GpuLaser::SetRayCount function? That should be fine and won't break API/ABI

    There is an integration test in test/integration/gpu_laser.cc. The test require some changes to ServerFixture::SpawnGpuRaySensor and that break API (probably done by me a while back). I'll look into reverting those changes. Once that's fixed and the test passes, you can create a PR targeting gazebo7.

    When the PR is merged, we'll manually merge the changes forward to gazebo8, 9, and the default branch and make new releases.

  10. Ian Chen reporter

    here's a patch that cleans up the tests and fixes the API problem.

    The LaserScanResolution test in gpu_laser.cc is directly ported from the test for the cpu version of the ray sensor. I noticed that it's failing. A quick look reveals that it's not due to the precision of the results but the size of the laser scan data is different.

    • gpu: scan size = hSamples * vSamples

    • cpu: scan size = hSamples * hResolution * vSamples * vResolution

    It's probably the way gpu laser is handling resolution params but it would be nice to keep the behavior consistent so that GpuRaySensor::Range() and RaySensor::Range() function has the same meaning.

    Other than that, I tested the sensor a bit and it's looking good!

  11. Jacob Seibert

    I see... I don't really get why the cpu laser behaves this way, because with vertical and horizontal sample count I already define how many ranges the resulting scan should have and from my understanding the resolution parameter should only affect internal sizes and not change the sample count of the resulting scan.

    But I agree with you that it should be consistent, so I'm fine with making it consistent with the cpu laser and personally I will just always set that resolution to 1 ;)

  12. Kevin Allen

    Running on gazebo 9.2, my gzserver crahses if I'm subscribed to the GpuRaySensor's laser scan. It goes out of bounds on this line.

    [libprotobuf FATAL /usr/include/google/protobuf/repeated_field.h:1092] CHECK failed: (index) < (current_size_):
    terminate called after throwing an instance of 'google::protobuf::FatalException'
      what():  CHECK failed: (index) < (current_size_):
    
    Thread 1 "gzserver" received signal SIGABRT, Aborted.
    __GI_raise (sig=sig@entry=6)
        at ../sysdeps/unix/sysv/linux/raise.c:51
    51      ../sysdeps/unix/sysv/linux/raise.c: No such file or directory.
    (gdb) backtrace
    #0  0x00007ffff62f6e97 in __GI_raise (sig=sig@entry=6)
        at ../sysdeps/unix/sysv/linux/raise.c:51
    #1  0x00007ffff62f8801 in __GI_abort () at abort.c:79
    #2  0x00007ffff694d8fb in  ()
        at /usr/lib/x86_64-linux-gnu/libstdc++.so.6
    #3  0x00007ffff6953d3a in  ()
        at /usr/lib/x86_64-linux-gnu/libstdc++.so.6
    #4  0x00007ffff6953d95 in  ()
        at /usr/lib/x86_64-linux-gnu/libstdc++.so.6
    #5  0x00007ffff6953fe8 in  ()
        at /usr/lib/x86_64-linux-gnu/libstdc++.so.6
    #6  0x00007ffff46ddbb8 in google::protobuf::internal::LogMessage::Finish() () at /usr/lib/x86_64-linux-gnu/libprotobuf.so.10
    #7  0x00007ffff603851a in google::protobuf::RepeatedField<double>::Set(int, double const&) (value=<synthetic pointer>: <optimized out>, index=300, this=<optimized out>)
        at /usr/include/google/protobuf/repeated_field.h:1092
    #8  0x00007ffff603851a in gazebo::msgs::LaserScan::set_ranges(int, double) (value=-inf, index=300, this=<optimized out>)
        at ./obj-x86_64-linux-gnu/gazebo/msgs/laserscan.pb.h:644
    ---Type <return> to continue, or q <return> to quit---
    #9  0x00007ffff603851a in gazebo::sensors::GpuRaySensor::UpdateImpl(bool) (this=0x5555569c9f00) at ./gazebo/sensors/GpuRaySensor.cc:633
    #10 0x00007ffff6055129 in gazebo::sensors::Sensor::Update(bool) (this=0x5555569c9f00, _force=_force@entry=false)
        at ./gazebo/sensors/Sensor.cc:224
    #11 0x00007ffff605b1ed in gazebo::sensors::SensorManager::SensorContainer::Update(bool) (this=0x555555dc3790, _force=<optimized out>)
        at ./gazebo/sensors/SensorManager.cc:623
    #12 0x00007ffff605cddb in gazebo::sensors::SensorManager::Update(bool) (this=this@entry=0x7ffff62b4000 <SingletonT<gazebo::sensors::SensorManager>::GetInstance()::t>, _force=<optimized out>)
        at ./gazebo/sensors/SensorManager.cc:190
    #13 0x00007ffff60532db in gazebo::sensors::run_once(bool) (_force=_force@entry=false) at ./gazebo/sensors/SensorsIface.cc:118
    #14 0x00007ffff7ba059f in gazebo::Server::Run() (this=0x55555581f2a0) at ./gazebo/Server.cc:587
    #15 0x0000555555557e55 in main(int, char**) (argc=3, argv=0x7fffffffdd78) at ./gazebo/server_main.cc:42
    (gdb) frame 9
    #9  gazebo::sensors::GpuRaySensor::UpdateImpl (this=0x5555569c9f00)
        at ./gazebo/sensors/GpuRaySensor.cc:633
    633     ./gazebo/sensors/GpuRaySensor.cc: No such file or directory.
    (gdb) list
    628     in ./gazebo/sensors/GpuRaySensor.cc
    (gdb) print range
    $1 = -inf
    (gdb) print i
    $2 = 300
    (gdb) print RayCount()
    $3 = 30
    (gdb) print VerticalRayCount()
    $4 = 10
    (gdb) print dataIter
    $5 = {index = 300, data = 0x555600ea7d90, skip = 3,
      rangeOffset = 0, intensityOffset = 1, horizontalResolution = 30}
    (gdb) print dataEnd
    $6 = {index = 360, data = 0x555600ea7d90, skip = 3,
      rangeOffset = 0, intensityOffset = 1, horizontalResolution = 30}
    

    It seems the message is resized to VeritcalRayCount() * RayCount() (300 in my example), but the dataIter is size 360.

    This is my sensor sdf:

                    <sensor name="sensor_gpu_ray" type="gpu_ray">
                        <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
                        <ray>
                            <scan display="true">
                                <horizontal>
                                    <samples>30</samples>
                                    <resolution>1.0</resolution>
                                    <min_angle>-0.5236</min_angle>
                                    <max_angle>0.5236</max_angle>
                                </horizontal>
                                <vertical>
                                    <samples>10</samples>
                                    <resolution>1.0</resolution>
                                    <min_angle>-0.2</min_angle>
                                    <max_angle>0.2</max_angle>
                                </vertical>
                            </scan>
                            <range>
                                <min>0.05</min>
                                <max>50.0</max>
                            </range>
                        </ray>
                        <always_on>true</always_on>
                        <update_rate>1.0</update_rate>
                    </sensor>
    
  13. Ian Chen reporter

    I recall @Steven Peters mentioned that parts of the gpu laser sensor changes were not ported forward to gazebo9 due to iterator changes. So not sure if that would be the cause of this issue. To verify, we can try and see if the same problem occurs on gazebo7

  14. Arif Rahman

    Hello, Is there any plans yet to update Gazebo 9 on this issue? I'm also getting this error when trying to get GPU laser vertical rays;

    [libprotobuf FATAL /usr/include/google/protobuf/repeated_field.h:1092] CHECK failed: (index) < (current_size_):
    terminate called after throwing an instance of 'google::protobuf::FatalException'
      what():  CHECK failed: (index) < (current_size_):
    
  15. Ilea Baltashov

    I still have the same problem – the resolution for vertical scans is very rough. The problem is especially bad for low-lying lidars with a small vertical angular resolution and high working range, like, MRS1104C-111011. I have it on both gazebo 9.6 and 7.14.

    I tested it on:

    1. Ubuntu 16.04 + GTX 1050 + gazebo 9.6/gazebo 7.14 + nvidia 384/nvidia 415

    2. Ubuntu 16.04 + GTX 1080/GTX 780 + gazebo 9.6 + nvidia 384

    Here is my world (sdf) files and images of how they work in my case.

    ray_cpu.world

    <?xml version="1.0" ?>
    <sdf version="1.5">
      <world name="default">
        <!-- A global light source -->
        <include>
          <uri>model://sun</uri>
        </include>
        <!-- A ground plane -->
        <include>
          <uri>model://ground_plane</uri>
        </include>
        <model name="velodyne">
          <static>true</static>
          <pose>0 0 0.1 0 0 0</pose>
          <link name="link">
            <inertial>
              <mass>0.1</mass>
            </inertial>
            <collision name="collision">
              <geometry>
                <box>
                  <size>0.2 0.2 0.2</size>
                </box>
              </geometry>
            </collision>
            <visual name="visual">
              <geometry>
                <box>
                  <size>0.2 0.2 0.2</size>
                </box>
              </geometry>
            </visual>
            <sensor name="laser" type="ray">
              <pose>1.0 0 0.5 0 -0 0</pose>
              <ray>
                <scan>
                  <horizontal>
                    <samples>1100</samples>
                    <resolution>1</resolution>
                    <min_angle>-2.39</min_angle>
                    <max_angle>2.39</max_angle>
                  </horizontal>
                  <vertical>
                    <samples>2</samples>
                    <resolution>1</resolution>
                    <min_angle>-0.09</min_angle>
                    <max_angle>-0.045</max_angle>
                  </vertical>
                </scan>
                <range>
                  <min>0.3</min>
                  <max>64</max>
                  <resolution>0.001</resolution>
                </range>
              </ray>
              <always_on>1</always_on>
              <update_rate>30</update_rate>
              <visualize>true</visualize>
            </sensor>
          </link>
        </model>
      </world>
    </sdf>
    

    ray_gpu.world

    <?xml version="1.0" ?>
    <sdf version="1.5">
      <world name="default">
        <!-- A global light source -->
        <include>
          <uri>model://sun</uri>
        </include>
        <!-- A ground plane -->
        <include>
          <uri>model://ground_plane</uri>
        </include>
        <model name="velodyne">
          <static>true</static>
          <pose>0 0 0.1 0 0 0</pose>
          <link name="link">
            <inertial>
              <mass>0.1</mass>
            </inertial>
            <collision name="collision">
              <geometry>
                <box>
                  <size>0.2 0.2 0.2</size>
                </box>
              </geometry>
            </collision>
            <visual name="visual">
              <geometry>
                <box>
                  <size>0.2 0.2 0.2</size>
                </box>
              </geometry>
            </visual>
            <sensor name="laser" type="gpu_ray">
              <pose>1.0 0 0.5 0 -0 0</pose>
              <ray>
                <scan>
                  <horizontal>
                    <samples>1100</samples>
                    <resolution>1</resolution>
                    <min_angle>-2.39</min_angle>
                    <max_angle>2.39</max_angle>
                  </horizontal>
                  <vertical>
                    <samples>2</samples>
                    <resolution>1</resolution>
                    <min_angle>-0.09</min_angle>
                    <max_angle>-0.045</max_angle>
                  </vertical>
                </scan>
                <range>
                  <min>0.3</min>
                  <max>64</max>
                  <resolution>0.001</resolution>
                </range>
              </ray>
              <always_on>1</always_on>
              <update_rate>30</update_rate>
              <visualize>true</visualize>
            </sensor>
          </link>
        </model>
      </world>
    </sdf>
    

    gazebo 7.14 - cpu ray 7-cpu.png gazebo 7.14 - gpu ray 7-gpu.png gazebo 9.6 - cpu ray 9-cpu.png gazebo 9.6 - gpu ray 9-gpu.png

  16. Log in to comment