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 <iostream> #include <vector> #include <cmath> #include <pthread.h> #include <opencv2/opencv.hpp> 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<uint8_t*>(_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<uint16_t*>(_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 <iostream> #include <math.h> #include <opencv2/opencv.hpp> 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 <vector> #include <pcl/common/transforms.h> #include <pcl/io/pcd_io.h> #include <Eigen/Core> #include <Eigen/Geometry> #include <opencv2/core/eigen.hpp> #include <octomap/octomap.h> #define ROBOT_HEIGHT 200.0 #define ROBOT_WIDTH 150.0 class DataStruct { public: DepthIndex *depthInfo; DataStream dataStream; }; pcl::PointCloud<pcl::PointXYZRGBA>::Ptr worldPt2cloud(DataStruct data) { pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBA>); int cnt = 0; for(int y=0;y<ROWS;y++) for(int x=0;x<COLS;x++) { int index = x+y*COLS; cv::Point3f pt = data.depthInfo[index].worldPt; if(pt.z == 0) continue; //cv::Vec3b color = data.dataStream.color.at<cv::Vec3b>(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:"<<cnt<<std::endl; cloud->height = 1; cloud->width = cloud->points.size(); cloud->is_dense = false; return cloud; } pcl::PointCloud<pcl::PointXYZRGBA>::Ptr gnd2cloud(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud) { pcl::PointCloud<pcl::PointXYZRGBA>::Ptr gndCloud(new pcl::PointCloud<pcl::PointXYZRGBA>); int cnt = 0; for(int i=0;i<cloud->points.size();i++) if(cloud->points[i].b == 255) { gndCloud->points.push_back(cloud->points[i]); cnt++; } std::cout<<"cnt:"<<cnt<<std::endl; gndCloud->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<ROWS;y++) for(int x=0;x<COLS;x++) if( INDEX(cv::Point(x,y),0,0).isGnd) color.at<cv::Vec3b>(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<float>(2,0)) != 1) { angle[0] = -asin(R.at<float>(2,0)); angle[1] = atan2(R.at<float>(2,1)/cos(angle[0]), R.at<float>(2,2)/cos(angle[0])); angle[2] = atan2(R.at<float>(1,0)/cos(angle[0]), R.at<float>(0,0)/cos(angle[0])); } else { angle[2] = 0; if(R.at<float>(2,0) == -1) { angle[0] = PI/2; angle[1] = angle[2]+atan2(R.at<float>(0,1), R.at<float>(0,2)); } else { angle[0] = -PI/2; angle[1] = -angle[2]+atan2(-R.at<float>(0,1), -R.at<float>(0,2)); } } for(int i=0;i<3;i++) angle[i] = angle[i]*180/PI; return angle; } void translate2gnd(pcl::PointCloud<pcl::PointXYZRGBA>::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;i<currentCloud->points.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<pcl::PointXYZRGBA>::Ptr output) { bool calcStart(false); for(int i=0;i<output->points.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:"<<pos<<std::endl; } cv::Mat gnd2img(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud) { int z_min=9999,z_max=-9999,x_min=9999,x_max=-9999; std::cout<<"gnd2img init...."<<std::endl; for(int i = 0; i<cloud->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:"<<x_max<<","<<z_max<<"//"<<x_min<<","<<z_min<<std::endl; cv::Mat map(cv::Size(int(abs(x_max-x_min)),int(abs(z_max-z_min))),CV_8UC3,cv::Scalar(255,255,255)); for(int i=0;i<cloud->points.size();i++) { pcl::PointXYZRGBA pt = cloud->points[i]; //map.at<uchar>(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<pcl::PointXYZRGBA>::Ptr currentCloud,pcl::PointCloud<pcl::PointXYZRGBA>::Ptr &output) { bool calcStart(false); double pos_now(0); cv::Mat map_part; for(int i=0;i<currentCloud->points.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:"<<pos<<","<<pos_now<<","<<move_pos<<std::endl; if(abs(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;i<currentCloud->points.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<pcl::PointXYZRGBA>::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<cv::KeyPoint> kp[2]; cv::Mat im[2]; cv::OrbFeatureDetector ofd; cv::OrbDescriptorExtractor ode; cout<<"obj create done !"<<endl; for(int i=0;i<2;i++) { cv::blur(img[i],img[i],cv::Size(3,3)); cv::cvtColor(img[i],img[i+2],CV_BGR2GRAY); ofd.detect(img[i+2],kp[i]); ode.compute(img[i+2],kp[i],im[i]); if(im[i].empty() || img[i+2].empty()) return false; } cv::BFMatcher matcher(cv::NORM_L2); std::vector<cv::DMatch> matches; std::cout<<"create val ok"<<std::endl; matcher.match(im[0],im[1],matches); std::cout<<"matches size: "<<matches.size()<<std::endl; if(matches.size() < 2) return false; std::vector<cv::DMatch> goodMatches; double minDist = 9999; for(int i=0;i<matches.size();i++) if(matches[i].distance != 0 && matches[i].distance < minDist) minDist = matches[i].distance; for(int i=0;i<matches.size();i++) if(matches[i].distance < dist_norm_l2*minDist) goodMatches.push_back(matches[i]); std::cout<<"goodMatches size: "<<goodMatches.size()<<std::endl; if(goodMatches.size() < 3) return false; cv::Mat dst; cv::drawMatches(img[0],kp[0],img[1],kp[1],goodMatches,dst); cv::imshow("dst",dst); std::vector<cv::Point3f> wpt; std::vector<cv::Point2f> cpt; for(int i=0;i<goodMatches.size();i++) { cv::Point2f p = kp[0][goodMatches[i].queryIdx].pt; cv::Point pt = cv::Point(p.x,p.y); if(INDEX(pt,0,0).worldPt.z == 0) continue; cpt.push_back(cv::Point2f(kp[1][goodMatches[i].trainIdx].pt)); wpt.push_back(INDEX(pt,0,0).worldPt); } if(wpt.size() < 2) return false; CameraData cameraData; double camera_matrix_data[3][3] = { {cameraData.fx,0,cameraData.cx}, {0,cameraData.fy,cameraData.cy}, {0,0,1} }; cv::Mat cameraMatrix(3,3,CV_64F,camera_matrix_data); cv::Mat rvec,tvec,inliers; cv::solvePnPRansac(wpt,cpt,cameraMatrix,cv::Mat(),rvec,tvec,false,100,1.0,100,inliers); for(int i=0;i<3;i++) if( abs(tvec.at<double>(0,i)) > 200 || abs(rvec.at<double>(0,i)) > 200 ) return false; std::cout<<"inliers: "<<inliers.rows<<std::endl; std::cout<<"R="<<rvec<<std::endl; std::cout<<"T="<<tvec<<std::endl; cv::Mat R; Eigen::Matrix3d r; cv::Rodrigues(rvec,R); cv::cv2eigen(R,r); Eigen::Isometry3d T = Eigen::Isometry3d::Identity(); Eigen::AngleAxisd angle(r); Eigen::Translation<double,3> trans(tvec.at<double>(0,0),tvec.at<double>(0,1),tvec.at<double>(0,2)); T = angle; // 012 021 102 120 201 210 T(0,3) = tvec.at<double>(0,1); T(1,3) = tvec.at<double>(0,2); T(2,3) = tvec.at<double>(0,0); pcl::PointCloud<pcl::PointXYZRGBA>::Ptr currentCloud = worldPt2cloud(dataStruct[0]); pcl::PointCloud<pcl::PointXYZRGBA>::Ptr currentGnd = gnd2cloud(currentCloud); pcl::transformPointCloud( *currentGnd, *currentGnd, T.matrix() ); translate2gnd(currentGnd); if(!combine(currentGnd,output)) std::cout<<"failed to combine"<<std::endl; //pcl::io::savePCDFileASCII("./pcd/cloud_final_gnd.pcd",*output); std::cout<<"done!"<<std::endl; return true; } #endif -
uhoo_tools.h -- 在重建的三维坐标提取地面信息用到的函数
#ifndef _UHOO_TOOLS_H
#define _UHOO_TOOLS_H#include "imgStream.h" #include <sys/time.h> #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: ["<<a<<","<<b<<","<<c<<","<<d<<" ]"<<std::endl; //std::cout<<"[/] angle: ["<<gnd_camera_pitch<<","<<gnd_camera_roll<<","<<gnd_camera_yaw<<"]"<<std::endl; } bool isGnd_angle(int pitchSet,int rollSet,int yawSet) { get_angle(); bool is_gnd_pitch = ((gnd_camera_pitch < pitchSet && gnd_camera_pitch > 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: ["<<is_gnd_pitch<<","<<is_gnd_roll<<","<<is_gnd_yaw<<"]"<<std::endl; return (is_gnd_pitch && is_gnd_roll && is_gnd_yaw); } bool find_gnd(DataStream dataStream,int minDist,int pitchSet,int rollSet,int yawSet) { cv::Mat depthMat = dataStream.depth; //cv::Mat show_all(ROWS,COLS,CV_8UC3,cv::Scalar::all(0)); int ylistCnt = 0; for(int y=0;y<ROWS;y++) for(int x=0;x<COLS;x++) { int ind = COLS*y+x; depthIndex[ind].worldPt = depth_to_world(x,y,depthMat.at<unsigned short>(y,x)); depthIndex[ind].cameraPt = cv::Point(x,y); depthIndex[ind].color = dataStream.color.at<cv::Vec3b>(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<COLS*ROWS;i++) if(depthIndex[i].worldPt.y > gnd - 25) { gndCandidate[candidateCnt] = depthIndex[i]; candidateCnt++; //cv::circle(show_all,gndCandidate[candidateCnt].cameraPt,2,cv::Scalar(0,200,150)); } //std::cout<<"candidateCnt: "<<candidateCnt<<std::endl; if(candidateCnt < 50) return false; cv::Point planePt[4]; cv::Point3f planeWPt[4]; int ptCnt(0); vector<float> 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: "<<where<<" Pt:"<<randomCandidate.cameraPt<<std::endl; int b1 = randomCandidate.cameraPt.y-randomCandidate.cameraPt.x; int b2 = randomCandidate.cameraPt.y+randomCandidate.cameraPt.x; planePt[0] = cv::Point(randomCandidate.cameraPt.x+minDist,randomCandidate.cameraPt.x+minDist+b1); planePt[1] = cv::Point(randomCandidate.cameraPt.x-minDist,randomCandidate.cameraPt.x-minDist+b1); planePt[2] = cv::Point(randomCandidate.cameraPt.x-minDist,-randomCandidate.cameraPt.x+minDist+b2); planePt[3] = cv::Point(randomCandidate.cameraPt.x+minDist,-randomCandidate.cameraPt.x-minDist+b2); for(int j=0;j<4;j++) { //std::cout<<" Pt:"<<planePt[j]<<" "; planeWPt[j] = cv::Point3f(.0,.0,.0); //cv::circle(show_all,planePt[j],3,cv::Scalar(200,100,0)); if(planePt[j].x > COLS || planePt[j].y > ROWS || planePt[j].x < 0 || planePt[j].y < 0) badPt = true; } //std::cout<<std::endl; if(badPt) continue; for(int j=0;j<candidateCnt;j++) { for(int k=0;k<4;k++) { if(!isCheck[k]) { if( abs(planePt[k].x - gndCandidate[j].cameraPt.x) < 2 && abs(planePt[k].y - gndCandidate[j].cameraPt.y) < 2 ) { //std::cout<<"================================================================================"<<std::endl; planeWPt[k] = gndCandidate[j].worldPt; isCheck[k] = true; } } if( isCheck[0] && isCheck[1] && isCheck[2] && isCheck[3] ) { GoodPt = true; break; } } } //if(!GoodPt) continue; //cv::imshow("test",show_all); //cv::waitKey(0); //for(int j=0;j<4;j++) std::cout<<planeWPt[j]<" "; //std::cout<<std::endl; if( planeWPt[0] != cv::Point3f(.0,.0,.0) && planeWPt[2] != cv::Point3f(.0,.0,.0) ) { get_param( randomCandidate.worldPt, planeWPt[0], planeWPt[2]); if( isGnd_angle(pitchSet,rollSet,yawSet) ) { va.push_back(a);vb.push_back(b); vc.push_back(c);vd.push_back(d); } } if( planeWPt[0] != cv::Point3f(.0,.0,.0) && planeWPt[3] != cv::Point3f(.0,.0,.0) ) { get_param( randomCandidate.worldPt, planeWPt[0], planeWPt[3]); if( isGnd_angle(pitchSet,rollSet,yawSet) ) { va.push_back(a);vb.push_back(b); vc.push_back(c);vd.push_back(d); } } if( planeWPt[1] != cv::Point3f(.0,.0,.0) && planeWPt[2] != cv::Point3f(.0,.0,.0) ) { get_param( randomCandidate.worldPt, planeWPt[1], planeWPt[2]); if( isGnd_angle(pitchSet,rollSet,yawSet) ) { va.push_back(a);vb.push_back(b); vc.push_back(c);vd.push_back(d); } } if( planeWPt[1] != cv::Point3f(.0,.0,.0) && planeWPt[3] != cv::Point3f(.0,.0,.0) ) { get_param( randomCandidate.worldPt, planeWPt[1], planeWPt[3]); if( isGnd_angle(pitchSet,rollSet,yawSet) ) { va.push_back(a);vb.push_back(b); vc.push_back(c);vd.push_back(d); } } ptCnt++; } //std::cout<<"param_num size:"<<va.size()<<std::endl; if( va.size() == 0 ) return false; //for(int i=0;i<va.size();i++) std::cout<<"[/] vector final [ "<<va[i]<<","<<vb[i]<<","<<vc[i]<<","<<vd[i]<<" ]"<<std::endl; double sum[] = {.0, .0, .0, .0}; for(int j=0;j<va.size();j++) { sum[0] += va[j];sum[1] += vb[j]; sum[2] += vc[j];sum[3] += vd[j]; } a = sum[0]/va.size()*1.0; b = sum[1]/va.size()*1.0; c = sum[2]/va.size()*1.0; d = sum[3]/va.size()*1.0; //std::cout<<"params: "<<a<<" "<<b<<" "<<c<<" "<<d<<std::endl; return true; } #endif -
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<MyFreenectDevice>(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<pcl::PointXYZRGBA>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGBA>); 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; }
