t_jit_err cv_jit_shift_matrix_calc(t_cv_jit_shift *x, void *inputs, void *outputs)
{
t_jit_err err=JIT_ERR_NONE;
long in_savelock = 0;
t_jit_matrix_info in_minfo;
void *in_matrix;
CvMat source;
CvRect rectangle;
CvBox2D box;
CvConnectedComp component;
CvPoint2D32f vertices[4];
float w,h,c,s;
//Get pointer to matrix
in_matrix = jit_object_method(inputs,_jit_sym_getindex,0);
if (x&&in_matrix)
{
//Lock the matrix
in_savelock = (long) jit_object_method(in_matrix,_jit_sym_lock,1);
//Make sure input is of proper format
jit_object_method(in_matrix,_jit_sym_getinfo,&in_minfo);
if(in_minfo.dimcount != 2)
{
err = JIT_ERR_MISMATCH_DIM;
goto out;
}
if(in_minfo.planecount != 1)
{
err = JIT_ERR_MISMATCH_PLANE;
goto out;
}
if(in_minfo.type != _jit_sym_char)
{
err = JIT_ERR_MISMATCH_TYPE;
goto out;
}
//Don't process if image is too small
if((in_minfo.dim[0] < 2)||(in_minfo.dim[1] < 2))
goto out;
//Calculate start rectangle:
rectangle = cvRect(x->rect[0],x->rect[1],x->rect[2]-x->rect[0],x->rect[3]-x->rect[1]);
CLIP_ASSIGN(rectangle.x,0,in_minfo.dim[0]-1);
CLIP_ASSIGN(rectangle.y,0,in_minfo.dim[1]-1);
CLIP_ASSIGN(rectangle.width,1,in_minfo.dim[0]-rectangle.x);
CLIP_ASSIGN(rectangle.height,1,in_minfo.dim[1]-rectangle.y);
//Convert Jitter matrix to OpenCV matrix
cvJitter2CvMat(in_matrix, &source);
//Calculate camshift
if(x->mode == 1) //Use camshift
cvCamShift(&source, rectangle, cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,(int)x->maxiters,x->epsilon), &component, &box );
else {
cvMeanShift(&source, rectangle, cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,(int)x->maxiters,x->epsilon), &component);
box.angle = 90.f;
box.size = cvSize2D32f(component.rect.width, component.rect.height);
box.center = cvPoint2D32f((float)component.rect.x + (float)component.rect.width * 0.5f,(float)component.rect.y + (float)component.rect.height * 0.5f);
}
//Prepare output
//
jit_atom_setlong(&x->box[0],component.rect.x);
jit_atom_setlong(&x->box[1],component.rect.y);
jit_atom_setlong(&x->box[2],component.rect.x + component.rect.width);
jit_atom_setlong(&x->box[3],component.rect.y + component.rect.height);
x->rect[0]=component.rect.x;
x->rect[1]=component.rect.y;
x->rect[2]=component.rect.x + component.rect.width;
x->rect[3]=component.rect.y + component.rect.height;
//cvBoxPoints(box,vertices);
w = box.size.width * 0.5;
h = box.size.height * 0.5;
c = cos((box.angle - 90.f) * -0.01745329252);
s = sin((box.angle - 90.f) * -0.01745329252);
vertices[0].x = box.center.x - s*h - c*w;
vertices[0].y = box.center.y - c*h + s*w;
vertices[1].x = box.center.x - s*h + c*w;
vertices[1].y = box.center.y - c*h - s*w;
vertices[2].x = box.center.x + s*h + c*w;
vertices[2].y = box.center.y + c*h - s*w;
vertices[3].x = box.center.x + s*h - c*w;
vertices[3].y = box.center.y + c*h + s*w;
jit_atom_setlong(&x->frame[0],(long)vertices[0].x);
jit_atom_setlong(&x->frame[1],(long)vertices[0].y);
jit_atom_setlong(&x->frame[2],(long)vertices[1].x);
jit_atom_setlong(&x->frame[3],(long)vertices[1].y);
jit_atom_setlong(&x->frame[4],(long)vertices[2].x);
jit_atom_setlong(&x->frame[5],(long)vertices[2].y);
jit_atom_setlong(&x->frame[6],(long)vertices[3].x);
jit_atom_setlong(&x->frame[7],(long)vertices[3].y);
x->mass = (float)(component.area / 256.);
}
out:
jit_object_method(in_matrix,gensym("lock"),in_savelock);
return err;
}
t_jit_err cv_jit_LKflow_matrix_calc(t_cv_jit_LKflow *x, void *inputs, void *outputs)
{
t_jit_err err=JIT_ERR_NONE;
long in_savelock,out_savelock;
t_jit_matrix_info in_minfo,out_minfo;
char *in_bp,*out_bp;
long i,j;
void *in_matrix,*out_matrix;
float *out, *outX, *outY;
int stepX, stepY;
int radius = x->radius * 2 + 1;
CvMat current;
CvMat *flowX=0, *flowY=0;
//First, get pointers to matrices
in_matrix = jit_object_method(inputs,_jit_sym_getindex,0);
out_matrix = jit_object_method(outputs,_jit_sym_getindex,0);
if (x&&in_matrix&&out_matrix)
{
in_savelock = (long) jit_object_method(in_matrix,_jit_sym_lock,1);
out_savelock = (long) jit_object_method(out_matrix,_jit_sym_lock,1);
//Get the matrix info
jit_object_method(in_matrix,_jit_sym_getinfo,&in_minfo);
jit_object_method(out_matrix,_jit_sym_getinfo,&out_minfo);
//Get pointers to the actual matrix data
jit_object_method(in_matrix,_jit_sym_getdata,&in_bp);
jit_object_method(out_matrix,_jit_sym_getdata,&out_bp);
//If something is wrong with the pointer...
if (!in_bp) { err=JIT_ERR_INVALID_INPUT; goto out;}
if (!out_bp) { err=JIT_ERR_INVALID_OUTPUT; goto out;}
//compatible types?
if ((in_minfo.type!=_jit_sym_char)||(out_minfo.type!=_jit_sym_float32)) {
err=JIT_ERR_MISMATCH_TYPE;
goto out;
}
//compatible planes?
if ((in_minfo.planecount!=1)||(out_minfo.planecount!=2)) { //Accepts only 1-plane matrices
err=JIT_ERR_MISMATCH_PLANE;
goto out;
}
//compatible dims?
if ((in_minfo.dimcount!=2)){
err=JIT_ERR_MISMATCH_DIM;
goto out;
}
//Check to see if image isn't too small
if((in_minfo.dim[0] < radius)||(in_minfo.dim[1] < radius)){
error("Matrix height and width must be at least %d", radius);
err = JIT_ERR_GENERIC;
goto out;
}
//Convert Jitter matrix to OpenCV matrix
cvJitter2CvMat(in_matrix, ¤t);
flowX = cvCreateMat(current.rows, current.cols,CV_32FC1);
flowY = cvCreateMat(current.rows, current.cols,CV_32FC1);
if(!flowX || !flowY){
error("Failed to create internal data.");
goto out;
}
if(!x->previous){
x->previous = cvCreateMat(current.rows, current.cols, current.type);
if(!x->previous){
error("Failed to create internal matrix.");
goto out;
}
}
else if((current.cols != x->previous->cols)||(current.rows != x->previous->rows)||(current.step != x->previous->step)||(x->previous->data.ptr == NULL)){
cvReleaseMat(&x->previous);
//Because we cast a Jitter matrix into a CvMat, we cannot assume that the step is going to be the same as that
//returned by cvCreateMat, hence we need to fudge things by hand.
x->previous = cvCreateMatHeader(current.rows, current.cols, current.type);
if(!x->previous){
error("Failed to create internal matrix (2).");
err = JIT_ERR_GENERIC;
goto out;
}
cvInitMatHeader(x->previous, current.rows, current.cols, current.type, 0, current.step);
cvCreateData(x->previous);
if(!x->previous->data.ptr){
error("Failed to allocate internal memory.");
err = JIT_ERR_GENERIC;
cvReleaseMat(&x->previous);
goto out;
}
//jit_object_method(out_matrix, _jit_sym_clear);
}else {
//Calculate optical flow
cvCalcOpticalFlowLK(x->previous, ¤t, cvSize(radius, radius),flowX, flowY);
//Copy to output
out = (float *)out_bp;
outX = flowX->data.fl;
outY = flowY->data.fl;
stepX = flowX->step / sizeof(float);
stepY = flowY->step / sizeof(float);
for(i=0;i<out_minfo.dim[1];i++){
out=(float *)(out_bp+i*out_minfo.dimstride[1]);
outX=flowX->data.fl+i*stepX;
outY=flowY->data.fl+i*stepY;
for(j=0;j<out_minfo.dim[0];j++){
out[0] = *outX;
out[1] = *outY;
out+=2;
outX++;
outY++;
}
}
}
cvCopy(¤t, x->previous, 0);
}
else
{
return JIT_ERR_INVALID_PTR;
}
out:
if(flowX)cvReleaseMat(&flowX);
if(flowY)cvReleaseMat(&flowY);
jit_object_method(out_matrix, gensym("lock"),out_savelock);
jit_object_method(in_matrix, gensym("lock"),in_savelock);
return err;
}
t_jit_err cv_jit_opticalflow_matrix_calc(t_cv_jit_opticalflow *x, void *inputs, void *outputs)
{
t_jit_err err=JIT_ERR_NONE;
long in_savelock=0,out_savelockX=0,out_savelockY=0;
t_jit_matrix_info in_minfo,out_minfoX,out_minfoY;
void *in_matrix,*out_matrixX,*out_matrixY;
CvMat inmat;
CvMat xmat, ymat;
in_matrix = jit_object_method(inputs,_jit_sym_getindex,0);
out_matrixX = jit_object_method(outputs,_jit_sym_getindex,0);
out_matrixY = jit_object_method(outputs,_jit_sym_getindex,1);
if (x&&in_matrix&&out_matrixX&&out_matrixY) {
in_savelock = (long) jit_object_method(in_matrix,_jit_sym_lock,1);
out_savelockX = (long) jit_object_method(out_matrixX,_jit_sym_lock,1);
out_savelockY = (long) jit_object_method(out_matrixY,_jit_sym_lock,1);
jit_object_method(in_matrix,_jit_sym_getinfo,&in_minfo);
jit_object_method(out_matrixX,_jit_sym_getinfo,&out_minfoX);
jit_object_method(out_matrixY,_jit_sym_getinfo,&out_minfoY);
if (in_minfo.type != _jit_sym_char)
{
err=JIT_ERR_MISMATCH_TYPE;
goto out;
}
//compatible planes?
if (in_minfo.planecount!=1) {
err=JIT_ERR_MISMATCH_PLANE;
goto out;
}
if (in_minfo.dimcount!=2) {
err=JIT_ERR_MISMATCH_DIM;
goto out;
}
//Convert matrix header
cvJitter2CvMat(in_matrix, &inmat);
//calculate
x->of->compute(&inmat);
xmat = (CvMat)x->of->getXflow();
ymat = (CvMat)x->of->getYflow();
//Copy to output
cvMat2Jitter(&xmat, out_matrixX);
cvMat2Jitter(&ymat, out_matrixY);
} else {
return JIT_ERR_INVALID_PTR;
}
out:
jit_object_method(out_matrixX,gensym("lock"),out_savelockX);
jit_object_method(out_matrixY,gensym("lock"),out_savelockY);
jit_object_method(in_matrix,gensym("lock"),in_savelock);
return err;
}
t_jit_err cv_jit_lines_matrix_calc(t_cv_jit_lines *x, void *inputs, void *outputs)
{
t_jit_err err = JIT_ERR_NONE;
long in_savelock,out_savelock;
t_jit_matrix_info in_minfo,out_minfo;
char *in_bp,*out_bp;
long i,dimcount,dim[JIT_MATRIX_MAX_DIMCOUNT];
void *in_matrix,*out_matrix;
t_int32 *out;
double thresh1, thresh2, theta, rho;
int houghThresh;
CvMat source;
CvPoint *ln;
in_matrix = jit_object_method(inputs,_jit_sym_getindex,0);
out_matrix = jit_object_method(outputs,_jit_sym_getindex,0);
if (x&&in_matrix&&out_matrix) {
in_savelock = (long) jit_object_method(in_matrix,_jit_sym_lock,1);
out_savelock = (long) jit_object_method(out_matrix,_jit_sym_lock,1);
jit_object_method(in_matrix,_jit_sym_getinfo,&in_minfo);
jit_object_method(out_matrix,_jit_sym_getinfo,&out_minfo);
jit_object_method(in_matrix,_jit_sym_getdata,&in_bp);
if (!in_bp) { err=JIT_ERR_INVALID_INPUT; goto out;}
//compatible types?
if (in_minfo.type!=_jit_sym_char) {
err=JIT_ERR_MISMATCH_TYPE;
goto out;
}
//compatible planes?
if ((in_minfo.planecount!=1)||(out_minfo.planecount!=4)) {
err=JIT_ERR_MISMATCH_PLANE;
goto out;
}
//get dimensions/planecount
dimcount = in_minfo.dimcount;
for (i=0;i<dimcount;i++) {
dim[i] = MIN(in_minfo.dim[i],out_minfo.dim[i]);
}
//Convert input matrix to OpenCV matrices
cvJitter2CvMat(in_matrix, &source);
//Adjust size of edge matrix if need be
if((x->edges->rows != source.rows)||(x->edges->cols != source.cols))
{
cvReleaseMat(&(x->edges));
x->edges = cvCreateMat( source.rows, source.cols, CV_8UC1 );
}
//Calculate parameter values for Hough and Canny algorithms
thresh1 = x->threshold - THRESHOLD_RANGE;
thresh2 = x->threshold + THRESHOLD_RANGE;
CLIP_ASSIGN(thresh1,0,255);
CLIP_ASSIGN(thresh2,0,255);
theta = CV_PI / (180 / (double)x->resolution);
rho = (double)x->resolution;
houghThresh = x->sensitivity;
x->gap = MAX(0,x->gap);
x->length = MAX(0,x->length);
//calculate edges using Canny algorithm
cvCanny( &source, x->edges, thresh1, thresh2, 3 );
//Find lines using the probabilistic Hough transform method
x->lines = cvHoughLines2( x->edges, x->storage, CV_HOUGH_PROBABILISTIC, rho, theta, houghThresh, x->length, x->gap );
//Transfer line information to output matrix
//First adjust matrix size
out_minfo.dim[0] = x->lines->total;
jit_object_method(out_matrix,_jit_sym_setinfo,&out_minfo);
jit_object_method(out_matrix,_jit_sym_getinfo,&out_minfo);
jit_object_method(out_matrix,_jit_sym_getdata,&out_bp);
if (!out_bp) { err=JIT_ERR_INVALID_OUTPUT; goto out;}
//Copy...
out = (t_int32 *)out_bp;
for( i = 0; i < x->lines->total; i++ )
{
ln = (CvPoint*)cvGetSeqElem(x->lines,i);
out[0] = ln[0].x;
out[1] = ln[0].y;
out[2] = ln[1].x;
out[3] = ln[1].y;
out+=4;
}
} else {
return JIT_ERR_INVALID_PTR;
}
out:
jit_object_method(out_matrix,gensym("lock"),out_savelock);
jit_object_method(in_matrix,gensym("lock"),in_savelock);
return err;
}
t_jit_err cv_jit_features_matrix_calc(t_cv_jit_features *x, void *inputs, void *outputs)
{
t_jit_err err=JIT_ERR_NONE;
long in_savelock=0,out_savelock=0;
t_jit_matrix_info in_minfo,out_minfo;
char *out_bp, *in_bp;
void *in_matrix,*out_matrix;
int i;
int roi_w,roi_h,roi_offset;
float *out_data;
CvMat source;
int featureCount = 2048;
//Get pointers to matrices
in_matrix = jit_object_method(inputs,_jit_sym_getindex,0);
out_matrix = jit_object_method(outputs,_jit_sym_getindex,0);
if (x&&in_matrix&&out_matrix)
{
//Lock the matrices
in_savelock = (long) jit_object_method(in_matrix,_jit_sym_lock,1);
out_savelock = (long) jit_object_method(out_matrix,_jit_sym_lock,1);
//Make sure input is of proper format
jit_object_method(in_matrix,_jit_sym_getinfo,&in_minfo);
jit_object_method(out_matrix,_jit_sym_getinfo,&out_minfo);
if(in_minfo.dimcount != 2)
{
err = JIT_ERR_MISMATCH_DIM;
goto out;
}
if(in_minfo.planecount != 1)
{
err = JIT_ERR_MISMATCH_PLANE;
goto out;
}
if(in_minfo.type != _jit_sym_char)
{
err = JIT_ERR_MISMATCH_TYPE;
goto out;
}
//Don't process if one dimension is < 2
if((in_minfo.dim[0] < 2)||(in_minfo.dim[1] < 2))
goto out;
if(x->useroi)
{
CLIP_ASSIGN(x->roi[0],0,in_minfo.dim[0]);
CLIP_ASSIGN(x->roi[1],0,in_minfo.dim[1]);
CLIP_ASSIGN(x->roi[2],0,in_minfo.dim[0]);
CLIP_ASSIGN(x->roi[3],0,in_minfo.dim[1]);
x->roi[0] = MIN(x->roi[0], x->roi[2]);
x->roi[1] = MIN(x->roi[1], x->roi[3]);
x->roi[2] = MAX(x->roi[0], x->roi[2]);
x->roi[3] = MAX(x->roi[1], x->roi[3]);
roi_w = x->roi[2] - x->roi[0];
roi_h = x->roi[3] - x->roi[1];
if(roi_w == 0)
roi_w = in_minfo.dim[0] - x->roi[0];
if(roi_h == 0)
roi_h = in_minfo.dim[1] - x->roi[1];
roi_offset = x->roi[1] * in_minfo.dimstride[1] + x->roi[0];
jit_object_method(in_matrix,_jit_sym_getdata,&in_bp);
//Convert Jitter matrix to OpenCV matrix
cvInitMatHeader( &source, roi_h, roi_w, CV_8UC1, in_bp + roi_offset, in_minfo.dimstride[1] );
}
else
{
//Convert Jitter matrix to OpenCV matrix
cvJitter2CvMat(in_matrix, &source);
}
//Adjust the size of eigImage and tempImage if need be
if((source.cols != x->eigImage->cols)||(source.rows != x->eigImage->rows))
{
cvReleaseMat(&(x->eigImage));
x->eigImage = cvCreateMat( source.rows, source.cols, CV_32FC1 );
cvReleaseMat(&(x->tempImage));
x->tempImage = cvCreateMat( source.rows, source.cols, CV_32FC1 );
}
//Adjust parameters
x->threshold = MAX(0.001,x->threshold);
x->distance = MAX(1,x->distance);
//Calculate
cvGoodFeaturesToTrack( &source, x->eigImage, x->tempImage,x->features, &featureCount,x->threshold, x->distance,NULL, x->aperture,0, 0.04 );
if(x->precision == 1){
int minsize = (x->aperture*2)+5;
//Error check for cvFindCornerSubPix
if((featureCount>0)&&(source.cols > minsize)&&(source.rows > minsize))
cvFindCornerSubPix( &source, x->features, featureCount, cvSize(x->aperture,x->aperture),cvSize(-1,-1),cvTermCriteria(CV_TERMCRIT_ITER, 10, 0.1f));
}
//Prepare output
out_minfo.dim[0] = featureCount;
jit_object_method(out_matrix,_jit_sym_setinfo,&out_minfo);
jit_object_method(out_matrix,_jit_sym_getinfo,&out_minfo);
jit_object_method(out_matrix,_jit_sym_getdata,&out_bp);
if (!out_bp) { err=JIT_ERR_INVALID_OUTPUT; goto out;}
out_data = (float *)out_bp;
if(x->useroi)
{
for(i=0; i < featureCount; i++)
{
out_data[0] = x->features[i].x + x->roi[0];
out_data[1] = x->features[i].y + x->roi[1];
out_data += 2;
}
}
else
{
for(i=0; i < featureCount; i++)
{
out_data[0] = x->features[i].x;
out_data[1] = x->features[i].y;
out_data += 2;
}
}
}
out:
jit_object_method(out_matrix,gensym("lock"),out_savelock);
jit_object_method(in_matrix,gensym("lock"),in_savelock);
return err;
}
t_jit_err cv_jit_calibration_matrix_calc(t_cv_jit_calibration *x, void *inputs, void *outputs)
{
t_jit_err err = JIT_ERR_NONE;
long in_savelock;
long out_savelock;
t_jit_matrix_info in_minfo;
t_jit_matrix_info out_minfo;
char *in_bp;
char *out_bp;
void *in_matrix;
void *out_matrix;
CvMat in_cv,out_cv;
in_matrix = jit_object_method(inputs,_jit_sym_getindex,0);
out_matrix = jit_object_method(outputs,_jit_sym_getindex,0);
if (x && in_matrix && out_matrix) {
in_savelock = (long) jit_object_method(in_matrix, _jit_sym_lock, 1);
out_savelock = (long) jit_object_method(out_matrix, _jit_sym_lock, 1);
jit_object_method(in_matrix, _jit_sym_getinfo, &in_minfo);
jit_object_method(out_matrix, _jit_sym_getinfo, &out_minfo);
jit_object_method(in_matrix, _jit_sym_getdata, &in_bp);
jit_object_method(out_matrix, _jit_sym_getdata, &out_bp);
if (!in_bp) {
err=JIT_ERR_INVALID_INPUT;
goto out;
}
if (!out_bp) {
err=JIT_ERR_INVALID_OUTPUT;
goto out;
}
if (in_minfo.type != _jit_sym_char) {
err = JIT_ERR_MISMATCH_TYPE;
goto out;
}
if (in_minfo.planecount != 4 && in_minfo.planecount != 1) {
err=JIT_ERR_MISMATCH_PLANE;
goto out;
}
if (in_minfo.dimcount != 2) {
err=JIT_ERR_MISMATCH_DIM;
goto out;
}
if ( x->dim[0] != in_minfo.dim[0] || x->dim[1] != in_minfo.dim[1]) {
x->dim[0] = in_minfo.dim[0];
x->dim[1] = in_minfo.dim[1];
cv_jit_calibration_load_param(x);
}
cvJitter2CvMat(in_matrix, &in_cv); // convert Jitter matrix into CvMat
cvJitter2CvMat(out_matrix, &out_cv);
// this will loop until we got enought views (x->board_view_nb) with all corners visible
if ( x->success_count < x->board_view_nb && x->calibration != 0 ) {
cv_jit_calibration_findcorners(x, in_minfo, out_minfo, in_matrix, out_matrix, in_cv, out_bp );
}
if ( x->success_count >= x->board_view_nb && x->calibration != 0 ) {
//CALIBRATE THE CAMERA!
cvCalibrateCamera2(x->object_points,
x->image_points,
x->point_counts,
cvSize( in_minfo.dim[0], in_minfo.dim[1] ),
x->intrinsic_matrix,
x->distortion_coeffs,
NULL,
NULL,
0);
//cv_jit_calibration_print_parameters(x);
cv_jit_calibration_build_undistort_map(x);
x->calibration = 0;
}
if (x->calibration == 0) {
// undistort the input image
cvRemap( &in_cv, &out_cv, x->mapx, x->mapy ); // Undistort image
}
}
else
return JIT_ERR_INVALID_PTR;
out:
jit_object_method(out_matrix,_jit_sym_lock,out_savelock);
jit_object_method(in_matrix,_jit_sym_lock,in_savelock);
return err;
}
