13 Replies Latest reply on Mar 20, 2018 7:37 AM by thePaintedSnipe

    How to project points to pixel? Librealsense SDK 2.0

    svebert

      Hi,

       

      With the help of rs2::pointcloud I could compute 3D-Points from a depth_frame in the color-camera-coordinate system.

      The examples are very short and the documentation is missing. Therefore, I need first a confirmation of the following to be true:

       

      rs2::frameset m_oFrameset;

      rs2::points m_oPoints;

      rs2::pointcloud pc;

      pc.map_to(m_oFrameset.get_color_frame()); //the object pc is set to the coordinate system of the color-camera

      m_oPoints = pc.calculate(m_oFrameset.get_depth_frame()); //de-projects every depth_frame pixel to a 3D-Point; the coordinates for the points are given with respect to the color-camera coordinate system

      pPoints2DepthBuffer = (void *) m_oPoints.get_vertices(); //pPoints2DepthBuffer points to the begining of an array with the same size like the number of pixels in the depth_frame. Each element being a rs2::vertex object with x,y,z elments. And these x,y,z are coordinates in the color-camera coordinate system with units in meter

       

      Now, how do I project m_oPoints back to the "pixel space" of the color_frame?

      What does m_oPoints.get_texture_coordinates() do? In what units are these coordinates? 

      Thank you,

      Sven

       

      (I am working with the D435)

        • 1. Re: How to project points to pixel? Librealsense SDK 2.0
          MartyG

          Whilst I do not have information on your technical questions, you may be interested in the meantime on reading the documentation of the Projection system that SDK 2.0 uses .

           

          Projection in RealSense SDK 2.0 · IntelRealSense/librealsense Wiki · GitHub

          • 2. Re: How to project points to pixel? Librealsense SDK 2.0
            svebert

            Hi MartyG,

            thank you

            I did read this part already. It seems like I need the function "rs2_project_point_to_pixel"

            But I am confused by several things:

            -Why is this function not living in the rs.hpp? And where do I get the camera intrinsics from?

            -Do I really have to apply this function for every pixel by myself? Is there not a wrapper or something in the pointcloud-class?

            • 3. Re: How to project points to pixel? Librealsense SDK 2.0
              MartyG

              rs2_project_point_to_pixel is referenced in the file rsutil.h.  A Python binding for it in this file location was added in the 2.10.0 SDK, the version before the current one.  In version, 2.10.0, a feature to use Python to "allow extrinsics and intrinsics creation and modification" was also added.

              • 4. Re: How to project points to pixel? Librealsense SDK 2.0
                svebert

                Hi MartyG,

                I am using C++...

                I will wait for some technical answers about my understanding of the pointcloud-class.

                • 5. Re: How to project points to pixel? Librealsense SDK 2.0
                  jb455

                  I don't think pc.map_to actually maps the point cloud to the colour image. I believe it instead creates a mapping from the colour image to the point cloud. So in your case, the point cloud is still aligned to the depth image instead of colour

                   

                  It looks like this code may help you: librealsense/model-views.cpp at master · IntelRealSense/librealsense · GitHub

                  The texture coordinates return uv coordinates mapping to the colour image with the coordinates normalised to 0-1. So to get from uv to xy coordinates (ie, pixel coordinates in the colour image) you have to multiply the u and v values by the colour image width and height (though that code is slightly more complex to account for the extreme edges of the image).

                   

                  Alternatively, I believe you can first align the depth image to colour using Align, then pass that aligned depth image to the point cloud to get your point cloud aligned to the colour image if you'd prefer. Then getting the colour of a vertex or the vertex of a colour pixel is simple as they will have the same indices of their respective arrays. (so (x,y) in colour image has corresponding vertex points[x + y * colourWidth] and vice-versa)

                  • 6. Re: How to project points to pixel? Librealsense SDK 2.0
                    svebert

                    Hi jb455,

                     

                    thank you for the clarification. Actually, I did already multiply the image dimensions to the texture_coordinates but I did not trust in the result Now I trust a little bit more in this projection.

                    The route using the align class is also an option to solve my projection problem, thank you.

                    Still, I have some more questions about the map_to function of the pointcloud class:

                    The Kinect API did work with three different coordinate systems: Camera Space (3D, origin at IR sensor), Color Space (2D, plane of rgbx image) and Depth Space (2D, plane of depth image).

                    Now, they had three mapping functions (and many more):

                    -MapDepthFrameToColorSpace -->results in 2D point cloud, mapping every depth pixel to a corresponding pixel of the rgb camera

                    -MapDepthFrameToCameraSpace  --> results in 3D point cloud. Every depth pixel is mapped to a 3D point in camera space

                    -MapColorFrameToCameraSpace -->results in 3D point cloud. Every rgbx pixel is mapped to a 3D point in camera space

                     

                    How do use the pointcloud class to get similar results. What is the origin of the pointcloud returned by the "calculate" function? Is it the mid-point between the two IR cameras of the D415/435?

                    As I understood, "MapDepthFrameToColorSpace" has to be replicated via the align class or via get_texture_coordinates with additional scaling.

                    • 7. Re: How to project points to pixel? Librealsense SDK 2.0
                      jb455

                      I believe the point cloud is aligned to the depth image, which itself is aligned to one of the IR images (from streaming with the viewer app and putting my finger over the lenses in turn, it appears to be the left lens as you're looking at it, which is called "Infrared 1" in the UI).

                      From what I understand, the standard point cloud is equivalent to MapDepthFrameToCameraSpace, using align will be like MapDepthFrameToColorSpace, and passing the aligned depth image to pointcloud should be like MapColorFrameToCameraSpace.

                       

                      We're fast approaching the limits of my knowledge here so hopefully someone else will be able to double-check my assertions!

                      • 8. Re: How to project points to pixel? Librealsense SDK 2.0
                        svebert

                        void * MapDepthFrameToColorSpace()

                        {

                             void * pDepthProjected = NULL;

                             if(!m_bIsAligned) //boolean which flags whether the alignment witht the proedure align::process is used or not (somewhere else in the class)

                             {

                                  rs2::pointcloud pc;

                                  pc.map_to(m_oFrameset.get_color_frame()); //all points have coordinate system of the color camera

                                  m_oDepthPointsInColorCoords = pc.calculate(m_oFrameset.get_depth_frame()); //calculate the coordinates of the depth frame in the color camera coordinate system

                                  pDepthProjected = (void *)m_oDepthPointsInColorCoords.get_texture_coordinates(); //project 3D-points to color-camera plane //be aware, that these coordinates are in an interval [0..1], which has to be scaled by the image width and height

                              

                            }

                            else

                            {

                                  //if depth and color are aligned and the mapping was not produced before, compute (trivial) mapping

                                  if (m_pDepthPointsInColorSpace == NULL) //compute mapping only once

                                  {

                                       int width = m_oFrameset.get_depth_frame().get_width();

                                       int height = m_oFrameset.get_depth_frame().get_height();

                                       m_pDepthPointsInColorSpace = new point2D[width*height];

                                       point2D * pTmp = m_pDepthPointsInColorSpace;

                                       for (int y = 0; y < height; y++)

                                       {

                                            for (int x = 0; x < width; x++)

                                            {

                                                 pTmp->x = x;

                                                 pTmp->y = y;

                                            }

                                       }

                                  }

                                  pDepthProjected = m_pDepthPointsInColorSpace;

                            }

                          return pDepthProjected;

                        }

                         

                        void * MapDepthFrameToCameraSpace()

                        {

                             void * pVertices = NULL;

                             rs2::pointcloud pc;

                             pc.map_to(m_oFrameset.get_depth_frame()); //all points have coordinate system of the depth camera

                             m_oDepthPointsInDepthCoords = pc.calculate(m_oFrameset.get_depth_frame()); //calculate the coordinates of the depth frame in the depth camera coordinate system

                             pVerticesTmp = m_oDepthPointsInDepthCoords.get_vertices();

                             pVertices = (void *)pVerticesTmp;

                             return pVertices;

                        }

                         

                        void * ClRealSenseD400::MapColorFrameToCameraSpace()

                        {

                             void * pVertices = NULL;

                             rs2::pointcloud pc;

                             pc.map_to(m_oFrameset.get_depth_frame()); //all points have coordinate system of the depth camera

                             m_oColorPointsInColorCoords = pc.calculate(m_oFrameset.get_color_frame()); //calculate the coordinates of the color frame in the depth camera coordinate system

                             pVertices = (void *) m_oColorPointsInColorCoords.get_vertices();

                             return pVertices;

                        }

                         

                        I wrote three functions to resemble the three functions MapDepthFrameToColorSpace, MapDepthFrameToCameraSpace and MapColorFrameToCameraSpace; I am ok with the ...ToCameraSpace functions.

                         

                        I am still unhappy with the solution of MapDepthFrameToColorSpace, because get_texture_coordinates does not map directly to the color frame pixels but to an interval [0...1], so that the coordinates have to be scaled by color image width and height.

                        Also, the approach with the align class is not nice, because now, color or depth image dimension will be changed to have same image dimension in depth and color frame

                        A different approach would use rs2_project_point_to_pixel from librealsense2/rsutil.h instead of get_texture_coordinates(), but

                        • I am not sure which camera intrinsics I need to use. color or depth stream?
                        • These functions work only per pixel. Thus I have to loop over every space point and apply the function; In my case, where I want to return a pointer to the pointcloud, I would need to allocate a second pointcloud in memory with the result of rs2_project_point_to_pixel;

                         

                        As this is a very complicated topic, I wonder why there is no function for accomplishing MapDepthFrameToColorSpace in the SDK2.0;

                         

                        • 9. Re: How to project points to pixel? Librealsense SDK 2.0
                          jb455

                          The intrinsics to use in project_point_to_pixel will be whichever image your depth is aligned to. So if it's the raw depth image, use depth intrinsics; if it's aligned to colour use colour intrinsics.

                           

                          I'm not aware of a way to map colour to depth without going via camera space, though I haven't had cause to try it myself. Perhaps one of the devs/Intel staff will be able to give you advice on GitHub: Issues · IntelRealSense/librealsense · GitHub

                          • 10. Re: How to project points to pixel? Librealsense SDK 2.0
                            thePaintedSnipe

                            Hi MartyG,

                             

                              Sorry for hijacking this thread. I had a question which I wanted to ask, and can't resist.

                              Question relates to the camera extrinsic parameter on D435.

                             

                            The Wiki page "Projection in RealSense" is very interesting.

                            This page says that there is no rotation between infra1 and infra2 on D4 series. (under last section Model specific details.)

                            Also, there is translation only on one axis.

                             

                              Can I assume that same is the case with Depth to Color transformation?

                              i.e. If I have depth points, then can I obtain color points just by translation on x-axis [translation on y & z axis being zero].

                             

                              Also is there any way to grab the extrinsic parameters, without having to set up a full fledge application?

                             

                              For example, I am using D4xx under ROS, and I am just able to dump the intrinsic matrix from camera_info message.

                              Is there a similar easy way to dump extrinsic matrix somewhere?

                             

                            Regards,

                            PS

                            • 11. Re: How to project points to pixel? Librealsense SDK 2.0
                              MartyG

                              Your translation question is a bit outside of my programming experience.  I will field it to stream programming expert jb455 to see if he can offer advice on it. 

                               

                              In regard to your other question, the ability to create and edit intrinsics and extrinsics using the Python language was recently added to the SDK.

                              1 of 1 people found this helpful
                              • 12. Re: How to project points to pixel? Librealsense SDK 2.0
                                jb455

                                I would suspect that the simple translation between the IR cameras will be as a result of them being identical sensors fixed on the same board to be in-line on the horizontal axis. The colour camera will likely be different, with different intrinsics, and possibly on a different plane to the others. Maybe you can get by in your app with a very rough depth to colour translation but I don't think it would be very accurate across the whole image.

                                 

                                I'm not familiar with ROS, but in C# getting extrinsics is pretty simple:

                                var pipeline = new Pipeline();
                                
                                var cfg = new Config();
                                cfg.EnableStream(Stream.Depth, 640, 480);
                                cfg.EnableStream(Stream.Color, Format.Bgr8);
                                
                                var profile = pipeline.Start(cfg);
                                var depthStream = profile.GetStream(Stream.Depth);
                                var colourStream = profile.GetStream(Stream.Color);
                                
                                var extrinsics = depthStream.GetExtrinsicsTo(colourStream);
                                

                                 

                                Hopefully that's easy to translate!

                                • 13. Re: How to project points to pixel? Librealsense SDK 2.0
                                  thePaintedSnipe

                                  Hi JB,

                                   

                                    Thanks for your suggestion.

                                   

                                    I managed to print the Depth to Color extrinsics under ROS. There is a code section where these matrices are computed, and I just added logs around that place.

                                   

                                    Regarding camera calibration, there are two things: Extrinsics, Intrinsics.

                                    While Intrinsic depends on camera construction [focal length, aspect ratio etc], the extrinsic depends purely on the Camera Pose.

                                   

                                    If you consider the pose of Color sensor w.r.t. Depth sensor in a 3-D co-ordinate system, then it is similar to how two infra cameras are positioned w.r.t. each other.

                                    So, I had a feel that it will require only X translation. Which means that if you move these cameras on x-axis w.r.t. each other, their centres will eventually align.

                                    Here are the rotation and translation value which I got, and which confirm this understanding

                                   

                                  Translation (if we just consider 3-decimal points then y & z are 0)

                                  0.014696 -0.000154 -0.000151

                                   

                                  Rotation (if we round off the values, its an identity matrix almost)

                                  0.999958 -0.005230 -0.007479

                                  0.005223  0.999986 -0.000844

                                  0.007483  0.000805  0.999972

                                   

                                  This resolves my question. I posted the above information here in case it will benefit any one else.