SkiMap integration with Velodyne Lidar about skimap_ros HOT 15 CLOSED

@BrunoEduardoCSantos please post a screenshot when you resolve the problem (because i'm sure you will resolve it :) ) just for other user .. thanks a lot

from skimap_ros.

hi @BrunoEduardoCSantos, SkiMap works with any type of 3D data, given that is a generic 3D Indexing Data Structure. In the Basic Tutorial is explained how to integrate a generic 3D information in SkiMap (without a direct ROS integration).

from skimap_ros.

Thanks @m4nh for your feedback .
Once I subscribe a topic from the registered point cloud coming from a SLAM algorithm, I can use this basic tutorial to integrate as SkiMap, right?

from skimap_ros.

@BrunoEduardoCSantos yes sure

from skimap_ros.

Hi @BrunoEduardoCSantos ,
Currently, I am trying to get skimap viz on rviz using as example you skipmap_live.
Although, instead rgb-d data, I am using PointXYZI. After getting the visualization in rviz the voxel colors were a mixed of colors which doesnt make sense at all.
My code to integrate the point cloud is the following:

for(int i=0; i<pc->points.size();i++){
    double x = pc->points[i].x;
    double y = pc->points[i].y;
    double z = pc->points[i].z;
     //store voxel
     VoxelDataColor voxel;
     voxel.r = 0.1;
     voxel.g = 0.1;
     voxel.b = 0.1;
     voxel.w = 1.
     map_->integrateVoxel(CoordinateType(x), CoordinateType(y),
   CoordinateType(z), &voxel);

 }

Is there anyway of doing using intensity to assign the voxel color?
Regards,
Bruno

from skimap_ros.

hi @BrunoEduardoCSantos ! can you send me a screeshot of the visualization?

from skimap_ros.

@BrunoEduardoCSantos I think you set the launch parameter

to TRUE. With this parameter enabled the coloration is based on the Z value of each voxel... it is just a demonstration for maps with the Z oriented like gravity. Try to set the parameter to FALSE

from skimap_ros.

from skimap_ros.

I create my own node . I am not using really the skimap_live. Just a version of it.

void SkimapPathPlanning::fillVisualizationMarkerWithVoxels(
    visualization_msgs::Marker &voxels_marker, std::vector<Voxel3D> &voxels) {

  cv::Mat colorSpace(1, voxels.size(), CV_32FC3);
//   if (mapParameters.height_color) {
    for (int i = 0; i < voxels.size(); i++) {
      colorSpace.at<cv::Vec3f>(i)[0] = 180 - (voxels[i].z / 2) * 180;
      colorSpace.at<cv::Vec3f>(i)[1] = 1;
      colorSpace.at<cv::Vec3f>(i)[2] = 1;
    }
    cv::cvtColor(colorSpace, colorSpace, CV_HSV2BGR);
//   }

  for (int i = 0; i < voxels.size(); i++) {

    /**
     * Create 3D Point from 3D Voxel
     */
    geometry_msgs::Point point;
    point.x = voxels[i].x;
    point.y = voxels[i].y;
    point.z = voxels[i].z;

    /**
     * Assign Cube Color from Voxel Color
     */
    std_msgs::ColorRGBA color;
    // if (mapParameters.height_color) {
      color.r = colorSpace.at<cv::Vec3f>(i)[2];
      color.g = colorSpace.at<cv::Vec3f>(i)[1];
      color.b = colorSpace.at<cv::Vec3f>(i)[0];
    // } else {
    //   color.r = float(voxels[i].data->r) / 255.0;
    //   color.g = float(voxels[i].data->g) / 255.0;
    //   color.b = float(voxels[i].data->b) / 255.0;
    // }
    color.a = 1;

    voxels_marker.points.push_back(point);
    voxels_marker.colors.push_back(color);
  }
}

Since I want a height map, is this the way to do it?
Thanks again.

from skimap_ros.

Yes you commented:

    //   color.r = float(voxels[i].data->r) / 255.0;
    //   color.g = float(voxels[i].data->g) / 255.0;
    //   color.b = float(voxels[i].data->b) / 255.0;

so you are using the colors contained in the vector colorSpace. But maybe your map has the Z axis not oriented with gravity.. try to modify the row containing:

colorSpace.at<cv::Vec3f>(i)[0] = 180 - (voxels[i].z / 2) * 180;

by replacing .z with .x or .y.. just to debug the issue..

from skimap_ros.

It worked just replacing by .y . Thanks for your help!

from skimap_ros.

But it still has some noisy data.

from skimap_ros.

by noisy I mean non related color data.

from skimap_ros.

@BrunoEduardoCSantos .. sorry but the skimap_live is just a prototype for demo purposes :) ... however when you set:

colorSpace.at<cv::Vec3f>(i)[0] = 180 - (voxels[i].y / 2) * 180;

you are codifying the HUE of the color.. in this equation the max allowed y is obviously between [0,2].. probably negative values are creating noisy data... try to change this function for your custom purposes.. maybe managing negative numbers, changing the maximum range (>2) and so on..

from skimap_ros.

Thanks @m4nh for your help. I will custom this parameter according to your suggestions.
Thanks a lot.

from skimap_ros.