[kinect]地面信息提取算法

created by Dejavu
(不断更新中)

---

• 简介

地面信息的提取对于车形的智能机器人来说十分重要，之前一直采用双目视觉来绘制周边环境的三维点云，但是由于双目视觉两个CCD摄像头采集速率无法做到绝对的同步，所以在动态计算的过程中难免会出现较大的重建误差，所以现在采用了kinect来做动态的重建工作。
这里的算法在kinect处于静态的环境下，且kinect水平面与地面呈(0~45°)之间时，算法的地面提取效果最佳，由于对于动态精度较高的slam算法还在学习中，这里的算法只做学习用，有很多优化还没做，在ubuntu下运行大概是10fps的样子。

点云导入octovis后的3D图

地面三维坐标二维化并且做简单的slam拼合

• 用到的库和算法

（有空补齐）

• c++代码

• imgStream.h --- 包含kinect驱动部分代码，一些基本的空间运算方法
  #ifndef _IMGSTREAM_H
  #define _IMGSTREAM_H

   #include "libfreenect.hpp"
   #include 
   #include 
   #include 
   #include 
   #include 
  
   using namespace std;
  
   #define COLS 640
   #define ROWS 480
   #define INDEX(pt,xoffset,yoffset) depthIndex[pt.x+xoffset+(pt.y+yoffset)*COLS]
  
   typedef unsigned short US;
  
   class Mutex
   {
   public:
       Mutex()
       {
           pthread_mutex_init(&m_mutex, NULL);
       }
  
       void lock()
       {
           pthread_mutex_lock(&m_mutex);
       }
  
       void unlock()
       {
           pthread_mutex_unlock(&m_mutex);
       }
  
       class ScopedLock
       {
       public:
           ScopedLock(Mutex &mutex) : _mutex(mutex)
           {
               _mutex.lock();
           }
  
           ~ScopedLock()
           {
               _mutex.unlock();
           }
  
       private:
           Mutex &_mutex;
       };
  
   private:
       pthread_mutex_t m_mutex;
   };
  
   class MyFreenectDevice : public Freenect::FreenectDevice
   {
  
   public:
       MyFreenectDevice(freenect_context *_ctx, int _index)
           : Freenect::FreenectDevice(_ctx, _index),
           rgbMat(cv::Size(640, 480), CV_8UC3, cv::Scalar::all(0)),
           depthMat(cv::Size(640, 480), CV_16UC1),
           m_new_rgb_frame(false), m_new_depth_frame(false)
       {
           setDepthFormat(FREENECT_DEPTH_REGISTERED);
       }
  
       // Do not call directly, even in child
       void VideoCallback(void *_rgb, uint32_t timestamp)
       {
           Mutex::ScopedLock lock(m_rgb_mutex);
           uint8_t* rgb = static_cast(_rgb);
           rgbMat.data = rgb;
           m_new_rgb_frame = true;
       }
       // Do not call directly, even in child
       void DepthCallback(void *_depth, uint32_t timestamp)
       {
           Mutex::ScopedLock lock(m_depth_mutex);
           uint16_t* depth = static_cast(_depth);
           depthMat.data = (uchar*)depth;
           m_new_depth_frame = true;
       }
  
       bool getRGB(cv::Mat &buffer)
       {
           Mutex::ScopedLock lock(m_rgb_mutex);
  
           if (!m_new_rgb_frame)
               return false;
  
           cv::cvtColor(rgbMat, buffer, CV_RGB2BGR);
           m_new_rgb_frame = false;
  
           return true;
       }
  
       bool getDepth(cv::Mat &buffer)
       {
           Mutex::ScopedLock lock(m_depth_mutex);
  
           if (!m_new_depth_frame)
               return false;
  
           buffer = depthMat.clone();
           m_new_depth_frame = false;
  
           return true;
       }
  
   private:
       Mutex m_rgb_mutex;
       Mutex m_depth_mutex;
       cv::Mat rgbMat;
       cv::Mat depthMat;
       bool m_new_rgb_frame;
       bool m_new_depth_frame;
   };
  
   struct DepthIndex {
       cv::Point cameraPt;
       cv::Point3f worldPt;
       cv::Vec3b color;
       int planeFlags;
       bool isGnd;
   };
  
   struct DataStream {
       cv::Mat color;
       cv::Mat depth;
   };
  
   class CameraData {
   public:
       static const float fx = 594.21f;
       static const float fy = 591.04f;
       static const float cx = 339.5f;
       static const float cy = 242.7f;
   };
  
   cv::Point3f depth_to_world(int camerax, int cameray, int depth) {
       float f = 595.f;
       return cv::Point3f((camerax - (640 - 1) / 2.f)*depth / f, (cameray - (480 - 1) / 2.f)*depth / f, depth);
   }
  
   double a,b,c,d;
   double staticA,staticB,staticC;
   float gnd_camera_pitch,gnd_camera_roll,gnd_camera_yaw;
   //typical plane ax+by+cz+d=0
   bool get_param(cv::Point3f p1, cv::Point3f p2, cv::Point3f p3) {
       if ((p1.x*p2.y - p2.x*p1.y) + (p1.y*p2.z - p2.y*p1.z) + (p1.z*p2.x - p2.z*p1.x) == 0)
           return false;
       a = ((p2.y - p1.y)*(p3.z - p1.z) - (p2.z - p1.z)*(p3.y - p1.y));
       b = ((p2.z - p1.z)*(p3.x - p1.x) - (p2.x - p1.x)*(p3.z - p1.z));
       c = ((p2.x - p1.x)*(p3.y - p1.y) - (p2.y - p1.y)*(p3.x - p1.x));
       d = -(a*p1.x + b*p1.y + c*p1.z);
       return true;
   }
  
   double get_distance(cv::Point3f pt) {
       return abs(a*pt.x + b*pt.y + c*pt.z + d) / sqrt(a*a + b*b + c*c);
   }
  
   double get_2d_pt2pt_distance(cv::Point p1, cv::Point p2) {
       return sqrt((p1.x - p2.x)*(p1.x - p2.x) + (p1.y - p2.y)*(p1.y - p2.y));
   }
  
   void get_cross(cv::Point p1, cv::Point p2, float &line_k, float &line_offset) {
       line_k = -1 / ((p1.y - p2.y) / (p1.x - p2.x));
       line_offset = (p2.y + p1.y) / 2.0 - line_k*(p2.x + p1.x) / 2.0;
   }
  
   #endif
  

• rotateQuater.h --- 四元数旋转用到的一些函数
  #ifndef _ROTATEQUATER_H
  #define _ROTATEQUATER_H

   #include 
   #include 
   #include 
  
   struct Quater {
       double t;
       double x;
       double y;
       double z;
   };
  
   struct rotateAction {
       cv::Point3f vec;
       double radian;
   };
  
   Quater cross(Quater l, Quater r) {
       Quater ans;
       double d1, d2, d3, d4;
  
       d1 = l.t*r.t;
       d2 = -l.x*r.x;
       d3 = -l.y*r.y;
       d4 = -l.z*r.z;
       ans.t = d1 + d2 + d3 + d4;
  
       d1 = l.t*r.x;
       d2 = l.x*r.t;
       d3 = l.y*r.z;
       d4 = -l.z*r.y;
       ans.x = d1 + d2 + d3 + d4;
  
       d1 = l.t*r.y;
       d2 = l.y*r.t;
       d3 = l.z*r.x;
       d4 = -l.x*r.z;
       ans.y = d1 + d2 + d3 + d4;
  
       d1 = l.t*r.z;
       d2 = l.z*r.t;
       d3 = l.x*r.y;
       d4 = -l.y*r.x;
       ans.z = d1 + d2 + d3 + d4;
       return ans;
   }
  
   rotateAction getGndVector(cv::Point3f vec) {
       cv::Point3f GND = cv::Point3f(0, 1, 0);
  
       rotateAction data;
       data.vec.x = GND.y*vec.z - GND.z*vec.y;
       data.vec.y = GND.z*vec.x - GND.x*vec.z;
       data.vec.z = GND.x*vec.y - GND.y*vec.x;
  
       data.radian = 90 * 3.1415926 / 180.0 - acos(vec.z / sqrt(vec.x*vec.x + vec.y*vec.y + vec.z*vec.z));
  
       return data;
   }
  
   Quater rotationalQuater(double radian, cv::Point3f axis) {
       Quater ans;
       double norm;
       double tmp_sin;
  
       norm = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z;
       if (norm <= 0.0) return ans;
  
       norm = 1.0 / sqrt(norm);
       axis.x *= norm;
       axis.y *= norm;
       axis.z *= norm;
  
       tmp_sin = sin(0.5*radian);
  
       ans.t = cos(0.5*radian);
       ans.x = tmp_sin*axis.x;
       ans.y = tmp_sin*axis.y;
       ans.z = tmp_sin*axis.z;
  
       return ans;
   }
  
   cv::Point3f rotate(Quater quater[], cv::Point3f pt) {
       Quater quater_pt;
       quater_pt.t = 0.0;
       quater_pt.x = pt.x;
       quater_pt.y = pt.y;
       quater_pt.z = pt.z;
  
       Quater tmp = cross(quater[1], quater_pt);
       tmp = cross(quater_pt, quater[0]);
  
       cv::Point3f data;
       data.x = tmp.x;
       data.y = tmp.y;
       data.z = tmp.z;
       return data;
   }
  
   #endif
  

• uhoo_cloud.h --- 一些三维点云处理的函数
  #ifndef _UHOO_CLOUD_H
  #define _UHOO_CLOUD_H

   #include "uhoo_tools.h"
   #include "imgStream.h"
   #include "rotateQuater.h"
   #include 
   #include 
   #include 
   #include 
   #include 
   #include 
   #include 
  
   #define ROBOT_HEIGHT 200.0
   #define ROBOT_WIDTH  150.0
  
   class DataStruct {
   public:
       DepthIndex *depthInfo;
       DataStream dataStream;
   };
  
   pcl::PointCloud::Ptr worldPt2cloud(DataStruct data) {
       pcl::PointCloud::Ptr cloud(new pcl::PointCloud);
       int cnt = 0;
       for(int y=0;y(y,x);
               pcl::PointXYZRGBA p;
               p.x = pt.x;
               p.y = pt.y;
               p.z = pt.z;
               if( data.depthInfo[index].isGnd ){
                   p.b = 255;
                   p.g = 0;
                   p.r = 0;
                   cnt++;
               }
               else {
                   p.b = 0;
                   p.g = 150;
                   p.r = 50;
               }
               cloud->points.push_back(p);
           }
       std::cout<<"src gnd cnt:"<height = 1;
       cloud->width = cloud->points.size();
       cloud->is_dense = false;
  
       return cloud;
   }
  
   pcl::PointCloud::Ptr gnd2cloud(pcl::PointCloud::Ptr cloud) {
       pcl::PointCloud::Ptr gndCloud(new pcl::PointCloud);
       int cnt = 0;
       for(int i=0;ipoints.size();i++) if(cloud->points[i].b == 255) {
           gndCloud->points.push_back(cloud->points[i]);
           cnt++;
       }
  
       std::cout<<"cnt:"<height = 1;
       gndCloud->width = cnt;
       gndCloud->is_dense = false;
  
       return gndCloud;
   }
  
   void colorToPlane(cv::Mat rgb) {
           cv::Mat color(ROWS,COLS,CV_8UC3,cv::Scalar::all(0));
           //jump = 17
  
           for(int y=0;y(y,x) = cv::Vec3b(150,50,20);
       cv::addWeighted(color,0.5,rgb,0.5,0,color);
           cv::imshow("color",color);
   }
  
   cv::Vec3f matrix2angle(cv::Mat R) {
       cv::Vec3f angle;
       if(abs(R.at(2,0)) != 1) {
                   angle[0] = -asin(R.at(2,0));
                   angle[1] = atan2(R.at(2,1)/cos(angle[0]),
                                    R.at(2,2)/cos(angle[0]));
                   angle[2] = atan2(R.at(1,0)/cos(angle[0]),
                                    R.at(0,0)/cos(angle[0]));
           }
           else {
                   angle[2] = 0;
                   if(R.at(2,0) == -1) {
                           angle[0] = PI/2;
                           angle[1] = angle[2]+atan2(R.at(0,1),
                                                     R.at(0,2));
                   }
                   else {
                           angle[0] = -PI/2;
                           angle[1] = -angle[2]+atan2(-R.at(0,1),
                                                      -R.at(0,2));
                   }
           }
       for(int i=0;i<3;i++) angle[i] = angle[i]*180/PI;
           return angle;
   }
  
   void translate2gnd(pcl::PointCloud::Ptr ¤tCloud) {
       // take gnd -> combine them to one -> set range -> 空洞矩形，宁可多取不可少取
       rotateAction vec = getGndVector(cv::Point3f(staticA,staticB,staticC));
       Quater quater[2];
  
       quater[0] = rotationalQuater(vec.radian,vec.vec);
       quater[1] = rotationalQuater(-vec.radian,vec.vec);
  
       for(int i=0;ipoints.size();i++) {
           cv::Point3f pt;
           pt = rotate(quater,cv::Point3f(currentCloud->points[i].x,currentCloud->points[i].y,currentCloud->points[i].z));
           currentCloud->points[i].x = pt.x;
           currentCloud->points[i].y = pt.y;
           currentCloud->points[i].z = pt.z;
       }
   }
  
   double pos(0);
   void initCloud(pcl::PointCloud::Ptr output) {
       bool calcStart(false);
       for(int i=0;ipoints.size()*0.3;i++) {
           double data = output->points[random()%output->points.size()].y;
           pos += data;
           if(calcStart) pos /= 2;
           else calcStart = true;
       }
       std::cout<<"---->pos:"<::Ptr cloud) {
       int z_min=9999,z_max=-9999,x_min=9999,x_max=-9999;
       std::cout<<"gnd2img init...."<points.size(); i++) {
           pcl::PointXYZRGBA pt = cloud->points[i];
           if(pt.z > z_max) z_max = pt.z;
           if(pt.z < z_min) z_min = pt.z;
           if(pt.x > x_max) x_max = pt.x;
           if(pt.x < x_min) x_min = pt.x;
       }
  
       std::cout<<"max//min:"<points.size();i++) {
           pcl::PointXYZRGBA pt = cloud->points[i];
           //map.at(int(pt.z-z_min),int(pt.x-x_min)) = 100;
           circle(map,cv::Point(int(pt.x-x_min),int(pt.z-z_min)),4,cv::Scalar(0,200,50),-1);
       }
       cv::resize(map,map,cv::Size(map.cols/4,map.rows/4));
       cv::imwrite("uhoo_frist_map'_'.jpg",map);
       return map;
   }
  
   bool combine(pcl::PointCloud::Ptr currentCloud,pcl::PointCloud::Ptr &output) {
       bool calcStart(false);
       double pos_now(0);
  
       cv::Mat map_part;
  
       for(int i=0;ipoints.size()*0.3;i++) {
                   pos_now += currentCloud->points[i].y;
                   if(calcStart) pos_now /= 2;
                   else calcStart = true;
           }
  
       int move_pos = pos - pos_now;
  
       std::cout<<"pos,pos_now,move_pos:"< 300) return false;
       if(move_pos < 25 && move_pos > -25) {
           *output += *currentCloud;
           map_part = gnd2img(output);
           cv::imshow("map",map_part);
           return true;
       } else {
           for(int i=0;ipoints.size();i++)
               currentCloud->points[i].y += move_pos;
           *output += *currentCloud;
           map_part = gnd2img(output);
           cv::imshow("map",map_part);
           return true;
       }
   }
  
   bool bf_match(DataStruct dataStruct[],pcl::PointCloud::Ptr &output,int dist_norm_l2) {
       cv::Mat img[4];
       img[0] = dataStruct[0].dataStream.color;
       img[1] = dataStruct[1].dataStream.color;
       if(img[0].empty() || img[1].empty()) return false;
  
       std::vector kp[2];
       cv::Mat im[2];
       cv::OrbFeatureDetector ofd;
       cv::OrbDescriptorExtractor ode;
  
       cout<<"obj create done !"< matches;
       std::cout<<"create val ok"< goodMatches;
       double minDist = 9999;
       for(int i=0;i wpt;
       std::vector cpt;
       for(int i=0;i(0,i)) > 200 || abs(rvec.at(0,i)) > 200 )
                   return false;
  
       std::cout<<"inliers: "<

• uhoo_tools.h -- 在重建的三维坐标提取地面信息用到的函数
  #ifndef _UHOO_TOOLS_H
  #define _UHOO_TOOLS_H

   #include "imgStream.h"
   #include 
  
   #define PI 3.14159
  
   #define COLS 640
   #define ROWS 480
  
   DepthIndex depthIndex[COLS*ROWS];
   DepthIndex gndCandidate[COLS*ROWS];
   DepthIndex depthIndex_previous[COLS*ROWS];
   DepthIndex ydepthIndex[COLS*ROWS];
   DepthIndex radixArrays[10][COLS*ROWS];
  
   int GetNumInPos(int num,int pos)
   {
           int temp = 1;
           for (int i = 0; i < pos - 1; i++)
                   temp *= 10;
  
           return (num / temp) % 10;
   }
  
   void get_angle() {
           gnd_camera_pitch = atan2(c,b)*180.0/PI;
           gnd_camera_roll = atan2(b,a)*180.0/PI;
           gnd_camera_yaw = atan2(c,a)*180.0/PI;
           //std::cout<<"[/] vector: ["< 0) || (gnd_camera_pitch < -180+pitchSet && gnd_camera_pitch > -180));
           bool is_gnd_roll = ((gnd_camera_roll < 180-rollSet && gnd_camera_roll > rollSet) || (gnd_camera_roll < -rollSet && gnd_camera_roll > -180+rollSet));
           bool is_gnd_yaw = ((gnd_camera_yaw < 180-yawSet && gnd_camera_yaw > yawSet) || (gnd_camera_yaw < -yawSet && gnd_camera_yaw > -180+yawSet));
           //std::cout<<"[+] bool:  ["<(y,x));
                           depthIndex[ind].cameraPt = cv::Point(x,y);
               depthIndex[ind].color = dataStream.color.at(y,x);
                           depthIndex[ind].planeFlags = ind;
                           depthIndex[ind].isGnd = false;
                           if( depthIndex[ind].worldPt.y > 0 ){
                                   ydepthIndex[ylistCnt] = depthIndex[ind];
                                   ylistCnt++;
                           }
                   }
  
           //基数排序 awesome !!!!
           for (int i = 0; i < 10; i++)
                   radixArrays[i][0].worldPt = cv::Point3f(0,0,0);    //index为0处记录这组数据的个数
  
           for (int pos = 1; pos <= 4; pos++)
           {
                   for (int i = 0; i < ylistCnt; i++)
                   {
                           int pty = abs(ydepthIndex[i].worldPt.y);
                           int num = GetNumInPos(pty, pos);
                           int ptytmp = radixArrays[num][0].worldPt.y;
                           int index = ++ptytmp;
                           radixArrays[num][0].worldPt.y = ptytmp;
                           radixArrays[num][index] = ydepthIndex[i];
                   }
  
                   for (int i = 0, j =0; i < 10; i++)
                   {
                           for (int k = 1; k <= (int)radixArrays[i][0].worldPt.y; k++)
                                   ydepthIndex[j++] = radixArrays[i][k];
                           radixArrays[i][0].worldPt = cv::Point3f(0,0,0);
                   }
           }
  
           int gnd = ydepthIndex[ylistCnt-50].worldPt.y;
           int candidateCnt = 0;
           for(int i=0;i gnd - 25) {
                   gndCandidate[candidateCnt] = depthIndex[i];
                   candidateCnt++;
                   //cv::circle(show_all,gndCandidate[candidateCnt].cameraPt,2,cv::Scalar(0,200,150));
           }
           //std::cout<<"candidateCnt: "< va,vb,vc,vd;
           for(int i=1;i<10;i++) {
                   bool badPt(false),GoodPt(false);
                   bool isCheck[] = {false,false,false,false};
                   int where = rand()%candidateCnt;
                   DepthIndex randomCandidate = gndCandidate[where];
                   //std::cout<<"where: "< COLS || planePt[j].y > ROWS || planePt[j].x < 0 || planePt[j].y < 0)
                                   badPt = true;
                   }
                   //std::cout<

• func.cpp -- 主文件
  #include "uhoo_cloud.h"

   int main() {
       bool die(false);
  
       DataStream dataStream;
       DataStruct dataStruct, match, payload[2];
       cv::Mat depthMat(cv::Size(COLS, ROWS), CV_16UC1);
       cv::Mat colorMat(cv::Size(COLS, ROWS), CV_8UC3);
       Freenect::Freenect freenect;
       MyFreenectDevice& device = freenect.createDevice(0);
  
       device.startVideo();
       device.startDepth();
  
       while (1) {
           device.getDepth(depthMat);
           device.getRGB(colorMat);
           cv::imshow("rgb", colorMat);
           if (char(cv::waitKey(1)) == 27) break;
       }
  
       cv::Mat src;
       pcl::PointCloud::Ptr output(new pcl::PointCloud);
       bool isGND_previous(false);
       while (!die) {
           bool isGND_current(false);
  
           device.getDepth(depthMat);
           //src = depthMat.clone();
  
           dataStream.depth = depthMat;
           dataStream.color = colorMat;
           dataStruct.dataStream = dataStream;
           dataStruct.depthInfo = depthIndex;
  
           std::cout << "try to find gnd now !" << std::endl;
           if (find_gnd(dataStream, 20, 30, 60, 60)) {
               for (int y = 0;y < ROWS;y++)
                   for (int x = 0;x < COLS;x++) {
                       cv::Point pt = cv::Point(x, y);
                       if (get_distance(INDEX(pt, 0, 0).worldPt) < 50)
                           INDEX(pt, 0, 0).isGnd = true;
                   }
               isGND_current = true;
           }
           else continue;
  
           cout << "pass gnd check" << endl;
           dataStruct.depthInfo = depthIndex;
  
           colorToPlane(colorMat);
           device.getRGB(colorMat);
           cv::imshow("rgb", colorMat);
  
           payload[0] = dataStruct;
           payload[1] = match;
  
           std::cout << "check match now" << std::endl;
           if (isGND_previous) {
               bf_match(payload, output, 2);
           }
           else {
               staticA = a;
               staticB = b;
               staticC = c;
               isGND_previous = true;
               output = worldPt2cloud(dataStruct);
               translate2gnd(output);
               std::cout << "line params:" << a << "," << b << "," << c << "," << d << std::endl;
               initCloud(output);
           }
           if (char(cv::waitKey(1)) == 27) break;
  
           std::cout << "copy depth data now" << std::endl;
           for (int i = 0;i < COLS*ROWS;i++) depthIndex_previous[i] = depthIndex[i];
  
           std::cout << "copy complieted!" << std::endl;
           std::cout << "data trans now" << std::endl;;
           match = dataStruct; // need create a new space to save depthInfo
           match.depthInfo = depthIndex_previous;
           std::cout << "trans complieted !" << std::endl;
           match.dataStream.color = colorMat.clone();
           match.dataStream.depth = depthMat.clone();
       }
  
       return 0;
   }