深度圖轉(zhuǎn)激光

• 預(yù)備知識
• • 相機模型
  • 計算機中圖像的表示
  • 世界坐標到圖像的映射關(guān)系
• 實現(xiàn)步驟
• • 1. 深度圖坐標點到世界坐標的轉(zhuǎn)換
  • 2. 計算坐標點的投影角度
  • 3. 映射到激光數(shù)據(jù)
  • 4. 對深度圖像掃描高度
• 代碼解析

預(yù)備知識

參考 https://www.cnblogs.com/gemstone/articles/2293806.html

相機模型

計算機中圖像的表示

世界坐標到圖像的映射關(guān)系

實現(xiàn)步驟

深度圖轉(zhuǎn)激光的主要思想是將一定高度范圍內(nèi)的數(shù)據(jù)進行投影，與點云圖轉(zhuǎn)激光類似。
具體步驟如下：

1. 深度圖坐標點到世界坐標的轉(zhuǎn)換

通過深度圖可以獲取到
（1）圖像坐標p(x,y)
深度信息depth得知
（2）相機原點Oc到物體所在平面的距離Zc
相機內(nèi)參得知
（3）焦距f

由(1) (2) (3)，根據(jù)針孔相機模型，可求出世界坐標P(Xw,Yw,Zw)，P點的相機坐標P(Xc,Yc,Zc)

2. 計算坐標點的投影角度

如圖示，計算直線𝐴𝑂c和B𝑂c的夾角𝐴𝑂cB，計算公式如下：
𝜃=𝑎𝑡𝑎𝑛(𝑥/𝑧)

3. 映射到激光數(shù)據(jù)

將角𝐴𝑂cB影射到相應(yīng)的激光數(shù)據(jù)槽中。

已知激光的最小最大范圍[𝛼,𝛽],激光束共細分為𝑁分，那么可用數(shù)組𝑙𝑎𝑠𝑒𝑟[𝑁]表示激光數(shù)據(jù)。那么點P投影到數(shù)組laser中的索引值𝑛

可如下計算：

𝑛=(𝜃?𝛼)/((𝛽?𝛼)/𝑁)=𝑁(𝜃?𝛼)/(𝛽?𝛼)

𝑙𝑎𝑠𝑒𝑟[𝑛]的值為P在Xc𝑂cZc面上投影的點B到相機光心𝑂c的距離𝑟，即

𝑙𝑎𝑠𝑒𝑟[𝑛]=𝑟=𝑂B=sqrt(𝑧^2 + 𝑥^2)

4. 對深度圖像掃描高度

比較一定高度范圍內(nèi)，如果有距離最小的點，則替換原來距離𝑟。

代碼解析

深度圖轉(zhuǎn)激光源碼參考在ROS包depthimage_to_laserscan中的實現(xiàn)。

核心思路：先行掃描將數(shù)據(jù)存到ranges[]中，然后再高度掃面，比較下一行與原來數(shù)據(jù)ranges[]，小數(shù)據(jù)替換原來ranges[]中的大數(shù)據(jù)，這樣就將高度給壓縮了。

/*** Converts the depth image to a laserscan using the DepthTraits to assist....* @param depth_msg The UInt16 or Float32 encoded depth message.* @param cam_model The image_geometry camera model for this image.* @param scan_msg The output LaserScan.* @param scan_height The number of vertical pixels to feed into each angular_measurement.* */template<typename T>void convert(const sensor_msgs::ImageConstPtr& depth_msg, const image_geometry::PinholeCameraModel& cam_model, const sensor_msgs::LaserScanPtr& scan_msg, const int& scan_height) const{// Use correct principal point from calibration 使用校正的正確的主要點float center_x = cam_model.cx();//圖像中心位置xfloat center_y = cam_model.cy();//圖像中心位置y// Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y)//在計算(X，Y)的時候，結(jié)合單位轉(zhuǎn)換(如果必要的話)double unit_scaling = depthimage_to_laserscan::DepthTraits<T>::toMeters( T(1) );float constant_x = unit_scaling / cam_model.fx();float constant_y = unit_scaling / cam_model.fy();const T* depth_row = reinterpret_cast<const T*>(&depth_msg->data[0]);int row_step = depth_msg->step / sizeof(T);int offset = (int)(cam_model.cy()-scan_height/2);depth_row += offset*row_step; // Offset to center of image// 高度遍歷for(int v = offset; v < offset+scan_height_; v++, depth_row += row_step){ // 行遍歷計算距離r,保存到一維雷達數(shù)組中ranges[]中for (int u = 0; u < (int)depth_msg->width; u++) // Loop over each pixel in row{ T depth = depth_row[u];double r = depth; // Assign to pass through NaNs and Infsdouble th = -atan2((double)(u - center_x) * constant_x, unit_scaling); // 計算夾角AOC，Atan2(x, z),實際上這里省略了深度值，因為分子分母都有，所以就略去了 but depth divides outint index = (th - scan_msg->angle_min) / scan_msg->angle_increment;//計算對應(yīng)激光數(shù)據(jù)的索引if (depthimage_to_laserscan::DepthTraits<T>::valid(depth)){ // Not NaN or Inf// Calculate in XYZ,計算XYZdouble x = (u - center_x) * depth * constant_x;double z = depthimage_to_laserscan::DepthTraits<T>::toMeters(depth);// Calculate actual distance，計算激光的真實距離r = sqrt(pow(x, 2.0) + pow(z, 2.0));}// Determine if this point should be used.判斷激光距離是否超出預(yù)設(shè)的有效范圍if(use_point(r, scan_msg->ranges[index], scan_msg->range_min, scan_msg->range_max)){scan_msg->ranges[index] = r;}}}}image_geometry::PinholeCameraModel cam_model_; ///< image_geometry helper class for managing sensor_msgs/CameraInfo messages.float scan_time_; ///< Stores the time between scans.float range_min_; ///< Stores the current minimum range to use.float range_max_; ///< Stores the current maximum range to use.int scan_height_; ///< Number of pixel rows to use when producing a laserscan from an area.std::string output_frame_id_; ///< Output frame_id for each laserscan. This is likely NOT the camera's frame_id.};

總結(jié)

以上是生活随笔為你收集整理的深度图转激光原理以及代码解析的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。