talksik/mediapipe
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Project import generated by Copybara.

Browse Source 
GitOrigin-RevId: 27c70b5fe62ab71189d358ca122ee4b19c817a8f
This commit is contained in:
MediaPipe Team
2021-07-27 19:36:32 -04:00
committed by chuoling
parent 374f5e2e7e
commit 50c92c6623
158 changed files with 4704 additions and 621 deletions
Download Patch File Download Diff File Expand all files Collapse all files
mediapipe/modules/face_landmark/BUILD
+20
View File
@@ -95,6 +95,26 @@ mediapipe_simple_subgraph(
],
)
mediapipe_simple_subgraph(
name = "face_landmark_front_cpu_image",
graph = "face_landmark_front_cpu_image.pbtxt",
register_as = "FaceLandmarkFrontCpuImage",
deps = [
":face_landmark_front_cpu",
"//mediapipe/calculators/util:from_image_calculator",
],
)
mediapipe_simple_subgraph(
name = "face_landmark_front_gpu_image",
graph = "face_landmark_front_gpu_image.pbtxt",
register_as = "FaceLandmarkFrontGpuImage",
deps = [
":face_landmark_front_gpu",
"//mediapipe/calculators/util:from_image_calculator",
],
)
exports_files(
srcs = [
"face_landmark.tflite",
mediapipe/modules/face_landmark/face_landmark_front_cpu_image.pbtxt
+44
View File
@@ -0,0 +1,44 @@
# MediaPipe graph to detect/predict face landmarks on CPU.
type: "FaceLandmarkFrontCpuImage"
# Input image. (Image)
input_stream: "IMAGE:image"
# Max number of faces to detect/track. (int)
input_side_packet: "NUM_FACES:num_faces"
# Collection of detected/predicted faces, each represented as a list of 468 face
# landmarks. (std::vector<NormalizedLandmarkList>)
# NOTE: there will not be an output packet in the LANDMARKS stream for this
# particular timestamp if none of faces detected. However, the MediaPipe
# framework will internally inform the downstream calculators of the absence of
# this packet so that they don't wait for it unnecessarily.
output_stream: "LANDMARKS:multi_face_landmarks"
# Extra outputs (for debugging, for instance).
# Detected faces. (std::vector<Detection>)
output_stream: "DETECTIONS:face_detections"
# Regions of interest calculated based on landmarks.
# (std::vector<NormalizedRect>)
output_stream: "ROIS_FROM_LANDMARKS:face_rects_from_landmarks"
# Regions of interest calculated based on face detections.
# (std::vector<NormalizedRect>)
output_stream: "ROIS_FROM_DETECTIONS:face_rects_from_detections"
# Converts Image to ImageFrame for FaceLandmarkFrontCpu to consume.
node {
calculator: "FromImageCalculator"
input_stream: "IMAGE:image"
output_stream: "IMAGE_CPU:image_frame"
}
node {
calculator: "FaceLandmarkFrontCpu"
input_stream: "IMAGE:image_frame"
input_side_packet: "NUM_FACES:num_faces"
output_stream: "LANDMARKS:multi_face_landmarks"
output_stream: "DETECTIONS:face_detections"
output_stream: "ROIS_FROM_LANDMARKS:face_rects_from_landmarks"
output_stream: "ROIS_FROM_DETECTIONS:face_rects_from_detections"
}
mediapipe/modules/face_landmark/face_landmark_front_gpu_image.pbtxt
+44
View File
@@ -0,0 +1,44 @@
# MediaPipe graph to detect/predict face landmarks on GPU.
type: "FaceLandmarkFrontGpuImage"
# Input image. (Image)
input_stream: "IMAGE:image"
# Max number of faces to detect/track. (int)
input_side_packet: "NUM_FACES:num_faces"
# Collection of detected/predicted faces, each represented as a list of 468 face
# landmarks. (std::vector<NormalizedLandmarkList>)
# NOTE: there will not be an output packet in the LANDMARKS stream for this
# particular timestamp if none of faces detected. However, the MediaPipe
# framework will internally inform the downstream calculators of the absence of
# this packet so that they don't wait for it unnecessarily.
output_stream: "LANDMARKS:multi_face_landmarks"
# Extra outputs (for debugging, for instance).
# Detected faces. (std::vector<Detection>)
output_stream: "DETECTIONS:face_detections"
# Regions of interest calculated based on landmarks.
# (std::vector<NormalizedRect>)
output_stream: "ROIS_FROM_LANDMARKS:face_rects_from_landmarks"
# Regions of interest calculated based on face detections.
# (std::vector<NormalizedRect>)
output_stream: "ROIS_FROM_DETECTIONS:face_rects_from_detections"
# Converts Image to GpuBuffer for FaceLandmarkFrontGpu to consume.
node {
calculator: "FromImageCalculator"
input_stream: "IMAGE:image"
output_stream: "IMAGE_GPU:gpu_buffer"
}
node {
calculator: "FaceLandmarkFrontGpu"
input_stream: "IMAGE:gpu_buffer"
input_side_packet: "NUM_FACES:num_faces"
output_stream: "LANDMARKS:multi_face_landmarks"
output_stream: "DETECTIONS:face_detections"
output_stream: "ROIS_FROM_LANDMARKS:face_rects_from_landmarks"
output_stream: "ROIS_FROM_DETECTIONS:face_rects_from_detections"
}
mediapipe/modules/hand_landmark/hand_landmark_tracking_cpu_image.pbtxt
+1 -1
View File
@@ -38,7 +38,7 @@ output_stream: "HAND_ROIS_FROM_LANDMARKS:hand_rects"
# (std::vector<NormalizedRect>)
output_stream: "HAND_ROIS_FROM_PALM_DETECTIONS:hand_rects_from_palm_detections"
# Converts Image to GpuBuffer for HandLandmarkTrackingGpu to consume.
# Converts Image to ImageFrame for HandLandmarkTrackingCpu to consume.
node {
calculator: "FromImageCalculator"
input_stream: "IMAGE:image"
mediapipe/modules/objectron/calculators/BUILD
+18
View File
@@ -34,6 +34,7 @@ mediapipe_proto_library(
mediapipe_proto_library(
name = "annotation_proto",
srcs = ["annotation_data.proto"],
def_options_lib = False,
visibility = ["//visibility:public"],
deps = [
":a_r_capture_metadata_proto",
@@ -152,6 +153,22 @@ cc_library(
],
)
cc_library(
name = "epnp",
srcs = [
"epnp.cc",
],
hdrs = [
"epnp.h",
],
deps = [
"//mediapipe/framework/port:logging",
"@com_google_absl//absl/status",
"@com_google_absl//absl/strings:str_format",
"@eigen_archive//:eigen3",
],
)
cc_library(
name = "decoder",
srcs = [
@@ -164,6 +181,7 @@ cc_library(
":annotation_cc_proto",
":belief_decoder_config_cc_proto",
":box",
":epnp",
"//mediapipe/framework/port:logging",
"//mediapipe/framework/port:opencv_core",
"//mediapipe/framework/port:opencv_imgproc",
mediapipe/modules/objectron/calculators/box.cc
+8
View File
@@ -245,6 +245,14 @@ void Box::Fit(const std::vector<T>& vertices) {
auto system_g = system_h.colPivHouseholderQr();
auto solution = system_g.solve(v).eval();
transformation_.topLeftCorner<3, 4>() = solution.transpose();
// Adjust rotation matrix to its nearest orthogonal matrix.
const auto rotation = transformation_.topLeftCorner<3, 3>();
Eigen::JacobiSVD<Eigen::Matrix3f> svd(
rotation, Eigen::ComputeFullV | Eigen::ComputeFullU);
const Eigen::Matrix3f matrix_u = svd.matrixU();
const Eigen::Matrix3f matrix_v = svd.matrixV();
transformation_.topLeftCorner<3, 3>() = matrix_u * matrix_v.transpose();
Update();
}
mediapipe/modules/objectron/calculators/decoder.cc
+44 -78
View File
@@ -15,24 +15,33 @@
#include "mediapipe/modules/objectron/calculators/decoder.h"
#include <limits>
#include <vector>
#include "Eigen/Core"
#include "Eigen/Dense"
#include "absl/status/status.h"
#include "mediapipe/framework/port/canonical_errors.h"
#include "mediapipe/framework/port/logging.h"
#include "mediapipe/framework/port/opencv_imgproc_inc.h"
#include "mediapipe/framework/port/status.h"
#include "mediapipe/modules/objectron/calculators/annotation_data.pb.h"
#include "mediapipe/modules/objectron/calculators/box.h"
#include "mediapipe/modules/objectron/calculators/epnp.h"
#include "mediapipe/modules/objectron/calculators/types.h"
namespace mediapipe {
constexpr int Decoder::kNumOffsetmaps = 16;
constexpr int kNumKeypoints = 9;
namespace {
void SetPoint3d(float x, float y, float z, Point3D* point_3d) {
point_3d->set_x(x);
point_3d->set_y(y);
point_3d->set_z(z);
inline void SetPoint3d(const Eigen::Vector3f& point_vec, Point3D* point_3d) {
point_3d->set_x(point_vec.x());
point_3d->set_y(point_vec.y());
point_3d->set_z(point_vec.z());
}
} // namespace
FrameAnnotation Decoder::DecodeBoundingBoxKeypoints(
@@ -193,92 +202,49 @@ absl::Status Decoder::Lift2DTo3D(
const Eigen::Matrix<float, 4, 4, Eigen::RowMajor>& projection_matrix,
bool portrait, FrameAnnotation* estimated_box) const {
CHECK(estimated_box != nullptr);
const float fx = projection_matrix(0, 0);
const float fy = projection_matrix(1, 1);
const float cx = projection_matrix(0, 2);
const float cy = projection_matrix(1, 2);
for (auto& annotation : *estimated_box->mutable_annotations()) {
Eigen::Matrix<float, 16, 12, Eigen::RowMajor> m =
Eigen::Matrix<float, 16, 12, Eigen::RowMajor>::Zero(16, 12);
CHECK_EQ(9, annotation.keypoints_size());
float u, v;
for (int i = 0; i < 8; ++i) {
const auto& keypoint2d = annotation.keypoints(i + 1).point_2d();
// Convert 2d point from screen coordinates to NDC coordinates([-1, 1]).
if (portrait) {
// Swap x and y given that our image is in portrait orientation
u = keypoint2d.y() * 2 - 1;
v = keypoint2d.x() * 2 - 1;
} else {
u = keypoint2d.x() * 2 - 1;
v = 1 - keypoint2d.y() * 2; // (1 - keypoint2d.y()) * 2 - 1
}
for (int j = 0; j < 4; ++j) {
// For each of the 4 control points, formulate two rows of the
// m matrix (two equations).
const float control_alpha = epnp_alpha_(i, j);
m(i * 2, j * 3) = fx * control_alpha;
m(i * 2, j * 3 + 2) = (cx + u) * control_alpha;
m(i * 2 + 1, j * 3 + 1) = fy * control_alpha;
m(i * 2 + 1, j * 3 + 2) = (cy + v) * control_alpha;
}
}
// This is a self adjoint matrix. Use SelfAdjointEigenSolver for a fast
// and stable solution.
Eigen::Matrix<float, 12, 12, Eigen::RowMajor> mt_m = m.transpose() * m;
Eigen::SelfAdjointEigenSolver<Eigen::Matrix<float, 12, 12, Eigen::RowMajor>>
eigen_solver(mt_m);
if (eigen_solver.info() != Eigen::Success) {
return absl::AbortedError("Eigen decomposition failed.");
}
CHECK_EQ(12, eigen_solver.eigenvalues().size());
// Eigenvalues are sorted in increasing order for SelfAdjointEigenSolver
// only! If you use other Eigen Solvers, it's not guaranteed to be in
// increasing order. Here, we just take the eigen vector corresponding
// to first/smallest eigen value, since we used SelfAdjointEigenSolver.
Eigen::VectorXf eigen_vec = eigen_solver.eigenvectors().col(0);
Eigen::Map<Eigen::Matrix<float, 4, 3, Eigen::RowMajor>> control_matrix(
eigen_vec.data());
// All 3d points should be in front of camera (z < 0).
if (control_matrix(0, 2) > 0) {
control_matrix = -control_matrix;
}
// First set the center keypoint.
SetPoint3d(control_matrix(0, 0), control_matrix(0, 1), control_matrix(0, 2),
annotation.mutable_keypoints(0)->mutable_point_3d());
// Then set the 8 vertices.
Eigen::Matrix<float, 8, 3, Eigen::RowMajor> vertices =
epnp_alpha_ * control_matrix;
CHECK_EQ(kNumKeypoints, annotation.keypoints_size());
std::vector<Eigen::Vector3f> vertices_vec;
vertices_vec.emplace_back(Eigen::Vector3f(
control_matrix(0, 0), control_matrix(0, 1), control_matrix(0, 2)));
for (int i = 0; i < 8; ++i) {
SetPoint3d(vertices(i, 0), vertices(i, 1), vertices(i, 2),
annotation.mutable_keypoints(i + 1)->mutable_point_3d());
vertices_vec.emplace_back(
Eigen::Vector3f(vertices(i, 0), vertices(i, 1), vertices(i, 2)));
// Fill input 2D Points;
std::vector<Vector2f> input_points_2d;
input_points_2d.reserve(kNumKeypoints);
for (const auto& keypoint : annotation.keypoints()) {
input_points_2d.emplace_back(keypoint.point_2d().x(),
keypoint.point_2d().y());
}
// Fit a box to the vertices to get box scale, rotation, translation.
// Run EPnP.
std::vector<Vector3f> output_points_3d;
output_points_3d.reserve(kNumKeypoints);
auto status = SolveEpnp(projection_matrix, portrait, input_points_2d,
&output_points_3d);
if (!status.ok()) {
LOG(ERROR) << status;
return status;
}
// Fill 3D keypoints;
for (int i = 0; i < kNumKeypoints; ++i) {
SetPoint3d(output_points_3d[i],
annotation.mutable_keypoints(i)->mutable_point_3d());
}
// Fit a box to the 3D points to get box scale, rotation, translation.
Box box("category");
box.Fit(vertices_vec);
box.Fit(output_points_3d);
const Eigen::Matrix<float, 3, 3, Eigen::RowMajor> rotation =
box.GetRotation();
const Eigen::Vector3f translation = box.GetTranslation();
const Eigen::Vector3f scale = box.GetScale();
// Fill box rotation.
std::vector<float> rotation_vec(rotation.data(),
rotation.data() + rotation.size());
*annotation.mutable_rotation() = {rotation_vec.begin(), rotation_vec.end()};
*annotation.mutable_rotation() = {rotation.data(),
rotation.data() + rotation.size()};
// Fill box translation.
std::vector<float> translation_vec(translation.data(),
translation.data() + translation.size());
*annotation.mutable_translation() = {translation_vec.begin(),
translation_vec.end()};
*annotation.mutable_translation() = {
translation.data(), translation.data() + translation.size()};
// Fill box scale.
std::vector<float> scale_vec(scale.data(), scale.data() + scale.size());
*annotation.mutable_scale() = {scale_vec.begin(), scale_vec.end()};
*annotation.mutable_scale() = {scale.data(), scale.data() + scale.size()};
}
return absl::OkStatus();
}
mediapipe/modules/objectron/calculators/epnp.cc
+167
View File
@@ -0,0 +1,167 @@
// Copyright 2021 The MediaPipe Authors.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "mediapipe/modules/objectron/calculators/epnp.h"
namespace mediapipe {
namespace {
// NUmber of keypoints.
constexpr int kNumKeypoints = 9;
using Eigen::Map;
using Eigen::Matrix;
using Eigen::Matrix4f;
using Eigen::Vector2f;
using Eigen::Vector3f;
} // namespace
absl::Status SolveEpnp(const float focal_x, const float focal_y,
const float center_x, const float center_y,
const bool portrait,
const std::vector<Vector2f>& input_points_2d,
std::vector<Vector3f>* output_points_3d) {
if (input_points_2d.size() != kNumKeypoints) {
return absl::InvalidArgumentError(
absl::StrFormat("Input must has %d 2D points.", kNumKeypoints));
}
if (output_points_3d == nullptr) {
return absl::InvalidArgumentError(
"Output pointer output_points_3d is Null.");
}
Matrix<float, (kNumKeypoints - 1) * 2, 12> m =
Matrix<float, (kNumKeypoints - 1) * 2, 12>::Zero();
Matrix<float, kNumKeypoints - 1, 4> epnp_alpha;
// The epnp_alpha is the Nx4 weight matrix from the EPnP paper, which is used
// to express the N box vertices as the weighted sum of 4 control points. The
// value of epnp_alpha is depedent on the set of control points been used.
// In our case we used the 4 control points as below (coordinates are in world
// coordinate system):
// c0 = (0.0, 0.0, 0.0) // Box center
// c1 = (1.0, 0.0, 0.0) // Right face center
// c2 = (0.0, 1.0, 0.0) // Top face center
// c3 = (0.0, 0.0, 1.0) // Front face center
//
// 3 + + + + + + + + 7
// +\ +\ UP
// + \ + \
// + \ + \ |
// + 4 + + + + + + + + 8 | y
// + + + + |
// + + + + |
// + + (0) + + .------- x
// + + + + \
// 1 + + + + + + + + 5 + \
// \ + \ + \ z
// \ + \ + \
// \+ \+
// 2 + + + + + + + + 6
//
// For each box vertex shown above, we have the below weighted sum expression:
// v1 = c0 - (c1 - c0) - (c2 - c0) - (c3 - c0) = 4*c0 - c1 - c2 - c3;
// v2 = c0 - (c1 - c0) - (c2 - c0) + (c3 - c0) = 2*c0 - c1 - c2 + c3;
// v3 = c0 - (c1 - c0) + (c2 - c0) - (c3 - c0) = 2*c0 - c1 + c2 - c3;
// ...
// Thus we can determine the value of epnp_alpha as been used below.
//
// clang-format off
epnp_alpha << 4.0f, -1.0f, -1.0f, -1.0f,
2.0f, -1.0f, -1.0f, 1.0f,
2.0f, -1.0f, 1.0f, -1.0f,
0.0f, -1.0f, 1.0f, 1.0f,
2.0f, 1.0f, -1.0f, -1.0f,
0.0f, 1.0f, -1.0f, 1.0f,
0.0f, 1.0f, 1.0f, -1.0f,
-2.0f, 1.0f, 1.0f, 1.0f;
// clang-format on
for (int i = 0; i < input_points_2d.size() - 1; ++i) {
// Skip 0th landmark which is object center.
const auto& point_2d = input_points_2d[i + 1];
// Convert 2d point from `pixel coordinates` to `NDC coordinates`([-1, 1])
// following to the definitions in:
// https://google.github.io/mediapipe/solutions/objectron#ndc-space
// If portrait mode is been used, it's the caller's responsibility to
// convert the input 2d points' coordinates.
float x_ndc, y_ndc;
if (portrait) {
x_ndc = point_2d.y() * 2 - 1;
y_ndc = point_2d.x() * 2 - 1;
} else {
x_ndc = point_2d.x() * 2 - 1;
y_ndc = 1 - point_2d.y() * 2;
}
for (int j = 0; j < 4; ++j) {
// For each of the 4 control points, formulate two rows of the
// m matrix (two equations).
const float control_alpha = epnp_alpha(i, j);
m(i * 2, j * 3) = focal_x * control_alpha;
m(i * 2, j * 3 + 2) = (center_x + x_ndc) * control_alpha;
m(i * 2 + 1, j * 3 + 1) = focal_y * control_alpha;
m(i * 2 + 1, j * 3 + 2) = (center_y + y_ndc) * control_alpha;
}
}
// This is a self adjoint matrix. Use SelfAdjointEigenSolver for a fast
// and stable solution.
Matrix<float, 12, 12> mt_m = m.transpose() * m;
Eigen::SelfAdjointEigenSolver<Matrix<float, 12, 12>> eigen_solver(mt_m);
if (eigen_solver.info() != Eigen::Success) {
return absl::AbortedError("Eigen decomposition failed.");
}
CHECK_EQ(12, eigen_solver.eigenvalues().size());
// Eigenvalues are sorted in increasing order for SelfAdjointEigenSolver
// only! If you use other Eigen Solvers, it's not guaranteed to be in
// increasing order. Here, we just take the eigen vector corresponding
// to first/smallest eigen value, since we used SelfAdjointEigenSolver.
Eigen::VectorXf eigen_vec = eigen_solver.eigenvectors().col(0);
Map<Matrix<float, 4, 3, Eigen::RowMajor>> control_matrix(eigen_vec.data());
// All 3D points should be in front of camera (z < 0).
if (control_matrix(0, 2) > 0) {
control_matrix = -control_matrix;
}
Matrix<float, kNumKeypoints - 1, 3> vertices = epnp_alpha * control_matrix;
// Fill 0th 3D points.
output_points_3d->emplace_back(control_matrix(0, 0), control_matrix(0, 1),
control_matrix(0, 2));
// Fill the rest 3D points.
for (int i = 0; i < kNumKeypoints - 1; ++i) {
output_points_3d->emplace_back(vertices(i, 0), vertices(i, 1),
vertices(i, 2));
}
return absl::OkStatus();
}
absl::Status SolveEpnp(const Eigen::Matrix4f& projection_matrix,
const bool portrait,
const std::vector<Vector2f>& input_points_2d,
std::vector<Vector3f>* output_points_3d) {
const float focal_x = projection_matrix(0, 0);
const float focal_y = projection_matrix(1, 1);
const float center_x = projection_matrix(0, 2);
const float center_y = projection_matrix(1, 2);
return SolveEpnp(focal_x, focal_y, center_x, center_y, portrait,
input_points_2d, output_points_3d);
}
} // namespace mediapipe
mediapipe/modules/objectron/calculators/epnp.h
+62
View File
@@ -0,0 +1,62 @@
// Copyright 2021 The MediaPipe Authors.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef MEDIAPIPE_MODULES_OBJECTRON_CALCULATORS_EPNP_H_
#define MEDIAPIPE_MODULES_OBJECTRON_CALCULATORS_EPNP_H_
#include <vector>
#include "Eigen/Dense"
#include "absl/status/status.h"
#include "absl/strings/str_format.h"
#include "mediapipe/framework/port/logging.h"
namespace mediapipe {
// This function performs EPnP algorithm, lifting normalized 2D points in pixel
// space to 3D points in camera coordinate.
//
// Inputs:
// focal_x: camera focal length along x.
// focal_y: camera focal length along y.
// center_x: camera center along x.
// center_y: camera center along y.
// portrait: a boolen variable indicating whether our images are obtained in
// portrait orientation or not.
// input_points_2d: input 2D points to be lifted to 3D.
// output_points_3d: ouput 3D points in camera coordinate.
absl::Status SolveEpnp(const float focal_x, const float focal_y,
const float center_x, const float center_y,
const bool portrait,
const std::vector<Eigen::Vector2f>& input_points_2d,
std::vector<Eigen::Vector3f>* output_points_3d);
// This function performs EPnP algorithm, lifting normalized 2D points in pixel
// space to 3D points in camera coordinate.
//
// Inputs:
// projection_matrix: the projection matrix from 3D coordinate
// to screen coordinate.
// portrait: a boolen variable indicating whether our images are obtained in
// portrait orientation or not.
// input_points_2d: input 2D points to be lifted to 3D.
// output_points_3d: ouput 3D points in camera coordinate.
absl::Status SolveEpnp(const Eigen::Matrix4f& projection_matrix,
const bool portrait,
const std::vector<Eigen::Vector2f>& input_points_2d,
std::vector<Eigen::Vector3f>* output_points_3d);
} // namespace mediapipe
#endif // MEDIAPIPE_MODULES_OBJECTRON_CALCULATORS_EPNP_H_
mediapipe/modules/objectron/calculators/epnp_test.cc
+169
View File
@@ -0,0 +1,169 @@
#include "mediapipe/modules/objectron/calculators/epnp.h"
#include "mediapipe/framework/port/gmock.h"
#include "mediapipe/framework/port/gtest.h"
#include "mediapipe/framework/tool/test_util.h"
namespace mediapipe {
namespace {
using Eigen::AngleAxisf;
using Eigen::Map;
using Eigen::Matrix;
using Eigen::Matrix4f;
using Eigen::RowMajor;
using Eigen::Vector2f;
using Eigen::Vector3f;
using ::testing::HasSubstr;
using ::testing::Test;
using ::testing::status::StatusIs;
using Matrix3f = Eigen::Matrix<float, 3, 3, Eigen::RowMajor>;
constexpr uint8_t kNumKeypoints = 9;
// clang-format off
constexpr float kUnitBox[] = { 0.0f, 0.0f, 0.0f,
-0.5f, -0.5f, -0.5f,
-0.5f, -0.5f, 0.5f,
-0.5f, 0.5f, -0.5f,
-0.5f, 0.5f, 0.5f,
0.5f, -0.5f, -0.5f,
0.5f, -0.5f, 0.5f,
0.5f, 0.5f, -0.5f,
0.5f, 0.5f, 0.5f, };
// clang-format on
constexpr float kFocalX = 1.0f;
constexpr float kFocalY = 1.0f;
constexpr float kCenterX = 0.0f;
constexpr float kCenterY = 0.0f;
constexpr float kAzimuth = 90.0f * M_PI / 180.0f;
constexpr float kElevation = 45.0f * M_PI / 180.0f;
constexpr float kTilt = 15.0f * M_PI / 180.0f;
constexpr float kTranslationArray[] = {0.0f, 0.0f, -100.0f};
constexpr float kScaleArray[] = {50.0f, 50.0f, 50.0f};
class SolveEpnpTest : public Test {
protected:
SolveEpnpTest() {}
void SetUp() override {
// Create vertices in world frame.
Map<const Matrix<float, kNumKeypoints, 3, RowMajor>> vertices_w(kUnitBox);
// Create Pose.
Matrix3f rotation;
rotation = AngleAxisf(kTilt, Vector3f::UnitZ()) *
AngleAxisf(kElevation, Vector3f::UnitX()) *
AngleAxisf(kAzimuth, Vector3f::UnitY());
Map<const Vector3f> translation(kTranslationArray);
Map<const Vector3f> scale(kScaleArray);
// Generate 3d vertices in camera frame.
const auto vertices_c =
((rotation * scale.asDiagonal() * vertices_w.transpose()).colwise() +
translation)
.transpose();
// Generate input 2d points.
std::vector<Vector2f> input_2d_points;
std::vector<Vector3f> expected_3d_points;
for (int i = 0; i < kNumKeypoints; ++i) {
const auto x = vertices_c(i, 0);
const auto y = vertices_c(i, 1);
const auto z = vertices_c(i, 2);
const float x_ndc = -kFocalX * x / z + kCenterX;
const float y_ndc = -kFocalY * y / z + kCenterY;
const float x_pixel = (1.0f + x_ndc) / 2.0f;
const float y_pixel = (1.0f - y_ndc) / 2.0f;
expected_3d_points_.emplace_back(x, y, z);
input_2d_points_.emplace_back(x_pixel, y_pixel);
}
}
void VerifyOutput3dPoints(const std::vector<Vector3f>& output_3d_points) {
EXPECT_EQ(kNumKeypoints, output_3d_points.size());
const float scale = output_3d_points[0].z() / expected_3d_points_[0].z();
for (int i = 0; i < kNumKeypoints; ++i) {
EXPECT_NEAR(output_3d_points[i].x(), expected_3d_points_[i].x() * scale,
1.e-6f);
EXPECT_NEAR(output_3d_points[i].y(), expected_3d_points_[i].y() * scale,
1.e-6f);
EXPECT_NEAR(output_3d_points[i].z(), expected_3d_points_[i].z() * scale,
1.e-6f);
}
}
std::vector<Vector2f> input_2d_points_;
std::vector<Vector3f> expected_3d_points_;
};
TEST_F(SolveEpnpTest, SolveEpnp) {
std::vector<Vector3f> output_3d_points;
MP_ASSERT_OK(SolveEpnp(kFocalX, kFocalY, kCenterX, kCenterY,
/*portrait*/ false, input_2d_points_,
&output_3d_points));
// Test output 3D points.
VerifyOutput3dPoints(output_3d_points);
}
TEST_F(SolveEpnpTest, SolveEpnppPortrait) {
std::vector<Vector3f> output_3d_points;
MP_ASSERT_OK(SolveEpnp(kFocalX, kFocalY, kCenterX, kCenterY,
/*portrait*/ true, input_2d_points_,
&output_3d_points));
// Test output 3D points.
for (auto& point_3d : output_3d_points) {
const auto x = point_3d.x();
const auto y = point_3d.y();
// Convert from portrait mode to normal mode, y => x, x => -y.
point_3d.x() = y;
point_3d.y() = -x;
}
VerifyOutput3dPoints(output_3d_points);
}
TEST_F(SolveEpnpTest, SolveEpnpProjectionMatrix) {
Matrix4f projection_matrix;
// clang-format off
projection_matrix << kFocalX, 0.0f, kCenterX, 0.0f,
0.0f, kFocalY, kCenterY, 0.0f,
0.0f, 0.0f, -1.0f, 0.0f,
0.0f, 0.0f, -1.0f, 0.0f;
// clang-format on
std::vector<Vector3f> output_3d_points;
MP_ASSERT_OK(SolveEpnp(projection_matrix, /*portrait*/ false,
input_2d_points_, &output_3d_points));
// Test output 3D points.
VerifyOutput3dPoints(output_3d_points);
}
TEST_F(SolveEpnpTest, BadInput2dPoints) {
// Generate empty input 2D points.
std::vector<Vector2f> input_2d_points;
std::vector<Vector3f> output_3d_points;
EXPECT_THAT(SolveEpnp(kFocalX, kFocalY, kCenterX, kCenterY,
/*portrait*/ false, input_2d_points, &output_3d_points),
StatusIs(absl::StatusCode::kInvalidArgument,
HasSubstr("Input must has")));
}
TEST_F(SolveEpnpTest, BadOutput3dPoints) {
// Generate null output 3D points.
std::vector<Vector3f>* output_3d_points = nullptr;
EXPECT_THAT(SolveEpnp(kFocalX, kFocalY, kCenterX, kCenterY,
/*portrait*/ false, input_2d_points_, output_3d_points),
StatusIs(absl::StatusCode::kInvalidArgument,
"Output pointer output_points_3d is Null."));
}
} // namespace
} // namespace mediapipe