127 lines
4.9 KiB
C++
127 lines
4.9 KiB
C++
/*###############################################################################
|
|
#
|
|
# Copyright 2020 NVIDIA Corporation
|
|
#
|
|
# Permission is hereby granted, free of charge, to any person obtaining a copy of
|
|
# this software and associated documentation files (the "Software"), to deal in
|
|
# the Software without restriction, including without limitation the rights to
|
|
# use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
|
|
# the Software, and to permit persons to whom the Software is furnished to do so,
|
|
# subject to the following conditions:
|
|
#
|
|
# The above copyright notice and this permission notice shall be included in all
|
|
# copies or substantial portions of the Software.
|
|
#
|
|
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
|
|
# FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
|
|
# COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
|
|
# IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
|
|
# CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
|
#
|
|
###############################################################################*/
|
|
#include "RenderingUtils.h"
|
|
#include "nvAR_defs.h"
|
|
#include "glm/gtc/quaternion.hpp"
|
|
#include "glm/gtx/transform.hpp"
|
|
|
|
|
|
glm::mat4x4 get_modelview(const NvAR_RenderingParams& rp) {
|
|
glm::quat rotation(rp.rotation.w, rp.rotation.x, rp.rotation.y, rp.rotation.z);
|
|
glm::mat4x4 modelview = glm::mat4_cast(rotation);
|
|
modelview[3][0] = rp.translation.vec[0];
|
|
modelview[3][1] = rp.translation.vec[1];
|
|
return modelview;
|
|
};
|
|
|
|
|
|
void draw_wireframe(const cv::Mat& image, const NvAR_FaceMesh& mesh, const NvAR_RenderingParams &rp, cv::Scalar color) {
|
|
glm::mat4x4 modelview = get_modelview(rp);
|
|
glm::mat4x4 projection = get_projection(rp);
|
|
glm::vec4 viewport = get_opencv_viewport(image.cols, image.rows);
|
|
for (int i = 0; i < mesh.num_triangles; i++)
|
|
{
|
|
const auto& triangle = mesh.tvi[i];
|
|
const auto p1 =
|
|
glm::project({mesh.vertices[triangle.vec[0]].vec[0], mesh.vertices[triangle.vec[0]].vec[1], mesh.vertices[triangle.vec[0]].vec[2]},
|
|
modelview, projection, viewport);
|
|
const auto p2 =
|
|
glm::project({mesh.vertices[triangle.vec[1]].vec[0], mesh.vertices[triangle.vec[1]].vec[1], mesh.vertices[triangle.vec[1]].vec[2]},
|
|
modelview, projection, viewport);
|
|
const auto p3 =
|
|
glm::project({mesh.vertices[triangle.vec[2]].vec[0], mesh.vertices[triangle.vec[2]].vec[1], mesh.vertices[triangle.vec[2]].vec[2]},
|
|
modelview, projection, viewport);
|
|
if (are_vertices_ccw_in_screen_space(glm::vec2(p1), glm::vec2(p2), glm::vec2(p3))) {
|
|
cv::line(image, cv::Point2f(p1.x, p1.y), cv::Point2f(p2.x, p2.y), color);
|
|
cv::line(image, cv::Point2f(p2.x, p2.y), cv::Point2f(p3.x, p3.y), color);
|
|
cv::line(image, cv::Point2f(p3.x, p3.y), cv::Point2f(p1.x, p1.y), color);
|
|
}
|
|
}
|
|
};
|
|
|
|
|
|
// Averaging Quaternions, by Markley, Cheng, Crassisdis & Oshman
|
|
void average_poses(NvAR_Quaternion *q, unsigned n)
|
|
{
|
|
float acc[10];
|
|
memset(acc, 0, sizeof(acc));
|
|
for (NvAR_Quaternion *qEnd = q + n; q != qEnd; ++q) {
|
|
acc[0] += q->x * q->x; // Compute the normal matrix
|
|
acc[1] += q->x * q->y;
|
|
acc[2] += q->x * q->z;
|
|
acc[3] += q->x * q->w;
|
|
acc[4] += q->y * q->y;
|
|
acc[5] += q->y * q->z;
|
|
acc[6] += q->y * q->w;
|
|
acc[7] += q->z * q->z;
|
|
acc[8] += q->z * q->w;
|
|
acc[9] += q->w * q->w;
|
|
}
|
|
q -= n; // Reset q to its initial value
|
|
cv::Mat M = (cv::Mat_<float>(4, 4) <<
|
|
acc[0], acc[1], acc[2], acc[3], // Normal matrix
|
|
acc[1], acc[4], acc[5], acc[6],
|
|
acc[2], acc[5], acc[7], acc[8],
|
|
acc[3], acc[6], acc[8], acc[9]);
|
|
cv::Mat r = (cv::Mat_<float>(4, 1) << q->x, q->y, q->z, q->w);
|
|
for (unsigned i = 6; i--; ) // Use power method to get the dominant eigenvector
|
|
r = M * r; // It usually converges in 2 iterations, except for sprays > 90 deg.
|
|
float k = r.at<float>(0) * r.at<float>(0)
|
|
+ r.at<float>(1) * r.at<float>(1)
|
|
+ r.at<float>(2) * r.at<float>(2)
|
|
+ r.at<float>(3) * r.at<float>(3);
|
|
if (k) k = 1.f / sqrtf(k);
|
|
q->x = r.at<float>(0) * k; // Normalize quaternion
|
|
q->y = r.at<float>(1) * k;
|
|
q->z = r.at<float>(2) * k;
|
|
q->w = r.at<float>(3) * k;
|
|
}
|
|
|
|
|
|
void set_rotation_from_quaternion(const NvAR_Quaternion *quat, float M[9])
|
|
{
|
|
float a, b, c;
|
|
|
|
a = quat->x * quat->x;
|
|
b = quat->y * quat->y;
|
|
c = quat->z * quat->z;
|
|
M[0] = 1.f - 2.f * (b + c);
|
|
M[4] = 1.f - 2.f * (a + c);
|
|
M[8] = 1.f - 2.f * (a + b);
|
|
|
|
a = quat->x * quat->y;
|
|
b = quat->z * quat->w;
|
|
M[1] = 2.f * (a + b);
|
|
M[3] = 2.f * (a - b);
|
|
|
|
a = quat->y * quat->z;
|
|
b = quat->w * quat->x;
|
|
M[5] = 2.f * (a + b);
|
|
M[7] = 2.f * (a - b);
|
|
|
|
a = quat->x * quat->z;
|
|
b = quat->w * quat->y;
|
|
M[6] = 2.f * (a + b);
|
|
M[2] = 2.f * (a - b);
|
|
}
|