Merge pull request #7 from AIRLegend/dev

Dev

Author: AIRLegend

AITracker/AITracker.vcxproj

@@ -189,6 +189,7 @@
       <ConformanceMode>true</ConformanceMode>
       <AdditionalIncludeDirectories>$(SolutionDir)Dependencies\OpenCV\include\;$(SolutionDir)Dependencies\onnxruntime\include\;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories>
       <RuntimeLibrary>MultiThreadedDLL</RuntimeLibrary>
+      <Optimization>Disabled</Optimization>
     </ClCompile>
     <Link>
       <SubSystem>Console</SubSystem>

AITracker/src/PositionSolver.cpp

@@ -1,20 +1,27 @@
 #include "PositionSolver.h"
 
 
-PositionSolver::PositionSolver(int width, int height, 
-    float prior_pitch, float prior_yaw, float prior_distance):
-    contour_indices{ 0,1,8,15,16,27,28,29,30,31,32,33,34,35,36,39,42,45 }
+PositionSolver::PositionSolver(int width, int height,
+    float prior_pitch, float prior_yaw, float prior_distance) :
+    contour_indices{ 0,1,8,15,16,27,28,29,30,31,32,33,34,35,36,39,42,45 },
+    landmark_points_buffer(18, 1, CV_32FC2),
+    rv({ 0, 0, 0 }),
+    tv({ 0, 0, 0 })
 {
     this->prior_pitch = (1.1f * (prior_pitch + 90.f) / 180.f) - (double)2.5f;
-    this->prior_distance = prior_distance * -2. ;
-    this->prior_yaw = (1.84f*(prior_yaw + 90.f)/180.f) - (double)3.14f;
+    this->prior_distance = prior_distance * -2.;
+    this->prior_yaw = (1.84f * (prior_yaw + 90.f) / 180.f) - (double)3.14f;
+
+    this->rv[0] = this->prior_pitch;
+    this->rv[1] = this->prior_yaw;
+    this->tv[2] = this->prior_distance;
 
     //std::cout << "PRIORS CALCULATED: \nPITCH: " <<this->prior_pitch << "  YAW: " << this->prior_yaw << "  DISTANCE: " << this->prior_distance;
 
     mat3dcontour = (cv::Mat_<double>(18, 3) <<
         0.45517698, -0.30089578, 0.76442945,
         0.44899884, -0.16699584, 0.765143,
-        0.,          0.621079,   0.28729478,
+        0., 0.621079, 0.28729478,
         -0.44899884, -0.16699584, 0.765143,
         -0.45517698, -0.30089578, 0.76442945,
         0., -0.2933326, 0.1375821,
@@ -32,124 +39,39 @@ PositionSolver::PositionSolver(int width, int height,
         -0.31590518, -0.2983375, 0.2851074
         );
 
-    /*mat3dface = (cv::Mat_<double>(70, 3) <<
-        0.4551769692672, 0.300895790030204, -0.764429433974752,
-        0.448998827123556, 0.166995837790733, -0.765143004071253,
-        0.437431554952677, 0.022655479179981, -0.739267175112735,
-        0.415033422928434, -0.088941454648772, -0.747947437846473,
-        0.389123587370091, -0.232380029794684, -0.704788385327458,
-        0.334630113904382, -0.361265387599081, -0.615587579236862,
-        0.263725112132858, -0.460009725616771, -0.491479221041573,
-        0.16241621322721, -0.558037146073869, -0.339445180872282,
-        0., -0.621079019321682, -0.287294770748887,
-        -0.16241621322721, -0.558037146073869, -0.339445180872282,
-        -0.263725112132858, -0.460009725616771, -0.491479221041573,
-        -0.334630113904382, -0.361265387599081, -0.615587579236862,
-        -0.389123587370091, -0.232380029794684, -0.704788385327458,
-        -0.415033422928434, -0.088941454648772, -0.747947437846473,
-        -0.437431554952677, 0.022655479179981, -0.739267175112735,
-        -0.448998827123556, 0.166995837790733, -0.765143004071253,
-        -0.4551769692672, 0.300895790030204, -0.764429433974752,
-        0.385529968662985, 0.402800553948697, -0.310031082540741,
-        0.322196658344302, 0.464439136821772, -0.250558059367669,
-        0.25409760441282, 0.46420381416882, -0.208177722146526,
-        0.186875436782135, 0.44706071961879, -0.145299823706503,
-        0.120880983543622, 0.423566314072968, -0.110757158774771,
-        -0.120880983543622, 0.423566314072968, -0.110757158774771,
-        -0.186875436782135, 0.44706071961879, -0.145299823706503,
-        -0.25409760441282, 0.46420381416882, -0.208177722146526,
-        -0.322196658344302, 0.464439136821772, -0.250558059367669,
-        -0.385529968662985, 0.402800553948697, -0.310031082540741,
-        0., 0.293332603215811, -0.137582088779393,
-        0., 0.194828701837823, -0.069158109325951,
-        0., 0.103844017393155, -0.009151819844964,
-        0., 0., 0.,
-        0.080626352317973, -0.041276068128093, -0.134161035564826,
-        0.046439347377934, -0.057675223874769, -0.102990627164664,
-        0., -0.068753126205604, -0.090545348482397,
-        -0.046439347377934, -0.057675223874769, -0.102990627164664,
-        -0.080626352317973, -0.041276068128093, -0.134161035564826,
-        0.315905195966084, 0.298337502555443, -0.285107407636464,
-        0.275252345439353, 0.312721904921771, -0.244558251170671,
-        0.176394511553111, 0.311907184376107, -0.219205360345231,
-        0.131229723798772, 0.284447361805627, -0.234239149487417,
-        0.184124948330084, 0.260179585304867, -0.226590776513707,
-        0.279433549294448, 0.267363071770222, -0.248441437111633,
-        -0.131229723798772, 0.284447361805627, -0.234239149487417,
-        -0.176394511553111, 0.311907184376107, -0.219205360345231,
-        -0.275252345439353, 0.312721904921771, -0.244558251170671,
-        -0.315905195966084, 0.298337502555443, -0.285107407636464,
-        -0.279433549294448, 0.267363071770222, -0.248441437111633,
-        -0.184124948330084, 0.260179585304867, -0.226590776513707,
-        0.121155252430729, -0.208988660580347, -0.160606287940521,
-        0.041356305910044, -0.194484199722098, -0.096159882202821,
-        0., -0.205180167345702, -0.083299217789729,
-        -0.041356305910044, -0.194484199722098, -0.096159882202821,
-        -0.121155252430729, -0.208988660580347, -0.160606287940521,
-        -0.132325402795928, -0.290857984604968, -0.187067868218105,
-        -0.064137791831655, -0.325377847425684, -0.158924039726607,
-        0., -0.343742581679188, -0.113925986025684,
-        0.064137791831655, -0.325377847425684, -0.158924039726607,
-        0.132325402795928, -0.290857984604968, -0.187067868218105,
-        0.181481567104525, -0.243239316141725, -0.231284988892766,
-        0.083999507750469, -0.239717753728704, -0.155256465640701,
-        0., -0.256058040176369, -0.0950619498899,
-        -0.083999507750469, -0.239717753728704, -0.155256465640701,
-        -0.181481567104525, -0.243239316141725, -0.231284988892766,
-        -0.074036069749345, -0.250689938345682, -0.177346470406188,
-        0., -0.264945854681568, -0.112349967428413,
-        0.074036069749345, -0.250689938345682, -0.177346470406188,
-        0.257990002632141, 0.276080012321472, -0.219998998939991,
-        -0.257990002632141, 0.276080012321472, -0.219998998939991,
-        0.257990002632141, 0.276080012321472, -0.324570998549461,
-        -0.257990002632141, 0.276080012321472, -0.324570998549461
-        );*/
-
     camera_matrix = (cv::Mat_<double>(3, 3) <<
         height, 0, height / 2,
         0, height, width / 2,
         0, 0, 1
-    );
+        );
 
     camera_distortion = (cv::Mat_<double>(4, 1) << 0, 0, 0, 0);
 }
 
 void PositionSolver::solve_rotation(FaceData* face_data)
 {
-    std::vector<cv::Point2f> landmarkPoints;
-
-    for (int i : this->contour_indices)
+    int contour_idx = 0;
+    for (int j = 0; j < 2; j++)
     {
-        landmarkPoints.push_back(
-            cv::Point(face_data->landmark_coords[2 * i], face_data->landmark_coords[2 * i + 1])
-        );
-    }
+        for (int i = 0; i < NB_CONTOUR_POINTS; i++)
+        {
+            contour_idx = contour_indices[i];
+            landmark_points_buffer.at<float>(i, j) = (int)face_data->landmark_coords[2 * contour_idx + j];
 
-    std::vector<double> rv({prior_pitch, prior_yaw,0 }), tv({ 0, 0, prior_distance });
-    cv::Mat rvec(rv), tvec(tv);
+        }
+    }
 
-    cv::Mat ip(landmarkPoints);
+    cv::Mat rvec(rv, true), tvec(tv, true);
 
-    
     solvePnP(mat3dcontour,
-        ip, 
-        this->camera_matrix, 
-        this->camera_distortion, 
-        rvec, 
-        tvec, 
-        true, //extrinsic guess
-        cv::SOLVEPNP_ITERATIVE
-        );
-    
-    /*cv::solvePnPRefineLM(
-        mat3dcontour,
-        ip,
+        landmark_points_buffer,
         this->camera_matrix,
         this->camera_distortion,
         rvec,
-        tvec
-    );*/
-
+        tvec,
+        true, //extrinsic guess
+        cv::SOLVEPNP_ITERATIVE
+    );
 
     get_euler(rvec, tvec);
 
@@ -160,28 +82,43 @@ void PositionSolver::solve_rotation(FaceData* face_data)
         face_data->translation[i] = tvec.at<double>(i, 0);
     }
 
- }
+}
+
+void PositionSolver::set_prior_pitch(float new_pitch)
+{
+    this->prior_pitch = new_pitch;
+    this->rv[0] = this->prior_pitch;
+}
+
+void PositionSolver::set_prior_yaw(float new_yaw)
+{
+    this->prior_yaw = new_yaw;
+    this->rv[1] = this->prior_yaw;
+}
+
+void PositionSolver::set_prior_distance(float new_distance)
+{
+    this->prior_distance = new_distance;
+    this->tv[2] = this->prior_distance;
+}
 
 
 void PositionSolver::get_euler(cv::Mat& rvec, cv::Mat& tvec)
 {
     cv::Mat rotM(3, 3, CV_64FC1);
     cv::Rodrigues(rvec, rotM);
 
-   
-
     cv::Mat concated(3, 4, CV_64FC1);
     cv::hconcat(rotM, tvec, concated);
 
-
     cv::decomposeProjectionMatrix(
         concated,
         cv::Mat(3, 3, CV_64FC1),
         cv::Mat(3, 3, CV_64FC1),
         cv::Mat(4, 1, CV_64FC1),
-        cv::Mat(3, 3, CV_64FC1),
-        cv::Mat(3, 3, CV_64FC1),
-        cv::Mat(3, 3, CV_64FC1),
+        cv::noArray(),
+        cv::noArray(),
+        cv::noArray(),
         rvec
     );
 

AITracker/src/PositionSolver.h

@@ -4,7 +4,6 @@
 #include "opencv.hpp"
 #include "data.h"
 
-
 /**
 	This class is responsible of mapping 2D facial landmarks to its
 	corresponding 3D world coordinates for pose estimation (Position and rotation).
@@ -14,7 +13,7 @@ class PositionSolver
 
 public:
 
-	double prior_pitch, prior_yaw, prior_distance;
+	double prior_pitch = -2.f, prior_yaw = -2.f, prior_distance = -1.f;
 
 	PositionSolver(
 		int im_width, 
@@ -28,16 +27,27 @@ class PositionSolver
 		@param face_data FaceData instance with 2D landmark coordinates detected
 	*/
 	void solve_rotation(FaceData* face_data);
+	void set_prior_pitch(float new_pitch);
+	void set_prior_yaw(float new_yaw);
+	void set_prior_distance(float new_distance);
 
 private:
+	static const int NB_CONTOUR_POINTS = 18;
+
 	cv::Mat mat3dface;
 	cv::Mat mat3dcontour;
-	int contour_indices[18];
+	int contour_indices[NB_CONTOUR_POINTS];  // Facial landmarks that interest us
+
+	//Buffer so we dont have to allocate a list on every solve_rotation call.
+	cv::Mat landmark_points_buffer;
+	//Prior rotations and translation as a vector so we dont have to allocate them.
+	std::vector<double> rv, tv;  
 
 	int width, height;
 
 	cv::Mat camera_matrix, camera_distortion;
 
+
 	/**
 		Gets euler angles from rotation matrix.
 	*/

AITracker/src/imageprocessor.cpp

@@ -17,7 +17,7 @@ void ImageProcessor::normalize(cv::Mat& image)
 }
 
 
-void ImageProcessor::cvt_format(float* from, float* dest, int dim)
+/*void ImageProcessor::cvt_format(float* from, float* dest, int dim_x, int dim_y)
 {
     for (int channel = 1; channel < 4; channel++)
     {
@@ -31,7 +31,7 @@ void ImageProcessor::cvt_format(float* from, float* dest, int dim)
     }
 }
 
-void ImageProcessor::transpose(float* from, float* dest, int dim)
+void ImageProcessor::transpose(float* from, float* dest, int dim_x, int dim_y)
 {
     int stride = 224 * 224;
 
@@ -42,6 +42,34 @@ void ImageProcessor::transpose(float* from, float* dest, int dim)
             dest[i + stride*c] = from[c + i*3];
         }
     }
+}*/
+
+
+void ImageProcessor::cvt_format(float* from, float* dest, int dim_x, int dim_y)
+{
+    for (int channel = 1; channel < 4; channel++)
+    {
+        for (int row = 0; row < dim_x; row++)
+        {
+            for (int col = 0; col < dim_y; col++)
+            {
+                dest[((channel - 1) * dim_x * dim_y) + (dim_y * col + row)] = from[3 * (dim_y * col + row)];
+            }
+        }
+    }
+}
+
+void ImageProcessor::transpose(float* from, float* dest, int dim_x, int dim_y)
+{
+    int stride = dim_x * dim_y;
+
+    for (int c = 0; c < 3; c++)
+    {
+        for (int i = 0; i < dim_x * dim_y; i++)
+        {
+            dest[i + stride*c] = from[c + i*3];
+        }
+    }
 }
 
 

AITracker/src/imageprocessor.h

@@ -7,8 +7,8 @@ class ImageProcessor
 public:
 	ImageProcessor();
 	void normalize(cv::Mat& image);
-	void cvt_format(float* from, float* dest, int dim = 224);
-	void transpose(float* from, float* dest, int dim = 224);
+	void cvt_format(float* from, float* dest, int dim_x = 224, int dim_y=224);
+	void transpose(float* from, float* dest, int dim_x = 224, int dim_y=224);
 
 private:
 	cv::Scalar mean_scaling, std_scaling;