Color for pointcloud in occupancyMap3d

2 次查看（过去 30 天）

Nicolas Jorquera Martinez 2024-2-15

0
链接

此问题的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/2082678-color-for-pointcloud-in-occupancymap3d

评论： Nicolas Jorquera Martinez 2024-2-19

I want to make a 3d map with odometry + pointcloud, and for that im using this example (using pointcloud instead of just points). My robot gives me the pc in pointcloud2 format but i need the pointcloud format, so i just change it by doing

pc = pointCloud(pc_face.readXYZ, 'Color', pc_face.readRGB); %pc_face (pointcloud2)/ pc (pointcloud)

This works, but for some reason, the colors are not working for the pointcloud (photo at the end).

rosshutdown;
rosinit('http://192.168.123.161:11311');
go1_odom = rossubscriber('/tf');
fqGet = 1000; %frequency for getting topics
maxRange = 2;
go1_pc_face = rossubscriber('/camera1/point_cloud_face');
tStartOdom = tic;
[odomTF, status1, statusText1] = receive(go1_odom);
[pc_face, status2, statusText2] = receive(go1_pc_face);
map3D = occupancyMap3D(20);
while 1
    [go1_test_msg, status1, statusText1] = receive(go1_odom);
    
    if strcmp(go1_test_msg.Transforms.ChildFrameId, 'trunk')
        odomTF = go1_test_msg;
    end
    tEndOdom = toc(tStartOdom);
    %disp(1/tEndOdom)
    if round(1/tEndOdom) <= fqGet
        [pc_face, status2, statusText2] = receive(go1_pc_face);
        %showdetails(odomTF);
        %showdetails(pc_face);
        
        xPos = odomTF.Transforms.Transform.Translation.X;
        yPos = odomTF.Transforms.Transform.Translation.Y;
        zPos = odomTF.Transforms.Transform.Translation.Z;
    
        xRot = odomTF.Transforms.Transform.Rotation.X;
        yRot = odomTF.Transforms.Transform.Rotation.Y;
        zRot = odomTF.Transforms.Transform.Rotation.Z;
        wRot = odomTF.Transforms.Transform.Rotation.W;
    
        pose = [ xPos yPos zPos wRot xRot yRot zRot];
        points = readXYZ(pc_face);
        pc = pointCloud(pc_face.readXYZ, 'Color', pc_face.readRGB);
        
        
        insertPointCloud(map3D,pose,pc,maxRange);
        show(map3D);
        fprintf('%d HZ\n',round(1/tEndOdom));
        disp('-------------------------------------------------');
        tStartOdom = tic;
    end
end

4 个评论
显示 2更早的评论隐藏 2更早的评论

Mann Baidi 2024-2-19

Is the image above of "pc"(PointCloud)?

if yes, what is the issue with the color?

Nicolas Jorquera Martinez 2024-2-19

Yes, it's "pc" (PointCloud) cause it gave me an error when I tried to do it with "pc_face" (PointCloud2).

The problem is that the camera captures color (of the surface it's looking at), that information is in the "pc_face" and "pc" variable, but the map doesn't show this colors.

As an example, if the camera sees a red floor I want the map to show a red floor.

请先登录，再进行评论。

请先登录，再回答此问题。