基于C++实现的三维点云的深度投影和纹理投影源码.zip

#include "SWFaceMatchApi.h" #include "facerecon.h" #include "dataprocess.h" #include <fstream> #include <time.h> #include <math.h> #include <stddef.h> #include <stdlib.h> #include <string.h> #include <iostream> #include <list> using namespace cv; using namespace std; double g_meanface[15] = { -31.1297 , 34.0010 , 122.4674, 30.9267 , 33.7814 , 122.1970 , 0.0132 , 4.4303 , 161.5600 , -29.3909 , -33.8801 , 128.1061 , 29.1029 , -33.5093 , 127.8227}; double g_meanfacelm2[10] = { 30.2946, 51.6963 - 25, 65.5318, 51.5014 - 25, 48.0252, 71.7366, 33.5493, 92.3615, 62.7299, 92.2041 }; int g_img_size = 450; Size m_cropimg_size = Size(96,112); extern int g_bnormalize ; extern int g_bnormalizeX ; extern int g_bnormalizeY ; //vector<Point2f> g_meanface2D = {Point2f(30.2946, 51.6963), // Point2f(65.5318, 51.5014), // Point2f(48.0252, 71.7366), // Point2f(33.5493, 92.3655), // Point2f(62.7299, 92.2041)}; Mat tformfwd(const Mat& trans, const Mat& uv) { Mat uv_h = Mat::ones(uv.rows, 3, CV_64FC1); uv.copyTo(uv_h(Rect(0, 0, 2, uv.rows))); Mat xv_h = uv_h*trans; return xv_h(Rect(0, 0, 2, uv.rows)); } Mat find_none_flectives_similarity(const Mat& uv, const Mat& xy) { Mat A = Mat::zeros(2 * xy.rows, 4, CV_64FC1); Mat b = Mat::zeros(2 * xy.rows, 1, CV_64FC1); Mat x = Mat::zeros(4, 1, CV_64FC1); xy(Rect(0, 0, 1, xy.rows)).copyTo(A(Rect(0, 0, 1, xy.rows)));//x xy(Rect(1, 0, 1, xy.rows)).copyTo(A(Rect(1, 0, 1, xy.rows)));//y A(Rect(2, 0, 1, xy.rows)).setTo(1.); xy(Rect(1, 0, 1, xy.rows)).copyTo(A(Rect(0, xy.rows, 1, xy.rows)));//y (xy(Rect(0, 0, 1, xy.rows))).copyTo(A(Rect(1, xy.rows, 1, xy.rows)));//-x A(Rect(1, xy.rows, 1, xy.rows)) *= -1; A(Rect(3, xy.rows, 1, xy.rows)).setTo(1.); uv(Rect(0, 0, 1, uv.rows)).copyTo(b(Rect(0, 0, 1, uv.rows))); uv(Rect(1, 0, 1, uv.rows)).copyTo(b(Rect(0, uv.rows, 1, uv.rows))); cv::solve(A, b, x, cv::DECOMP_SVD); Mat trans_inv = (Mat_<double>(3, 3) << x.at<double>(0), -x.at<double>(1), 0, x.at<double>(1), x.at<double>(0), 0, x.at<double>(2), x.at<double>(3), 1); Mat trans = trans_inv.inv(cv::DECOMP_SVD); trans.at<double>(0, 2) = 0; trans.at<double>(1, 2) = 0; trans.at<double>(2, 2) = 1; return trans; } Mat find_similarity(const Mat& uv, const Mat& xy) { Mat trans1 = find_none_flectives_similarity(uv, xy); Mat xy_reflect = xy; xy_reflect(Rect(0, 0, 1, xy.rows)) *= -1; Mat trans2r = find_none_flectives_similarity(uv, xy_reflect); Mat reflect = (Mat_<double>(3, 3) << -1, 0, 0, 0, 1, 0, 0, 0, 1); Mat trans2 = trans2r*reflect; Mat xy1 = tformfwd(trans1, uv); double norm1 = cv::norm(xy1 - xy); Mat xy2 = tformfwd(trans2, uv); double norm2 = cv::norm(xy2 - xy); Mat trans; if (norm1 < norm2){ trans = trans1; } else { trans = trans2; } return trans; } Mat get_similarity_transform(const vector<Point2f>& src_points, const vector<Point2f>& dst_points, bool reflective = true) { Mat trans; Mat src((int)src_points.size(), 2, CV_32FC1, (void*)(&src_points[0].x)); src.convertTo(src, CV_64FC1); Mat dst((int)dst_points.size(), 2, CV_32FC1, (void*)(&dst_points[0].x)); dst.convertTo(dst, CV_64FC1); if (reflective){ trans = find_similarity(src, dst); } else { trans = find_none_flectives_similarity(src, dst); } Mat trans_cv2 = trans(Rect(0, 0, 2, trans.rows)).t(); return trans_cv2; } Mat align_face(const cv::Mat& src, const double* landmark) { vector<Point2f> detect_points; for (int i = 0; i < 5; i++) { detect_points.push_back(Point2f(landmark[i], landmark[i + 5])); } vector<Point2f> reference_points; reference_points.push_back(cv::Point2f(30.2946, 51.6963 - 25)); reference_points.push_back(cv::Point2f(65.5318, 51.5014 - 25)); reference_points.push_back(cv::Point2f(48.0252, 71.7366)); reference_points.push_back(cv::Point2f(33.5493, 92.3615)); reference_points.push_back(cv::Point2f(62.7299, 92.2041)); Mat tfm = get_similarity_transform(detect_points, reference_points); Mat aligned_face; warpAffine(src, aligned_face, tfm, m_cropimg_size); return aligned_face; } Mat align_face(const cv::Mat& src, const double* landmark, int ptCount, int lossindex) { std::vector<Point2f> detect_points; for (int i = 0; i < ptCount; i++) { detect_points.push_back(Point2f(landmark[i], landmark[i + ptCount])); } std::vector<Point2f> reference_points; for (int i = 0, n = 0; i < ptCount; n++, i++) { if (n == lossindex) n++; reference_points.push_back(cv::Point2f(g_meanfacelm2[2 * n], g_meanfacelm2[2 * n + 1])); } Mat tfm = get_similarity_transform(detect_points, reference_points); Mat aligned_face; warpAffine(src, aligned_face, tfm, m_cropimg_size); return aligned_face; } Mat align_face2(const cv::Mat& src, const double* landmark, int ptCount, int lossindex) { std::vector<Point2f> detect_points; for (int i = 0; i < ptCount; i++) { detect_points.push_back(Point2f(landmark[i * 2], landmark[i * 2 + 1])); } std::vector<Point2f> reference_points; for (int i = 0, n = 0; i < ptCount; n++, i++) { if (n == lossindex) n++; reference_points.push_back(cv::Point2f(g_meanfacelm2[2 * n], g_meanfacelm2[2 * n + 1])); } Mat tfm = get_similarity_transform(detect_points, reference_points); Mat aligned_face; warpAffine(src, aligned_face, tfm, m_cropimg_size); return aligned_face; } Mat align_face224(const cv::Mat& src, const double* landmark, int ptCount, int lossindex) { std::vector<Point2f> detect_points; for (int i = 0; i < ptCount; i++) { detect_points.push_back(Point2f(landmark[i * 2], landmark[i * 2 + 1])); } std::vector<Point2f> reference_points; for (int i = 0, n = 0; i < ptCount; n++, i++) { if (n == lossindex) n++; reference_points.push_back(cv::Point2f(g_meanfacelm2[2 * n] * 2, g_meanfacelm2[2 * n + 1] * 2)); } Mat tfm = get_similarity_transform(detect_points, reference_points); Mat aligned_face; warpAffine(src, aligned_face, tfm, m_cropimg_size * 2); return aligned_face; } template<typename Ty> void savedata(const char* filename, Ty *data,int num,int dim) { ofstream ofile; //定义输出文件 ofile.open(filename, ios::out); //作为输出文件打开 for(int i =0;i<num;i++) { for(int j =0;j<dim;j++) ofile << data[i+j*num] << " "; ofile<<endl; } ofile.close(); } Mat procrustes(double *meanfacelm3, double *facelm3, int keycount, double *data, int nPtCount, int dim = 3) { Mat meanface(keycount, 1, CV_64FC3, meanfacelm3); meanface = meanface.t(); Mat face(keycount, 1, CV_64FC3, facelm3); face = face.t(); Scalar mu_x = cv::mean(meanface); Mat tempmeanface = meanface - Mat(meanface.size(), meanface.type(), mu_x); double normX = cv::norm(tempmeanface); tempmeanface = tempmeanface / normX; tempmeanface = tempmeanface.reshape(1, keycount); Scalar mu_y = cv::mean(face); Mat tempface = face - Mat(face.size(), face.type(), mu_y); double normY = cv::norm(tempface); tempface = tempface / normY; tempface = tempface.reshape(1, keycount); Mat A = tempmeanface.t()*tempface; Mat w, u, vt; SVDecomp(A, w, u, vt); Mat T = vt.t()*u.t(); double traceTA = cv::sum(w)[0]; double b = traceTA * normX / normY; double d = 1 - traceTA *traceTA; Mat muY = Mat(Size(3, 1), CV_64FC3, mu_y); muY = muY.reshape(1, 3); Mat muX = Mat(Size(3, 1), CV_64FC3, mu_x); muX = muX.reshape(1, 3); Mat c = muX - b*muY*T; Mat trans = -1 / b *c*T.t(); Mat transM = Mat::zeros(4, 4, CV_64FC1); for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { transM.at<double>(i, j) = 1.0 / b * T.at<double>(i, j); } } transM.at<double>(0, 3) = trans.at<double>(0, 0); transM.at<double>(1, 3) = trans.at<double>(0, 1); transM.at<doubl