Depthimage_to_laserscan Code Interpretation

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 = Cam_model.cx ();//6         floatCenter_y =cam_model.cy ();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) (Cam_model.cy ()-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].

Conclusion:

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