Пример #1
0
void BackgroundSubtractorKNNImpl::getBackgroundImage(OutputArray backgroundImage) const
{
    int nchannels = CV_MAT_CN(frameType);
    //CV_Assert( nchannels == 3 );
    Mat meanBackground(frameSize, CV_8UC3, Scalar::all(0));

    int ndata=nchannels+1;
    int modelstep=(ndata * nN * 3);

    const uchar* pbgmodel=bgmodel.ptr(0);
    for(int row=0; row<meanBackground.rows; row++)
    {
        for(int col=0; col<meanBackground.cols; col++)
        {
            for (int n = 0; n < nN*3; n++)
            {
                const uchar* mean_m = &pbgmodel[n*ndata];
                if (mean_m[nchannels])
                {
                    meanBackground.at<Vec3b>(row, col) = Vec3b(mean_m);
                    break;
                }
            }
            pbgmodel=pbgmodel+modelstep;
        }
    }

    switch(CV_MAT_CN(frameType))
    {
        case 1:
        {
            std::vector<Mat> channels;
            split(meanBackground, channels);
            channels[0].copyTo(backgroundImage);
            break;
        }
        case 3:
        {
            meanBackground.copyTo(backgroundImage);
            break;
        }
        default:
            CV_Error(Error::StsUnsupportedFormat, "");
    }
}
Пример #2
0
int GateDetector::CountAndDrawWhitePixels(Mat& BWImage, Mat&outputImageToDraw, float top, float buttom, float left, float right)
{
	int counter = 0;

	for (size_t y = top; y < buttom; y++)
	{
		for (size_t x = left; x < right; x++)
		{
			if (BWImage.at<unsigned char>(y, x) > 100)
			{
				counter++;
				outputImageToDraw.at<Vec3b>(y, x) = Vec3b(255,0,0);
			}
		}
	}

	return counter;
}
Пример #3
0
void HumanBodyLaser::Disp2Point(const Mat& disp, const Point2f& stPt, const Mat& im, const Mat& msk, const Mat& Qmtx, vector<Point3f>& points, vector<Vec3b>& colors, vector<Vec3f>& normals)
{
	int height = disp.size().height;
	int width = disp.size().width;
	
	for(int y = 0; y < height; ++y)
	{
		uchar * pmsk = (uchar *)msk.ptr<uchar>(y);
		float * pdsp = (float *)disp.ptr<float>(y);
		for(int x = 0; x < width; ++x)
		{
			if(pmsk[x] == 0 || pdsp[x] == 0)
			{
				points.push_back(Point3f(PT_UNDEFINED, PT_UNDEFINED, PT_UNDEFINED));
				colors.push_back(Vec3b(0,0,0));
				normals.push_back(Vec3f(0,0,0));
			}
			else
			{				
				Mat tvec(4,1,CV_64FC1);
				tvec.at<double>(0,0) = stPt.x + x;
				tvec.at<double>(1,0) = stPt.y + y;
				tvec.at<double>(2,0) = pdsp[x];
				tvec.at<double>(3,0) = 1.0f;

				//cout << "Qmtx: " << Qmtx << endl;
				//cout << "tvec: " << tvec << endl;
				Mat hicoord = Qmtx * tvec;
				//cout << "hicoord: " << hicoord << endl;
				
				Point3f tpt;
				tpt.x = hicoord.at<double>(0,0) / hicoord.at<double>(3,0);
				tpt.y = hicoord.at<double>(1,0) / hicoord.at<double>(3,0);
				tpt.z = hicoord.at<double>(2,0) / hicoord.at<double>(3,0);

				Point3f tn = Point3f(0.f,0.f,0.f) - tpt;

				points.push_back(tpt);
				normals.push_back(Vec3f(tn.x, tn.y, tn.z));
				colors.push_back(im.at<Vec3b>(y,x));				
			}
		}
	}
}
Пример #4
0
void HumanBodyLaser::refine3DPoints(vector<Point3f>& points, vector<Vec3b>& colors, int& validnum)
{
	float maxdepth = MAXDEPTH;
	float mindepth = MINDEPTH;

	int size = points.size();
	for(int i = 0; i < size; ++i)
	{
		if(points[i].z == PT_UNDEFINED) continue;
		
		float z = points[i].z;
		if(z < mindepth || z > maxdepth)
		{
			points[i] = Point3f(PT_UNDEFINED, PT_UNDEFINED, PT_UNDEFINED);
			colors[i] = Vec3b(0,0,0);
			validnum --;
		}
	}
}
Пример #5
0
/* Draws the heatmap on top of a frame. The frame must be the same size as
 * the heatmap. 
 */
void AttentionMap::overlay(unsigned char* pDestImage, int imageWidth, int imageHeight)
{
	update();

	// Make sure all values are capped at one
	m_heatmap = min(m_ones, m_heatmap);

	Mat temp_map;
	blur(m_heatmap, temp_map, Size(15, 15));

	for (int r = 0; r < m_heatmap.rows; ++r)
	{
		//Vec3b* f_ptr = (Vec3b *)pDestImage;
		float* h_ptr = temp_map.ptr<float>(r);
		for (int c = 0; c < m_heatmap.cols; ++c)
		{
			const float heat_mix = h_ptr[c];
			if (heat_mix > 0.0)
			{
				// in BGR
				const Vec3b i_color = Vec3b(pDestImage[0], pDestImage[1], pDestImage[2]);

				const Vec3b heat_color = 
					hsv_to_bgr(interpolate_hsv(g_heat_color2, g_heat_color1, heat_mix));

				const float heat_mix2 = std::min(heat_mix, g_max_transparency);

				const Vec3b final_color = interpolate(i_color, heat_color, heat_mix2);
				
				//f_ptr[c] = final_color;
				pDestImage[0] = final_color[0];
				pDestImage[1] = final_color[1];
				pDestImage[2] = final_color[2];
			}

			pDestImage+=3;
		}

		pDestImage += (imageWidth - m_heatmap.cols) *3;
	}

	fade();
}
Пример #6
0
void Swt::displayStrokeWidth(vector< vector<cv::Point2i> >& cc)
{
	cv::Mat show = cv::Mat::zeros(height,width,CV_8UC3);
	cv::Vec3b  point = Vec3b(0,0,0);

	int ccsize = cc.size();
	for(int i = 0 ;i< ccsize;i++)
	{
		int ccisize = cc[i].size();
		for(int j = 0; j< ccisize;j++)
		{
			int iindex = cc[i][j].y,jindex = cc[i][j].x;
			if(StrokeImage.at<float>(iindex,jindex) == 151) 
				StrokeImage.at<float>(iindex,jindex) = 0; 
			point[1] = int(StrokeImage.at<float>(iindex,jindex)*5);
			show.at<cv::Vec3b>(iindex,jindex) = point;
		}
	}
	cv::imshow(" inner Stroke width",show);
	cv::waitKey(10);
}
Пример #7
0
void draw_circle(Mat& image, Point x,double alpha, bool flag, Vec2d scale)
{
	int thickness = -1;
	int lineType = 8;
	int radius = 10;
	Scalar color;
	if(flag)
		color = Scalar(0,0,255);
	else
		color = Scalar(0,255,0);

	Mat img(image.rows, image.cols, CV_8UC3, Scalar(0,0,0));
	circle(img, x * scale[0] * scale[1], radius * scale[1] , color, thickness, lineType);

	for(int i = 0; i < image.rows; i++)
	{
		for(int j = 0; j < image.cols; j++)
			if(img.at<Vec3b>(i,j) != Vec3b(0,0,0))
				image.at<Vec3b>(i,j) = (1- alpha) * image.at<Vec3b>(i,j) + alpha * img.at<Vec3b>(i,j);
	}
}
Пример #8
0
void TableObjectDetector::draw3DPointsIn2D(Mat img, const Mat P, KinectCalibParams* calib) {
    Mat Pt; P.copyTo(Pt); transpose(Pt, Pt);
    Mat K = calib->getRGBIntrinsics();
    Mat R; (calib->getR()).copyTo(R); transpose(R, R);
    Mat T = calib->getT();
    // Apply extrinsics
    Pt = R*Pt;
    for (int i=0; i<Pt.cols; i++) {
        Pt.col(i) = Pt.col(i)-T;
    }
    
    Mat Pim = K*Pt;
    for (int i=0; i<Pim.cols; i++) {
        double px = Pim.at<double>(0, i)/Pim.at<double>(2, i);
        double py = Pim.at<double>(1, i)/Pim.at<double>(2, i);
       
        img.at<Vec3b>(py, px) = Vec3b(0, 0, 255);
        
    }

}
Пример #9
0
Mat InputStream::getRawMat()
{

    int changedStreamDummy;
    VideoStream* pStream = &depth;
    rc_depth = OpenNI::waitForAnyStream(&pStream, 1, &changedStreamDummy, 1000);

    if (rc_depth != STATUS_OK)
    {
        printf("Wait failed! (timeout is %d ms)\n%s\n", 1000, OpenNI::getExtendedError());

        exit(0);
    }

    unsigned short data = 0;

    frame = new VideoFrameRef;
    rc_depth = depth.readFrame(frame);

    int j = 0;

    openni::DepthPixel* pDepth = (openni::DepthPixel*)frame->getData();
    delete frame;

    uint16_t max = 0;

    for (int i = 0; i < width * height; i++)
        if (raw[i] > max)
            max = raw[i];



    for (int y = 0; y < raw_mat.rows; y++)
        for (int x = 0; x < raw_mat.cols; x++)
        {
            raw_mat.at<Vec3b>(Point(x,y)) = Vec3b(0, 0, uchar((float)raw[y * raw_mat.cols + x] * 255.0 / (float)max));
        }

    return raw_mat;
}
void draw_ui(Mat& a, Mat& b, Mat& dst)
{
    Mat ra, rb;
    resize(a, ra, Size(4 * rows, 4 * cols));
    resize(b, rb, Size(4 * rows, 4 * cols));
    
    dst = Mat::zeros(ra.rows + 20, ra.cols * 2, CV_8UC3);
    
    Mat dst_roi_a = dst(Rect(0, 0, ra.cols, ra.rows));
    ra.copyTo(dst_roi_a);
    
    Mat dst_roi_b = dst(Rect(ra.cols, 0, ra.cols, ra.rows));
    rb.copyTo(dst_roi_b);
    
    if ( better != -1 ){
        for ( int r = 0; r < 20; r++ ){
            for ( int c = 0; c < 4 * cols; c++ ){
                dst.at<Vec3b>(ra.rows + r, 4 * cols * (1 - better) + c) =
                    Vec3b(255, 0, 0);
            }
        }
    }
}
  TEST(colorHistDetectorTestO, Operators)
  {
    // Create frames
    int framesCount = 4, rows = 30, cols = 40;
    vector <Frame*> frames;
    for (int i = 0; i < framesCount; i++)
    {
      Mat image = Mat(Size(cols, rows), CV_8UC3, Scalar(i, i, i));
      Frame * frame = new Keyframe();
      frames.push_back(frame);
      frames[i]->setID(i);
      frames[i]->setImage(image);
      frames[i]->setMask(image);
      image.release();
    }

    // Create empty skeleton
    Skeleton skeleton;
    tree<BodyPart> partTree;
    tree<BodyJoint> jointTree;
    skeleton.setPartTree(partTree);
    skeleton.setJointTree(jointTree);

    // Run "train" for setting frames in "chd"
    ColorHistDetector chd0;
    map <string, float> params;
    chd0.train(frames, params);
    frames.clear();

    // Using operator "="
    ColorHistDetector copyed_chd = chd0;

    // Get "chd" frames and compare
    vector <Frame*> actual_frames = copyed_chd.getFrames();
    for (int i = 0; i < frames.size(); i++)
      EXPECT_EQ(Vec3b(i, i, i), actual_frames[i]->getImage().at<Vec3b>(0, 0));
  }
Пример #12
0
Vec3b HSV2RGB(float hue, float sat, float val)
{
	float x, y, z;

	if(hue == 1) hue = 0;
	else         hue *= 6;

	int i = static_cast<int>(floorf(hue));
	float f = hue - i;
	float p = val * (1 - sat);
	float q = val * (1 - (sat * f));
	float t = val * (1 - (sat * (1 - f)));

	switch(i)
	{
		case 0: x = val; y = t; z = p; break;
		case 1: x = q; y = val; z = p; break;
		case 2: x = p; y = val; z = t; break;
		case 3: x = p; y = q; z = val; break;
		case 4: x = t; y = p; z = val; break;
		case 5: x = val; y = p; z = q; break;
	}
	return Vec3b((uchar)(x * 255), (uchar)(y * 255), (uchar)(z * 255));
}
Пример #13
0
void FillSimulation(vector <Mat> fillRegions, vector<Scalar> colorValue, vector<StrokeCluster> &fisrtDrawCluster){
	float size = 0;
	int gap = 15;
	int lineWidth = 10;
	
	//Filling regions
	for (int i = 0; i < fillRegions.size(); i++) {
		// Boundary initialization
		int ys = 1;
		int ye = fillRegions[i].rows - 1;
		int xs = 1;
		int xe = fillRegions[i].cols - 1;
		Mat fillRgionBlack;
		cvtColor(fillRegions[i], fillRgionBlack, CV_RGB2GRAY);
		fillRgionBlack = fillRgionBlack > 245;
		ofstream outputFile;
		string fileName = outputFileName("drawPoints/fill", i, ".txt");
		outputFile.open(fileName);
		Point previousPoint;
		Vec3b fillColor = Vec3b(colorValue[i][0], colorValue[i][1], colorValue[i][2]);
		Vec4f cmyk;
		rgb2cmyk(fillColor, cmyk);
		// Write indexing of color
		outputFile << (int)fillColor[0] << " " << (int)fillColor[1] << " " << (int)fillColor[2] << endl;
		outputFile << (float)cmyk[0] << " " << (float)cmyk[1] << " " << (float)cmyk[2] <<" " << (float)cmyk[3] << endl;

		// Find draw points
		for (int j = ys; j <= ye; j = j + gap)
			FindDrawPoints(j, xs, ys, xe, fillRgionBlack, outputFile, size, fillColor, cmyk, lineWidth, fisrtDrawCluster[i]);
		// Last Row
		for (int k = xs; k <= xe; k = k + gap)
			FindDrawPoints(ye, k, ys, xe, fillRgionBlack, outputFile, size, fillColor, cmyk, lineWidth, fisrtDrawCluster[i]);
		outputFile.close();
	}
	cout << "\nTotal Number of fill lines: " << fillLines << endl << endl;
}
Пример #14
0
void GraphMatching::depthTransfer(Mat& result, OneMatch& match, string depthDir){
	SubGraph targ = match.targ;
	SubGraph cadi = match.cadi;

	//targ's srcImg have the same size of result
	string section = match.cadi._centerNode._section;
	string depthPath = depthDir + "\\" + section + ".png";
	if(!fs::exists(depthPath)) depthPath = depthDir + "\\" + section + ".jpg";

	int inferredDepth = cadi._centerNode._depth;
	Mat depthMap = imread(depthPath,0);

	for(int i=0;i<cadi._centerNode._contour.size();i++){
			inferredDepth = depthMap.at<uchar>(cadi._centerNode._contour[i].x,cadi._centerNode._contour[i].y);
	}

	for(int i=0;i<targ._centerNode._contour.size();i++){
		if(result.channels() == 1)
			result.at<uchar>(targ._centerNode._contour[i].x,targ._centerNode._contour[i].y) = inferredDepth;
		else
			result.at<Vec3b>(targ._centerNode._contour[i].x,targ._centerNode._contour[i].y) = Vec3b(inferredDepth,inferredDepth,inferredDepth);
	}

}
Пример #15
0
// triangles = warped triangles
Mat warpImage(vector<vector<Point2f> > triangles, Mat srcIm, vector<Mat> affines)
{
    Mat warpedIm = srcIm.clone();
    //*
     for (int i = 0; i < warpedIm.rows; i++)
        for (int j = 0; j < warpedIm.cols; j++)
            warpedIm.at<Vec3b>(i,j) = Vec3b(0,0,0);
    //*/
    
    for (int i = 0; i < warpedIm.cols; i++)//
    {
        for (int j = 0; j < warpedIm.rows; j++)
        {
            // get warped triangle belonged to
            int idx = getAffineIndex(triangles, i,j);
            if (idx == -1)
            {
                //cout << "bad triangle index found" << endl;
                continue;
            }
            
            Mat curA = affines[idx];
            
            //*
            // affine mat: [ R     | T ] inv: [ R^-1  | -R^-1 T]
            //             [ 0 0 0 | 1 ] inv: [ 0 0 0 | 1      ]
            Mat R(2, 2, CV_64F); // NOT JUST ROTATION: rotation, scaling, shear
            R.at<double>(0,0) = curA.at<double>(0,0);
            R.at<double>(0,1) = curA.at<double>(0,1);
            R.at<double>(1,0) = curA.at<double>(1,0);
            R.at<double>(1,1) = curA.at<double>(1,1);
            Mat invR = R.inv(DECOMP_SVD);
            Mat invA(3, 3, CV_64F, 0.); // for (3,0-2)
            // R^-1
            invA.at<double>(0,0) = invR.at<double>(0,0);
            invA.at<double>(0,1) = invR.at<double>(0,1);
            invA.at<double>(1,0) = invR.at<double>(1,0);
            invA.at<double>(1,1) = invR.at<double>(1,1);
            // -R^-1 T
            invA.at<double>(0,2) = -invR.at<double>(0,0)* curA.at<double>(0,2) -invR.at<double>(0,1)* curA.at<double>(1,2);
            invA.at<double>(1,2) = -invR.at<double>(1,0)* curA.at<double>(0,2) -invR.at<double>(1,1)* curA.at<double>(1,2);
            // 1
            //invA.at<double>(2,2) = 1.;
            
            //*/
            //Mat invA = curA.inv(DECOMP_SVD);
            //
            double x(i), y(j);
            
            double srcJ = invA.at<double>(0,0)*x + invA.at<double>(0,1)*y + invA.at<double>(0,2);
            double srcI = invA.at<double>(1,0)*x + invA.at<double>(1,1)*y + invA.at<double>(1,2);
            
            if (srcI < 0 || srcJ < 0)
                warpedIm.at<Vec3b>(j, i) =Vec3b(0,0,0);//srcIm.at<Vec3b>(j,i);//i, j); // hack
            else
                warpedIm.at<Vec3b>(j, i) = srcIm.at<Vec3b>(srcI,srcJ);
        }
    }
    
    //drawAllTriangles(warpedIm.clone(), triangles, "post warp", false);
    //waitKey();
    return warpedIm;
}
Пример #16
0
void makeEigenfaces(vector<Mat> meanIms, Mat meanOfAll)
{
    // since it is a gray image, just need to consider one channel (since R = G = B for gray)
    
    // 1. Do PCA (principle component analysis) by computing eigenfaces:
    
    // - get difference of warped-to-mean individual images from mean image
    // - put into one long vector, getting M number of long vectors (M is number of images)
    // - Let A = (R*C)xM-matrix, where R*C is row * col size of the image
    // - compute covariance matrix C by A' * A (getting MxM-matrix), and calculate the eigenvectors and eigenvalues
    // - reverse eigenvectors from one long vector back to RxC-matrix, giving eigenface
    
    int numMeanIms = static_cast<int>(meanIms.size());
    
    cout << "numMeanIms: " << numMeanIms << endl;
    int R = meanIms[0].rows, C = meanIms[0].cols;
    int RC = R*C;
    
    Mat A(RC, numMeanIms, CV_32F, Scalar(0));
    // Cov = Covariance matrix
    // Cov = A'*A, where A is made up of the long vectors (not A*A' since that matrix will be too big)
    Mat Cov;
    
    int i = 0, j = 0, d = 0;
    for (int k = 0; k < numMeanIms; k++)
    {
        Mat cur = meanIms[k];
        Mat diff = meanOfAll - cur;
        stringstream ss;
        ss << k;
        string ns = ss.str();
        string name(ns+"_diff.jpg");
        imshow(name, diff);
        imwrite(name, diff);
        for (int r = 0; r < RC; r++)
        {
            i = r/C;
            j = r - i*C;
            d = diff.at<Vec3b>(i,j)[0];
            A.at<float>(r, k) = d;
        }
    }

    Mat At = A.t();
    
    cout << "A:r,c: " << A.rows << ", " << A.cols << endl;
    cout << "At:r,c: " << At.rows << ", " << At.cols << endl;
    
    Cov = At * A;
    //cout << Cov << endl;
    //waitKey();
    Mat eigenvalues, eigenvectors;
    eigen(Cov, eigenvalues, eigenvectors);
    
    // eigen() gives back the eigenvectors in this order: from largest eigenvalue to smallest eigenvalue
    // eigenvectors should be normalized, and in range -1 to 1
    cout << "EIGENVECTORS\n" << eigenvectors << "\nEIGENVALUES:\n" << eigenvalues << endl;
    //waitKey();
    
    // we have the eigenvectors of A'*A.
    // to get the eigenvectors of A*A', which are the eigenfaces,
    // A * eigenVector
    Mat allEigenfaces = A * eigenvectors.t(); // eigenvectors.t() for opencv's sake (eigenvectors in rows)
    
    Mat eigenBoss = allEigenfaces;
    
    // recover eigenfaces
    vector<Mat> eigenfaces;
    
    Mat blank = meanIms[0].clone();
    for (int i = 0; i < blank.rows; i++)
        for (int j = 0; j < blank.cols; j++)
            blank.at<Vec3b>(i,j) = Vec3b(0,0,0);
    
    for (int k = 0; k < numMeanIms; k++)
    {
        Mat cur = blank.clone();
        for (int r = 0; r < RC; r++)
        {
            i = r/C;
            j = r - i*C;
            int c = 0;
            float t = eigenBoss.at<float>(r,k);
            if (t != 0.)
                c = (t+1.)*255./2.;
            if (c > 255) c = 255; // clamp values
            if (c < 0)   c = 0;
            cur.at<Vec3b>(i, j) = Vec3b(c,c,c);
        }
        eigenfaces.push_back(cur);
    }
    
    // show and write all eigenfaces
    for (int k = 0; k < eigenfaces.size(); k++)
    {
        stringstream ss;
        ss << k;
        string ns = ss.str();
        string name(ns+"_ef.jpg");
        imshow(name, eigenfaces[k]);
        imwrite(name, eigenfaces[k]);
    }
    cout << "Finished making eigenfaces. Press ANY key to continue... " << endl;
    waitKey();

}
Пример #17
0
void getMeanIm(vector<vector<Mat> > &affinesToMean, vector<Mat> &meanIms, Mat &meanOfAll)
{
    // get meanTriangles
    // warp all images to meanTriangles mesh
    // mean image = average of warped-to-mean images
    
    // get meanTriangles
    //vector<vector<Mat> > affinesToMean;
    vector<vector<vector<Point2f> > > oriTriangles;
    //vector<Mat> meanIms;
    Mat meanIm;
    Mat im0 = forMeanIms[0].clone();
    Mat im1 = forMeanIms[1];
    vector<vector<Point2f> > triangles0;
    vector<vector<Point2f> > triangles1;
    vector<Point2f> facepoints0 = meanFacePoints[0];
    vector<Point2f> facepoints1 = meanFacePoints[1];
    string name0 = "0"; string name1 = "1";
    delaunay2Faces(im0, im1, triangles0, triangles1, facepoints0, facepoints1, name0, name1);
    
    oriTriangles.push_back(triangles0);
    oriTriangles.push_back(triangles1);
    
    vector<vector<vector<Point2f> > > XtoYTriangles;
    getAllTriangles(triangles0, triangles1, XtoYTriangles);
    
    vector<vector<Point2f> > meanTriangles = XtoYTriangles[XtoYTriangles.size()/2];
//    drawAllTriangles(im0.clone(), meanTriangles, "see triangles");
//    vector<Mat> curAffines = getAllAffineTransforms(triangles0 , meanTriangles);
//    meanIm = warpImage(meanTriangles, im0.clone(), curAffines);
//    imshow("MEAN", meanIm);
//    meanIms.push_back(meanIm);
//    waitKey();
    for (int k = 2; k < forMeanIms.size(); k++)
    {
        im0 = meanIm;
        im1 = forMeanIms[k];
        vector<vector<Point2f> > triangles1;
        vector<Point2f> facepoints1 = meanFacePoints[k];
        replicateDelaunay(triangles0, triangles1, facepoints0, facepoints1);
        
        Mat triangulated = im1.clone();
        stringstream ss;
        ss << k;
        string ns = ss.str();
        string name(ns+"delau.jpg");
        drawAllTriangles(triangulated, triangles1, "cur delaunay replicated");
        imwrite(name, triangulated);
        //waitKey();
        
        oriTriangles.push_back(triangles1);
        
        vector<vector<vector<Point2f> > > XtoYTriangles;
        getAllTriangles(meanTriangles, triangles1, XtoYTriangles);
        
        meanTriangles = XtoYTriangles[XtoYTriangles.size()/2];
        
    }
    // warp all images to mean mesh (meanTriangles)
    for (int k = 0; k < forMeanIms.size(); k++)
    {
        vector<vector<Point2f> > curTriangles = oriTriangles[k];
        vector<Mat> curAffines = getAllAffineTransforms(curTriangles , meanTriangles);
        meanIm = warpImage(meanTriangles, forMeanIms[k].clone(), curAffines);
        stringstream ss;
        ss << k;
        string ns = ss.str();
        string name(ns+"_warpedToMean.jpg");
        cout << "done getting mean from " << k << "..." << endl;
        imshow(name, meanIm);
        imwrite(name, meanIm);
        meanIms.push_back(meanIm);
    }
    
    //Mat meanOfAll = meanIms[0].clone();
    meanOfAll = meanIms[0].clone();
    
    int numMeans = static_cast<int>(meanIms.size());
    for (int i = 0; i < meanOfAll.rows; i++)
    {
        for (int j =0; j < meanOfAll.cols; j++)
        {
            int B = 0, G = 0, R = 0;
            for (int k = 0; k < numMeans; k++)
            {
                B += meanIms[k].at<Vec3b>(i,j)[0];
                G += meanIms[k].at<Vec3b>(i,j)[1];
                R += meanIms[k].at<Vec3b>(i,j)[2];
            }
            B /= numMeans;
            if (B > 255) B = 255; // clamp
            G /= numMeans;
            if (G > 255) G = 255;
            R /= numMeans;
            if (R > 255) R = 255;
            
            meanOfAll.at<Vec3b>(i,j) = Vec3b(B, G, R);
        }
    }
    imshow("MEAN of all MEANS", meanOfAll);
    imwrite("MEAN_img.jpg", meanOfAll);
    cout << "Finished getting mean. Press ANY key to continue..." << endl;
    waitKey();
}
Пример #18
0
///////////////////////////
// Author: Brian Fehrman
// Creates a mosaic given a p and p_prime image. Assumes P image is to the
// left and P_prime is to the right (this may not be a needed assumption)
// Assumes H matrix takes p to p_prime.
///////////////////////////
void stitcher::create_mosaic( Mat& dst_mat)
{
   double curr_tran_x = 0.0, curr_tran_y = 0.0;
   Mat scaled_mat;
   Mat H_matrix_inv = H_matrix.inv();
   double scale_val = 0.0;
   Vec3d extrema;
   Vec3d point_offsets;
   Mat temp_mat;
   
   find_extrema( extrema, point_offsets );
   
   temp_mat.create( (int) extrema[1], (int) extrema[0], CV_8UC3 );

   for( int curr_row = 0; curr_row < temp_mat.rows; curr_row++)
   {
      for( int curr_col = 0; curr_col < temp_mat.cols; curr_col++)
      {
         temp_mat.at<Vec3b>( curr_row, curr_col) = Vec3b( 0, 0, 0);
      }
   }

   for(double curr_x = 0; curr_x < p_prime_image.cols - 1; curr_x += 1 )
   {   
      for( double curr_y = 0; curr_y < p_prime_image.rows - 1; curr_y += 1 )
      {
         scale_val = curr_x * H_matrix_inv.at<double>(2,0)  + curr_y * H_matrix_inv.at<double>(2,1) + H_matrix_inv.at<double>(2,2);
         curr_tran_x = (curr_x * H_matrix_inv.at<double>(0,0) + curr_y * H_matrix_inv.at<double>(0,1) + H_matrix_inv.at<double>(0,2))/scale_val;
         curr_tran_y = (curr_x * H_matrix_inv.at<double>(1,0) + curr_y * H_matrix_inv.at<double>(1,1) + H_matrix_inv.at<double>(1,2))/scale_val;
         
         if( curr_tran_y > 0 && curr_tran_x > 0 && curr_tran_y < temp_mat.rows && curr_tran_x < temp_mat.cols)
         temp_mat.at<Vec3b>( curr_tran_y, curr_tran_x) = p_prime_image.at<Vec3b>( curr_y, curr_x );
   
      }
   }
   
   for(double curr_x = 0; curr_x < temp_mat.cols - 1; curr_x += 1 )
   {   
      for( double curr_y = 0; curr_y < temp_mat.rows - 1; curr_y += 1 )
      {
         scale_val = curr_x * H_matrix.at<double>(2,0)  + curr_y * H_matrix.at<double>(2,1) + H_matrix.at<double>(2,2);
         curr_tran_x = (curr_x * H_matrix.at<double>(0,0) + curr_y * H_matrix.at<double>(0,1) + H_matrix.at<double>(0,2))/scale_val;
         curr_tran_y = (curr_x * H_matrix.at<double>(1,0) + curr_y * H_matrix.at<double>(1,1) + H_matrix.at<double>(1,2))/scale_val;
         
         if( curr_tran_y > 0 && curr_tran_x > 0 && curr_tran_y < p_prime_image.rows && curr_tran_x < p_prime_image.cols)
            temp_mat.at<Vec3b>( curr_y, curr_x) = p_prime_image.at<Vec3b>( curr_tran_y, curr_tran_x );
   
      }
   }

   for(double curr_x = 0; curr_x < p_image.cols - 1; curr_x += 1 )
   {   
      for( double curr_y = 0; curr_y < p_image.rows - 1; curr_y += 1 )
      {
         temp_mat.at<Vec3b>( curr_y, curr_x) = p_image.at<Vec3b>( curr_y, curr_x );
      }
   }

   temp_mat.copyTo( dst_mat);
   
   imwrite(output_mosaic_name, temp_mat);
}
Пример #19
0
void ObjectTester::RunVideoDemo()
{
	GenericObjectDetector detector;

	KinectDataMan kinectDM;

	if( !kinectDM.InitKinect() )
		return;

	bool doRank = true;
	// start fetching stream data
	while(1)
	{
		Mat cimg, dmap;
		kinectDM.GetColorDepth(cimg, dmap);

		// resize image
		Size newsz;
		ToolFactory::compute_downsample_ratio(Size(cimg.cols, cimg.rows), 300, newsz);
		resize(cimg, cimg, newsz);
		//resize(dmap, dmap, newsz);
		//normalize(dmap, dmap, 0, 255, NORM_MINMAX);

		// get objects
		vector<ImgWin> objwins, salwins;
		if( !detector.ProposeObjects(cimg, dmap, objwins, salwins, doRank) )
			continue;

		//////////////////////////////////////////////////////////////////////////
		// draw best k windows
		int topK = MIN(6, objwins.size());
		int objimgsz = newsz.height / topK;
		int canvas_h = newsz.height;
		int canvas_w = newsz.width + 10 + objimgsz*2;
		Mat canvas(canvas_h, canvas_w, CV_8UC3);
		canvas.setTo(Vec3b(0,0,0));
		// get top windows
		vector<Mat> detimgs(topK);
		vector<Mat> salimgs(topK);
		for (int i=0; i<topK; i++)
		{
			cimg(objwins[i]).copyTo(detimgs[i]);
			resize(detimgs[i], detimgs[i], Size(objimgsz, objimgsz));
			cimg(salwins[i]).copyTo(salimgs[i]);
			resize(salimgs[i], salimgs[i], Size(objimgsz, objimgsz));
		}

		// draw boxes on input
		Scalar objcolor(0, 255, 0);
		Scalar salcolor(0, 0, 255);
		for(int i=0; i<topK; i++)
		{
			rectangle(cimg, objwins[i], objcolor);
			rectangle(cimg, salwins[i], salcolor);
		}
		circle(cimg, Point(cimg.cols/2, cimg.rows/2), 2, CV_RGB(255,0,0));
		// copy input image
		cimg.copyTo(canvas(Rect(0, 0, cimg.cols, cimg.rows)));

		// copy subimg
		for (int i=0; i<detimgs.size(); i++)
		{
			Rect objbox(cimg.cols+10, i*objimgsz, objimgsz, objimgsz);
			detimgs[i].copyTo(canvas(objbox));
			Rect salbox(cimg.cols+10+objimgsz, i*objimgsz, objimgsz, objimgsz);
			salimgs[i].copyTo(canvas(salbox));
		}

		resize(canvas, canvas, Size(canvas.cols*2, canvas.rows*2));
		//if(doRank)
			//putText(canvas, "Use Ranking", Point(50, 50), CV_FONT_HERSHEY_COMPLEX, 1, CV_RGB(255, 0, 0));
		imshow("object proposals", canvas);
		if( waitKey(10) == 'q' )
			break;
	}
}
void FaceMasker::Run(int min_depth, int min_pixels, int open_size,
                     int head_width, int head_height, int head_depth,
                     int face_size, int extended_size,
                     int window_size, int width, int height,
                     float focal_length, const Depth *depth,
                     Color *color) {
  width_ = width;
  height_ = height;
  window_size_ = window_size;

  if (size_ < (width_ * height_)) {
    size_ = width_ * height_;

    if (valid_mask_ != NULL)
      delete [] valid_mask_;
    if (head_mask_ != NULL)
      delete [] head_mask_;
    if (depth_integral_ != NULL)
      delete [] depth_integral_;
    if (valid_integral_ != NULL)
      delete [] valid_integral_;
    if (head_integral_ != NULL)
      delete [] head_integral_;
    if (min_sizes_ != NULL)
      delete [] min_sizes_;
    if (max_sizes_ != NULL)
      delete [] max_sizes_;

    valid_mask_ = new bool[size_];
    head_mask_ = new bool[size_];
    depth_integral_ = new int[size_];
    valid_integral_ = new int[size_];
    head_integral_ = new int[size_];
    min_sizes_ = new float[size_];
    max_sizes_ = new float[size_];
  }

  int max_depth = (head_width * focal_length) / min_pixels;

  #pragma omp parallel for
  for (int i = 0; i < (width * height); i++) {
    valid_mask_[i] = ((depth[i] > min_depth) && (depth[i] < max_depth)) ?
                     true : false;
  }

  Integral(width, height, true, valid_mask_, valid_integral_);
  Integral(width, height, valid_mask_, depth, depth_integral_);

  #pragma omp parallel for
  for (int y = 0; y < height; y++) {
    for (int x = 0; x < width; x++) {
      int i = x + y * width;

      head_mask_[i] = false;
      if (valid_mask_[i]) {
        int head_cols = (int)((head_width * focal_length) / depth[i]);
        int head_rows = (int)((head_height * focal_length) / depth[i]);

        int center_average = Mean(width, height,
            x - head_cols / 2, x + head_cols / 2,
            y - head_rows / 2, y + head_rows / 2,
            depth_integral_, valid_integral_);

        int left_average = Mean(width, height,
            x - (5 * head_cols / 4), x - (3 * head_cols / 4),
            y - head_rows / 2, y + head_rows / 2,
            depth_integral_, valid_integral_);

        int right_average = Mean(width, height,
            x + (3 * head_cols / 4), x + (5 * head_cols / 4),
            y - head_rows / 2, y + head_rows / 2,
            depth_integral_, valid_integral_);

        int top_average = Mean(width, height,
            x - head_cols / 2, x + head_cols / 2,
            y - (5 * head_rows / 4), y - (3 * head_rows / 4),
            depth_integral_, valid_integral_);

        int top_left_average = Mean(width, height,
            x - (5 * head_cols / 4), x - (3 * head_cols / 4),
            y - (5 * head_rows / 4), y - (3 * head_rows / 4),
            depth_integral_, valid_integral_);

        int top_right_average = Mean(width, height,
            x + (3 * head_cols / 4), x + (5 * head_cols / 4),
            y - (5 * head_rows / 4), y - (3 * head_rows / 4),
            depth_integral_, valid_integral_);

        int center_difference = ABS(depth[i] - center_average);
        int left_difference = ABS(depth[i] - left_average);
        int right_difference = ABS(depth[i] - right_average);
        int top_difference = ABS(depth[i] - top_average);
        int top_left_difference = ABS(depth[i] - top_left_average);
        int top_right_difference = ABS(depth[i] - top_right_average);

        int alpha = head_depth;
        int beta = 2 * head_depth;
        head_mask_[i] = ((center_difference < alpha) &&
                         (left_difference > beta) &&
                         (right_difference > beta) &&
                         (top_difference > beta) &&
                         (top_left_difference > beta) &&
                         (top_right_difference > beta)) ? true : false;
      }
    }
  }

  Integral(width, height, false, head_mask_, head_integral_);
  Erode(width, height, open_size, head_integral_, head_mask_);
  Integral(width, height, true, head_mask_, head_integral_);
  Dilate(width, height, open_size, head_integral_, head_mask_);

  Integral(width, height, true, head_mask_, head_integral_);

  #pragma omp parallel for
  for (int y = 0; y < height; y++) {
    for (int x = 0; x < width; x++) {
      int i = x + y * width;

      min_sizes_[i] = max_sizes_[i] = 0.0f;
      if (valid_mask_[i]) {
        int face_pixels = (int)((face_size * focal_length) / depth[i]);

        if (Sum(width, height, x - face_pixels / 2, x + face_pixels / 2,
                y - face_pixels / 2, y + face_pixels / 2, head_integral_) > 0) {
          int extended_pixels =(int)((extended_size * focal_length) / depth[i]);

          min_sizes_[i] = face_pixels - extended_pixels;
          max_sizes_[i] = face_pixels + extended_pixels;
        }
      }
    }
  }

  frame_++;
  scale_ = 0;

//#define HEAD_DEBUG
#ifdef HEAD_DEBUG
  Mat image(height_, width_, CV_8UC3, color);
  Mat head_region = Mat::zeros(height_, width_, CV_8UC3);

  #pragma omp parallel for
  for (int y = 0; y < height_; y++) {
    for (int x = 0; x < width_; x++) {
      int i = x + y * width_;

      if (head_mask_[i])
        head_region.at<Vec3b>(y, x) = Vec3b(255, 255, 255);
    }
  }

  char filename[256];
  sprintf(filename, "head-%d.png", frame_);

  Mat output;
  addWeighted(image, 0.5, head_region, 0.5, 0.0, output);

  imwrite(filename, output);
  scale_++;
#endif
}
void LzCalculator::calc(Mat &_LEFT, Mat &_RIGHT, Mat &_Ledge, Mat &_Redge)
{
    	is_valid = false;

	    Mat img_L,img_R;
		rctfndroi(_LEFT, _RIGHT, img_L ,img_R);

		Mat _Lamp, _Ramp;
		argument(_LEFT, _Lamp);
		argument(_RIGHT, _Ramp); 

		GaussianBlur(_Lamp, _Lamp, Size(5,5), 1.5, 1.5);
		GaussianBlur(_Ramp, _Ramp, Size(5,5), 1.5, 1.5);

		_Lem.markedge( _Lamp, _Ledge );
		_Rem.markedge( _Ramp, _Redge );

		Mat _LP,_RP,_LsubP,_RsubP;
		L_points.resize(_Ledge.rows,vector<double>(0));
		R_points.resize(_Redge.rows,vector<double>(0));
		record_L.resize(_Ledge.rows,0);
		record_R.resize(_Redge.rows,0);
		L_opt_ps.resize(_Ledge.rows,-1);
        R_opt_ps.resize(_Redge.rows,-1);
		calccenters(_Ledge, _LEFT, _LP, _LsubP,1);
		calccenters(_Redge, _RIGHT, _RP, _RsubP,2);
     	center_match(L_points,R_points,L_opt_ps,R_opt_ps);
		calc3Dpts();
		coordProject();
	    /*ofstream SaveFile("D:\\Output.txt");
		for(int i=0; i<_rect_Pnts3d.cols; i++)
	    {
			SaveFile<<"第"<<i<<"点坐标:"<<endl;
			SaveFile<<"高度: "<<*_rect_Pnts3d.ptr<float>(0,i)<<" 原点:"<<*_Pnts3d.ptr<float>(0,i)<<endl;
			SaveFile<<"水平: "<<*_rect_Pnts3d.ptr<float>(1,i)<<" 原点:"<<*_Pnts3d.ptr<float>(1,i)<<endl;
			SaveFile<<"Z坐标:"<<*_rect_Pnts3d.ptr<float>(2,i)<<" 原点:"<<*_Pnts3d.ptr<float>(2,i)<<endl;
			SaveFile<<endl;

		}
		SaveFile.close();*/
		for(int i=0;i<lft_pts.size();i++)
		{
			img_L.at<Vec3b>(int(lft_pts[i].y),int(lft_pts[i].x)) = Vec3b(0,255,0);
			//cout<<i<<endl;
		}
		for(int i=0;i<rgt_pts.size();i++)
		{
			img_R.at<Vec3b>(int(rgt_pts[i].y),int(rgt_pts[i].x)) = Vec3b(0,255,0);
			//cout<<i<<endl;
		}
		/*imwrite("C:\\Users\\Administrator\\Desktop\\标定数据_1127\\代码_TSH_1125\\兰州代码11_13\\Lz_Calc\\L1.bmp",img_L);
		imwrite("C:\\Users\\Administrator\\Desktop\\标定数据_1127\\代码_TSH_1125\\兰州代码11_13\\Lz_Calc\\R1.bmp",img_R);
        imwrite("L1.bmp",_LP);
		imwrite("R1.bmp",_RP);*/
		L_opt_ps.clear();
		R_opt_ps.clear();
		L_points.clear();
		R_points.clear();
		single_seed.clear();
		record_L.clear();
		record_R.clear();
	    lft_pts.clear();
	    rgt_pts.clear();

}
  //Testing function "Train"
  TEST(colorHistDetectorTest, Train)
  {
    //Load the input data
    vector<Frame*> frames = LoadTestProject("speltests_TestData/CHDTrainTestData/", "trijumpSD_50x41.xml");

    //Setting parameters 
    auto seq = new Sequence();
    map <string, float> params = SetParams(frames, &seq);
	
    for (auto f : frames)
      delete f;
    frames.clear();
    frames = seq->getFrames();
    
    //Counting a keyframes
    int FirstKeyframe = FirstKeyFrameNum(frames);
    int KeyframesCount = keyFramesCount(frames);

    //Copy image and skeleton from keyframe 
    Mat image = frames[FirstKeyframe]->getImage();
    Mat image1;
    image.copyTo(image1);
    Frame *frame = frames[FirstKeyframe];
    Skeleton skeleton = frame->getSkeleton();
    tree<BodyPart> PartTree = skeleton.getPartTree();

    //Build the rectangles for all of bodyparts
    map<int, POSERECT<Point2f>> Rects = SkeletonRects(skeleton);

    //Run "Train()"
    ColorHistDetector detector;
    detector.train(frames, params);

    //Calculate the polygons occlusion
    //Polygons layers:
    map<int, int> depth = { { 0, 2 }, { 1, 1 }, { 2, 3 }, { 3, 2 }, { 4, 4 }, { 5, 4 }, { 6, 1 }, { 7, 3 }, { 8, 2 }, { 9, 0 }, { 10, 4 }, { 11, 1 }, { 12, 3 }, { 13, 0 }, { 14, 4 }, { 15, 1 }, { 16, 3 } };
    //Polygons occlusion:
    vector<vector<pair<int, int>>> Crossings = CrossingsList(Rects, depth);

    //Calculate the parts histograms
    map <int32_t, ColorHistDetector::PartModel> partModels;
    for (int i = 0; i < Rects.size(); i++)
    {
      ColorHistDetector::PartModel Model(8);
      Model.sizeFG = 0;
      float xmin, ymin, xmax, ymax;
      Rects[i].GetMinMaxXY <float>(xmin, ymin, xmax, ymax);
      for (int x = xmin; x < xmax; x++)
      {
        for (int y = ymin; y < ymax; y++)
        {
          bool b = true;
          if (Rects[i].containsPoint(Point2f(x, y)) > 0)
          {
            int k = 0;

            while ((k < Crossings[i].size()) && b)
            {
              if (Rects[Crossings[i][k].first].containsPoint(Point2f(x, y)) > 0)
                b = false;
              k++;
            }
            if (b)
            {
              int c = 50 + i * 10;
              image1.at<Vec3b>(y, x) = Vec3b(c, c, c);
              Vec3b color = image.at<Vec3b>(y, x);
              Model.partHistogram[color[0] / Factor][color[1] / Factor][color[2] / Factor]++;
              Model.sizeFG++;
            }
          }
        }
      }
      partModels.emplace(pair<int32_t, ColorHistDetector::PartModel>(i, Model));
    }

    //Put results
    int nBins = detector.nBins;
    bool AllValuesEqual = true;
    int delta = 2; // tolerable linear error

    ofstream fout("TrainUnitTest_Output.txt");
    fout << "\n--------------------------Don't equal----------------------\n";
    cout << "\nTolerable error: " << delta << endl;
    fout << "Tolerable error: " << delta << endl;
    for (int i = 0; i < partModels.size(); i++)
    {
      for (int r = 0; r < nBins; r++)
        for (int g = 0; g < nBins; g++)
          for (int b = 0; b < nBins; b++)
          {
            int expected = int(partModels[i].partHistogram[b][g][r]);
            int actual = int(detector.partModels[i].partHistogram[r][g][b] * detector.partModels[i].sizeFG / KeyframesCount);
            if (abs(expected - actual) > delta)
            {
              cout << "Part[" << i << "]." << "Histogram[" << r << ", " << g << ", " << b << "]:    Expected = " << expected << ",   Actual = " << actual << endl;
              fout << "Part[" << i << "]." << "Histogram[" << r << ", " << g << ", " << b << "]:    Expected = " << expected << ",   Actual = " << actual << endl;
              if (!(r*g*b == 0)) AllValuesEqual = false;
            }
          }
    }
    if (AllValuesEqual) fout << "none";

    cout << "Output files: TrainUnitTest_Output.txt, UsedPixels.png\n\n";
    EXPECT_TRUE(AllValuesEqual);

    fout << "\n-----------Expected histogram-----------\n";
    fout << "In format:\nHistogramm[r, g, b] = pixelsCount\n";
    for (int i = 0; i < partModels.size(); i++)
    {
      fout << endl << "Rect[" << i << "]:" << endl;
      PutHistogram(fout, partModels[i].partHistogram, 1);
    }

    fout << "\n-----------Actual histogram-----------\n";
    fout << "In format:\nHistogramm[b, g, r] = Histogram[b, g, r]*Part.SizeFG/KeyframesCout\n";
    for (int i = 0; i < detector.partModels.size(); i++)
    {
      fout << endl << "Rect[" << i << "]:" << endl;
      PutHistogram(fout, detector.partModels[i].partHistogram, detector.partModels[i].sizeFG / KeyframesCount);
    }

    fout << "\n------------Occluded polygons-----------\nSorted by layer\n";
    for (int i = 0; i < Crossings.size(); i++)
    {
      fout << "\nPolygon[" << i << "] crossed by polygons: ";
      for (int k = 0; k < Crossings[i].size(); k++)
        fout << Crossings[i][k].first << "; ";
      Crossings[i].clear();
    }
    imwrite("UsedPixels.png", image1);

    fout.close();

    frames.clear();
    params.clear();
    Crossings.clear();
    partModels.clear();

    image.release();
    image1.release();

    delete seq;
  }
Пример #23
0
UINT ThreadProc(LPVOID pParam)
{
	HWND hwnd = AfxGetMainWnd()->GetSafeHwnd();
	LPTHREADDATA pData = (LPTHREADDATA)pParam;

	Mat dst_temp = Mat::zeros(pData->Roi_dst->size(), 16);
	//Vec3b NullPixel = Vec3b(255, 255, 255);
	float x_, y_;
	Point StartPoint;
	Point EndPoint;
	StartPoint = pData->Roi->tl();
	EndPoint = pData->Roi->br();
	int x_s, x_l, y_s, y_l;
	Vec3b* src_tl;
	Vec3b* dst_;
	float* xmap_;
	float* ymap_;
	unsigned char* mask_;
	int CamIdx;
	int i = 1;
	while (TRUE)
	{

		//MessageBox(NULL, _T("checkpoint1"),NULL , MB_OK);
		//WaitForSingleObject(g_event[pData->ID],INFINITE);
		g_Semaphore.Lock();
		//MessageBox(NULL, _T("checkpoint2"), NULL, MB_OK);

		for (int y = StartPoint.y; y < EndPoint.y; y++)
		{
			//Vec3b* dst_ = pData->Obj->m_dst.ptr<Vec3b>(y - StartPoint.y);
			dst_ = dst_temp.ptr<Vec3b>(y - StartPoint.y);
			xmap_ = pData->Obj->m_xmap.ptr<float>(y);
			ymap_ = pData->Obj->m_ymap.ptr<float>(y);
			mask_ = pData->Obj->m_mask.ptr<unsigned char>(y);

			for (int x = StartPoint.x; x < EndPoint.x; x++)
			{
				CamIdx = (int)mask_[x];
				if (CamIdx == 255)
				{
					//dst_[x] = NullPixel;
					dst_[x - StartPoint.x] = Vec3b(0, 0, 0);
				}
				else
				{
					x_ = xmap_[x];
					y_ = ymap_[x];

					x_s = (int)floor(x_);
					x_l = (int)ceil(x_);
					y_s = (int)floor(y_);
					y_l = (int)ceil(y_);
					//float n = y_ - y_s, m = x_ - x_s;
					if (x_s < 0 || y_s < 0 || x_l>(*pData->Images)[CamIdx].cols - 1 || y_l>(*pData->Images)[CamIdx].rows - 1)
					{
						//cout << "out of range" << endl;
						dst_[x - StartPoint.x] = Vec3b(0, 0, 0);
					}
					else
					{

						src_tl = (*pData->Images)[CamIdx].ptr<Vec3b>(y_s, x_s);
						dst_[x - StartPoint.x] = *src_tl;
					}
				}
			}
		}

		CCritical.Lock();
		pData->Obj->m_dst = pData->Obj->m_dst | dst_temp;
		//signal++;
		::PostMessage(hwnd, WM_LOOP, NULL, NULL);
		CCritical.Unlock();
		//MessageBox(NULL, _T("threadend"), NULL, NULL);
		//Sleep(10);

		//i--;



	}

	dst_temp.release();
	return 0;
};
Пример #24
0
/* compute the CSS image */
vector<int> ComputeCSSImageMaximas(const vector<double>& contourx_, const vector<double>& contoury_,
	vector<double>& contourx, vector<double>& contoury,
	bool isClosedCurve
	)
{
	ResampleCurve(contourx_, contoury_, contourx, contoury, 200, !isClosedCurve);
	vector<Point2d> pl; PolyLineMerge(pl, contourx, contoury);

	map<int, double> maximas;

	Mat_<Vec3b> img(500, 200, Vec3b(0, 0, 0)), contourimg(350, 350, Vec3b(0, 0, 0));
	bool done = false;
	//#pragma omp parallel for
	for (int i = 0; i<490; i++)
	{
		if (!done)
		{
			double sigma = 1.0 + ((double)i)*0.1;
			vector<double> kappa, smoothx, smoothy;
			ComputeCurveCSS(contourx, contoury, kappa, smoothx, smoothy, sigma);

			//			vector<vector<Point> > contours(1);
			//			PolyLineMerge(contours[0], smoothx, smoothy);
			//			contourimg = Vec3b(0,0,0);
			//			drawContours(contourimg, contours, 0, Scalar(255,255,255), CV_FILLED);

			vector<int> crossings = FindCSSInterestPointsZero(kappa);
			if (crossings.size() > 0)
			{
				for (int c = 0; c<crossings.size(); c++)
				{
					img(i, crossings[c]) = Vec3b(0, 255, 0);
					//					circle(contourimg, contours[0][crossings[c]], 5, Scalar(0,0,255), CV_FILLED);

					if (c < crossings.size() - 1) {
						if (fabs((float)crossings[c] - crossings[c + 1]) < 5.0)//fabs计算绝对值
						{
							//this is a maxima
							int idx = (crossings[c] + crossings[c + 1]) / 2;
							//#pragma omp critical
							maximas[idx] = (maximas[idx] < sigma) ? sigma : maximas[idx];

							circle(img, Point(idx, i), 3, Scalar(0, 0, 255), CV_FILLED);
						}
					}
				}
				//				char buf[128]; sprintf(buf, "evolution_%05d.png", i);
				//				imwrite(buf, contourimg);
				//				imshow("evolution", contourimg);
				//				waitKey(30);
			}
			else
			{
				done = true;
			}

		}
	}

	//find largest sigma
	double max_sigma = 0.0;
	for (map<int, double>::iterator itr = maximas.begin(); itr != maximas.end(); ++itr)
	{
		if (max_sigma < (*itr).second)
		{
			max_sigma = (*itr).second;
		}
	}
	//get segments with largest sigma
	vector<int> maximasv;
	for (map<int, double>::iterator itr = maximas.begin(); itr != maximas.end(); ++itr)
	{
		if ((*itr).second > max_sigma / 8.0)
		{
			maximasv.push_back((*itr).first);
		}
	}
	//eliminate degenerate segments (of very small length)
	vector<int> maximasvv = EliminateCloseMaximas(maximasv, maximas);	//1st pass
	maximasvv = EliminateCloseMaximas(maximasvv, maximas);				//2nd pass
	maximasv = maximasvv;
	for (vector<int>::iterator itr = maximasv.begin(); itr != maximasv.end(); ++itr) {
		cout << *itr << " - " << maximas[*itr] << endl;
	}
	//	Mat zoom; resize(img,zoom,Size(img.rows*2,img.cols*2));
	imshow("css image", img);
	//waitKey();
	return maximasv;
}
Пример #25
0
void testgen_lines2points_2d( string res_folder )
{
  //HCoords c(1024, 512);
  //cout << c.width << " " << c.height << " " << c.depth << endl;
  c.depth = c.width/2;
  for ( double num_clusters_f = 1; num_clusters_f < 10; num_clusters_f *= 1.2 )
  {
    Mat3b res(c.height, c.width, Vec3b(255, 255, 255) );
    // generate clusters  
    int num_clusters = int(num_clusters_f);
    vector< pair< Point3d, Point3d > > clusters; //кластер = 2 прямые 
    for (int i_cluster = 0; i_cluster < num_clusters; i_cluster++)
    {
      Point3d a1(rand() % c.width, rand() % c.height, c.depth);
      Point3d a2(rand() % c.width, rand() % c.height, c.depth);
      while (a1 == a2)
      {
        a2 = Point3d(rand() % c.width, rand() % c.height, c.depth);
      }
      Point3d b1(rand() % c.width, rand() % c.height, c.depth);
      Point3d b2(rand() % c.width, rand() % c.height, c.depth);
      while (b1 == b2)
      {
        b2 = Point3d(rand() % c.width, rand() % c.height, c.depth);
      }

      a1.x -= c.width / 2;
      a1.y -= c.height / 2;
      a2.x -= c.width / 2;
      a2.y -= c.height / 2;
      b1.x -= c.width / 2;
      b1.y -= c.height / 2;
      b2.x -= c.width / 2;
      b2.y -= c.height / 2;
      //cout << a1.x << " " << a1.y << " " << a1.z << endl;
      //cout << a2.x << " " << a2.y << " " << a2.z << endl;

      Point3d l1 = a1.cross(a2);
      Point3d l2 = b1.cross(b2);
      l1 = normalize(l1);
      l2 = normalize(l2);
      //cout << l1.x << " " << l1.y << " " << l1.z << endl;

      pair<Point3d, Point3d> center = make_pair(l1, l2);
      clusters.push_back(center);
        
      draw_Line(res, l1, 0);
      draw_Line(res, l2, 0);
    }

    ////////// generate points around clusters
    vector<Point3d> p;// все прямые в кластере
    for (int i_cluster = 0; i_cluster < num_clusters; i_cluster++)
    {
      Scalar color = Scalar(rand() % 256, rand() % 256, rand() % 256) ;
      for (int i1 = 0; i1 < countLines; ++i1)
      {
        double k = 1.0 * (rand() % 1000) / 1000.0 ;
        Point3d c = clusters[i_cluster].first * k + clusters[i_cluster].second * (1.0 - k);
        p.push_back(gen_point_on_sphere(c, sigma));// немного сдвинутая прямая
        //p.push_back(c);
        //cout << c.x << " " << c.y << " " << c.z << endl;
        //cout << p.back().x << " " << p.back().y << " " << p.back().z << endl;
        //cout << endl;
        draw_Line(res, p.back(), color);
      }
    }
    random_shuffle(p.begin(), p.end());

    string test_name = res_folder + format( "line%.03d", num_clusters );
  //  cerr << res_folder << endl;
    ofstream out((test_name + ".txt").c_str());
    out << c.width << " " << c.height << endl;
    out << num_clusters << endl;

    out.precision(6);
    for (int i=0; i<clusters.size(); i++)
    {
      //draw_Line(res, clusters[i].first);
      //draw_Line(res, clusters[i].second);
      out << fixed <<  clusters[i].first.x << "\t" <<  clusters[i].first.y << "\t" << clusters[i].first.z  << "\t" <<  clusters[i].second.x << "\t" << clusters[i].second.y << "\t" << clusters[i].second.z << "\t" << sigma << endl;
    }

    out << p.size() << endl;
    for (int i = 0; i < p.size(); ++i)
    {

      out << fixed << p[i].x << "\t" << p[i].y << "\t" << p[i].z << endl;
    }
    
    imwrite( test_name+".png", res );
   }  
}
void OpenniGrabber :: run()
{
    m_should_exit = false;
    m_current_image.setCalibration(m_calib_data);
    m_rgbd_image.setCalibration(m_calib_data);

    // Depth
    m_rgbd_image.rawDepthRef() = Mat1f(m_calib_data->raw_depth_size);
    m_rgbd_image.rawDepthRef() = 0.f;
    m_rgbd_image.depthRef() = m_rgbd_image.rawDepthRef();
    m_current_image.rawDepthRef() = Mat1f(m_calib_data->raw_depth_size);
    m_current_image.rawDepthRef() = 0.f;
    m_current_image.depthRef() = m_current_image.rawDepthRef();

    // Color
    if (m_has_rgb)
    {
        m_rgbd_image.rawRgbRef() = Mat3b(m_calib_data->rawRgbSize());
        m_rgbd_image.rawRgbRef() = Vec3b(0,0,0);
        m_rgbd_image.rgbRef() = m_rgbd_image.rawRgbRef();
        m_current_image.rawRgbRef() = Mat3b(m_calib_data->rawRgbSize());
        m_current_image.rawRgbRef() = Vec3b(0,0,0);
        m_current_image.rgbRef() = m_current_image.rawRgbRef();

        m_rgbd_image.rawIntensityRef() = Mat1f(m_calib_data->rawRgbSize());
        m_rgbd_image.rawIntensityRef() = 0.f;
        m_rgbd_image.intensityRef() = m_rgbd_image.rawIntensityRef();
        m_current_image.rawIntensityRef() = Mat1f(m_calib_data->rawRgbSize());
        m_current_image.rawIntensityRef() = 0.f;
        m_current_image.intensityRef() = m_current_image.rawIntensityRef();
    }

    // User tracking
    m_rgbd_image.userLabelsRef() = cv::Mat1b(m_calib_data->raw_depth_size);
    m_rgbd_image.userLabelsRef() = 0u;

    if (m_track_users)
        m_rgbd_image.setSkeletonData(new Skeleton());

    m_current_image.userLabelsRef() = cv::Mat1b(m_calib_data->raw_depth_size);
    m_current_image.userLabelsRef() = 0u;

    if (m_track_users)
        m_current_image.setSkeletonData(new Skeleton());

    if (m_has_rgb)
    {
        bool mapping_required = m_calib_data->rawRgbSize() != m_calib_data->raw_depth_size;
        if (!mapping_required)
        {
            m_rgbd_image.mappedRgbRef() = m_rgbd_image.rawRgbRef();
            m_rgbd_image.mappedDepthRef() = m_rgbd_image.rawDepthRef();
            m_current_image.mappedRgbRef() = m_current_image.rawRgbRef();
            m_current_image.mappedDepthRef() = m_current_image.rawDepthRef();
        }
        else
        {
            m_rgbd_image.mappedRgbRef() = Mat3b(m_calib_data->raw_depth_size);
            m_rgbd_image.mappedRgbRef() = Vec3b(0,0,0);
            m_rgbd_image.mappedDepthRef() = Mat1f(m_calib_data->rawRgbSize());
            m_rgbd_image.mappedDepthRef() = 0.f;
            m_current_image.mappedRgbRef() = Mat3b(m_calib_data->rawDepthSize());
            m_current_image.mappedRgbRef() = Vec3b(0,0,0);
            m_current_image.mappedDepthRef() = Mat1f(m_calib_data->rawRgbSize());
            m_current_image.mappedDepthRef() = 0.f;
        }
    }

    m_rgbd_image.setCameraSerial(cameraSerial());
    m_current_image.setCameraSerial(cameraSerial());

    xn::SceneMetaData sceneMD;
    xn::DepthMetaData depthMD;
    xn::ImageMetaData rgbMD;
    xn::IRMetaData irMD;

    ImageBayerGRBG bayer_decoder(ImageBayerGRBG::EdgeAware);

    RGBDImage oversampled_image;
    if (m_subsampling_factor != 1)
    {
        oversampled_image.rawDepthRef().create(m_calib_data->rawDepthSize()*m_subsampling_factor);
        oversampled_image.userLabelsRef().create(oversampled_image.rawDepth().size());
    }

    while (!m_should_exit)
    {
        waitForNewEvent();
        ntk_dbg(2) << format("[%x] running iteration", this);

        {
            // OpenNI calls do not seem to be thread safe.
            QMutexLocker ni_locker(&m_ni_mutex);
            waitAndUpdateActiveGenerators();
        }

        if (m_track_users && m_body_event_detector)
            m_body_event_detector->update();

        m_ni_depth_generator.GetMetaData(depthMD);
        if (m_has_rgb)
        {
            if (m_get_infrared)
            {
                m_ni_ir_generator.GetMetaData(irMD);
            }
            else
            {
                m_ni_rgb_generator.GetMetaData(rgbMD);
            }
        }

        RGBDImage& temp_image =
                m_subsampling_factor == 1 ? m_current_image : oversampled_image;

        const XnDepthPixel* pDepth = depthMD.Data();
        ntk_assert((depthMD.XRes() == temp_image.rawDepth().cols)
                   && (depthMD.YRes() == temp_image.rawDepth().rows),
                   "Invalid image size.");

        // Convert to meters.
        const float depth_correction_factor = 1.0;
        float* raw_depth_ptr = temp_image.rawDepthRef().ptr<float>();
        for (int i = 0; i < depthMD.XRes()*depthMD.YRes(); ++i)
            raw_depth_ptr[i] = depth_correction_factor * pDepth[i]/1000.f;

        if (m_has_rgb)
        {
            if (m_get_infrared)
            {
                const XnGrayscale16Pixel* pImage = irMD.Data();
                m_current_image.rawIntensityRef().create(irMD.YRes(), irMD.XRes());
                float* raw_img_ptr = m_current_image.rawIntensityRef().ptr<float>();
                for (int i = 0; i < irMD.XRes()*irMD.YRes(); ++i)
                {
                    raw_img_ptr[i] = pImage[i];
                }
            }
            else
            {
                if (m_custom_bayer_decoding)
                {
                    uchar* raw_rgb_ptr = m_current_image.rawRgbRef().ptr<uchar>();
                    bayer_decoder.fillRGB(rgbMD,
                                          m_current_image.rawRgb().cols, m_current_image.rawRgb().rows,
                                          raw_rgb_ptr);
                    cvtColor(m_current_image.rawRgbRef(), m_current_image.rawRgbRef(), CV_RGB2BGR);
                }
                else
                {
                    const XnUInt8* pImage = rgbMD.Data();
                    ntk_assert(rgbMD.PixelFormat() == XN_PIXEL_FORMAT_RGB24, "Invalid RGB format.");
                    uchar* raw_rgb_ptr = m_current_image.rawRgbRef().ptr<uchar>();
                    for (int i = 0; i < rgbMD.XRes()*rgbMD.YRes()*3; i += 3)
                        for (int k = 0; k < 3; ++k)
                        {
                            raw_rgb_ptr[i+k] = pImage[i+(2-k)];
                        }
                }
            }
        }

        if (m_track_users)
        {
            m_ni_user_generator.GetUserPixels(0, sceneMD);
            uchar* user_mask_ptr = temp_image.userLabelsRef().ptr<uchar>();
            const XnLabel* pLabel = sceneMD.Data();
            for (int i = 0; i < sceneMD.XRes()*sceneMD.YRes(); ++i)
            {
                user_mask_ptr[i] = pLabel[i];
            }

            XnUserID user_ids[15];
            XnUInt16 num_users = 15;
            m_ni_user_generator.GetUsers(user_ids, num_users);

            // FIXME: only one user supported.
            for (int i = 0; i < num_users; ++i)
            {
                XnUserID user_id = user_ids[i];
                if (m_ni_user_generator.GetSkeletonCap().IsTracking(user_id))
                {
                    m_current_image.skeletonRef()->computeJoints(user_id, m_ni_user_generator, m_ni_depth_generator);
                    break;
                }
            }
        }

        if (m_subsampling_factor != 1)
        {
            // Cannot use interpolation here, since this would
            // spread the invalid depth values.
            cv::resize(oversampled_image.rawDepth(),
                       m_current_image.rawDepthRef(),
                       m_current_image.rawDepth().size(),
                       0, 0, INTER_NEAREST);
            // we have to repeat this, since resize can change the pointer.
            // m_current_image.depthRef() = m_current_image.rawDepthRef();
            cv::resize(oversampled_image.userLabels(),
                       m_current_image.userLabelsRef(),
                       m_current_image.userLabels().size(),
                       0, 0, INTER_NEAREST);
        }

        m_current_image.setTimestamp(getCurrentTimestamp());

        {
            QWriteLocker locker(&m_lock);
            m_current_image.swap(m_rgbd_image);
        }

        advertiseNewFrame();
    }
    ntk_dbg(1) << format("[%x] finishing", this);
}
Пример #27
0
#include "StdAfx.h"
#include "CmShow.h"

const Vec3b CmShow::gColors[CmShow::COLOR_NUM] =
{
    Vec3b(0, 0, 255),	  Vec3b(0, 255, 0),		Vec3b(255, 0, 0),     Vec3b(153, 0, 48),	Vec3b(0, 183, 239),
    Vec3b(255, 255, 0),   Vec3b(255, 126, 0),   Vec3b(255, 194, 14),  Vec3b(168, 230, 29),
    Vec3b(237, 28, 36),   Vec3b(77, 109, 243),  Vec3b(47, 54, 153),   Vec3b(111, 49, 152),  Vec3b(156, 90, 60),
    Vec3b(255, 163, 177), Vec3b(229, 170, 122), Vec3b(245, 228, 156), Vec3b(255, 249, 189), Vec3b(211, 249, 188),
    Vec3b(157, 187, 97),  Vec3b(153, 217, 234), Vec3b(112, 154, 209), Vec3b(84, 109, 142),  Vec3b(181, 165, 213),
    Vec3b(40, 40, 40),	  Vec3b(70, 70, 70),	Vec3b(120, 120, 120), Vec3b(180, 180, 180), Vec3b(220, 220, 220)
};


const Vec3b CmShow::Black(0, 0, 0);
const Vec3b CmShow::Blue(255, 0, 0);
const Vec3b CmShow::Brown(42, 42, 165);
const Vec3b CmShow::Gray(128, 128, 128);
const Vec3b CmShow::Green(0, 255, 0);
const Vec3b CmShow::Orange(0, 165, 255);
const Vec3b CmShow::Pink(203, 192, 255);
const Vec3b CmShow::Purple(128, 0, 128);
const Vec3b CmShow::Red(0, 0, 255);
const Vec3b CmShow::White(255, 255, 255);
const Vec3b CmShow::Yellow(0, 255, 255);

const Scalar CmShow::BLACK = Black;
const Scalar CmShow::BLUE = Blue;
const Scalar CmShow::BROWN = Brown;
const Scalar CmShow::GRAY = Gray;
const Scalar CmShow::GREEN = Green;
void LaneDetection::AlgoFilterLanes_back(ntuple_list line_out){
	unsigned int dim = line_out->dim;
	int n_size = line_out->size;
	vector< vector<int> > pairs;//each element is a vector of index of segment(s)
	pairs.resize(n_size);

	for (int i = 0; i < n_size; i++)
	{
		const Point2d p1(line_out->values[i * dim + 0], line_out->values[i * dim + 1]);
		const Point2d p2(line_out->values[i * dim + 2], line_out->values[i * dim + 3]);
		double longeur_p12 = dist_p2p(p1, p2);
		Segment2d s(p1, p2);

		//if (line_out->values[i * dim + 0] > 662 && line_out->values[i * dim + 0] < 664)
		//{
		//	int c = 0;
		//}
		//else
		//	continue;

		Point2d milieu_p12 = (p1 + p2) / 2;
		for (int j = 0; j < n_size; j++)
		{
			if (j == i) continue;
			Point2d seg1(line_out->values[j * dim + 0], line_out->values[j * dim + 1]);
			Point2d seg2(line_out->values[j * dim + 2], line_out->values[j * dim + 3]);
			Segment2d seg(seg1, seg2);


			//simple check
			if (!s.isNeighbor(seg))
				continue;

			//not same side
			//if ((cross(p1, seg1, seg2) > -0.0 ? 1 : -1) * (cross(p2, seg1, seg2) > -0.0 ? 1 : -1) < 0)
			//{
			//	continue;
			//}
			
			//slope difference
			double slope_dif = abs(atan(slope_seg(p1, p2)) - atan(slope_seg(seg1, seg2)));
			if (slope_dif > 10 * CV_PI / 180)
			{
				continue;
			}

			double longeur_seg = dist_p2p(seg1, seg2);
			Point2d t_p1, t_p2, t_seg1, t_seg2;
			if (longeur_p12 > longeur_seg * 3)
			{
				if (foot_p2segment(seg1, p1, p2, t_p1) && foot_p2segment(seg2, p1, p2, t_p2))
				{
					t_seg1 = seg1;
					t_seg2 = seg2;
				}
				else
				{
					t_p1 = p1;
					t_p2 = p2;
					t_seg1 = seg1;
					t_seg2 = seg2;
				}
			}
			else if (longeur_seg > longeur_p12 * 3)
			{
				if (foot_p2segment(p1, seg1, seg2, t_seg1) && foot_p2segment(p2, seg1, seg2, t_seg2))
				{
					t_p1 = p1;
					t_p2 = p2;
				}
				else
				{
					t_p1 = p1;
					t_p2 = p2;
					t_seg1 = seg1;
					t_seg2 = seg2;
				}
			}
			else
			{
				t_p1 = p1;
				t_p2 = p2;
				t_seg1 = seg1;
				t_seg2 = seg2;
			}

			Point2d p_start = (t_p1 + t_p2) / 2;
			Point2d p_end = (t_seg1 + t_seg2) / 2;

			//distance
			double thresh = 20;
			if (p_start.y < 2 * processImage.rows / 3 && p_end.y < 2 * processImage.rows / 3)
				thresh = 10;
			if (dist_p2segment(t_p1, t_seg1, t_seg2) > thresh && dist_p2segment(t_p2, t_seg1, t_seg2) > thresh)
			{
				continue;
			}

			//color condition
			int mean_color[3] = { 0, 0, 0 };
			int num[3] = { 0, 0, 0 };
			
			Point2d translation[3];
			translation[0] = Point2d(0, 0);
			translation[1] = p_start - p_end;
			translation[2] = -translation[1];

			for (int _trans = 0; _trans < 3; _trans++)
			{
				Rect box = getBoundingBox(t_p1 + translation[_trans], t_p2 + translation[_trans],
					t_seg1 + translation[_trans], t_seg2 + translation[_trans]);

				Point2d milieu = (p_start + p_end) / 2 + translation[_trans];

				//check direction of cross.
				int direc = (cross(milieu, t_seg1 + translation[_trans], t_seg2 + translation[_trans]) > -EPS_HERE ? 1 : -1) *
					(cross(milieu, t_p1 + translation[_trans], t_p2 + translation[_trans]) > -EPS_HERE ? 1 : -1);
					

				for (int _y = box.y; _y < box.y + box.height; _y++)
				{
					if (_y >= processImage.rows || _y < 0) continue;
					uchar* ptr_row_processImage = ipmImage.ptr<uchar>(_y);
					for (int _x = box.x; _x < box.x + box.width; _x++)
					{
						if (_x >= processImage.cols || _x < 0) continue;
						Point2d p(_x, _y);
						if (direc != (cross(p, t_seg1 + translation[_trans], t_seg2 + translation[_trans]) > -EPS_HERE ? 1 : -1) *
							(cross(p, t_p1 + translation[_trans], t_p2 + translation[_trans]) > -EPS_HERE ? 1 : -1))
						{
							continue;
						}
						mean_color[_trans] += ptr_row_processImage[_x];
						num[_trans]++;
					}
				}
				mean_color[_trans] = (double)mean_color[_trans] / num[_trans];
			}

			bool color_matched = (mean_color[0] > mean_color[1] + 30) && (mean_color[0] > mean_color[2] + 30);

			if (!color_matched)
			{
				//line(processImage, p1, p2, Scalar(255, 0, 0));
				continue;
			}


			Rect box = getBoundingBox(t_p1, t_p2,
				t_seg1, t_seg2);

			Point2d milieu = (p_start + p_end) / 2;
			
			int _trans = 0;
			//check direction of cross.
			int direc = (cross(milieu, t_seg1 + translation[_trans], t_seg2 + translation[_trans]) > -EPS_HERE ? 1 : -1) *
				(cross(milieu, t_p1 + translation[_trans], t_p2 + translation[_trans]) > -EPS_HERE ? 1 : -1);


			for (int _y = box.y; _y < box.y + box.height; _y++)
			{
				if (_y >= processImage.rows || _y < 0) continue;
				Vec3b* ptr_row_processImage = processImage.ptr<Vec3b>(_y);
				for (int _x = box.x; _x < box.x + box.width; _x++)
				{
					if (_x >= processImage.cols || _x < 0) continue;
					Point2d p(_x, _y);
					if (direc != (cross(p, t_seg1 + translation[_trans], t_seg2 + translation[_trans]) > -EPS_HERE ? 1 : -1) *
						(cross(p, t_p1 + translation[_trans], t_p2 + translation[_trans]) > -EPS_HERE ? 1 : -1))
					{
						continue;
					}
					ptr_row_processImage[_x] = Vec3b(255, 0, 255);
				}
			}
			
			//TODO: ADD MORE CONDITIONS to check if they are a pair.
			//line(processImage, p1, p2, Scalar(0, 255, 0));
			//break;
			//add candidate pair to vector.
		}
	}

	ofstream fout("pairs.txt");
	for (int i = 0; i < n_size; i++)
	{
		const Point2d p1(line_out->values[i * dim + 0], line_out->values[i * dim + 1]);
		const Point2d p2(line_out->values[i * dim + 2], line_out->values[i * dim + 3]);

		fout << i << "{" << p1 << "; " << p2 << "}" << " pairs: ";
		int b = (unsigned)theRNG() & 255;
		int g = (unsigned)theRNG() & 255;
		int r = (unsigned)theRNG() & 255;

		if (pairs[i].size() <= 1)
			continue;

		for (int j = 0; j < pairs[i].size(); j++)
		{
			Point2d seg1(line_out->values[pairs[i][j] * dim + 0], line_out->values[pairs[i][j] * dim + 1]);
			Point2d seg2(line_out->values[pairs[i][j] * dim + 2], line_out->values[pairs[i][j] * dim + 3]);



			fout << "[" << seg1 << "; " << seg2 << " ],";
		}




		//line(processImage, p1, p2, Scalar(b, g, r));
		//line(processImage, seg1, seg2, Scalar(b, g, r));

		fout << "--------" << endl;
	}

}
Пример #29
0
bool find_square(Mat grayImage)
{
    if (grayImage.channels() != 1)
    {
        std::cout << "[find_square]: expected grayscale image, returning false" << std::endl;
        return false;
    }

    Mat adapt;
    adaptiveThreshold(
        grayImage,
        adapt,
        255,
        ADAPTIVE_THRESH_GAUSSIAN_C,
        THRESH_BINARY,
        7,
        10
    );

    int erosion_size = 1;
    int square_size  = erosion_size * 2 + 1;
    Mat element = getStructuringElement(
        MORPH_RECT,
        Size(square_size, square_size),
        Point(erosion_size, erosion_size)
    );

    erode(adapt, adapt, element);

    
    disjoint_set ds;
    Mat labels = Mat::zeros(grayImage.size(), CV_32SC1);

    clock_t t = clock();
    // do label based connected components with a
    // disjoint set data structure.
    int label = 0;
    for (int i = 0; i < adapt.rows; ++i)
    {
        for (int j = 0; j < adapt.cols; ++j)
        {

            uchar   cur_pixel = adapt.at<uchar>(i, j);
            int&    cur_label = labels.at<int>(i, j);
            int      up_label = (i == 0) ? -1 : labels.at< int >(i - 1, j    );
            int    left_label = (j == 0) ? -1 : labels.at< int >(i    , j - 1);
            uchar    up_pixel = (i == 0) ? -1 :  adapt.at<uchar>(i - 1, j    );
            uchar  left_pixel = (j == 0) ? -1 :  adapt.at<uchar>(i    , j - 1);

            bool same_left = left_pixel == cur_pixel;
            bool same_up   =   up_pixel == cur_pixel;

            if (same_left)
            {
                if (same_up)
                {
                    cur_label = min(up_label, left_label);
                    ds.join(up_label, left_label);
                }
                else
                {
                    cur_label = left_label;
                }
            }
            else if (same_up)
            {
                cur_label = up_label;
            }
            else
            {
                ++label;
                labels.at<uint>(i, j) = label;
                ds.add(label);
            }
        }
    }

    if (0)
    {
        RNG rng(0);
        map<int, Vec3b> colorMap;
        for (int i = 1; i <= label; ++i)
        {
            colorMap[i] = Vec3b(rng.uniform(0, 255), rng.uniform(0, 255), rng.uniform(0, 255));
        }

        Mat assignment = Mat::zeros(grayImage.size(), CV_8UC3);
        for (int i = 0; i < assignment.rows; ++i)
        {
            for (int j = 0; j < assignment.cols; ++j)
            {
                int label_value = ds.find(labels.at<int>(i, j));
                assignment.at<Vec3b>(i, j) = colorMap[label_value];
            }
        }

        imshow("Connections", assignment);
        waitKey();
    }

    t = clock() - t;
    cout << ((float) t) / CLOCKS_PER_SEC << endl;
    
    // imshow("Find", adapt);
    return true;
}
void Lz_RT_Clac::calc(Mat &_LEFT, Mat &_RIGHT, Mat &_Ledge, Mat &_Redge,int flag)        //判断相机(R相机标定反了)
{
       	is_valid = false;

	    Mat img_L,img_R;
		rctfndroi(_LEFT, _RIGHT, img_L ,img_R);

		Mat _Lamp, _Ramp;
		argument(_LEFT, _Lamp);
		argument(_RIGHT, _Ramp); 

		GaussianBlur(_Lamp, _Lamp, Size(5,5), 1.5, 1.5);
		GaussianBlur(_Ramp, _Ramp, Size(5,5), 1.5, 1.5);

		_Lem.markedge( _Lamp, _Ledge );
		_Rem.markedge( _Ramp, _Redge );

		Mat _LP,_RP,_LsubP,_RsubP;
		L_points.resize(_Ledge.rows,vector<double>(0));
		R_points.resize(_Redge.rows,vector<double>(0));
		record_L.resize(_Ledge.rows,0);
		record_R.resize(_Redge.rows,0);
		L_opt_ps.resize(_Ledge.rows,-1);
        R_opt_ps.resize(_Redge.rows,-1);
		calccenters(_Ledge, _LEFT, _LP, _LsubP,1);
		calccenters(_Redge, _RIGHT, _RP, _RsubP,2);
     	center_match(L_points,R_points,L_opt_ps,R_opt_ps);             //匹配左右轨点,种子填充算法,L_points为检测出的点,L_opt_ps为配准中心后的点
		//get_rail_ps(_LEFT,L_opt_ps,L_rail_ps);                         //从配准中心点中获得左轨点
		//get_rail_ps(_RIGHT,R_opt_ps,R_rail_ps);                        //从配准中心点中获得右轨点
		calc3Dpts();                                                   //如果当前左右轨点个数不为0,计算轨点三维坐标。
		coordProject(flag);                                                //相机坐标到世界坐标的转换
		rail_pnts_detect(flag);
        //get_rail(_rect_Pnts3d);                                        //获得当前相机的铁轨轨高和轨距。
/*	    ofstream SaveFile("C:\\Users\\Administrator\\Desktop\\Output.txt");
		for(int i=0; i<_rect_Pnts3d.cols; i++)
	    {
			SaveFile<<"第"<<i<<"点坐标:"<<endl;
			SaveFile<<"高度: "<<*_rect_Pnts3d.ptr<float>(0,i)<<" 原点:"<<*_Pnts3d.ptr<float>(0,i)<<endl;
			SaveFile<<"水平: "<<*_rect_Pnts3d.ptr<float>(1,i)<<" 原点:"<<*_Pnts3d.ptr<float>(1,i)<<endl;
			SaveFile<<"Z坐标:"<<*_rect_Pnts3d.ptr<float>(2,i)<<" 原点:"<<*_Pnts3d.ptr<float>(2,i)<<endl;
			SaveFile<<endl;

		}
		SaveFile.close();*/
		for(int i=0;i<lft_pts.size();i++)
		{
			if(lft_pts[i].x!=-1)
			   img_L.at<Vec3b>(int(lft_pts[i].y),int(lft_pts[i].x)) = Vec3b(0,255,0);
			//cout<<i<<endl;
		}
		for(int i=0;i<rgt_pts.size();i++)
		{
			if(rgt_pts[i].x!=-1)
			   img_R.at<Vec3b>(int(rgt_pts[i].y),int(rgt_pts[i].x)) = Vec3b(0,255,0);
			//cout<<i<<endl;
		}
		/*imwrite("C:\\Users\\Administrator\\Desktop\\标定数据_1127\\代码_TSH_1125\\兰州代码11_13\\Lz_Calc\\L1.bmp",img_L);
		imwrite("C:\\Users\\Administrator\\Desktop\\标定数据_1127\\代码_TSH_1125\\兰州代码11_13\\Lz_Calc\\R1.bmp",img_R);
        imwrite("L1.bmp",_LP);
		imwrite("R1.bmp",_RP);*/
 		L_opt_ps.clear();
		R_opt_ps.clear();
		L_points.clear();
		R_points.clear();
		single_seed.clear();
		record_L.clear();
		record_R.clear();
	    lft_pts.clear();
	    rgt_pts.clear();
		R_rail_ps.clear();
		L_rail_ps.clear();

}