本文不僅解釋深度影象如何轉化為鐳射雷達，更通過筆者的親測闡釋了為什麼kinect深度影象轉化的資料只能檢測到平行kinect的障礙物，而較低的障礙物或者較高的障礙物檢測不到。幫助新手少走彎路，當然有些知識和圖片偷襲別人的。

首先先看下原理：

1. 深度圖轉鐳射原理

原理如圖（1）所示。深度圖轉鐳射中，對任意給定的一個深度影象點m(u,v,z)

，其轉換鐳射的步驟為：

a.將深度影象的點m(u,v,z)

b.計算直線AO

，計算公式如下：θ=atan(x/z)

c.將叫AOC

可如下計算：

 laser[n]的值為M在x軸上投影的點C到相機光心O的距離r

，即

核心程式碼

我們的想法是取深度影象上每一列最小值依次儲存到雷達ranges[]陣列中，這樣理論上，我們將會獲kinect前方縱向上最近的障礙物距離，ranges[]陣列體現了橫向每個障礙點到kinect距離。但是實際，縱向上掃面的高度極為苛刻，並不能把地面到到0.6米的高度都掃一遍，因為什麼呢，請看後面總結。先看核心程式碼，思路：先行掃描將資料存到ranges[]中，然後再高度掃面，比較下一行與原來資料ranges[]，小資料替換原來ranges[]中的大資料，這樣就將高度給壓縮了。

  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();  中心點
      float center_y = cam_model.cy();   
      // Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y)
      //在計算(X，Y)的時候，結合單位轉換(如果必要的話)
      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){ 高度遍歷，層次遍歷
	 for (int u = 0; u < (int)depth_msg->width; u++) // Loop over遍歷 each pixel in row
	{	行遍歷計算距離r,儲存到一維雷達陣列中ranges[]中
		T depth = depth_row[u];
		double r = depth; // Assign to pass through NaNs and Infs
		double th = -atan2((double)(u - center_x) * constant_x, unit_scaling); // Atan2(x, z), but depth divides out
		int index = (th - scan_msg->angle_min) / scan_msg->angle_increment; //當前資料所在ranges[]的下標
      
		if (depthimage_to_laserscan::DepthTraits<T>::valid(depth)){ // Not NaN or Inf
		   // Calculate 計算in XYZ
		double 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.
	    	if(use_point(r, scan_msg->ranges[index], scan_msg->range_min, scan_msg->range_max)){
	       	scan_msg->ranges[index] = r;//此函式進行此障礙點到相機距離r與其列存的當前值ranges[index]比較,
	       }//返回ture,說明r值小，找出了當前此列高度上距離最近的障礙物
	 }
    }
 }

重點

現實世界的實際距離是通過深度影象轉化的，而我們的scan_hight是針對深度影象掃描高度，深度影象類似一張照片，如

這張圖偏中是下方基本都是地面，當我掃面高度超過一定範圍，就會把地面當成障礙物引入。所以如果我想檢測相機前方1米高度為10CM的障礙物，你就得把scan_hight設定到200（親測），這樣是可以檢測到了，但是再遠1米的地面都會被檢測，都會引入當成障礙物，這樣的結果是，導致全域性都是障礙物。可能會想，找到一個合適scan_hight,並沒有，通常設定為10。scan_hight對掃面的高度影響極小，但對遠處的障礙物影響很大，所以稍微改動，得不到想要的結果，反而影響的全域性了規劃。

所以，最好的辦法就是給機器人另加感測器。