float CalculateError(const Mat_<float>& ground_truth_shape, const Mat_<float>& predicted_shape) { Mat_<float> temp; //temp = ground_truth_shape.rowRange(36, 37)-ground_truth_shape.rowRange(45, 46); temp = ground_truth_shape.rowRange(1, 2) - ground_truth_shape.rowRange(6, 7); float x = mean(temp.col(0))[0]; float y = mean(temp.col(1))[1]; float interocular_distance = sqrt(x*x + y*y); float sum = 0; for (int i = 0; i < ground_truth_shape.rows; i++) { sum += norm(ground_truth_shape.row(i) - predicted_shape.row(i)); } return sum / (ground_truth_shape.rows*interocular_distance); }
Mat_<double> estimatePose(Mat_<double> const imagePts) { // Note that given R, t, the camera location in world // coordinates is not just t, but instead -inv(R)*t. Mat_<double> const worldPts = getWorldPts(); Mat_<double> const rotTransl = estimateRotTransl(worldPts, imagePts); Mat_<double> rotMatrix(3, 3); Mat_<double> translation(3, 1); rotTransl.col(0).copyTo(rotMatrix.col(0)); rotTransl.col(1).copyTo(rotMatrix.col(1)); rotTransl.col(2).copyTo(rotMatrix.col(2)); rotTransl.col(3).copyTo(translation); Mat_<double> cameraLoc = -rotMatrix.t() * translation; Mat_<double> simplePose(4, 1); simplePose(0) = cameraLoc(0); // x simplePose(1) = cameraLoc(1); // y simplePose(2) = cameraLoc(2); // z // Yaw (rotation around z axis): // See http://planning.cs.uiuc.edu/node103.html simplePose(3) = atan2(rotMatrix(1, 0), rotMatrix(0, 0)); // cout << "simplePose: " << simplePose << endl; return simplePose; }
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); }
// Need to move this all to opengl void DrawBox(Mat image, Vec6d pose, Scalar color, int thickness, float fx, float fy, float cx, float cy) { float boxVerts[] = {-1, 1, -1, 1, 1, -1, 1, 1, 1, -1, 1, 1, 1, -1, 1, 1, -1, -1, -1, -1, -1, -1, -1, 1}; Mat_<float> box = Mat(8, 3, CV_32F, boxVerts).clone() * 100; Matx33f rot = Euler2RotMat(Vec3d(pose[3], pose[4], pose[5])); Mat_<float> rotBox; Mat((Mat(rot) * box.t())).copyTo(rotBox); rotBox = rotBox.t(); rotBox.col(0) = rotBox.col(0) + pose[0]; rotBox.col(1) = rotBox.col(1) + pose[1]; rotBox.col(2) = rotBox.col(2) + pose[2]; // draw the lines Mat_<float> rotBoxProj; Project(rotBoxProj, rotBox, image.size(), fx, fy, cx, cy); Mat begin; Mat end; rotBoxProj.row(0).copyTo(begin); rotBoxProj.row(1).copyTo(end); cv::line(image, Point((int)begin.at<float>(0), (int)begin.at<float>(1)), Point((int)end.at<float>(0), (int)end.at<float>(1)), color, thickness); rotBoxProj.row(1).copyTo(begin); rotBoxProj.row(2).copyTo(end); cv::line(image, Point((int)begin.at<float>(0), (int)begin.at<float>(1)), Point((int)end.at<float>(0), (int)end.at<float>(1)), color, thickness); rotBoxProj.row(2).copyTo(begin); rotBoxProj.row(3).copyTo(end); cv::line(image, Point((int)begin.at<float>(0), (int)begin.at<float>(1)), Point((int)end.at<float>(0), (int)end.at<float>(1)), color, thickness); rotBoxProj.row(0).copyTo(begin); rotBoxProj.row(3).copyTo(end); //std::cout << begin <<endl; //std::cout << end <<endl; cv::line(image, Point((int)begin.at<float>(0), (int)begin.at<float>(1)), Point((int)end.at<float>(0), (int)end.at<float>(1)), color, thickness); rotBoxProj.row(2).copyTo(begin); rotBoxProj.row(4).copyTo(end); cv::line(image, Point((int)begin.at<float>(0), (int)begin.at<float>(1)), Point((int)end.at<float>(0), (int)end.at<float>(1)), color, thickness); rotBoxProj.row(1).copyTo(begin); rotBoxProj.row(5).copyTo(end); cv::line(image, Point((int)begin.at<float>(0), (int)begin.at<float>(1)), Point((int)end.at<float>(0), (int)end.at<float>(1)), color, thickness); rotBoxProj.row(0).copyTo(begin); rotBoxProj.row(6).copyTo(end); cv::line(image, Point((int)begin.at<float>(0), (int)begin.at<float>(1)), Point((int)end.at<float>(0), (int)end.at<float>(1)), color, thickness); rotBoxProj.row(3).copyTo(begin); rotBoxProj.row(7).copyTo(end); cv::line(image, Point((int)begin.at<float>(0), (int)begin.at<float>(1)), Point((int)end.at<float>(0), (int)end.at<float>(1)), color, thickness); rotBoxProj.row(6).copyTo(begin); rotBoxProj.row(5).copyTo(end); cv::line(image, Point((int)begin.at<float>(0), (int)begin.at<float>(1)), Point((int)end.at<float>(0), (int)end.at<float>(1)), color, thickness); rotBoxProj.row(5).copyTo(begin); rotBoxProj.row(4).copyTo(end); cv::line(image, Point((int)begin.at<float>(0), (int)begin.at<float>(1)), Point((int)end.at<float>(0), (int)end.at<float>(1)), color, thickness); rotBoxProj.row(4).copyTo(begin); rotBoxProj.row(7).copyTo(end); cv::line(image, Point((int)begin.at<float>(0), (int)begin.at<float>(1)), Point((int)end.at<float>(0), (int)end.at<float>(1)), color, thickness); rotBoxProj.row(7).copyTo(begin); rotBoxProj.row(6).copyTo(end); cv::line(image, Point((int)begin.at<float>(0), (int)begin.at<float>(1)), Point((int)end.at<float>(0), (int)end.at<float>(1)), color, thickness); }
void Tree::Splitnode(const vector<Mat_<uchar> >& images, const vector<Mat_<double> >& ground_truth_shapes, const vector<Mat_<double> >& current_shapes, const vector<BoundingBox> & bounding_box, const Mat_<double>& mean_shape, const Mat_<double>& shapes_residual, const vector<int> &ind_samples, // output double& thresh, double* feat, bool& isvaild, vector<int>& lcind, vector<int>& rcind ){ if (ind_samples.size() == 0){ thresh = 0; feat = new double[4]; feat[0] = 0; feat[1] = 0; feat[2] = 0; feat[3] = 0; lcind.clear(); rcind.clear(); isvaild = 1; return; } // get candidate pixel locations static int randomseedforfeature = 0; RNG random_generator(getTickCount()); Mat_<double> candidate_pixel_locations(max_numfeats_,4); for(unsigned int i = 0;i < max_numfeats_;i++){ double x1 = random_generator.uniform(-1.0,1.0); double y1 = random_generator.uniform(-1.0,1.0); double x2 = random_generator.uniform(-1.0,1.0); double y2 = random_generator.uniform(-1.0,1.0); //伪随机数 //double x1 = -1.0 + randomseedforfeature * 17%57 * (2.0/57); //randomseedforfeature++; //double y1 = -1.0 + randomseedforfeature * 17%57 * (2.0/57); //randomseedforfeature++; //double x2 = -1.0 + randomseedforfeature * 17%57 * (2.0/57); //randomseedforfeature++; //double y2 = -1.0 + randomseedforfeature * 17%57 * (2.0/57); //randomseedforfeature++; if((x1*x1 + y1*y1 > 1.0)||(x2*x2 + y2*y2 > 1.0)){ i--; continue; } // cout << x1 << " "<<y1 <<" "<< x2<<" "<< y2<<endl; candidate_pixel_locations(i,0) = x1 * max_radio_radius_; candidate_pixel_locations(i,1) = y1 * max_radio_radius_; candidate_pixel_locations(i,2) = x2 * max_radio_radius_; candidate_pixel_locations(i,3) = y2 * max_radio_radius_; } // get pixel difference feature Mat_<int> densities(max_numfeats_,(int)ind_samples.size()); for (int i = 0;i < ind_samples.size();i++){ Mat_<double> rotation; double scaleX, scaleY; Mat_<double> temp = ProjectShape(current_shapes[ind_samples[i]],bounding_box[ind_samples[i]]); SimilarityTransform(temp,mean_shape,rotation,scaleX, scaleY); // whether transpose or not ???? for(int j = 0;j < max_numfeats_;j++){ //some problems??? Ming /*fixed bug, by Ming, 2015.08.20*/ //原来代码似乎没错.. 先改回去 2015.12.09, Ming double project_x1 = rotation(0,0) * candidate_pixel_locations(j,0) + rotation(0,1) * candidate_pixel_locations(j,1); double project_y1 = rotation(1,0) * candidate_pixel_locations(j,0) + rotation(1,1) * candidate_pixel_locations(j,1); //double project_x1 = rotation(0,0) * candidate_pixel_locations(j,0) + rotation(1,0) * candidate_pixel_locations(j,1); //double project_y1 = rotation(0,1) * candidate_pixel_locations(j,0) + rotation(1,1) * candidate_pixel_locations(j,1); project_x1 = scaleX * project_x1 * bounding_box[ind_samples[i]].width / 2.0; project_y1 = scaleY * project_y1 * bounding_box[ind_samples[i]].height / 2.0; int real_x1 = project_x1 + current_shapes[ind_samples[i]](landmarkID_,0); int real_y1 = project_y1 + current_shapes[ind_samples[i]](landmarkID_,1); real_x1 = max(0.0,min((double)real_x1,images[ind_samples[i]].cols-1.0)); real_y1 = max(0.0,min((double)real_y1,images[ind_samples[i]].rows-1.0)); /*fixed bug, by Ming, 2015.08.20*/ //原来代码似乎没错.. 先改回去 2015.12.09, Ming double project_x2 = rotation(0,0) * candidate_pixel_locations(j,2) + rotation(0,1) * candidate_pixel_locations(j,3); double project_y2 = rotation(1,0) * candidate_pixel_locations(j,2) + rotation(1,1) * candidate_pixel_locations(j,3); //double project_x2 = rotation(0,0) * candidate_pixel_locations(j,2) + rotation(1,0) * candidate_pixel_locations(j,3); //double project_y2 = rotation(0,1) * candidate_pixel_locations(j,2) + rotation(1,1) * candidate_pixel_locations(j,3); project_x2 = scaleX * project_x2 * bounding_box[ind_samples[i]].width / 2.0; project_y2 = scaleY * project_y2 * bounding_box[ind_samples[i]].height / 2.0; int real_x2 = project_x2 + current_shapes[ind_samples[i]](landmarkID_,0); int real_y2 = project_y2 + current_shapes[ind_samples[i]](landmarkID_,1); real_x2 = max(0.0,min((double)real_x2,images[ind_samples[i]].cols-1.0)); real_y2 = max(0.0,min((double)real_y2,images[ind_samples[i]].rows-1.0)); densities(j,i) = ((int)(images[ind_samples[i]](real_y1,real_x1))-(int)(images[ind_samples[i]](real_y2,real_x2))); } } // pick the feature Mat_<int> densities_sorted = densities.clone(); cv::sort(densities, densities_sorted, CV_SORT_ASCENDING); vector<double> lc1,lc2; vector<double> rc1,rc2; lc1.reserve(ind_samples.size()); rc1.reserve(ind_samples.size()); lc2.reserve(ind_samples.size()); rc2.reserve(ind_samples.size()); // double E_x_2 = mean(shapes_residual.col(0).mul(shapes_residual.col(0)))[0]; // double E_x = mean(shapes_residual.col(0))[0]; // double E_y_2 = mean(shapes_residual.col(1).mul(shapes_residual.col(1)))[0]; // double E_y = mean(shapes_residual.col(1))[0]; // double var_overall = ind_samples.size()*((E_x_2 - E_x*E_x) + (E_y_2 - E_y*E_y)); double var_overall =(calculate_var(shapes_residual.col(0))+calculate_var(shapes_residual.col(1))) * ind_samples.size(); double max_var_reductions = 0; double threshold = 0; double var_lc = 0; double var_rc = 0; double var_reduce = 0; double max_id = 0; int thresholdTempNum = 10; for (int i = 0; i <max_numfeats_;i++){ double bestReduce = -999999; double bestTempThresd = 0; for(int k=0; k<thresholdTempNum; ++k) { lc1.clear(); lc2.clear(); rc1.clear(); rc2.clear(); int ind =(int)(ind_samples.size() * random_generator.uniform(0.05,0.95)); //int ind =(int)(ind_samples.size() * 0.05 + k*0.9/thresholdTempNum); //printf("%d th numfeats | %d th img sample \n",i,ind); threshold = densities_sorted(i,ind); for (int j=0;j < ind_samples.size();j++){ if (densities(i,j) < threshold){ lc1.push_back(shapes_residual(j,0)); lc2.push_back(shapes_residual(j,1)); } else{ rc1.push_back(shapes_residual(j,0)); rc2.push_back(shapes_residual(j,1)); } } var_lc = (calculate_var(lc1)+calculate_var(lc2)) * lc1.size(); var_rc = (calculate_var(rc1)+calculate_var(rc2)) * rc1.size(); var_reduce = var_overall - var_lc - var_rc; if(var_reduce > bestReduce) { bestReduce = var_reduce; bestTempThresd = threshold; } } //printf("var_reduce : %lf\n",var_reduce); // cout << var_reduce<<endl; if (bestReduce > max_var_reductions){ max_var_reductions = bestReduce; thresh = bestTempThresd; max_id = i; } } //printf("max_var_reductions : %lf\n",max_var_reductions); isvaild = 1; //feat[0] =candidate_pixel_locations(max_id,0)/max_radio_radius_; //feat[1] =candidate_pixel_locations(max_id,1)/max_radio_radius_; //feat[2] =candidate_pixel_locations(max_id,2)/max_radio_radius_; //feat[3] =candidate_pixel_locations(max_id,3)/max_radio_radius_; feat[0] =candidate_pixel_locations(max_id,0); feat[1] =candidate_pixel_locations(max_id,1); feat[2] =candidate_pixel_locations(max_id,2); feat[3] =candidate_pixel_locations(max_id,3); // cout << max_id<< " "<<max_var_reductions <<endl; // cout << feat[0] << " "<<feat[1] <<" "<< feat[2]<<" "<< feat[3]<<endl; lcind.clear(); rcind.clear(); for (int j=0;j < ind_samples.size();j++){ if (densities(max_id,j) < thresh){ lcind.push_back(ind_samples[j]); } else{ rcind.push_back(ind_samples[j]); } } }
Mat_<double> estimateRotTransl( Mat_<double> const worldPts, Mat_<double> const imagePts) { assert(imagePts.cols == 2); assert(worldPts.cols == 3); assert(imagePts.rows == worldPts.rows); // TODO verify all worldPts have z=0 // See "pose estimation" section in the paper. // Set up linear system of equations. int const n = imagePts.rows; Mat_<double> F(2 * n, 9); for(int i = 0; i < n; i++) { F(2 * i, 0) = worldPts(i, 0); F(2 * i, 1) = 0; F(2 * i, 2) = -worldPts(i, 0) * imagePts(i, 0); F(2 * i, 3) = worldPts(i, 1); F(2 * i, 4) = 0; F(2 * i, 5) = -worldPts(i, 1) * imagePts(i, 0); F(2 * i, 6) = 1; F(2 * i, 7) = 0; F(2 * i, 8) = -imagePts(i, 0); F(2 * i + 1, 0) = 0; F(2 * i + 1, 1) = worldPts(i, 0); F(2 * i + 1, 2) = -worldPts(i, 0) * imagePts(i, 1); F(2 * i + 1, 3) = 0; F(2 * i + 1, 4) = worldPts(i, 1); F(2 * i + 1, 5) = -worldPts(i, 1) * imagePts(i, 1); F(2 * i + 1, 6) = 0; F(2 * i + 1, 7) = 1; F(2 * i + 1, 8) = -imagePts(i, 1); } // Find least-squares estimate of rotation + translation. SVD svd(F); Mat_<double> rrp = svd.vt.row(8); rrp = rrp.clone().reshape(0, 3).t(); if(rrp(2, 2) < 0) { rrp *= -1; // make sure depth is positive } // cout << "rrp: " << rrp << endl; Mat_<double> transl = \ 2 * rrp.col(2) / (norm(rrp.col(0)) + norm(rrp.col(1))); // cout << "transl: " << transl << endl; Mat_<double> rot = Mat_<double>::zeros(3, 3); rrp.col(0).copyTo(rot.col(0)); rrp.col(1).copyTo(rot.col(1)); SVD svd2(rot); rot = svd2.u * svd2.vt; if(determinant(rot) < 0) { rot.col(2) *= -1; // make sure it's a valid rotation matrix } if(abs(determinant(rot) - 1) > 1e-10) { cerr << "Warning: rotation matrix has determinant " \ << determinant(rot) << " where expected 1." << endl; } // cout << "rot: " << rot << endl; Mat_<double> rotTransl(3, 4); rot.col(0).copyTo(rotTransl.col(0)); rot.col(1).copyTo(rotTransl.col(1)); rot.col(2).copyTo(rotTransl.col(2)); transl.copyTo(rotTransl.col(3)); return rotTransl; }