void Lane(IplImage* src, IplImage* dst)
{
int startR;
int startC;
int curR;
int curC;
bool found = false;
int posR[] = { -1, -1, -1, 0, 1, 1, 1, 0 };
int posC[] = { -1, 0, 1, 1, 1, 0, -1, -1 };
int prevP = 0;
cvSet(dst, cvScalar(255));
for (int r = 0; r < src->height; ++r)
{
for (int c = 0; c < src->width; ++c)
{
if (*cvPtr2D(src, r, c) == 0)
{
*cvPtr2D(dst, r, c) = 0;
startR = r;
startC = c;
curR = r;
curC = c;
found = true;
break;
}
}
if (found)
break;
}
do
{
for (int p = 0; p < 8; ++p)
{
int curP = (p + prevP) % 8;
if (*cvPtr2D(src, curR + posR[curP], curC + posC[curP]) == 0)
{
curR += posR[curP];
curC += posC[curP];
*cvPtr2D(dst, curR, curC) = 0;
prevP = (curP + 5) % 8;
break;
}
}
} while (curR != startR || curC != startC);
}
DOUBLEVECT HoughAccumulator::FindBest()
{
DOUBLEVECT v;
CvMat temp;
CvMat* locMat = cvGetMat(acc, &temp, NULL, 1);
// int rowsize = 4 * ((acc->dims / 4) +
// ((acc->dims % 4 > 0) ? 1 : 0));
double max_val;
CvPoint max_loc;
cvMinMaxLoc(locMat, NULL, &max_val, NULL, &max_loc, NULL);
int indraw = max_loc.x + max_loc.y * locMat->step;
uchar* pValue = cvPtr2D(locMat, max_loc.y, max_loc.x);
if (*pValue < 10)
return v;
indices[0] = indraw / acc->dim[0].step;
indices[acc->dims - 1] = indraw % acc->dim[acc->dims - 2].step;
for (int i = 1; i < acc->dims - 1; i ++)
indices[i] = (indraw % acc->dim[i - 1].step) / acc->dim[i].step;
for (int j = 0; j < acc->dims; j++)
{
double d = indices[j] / (float)precision + paramRanges[j].min;
v.push_back(d);
}
return v;
}
int main()
{
// 创建一个三通道RGB图像
IplImage* image = cvCreateImage(cvSize(IMG_WIDTH, IMG_HEIGHT), 8, 3 );
// 将矩阵元素全部置零
cvZero(image);
uchar* p;
for(int i = 0; i < image->height; i++)
{
for(int j = 0; j < image->width; j++)
{
p = cvPtr2D(image, i, j, 0);
if(isThePointInRectangle(i, j) == true)
{
*(++p) = 255;
}
}
}
cvNamedWindow("img", 1);
cvShowImage("img", image);
cvWaitKey(0);
cvReleaseImage(&image);
cvDestroyWindow("img");
return 0;
}
void HarrisBuffer::OpticalFlowFromLK()
{
//cvCalcOpticalFlowLK(prevgray, gray, cvSize(15,15), OFx, OFy);
//cvCalcOpticalFlowHS(prevgray, gray, 0, OFx, OFy, 0.1, cvTermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS,100,1e5));
float subf=5;
int xsz=gray->width, ysz=gray->height;
int pxn=int(xsz/subf), pyn=int(ysz/subf);
CvPoint2D32f *p1 = new CvPoint2D32f[pxn*pyn];
CvPoint2D32f *p2 = new CvPoint2D32f[pxn*pyn];
for (int i=0; i<pyn; i++)
for (int j=0; j<pxn; j++){
p1[i*pxn+j]=cvPoint2D32f(j*subf,i*subf);
p2[i*pxn+j]=cvPoint2D32f(j*subf,i*subf);
}
char *sts = new char[pxn*pyn];
CvTermCriteria termination = cvTermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 100, 1e5);
cvCalcOpticalFlowPyrLK(prevgray, gray, NULL, NULL,
p1, p2, int(pxn*pyn), cvSize(int(10),int(10)),
3,sts,NULL,termination,CV_LKFLOW_INITIAL_GUESSES);
IplImage* OFxsub= cvCreateImage(cvSize(pxn,pyn),IMGTYPE,1);
IplImage* OFysub= cvCreateImage(cvSize(pxn,pyn),IMGTYPE,1);
IMG_ELEM_TYPE *ptrOFxsub=(IMG_ELEM_TYPE*)cvPtr2D(OFxsub,0,0);
IMG_ELEM_TYPE *ptrOFysub=(IMG_ELEM_TYPE*)cvPtr2D(OFysub,0,0);
for (int i=0; i<pyn; i++)
for (int j=0; j<pxn; j++){
ptrOFxsub[i*pxn+j]=p2[i*pxn+j].x-p1[i*pxn+j].x;
ptrOFysub[i*pxn+j]=p2[i*pxn+j].y-p1[i*pxn+j].y;
}
cvResize(OFxsub,OFx,CV_INTER_NN);
cvResize(OFysub,OFy,CV_INTER_NN);
cvReleaseImage(&OFxsub);
cvReleaseImage(&OFysub);
}
void OpticalFlow::GetPixel(IplImage* rgbImage, int x, int y, ColorBGR** color)
{
ASSERT(rgbImage->nChannels==3);
*color = (ColorBGR*) cvPtr2D(rgbImage, y, x);
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
int feature_dim = mxGetPr(prhs[3])[0];
int img_h = mxGetM(prhs[0]);
int img_w = mxGetN(prhs[0])/feature_dim;
int N = img_h*img_w;
int K = mxGetPr(prhs[2])[0];
int dims[2] = {N, N};
CvSparseMat* affinityMatrix = cvCreateSparseMat(2, dims, CV_32FC1);
double *nMap = mxGetPr(prhs[0]); // normal Map
double *vMap = mxGetPr(prhs[1]); // variance Map
int g_size = mxGetPr(prhs[4])[0];
int ngrid_w = ceil(img_w / (float)g_size);
int ngrid_h = ceil(img_h / (float)g_size);
int Ngrid = ngrid_w * ngrid_h;
int *x_pos = new int[Ngrid];
int *y_pos = new int[Ngrid];
cv::Mat1f X(Ngrid, feature_dim);
for(int j=0, n=0;j<img_h;j+=g_size) { // grid iteration
for(int i=0;i<img_w;i+=g_size) {
double vmin = 99999;
for(int gj=0;gj<g_size && j+gj < img_h;gj++) { // y pos in grid
for(int gi=0;gi<g_size && i+gi < img_w;gi++) { // x pos
double var = vMap[(i+gi)*img_h + (j+gj)];
if(var < vmin) {
vmin = var;
x_pos[n] = (i+gi);
y_pos[n] = (j+gj);
}
}
}
for(int k=0;k<feature_dim;k++)
X(n, k) = nMap[k*img_h*img_w+x_pos[n]*img_h+y_pos[n]];
n++;
}
}
cv::Mat1f W(Ngrid, K);
cv::Mat1i neighbors(Ngrid, K);
LLE(X, W, neighbors, Ngrid, feature_dim, K);
//plhs[0] = mxCreateDoubleMatrix(img_w, img_h, mxREAL);
plhs[0] = mxCreateDoubleMatrix(Ngrid, K+1, mxREAL);
double *neighborPixels = mxGetPr(plhs[0]);
for(int n=0;n<Ngrid;n++) {
int xp = x_pos[n];
int yp = y_pos[n];
//int p = yp * img_w + xp;
int p = xp * img_h + yp;
neighborPixels[n] = p + 1;
for(int k=0;k<K;k++) {
if(W(n, k) != 0) {
int nIdx = neighbors(n, k);
if(nIdx >= 0) {
int xq = x_pos[nIdx];
int yq = y_pos[nIdx];
//int q = yq * img_w + xq;
int q = xq * img_h + yq;
((float*)cvPtr2D(affinityMatrix, p, q))[0] = W(n, k);
neighborPixels[(k+1)*Ngrid + n] = q + 1;
}
}
}
}
pushSparseMatrix(affinityMatrix, "LLENORMAL");
cvReleaseSparseMat(&affinityMatrix);
delete [] x_pos;
delete [] y_pos;
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
int feature_dim = mxGetPr(prhs[5])[0];
int img_h = mxGetM(prhs[0]); //imgHeight
int img_w = mxGetN(prhs[0])/feature_dim; //imgWidth
int N = img_h*img_w;
int K = mxGetPr(prhs[4])[0]; //k-NN
int dims[2] = {N, N};
CvSparseMat* affinityMatrix = cvCreateSparseMat(2, dims, CV_32FC1);
double *fMap = mxGetPr(prhs[0]); // feature Map
double *rIdx = mxGetPr(prhs[1]); // min vMap rIdx
double *cIdx = mxGetPr(prhs[2]); // min vMap cIdx
double *gbox = mxGetPr(prhs[3]); //global proposals
int gbox_num = mxGetM(prhs[2]);
mexPrintf("img_h=%d img_w=%d N=%d K=%d gbox_num=%d\n",img_h, img_w, N, K, gbox_num);
int *c_pos = new int[gbox_num];
int *r_pos = new int[gbox_num];
cv::Mat1f X(gbox_num, feature_dim);
double tol_d = mxGetPr(prhs[6])[0]; //LLE tolerance
float tol;
tol = (float)tol_d;
mexPrintf("tol=%f \n",tol);
for(int b=0; b<gbox_num; b++){ //gbox iteration
int cmin = gbox[(0 + b*4)];
int rmin = gbox[(1 + b*4)];
int cmax = gbox[(2 + b*4)];
int rmax = gbox[(3 + b*4)];
c_pos[b] = int(rIdx[b]);
r_pos[b] = int(cIdx[b]);
// mexPrintf("b=%d\n",b);
for(int k=0; k<feature_dim; k++)
{
//X(b,k) = fMap[ k*img_h*img_w + r_pos[b]*img_h + c_pos[b] ];
X(b,k) = fMap[ k*img_h*img_w + r_pos[b]*img_h + c_pos[b] ];
}
}
// cv::FileStorage fileX("X.yml", cv::FileStorage::WRITE);
// fileX << "X" << X;
//mexPrintf("%f %f %f %f %f %f %f %f %f %f \n",X(3,0),X(3,1),X(3,2),X(3,3),X(3,4),X(3,5),X(3,6),X(3,7),X(3,8),X(3,9));
// mexPrintf("Checkpoint 1\n");
cv::Mat1f W(gbox_num, K);
cv::Mat1i neighbors(gbox_num, K);
LLE(X, W, neighbors, gbox_num, feature_dim, tol, K);
// cv::FileStorage fileW("W.yml", cv::FileStorage::WRITE);
// fileX << "W" << W;
// mexPrintf("Checkpoint 2\n");
plhs[0] = mxCreateDoubleMatrix(gbox_num, K+1, mxREAL);
double *neighborPixels = mxGetPr(plhs[0]);
for(int n=0;n<gbox_num;n++) {
int xp = r_pos[n];
int yp = c_pos[n];
int p = xp * img_h + yp;
neighborPixels[n] = p + 1;
for(int k=0;k<K;k++) {
if(W(n, k) != 0) {
int nIdx = neighbors(n, k);
if(nIdx >= 0) {
int xq = r_pos[nIdx];
int yq = c_pos[nIdx];
//int q = yq * img_w + xq;
int q = xq * img_h + yq;
((float*)cvPtr2D(affinityMatrix, p, q))[0] = W(n, k);
neighborPixels[(k+1)*gbox_num + n] = q + 1;
}
}
}
}
pushSparseMatrix(affinityMatrix, "LLEGLOBAL");
cvReleaseSparseMat(&affinityMatrix);
// mexPrintf("Checkpoint 3\n");
delete [] c_pos;
delete [] r_pos;
}
void Beatle(IplImage* src, IplImage* dst)
{
enum Direction
{
up,
down,
left,
right
};
int startR;
int startC;
int curR;
int curC;
Direction dir = up;
bool found = false;
cvSet(dst, cvScalar(255));
for (int r = 0; r < src->height; ++r)
{
for (int c = 0; c < src->width; ++c)
{
if (*cvPtr2D(src, r, c) == 0)
{
*cvPtr2D(dst, r, c) = 0;
startR = r;
startC = c;
curR = r;
curC = c;
found = true;
break;
}
}
if (found)
break;
}
do
{
switch (dir)
{
case up:
--curR;
if (*cvPtr2D(src, curR, curC) == 255)
{
*cvPtr2D(dst, curR, curC) = 0;
dir = right;
}
else
dir = left;
break;
case down:
++curR;
if (*cvPtr2D(src, curR, curC) == 255)
{
*cvPtr2D(dst, curR, curC) = 0;
dir = left;
}
else
dir = right;
break;
case left:
--curC;
if (*cvPtr2D(src, curR, curC) == 255)
{
*cvPtr2D(dst, curR, curC) = 0;
dir = up;
}
else
dir = down;
break;
case right:
++curC;
if (*cvPtr2D(src, curR, curC) == 255)
{
*cvPtr2D(dst, curR, curC) = 0;
dir = down;
}
else
dir = up;
}
} while (curR != startR || curC != startC);
}
int main(int argc, char ** argv)
{
IplImage* image = cvLoadImage("1.png");
IplImage* gray = cvCreateImage(cvGetSize(image), 8, 1);
IplImage* bin = cvCreateImage(cvGetSize(image), 8, 1);
IplImage* contourBeatle = cvCreateImage(cvGetSize(image), 8, 1);
IplImage* contourScan = cvCreateImage(cvGetSize(image), 8, 1);
IplImage* contourLane = cvCreateImage(cvGetSize(image), 8, 1);
IplImage* rgbBeatle = cvCloneImage(image);
IplImage* rgbScan = cvCloneImage(image);
IplImage* rgbLane = cvCloneImage(image);
cvCvtColor(image, gray, CV_BGR2GRAY);
cvThreshold(gray, bin, 0, 255, CV_THRESH_OTSU);
Beatle(bin, contourBeatle);
Scan(bin, contourScan);
Lane(bin, contourLane);
for (int r = 0; r < image->height; ++r)
{
for (int c = 0; c < image->width; ++c)
{
if (*cvPtr2D(contourBeatle, r, c) == 0)
{
*cvPtr2D(rgbBeatle, r, c) = 0; // B
*(cvPtr2D(rgbBeatle, r, c) + 1) = 0; // G
*(cvPtr2D(rgbBeatle, r, c) + 2) = 0; // R
}
if (*cvPtr2D(contourScan, r, c) == 0)
{
*cvPtr2D(rgbScan, r, c) = 0; // B
*(cvPtr2D(rgbScan, r, c) + 1) = 0; // G
*(cvPtr2D(rgbScan, r, c) + 2) = 0; // R
}
if (*cvPtr2D(contourLane, r, c) == 0)
{
*cvPtr2D(rgbLane, r, c) = 0; // B
*(cvPtr2D(rgbLane, r, c) + 1) = 0; // G
*(cvPtr2D(rgbLane, r, c) + 2) = 0; // R
}
}
}
cvShowImage("image", image);
cvShowImage("gray", gray);
cvShowImage("bin", bin);
cvShowImage("contourBeatle", contourBeatle);
cvShowImage("contourScan", contourScan);
cvShowImage("contourLane", contourLane);
cvShowImage("rgbBeatle", rgbBeatle);
cvShowImage("rgbScan", rgbScan);
cvShowImage("rgbLane", rgbLane);
cvSaveImage("im_contourBeatle.bmp", contourBeatle);
cvSaveImage("im_contourScan.bmp", contourScan);
cvSaveImage("im_contourLane.bmp", contourLane);
cvSaveImage("im_rgbBeatle.bmp", rgbBeatle);
cvSaveImage("im_rgbScan.bmp", rgbScan);
cvSaveImage("im_rgbLane.bmp", rgbLane);
while (true)
{
int c = cvWaitKey();
if ((char)c == 27)
break;
}
}
void Scan(IplImage* src, IplImage* dst)
{
cvSet(dst, cvScalar(255));
for (int r = 0; r < src->height; ++r)
{
for (int c = 0; c < src->width - 1; ++c)
{
if (*cvPtr2D(src, r, c) == 255 && *cvPtr2D(src, r, c + 1) == 0)
*cvPtr2D(dst, r, c + 1) = 0;
else if (*cvPtr2D(src, r, c) == 0 && *cvPtr2D(src, r, c + 1) == 255)
*cvPtr2D(dst, r, c) = 0;
}
}
for (int c = 0; c < src->width; ++c)
{
for (int r = 0; r < src->height - 1; ++r)
{
if (*cvPtr2D(src, r, c) == 255 && *cvPtr2D(src, r + 1, c) == 0)
*cvPtr2D(dst, r + 1, c) = 0;
else if (*cvPtr2D(src, r, c) == 0 && *cvPtr2D(src, r + 1, c) == 255)
*cvPtr2D(dst, r, c) = 0;
}
}
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
int h = mxGetM(prhs[0]);
int w = mxGetN(prhs[0])/3;
int i, j, k;
int nx[] = {0, 0, 1, -1, -1, 1, 1, -1};
int ny[] = {1, -1, 0, 0, -1, 1, -1, 1};
double cp[3], cq[3];
double *chroma = mxGetPr(prhs[0]);
double dist;
plhs[0] = mxCreateDoubleMatrix(h*w, 1, mxREAL);
plhs[1] = mxCreateDoubleMatrix(h, w, mxREAL);
int dims[2] = {h*w, h*w};
CvSparseMat* m_refConsMat = cvCreateSparseMat(2, dims, CV_32FC1);
float sig_c= mxGetPr(prhs[1])[0];
float sig_i= mxGetPr(prhs[2])[0];
double *image = mxGetPr(prhs[3]);
double ip[3], iq[3];
double lp, lq;
for(j=0; j<h; j++)
{
for(i=0; i<w; i++)
{
int p = i*h+j;
cp[0] = chroma[i*h+j];
cp[1] = chroma[h*w+i*h+j];
cp[2] = chroma[2*h*w+i*h+j];
ip[0] = image[i*h+j];
ip[1] = image[h*w+i*h+j];
ip[2] = image[2*h*w+i*h+j];
lp = log(MAX(sqrt(ip[0]*ip[0] + ip[1]*ip[1] + ip[2]*ip[2]), 0.0001));
for(k=0; k<8; k++)
{
int qi = i + nx[k];
int qj = j + ny[k];
int q = qi*h+qj;
if(qi < 0 || qj < 0 || qi >= w || qj >= h)
{
continue;
}
cq[0] = chroma[qi*h+qj];
cq[1] = chroma[h*w+qi*h+qj];
cq[2] = chroma[2*h*w+qi*h+qj];
iq[0] = image[qi*h+qj];
iq[1] = image[h*w+qi*h+qj];
iq[2] = image[2*h*w+qi*h+qj];
lq = log(MAX(sqrt(iq[0]*iq[0] + iq[1]*iq[1] + iq[2]*iq[2]), 0.0001));
dist = 2.0 * (1.0 - (cp[0]*cq[0]+cp[1]*cq[1]+cp[2]*cq[2]));
float weight = (1 + exp(-exp(lp) * exp(lp) / (sig_i*sig_i) - exp(lq)*exp(lq) / (sig_i*sig_i)));
weight = weight * (exp(-dist*dist/(sig_c*sig_c)));
if(k == 2)
mxGetPr(plhs[1])[p] = weight;
if(_isnan(weight)) weight = 0;
((float*)cvPtr2D(m_refConsMat, p, p))[0] += weight;
((float*)cvPtr2D(m_refConsMat, q, q))[0] += weight;
((float*)cvPtr2D(m_refConsMat, p, q))[0] += -weight;
((float*)cvPtr2D(m_refConsMat, q, p))[0] += -weight;
float dI = lp - lq;
mxGetPr(plhs[0])[p] += weight * dI;
mxGetPr(plhs[0])[q] -= weight * dI;
}
}
}
pushSparseMatrix(m_refConsMat, "WRC");
cvReleaseSparseMat(&m_refConsMat);
}
void CV_QueryHistTest::run_func(void)
{
int i, iters = values->cols;
CvArr* h = hist[0]->bins;
const int* idx = indices->data.i;
float* val = values->data.fl;
float default_value = 0.f;
// stage 1: write bins
if( cdims == 1 )
for( i = 0; i < iters; i++ )
{
float v0 = values0->data.fl[i];
if( fabs(v0 - default_value) < FLT_EPSILON )
continue;
if( !(i % 2) )
{
if( !(i % 4) )
cvSetReal1D( h, idx[i], v0 );
else
*(float*)cvPtr1D( h, idx[i] ) = v0;
}
else
cvSetRealND( h, idx+i, v0 );
}
else if( cdims == 2 )
for( i = 0; i < iters; i++ )
{
float v0 = values0->data.fl[i];
if( fabs(v0 - default_value) < FLT_EPSILON )
continue;
if( !(i % 2) )
{
if( !(i % 4) )
cvSetReal2D( h, idx[i*2], idx[i*2+1], v0 );
else
*(float*)cvPtr2D( h, idx[i*2], idx[i*2+1] ) = v0;
}
else
cvSetRealND( h, idx+i*2, v0 );
}
else if( cdims == 3 )
for( i = 0; i < iters; i++ )
{
float v0 = values0->data.fl[i];
if( fabs(v0 - default_value) < FLT_EPSILON )
continue;
if( !(i % 2) )
{
if( !(i % 4) )
cvSetReal3D( h, idx[i*3], idx[i*3+1], idx[i*3+2], v0 );
else
*(float*)cvPtr3D( h, idx[i*3], idx[i*3+1], idx[i*3+2] ) = v0;
}
else
cvSetRealND( h, idx+i*3, v0 );
}
else
for( i = 0; i < iters; i++ )
{
float v0 = values0->data.fl[i];
if( fabs(v0 - default_value) < FLT_EPSILON )
continue;
if( !(i % 2) )
cvSetRealND( h, idx+i*cdims, v0 );
else
*(float*)cvPtrND( h, idx+i*cdims ) = v0;
}
// stage 2: read bins
if( cdims == 1 )
for( i = 0; i < iters; i++ )
{
if( !(i % 2) )
val[i] = *(float*)cvPtr1D( h, idx[i] );
else
val[i] = (float)cvGetReal1D( h, idx[i] );
}
else if( cdims == 2 )
for( i = 0; i < iters; i++ )
{
if( !(i % 2) )
val[i] = *(float*)cvPtr2D( h, idx[i*2], idx[i*2+1] );
else
val[i] = (float)cvGetReal2D( h, idx[i*2], idx[i*2+1] );
}
else if( cdims == 3 )
for( i = 0; i < iters; i++ )
{
if( !(i % 2) )
val[i] = *(float*)cvPtr3D( h, idx[i*3], idx[i*3+1], idx[i*3+2] );
else
val[i] = (float)cvGetReal3D( h, idx[i*3], idx[i*3+1], idx[i*3+2] );
}
else
for( i = 0; i < iters; i++ )
{
if( !(i % 2) )
val[i] = *(float*)cvPtrND( h, idx+i*cdims );
else
val[i] = (float)cvGetRealND( h, idx+i*cdims );
}
}
