11#include " PositionSolver.h"
22
33
4- PositionSolver::PositionSolver (int width, int height):
4+ PositionSolver::PositionSolver (int width, int height,
5+ float prior_pitch, float prior_yaw, float prior_distance):
56 contour_indices{ 0 ,1 ,8 ,15 ,16 ,27 ,28 ,29 ,30 ,31 ,32 ,33 ,34 ,35 ,36 ,39 ,42 ,45 }
67{
8+ this ->prior_pitch = (1 .1f * (prior_pitch + 90 .f ) / 180 .f ) - (double )2 .5f ;
9+ this ->prior_distance = prior_distance * -2 . ;
10+ this ->prior_yaw = (1 .84f *(prior_yaw + 90 .f )/180 .f ) - (double )3 .14f ;
11+
12+ std::cout << " PRIORS CALCULATED: \n PITCH: " <<this ->prior_pitch << " YAW: " << this ->prior_yaw << " DISTANCE: " << this ->prior_distance ;
13+
714 mat3dcontour = (cv::Mat_<double >(18 , 3 ) <<
815 0.45517698 , 0.30089578 , -0.76442945 ,
916 0.44899884 , 0.16699584 , -0.765143 ,
@@ -104,13 +111,6 @@ PositionSolver::PositionSolver(int width, int height):
104111 0 , 0 , 1
105112 );
106113
107- /* camera_matrix = (cv::Mat_<double>(3, 3) <<
108- 34.32, 0, 307.3,
109- 0, 34.17, 251.4,
110- 0, 0, 1
111- );*/
112-
113-
114114 camera_distortion = (cv::Mat_<double >(4 , 1 ) << 0 , 0 , 0 , 0 );
115115}
116116
@@ -125,7 +125,7 @@ void PositionSolver::solve_rotation(FaceData* face_data)
125125 );
126126 }
127127
128- std::vector<double > rv ({ - 2 , - 2 , 0 }), tv ({0 ,0 ,- 3 });
128+ std::vector<double > rv ({ prior_pitch, prior_yaw , 0 }), tv ({0 , 0 , prior_distance });
129129 cv::Mat rvec (rv), tvec (tv);
130130
131131 cv::Mat ip (landmarkPoints);
@@ -142,15 +142,11 @@ void PositionSolver::solve_rotation(FaceData* face_data)
142142
143143 // std::cout << rvec << std::endl;
144144
145-
146- // double rot[9] = { 0 };
147145 cv::Mat rotM (3 , 3 , CV_64FC1);
148146 cv::Rodrigues (rvec, rotM);
149147
150148
151149 cv::Mat concated (3 , 4 , CV_64FC1);
152- // cv::Mat transs(3, 1, CV_64FC1);
153- // cv::hconcat(rotM, transs, concated);
154150 cv::hconcat (rotM, tvec, concated);
155151
156152
@@ -166,12 +162,12 @@ void PositionSolver::solve_rotation(FaceData* face_data)
166162 );
167163
168164
169- double d = 6 ;
165+ // double d = 6;
170166
171167 // Correct relative translation
172- double yaw_angle = rvec.at <double >(1 ,0 );
173- double y_trans = tvec.at <double >(1 , 0 ); // Bien
174- double correction = y_trans <= 0 ? yaw_angle + std::atan (y_trans / d) : yaw_angle - std::atan (y_trans / d);
168+ // double yaw_angle = rvec.at<double>(1,0);
169+ // double y_trans = tvec.at<double>(1, 0); //Bien
170+ // double correction = y_trans <= 0 ? yaw_angle + std::atan(y_trans / d) : yaw_angle - std::atan(y_trans / d);
175171
176172
177173 for (int i = 0 ; i < 3 ; i++)
0 commit comments