void findEssentialMatrix(MFramePair& pair, Mat_<double> K) { vector<Point2f> k1, k2,n1,n2; vector<int> usedIndex1,usedIndex2; vector<uchar> status(pair.imgpts1.size()); Mat F = findFundamentalMat(pair.matchPts1, pair.matchPts2, CV_FM_RANSAC, 0.2, 0.9, status); Mat_<double> E; E = K.t() * F * K; //according to HZ (9.12) for (unsigned int i = 0; i < status.size(); i++) { // queryIdx is the "left" image if (status[i]) { usedIndex1.push_back(pair.matchedIndex1[i]); k1.push_back(pair.matchPts1[i]); usedIndex2.push_back(pair.matchedIndex2[i]); k2.push_back(pair.matchPts2[i]); } } correctMatches(F,k1,k2,n1,n2); pair.matchPts1 = n1; pair.matchPts2 = n2; pair.matchedIndex1=usedIndex1; pair.matchedIndex2=usedIndex2; pair.F = F; pair.E = E; }
float TrainableStatModel::leaveOneOutCrossValidation(const Mat_<float> &samples, const Mat_<int> &classes) { int correctResults = 0; Mat_<int> sampleIdx(samples.rows - 1, 1); for (int i = 1; i < samples.rows; i++) { sampleIdx(i - 1, 0) = i; } for (int i = 0; i < samples.rows; i++) { this->clear(); this->train(samples, classes, sampleIdx); int actual = (float)this->predict(samples.row(i)); if (actual == classes(i,0)) { correctResults++; } cout<<"actual = "<<actual<<", expected = "<<classes(i,0)<<endl; sampleIdx(i, 0) = i; } return (float)correctResults/(float)samples.rows; }
static void drawOpticalFlow(const Mat_<Point2f>& flow, Mat& dst, float maxmotion = -1) { dst.create(flow.size(), CV_8UC3); dst.setTo(Scalar::all(0)); // determine motion range: float maxrad = maxmotion; if (maxmotion <= 0) { maxrad = 1; for (int y = 0; y < flow.rows; ++y) { for (int x = 0; x < flow.cols; ++x) { Point2f u = flow(y, x); if (!isFlowCorrect(u)) continue; maxrad = max(maxrad, sqrt(u.x * u.x + u.y * u.y)); } } } for (int y = 0; y < flow.rows; ++y) { for (int x = 0; x < flow.cols; ++x) { Point2f u = flow(y, x); if (isFlowCorrect(u)) dst.at<Vec3b>(y, x) = computeColor(u.x / maxrad, u.y / maxrad); } } }
bool SimpleNN::predict(const Mat_<double> &test_X, Mat_<double> &result, string &err_msg){ Mat_<double> input_data = test_X.reshape(0, test_X.rows*test_X.cols); // make it column vector if (input_data.rows != this->structure[0]){ err_msg = "wrong input size"; return false; } for (int row_index = 1; row_index < this->layers[0].rows; ++row_index){ this->layers[0](row_index, 0) = input_data(row_index-1, 0); } int num_layers = (int) this->layers.size(); for (int layer_id = 0; layer_id < num_layers - 2; ++layer_id){ Mat_<double> product = tanh(this->weights[layer_id]*this->layers[layer_id]); for (int row_index = 1; row_index < this->layers[layer_id+1].rows; ++row_index){ this->layers[layer_id+1](row_index, 0) = product(row_index-1, 0); } } // compute the output layer { int layer_id = num_layers - 2; this->layers[layer_id + 1] = tanh(this->weights[layer_id] * this->layers[layer_id]); } result = this->layers[num_layers - 1]; // return last layers (output layer). cout << "result:\n" << result << endl; err_msg = ""; return true; }
Mat optical_flow(const Mat_<float>& ImgA, const Mat_<float>& ImgB, int num_it, float threshold) { // Compute Gaussin Pyramid int nl = 5; float ds = 0.5; stack<pair<Mat, Mat> > gp = compute_gaussian_pyramids(ImgA, ImgB, nl, ds); Mat_<float> u = Mat::zeros(ImgA.size(), CV_32F); Mat_<float> v = Mat::zeros(ImgA.size(), CV_32F); while (!gp.empty()) { Mat imgA = (gp.top()).first; Mat imgB = (gp.top()).second; // Warp the first image. Mat_<float> imgBw = imgB.clone(); /* Compute warping here from u and v. */ imgBw = compute_warp(imgB, u, v, ds); /* Compute the derivatives. */ Mat_<float> Ix, Iy, It; compute_derivatives(imgBw, imgA, Ix, Iy, It); // papers Mat_<float> du = Mat::zeros(imgA.size(), CV_32F); Mat_<float> dv = Mat::zeros(imgA.size(), CV_32F); for (int i = 0; i < 500; i ++) iterative_computation(du, dv, Ix, Iy, It); u = u - du; v = v - dv; gp.pop(); } Mat Mflow = color_map(u, v); return Mflow; }
void NNLSOptimizer::scannls(const Mat& A, const Mat& b,Mat &x) { int iter = 0; int m = A.size().height; int n = A.size().width; Mat_<double> AT = A.t(); double error = 1e-8; Mat_<double> H = AT*A; Mat_<double> f = -AT*b; Mat_<double> x_old = Mat_<double>::zeros(n,1); Mat_<double> x_new = Mat_<double>::zeros(n,1); Mat_<double> mu_old = Mat_<double>::zeros(n,1); Mat_<double> mu_new = Mat_<double>::zeros(n,1); Mat_<double> r = Mat_<double>::zeros(n,1); f.copyTo(mu_old); while(iter < NNLS_MAX_ITER) { iter++; for(int k=0;k<n;k++) { x_old.copyTo(x_new); x_new(k,0) = std::max(0.0, x_old(k,0) - (mu_old(k,0)/H(k,k)) ); if(x_new(k,0) != x_old(k,0)) { r = mu_old + (x_new(k,0) - x_old(k,0))*H.col(k); r.copyTo(mu_new); } x_new.copyTo(x_old); mu_new.copyTo(mu_old); } if(eKKT(H,f,x_new,error) == true) { break; } } x_new.copyTo(x); }
void SegmenterHumanSimple::segment(const cv::Mat& img, Mat_<uchar>& mask) { Mat imgBGR; Mat imgLAB; Mat imgBGRo; float rate = 500.0f/img.cols; GaussianBlur(img,imgBGRo,Size(),0.8,0.8); vector<Rect> faces; resize(imgBGRo,imgBGRo,Size(),rate,rate); cv::CascadeClassifier faceModel(this->_m_filenameFaceModel); faceModel.detectMultiScale(imgBGRo,faces); imgBGRo.convertTo( imgBGR, CV_32F, 1.0/255. ); cvtColor( imgBGR, imgLAB, CV_BGR2Lab ); Superpixel sp(1000,1,5); Mat_<int> segmentation = sp.segment(imgLAB); vector<SuperpixelStatistic> stat = sp.stat(imgLAB,imgBGR,segmentation); Mat_<float> prob; this->getPixelProbability(imgBGRo,prob,faces); Mat_<float> sprob; UtilsSuperpixel::Stat(segmentation,prob,stat,sprob); Mat_<int> initial(int(stat.size()),1); initial.setTo(1,sprob>0.5); initial.setTo(0,sprob<=0.5); Mat_<float> probaColor; int myx = cv::countNonZero(initial); this->_getColorProba(stat,initial,probaColor); Mat_<float> fgdInit,bgdInit,fgdColor,bgdColor; this->_prob2energy(sprob,fgdInit,bgdInit); this->_prob2energy(probaColor,fgdColor,bgdColor); Mat_<float> fgdEnergy, bgdEnergy; fgdEnergy = fgdInit + fgdColor; bgdEnergy = bgdInit + bgdColor; Mat_<int> label; mask.create(imgBGRo.rows,imgBGRo.cols); UtilsSegmentation::MaxFlowSuperpixel(stat,fgdEnergy,bgdEnergy,50.0,label); for( int i=0;i<mask.rows;i++) { for(int j=0;j<mask.cols;j++) { if ( label(segmentation(i,j)) > 0.5) { mask(i,j) = 255; } else { mask(i,j) = 0; } } } cv::resize(mask,mask,Size(img.cols,img.rows)); mask.setTo(255,mask>128); mask.setTo(0,mask<=128); }
void run(int /* start_from */ ) { CvMat zeros; memset(&zeros, 0, sizeof(zeros)); C_Caller caller, bad_caller; CvMat objectPoints_c, r_vec_c, t_vec_c, A_c, distCoeffs_c, imagePoints_c, dpdr_c, dpdt_c, dpdf_c, dpdc_c, dpdk_c; const int n = 10; Mat imagePoints_cpp(1, n, CV_32FC2); imagePoints_c = imagePoints_cpp; Mat objectPoints_cpp(1, n, CV_32FC3); randu(objectPoints_cpp, Scalar::all(1), Scalar::all(10)); objectPoints_c = objectPoints_cpp; Mat t_vec_cpp(Mat::zeros(1, 3, CV_32F)); t_vec_c = t_vec_cpp; Mat r_vec_cpp; Rodrigues(Mat::eye(3, 3, CV_32F), r_vec_cpp); r_vec_c = r_vec_cpp; Mat A_cpp = camMat.clone(); A_c = A_cpp; Mat distCoeffs_cpp = distCoeffs.clone(); distCoeffs_c = distCoeffs_cpp; Mat dpdr_cpp(2*n, 3, CV_32F); dpdr_c = dpdr_cpp; Mat dpdt_cpp(2*n, 3, CV_32F); dpdt_c = dpdt_cpp; Mat dpdf_cpp(2*n, 2, CV_32F); dpdf_c = dpdf_cpp; Mat dpdc_cpp(2*n, 2, CV_32F); dpdc_c = dpdc_cpp; Mat dpdk_cpp(2*n, 4, CV_32F); dpdk_c = dpdk_cpp; caller.aspectRatio = 1.0; caller.objectPoints = &objectPoints_c; caller.r_vec = &r_vec_c; caller.t_vec = &t_vec_c; caller.A = &A_c; caller.distCoeffs = &distCoeffs_c; caller.imagePoints = &imagePoints_c; caller.dpdr = &dpdr_c; caller.dpdt = &dpdt_c; caller.dpdf = &dpdf_c; caller.dpdc = &dpdc_c; caller.dpdk = &dpdk_c; /********************/ int errors = 0; bad_caller = caller; bad_caller.objectPoints = 0; errors += run_test_case( CV_StsBadArg, "Zero objectPoints", bad_caller ); bad_caller = caller; bad_caller.r_vec = 0; errors += run_test_case( CV_StsBadArg, "Zero r_vec", bad_caller ); bad_caller = caller; bad_caller.t_vec = 0; errors += run_test_case( CV_StsBadArg, "Zero t_vec", bad_caller ); bad_caller = caller; bad_caller.A = 0; errors += run_test_case( CV_StsBadArg, "Zero camMat", bad_caller ); bad_caller = caller; bad_caller.imagePoints = 0; errors += run_test_case( CV_StsBadArg, "Zero imagePoints", bad_caller ); /****************************/ Mat bad_r_vec_cpp1(r_vec_cpp.size(), CV_32S); CvMat bad_r_vec_c1 = bad_r_vec_cpp1; Mat bad_r_vec_cpp2(2, 2, CV_32F); CvMat bad_r_vec_c2 = bad_r_vec_cpp2; Mat bad_r_vec_cpp3(r_vec_cpp.size(), CV_32FC2); CvMat bad_r_vec_c3 = bad_r_vec_cpp3; bad_caller = caller; bad_caller.r_vec = &bad_r_vec_c1; errors += run_test_case( CV_StsBadArg, "Bad rvec format", bad_caller ); bad_caller = caller; bad_caller.r_vec = &bad_r_vec_c2; errors += run_test_case( CV_StsBadArg, "Bad rvec format", bad_caller ); bad_caller = caller; bad_caller.r_vec = &bad_r_vec_c3; errors += run_test_case( CV_StsBadArg, "Bad rvec format", bad_caller ); /****************************/ Mat bad_t_vec_cpp1(t_vec_cpp.size(), CV_32S); CvMat bad_t_vec_c1 = bad_t_vec_cpp1; Mat bad_t_vec_cpp2(2, 2, CV_32F); CvMat bad_t_vec_c2 = bad_t_vec_cpp2; Mat bad_t_vec_cpp3(1, 1, CV_32FC2); CvMat bad_t_vec_c3 = bad_t_vec_cpp3; bad_caller = caller; bad_caller.t_vec = &bad_t_vec_c1; errors += run_test_case( CV_StsBadArg, "Bad tvec format", bad_caller ); bad_caller = caller; bad_caller.t_vec = &bad_t_vec_c2; errors += run_test_case( CV_StsBadArg, "Bad tvec format", bad_caller ); bad_caller = caller; bad_caller.t_vec = &bad_t_vec_c3; errors += run_test_case( CV_StsBadArg, "Bad tvec format", bad_caller ); /****************************/ Mat bad_A_cpp1(A_cpp.size(), CV_32S); CvMat bad_A_c1 = bad_A_cpp1; Mat bad_A_cpp2(2, 2, CV_32F); CvMat bad_A_c2 = bad_A_cpp2; bad_caller = caller; bad_caller.A = &bad_A_c1; errors += run_test_case( CV_StsBadArg, "Bad A format", bad_caller ); bad_caller = caller; bad_caller.A = &bad_A_c2; errors += run_test_case( CV_StsBadArg, "Bad A format", bad_caller ); /****************************/ Mat bad_distCoeffs_cpp1(distCoeffs_cpp.size(), CV_32S); CvMat bad_distCoeffs_c1 = bad_distCoeffs_cpp1; Mat bad_distCoeffs_cpp2(2, 2, CV_32F); CvMat bad_distCoeffs_c2 = bad_distCoeffs_cpp2; Mat bad_distCoeffs_cpp3(1, 7, CV_32F); CvMat bad_distCoeffs_c3 = bad_distCoeffs_cpp3; bad_caller = caller; bad_caller.distCoeffs = &zeros; errors += run_test_case( CV_StsBadArg, "Bad distCoeffs format", bad_caller ); bad_caller = caller; bad_caller.distCoeffs = &bad_distCoeffs_c1; errors += run_test_case( CV_StsBadArg, "Bad distCoeffs format", bad_caller ); bad_caller = caller; bad_caller.distCoeffs = &bad_distCoeffs_c2; errors += run_test_case( CV_StsBadArg, "Bad distCoeffs format", bad_caller ); bad_caller = caller; bad_caller.distCoeffs = &bad_distCoeffs_c3; errors += run_test_case( CV_StsBadArg, "Bad distCoeffs format", bad_caller ); /****************************/ Mat bad_dpdr_cpp1(dpdr_cpp.size(), CV_32S); CvMat bad_dpdr_c1 = bad_dpdr_cpp1; Mat bad_dpdr_cpp2(dpdr_cpp.cols+1, 3, CV_32F); CvMat bad_dpdr_c2 = bad_dpdr_cpp2; Mat bad_dpdr_cpp3(dpdr_cpp.cols, 7, CV_32F); CvMat bad_dpdr_c3 = bad_dpdr_cpp3; bad_caller = caller; bad_caller.dpdr = &zeros; errors += run_test_case( CV_StsBadArg, "Bad dpdr format", bad_caller ); bad_caller = caller; bad_caller.dpdr = &bad_dpdr_c1; errors += run_test_case( CV_StsBadArg, "Bad dpdr format", bad_caller ); bad_caller = caller; bad_caller.dpdr = &bad_dpdr_c2; errors += run_test_case( CV_StsBadArg, "Bad dpdr format", bad_caller ); bad_caller = caller; bad_caller.dpdr = &bad_dpdr_c3; errors += run_test_case( CV_StsBadArg, "Bad dpdr format", bad_caller ); /****************************/ bad_caller = caller; bad_caller.dpdt = &zeros; errors += run_test_case( CV_StsBadArg, "Bad dpdt format", bad_caller ); bad_caller = caller; bad_caller.dpdt = &bad_dpdr_c1; errors += run_test_case( CV_StsBadArg, "Bad dpdt format", bad_caller ); bad_caller = caller; bad_caller.dpdt = &bad_dpdr_c2; errors += run_test_case( CV_StsBadArg, "Bad dpdt format", bad_caller ); bad_caller = caller; bad_caller.dpdt = &bad_dpdr_c3; errors += run_test_case( CV_StsBadArg, "Bad dpdt format", bad_caller ); /****************************/ Mat bad_dpdf_cpp2(dpdr_cpp.cols+1, 2, CV_32F); CvMat bad_dpdf_c2 = bad_dpdf_cpp2; bad_caller = caller; bad_caller.dpdf = &zeros; errors += run_test_case( CV_StsBadArg, "Bad dpdf format", bad_caller ); bad_caller = caller; bad_caller.dpdf = &bad_dpdr_c1; errors += run_test_case( CV_StsBadArg, "Bad dpdf format", bad_caller ); bad_caller = caller; bad_caller.dpdf = &bad_dpdf_c2; errors += run_test_case( CV_StsBadArg, "Bad dpdf format", bad_caller ); bad_caller = caller; bad_caller.dpdf = &bad_dpdr_c3; errors += run_test_case( CV_StsBadArg, "Bad dpdf format", bad_caller ); /****************************/ bad_caller = caller; bad_caller.dpdc = &zeros; errors += run_test_case( CV_StsBadArg, "Bad dpdc format", bad_caller ); bad_caller = caller; bad_caller.dpdc = &bad_dpdr_c1; errors += run_test_case( CV_StsBadArg, "Bad dpdc format", bad_caller ); bad_caller = caller; bad_caller.dpdc = &bad_dpdf_c2; errors += run_test_case( CV_StsBadArg, "Bad dpdc format", bad_caller ); bad_caller = caller; bad_caller.dpdc = &bad_dpdr_c3; errors += run_test_case( CV_StsBadArg, "Bad dpdc format", bad_caller ); /****************************/ bad_caller = caller; bad_caller.dpdk = &zeros; errors += run_test_case( CV_StsBadArg, "Bad dpdk format", bad_caller ); bad_caller = caller; bad_caller.dpdk = &bad_dpdr_c1; errors += run_test_case( CV_StsBadArg, "Bad dpdk format", bad_caller ); bad_caller = caller; bad_caller.dpdk = &bad_dpdf_c2; errors += run_test_case( CV_StsBadArg, "Bad dpdk format", bad_caller ); bad_caller = caller; bad_caller.dpdk = &bad_dpdr_c3; errors += run_test_case( CV_StsBadArg, "Bad dpdk format", bad_caller ); bad_caller = caller; bad_caller.distCoeffs = 0; errors += run_test_case( CV_StsNullPtr, "distCoeffs is NULL while dpdk is not", bad_caller ); if (errors) ts->set_failed_test_info(cvtest::TS::FAIL_MISMATCH); else ts->set_failed_test_info(cvtest::TS::OK); }
//=========================================================================== void SVR_patch_expert::Response(const Mat_<float>& area_of_interest, Mat_<double>& response) { int response_height = area_of_interest.rows - weights.rows + 1; int response_width = area_of_interest.cols - weights.cols + 1; // the patch area on which we will calculate reponses cv::Mat_<float> normalised_area_of_interest; if(response.rows != response_height || response.cols != response_width) { response.create(response_height, response_width); } // If type is raw just normalise mean and standard deviation if(type == 0) { // Perform normalisation across whole patch cv::Scalar mean; cv::Scalar std; cv::meanStdDev(area_of_interest, mean, std); // Avoid division by zero if(std[0] == 0) { std[0] = 1; } normalised_area_of_interest = (area_of_interest - mean[0]) / std[0]; } // If type is gradient, perform the image gradient computation else if(type == 1) { Grad(area_of_interest, normalised_area_of_interest); } else { printf("ERROR(%s,%d): Unsupported patch type %d!\n", __FILE__,__LINE__, type); abort(); } Mat_<float> svr_response; // The empty matrix as we don't pass precomputed dft's of image Mat_<double> empty_matrix_0(0,0,0.0); Mat_<float> empty_matrix_1(0,0,0.0); Mat_<float> empty_matrix_2(0,0,0.0); // Efficient calc of patch expert SVR response across the area of interest matchTemplate_m(normalised_area_of_interest, empty_matrix_0, empty_matrix_1, empty_matrix_2, weights, weights_dfts, svr_response, CV_TM_CCOEFF_NORMED); response.create(svr_response.size()); MatIterator_<double> p = response.begin(); cv::MatIterator_<float> q1 = svr_response.begin(); // respone for each pixel cv::MatIterator_<float> q2 = svr_response.end(); while(q1 != q2) { // the SVR response passed into logistic regressor *p++ = 1.0/(1.0 + exp( -(*q1++ * scaling + bias ))); } }
/** * @author JIA Pei * @version 2010-05-20 * @brief Basic AAM Fitting, for dynamic image sequence * @param iImg Input - image to be fitted * @param ioShape Input and Output - the fitted shape * @param oImg Output - the fitted image * @param epoch Input - the iteration epoch */ float VO_FittingAAMBasic::VO_BasicAAMFitting(const Mat& iImg, VO_Shape& ioShape, Mat& oImg, unsigned int epoch) { this->m_VOFittingShape.clone(ioShape); double t = (double)cvGetTickCount(); this->SetProcessingImage(iImg, this->m_VOAAMBasic); this->m_iIteration = 0; // Get m_MatModelAlignedShapeParam and m_fScale, m_vRotateAngles, m_MatCenterOfGravity this->m_VOAAMBasic->VO_CalcAllParams4AnyShapeWithConstrain( this->m_VOFittingShape, this->m_MatModelAlignedShapeParam, this->m_fScale, this->m_vRotateAngles, this->m_MatCenterOfGravity); this->m_VOFittingShape.ConstrainShapeInImage(this->m_ImageProcessing); // Get m_MatModelNormalizedTextureParam VO_TextureModel::VO_LoadOneTextureFromShape(this->m_VOFittingShape, this->m_ImageProcessing, this->m_vTriangle2D, this->m_vPointWarpInfo, this->m_VOFittingTexture ); // estimate the texture model parameters this->m_VOAAMBasic->VO_CalcAllParams4AnyTexture(this->m_VOFittingTexture, this->m_MatModelNormalizedTextureParam); // Calculate m_MatCurrentC this->m_VOAAMBasic->VO_SParamTParamProjectToCParam( this->m_MatModelAlignedShapeParam, this->m_MatModelNormalizedTextureParam, this->m_MatCurrentC ); // Set m_MatCurrentT, m_MatDeltaT, m_MatEstimatedT, m_MatDeltaC, m_MatEstimatedC, etc. this->m_MatCurrentT = Mat_<float>::zeros(this->m_MatCurrentT.size()); this->m_MatDeltaT = Mat_<float>::zeros(this->m_MatDeltaT.size()); this->m_MatEstimatedT = Mat_<float>::zeros(this->m_MatEstimatedT.size()); this->m_MatDeltaC = Mat_<float>::zeros(this->m_MatDeltaC.size()); this->m_MatEstimatedC = Mat_<float>::zeros(this->m_MatEstimatedC.size()); ////////////////////////////////////////////////////////////////////////////////////////////////////// // explained by JIA Pei. 2010-05-20 // For the first round, this->m_VOFittingShape should not change after calling "VO_CParamTParam2FittingShape" // But this is not the case. why? // Before calling VO_CParamTParam2FittingShape, this->m_VOFittingShape is calculated by // a) assigning m_VOTemplateAlignedShape // b) align to the real-size face using detected eyes and mouth // c) constrain the shape within the image // d) constrain the shape parameters and calculate those rigid transform parameters // cout << this->m_VOFittingShape << endl; ////////////////////////////////////////////////////////////////////////////////////////////////////// // Estimate m_VOFittingShape and m_VOFittingTexture this->VO_CParamTParam2FittingShape( this->m_MatCurrentC, this->m_MatCurrentT, this->m_VOModelNormalizedTexture, this->m_VOFittingShape, this->m_fScale, this->m_vRotateAngles, this->m_MatCenterOfGravity ); this->m_VOFittingShape.ConstrainShapeInImage(this->m_ImageProcessing); // Remember to call ConstrainShapeInImage() whenever you update m_VOFittingShape ////////////////////////////////////////////////////////////////////////////////////////////////////// // When calling VO_CParamTParam2FittingShape, this->m_VOFittingShape is calculated by // a) c parameters to reconstruct shape parameters // b) shape parameters to reconstruct shape // c) align to the real-size face by global shape normalization // cout << this->m_VOFittingShape << endl; ////////////////////////////////////////////////////////////////////////////////////////////////////// this->m_E_previous = this->m_E = this->VO_CalcErrorImage(this->m_ImageProcessing, this->m_VOFittingShape, this->m_VOModelNormalizedTexture, this->m_VOTextureError); do { float estScale = this->m_fScale; vector<float> estRotateAngles = this->m_vRotateAngles; Mat_<float> estCOG = this->m_MatCenterOfGravity.clone(); bool cBetter = false; bool poseBetter = false; /**First shape parameters, c parameters. refer to equation (9.3) * Cootes "Statistical Model of Appearance for Computer Vision" */ cv::gemm(this->m_VOTextureError.GetTheTextureInARow(), this->m_VOAAMBasic->m_MatRc, -1, Mat(), 0.0, this->m_MatDeltaC, GEMM_2_T); // damp -- C for(unsigned int i = 0; i < k_values.size(); i++) { // make damped c prediction cv::scaleAdd(this->m_MatDeltaC, k_values[i], this->m_MatCurrentC, this->m_MatEstimatedC); // make sure m_MatEstimatedC are constrained this->m_VOAAMBasic->VO_AppearanceParameterConstraint(this->m_MatEstimatedC); this->VO_CParamTParam2FittingShape( this->m_MatEstimatedC, this->m_MatCurrentT, this->m_VOModelNormalizedTexture, this->m_VOEstimatedShape, estScale, estRotateAngles, estCOG); if ( !VO_ShapeModel::VO_IsShapeInsideImage(this->m_VOEstimatedShape, this->m_ImageProcessing) ) continue; else this->m_E = this->VO_CalcErrorImage(this->m_ImageProcessing, this->m_VOEstimatedShape, this->m_VOModelNormalizedTexture, this->m_VOEstimatedTextureError); if (this->m_E < this->m_E_previous) { this->m_MatEstimatedC.copyTo(this->m_MatCurrentC); this->m_VOFittingShape.clone(this->m_VOEstimatedShape); this->m_VOTextureError.clone(this->m_VOEstimatedTextureError); this->m_E_previous = this->m_E; cBetter = true; this->m_fScale = estScale; this->m_vRotateAngles = estRotateAngles; this->m_MatCenterOfGravity = estCOG.clone(); break; } } /** Second pose, t parameters. refer to equation (9.3) * Cootes "Statistical Model of Appearance for Computer Vision" */ cv::gemm(this->m_VOTextureError.GetTheTextureInARow(), this->m_VOAAMBasic->m_MatRt, -1, Mat(), 0, this->m_MatDeltaT, GEMM_2_T); // damp -- T for(unsigned int i = 0; i < k_values.size(); i++) { // make damped c/pose prediction cv::scaleAdd(this->m_MatDeltaT, k_values[i], this->m_MatCurrentT, this->m_MatEstimatedT); this->VO_CParamTParam2FittingShape( this->m_MatCurrentC, this->m_MatEstimatedT, this->m_VOModelNormalizedTexture, this->m_VOEstimatedShape, estScale, estRotateAngles, estCOG); if ( !VO_ShapeModel::VO_IsShapeInsideImage(this->m_VOEstimatedShape, this->m_ImageProcessing) ) continue; else this->m_E = this->VO_CalcErrorImage(this->m_ImageProcessing, this->m_VOEstimatedShape, this->m_VOModelNormalizedTexture, this->m_VOEstimatedTextureError); if (this->m_E < this->m_E_previous) { // Since m_fScale, m_vRotateAngles and m_MatCenterOfGravity have been updated, // m_MatCurrentT should be assigned to 0 now! this->m_MatCurrentT = Mat_<float>::zeros(this->m_MatCurrentT.size()); // this->m_MatEstimatedT.copyTo(this->m_MatCurrentT); this->m_VOFittingShape.clone(this->m_VOEstimatedShape); this->m_VOTextureError.clone(this->m_VOEstimatedTextureError); this->m_E_previous = this->m_E; poseBetter = true; this->m_fScale = estScale; this->m_vRotateAngles = estRotateAngles; this->m_MatCenterOfGravity = estCOG.clone(); break; } } if( cBetter || poseBetter) { ioShape.clone(this->m_VOFittingShape); } else break; ++this->m_iIteration; }while( ( fabs(this->m_E) > FLT_EPSILON ) && (this->m_iIteration < epoch)/* && (cv::norm(this->m_MatDeltaC) > FLT_EPSILON) */ ); t = ((double)cvGetTickCount() - t )/ (cvGetTickFrequency()*1000.); cout << "Basic fitting time cost: " << t << " millisec" << endl; this->m_fFittingTime = t; VO_Fitting2DSM::VO_DrawMesh(ioShape, this->m_VOAAMBasic, oImg); return t; }
int BaseDecisionTree::fit(Mat_<double> _X, Mat_<double> _y, Mat_<double> sample_weight) { // Validation if (_X.rows == 0 || _X.cols == 0) return 1; // Determine output setting _n_samples = _X.rows; _n_features = _X.cols; // Reshape y to shape[n_samples, 1] _y = _y.reshape(1, _y.total()); // Validation if (_y.rows != _n_samples) return 2; // Calculate class_weight Mat expended_class_weight(0, 0, CV_32F); // Get class_weight if (_class_weight.total() != 0) expended_class_weight = compute_sample_weight(_class_weight, _y); // Validation if (_max_depth <= 0) _max_depth = static_cast<int>(pow(2, 31) - 1); if (_max_leaf_nodes <= 0) _max_leaf_nodes = -1; if (_max_features <= 0) _max_features = _n_features; if (_max_leaf_nodes > -1 && _max_leaf_nodes < 2) return 3; if (_min_samples_split <= 0) return 4; if (_min_samples_leaf <= 0) return 5; if (_min_weight_fraction_leaf >= 0 && _min_weight_fraction_leaf <= 0.5) return 6; // Set samples' weight if (expended_class_weight.total()) { for (int i = 0; i < sample_weight.total(); i++) { sample_weight.at<double>(i, 0) = sample_weight.at<double>(i, 0) * \ expended_class_weight.at<double>(i, 0); } } else { sample_weight = expended_class_weight; } // Set min_weight_fraction_leaf if (_min_weight_fraction_leaf != 0.) _min_weight_fraction_leaf = _min_weight_fraction_leaf * cv::sum(sample_weight); else _min_weight_fraction_leaf = 0.; // Set min_samples_split _min_samples_split = max(_min_samples_split, 2 * _min_samples_leaf); }
/** * @param avgSParam - input mean shape parameters * @param icovSParam - input covariance matrix of shape parameters * @param avgTParam - input mean texture parameters * @param icovTParam - input covariance matrix of texture parameters * @param iSParams - input the vector of multiple input shape parameters * @param iTParams - input the vector of multiple input texture parameter * @param ShapeDistMean - input mean texture parameters * @param ShapeDistStddev - input covariance matrix of texture parameters * @param TextureDistMean - input the input shape parameter * @param TextureDistStddev - input the input texture parameter * @param WeakFitting - input only shape parameter is used? * @return whether the fitting is acceptable */ bool CRecognitionAlgs::CalcFittingEffect4ImageSequence( const Mat_<float>& avgSParam, const Mat_<float>& icovSParam, const Mat_<float>& avgTParam, const Mat_<float>& icovTParam, const Mat_<float>& iSParams, const Mat_<float>& iTParams, const Scalar& ShapeDistMean, const Scalar& ShapeDistStddev, const Scalar& TextureDistMean, const Scalar& TextureDistStddev, bool WeakFitting ) { assert(iSParams.rows == iTParams.rows); unsigned int NbOfSamples = iSParams.rows; vector<float> sDists, tDists; sDists.resize(NbOfSamples); tDists.resize(NbOfSamples); for(unsigned int i = 0; i < NbOfSamples; ++i) { CRecognitionAlgs::CalcFittingEffect4StaticImage( avgSParam, icovSParam, avgTParam, icovTParam, iSParams.row(i), iTParams.row(i), ShapeDistMean, ShapeDistStddev, TextureDistMean, TextureDistStddev, sDists[i], tDists[i], WeakFitting ); } unsigned int NbOfGood1 = 0; unsigned int NbOfGood2 = 0; for(unsigned int i = 0; i < NbOfSamples; ++i) { if( ( fabs( sDists[i] - ShapeDistMean.val[0] ) < 1.5f * ShapeDistStddev.val[0] ) ) { NbOfGood1++; if( ( fabs( tDists[i] - TextureDistMean.val[0] ) < 3.0f*TextureDistStddev.val[0] ) ) { NbOfGood2++; } } } if(WeakFitting) { if(NbOfGood1 >= (unsigned int )(0.75*NbOfSamples) ) return true; else return false; } else { if(NbOfGood2 >= (unsigned int )(0.75*NbOfGood1) ) return true; else return false; } }
void UtilsSegmentation::MaxFlowSuperpixel(std::vector<SuperpixelStatistic>& spstat, const Mat_<float>& fgdEnergy, const Mat_<float>& bgdEnergy, float gamma, Mat_<int>& label) { //::Graph<float,float,float> graph(nNode,nEdge,errfunc); //graph int nEdge = UtilsSuperpixel::CountEdge(spstat); Mat_<int> edgeVertex(nEdge,2); Mat_<float> edgeWeight(nEdge,1); Mat_<float> edgeLen(nEdge,1); int idx = 0; for(int i=0;i<spstat.size();i++) { SuperpixelStatistic& sp = spstat[i]; for( set<int>::iterator j=sp.conn.begin(); j!= sp.conn.end(); j++) { int d = (*j); SuperpixelStatistic& dsp = spstat[d]; if ( i != d) { edgeVertex(idx,0) = min(i,d); edgeVertex(idx,1) = max(i,d); float diff = (float) norm(sp.mean_color_ - dsp.mean_color_); edgeWeight(idx) = diff*diff; edgeLen(idx) = (float) cv::norm(sp.mean_position_-dsp.mean_position_); idx++; } } } float beta = (float) cv::mean(edgeWeight)[0]; Graph<float,float,float> graph((int)spstat.size(), nEdge, errfunc); graph.add_node((int)spstat.size()); for(int i=0;i<fgdEnergy.total();i++) { graph.add_tweights(i,bgdEnergy(i),fgdEnergy(i)); } edgeWeight = - edgeWeight / beta; cv::exp(edgeWeight,edgeWeight); edgeWeight *= gamma; cv::divide(edgeWeight, edgeLen,edgeWeight); for(int i=0;i<nEdge;i++) { float w = edgeWeight(i); graph.add_edge(edgeVertex(i,0),edgeVertex(i,1),w,w); } graph.maxflow(); label.create((int)spstat.size(),1); for(int i=0;i<spstat.size();i++) { if ( graph.what_segment(i) == Graph<float,float,float>::SOURCE) { label(i) = 1; } else { label(i) = 0; } } }
bool c_FourierTransfrom::ifftw_complex_3d(const Mat_<Vec6d> &_input, Mat_<Vec6d> &_output) { size_t height = _input.rows; size_t width = _input.cols; size_t n_channels = _input.channels() / 2; size_t n_pixels = height * width; size_t n_data = n_pixels * n_channels; fftw_complex *in, *out; fftw_plan p; in = (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * n_data); out = (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * n_data); p = fftw_plan_dft_3d(height, width, n_channels, in, out, FFTW_BACKWARD, FFTW_ESTIMATE); /*!< prepare the data */ for (size_t i_row = 0; i_row < height; ++i_row) { const Vec3d *p = _input.ptr<Vec3d>(i_row); for (size_t i_col = 0; i_col < width; ++i_col) { size_t index = i_row * width + i_col; for (size_t k = 0; k < n_channels; ++k) { in[n_pixels * k + index][0] = p[i_col][k]; in[n_pixels * k + index][1] = p[i_col][k + n_channels]; } #if 0 in[index][0] = p[i_col][4]; in[index][1] = p[i_col][5]; in[n_pixels + index][0] = p[i_col][2]; in[n_pixels + index][1] = p[i_col][3]; in[n_pixels * 2 + index][0] = p[i_col][0]; in[n_pixels * 2 + index][1] = p[i_col][1]; #endif } } fftw_execute(p); /*!< write back data */ _output = Mat_<Vec6d>::zeros(_input.size()); for (size_t i_row = 0; i_row < height; ++i_row) { Vec6d *p = _output.ptr<Vec6d>(i_row); for (size_t i_col = 0; i_col < width; ++i_col) { size_t index = i_row * width + i_col; for (size_t k = 0; k < n_channels; ++k) { p[i_col][k] = out[n_pixels * k + index][0]; p[i_col][k + n_channels] = out[n_pixels * k + index][1]; } #if 0 p[i_col][0] = out[n_pixels * 2 + index][0]; p[i_col][1] = out[n_pixels + index][0]; p[i_col][2] = out[index][0]; p[i_col][3] = out[n_pixels * 2 + index][1]; p[i_col][4] = out[n_pixels + index][1]; p[i_col][5] = out[index][1]; #endif } } _output /= n_data; fftw_destroy_plan(p); fftw_free(in); fftw_free(out); return true; }
void collectData(int subjId, CascadeClassifier &classifier, ShapePredictor &predictor, Mat_<float> &labels, Mat_<float> &multihog, Mat_<float> &landmarks) { int H[] = { -15, -10, -5, 0, 5, 10, 15 }; int V[] = { -10, 0, 10 }; string path = to_string(subjId) + "/"; if (subjId < 10) path = "columbia/000" + path; else path = "columbia/00" + path; ifstream fin(path + "annotation.txt"); for (int imgId = 0; imgId < 105; imgId++) { int p, v, h; fin >> p >> v >> h; if (abs(p) > 15) continue; string imgpath = path + to_string(imgId) + ".jpg"; Mat_<uchar> img = imread(imgpath, 0); BBox bbox = getTestBBox(img, classifier); if (EmptyBox(bbox)) continue; int l = 0; // EYE, MOUTH, NOF if (abs(h) <= 5 && v == 0) l = 0; else if (abs(h) <= 5 && v == -10) l = 1; else l = 2; if (l == 2) { RNG rng(getTickCount()); double num = rng.uniform(0.0, 1.0); if (num > 0.5) continue; } // 上中下 /*if (v < 0) l = 0; else if (v == 0) l = 1; else l = 2;*/ // 9分类 /*if (h < -5) l += 0; else if (h > 5) l += 2; else l += 1; if (v < 0) l += 0; else if (v > 0) l += 2 * 3; else l += 1 * 3;*/ Mat_<float> lab = l*Mat_<float>::ones(1, 1); labels.push_back(lab); Mat_<double> shape = predictor(img, bbox); Geom G; initGeom(shape, G); Pose P; calcPose(G, P); Mat_<uchar> lEye, rEye; regularize(img, bbox, P, shape, lEye, rEye); vector<float> lRlt; vector<float> rRlt; calcMultiHog(lEye, lRlt); calcMultiHog(rEye, rRlt); vector<float> _hog2nd_vec; for (int k = 0; k < lRlt.size(); k++) _hog2nd_vec.push_back(lRlt[k]); for (int k = 0; k < rRlt.size(); k++) _hog2nd_vec.push_back(rRlt[k]); Mat_<float> _hog2nd_row = Mat_<float>(_hog2nd_vec).reshape(1, 1); multihog.push_back(_hog2nd_row); vector<float> _ldmks; for (int i = 28; i < 48; i++) { _ldmks.push_back((shape(i, 0) - bbox.cx) / bbox.w); _ldmks.push_back((shape(i, 1) - bbox.cy) / bbox.h); } float mouthx = (shape(51, 0) + shape(62, 0) + shape(66, 0) + shape(57, 0)) / 4; float mouthy = (shape(51, 1) + shape(62, 1) + shape(66, 1) + shape(57, 1)) / 4; _ldmks.push_back((mouthx - bbox.cx) / bbox.w); _ldmks.push_back((mouthy - bbox.cy) / bbox.h); float maxVal = *std::max_element(_ldmks.begin(), _ldmks.end()); for (int i = 0; i < _ldmks.size(); i++) _ldmks[i] *= 1.0 / maxVal; // scale to [-1, 1] Mat_<float> ldmks = Mat_<float>(_ldmks).reshape(1, 1); landmarks.push_back(ldmks); } fin.close(); }
void computePoseDifference(Mat img1, Mat img2, CommandArgs args, Mat k, Mat& dist_coefficients, double& worldScale, Mat& R, Mat& t, Mat& img_matches) { cout << "%===============================================%" << endl; Mat camera_matrix = k.clone(); if (args.resize_factor > 1) { resize(img1, img1, Size(img1.cols / args.resize_factor, img1.rows / args.resize_factor)); // make smaller for performance and displayablity resize(img2, img2, Size(img2.cols / args.resize_factor, img2.rows / args.resize_factor)); // scale matrix down according to changed resolution camera_matrix = camera_matrix / args.resize_factor; camera_matrix.at<double>(2,2) = 1; } Mat K1, K2; K1 = K2 = camera_matrix; if (img1.rows > img1.cols) // it is assumed the camera has been calibrated in landscape mode, so undistortion must also be performed in landscape orientation, or the camera matrix must be modified (fx,fy and cx,cy need to be exchanged) { swap(K1.at<double>(0,0), K1.at<double>(1,1)); swap(K1.at<double>(0,2), K1.at<double>(1,2)); } if (img2.rows > img2.cols) { swap(K2.at<double>(0,0), K2.at<double>(1,1)); swap(K2.at<double>(0,2), K2.at<double>(1,2)); } // Feature detection + extraction vector<KeyPoint> KeyPoints_1, KeyPoints_2; Mat descriptors_1, descriptors_2; Ptr<Feature2D> feat_detector; if (args.detector == DETECTOR_KAZE) { feat_detector = AKAZE::create(args.detector_data.upright ? AKAZE::DESCRIPTOR_MLDB_UPRIGHT : AKAZE::DESCRIPTOR_MLDB, args.detector_data.descriptor_size, args.detector_data.descriptor_channels, args.detector_data.threshold, args.detector_data.nOctaves, args.detector_data.nOctaveLayersAkaze); } else if (args.detector == DETECTOR_SURF) { feat_detector = xfeatures2d::SURF::create(args.detector_data.minHessian, args.detector_data.nOctaves, args.detector_data.nOctaveLayersAkaze, args.detector_data.extended, args.detector_data.upright); } else if (args.detector == DETECTOR_SIFT) { feat_detector = xfeatures2d::SIFT::create(args.detector_data.nFeatures, args.detector_data.nOctaveLayersSift, args.detector_data.contrastThreshold, args.detector_data.sigma); } feat_detector->detectAndCompute(img1, noArray(), KeyPoints_1, descriptors_1); feat_detector->detectAndCompute(img2, noArray(), KeyPoints_2, descriptors_2); cout << "Number of feature points (img1, img2): " << "(" << KeyPoints_1.size() << ", " << KeyPoints_2.size() << ")" << endl; // Find correspondences BFMatcher matcher; vector<DMatch> matches; if (args.use_ratio_test) { if (args.detector == DETECTOR_KAZE) matcher = BFMatcher(NORM_HAMMING, false); else matcher = BFMatcher(NORM_L2, false); vector<vector<DMatch>> match_candidates; const float ratio = args.ratio; matcher.knnMatch(descriptors_1, descriptors_2, match_candidates, 2); for (int i = 0; i < match_candidates.size(); i++) if (match_candidates[i][0].distance < ratio * match_candidates[i][1].distance) matches.push_back(match_candidates[i][0]); cout << "Number of matches passing ratio test: " << matches.size() << endl; } else { if (args.detector == DETECTOR_KAZE) matcher = BFMatcher(NORM_HAMMING, true); else matcher = BFMatcher(NORM_L2, true); matcher.match(descriptors_1, descriptors_2, matches); cout << "Number of matching feature points: " << matches.size() << endl; } // Convert correspondences to vectors vector<Point2f>imgpts1,imgpts2; for(unsigned int i = 0; i < matches.size(); i++) { imgpts1.push_back(KeyPoints_1[matches[i].queryIdx].pt); imgpts2.push_back(KeyPoints_2[matches[i].trainIdx].pt); } Mat mask; // inlier mask if (args.undistort) { undistortPoints(imgpts1, imgpts1, K1, dist_coefficients, noArray(), K1); undistortPoints(imgpts2, imgpts2, K2, dist_coefficients, noArray(), K2); } double focal = camera_matrix.at<double>(0,0); Point2d principalPoint(camera_matrix.at<double>(0,2),camera_matrix.at<double>(1,2)); Mat E = findEssentialMat(imgpts1, imgpts2, focal, principalPoint, RANSAC, 0.999, 1, mask); /* Mat F = camera_matrix.t().inv() * E * camera_matrix.inv(); */ Mat F = findFundamentalMat(imgpts1, imgpts2, CV_FM_RANSAC); correctMatches(F, imgpts1, imgpts2, imgpts1, imgpts2); cout << "Reprojection error:\n " << computeReprojectionError(imgpts1, imgpts2, mask, F) << endl; int inliers = recoverPose(E, imgpts1, imgpts2, R, t, focal, principalPoint, mask); cout << "Matches used for pose recovery:\n " << inliers << endl; /* Mat R1, R2, ProjMat1, ProjMat2, Q; */ /* stereoRectify(camera_matrix, dist_coefficients, camera_matrix, dist_coefficients, img1.size(), R, t, R1, R2, ProjMat1, ProjMat2, Q); */ /* cout << "P1=" << ProjMat1 << endl; */ /* cout << "P2=" << ProjMat2 << endl; */ /* cout << "Q=" << Q << endl; */ Mat mtxR, mtxQ; Mat Qx, Qy, Qz; Vec3d angles = RQDecomp3x3(R, mtxR, mtxQ, Qx, Qy, Qz); /* cout << "Qx:\n " << Qx << endl; */ /* cout << "Qy:\n " << Qy << endl; */ /* cout << "Qz:\n " << Qz << endl; */ cout << "Translation:\n " << t.t() << endl; cout << "Euler angles [x y z] in degrees:\n " << angles.t() << endl; if (args.epilines) { drawEpilines(Mat(imgpts1), 1, F, img2); drawEpilines(Mat(imgpts2), 2, F, img1); } drawMatches(img1, KeyPoints_1, img2, KeyPoints_2, // draw only inliers given by mask matches, img_matches, Scalar::all(-1), Scalar::all(-1), mask); vector<Point2f> imgpts1_masked, imgpts2_masked; for (int i = 0; i < imgpts1.size(); i++) { if (mask.at<uchar>(i,0) == 1) { imgpts1_masked.push_back(imgpts1[i]); imgpts2_masked.push_back(imgpts2[i]); } } Mat pnts4D; Mat P1 = camera_matrix * Mat::eye(3, 4, CV_64FC1), P2; Mat p2[2] = { R, t }; hconcat(p2, 2, P2); P2 = camera_matrix * P2; #define USE_OPENCV_TRIANGULATION #ifndef USE_OPENCV_TRIANGULATION // strangely, both methods yield identical results vector<Point3d> homogPoints1, homogPoints2; for (int i = 0; i < imgpts1_masked.size(); i++) { Point2f currentPoint1 = imgpts1_masked[i]; homogPoints1.push_back(Point3d(currentPoint1.x, currentPoint1.y, 1)); Point2f currentPoint2 = imgpts2_masked[i]; homogPoints2.push_back(Point3d(currentPoint2.x, currentPoint2.y, 1)); } Mat dehomogenized(imgpts1_masked.size(), 3, CV_64FC1); for (int i = 0; i < imgpts1_masked.size(); i++) { Mat_<double> triangulatedPoint = IterativeLinearLSTriangulation(homogPoints1[i], P1, homogPoints2[i], P2); Mat r = triangulatedPoint.t(); r.colRange(0,3).copyTo(dehomogenized.row(i)); // directly assigning to dehomogenized.row(i) compiles but does nothing, wtf? } #else triangulatePoints(P1, P2, imgpts1_masked, imgpts2_masked, pnts4D); pnts4D = pnts4D.t(); Mat dehomogenized; convertPointsFromHomogeneous(pnts4D, dehomogenized); dehomogenized = dehomogenized.reshape(1); // instead of 3 channels and 1 col, we want 1 channel and 3 cols #endif double mDist = 0; int n = 0; int pos = 0, neg = 0; /* Write ply file header */ ofstream ply_file("points.ply", ios_base::trunc); ply_file << "ply\n" "format ascii 1.0\n" "element vertex " << dehomogenized.rows << "\n" "property float x\n" "property float y\n" "property float z\n" "property uchar red\n" "property uchar green\n" "property uchar blue\n" "end_header" << endl; Mat_<double> row; for (int i = 0; i < dehomogenized.rows; i++) { row = dehomogenized.row(i); double d = row(2); if (d > 0) { pos++; mDist += norm(row); n++; /* float startx=imgpts1_masked[i].x - 1, starty=imgpts1_masked[i].y - 1, endx=imgpts1_masked[i].x + 1, endy=imgpts1_masked[i].y + 1; */ /* cout << "startx,endx = " << startx << "," << endx << endl; */ /* cout << "starty,endy = " << starty << "," << endy << endl; */ Vec3b rgb = img1.at<Vec3b>(imgpts1_masked[i].x, imgpts1_masked[i].y); ply_file << row(0) << " " << row(1) << " " << row(2) << " " << (int)rgb[2] << " " << (int)rgb[1] << " " << (int)rgb[0] << "\n"; } else { neg++; ply_file << 0 << " " << 0 << " " << 0 << " " << 0 << " " << 0 << " " << 0 << "\n"; } } ply_file.close(); mDist /= n; worldScale = mDist; cout << "Mean distance of " << n << " points to camera:\n " << mDist << " (dehomogenized)" << endl; cout << "pos=" << pos << ", neg=" << neg << endl; /* char filename[100]; */ /* sprintf(filename, "mat_1%d", i+1); */ /* Ptr<Formatter> formatter = Formatter::get(Formatter::FMT_CSV); */ /* Ptr<Formatted> formatted = formatter->format(dehomogenized); */ /* ofstream file(filename, ios_base::trunc); */ /* file << formatted << endl; */ /* Removed until cmake has been fathomed */ /* vector< Point3d > points3D; */ /* vector< vector< Point2d > > pointsImg; */ /* int NPOINTS=dehomogenized.rows; // number of 3d points */ /* int NCAMS=2; // number of cameras */ /* points3D.resize(NPOINTS); */ /* for (int i = 0; i < NPOINTS; i++) */ /* { */ /* points3D[i] = Point3d(dehomogenized.at<double>(i,0), */ /* dehomogenized.at<double>(i,1), */ /* dehomogenized.at<double>(i,2) */ /* ); */ /* } */ /* // fill image projections */ /* vector<vector<int> > visibility(2, vector<int>(NPOINTS, 1)); */ /* vector<Mat> camera_matrices(2, camera_matrix); */ /* vector<Mat> Rs(2); */ /* Rodrigues(Mat::eye(3, 3, CV_64FC1), Rs[0]); */ /* Rodrigues(R, Rs[0]); */ /* vector<Mat> Ts = { Mat::zeros(3,1, CV_64FC1), t }; */ /* vector<Mat> dist_coefficientss(2, dist_coefficients); */ /* pointsImg.resize(NCAMS); */ /* for(int i=0; i<NCAMS; i++) pointsImg[i].resize(NPOINTS); */ /* for (int i = 0; i < NPOINTS; i++) */ /* { */ /* pointsImg[0][i] = Point2d(imgpts1_masked[i].x, imgpts1_masked[i].y); */ /* pointsImg[1][i] = Point2d(imgpts2_masked[i].x, imgpts2_masked[i].y); */ /* } */ /* cvsba::Sba sba; */ /* sba.run(points3D, pointsImg, visibility, camera_matrices, Rs, Ts, dist_coefficientss); */ /* cout<<"Initial error="<<sba.getInitialReprjError()<<". "<< */ /* "Final error="<<sba.getFinalReprjError()<<endl; */ cout << "%===============================================%" << endl; }
int main(int argc, char **argv) { ros::init(argc, argv, "rovioTest"); ros::NodeHandle n; imageClient = n.serviceClient<rovio_base::image>("rovioImage"); controlClient = n.serviceClient<rovio_base::manDrv>("rovioControl"); reportClient = n.serviceClient<rovio_base::report>("rovioReport"); Mat imgOrd = getImage(); cvtColor(imgOrd, imgOrd, CV_RGB2GRAY); const int AREA_SIZE = 16; int ordRows = imgOrd.rows; int ordCols = imgOrd.cols; int areaNumY = ordRows / AREA_SIZE; int areaNumX = ordCols / AREA_SIZE; int marginY = ordRows % AREA_SIZE; int areaNum = areaNumX * areaNumY; Rect ior(0, marginY, areaNumX * AREA_SIZE, areaNumY * AREA_SIZE); Mat img = getImg(imgOrd, ior); VideoWriter videoWriter("/home/viki/Rovio.avi", CV_FOURCC('M', 'J', 'P', 'G'), 3.0, Size(img.cols, img.rows)); int lastDirection = 1; for (int i = 0; i < 1000; i++) { Mat img = getImg(getImage(), ior); Mat_<int> regionMap(img.size()); regionMap.setTo(0); regionMap(regionMap.rows - 18, regionMap.cols / 2) = 1; regionMap(regionMap.rows - 10, regionMap.cols / 2) = 1; RegionGrowthAlg alg; alg.calcRegionMap(img, regionMap); // Detect forward int fAreaWidth = img.cols - 200; int fAreaHeight = img.rows / 4; int fTopX = (img.cols - fAreaWidth) / 2; int fTopY = img.rows - fAreaHeight; int ignorePixels = 1000; Mat_<int> fArea = regionMap(Rect(fTopX, fTopY, fAreaWidth, fAreaHeight)); int fAreaSum = 0; for (int i = 0; i < fArea.rows; i++) { int* pRow = fArea.ptr<int>(i); for (int j = 0; j < fArea.cols; j++) { if (pRow[j] == 0) fAreaSum++; } } bool flagForward = fAreaSum < ignorePixels; // Detect left and right int lrAreaWidth = 100; int marginX = 0; int lrAreaHeight = fAreaHeight; int lrTopY = img.rows - lrAreaHeight; int lTopX = marginX; int rTopX = img.cols - marginX - lrAreaWidth; int lrIgnorePixels = 1000; Mat_<int> lArea = regionMap(Rect(lTopX, lrTopY, lrAreaWidth, lrAreaHeight)); Mat_<int> rArea = regionMap(Rect(rTopX, lrTopY, lrAreaWidth, lrAreaHeight)); int lAreaSum = 0; int rAreaSum = 0; for (int i = 0; i < lArea.rows; i++) { int* plRow = lArea.ptr<int>(i); int* prRow = rArea.ptr<int>(i); for (int j = 0; j < lArea.cols; j++) { if (plRow[j] == 0) lAreaSum++; if (prRow[j] == 0) rAreaSum++; } } bool flagLeft = lAreaSum < lrIgnorePixels; bool flagRight = rAreaSum < lrIgnorePixels; //fArea.setTo(2); lArea.setTo(3); rArea.setTo(4); Utility util; util.drawSegmentBorder(img, regionMap); // Mark info //标记 int leftSum = 0; int rightSum = 0; int loopi = img.rows; int loopj = img.cols; for (int i = 0; i < loopi; i++) { int* pLeftRow = regionMap.ptr<int>(i); int* pRIghtRow = pLeftRow + loopj / 2; int loop = loopj / 2; for (int j = 0; j < loop; j++) { if (pLeftRow[j] > 0) { leftSum++; } if (pRIghtRow[j] > 0) { rightSum++; } } } Point pos(loopj / 2 - 150, loopi / 2); std::stringstream ss; string tmp; ss << leftSum; ss >> tmp; putText(img, tmp, pos, FONT_HERSHEY_SIMPLEX, 0.6, Scalar(0, 255, 0)); pos.x = loopj / 2 + 100; ss.str(""); ss.clear(); ss << rightSum; ss >> tmp; putText(img, tmp, pos, FONT_HERSHEY_SIMPLEX, 0.6, Scalar(0, 255, 0)); int textLen = 40; pos.x = fArea.cols / 2 - textLen + fTopX; pos.y = fArea.rows / 2 + fTopY; ss.str(""); ss.clear(); ss << fAreaSum; ss >> tmp; putText(img, tmp, pos, FONT_HERSHEY_SIMPLEX, 0.6, Scalar(0, 50, 255)); pos.x = lArea.cols / 2 - textLen + lTopX; pos.y = lArea.rows / 2 + lrTopY; ss.str(""); ss.clear(); ss << lAreaSum; ss >> tmp; putText(img, tmp, pos, FONT_HERSHEY_SIMPLEX, 0.6, Scalar(0, 50, 255)); pos.x = rArea.cols / 2 - textLen + rTopX; pos.y = rArea.rows / 2 + lrTopY; ss.str(""); ss.clear(); ss << rAreaSum; ss >> tmp; putText(img, tmp, pos, FONT_HERSHEY_SIMPLEX, 0.6, Scalar(0, 50, 255)); //检测直线区域 int lineLen = 200; int lineStartX = img.cols / 2 - lineLen / 2; int lineEndX = img.cols / 2 + lineLen / 2; int lineY = img.rows - 140; Point lineStart(lineStartX, lineY); Point lineEnd(lineEndX, lineY); line(img, lineStart, lineEnd, Scalar(255, 20, 20)); int blockNum = 0; int* pLineRow = regionMap.ptr<int>(lineY); for (int j = lineStartX; j < lineEndX; j++) { if (pLineRow[j] == 0) { blockNum++; } } bool isBlocked = blockNum > lineLen / 2; //视频 //cvtColor(img, img, CV_GRAY2RGB); imshow("", img); waitKey(10); videoWriter << img; //控制 isBlocked = (!flagLeft && !flagRight) || isBlocked; int waitTime = 1; rvMCUReport rvMcu = getReport(); if (rvMcu.isIrOn && rvMcu.isDetectedBarrier) isBlocked = true; if (true) { if (isBlocked) { int maxDif = 5000; if (leftSum - rightSum > maxDif) { lastDirection = -1; } else if (rightSum - leftSum > maxDif) { lastDirection = 1; } if (lastDirection == -1) { control(5, 8); waitKey(waitTime); } else { control(6, 8); waitKey(waitTime); } } else if (flagForward) { control(1, 6); } else if (flagLeft) { control(3, 8); } else if (flagRight) { control(4, 8); } else { printf("Error control"); } } } return 0; }
// We are implementing SLIC here. I'm too lazy to enfoce the connectivity Mat_< int > Superpixel::slic( const Mat_< Vec3f >& im ) const { // Compute the spacing and grid size of the superpixels double sp_area = 1.0 * im.cols * im.rows / K_; int Kx = 0.5 + im.cols / sqrt( sp_area ), Ky = 0.5 + im.rows / sqrt( sp_area ); int K = Kx*Ky; int win_sz = 1.0 * sqrt(sp_area) + 1; // Initialize the seeds on a regular grid std::vector< int64_t > cnt( K ); std::vector< Point2d > seedsd( K ); std::vector< Point > seeds( K ); for( int i=0,k=0; i<Kx; i++ ) for( int j=0; j<Ky; j++, k++ ) seeds[k] = Point( (i+0.5)*(im.cols-1)/Kx, (j+0.5)*(im.rows-1)/Ky ); // Run k-means Mat_<float> dist( im.size() ); Mat_<int> label( im.size() ); for( int it=0; it<n_iter_; it++ ) { // Assignment step dist = std::numeric_limits<float>::max(); label = -1; for( int k=0; k<K; k++ ) { Vec3f c = im( seeds[k] ); for( int j=std::max(0,seeds[k].y-win_sz); j<im.rows && j<=seeds[k].y+win_sz; j++ ) for( int i=std::max(0,seeds[k].x-win_sz); i<im.cols && i<=seeds[k].x+win_sz; i++ ){ double d = (i-seeds[k].x) * (i-seeds[k].x) + (j-seeds[k].y) * (j-seeds[k].y); double cd = ( im( j, i ) - c ).dot( im( j, i ) - c ); d += col_w_ * col_w_ * cd; if( d < dist( j, i ) ) { dist( j, i ) = d; label( j, i ) = k; } } } // Update for( int k=0; k<K; k++ ) { seedsd[k] = Point2d(0,0); cnt[k] = 0; } for( int j=0; j<im.rows; j++ ) for( int i=0; i<im.cols; i++ ) { // Fix all the pixels we messed up! if ( label( j, i ) < 0 ) { // printf("Oops that wasn't very slick :(\n"); for( int k=0; k<K; k++ ){ Vec3f c = im( seeds[k] ); double d = (i-seeds[k].x) * (i-seeds[k].x) + (j-seeds[k].y) * (j-seeds[k].y); double cd = ( im( j, i ) - c ).dot( im( j, i ) - c ); d += col_w_ * col_w_ * cd; if( d < dist( j, i ) ) { dist( j, i ) = d; label( j, i ) = k; } } } seedsd[ label( j, i ) ] += Point2d( i, j ); cnt[ label( j, i ) ] += 1; } for( int k=0; k<K; k++ ) if (cnt[k] > 0) seeds[k] = Point( 0.5 + seedsd[k].x / cnt[k], 0.5 + seedsd[k].y / cnt[k] ); } return label; }
int copyMakeBorder(/*const*/ Mat_<_Tp, chs>& src, Mat_<_Tp, chs>& dst, int top, int bottom, int left, int right, int borderType, const Scalar& value = Scalar()) { FBC_Assert(top >= 0 && bottom >= 0 && left >= 0 && right >= 0); if (src.isSubmatrix() && (borderType & BORDER_ISOLATED) == 0) { Size wholeSize; Point ofs; src.locateROI(wholeSize, ofs); int dtop = std::min(ofs.y, top); int dbottom = std::min(wholeSize.height - src.rows - ofs.y, bottom); int dleft = std::min(ofs.x, left); int dright = std::min(wholeSize.width - src.cols - ofs.x, right); src.adjustROI(dtop, dbottom, dleft, dright); top -= dtop; left -= dleft; bottom -= dbottom; right -= dright; } if (dst.empty() || dst.rows != (src.rows + top + bottom) || dst.cols != (src.cols + left + right)) { dst.release(); dst = Mat_<_Tp, chs>(src.rows + top + bottom, src.cols + left + right); } if (top == 0 && left == 0 && bottom == 0 && right == 0) { if (src.data != dst.data || src.step != dst.step) src.copyTo(dst); return 0; } borderType &= ~BORDER_ISOLATED; if (borderType != BORDER_CONSTANT) { copyMakeBorder_8u(src.ptr(), src.step, src.size(), dst.ptr(), dst.step, dst.size(), top, left, src.elemSize(), borderType); } else { int cn = src.channels, cn1 = cn; AutoBuffer<double> buf(cn); scalarToRawData<_Tp, chs>(value, buf, cn); copyMakeConstBorder_8u(src.ptr(), src.step, src.size(), dst.ptr(), dst.step, dst.size(), top, left, (int)src.elemSize(), (uchar*)(double*)buf); } return 0; }
Mat_< int > Superpixel::geodesicSegmentation( const Mat_< Vec3f >& im ) const { //std::tr1::uniform_int<int> distribution(-2, 2); //std::tr1::mt19937 engine; // Mersenne twister MT19937 //auto randint = std::tr1::bind(distribution, engine); int s; srand((unsigned)time(NULL)); s=rand()%4-2; // Compute the spacing and grid size of the superpixels double sp_area = 1.0 * im.cols * im.rows / K_; int Kx = 0.5 + im.cols / sqrt( sp_area ), Ky = 0.5 + im.rows / sqrt( sp_area ); int K = Kx*Ky; int win_sz = 1.0 * sqrt(sp_area) + 1; // Initialize the seeds on a regular grid std::vector< int64_t > cnt( K ); std::vector< Point2d > seedsd( K ); std::vector< Point > seeds( K ); for( int i=0,k=0; i<Kx; i++ ) for( int j=0; j<Ky; j++, k++ ) seeds[k] = Point( (i+0.5)*(im.cols-1)/Kx, (j+0.5)*(im.rows-1)/Ky ) + Point(rand()%4-2,rand()%4-2);/*Point( randint(), randint() );*/ Mat_<float> dx( im.size() ), dy( im.size() ); for( int j=0; j<im.rows; j++ ) for( int i=0; i<im.cols; i++ ) { if (i) dx(j,i-1) = col_w_*sqrt( (im(j,i)-im(j,i-1)).dot(im(j,i)-im(j,i-1)) ) + 1; if (j) dy(j-1,i) = col_w_*sqrt( (im(j-1,i)-im(j,i)).dot(im(j-1,i)-im(j,i)) ) + 1; } // Run k-means Mat_<float> dist( im.size() ); Mat_<int> label( im.size() ); for( int it=0; it<n_iter_; it++ ) { // Assignment step dist = std::numeric_limits<float>::max(); label = -1; for( int k=0; k<K; k++ ) { dist( seeds[k] ) = 0; label( seeds[k] ) = k; } for( int IT=0; IT<2; IT++ ){ for( int j=0; j<im.rows; j++ ) for( int i=0; i<im.cols; i++ ) { if (i && dist(j,i-1) + dx(j,i-1) < dist(j,i)) { dist(j,i) = dist(j,i-1) + dx(j,i-1); label(j,i) = label(j,i-1); } if (j && dist(j-1,i) + dx(j-1,i) < dist(j,i)) { dist(j,i) = dist(j-1,i) + dy(j-1,i); label(j,i) = label(j-1,i); } } for( int j=im.rows-1; j>=0; j-- ) for( int i=im.cols-1; i>=0; i-- ) { if (i && dist(j,i) + dx(j,i-1) < dist(j,i-1)) { dist(j,i-1) = dist(j,i) + dx(j,i-1); label(j,i-1) = label(j,i); } if (j && dist(j,i) + dx(j-1,i) < dist(j-1,i)) { dist(j-1,i) = dist(j,i) + dy(j-1,i); label(j-1,i) = label(j,i); } } } for( int k=0; k<K; k++ ) { Vec3f c = im( seeds[k] ); for( int j=std::max(0,seeds[k].y-win_sz); j<im.rows && j<=seeds[k].y+win_sz; j++ ) for( int i=std::max(0,seeds[k].x-win_sz); i<im.cols && i<=seeds[k].x+win_sz; i++ ){ double d = (i-seeds[k].x) * (i-seeds[k].x) + (j-seeds[k].y) * (j-seeds[k].y); double cd = ( im( j, i ) - c ).dot( im( j, i ) - c ); d += col_w_ * col_w_ * cd; if( d < dist( j, i ) ) { dist( j, i ) = d; label( j, i ) = k; } } } // Update for( int k=0; k<K; k++ ) { seedsd[k] = Point2d(0,0); cnt[k] = 0; } for( int j=0; j<im.rows; j++ ) for( int i=0; i<im.cols; i++ ) { // Fix all the pixels we messed up! if ( label( j, i ) < 0 ) { // printf("Oops that wasn't very slick :(\n"); for( int k=0; k<K; k++ ){ Vec3f c = im( seeds[k] ); double d = (i-seeds[k].x) * (i-seeds[k].x) + (j-seeds[k].y) * (j-seeds[k].y); double cd = ( im( j, i ) - c ).dot( im( j, i ) - c ); d += col_w_ * col_w_ * cd; if( d < dist( j, i ) ) { dist( j, i ) = d; label( j, i ) = k; } } } seedsd[ label( j, i ) ] += Point2d( i, j ); cnt[ label( j, i ) ] += 1; } for( int k=0; k<K; k++ ) if (cnt[k] > 0) seeds[k] = Point( 0.5 + seedsd[k].x / cnt[k], 0.5 + seedsd[k].y / cnt[k] ); } return label; }
void FaceAnalyser::AddNextFrame(const cv::Mat& frame, const CLMTracker::CLM& clm_model, double timestamp_seconds, bool online, bool visualise) { // Check if a reset is needed first (TODO same person no reset) //if(face_bounding_box.area() > 0) //{ // Rect_<double> new_bounding_box = clm.GetBoundingBox(); // // If the box overlaps do not need a reset // double intersection_area = (face_bounding_box & new_bounding_box).area(); // double union_area = face_bounding_box.area() + new_bounding_box.area() - 2 * intersection_area; // // If the model is already tracking what we're detecting ignore the detection, this is determined by amount of overlap // if( intersection_area/union_area < 0.5) // { // this->Reset(); // } // face_bounding_box = new_bounding_box; //} //if(!clm.detection_success) //{ // this->Reset(); //} frames_tracking++; // First align the face if tracking was successfull if(clm_model.detection_success) { AlignFaceMask(aligned_face, frame, clm_model, triangulation, true, align_scale, align_width, align_height); } else { aligned_face = Mat(align_height, align_width, CV_8UC3); aligned_face.setTo(0); } if(aligned_face.channels() == 3) { cvtColor(aligned_face, aligned_face_grayscale, CV_BGR2GRAY); } else { aligned_face_grayscale = aligned_face.clone(); } // Extract HOG descriptor from the frame and convert it to a useable format Mat_<double> hog_descriptor; Extract_FHOG_descriptor(hog_descriptor, aligned_face, this->num_hog_rows, this->num_hog_cols); // Store the descriptor hog_desc_frame = hog_descriptor; Vec3d curr_orient(clm_model.params_global[1], clm_model.params_global[2], clm_model.params_global[3]); int orientation_to_use = GetViewId(this->head_orientations, curr_orient); // Only update the running median if predictions are not high // That is don't update it when the face is expressive (just retrieve it) bool update_median = true; // TODO test if this would be useful or not //if(!this->AU_predictions.empty()) //{ // for(size_t i = 0; i < this->AU_predictions.size(); ++i) // { // if(this->AU_predictions[i].second > 1) // { // update_median = false; // break; // } // } //} update_median = update_median & clm_model.detection_success; // A small speedup if(frames_tracking % 2 == 1) { UpdateRunningMedian(this->hog_desc_hist[orientation_to_use], this->hog_hist_sum[orientation_to_use], this->hog_desc_median, hog_descriptor, update_median, this->num_bins_hog, this->min_val_hog, this->max_val_hog); } // Geom descriptor and its median geom_descriptor_frame = clm_model.params_local.t(); if(!clm_model.detection_success) { geom_descriptor_frame.setTo(0); } // Stack with the actual feature point locations (without mean) Mat_<double> locs = clm_model.pdm.princ_comp * geom_descriptor_frame.t(); cv::hconcat(locs.t(), geom_descriptor_frame.clone(), geom_descriptor_frame); // A small speedup if(frames_tracking % 2 == 1) { UpdateRunningMedian(this->geom_desc_hist, this->geom_hist_sum, this->geom_descriptor_median, geom_descriptor_frame, update_median, this->num_bins_geom, this->min_val_geom, this->max_val_geom); } // First convert the face image to double representation as a row vector Mat_<uchar> aligned_face_cols(1, aligned_face.cols * aligned_face.rows * aligned_face.channels(), aligned_face.data, 1); Mat_<double> aligned_face_cols_double; aligned_face_cols.convertTo(aligned_face_cols_double, CV_64F); // TODO get rid of this completely as it takes too long? //UpdateRunningMedian(this->face_image_hist[orientation_to_use], this->face_image_hist_sum[orientation_to_use], this->face_image_median, aligned_face_cols_double, update_median, 256, 0, 255); // Visualising the median HOG if(visualise) { FaceAnalysis::Visualise_FHOG(hog_descriptor, num_hog_rows, num_hog_cols, hog_descriptor_visualisation); } // Perform AU prediction AU_predictions_reg = PredictCurrentAUs(orientation_to_use); std::vector<std::pair<std::string, double>> AU_predictions_reg_corrected; if(online) { AU_predictions_reg_corrected = CorrectOnlineAUs(AU_predictions_reg, orientation_to_use, true, false, clm_model.detection_success); } // Keep only closer to in-plane faces double angle_norm = cv::sqrt(clm_model.params_global[2] * clm_model.params_global[2] + clm_model.params_global[3] * clm_model.params_global[3]); // Add the reg predictions to the historic data for (size_t au = 0; au < AU_predictions_reg.size(); ++au) { // Find the appropriate AU (if not found add it) // Only add if the detection was successful and not too out of plane if(clm_model.detection_success && angle_norm < 0.4) { AU_predictions_reg_all_hist[AU_predictions_reg[au].first].push_back(AU_predictions_reg[au].second); } else { AU_predictions_reg_all_hist[AU_predictions_reg[au].first].push_back(-100.0); } } AU_predictions_class = PredictCurrentAUsClass(orientation_to_use); for (size_t au = 0; au < AU_predictions_class.size(); ++au) { // Find the appropriate AU (if not found add it) // Only add if the detection was successful and not too out of plane if(clm_model.detection_success && angle_norm < 0.4) { AU_predictions_class_all_hist[AU_predictions_class[au].first].push_back(AU_predictions_class[au].second); } else { AU_predictions_class_all_hist[AU_predictions_class[au].first].push_back(-100.0); } } if(online) { AU_predictions_reg = AU_predictions_reg_corrected; } this->current_time_seconds = timestamp_seconds; view_used = orientation_to_use; bool success = clm_model.detection_success && angle_norm < 0.4; confidences.push_back(clm_model.detection_certainty); valid_preds.push_back(success); }
void spm_bp::runspm_bp(cv::Mat_<cv::Vec2f>& flowResult) { clock_t start_disp, finish_disp; start_disp = clock(); printf("==================================================\n"); printf("SPM-BP begins\n"); Mat_<Vec2f> label_k(height1 * width1, NUM_TOP_K); Mat_<float> dcost_k(height1 * width1, NUM_TOP_K); #ifdef _WIN32 printf("Running on Windows\n"); // Dummy code that speeds up program on windows considerably // Save label vector<vector<Vec2f> > label_saved(numOfSP1); //label_saved.reserve(10000); vector<vector<Mat_<float> > > dcost_saved(numOfSP1); //dcost_saved.reserve(10000); for (int i = 0; i < numOfSP1; i++) { label_saved[i].reserve(100000); dcost_saved[i].reserve(100000); } #endif //initiate labels init_label_super(label_k, dcost_k); //MESSAGE 0:left, 1:right, 2:up, 3:down Mat_<Vec<float, NUM_TOP_K> > message(height1 * width1, 4); message.setTo(0); //precompute smooth weight float omega[256]; for (int i = 0; i < 256; ++i) omega[i] = lambda_smooth * std::exp(-float(i) / 20); // TODO: Change to Mat4f Mat_<Vec4f> smoothWt(height1, width1); smoothWt.setTo(lambda_smooth); // # pragma omp parallel for for (int i = 1; i < height1 - 1; ++i) { for (int j = 1; j < width1 - 1; ++j) { const Vec3f &ref = im1f[i][j]; // TODO: Don't need abs here since norm >= 0 // smoothWt[i][j][0] = omega[int(abs(norm(ref - im1f[i][j - 1])))]; // smoothWt[i][j][1] = omega[int(abs(norm(ref - im1f[i][j + 1])))]; // smoothWt[i][j][2] = omega[int(abs(norm(ref - im1f[i - 1][j])))]; // smoothWt[i][j][3] = omega[int(abs(norm(ref - im1f[i + 1][j])))]; smoothWt[i][j][0] = omega[int(norm(ref - im1f[i][j - 1]))]; smoothWt[i][j][1] = omega[int(norm(ref - im1f[i][j + 1]))]; smoothWt[i][j][2] = omega[int(norm(ref - im1f[i - 1][j]))]; smoothWt[i][j][3] = omega[int(norm(ref - im1f[i + 1][j]))]; } } float dis_belief_l[NUM_TOP_K]; float dis_belief_r[NUM_TOP_K]; float dis_belief_u[NUM_TOP_K]; float dis_belief_d[NUM_TOP_K]; const int BUFSIZE = NUM_TOP_K * 50; vector<Vec2f> vec_label(BUFSIZE), vec_label_nei(BUFSIZE); vector<float> vec_mes_l(BUFSIZE), vec_mes_r(BUFSIZE), vec_mes_u(BUFSIZE), vec_mes_d(BUFSIZE), vec_belief(BUFSIZE), vec_d_cost(BUFSIZE); Mat_<float> DataCost_nei; start_disp = clock(); double wall_timer = omp_get_wtime(); if (display) Show_WTA_Flow(-1, label_k, dcost_k, message, flowResult); int spBegin, spEnd, spStep; for (int iter = 0; iter < iterNum; iter++) { if (iter % 2) { spBegin = numOfSP1 - 1, spEnd = -1, spStep = -1; } else { spBegin = 0, spEnd = numOfSP1, spStep = 1; } for (int sp = spBegin; sp != spEnd; sp += spStep) { int curSPP = superpixelsList1[sp][0]; Vec4i curRange = subRange1[curSPP]; int y = curRange[0]; int x = curRange[1]; int w = curRange[2] - curRange[0] + 1; int h = curRange[3] - curRange[1] + 1; // rand neighbor pixel and store vec_label_nei.clear(); std::set<int>::iterator sIt; std::set<int>& sAdj = spGraph1[iter % 2].adjList[sp]; for (sIt = sAdj.begin(); sIt != sAdj.end(); ++sIt) { repPixels1[*sIt] = superpixelsList1[*sIt][rand() % superpixelsList1[*sIt].size()]; Vec2f test_label; //vector<Vec2f> & cur_label_k = label_k[repPixels1[*sIt]]; for (int k = 0; k < NUM_TOP_K; k++) { //test_label= cur_label_k[k]; test_label = label_k[repPixels1[*sIt]][k]; if (isNewLabel_PMF_PMBP(vec_label_nei, test_label) == -1) vec_label_nei.push_back(test_label); } } repPixels1[sp] = superpixelsList1[sp][rand() % superpixelsList1[sp].size()]; for (int k = 0; k < NUM_TOP_K; k++) { Vec2f test_label = label_k[repPixels1[sp]][k]; float mag = min(disp_range_u, disp_range_v) / 8; for (; mag >= (1.0 / upScale); mag /= 2.0) { Vec2f test_label_random; float tmpVerLabel = test_label[0] + ((float(rand()) / RAND_MAX) - 0.5) * 2.0 * mag; float tmpHorLabel = test_label[1] + ((float(rand()) / RAND_MAX) - 0.5) * 2.0 * mag; test_label_random[0] = floor(tmpVerLabel * upScale + 0.5) / upScale; test_label_random[1] = floor(tmpHorLabel * upScale + 0.5) / upScale; if (test_label_random[0] >= -disp_range_u && test_label_random[0] <= disp_range_u && test_label_random[1] >= -disp_range_v && test_label_random[1] <= disp_range_v && isNewLabel_PMF_PMBP(vec_label_nei, test_label_random) == -1) //here { vec_label_nei.push_back(test_label_random); } } } const int vec_size = vec_label_nei.size(); // DataCost_nei.release(); DataCost_nei.create(h, w * vec_size); DataCost_nei.setTo(0); #if USE_CLMF0_TO_AGGREGATE_COST cv::Mat_<cv::Vec4b> leftCombinedCrossMap; leftCombinedCrossMap.create(h, w); subCrossMap1[sp].copyTo(leftCombinedCrossMap); CFFilter cff; #endif // Compute matching costs for each candidate particles // printf("Computing matching costs start\n"); #pragma omp parallel for num_threads(NTHREADS) for (int i = 0; i < vec_size; ++i) { int kx = i * w; Mat_<float> rawCost; getLocalDataCostPerlabel(sp, vec_label_nei[i], rawCost); #if USE_CLMF0_TO_AGGREGATE_COST cff.FastCLMF0FloatFilterPointer(leftCombinedCrossMap, rawCost, rawCost); #endif rawCost.copyTo(DataCost_nei(cv::Rect(kx, 0, w, h))); } //getLocalDataCost(sp, vec_label_nei, DataCost_nei); // printf("Computing matching costs end\n"); Vec4i curRange_s = spRange1[curSPP]; // int spw = curRange_s[2] - curRange_s[0] + 1; // int sph = curRange_s[3] - curRange_s[1] + 1; int spy_s, spy_e, spx_s, spx_e, spx_step, spy_step; spy_s = curRange_s[0]; spy_e = curRange_s[2] + 1; spx_s = curRange_s[1]; spx_e = curRange_s[3] + 1; spx_step = 1; spy_step = 1; if (iter % 4 == 1) { spy_s = curRange_s[2]; spy_e = curRange_s[0] - 1; spx_s = curRange_s[3]; spx_e = curRange_s[1] - 1; spx_step = -1; spy_step = -1; } else if (iter % 4 == 2) { spy_s = curRange_s[2]; spy_e = curRange_s[0] - 1; spy_step = -1; spx_s = curRange_s[1]; spx_e = curRange_s[3] + 1; spx_step = 1; } else if (iter % 4 == 3) { spy_s = curRange_s[0]; spy_e = curRange_s[2] + 1; spy_step = 1; spx_s = curRange_s[3]; spx_e = curRange_s[1] - 1; spx_step = -1; } for (int bi = spx_s; bi != spx_e; bi = bi + spx_step) for (int bj = spy_s; bj != spy_e; bj = bj + spy_step) { int p1 = bi * width1 + bj; int pl = bi * width1 + (bj - 1); int pu = (bi - 1) * width1 + bj; int pr = bi * width1 + (bj + 1); int pd = (bi + 1) * width1 + bj; //Compute disbelief: three incoming message + data_cost for (int k = 0; k < NUM_TOP_K; ++k) { if (bj != 0) dis_belief_l[k] = message[pl][0][k] + message[pl][2][k] + message[pl][3][k] + dcost_k[pl][k]; if (bj != width1 - 1) dis_belief_r[k] = message[pr][1][k] + message[pr][2][k] + message[pr][3][k] + dcost_k[pr][k]; if (bi != 0) dis_belief_u[k] = message[pu][0][k] + message[pu][1][k] + message[pu][2][k] + dcost_k[pu][k]; if (bi != height1 - 1) dis_belief_d[k] = message[pd][0][k] + message[pd][1][k] + message[pd][3][k] + dcost_k[pd][k]; } vec_label.clear(); vec_mes_l.clear(); vec_mes_r.clear(); vec_mes_u.clear(); vec_mes_d.clear(); vec_belief.clear(); vec_d_cost.clear(); // Update and save messages with current reference pixel's labels Vec4f wt_s = smoothWt[bi][bj]; for (int k = 0; k < NUM_TOP_K; ++k) { Vec2f test_label = label_k[p1][k]; vec_label.push_back(test_label); float dcost = dcost_k[p1][k]; vec_d_cost.push_back(dcost); //start_disp = clock(); float _mes_l = 0, _mes_r = 0, _mes_u = 0, _mes_d = 0; if (bj != 0) { _mes_l = ComputeMes_PMBP_per_label(dis_belief_l, label_k, pl, test_label, wt_s[0], tau_s); vec_mes_l.push_back(_mes_l); } if (bj != width1 - 1) { _mes_r = ComputeMes_PMBP_per_label(dis_belief_r, label_k, pr, test_label, wt_s[1], tau_s); vec_mes_r.push_back(_mes_r); } if (bi != 0) { _mes_u = ComputeMes_PMBP_per_label(dis_belief_u, label_k, pu, test_label, wt_s[2], tau_s); vec_mes_u.push_back(_mes_u); } if (bi != height1 - 1) { _mes_d = ComputeMes_PMBP_per_label(dis_belief_d, label_k, pd, test_label, wt_s[3], tau_s); vec_mes_d.push_back(_mes_d); } vec_belief.push_back(_mes_l + _mes_r + _mes_u + _mes_d + dcost); } //propagation + random search int kx; for (int test_id = 0; test_id < vec_label_nei.size(); ++test_id) { Vec2f test_label = vec_label_nei[test_id]; //if(isNewLabel_PMF_PMBP(vec_label,test_label)==-1) { vec_label.push_back(test_label); kx = test_id * w; const Mat_<float>& local = DataCost_nei(cv::Rect(kx, 0, w, h)); float dcost = local[bi - x][bj - y]; vec_d_cost.push_back(dcost); //start_disp = clock(); float _mes_l = 0, _mes_r = 0, _mes_u = 0, _mes_d = 0; if (bj != 0) { _mes_l = ComputeMes_PMBP_per_label(dis_belief_l, label_k, pl, test_label, wt_s[0], tau_s); vec_mes_l.push_back(_mes_l); } if (bj != width1 - 1) { _mes_r = ComputeMes_PMBP_per_label(dis_belief_r, label_k, pr, test_label, wt_s[1], tau_s); vec_mes_r.push_back(_mes_r); } if (bi != 0) { _mes_u = ComputeMes_PMBP_per_label(dis_belief_u, label_k, pu, test_label, wt_s[2], tau_s); vec_mes_u.push_back(_mes_u); } if (bi != height1 - 1) { _mes_d = ComputeMes_PMBP_per_label(dis_belief_d, label_k, pd, test_label, wt_s[3], tau_s); vec_mes_d.push_back(_mes_d); } vec_belief.push_back(_mes_l + _mes_r + _mes_u + _mes_d + dcost); } } Compute_top_k(vec_belief, vec_label, vec_mes_l, vec_mes_r, vec_mes_u, vec_mes_d, vec_d_cost, message, label_k, dcost_k, p1, NUM_TOP_K); //cout<<label_k[p1][0]<<endl; Message_normalization_PMF_PMBP(message, p1, NUM_TOP_K); } //cout<<"Message "<<clock()-start_disp<<endl; // propagation end } //superpixel scan end if (display) Show_WTA_Flow(iter, label_k, dcost_k, message, flowResult); finish_disp = clock(); std::cout << " time on clock(): " << (double)(clock() - start_disp) / CLOCKS_PER_SEC << " time on wall: " << omp_get_wtime() - wall_timer << "\n"; } //iteration Show_WTA_Flow(iterNum, label_k, dcost_k, message, flowResult); printf("==================================================\n"); printf("SPM-BP finished\n==================================================\n"); }
void CThinPlateSpline::computeSplineCoeffs(std::vector<Point>& iPIn, std::vector<Point>& iiPIn, float lambda,const TPS_INTERPOLATION tpsInter) { std::vector<Point>* iP = NULL; std::vector<Point>* iiP = NULL; if(tpsInter == FORWARD_WARP) { iP = &iPIn; iiP = &iiPIn; // keep information which coefficients are set FLAG_COEFFS_BACK_WARP_SET = true; FLAG_COEFFS_FORWARD_WARP_SET = false; } else if(tpsInter == BACK_WARP) { iP = &iiPIn; iiP = &iPIn; // keep information which coefficients are set FLAG_COEFFS_BACK_WARP_SET = false; FLAG_COEFFS_FORWARD_WARP_SET = true; } //get number of corresponding points int dim = 2; int n = iP->size(); //Initialize mathematical datastructures Mat_<float> V(dim,n+dim+1,0.0); Mat_<float> P(n,dim+1,1.0); Mat_<float> K = (K.eye(n,n)*lambda); Mat_<float> L(n+dim+1,n+dim+1,0.0); // fill up K und P matrix std::vector<Point>::iterator itY; std::vector<Point>::iterator itX; int y = 0; for (itY = iP->begin(); itY != iP->end(); ++itY, y++) { int x = 0; for (itX = iP->begin(); itX != iP->end(); ++itX, x++) { if (x != y) { K(y, x) = (float)fktU(*itY, *itX); } } P(y,1) = (float)itY->y; P(y,2) = (float)itY->x; } Mat Pt; transpose(P,Pt); // insert K into L Rect range = Rect(0, 0, n, n); Mat Lr(L,range); K.copyTo(Lr); // insert P into L range = Rect(n, 0, dim + 1, n); Lr = Mat(L,range); P.copyTo(Lr); // insert Pt into L range = Rect(0,n,n,dim+1); Lr = Mat(L,range); Pt.copyTo(Lr); // fill V array std::vector<Point>::iterator it; int u = 0; for(it = iiP->begin(); it != iiP->end(); ++it) { V(0,u) = (float)it->y; V(1,u) = (float)it->x; u++; } // transpose V Mat Vt; transpose(V,Vt); cMatrix = Mat_<float>(n+dim+1,dim,0.0); // invert L Mat invL; invert(L,invL,DECOMP_LU); //multiply(invL,Vt,cMatrix); cMatrix = invL * Vt; //compensate for rounding errors for(int row = 0; row < cMatrix.rows; row++) { for(int col = 0; col < cMatrix.cols; col++) { double v = cMatrix(row,col); if(v > (-1.0e-006) && v < (1.0e-006) ) { cMatrix(row,col) = 0.0; } } } }
void spm_bp::init_label_super(Mat_<Vec2f>& label_super_k, Mat_<float>& dCost_super_k) //, vector<vector<Vec2f> > &label_saved, vector<vector<Mat_<float> > > &dcost_saved) { printf("==================================================\n"); printf("Initiating particles...Done!\n"); vector<Vec2f> label_vec; Mat_<float> localDataCost; for (int sp = 0; sp < numOfSP1; ++sp) { int id = repPixels1[sp]; int y = subRange1[id][0]; int x = subRange1[id][1]; int h = subRange1[id][3] - subRange1[id][1] + 1; int w = subRange1[id][2] - subRange1[id][0] + 1; label_vec.clear(); int k = 0; while (k < NUM_TOP_K) { float du = (float(rand()) / RAND_MAX - 0.5) * 2 * (float)disp_range_u; float dv = (float(rand()) / RAND_MAX - 0.5) * 2 * (float)disp_range_v; du = floor(du * upScale + 0.5) / upScale; dv = floor(dv * upScale + 0.5) / upScale; if (du >= -disp_range_u && du <= disp_range_u && dv >= -disp_range_v && dv <= disp_range_v) { int index_tp = 1; for (int k1 = 0; k1 < k; ++k1) { if (checkEqual_PMF_PMBP(label_super_k[repPixels1[sp]][k1], Vec2f(du, dv))) index_tp = 0; } if (index_tp == 1) { for (int ii = 0; ii < superpixelsList1[sp].size(); ++ii) label_super_k[superpixelsList1[sp][ii]][k] = Vec2f(du, dv); label_vec.push_back(Vec2f(du, dv)); ++k; } } } #if USE_CLMF0_TO_AGGREGATE_COST cv::Mat_<cv::Vec4b> leftCombinedCrossMap; leftCombinedCrossMap.create(h, w); subCrossMap1[sp].copyTo(leftCombinedCrossMap); CFFilter cff; #endif int vec_size = label_vec.size(); localDataCost.create(h, w * vec_size); localDataCost.setTo(0); #pragma omp parallel for num_threads(NTHREADS) for (int i = 0; i < vec_size; ++i) { int kx = i * w; Mat_<float> rawCost; getLocalDataCostPerlabel(sp, label_vec[i], rawCost); #if USE_CLMF0_TO_AGGREGATE_COST cff.FastCLMF0FloatFilterPointer(leftCombinedCrossMap, rawCost, rawCost); #endif rawCost.copyTo(localDataCost(cv::Rect(kx, 0, w, h))); } //getLocalDataCost( sp, label_vec, localDataCost); int pt, px, py, kx; for (int ii = 0; ii < superpixelsList1[sp].size(); ++ii) { //cout<<ii<<endl; pt = superpixelsList1[sp][ii]; px = pt / width1; py = pt % width1; for (int kk = 0; kk < NUM_TOP_K; kk++) { kx = kk * w; const Mat_<float>& local = localDataCost(cv::Rect(kx, 0, w, h)); dCost_super_k[pt][kk] = local[px - x][py - y]; } } } printf("==================================================\n"); }
void visualize_3d_points(const Mat_<cv::Vec3f>& points_3d, std::string name) { Mat points_3d_display; points_3d.convertTo(points_3d_display, CV_8UC3, 100); imshow(name, points_3d_display); }
void spm_bp::getLocalDataCost(int sp, vector<Vec2f>& flowList, Mat_<float>& localDataCost) //, vector<vector<Vec2f> > &label_saved, vector<vector<Mat_<float> > > &dcost_saved) { int dSize = flowList.size(); //USE_COLOR_FEATURES cv::Mat_<cv::Vec3f> subLt = subImage1[sp]; #if USE_CENSUS vector<vector<bitset<CENSUS_SIZE_OF> > > subLt_css = subCensusBS1[sp]; //cv::Mat_<Vec_css> subLt_css = subCensus1[sp]; //cv::Mat_<Vec_css_bit> subLt_css_bit = subCensus_bit1[sp]; #endif int upHeight, upWidth; upHeight = im1Up.rows; upWidth = im1Up.cols; // extract sub-image from subrange int p1 = repPixels1[sp]; int w = subRange1[p1][2] - subRange1[p1][0] + 1; int h = subRange1[p1][3] - subRange1[p1][1] + 1; int x = subRange1[p1][0]; int y = subRange1[p1][1]; #if USE_CLMF0_TO_AGGREGATE_COST cv::Mat_<cv::Vec4b> leftCombinedCrossMap; leftCombinedCrossMap.create(h, w); subCrossMap1[sp].copyTo(leftCombinedCrossMap); CFFilter cff; #endif Mat_<float> rawCost(h, w); cv::Mat_<Vec3f> subRt(h, w); cv::Mat_<Vec2f> subRt_g(h, w); #if USE_CENSUS vector<vector<bitset<CENSUS_SIZE_OF> > > subRt_css(h, vector<bitset<CENSUS_SIZE_OF> >(w)); #endif cv::Mat_<float> tmpCost(h, w); // localDataCost.release(); localDataCost.create(h, w * dSize); int kx; for (int kd = 0; kd < dSize; ++kd) { kx = kd * w; clock_t start = clock(); clock_t end; //int check_id = isNewLabel_PMF_PMBP(label_saved[sp],flowList[kd]); #if SAVE_DCOST if (check_id >= 0) { cout << "hit" << endl; //dcost_saved[sp][check_id].copyTo(localDataCost(cv::Rect(kx, 0, w, h))); //cout<<"hit"<<end-start<<endl; //continue; } #endif start = clock(); Vec2f fl = flowList[kd]; Vec3f* subLtPtr = (cv::Vec3f*)(subLt.ptr(0)); Vec3f* subRtPtr = (cv::Vec3f*)(subRt.ptr(0)); float* rawCostPtr = (float*)(rawCost.ptr(0)); int cy, cx, oy, ox; oy = y; for (cy = 0; cy < h; ++cy, ++oy) { ox = x; for (cx = 0; cx < w; ++cx, ++ox) { int oyUp, oxUp; oyUp = (oy + fl[0]) * upScale; oxUp = (ox + fl[1]) * upScale; if (oyUp < 0) oyUp = 0; if (oyUp >= upHeight) oyUp = upHeight - 1; if (oxUp < 0) oxUp = 0; if (oxUp >= upWidth) oxUp = upWidth - 1; #if USE_POINTER_WISE *subRtPtr++ = im2Up[oyUp][oxUp]; #else subRt[cy][cx] = im2Up[oyUp][oxUp]; #endif #if USE_CENSUS subRt_css[cy][cx] = censusBS2[oyUp][oxUp]; #endif } } // calculate raw cost subLtPtr = (cv::Vec3f*)(subLt.ptr(0)); subRtPtr = (cv::Vec3f*)(subRt.ptr(0)); rawCostPtr = (float*)(rawCost.ptr(0)); int iy, ix; for (iy = 0; iy < h; ++iy) { for (ix = 0; ix < w; ++ix) { #if DATA_COST_ADCENSUS bitset<CENSUS_SIZE_OF> tmpBS = subRt_css[iy][ix] ^ subLt_css[iy][ix]; #if USE_POINTER_WISE float dist_c = fabs((float)(*subLtPtr)[0] - (*subRtPtr)[0]) + fabs((float)(*subLtPtr)[1] - (*subRtPtr)[1]) + fabs((float)(*subLtPtr++)[2] - (*subRtPtr++)[2]); #else float dist_c = std::abs(subLt[iy][ix][0] - subRt[iy][ix][0]) + std::abs(subLt[iy][ix][1] - subRt[iy][ix][1]) + std::abs(subLt[iy][ix][2] - subRt[iy][ix][2]); #endif float dist_css = expCensusDiffTable[tmpBS.count()]; //cout<<dist_c/3<<endl; float dist_ce = expColorDiffTable[int(dist_c / 3)]; //cout<<dist_css<<"and "<<dist_c1<<endl; //float dist_css = 1-std::exp( - float(HammingDis_bit(subLt_css_bit[iy][ix],subRt_css_bit[iy][ix]))/miu); #if USE_POINTER_WISE *rawCostPtr++ = 255 * (dist_css + dist_ce); #else rawCost[iy][ix] = 255 * (dist_css + dist_ce); //beta*min(dist_c/3,tau_c);//beta*min(dist_c/3,tau_c);//*255 + beta*min(dist_c/3,tau_c); #endif #endif } } #if USE_CLMF0_TO_AGGREGATE_COST cff.FastCLMF0FloatFilterPointer(leftCombinedCrossMap, rawCost, tmpCost); //cout<<"filtering"<<clock()-end<<endl; #endif #if SAVE_DCOST label_saved[sp].push_back(fl); //dcost_saved[sp].push_back(tmpCost); #endif tmpCost.copyTo(localDataCost(cv::Rect(kx, 0, w, h))); end = clock(); //cout<<"not hit"<<end-start<<endl; } //cout<<label_saved[sp].size()<<endl; }
int main(int argc, const char * argv[]) { //---------------------- // Open an OpenNI device //---------------------- //TODO: You'll want to open an RGB camera stream here too (the one with wich you wish to register the depth) cout << "Device opening ..." << endl; VideoCapture capture; capture.open( CAP_OPENNI ); if( !capture.isOpened() ) { cout << "Can not open a capture object." << endl; return -1; } // We don't want registration on, since we're going to do it ourselves. // Some devices with RGB cameras can perform registration on device bool modeRes=false; modeRes = capture.set( CAP_PROP_OPENNI_REGISTRATION, 0 ); if (!modeRes) { cout << "Can't disable registration. That's crazy!\n" << endl; return -1; } // Display the current configuration cout << "\nDepth generator output mode:" << endl << "FRAME_WIDTH " << capture.get( CAP_PROP_FRAME_WIDTH ) << endl << "FRAME_HEIGHT " << capture.get( CAP_PROP_FRAME_HEIGHT ) << endl << "FRAME_MAX_DEPTH " << capture.get( CAP_PROP_OPENNI_FRAME_MAX_DEPTH ) << " mm" << endl << "FPS " << capture.get( CAP_PROP_FPS ) << endl << "REGISTRATION " << capture.get( CAP_PROP_OPENNI_REGISTRATION ) << endl; //--------------------------------------- // Specify camera properties and geometry //-------------------------------------- //TODO: Fill in the values for your setup. // Depth camera intrinsics Matx33f unregisteredCameraMatrix = Matx33f::eye(); unregisteredCameraMatrix(0,0) = 570.0f; unregisteredCameraMatrix(1,1) = 570.0f; unregisteredCameraMatrix(0,2) = 320.0f-0.5f; unregisteredCameraMatrix(1,2) = 240.0f-0.5f; // NOTE: The depth distortion coefficients are currently not used by the Registration class. Vec<float, 5> unregisteredDistCoeffs(0,0,0,0,0); // RGB camera intrinsics Matx33f registeredCameraMatrix = Matx33f::eye(); registeredCameraMatrix(0,0) = 570.0f; registeredCameraMatrix(1,1) = 570.0f; registeredCameraMatrix(0,2) = 320.0f-0.5f; registeredCameraMatrix(1,2) = 240.0f-0.5f; Vec<float, 5> registeredDistCoeffs(0,0,0,0,0); Size2i registeredImagePlaneSize = Size2i(640, 480); // The rigid body transformation between cameras. // Used as: uv_rgb = K_rgb * [R | t] * z * inv(K_ir) * uv_ir Matx44f registrationRbt = Matx44f::eye(); registrationRbt(0,3) = .04; //------------------------------ // Create our registration class //------------------------------ oc::Registration registration(unregisteredCameraMatrix, unregisteredDistCoeffs, registeredCameraMatrix, registeredDistCoeffs, registrationRbt); for (;;) { Mat_<uint16_t> depthMap; if( !capture.grab() ) { cout << "Can't grab depth." << endl; return -1; } else { if( capture.retrieve( depthMap, CAP_OPENNI_DEPTH_MAP ) ) { // Actually perform the registration Mat_<uint16_t> registeredDepth; bool performDilation = false; registration.registerDepthToColor(depthMap, registeredImagePlaneSize, registeredDepth, performDilation); //Display the unregistered and registered depth const float scaleFactor = 0.05f; { Mat_<uint8_t> show; depthMap.convertTo( show, CV_8UC1, scaleFactor ); imshow( "depth map", show ); } { Mat_<uint8_t> show; registeredDepth.convertTo( show, CV_8UC1, scaleFactor ); imshow( "registered map", show ); } } } if( waitKey( 1 ) >= 0 ) break; } return 0; }
void spm_bp::getLocalDataCostPerlabel(int sp, const Vec2f& fl, Mat_<float>& localDataCost) { //USE_COLOR_FEATURES cv::Mat_<cv::Vec3f> subLt = subImage1[sp]; #if USE_CENSUS vector<vector<bitset<CENSUS_SIZE_OF> > > subLt_css = subCensusBS1[sp]; #endif int upHeight, upWidth; upHeight = im1Up.rows; upWidth = im1Up.cols; // extract sub-image from subrange int p1 = repPixels1[sp]; int w = subRange1[p1][2] - subRange1[p1][0] + 1; int h = subRange1[p1][3] - subRange1[p1][1] + 1; int x = subRange1[p1][0]; int y = subRange1[p1][1]; // localDataCost.release(); localDataCost.create(h, w); //Mat_<float> rawCost(h, w); Mat_<Vec3f> subRt(h, w); vector<vector<bitset<CENSUS_SIZE_OF> > > subRt_css(h, vector<bitset<CENSUS_SIZE_OF> >(w)); #if SAVE_DCOST if (check_id >= 0) { cout << "hit" << endl; } #endif Vec3f* subLtPtr = (cv::Vec3f*)(subLt.ptr(0)); Vec3f* subRtPtr = (cv::Vec3f*)(subRt.ptr(0)); float* rawCostPtr = (float*)(localDataCost.ptr(0)); cv::Vec3f *im2UpPtr = (cv::Vec3f*) im2Up.ptr(0); int im2UpWidth = im2Up.cols; int maxHeight = upHeight - 1; int maxWidth = upWidth - 1; int cy, cx, oy, ox; float fl0 = fl[0], fl1 = fl[1]; #pragma omp parallel for for (cy = 0; cy < h; ++cy) { oy = y + cy; int oyUp = (oy + fl0) * upScale; if (oyUp < 0) oyUp = 0; if (oyUp >= upHeight) oyUp = maxHeight; for (cx = 0; cx < w; ++cx) { ox = x + cx; int oxUp = (ox + fl1) * upScale; if (oxUp < 0) oxUp = 0; if (oxUp >= upWidth) oxUp = maxWidth; #if USE_POINTER_WISE // *subRtPtr++ = im2Up[oyUp][oxUp]; subRtPtr[cy * w + cx] = im2UpPtr[oyUp * im2UpWidth + oxUp]; #else subRt[cy][cx] = im2Up[oyUp][oxUp]; #endif #if USE_CENSUS subRt_css[cy][cx] = censusBS2[oyUp][oxUp]; #endif } } // calculate raw cost subLtPtr = (cv::Vec3f*)(subLt.ptr(0)); subRtPtr = (cv::Vec3f*)(subRt.ptr(0)); rawCostPtr = (float*)(localDataCost.ptr(0)); int iy, ix; for (iy = 0; iy < h; ++iy) { for (ix = 0; ix < w; ++ix) { #if DATA_COST_ADCENSUS bitset<CENSUS_SIZE_OF> tmpBS = subRt_css[iy][ix] ^ subLt_css[iy][ix]; #if USE_POINTER_WISE float dist_c = fabsf((*subLtPtr)[0] - (*subRtPtr)[0]) + fabsf((*subLtPtr)[1] - (*subRtPtr)[1]) + fabsf((*subLtPtr)[2] - (*subRtPtr)[2]); ++subLtPtr; ++subRtPtr; #else float dist_c = std::abs(subLt[iy][ix][0] - subRt[iy][ix][0]) + std::abs(subLt[iy][ix][1] - subRt[iy][ix][1]) + std::abs(subLt[iy][ix][2] - subRt[iy][ix][2]); #endif float dist_css = expCensusDiffTable[tmpBS.count()]; float dist_ce = expColorDiffTable[int(dist_c / 3)]; #if USE_POINTER_WISE *rawCostPtr++ = 255 * (dist_css + dist_ce); #else localDataCost[iy][ix] = 255 * (dist_css + dist_ce); //beta*min(dist_c/3,tau_c);//beta*min(dist_c/3,tau_c);//*255 + beta*min(dist_c/3,tau_c); #endif #endif } } #if SAVE_DCOST label_saved[sp].push_back(fl); #endif }
int main(int argc, char** argv) { /*Polynomial2 poly2; poly2.kuu = -1; poly2.kuv = 1; poly2.kvv= -1; poly2.ku = 0.25; poly2.kv = 0.25; poly2.k1 = 5; CurveRasterizer<Polynomial2> raster(1, 1, -100, 100, poly2); CurveRasterizer2<Polynomial2> raster2(1, 1, -100, 100, poly2); auto tr0 = clock(); int x1 = 0; int x2 = 0; for (int i = 0; i < 10000000; i++) { raster.step(); x1 += raster.x; } auto tr1 = clock(); for (int i = 0; i < 10000000; i++) { raster2.step(); x2 += raster2.x; } auto tr2 = clock(); cout << "optimized " << double(tr1 - tr0) / CLOCKS_PER_SEC << endl; cout << "simple " << double(tr2 - tr1) / CLOCKS_PER_SEC << endl; cout << x1 << " " << x2 << endl; return 0;*/ ifstream paramFile(argv[1]); if (not paramFile.is_open()) { cout << argv[1] << " : ERROR, file is not found" << endl; return 0; } array<double, 6> params; cout << "EU Camera model parameters :" << endl; for (auto & p: params) { paramFile >> p; cout << setw(15) << p; } cout << endl; paramFile.ignore(); array<double, 6> cameraPose; cout << "Camera pose wrt the robot :" << endl; for (auto & e: cameraPose) { paramFile >> e; cout << setw(15) << e; } cout << endl; paramFile.ignore(); Transformation<double> TbaseCamera(cameraPose.data()); array<double, 6> planePose; cout << "Plane pose :" << endl; for (auto & e: cameraPose) { paramFile >> e; cout << setw(15) << e; } cout << endl; paramFile.ignore(); Transformation<double> TbasePlane(cameraPose.data()); StereoParameters stereoParams; paramFile >> stereoParams.u0; paramFile >> stereoParams.v0; paramFile >> stereoParams.disparityMax; paramFile >> stereoParams.blockSize; paramFile.ignore(); string imageDir; getline(paramFile, imageDir); string imageInfo, imageName; array<double, 6> robotPose1, robotPose2; getline(paramFile, imageInfo); istringstream imageStream(imageInfo); imageStream >> imageName; for (auto & x : robotPose1) imageStream >> x; Mat8 img1 = imread(imageDir + imageName, 0); int counter = 2; while (getline(paramFile, imageInfo)) { istringstream imageStream(imageInfo); imageStream >> imageName; for (auto & x : robotPose2) imageStream >> x; Transformation<double> T01(robotPose1.data()), T02(robotPose2.data()); Transformation<double> TleftRight = T01.compose(TbaseCamera).inverseCompose(T02.compose(TbaseCamera)); Mat8 img2 = imread(imageDir + imageName, 0); EnhancedStereo stereo(TleftRight, img1.cols, img1.rows, params.data(), params.data(), stereoParams); cv::Mat_<uint8_t> res; auto t2 = clock(); stereo.comuteStereo(img1, img2, res); auto t3 = clock(); // cout << double(t3 - t2) / CLOCKS_PER_SEC << endl; Mat_<float> distMat; Mat_<float> planeMat; stereo.computeDistance(distMat); Transformation<double> T0Camera = T01.compose(TbaseCamera); stereo.generatePlane(T0Camera.inverseCompose(TbasePlane), planeMat, vector<Vector3d>{Vector3d(-0.1, -0.1, 0), Vector3d(-0.1 + 3 * 0.45, -0.1, 0), Vector3d(-0.1 + 3 * 0.45, 0.5, 0), Vector3d(-0.1, 0.5, 0) } ); imshow("dist" + to_string(counter) , distMat); imshow("plane" , planeMat); imwrite("/home/bogdan/projects/plane.png", planeMat); double err = 0; double err2 = 0; double dist = 0; int N = 0; int Nmax = 0; Mat_<float> inlierMat(planeMat.size()); inlierMat.setTo(0); for (int u = 0; u < distMat.cols; u++) { for (int v = 0; v < distMat.rows; v++) { if (planeMat(v, u) == 0) continue; Nmax++; dist += planeMat(v, u); inlierMat(v, u) = 1; if (distMat(v, u) == 0 or distMat(v, u) != distMat(v, u) or planeMat(v, u) != planeMat(v, u)) continue; if (abs(distMat(v, u) - planeMat(v, u)) > 0.10) continue; inlierMat(v, u) = 0; err += distMat(v, u) - planeMat(v, u); err2 += pow(distMat(v, u) - planeMat(v, u), 2); N++; } } // cout << (counter - 1) * 7 << " & " << dist/ Nmax * 1000 << " & " << err / N *1000 << " & " << sqrt(err2 / N)*1000 // << " & " << 100 * N / double(Nmax) << "\\\\" << endl << "\\hline" << endl; cout << "avg err : " << err / N *1000 << " avg err2 : " << sqrt(err2 / N)*1000 << " number of inliers : " << 100 * N / double(Nmax) << endl; imshow("diff" + to_string(counter), abs(planeMat - distMat)); imshow("inliers" + to_string(counter), inlierMat); counter++; } waitKey(); return 0; }
// Pick only the more stable/rigid points under changes of expression void extract_rigid_points(Mat_<double>& source_points, Mat_<double>& destination_points) { if(source_points.rows == 68) { Mat_<double> tmp_source = source_points.clone(); source_points = Mat_<double>(); // Push back the rigid points (some face outline, eyes, and nose) source_points.push_back(tmp_source.row(1)); source_points.push_back(tmp_source.row(2)); source_points.push_back(tmp_source.row(3)); source_points.push_back(tmp_source.row(4)); source_points.push_back(tmp_source.row(12)); source_points.push_back(tmp_source.row(13)); source_points.push_back(tmp_source.row(14)); source_points.push_back(tmp_source.row(15)); source_points.push_back(tmp_source.row(27)); source_points.push_back(tmp_source.row(28)); source_points.push_back(tmp_source.row(29)); source_points.push_back(tmp_source.row(31)); source_points.push_back(tmp_source.row(32)); source_points.push_back(tmp_source.row(33)); source_points.push_back(tmp_source.row(34)); source_points.push_back(tmp_source.row(35)); source_points.push_back(tmp_source.row(36)); source_points.push_back(tmp_source.row(39)); source_points.push_back(tmp_source.row(40)); source_points.push_back(tmp_source.row(41)); source_points.push_back(tmp_source.row(42)); source_points.push_back(tmp_source.row(45)); source_points.push_back(tmp_source.row(46)); source_points.push_back(tmp_source.row(47)); Mat_<double> tmp_dest = destination_points.clone(); destination_points = Mat_<double>(); // Push back the rigid points destination_points.push_back(tmp_dest.row(1)); destination_points.push_back(tmp_dest.row(2)); destination_points.push_back(tmp_dest.row(3)); destination_points.push_back(tmp_dest.row(4)); destination_points.push_back(tmp_dest.row(12)); destination_points.push_back(tmp_dest.row(13)); destination_points.push_back(tmp_dest.row(14)); destination_points.push_back(tmp_dest.row(15)); destination_points.push_back(tmp_dest.row(27)); destination_points.push_back(tmp_dest.row(28)); destination_points.push_back(tmp_dest.row(29)); destination_points.push_back(tmp_dest.row(31)); destination_points.push_back(tmp_dest.row(32)); destination_points.push_back(tmp_dest.row(33)); destination_points.push_back(tmp_dest.row(34)); destination_points.push_back(tmp_dest.row(35)); destination_points.push_back(tmp_dest.row(36)); destination_points.push_back(tmp_dest.row(39)); destination_points.push_back(tmp_dest.row(40)); destination_points.push_back(tmp_dest.row(41)); destination_points.push_back(tmp_dest.row(42)); destination_points.push_back(tmp_dest.row(45)); destination_points.push_back(tmp_dest.row(46)); destination_points.push_back(tmp_dest.row(47)); } }