How to publish pointclouds in ros?

[sammyjan]

I want to publish pointcloud,camera pose and some other data in ROS, to make them can be visualised in the Rviz. I have read the codes for a few days, but I still don't have any ideas.

[s3r637]

Hi @sammyjan,

I assume that this is not your first ROS assignment... Therefore, the following link should be a good starting point for you to master the task without any hiccups.

https://github.com/JakobEngel/dso/blob/master/README.md#34-accessing-data

Additional links help you reach the goal faster.

https://github.com/JakobEngel/dso/blob/master/src/IOWrapper/Output3DWrapper.h https://github.com/JakobEngel/dso/blob/master/src/IOWrapper/OutputWrapper/SampleOutputWrapper.h https://github.com/JakobEngel/dso/blob/master/src/IOWrapper/Pangolin/PangolinDSOViewer.h https://github.com/JakobEngel/dso/blob/master/src/main_dso_pangolin.cpp#L407

Furthermore, you are not alone ... look at the past issues.

https://github.com/JakobEngel/dso/issues?utf8=%E2%9C%93&q=ROS https://github.com/JakobEngel/dso/issues?utf8=%E2%9C%93&q=point+cloud+ https://github.com/JakobEngel/dso/issues?utf8=%E2%9C%93&q=pose

Cheers & happy coding!

[sammyjan]

@s3r637 Thank you for your detailed advice, I'm sure it'll help me a lot. Thanks again!

[sammyjan]

@s3r637 Hello. Thanks for your help, I can publish pointcloud and pose in the Rviz now.But the pointcloud map is skew in the Rviz. How can I solve it?

[Hydrorion]

Hello everyone,

It's my first project on ROS (maybe it's not the best way to start) and I would like to implement DSO in my project.

Regarding on your result @sammyjan, please could you explain me how did you manage to get the pointclouds information (position - u,v,idepth) ?

Actually, we have graphic result thanks to Pangolin, but we really would like to access the coordinate of each point in order to use them in another program. The thing is that the READ ME only explain that we can find these information in the Output3DWrapper.h files but does not really explain how to get them using the code (Which function use to print the coordinate in the consol I mean).

I will be very grateful for your help.

Sincerely,

[sammyjan]

Hello @Hydrorion , I modify the code on the basis of the package from https://github.com/Neoplanetz/dso_with_saving_pcl. What's more, this issue https://github.com/JakobEngel/dso/issues/55 may help you a lot.To be honest , I am a beginner, so I am not clear about some details . I modify the SampleOutputWrapper.h , I show a part of the code as follows, you can take a reference. Hope you can reach your goal successfully. Cheers!

 virtual void publishKeyframes( std::vector<FrameHessian*> &frames, bool final, CalibHessian* HCalib) override
        {
            float fx, fy, cx, cy;
            float fxi, fyi, cxi, cyi;
            //float colorIntensity = 1.0f;
            fx = HCalib->fxl();
            fy = HCalib->fyl();
            cx = HCalib->cxl();
            cy = HCalib->cyl();
            fxi = 1 / fx;
            fyi = 1 / fy;
            cxi = -cx / fx;
            cyi = -cy / fy;
             
            if (final)
            { sensor_msgs::PointCloud cloud;
              cloud.header.frame_id = "world";
              
                for (FrameHessian* f : frames)
                {
                    if (f->shell->poseValid)
                    {
                        auto const& m = f->shell->camToWorld.matrix3x4();
      
                        // use only marginalized points.
                        auto const& points = f->pointHessiansMarginalized;

                        for (auto const* p : points)
                        {
                            float depth = 1.0f / p->idepth;
                            auto const x = (p->u * fxi + cxi) * depth;
                            auto const y = (p->v * fyi + cyi) * depth;
                            auto const z = depth * (1 + 2 * fxi);
                       
                            Eigen::Vector4d camPoint(x, y, z, 1.f);
                            Eigen::Vector3d worldPoint = m * camPoint;

                            if (isSavePCL && pclFile.is_open())
                            {
                                isWritePCL = true;

                                pclFile << worldPoint[0] << " " << worldPoint[1] << " " << worldPoint[2] << "\n";

                                printf("[%d] Point Cloud Coordinate> X: %.2f, Y: %.2f, Z: %.2f\n",
                                         numPCL,
                                         worldPoint[0],
                                         worldPoint[1],
                                         worldPoint[2]);
                        geometry_msgs::Point32 h;
                        h.x = worldPoint[0];
                        h.y = worldPoint[1];
                        h.z = worldPoint[2];
                        cloud.points.push_back(h);                                               
                                numPCL++;
                             isWritePCL = false;                  
                            }
                             
                            else
                            {
                                if (!isPCLfileClose)
                                {
                                    if (pclFile.is_open())
                                    {
                                        pclFile.flush();
                                        pclFile.close();
                                        isPCLfileClose = true;
                                    }
                                }
                            }
                 cloud_pub.publish(cloud);
                         }
                
                    }
              
                }
            }

[EdwinvanEmmerik]

@sammyjan Would it be possible to share your outputWrapper for ROS-rviz implementation?