JNIEXPORT jintArray JNICALL Java_com_example_uemcar_Camera_FindFeatures(JNIEnv *env, jclass cls,
jlong frame, jint mode) {
/**
* Leer los datos de entrenamiento una sola vez
* Configurar el modelo KNN
*/
if (trained == 0) {
fillData();
trainingData = TrainData::create(sample, SampleTypes::ROW_SAMPLE, response);
knn->setIsClassifier(true);
knn->setAlgorithmType(KNearest::Types::BRUTE_FORCE);
knn->setDefaultK(1);
knn->train(trainingData); // Train with sample and responses
trained = 1;
}
/**
* Preparación para devolver un array de 5 ints
* https://stackoverflow.com/questions/1610045/how-to-return-an-array-from-jni-to-java
*/
jintArray result;
result = (env)->NewIntArray(5);
if (result == NULL)
return NULL; /* out of memory error thrown */
jint fill[256];
int numSe = 0;
for(int i = 0; i < 5; i++)
fill[i] = 1;
// Frame de la camara pasada desde el JNI
Mat &src = *(Mat *) frame; // Formato RGB!! (por defecto OpenCV usa BGR)
Mat originalSrc;
src.copyTo(originalSrc);
// CLAHE demasiado lento - ajuste dinamico??
// Erode & Dilate quita muchos puntos innecesarios
erode(src, src, Mat());
dilate(src, src, Mat());
// Pasar a HSV
Mat mHSV, mHSV2;
cvtColor(src, mHSV, COLOR_RGB2HSV);
mHSV.copyTo(mHSV2);
// Regiones rojas
cv::Mat mRedHue;
cv::Mat lower_red_hue_range;
cv::Mat upper_red_hue_range;
cv::inRange(mHSV, cv::Scalar(0, 100, 100), cv::Scalar(10, 255, 255), lower_red_hue_range);
cv::inRange(mHSV, cv::Scalar(160, 100, 100), cv::Scalar(179, 255, 255), upper_red_hue_range);
// Gaussian blur
cv::addWeighted(lower_red_hue_range, 1.0, upper_red_hue_range, 1.0, 0.0, mRedHue);
cv::GaussianBlur(mRedHue, mRedHue, cv::Size(9, 9), 2, 2);
Mat mRedHue2;
mRedHue.copyTo(mRedHue2);
// Extraer contornos (blobs) usando Canny y findContours
std::vector<std::vector<cv::Point> > contours;
vector< Vec4i > hierarchy;
Canny(mRedHue, mRedHue, 10, 25, 3);
cv::findContours(mRedHue, contours, hierarchy,
CV_RETR_EXTERNAL, CHAIN_APPROX_SIMPLE, Point(0, 0));
// Recorrer todos los contornos con nivel jerárquico 0
for (int i = 0; i < contours.size(); i = hierarchy[i][0]) {
//approxPolyDP para el STOP?
Rect r = boundingRect(contours[i]);
Mat ROI = src(r);
// Comprobar tamaño y ratio - si es demasiado pequeño salir de iteración
if ((ROI.cols > 2* ROI.rows) || (ROI.rows > ROI.cols*2 ) || ROI.cols < 20 || ROI.rows < 20)
continue;
// Dibujar circulos en Mat
drawContours(src, contours, -1, Scalar(0, 255, 0));
std::vector<cv::Vec3f> circles;
Mat ROIg;
// Detectar circulos
cvtColor(ROI, ROIg, CV_RGB2GRAY); // HoughCircles acepta solo imagenes en escala de grises
// Param2 deberia ser relativo al tamaño del ROI (2*3.14*ROI.rows*porcentajePtos)
cv::HoughCircles(ROIg, circles, CV_HOUGH_GRADIENT, 1, ROIg.rows, 200, 60, 25, 360);
// Dibujar circulos en Mat
for (Vec3f c : circles)
circle(ROI, Point(c[0], c[1]), c[2], Scalar(0, 255, 0));
// Analizar blob si hemos encontrado por lo menos un circulo
if (circles.size() != 0) {
Mat tmp;
resize(ROIg, tmp, Size(50, 50), 0, 0, INTER_LINEAR); // Normalizar a 50x50
tmp.convertTo(tmp, CV_32FC1); // Convertir a coma flotante
// Comprarar usando KNN
int found = knn->findNearest(tmp.reshape(1, 1), knn->getDefaultK(), tmp);
putText(src, to_string(found), Point(r.x, r.y + r.height), 0, 1,
Scalar(0, 255, 0), 2, 8); // Poner texto en imagen
rectangle(src, r, Scalar(0, 0, 255)); // Dibujar rectangulo del ROI
fill[numSe] = found; // Añadir resultado al array de señales encontradas
numSe++;
}
}
// Modo de visión seleccionado
switch(mode) {
default: // Result
break;
case(1): // RGB
src = originalSrc;
break;
case(2): // Gray
cvtColor(originalSrc, src, CV_RGB2GRAY);
break;
case(3): // HSV
src = mHSV2;
break;
case(4): // Red Hue
src = mRedHue2;
break;
case(5): // Contours
src = mRedHue;
break;
}
// Devolver array con señales encontradas
(env)->SetIntArrayRegion(result, 0, 5, fill);
return result;
}
int main(int argc, char *argv[])
{
//KALMAN
measurement.setTo(Scalar(0));
measurement = Mat_<float>(2,1);
KFrightEye = KalmanFilter(4, 2, 0);
KFleftEye = KalmanFilter(4, 2, 0);
state = Mat_<float>(4, 1);
processNoise = Mat(4, 1, CV_32F);
// Reconhecimento continuo
float P_Safe_Ultimo = 1.0, P_notSafe_Ultimo = 0.0;
float P_Atual_Safe, P_Atual_notSafe;
float P_Safe_Atual, P_notSafe_Atual;
float P_Safe;
float timeLastObs = 0, timeAtualObs = 0, tempo = 0;
// These vectors hold the images and corresponding labels:
vector<Mat> images;
vector<Mat> video;
vector<int> labels;
// Create a FaceRecognizer and train it on the given images:
Ptr<FaceRecognizer> model = createLBPHFaceRecognizer();
//leitura dos frames
STREAM *ir = createStream("../build/ir.log");
bool flag_ir;
flag_ir = streamNext(ir);
struct timeval now;
gettimeofday(&now, NULL);
CascadeClassifier haar_cascade;
haar_cascade.load(PATH_CASCADE_FACE);
bool sucess = false;
int login = 0;
Point ultimaFace = Point(-1, -1);
while(flag_ir) {
Mat frame = ir->infrared;
frame.convertTo(frame, CV_8UC3, 255, 0);
vector< Rect_<int> > faces;
// Find the faces in the frame:
haar_cascade.detectMultiScale(frame, faces);
tempo = getTickCount();
for(int j = 0; j < faces.size(); j++) {
Mat face = frame(faces[j]);
Rect faceRect = faces[j];
Point center = Point(faceRect.x + faceRect.width/2, faceRect.y + faceRect.width/2);
if(ultimaFace.x < 0) {
ultimaFace.x = center.x;
ultimaFace.y = center.y;
}
else {
double res = cv::norm(ultimaFace-center);
if(res < 50.0) {
ultimaFace.x = center.x;
ultimaFace.y = center.y;
Mat faceNormalized = faceNormalize(face, FACE_SIZE, sucess);
if(sucess) {
if(login < FRAMES_LOGIN) {
images.push_back(faceNormalized);
labels.push_back(0);
login++;
if(login == FRAMES_LOGIN)
model->train(images, labels);
}
else {
timeLastObs= timeAtualObs;
timeAtualObs = tempo;
double confidence;
int prediction;
model->predict(faceNormalized, prediction, confidence);
//reconhecimento continuo
if(timeLastObs == 0) {
P_Atual_Safe = 1 - ((1 + erf((confidence-Usafe) / (Rsafe*sqrt(2))))/2);
P_Atual_notSafe = ((1 + erf((confidence-UnotSafe) / (RnotSafe*sqrt(2))))/2);
float deltaT = (timeLastObs - timeAtualObs)/getTickFrequency();
float elevado = (deltaT * LN2)/K_DROP;
P_Safe_Atual = P_Atual_Safe + pow(E,elevado) * P_Safe_Ultimo;
P_notSafe_Atual = P_Atual_notSafe + pow(E,elevado) * P_notSafe_Ultimo;
}
else {
P_Atual_Safe = 1 - ((1 + erf((confidence-Usafe) / (Rsafe*sqrt(2))))/2);
P_Atual_notSafe = ((1 + erf((confidence-UnotSafe) / (RnotSafe*sqrt(2))))/2);
P_Safe_Ultimo = P_Safe_Atual;
P_notSafe_Ultimo = P_notSafe_Atual;
float deltaT = (timeLastObs - timeAtualObs)/getTickFrequency();
float elevado = (deltaT * LN2)/K_DROP;
P_Safe_Atual = P_Atual_Safe + pow(E,elevado) * P_Safe_Ultimo;
P_notSafe_Atual = P_Atual_notSafe + pow(E,elevado) * P_notSafe_Ultimo;
}
}
}
}
}
}
if(P_Safe_Atual != 0) {
float deltaT = -(tempo - timeAtualObs)/getTickFrequency();
float elevado = (deltaT * LN2)/K_DROP;
P_Safe = (pow(E,elevado) * P_Safe_Atual)/(P_Safe_Atual+P_notSafe_Atual);
if(P_Safe > 0)
cout << P_Safe << endl;
}
flag_ir = streamNext(ir);
}
return 0;
}
Mat GMRsaliency::GetSal(Mat &img)
{
int wcut[6];
img=RemoveFrame(img,wcut);
Mat suplabel(img.size(),CV_16U);
suplabel=GetSup(img);
Mat adj(Size(spcounta,spcounta),CV_16U);
adj=GetAdjLoop(suplabel);
Mat tImg;
img.convertTo(tImg,CV_32FC3,1.0/255);
cvtColor(tImg, tImg, CV_BGR2Lab);
Mat W(adj.size(),CV_32F);
W=GetWeight(tImg,suplabel,adj);
Mat optAff(W.size(),CV_32F);
optAff=GetOptAff(W);
Mat salt,sald,sall,salr;
Mat bdy;
bdy=GetBdQuery(suplabel,1);
salt=optAff*bdy;
normalize(salt, salt, 0, 1, NORM_MINMAX);
salt=1-salt;
bdy=GetBdQuery(suplabel,2);
sald=optAff*bdy;
normalize(sald, sald, 0, 1, NORM_MINMAX);
sald=1-sald;
bdy=GetBdQuery(suplabel,3);
sall=optAff*bdy;
normalize(sall, sall, 0, 1, NORM_MINMAX);
sall=1-sall;
bdy=GetBdQuery(suplabel,4);
salr=optAff*bdy;
normalize(salr, salr, 0, 1, NORM_MINMAX);
salr=1-salr;
Mat salb;
salb=salt;
salb=salb.mul(sald);
salb=salb.mul(sall);
salb=salb.mul(salr);
double thr=mean(salb)[0];
Mat fgy;
threshold(salb,fgy,thr,1,THRESH_BINARY);
Mat salf;
salf=optAff*fgy;
Mat salMap(img.size(),CV_32F);
for(int i=0;i<salMap.rows;i++)
{
for(int j=0;j<salMap.cols;j++)
{
salMap.at<float>(i,j)=salf.at<float>(suplabel.at<ushort>(i,j));
}
}
normalize(salMap, salMap, 0, 1, NORM_MINMAX);
Mat outMap(Size(wcut[1],wcut[0]),CV_32F,Scalar(0));
Mat subMap=outMap(Range(wcut[2],wcut[3]),Range(wcut[4],wcut[5]));
salMap.convertTo(subMap,subMap.type());
return outMap;
}
static void findConstrainedCorrespondences(const Mat& _F,
const vector<KeyPoint>& keypoints1,
const vector<KeyPoint>& keypoints2,
const Mat& descriptors1,
const Mat& descriptors2,
vector<Vec2i>& matches,
double eps, double ratio)
{
float F[9]={0};
int dsize = descriptors1.cols;
Mat Fhdr = Mat(3, 3, CV_32F, F);
_F.convertTo(Fhdr, CV_32F);
matches.clear();
for( int i = 0; i < (int)keypoints1.size(); i++ )
{
Point2f p1 = keypoints1[i].pt;
double bestDist1 = DBL_MAX, bestDist2 = DBL_MAX;
int bestIdx1 = -1;//, bestIdx2 = -1;
const float* d1 = descriptors1.ptr<float>(i);
for( int j = 0; j < (int)keypoints2.size(); j++ )
{
Point2f p2 = keypoints2[j].pt;
double e = p2.x*(F[0]*p1.x + F[1]*p1.y + F[2]) +
p2.y*(F[3]*p1.x + F[4]*p1.y + F[5]) +
F[6]*p1.x + F[7]*p1.y + F[8];
if( fabs(e) > eps )
continue;
const float* d2 = descriptors2.ptr<float>(j);
double dist = 0;
int k = 0;
for( ; k <= dsize - 8; k += 8 )
{
float t0 = d1[k] - d2[k], t1 = d1[k+1] - d2[k+1];
float t2 = d1[k+2] - d2[k+2], t3 = d1[k+3] - d2[k+3];
float t4 = d1[k+4] - d2[k+4], t5 = d1[k+5] - d2[k+5];
float t6 = d1[k+6] - d2[k+6], t7 = d1[k+7] - d2[k+7];
dist += t0*t0 + t1*t1 + t2*t2 + t3*t3 +
t4*t4 + t5*t5 + t6*t6 + t7*t7;
if( dist >= bestDist2 )
break;
}
if( dist < bestDist2 )
{
for( ; k < dsize; k++ )
{
float t = d1[k] - d2[k];
dist += t*t;
}
if( dist < bestDist1 )
{
bestDist2 = bestDist1;
//bestIdx2 = bestIdx1;
bestDist1 = dist;
bestIdx1 = (int)j;
}
else if( dist < bestDist2 )
{
bestDist2 = dist;
//bestIdx2 = (int)j;
}
}
}
if( bestIdx1 >= 0 && bestDist1 < bestDist2*ratio )
{
Point2f p2 = keypoints1[bestIdx1].pt;
double e = p2.x*(F[0]*p1.x + F[1]*p1.y + F[2]) +
p2.y*(F[3]*p1.x + F[4]*p1.y + F[5]) +
F[6]*p1.x + F[7]*p1.y + F[8];
if( e > eps*0.25 )
continue;
double threshold = bestDist1/ratio;
const float* d22 = descriptors2.ptr<float>(bestIdx1);
int i1 = 0;
for( ; i1 < (int)keypoints1.size(); i1++ )
{
if( i1 == i )
continue;
Point2f pt1 = keypoints1[i1].pt;
const float* d11 = descriptors1.ptr<float>(i1);
double dist = 0;
e = p2.x*(F[0]*pt1.x + F[1]*pt1.y + F[2]) +
p2.y*(F[3]*pt1.x + F[4]*pt1.y + F[5]) +
F[6]*pt1.x + F[7]*pt1.y + F[8];
if( fabs(e) > eps )
continue;
for( int k = 0; k < dsize; k++ )
{
float t = d11[k] - d22[k];
dist += t*t;
if( dist >= threshold )
break;
}
if( dist < threshold )
break;
}
if( i1 == (int)keypoints1.size() )
matches.push_back(Vec2i(i,bestIdx1));
}
}
}
int main(int argc, char **argv)
{
//Read a single-channel image
const char* filename = "imageText.jpg";
Mat srcImg = imread(filename, CV_LOAD_IMAGE_GRAYSCALE);
if(srcImg.empty())
return -1;
imshow("source", srcImg);
Point center(srcImg.cols/2, srcImg.rows/2);
#ifdef DEGREE
//Rotate source image
Mat rotMatS = getRotationMatrix2D(center, DEGREE, 1.0);
warpAffine(srcImg, srcImg, rotMatS, srcImg.size(), 1, 0, Scalar(255,255,255));
imshow("RotatedSrc", srcImg);
//imwrite("imageText_R.jpg",srcImg);
#endif
//Expand image to an optimal size, for faster processing speed
//Set widths of borders in four directions
//If borderType==BORDER_CONSTANT, fill the borders with (0,0,0)
Mat padded;
int opWidth = getOptimalDFTSize(srcImg.rows);
int opHeight = getOptimalDFTSize(srcImg.cols);
copyMakeBorder(srcImg, padded, 0, opWidth-srcImg.rows, 0, opHeight-srcImg.cols, BORDER_CONSTANT, Scalar::all(0));
Mat planes[] = {Mat_<float>(padded), Mat::zeros(padded.size(), CV_32F)};
Mat comImg;
//Merge into a double-channel image
merge(planes,2,comImg);
//Use the same image as input and output,
//so that the results can fit in Mat well
dft(comImg, comImg);
//Compute the magnitude
//planes[0]=Re(DFT(I)), planes[1]=Im(DFT(I))
//magnitude=sqrt(Re^2+Im^2)
split(comImg, planes);
magnitude(planes[0], planes[1], planes[0]);
//Switch to logarithmic scale, for better visual results
//M2=log(1+M1)
Mat magMat = planes[0];
magMat += Scalar::all(1);
log(magMat, magMat);
//Crop the spectrum
//Width and height of magMat should be even, so that they can be divided by 2
//-2 is 11111110 in binary system, operator & make sure width and height are always even
magMat = magMat(Rect(0, 0, magMat.cols & -2, magMat.rows & -2));
//Rearrange the quadrants of Fourier image,
//so that the origin is at the center of image,
//and move the high frequency to the corners
int cx = magMat.cols/2;
int cy = magMat.rows/2;
Mat q0(magMat, Rect(0, 0, cx, cy));
Mat q1(magMat, Rect(0, cy, cx, cy));
Mat q2(magMat, Rect(cx, cy, cx, cy));
Mat q3(magMat, Rect(cx, 0, cx, cy));
Mat tmp;
q0.copyTo(tmp);
q2.copyTo(q0);
tmp.copyTo(q2);
q1.copyTo(tmp);
q3.copyTo(q1);
tmp.copyTo(q3);
//Normalize the magnitude to [0,1], then to[0,255]
normalize(magMat, magMat, 0, 1, CV_MINMAX);
Mat magImg(magMat.size(), CV_8UC1);
magMat.convertTo(magImg,CV_8UC1,255,0);
imshow("magnitude", magImg);
//imwrite("imageText_mag.jpg",magImg);
//Turn into binary image
threshold(magImg,magImg,GRAY_THRESH,255,CV_THRESH_BINARY);
imshow("mag_binary", magImg);
//imwrite("imageText_bin.jpg",magImg);
//Find lines with Hough Transformation
vector<Vec2f> lines;
float pi180 = (float)CV_PI/180;
Mat linImg(magImg.size(),CV_8UC3);
HoughLines(magImg,lines,1,pi180,HOUGH_VOTE,0,0);
int numLines = lines.size();
for(int l=0; l<numLines; l++)
{
float rho = lines[l][0], theta = lines[l][1];
Point pt1, pt2;
double a = cos(theta), b = sin(theta);
double x0 = a*rho, y0 = b*rho;
pt1.x = cvRound(x0 + 1000*(-b));
pt1.y = cvRound(y0 + 1000*(a));
pt2.x = cvRound(x0 - 1000*(-b));
pt2.y = cvRound(y0 - 1000*(a));
line(linImg,pt1,pt2,Scalar(255,0,0),3,8,0);
}
imshow("lines",linImg);
//imwrite("imageText_line.jpg",linImg);
if(lines.size() == 3){
cout << "found three angels:" << endl;
cout << lines[0][1]*180/CV_PI << endl << lines[1][1]*180/CV_PI << endl << lines[2][1]*180/CV_PI << endl << endl;
}
//Find the proper angel from the three found angels
float angel=0;
float piThresh = (float)CV_PI/90;
float pi2 = CV_PI/2;
for(int l=0; l<numLines; l++)
{
float theta = lines[l][1];
if(abs(theta) < piThresh || abs(theta-pi2) < piThresh)
continue;
else{
angel = theta;
break;
}
}
//Calculate the rotation angel
//The image has to be square,
//so that the rotation angel can be calculate right
angel = angel<pi2 ? angel : angel-CV_PI;
if(angel != pi2){
float angelT = srcImg.rows*tan(angel)/srcImg.cols;
angel = atan(angelT);
}
float angelD = angel*180/(float)CV_PI;
cout << "the rotation angel to be applied:" << endl << angelD << endl << endl;
//Rotate the image to recover
Mat rotMat = getRotationMatrix2D(center,angelD,1.0);
Mat dstImg = Mat::ones(srcImg.size(),CV_8UC3);
warpAffine(srcImg,dstImg,rotMat,srcImg.size(),1,0,Scalar(255,255,255));
imshow("result",dstImg);
//imwrite("imageText_D.jpg",dstImg);
waitKey(0);
return 0;
}
void colorSpaceAnalysis( Mat frameBGR, CvSize size )
{
IplImage *t1 = cvCreateImage( size, IPL_DEPTH_8U, 3 );
IplImage *t2 = cvCreateImage( size, IPL_DEPTH_8U, 1 );
IplImage *t3 = cvCreateImage( size, IPL_DEPTH_8U, 1 );
IplImage *t4 = cvCreateImage( size, IPL_DEPTH_8U, 1 );
IplImage *t5 = cvCreateImage( size, IPL_DEPTH_16S, 1 );
IplImage *t6 = cvCreateImage( size, IPL_DEPTH_8U, 1 );
IplImage *t7 = cvCreateImage( size, IPL_DEPTH_16S, 1 );
IplImage *t8 = cvCreateImage( size, IPL_DEPTH_8U, 1 );
IplImage *t9 = cvCreateImage( size, IPL_DEPTH_16S, 1 );
IplImage *t0 = cvCreateImage( size, IPL_DEPTH_8U, 1 );
IplImage *tA = cvCreateImage( size, IPL_DEPTH_16S, 1 );
IplImage *tB = cvCreateImage( size, IPL_DEPTH_8U, 1 );
IplImage *tC = cvCreateImage( size, IPL_DEPTH_16S, 1 );
IplImage *tD = cvCreateImage( size, IPL_DEPTH_8U, 1 );
IplImage *tE = cvCreateImage( size, IPL_DEPTH_16S, 1 );
IplImage *tF = cvCreateImage( size, IPL_DEPTH_8U, 1 );
unsigned char new_pixel[8][3] = {
{ 0, 0, 0 },
{ 0, 255, 255 },
{ 255, 255, 0 },
{ 255, 0, 255 },
{ 255, 0, 0 },
{ 0, 255, 0 },
{ 0, 0, 255 },
{ 255, 255, 255 } };
Mat frame2 = frameBGR;
Mat frameXYZ;
Mat frameYBR;
Mat frameHSV;
Mat frameHLS;
Mat frameLAB;
Mat frameLUV;
IplImage* result = cvCreateImage( size, IPL_DEPTH_8U, 3 );
IplImage* resultXYZ = cvCreateImage( size, IPL_DEPTH_8U, 3 );
IplImage* resultYBR = cvCreateImage( size, IPL_DEPTH_8U, 3 );
IplImage* resultHSV = cvCreateImage( size, IPL_DEPTH_8U, 3 );
IplImage* resultHLS = cvCreateImage( size, IPL_DEPTH_8U, 3 );
IplImage* resultLAB = cvCreateImage( size, IPL_DEPTH_8U, 3 );
IplImage* resultLUV = cvCreateImage( size, IPL_DEPTH_8U, 3 );
int cluster_count = 4;
cvtColor( frameBGR, frameXYZ, CV_BGR2XYZ, 0 );
cvtColor( frameBGR, frameYBR, CV_BGR2YCrCb, 0 );
cvtColor( frameBGR, frameHSV, CV_BGR2HSV, 0 );
cvtColor( frameBGR, frameHLS, CV_BGR2HLS, 0 );
cvtColor( frameBGR, frameLAB, CV_BGR2Lab, 0 );
cvtColor( frameBGR, frameLUV, CV_BGR2Luv, 0 );
Mat points = frameBGR.reshape( 1, frameBGR.cols*frameBGR.rows );
//Mat points = frame2.reshape( 1, frame2.cols*frame2.rows );
Mat clusters, centers;
points.convertTo(points, CV_32FC3, 1.0/255.0);
Mat pointsXYZ = frameXYZ.reshape( 1, size.height*size.width );
//pointsXYZ = pointsXYZ.colRange(0, 1);
Mat clustersXYZ, centersXYZ;
pointsXYZ.convertTo(pointsXYZ, CV_32FC3, 1.0/255.0);
Mat pointsYBR = frameYBR.reshape( 1, size.height*size.width );
//pointsYBR = pointsYBR.col(0);
Mat clustersYBR, centersYBR;
pointsYBR.convertTo(pointsYBR, CV_32FC3, 1.0/255.0);
Mat pointsHSV = frameHSV.reshape( 1, size.height*size.width );
//pointsHSV = pointsHSV.colRange( 0, 1 );
Mat clustersHSV, centersHSV;
pointsHSV.convertTo(pointsHSV, CV_32FC3, 1.0/255.0);
Mat pointsHLS = frameHLS.reshape( 1, size.height*size.width );
//pointsHLS = pointsHLS.colRange( 0, 1 );
Mat clustersHLS, centersHLS;
pointsHLS.convertTo(pointsHLS, CV_32FC3, 1.0/255.0);
Mat pointsLAB = frameLAB.reshape( 1, size.height*size.width );
//pointsLAB = pointsLAB.col(0);
Mat clustersLAB, centersLAB;
pointsLAB.convertTo(pointsLAB, CV_32FC3, 1.0/255.0);
Mat pointsLUV = frameLUV.reshape( 1, size.height*size.width );
//pointsLUV = pointsLUV.col(0);
Mat clustersLUV, centersLUV;
pointsLUV.convertTo(pointsLUV, CV_32FC3, 1.0/255.0);
kmeans( points, cluster_count, clusters,
TermCriteria( CV_TERMCRIT_ITER, 1, 10.0 ),
1, KMEANS_RANDOM_CENTERS, centers );
kmeans( pointsXYZ, cluster_count, clustersXYZ,
TermCriteria( CV_TERMCRIT_ITER, 1, 10.0 ),
1, KMEANS_RANDOM_CENTERS, centers );
kmeans( pointsYBR, cluster_count, clustersYBR,
TermCriteria( CV_TERMCRIT_ITER, 1, 10.0 ),
1, KMEANS_RANDOM_CENTERS, centers );
kmeans( pointsHSV, cluster_count, clustersHSV,
TermCriteria( CV_TERMCRIT_ITER, 1, 10.0 ),
1, KMEANS_RANDOM_CENTERS, centers );
kmeans( pointsHLS, cluster_count, clustersHLS,
TermCriteria( CV_TERMCRIT_ITER, 1, 10.0 ),
1, KMEANS_RANDOM_CENTERS, centers );
kmeans( pointsLAB, cluster_count, clustersLAB,
TermCriteria( CV_TERMCRIT_ITER, 1, 10.0 ),
1, KMEANS_RANDOM_CENTERS, centers );
kmeans( pointsLUV, cluster_count, clustersLUV,
TermCriteria( CV_TERMCRIT_ITER, 1, 10.0 ),
1, KMEANS_RANDOM_CENTERS, centers );
int r2 = 0;
for(int row=0; row<frameBGR.rows; row++)
for(int col=0; col<frameBGR.cols; col++)
{
int a = clusters.at<int>(r2, 0);
PUTPIXELMACRO( result, col, row, new_pixel[a], result->widthStep, (result->widthStep/result->width), 3 );
a = clustersXYZ.at<int>(r2, 0);
PUTPIXELMACRO( resultXYZ, col, row, new_pixel[a], result->widthStep, (result->widthStep/result->width), 3 );
a = clustersYBR.at<int>(r2, 0);
PUTPIXELMACRO( resultYBR, col, row, new_pixel[a], result->widthStep, (result->widthStep/result->width), 3 );
a = clustersHSV.at<int>(r2, 0);
PUTPIXELMACRO( resultHSV, col, row, new_pixel[a], result->widthStep, (result->widthStep/result->width), 3 );
a = clustersHLS.at<int>(r2, 0);
PUTPIXELMACRO( resultHLS, col, row, new_pixel[a], result->widthStep, (result->widthStep/result->width), 3 );
a = clustersLAB.at<int>(r2, 0);
PUTPIXELMACRO( resultLAB, col, row, new_pixel[a], result->widthStep, (result->widthStep/result->width), 3 );
a = clustersLUV.at<int>(r2, 0);
PUTPIXELMACRO( resultLUV, col, row, new_pixel[a], result->widthStep, (result->widthStep/result->width), 3 );
r2++;
}
namedWindow( "INPUT" );
namedWindow( "OUTPUT" );
namedWindow( "TEMP" );
namedWindow( "OUTPUTXYZ" );
namedWindow( "OUTPUTYBR" );
namedWindow( "OUTPUTHSV" );
namedWindow( "OUTPUTHLS" );
namedWindow( "OUTPUTLAB" );
namedWindow( "OUTPUTLUV" );
imshow( "INPUT", frameBGR );
cvShowImage( "OUTPUT", result );
imshow( "TEMP", clusters );
cvShowImage( "OUTPUTXYZ", resultXYZ );
cvShowImage( "OUTPUTYBR", resultYBR );
cvShowImage( "OUTPUTHSV", resultHSV );
cvShowImage( "OUTPUTHLS", resultHLS );
cvShowImage( "OUTPUTLAB", resultLAB );
cvShowImage( "OUTPUTLUV", resultLUV );
/*
*t1 = frame;
cvCvtColor( t1, t2, CV_RGB2GRAY );
cvCvtColor( result, t3, CV_RGB2GRAY );
cvSobel( t2, t7, 1, 0, 9 );
cvThreshold( t7, t8, 250, 255, CV_THRESH_BINARY );
cvSobel( t3, t9, 1, 0, 9 );
cvThreshold( t9, t0, 250, 255, CV_THRESH_BINARY );
cvSobel( t2, tE, 1, 0, 7 );
cvThreshold( tE, tF, 250, 255, CV_THRESH_BINARY );
cvSobel( t3, tC, 1, 0, 7 );
cvThreshold( tC, tD, 250, 255, CV_THRESH_BINARY );
cvSobel( t2, tA, 1, 0, 5 );
cvThreshold( tA, tB, 250, 255, CV_THRESH_BINARY );
cvSobel( t3, t5, 1, 0, 5 );
cvThreshold( t5, t6, 250, 255, CV_THRESH_BINARY );
cvShowImage( "T7", t7 );
cvShowImage( "T8", t8 );
cvShowImage( "T9", t9 );
cvShowImage( "T0", t0 );
cvShowImage( "T5", t5 );
cvShowImage( "T6", t6 );
cvShowImage( "TA", tA );
cvShowImage( "TB", tB );
cvShowImage( "TC", tC );
cvShowImage( "TD", tD );
cvShowImage( "TE", tE );
cvShowImage( "TF", tF );
*/
// RELEASE?
}
int main( int argc, char** argv )
{
printf( "Scale Space Cost Aggregation\n" );
if( argc != 11 ) {
printf( "Usage: [CC_METHOD] [CA_METHOD] [PP_METHOD] [C_ALPHA] [lImg] [rImg] [lDis] [rDis] [maxDis] [disSc]\n" );
printf( "\nPress any key to continue...\n" );
getchar();
return -1;
}
string ccName = argv[ 1 ];
string caName = argv[ 2 ];
string ppName = argv[ 3 ];
double costAlpha = atof( argv[ 4 ] );
string lFn = argv[ 5 ];
string rFn = argv[ 6 ];
string lDisFn = argv[ 7 ];
string rDisFn = argv[ 8 ];
int maxDis = atoi( argv[ 9 ] );
int disSc = atoi( argv[ 10 ] );
//
// Load left right image
//
printf( "\n--------------------------------------------------------\n" );
printf( "Load Image: (%s) (%s)\n", argv[ 5 ], argv[ 6 ] );
printf( "--------------------------------------------------------\n" );
Mat lImg = imread( lFn, CV_LOAD_IMAGE_COLOR );
Mat rImg = imread( rFn, CV_LOAD_IMAGE_COLOR );
if( !lImg.data || !rImg.data ) {
printf( "Error: can not open image\n" );
printf( "\nPress any key to continue...\n" );
getchar();
return -1;
}
// set image format
cvtColor( lImg, lImg, CV_BGR2RGB );
cvtColor( rImg, rImg, CV_BGR2RGB );
lImg.convertTo( lImg, CV_64F, 1 / 255.0f );
rImg.convertTo( rImg, CV_64F, 1 / 255.0f );
// time
double duration;
duration = static_cast<double>(getTickCount());
//
// Stereo Match at each pyramid
//
int PY_LVL = 5;
// build pyramid and cost volume
Mat lP = lImg.clone();
Mat rP = rImg.clone();
SSCA** smPyr = new SSCA*[ PY_LVL ];
CCMethod* ccMtd = getCCType( ccName );
CAMethod* caMtd = getCAType( caName );
PPMethod* ppMtd = getPPType( ppName );
for( int p = 0; p < PY_LVL; p ++ ) {
if( maxDis < 5 ) {
PY_LVL = p;
break;
}
printf( "\n\tPyramid: %d:", p );
smPyr[ p ] = new SSCA( lP, rP, maxDis, disSc );
smPyr[ p ]->CostCompute( ccMtd );
smPyr[ p ]->CostAggre( caMtd );
// pyramid downsample
maxDis = maxDis / 2 + 1;
disSc *= 2;
pyrDown( lP, lP );
pyrDown( rP, rP );
}
printf( "\n--------------------------------------------------------\n" );
printf( "\n Cost Aggregation in Scale Space\n" );
printf( "\n--------------------------------------------------------\n" );
// new method
SolveAll( smPyr, PY_LVL, costAlpha );
// old method
//for( int p = PY_LVL - 2 ; p >= 0; p -- ) {
// smPyr[ p ]->AddPyrCostVol( smPyr[ p + 1 ], costAlpha );
//}
//
// Match + Postprocess
//
smPyr[ 0 ]->Match();
smPyr[ 0 ]->PostProcess( ppMtd );
Mat lDis = smPyr[ 0 ]->getLDis();
Mat rDis = smPyr[ 0 ]->getRDis();
#ifdef _DEBUG
for( int s = 0; s < PY_LVL; s ++ ) {
smPyr[ s ]->Match();
Mat sDis = smPyr[ s ]->getLDis();
ostringstream sStr;
sStr << s;
string sFn = sStr.str( ) + "_ld.png";
imwrite( sFn, sDis );
}
saveOnePixCost( smPyr, PY_LVL );
#endif
#ifdef USE_MEDIAN_FILTER
//
// Median Filter Output
//
MeanFilter( lDis, lDis, 3 );
#endif
duration = static_cast<double>(getTickCount())-duration;
duration /= cv::getTickFrequency(); // the elapsed time in sec
printf( "\n--------------------------------------------------------\n" );
printf( "Total Time: %.2lf s\n", duration );
printf( "--------------------------------------------------------\n" );
//
// Save Output
//
imwrite( lDisFn, lDis );
imwrite( rDisFn, rDis );
delete [] smPyr;
delete ccMtd;
delete caMtd;
delete ppMtd;
return 0;
}
void ShowImage(Mat image,int type,string WindowName)
{
namedWindow( WindowName, WINDOW_AUTOSIZE );// Create a window for display.
image.convertTo(image,type);
imshow( WindowName, image);
}
void HandPixel(Mat depth, Mat RGB,Mat Mask, float dis,float dis2, int it,String name){
const float bad_point = std::numeric_limits<float>::quiet_NaN();
pcl::PointCloud<pcl::PointXYZRGB>::Ptr nube(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr nubef(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr nubef2(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr nubef3(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr nube2(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZ>::Ptr nubep(new pcl::PointCloud<pcl::PointXYZ>);
Mat Ultima = RGB.clone();
Mat Depth2 = depth.clone();
Mat inpaintMask = Mat::zeros(RGB.size(), CV_8U);
uint16_t b = depth.at<uint16_t>(0,0);
for(int i = 0; i < Ultima.rows; i++)
{
for(int j = 0; j < Ultima.cols; j++)
{
uint16_t x = depth.at<uint16_t>(i,j);
if (x == b){
inpaintMask.at<uint8_t>(i,j) = 1;
}
}
}
int Hist[640]={};
Mat d = depth;
d.convertTo(d, CV_8U,1.0/256, 0);
int min = 1000000;
int max = 0;
//inpaint(d, inpaintMask, Depth2, 1, INPAINT_TELEA);
imwrite("Depth.png",Depth2);
uint8_t black = depth.at<uint8_t>(0,0);
nubef->width = Ultima.cols;
nubef->height = Ultima.rows;
nubef->resize(nubef->width*nubef->height);
for(int i = 0; i < Ultima.rows; i++)
{
for(int j = 0; j < Ultima.cols; j++)
{
cv::Vec3b& bgr = RGB.at<cv::Vec3b>(i,j);
cv::Vec3b& na = Mask.at<cv::Vec3b>(i,j);
pcl::PointXYZRGB p;
p.r = bgr[2];
p.g = bgr[1];
p.b = bgr[0];
p.x=j;
p.y=i;
int dep = Depth2.at<uint16_t>(i,j);
p.z= dep/100;
if((dep/100) > 44 && (dep/100) < 462 && p.y >290){
//Hist[dep/100] +=1;
//float distancia = ((0.00170869*p.x+0.435266*p.y-236.199)/0.9003)-dep;
//cout << distancia << endl;
if(min > p.z) min = p.z;
if(max < p.z) max =p.z;
nube->push_back(p);}
nubef->at(j,i) = p;
/*else if (p.y > 320){
float M = -0.0528487*p.x-0.282845*p.x- 193.149;
M = M/-0.957709 ;
p.z =M;
nube -> push_back(p);
}*/
//nube->at(j,i)=p;//}
/*int distancia = 0.15228*p.z+0.0539963*p.x-0.986861*p.y;
cv::Vec3b blck = (distancia,distancia,distancia);
Ultima.at<cv::Vec3b>(i,j) = blck;*/
}
}
if (nube->isOrganized()){
cout << nube->height << " " << nube->width << endl;
cout << "Bucle Acabado1 " << nube->size() << endl;
cout << " Min: "<< min << " Max: " << max <<endl;}
/*pcl::PassThrough<pcl::PointXYZRGB> pass;
pass.setInputCloud (nube);
pass.setFilterFieldName ("z");
//pass.setKeepOrganized(true);
pass.setFilterLimits (30.0, 150.0);
//pass.setFilterLimitsNegative (true);
pass.filter (*nubef);
pass.setInputCloud (nubef);
pass.setFilterFieldName ("y");
//pass.setKeepOrganized(true);
pass.setFilterLimits ( 200.0, 600.0);
//pass.setFilterLimitsNegative (true);
pass.filter (*nubef2);*/
nube2 = Region(nube,dis,dis2,7);
cout << "Bucle Acabado2 " << nube2->size() << endl;
/*pcl::SampleConsensusModelPlane<pcl::PointXYZRGB>::Ptr dit (new pcl::SampleConsensusModelPlane<pcl::PointXYZRGB> (nube));
Eigen::Vector4f coefficients = Eigen::Vector4f(0.00170869,0.435266,0.9003,-236.199);
std::vector<int> inliers;
dit -> selectWithinDistance (coefficients, dis, inliers);
pcl::copyPointCloud<pcl::PointXYZRGB>(*nube, inliers, *nube2); */
cout << "Bucle Acabado3 " << nube2->size() << endl;
//for (int i =0;i < 640 ; i++) cout <<i << " : " << Hist[i] << ";"<<endl;
//Vis(nube);
cv::Vec3b blck = (0,0,0);
if (nube2->size() > 0){
for (int pit = 0; pit != nube2->size(); ++pit){
nubef->at(nube2->at(pit).x,nube2->at(pit).y).x = bad_point;
nubef->at(nube2->at(pit).x,nube2->at(pit).y).y = bad_point;
nubef->at(nube2->at(pit).x,nube2->at(pit).y).z = bad_point;
Ultima.at<cv::Vec3b>(nube2->at(pit).y,nube2->at(pit).x) = blck;
}
imwrite(name,Ultima);
pcl::io::savePCDFile (name+".pcd", *nubef,false);
imwrite("depth.png",Depth2);
imwrite("rgb.jpg",RGB);
}
/*nubep->resize(nube2->size());
for (size_t i = 0; i < nube2->points.size(); i++) {
nubep->points[i].x = nube2->points[i].x;
nubep->points[i].y = nube2->points[i].y;
nubep->points[i].z = nube2->points[i].z;
}*/
//pcl::io::savePCDFile ("test_pcd.pcd", *nubep,false);
//}
//Search_segmentation(nube);
/*pcl::visualization::CloudViewer viewer ("Cluster viewer");
viewer.showCloud(nube2);
while (!viewer.wasStopped ())
{
}
pcl::visualization::CloudViewer viewer2 ("Cluster2 viewer");
viewer2.showCloud(nube);
while (!viewer2.wasStopped ())
{
}*/
}
bool ObjPatchMatcher::Match(const Mat& cimg, const Mat& dmap_raw, Mat& mask_map) {
/*
* precompute feature maps
*/
// gradient
Mat gray_img, gray_img_float, edge_map;
cvtColor(cimg, gray_img, CV_BGR2GRAY);
gray_img.convertTo(gray_img_float, CV_32F, 1.f/255);
Canny(gray_img, edge_map, 10, 50);
cv::imshow("edge", edge_map);
cv::imshow("color", cimg);
cv::waitKey(10);
Mat grad_x, grad_y, grad_mag;
Sobel(gray_img_float, grad_x, CV_32F, 1, 0);
Sobel(gray_img_float, grad_y, CV_32F, 0, 1);
magnitude(grad_x, grad_y, grad_mag);
// depth
Mat dmap_float, pts3d, normal_map;
if( use_depth ) {
Feature3D feat3d;
dmap_raw.convertTo(dmap_float, CV_32F);
Mat cmp_mask;
compare(dmap_float, 800, cmp_mask, CMP_LT);
dmap_float.setTo(800, cmp_mask);
compare(dmap_float, 7000, cmp_mask, CMP_GT);
dmap_float.setTo(7000, cmp_mask);
dmap_float = (dmap_float-800)/(7000-800);
feat3d.ComputeKinect3DMap(dmap_float, pts3d, false);
feat3d.ComputeNormalMap(pts3d, normal_map);
}
/*
* start searching
*/
// init searcher
//searcher.Build(patch_data, BruteForce_L2); // opencv bfmatcher has size limit: maximum 2^31
LSHCoder lsh_coder;
if(use_code) {
lsh_coder.Load();
}
Mat score_map = Mat::zeros(edge_map.rows, edge_map.cols, CV_32F);
Mat mask_vote_map = Mat::zeros(cimg.rows, cimg.cols, CV_32F);
mask_map = Mat::zeros(cimg.rows, cimg.cols, CV_32F);
Mat mask_count = Mat::zeros(cimg.rows, cimg.cols, CV_32S); // number of mask overlapped on each pixel
Mat feat;
int topK = 40;
int total_cnt = countNonZero(edge_map);
vector<VisualObject> query_patches;
query_patches.reserve(total_cnt);
cout<<"Start match..."<<endl;
float max_dist = 0;
int cnt = 0;
char str[30];
double start_t = getTickCount();
//#pragma omp parallel for
for(int r=patch_size.height/2; r<gray_img.rows-patch_size.height/2; r+=3) {
for(int c=patch_size.width/2; c<gray_img.cols-patch_size.width/2; c+=3) {
/*int rand_r = rand()%gray_img.rows;
int rand_c = rand()%gray_img.cols;
if(rand_r < patch_size.height/2 || rand_r > gray_img.rows-patch_size.height/2 ||
rand_c < patch_size.width/2 || rand_c > gray_img.cols-patch_size.width/2) continue;*/
int rand_r = r, rand_c = c;
if(edge_map.at<uchar>(rand_r, rand_c) > 0)
{
cnt++;
destroyAllWindows();
Rect box(rand_c-patch_size.width/2, rand_r-patch_size.height/2, patch_size.width, patch_size.height);
MatFeatureSet featset;
gray_img_float(box).copyTo(featset["gray"]);
//grad_mag(box).copyTo(featset["gradient"]);
if(use_depth)
{
normal_map(box).copyTo(featset["normal"]);
dmap_float(box).copyTo(featset["depth"]);
}
ComputePatchFeat(featset, feat);
vector<DMatch> matches;
if(use_code)
{
BinaryCodes codes;
HashKey key_val;
lsh_coder.ComputeCodes(feat, codes);
HashingTools<HashKeyType>::CodesToKey(codes, key_val);
MatchCode(key_val, topK, matches);
}
else
{
MatchPatch(feat, topK, matches);
}
if(matches[0].distance < 0 || matches[0].distance > 1000) {
cout<<"match dist: "<<matches[0].distance<<endl;
double minv, maxv;
cout<<norm(feat, patch_data.row(matches[0].trainIdx), NORM_L2)<<endl;
minMaxLoc(feat, &minv, &maxv);
cout<<minv<<" "<<maxv<<endl;
cout<<feat<<endl<<endl;
minMaxLoc(patch_data.row(matches[0].trainIdx), &minv, &maxv);
cout<<minv<<" "<<maxv<<endl;
cout<<patch_data.row(matches[0].trainIdx)<<endl;
imshow("cimg", cimg);
waitKey(0);
}
vector<vector<Mat>> pixel_mask_vals(patch_size.height, vector<Mat>(patch_size.width, Mat::zeros(1, topK, CV_32F)));
VisualObject cur_query;
cur_query.visual_data.bbox = box;
cur_query.visual_data.mask = Mat::zeros(patch_size.height, patch_size.width, CV_32F);
for(size_t i=0; i<topK; i++) {
score_map.at<float>(rand_r,rand_c) += matches[i].distance;
cur_query.visual_data.mask += patch_meta.objects[matches[i].trainIdx].visual_data.mask;
for(int mr=0; mr<patch_size.height; mr++) for(int mc=0; mc<patch_size.width; mc++) {
pixel_mask_vals[mr][mc].at<float>(i) =
patch_meta.objects[matches[i].trainIdx].visual_data.mask.at<float>(mr, mc);
}
}
score_map.at<float>(rand_r,rand_c) /= topK;
cur_query.visual_data.mask /= topK; // average returned mask
// compute mask quality
Scalar mean_, std_;
/*ofstream out("pixel_mask_std_100.txt", ios::app);
for(int mr=0; mr<patch_size.height; mr++) for(int mc=0; mc<patch_size.width; mc++) {
meanStdDev(pixel_mask_vals[mr][mc], mean_, std_);
out<<std_.val[0]<<" ";
}
out<<endl;*/
meanStdDev(cur_query.visual_data.mask, mean_, std_);
cur_query.visual_data.scores.push_back(mean_.val[0]);
cur_query.visual_data.scores.push_back(std_.val[0]);
Mat align_mask = Mat::zeros(cimg.rows, cimg.cols, CV_8U);
int gt_mask_id = patch_meta.objects[matches[0].trainIdx].meta_data.category_id;
if(gt_mask_id != -1) {
Mat nn_mask = gt_obj_masks[gt_mask_id];
//imshow("gt mask", nn_mask*255);
//waitKey(10);
Rect gt_box = patch_meta.objects[matches[0].trainIdx].visual_data.bbox;
Rect align_box = AlignBox(box, gt_box, cimg.cols, cimg.rows);
vector<ImgWin> boxes; boxes.push_back(align_box);
//ImgVisualizer::DrawWinsOnImg("alignbox", cimg, boxes);
//waitKey(10);
Rect target_box = Rect(box.x-(gt_box.x-align_box.x), box.y-(gt_box.y-align_box.y), align_box.width, align_box.height);
cout<<target_box<<endl;
nn_mask(align_box).copyTo(align_mask(target_box));
}
align_mask.convertTo(align_mask, CV_32F);
mask_map += align_mask * matches[0].distance; //*score_map.at<float>(r,c);
//mask_count(box) = mask_count(box) + 1;
//cout<<score_map.at<float>(r,c)<<endl;
max_dist = MAX(max_dist, matches[0].distance);
query_patches.push_back(cur_query);
// vote object regions
/*Point3f line_ori;
int obj_pt_sign;
ComputeDominantLine(cur_query.visual_desc.mask, box.tl(), line_ori, obj_pt_sign);
for(int rr=0; rr<cimg.rows; rr++) for(int cc=0; cc<cimg.cols; cc++) {
float line_val = line_ori.x*cc+line_ori.y*rr+line_ori.z;
if((line_val>0?1:-1)==obj_pt_sign) mask_vote_map.at<float>(rr, cc)++;
}*/
#ifdef VERBOSE
// current patch
Mat disp, patch_gray, patch_grad, patch_normal, patch_depth;
disp = cimg.clone();
rectangle(disp, box, CV_RGB(255,0,0), 2);
resize(gray_img(box), patch_gray, Size(50,50));
resize(grad_mag(box), patch_grad, Size(50,50));
Mat cur_mask;
resize(cur_query.visual_desc.mask, cur_mask, Size(50,50));
if(use_depth)
{
resize(normal_map(box), patch_normal, Size(50,50));
normalize(dmap_float(box), patch_depth, 1, 0, NORM_MINMAX);
patch_depth.convertTo(patch_depth, CV_8U, 255);
//dmap_float(box).convertTo(patch_depth, CV_8U, 255);
resize(patch_depth, patch_depth, Size(50,50));
}
Mat onormal;
sprintf_s(str, "query_gray_%d.jpg", cnt);
imshow(str, patch_gray);
imwrite(str, patch_gray);
/*sprintf_s(str, "query_grad_%d.jpg", cnt);
ImgVisualizer::DrawFloatImg(str, patch_grad, onormal, true);
imwrite(str, onormal);*/
sprintf_s(str, "query_depth_%d.jpg", cnt);
imshow(str, patch_depth);
imwrite(str, patch_depth);
sprintf_s(str, "query_normal_%d.jpg", cnt);
ImgVisualizer::DrawNormals(str, patch_normal, onormal, true);
imwrite(str, onormal);
sprintf_s(str, "query_box_%d.jpg", cnt);
imshow(str, disp);
imwrite(str, disp);
//imshow("align mask", align_mask*255);
cur_mask.convertTo(cur_mask, CV_8U, 255);
sprintf_s(str, "query_tmask_%d.jpg", cnt);
imshow(str, cur_mask);
imwrite(str, cur_mask);
// show match results
vector<Mat> res_imgs(topK);
vector<Mat> res_gradients(topK);
vector<Mat> res_normals(topK);
vector<Mat> res_depth(topK);
vector<Mat> db_boxes(topK);
vector<Mat> res_masks(topK);
for(size_t i=0; i<topK; i++) {
VisualObject& cur_obj = patch_meta.objects[matches[i].trainIdx];
// mask
cur_obj.visual_desc.mask.convertTo(res_masks[i], CV_8U, 255);
// gray
cur_obj.visual_desc.extra_features["gray"].convertTo(res_imgs[i], CV_8U, 255);
// gradient
//ImgVisualizer::DrawFloatImg("", cur_obj.visual_desc.extra_features["gradient"], res_gradients[i], false);
// 3D
if(use_depth)
{
// normal
tools::ImgVisualizer::DrawNormals("", cur_obj.visual_desc.extra_features["normal"], res_normals[i]);
// depth
normalize(cur_obj.visual_desc.extra_features["depth"], res_depth[i], 1, 0, NORM_MINMAX);
res_depth[i].convertTo(res_depth[i], CV_8U, 255);
//cur_obj.visual_desc.extra_features["depth"].convertTo(res_depth[i], CV_8U, 255);
}
// box on image
db_boxes[i] = imread(patch_meta.objects[matches[i].trainIdx].imgpath);
resize(db_boxes[i], db_boxes[i], Size(cimg.cols, cimg.rows));
rectangle(db_boxes[i], patch_meta.objects[matches[i].trainIdx].visual_desc.box, CV_RGB(255,0,0), 2);
}
Mat out_img;
sprintf_s(str, "res_gray_%d.jpg", cnt);
ImgVisualizer::DrawImgCollection(str, res_imgs, topK, Size(50,50), out_img);
imwrite(str, out_img);
sprintf_s(str, "res_normal_%d.jpg", cnt);
ImgVisualizer::DrawImgCollection(str, res_normals, topK, Size(50,50), out_img);
imwrite(str, out_img);
sprintf_s(str, "res_depth_%d.jpg", cnt);
ImgVisualizer::DrawImgCollection(str, res_depth, topK, Size(50,50), out_img);
imwrite(str, out_img);
/*sprintf_s(str, "res_gradient_%d.jpg", cnt);
tools::ImgVisualizer::DrawImgCollection(str, res_gradients, topK, Size(50,50), out_img);
imwrite(str, out_img);*/
sprintf_s(str, "res_mask_%d.jpg", cnt);
tools::ImgVisualizer::DrawImgCollection(str, res_masks, topK, Size(50,50), out_img);
imwrite(str, out_img);
sprintf_s(str, "res_box_%d.jpg", cnt);
tools::ImgVisualizer::DrawImgCollection(str, db_boxes, topK/2, Size(200, 200), out_img);
imwrite(str, out_img);
waitKey(0);
#endif
cout<<total_cnt--<<endl;
}
}
}
cout<<"match done. Time cost: "<<(getTickCount()-start_t)/getTickFrequency()<<"s."<<endl;
//score_map(Rect(patch_size.width/2, patch_size.height/2, score_map.cols-patch_size.width/2, score_map.rows-patch_size.height/2)).copyTo(score_map);
//score_map.setTo(max_dist, 255-edge_map);
normalize(score_map, score_map, 1, 0, NORM_MINMAX);
score_map = 1-score_map;
//tools::ImgVisualizer::DrawFloatImg("bmap", score_map);
mask_map /= max_dist;
cout<<max_dist<<endl;
normalize(mask_map, mask_map, 1, 0, NORM_MINMAX);
//tools::ImgVisualizer::DrawFloatImg("maskmap", mask_map);
//normalize(mask_vote_map, mask_vote_map, 1, 0, NORM_MINMAX);
//ImgVisualizer::DrawFloatImg("vote map", mask_vote_map);
//waitKey(0);
return true;
// pick top weighted points to see if they are inside objects
// try graph-cut for region proposal
// among all retrieved mask patch, select most discriminative one and do graph-cut
sort(query_patches.begin(), query_patches.end(), [](const VisualObject& a, const VisualObject& b) {
return a.visual_data.scores[1] > b.visual_data.scores[1]; });
for(size_t i=0; i<query_patches.size(); i++) {
Mat disp_img = cimg.clone();
rectangle(disp_img, query_patches[i].visual_data.bbox, CV_RGB(255,0,0));
imshow("max std box", disp_img);
Mat big_mask;
resize(query_patches[i].visual_data.mask, big_mask, Size(50,50));
ImgVisualizer::DrawFloatImg("max std mask", big_mask);
waitKey(0);
// use mask to do graph-cut
Mat fg_mask(cimg.rows, cimg.cols, CV_8U);
fg_mask.setTo(cv::GC_PR_FGD);
Mat th_mask;
threshold(query_patches[i].visual_data.mask, th_mask, query_patches[i].visual_data.scores[0], 1, CV_THRESH_BINARY);
th_mask.convertTo(th_mask, CV_8U);
fg_mask(query_patches[i].visual_data.bbox).setTo(cv::GC_FGD, th_mask);
th_mask = 1-th_mask;
fg_mask(query_patches[i].visual_data.bbox).setTo(cv::GC_BGD, th_mask);
cv::grabCut(cimg, fg_mask, Rect(0,0,1,1), Mat(), Mat(), 3, cv::GC_INIT_WITH_MASK);
fg_mask = fg_mask & 1;
disp_img.setTo(Vec3b(0,0,0));
cimg.copyTo(disp_img, fg_mask);
cv::imshow("cut", disp_img);
cv::waitKey(0);
}
float ths[] = {0.9f, 0.8f, 0.7f, 0.6f, 0.5f, 0.4f, 0.3f, 0.2f};
for(size_t i=0; i<8; i++) {
Mat th_mask;
threshold(mask_map, th_mask, ths[i], 1, CV_THRESH_BINARY);
char str[30];
sprintf_s(str, "%f", ths[i]);
ImgVisualizer::DrawFloatImg(str, th_mask);
waitKey(0);
}
return true;
}
Mat process()
{
watershed(image, watershed_markers);
watershed_markers.convertTo(watershed_markers,CV_8U);
return watershed_markers;
}
int main(int argc, char** argv) {
initSurround360(argc, argv);
requireArg(FLAGS_data_path, "data_path");
if (FLAGS_test_image_path != "" && FLAGS_test_isp_path == "") {
throw VrCamException("Need ISP file for test image");
}
string dataDir = FLAGS_data_path.substr(0, FLAGS_data_path.find_last_of('/'));
const string outputDir =
FLAGS_output_dir.empty() ? dataDir + "/output" : FLAGS_output_dir;
system(string("mkdir -p \"" + outputDir + "\"").c_str());
LOG(INFO) << "Loading data...";
// Assuming JSON file with format
// [
// {
// "location" : [x1, y1],
// "rgbmedian" : [r1, g1, b1]
// },
// ...
// {
// "location" : [xN, yN],
// "rgbmedian" : [rN, gN, bN]
// }
// ]
string json;
folly::readFile(FLAGS_data_path.c_str(), json);
if (json.empty()) {
throw VrCamException("Failed to load JSON file: " + FLAGS_data_path);
}
vector<Point2f> vignetteMapLocs;
vector<Vec3f> vignetteMapRGBValues;
folly::dynamic data = folly::parseJson(json);
static const int kNumChannels = 3;
vector<float> minRGB(kNumChannels, INT_MAX);
vector<float> maxRGB(kNumChannels, 0);
for (const auto& sample : data) {
const folly::dynamic loc = sample["location"];
const folly::dynamic median = sample["rgbmedian"];
vignetteMapLocs.push_back(Point2f(loc[0].asDouble(), loc[1].asDouble()));
const Vec3f v = Vec3f(
median[0].asDouble(),
median[1].asDouble(),
median[2].asDouble());
vignetteMapRGBValues.push_back(v);
// Find min and max of each channel as we load data
for (int ch = 0; ch < kNumChannels; ++ch) {
minRGB[ch] = std::min(minRGB[ch], v[ch]);
maxRGB[ch] = std::max(maxRGB[ch], v[ch]);
}
}
if (FLAGS_save_debug_images) {
vector<Mat> vignetteMapPlotRGB;
for (int i = 0; i < kNumChannels; ++i) {
Mat ch = Mat::zeros(FLAGS_image_height, FLAGS_image_width, CV_8UC1);
vignetteMapPlotRGB.push_back(ch);
}
// Stretch data for display purposes
for (int i = 0; i < vignetteMapLocs.size(); ++i) {
for (int ch = 0; ch < kNumChannels; ++ch) {
const float v = vignetteMapRGBValues[i][ch];
const float valNorm =
255.0f * (v - minRGB[ch]) / (maxRGB[ch] - minRGB[ch]);
vignetteMapPlotRGB[ch].at<uchar>(vignetteMapLocs[i]) = valNorm;
static const int kRadius = 5;
static const int kThicknessFill = -1;
circle(
vignetteMapPlotRGB[ch],
vignetteMapLocs[i],
kRadius,
valNorm,
kThicknessFill);
circle(
vignetteMapPlotRGB[ch],
vignetteMapLocs[i],
kRadius,
255);
}
}
for (int ch = 0; ch < kNumChannels; ++ch) {
Mat vignetteMapPlotJet;
applyColorMap(vignetteMapPlotRGB[ch], vignetteMapPlotJet, COLORMAP_JET);
const char chName = "rgb"[ch];
const string vignetteMapPlotFilename =
outputDir + "/sample_map_" + chName + "_stretched.png";
imwriteExceptionOnFail(vignetteMapPlotFilename, vignetteMapPlotJet);
}
}
// Approximating falloff using Nth order Bezier surface
// p(u, v) = (sum_i=0^N b_i * B_i^N(u)) * (sum_j=0^N b_j * B_j^N(v))
static const int kBezierX = 4;
static const int kBezierY = 4;
vector<Point3f> vignetteRollOffH(kBezierX + 1);
vector<Point3f> vignetteRollOffV(kBezierY + 1);
const int maxDimension = std::max(FLAGS_image_width, FLAGS_image_height);
for (int ch = 0; ch < kNumChannels; ++ch) {
ceres::Problem problem;
// Initialize estimates
vector<double> paramsX(kBezierX + 1, 0.5);
vector<double> paramsY(kBezierY + 1, 0.5);
LOG(INFO) << "Creating residuals...";
for (int i = 0; i < vignetteMapLocs.size(); ++i) {
const double x = double(vignetteMapLocs[i].x) / double(maxDimension);
const double y = double(vignetteMapLocs[i].y) / double(maxDimension);
const float v = vignetteMapRGBValues[i][ch] / maxRGB[ch];
// Use inverse to directly get vignetting correction surface
BezierFunctor<kBezierX, kBezierX>::addResidual(
problem, paramsX, paramsY, x, y, 1.0f / v);
}
LOG(INFO) << "Solving...";
ceres::Solver::Options options;
options.max_num_iterations = 1000;
options.minimizer_progress_to_stdout = FLAGS_save_debug_images;
ceres::Solver::Summary summary;
Solve(options, &problem, &summary);
LOG(INFO) << summary.FullReport();
LOG(INFO) << "curveFit parameters...";
std::ostringstream paramsStreamX;
for (int i = 0; i <= kBezierX; ++i) {
if (ch == 0) {
vignetteRollOffH[i].x = paramsX[i];
} else if (ch == 1) {
vignetteRollOffH[i].y = paramsX[i];
} else {
vignetteRollOffH[i].z = paramsX[i];
}
paramsStreamX << "X" << i << ":" << paramsX[i] << " ";
}
std::ostringstream paramsStreamY;
for (int i = 0; i <= kBezierY; ++i) {
if (ch == 0) {
vignetteRollOffV[i].x = paramsY[i];
} else if (ch == 1) {
vignetteRollOffV[i].y = paramsY[i];
} else {
vignetteRollOffV[i].z = paramsY[i];
}
paramsStreamY << "Y" << i << ":" << paramsY[i] << " ";
}
LOG(INFO) << paramsStreamX.str();
LOG(INFO) << paramsStreamY.str();
// Save report to file
const char chName = "rgb"[ch];
const string ceresFilename = outputDir + "/ceres_report_" + chName + ".txt";
ofstream ceresStream(ceresFilename, ios::out);
if (!ceresStream) {
throw VrCamException("file open failed: " + ceresFilename);
}
ceresStream << summary.FullReport();
ceresStream << paramsStreamX.str() << "\n" << paramsStreamY.str();
ceresStream.close();
if (FLAGS_save_debug_images) {
LOG(INFO) << "Creating surface fit for channel " << ch << "...";
BezierCurve<float, double> bezierX(paramsX);
BezierCurve<float, double> bezierY(paramsY);
Mat surfaceFit = Mat::zeros(FLAGS_image_height, FLAGS_image_width, CV_32FC1);
for (int x = 0; x < FLAGS_image_width; ++x) {
const float vx = bezierX(float(x) / float(maxDimension));
for (int y = 0; y < FLAGS_image_height; ++y) {
const float vy = bezierY(float(y) / float(maxDimension));
surfaceFit.at<float>(Point(x, y)) = vx * vy;
}
}
double minVal;
double maxVal;
Point maxLoc;
Point minLoc;
minMaxLoc(surfaceFit, &minVal, &maxVal, &minLoc, &maxLoc);
LOG(INFO) << "surfaceFit min: " << minVal << ", at " << minLoc;
LOG(INFO) << "surfaceFit max: " << maxVal << ", at " << maxLoc;
Mat surfacePlot = 255.0f * surfaceFit / maxVal;
surfacePlot.convertTo(surfacePlot, CV_8UC1);
// Plot parameters
static const Point kTextLoc = Point(0, 50);
static const Scalar kColor = 0;
putTextShift(surfacePlot, paramsStreamX.str(), kTextLoc, kColor);
putTextShift(surfacePlot, paramsStreamY.str(), kTextLoc * 2, kColor);
// Plot image center and surface minima location
putCircleAndText(surfacePlot, minLoc, kColor);
Point imageCenter = Point(FLAGS_image_width / 2, FLAGS_image_height / 2);
putCircleAndText(surfacePlot, imageCenter, kColor);
Mat surfacePlotJet;
applyColorMap(surfacePlot, surfacePlotJet, COLORMAP_JET);
const string surfaceFitPlotFilename =
outputDir + "/curveFit_fit_" + chName + "_stretched.png";
imwriteExceptionOnFail(surfaceFitPlotFilename, surfacePlotJet);
}
}
if (FLAGS_test_image_path == "") {
return EXIT_SUCCESS;
}
Mat rawTest = imreadExceptionOnFail(
FLAGS_test_image_path, CV_LOAD_IMAGE_GRAYSCALE | CV_LOAD_IMAGE_ANYDEPTH);
const uint8_t rawDepth = rawTest.type() & CV_MAT_DEPTH_MASK;
if (rawDepth == CV_8U) {
rawTest = convert8bitTo16bit(rawTest);
} else if (rawDepth != CV_16U) {
throw VrCamException("Test image is not 8-bit or 16-bit");
}
LOG(INFO) << "Updating ISP config file...";
string ispOutFilename = outputDir + "/isp_out.json";
CameraIsp cameraIsp(getJson(FLAGS_test_isp_path), getBitsPerPixel(rawTest));
cameraIsp.setVignetteRollOffH(vignetteRollOffH);
cameraIsp.setVignetteRollOffV(vignetteRollOffV);
cameraIsp.setup();
cameraIsp.loadImage(rawTest.clone());
cameraIsp.dumpConfigFile(ispOutFilename);
if (FLAGS_save_debug_images) {
LOG(INFO) << "Testing vignetting...";
const string rawTestFilename = outputDir + "/test_in.png";
imwriteExceptionOnFail(rawTestFilename, 255.0f * cameraIsp.getRawImage());
static const int kOutputBpp = 16;
Mat rawTestIspOut(rawTest.size(), CV_16UC3);
#ifdef USE_HALIDE
static const bool kFast = false;
CameraIspPipe cameraIspTest(getJson(ispOutFilename), kFast, kOutputBpp);
#else
CameraIsp cameraIspTest(getJson(ispOutFilename), kOutputBpp);
#endif
cameraIspTest.setup();
cameraIspTest.loadImage(rawTest);
cameraIspTest.getImage(rawTestIspOut);
const string rawTestIspOutFilename = outputDir + "/test_out_rgb.png";
imwriteExceptionOnFail(rawTestIspOutFilename, rawTestIspOut);
}
return EXIT_SUCCESS;
}
double cv::findTransformECC(InputArray templateImage,
InputArray inputImage,
InputOutputArray warpMatrix,
int motionType,
TermCriteria criteria)
{
Mat src = templateImage.getMat();//template iamge
Mat dst = inputImage.getMat(); //input image (to be warped)
Mat map = warpMatrix.getMat(); //warp (transformation)
CV_Assert(!src.empty());
CV_Assert(!dst.empty());
if( ! (src.type()==dst.type()))
CV_Error( CV_StsUnmatchedFormats, "Both input images must have the same data type" );
//accept only 1-channel images
if( src.type() != CV_8UC1 && src.type()!= CV_32FC1)
CV_Error( CV_StsUnsupportedFormat, "Images must have 8uC1 or 32fC1 type");
if( map.type() != CV_32FC1)
CV_Error( CV_StsUnsupportedFormat, "warpMatrix must be single-channel floating-point matrix");
CV_Assert (map.cols == 3);
CV_Assert (map.rows == 2 || map.rows ==3);
CV_Assert (motionType == MOTION_AFFINE || motionType == MOTION_HOMOGRAPHY ||
motionType == MOTION_EUCLIDEAN || motionType == MOTION_TRANSLATION);
if (motionType == MOTION_HOMOGRAPHY){
CV_Assert (map.rows ==3);
}
CV_Assert (criteria.type & TermCriteria::COUNT || criteria.type & TermCriteria::EPS);
const int numberOfIterations = (criteria.type & TermCriteria::COUNT) ? criteria.maxCount : 200;
const double termination_eps = (criteria.type & TermCriteria::EPS) ? criteria.epsilon : -1;
int paramTemp = 6;//default: affine
switch (motionType){
case MOTION_TRANSLATION:
paramTemp = 2;
break;
case MOTION_EUCLIDEAN:
paramTemp = 3;
break;
case MOTION_HOMOGRAPHY:
paramTemp = 8;
break;
}
const int numberOfParameters = paramTemp;
const int ws = src.cols;
const int hs = src.rows;
const int wd = dst.cols;
const int hd = dst.rows;
Mat Xcoord = Mat(1, ws, CV_32F);
Mat Ycoord = Mat(hs, 1, CV_32F);
Mat Xgrid = Mat(hs, ws, CV_32F);
Mat Ygrid = Mat(hs, ws, CV_32F);
float* XcoPtr = Xcoord.ptr<float>(0);
float* YcoPtr = Ycoord.ptr<float>(0);
int j;
for (j=0; j<ws; j++)
XcoPtr[j] = (float) j;
for (j=0; j<hs; j++)
YcoPtr[j] = (float) j;
repeat(Xcoord, hs, 1, Xgrid);
repeat(Ycoord, 1, ws, Ygrid);
Xcoord.release();
Ycoord.release();
Mat templateZM = Mat(hs, ws, CV_32F);// to store the (smoothed)zero-mean version of template
Mat templateFloat = Mat(hs, ws, CV_32F);// to store the (smoothed) template
Mat imageFloat = Mat(hd, wd, CV_32F);// to store the (smoothed) input image
Mat imageWarped = Mat(hs, ws, CV_32F);// to store the warped zero-mean input image
Mat allOnes = Mat::ones(hd, wd, CV_8U); //to use it for mask warping
Mat imageMask = Mat(hs, ws, CV_8U); //to store the final mask
//gaussian filtering is optional
src.convertTo(templateFloat, templateFloat.type());
GaussianBlur(templateFloat, templateFloat, Size(5, 5), 0, 0);//is in-place filtering slower?
dst.convertTo(imageFloat, imageFloat.type());
GaussianBlur(imageFloat, imageFloat, Size(5, 5), 0, 0);
// needed matrices for gradients and warped gradients
Mat gradientX = Mat::zeros(hd, wd, CV_32FC1);
Mat gradientY = Mat::zeros(hd, wd, CV_32FC1);
Mat gradientXWarped = Mat(hs, ws, CV_32FC1);
Mat gradientYWarped = Mat(hs, ws, CV_32FC1);
// calculate first order image derivatives
Matx13f dx(-0.5f, 0.0f, 0.5f);
filter2D(imageFloat, gradientX, -1, dx);
filter2D(imageFloat, gradientY, -1, dx.t());
// matrices needed for solving linear equation system for maximizing ECC
Mat jacobian = Mat(hs, ws*numberOfParameters, CV_32F);
Mat hessian = Mat(numberOfParameters, numberOfParameters, CV_32F);
Mat hessianInv = Mat(numberOfParameters, numberOfParameters, CV_32F);
Mat imageProjection = Mat(numberOfParameters, 1, CV_32F);
Mat templateProjection = Mat(numberOfParameters, 1, CV_32F);
Mat imageProjectionHessian = Mat(numberOfParameters, 1, CV_32F);
Mat errorProjection = Mat(numberOfParameters, 1, CV_32F);
Mat deltaP = Mat(numberOfParameters, 1, CV_32F);//transformation parameter correction
Mat error = Mat(hs, ws, CV_32F);//error as 2D matrix
const int imageFlags = CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS+CV_WARP_INVERSE_MAP;
const int maskFlags = CV_INTER_NN+CV_WARP_FILL_OUTLIERS+CV_WARP_INVERSE_MAP;
// iteratively update map_matrix
double rho = -1;
double last_rho = - termination_eps;
for (int i = 1; (i <= numberOfIterations) && (fabs(rho-last_rho)>= termination_eps); i++)
{
// warp-back portion of the inputImage and gradients to the coordinate space of the templateImage
if (motionType != MOTION_HOMOGRAPHY)
{
warpAffine(imageFloat, imageWarped, map, imageWarped.size(), imageFlags);
warpAffine(gradientX, gradientXWarped, map, gradientXWarped.size(), imageFlags);
warpAffine(gradientY, gradientYWarped, map, gradientYWarped.size(), imageFlags);
warpAffine(allOnes, imageMask, map, imageMask.size(), maskFlags);
}
else
{
warpPerspective(imageFloat, imageWarped, map, imageWarped.size(), imageFlags);
warpPerspective(gradientX, gradientXWarped, map, gradientXWarped.size(), imageFlags);
warpPerspective(gradientY, gradientYWarped, map, gradientYWarped.size(), imageFlags);
warpPerspective(allOnes, imageMask, map, imageMask.size(), maskFlags);
}
Scalar imgMean, imgStd, tmpMean, tmpStd;
meanStdDev(imageWarped, imgMean, imgStd, imageMask);
meanStdDev(templateFloat, tmpMean, tmpStd, imageMask);
subtract(imageWarped, imgMean, imageWarped, imageMask);//zero-mean input
subtract(templateFloat, tmpMean, templateZM, imageMask);//zero-mean template
const double tmpNorm = std::sqrt(countNonZero(imageMask)*(tmpStd.val[0])*(tmpStd.val[0]));
const double imgNorm = std::sqrt(countNonZero(imageMask)*(imgStd.val[0])*(imgStd.val[0]));
// calculate jacobian of image wrt parameters
switch (motionType){
case MOTION_AFFINE:
image_jacobian_affine_ECC(gradientXWarped, gradientYWarped, Xgrid, Ygrid, jacobian);
break;
case MOTION_HOMOGRAPHY:
image_jacobian_homo_ECC(gradientXWarped, gradientYWarped, Xgrid, Ygrid, map, jacobian);
break;
case MOTION_TRANSLATION:
image_jacobian_translation_ECC(gradientXWarped, gradientYWarped, jacobian);
break;
case MOTION_EUCLIDEAN:
image_jacobian_euclidean_ECC(gradientXWarped, gradientYWarped, Xgrid, Ygrid, map, jacobian);
break;
}
// calculate Hessian and its inverse
project_onto_jacobian_ECC(jacobian, jacobian, hessian);
hessianInv = hessian.inv();
const double correlation = templateZM.dot(imageWarped);
// calculate enhanced correlation coefficiont (ECC)->rho
last_rho = rho;
rho = correlation/(imgNorm*tmpNorm);
// project images into jacobian
project_onto_jacobian_ECC( jacobian, imageWarped, imageProjection);
project_onto_jacobian_ECC(jacobian, templateZM, templateProjection);
// calculate the parameter lambda to account for illumination variation
imageProjectionHessian = hessianInv*imageProjection;
const double lambda_n = (imgNorm*imgNorm) - imageProjection.dot(imageProjectionHessian);
const double lambda_d = correlation - templateProjection.dot(imageProjectionHessian);
if (lambda_d <= 0.0)
{
rho = -1;
CV_Error(CV_StsNoConv, "The algorithm stopped before its convergence. The correlation is going to be minimized. Images may be uncorrelated or non-overlapped");
}
const double lambda = (lambda_n/lambda_d);
// estimate the update step delta_p
error = lambda*templateZM - imageWarped;
project_onto_jacobian_ECC(jacobian, error, errorProjection);
deltaP = hessianInv * errorProjection;
// update warping matrix
update_warping_matrix_ECC( map, deltaP, motionType);
}
// return final correlation coefficient
return rho;
}
bool allFilled(const Mat &mat){
Mat tmp = (mat != 0);
tmp.convertTo(tmp, CV_64FC1, 1.0 / 255.0);
return sum(tmp)[0] == (tmp.cols * tmp.rows);
}
Mat PreGraph::GeneWeight(const vector<float> &feaSpL, const vector<float> &feaSpA, const vector<float> &feaSpB, const Mat &superpixels, const vector<int> &bd, const Mat &adj)
{
Mat weightMat(Size(spNum, spNum), CV_32F, Scalar(-1));
int dist = 0;
float minv = (float)numeric_limits<float>::max();
float maxv = (float)numeric_limits<float>::min();
for (int i = 0; i < spNum; ++i)
{
for (int j = 0; j < spNum; ++j)
{
if (adj.at<ushort>(i, j) == 1)
{
dist = sqrt(pow(feaSpL[i] - feaSpL[j], 2)) + sqrt(pow(feaSpA[i] - feaSpA[j], 2)) + sqrt(pow(feaSpB[i] - feaSpB[j], 2));
weightMat.at<float>(i, j) = dist;
if (dist < minv)
minv = dist;
if (dist > maxv)
maxv = dist;
for (int k = 0; k < spNum; ++k)
{
if (adj.at<ushort>(j, k) == 1)
{
dist = sqrt(pow(feaSpL[k] - feaSpL[i], 2)) + sqrt(pow(feaSpA[k] - feaSpA[i], 2)) + sqrt(pow(feaSpB[k] - feaSpB[i], 2));
weightMat.at<float>(i, k) = dist;
if (dist < minv)
minv = dist;
if (dist > maxv)
maxv = dist;
}
}
}
}
}
for (int i = 0; i < bd.size(); ++i)
{
for (int j = 0; j < bd.size(); ++j)
{
dist = sqrt(pow(feaSpL[bd[i]] - feaSpL[bd[j]], 2)) + sqrt(pow(feaSpA[bd[i]] - feaSpA[bd[j]], 2)) + sqrt(pow(feaSpB[bd[i]] - feaSpB[bd[j]], 2));
weightMat.at<float>(bd[i], bd[j]) = dist;
if (dist < minv)
minv = dist;
if (dist > maxv)
maxv = dist;
}
}
for (int i = 0; i < spNum; ++i)
{
for (int j = 0; j < spNum; ++j)
{
if (weightMat.at<float>(i, j)>-1)
{
weightMat.at<float>(i, j) = (weightMat.at<float>(i, j) - minv) / (maxv - minv);
weightMat.at<float>(i, j) = exp(-weightMat.at<float>(i, j) / theta);
}
else
weightMat.at<float>(i, j) = 0;
}
}
Mat tmpsuperpixels;
normalize(weightMat, tmpsuperpixels, 255.0, 0.0, NORM_MINMAX);
tmpsuperpixels.convertTo(tmpsuperpixels, CV_8UC3, 1.0);
imshow("sp", tmpsuperpixels);
waitKey();
return weightMat;
}
void CV_StereoMatchingTest::run(int)
{
string dataPath = ts->get_data_path();
string algorithmName = name;
assert( !algorithmName.empty() );
if( dataPath.empty() )
{
ts->printf( cvtest::TS::LOG, "dataPath is empty" );
ts->set_failed_test_info( cvtest::TS::FAIL_BAD_ARG_CHECK );
return;
}
FileStorage datasetsFS( dataPath + DATASETS_DIR + DATASETS_FILE, FileStorage::READ );
int code = readDatasetsParams( datasetsFS );
if( code != cvtest::TS::OK )
{
ts->set_failed_test_info( code );
return;
}
FileStorage runParamsFS( dataPath + ALGORITHMS_DIR + algorithmName + RUN_PARAMS_FILE, FileStorage::READ );
code = readRunParams( runParamsFS );
if( code != cvtest::TS::OK )
{
ts->set_failed_test_info( code );
return;
}
string fullResultFilename = dataPath + ALGORITHMS_DIR + algorithmName + RESULT_FILE;
FileStorage resFS( fullResultFilename, FileStorage::READ );
bool isWrite = true; // write or compare results
if( resFS.isOpened() )
isWrite = false;
else
{
resFS.open( fullResultFilename, FileStorage::WRITE );
if( !resFS.isOpened() )
{
ts->printf( cvtest::TS::LOG, "file %s can not be read or written\n", fullResultFilename.c_str() );
ts->set_failed_test_info( cvtest::TS::FAIL_BAD_ARG_CHECK );
return;
}
resFS << "stereo_matching" << "{";
}
int progress = 0, caseCount = (int)caseNames.size();
for( int ci = 0; ci < caseCount; ci++)
{
progress = update_progress( progress, ci, caseCount, 0 );
printf("progress: %d%%\n", progress);
fflush(stdout);
string datasetName = caseDatasets[ci];
string datasetFullDirName = dataPath + DATASETS_DIR + datasetName + "/";
Mat leftImg = imread(datasetFullDirName + LEFT_IMG_NAME);
Mat rightImg = imread(datasetFullDirName + RIGHT_IMG_NAME);
Mat trueLeftDisp = imread(datasetFullDirName + TRUE_LEFT_DISP_NAME, 0);
Mat trueRightDisp = imread(datasetFullDirName + TRUE_RIGHT_DISP_NAME, 0);
if( leftImg.empty() || rightImg.empty() || trueLeftDisp.empty() )
{
ts->printf( cvtest::TS::LOG, "images or left ground-truth disparities of dataset %s can not be read", datasetName.c_str() );
code = cvtest::TS::FAIL_INVALID_TEST_DATA;
continue;
}
int dispScaleFactor = datasetsParams[datasetName].dispScaleFactor;
Mat tmp; trueLeftDisp.convertTo( tmp, CV_32FC1, 1.f/dispScaleFactor ); trueLeftDisp = tmp; tmp.release();
if( !trueRightDisp.empty() )
trueRightDisp.convertTo( tmp, CV_32FC1, 1.f/dispScaleFactor ); trueRightDisp = tmp; tmp.release();
Mat leftDisp, rightDisp;
int ignBorder = max(runStereoMatchingAlgorithm(leftImg, rightImg, leftDisp, rightDisp, ci), EVAL_IGNORE_BORDER);
leftDisp.convertTo( tmp, CV_32FC1 ); leftDisp = tmp; tmp.release();
rightDisp.convertTo( tmp, CV_32FC1 ); rightDisp = tmp; tmp.release();
int tempCode = processStereoMatchingResults( resFS, ci, isWrite,
leftImg, rightImg, trueLeftDisp, trueRightDisp, leftDisp, rightDisp, QualityEvalParams(ignBorder));
code = tempCode==cvtest::TS::OK ? code : tempCode;
}
if( isWrite )
resFS << "}"; // "stereo_matching"
ts->set_failed_test_info( code );
}
int main(const int argc, const char *argv[])
{
cliini_args *args = cliini_parsopts(argc, argv, &group);
cliini_arg *input = cliargs_get(args, "input");
cliini_arg *output = cliargs_get(args, "output");
if (!args || cliargs_get(args, "help\n") || !input || !output) {
printf("TODO: print help!");
return EXIT_FAILURE;
}
string in_name(cliarg_nth_str(input,0));
string out_name(cliarg_nth_str(output,0));
ClifFile f_in(in_name, H5F_ACC_RDONLY);
Dataset *in_set = f_in.openDataset(0);
Subset3d *slice;
slice = new Subset3d(in_set);
int size[2];
size[0] = in_set->extent()[0]*scale;
size[1] = in_set->extent()[1]*scale;;
double focal_length[2];
in_set->get("calibration/intrinsics/checkers/projection", focal_length, 2);
focal_length[0] *= scale;
focal_length[1] *= scale;
Mat img;
readCvMat(in_set, in_set->Datastore::imgCount()/2, img, UNDISTORT, scale);
Mat epi;
Mat depth = Mat::zeros(Size(size[0], size[1]), CV_64F);
Mat score = Mat::zeros(Size(size[0], size[1]), CV_64F);
for(int l=500/y_step*y_step;l<550/*size[1]*/;l+=y_step) {
printf("line %d\n", l);
for(double d=6.5*scale;d>=5.0*scale;d-=0.5) {
//double d = 6.0;
slice->readEPI(epi, l, d, ClifUnit::PIXELS, UNDISTORT, Interpolation::LINEAR, scale);
structure_tensor_depth(epi, score, depth, d, l, (focal_length[0]+focal_length[1])/2, slice);
}
}
Mat d16;
depth.convertTo(d16, CV_16U);
imwrite(out_name, d16);
imwrite("out8bit.tif", depth*0.25);
int l = size[1];
write_ply_depth("points.ply", depth, focal_length, size[0], size[1], img, 0, l);
write_obj_depth("points.obj", depth, focal_length, size[0], size[1], img, 0, l);
Mat med, fmat;
depth.convertTo(fmat, CV_32F);
medianBlur(fmat, med, 5);
write_ply_depth("points_m5.ply", med, focal_length, size[0], size[1], img, 0, l);
write_obj_depth("points_m5.obj", med, focal_length, size[0], size[1], img, 0, l);
GaussianBlur(med, med, Size(3,3), 0);
write_ply_depth("points_m5_g3.ply", med, focal_length, size[0], size[1], img, 0, l);
write_obj_depth("points_m5_g3.obj", med, focal_length, size[0], size[1], img, 0, l);
return EXIT_SUCCESS;
}
void cv::decolor(InputArray _src, OutputArray _dst, OutputArray _color_boost)
{
Mat I = _src.getMat();
_dst.create(I.size(), CV_8UC1);
Mat dst = _dst.getMat();
_color_boost.create(I.size(), CV_8UC3);
Mat color_boost = _color_boost.getMat();
if(!I.data )
{
cout << "Could not open or find the image" << endl ;
return;
}
if(I.channels() !=3)
{
cout << "Input Color Image" << endl;
return;
}
// Parameter Setting
int maxIter = 15;
int iterCount = 0;
double tol = .0001;
double E = 0;
double pre_E = std::numeric_limits<double>::infinity();
Decolor obj;
Mat img;
img = Mat(I.size(),CV_32FC3);
I.convertTo(img,CV_32FC3,1.0/255.0);
// Initialization
obj.init();
vector <double> Cg;
vector < vector <double> > polyGrad;
vector < vector < int > > comb;
vector <double> alf;
obj.grad_system(img,polyGrad,Cg,comb);
obj.weak_order(img,alf);
// Solver
Mat Mt = Mat(polyGrad.size(),polyGrad[0].size(), CV_32FC1);
obj.wei_update_matrix(polyGrad,Cg,Mt);
vector <double> wei;
obj.wei_inti(comb,wei);
//////////////////////////////// main loop starting ////////////////////////////////////////
while(sqrt(pow(E-pre_E,2)) > tol)
{
iterCount +=1;
pre_E = E;
vector <double> G_pos;
vector <double> G_neg;
vector <double> temp;
vector <double> temp1;
double val = 0.0;
for(unsigned int i=0;i< polyGrad[0].size();i++)
{
val = 0.0;
for(unsigned int j =0;j<polyGrad.size();j++)
val = val + (polyGrad[j][i] * wei[j]);
temp.push_back(val - Cg[i]);
temp1.push_back(val + Cg[i]);
}
double pos = 0.0;
double neg = 0.0;
for(unsigned int i =0;i<alf.size();i++)
{
pos = ((1 + alf[i])/2) * exp((-1.0 * 0.5 * pow(temp[i],2))/pow(obj.sigma,2));
neg = ((1 - alf[i])/2) * exp((-1.0 * 0.5 * pow(temp1[i],2))/pow(obj.sigma,2));
G_pos.push_back(pos);
G_neg.push_back(neg);
}
vector <double> EXPsum;
vector <double> EXPterm;
for(unsigned int i = 0;i<G_pos.size();i++)
EXPsum.push_back(G_pos[i]+G_neg[i]);
vector <double> temp2;
for(unsigned int i=0;i<EXPsum.size();i++)
{
if(EXPsum[i] == 0)
temp2.push_back(1.0);
else
temp2.push_back(0.0);
}
for(unsigned int i =0; i < G_pos.size();i++)
EXPterm.push_back((G_pos[i] - G_neg[i])/(EXPsum[i] + temp2[i]));
double val1 = 0.0;
vector <double> wei1;
for(unsigned int i=0;i< polyGrad.size();i++)
{
val1 = 0.0;
for(unsigned int j =0;j<polyGrad[0].size();j++)
{
val1 = val1 + (Mt.at<float>(i,j) * EXPterm[j]);
}
wei1.push_back(val1);
}
for(unsigned int i =0;i<wei.size();i++)
wei[i] = wei1[i];
E = obj.energyCalcu(Cg,polyGrad,wei);
if(iterCount > maxIter)
break;
G_pos.clear();
G_neg.clear();
temp.clear();
temp1.clear();
EXPsum.clear();
EXPterm.clear();
temp2.clear();
wei1.clear();
}
Mat Gray = Mat::zeros(img.size(),CV_32FC1);
obj.grayImContruct(wei, img, Gray);
Gray.convertTo(dst,CV_8UC1,255);
/////////////////////////////////// Contrast Boosting /////////////////////////////////
Mat lab = Mat(img.size(),CV_8UC3);
Mat color = Mat(img.size(),CV_8UC3);
cvtColor(I,lab,COLOR_BGR2Lab);
vector <Mat> lab_channel;
split(lab,lab_channel);
dst.copyTo(lab_channel[0]);
merge(lab_channel,lab);
cvtColor(lab,color_boost,COLOR_Lab2BGR);
}
int main(int argc, char *argv[]) {
vector<string> teFaceFiles;
vector<int> teImageIDToSubjectIDMap;
vector<string> trFaceFiles;
vector<int> trImageIDToSubjectIDMap;
string trainingList = "./train_all_1-9.txt";
string testList = "./test_all_1-9.txt";
//doShow is originally set to false
bool isVerbose=false, doShow=true, doWrite=true;
char c;
while ( (c = getopt(argc, argv, ":ast:r:v")) != -1) {
switch (c) {
case 's':
doShow = true;
break;
case 'v':
isVerbose = true;
break;
case 'r':
testList = optarg;
break;
case 't':
trainingList = optarg;
break;
case 'h':
usage();
break;
case '?':
cout << "Invalid option: '-" << (char)optopt << "'" << endl;
usage();
break;
case ':':
cout << "Missing argument value for: '-" << (char)optopt << "'" << endl;
usage();
break;
default:
cerr << "getopt returned character code 0%o" << c << endl;
usage();
}
}
readFile(testList, teFaceFiles, teImageIDToSubjectIDMap);
readFile(trainingList, trFaceFiles, trImageIDToSubjectIDMap);
Mat teImg = imread(trFaceFiles[0], 0);
Mat trImg = imread(trFaceFiles[0], 0);
if(teImg.empty() || trImg.empty())
{
unableToLoad();
}
// Eigenfaces operates on a vector representation of the image so we calculate the
// size of this vector. Now we read the face images and reshape them into vectors
// for the face recognizer to operate on.
int imgVectorSize = teImg.cols * teImg.rows;
// Create a matrix that has 'imgVectorSize' rows and as many columns as there are images.
Mat trImgVectors(imgVectorSize, trFaceFiles.size(), CV_32FC1);
// Load the vector.
for(unsigned int i = 0; i < trFaceFiles.size(); i++)
{
Mat tmpTrImgVector = trImgVectors.col(i);
Mat tmp_img;
imread(trFaceFiles[i], 0).convertTo(tmp_img, CV_32FC1);
tmp_img.reshape(1, imgVectorSize).copyTo(tmpTrImgVector);
}
// On instantiating the FaceRecognizer object automatically processes
// the training images and projects them.
FaceRecognizer fr(trImgVectors, trImageIDToSubjectIDMap, 20);
if(isVerbose){
msgTestHeader();
}
// This vector keeps track of how many times each face is correctly recognized
// The index numbers stand for the ID number of a subject in the test set.
vector<int> corrPerId;
for(int i=0; i<40; i++)
{
corrPerId.push_back(0);
}
int correct=0, incorrect=0;
for(unsigned int i=0; i<teFaceFiles.size(); i++)
{
Mat rmat = imread(teFaceFiles[i],0);
rmat.convertTo(teImg, CV_32FC1);
// Look up the true subject ID of the current test subject.
int currentTestSubjectID = teImageIDToSubjectIDMap[i];
// Compare a face from the list of test faces with the set of training faces stored in
// the recognizer and try to find the closest match.
int recAs = fr.recognize(rmat.reshape(1, imgVectorSize));
// The recognizer returns the subject ID of the subject he thinks this is. If a subject is correctly
// recognized the subject ID returned by the recognizer should be the same as the subject ID from the
// test file so we must look up the ID from the test file.
bool isCorrectlyRec;
if(recAs == currentTestSubjectID)
{
isCorrectlyRec = true;
correct++; // Track total correct recognitions for all subjects.
corrPerId[recAs-1]++; // Track total correct recognitions for this subject
}
else
{
isCorrectlyRec = false;
incorrect++; // Total false recognitions for all subjects.
}
if(isVerbose) {
msgTestResult(teFaceFiles[i], recAs, currentTestSubjectID, isCorrectlyRec);
}
}
if(isVerbose){
msgSubjBySubj(corrPerId);
}
msgSummary(correct, incorrect, teFaceFiles.size());
// Just for fun let's show the average face and some of the test faces on screen.
if(doShow)
{
Mat oAvgImage = toGrayscale(fr.getAverage()).reshape(1, teImg.rows);
Mat sAvgImage;
resize(oAvgImage, sAvgImage, Size(teImg.cols*2, teImg.rows*2), 0, 0, INTER_LINEAR);
imshow("The average face", sAvgImage);
if (doWrite) {
imwrite("Average Eigenface.bmp", sAvgImage);
}
int eigenCount=6;
if(fr.getEigenvectors().rows < eigenCount){
eigenCount = fr.getEigenvectors().rows;
}
for(int i = 0; i < eigenCount; i++) {
stringstream windowTitle;
windowTitle << "Eigenface No. " << i;
Mat oEigenImage = toGrayscale(fr.getEigenvectors().row(i)).reshape(1, teImg.rows);
Mat sEigenImage;
resize(oEigenImage, sEigenImage, Size(teImg.cols*2, teImg.rows*2), 0, 0, INTER_LINEAR);
imshow(windowTitle.str(), sEigenImage);
if (doWrite) {
char filename[250];
strcpy( filename, (const char*)( windowTitle.str().c_str()));
windowTitle << ".bmp";
imwrite(windowTitle.str(), sEigenImage);
}
}
waitKey(0);
}
}
// Compute the color_contrast_score value for an object
void SVMDetectionEvaluator::computeColorContrastScore(const Mat& frame, const cvb::CvBlob& blob, float& result)
{
// Initialize result
result = -1;
// Initialize blobs
cvb::CvBlobs blobs_in, blobs_out;
try
{
// Copy parameters to local variables
int theta = parameters.get<int>("cc_theta");
int color_space = parameters.get<int>("cc_color_space");
int channel = parameters.get<int>("cc_channel");
float bin_step = parameters.get<float>("cc_bin_step");
// Convert image
Mat conv;
cvtColor(frame, conv, color_space);
//double m, M;
//minMaxLoc(conv, &m, &M);
//cout << "--- " << m << " --- " << M << endl;
conv.convertTo(conv, CV_32FC3, 1.0/180.0);
//minMaxLoc(conv, &m, &M);
//cout << "--- " << m << " --- " << M << endl;
// Convert blob to binary mask
Mat mask = blobToMat(blob, CV_POSITION_ORIGINAL, true, frame.cols, frame.rows);
// Dilate and erode mask by theta pixels
Mat mask_in, mask_out;
dilate(mask, mask_out, Mat(), Point(-1,-1), theta);
erode(mask, mask_in, Mat(), Point(-1,-1), theta);
// Compute new blobs
blobs_in = computeBlobs(mask_in);
blobs_out = computeBlobs(mask_out);
// Check number of blobs
if(blobs_in.size() == 0 || blobs_out.size() == 0)
{
throw MyException("Error computing inner and/or outer blobs");
}
// Get blobs references
cvb::CvBlob& blob_in = *(blobs_in.begin()->second);
cvb::CvBlob& blob_out = *(blobs_out.begin()->second);
// Initialize value vectors
vector<float> values_in, values_out;
// Initialize min and max values
float min_value=INT_MAX, max_value=INT_MIN;
// Select inner pixels
Point current_in = blob_in.contour.startingPoint;
Vec3f pixel_in = conv.at<Vec3f>(current_in.y, current_in.x);
values_in.push_back(pixel_in[channel]);
if(pixel_in[channel] < min_value) min_value = pixel_in[channel];
if(pixel_in[channel] > max_value) max_value = pixel_in[channel];
const cvb::CvChainCodes& chain_code_in = blob_in.contour.chainCode;
for(cvb::CvChainCodes::const_iterator it=chain_code_in.begin(); it != chain_code_in.end(); it++)
{
current_in.x += cvb::cvChainCodeMoves[*it][0];
current_in.y += cvb::cvChainCodeMoves[*it][1];
pixel_in = conv.at<Vec3f>(current_in.y, current_in.x);
values_in.push_back(pixel_in[channel]);
if(pixel_in[channel] < min_value) min_value = pixel_in[channel];
if(pixel_in[channel] > max_value) max_value = pixel_in[channel];
}
// Select outer pixels
Point current_out = blob_out.contour.startingPoint;
Vec3f pixel_out = conv.at<Vec3f>(current_out.y, current_out.x);
values_out.push_back(pixel_out[channel]);
if(pixel_out[channel] < min_value) min_value = pixel_out[channel];
if(pixel_out[channel] > max_value) max_value = pixel_out[channel];
const cvb::CvChainCodes& chain_code_out = blob_out.contour.chainCode;
for(cvb::CvChainCodes::const_iterator it=chain_code_out.begin(); it != chain_code_out.end(); it++)
{
current_out.x += cvb::cvChainCodeMoves[*it][0];
current_out.y += cvb::cvChainCodeMoves[*it][1];
pixel_out = conv.at<Vec3f>(current_out.y, current_out.x);
values_out.push_back(pixel_out[channel]);
if(pixel_out[channel] < min_value) min_value = pixel_out[channel];
if(pixel_out[channel] > max_value) max_value = pixel_out[channel];
}
// Check list sizes
if(values_in.size() == 0 || values_out.size() == 0)
{
throw MyException("Empty histograms for color boundary.");
}
// Compute histograms
XHistogram hist_in;
hist_in.computeHistogram(values_in, bin_step, min_value, max_value);
//cout << hist_in << endl << endl;
XHistogram hist_out;
hist_out.computeHistogram(values_out, bin_step, min_value, max_value);
//cout << hist_out << endl << endl;
// Compute features
float dist = XHistogram::ChiSquaredDistance(hist_in, hist_out);
// Compute result
result = dist;
// Free blobs
cvReleaseBlobs(blobs_in);
cvReleaseBlobs(blobs_out);
}
catch(MyException& e)
{
log_mutex.lock();
Log::warning() << ShellUtils::RED << "Error computing color contrast score: " << e.what() << ShellUtils::NORMAL << endl;
log_mutex.unlock();
// Free blobs
cvReleaseBlobs(blobs_in);
cvReleaseBlobs(blobs_out);
}
catch(...)
{
log_mutex.lock();
Log::warning() << ShellUtils::RED << "Error computing color contrast score." << ShellUtils::NORMAL << endl;
log_mutex.unlock();
// Free blobs
cvReleaseBlobs(blobs_in);
cvReleaseBlobs(blobs_out);
}
}
void cv::gpu::LUT(const GpuMat& src, const Mat& lut, GpuMat& dst, Stream& s)
{
const int cn = src.channels();
CV_Assert( src.type() == CV_8UC1 || src.type() == CV_8UC3 );
CV_Assert( lut.depth() == CV_8U );
CV_Assert( lut.channels() == 1 || lut.channels() == cn );
CV_Assert( lut.rows * lut.cols == 256 && lut.isContinuous() );
dst.create(src.size(), CV_MAKE_TYPE(lut.depth(), cn));
NppiSize sz;
sz.height = src.rows;
sz.width = src.cols;
Mat nppLut;
lut.convertTo(nppLut, CV_32S);
int nValues3[] = {256, 256, 256};
Npp32s pLevels[256];
for (int i = 0; i < 256; ++i)
pLevels[i] = i;
const Npp32s* pLevels3[3];
#if (CUDA_VERSION <= 4020)
pLevels3[0] = pLevels3[1] = pLevels3[2] = pLevels;
#else
GpuMat d_pLevels;
d_pLevels.upload(Mat(1, 256, CV_32S, pLevels));
pLevels3[0] = pLevels3[1] = pLevels3[2] = d_pLevels.ptr<Npp32s>();
#endif
cudaStream_t stream = StreamAccessor::getStream(s);
NppStreamHandler h(stream);
if (src.type() == CV_8UC1)
{
#if (CUDA_VERSION <= 4020)
nppSafeCall( nppiLUT_Linear_8u_C1R(src.ptr<Npp8u>(), static_cast<int>(src.step),
dst.ptr<Npp8u>(), static_cast<int>(dst.step), sz, nppLut.ptr<Npp32s>(), pLevels, 256) );
#else
GpuMat d_nppLut(Mat(1, 256, CV_32S, nppLut.data));
nppSafeCall( nppiLUT_Linear_8u_C1R(src.ptr<Npp8u>(), static_cast<int>(src.step),
dst.ptr<Npp8u>(), static_cast<int>(dst.step), sz, d_nppLut.ptr<Npp32s>(), d_pLevels.ptr<Npp32s>(), 256) );
#endif
}
else
{
const Npp32s* pValues3[3];
Mat nppLut3[3];
if (nppLut.channels() == 1)
{
#if (CUDA_VERSION <= 4020)
pValues3[0] = pValues3[1] = pValues3[2] = nppLut.ptr<Npp32s>();
#else
GpuMat d_nppLut(Mat(1, 256, CV_32S, nppLut.data));
pValues3[0] = pValues3[1] = pValues3[2] = d_nppLut.ptr<Npp32s>();
#endif
}
else
{
cv::split(nppLut, nppLut3);
#if (CUDA_VERSION <= 4020)
pValues3[0] = nppLut3[0].ptr<Npp32s>();
pValues3[1] = nppLut3[1].ptr<Npp32s>();
pValues3[2] = nppLut3[2].ptr<Npp32s>();
#else
GpuMat d_nppLut0(Mat(1, 256, CV_32S, nppLut3[0].data));
GpuMat d_nppLut1(Mat(1, 256, CV_32S, nppLut3[1].data));
GpuMat d_nppLut2(Mat(1, 256, CV_32S, nppLut3[2].data));
pValues3[0] = d_nppLut0.ptr<Npp32s>();
pValues3[1] = d_nppLut1.ptr<Npp32s>();
pValues3[2] = d_nppLut2.ptr<Npp32s>();
#endif
}
nppSafeCall( nppiLUT_Linear_8u_C3R(src.ptr<Npp8u>(), static_cast<int>(src.step),
dst.ptr<Npp8u>(), static_cast<int>(dst.step), sz, pValues3, pLevels3, nValues3) );
}
if (stream == 0)
cudaSafeCall( cudaDeviceSynchronize() );
}
Mat Watershed(Mat imagem, int winSize){
new_label = 0.0;
scan_step2 = 1;
scan_step3 = 1;
windowSize = winSize;
img = imagem;
img.convertTo(img, CV_32FC3);
VMAX = 100000000000;
//inicializar o tamanho da matriz
lab = img.clone();
val = img.clone();
//inicializar os valores
for(int x = 0; x < img.rows; x++){
for(int y = 0; y < img.cols; y++){
lab.at<float>(x,y) = INIT;
val.at<float>(x,y) = INIT;
}
}
//encontra menor pixel de cada vizinhanca
for(int x = 0; x < img.rows; x++){
for(int y = 0; y < img.cols; y++){
step1(x, y);
}
}
cout<<"Step 2"<<endl;
//encontrar os plateaus mesmo pixel greyscale a partir dos minimos
while(scan_step2 == 1){
changed = 0;
//scan top left >> bottom right
for(int x = 0; x < img.rows; x++){
for(int y = 0; y < img.cols; y++){
step2(x, y);
}
}
if(changed == 0){
scan_step2 = 0;
}else{
changed = 0;
//scan bottom right >> top left
for(int x = img.rows -1; x >= 0; x--){
for(int y = img.cols -1; y >= 0; y--){
step2(x, y);
}
}
if(changed == 0){
scan_step2 = 0;
}
}
}
int limite = 0;
cout<<"Step 3"<<endl;
while(scan_step3 == 1){
limite++;
if(limite == 10) break;
changed = 0;
//scan top left >> bottom right
for(int x = 0; x < img.rows; x++){
for(int y = 0; y < img.cols; y++){
step3(x, y);
}
}
if(changed == 0){
scan_step3 = 0;
}else{
changed = 0;
//scan bottom right >> top left
for(int x = img.rows -1; x >= 0; x--){
for(int y = img.cols -1; y >= 0; y--){
step3(x, y);
}
}
if(changed == 0){
scan_step3 = 0;
}
}
}
max_pixel=0;
for(int x = 0; x < lab.rows; x++){
for(int y = 0; y < lab.cols; y++){
if(max_pixel < lab.at<float>(x,y)) max_pixel = lab.at<float>(x,y);
}
}
lab.convertTo(lab, CV_8U,255.0/(max_pixel));
return lab;
}
static void build3dmodel( const Ptr<FeatureDetector>& detector,
const Ptr<DescriptorExtractor>& descriptorExtractor,
const vector<Point3f>& /*modelBox*/,
const vector<string>& imageList,
const vector<Rect>& roiList,
const vector<Vec6f>& poseList,
const Mat& cameraMatrix,
PointModel& model )
{
int progressBarSize = 10;
const double Feps = 5;
const double DescriptorRatio = 0.7;
vector<vector<KeyPoint> > allkeypoints;
vector<int> dstart;
vector<float> alldescriptorsVec;
vector<Vec2i> pairwiseMatches;
vector<Mat> Rs, ts;
int descriptorSize = 0;
Mat descriptorbuf;
Set2i pairs, keypointsIdxMap;
model.points.clear();
model.didx.clear();
dstart.push_back(0);
size_t nimages = imageList.size();
size_t nimagePairs = (nimages - 1)*nimages/2 - nimages;
printf("\nComputing descriptors ");
// 1. find all the keypoints and all the descriptors
for( size_t i = 0; i < nimages; i++ )
{
Mat img = imread(imageList[i], 1), gray;
cvtColor(img, gray, COLOR_BGR2GRAY);
vector<KeyPoint> keypoints;
detector->detect(gray, keypoints);
descriptorExtractor->compute(gray, keypoints, descriptorbuf);
Point2f roiofs = roiList[i].tl();
for( size_t k = 0; k < keypoints.size(); k++ )
keypoints[k].pt += roiofs;
allkeypoints.push_back(keypoints);
Mat buf = descriptorbuf;
if( !buf.isContinuous() || buf.type() != CV_32F )
{
buf.release();
descriptorbuf.convertTo(buf, CV_32F);
}
descriptorSize = buf.cols;
size_t prev = alldescriptorsVec.size();
size_t delta = buf.rows*buf.cols;
alldescriptorsVec.resize(prev + delta);
std::copy(buf.ptr<float>(), buf.ptr<float>() + delta,
alldescriptorsVec.begin() + prev);
dstart.push_back(dstart.back() + (int)keypoints.size());
Mat R, t;
unpackPose(poseList[i], R, t);
Rs.push_back(R);
ts.push_back(t);
if( (i+1)*progressBarSize/nimages > i*progressBarSize/nimages )
{
putchar('.');
fflush(stdout);
}
}
Mat alldescriptors((int)alldescriptorsVec.size()/descriptorSize, descriptorSize, CV_32F,
&alldescriptorsVec[0]);
printf("\nOk. total images = %d. total keypoints = %d\n",
(int)nimages, alldescriptors.rows);
printf("\nFinding correspondences ");
int pairsFound = 0;
vector<Point2f> pts_k(2);
vector<Mat> Rs_k(2), ts_k(2);
//namedWindow("img1", 1);
//namedWindow("img2", 1);
// 2. find pairwise correspondences
for( size_t i = 0; i < nimages; i++ )
for( size_t j = i+1; j < nimages; j++ )
{
const vector<KeyPoint>& keypoints1 = allkeypoints[i];
const vector<KeyPoint>& keypoints2 = allkeypoints[j];
Mat descriptors1 = alldescriptors.rowRange(dstart[i], dstart[i+1]);
Mat descriptors2 = alldescriptors.rowRange(dstart[j], dstart[j+1]);
Mat F = getFundamentalMat(Rs[i], ts[i], Rs[j], ts[j], cameraMatrix);
findConstrainedCorrespondences( F, keypoints1, keypoints2,
descriptors1, descriptors2,
pairwiseMatches, Feps, DescriptorRatio );
//pairsFound += (int)pairwiseMatches.size();
//Mat img1 = imread(format("%s/frame%04d.jpg", model.name.c_str(), (int)i), 1);
//Mat img2 = imread(format("%s/frame%04d.jpg", model.name.c_str(), (int)j), 1);
//double avg_err = 0;
for( size_t k = 0; k < pairwiseMatches.size(); k++ )
{
int i1 = pairwiseMatches[k][0], i2 = pairwiseMatches[k][1];
pts_k[0] = keypoints1[i1].pt;
pts_k[1] = keypoints2[i2].pt;
Rs_k[0] = Rs[i]; Rs_k[1] = Rs[j];
ts_k[0] = ts[i]; ts_k[1] = ts[j];
Point3f objpt = triangulatePoint(pts_k, Rs_k, ts_k, cameraMatrix);
vector<Point3f> objpts;
objpts.push_back(objpt);
vector<Point2f> imgpts1, imgpts2;
projectPoints(Mat(objpts), Rs_k[0], ts_k[0], cameraMatrix, Mat(), imgpts1);
projectPoints(Mat(objpts), Rs_k[1], ts_k[1], cameraMatrix, Mat(), imgpts2);
double e1 = norm(imgpts1[0] - keypoints1[i1].pt);
double e2 = norm(imgpts2[0] - keypoints2[i2].pt);
if( e1 + e2 > 5 )
continue;
pairsFound++;
//model.points.push_back(objpt);
pairs[Pair2i(i1+dstart[i], i2+dstart[j])] = 1;
pairs[Pair2i(i2+dstart[j], i1+dstart[i])] = 1;
keypointsIdxMap[Pair2i((int)i,i1)] = 1;
keypointsIdxMap[Pair2i((int)j,i2)] = 1;
//CV_Assert(e1 < 5 && e2 < 5);
//Scalar color(rand()%256,rand()%256, rand()%256);
//circle(img1, keypoints1[i1].pt, 2, color, -1, CV_AA);
//circle(img2, keypoints2[i2].pt, 2, color, -1, CV_AA);
}
//printf("avg err = %g\n", pairwiseMatches.size() ? avg_err/(2*pairwiseMatches.size()) : 0.);
//imshow("img1", img1);
//imshow("img2", img2);
//waitKey();
if( (i+1)*progressBarSize/nimagePairs > i*progressBarSize/nimagePairs )
{
putchar('.');
fflush(stdout);
}
}
printf("\nOk. Total pairs = %d\n", pairsFound );
// 3. build the keypoint clusters
vector<Pair2i> keypointsIdx;
Set2i::iterator kpidx_it = keypointsIdxMap.begin(), kpidx_end = keypointsIdxMap.end();
for( ; kpidx_it != kpidx_end; ++kpidx_it )
keypointsIdx.push_back(kpidx_it->first);
printf("\nClustering correspondences ");
vector<int> labels;
int nclasses = partition( keypointsIdx, labels, EqKeypoints(&dstart, &pairs) );
printf("\nOk. Total classes (i.e. 3d points) = %d\n", nclasses );
model.descriptors.create((int)keypointsIdx.size(), descriptorSize, CV_32F);
model.didx.resize(nclasses);
model.points.resize(nclasses);
vector<vector<Pair2i> > clusters(nclasses);
for( size_t i = 0; i < keypointsIdx.size(); i++ )
clusters[labels[i]].push_back(keypointsIdx[i]);
// 4. now compute 3D points corresponding to each cluster and fill in the model data
printf("\nComputing 3D coordinates ");
int globalDIdx = 0;
for( int k = 0; k < nclasses; k++ )
{
int i, n = (int)clusters[k].size();
pts_k.resize(n);
Rs_k.resize(n);
ts_k.resize(n);
model.didx[k].resize(n);
for( i = 0; i < n; i++ )
{
int imgidx = clusters[k][i].first, ptidx = clusters[k][i].second;
Mat dstrow = model.descriptors.row(globalDIdx);
alldescriptors.row(dstart[imgidx] + ptidx).copyTo(dstrow);
model.didx[k][i] = globalDIdx++;
pts_k[i] = allkeypoints[imgidx][ptidx].pt;
Rs_k[i] = Rs[imgidx];
ts_k[i] = ts[imgidx];
}
Point3f objpt = triangulatePoint(pts_k, Rs_k, ts_k, cameraMatrix);
model.points[k] = objpt;
if( (i+1)*progressBarSize/nclasses > i*progressBarSize/nclasses )
{
putchar('.');
fflush(stdout);
}
}
Mat img(768, 1024, CV_8UC3);
vector<Point2f> imagePoints;
namedWindow("Test", 1);
// visualize the cloud
for( size_t i = 0; i < nimages; i++ )
{
img = imread(format("%s/frame%04d.jpg", model.name.c_str(), (int)i), 1);
projectPoints(Mat(model.points), Rs[i], ts[i], cameraMatrix, Mat(), imagePoints);
for( int k = 0; k < (int)imagePoints.size(); k++ )
circle(img, imagePoints[k], 2, Scalar(0,255,0), -1, CV_AA, 0);
imshow("Test", img);
int c = waitKey();
if( c == 'q' || c == 'Q' )
break;
}
}
MetaData_YML_Backed* load_ICL(string base_path,string annotation,bool training)
{
// parse the annotation
vector<double> labels;
istringstream iss(annotation);
string filename; iss >> filename; // discard.
cout << "annotation " << annotation << endl;
cout << "filename " << filename << endl;
while(iss)
{
double v; iss >> v;
labels.push_back(v);
}
// calc the name for this metadata
string metadata_filename = base_path + filename;
// load the raw data
float active_min = training?0:params::MIN_Z();
string frame_file = base_path + filename;
Mat depth = imread(frame_file,-1);
assert(!depth.empty());
CustomCamera pxc_camera(params::H_RGB_FOV,params::V_RGB_FOV, depth.cols,depth.rows);
depth.convertTo(depth,DataType<float>::type);
depth /= 10;
for(int rIter = 0; rIter < depth.rows; ++rIter)
for(int cIter = 0; cIter < depth.cols; ++cIter)
{
float & d = depth.at<float>(rIter,cIter);
if(d <= active_min)
d = inf;
}
//depth = pointCloudToDepth(depth,pxc_camera);
if(depth.empty())
{
log_once(printfpp("ICL_Video cannot load %s",metadata_filename.c_str()));
log_once(printfpp("Couldn't open %s",frame_file.c_str()));
return nullptr;
}
//Mat RGB (depth.rows,depth.cols,DataType<Vec3b>::type,Scalar(122,150,233));
Mat RGB = imageeq("",depth,false,false);
for(int rIter = 0; rIter < RGB.rows; rIter++)
for(int cIter = 0; cIter < RGB.cols; cIter++)
RGB.at<Vec3b>(rIter,cIter)[2] = 255;
// create an image
shared_ptr<ImRGBZ> im =
make_shared<ImRGBZ>(RGB,depth,metadata_filename + "image",pxc_camera);
// create the metadata object
Metadata_Simple* metadata = new Metadata_Simple(metadata_filename+".yml",true,true,false);
metadata->setIm(*im);
Point2d hand_root(labels[0 + 0*3],labels[1 + 0*3]);
suppress_flood_fill_hack(im->Z,hand_root);
metadata->setIm(*im);
Rect handBB = set_annotations(metadata,labels,im);
suppress_most_common_pixel_hack(im->Z,handBB);
metadata->setIm(*im);
log_im_decay_freq("ICL_loaded",[&]()
{
return image_datum(*metadata);
});
return metadata;
}
void myPCA(Mat data, Mat &mean, Mat &eigenvectors, Mat &eigenvalues, int maxComponents)
{
int covar_flags = CV_COVAR_SCALE;
int i, len, in_count;
Size mean_sz;
CV_Assert( data.channels() == 1 );
len = data.cols;
in_count = data.rows;
covar_flags |= CV_COVAR_ROWS;
mean_sz = Size(len, 1);
int count = std::min(len, in_count), out_count = count;
if( maxComponents > 0 )
out_count = std::min(count, maxComponents);
if( len <= in_count )
covar_flags |= CV_COVAR_NORMAL;
int ctype = std::max(CV_32F, data.depth());
mean.create( mean_sz, ctype );
Mat covar( count, count, ctype );
//计算协方差矩阵，均值
calcCovarMatrix( data, covar, mean, covar_flags, ctype );
//计算特征值，特征向量
eigen( covar, eigenvalues, eigenvectors );
//归一化
if( !(covar_flags & CV_COVAR_NORMAL) )
{
Mat tmp_data, tmp_mean = repeat(mean, data.rows/mean.rows, data.cols/mean.cols);
if( data.type() != ctype || tmp_mean.data == mean.data )
{
data.convertTo( tmp_data, ctype );
subtract( tmp_data, tmp_mean, tmp_data );
}
else
{
subtract( data, tmp_mean, tmp_mean );
tmp_data = tmp_mean;
}
Mat evects1(count, len, ctype);
gemm( eigenvectors, tmp_data, 1, Mat(), 0, evects1, 0);
eigenvectors = evects1;
// normalize eigenvectors
for( i = 0; i < out_count; i++ )
{
Mat vec = eigenvectors.row(i);
normalize(vec, vec);
}
}
if( count > out_count )
{
// use clone() to physically copy the data and thus deallocate the original matrices
eigenvalues = eigenvalues.rowRange(0,out_count).clone();
eigenvectors = eigenvectors.rowRange(0,out_count).clone();
}
}
void createAnisotropyTensor(
Mat_<Tensor>& tensors,
Mat& in,
double sigma,
double rho,
double gamma,
cv::Mat& blur,
cv::Mat& edge,
cv::Mat& structure,
cv::Mat& color
) {
Mat grad;
/* Apply blur by sigma. */
GaussianBlur(in, blur, Size(0,0), sigma, 0, BORDER_REFLECT);
Mat grad_x, grad_y;
Mat kernel;
Point anchor = Point(-1, -1);
kernel = Mat::zeros(1, 3, CV_16S);
kernel.at<short>(0, 0) = -1;
kernel.at<short>(0, 1) = 0;
kernel.at<short>(0, 2) = 1;
/* Calculate gradient in x and y. */
filter2D(blur, grad_x, CV_64F, kernel, anchor, 0, BORDER_REFLECT);
transpose(kernel, kernel);
filter2D(blur, grad_y, CV_64F, kernel, anchor, 0, BORDER_REFLECT);
Mat x_sq, y_sq, xy;
Mat x_sqr, y_sqr, xyr;
/* Element of outer product. */
x_sq = grad_x.mul(grad_x);
y_sq = grad_y.mul(grad_y);
xy = grad_x.mul(grad_y);
/* Integration scale (rho) smoothing. */
GaussianBlur(x_sq, x_sqr, Size(0,0), rho, 0, BORDER_REFLECT);
GaussianBlur(y_sq, y_sqr, Size(0,0), rho, 0, BORDER_REFLECT);
GaussianBlur(xy , xyr , Size(0,0), rho, 0, BORDER_REFLECT);
Mat evec, eval;
Mat h = Mat::zeros(in.size(), CV_64F);
Mat s = Mat::zeros(in.size(), CV_64F);
Mat v = Mat::zeros(in.size(), CV_64F);
Mat hsv;
vector<Mat> channels;
edge.create(in.size(), CV_64F);
for (int i = 0; i < in.rows; ++i) {
for (int j = 0; j < in.cols; ++j) {
/* Structure tensor. */
Tensor b = Tensor(
x_sqr.at<double>(i,j),
xyr.at<double>(i,j),
xyr.at<double>(i,j),
y_sqr.at<double>(i,j)
);
/* Structure tensor without rho smoothing, for the edge detector. */
Tensor c = Tensor(
x_sq.at<double>(i,j),
xy.at<double>(i,j),
xy.at<double>(i,j),
y_sq.at<double>(i,j)
);
/* Returns the eigenvectors as row vectors! */
eigen(b, eval, evec);
double s1 = eval.at<double>(0);
double s2 = eval.at<double>(1);
if (s2 > s1)
fprintf(stderr, "OOPS: Wrong ordering of eigenvalues\n");
double l1 = 1.0;
double l2 = 1.0 / (1.0 + (s1 - s2) * (s1 - s2) / (gamma*gamma));
Mat eval2 = eval.clone();
eval2.at<double>(0) = l1;
eval2.at<double>(1) = l2;
tensors(i, j) = Tensor(Mat(evec.t() * Mat::diag(eval2) * evec));
h.at<double>(i, j) = fmod(atan2(evec.at<double>(1), evec.at<double>(0))
* 180.0 / M_PI + 180.0, 180.0);
s.at<double>(i, j) = 0;
v.at<double>(i, j) = 1.0/(1.0 + l2);
/* Returns the eigenvectors as row vectors! */
eigen(c, eval, evec);
s1 = eval.at<double>(0);
s2 = eval.at<double>(1);
edge.at<double>(i, j) = s1;
}
}
normalize(edge, edge, 0, 255, NORM_MINMAX, CV_8U);
/* Create tensor visualization, double the size. */
structure.create(in.size(), CV_64F);
GaussianBlur(in, structure, Size(0,0), sigma, 0, BORDER_REFLECT);
cvtColor(structure, structure, CV_GRAY2BGR);
resize(structure, structure, Size(0, 0), 2, 2);
for (int i = 0; i < in.rows; i += 15) {
for (int j = 0; j < in.cols; j += 15) {
Tensor b = tensors(i, j);
eigen(b, eval, evec);
double s1 = eval.at<double>(0);
double s2 = eval.at<double>(1);
Point2f p1(evec.row(0));
Point2f p2(evec.row(1));
line(structure, 2*Point(j, i), 2*Point2f(j, i) + 20 * s2 * p1,
CV_RGB(255,0,0), 1.2, CV_AA);
line(structure, 2*Point(j, i), 2*Point2f(j, i) + 20 * s1 * p2,
CV_RGB(0,0,0), 1.2, CV_AA);
}
}
Mat ho, so, vo;
h.convertTo(ho, CV_8U);
normalize(s, so, 255, 255, NORM_MINMAX, CV_8U);
normalize(v, vo, 0, 255, NORM_MINMAX, CV_8U);
channels.push_back(ho);
channels.push_back(so);
channels.push_back(vo);
merge(channels, hsv);
cvtColor(hsv, color, CV_HSV2BGR);
}
void CV_PlaneTest::run( int )
{
string folder = cvtest::TS::ptr()->get_data_path() + "/" + STRUCTURED_LIGHT_DIR + "/" + FOLDER_DATA + "/";
structured_light::GrayCodePattern::Params params;
params.width = 1280;
params.height = 800;
// Set up GraycodePattern with params
Ptr<structured_light::GrayCodePattern> graycode = structured_light::GrayCodePattern::create( params );
size_t numberOfPatternImages = graycode->getNumberOfPatternImages();
FileStorage fs( folder + "calibrationParameters.yml", FileStorage::READ );
if( !fs.isOpened() )
{
ts->set_failed_test_info( cvtest::TS::FAIL_INVALID_TEST_DATA );
}
FileStorage fs2( folder + "gt_plane.yml", FileStorage::READ );
if( !fs.isOpened() )
{
ts->set_failed_test_info( cvtest::TS::FAIL_INVALID_TEST_DATA );
}
// Loading ground truth plane parameters
Vec4f plane_coefficients;
Vec3f m;
fs2["plane_coefficients"] >> plane_coefficients;
fs2["m"] >> m;
// Loading calibration parameters
Mat cam1intrinsics, cam1distCoeffs, cam2intrinsics, cam2distCoeffs, R, T;
fs["cam1_intrinsics"] >> cam1intrinsics;
fs["cam2_intrinsics"] >> cam2intrinsics;
fs["cam1_distorsion"] >> cam1distCoeffs;
fs["cam2_distorsion"] >> cam2distCoeffs;
fs["R"] >> R;
fs["T"] >> T;
// Loading white and black images
vector<Mat> blackImages;
vector<Mat> whiteImages;
blackImages.resize( 2 );
whiteImages.resize( 2 );
whiteImages[0] = imread( folder + "pattern_cam1_im43.jpg", 0 );
whiteImages[1] = imread( folder + "pattern_cam2_im43.jpg", 0 );
blackImages[0] = imread( folder + "pattern_cam1_im44.jpg", 0 );
blackImages[1] = imread( folder + "pattern_cam2_im44.jpg", 0 );
Size imagesSize = whiteImages[0].size();
if( ( !cam1intrinsics.data ) || ( !cam2intrinsics.data ) || ( !cam1distCoeffs.data ) || ( !cam2distCoeffs.data ) || ( !R.data )
|| ( !T.data ) || ( !whiteImages[0].data ) || ( !whiteImages[1].data ) || ( !blackImages[0].data )
|| ( !blackImages[1].data ) )
{
ts->set_failed_test_info( cvtest::TS::FAIL_INVALID_TEST_DATA );
}
// Computing stereo rectify parameters
Mat R1, R2, P1, P2, Q;
Rect validRoi[2];
stereoRectify( cam1intrinsics, cam1distCoeffs, cam2intrinsics, cam2distCoeffs, imagesSize, R, T, R1, R2, P1, P2, Q, 0,
-1, imagesSize, &validRoi[0], &validRoi[1] );
Mat map1x, map1y, map2x, map2y;
initUndistortRectifyMap( cam1intrinsics, cam1distCoeffs, R1, P1, imagesSize, CV_32FC1, map1x, map1y );
initUndistortRectifyMap( cam2intrinsics, cam2distCoeffs, R2, P2, imagesSize, CV_32FC1, map2x, map2y );
vector<vector<Mat> > captured_pattern;
captured_pattern.resize( 2 );
captured_pattern[0].resize( numberOfPatternImages );
captured_pattern[1].resize( numberOfPatternImages );
// Loading and rectifying pattern images
for( size_t i = 0; i < numberOfPatternImages; i++ )
{
ostringstream name1;
name1 << "pattern_cam1_im" << i + 1 << ".jpg";
captured_pattern[0][i] = imread( folder + name1.str(), 0 );
ostringstream name2;
name2 << "pattern_cam2_im" << i + 1 << ".jpg";
captured_pattern[1][i] = imread( folder + name2.str(), 0 );
if( (!captured_pattern[0][i].data) || (!captured_pattern[1][i].data) )
{
ts->set_failed_test_info( cvtest::TS::FAIL_INVALID_TEST_DATA );
}
remap( captured_pattern[0][i], captured_pattern[0][i], map2x, map2y, INTER_NEAREST, BORDER_CONSTANT, Scalar() );
remap( captured_pattern[1][i], captured_pattern[1][i], map1x, map1y, INTER_NEAREST, BORDER_CONSTANT, Scalar() );
}
// Rectifying white and black images
remap( whiteImages[0], whiteImages[0], map2x, map2y, INTER_NEAREST, BORDER_CONSTANT, Scalar() );
remap( whiteImages[1], whiteImages[1], map1x, map1y, INTER_NEAREST, BORDER_CONSTANT, Scalar() );
remap( blackImages[0], blackImages[0], map2x, map2y, INTER_NEAREST, BORDER_CONSTANT, Scalar() );
remap( blackImages[1], blackImages[1], map1x, map1y, INTER_NEAREST, BORDER_CONSTANT, Scalar() );
// Setting up threshold parameters to reconstruct only the plane in foreground
graycode->setBlackThreshold( 55 );
graycode->setWhiteThreshold( 10 );
// Computing the disparity map
Mat disparityMap;
bool decoded = graycode->decode( captured_pattern, disparityMap, blackImages, whiteImages,
structured_light::DECODE_3D_UNDERWORLD );
EXPECT_TRUE( decoded );
// Computing the point cloud
Mat pointcloud;
disparityMap.convertTo( disparityMap, CV_32FC1 );
reprojectImageTo3D( disparityMap, pointcloud, Q, true, -1 );
// from mm (unit of calibration) to m
pointcloud = pointcloud / 1000;
// Setting up plane with ground truth plane values
Vec3f normal( plane_coefficients.val[0], plane_coefficients.val[1], plane_coefficients.val[2] );
Ptr<PlaneBase> plane = Ptr<PlaneBase>( new Plane( m, normal, 0 ) );
// Computing the distance of every point of the pointcloud from ground truth plane
float sum_d = 0;
int cont = 0;
for( int i = 0; i < disparityMap.rows; i++ )
{
for( int j = 0; j < disparityMap.cols; j++ )
{
float value = disparityMap.at<float>( i, j );
if( value != 0 )
{
Vec3f point = pointcloud.at<Vec3f>( i, j );
sum_d += plane->distance( point );
cont++;
}
}
}
sum_d /= cont;
// test pass if the mean of points distance from ground truth plane is lower than 3 mm
EXPECT_LE( sum_d, 0.003 );
}
void MultiBandBlender::feed(const Mat &img, const Mat &mask, Point tl)
{
CV_Assert(img.type() == CV_16SC3 || img.type() == CV_8UC3);
CV_Assert(mask.type() == CV_8U);
// Keep source image in memory with small border
int gap = 3 * (1 << num_bands_);
Point tl_new(max(dst_roi_.x, tl.x - gap),
max(dst_roi_.y, tl.y - gap));
Point br_new(min(dst_roi_.br().x, tl.x + img.cols + gap),
min(dst_roi_.br().y, tl.y + img.rows + gap));
// Ensure coordinates of top-left, bottom-right corners are divided by (1 << num_bands_).
// After that scale between layers is exactly 2.
//
// We do it to avoid interpolation problems when keeping sub-images only. There is no such problem when
// image is bordered to have size equal to the final image size, but this is too memory hungry approach.
tl_new.x = dst_roi_.x + (((tl_new.x - dst_roi_.x) >> num_bands_) << num_bands_);
tl_new.y = dst_roi_.y + (((tl_new.y - dst_roi_.y) >> num_bands_) << num_bands_);
int width = br_new.x - tl_new.x;
int height = br_new.y - tl_new.y;
width += ((1 << num_bands_) - width % (1 << num_bands_)) % (1 << num_bands_);
height += ((1 << num_bands_) - height % (1 << num_bands_)) % (1 << num_bands_);
br_new.x = tl_new.x + width;
br_new.y = tl_new.y + height;
int dy = max(br_new.y - dst_roi_.br().y, 0);
int dx = max(br_new.x - dst_roi_.br().x, 0);
tl_new.x -= dx; br_new.x -= dx;
tl_new.y -= dy; br_new.y -= dy;
int top = tl.y - tl_new.y;
int left = tl.x - tl_new.x;
int bottom = br_new.y - tl.y - img.rows;
int right = br_new.x - tl.x - img.cols;
// Create the source image Laplacian pyramid
Mat img_with_border;
copyMakeBorder(img, img_with_border, top, bottom, left, right,
BORDER_REFLECT);
vector<Mat> src_pyr_laplace;
if (can_use_gpu_ && img_with_border.depth() == CV_16S)
createLaplacePyrGpu(img_with_border, num_bands_, src_pyr_laplace);
else
createLaplacePyr(img_with_border, num_bands_, src_pyr_laplace);
// Create the weight map Gaussian pyramid
Mat weight_map;
vector<Mat> weight_pyr_gauss(num_bands_ + 1);
if(weight_type_ == CV_32F)
{
mask.convertTo(weight_map, CV_32F, 1./255.);
}
else// weight_type_ == CV_16S
{
mask.convertTo(weight_map, CV_16S);
add(weight_map, 1, weight_map, mask != 0);
}
copyMakeBorder(weight_map, weight_pyr_gauss[0], top, bottom, left, right, BORDER_CONSTANT);
for (int i = 0; i < num_bands_; ++i)
pyrDown(weight_pyr_gauss[i], weight_pyr_gauss[i + 1]);
int y_tl = tl_new.y - dst_roi_.y;
int y_br = br_new.y - dst_roi_.y;
int x_tl = tl_new.x - dst_roi_.x;
int x_br = br_new.x - dst_roi_.x;
// Add weighted layer of the source image to the final Laplacian pyramid layer
if(weight_type_ == CV_32F)
{
for (int i = 0; i <= num_bands_; ++i)
{
for (int y = y_tl; y < y_br; ++y)
{
int y_ = y - y_tl;
const Point3_<short>* src_row = src_pyr_laplace[i].ptr<Point3_<short> >(y_);
Point3_<short>* dst_row = dst_pyr_laplace_[i].ptr<Point3_<short> >(y);
const float* weight_row = weight_pyr_gauss[i].ptr<float>(y_);
float* dst_weight_row = dst_band_weights_[i].ptr<float>(y);
for (int x = x_tl; x < x_br; ++x)
{
int x_ = x - x_tl;
dst_row[x].x += static_cast<short>(src_row[x_].x * weight_row[x_]);
dst_row[x].y += static_cast<short>(src_row[x_].y * weight_row[x_]);
dst_row[x].z += static_cast<short>(src_row[x_].z * weight_row[x_]);
dst_weight_row[x] += weight_row[x_];
}
}
x_tl /= 2; y_tl /= 2;
x_br /= 2; y_br /= 2;
}
}
else// weight_type_ == CV_16S
{
for (int i = 0; i <= num_bands_; ++i)
{
for (int y = y_tl; y < y_br; ++y)
{
int y_ = y - y_tl;
const Point3_<short>* src_row = src_pyr_laplace[i].ptr<Point3_<short> >(y_);
Point3_<short>* dst_row = dst_pyr_laplace_[i].ptr<Point3_<short> >(y);
const short* weight_row = weight_pyr_gauss[i].ptr<short>(y_);
short* dst_weight_row = dst_band_weights_[i].ptr<short>(y);
for (int x = x_tl; x < x_br; ++x)
{
int x_ = x - x_tl;
dst_row[x].x += short((src_row[x_].x * weight_row[x_]) >> 8);
dst_row[x].y += short((src_row[x_].y * weight_row[x_]) >> 8);
dst_row[x].z += short((src_row[x_].z * weight_row[x_]) >> 8);
dst_weight_row[x] += weight_row[x_];
}
}
x_tl /= 2; y_tl /= 2;
x_br /= 2; y_br /= 2;
}
}
}
Mat TableObjectDetector::clusterObjects(Mat P, int K, bool removeOutliers) {
Mat L;
int attempts = 5;
P.convertTo(P, CV_32F);
kmeans(P, K, L, TermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS, 10000, 0.0001), attempts, KMEANS_PP_CENTERS);
// We remove outliers that are outside a number of standard deviations away
// from the object centroid. We do this by just setting their cluster label
// to -1
if (removeOutliers) {
float numStdDevs = 3;
float maxDist = 0.1;
// Caluclate centroids
vector<Mat> clusterData;
Mat clusterCentroids(K, 3, CV_32F);
for (int k=0; k<K; k++) {
Mat D = Mat(0, 3, CV_64F);
for (int i=0; i<L.rows; i++) {
if (L.at<int>(i)==k) {
D.push_back(P.row(i));
}
}
clusterData.push_back(D);
reduce(D, clusterCentroids.row(k), 0, CV_REDUCE_AVG);
}
// Now calculate distances of each point, and the std. devs. of each
// cluster
Mat Dist = Mat::zeros(P.rows, 1, CV_32F);
vector<Mat> centroidDistances;
for (int k=0; k<K; k++) {
centroidDistances.push_back(Mat(0, 1, CV_32F));
}
for (int i=0; i<L.rows; i++) {
Mat centroid = clusterCentroids.row(L.at<int>(i));
Mat pt = P.row(i);
int k = L.at<int>(i);
float d = std::sqrt(
std::pow(pt.at<float>(0) - centroid.at<float>(0), 2) +
std::pow(pt.at<float>(1) - centroid.at<float>(1), 2) +
std::pow(pt.at<float>(2) - centroid.at<float>(2), 2) );
Dist.at<float>(i) = d;
centroidDistances.at(k).push_back(d);
}
for (int k=0; k<K; k++) {
Mat ignore;
Mat std_dev;
meanStdDev(centroidDistances.at(k), ignore, std_dev);
float k_std = std_dev.at<Scalar>(0)(0);
for (int i=0; i<P.rows; i++) {
if (L.at<int>(i) == k) {
//if (Dist.at<float>(i) > numStdDevs*k_std) {
if (Dist.at<float>(i) > maxDist) {
L.at<int>(i) = -1;
}
}
}
}
// Now compute standard deviations for all clusters
//Mat centroidStdDevs = Mat::zeros(K, 1);
//for (int k=0; k<K; k++) {
//}
}
return L;
}
static inline Mat convertTo(const Mat& m, double a=1, double b=0 ){ Mat t; m.convertTo( t, CV_MAKETYPE(DataDepth<T>::value,m.channels()), a, b ); return t; }
