深度影象轉偽鐳射雷達depthimage_to_laserscan
本文不僅解釋深度影象如何轉化為鐳射雷達,更通過筆者的親測闡釋了為什麼kinect深度影象轉化的資料只能檢測到平行kinect的障礙物,而較低的障礙物或者較高的障礙物檢測不到。幫助新手少走彎路,當然有些知識和圖片偷襲別人的。
首先先看下原理:
1. 深度圖轉鐳射原理
原理如圖(1)所示。深度圖轉鐳射中,對任意給定的一個深度影象點m(u,v,z)
,其轉換鐳射的步驟為:
a.將深度影象的點m(u,v,z)
轉換到深度相機座標系下的座標點M(x,y,z)b.計算直線AO
和CO的夾角AOC,計算公式如下:θ=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對掃面的高度影響極小,但對遠處的障礙物影響很大,所以稍微改動,得不到想要的結果,反而影響的全域性了規劃。
所以,最好的辦法就是給機器人另加感測器。
n=N(θ−α)/(β−α)