谁看谁是我儿子

 

#include <CameraDriver/moCameraDriver.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h> 

#include <iostream>  
#include <string>
#include<algorithm>

#ifdef OPENCV_SHOW
#include <opencv2/opencv.hpp>
#endif

#define MO_CAMERA_IMAGE_WIDTH	(1280)
#define MO_CAMERA_IMAGE_HEIGHT	(720)


/* camera driver */
MO_CAM_DRIVER pCameraDriver;

/* camera parameters */
char nCamMode; //0: Camera
char* pCamPath;
int nStartFrmNo;
char nFlipMode;

/* camera output */
float fBF;
float fBase;
int nFrameNo,Dep1;
unsigned long long ullTime;


int nChannel, nChangeChn;
void *pGPSData, *pImuData;

unsigned long long  Dep=0;
unsigned char* pDspData;
unsigned char* pRefData;
unsigned char* pColorData;
double *pDepthData;

/* show image */
#ifdef OPENCV_SHOW
cv::Mat RefImg;
cv::Mat DepImg;
#endif

using namespace cv; 
using namespace std;

bool leftButtonDownFlag=false; //左键单击后的标志位 
bool leftButtonUpFlag=false; //左键单击后松开的标志位
Point originalPoint; //矩形框起点 
Point originalPoint2; //矩形框鼠标左键弹起来的终点 
Point processPoint; //矩形框移动的终点 

//根据视差值获取RGB色值
Vec3b depthToRGB(int iz) 
{

     Vec3b pixel;
     //黑色显示
     if(iz ==0){
	pixel[0] = 0; //Blue
	pixel[1] = 0; //Green
	pixel[2] = 0; //Red
	
      }//白色显示
      if(iz >=1 && iz <= 5){
      
	pixel[0] = 255; //Blue
	pixel[1] = 255; //Green
	pixel[2] = 255; //Red
	
      }//白到紫色 G 255到0
      if(iz >5 && iz <= 50){
	int iz_1 = iz-5;
	int iRGB0 = (256*iz_1)/45;
	if(iRGB0 <= 0){
	  iRGB0 = 1;
	}
	
	pixel[0] = 255; //Blue
	pixel[1] = 256-iRGB0; //Green
	pixel[2] = 255; //Red
	
      }//紫到蓝色 R 255到0
      if(iz >50 && iz <= 150){
	//int iRGB0 = (256*（iz-100）)/100;
	int iz_1 = iz-50;
	int iRGB0 = (256*iz_1)/100;
	if(iRGB0 <= 0){
	  iRGB0 = 1;
	}
	
	pixel[0] = 255; //Blue
	pixel[1] = 0; //Green
	pixel[2] = 256-iRGB0; //Red
	
      }//蓝到青色 G 0到255
      if(iz >150 && iz <= 300){
	//int iRGB0 = (256*（iz-200）)/100;
	int iz_1 = iz-150;
	int iRGB0 = (256*iz_1)/150;
	
	pixel[0] = 255; //Blue
	pixel[1] = iRGB0-1; //Green
	pixel[2] = 0; //Red
	
      }//青到绿色 B 255到0
      if(iz >300 && iz <= 500){
	//int iRGB0 = (256*（iz-300）)/100;
	int iz_1 = iz-300;
	int iRGB0 = (256*iz_1)/200;
	if(iRGB0 <= 0){
	  iRGB0 = 1;
	}
	pixel[0] = 256-iRGB0; //Blue
	pixel[1] = 255; //Green
	pixel[2] = 0; //Red
	
      }//绿到黄色 R 0到255
      if(iz >500 && iz <= 700){
	//int iRGB0 = (256*（iz-400）)/400;
	int iz_1 = iz-500;
	int iRGB0 = (256*iz_1)/200;
	
	pixel[0] = 0; //Blue
	pixel[1] = 255; //Green
	pixel[2] = iRGB0-1; //Red
	
      }//黄到红色 G 255到0
      if(iz >700 && iz <= 1500){
	//int iRGB0 = (256*（iz-800）)/700;
	int iz_1 = iz-700;
	int iRGB0 = (256*iz_1)/800;
	if(iRGB0 <= 0){
	  iRGB0 = 1;
	}
	
	pixel[0] = 0; //Blue
	pixel[1] = 256-iRGB0; //Green
	pixel[2] = 255; //Red
	
      }
      if(iz >1500){
	pixel[0] = 0; //Blue
	pixel[1] = 0; //Green
	pixel[2] = 255; //Red
	
      }
    
    return pixel;
}
//获取RGB色值
Vec3b depthToRGB2(int iRGB) 
{

    Vec3b pixel;
    if(iRGB >= 0 && iRGB <= 255){//白到紫色 G 255到0
	pixel[0] = 255; //Blue
	pixel[1] = 255-iRGB; //Green
	pixel[2] = 255; //Red
    }//紫到蓝色 R 255到0
    if(iRGB >255 && iRGB <= 511){
      pixel[0] = 255; //Blue
      pixel[1] = 0; //Green
      pixel[2] = 511-iRGB; //Red
      
    }//蓝到青色 G 0到255
    if(iRGB >511 && iRGB <= 767){
      pixel[0] = 255; //Blue
      pixel[1] = iRGB-512; //Green
      pixel[2] = 0; //Red
      
    }//青到绿色 B 255到0
    if(iRGB >767 && iRGB <= 1023){
      pixel[0] = 1023-iRGB; //Blue
      pixel[1] = 255; //Green
      pixel[2] = 0; //Red
      
    }//绿到黄色 R 0到255
    if(iRGB >1023 && iRGB <= 1279){
      pixel[0] = 0; //Blue
      pixel[1] = 255; //Green
      pixel[2] = iRGB-1024; //Red
      
    }//黄到红色 G 255到0
    if(iRGB >1279 && iRGB <= 1535){
      pixel[0] = 0; //Blue
      pixel[1] = 1535-iRGB; //Green
      pixel[2] = 255; //Red
      
    }
    if(iRGB >1500){
      pixel[0] = 0; //Blue
      pixel[1] = 0; //Green
      pixel[2] = 255; //Red
      
    }
    
    return pixel;
}
//鼠标回调函数    
void onMouse(int event,int x,int y,int flags,void *ustc)    
{       
  
    if(event==CV_EVENT_LBUTTONDOWN)    
    {    
        leftButtonDownFlag=true; //标志位  
        leftButtonUpFlag=false;
        originalPoint=Point(x,y);  //设置左键按下点的矩形起点  
        processPoint=originalPoint;  
    }    
    if(event == CV_EVENT_MOUSEMOVE && leftButtonDownFlag)    
    {         
        processPoint=Point(x,y);          
    }    
    if(event==CV_EVENT_LBUTTONUP)    
    {    
        leftButtonDownFlag=false; 
	leftButtonUpFlag=true;
	originalPoint2=Point(x,y);        
    }      
}

//三维中计算两点之间的距离
double TwoPointDistance3D(int ix1,int ix2,int iy1,int iy2,int iz1,int iz2) 
{
    double d1 = sqrt((ix1-ix2) * (ix1-ix2)+ (iy1 - iy2) * (iy1 - iy2)+ (iz1 - iz2) * (iz1 - iz2));
    return d1;
}

int main(int argc, char ** argv) {

    nCamMode = 0;
    pCamPath = (char*) "192.168.80.10";
    nStartFrmNo = 0;
    nFlipMode = 0;

    if (argc > 1) {
        nCamMode = atoi(argv[1]);
    }
    if (argc > 2) {
        pCamPath = argv[2];
    }
    if (argc > 3) {
        nStartFrmNo = atoi(argv[3]);
    }
    if (argc > 4) {
        nFlipMode = atoi(argv[4]);
    }

    pCameraDriver = moNewCameraDriver(nCamMode, pCamPath, nStartFrmNo, nFlipMode);

    float BFBase[2];
    moGetBFBase(pCameraDriver, BFBase);
    fBF = BFBase[0];
    fBase = BFBase[1];

#ifdef OPENCV_SHOW
    RefImg = cv::Mat::zeros(MO_CAMERA_IMAGE_HEIGHT, MO_CAMERA_IMAGE_WIDTH, CV_8UC3);  //一幅图
    DepImg = cv::Mat::zeros(MO_CAMERA_IMAGE_HEIGHT, MO_CAMERA_IMAGE_WIDTH, CV_8UC1);
#endif
    
    int IMG_W = 1280;
    int IMG_H = 720;
    Mat newimage(720,1280,CV_8UC3,Scalar(0,0,0));
   // Mat RGBimage(720,1280,CV_8UC3,Scalar(0,0,0));
      namedWindow("star1");
    setMouseCallback("star1",onMouse,0);  
    
    float WWX=0,WHX=0;
    int kk = 0,kb=0;;
    int xnob=0;
    int wx1,wx2,hy1,hy2,wx3,wx4,hy3,hy4,dz1,dz2,dz3,dz4,flag1;
    int ix2=640,iy2=360;
    string strFrameNo2[256],strFrameNo1;
    int IMGH=0,IMGW=0;
   // string ji=0;

    pDepthData = (double*)malloc(MO_CAMERA_IMAGE_WIDTH*MO_CAMERA_IMAGE_HEIGHT*sizeof(double));    
    while(1)
    {
        if(0 != moGetNextFrameData(pCameraDriver, &nFrameNo, &ullTime, &pGPSData, &pImuData, &pColorData, &pRefData, &pDspData))
            break;
      //  printf("Frame No:%d\n", nFrameNo);

   	//cout << "fBFs = " << endl << " " << fBF << endl;
	int iarray1[720][1280];
	float izd=0;
	int i1=0,iX5=0,iZ5=0;
	int x1=0,x2=0,y1=0,y2=0;
        double *pDepthDataHeader = pDepthData;
	memcpy(RefImg.data, pColorData, MO_CAMERA_IMAGE_WIDTH*MO_CAMERA_IMAGE_HEIGHT*3);    /* 8 bit grey image */
 	unsigned short *pDspDataHeader = (unsigned short*)pDspData;
        for(int i = 0; i < MO_CAMERA_IMAGE_WIDTH*MO_CAMERA_IMAGE_HEIGHT; i++)
        {
	    unsigned int iZ_1 = 0;
            if(*pDspDataHeader != 0){
   
		iZ_1 = *pDspDataHeader;
		//printf("Frame No:%d\n", *pDspDataHeader);
	    }
	    else iZ_1=0;
            pDspDataHeader++;
	    
	    int ix0 = 0;
	    int iy0 = 0;
	    if(i < 1280){
	      ix0 = i;
	      iy0 = 0;
	    }else{
	      iy0 = i/1280;
	      ix0 = i%1280;
	    }
	     newimage.at<Vec3b>(iy0,ix0) = depthToRGB(iZ_1);	
	     iarray1[iy0][ix0]=iZ_1;	
	    }
	    if(kb==2)
	    {
	    for(int i=0;i<256;i++)
	    {
	    izd=0;
	    i1=0;iX5=0;iZ5=0;
	    for(int y=0;y<45;y++)    //180
	    {
	      for(int x=0;x<80;x++)   //320
	      {
		if(iarray1[((i/16)*45)+y][((i%16)*80)+x]>0)
		{
		  izd +=iarray1[((i/16)*45)+y][((i%16)*80)+x];
		  i1++;	
		}
	      }
	      
	    }
	    iX5=izd/i1;
	    float pDepthDataHeader11 = 32*fBF/iX5; 
		    
	    ostringstream os;  
	    if (os << pDepthDataHeader11){
	      string str1 =  os.str(); 
	      iZ5 = atoi(str1.c_str());   
	    }
	    stringstream ss;
	    ss<<iZ5;
	    ss>>strFrameNo2[i];	
	     int ix=0,iy=30;

	 putText(RefImg,strFrameNo2[i] +"mm",Point(((i%16)*80+ix), ((i/16)*45)+iy),1,1,Scalar(0,255,255));	  
	 putText(newimage,strFrameNo2[i] +"mm",Point(((i%16)*80+ix), ((i/16)*45)+iy),1,1,Scalar(255,255,255));	
	}
	for(int i3=0;i3<16;i3++)
	{
	 rectangle( RefImg,Point( 0, i3*45),Point(1280, i3*45),Scalar( 255, 0, 255 ),0,8 );
	 rectangle( RefImg,Point( i3*80,0 ),Point(i3*80, 720),Scalar( 255, 0, 255 ),0,8 );
	 rectangle( newimage,Point( 0, i3*45),Point(1280, i3*45),Scalar( 255, 0, 255 ),0,8 );
	 rectangle( newimage,Point( i3*80,0 ),Point(i3*80, 720),Scalar( 255, 0, 255 ),0,8 );
	}
        }
	if(kb==3) 
	{
	    if(originalPoint!=originalPoint2) { rectangle(RefImg,originalPoint,processPoint,Scalar(255,0,255),0,8);
	    if(leftButtonUpFlag)
	    {
	      izd=0;
	      wx1=0;wx2=0;hy1=0;hy2=0;dz1=0;dz2=0;dz3=0;dz4=0;flag1=0;
	      wx3=0;wx4=0;hy3=0;hy4=0;
	      i1=0;iX5=0;iZ5=0;
	      if(originalPoint.x<processPoint.x) {x1=processPoint.x;x2=originalPoint.x;}
	      else {x2=processPoint.x;x1=originalPoint.x;}
	      if(originalPoint.y<processPoint.y) {y1=processPoint.y;y2=originalPoint.y;}
	      else {y2=processPoint.y;y1=originalPoint.y;}
	      for(int y=y2;y<y1;y++)    
	      {
		for(int x=x2;x<x1;x++)   
		{
		  if(iarray1[y][x]>0)
		  {
		    if(wx1==0) {wx1=x;hy1=y;dz1=iarray1[y][x];}
		    if(y==y2) {wx2=x;hy2=y;dz2=iarray1[y][x];}
		    if(y==(y1-1))
		      {
			wx4=x;
			hy4=y;
			dz4=iarray1[y][x];
			if(flag1==0) {wx3=x;hy3=y;flag1=1;dz3=iarray1[y][x];}
		      }
		    circle(RefImg, Point(x,y), 1,Scalar(0,0,126),-1);
		    izd +=iarray1[y][x];
		    i1++;	
		  }
		}      
	      }
	      wx1=32*fBase*(wx1-ix2)/dz1;
	      hy1=32*fBase*(hy1-iy2)/dz1;
	      wx2=32*fBase*(wx2-ix2)/dz2;
	      hy2=32*fBase*(hy2-iy2)/dz2;
	      
	      wx3=32*fBase*(wx3-ix2)/dz3;
	      hy3=32*fBase*(hy3-iy2)/dz3;
	      wx4=32*fBase*(wx4-ix2)/dz4;
	      hy4=32*fBase*(hy4-iy2)/dz4;	      
	      iX5=izd/i1;
	      float pDepthDataHeader11 = 32*fBF/iX5;    
	      ostringstream os;  
	      os << pDepthDataHeader11;
	      string str1 =  os.str(); 
	      iZ5 = atoi(str1.c_str());   
	      stringstream ss;
	      ss<<iZ5;
	      ss>>strFrameNo1;	
	      putText(RefImg,"Depth =: "+strFrameNo1 +"(mm)",Point(100,100),2,2,Scalar(255,0,255));
	      WWX=TwoPointDistance3D(wx1,wx2,hy1,hy2,dz1,dz2);
	      WHX=TwoPointDistance3D(wx1,wx3,hy1,hy3,dz1,dz3);
	      stringstream ss1;
	      ss1<<WWX;
	      ss1>>strFrameNo1;
	      putText(RefImg,"Width =: "+strFrameNo1 +"(mm)",Point(x1+5,y2),1,1,Scalar(255,0,255));
	      stringstream ss2;
	      ss2<<WHX;
	      ss2>>strFrameNo1;     
	      putText(RefImg,"Heigh =: "+strFrameNo1 +"(mm)",Point(x1+5,(y2+20)),1,1,Scalar(255,0,255));
	      
	      stringstream ss3;
	      ss3<<(WHX*WWX/1000000);
	      ss3>>strFrameNo1;     
	      putText(RefImg,"size =: "+strFrameNo1 +"(m²)",Point(x1+5,(y2+40)),1,1,Scalar(255,0,255));	      
	    }
	  }
	}
        int iRGB1 = 0;
	for (int i1=0;i1<720;i1++)
	{  
	    for (int j=1200;j<1280;j++)  
	    {  
		newimage.at<Vec3b>(i1,j) = depthToRGB2(iRGB1);
		//putText(newimage,"5000",Point(1170, i1),1,1,Scalar(255,255,255));	
	    }
	    iRGB1++;
	    iRGB1++;
	    iRGB1++;
	}

#ifdef OPENCV_SHOW
     //   cv::imshow("depth", DepImg);
	namedWindow("star");
	namedWindow("star1");
	//namedWindow("star2");

	imshow("star",newimage);
	imshow("star1", RefImg);
	//imshow("star2", DepImg);	
	char keynb=cvWaitKey(1);    //check keybord
	if(keynb=='2')  kb=2;
	if(keynb=='1')  kb=1;
	if(keynb=='3')  kb=3;
	if(keynb=='4')  kb=4;
	waitKey(1);
#endif
    }

    free(pDepthData);
    moDeleteCameraDriver(pCameraDriver);
}