diff --git a/exe/FaceLandmarkImg/FaceLandmarkImg.cpp b/exe/FaceLandmarkImg/FaceLandmarkImg.cpp index 718d3eca..233fafaa 100644 --- a/exe/FaceLandmarkImg/FaceLandmarkImg.cpp +++ b/exe/FaceLandmarkImg/FaceLandmarkImg.cpp @@ -466,7 +466,7 @@ int main (int argc, char **argv) bool success = LandmarkDetector::DetectLandmarksInImage(grayscale_image, depth_image, face_detections[face], clnf_model, det_parameters); // Estimate head pose and eye gaze - cv::Vec6d headPose = LandmarkDetector::GetCorrectedPoseWorld(clnf_model, fx, fy, cx, cy); + cv::Vec6d headPose = LandmarkDetector::GetPose(clnf_model, fx, fy, cx, cy); // Gaze tracking, absolute gaze direction cv::Point3f gazeDirection0(0, 0, -1); @@ -521,7 +521,7 @@ int main (int argc, char **argv) if (det_parameters.track_gaze) { - cv::Vec6d pose_estimate_to_draw = LandmarkDetector::GetCorrectedPoseWorld(clnf_model, fx, fy, cx, cy); + cv::Vec6d pose_estimate_to_draw = LandmarkDetector::GetPose(clnf_model, fx, fy, cx, cy); // Draw it in reddish if uncertain, blueish if certain LandmarkDetector::DrawBox(read_image, pose_estimate_to_draw, cv::Scalar(255.0, 0, 0), 3, fx, fy, cx, cy); @@ -582,7 +582,7 @@ int main (int argc, char **argv) LandmarkDetector::DetectLandmarksInImage(grayscale_image, bounding_boxes[i], clnf_model, det_parameters); // Estimate head pose and eye gaze - cv::Vec6d headPose = LandmarkDetector::GetCorrectedPoseWorld(clnf_model, fx, fy, cx, cy); + cv::Vec6d headPose = LandmarkDetector::GetPose(clnf_model, fx, fy, cx, cy); // Gaze tracking, absolute gaze direction cv::Point3f gazeDirection0(0, 0, -1); @@ -615,7 +615,7 @@ int main (int argc, char **argv) if (det_parameters.track_gaze) { - cv::Vec6d pose_estimate_to_draw = LandmarkDetector::GetCorrectedPoseWorld(clnf_model, fx, fy, cx, cy); + cv::Vec6d pose_estimate_to_draw = LandmarkDetector::GetPose(clnf_model, fx, fy, cx, cy); // Draw it in reddish if uncertain, blueish if certain LandmarkDetector::DrawBox(read_image, pose_estimate_to_draw, cv::Scalar(255.0, 0, 0), 3, fx, fy, cx, cy); diff --git a/exe/FaceLandmarkVid/FaceLandmarkVid.cpp b/exe/FaceLandmarkVid/FaceLandmarkVid.cpp index 5846e533..b945808f 100644 --- a/exe/FaceLandmarkVid/FaceLandmarkVid.cpp +++ b/exe/FaceLandmarkVid/FaceLandmarkVid.cpp @@ -136,7 +136,7 @@ void visualise_tracking(cv::Mat& captured_image, cv::Mat_& depth_image, c // A rough heuristic for box around the face width int thickness = (int)std::ceil(2.0* ((double)captured_image.cols) / 640.0); - cv::Vec6d pose_estimate_to_draw = LandmarkDetector::GetCorrectedPoseWorld(face_model, fx, fy, cx, cy); + cv::Vec6d pose_estimate_to_draw = LandmarkDetector::GetPose(face_model, fx, fy, cx, cy); // Draw it in reddish if uncertain, blueish if certain LandmarkDetector::DrawBox(captured_image, pose_estimate_to_draw, cv::Scalar((1 - vis_certainty)*255.0, 0, vis_certainty * 255), thickness, fx, fy, cx, cy); @@ -192,9 +192,8 @@ int main (int argc, char **argv) // Get the input output file parameters // Indicates that rotation should be with respect to world or camera coordinates - bool u; string output_codec; - LandmarkDetector::get_video_input_output_params(files, depth_directories, out_dummy, output_video_files, u, output_codec, arguments); + LandmarkDetector::get_video_input_output_params(files, depth_directories, out_dummy, output_video_files, output_codec, arguments); // The modules that are being used for tracking LandmarkDetector::CLNF clnf_model(det_parameters.model_location); diff --git a/exe/FaceLandmarkVidMulti/FaceLandmarkVidMulti.cpp b/exe/FaceLandmarkVidMulti/FaceLandmarkVidMulti.cpp index 66b231e1..3f8ef907 100644 --- a/exe/FaceLandmarkVidMulti/FaceLandmarkVidMulti.cpp +++ b/exe/FaceLandmarkVidMulti/FaceLandmarkVidMulti.cpp @@ -150,9 +150,8 @@ int main (int argc, char **argv) det_parameters.push_back(det_params); // Get the input output file parameters - bool u; string output_codec; - LandmarkDetector::get_video_input_output_params(files, depth_directories, dummy_out, tracked_videos_output, u, output_codec, arguments); + LandmarkDetector::get_video_input_output_params(files, depth_directories, dummy_out, tracked_videos_output, output_codec, arguments); // Get camera parameters LandmarkDetector::get_camera_params(device, fx, fy, cx, cy, arguments); @@ -404,7 +403,7 @@ int main (int argc, char **argv) int thickness = (int)std::ceil(2.0* ((double)captured_image.cols) / 640.0); // Work out the pose of the head from the tracked model - cv::Vec6d pose_estimate = LandmarkDetector::GetCorrectedPoseWorld(clnf_models[model], fx, fy, cx, cy); + cv::Vec6d pose_estimate = LandmarkDetector::GetPose(clnf_models[model], fx, fy, cx, cy); // Draw it in reddish if uncertain, blueish if certain LandmarkDetector::DrawBox(disp_image, pose_estimate, cv::Scalar((1-detection_certainty)*255.0,0, detection_certainty*255), thickness, fx, fy, cx, cy); diff --git a/exe/FeatureExtraction/FeatureExtraction.cpp b/exe/FeatureExtraction/FeatureExtraction.cpp index 8f80ccdd..df466594 100644 --- a/exe/FeatureExtraction/FeatureExtraction.cpp +++ b/exe/FeatureExtraction/FeatureExtraction.cpp @@ -190,7 +190,7 @@ void visualise_tracking(cv::Mat& captured_image, const LandmarkDetector::CLNF& f // A rough heuristic for box around the face width int thickness = (int)std::ceil(2.0* ((double)captured_image.cols) / 640.0); - cv::Vec6d pose_estimate_to_draw = LandmarkDetector::GetCorrectedPoseWorld(face_model, fx, fy, cx, cy); + cv::Vec6d pose_estimate_to_draw = LandmarkDetector::GetPose(face_model, fx, fy, cx, cy); // Draw it in reddish if uncertain, blueish if certain LandmarkDetector::DrawBox(captured_image, pose_estimate_to_draw, cv::Scalar((1 - vis_certainty)*255.0, 0, vis_certainty * 255), thickness, fx, fy, cx, cy); @@ -252,9 +252,8 @@ int main (int argc, char **argv) // Get the input output file parameters // Indicates that rotation should be with respect to camera or world coordinates - bool use_world_coordinates; string output_codec; //not used but should - LandmarkDetector::get_video_input_output_params(input_files, depth_directories, output_files, tracked_videos_output, use_world_coordinates, output_codec, arguments); + LandmarkDetector::get_video_input_output_params(input_files, depth_directories, output_files, tracked_videos_output, output_codec, arguments); bool video_input = true; bool verbose = true; @@ -596,15 +595,7 @@ int main (int argc, char **argv) } // Work out the pose of the head from the tracked model - cv::Vec6d pose_estimate; - if(use_world_coordinates) - { - pose_estimate = LandmarkDetector::GetCorrectedPoseWorld(face_model, fx, fy, cx, cy); - } - else - { - pose_estimate = LandmarkDetector::GetCorrectedPoseCamera(face_model, fx, fy, cx, cy); - } + cv::Vec6d pose_estimate = LandmarkDetector::GetPose(face_model, fx, fy, cx, cy); if(hog_output_file.is_open()) { diff --git a/lib/local/CppInerop/LandmarkDetectorInterop.h b/lib/local/CppInerop/LandmarkDetectorInterop.h index c3cb895e..d6eea8d9 100644 --- a/lib/local/CppInerop/LandmarkDetectorInterop.h +++ b/lib/local/CppInerop/LandmarkDetectorInterop.h @@ -263,7 +263,7 @@ namespace CppInterop { } void GetCorrectedPoseCamera(System::Collections::Generic::List^ pose, double fx, double fy, double cx, double cy) { - auto pose_vec = ::LandmarkDetector::GetCorrectedPoseCamera(*clnf, fx, fy, cx, cy); + auto pose_vec = ::LandmarkDetector::GetPoseWRTCamera(*clnf, fx, fy, cx, cy); pose->Clear(); for(int i = 0; i < 6; ++i) { @@ -272,7 +272,7 @@ namespace CppInterop { } void GetCorrectedPoseWorld(System::Collections::Generic::List^ pose, double fx, double fy, double cx, double cy) { - auto pose_vec = ::LandmarkDetector::GetCorrectedPoseWorld(*clnf, fx, fy, cx, cy); + auto pose_vec = ::LandmarkDetector::GetPose(*clnf, fx, fy, cx, cy); pose->Clear(); for(int i = 0; i < 6; ++i) { @@ -322,7 +322,7 @@ namespace CppInterop { System::Collections::Generic::List^>^ CalculateBox(float fx, float fy, float cx, float cy) { - cv::Vec6d pose = ::LandmarkDetector::GetCorrectedPoseWorld(*clnf, fx,fy, cx, cy); + cv::Vec6d pose = ::LandmarkDetector::GetPose(*clnf, fx,fy, cx, cy); vector> vecLines = ::LandmarkDetector::CalculateBox(pose, fx, fy, cx, cy); diff --git a/lib/local/FaceAnalyser/src/GazeEstimation.cpp b/lib/local/FaceAnalyser/src/GazeEstimation.cpp index 58b03fb1..962154ea 100644 --- a/lib/local/FaceAnalyser/src/GazeEstimation.cpp +++ b/lib/local/FaceAnalyser/src/GazeEstimation.cpp @@ -112,7 +112,7 @@ cv::Point3f FaceAnalysis::GetPupilPosition(cv::Mat_ eyeLdmks3d){ void FaceAnalysis::EstimateGaze(const LandmarkDetector::CLNF& clnf_model, cv::Point3f& gaze_absolute, float fx, float fy, float cx, float cy, bool left_eye) { - cv::Vec6d headPose = LandmarkDetector::GetPoseCamera(clnf_model, fx, fy, cx, cy); + cv::Vec6d headPose = LandmarkDetector::GetPose(clnf_model, fx, fy, cx, cy); cv::Vec3d eulerAngles(headPose(3), headPose(4), headPose(5)); cv::Matx33d rotMat = LandmarkDetector::Euler2RotationMatrix(eulerAngles); diff --git a/lib/local/LandmarkDetector/include/LandmarkDetectorFunc.h b/lib/local/LandmarkDetector/include/LandmarkDetectorFunc.h index c5833f3a..bc93c734 100644 --- a/lib/local/LandmarkDetector/include/LandmarkDetectorFunc.h +++ b/lib/local/LandmarkDetector/include/LandmarkDetectorFunc.h @@ -100,20 +100,12 @@ namespace LandmarkDetector //================================================================ // Helper function for getting head pose from CLNF parameters - // Return the current estimate of the head pose, this can be either in camera or world coordinate space + // Return the current estimate of the head pose in world coordinates with camera at origin (0,0,0) // The format returned is [Tx, Ty, Tz, Eul_x, Eul_y, Eul_z] - cv::Vec6d GetPoseCamera(const CLNF& clnf_model, double fx, double fy, double cx, double cy); - cv::Vec6d GetPoseWorld(const CLNF& clnf_model, double fx, double fy, double cx, double cy); - - // Getting a head pose estimate from the currently detected landmarks, with appropriate correction for perspective - // This is because rotation estimate under orthographic assumption is only correct close to the centre of the image - // These methods attempt to correct for that - // The pose returned can be either in camera or world coordinates + cv::Vec6d GetPose(const CLNF& clnf_model, double fx, double fy, double cx, double cy); + + // Return the current estimate of the head pose in world coordinates with camera at origin (0,0,0), but with rotation representing if the head is looking at the camera // The format returned is [Tx, Ty, Tz, Eul_x, Eul_y, Eul_z] - cv::Vec6d GetCorrectedPoseCamera(const CLNF& clnf_model, double fx, double fy, double cx, double cy); - cv::Vec6d GetCorrectedPoseWorld(const CLNF& clnf_model, double fx, double fy, double cx, double cy); - - //=========================================================================== - + cv::Vec6d GetPoseWRTCamera(const CLNF& clnf_model, double fx, double fy, double cx, double cy); } #endif diff --git a/lib/local/LandmarkDetector/include/LandmarkDetectorUtils.h b/lib/local/LandmarkDetector/include/LandmarkDetectorUtils.h index 450c1e12..daa16a32 100644 --- a/lib/local/LandmarkDetector/include/LandmarkDetectorUtils.h +++ b/lib/local/LandmarkDetector/include/LandmarkDetectorUtils.h @@ -77,7 +77,7 @@ namespace LandmarkDetector // Helper functions for parsing the inputs //============================================================================================= void get_video_input_output_params(vector &input_video_file, vector &depth_dir, vector &output_files, - vector &output_video_files, bool& world_coordinates_pose, string &output_codec, vector &arguments); + vector &output_video_files, string &output_codec, vector &arguments); void get_camera_params(int &device, float &fx, float &fy, float &cx, float &cy, vector &arguments); diff --git a/lib/local/LandmarkDetector/src/LandmarkDetectorFunc.cpp b/lib/local/LandmarkDetector/src/LandmarkDetectorFunc.cpp index 0bd820db..e44f6688 100644 --- a/lib/local/LandmarkDetector/src/LandmarkDetectorFunc.cpp +++ b/lib/local/LandmarkDetector/src/LandmarkDetectorFunc.cpp @@ -70,63 +70,11 @@ using namespace LandmarkDetector; -// Getting a head pose estimate from the currently detected landmarks (rotation with respect to point camera) +// Getting a head pose estimate from the currently detected landmarks, with appropriate correction due to the PDM assuming an orthographic camera +// which is only correct close to the centre of the image +// This method returns a corrected pose estimate with respect to world coordinates with camera at origin (0,0,0) // The format returned is [Tx, Ty, Tz, Eul_x, Eul_y, Eul_z] -cv::Vec6d LandmarkDetector::GetPoseCamera(const CLNF& clnf_model, double fx, double fy, double cx, double cy) -{ - if(!clnf_model.detected_landmarks.empty() && clnf_model.params_global[0] != 0) - { - double Z = fx / clnf_model.params_global[0]; - - double X = ((clnf_model.params_global[4] - cx) * (1.0/fx)) * Z; - double Y = ((clnf_model.params_global[5] - cy) * (1.0/fy)) * Z; - - return cv::Vec6d(X, Y, Z, clnf_model.params_global[1], clnf_model.params_global[2], clnf_model.params_global[3]); - } - else - { - return cv::Vec6d(0,0,0,0,0,0); - } -} - -// Getting a head pose estimate from the currently detected landmarks (rotation in world coordinates) -// The format returned is [Tx, Ty, Tz, Eul_x, Eul_y, Eul_z] -cv::Vec6d LandmarkDetector::GetPoseWorld(const CLNF& clnf_model, double fx, double fy, double cx, double cy) -{ - if(!clnf_model.detected_landmarks.empty() && clnf_model.params_global[0] != 0) - { - double Z = fx / clnf_model.params_global[0]; - - double X = ((clnf_model.params_global[4] - cx) * (1.0/fx)) * Z; - double Y = ((clnf_model.params_global[5] - cy) * (1.0/fy)) * Z; - - // Here we correct for the camera orientation, for this need to determine the angle the camera makes with the head pose - double z_x = cv::sqrt(X * X + Z * Z); - double eul_x = atan2(Y, z_x); - - double z_y = cv::sqrt(Y * Y + Z * Z); - double eul_y = -atan2(X, z_y); - - cv::Matx33d camera_rotation = LandmarkDetector::Euler2RotationMatrix(cv::Vec3d(eul_x, eul_y, 0)); - cv::Matx33d head_rotation = LandmarkDetector::AxisAngle2RotationMatrix(cv::Vec3d(clnf_model.params_global[1], clnf_model.params_global[2], clnf_model.params_global[3])); - - cv::Matx33d corrected_rotation = camera_rotation.t() * head_rotation; - - cv::Vec3d euler_corrected = LandmarkDetector::RotationMatrix2Euler(corrected_rotation); - - return cv::Vec6d(X, Y, Z, euler_corrected[0], euler_corrected[1], euler_corrected[2]); - } - else - { - return cv::Vec6d(0,0,0,0,0,0); - } -} - -// Getting a head pose estimate from the currently detected landmarks, with appropriate correction due to orthographic camera issue -// This is because rotation estimate under orthographic assumption is only correct close to the centre of the image -// This method returns a corrected pose estimate with respect to world coordinates (Experimental) -// The format returned is [Tx, Ty, Tz, Eul_x, Eul_y, Eul_z] -cv::Vec6d LandmarkDetector::GetCorrectedPoseWorld(const CLNF& clnf_model, double fx, double fy, double cx, double cy) +cv::Vec6d LandmarkDetector::GetPose(const CLNF& clnf_model, double fx, double fy, double cx, double cy) { if(!clnf_model.detected_landmarks.empty() && clnf_model.params_global[0] != 0) { @@ -161,7 +109,7 @@ cv::Vec6d LandmarkDetector::GetCorrectedPoseWorld(const CLNF& clnf_model, double cv::Vec3d euler = LandmarkDetector::AxisAngle2Euler(vec_rot); - return cv::Vec6d(vec_trans[0], vec_trans[1], vec_trans[2], vec_rot[0], vec_rot[1], vec_rot[2]); + return cv::Vec6d(vec_trans[0], vec_trans[1], vec_trans[2], euler[0], euler[1], euler[2]); } else { @@ -170,9 +118,9 @@ cv::Vec6d LandmarkDetector::GetCorrectedPoseWorld(const CLNF& clnf_model, double } // Getting a head pose estimate from the currently detected landmarks, with appropriate correction due to perspective projection -// This method returns a corrected pose estimate with respect to a point camera (NOTE not the world coordinates) (Experimental) +// This method returns a corrected pose estimate with respect to a point camera (NOTE not the world coordinates), which is useful to find out if the person is looking at a camera // The format returned is [Tx, Ty, Tz, Eul_x, Eul_y, Eul_z] -cv::Vec6d LandmarkDetector::GetCorrectedPoseCamera(const CLNF& clnf_model, double fx, double fy, double cx, double cy) +cv::Vec6d LandmarkDetector::GetPoseWRTCamera(const CLNF& clnf_model, double fx, double fy, double cx, double cy) { if(!clnf_model.detected_landmarks.empty() && clnf_model.params_global[0] != 0) { diff --git a/lib/local/LandmarkDetector/src/LandmarkDetectorUtils.cpp b/lib/local/LandmarkDetector/src/LandmarkDetectorUtils.cpp index 60a2d65c..413d5891 100644 --- a/lib/local/LandmarkDetector/src/LandmarkDetectorUtils.cpp +++ b/lib/local/LandmarkDetector/src/LandmarkDetectorUtils.cpp @@ -121,7 +121,7 @@ void create_directories(string output_path) // Extracting the following command line arguments -f, -fd, -op, -of, -ov (and possible ordered repetitions) void get_video_input_output_params(vector &input_video_files, vector &depth_dirs, vector &output_files, - vector &output_video_files, bool& world_coordinates_pose, string& output_codec, vector &arguments) + vector &output_video_files, string& output_codec, vector &arguments) { bool* valid = new bool[arguments.size()]; @@ -130,9 +130,6 @@ void get_video_input_output_params(vector &input_video_files, vector &input_video_files, vector