예제 #1
0
 //! Return a tuple containing the values of the iterators
 const  ElementPtrTuple dereference() const 
 {
     ElementPtrTuple ept( *(iterTuple_.template get<0>() ),
                          *(iterTuple_.template get<1>() ),
                          *(iterTuple_.template get<2>() ),
                          *(iterTuple_.template get<3>() ),
                          *(iterTuple_.template get<4>() ),
                          *(iterTuple_.template get<5>() ) );
     return ept;
 }
예제 #2
0
파일: lineslam.cpp 프로젝트: caomw/LineSLAM
void Node::detect3DLines(const cv::Mat& gray_uchar, const cv::Mat& depth_float, double line2d_len_thres, 
                      const cv::Mat& K,double ratio_of_collinear_pts, double line_3d_len_thres_m, double depth_scaling, string algorithm)
{
//	MyTimer tm; tm.start();	
	
	vector<FrameLine> allLines; // 2d and 3d lines
	if(algorithm == "LSD") { // using LSD, 100+ ms
		IplImage pImg = gray_uchar;
		ntuple_list  lsdOut;	
		lsdOut = callLsd(&pImg);// use LSD method
		int dim = lsdOut->dim;
		double a,b,c,d;
		allLines.reserve(lsdOut->size);
		for(int i=0; i<lsdOut->size; i++) {// store LSD output to lineSegments 
			a = lsdOut->values[i*dim];
			b = lsdOut->values[i*dim+1];
			c = lsdOut->values[i*dim+2];
			d = lsdOut->values[i*dim+3];
			if ( sqrt((a-c)*(a-c)+(b-d)*(b-d)) > line2d_len_thres) {
				allLines.push_back(FrameLine(cv::Point2d(a,b), cv::Point2d(c,d)));
			}
		}	
		
	}
    
	if(algorithm == "EDLINES") { // using EDlines, 15 ms
		int n;
		LS* ls = callEDLines(gray_uchar, &n);
		allLines.reserve(n);
		for(int i=0; i<n; i++) {// store output to lineSegments 
			if ((ls[i].sx-ls[i].ex)*(ls[i].sx-ls[i].ex) +(ls[i].sy-ls[i].ey)*(ls[i].sy-ls[i].ey) 
				> line2d_len_thres*line2d_len_thres) {
				allLines.push_back(FrameLine(cv::Point2d(ls[i].sx,ls[i].sy), cv::Point2d(ls[i].ex,ls[i].ey)));
			}
		}
	}

	Eigen::Matrix3d eK;
	for(int i=0; i<3; ++i)
		for(int j=0; j<3; ++j)
			eK(i,j) = K.at<double>(i,j);
	Eigen::Matrix3d Kinv = eK.inverse();	
 

 	
 	int depth_CVMatDepth = depth_float.depth();
  	#pragma omp parallel for
	for(int i=0; i<allLines.size(); ++i)	{ // 20 -30 ms
		double len = cv::norm(allLines[i].p - allLines[i].q);		
		// iterate through a line
		double numSmp = min(max(len/sysPara.line_sample_interval, (double)sysPara.line_sample_min_num), (double)sysPara.line_sample_max_num);  // smaller numSmp for fast speed, larger numSmp should be more accurate	
   		vector<cv::Point3d> pts3d; pts3d.reserve(numSmp);
		for(int j=0; j<=numSmp; ++j) {
			// use nearest neighbor to querry depth value
			// assuming position (0,0) is the top-left corner of image, then the
			// top-left pixel's center would be (0.5,0.5)
			cv::Point2d pt = allLines[i].p * (1-j/numSmp) + allLines[i].q * (j/numSmp);
			if(pt.x<0 || pt.y<0 || pt.x >= depth_float.cols || pt.y >= depth_float.rows ) continue;
			int row, col; // nearest pixel for pt
			if((floor(pt.x) == pt.x) && (floor(pt.y) == pt.y)) {// boundary issue
				col = max(int(pt.x-1),0);
				row = max(int(pt.y-1),0);
			} else {
				col = int(pt.x);
				row = int(pt.y);
			}
			double zval = -1;
			double depval;
			if(depth_CVMatDepth == CV_32F) 
				depval = depth_float.at<float>(row,col);
			else if (depth_CVMatDepth == CV_64F) 
				depval = depth_float.at<double>(row,col);
			else {
				cerr<<"Node::extractLineDepth: depth image matrix type is not float/double\n";
				exit(0);	
			}	
			if(depval < EPS || isnan((float)depval)) { // no depth info

			} else {
				zval = depval/depth_scaling; // in meter, z-value
			}

			if (zval > 0 ) {
				Eigen::Vector3d ept(pt.x, pt.y, 1);
				Eigen::Vector3d xy3d = Kinv * ept;
				xy3d = xy3d/xy3d(2);
				pts3d.push_back(cv::Point3d(xy3d(0)*zval, xy3d(1)*zval, zval));								
			}
		}
		if (pts3d.size() < max(10.0, numSmp *ratio_of_collinear_pts))
			continue;

		RandomLine3d tmpLine;		
		vector<RandomPoint3d> rndpts3d;
		rndpts3d.reserve(pts3d.size());
		// compute uncertainty of 3d points
		for(int j=0; j<pts3d.size();++j) {
			rndpts3d.push_back(compPt3dCov(pts3d[j], K, asynch_time_diff_sec_));
		}
		// using ransac to extract a 3d line from 3d pts
		tmpLine = extract3dline_mahdist(rndpts3d);
		
		if(tmpLine.pts.size()/numSmp > ratio_of_collinear_pts	&&
			cv::norm(tmpLine.A - tmpLine.B) > line_3d_len_thres_m) {
				allLines[i].haveDepth = true;
				allLines[i].line3d = tmpLine;
				
		}		
	}	

//// prepare for compute msld line descriptors
	cv::Mat xGradImg, yGradImg;
	int ddepth = CV_64F;	
	cv::Sobel(gray_uchar, xGradImg, ddepth, 1, 0, 5); // Gradient X
	cv::Sobel(gray_uchar, yGradImg, ddepth, 0, 1, 5); // Gradient Y
	double	*xG, *yG;
	if(xGradImg.isContinuous() && yGradImg.isContinuous()) {
		xG = (double*) xGradImg.data;
		yG = (double*) yGradImg.data;
	} else { 
		xG = new double[xGradImg.rows*xGradImg.cols];
 		yG = new double[yGradImg.rows*yGradImg.cols];
 		int idx = 0;
    	for(int i=0; i<xGradImg.rows; ++i) {
    		for(int j=0; j<xGradImg.cols; ++j) {
    			xG[idx] = xGradImg.at<double>(i,j);
    			yG[idx] = yGradImg.at<double>(i,j);
    			++idx;
    		}
    	}
	}

	lines.reserve(allLines.size());
	// NOTE this for-loop mustnot be parallized
	for(int i=0; i<allLines.size(); ++i) {
		if(allLines[i].haveDepth) { 
			lines.push_back(allLines[i]);
			lines.back().lid = lines.size()-1;
			lines.back().complineEq2d();				
			lines.back().r = lines.back().getGradient(&xGradImg, &yGradImg);
		}
	}
	

	#pragma omp parallel for
	for(int i=0; i<lines.size(); ++i) {		// 60 ms, 0.6ms/line
		computeMSLD(lines[i], xG, yG, gray_uchar.cols, gray_uchar.rows); // 0.1 ms/line
		MLEstimateLine3d(lines[i].line3d, sysPara.line3d_mle_iter_num);
		vector<RandomPoint3d> ().swap(lines[i].line3d.pts); // swap with a empty vector effectively free up the memory, clear() doesn't.
	}
//	tm.end(); cout<<"detect 3d lines "<<tm.time_ms<<" ms\n";

	if(!xGradImg.isContinuous() || !yGradImg.isContinuous()) { // dynamic allocation
		delete[] xG; 
		delete[] yG;
	}

}