void BackgroundSubtractorKNNImpl::getBackgroundImage(OutputArray backgroundImage) const
{
int nchannels = CV_MAT_CN(frameType);
//CV_Assert( nchannels == 3 );
Mat meanBackground(frameSize, CV_8UC3, Scalar::all(0));
int ndata=nchannels+1;
int modelstep=(ndata * nN * 3);
const uchar* pbgmodel=bgmodel.ptr(0);
for(int row=0; row<meanBackground.rows; row++)
{
for(int col=0; col<meanBackground.cols; col++)
{
for (int n = 0; n < nN*3; n++)
{
const uchar* mean_m = &pbgmodel[n*ndata];
if (mean_m[nchannels])
{
meanBackground.at<Vec3b>(row, col) = Vec3b(mean_m);
break;
}
}
pbgmodel=pbgmodel+modelstep;
}
}
switch(CV_MAT_CN(frameType))
{
case 1:
{
std::vector<Mat> channels;
split(meanBackground, channels);
channels[0].copyTo(backgroundImage);
break;
}
case 3:
{
meanBackground.copyTo(backgroundImage);
break;
}
default:
CV_Error(Error::StsUnsupportedFormat, "");
}
}
int GateDetector::CountAndDrawWhitePixels(Mat& BWImage, Mat&outputImageToDraw, float top, float buttom, float left, float right)
{
int counter = 0;
for (size_t y = top; y < buttom; y++)
{
for (size_t x = left; x < right; x++)
{
if (BWImage.at<unsigned char>(y, x) > 100)
{
counter++;
outputImageToDraw.at<Vec3b>(y, x) = Vec3b(255,0,0);
}
}
}
return counter;
}
void HumanBodyLaser::Disp2Point(const Mat& disp, const Point2f& stPt, const Mat& im, const Mat& msk, const Mat& Qmtx, vector<Point3f>& points, vector<Vec3b>& colors, vector<Vec3f>& normals)
{
int height = disp.size().height;
int width = disp.size().width;
for(int y = 0; y < height; ++y)
{
uchar * pmsk = (uchar *)msk.ptr<uchar>(y);
float * pdsp = (float *)disp.ptr<float>(y);
for(int x = 0; x < width; ++x)
{
if(pmsk[x] == 0 || pdsp[x] == 0)
{
points.push_back(Point3f(PT_UNDEFINED, PT_UNDEFINED, PT_UNDEFINED));
colors.push_back(Vec3b(0,0,0));
normals.push_back(Vec3f(0,0,0));
}
else
{
Mat tvec(4,1,CV_64FC1);
tvec.at<double>(0,0) = stPt.x + x;
tvec.at<double>(1,0) = stPt.y + y;
tvec.at<double>(2,0) = pdsp[x];
tvec.at<double>(3,0) = 1.0f;
//cout << "Qmtx: " << Qmtx << endl;
//cout << "tvec: " << tvec << endl;
Mat hicoord = Qmtx * tvec;
//cout << "hicoord: " << hicoord << endl;
Point3f tpt;
tpt.x = hicoord.at<double>(0,0) / hicoord.at<double>(3,0);
tpt.y = hicoord.at<double>(1,0) / hicoord.at<double>(3,0);
tpt.z = hicoord.at<double>(2,0) / hicoord.at<double>(3,0);
Point3f tn = Point3f(0.f,0.f,0.f) - tpt;
points.push_back(tpt);
normals.push_back(Vec3f(tn.x, tn.y, tn.z));
colors.push_back(im.at<Vec3b>(y,x));
}
}
}
}
void HumanBodyLaser::refine3DPoints(vector<Point3f>& points, vector<Vec3b>& colors, int& validnum)
{
float maxdepth = MAXDEPTH;
float mindepth = MINDEPTH;
int size = points.size();
for(int i = 0; i < size; ++i)
{
if(points[i].z == PT_UNDEFINED) continue;
float z = points[i].z;
if(z < mindepth || z > maxdepth)
{
points[i] = Point3f(PT_UNDEFINED, PT_UNDEFINED, PT_UNDEFINED);
colors[i] = Vec3b(0,0,0);
validnum --;
}
}
}
/* Draws the heatmap on top of a frame. The frame must be the same size as
* the heatmap.
*/
void AttentionMap::overlay(unsigned char* pDestImage, int imageWidth, int imageHeight)
{
update();
// Make sure all values are capped at one
m_heatmap = min(m_ones, m_heatmap);
Mat temp_map;
blur(m_heatmap, temp_map, Size(15, 15));
for (int r = 0; r < m_heatmap.rows; ++r)
{
//Vec3b* f_ptr = (Vec3b *)pDestImage;
float* h_ptr = temp_map.ptr<float>(r);
for (int c = 0; c < m_heatmap.cols; ++c)
{
const float heat_mix = h_ptr[c];
if (heat_mix > 0.0)
{
// in BGR
const Vec3b i_color = Vec3b(pDestImage[0], pDestImage[1], pDestImage[2]);
const Vec3b heat_color =
hsv_to_bgr(interpolate_hsv(g_heat_color2, g_heat_color1, heat_mix));
const float heat_mix2 = std::min(heat_mix, g_max_transparency);
const Vec3b final_color = interpolate(i_color, heat_color, heat_mix2);
//f_ptr[c] = final_color;
pDestImage[0] = final_color[0];
pDestImage[1] = final_color[1];
pDestImage[2] = final_color[2];
}
pDestImage+=3;
}
pDestImage += (imageWidth - m_heatmap.cols) *3;
}
fade();
}
void Swt::displayStrokeWidth(vector< vector<cv::Point2i> >& cc)
{
cv::Mat show = cv::Mat::zeros(height,width,CV_8UC3);
cv::Vec3b point = Vec3b(0,0,0);
int ccsize = cc.size();
for(int i = 0 ;i< ccsize;i++)
{
int ccisize = cc[i].size();
for(int j = 0; j< ccisize;j++)
{
int iindex = cc[i][j].y,jindex = cc[i][j].x;
if(StrokeImage.at<float>(iindex,jindex) == 151)
StrokeImage.at<float>(iindex,jindex) = 0;
point[1] = int(StrokeImage.at<float>(iindex,jindex)*5);
show.at<cv::Vec3b>(iindex,jindex) = point;
}
}
cv::imshow(" inner Stroke width",show);
cv::waitKey(10);
}
void draw_circle(Mat& image, Point x,double alpha, bool flag, Vec2d scale)
{
int thickness = -1;
int lineType = 8;
int radius = 10;
Scalar color;
if(flag)
color = Scalar(0,0,255);
else
color = Scalar(0,255,0);
Mat img(image.rows, image.cols, CV_8UC3, Scalar(0,0,0));
circle(img, x * scale[0] * scale[1], radius * scale[1] , color, thickness, lineType);
for(int i = 0; i < image.rows; i++)
{
for(int j = 0; j < image.cols; j++)
if(img.at<Vec3b>(i,j) != Vec3b(0,0,0))
image.at<Vec3b>(i,j) = (1- alpha) * image.at<Vec3b>(i,j) + alpha * img.at<Vec3b>(i,j);
}
}
void TableObjectDetector::draw3DPointsIn2D(Mat img, const Mat P, KinectCalibParams* calib) {
Mat Pt; P.copyTo(Pt); transpose(Pt, Pt);
Mat K = calib->getRGBIntrinsics();
Mat R; (calib->getR()).copyTo(R); transpose(R, R);
Mat T = calib->getT();
// Apply extrinsics
Pt = R*Pt;
for (int i=0; i<Pt.cols; i++) {
Pt.col(i) = Pt.col(i)-T;
}
Mat Pim = K*Pt;
for (int i=0; i<Pim.cols; i++) {
double px = Pim.at<double>(0, i)/Pim.at<double>(2, i);
double py = Pim.at<double>(1, i)/Pim.at<double>(2, i);
img.at<Vec3b>(py, px) = Vec3b(0, 0, 255);
}
}
Mat InputStream::getRawMat()
{
int changedStreamDummy;
VideoStream* pStream = &depth;
rc_depth = OpenNI::waitForAnyStream(&pStream, 1, &changedStreamDummy, 1000);
if (rc_depth != STATUS_OK)
{
printf("Wait failed! (timeout is %d ms)\n%s\n", 1000, OpenNI::getExtendedError());
exit(0);
}
unsigned short data = 0;
frame = new VideoFrameRef;
rc_depth = depth.readFrame(frame);
int j = 0;
openni::DepthPixel* pDepth = (openni::DepthPixel*)frame->getData();
delete frame;
uint16_t max = 0;
for (int i = 0; i < width * height; i++)
if (raw[i] > max)
max = raw[i];
for (int y = 0; y < raw_mat.rows; y++)
for (int x = 0; x < raw_mat.cols; x++)
{
raw_mat.at<Vec3b>(Point(x,y)) = Vec3b(0, 0, uchar((float)raw[y * raw_mat.cols + x] * 255.0 / (float)max));
}
return raw_mat;
}
void draw_ui(Mat& a, Mat& b, Mat& dst)
{
Mat ra, rb;
resize(a, ra, Size(4 * rows, 4 * cols));
resize(b, rb, Size(4 * rows, 4 * cols));
dst = Mat::zeros(ra.rows + 20, ra.cols * 2, CV_8UC3);
Mat dst_roi_a = dst(Rect(0, 0, ra.cols, ra.rows));
ra.copyTo(dst_roi_a);
Mat dst_roi_b = dst(Rect(ra.cols, 0, ra.cols, ra.rows));
rb.copyTo(dst_roi_b);
if ( better != -1 ){
for ( int r = 0; r < 20; r++ ){
for ( int c = 0; c < 4 * cols; c++ ){
dst.at<Vec3b>(ra.rows + r, 4 * cols * (1 - better) + c) =
Vec3b(255, 0, 0);
}
}
}
}
TEST(colorHistDetectorTestO, Operators)
{
// Create frames
int framesCount = 4, rows = 30, cols = 40;
vector <Frame*> frames;
for (int i = 0; i < framesCount; i++)
{
Mat image = Mat(Size(cols, rows), CV_8UC3, Scalar(i, i, i));
Frame * frame = new Keyframe();
frames.push_back(frame);
frames[i]->setID(i);
frames[i]->setImage(image);
frames[i]->setMask(image);
image.release();
}
// Create empty skeleton
Skeleton skeleton;
tree<BodyPart> partTree;
tree<BodyJoint> jointTree;
skeleton.setPartTree(partTree);
skeleton.setJointTree(jointTree);
// Run "train" for setting frames in "chd"
ColorHistDetector chd0;
map <string, float> params;
chd0.train(frames, params);
frames.clear();
// Using operator "="
ColorHistDetector copyed_chd = chd0;
// Get "chd" frames and compare
vector <Frame*> actual_frames = copyed_chd.getFrames();
for (int i = 0; i < frames.size(); i++)
EXPECT_EQ(Vec3b(i, i, i), actual_frames[i]->getImage().at<Vec3b>(0, 0));
}
Vec3b HSV2RGB(float hue, float sat, float val)
{
float x, y, z;
if(hue == 1) hue = 0;
else hue *= 6;
int i = static_cast<int>(floorf(hue));
float f = hue - i;
float p = val * (1 - sat);
float q = val * (1 - (sat * f));
float t = val * (1 - (sat * (1 - f)));
switch(i)
{
case 0: x = val; y = t; z = p; break;
case 1: x = q; y = val; z = p; break;
case 2: x = p; y = val; z = t; break;
case 3: x = p; y = q; z = val; break;
case 4: x = t; y = p; z = val; break;
case 5: x = val; y = p; z = q; break;
}
return Vec3b((uchar)(x * 255), (uchar)(y * 255), (uchar)(z * 255));
}
void FillSimulation(vector <Mat> fillRegions, vector<Scalar> colorValue, vector<StrokeCluster> &fisrtDrawCluster){
float size = 0;
int gap = 15;
int lineWidth = 10;
//Filling regions
for (int i = 0; i < fillRegions.size(); i++) {
// Boundary initialization
int ys = 1;
int ye = fillRegions[i].rows - 1;
int xs = 1;
int xe = fillRegions[i].cols - 1;
Mat fillRgionBlack;
cvtColor(fillRegions[i], fillRgionBlack, CV_RGB2GRAY);
fillRgionBlack = fillRgionBlack > 245;
ofstream outputFile;
string fileName = outputFileName("drawPoints/fill", i, ".txt");
outputFile.open(fileName);
Point previousPoint;
Vec3b fillColor = Vec3b(colorValue[i][0], colorValue[i][1], colorValue[i][2]);
Vec4f cmyk;
rgb2cmyk(fillColor, cmyk);
// Write indexing of color
outputFile << (int)fillColor[0] << " " << (int)fillColor[1] << " " << (int)fillColor[2] << endl;
outputFile << (float)cmyk[0] << " " << (float)cmyk[1] << " " << (float)cmyk[2] <<" " << (float)cmyk[3] << endl;
// Find draw points
for (int j = ys; j <= ye; j = j + gap)
FindDrawPoints(j, xs, ys, xe, fillRgionBlack, outputFile, size, fillColor, cmyk, lineWidth, fisrtDrawCluster[i]);
// Last Row
for (int k = xs; k <= xe; k = k + gap)
FindDrawPoints(ye, k, ys, xe, fillRgionBlack, outputFile, size, fillColor, cmyk, lineWidth, fisrtDrawCluster[i]);
outputFile.close();
}
cout << "\nTotal Number of fill lines: " << fillLines << endl << endl;
}
void GraphMatching::depthTransfer(Mat& result, OneMatch& match, string depthDir){
SubGraph targ = match.targ;
SubGraph cadi = match.cadi;
//targ's srcImg have the same size of result
string section = match.cadi._centerNode._section;
string depthPath = depthDir + "\\" + section + ".png";
if(!fs::exists(depthPath)) depthPath = depthDir + "\\" + section + ".jpg";
int inferredDepth = cadi._centerNode._depth;
Mat depthMap = imread(depthPath,0);
for(int i=0;i<cadi._centerNode._contour.size();i++){
inferredDepth = depthMap.at<uchar>(cadi._centerNode._contour[i].x,cadi._centerNode._contour[i].y);
}
for(int i=0;i<targ._centerNode._contour.size();i++){
if(result.channels() == 1)
result.at<uchar>(targ._centerNode._contour[i].x,targ._centerNode._contour[i].y) = inferredDepth;
else
result.at<Vec3b>(targ._centerNode._contour[i].x,targ._centerNode._contour[i].y) = Vec3b(inferredDepth,inferredDepth,inferredDepth);
}
}
// triangles = warped triangles
Mat warpImage(vector<vector<Point2f> > triangles, Mat srcIm, vector<Mat> affines)
{
Mat warpedIm = srcIm.clone();
//*
for (int i = 0; i < warpedIm.rows; i++)
for (int j = 0; j < warpedIm.cols; j++)
warpedIm.at<Vec3b>(i,j) = Vec3b(0,0,0);
//*/
for (int i = 0; i < warpedIm.cols; i++)//
{
for (int j = 0; j < warpedIm.rows; j++)
{
// get warped triangle belonged to
int idx = getAffineIndex(triangles, i,j);
if (idx == -1)
{
//cout << "bad triangle index found" << endl;
continue;
}
Mat curA = affines[idx];
//*
// affine mat: [ R | T ] inv: [ R^-1 | -R^-1 T]
// [ 0 0 0 | 1 ] inv: [ 0 0 0 | 1 ]
Mat R(2, 2, CV_64F); // NOT JUST ROTATION: rotation, scaling, shear
R.at<double>(0,0) = curA.at<double>(0,0);
R.at<double>(0,1) = curA.at<double>(0,1);
R.at<double>(1,0) = curA.at<double>(1,0);
R.at<double>(1,1) = curA.at<double>(1,1);
Mat invR = R.inv(DECOMP_SVD);
Mat invA(3, 3, CV_64F, 0.); // for (3,0-2)
// R^-1
invA.at<double>(0,0) = invR.at<double>(0,0);
invA.at<double>(0,1) = invR.at<double>(0,1);
invA.at<double>(1,0) = invR.at<double>(1,0);
invA.at<double>(1,1) = invR.at<double>(1,1);
// -R^-1 T
invA.at<double>(0,2) = -invR.at<double>(0,0)* curA.at<double>(0,2) -invR.at<double>(0,1)* curA.at<double>(1,2);
invA.at<double>(1,2) = -invR.at<double>(1,0)* curA.at<double>(0,2) -invR.at<double>(1,1)* curA.at<double>(1,2);
// 1
//invA.at<double>(2,2) = 1.;
//*/
//Mat invA = curA.inv(DECOMP_SVD);
//
double x(i), y(j);
double srcJ = invA.at<double>(0,0)*x + invA.at<double>(0,1)*y + invA.at<double>(0,2);
double srcI = invA.at<double>(1,0)*x + invA.at<double>(1,1)*y + invA.at<double>(1,2);
if (srcI < 0 || srcJ < 0)
warpedIm.at<Vec3b>(j, i) =Vec3b(0,0,0);//srcIm.at<Vec3b>(j,i);//i, j); // hack
else
warpedIm.at<Vec3b>(j, i) = srcIm.at<Vec3b>(srcI,srcJ);
}
}
//drawAllTriangles(warpedIm.clone(), triangles, "post warp", false);
//waitKey();
return warpedIm;
}
void makeEigenfaces(vector<Mat> meanIms, Mat meanOfAll)
{
// since it is a gray image, just need to consider one channel (since R = G = B for gray)
// 1. Do PCA (principle component analysis) by computing eigenfaces:
// - get difference of warped-to-mean individual images from mean image
// - put into one long vector, getting M number of long vectors (M is number of images)
// - Let A = (R*C)xM-matrix, where R*C is row * col size of the image
// - compute covariance matrix C by A' * A (getting MxM-matrix), and calculate the eigenvectors and eigenvalues
// - reverse eigenvectors from one long vector back to RxC-matrix, giving eigenface
int numMeanIms = static_cast<int>(meanIms.size());
cout << "numMeanIms: " << numMeanIms << endl;
int R = meanIms[0].rows, C = meanIms[0].cols;
int RC = R*C;
Mat A(RC, numMeanIms, CV_32F, Scalar(0));
// Cov = Covariance matrix
// Cov = A'*A, where A is made up of the long vectors (not A*A' since that matrix will be too big)
Mat Cov;
int i = 0, j = 0, d = 0;
for (int k = 0; k < numMeanIms; k++)
{
Mat cur = meanIms[k];
Mat diff = meanOfAll - cur;
stringstream ss;
ss << k;
string ns = ss.str();
string name(ns+"_diff.jpg");
imshow(name, diff);
imwrite(name, diff);
for (int r = 0; r < RC; r++)
{
i = r/C;
j = r - i*C;
d = diff.at<Vec3b>(i,j)[0];
A.at<float>(r, k) = d;
}
}
Mat At = A.t();
cout << "A:r,c: " << A.rows << ", " << A.cols << endl;
cout << "At:r,c: " << At.rows << ", " << At.cols << endl;
Cov = At * A;
//cout << Cov << endl;
//waitKey();
Mat eigenvalues, eigenvectors;
eigen(Cov, eigenvalues, eigenvectors);
// eigen() gives back the eigenvectors in this order: from largest eigenvalue to smallest eigenvalue
// eigenvectors should be normalized, and in range -1 to 1
cout << "EIGENVECTORS\n" << eigenvectors << "\nEIGENVALUES:\n" << eigenvalues << endl;
//waitKey();
// we have the eigenvectors of A'*A.
// to get the eigenvectors of A*A', which are the eigenfaces,
// A * eigenVector
Mat allEigenfaces = A * eigenvectors.t(); // eigenvectors.t() for opencv's sake (eigenvectors in rows)
Mat eigenBoss = allEigenfaces;
// recover eigenfaces
vector<Mat> eigenfaces;
Mat blank = meanIms[0].clone();
for (int i = 0; i < blank.rows; i++)
for (int j = 0; j < blank.cols; j++)
blank.at<Vec3b>(i,j) = Vec3b(0,0,0);
for (int k = 0; k < numMeanIms; k++)
{
Mat cur = blank.clone();
for (int r = 0; r < RC; r++)
{
i = r/C;
j = r - i*C;
int c = 0;
float t = eigenBoss.at<float>(r,k);
if (t != 0.)
c = (t+1.)*255./2.;
if (c > 255) c = 255; // clamp values
if (c < 0) c = 0;
cur.at<Vec3b>(i, j) = Vec3b(c,c,c);
}
eigenfaces.push_back(cur);
}
// show and write all eigenfaces
for (int k = 0; k < eigenfaces.size(); k++)
{
stringstream ss;
ss << k;
string ns = ss.str();
string name(ns+"_ef.jpg");
imshow(name, eigenfaces[k]);
imwrite(name, eigenfaces[k]);
}
cout << "Finished making eigenfaces. Press ANY key to continue... " << endl;
waitKey();
}
void getMeanIm(vector<vector<Mat> > &affinesToMean, vector<Mat> &meanIms, Mat &meanOfAll)
{
// get meanTriangles
// warp all images to meanTriangles mesh
// mean image = average of warped-to-mean images
// get meanTriangles
//vector<vector<Mat> > affinesToMean;
vector<vector<vector<Point2f> > > oriTriangles;
//vector<Mat> meanIms;
Mat meanIm;
Mat im0 = forMeanIms[0].clone();
Mat im1 = forMeanIms[1];
vector<vector<Point2f> > triangles0;
vector<vector<Point2f> > triangles1;
vector<Point2f> facepoints0 = meanFacePoints[0];
vector<Point2f> facepoints1 = meanFacePoints[1];
string name0 = "0"; string name1 = "1";
delaunay2Faces(im0, im1, triangles0, triangles1, facepoints0, facepoints1, name0, name1);
oriTriangles.push_back(triangles0);
oriTriangles.push_back(triangles1);
vector<vector<vector<Point2f> > > XtoYTriangles;
getAllTriangles(triangles0, triangles1, XtoYTriangles);
vector<vector<Point2f> > meanTriangles = XtoYTriangles[XtoYTriangles.size()/2];
// drawAllTriangles(im0.clone(), meanTriangles, "see triangles");
// vector<Mat> curAffines = getAllAffineTransforms(triangles0 , meanTriangles);
// meanIm = warpImage(meanTriangles, im0.clone(), curAffines);
// imshow("MEAN", meanIm);
// meanIms.push_back(meanIm);
// waitKey();
for (int k = 2; k < forMeanIms.size(); k++)
{
im0 = meanIm;
im1 = forMeanIms[k];
vector<vector<Point2f> > triangles1;
vector<Point2f> facepoints1 = meanFacePoints[k];
replicateDelaunay(triangles0, triangles1, facepoints0, facepoints1);
Mat triangulated = im1.clone();
stringstream ss;
ss << k;
string ns = ss.str();
string name(ns+"delau.jpg");
drawAllTriangles(triangulated, triangles1, "cur delaunay replicated");
imwrite(name, triangulated);
//waitKey();
oriTriangles.push_back(triangles1);
vector<vector<vector<Point2f> > > XtoYTriangles;
getAllTriangles(meanTriangles, triangles1, XtoYTriangles);
meanTriangles = XtoYTriangles[XtoYTriangles.size()/2];
}
// warp all images to mean mesh (meanTriangles)
for (int k = 0; k < forMeanIms.size(); k++)
{
vector<vector<Point2f> > curTriangles = oriTriangles[k];
vector<Mat> curAffines = getAllAffineTransforms(curTriangles , meanTriangles);
meanIm = warpImage(meanTriangles, forMeanIms[k].clone(), curAffines);
stringstream ss;
ss << k;
string ns = ss.str();
string name(ns+"_warpedToMean.jpg");
cout << "done getting mean from " << k << "..." << endl;
imshow(name, meanIm);
imwrite(name, meanIm);
meanIms.push_back(meanIm);
}
//Mat meanOfAll = meanIms[0].clone();
meanOfAll = meanIms[0].clone();
int numMeans = static_cast<int>(meanIms.size());
for (int i = 0; i < meanOfAll.rows; i++)
{
for (int j =0; j < meanOfAll.cols; j++)
{
int B = 0, G = 0, R = 0;
for (int k = 0; k < numMeans; k++)
{
B += meanIms[k].at<Vec3b>(i,j)[0];
G += meanIms[k].at<Vec3b>(i,j)[1];
R += meanIms[k].at<Vec3b>(i,j)[2];
}
B /= numMeans;
if (B > 255) B = 255; // clamp
G /= numMeans;
if (G > 255) G = 255;
R /= numMeans;
if (R > 255) R = 255;
meanOfAll.at<Vec3b>(i,j) = Vec3b(B, G, R);
}
}
imshow("MEAN of all MEANS", meanOfAll);
imwrite("MEAN_img.jpg", meanOfAll);
cout << "Finished getting mean. Press ANY key to continue..." << endl;
waitKey();
}
///////////////////////////
// Author: Brian Fehrman
// Creates a mosaic given a p and p_prime image. Assumes P image is to the
// left and P_prime is to the right (this may not be a needed assumption)
// Assumes H matrix takes p to p_prime.
///////////////////////////
void stitcher::create_mosaic( Mat& dst_mat)
{
double curr_tran_x = 0.0, curr_tran_y = 0.0;
Mat scaled_mat;
Mat H_matrix_inv = H_matrix.inv();
double scale_val = 0.0;
Vec3d extrema;
Vec3d point_offsets;
Mat temp_mat;
find_extrema( extrema, point_offsets );
temp_mat.create( (int) extrema[1], (int) extrema[0], CV_8UC3 );
for( int curr_row = 0; curr_row < temp_mat.rows; curr_row++)
{
for( int curr_col = 0; curr_col < temp_mat.cols; curr_col++)
{
temp_mat.at<Vec3b>( curr_row, curr_col) = Vec3b( 0, 0, 0);
}
}
for(double curr_x = 0; curr_x < p_prime_image.cols - 1; curr_x += 1 )
{
for( double curr_y = 0; curr_y < p_prime_image.rows - 1; curr_y += 1 )
{
scale_val = curr_x * H_matrix_inv.at<double>(2,0) + curr_y * H_matrix_inv.at<double>(2,1) + H_matrix_inv.at<double>(2,2);
curr_tran_x = (curr_x * H_matrix_inv.at<double>(0,0) + curr_y * H_matrix_inv.at<double>(0,1) + H_matrix_inv.at<double>(0,2))/scale_val;
curr_tran_y = (curr_x * H_matrix_inv.at<double>(1,0) + curr_y * H_matrix_inv.at<double>(1,1) + H_matrix_inv.at<double>(1,2))/scale_val;
if( curr_tran_y > 0 && curr_tran_x > 0 && curr_tran_y < temp_mat.rows && curr_tran_x < temp_mat.cols)
temp_mat.at<Vec3b>( curr_tran_y, curr_tran_x) = p_prime_image.at<Vec3b>( curr_y, curr_x );
}
}
for(double curr_x = 0; curr_x < temp_mat.cols - 1; curr_x += 1 )
{
for( double curr_y = 0; curr_y < temp_mat.rows - 1; curr_y += 1 )
{
scale_val = curr_x * H_matrix.at<double>(2,0) + curr_y * H_matrix.at<double>(2,1) + H_matrix.at<double>(2,2);
curr_tran_x = (curr_x * H_matrix.at<double>(0,0) + curr_y * H_matrix.at<double>(0,1) + H_matrix.at<double>(0,2))/scale_val;
curr_tran_y = (curr_x * H_matrix.at<double>(1,0) + curr_y * H_matrix.at<double>(1,1) + H_matrix.at<double>(1,2))/scale_val;
if( curr_tran_y > 0 && curr_tran_x > 0 && curr_tran_y < p_prime_image.rows && curr_tran_x < p_prime_image.cols)
temp_mat.at<Vec3b>( curr_y, curr_x) = p_prime_image.at<Vec3b>( curr_tran_y, curr_tran_x );
}
}
for(double curr_x = 0; curr_x < p_image.cols - 1; curr_x += 1 )
{
for( double curr_y = 0; curr_y < p_image.rows - 1; curr_y += 1 )
{
temp_mat.at<Vec3b>( curr_y, curr_x) = p_image.at<Vec3b>( curr_y, curr_x );
}
}
temp_mat.copyTo( dst_mat);
imwrite(output_mosaic_name, temp_mat);
}
void ObjectTester::RunVideoDemo()
{
GenericObjectDetector detector;
KinectDataMan kinectDM;
if( !kinectDM.InitKinect() )
return;
bool doRank = true;
// start fetching stream data
while(1)
{
Mat cimg, dmap;
kinectDM.GetColorDepth(cimg, dmap);
// resize image
Size newsz;
ToolFactory::compute_downsample_ratio(Size(cimg.cols, cimg.rows), 300, newsz);
resize(cimg, cimg, newsz);
//resize(dmap, dmap, newsz);
//normalize(dmap, dmap, 0, 255, NORM_MINMAX);
// get objects
vector<ImgWin> objwins, salwins;
if( !detector.ProposeObjects(cimg, dmap, objwins, salwins, doRank) )
continue;
//////////////////////////////////////////////////////////////////////////
// draw best k windows
int topK = MIN(6, objwins.size());
int objimgsz = newsz.height / topK;
int canvas_h = newsz.height;
int canvas_w = newsz.width + 10 + objimgsz*2;
Mat canvas(canvas_h, canvas_w, CV_8UC3);
canvas.setTo(Vec3b(0,0,0));
// get top windows
vector<Mat> detimgs(topK);
vector<Mat> salimgs(topK);
for (int i=0; i<topK; i++)
{
cimg(objwins[i]).copyTo(detimgs[i]);
resize(detimgs[i], detimgs[i], Size(objimgsz, objimgsz));
cimg(salwins[i]).copyTo(salimgs[i]);
resize(salimgs[i], salimgs[i], Size(objimgsz, objimgsz));
}
// draw boxes on input
Scalar objcolor(0, 255, 0);
Scalar salcolor(0, 0, 255);
for(int i=0; i<topK; i++)
{
rectangle(cimg, objwins[i], objcolor);
rectangle(cimg, salwins[i], salcolor);
}
circle(cimg, Point(cimg.cols/2, cimg.rows/2), 2, CV_RGB(255,0,0));
// copy input image
cimg.copyTo(canvas(Rect(0, 0, cimg.cols, cimg.rows)));
// copy subimg
for (int i=0; i<detimgs.size(); i++)
{
Rect objbox(cimg.cols+10, i*objimgsz, objimgsz, objimgsz);
detimgs[i].copyTo(canvas(objbox));
Rect salbox(cimg.cols+10+objimgsz, i*objimgsz, objimgsz, objimgsz);
salimgs[i].copyTo(canvas(salbox));
}
resize(canvas, canvas, Size(canvas.cols*2, canvas.rows*2));
//if(doRank)
//putText(canvas, "Use Ranking", Point(50, 50), CV_FONT_HERSHEY_COMPLEX, 1, CV_RGB(255, 0, 0));
imshow("object proposals", canvas);
if( waitKey(10) == 'q' )
break;
}
}
void FaceMasker::Run(int min_depth, int min_pixels, int open_size,
int head_width, int head_height, int head_depth,
int face_size, int extended_size,
int window_size, int width, int height,
float focal_length, const Depth *depth,
Color *color) {
width_ = width;
height_ = height;
window_size_ = window_size;
if (size_ < (width_ * height_)) {
size_ = width_ * height_;
if (valid_mask_ != NULL)
delete [] valid_mask_;
if (head_mask_ != NULL)
delete [] head_mask_;
if (depth_integral_ != NULL)
delete [] depth_integral_;
if (valid_integral_ != NULL)
delete [] valid_integral_;
if (head_integral_ != NULL)
delete [] head_integral_;
if (min_sizes_ != NULL)
delete [] min_sizes_;
if (max_sizes_ != NULL)
delete [] max_sizes_;
valid_mask_ = new bool[size_];
head_mask_ = new bool[size_];
depth_integral_ = new int[size_];
valid_integral_ = new int[size_];
head_integral_ = new int[size_];
min_sizes_ = new float[size_];
max_sizes_ = new float[size_];
}
int max_depth = (head_width * focal_length) / min_pixels;
#pragma omp parallel for
for (int i = 0; i < (width * height); i++) {
valid_mask_[i] = ((depth[i] > min_depth) && (depth[i] < max_depth)) ?
true : false;
}
Integral(width, height, true, valid_mask_, valid_integral_);
Integral(width, height, valid_mask_, depth, depth_integral_);
#pragma omp parallel for
for (int y = 0; y < height; y++) {
for (int x = 0; x < width; x++) {
int i = x + y * width;
head_mask_[i] = false;
if (valid_mask_[i]) {
int head_cols = (int)((head_width * focal_length) / depth[i]);
int head_rows = (int)((head_height * focal_length) / depth[i]);
int center_average = Mean(width, height,
x - head_cols / 2, x + head_cols / 2,
y - head_rows / 2, y + head_rows / 2,
depth_integral_, valid_integral_);
int left_average = Mean(width, height,
x - (5 * head_cols / 4), x - (3 * head_cols / 4),
y - head_rows / 2, y + head_rows / 2,
depth_integral_, valid_integral_);
int right_average = Mean(width, height,
x + (3 * head_cols / 4), x + (5 * head_cols / 4),
y - head_rows / 2, y + head_rows / 2,
depth_integral_, valid_integral_);
int top_average = Mean(width, height,
x - head_cols / 2, x + head_cols / 2,
y - (5 * head_rows / 4), y - (3 * head_rows / 4),
depth_integral_, valid_integral_);
int top_left_average = Mean(width, height,
x - (5 * head_cols / 4), x - (3 * head_cols / 4),
y - (5 * head_rows / 4), y - (3 * head_rows / 4),
depth_integral_, valid_integral_);
int top_right_average = Mean(width, height,
x + (3 * head_cols / 4), x + (5 * head_cols / 4),
y - (5 * head_rows / 4), y - (3 * head_rows / 4),
depth_integral_, valid_integral_);
int center_difference = ABS(depth[i] - center_average);
int left_difference = ABS(depth[i] - left_average);
int right_difference = ABS(depth[i] - right_average);
int top_difference = ABS(depth[i] - top_average);
int top_left_difference = ABS(depth[i] - top_left_average);
int top_right_difference = ABS(depth[i] - top_right_average);
int alpha = head_depth;
int beta = 2 * head_depth;
head_mask_[i] = ((center_difference < alpha) &&
(left_difference > beta) &&
(right_difference > beta) &&
(top_difference > beta) &&
(top_left_difference > beta) &&
(top_right_difference > beta)) ? true : false;
}
}
}
Integral(width, height, false, head_mask_, head_integral_);
Erode(width, height, open_size, head_integral_, head_mask_);
Integral(width, height, true, head_mask_, head_integral_);
Dilate(width, height, open_size, head_integral_, head_mask_);
Integral(width, height, true, head_mask_, head_integral_);
#pragma omp parallel for
for (int y = 0; y < height; y++) {
for (int x = 0; x < width; x++) {
int i = x + y * width;
min_sizes_[i] = max_sizes_[i] = 0.0f;
if (valid_mask_[i]) {
int face_pixels = (int)((face_size * focal_length) / depth[i]);
if (Sum(width, height, x - face_pixels / 2, x + face_pixels / 2,
y - face_pixels / 2, y + face_pixels / 2, head_integral_) > 0) {
int extended_pixels =(int)((extended_size * focal_length) / depth[i]);
min_sizes_[i] = face_pixels - extended_pixels;
max_sizes_[i] = face_pixels + extended_pixels;
}
}
}
}
frame_++;
scale_ = 0;
//#define HEAD_DEBUG
#ifdef HEAD_DEBUG
Mat image(height_, width_, CV_8UC3, color);
Mat head_region = Mat::zeros(height_, width_, CV_8UC3);
#pragma omp parallel for
for (int y = 0; y < height_; y++) {
for (int x = 0; x < width_; x++) {
int i = x + y * width_;
if (head_mask_[i])
head_region.at<Vec3b>(y, x) = Vec3b(255, 255, 255);
}
}
char filename[256];
sprintf(filename, "head-%d.png", frame_);
Mat output;
addWeighted(image, 0.5, head_region, 0.5, 0.0, output);
imwrite(filename, output);
scale_++;
#endif
}
void LzCalculator::calc(Mat &_LEFT, Mat &_RIGHT, Mat &_Ledge, Mat &_Redge)
{
is_valid = false;
Mat img_L,img_R;
rctfndroi(_LEFT, _RIGHT, img_L ,img_R);
Mat _Lamp, _Ramp;
argument(_LEFT, _Lamp);
argument(_RIGHT, _Ramp);
GaussianBlur(_Lamp, _Lamp, Size(5,5), 1.5, 1.5);
GaussianBlur(_Ramp, _Ramp, Size(5,5), 1.5, 1.5);
_Lem.markedge( _Lamp, _Ledge );
_Rem.markedge( _Ramp, _Redge );
Mat _LP,_RP,_LsubP,_RsubP;
L_points.resize(_Ledge.rows,vector<double>(0));
R_points.resize(_Redge.rows,vector<double>(0));
record_L.resize(_Ledge.rows,0);
record_R.resize(_Redge.rows,0);
L_opt_ps.resize(_Ledge.rows,-1);
R_opt_ps.resize(_Redge.rows,-1);
calccenters(_Ledge, _LEFT, _LP, _LsubP,1);
calccenters(_Redge, _RIGHT, _RP, _RsubP,2);
center_match(L_points,R_points,L_opt_ps,R_opt_ps);
calc3Dpts();
coordProject();
/*ofstream SaveFile("D:\\Output.txt");
for(int i=0; i<_rect_Pnts3d.cols; i++)
{
SaveFile<<"第"<<i<<"点坐标："<<endl;
SaveFile<<"高度： "<<*_rect_Pnts3d.ptr<float>(0,i)<<" 原点:"<<*_Pnts3d.ptr<float>(0,i)<<endl;
SaveFile<<"水平： "<<*_rect_Pnts3d.ptr<float>(1,i)<<" 原点:"<<*_Pnts3d.ptr<float>(1,i)<<endl;
SaveFile<<"Z坐标："<<*_rect_Pnts3d.ptr<float>(2,i)<<" 原点:"<<*_Pnts3d.ptr<float>(2,i)<<endl;
SaveFile<<endl;
}
SaveFile.close();*/
for(int i=0;i<lft_pts.size();i++)
{
img_L.at<Vec3b>(int(lft_pts[i].y),int(lft_pts[i].x)) = Vec3b(0,255,0);
//cout<<i<<endl;
}
for(int i=0;i<rgt_pts.size();i++)
{
img_R.at<Vec3b>(int(rgt_pts[i].y),int(rgt_pts[i].x)) = Vec3b(0,255,0);
//cout<<i<<endl;
}
/*imwrite("C:\\Users\\Administrator\\Desktop\\标定数据_1127\\代码_TSH_1125\\兰州代码11_13\\Lz_Calc\\L1.bmp",img_L);
imwrite("C:\\Users\\Administrator\\Desktop\\标定数据_1127\\代码_TSH_1125\\兰州代码11_13\\Lz_Calc\\R1.bmp",img_R);
imwrite("L1.bmp",_LP);
imwrite("R1.bmp",_RP);*/
L_opt_ps.clear();
R_opt_ps.clear();
L_points.clear();
R_points.clear();
single_seed.clear();
record_L.clear();
record_R.clear();
lft_pts.clear();
rgt_pts.clear();
}
//Testing function "Train"
TEST(colorHistDetectorTest, Train)
{
//Load the input data
vector<Frame*> frames = LoadTestProject("speltests_TestData/CHDTrainTestData/", "trijumpSD_50x41.xml");
//Setting parameters
auto seq = new Sequence();
map <string, float> params = SetParams(frames, &seq);
for (auto f : frames)
delete f;
frames.clear();
frames = seq->getFrames();
//Counting a keyframes
int FirstKeyframe = FirstKeyFrameNum(frames);
int KeyframesCount = keyFramesCount(frames);
//Copy image and skeleton from keyframe
Mat image = frames[FirstKeyframe]->getImage();
Mat image1;
image.copyTo(image1);
Frame *frame = frames[FirstKeyframe];
Skeleton skeleton = frame->getSkeleton();
tree<BodyPart> PartTree = skeleton.getPartTree();
//Build the rectangles for all of bodyparts
map<int, POSERECT<Point2f>> Rects = SkeletonRects(skeleton);
//Run "Train()"
ColorHistDetector detector;
detector.train(frames, params);
//Calculate the polygons occlusion
//Polygons layers:
map<int, int> depth = { { 0, 2 }, { 1, 1 }, { 2, 3 }, { 3, 2 }, { 4, 4 }, { 5, 4 }, { 6, 1 }, { 7, 3 }, { 8, 2 }, { 9, 0 }, { 10, 4 }, { 11, 1 }, { 12, 3 }, { 13, 0 }, { 14, 4 }, { 15, 1 }, { 16, 3 } };
//Polygons occlusion:
vector<vector<pair<int, int>>> Crossings = CrossingsList(Rects, depth);
//Calculate the parts histograms
map <int32_t, ColorHistDetector::PartModel> partModels;
for (int i = 0; i < Rects.size(); i++)
{
ColorHistDetector::PartModel Model(8);
Model.sizeFG = 0;
float xmin, ymin, xmax, ymax;
Rects[i].GetMinMaxXY <float>(xmin, ymin, xmax, ymax);
for (int x = xmin; x < xmax; x++)
{
for (int y = ymin; y < ymax; y++)
{
bool b = true;
if (Rects[i].containsPoint(Point2f(x, y)) > 0)
{
int k = 0;
while ((k < Crossings[i].size()) && b)
{
if (Rects[Crossings[i][k].first].containsPoint(Point2f(x, y)) > 0)
b = false;
k++;
}
if (b)
{
int c = 50 + i * 10;
image1.at<Vec3b>(y, x) = Vec3b(c, c, c);
Vec3b color = image.at<Vec3b>(y, x);
Model.partHistogram[color[0] / Factor][color[1] / Factor][color[2] / Factor]++;
Model.sizeFG++;
}
}
}
}
partModels.emplace(pair<int32_t, ColorHistDetector::PartModel>(i, Model));
}
//Put results
int nBins = detector.nBins;
bool AllValuesEqual = true;
int delta = 2; // tolerable linear error
ofstream fout("TrainUnitTest_Output.txt");
fout << "\n--------------------------Don't equal----------------------\n";
cout << "\nTolerable error: " << delta << endl;
fout << "Tolerable error: " << delta << endl;
for (int i = 0; i < partModels.size(); i++)
{
for (int r = 0; r < nBins; r++)
for (int g = 0; g < nBins; g++)
for (int b = 0; b < nBins; b++)
{
int expected = int(partModels[i].partHistogram[b][g][r]);
int actual = int(detector.partModels[i].partHistogram[r][g][b] * detector.partModels[i].sizeFG / KeyframesCount);
if (abs(expected - actual) > delta)
{
cout << "Part[" << i << "]." << "Histogram[" << r << ", " << g << ", " << b << "]: Expected = " << expected << ", Actual = " << actual << endl;
fout << "Part[" << i << "]." << "Histogram[" << r << ", " << g << ", " << b << "]: Expected = " << expected << ", Actual = " << actual << endl;
if (!(r*g*b == 0)) AllValuesEqual = false;
}
}
}
if (AllValuesEqual) fout << "none";
cout << "Output files: TrainUnitTest_Output.txt, UsedPixels.png\n\n";
EXPECT_TRUE(AllValuesEqual);
fout << "\n-----------Expected histogram-----------\n";
fout << "In format:\nHistogramm[r, g, b] = pixelsCount\n";
for (int i = 0; i < partModels.size(); i++)
{
fout << endl << "Rect[" << i << "]:" << endl;
PutHistogram(fout, partModels[i].partHistogram, 1);
}
fout << "\n-----------Actual histogram-----------\n";
fout << "In format:\nHistogramm[b, g, r] = Histogram[b, g, r]*Part.SizeFG/KeyframesCout\n";
for (int i = 0; i < detector.partModels.size(); i++)
{
fout << endl << "Rect[" << i << "]:" << endl;
PutHistogram(fout, detector.partModels[i].partHistogram, detector.partModels[i].sizeFG / KeyframesCount);
}
fout << "\n------------Occluded polygons-----------\nSorted by layer\n";
for (int i = 0; i < Crossings.size(); i++)
{
fout << "\nPolygon[" << i << "] crossed by polygons: ";
for (int k = 0; k < Crossings[i].size(); k++)
fout << Crossings[i][k].first << "; ";
Crossings[i].clear();
}
imwrite("UsedPixels.png", image1);
fout.close();
frames.clear();
params.clear();
Crossings.clear();
partModels.clear();
image.release();
image1.release();
delete seq;
}
UINT ThreadProc(LPVOID pParam)
{
HWND hwnd = AfxGetMainWnd()->GetSafeHwnd();
LPTHREADDATA pData = (LPTHREADDATA)pParam;
Mat dst_temp = Mat::zeros(pData->Roi_dst->size(), 16);
//Vec3b NullPixel = Vec3b(255, 255, 255);
float x_, y_;
Point StartPoint;
Point EndPoint;
StartPoint = pData->Roi->tl();
EndPoint = pData->Roi->br();
int x_s, x_l, y_s, y_l;
Vec3b* src_tl;
Vec3b* dst_;
float* xmap_;
float* ymap_;
unsigned char* mask_;
int CamIdx;
int i = 1;
while (TRUE)
{
//MessageBox(NULL, _T("checkpoint1"),NULL , MB_OK);
//WaitForSingleObject(g_event[pData->ID],INFINITE);
g_Semaphore.Lock();
//MessageBox(NULL, _T("checkpoint2"), NULL, MB_OK);
for (int y = StartPoint.y; y < EndPoint.y; y++)
{
//Vec3b* dst_ = pData->Obj->m_dst.ptr<Vec3b>(y - StartPoint.y);
dst_ = dst_temp.ptr<Vec3b>(y - StartPoint.y);
xmap_ = pData->Obj->m_xmap.ptr<float>(y);
ymap_ = pData->Obj->m_ymap.ptr<float>(y);
mask_ = pData->Obj->m_mask.ptr<unsigned char>(y);
for (int x = StartPoint.x; x < EndPoint.x; x++)
{
CamIdx = (int)mask_[x];
if (CamIdx == 255)
{
//dst_[x] = NullPixel;
dst_[x - StartPoint.x] = Vec3b(0, 0, 0);
}
else
{
x_ = xmap_[x];
y_ = ymap_[x];
x_s = (int)floor(x_);
x_l = (int)ceil(x_);
y_s = (int)floor(y_);
y_l = (int)ceil(y_);
//float n = y_ - y_s, m = x_ - x_s;
if (x_s < 0 || y_s < 0 || x_l>(*pData->Images)[CamIdx].cols - 1 || y_l>(*pData->Images)[CamIdx].rows - 1)
{
//cout << "out of range" << endl;
dst_[x - StartPoint.x] = Vec3b(0, 0, 0);
}
else
{
src_tl = (*pData->Images)[CamIdx].ptr<Vec3b>(y_s, x_s);
dst_[x - StartPoint.x] = *src_tl;
}
}
}
}
CCritical.Lock();
pData->Obj->m_dst = pData->Obj->m_dst | dst_temp;
//signal++;
::PostMessage(hwnd, WM_LOOP, NULL, NULL);
CCritical.Unlock();
//MessageBox(NULL, _T("threadend"), NULL, NULL);
//Sleep(10);
//i--;
}
dst_temp.release();
return 0;
};
/* compute the CSS image */
vector<int> ComputeCSSImageMaximas(const vector<double>& contourx_, const vector<double>& contoury_,
vector<double>& contourx, vector<double>& contoury,
bool isClosedCurve
)
{
ResampleCurve(contourx_, contoury_, contourx, contoury, 200, !isClosedCurve);
vector<Point2d> pl; PolyLineMerge(pl, contourx, contoury);
map<int, double> maximas;
Mat_<Vec3b> img(500, 200, Vec3b(0, 0, 0)), contourimg(350, 350, Vec3b(0, 0, 0));
bool done = false;
//#pragma omp parallel for
for (int i = 0; i<490; i++)
{
if (!done)
{
double sigma = 1.0 + ((double)i)*0.1;
vector<double> kappa, smoothx, smoothy;
ComputeCurveCSS(contourx, contoury, kappa, smoothx, smoothy, sigma);
// vector<vector<Point> > contours(1);
// PolyLineMerge(contours[0], smoothx, smoothy);
// contourimg = Vec3b(0,0,0);
// drawContours(contourimg, contours, 0, Scalar(255,255,255), CV_FILLED);
vector<int> crossings = FindCSSInterestPointsZero(kappa);
if (crossings.size() > 0)
{
for (int c = 0; c<crossings.size(); c++)
{
img(i, crossings[c]) = Vec3b(0, 255, 0);
// circle(contourimg, contours[0][crossings[c]], 5, Scalar(0,0,255), CV_FILLED);
if (c < crossings.size() - 1) {
if (fabs((float)crossings[c] - crossings[c + 1]) < 5.0)//fabs计算绝对值
{
//this is a maxima
int idx = (crossings[c] + crossings[c + 1]) / 2;
//#pragma omp critical
maximas[idx] = (maximas[idx] < sigma) ? sigma : maximas[idx];
circle(img, Point(idx, i), 3, Scalar(0, 0, 255), CV_FILLED);
}
}
}
// char buf[128]; sprintf(buf, "evolution_%05d.png", i);
// imwrite(buf, contourimg);
// imshow("evolution", contourimg);
// waitKey(30);
}
else
{
done = true;
}
}
}
//find largest sigma
double max_sigma = 0.0;
for (map<int, double>::iterator itr = maximas.begin(); itr != maximas.end(); ++itr)
{
if (max_sigma < (*itr).second)
{
max_sigma = (*itr).second;
}
}
//get segments with largest sigma
vector<int> maximasv;
for (map<int, double>::iterator itr = maximas.begin(); itr != maximas.end(); ++itr)
{
if ((*itr).second > max_sigma / 8.0)
{
maximasv.push_back((*itr).first);
}
}
//eliminate degenerate segments (of very small length)
vector<int> maximasvv = EliminateCloseMaximas(maximasv, maximas); //1st pass
maximasvv = EliminateCloseMaximas(maximasvv, maximas); //2nd pass
maximasv = maximasvv;
for (vector<int>::iterator itr = maximasv.begin(); itr != maximasv.end(); ++itr) {
cout << *itr << " - " << maximas[*itr] << endl;
}
// Mat zoom; resize(img,zoom,Size(img.rows*2,img.cols*2));
imshow("css image", img);
//waitKey();
return maximasv;
}
void testgen_lines2points_2d( string res_folder )
{
//HCoords c(1024, 512);
//cout << c.width << " " << c.height << " " << c.depth << endl;
c.depth = c.width/2;
for ( double num_clusters_f = 1; num_clusters_f < 10; num_clusters_f *= 1.2 )
{
Mat3b res(c.height, c.width, Vec3b(255, 255, 255) );
// generate clusters
int num_clusters = int(num_clusters_f);
vector< pair< Point3d, Point3d > > clusters; //кластер = 2 прямые
for (int i_cluster = 0; i_cluster < num_clusters; i_cluster++)
{
Point3d a1(rand() % c.width, rand() % c.height, c.depth);
Point3d a2(rand() % c.width, rand() % c.height, c.depth);
while (a1 == a2)
{
a2 = Point3d(rand() % c.width, rand() % c.height, c.depth);
}
Point3d b1(rand() % c.width, rand() % c.height, c.depth);
Point3d b2(rand() % c.width, rand() % c.height, c.depth);
while (b1 == b2)
{
b2 = Point3d(rand() % c.width, rand() % c.height, c.depth);
}
a1.x -= c.width / 2;
a1.y -= c.height / 2;
a2.x -= c.width / 2;
a2.y -= c.height / 2;
b1.x -= c.width / 2;
b1.y -= c.height / 2;
b2.x -= c.width / 2;
b2.y -= c.height / 2;
//cout << a1.x << " " << a1.y << " " << a1.z << endl;
//cout << a2.x << " " << a2.y << " " << a2.z << endl;
Point3d l1 = a1.cross(a2);
Point3d l2 = b1.cross(b2);
l1 = normalize(l1);
l2 = normalize(l2);
//cout << l1.x << " " << l1.y << " " << l1.z << endl;
pair<Point3d, Point3d> center = make_pair(l1, l2);
clusters.push_back(center);
draw_Line(res, l1, 0);
draw_Line(res, l2, 0);
}
////////// generate points around clusters
vector<Point3d> p;// все прямые в кластере
for (int i_cluster = 0; i_cluster < num_clusters; i_cluster++)
{
Scalar color = Scalar(rand() % 256, rand() % 256, rand() % 256) ;
for (int i1 = 0; i1 < countLines; ++i1)
{
double k = 1.0 * (rand() % 1000) / 1000.0 ;
Point3d c = clusters[i_cluster].first * k + clusters[i_cluster].second * (1.0 - k);
p.push_back(gen_point_on_sphere(c, sigma));// немного сдвинутая прямая
//p.push_back(c);
//cout << c.x << " " << c.y << " " << c.z << endl;
//cout << p.back().x << " " << p.back().y << " " << p.back().z << endl;
//cout << endl;
draw_Line(res, p.back(), color);
}
}
random_shuffle(p.begin(), p.end());
string test_name = res_folder + format( "line%.03d", num_clusters );
// cerr << res_folder << endl;
ofstream out((test_name + ".txt").c_str());
out << c.width << " " << c.height << endl;
out << num_clusters << endl;
out.precision(6);
for (int i=0; i<clusters.size(); i++)
{
//draw_Line(res, clusters[i].first);
//draw_Line(res, clusters[i].second);
out << fixed << clusters[i].first.x << "\t" << clusters[i].first.y << "\t" << clusters[i].first.z << "\t" << clusters[i].second.x << "\t" << clusters[i].second.y << "\t" << clusters[i].second.z << "\t" << sigma << endl;
}
out << p.size() << endl;
for (int i = 0; i < p.size(); ++i)
{
out << fixed << p[i].x << "\t" << p[i].y << "\t" << p[i].z << endl;
}
imwrite( test_name+".png", res );
}
}
void OpenniGrabber :: run()
{
m_should_exit = false;
m_current_image.setCalibration(m_calib_data);
m_rgbd_image.setCalibration(m_calib_data);
// Depth
m_rgbd_image.rawDepthRef() = Mat1f(m_calib_data->raw_depth_size);
m_rgbd_image.rawDepthRef() = 0.f;
m_rgbd_image.depthRef() = m_rgbd_image.rawDepthRef();
m_current_image.rawDepthRef() = Mat1f(m_calib_data->raw_depth_size);
m_current_image.rawDepthRef() = 0.f;
m_current_image.depthRef() = m_current_image.rawDepthRef();
// Color
if (m_has_rgb)
{
m_rgbd_image.rawRgbRef() = Mat3b(m_calib_data->rawRgbSize());
m_rgbd_image.rawRgbRef() = Vec3b(0,0,0);
m_rgbd_image.rgbRef() = m_rgbd_image.rawRgbRef();
m_current_image.rawRgbRef() = Mat3b(m_calib_data->rawRgbSize());
m_current_image.rawRgbRef() = Vec3b(0,0,0);
m_current_image.rgbRef() = m_current_image.rawRgbRef();
m_rgbd_image.rawIntensityRef() = Mat1f(m_calib_data->rawRgbSize());
m_rgbd_image.rawIntensityRef() = 0.f;
m_rgbd_image.intensityRef() = m_rgbd_image.rawIntensityRef();
m_current_image.rawIntensityRef() = Mat1f(m_calib_data->rawRgbSize());
m_current_image.rawIntensityRef() = 0.f;
m_current_image.intensityRef() = m_current_image.rawIntensityRef();
}
// User tracking
m_rgbd_image.userLabelsRef() = cv::Mat1b(m_calib_data->raw_depth_size);
m_rgbd_image.userLabelsRef() = 0u;
if (m_track_users)
m_rgbd_image.setSkeletonData(new Skeleton());
m_current_image.userLabelsRef() = cv::Mat1b(m_calib_data->raw_depth_size);
m_current_image.userLabelsRef() = 0u;
if (m_track_users)
m_current_image.setSkeletonData(new Skeleton());
if (m_has_rgb)
{
bool mapping_required = m_calib_data->rawRgbSize() != m_calib_data->raw_depth_size;
if (!mapping_required)
{
m_rgbd_image.mappedRgbRef() = m_rgbd_image.rawRgbRef();
m_rgbd_image.mappedDepthRef() = m_rgbd_image.rawDepthRef();
m_current_image.mappedRgbRef() = m_current_image.rawRgbRef();
m_current_image.mappedDepthRef() = m_current_image.rawDepthRef();
}
else
{
m_rgbd_image.mappedRgbRef() = Mat3b(m_calib_data->raw_depth_size);
m_rgbd_image.mappedRgbRef() = Vec3b(0,0,0);
m_rgbd_image.mappedDepthRef() = Mat1f(m_calib_data->rawRgbSize());
m_rgbd_image.mappedDepthRef() = 0.f;
m_current_image.mappedRgbRef() = Mat3b(m_calib_data->rawDepthSize());
m_current_image.mappedRgbRef() = Vec3b(0,0,0);
m_current_image.mappedDepthRef() = Mat1f(m_calib_data->rawRgbSize());
m_current_image.mappedDepthRef() = 0.f;
}
}
m_rgbd_image.setCameraSerial(cameraSerial());
m_current_image.setCameraSerial(cameraSerial());
xn::SceneMetaData sceneMD;
xn::DepthMetaData depthMD;
xn::ImageMetaData rgbMD;
xn::IRMetaData irMD;
ImageBayerGRBG bayer_decoder(ImageBayerGRBG::EdgeAware);
RGBDImage oversampled_image;
if (m_subsampling_factor != 1)
{
oversampled_image.rawDepthRef().create(m_calib_data->rawDepthSize()*m_subsampling_factor);
oversampled_image.userLabelsRef().create(oversampled_image.rawDepth().size());
}
while (!m_should_exit)
{
waitForNewEvent();
ntk_dbg(2) << format("[%x] running iteration", this);
{
// OpenNI calls do not seem to be thread safe.
QMutexLocker ni_locker(&m_ni_mutex);
waitAndUpdateActiveGenerators();
}
if (m_track_users && m_body_event_detector)
m_body_event_detector->update();
m_ni_depth_generator.GetMetaData(depthMD);
if (m_has_rgb)
{
if (m_get_infrared)
{
m_ni_ir_generator.GetMetaData(irMD);
}
else
{
m_ni_rgb_generator.GetMetaData(rgbMD);
}
}
RGBDImage& temp_image =
m_subsampling_factor == 1 ? m_current_image : oversampled_image;
const XnDepthPixel* pDepth = depthMD.Data();
ntk_assert((depthMD.XRes() == temp_image.rawDepth().cols)
&& (depthMD.YRes() == temp_image.rawDepth().rows),
"Invalid image size.");
// Convert to meters.
const float depth_correction_factor = 1.0;
float* raw_depth_ptr = temp_image.rawDepthRef().ptr<float>();
for (int i = 0; i < depthMD.XRes()*depthMD.YRes(); ++i)
raw_depth_ptr[i] = depth_correction_factor * pDepth[i]/1000.f;
if (m_has_rgb)
{
if (m_get_infrared)
{
const XnGrayscale16Pixel* pImage = irMD.Data();
m_current_image.rawIntensityRef().create(irMD.YRes(), irMD.XRes());
float* raw_img_ptr = m_current_image.rawIntensityRef().ptr<float>();
for (int i = 0; i < irMD.XRes()*irMD.YRes(); ++i)
{
raw_img_ptr[i] = pImage[i];
}
}
else
{
if (m_custom_bayer_decoding)
{
uchar* raw_rgb_ptr = m_current_image.rawRgbRef().ptr<uchar>();
bayer_decoder.fillRGB(rgbMD,
m_current_image.rawRgb().cols, m_current_image.rawRgb().rows,
raw_rgb_ptr);
cvtColor(m_current_image.rawRgbRef(), m_current_image.rawRgbRef(), CV_RGB2BGR);
}
else
{
const XnUInt8* pImage = rgbMD.Data();
ntk_assert(rgbMD.PixelFormat() == XN_PIXEL_FORMAT_RGB24, "Invalid RGB format.");
uchar* raw_rgb_ptr = m_current_image.rawRgbRef().ptr<uchar>();
for (int i = 0; i < rgbMD.XRes()*rgbMD.YRes()*3; i += 3)
for (int k = 0; k < 3; ++k)
{
raw_rgb_ptr[i+k] = pImage[i+(2-k)];
}
}
}
}
if (m_track_users)
{
m_ni_user_generator.GetUserPixels(0, sceneMD);
uchar* user_mask_ptr = temp_image.userLabelsRef().ptr<uchar>();
const XnLabel* pLabel = sceneMD.Data();
for (int i = 0; i < sceneMD.XRes()*sceneMD.YRes(); ++i)
{
user_mask_ptr[i] = pLabel[i];
}
XnUserID user_ids[15];
XnUInt16 num_users = 15;
m_ni_user_generator.GetUsers(user_ids, num_users);
// FIXME: only one user supported.
for (int i = 0; i < num_users; ++i)
{
XnUserID user_id = user_ids[i];
if (m_ni_user_generator.GetSkeletonCap().IsTracking(user_id))
{
m_current_image.skeletonRef()->computeJoints(user_id, m_ni_user_generator, m_ni_depth_generator);
break;
}
}
}
if (m_subsampling_factor != 1)
{
// Cannot use interpolation here, since this would
// spread the invalid depth values.
cv::resize(oversampled_image.rawDepth(),
m_current_image.rawDepthRef(),
m_current_image.rawDepth().size(),
0, 0, INTER_NEAREST);
// we have to repeat this, since resize can change the pointer.
// m_current_image.depthRef() = m_current_image.rawDepthRef();
cv::resize(oversampled_image.userLabels(),
m_current_image.userLabelsRef(),
m_current_image.userLabels().size(),
0, 0, INTER_NEAREST);
}
m_current_image.setTimestamp(getCurrentTimestamp());
{
QWriteLocker locker(&m_lock);
m_current_image.swap(m_rgbd_image);
}
advertiseNewFrame();
}
ntk_dbg(1) << format("[%x] finishing", this);
}
#include "StdAfx.h"
#include "CmShow.h"
const Vec3b CmShow::gColors[CmShow::COLOR_NUM] =
{
Vec3b(0, 0, 255), Vec3b(0, 255, 0), Vec3b(255, 0, 0), Vec3b(153, 0, 48), Vec3b(0, 183, 239),
Vec3b(255, 255, 0), Vec3b(255, 126, 0), Vec3b(255, 194, 14), Vec3b(168, 230, 29),
Vec3b(237, 28, 36), Vec3b(77, 109, 243), Vec3b(47, 54, 153), Vec3b(111, 49, 152), Vec3b(156, 90, 60),
Vec3b(255, 163, 177), Vec3b(229, 170, 122), Vec3b(245, 228, 156), Vec3b(255, 249, 189), Vec3b(211, 249, 188),
Vec3b(157, 187, 97), Vec3b(153, 217, 234), Vec3b(112, 154, 209), Vec3b(84, 109, 142), Vec3b(181, 165, 213),
Vec3b(40, 40, 40), Vec3b(70, 70, 70), Vec3b(120, 120, 120), Vec3b(180, 180, 180), Vec3b(220, 220, 220)
};
const Vec3b CmShow::Black(0, 0, 0);
const Vec3b CmShow::Blue(255, 0, 0);
const Vec3b CmShow::Brown(42, 42, 165);
const Vec3b CmShow::Gray(128, 128, 128);
const Vec3b CmShow::Green(0, 255, 0);
const Vec3b CmShow::Orange(0, 165, 255);
const Vec3b CmShow::Pink(203, 192, 255);
const Vec3b CmShow::Purple(128, 0, 128);
const Vec3b CmShow::Red(0, 0, 255);
const Vec3b CmShow::White(255, 255, 255);
const Vec3b CmShow::Yellow(0, 255, 255);
const Scalar CmShow::BLACK = Black;
const Scalar CmShow::BLUE = Blue;
const Scalar CmShow::BROWN = Brown;
const Scalar CmShow::GRAY = Gray;
const Scalar CmShow::GREEN = Green;
void LaneDetection::AlgoFilterLanes_back(ntuple_list line_out){
unsigned int dim = line_out->dim;
int n_size = line_out->size;
vector< vector<int> > pairs;//each element is a vector of index of segment(s)
pairs.resize(n_size);
for (int i = 0; i < n_size; i++)
{
const Point2d p1(line_out->values[i * dim + 0], line_out->values[i * dim + 1]);
const Point2d p2(line_out->values[i * dim + 2], line_out->values[i * dim + 3]);
double longeur_p12 = dist_p2p(p1, p2);
Segment2d s(p1, p2);
//if (line_out->values[i * dim + 0] > 662 && line_out->values[i * dim + 0] < 664)
//{
// int c = 0;
//}
//else
// continue;
Point2d milieu_p12 = (p1 + p2) / 2;
for (int j = 0; j < n_size; j++)
{
if (j == i) continue;
Point2d seg1(line_out->values[j * dim + 0], line_out->values[j * dim + 1]);
Point2d seg2(line_out->values[j * dim + 2], line_out->values[j * dim + 3]);
Segment2d seg(seg1, seg2);
//simple check
if (!s.isNeighbor(seg))
continue;
//not same side
//if ((cross(p1, seg1, seg2) > -0.0 ? 1 : -1) * (cross(p2, seg1, seg2) > -0.0 ? 1 : -1) < 0)
//{
// continue;
//}
//slope difference
double slope_dif = abs(atan(slope_seg(p1, p2)) - atan(slope_seg(seg1, seg2)));
if (slope_dif > 10 * CV_PI / 180)
{
continue;
}
double longeur_seg = dist_p2p(seg1, seg2);
Point2d t_p1, t_p2, t_seg1, t_seg2;
if (longeur_p12 > longeur_seg * 3)
{
if (foot_p2segment(seg1, p1, p2, t_p1) && foot_p2segment(seg2, p1, p2, t_p2))
{
t_seg1 = seg1;
t_seg2 = seg2;
}
else
{
t_p1 = p1;
t_p2 = p2;
t_seg1 = seg1;
t_seg2 = seg2;
}
}
else if (longeur_seg > longeur_p12 * 3)
{
if (foot_p2segment(p1, seg1, seg2, t_seg1) && foot_p2segment(p2, seg1, seg2, t_seg2))
{
t_p1 = p1;
t_p2 = p2;
}
else
{
t_p1 = p1;
t_p2 = p2;
t_seg1 = seg1;
t_seg2 = seg2;
}
}
else
{
t_p1 = p1;
t_p2 = p2;
t_seg1 = seg1;
t_seg2 = seg2;
}
Point2d p_start = (t_p1 + t_p2) / 2;
Point2d p_end = (t_seg1 + t_seg2) / 2;
//distance
double thresh = 20;
if (p_start.y < 2 * processImage.rows / 3 && p_end.y < 2 * processImage.rows / 3)
thresh = 10;
if (dist_p2segment(t_p1, t_seg1, t_seg2) > thresh && dist_p2segment(t_p2, t_seg1, t_seg2) > thresh)
{
continue;
}
//color condition
int mean_color[3] = { 0, 0, 0 };
int num[3] = { 0, 0, 0 };
Point2d translation[3];
translation[0] = Point2d(0, 0);
translation[1] = p_start - p_end;
translation[2] = -translation[1];
for (int _trans = 0; _trans < 3; _trans++)
{
Rect box = getBoundingBox(t_p1 + translation[_trans], t_p2 + translation[_trans],
t_seg1 + translation[_trans], t_seg2 + translation[_trans]);
Point2d milieu = (p_start + p_end) / 2 + translation[_trans];
//check direction of cross.
int direc = (cross(milieu, t_seg1 + translation[_trans], t_seg2 + translation[_trans]) > -EPS_HERE ? 1 : -1) *
(cross(milieu, t_p1 + translation[_trans], t_p2 + translation[_trans]) > -EPS_HERE ? 1 : -1);
for (int _y = box.y; _y < box.y + box.height; _y++)
{
if (_y >= processImage.rows || _y < 0) continue;
uchar* ptr_row_processImage = ipmImage.ptr<uchar>(_y);
for (int _x = box.x; _x < box.x + box.width; _x++)
{
if (_x >= processImage.cols || _x < 0) continue;
Point2d p(_x, _y);
if (direc != (cross(p, t_seg1 + translation[_trans], t_seg2 + translation[_trans]) > -EPS_HERE ? 1 : -1) *
(cross(p, t_p1 + translation[_trans], t_p2 + translation[_trans]) > -EPS_HERE ? 1 : -1))
{
continue;
}
mean_color[_trans] += ptr_row_processImage[_x];
num[_trans]++;
}
}
mean_color[_trans] = (double)mean_color[_trans] / num[_trans];
}
bool color_matched = (mean_color[0] > mean_color[1] + 30) && (mean_color[0] > mean_color[2] + 30);
if (!color_matched)
{
//line(processImage, p1, p2, Scalar(255, 0, 0));
continue;
}
Rect box = getBoundingBox(t_p1, t_p2,
t_seg1, t_seg2);
Point2d milieu = (p_start + p_end) / 2;
int _trans = 0;
//check direction of cross.
int direc = (cross(milieu, t_seg1 + translation[_trans], t_seg2 + translation[_trans]) > -EPS_HERE ? 1 : -1) *
(cross(milieu, t_p1 + translation[_trans], t_p2 + translation[_trans]) > -EPS_HERE ? 1 : -1);
for (int _y = box.y; _y < box.y + box.height; _y++)
{
if (_y >= processImage.rows || _y < 0) continue;
Vec3b* ptr_row_processImage = processImage.ptr<Vec3b>(_y);
for (int _x = box.x; _x < box.x + box.width; _x++)
{
if (_x >= processImage.cols || _x < 0) continue;
Point2d p(_x, _y);
if (direc != (cross(p, t_seg1 + translation[_trans], t_seg2 + translation[_trans]) > -EPS_HERE ? 1 : -1) *
(cross(p, t_p1 + translation[_trans], t_p2 + translation[_trans]) > -EPS_HERE ? 1 : -1))
{
continue;
}
ptr_row_processImage[_x] = Vec3b(255, 0, 255);
}
}
//TODO: ADD MORE CONDITIONS to check if they are a pair.
//line(processImage, p1, p2, Scalar(0, 255, 0));
//break;
//add candidate pair to vector.
}
}
ofstream fout("pairs.txt");
for (int i = 0; i < n_size; i++)
{
const Point2d p1(line_out->values[i * dim + 0], line_out->values[i * dim + 1]);
const Point2d p2(line_out->values[i * dim + 2], line_out->values[i * dim + 3]);
fout << i << "{" << p1 << "; " << p2 << "}" << " pairs: ";
int b = (unsigned)theRNG() & 255;
int g = (unsigned)theRNG() & 255;
int r = (unsigned)theRNG() & 255;
if (pairs[i].size() <= 1)
continue;
for (int j = 0; j < pairs[i].size(); j++)
{
Point2d seg1(line_out->values[pairs[i][j] * dim + 0], line_out->values[pairs[i][j] * dim + 1]);
Point2d seg2(line_out->values[pairs[i][j] * dim + 2], line_out->values[pairs[i][j] * dim + 3]);
fout << "[" << seg1 << "; " << seg2 << " ],";
}
//line(processImage, p1, p2, Scalar(b, g, r));
//line(processImage, seg1, seg2, Scalar(b, g, r));
fout << "--------" << endl;
}
}
bool find_square(Mat grayImage)
{
if (grayImage.channels() != 1)
{
std::cout << "[find_square]: expected grayscale image, returning false" << std::endl;
return false;
}
Mat adapt;
adaptiveThreshold(
grayImage,
adapt,
255,
ADAPTIVE_THRESH_GAUSSIAN_C,
THRESH_BINARY,
7,
10
);
int erosion_size = 1;
int square_size = erosion_size * 2 + 1;
Mat element = getStructuringElement(
MORPH_RECT,
Size(square_size, square_size),
Point(erosion_size, erosion_size)
);
erode(adapt, adapt, element);
disjoint_set ds;
Mat labels = Mat::zeros(grayImage.size(), CV_32SC1);
clock_t t = clock();
// do label based connected components with a
// disjoint set data structure.
int label = 0;
for (int i = 0; i < adapt.rows; ++i)
{
for (int j = 0; j < adapt.cols; ++j)
{
uchar cur_pixel = adapt.at<uchar>(i, j);
int& cur_label = labels.at<int>(i, j);
int up_label = (i == 0) ? -1 : labels.at< int >(i - 1, j );
int left_label = (j == 0) ? -1 : labels.at< int >(i , j - 1);
uchar up_pixel = (i == 0) ? -1 : adapt.at<uchar>(i - 1, j );
uchar left_pixel = (j == 0) ? -1 : adapt.at<uchar>(i , j - 1);
bool same_left = left_pixel == cur_pixel;
bool same_up = up_pixel == cur_pixel;
if (same_left)
{
if (same_up)
{
cur_label = min(up_label, left_label);
ds.join(up_label, left_label);
}
else
{
cur_label = left_label;
}
}
else if (same_up)
{
cur_label = up_label;
}
else
{
++label;
labels.at<uint>(i, j) = label;
ds.add(label);
}
}
}
if (0)
{
RNG rng(0);
map<int, Vec3b> colorMap;
for (int i = 1; i <= label; ++i)
{
colorMap[i] = Vec3b(rng.uniform(0, 255), rng.uniform(0, 255), rng.uniform(0, 255));
}
Mat assignment = Mat::zeros(grayImage.size(), CV_8UC3);
for (int i = 0; i < assignment.rows; ++i)
{
for (int j = 0; j < assignment.cols; ++j)
{
int label_value = ds.find(labels.at<int>(i, j));
assignment.at<Vec3b>(i, j) = colorMap[label_value];
}
}
imshow("Connections", assignment);
waitKey();
}
t = clock() - t;
cout << ((float) t) / CLOCKS_PER_SEC << endl;
// imshow("Find", adapt);
return true;
}
void Lz_RT_Clac::calc(Mat &_LEFT, Mat &_RIGHT, Mat &_Ledge, Mat &_Redge,int flag) //判断相机（R相机标定反了）
{
is_valid = false;
Mat img_L,img_R;
rctfndroi(_LEFT, _RIGHT, img_L ,img_R);
Mat _Lamp, _Ramp;
argument(_LEFT, _Lamp);
argument(_RIGHT, _Ramp);
GaussianBlur(_Lamp, _Lamp, Size(5,5), 1.5, 1.5);
GaussianBlur(_Ramp, _Ramp, Size(5,5), 1.5, 1.5);
_Lem.markedge( _Lamp, _Ledge );
_Rem.markedge( _Ramp, _Redge );
Mat _LP,_RP,_LsubP,_RsubP;
L_points.resize(_Ledge.rows,vector<double>(0));
R_points.resize(_Redge.rows,vector<double>(0));
record_L.resize(_Ledge.rows,0);
record_R.resize(_Redge.rows,0);
L_opt_ps.resize(_Ledge.rows,-1);
R_opt_ps.resize(_Redge.rows,-1);
calccenters(_Ledge, _LEFT, _LP, _LsubP,1);
calccenters(_Redge, _RIGHT, _RP, _RsubP,2);
center_match(L_points,R_points,L_opt_ps,R_opt_ps); //匹配左右轨点，种子填充算法，L_points为检测出的点，L_opt_ps为配准中心后的点
//get_rail_ps(_LEFT,L_opt_ps,L_rail_ps); //从配准中心点中获得左轨点
//get_rail_ps(_RIGHT,R_opt_ps,R_rail_ps); //从配准中心点中获得右轨点
calc3Dpts(); //如果当前左右轨点个数不为0，计算轨点三维坐标。
coordProject(flag); //相机坐标到世界坐标的转换
rail_pnts_detect(flag);
//get_rail(_rect_Pnts3d); //获得当前相机的铁轨轨高和轨距。
/* ofstream SaveFile("C:\\Users\\Administrator\\Desktop\\Output.txt");
for(int i=0; i<_rect_Pnts3d.cols; i++)
{
SaveFile<<"第"<<i<<"点坐标："<<endl;
SaveFile<<"高度： "<<*_rect_Pnts3d.ptr<float>(0,i)<<" 原点:"<<*_Pnts3d.ptr<float>(0,i)<<endl;
SaveFile<<"水平： "<<*_rect_Pnts3d.ptr<float>(1,i)<<" 原点:"<<*_Pnts3d.ptr<float>(1,i)<<endl;
SaveFile<<"Z坐标："<<*_rect_Pnts3d.ptr<float>(2,i)<<" 原点:"<<*_Pnts3d.ptr<float>(2,i)<<endl;
SaveFile<<endl;
}
SaveFile.close();*/
for(int i=0;i<lft_pts.size();i++)
{
if(lft_pts[i].x!=-1)
img_L.at<Vec3b>(int(lft_pts[i].y),int(lft_pts[i].x)) = Vec3b(0,255,0);
//cout<<i<<endl;
}
for(int i=0;i<rgt_pts.size();i++)
{
if(rgt_pts[i].x!=-1)
img_R.at<Vec3b>(int(rgt_pts[i].y),int(rgt_pts[i].x)) = Vec3b(0,255,0);
//cout<<i<<endl;
}
/*imwrite("C:\\Users\\Administrator\\Desktop\\标定数据_1127\\代码_TSH_1125\\兰州代码11_13\\Lz_Calc\\L1.bmp",img_L);
imwrite("C:\\Users\\Administrator\\Desktop\\标定数据_1127\\代码_TSH_1125\\兰州代码11_13\\Lz_Calc\\R1.bmp",img_R);
imwrite("L1.bmp",_LP);
imwrite("R1.bmp",_RP);*/
L_opt_ps.clear();
R_opt_ps.clear();
L_points.clear();
R_points.clear();
single_seed.clear();
record_L.clear();
record_R.clear();
lft_pts.clear();
rgt_pts.clear();
R_rail_ps.clear();
L_rail_ps.clear();
}