Depthimage_to_laserscan Code Interpretation

Source: Internet
Author: User
Tags ranges

On the code:

1Template<typename t>2     voidConvertConstsensor_msgs::imageconstptr& Depth_msg,ConstImage_geometry::P inholecameramodel&Cam_model,3                  Constsensor_msgs::laserscanptr& Scan_msg,Const int& Scan_height)Const{4         //Use correct principal point from calibration5         floatcenter_x = ();//6         floatCenter_y ();7 8         //Combine Unit conversion (if necessary) with scaling by focal length for computing (x, y)9         Doubleunit_scaling = Depthimage_to_laserscan::D epthtraits<t>::tometers (T (1) );Ten         floatconstant_x = Unit_scaling/cam_model.fx ();// One         floatConstant_y = unit_scaling/cam_model.fy (); A  -         Constt* Depth_row = reinterpret_cast<ConstT*> (&depth_msg->data[0]); -         intRow_step = Depth_msg->step/sizeof(T); the  -         intOffset = (int) ( ()-scan_height/2); -Depth_row + = Offset*row_step;//Offset to center of image -  +          for(intv = offset; v < Offset+scan_height_; v++, Depth_row + =row_step) { -              for(intU =0; U < (int) depth_msg->width; u++)//Loop over each pixel in row +             { AT depth =Depth_row[u]; at  -                 Doubler = Depth;//Assign to pass through NaNs and INFs -                 Doubleth =-atan2 ((Double) (u-center_x) * constant_x, unit_scaling);//Atan2 (x, z), but depth divides out -                 intindex = (th-scan_msg->angle_min)/scan_msg->angle_increment; -  -                 if(Depthimage_to_laserscan::D epthtraits<t>::valid (depth)) {//Not NaN or Inf in                     //Calculate in XYZ -                     Doublex = (u-center_x) * depth *constant_x; to                     Doublez = depthimage_to_laserscan::D epthtraits<t>:: tometers (depth); +  -                     //Calculate Actual distance ther = sqrt (POW (x,2.0) + POW (z,2.0)); *                 } $ Panax Notoginseng                 //determine if this point should is used. -                 if(Use_point (R, Scan_msg->ranges[index], scan_msg->range_min, scan_msg->Range_max)) { theScan_msg->ranges[index] =R; +                 } A             } the         } +}

It is visible that the CONVERT function selects the center number of the depth graph row, then calculates the distance per pixel based on the center of the camera and selects the minimum distance from the same column (implemented by the function Use_point) to fill the Laserscan Data[index].


    1. Depthimage_to_laserscan is not effective in the complex 3D environment has a good projection into the effect of Laserscan
    2. If the range of columns is extended to the maximum, is there a better barrier-avoidance effect for 2D slam? Can real-time guarantee?
    3. For deep cameras, consider pointcloud_to_laserscan.

Depthimage_to_laserscan Code Interpretation

Contact Us

The content source of this page is from Internet, which doesn't represent Alibaba Cloud's opinion; products and services mentioned on that page don't have any relationship with Alibaba Cloud. If the content of the page makes you feel confusing, please write us an email, we will handle the problem within 5 days after receiving your email.

If you find any instances of plagiarism from the community, please send an email to: and provide relevant evidence. A staff member will contact you within 5 working days.

A Free Trial That Lets You Build Big!

Start building with 50+ products and up to 12 months usage for Elastic Compute Service

  • Sales Support

    1 on 1 presale consultation

  • After-Sales Support

    24/7 Technical Support 6 Free Tickets per Quarter Faster Response

  • Alibaba Cloud offers highly flexible support services tailored to meet your exact needs.