Closed brunoeducsantos closed 5 years ago
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).
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?
@BrunoEduardoCSantos yes sure
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
hi @BrunoEduardoCSantos ! can you send me a screeshot of the visualization?
@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
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.
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..
It worked just replacing by .y . Thanks for your help!
But it still has some noisy data.
by noisy I mean non related color data.
@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..
Thanks @m4nh for your help. I will custom this parameter according to your suggestions. Thanks a lot.
@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
Hi, Is there an integration with velodyne lidar? Or just with RGB-D cameras? Thanks in advance, Bruno