void carTest()
{
comhelper.Open();
comhelper.Set(9600);
comhelper.Write(str, 2);
//cout << 1 << endl;
str[0] = 'L';
comhelper.Write(str, 2);
//mycar.run();
}
static int execute_set_tx_frequency_arm(char *szPort, UINT8 carrier_on, UINT16 tx_frequency, int tx_power)
{
ComHelper SerialPort;
if (!SerialPort.OpenPort(szPort))
{
printf("Open %s port Failed\n", szPort);
return 0;
}
int chan_num = tx_frequency - 2400;
UINT8 hci_set_tx_frequency_arm[] = {0x01, 0x014, 0xfc, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
UINT8 hci_set_tx_frequency_arm_cmd_complete_event[] = {0x04, 0x0e, 0x04, 0x01, 0x014, 0xfc, 0x00};
hci_set_tx_frequency_arm[4] = (carrier_on == 0) ? 1 : 0;
hci_set_tx_frequency_arm[5] = (carrier_on == 1) ? chan_num : 2;
hci_set_tx_frequency_arm[6] = 0; // unmodulated
hci_set_tx_frequency_arm[7] = 0; // modulation type
hci_set_tx_frequency_arm[8] = (carrier_on == 1) ? 8 : 0;
hci_set_tx_frequency_arm[9] = tx_power;
printf ("Sending HCI Command:\n");
HexDump(hci_set_tx_frequency_arm, sizeof(hci_set_tx_frequency_arm));
// write HCI reset
SerialPort.Write(hci_set_tx_frequency_arm, sizeof(hci_set_tx_frequency_arm));
// read HCI response header
DWORD dwRead = SerialPort.Read((LPBYTE)&in_buffer[0], 3);
// read HCI response payload
if (dwRead == 3 && in_buffer[2] > 0)
dwRead += SerialPort.Read((LPBYTE)&in_buffer[3], in_buffer[2]);
printf ("Received HCI Event:\n");
HexDump(in_buffer, dwRead);
if (dwRead == sizeof(hci_set_tx_frequency_arm_cmd_complete_event))
{
if (memcmp(in_buffer, hci_set_tx_frequency_arm_cmd_complete_event, dwRead) == 0)
{
printf ("Success\n");
return 1;
}
}
return FALSE;
}
static int execute_le_transmitter_test(char *szPort, UINT8 chan_number, UINT8 length, UINT8 pattern)
{
ComHelper SerialPort;
if (!SerialPort.OpenPort(szPort))
{
printf("Open %s port Failed\n", szPort);
return 0;
}
UINT8 hci_le_transmitter_test[] = {0x01, 0x01E, 0x20, 0x03, 0x00, 0x00, 0x00};
UINT8 hci_le_transmitter_test_cmd_complete_event[] = {0x04, 0x0e, 0x04, 0x01, 0x01E, 0x20, 0x00};
hci_le_transmitter_test[4] = chan_number;
hci_le_transmitter_test[5] = length;
hci_le_transmitter_test[6] = pattern;
printf ("Sending HCI Command:\n");
HexDump(hci_le_transmitter_test, sizeof(hci_le_transmitter_test));
// write HCI reset
SerialPort.Write(hci_le_transmitter_test, sizeof(hci_le_transmitter_test));
// read HCI response header
DWORD dwRead = SerialPort.Read((LPBYTE)&in_buffer[0], 3);
// read HCI response payload
if (dwRead == 3 && in_buffer[2] > 0)
dwRead += SerialPort.Read((LPBYTE)&in_buffer[3], in_buffer[2]);
printf ("Received HCI Event:\n");
HexDump(in_buffer, dwRead);
if (dwRead == sizeof(hci_le_transmitter_test_cmd_complete_event))
{
if (memcmp(in_buffer, hci_le_transmitter_test_cmd_complete_event, dwRead) == 0)
{
printf("Success\n");
printf("LE Transmitter Test running, to stop execute le_test_end\n");
return 1;
}
}
return FALSE;
}
static int execute_le_test_end(char *szPort)
{
ComHelper SerialPort;
if (!SerialPort.OpenPort(szPort))
{
printf("Open %s port Failed\n", szPort);
return 0;
}
UINT8 hci_le_test_end[] = {0x01, 0x1f, 0x20, 0x00};
UINT8 hci_le_test_end_cmd_complete_event[] = {0x04, 0x0e, 0x06, 0x01, 0x1f, 0x20, 0x00};
printf ("Sending HCI Command:\n");
HexDump(hci_le_test_end, sizeof(hci_le_test_end));
// write HCI reset
SerialPort.Write(hci_le_test_end, sizeof(hci_le_test_end));
// read HCI response header
DWORD dwRead = SerialPort.Read((LPBYTE)&in_buffer[0], 3);
// read HCI response payload
if (dwRead == 3 && in_buffer[2] > 0)
dwRead += SerialPort.Read((LPBYTE)&in_buffer[3], in_buffer[2]);
printf ("Received HCI Event:\n");
HexDump(in_buffer, dwRead);
if ((dwRead > sizeof(hci_le_test_end_cmd_complete_event))
&& (memcmp(in_buffer, hci_le_test_end_cmd_complete_event, sizeof(hci_le_test_end_cmd_complete_event)) == 0))
{
printf("Success num_packets_received %d\n", in_buffer[7] + (in_buffer[8] << 8));
return TRUE;
}
else
{
return FALSE;
}
}
static int execute_reset(char *szPort)
{
ComHelper SerialPort;
if (!SerialPort.OpenPort(szPort))
{
printf("Open %s port Failed\n", szPort);
return 0;
}
UINT8 hci_reset[] = {0x01, 0x03, 0x0c, 0x00};
UINT8 hci_reset_cmd_complete_event[] = {0x04, 0x0e, 0x04, 0x01, 0x03, 0x0c, 0x00};
printf ("Sending HCI Command:\n");
HexDump(hci_reset, sizeof(hci_reset));
// write HCI reset
SerialPort.Write(hci_reset, sizeof(hci_reset));
// read HCI response header
DWORD dwRead = SerialPort.Read((LPBYTE)&in_buffer[0], 3);
// read HCI response payload
if (dwRead == 3 && in_buffer[2] > 0)
dwRead += SerialPort.Read((LPBYTE)&in_buffer[3], in_buffer[2]);
printf ("Received HCI Event:\n");
HexDump(in_buffer, dwRead);
if (dwRead == sizeof(hci_reset_cmd_complete_event))
{
if (memcmp(in_buffer, hci_reset_cmd_complete_event, dwRead) == 0)
{
printf ("Success\n");
return 1;
}
}
return 0;
}
int main()
{
//carTest();
//str[0] = 'A';
//if (comhelper.Write(str, 2) == false)
//{
//cout << "error" << endl;
//}
//str[0] = 'R';
comhelper.Open();
comhelper.Set(9600);
comhelper.Write(str, 2);
vis[0] = 1;
//mycar.run();
VideoCapture cap; //定义一个摄像头捕捉的类对象
Rect trackwindow1; //第一个标志
Rect trackwindow2; //第二个标志
int hsize = 16;
float hranges[] = { 0, 180 };//hranges在后面的计算直方图函数中要用到
const float* phranges1 = hranges;
const float* phranges2 = hranges;
Mat frame, hsv, hue1, hue2, mask1, mask2, hist1, hist2, histimg1 = Mat::zeros(200, 320, CV_8UC3), histimg2 = Mat::zeros(200, 320, CV_8UC3), backproj1, backproj2;
cap.open(0);//直接调用成员函数打开摄像头
if (!cap.isOpened())
{
cout << "Error" << endl;
return -1;
}
//namedWindow("hist1ogram", 0);
namedWindow("CamShift Demo", 0);
namedWindow("Rotated", 0);
//透视变换
while (1)
{
//cout << "test" << endl;
cap >> frame;
if (frame.empty())
{
break;
}
if (type == 0)
{
change(frame);
}
if (type == 1)
{
break;
}
char c = (char)waitKey(30);
if (c == 'p') //按下p表示开始惊醒透视变换选点，选点方向左上右上左下右下
{
type = 0;
}
imshow("CamShift Demo", frame);
}
imshow("Rotated", rotated);
//二值化
src = &IplImage(rotated);
gray = cvCreateImage(cvGetSize(src), IPL_DEPTH_8U, 1);
cvCvtColor(src, gray, CV_BGR2GRAY);
binary = cvCreateImage(cvGetSize(gray), IPL_DEPTH_8U, 1);
cvThreshold(gray, binary, 128, 1, CV_THRESH_BINARY);
cvNamedWindow("Binary Image", 0);
cvShowImage("Binary Image", binary);
int n = 0;
cvCreateTrackbar("二值化阈值", "Binary Image", &n, 254, call_back);
call_back(1);
cvWaitKey(0);
//细化
dst = cvCreateImage(cvGetSize(binary), binary->depth, binary->nChannels);
thinImage(binary, dst);
cvNamedWindow("细化", 0);
cvShowImage("细化", dst);
cvWaitKey(0);
//霍夫变换
cvNamedWindow("霍夫变换", 0);
storage = cvCreateMemStorage();
lines = cvHoughLines2(dst, storage, CV_HOUGH_PROBABILISTIC, 1, CV_PI / 180, 30, 30, 40);
img1 = cvCreateImage(cvGetSize(dst), 8, 1);
cvSetZero(img1);
for (int i = 0; i < lines->total; i++)
{
CvPoint* line = (CvPoint*)cvGetSeqElem(lines, i);
cvLine(img1, line[0], line[1], CvScalar(255));
sp.add(line[0], line[1],i);
//sp.initial();
}
cvShowImage("霍夫变换", img1);
int Thresh = 20;
int MinLength = 40;
int Terval = 40;
cvCreateTrackbar("阈值", "霍夫变换", &Thresh, 100, hough_call_back0);
cvCreateTrackbar("最短线段长度", "霍夫变换", &MinLength, 100, hough_call_back1);
cvCreateTrackbar("线段间隔最大值", "霍夫变换", &Terval, 100, hough_call_back2);
cvWaitKey(0);
cvDestroyWindow("Binary Image");
cvDestroyWindow("细化");
cvReleaseImage(&gray);
cvReleaseImageHeader(&binary);
//cvReleaseImage(&dst);
sp.initial();
sp.test();
cout << "Please select the start point: ";
cin >> startPoint;
if (startPoint == 1)
{
sp.reverse();
}
//追踪
setMouseCallback("Rotated", onMouse, 0);//消息响应机制
createTrackbar("Vmin", "Rotated", &vmin, 256, 0);//createTrackbar函数的功能是在对应的窗口创建滑动条，滑动条Vmin,vmin表示滑动条的值，最大为256
createTrackbar("Vmax", "Rotated", &vmax, 256, 0);//最后一个参数为0代表没有调用滑动拖动的响应函数
createTrackbar("Smin", "Rotated", &smin, 256, 0);//vmin,vmax,smin初始值分别为10,256,30
//bool paused = false;
while (1)
{
//carTest();
if (type == 10 && mark2 == true)
{
Turn t = getTurn();
if (t == Left)
{
str[0] = 'L';
comhelper.Write(str, 2);
//mycar.turnl();
cout << "Left" << endl;
}
else if (t == Right)
{
str[0] = 'R';
comhelper.Write(str, 2);
//mycar.turnr();
cout << "Right" << endl;
}
else if (t == Ahead)
{
str[0] = 'A';
comhelper.Write(str, 2);
//mycar.run();
cout << "Ahead" << endl;
}
else if (t == Stop)
{
for (int i = 0; i < 100; i++)
{
str[0] = 'S';
comhelper.Write(str, 2);
//mycar.stop();
cout << "Stop" << endl;
}
}
}
cap >> frame;//从摄像头抓取一帧图像并输出到frame中
if (frame.empty())
break;
warpPerspective(frame, image, warpMatrix, rotated.size(), INTER_LINEAR, BORDER_CONSTANT);
//frame.copyTo(image);
frame.copyTo(image2);
cvtColor(image, hsv, CV_BGR2HSV);//将rgb摄像头帧转化成hsv空间的
if (trackObject)//trackObject初始化为0,或者按完键盘的'c'键后也为0，当鼠标单击松开后为-1
{
int _vmin = vmin, _vmax = vmax;
inRange(hsv, Scalar(0, smin, MIN(_vmin, _vmax)),
Scalar(180, 256, MAX(_vmin, _vmax)), mask1);
int ch[] = { 0, 0 };
hue1.create(hsv.size(), hsv.depth());
hue2.create(hsv.size(), hsv.depth());
mixChannels(&hsv, 1, &hue1, 1, ch, 1);
mixChannels(&hsv, 1, &hue2, 1, ch, 1);
if (trackObject < 0)//鼠标选择区域松开后，该函数内部又将其赋值1
{
//ROI指感兴趣的区域
Mat roi(hue1, selection1), maskroi(mask1, selection1);//mask1保存的hsv的最小值
calcHist(&roi, 1, 0, maskroi, hist1, 1, &hsize, &phranges1);//将roi的0通道计算直方图并通过mask1放入hist1中，hsize为每一维直方图的大小
normalize(hist1, hist1, 0, 255, CV_MINMAX);//将hist1矩阵进行数组范围归一化，都归一化到0~255
trackwindow1 = selection1;
trackObject = 1;//只要鼠标选完区域松开后，且没有按键盘清0键'c'，则trackObject一直保持为1，因此该if函数只能执行一次，除非重新选择跟踪区域
histimg1 = Scalar::all(0);//清0
int binW = histimg1.cols / hsize;
Mat buf(1, hsize, CV_8UC3);
for (int i = 0; i < hsize; i++)
buf.at<Vec3b>(i) = Vec3b(saturate_cast<uchar>(i*180. / hsize), 255, 255);
cvtColor(buf, buf, CV_HSV2BGR);//将hsv转换成bgr
for (int i = 0; i < hsize; i++)
{
int val = saturate_cast<int>(hist1.at<float>(i)*histimg1.rows / 255);
rectangle(histimg1, Point(i*binW, histimg1.rows), Point((i + 1)*binW, histimg1.rows - val), Scalar(buf.at<Vec3b>(i)), -1, 8);
}
}
calcBackProject(&hue1, 1, 0, hist1, backproj1, &phranges1);//计算直方图的反向投影，计算hue1图像0通道直方图hist1的反向投影，并赋值到backproj1中
backproj1 &= mask1;
trackBox1 = CamShift(backproj1, trackwindow1, TermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 10, 1));
if (trackwindow1.area() <= 1)
{
int cols = backproj1.cols, rows = backproj1.rows, r = (MIN(cols, rows) + 5) / 6;
trackwindow1 = Rect(trackwindow1.x - r, trackwindow1.y - r, trackwindow1.x + r, trackwindow1.y + r) &Rect(0, 0, cols, rows);//Rect函数为矩阵的偏移和大小，即第一二个参数为矩阵的左上角点坐标，第三四个参数为矩阵的宽和高
}
if (backproj1Mode)
{
cvtColor(backproj1, image, CV_GRAY2BGR);//因此投影模式下显示的也是rgb图？
}
//cvtColor(backproj1, image, CV_GRAY2BGR);//因此投影模式下显示的也是rgb图？
if (trackBox1.size.width >= 0 && trackBox1.size.height >= 0)
{
ellipse(image, trackBox1, Scalar(0, 0, 255), 3, CV_AA); //画出椭圆
}
if (trackObject >= 2)//trackObject初始化为0,或者按完键盘的'c'键后也为0，当鼠标单击松开后为-1
{
int _vmin = vmin, _vmax = vmax;
//inRange函数的功能是检查输入数组每个元素大小是否在2个给定数值之间，可以有多通道,mask1保存0通道的最小值，也就是h分量
//这里利用了hsv的3个通道，比较h,0~180,s,smin~256,v,min(vmin,vmax),max(vmin,vmax)。如果3个通道都在对应的范围内，则
//mask1对应的那个点的值全为1(0xff)，否则为0(0x00).
inRange(hsv, Scalar(0, smin, MIN(_vmin, _vmax)),
Scalar(180, 256, MAX(_vmin, _vmax)), mask2);
int ch[] = { 0, 0 };
hue1.create(hsv.size(), hsv.depth());//hue1初始化为与hsv大小深度一样的矩阵，色调的度量是用角度表示的，红绿蓝之间相差120度，反色相差180度
hue2.create(hsv.size(), hsv.depth());
mixChannels(&hsv, 1, &hue1, 1, ch, 1);//将hsv第一个通道(也就是色调)的数复制到hue1中，0索引数组
mixChannels(&hsv, 1, &hue2, 1, ch, 1);
if (trackObject == 3)
{
//此处的构造函数roi用的是Mat hue1的矩阵头，且roi的数据指针指向hue1，即共用相同的数据，select为其感兴趣的区域
Mat roi2(hue2, selection2), maskroi(mask2, selection2);//mask1保存的hsv的最小值
//calchist1()函数第一个参数为输入矩阵序列，第2个参数表示输入的矩阵数目，第3个参数表示将被计算直方图维数通道的列表，第4个参数表示可选的掩码函数
//第5个参数表示输出直方图，第6个参数表示直方图的维数，第7个参数为每一维直方图数组的大小，第8个参数为每一维直方图bin的边界
calcHist(&roi2, 1, 0, maskroi, hist2, 1, &hsize, &phranges2);//将roi的0通道计算直方图并通过mask1放入hist1中，hsize为每一维直方图的大小
normalize(hist2, hist2, 0, 255, CV_MINMAX);//将hist1矩阵进行数组范围归一化，都归一化到0~255
trackwindow2 = selection2;
trackObject = 4;//只要鼠标选完区域松开后，且没有按键盘清0键'c'，则trackObject一直保持为1，因此该if函数只能执行一次，除非重新选择跟踪区域
histimg2 = Scalar::all(0);//与按下'c'键是一样的，这里的all(0)表示的是标量全部清0
int binW = histimg2.cols / hsize; //hist1ing是一个200*300的矩阵，hsize应该是每一个bin的宽度，也就是hist1ing矩阵能分出几个bin出来
Mat buf(1, hsize, CV_8UC3);//定义一个缓冲单bin矩阵
for (int i = 0; i < hsize; i++)//saturate_case函数为从一个初始类型准确变换到另一个初始类型
buf.at<Vec3b>(i) = Vec3b(saturate_cast<uchar>(i*180. / hsize), 255, 255);//Vec3b为3个char值的向量
cvtColor(buf, buf, CV_HSV2BGR);//将hsv又转换成bgr
for (int i = 0; i < hsize; i++)
{
//cout << "he" << endl;
int val = saturate_cast<int>(hist2.at<float>(i)*histimg2.rows / 255);//at函数为返回一个指定数组元素的参考值
rectangle(histimg2, Point(i*binW, histimg2.rows), //在一幅输入图像上画一个简单抽的矩形，指定左上角和右下角，并定义颜色，大小，线型等
Point((i + 1)*binW, histimg2.rows - val),
Scalar(buf.at<Vec3b>(i)), -1, 8);
}
}
calcBackProject(&hue2, 1, 0, hist2, backproj2, &phranges2);//计算直方图的反向投影，计算hue1图像0通道直方图hist1的反向投影，并赋值到backproj2中
backproj2 &= mask2;
trackBox2 = CamShift(backproj2, trackwindow2, TermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 10, 1));
if ( trackwindow2.area() <= 1)
{
int cols = backproj2.cols, rows = backproj2.rows, r = (MIN(cols, rows) + 5) / 6;
trackwindow2 = Rect(trackwindow2.x - r, trackwindow2.y - r, trackwindow2.x + r, trackwindow2.y + r) &Rect(0, 0, cols, rows);//Rect函数为矩阵的偏移和大小，即第一二个参数为矩阵的左上角点坐标，第三四个参数为矩阵的宽和高
}
if (backproj1Mode)
{
cvtColor(backproj2, image2, CV_GRAY2BGR);
}
if (trackBox2.size.width >= 0 && trackBox2.size.height >= 0)
{
ellipse(image, trackBox2, Scalar(0, 0, 255), 3, CV_AA); //画出椭圆
}
}
}
//拖出矩形框
if (mark1 == false && selectObject && selection1.width > 0 && selection1.height > 0)
{
Mat roi(image, selection1);
bitwise_not(roi, roi);//bitwise_not为将每一个bit位取反
}
else if (mark1 == true && selectObject && selection2.width > 0 && selection2.height > 0)
{
//cout << "he" << endl;
Mat roi2(image, selection2);
bitwise_not(roi2, roi2);//bitwise_not为将每一个bit位取反
}
imshow("CamShift Demo", frame);
imshow("Rotated", image);
imshow("histogram", histimg1);
imshow("histogram2", histimg2);
char c = (char)waitKey(40);
if (c == 27) //退出键
break;
switch (c)
{
case 's':
type = 10;
break;
case 'b': //反向投影模型交替
backproj1Mode = !backproj1Mode;
break;
case 'c': //清零跟踪目标对象
trackObject = 0;
histimg1 = Scalar::all(0);
mark1 = false;
mark2 = false;
break;
case '1': //标志标志1选取完成
trackObject=1;
mark1 = true;
break;
case '2':
trackObject++;
mark2 = true;
break;
case 'h': //显示直方图交替
showhist1 = !showhist1;
if (!showhist1)
{
destroyWindow("histogram");
destroyWindow("histogram2");
}
else
{
namedWindow("histogram", 1);
namedWindow("histogram2", 1);
}
break;
case 'p': //透视变形
type = 0;
break;
default:
;
}
}
return 0;
}