I have opencv code for detecting square. And now I want after detect square, the code run another command.
Here is the code:
#include "cv.h"
#include "cxcore.h"
#include "highgui.h"
#include "math.h"
#include <iostream>
#include <stdio.h>
#include <string.h>
#include <sstream>
using namespace std;
double angle( CvPoint* pt1, CvPoint* pt2, CvPoint* pt0 )
{
double dx1 = pt1->x - pt0->x;
double dy1 = pt1->y - pt0->y;
double dx2 = pt2->x - pt0->x;
double dy2 = pt2->y - pt0->y;
return (dx1*dx2 + dy1*dy2)/sqrt((dx1*dx1 + dy1*dy1)*(dx2*dx2 + dy2*dy2) + 1e-10);
} // angle
IplImage* findSquares4( IplImage* img, CvMemStorage* storage )
{
double s = 0, t = 0;
int sdetect =0, sqdetect = 0,sqt = 0;
CvSeq* contours;
CvSeq* result;
CvSeq* squares = cvCreateSeq( 0, sizeof( CvSeq), sizeof( CvPoint), storage );
IplImage* cny = cvCreateImage(cvGetSize(img), 8, 1);
cvCanny(img, cny, 5, 100, 3);
cvNamedWindow("canny",CV_WINDOW_AUTOSIZE);
cvShowImage("canny",cny);
cvFindContours( cny, storage, &contours, sizeof( CvContour), CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE, cvPoint( 0, 0) );
while( contours )
{
result = cvApproxPoly( contours,
sizeof( CvContour),
storage,
CV_POLY_APPROX_DP,
cvContourPerimeter( contours)*0.02, 0 );
if( result->total == 4 &&
fabs( cvContourArea( result, CV_WHOLE_SEQ)) > 1000 &&
cvCheckContourConvexity( result) )
{
s = 0;
for( int i = 2; i < 5; i++ )
{
{
t = fabs( angle(
( CvPoint*)cvGetSeqElem( result, i ),
( CvPoint*)cvGetSeqElem( result, i-2 ),
( CvPoint*)cvGetSeqElem( result, i-1 )));
s = s > t ? s : t;
cout << "s = " << s<< endl;
cout << "t = " << t<< endl;
}
} // for
if( s < 0.3 )
for( int i = 0; i < 4; i++ )
cvSeqPush( squares,
( CvPoint*)cvGetSeqElem( result, i ));
} // if
contours = contours->h_next;
} // while
if ((squares->total/4) = 1)
{
sdetect = 1;
} / /if
else
{
sdetect = 2;
sleep(0.5);
} // else
if (sqdetect != sdetect)
{
sqdetect=sdetect;
switch(sqdetect)
{
case 0 : system(NULL) ; break;
case 2 : cout<< "no "<< endl; break;
case 1 : system("./ambil1.sh"); break;
} // switch
} // if
sdetect = 0;
cout<<"Persegi : "<< squares->total/4 <<endl;
cvReleaseMemStorage(&storage);
cvClearSeq(squares);
} // findSquares4
void drawSquares(IplImage *img, CvSeq* squares )
{
CvFont font;
cvInitFont( &font, CV_FONT_HERSHEY_SIMPLEX, 0.4f, 0.4f, 0,1, 8 );
int i,j,sdetect,sqdetect = 0;
CvSeqReader reader;
cvStartReadSeq( squares, &reader, 0 );
for( i = 0; i < squares->total; i += 4 )
{
j++;
CvPoint pt[4], *rect = pt;
int count = 4;
// read 4 vertices
memcpy( pt, reader.ptr, squares->elem_size );
CV_NEXT_SEQ_ELEM( squares->elem_size, reader );
memcpy( pt + 1, reader.ptr, squares->elem_size );
CV_NEXT_SEQ_ELEM( squares->elem_size, reader );
memcpy( pt + 2, reader.ptr, squares->elem_size );
CV_NEXT_SEQ_ELEM( squares->elem_size, reader );
memcpy( pt + 3, reader.ptr, squares->elem_size );
CV_NEXT_SEQ_ELEM( squares->elem_size, reader );
cvPutText( img, "SQUARE", pt[i], &font, CV_RGB(20,255,0));
cvPolyLine( img, &rect, &count, 1, 1, CV_RGB(200,0,0), 4, CV_AA, 0 );
} // for
cvClearSeq(squares);
} // drawSquares
But what I got is that the program becomes laggy. So what is the right algorithm to place condition for executing system (./ambil1.sh)?
The program waits until the subordinate command finishes. During this time it will not respond.
If that's what you need, system is adequate.
If you want to fire and forget, you can either use the fork/exec pair, or just use the shell & operator to run stuff in the background.
If you need to fire, change your program's behaviour, then change it back when the subordinate command finishes, you almost have to use the fork/exec/SIGCHLD/waitpid combination.
Related
I'm trying to create some code that takes the color of some picture and transfere to another. For this I'm using opencv for segmentation and treatment of the pictures.
I create a class named Shape that contains the contour of one shape ( find with opencv findContours() that creates a std::vector> ), a random string that serves as id for the object and a cv::Vec3b for the color of the shape.
But when I iterate over the contours found by opencv, and create one object(Shape class) and want to added to a list, it just add some of them then thows Segmentation error on console.
Here is my code:
#include <random>
#include <string>
#include <algorithm>
#include <opencv2/opencv.hpp>
using contourType = std::vector < std::vector < cv::Point > >;
std::string randomString ( )
{
auto randchar = [ ]( ) -> char {
const char charset[ ] =
"0123456789"
"ABCDEFGHIJKLMNOPQRSTUVWXYZ"
"abcdefghijklmnopqrstuvwxyz";
const size_t max_index = ( sizeof ( charset ) - 1);
return charset[ rand ( ) % max_index ];
};
std::string str ( 14, 0 );
std::generate_n ( str.begin ( ), 14, randchar );
return str;
};
class Shape {
public:
std::string idShape;
std::vector < cv::Point > contour;
cv::Vec3b color;
Shape () {
};
void setShape ( std::vector < cv::Point > cnt, cv::Vec3b clr ) {
idShape = randomString ( );
contour = cnt;
color = clr;
};
cv::Point centroid ( bool showImage ) {
cv::Moments m = cv::moments ( contour, true );
cv::Point centroid ( m.m10 / m.m00, m.m01 / m.m00 );
return centroid;
};
};
class Frame {
public:
std::string idFrame;
cv::Size size;
int type;
std::vector < Shape > shapes;
Frame ( cv::Size imSize, int tp ) {
idFrame = randomString ( );
size = imSize;
type = tp;
};
int addShape ( Shape shape ) {
shapes.push_back ( shape );
return 0;
};
};
contourType findShapes ( cv::Mat img, bool showImage ) {
//Threshold
cv::adaptiveThreshold ( img, img, 255, cv::ADAPTIVE_THRESH_MEAN_C, CV_THRESH_BINARY, 5, 1 );
//Erotion
cv::Mat kernel = cv::getStructuringElement ( cv::MORPH_RECT, cv::Size ( 2, 2 ), cv::Point ( 0, 0 ) );
cv::erode ( img, img, kernel );
//Find contours
contourType contours;
cv::Mat contourImg = img.clone ( );
cv::findContours ( contourImg, contours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE );
shapes
//Show image if true 2nd. argument
if ( showImage == true ) {
cv::Mat contourImage ( img.size ( ), CV_8UC3, cv::Scalar ( 0, 0, 0 ) );
cv::Scalar colors[ 3 ];
colors[ 0 ] = cv::Scalar ( 255, 0, 0 );
colors[ 1 ] = cv::Scalar ( 0, 255, 0 );
colors[ 2 ] = cv::Scalar ( 0, 0, 255 );
for ( size_t idx = 0; idx < contours.size ( ); idx++ ) {
cv::drawContours ( contourImage, contours, idx, colors[ idx % 3 ], CV_FILLED /*thick*/ );
std::cout << idx;
};
cv::imshow ( "Contour Image", contourImage );
cv::waitKey ( 0 );
};
return contours;
};
cv::Vec3b findColor ( cv::Mat img, std::vector < cv::Point > contour, bool print ) {
//Read pixel intensity
cv::Vec3b intensity;
cv::Point coordinate = contour[ 1 ];
intensity = img.at < cv::Vec3b > ( coordinate );
//Print pixel values
if ( print == true ) {
std::cout << intensity << std::endl;
};
return intensity;
};
int main ( int argc, char** argv ) {
std::string url = argv[1];
int type = 0;
cv::Mat img;
img = cv::imread ( url, 0 );
Frame frame ( img.size ( ), type );
contourType shapes;
shapes = findShapes ( img, false );
cv::Mat imColor;
imColor = cv::imread ( url, 1 );
std::list<Shape> te;
for ( size_t i = 0; i < shapes.size ( ); i++ ) {
Shape shape;
shape.setShape ( shapes[i], findColor ( imColor, shapes[i], false ) );
te.push_back( shape ); //Here is where it fails after adding a few objects, running out of memory? The idea is to contain the Shape object inside the a Frame members shapes property. But i'm testing first like this, just adding it to a list.
};
return 0;
}
It adds only 29 shapes to the list, then the error "segmentation fault (core dumped)" appears in console. But the image is about 67 shapes.
I'm attempting to convert a MATLAB .mat file to openCV MAT and then applying several masks to those files. I am building from cvmatio source code. I am receiving the following error:
OpenCV Error: Assertion failed (A.size == arrays[i0]->size) in init,
file
/home/derek/Documents/Libraries/opencv-3.0.0-beta/modules/core/src/matrix.cpp,
line 4279 terminate called after throwing an instance of
'cv::Exception' what():
/home/derek/Documents/Libraries/opencv-3.0.0-beta/modules/core/src/matrix.cpp:4279:
error: (-215) A.size == arrays[i0]->size in function init
Here is the source file I've written. It occurs at the line with MixChannels. Note that SrcImage is a 3 channel Mat. lower and upper are the threshold values in an array who's length is equal to the number of channels.
/*
* Mask.cpp
*
* Created on: Mar 16, 2015
* Author: derek
*/
#include <cv.h>
#include <highgui.h>
#include "imgcodecs.hpp"
#include "highgui.hpp"
#include "imgproc.hpp"
using namespace cv;
Mat Mask(Mat SrcImage, double lower[], double upper[]){
int height=SrcImage.rows;
int width=SrcImage.cols;
int depth=SrcImage.depth();
Mat B2d = Mat::ones(height, width,depth);
Mat out(height, width, depth);
Mat outL(height, width, depth);
Mat outU(height,width, depth);
for (int i=1; i< SrcImage.channels(); i=i+1){
int from_to[]={i,1};
mixChannels(&SrcImage, 3, &out, 1, from_to, 1 );
threshold(out, outL, lower[i], 1, THRESH_BINARY);
threshold(out, outU, upper[i], 1, THRESH_BINARY);
bitwise_and(B2d, outL, B2d);
bitwise_and(B2d, outU, B2d);
}
return B2d;
}
Also, here is an excerpt of the actual CV_Assertion error location. As indicated in the error, it occurs at "(A.size == arrays[i0]->size)".
void NAryMatIterator::init(const Mat** _arrays, Mat* _planes, uchar** _ptrs, int _narrays)
{
CV_Assert( _arrays && (_ptrs || _planes) );
int i, j, d1=0, i0 = -1, d = -1;
arrays = _arrays;
ptrs = _ptrs;
planes = _planes;
narrays = _narrays;
nplanes = 0;
size = 0;
if( narrays < 0 )
{
for( i = 0; _arrays[i] != 0; i++ )
;
narrays = i;
CV_Assert(narrays <= 1000);
}
iterdepth = 0;
for( i = 0; i < narrays; i++ )
{
CV_Assert(arrays[i] != 0);
const Mat& A = *arrays[i];
if( ptrs )
ptrs[i] = A.data;
if( !A.data )
continue;
if( i0 < 0 )
{
i0 = i;
d = A.dims;
// find the first dimensionality which is different from 1;
// in any of the arrays the first "d1" step do not affect the continuity
for( d1 = 0; d1 < d; d1++ )
if( A.size[d1] > 1 )
break;
}
else
CV_Assert( A.size == arrays[i0]->size );
if( !A.isContinuous() )
{
CV_Assert( A.step[d-1] == A.elemSize() );
for( j = d-1; j > d1; j-- )
if( A.step[j]*A.size[j] < A.step[j-1] )
break;
iterdepth = std::max(iterdepth, j);
}
}
if( i0 >= 0 )
{
size = arrays[i0]->size[d-1];
for( j = d-1; j > iterdepth; j-- )
{
int64 total1 = (int64)size*arrays[i0]->size[j-1];
if( total1 != (int)total1 )
break;
size = (int)total1;
}
iterdepth = j;
if( iterdepth == d1 )
iterdepth = 0;
nplanes = 1;
for( j = iterdepth-1; j >= 0; j-- )
nplanes *= arrays[i0]->size[j];
}
else
iterdepth = 0;
idx = 0;
if( !planes )
return;
for( i = 0; i < narrays; i++ )
{
CV_Assert(arrays[i] != 0);
const Mat& A = *arrays[i];
if( !A.data )
{
planes[i] = Mat();
continue;
}
planes[i] = Mat(1, (int)size, A.type(), A.data);
}
}
Well it's obviously too late of an answer, but here is the reason why your code failed:
Since your SrcImage is a single Mat with multiple channels, you should'we written:
mixChannels(&SrcImage, 1, &out, 1, from_to, 1 );
(The assert error was related to this, since mixChannels expected 3 Mats, which is 3 times bigger than your Mat.)
Also opencv MixChannels labels channels from 0, not sure if the i=1 was intended, or just a typo.
Cheers!
In OpenCV CamShift implementation there is a line ( just before the switch statement in onMouse() function in the code below ) of code which goes like this ,
selection &= Rect(0, 0, image.cols, image.rows);
where, selection is a Rect datatype of user defined dimensions ( selected by mouse callback ) and image is a frame from a video. The dimensions of selection and image do not match. What I don't understand is what is the purpose of this &= operator. I commented out this line of code and I couldn't figure out any difference. The code compilers just fine. I am not sure about what goes on inside when two objects are AND-ed. I could not find any resource that addresses this particular issue.
This is the original code:
#include "opencv2/video/tracking.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/highgui/highgui.hpp"
#include <iostream>
#include <ctype.h>
using namespace cv;
using namespace std;
Mat image;
bool backprojMode = false;
bool selectObject = false;
int trackObject = 0;
bool showHist = true;
Point origin;
Rect selection;
int vmin = 10, vmax = 256, smin = 30;
static void onMouse( int event, int x, int y, int, void* )
{
if( selectObject )
{
selection.x = MIN(x, origin.x);
selection.y = MIN(y, origin.y);
selection.width = std::abs(x - origin.x);
selection.height = std::abs(y - origin.y);
selection &= Rect(0, 0, image.cols, image.rows);
}
switch( event )
{
case CV_EVENT_LBUTTONDOWN:
origin = Point(x,y);
selection = Rect(x,y,0,0);
selectObject = true;
break;
case CV_EVENT_LBUTTONUP:
selectObject = false;
if( selection.width > 0 && selection.height > 0 )
trackObject = -1;
break;
}
}
static void help()
{
cout << "\nThis is a demo that shows mean-shift based tracking\n"
"You select a color objects such as your face and it tracks it.\n"
"This reads from video camera (0 by default, or the camera number the user enters\n"
"Usage: \n"
" ./camshiftdemo [camera number]\n";
cout << "\n\nHot keys: \n"
"\tESC - quit the program\n"
"\tc - stop the tracking\n"
"\tb - switch to/from backprojection view\n"
"\th - show/hide object histogram\n"
"\tp - pause video\n"
"To initialize tracking, select the object with mouse\n";
}
const char* keys =
{
"{1| | 0 | camera number}"
};
int main( int argc, const char* argv[] )
{
help();
VideoCapture cap;
Rect trackWindow;
int hsize = 16;
float hranges[] = {0,180};
const float* phranges = hranges;
CommandLineParser parser(argc, argv, keys);
int camNum = parser.get<int>("1");
cap.open(argv[1]);
/* if( !cap.isOpened() )
{
help();
cout << "***Could not initialize capturing...***\n";
cout << "Current parameter's value: \n";
parser.printParams();
return -1;
}*/
namedWindow( "Histogram", 0 );
namedWindow( "CamShift Demo", 0 );
namedWindow( "ROI", 0 );
setMouseCallback( "CamShift Demo", onMouse, 0 );
createTrackbar( "Vmin", "CamShift Demo", &vmin, 256, 0 );
createTrackbar( "Vmax", "CamShift Demo", &vmax, 256, 0 );
createTrackbar( "Smin", "CamShift Demo", &smin, 256, 0 );
Mat frame, hsv, hue, mask, hist, histimg = Mat::zeros(200, 320, CV_8UC3), backproj;
bool paused = false;
for(;;)
{
if( !paused )
{
cap >> frame;
if( frame.empty() )
break;
}
frame.copyTo(image);
if( !paused )
{
cvtColor(image, hsv, COLOR_BGR2HSV);
if( trackObject )
{
int _vmin = vmin, _vmax = vmax;
inRange(hsv, Scalar(0, smin, MIN(_vmin,_vmax)),
Scalar(180, 256, MAX(_vmin, _vmax)), mask);
int ch[] = {0, 0};
hue.create(hsv.size(), hsv.depth());
mixChannels(&hsv, 1, &hue, 1, ch, 1);
if( trackObject < 0 )
{
Mat roi(hue, selection), maskroi(mask, selection);
calcHist(&roi, 1, 0, maskroi, hist, 1, &hsize, &phranges);
normalize(hist, hist, 0, 255, CV_MINMAX);
trackWindow = selection;
trackObject = 1;
histimg = Scalar::all(0);
int binW = histimg.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);
for( int i = 0; i < hsize; i++ )
{
int val = saturate_cast<int>(hist.at<float>(i)*histimg.rows/255);
rectangle( histimg, Point(i*binW,histimg.rows),
Point((i+1)*binW,histimg.rows - val),
Scalar(buf.at<Vec3b>(i)), -1, 8 );
}
imshow("ROI",roi);
}
calcBackProject(&hue, 1, 0, hist, backproj, &phranges);
backproj &= mask;
RotatedRect trackBox = CamShift(backproj, trackWindow,
TermCriteria( CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 10, 1 ));
cout<<trackWindow.x<<" "<<trackWindow.y<<endl;
if( trackWindow.area() <= 1 )
{
int cols = backproj.cols, rows = backproj.rows, r = (MIN(cols, rows) + 5)/6;
trackWindow = Rect(trackWindow.x - r, trackWindow.y - r,
trackWindow.x + r, trackWindow.y + r) &
Rect(0, 0, cols, rows);
}
if( backprojMode )
cvtColor( backproj, image, COLOR_GRAY2BGR );
ellipse( image, trackBox, Scalar(0,0,255), 3, CV_AA );
}
}
else if( trackObject < 0 )
paused = false;
if( selectObject && selection.width > 0 && selection.height > 0 )
{
Mat roi(image, selection);
bitwise_not(roi, roi);
}
imshow( "CamShift Demo", image );
//imshow( "Histogram", histimg );
char c = (char)waitKey(50);
if( c == 27 )
break;
switch(c)
{
case 'b':
backprojMode = !backprojMode;
break;
case 'c':
trackObject = 0;
histimg = Scalar::all(0);
break;
case 'h':
showHist = !showHist;
if( !showHist )
destroyWindow( "Histogram" );
else
namedWindow( "Histogram", 1 );
break;
case 'p':
paused = !paused;
break;
default:
int a=1;
//continue;
}
}
return 0;
}
Any comments or suggestions will be appreciated.
The &-operator of two Rect objects results in a new Rect object, which is their intersection. Here they intersect the selection with the entire image-frame. I suspect that this is merely a precaution, done so that selection does not accidentally exceed the boundaries of the image.
the & and | operators are overloaded for cv::Rect, & is the intersection, | the union of 2 Rects,
see for yourself:
Rect a(2,2,5,5);
Rect b(4,4,5,5);
Rect c = a & b;
Rect d = a | b;
cerr << a << endl << b << endl << c << endl << d << endl ;
[5 x 5 from (2, 2)]
[5 x 5 from (4, 4)]
[3 x 3 from (4, 4)] // intersection
[7 x 7 from (2, 2)] // union
in you code example above,
selection &= Rect(0, 0, image.cols, image.rows);
'selection' gets cropped to the valid image borders
I'm using Opencv2.4.6 and Netbeans IDE for C++.
I'm trying to do a program to detect a face with the CascadeClassifier, and then pass the Rect of the face to the Camshift function to track this face.
To do that, I took the sample code "Camshiftdemo.cpp" that comes in the samples folder of opencv and I've modified. In my code instead of use the mouse to select the region over which you want to do tracking, is the cascade classifier which passes that information.
#include "opencv2/video/tracking.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/objdetect/objdetect.hpp"
#include <iostream>
#include <ctype.h>
using namespace cv;
using namespace std;
Mat image;
int trackObject = 0;
Rect selection;
int main()
{
VideoCapture cap;
Rect trackWindow;
int hsize = 16;
float hranges[] = {0,180};
const float* phranges = hranges;
int matchesNum = 0;
CascadeClassifier cascade;
if (!cascade.load("C:/opencv/data/haarcascades/haarcascade_frontalface_default.xml")) {
cout << "Cannot load face xml!" << endl;
return -1;
}
cap.open("D:/Videos_Proy/ProgramacionII/CAMERA3_clase1.MP4");
if (!cap.isOpened()) {
cout << "***Could not initialize capturing...***\n";
return -1;
}
namedWindow( "Result", 1 );
Mat frame, hsv, hue, hist, mask, backproj;
for(;;)
{
cap >> frame;
if( frame.empty() )
break;
frame.copyTo(image);
if ( !trackObject )
{
Mat grayframe;
vector <Rect> facesBuf;
int detectionsNum = 0;
cvtColor(image, grayframe, CV_BGR2GRAY);
cascade.detectMultiScale(grayframe, facesBuf, 1.2, 4, CV_HAAR_FIND_BIGGEST_OBJECT |
CV_HAAR_SCALE_IMAGE, cvSize(0, 0));
detectionsNum = (int) facesBuf.size();
Rect *faceRects = &facesBuf[0];
//It must found faces in three consecutives frames to start the tracking to discard false positives
if (detectionsNum > 0)
matchesNum += 1;
else matchesNum = 0;
if ( matchesNum == 3 )
{
trackObject = -1;
selection = faceRects[0];
}
for (int i = 0; i < detectionsNum; i++)
{
Rect r = faceRects[i];
rectangle(image, Point(r.x, r.y), Point(r.x + r.width, r.y + r.height), CV_RGB(0, 255, 0));
}
}
if( trackObject )
{
cvtColor(image, hsv, CV_BGR2HSV);
inRange(hsv, Scalar(0, 69, 53),
Scalar(180, 256, 256), mask);
int ch[] = {0, 0};
hue.create(hsv.size(), hsv.depth());
mixChannels(&hsv, 1, &hue, 1, ch, 1);
if( trackObject < 0 )
{
Mat roi(hue, selection), maskroi(mask, selection);
calcHist(&roi, 1, 0, maskroi, hist, 1, &hsize, &phranges);
normalize(hist, hist, 0, 255, CV_MINMAX);
trackWindow = selection;
trackObject = 1;
}
calcBackProject(&hue, 1, 0, hist, backproj, &phranges);
backproj &= mask;
RotatedRect trackBox = CamShift(backproj, trackWindow,
TermCriteria( CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 10, 1 ));
if( trackWindow.area() <= 1 )
{
int cols = backproj.cols, rows = backproj.rows, r = (MIN(cols, rows) + 5)/6;
trackWindow = Rect(trackWindow.x - r, trackWindow.y - r,
trackWindow.x + r, trackWindow.y + r) &
Rect(0, 0, cols, rows);
}
ellipse( image, trackBox, Scalar(0,0,255), 3, CV_AA );
}
imshow( "Result", image );
if(waitKey(30) >= 0) break;
}
return 0;
}
This code makes sense to me, and when I Build it in Netbeans do not get any error, the problem is that don't run and Netbeans don't give any clue, only says: RUN FAILED (exit value -1.073.741.819, total time: 5s)
Anyone could help me and give any idea about what is happening. Thanks!!
I am trying to implement a Kalman filter based mouse tracking (as a test first) using a velocity-acceleration model.
I want to try this out this simple model, my state transition equations are:
X(k) = [x(k), y(k)]' (Position)
V(k) = [vx(k), vy(k)]' (Velocity)
X(k) = X(k-1) + dt*V(k-1) + 0.5*dt*dt*a(k-1)
V(k) = V(k-1) + t*a(k-1)
a(k) = a(k-1)
Using this I have basically wrote down the following piece of code:
#include <iostream>
#include <vector>
#include <cstdio>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/video/tracking.hpp>
using namespace cv;
using namespace std;
struct mouse_info_struct { int x,y; };
struct mouse_info_struct mouse_info = {-1,-1}, last_mouse;
void on_mouse(int event, int x, int y, int flags, void* param)
{
//if (event == CV_EVENT_LBUTTONUP)
{
last_mouse = mouse_info;
mouse_info.x = x;
mouse_info.y = y;
}
}
void printmat(const cv::Mat &__mat, std::string __str)
{
std::cout << "--------" << __str << "----------\n";
for (int i=0 ; i<__mat.rows ; ++i)
{
for (int j=0 ; j<__mat.cols ; ++j)
std::cout << __mat.at<double>(i,j) << " ";
std::cout << std::endl;
}
std::cout << "-------------------------------------\n";
}
int main (int argc, char * const argv[])
{
int nStates = 5, nMeasurements = 2, nInputs = 1;
Mat img(500, 900, CV_8UC3);
KalmanFilter KF(nStates, nMeasurements, nInputs, CV_64F);
Mat state(nStates, 1, CV_64F); /* (x, y, Vx, Vy, a) */
Mat measurement(nMeasurements,1,CV_64F); measurement.setTo(Scalar(0));
Mat prevMeasurement(nMeasurements,1,CV_64F); prevMeasurement.setTo(Scalar(0));
int key = -1, dt=100, T=1000;
float /*a=100, acclErrMag = 0.05,*/ measurementErrVar = 100, noiseVal=0.001, covNoiseVal=0.9e-4;
namedWindow("Mouse-Kalman");
setMouseCallback("Mouse-Kalman", on_mouse, 0);
//while ( (char)(key=cv::waitKey(100)) != 'q' )
{
/// A
KF.transitionMatrix.at<double>(0,0) = 1;
KF.transitionMatrix.at<double>(0,1) = 0;
KF.transitionMatrix.at<double>(0,2) = (dt/T);
KF.transitionMatrix.at<double>(0,3) = 0;
KF.transitionMatrix.at<double>(0,4) = 0.5*(dt/T)*(dt/T);
KF.transitionMatrix.at<double>(1,0) = 0;
KF.transitionMatrix.at<double>(1,1) = 1;
KF.transitionMatrix.at<double>(1,2) = 0;
KF.transitionMatrix.at<double>(1,3) = (dt/T);
KF.transitionMatrix.at<double>(1,4) = 0.5*(dt/T)*(dt/T);
KF.transitionMatrix.at<double>(2,0) = 0;
KF.transitionMatrix.at<double>(2,1) = 0;
KF.transitionMatrix.at<double>(2,2) = 1;
KF.transitionMatrix.at<double>(2,3) = 0;
KF.transitionMatrix.at<double>(2,4) = (dt/T);
KF.transitionMatrix.at<double>(3,0) = 0;
KF.transitionMatrix.at<double>(3,1) = 0;
KF.transitionMatrix.at<double>(3,2) = 0;
KF.transitionMatrix.at<double>(3,3) = 1;
KF.transitionMatrix.at<double>(3,4) = (dt/T);
KF.transitionMatrix.at<double>(4,0) = 0;
KF.transitionMatrix.at<double>(4,1) = 0;
KF.transitionMatrix.at<double>(4,2) = 0;
KF.transitionMatrix.at<double>(4,3) = 0;
KF.transitionMatrix.at<double>(4,4) = 1;
/// Initial estimate of state variables
KF.statePost = cv::Mat::zeros(nStates, 1,CV_64F);
KF.statePost.at<double>(0) = mouse_info.x;
KF.statePost.at<double>(1) = mouse_info.y;
KF.statePost.at<double>(2) = 0;
KF.statePost.at<double>(3) = 0;
KF.statePost.at<double>(4) = 0;
KF.statePre = KF.statePost;
/// Ex or Q
setIdentity(KF.processNoiseCov, Scalar::all(noiseVal));
/// Initial covariance estimate Sigma_bar(t) or P'(k)
setIdentity(KF.errorCovPre, Scalar::all(1000));
/// Sigma(t) or P(k)
setIdentity(KF.errorCovPost, Scalar::all(1000));
/// B
KF.controlMatrix = cv::Mat(nStates, nInputs,CV_64F);
KF.controlMatrix.at<double>(0,0) = 0;
KF.controlMatrix.at<double>(1,0) = 0;
KF.controlMatrix.at<double>(2,0) = 0;
KF.controlMatrix.at<double>(3,0) = 0;
KF.controlMatrix.at<double>(4,0) = 1;
/// H
KF.measurementMatrix = cv::Mat::eye(nMeasurements, nStates, CV_64F);
/// Ez or R
setIdentity(KF.measurementNoiseCov, Scalar::all(measurementErrVar*measurementErrVar));
printmat(KF.controlMatrix, "KF.controlMatrix");
printmat(KF.transitionMatrix, "KF.transitionMatrix");
printmat(KF.statePre,"KF.statePre");
printmat(KF.processNoiseCov, "KF.processNoiseCov");
printmat(KF.measurementMatrix, "KF.measurementMatrix");
printmat(KF.measurementNoiseCov, "KF.measurementNoiseCov");
printmat(KF.errorCovPost,"KF.errorCovPost");
printmat(KF.errorCovPre,"KF.errorCovPre");
printmat(KF.statePost,"KF.statePost");
while (mouse_info.x < 0 || mouse_info.y < 0)
{
imshow("Mouse-Kalman", img);
waitKey(30);
continue;
}
while ( (char)key != 's' )
{
/// MAKE A MEASUREMENT
measurement.at<double>(0) = mouse_info.x;
measurement.at<double>(1) = mouse_info.y;
/// MEASUREMENT UPDATE
Mat estimated = KF.correct(measurement);
/// STATE UPDATE
Mat prediction = KF.predict();
cv::Mat u(nInputs,1,CV_64F);
u.at<double>(0,0) = 0.0 * sqrt(pow((prevMeasurement.at<double>(0) - measurement.at<double>(0)),2)
+ pow((prevMeasurement.at<double>(1) - measurement.at<double>(1)),2));
/// STORE ALL DATA
Point predictPt(prediction.at<double>(0),prediction.at<double>(1));
Point estimatedPt(estimated.at<double>(0),estimated.at<double>(1));
Point measuredPt(measurement.at<double>(0),measurement.at<double>(1));
/// PLOT POINTS
#define drawCross( center, color, d ) \
line( img, Point( center.x - d, center.y - d ), \
Point( center.x + d, center.y + d ), color, 2, CV_AA, 0); \
line( img, Point( center.x + d, center.y - d ), \
Point( center.x - d, center.y + d ), color, 2, CV_AA, 0 )
/// DRAW ALL ON IMAGE
img = Scalar::all(0);
drawCross( predictPt, Scalar(255,255,255), 9 ); //WHITE
drawCross( estimatedPt, Scalar(0,0,255), 6 ); //RED
drawCross( measuredPt, Scalar(0,255,0), 3 ); //GREEN
line( img, estimatedPt, measuredPt, Scalar(100,255,255), 3, CV_AA, 0 );
line( img, estimatedPt, predictPt, Scalar(0,255,255), 3, CV_AA, 0 );
prevMeasurement = measurement;
imshow( "Mouse-Kalman", img );
key=cv::waitKey(10);
}
}
return 0;
}
Here is the output of the code: http://www.youtube.com/watch?v=9_xd4HSz8_g
As you can see that the tracking very very slow. I don't understand what is wrong with the model and why the estimation is so slow. I don't expect there should be any control input.
Can anyone explain this?
I have modified my code and I am posting it for those who want to tweak it to play around for more. The main problem was the choice of covariances.
#include <iostream>
#include <vector>
#include <cstdio>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/video/tracking.hpp>
using namespace cv;
using namespace std;
struct mouse_info_struct { int x,y; };
struct mouse_info_struct mouse_info = {-1,-1}, last_mouse;
vector<Point> mousev,kalmanv;
int trackbarProcessNoiseCov = 10, trackbarMeasurementNoiseCov = 10, trackbarStateEstimationErrorCov = 10;
float processNoiseCov=10, measurementNoiseCov = 1000, stateEstimationErrorCov = 50;
int trackbarProcessNoiseCovMax=10000, trackbarMeasurementNoiseCovMax = 10000,
trackbarStateEstimationErrorCovMax = 5000;
float processNoiseCovMin=0, measurementNoiseCovMin = 0,
stateEstimationErrorCovMin = 0;
float processNoiseCovMax=100, measurementNoiseCovMax = 5000,
stateEstimationErrorCovMax = 5000;
int nStates = 5, nMeasurements = 2, nInputs = 1;
KalmanFilter KF(nStates, nMeasurements, nInputs, CV_64F);
void on_mouse(int event, int x, int y, int flags, void* param)
{
last_mouse = mouse_info;
mouse_info.x = x;
mouse_info.y = y;
}
void on_trackbarProcessNoiseCov( int, void* )
{
processNoiseCov = processNoiseCovMin +
(trackbarProcessNoiseCov * (processNoiseCovMax-processNoiseCovMin)/trackbarProcessNoiseCovMax);
setIdentity(KF.processNoiseCov, Scalar::all(processNoiseCov));
std::cout << "\nProcess Noise Cov: " << processNoiseCov;
std::cout << "\nMeasurement Noise Cov: " << measurementNoiseCov << std::endl;
}
void on_trackbarMeasurementNoiseCov( int, void* )
{
measurementNoiseCov = measurementNoiseCovMin +
(trackbarMeasurementNoiseCov * (measurementNoiseCovMax-measurementNoiseCovMin)/trackbarMeasurementNoiseCovMax);
setIdentity(KF.measurementNoiseCov, Scalar::all(measurementNoiseCov));
std::cout << "\nProcess Noise Cov: " << processNoiseCov;
std::cout << "\nMeasurement Noise Cov: " << measurementNoiseCov << std::endl;
}
int main (int argc, char * const argv[])
{
Mat img(500, 1000, CV_8UC3);
Mat state(nStates, 1, CV_64F);/* (x, y, Vx, Vy, a) */
Mat measurementNoise(nMeasurements, 1, CV_64F), processNoise(nStates, 1, CV_64F);
Mat measurement(nMeasurements,1,CV_64F); measurement.setTo(Scalar(0.0));
Mat noisyMeasurement(nMeasurements,1,CV_64F); noisyMeasurement.setTo(Scalar(0.0));
Mat prevMeasurement(nMeasurements,1,CV_64F); prevMeasurement.setTo(Scalar(0.0));
Mat prevMeasurement2(nMeasurements,1,CV_64F); prevMeasurement2.setTo(Scalar(0.0));
int key = -1, dt=50, T=1000;
namedWindow("Mouse-Kalman");
setMouseCallback("Mouse-Kalman", on_mouse, 0);
createTrackbar( "Process Noise Cov", "Mouse-Kalman", &trackbarProcessNoiseCov,
trackbarProcessNoiseCovMax, on_trackbarProcessNoiseCov );
createTrackbar( "Measurement Noise Cov", "Mouse-Kalman", &trackbarMeasurementNoiseCov,
trackbarMeasurementNoiseCovMax, on_trackbarMeasurementNoiseCov );
on_trackbarProcessNoiseCov( trackbarProcessNoiseCov, 0 );
on_trackbarMeasurementNoiseCov( trackbarMeasurementNoiseCov, 0 );
//while ( (char)(key=cv::waitKey(100)) != 'q' )
{
/// A (TRANSITION MATRIX INCLUDING VELOCITY AND ACCELERATION MODEL)
KF.transitionMatrix.at<double>(0,0) = 1;
KF.transitionMatrix.at<double>(0,1) = 0;
KF.transitionMatrix.at<double>(0,2) = (dt/T);
KF.transitionMatrix.at<double>(0,3) = 0;
KF.transitionMatrix.at<double>(0,4) = 0.5*(dt/T)*(dt/T);
KF.transitionMatrix.at<double>(1,0) = 0;
KF.transitionMatrix.at<double>(1,1) = 1;
KF.transitionMatrix.at<double>(1,2) = 0;
KF.transitionMatrix.at<double>(1,3) = (dt/T);
KF.transitionMatrix.at<double>(1,4) = 0.5*(dt/T)*(dt/T);
KF.transitionMatrix.at<double>(2,0) = 0;
KF.transitionMatrix.at<double>(2,1) = 0;
KF.transitionMatrix.at<double>(2,2) = 1;
KF.transitionMatrix.at<double>(2,3) = 0;
KF.transitionMatrix.at<double>(2,4) = (dt/T);
KF.transitionMatrix.at<double>(3,0) = 0;
KF.transitionMatrix.at<double>(3,1) = 0;
KF.transitionMatrix.at<double>(3,2) = 0;
KF.transitionMatrix.at<double>(3,3) = 1;
KF.transitionMatrix.at<double>(3,4) = (dt/T);
KF.transitionMatrix.at<double>(4,0) = 0;
KF.transitionMatrix.at<double>(4,1) = 0;
KF.transitionMatrix.at<double>(4,2) = 0;
KF.transitionMatrix.at<double>(4,3) = 0;
KF.transitionMatrix.at<double>(4,4) = 1;
/// Initial estimate of state variables
KF.statePost = cv::Mat::zeros(nStates, 1,CV_64F);
KF.statePost.at<double>(0) = mouse_info.x;
KF.statePost.at<double>(1) = mouse_info.y;
KF.statePost.at<double>(2) = 0.1;
KF.statePost.at<double>(3) = 0.1;
KF.statePost.at<double>(4) = 0.1;
KF.statePre = KF.statePost;
state = KF.statePost;
/// Ex or Q (PROCESS NOISE COVARIANCE)
setIdentity(KF.processNoiseCov, Scalar::all(processNoiseCov));
/// Initial covariance estimate Sigma_bar(t) or P'(k)
setIdentity(KF.errorCovPre, Scalar::all(stateEstimationErrorCov));
/// Sigma(t) or P(k) (STATE ESTIMATION ERROR COVARIANCE)
setIdentity(KF.errorCovPost, Scalar::all(stateEstimationErrorCov));
/// B (CONTROL MATRIX)
KF.controlMatrix = cv::Mat(nStates, nInputs,CV_64F);
KF.controlMatrix.at<double>(0,0) = /*0.5*(dt/T)*(dt/T);//*/0;
KF.controlMatrix.at<double>(1,0) = /*0.5*(dt/T)*(dt/T);//*/0;
KF.controlMatrix.at<double>(2,0) = 0;
KF.controlMatrix.at<double>(3,0) = 0;
KF.controlMatrix.at<double>(4,0) = 1;
/// H (MEASUREMENT MATRIX)
KF.measurementMatrix = cv::Mat::eye(nMeasurements, nStates, CV_64F);
/// Ez or R (MEASUREMENT NOISE COVARIANCE)
setIdentity(KF.measurementNoiseCov, Scalar::all(measurementNoiseCov));
while (mouse_info.x < 0 || mouse_info.y < 0)
{
imshow("Mouse-Kalman", img);
waitKey(30);
continue;
}
prevMeasurement.at<double>(0,0) = 0;
prevMeasurement.at<double>(1,0) = 0;
prevMeasurement2 = prevMeasurement;
while ( (char)key != 's' )
{
/// STATE UPDATE
Mat prediction = KF.predict();
/// MAKE A MEASUREMENT
measurement.at<double>(0) = mouse_info.x;
measurement.at<double>(1) = mouse_info.y;
/// MEASUREMENT NOISE SIMULATION
randn( measurementNoise, Scalar(0),
Scalar::all(sqrtf(measurementNoiseCov)));
noisyMeasurement = measurement + measurementNoise;
/// MEASUREMENT UPDATE
Mat estimated = KF.correct(noisyMeasurement);
cv::Mat u(nInputs,1,CV_64F);
u.at<double>(0,0) = 0.0 * sqrtf(pow((prevMeasurement.at<double>(0) - measurement.at<double>(0)),2)
+ pow((prevMeasurement.at<double>(1) - measurement.at<double>(1)),2));
/// STORE ALL DATA
Point noisyPt(noisyMeasurement.at<double>(0),noisyMeasurement.at<double>(1));
Point estimatedPt(estimated.at<double>(0),estimated.at<double>(1));
Point measuredPt(measurement.at<double>(0),measurement.at<double>(1));
/// PLOT POINTS
#define drawCross( center, color, d ) \
line( img, Point( center.x - d, center.y - d ), \
Point( center.x + d, center.y + d ), color, 2, CV_AA, 0); \
line( img, Point( center.x + d, center.y - d ), \
Point( center.x - d, center.y + d ), color, 2, CV_AA, 0 )
/// DRAW ALL ON IMAGE
img = Scalar::all(0);
drawCross( noisyPt, Scalar(255,255,255), 9 ); //WHITE
drawCross( estimatedPt, Scalar(0,0,255), 6 ); //RED
drawCross( measuredPt, Scalar(0,255,0), 3 ); //GREEN
line( img, estimatedPt, measuredPt, Scalar(100,255,255), 3, CV_AA, 0 );
line( img, estimatedPt, noisyPt, Scalar(0,255,255), 3, CV_AA, 0 );
imshow( "Mouse-Kalman", img );
key=cv::waitKey(dt);
prevMeasurement = measurement;
}
}
return 0;
}