Intensity Server: recolor mesh doesn't work

[robbyHuelsi]

Hi, thanks for that great open source project!

I want to use the intensity server to map some intensity data to the Voxblox mesh. Therefore, in addition to lidar sensor data I give a CV_32FC1 intensity image in and I can see the /intensity_server/intensity_pointcloud topic in RViz. So far so good.

But I have some troubles with showing the Voxblox mesh in colors related to the right intensities. The /intensity_server/intensity_mesh outcome is equal to the outcome of /intensity_server/mesh and shows still the distances between lidar and detected surfaces as colors/intensities. I tried different values for intensity parameters like intensity_focal_length, subsample_factor, intensity_min_value, intensity_max_value, intensity_max_distance and intensity_colormap. But this did not affect the result.

For showing you the problem I just send intensities with value 0.5. In the picture you can see the discrepancy between the constant red colour of the pointcloud and the colourful mesh.

Is this a mistake of using intensity server or a bug (e.g. in method recolorVoxbloxMeshMsgByIntensity in intensity_vis.h or addIntensityBearingVectors in intensity_integrator.cc)?

I would be very happy about a quick response! Thanks a lot!