SDM:https://github.com/RoboPai/sdm
#include "stdafx.h"
#include <vector>
#include <iostream>
#include <fstream>
#include "opencv2/opencv.hpp"
#include "opencv2/core/core.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/objdetect/objdetect.hpp"
#include "ldmarkmodel.h"
using namespace std;
using namespace cv;
int main()
{
/*********************
std::vector<ImageLabel> mImageLabels;
if(!load_ImageLabels("mImageLabels-test.bin", mImageLabels)){
mImageLabels.clear();
ReadLabelsFromFile(mImageLabels, "labels_ibug_300W_test.xml");
save_ImageLabels(mImageLabels, "mImageLabels-test.bin");
}
std::cout << "测试数据一共有: " << mImageLabels.size() << std::endl;
*******************/
ldmarkmodel modelt;
std::string modelFilePath = "roboman-landmark-model.bin";
while (!load_ldmarkmodel(modelFilePath, modelt)){
std::cout << "文件打开错误,请重新输入文件路径." << std::endl;
std::cin >> modelFilePath;
}
cv::VideoCapture mCamera(0);
if (!mCamera.isOpened()){
std::cout << "Camera opening failed..." << std::endl;
system("pause");
return 0;
}
cv::Mat Image;
cv::Mat current_shape;
for (;;){
mCamera >> Image;
modelt.track(Image, current_shape);
cv::Vec3d eav;
modelt.EstimateHeadPose(current_shape, eav);
modelt.drawPose(Image, current_shape, 50);
int numLandmarks = current_shape.cols / 2;
for (int j = 0; j<numLandmarks; j++){
int x = current_shape.at<float>(j);
int y = current_shape.at<float>(j + numLandmarks);
std::stringstream ss;
ss << j;
// cv::putText(Image, ss.str(), cv::Point(x, y), 0.5, 0.5, cv::Scalar(0, 0, 255));
cv::circle(Image, cv::Point(x, y), 2, cv::Scalar(0, 0, 255), -1);
}
cv::imshow("Camera", Image);
if (27 == cv::waitKey(5)){
mCamera.release();
cv::destroyAllWindows();
break;
}
}
system("pause");
return 0;
}
void ldmarkmodel::EstimateHeadPose(cv::Mat ¤t_shape, cv::Vec3d &eav){
if(current_shape.empty())
return;
static const int samplePdim = 7;
float miny = 10000000000.0f;
float maxy = 0.0f;
float sumx = 0.0f;
float sumy = 0.0f;
for(int i=0; i<samplePdim; i++){
sumx += current_shape.at<float>(estimateHeadPosePointIndexs[i]);
float y = current_shape.at<float>(estimateHeadPosePointIndexs[i]+current_shape.cols/2);
sumy += y;
if(miny > y)
miny = y;
if(maxy < y)
maxy = y;
}
float dist = maxy - miny;
sumx = sumx/samplePdim;
sumy = sumy/samplePdim;
static cv::Mat tmp(1, 2*samplePdim+1, CV_32FC1);
for(int i=0; i<samplePdim; i++){
tmp.at<float>(i) = (current_shape.at<float>(estimateHeadPosePointIndexs[i])-sumx)/dist;
tmp.at<float>(i+samplePdim) = (current_shape.at<float>(estimateHeadPosePointIndexs[i]+current_shape.cols/2)-sumy)/dist;
}
tmp.at<float>(2*samplePdim) = 1.0f;
// cv::Mat predict = tmp*estimateHeadPoseMat;
// double _pm[12] = {predict.at<float>(0), predict.at<float>(1), predict.at<float>(2), 0,
// predict.at<float>(3), predict.at<float>(4), predict.at<float>(5), 0,
// predict.at<float>(6), predict.at<float>(7), predict.at<float>(8), 0};
// cv::Mat tmp0,tmp1,tmp2,tmp3,tmp4,tmp5;
// cv::decomposeProjectionMatrix(cv::Mat(3,4,CV_64FC1,_pm),tmp0,tmp1,tmp2,tmp3,tmp4,tmp5,eav);
cv::Mat predict = tmp*estimateHeadPoseMat2;
eav[0] = predict.at<float>(0);
eav[1] = predict.at<float>(1);
eav[2] = predict.at<float>(2);
// std::cout << eav[0] << " " << eav[1] << " " << eav[2] << std::endl;
// Pitch = eav[0];
// Yaw = eav[1];
// Roll = eav[2];
return;
}
Taily老段的微信公众号,欢迎交流学习
https://blog.csdn.net/taily_duan/article/details/81214815