closest_edge_fitting.hpp

/*
 * eos - A 3D Morphable Model fitting library written in modern C++11/14.
 *
 * File: include/eos/fitting/closest_edge_fitting.hpp
 *
 * Copyright 2016, 2023 Patrik Huber
 *
 * 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.
 */
#pragma once
 
#ifndef EOS_CLOSEST_EDGE_FITTING_HPP
#define EOS_CLOSEST_EDGE_FITTING_HPP
 
#include "eos/core/Mesh.hpp"
#include "eos/morphablemodel/EdgeTopology.hpp"
#include "eos/fitting/RenderingParameters.hpp"
#include "eos/render/normals.hpp"
#include "eos/render/vertex_visibility.hpp"
#include "eos/render/ProjectionType.hpp"
#include "eos/render/matrix_projection.hpp"
 
#include "nanoflann.hpp"
 
#include "Eigen/Core"
 
#include <vector>
#include <algorithm>
#include <utility>
#include <cstddef>
 
namespace eos {
namespace fitting {
 
inline std::vector<int> occluding_boundary_vertices(const core::Mesh& mesh,
                                                    const morphablemodel::EdgeTopology& edge_topology,
                                                    Eigen::Matrix3f R, render::ProjectionType projection_type,
                                                    bool perform_self_occlusion_check = true)
{
    // Rotate the mesh:
    std::vector<Eigen::Vector3f> rotated_vertices;
    std::for_each(begin(mesh.vertices), end(mesh.vertices), [&rotated_vertices, &R](const auto& v) {
        rotated_vertices.push_back(R * v);
    });
 
    // Compute the face normals of the rotated mesh:
    std::vector<Eigen::Vector3f> facenormals;
    for (const auto& f : mesh.tvi)
    { // for each face (triangle):
        const auto n = render::compute_face_normal(rotated_vertices[f[0]], rotated_vertices[f[1]],
                                                   rotated_vertices[f[2]]);
        facenormals.push_back(n);
    }
 
    // Find occluding edges:
    std::vector<int> occluding_edges_indices;
    for (int edge_idx = 0; edge_idx < edge_topology.adjacent_faces.size();
         ++edge_idx) // For each edge... Ef contains the indices of the two adjacent faces
    {
        const auto& edge = edge_topology.adjacent_faces[edge_idx];
        if (edge[0] == 0) // ==> NOTE/Todo Need to change this if we use 0-based indexing!
        {
            // Edges with a zero index lie on the mesh boundary, i.e. they are only
            // adjacent to one face.
            continue;
        }
        // Compute the occluding edges as those where the two adjacent face normals
        // differ in the sign of their z-component:
        // Changing from 1-based indexing to 0-based!
        if (core::sign(facenormals[edge[0] - 1].z()) != core::sign(facenormals[edge[1] - 1].z()))
        {
            // It's an occluding edge, store the index:
            occluding_edges_indices.push_back(edge_idx);
        }
    }
    // Select the vertices lying at the two ends of the occluding edges and remove duplicates:
    // (This is what EdgeTopology::adjacent_vertices is needed for).
    std::vector<int> occluding_vertices; // The model's contour vertices
    for (auto edge_idx : occluding_edges_indices)
    {
        // Changing from 1-based indexing to 0-based!
        occluding_vertices.push_back(edge_topology.adjacent_vertices[edge_idx][0] - 1);
        occluding_vertices.push_back(edge_topology.adjacent_vertices[edge_idx][1] - 1);
    }
    // Remove duplicate vertex id's (std::unique works only on sorted sequences):
    std::sort(begin(occluding_vertices), end(occluding_vertices));
    occluding_vertices.erase(std::unique(begin(occluding_vertices), end(occluding_vertices)),
                             end(occluding_vertices));
 
    if (perform_self_occlusion_check)
    {
        // Perform ray-casting to find out which vertices are not visible (i.e. self-occluded):
        // We have to know the projection type, and then use different vector directions depending on
        // the projection type:
        using render::detail::RayDirection;
        RayDirection ray_direction_type;
        if (projection_type == render::ProjectionType::Orthographic)
        {
            ray_direction_type = RayDirection::Parallel;
        } else
        {
            ray_direction_type = RayDirection::TowardsOrigin;
        }
        std::vector<bool> visibility;
        for (auto vertex_idx : occluding_vertices)
        {
            const bool visible = render::is_vertex_visible(rotated_vertices[vertex_idx], rotated_vertices,
                                                           mesh.tvi, ray_direction_type);
            visibility.push_back(visible);
        }
 
        // Remove vertices from occluding boundary list that are not visible:
        std::vector<int> final_vertex_ids;
        for (int i = 0; i < occluding_vertices.size(); ++i)
        {
            if (visibility[i] == true)
            {
                final_vertex_ids.push_back(occluding_vertices[i]);
            }
        }
        return final_vertex_ids;
    } else
    {
        return occluding_vertices;
    }
};
 
template <class VectorOfVectorsType, typename num_t = double, int DIM = -1,
          class Distance = nanoflann::metric_L2, typename IndexType = size_t>
struct KDTreeVectorOfVectorsAdaptor
{
    typedef KDTreeVectorOfVectorsAdaptor<VectorOfVectorsType, num_t, DIM, Distance> self_t;
    typedef typename Distance::template traits<num_t, self_t>::distance_t metric_t;
    typedef nanoflann::KDTreeSingleIndexAdaptor<metric_t, self_t, DIM, IndexType> index_t;
 
    index_t* index; 
 
    // Make sure the data is kept alive while the kd-tree is in use.
    KDTreeVectorOfVectorsAdaptor(const int dimensionality, const VectorOfVectorsType& mat,
                                 const int leaf_max_size = 10)
        : m_data(mat)
    {
        assert(mat.size() != 0 && mat[0].size() != 0);
        const size_t dims = mat[0].size();
        if (DIM > 0 && static_cast<int>(dims) != DIM)
            throw std::runtime_error("Data set dimensionality does not match the 'DIM' template argument");
        index = new index_t(dims, *this /* adaptor */, nanoflann::KDTreeSingleIndexAdaptorParams(leaf_max_size));
        index->buildIndex();
    }
 
    ~KDTreeVectorOfVectorsAdaptor() { delete index; }
 
    const VectorOfVectorsType& m_data;
 
    inline void query(const num_t* query_point, const size_t num_closest, IndexType* out_indices,
                      num_t* out_distances_sq, const int nChecks_IGNORED = 10) const
    {
        nanoflann::KNNResultSet<num_t, IndexType> resultSet(num_closest);
        resultSet.init(out_indices, out_distances_sq);
        index->findNeighbors(resultSet, query_point, nanoflann::SearchParams());
    }
 
    const self_t& derived() const { return *this; }
    self_t& derived() { return *this; }
 
    // Must return the number of data points
    inline size_t kdtree_get_point_count() const { return m_data.size(); }
 
    // Returns the distance between the vector "p1[0:size-1]" and the data point with index "idx_p2" stored in
    // the class:
    inline num_t kdtree_distance(const num_t* p1, const size_t idx_p2, size_t size) const
    {
        num_t s = 0;
        for (size_t i = 0; i < size; i++)
        {
            const num_t d = p1[i] - m_data[idx_p2][i];
            s += d * d;
        }
        return s;
    }
 
    // Returns the dim'th component of the idx'th point in the class:
    inline num_t kdtree_get_pt(const size_t idx, int dim) const
    {
        return m_data[idx][dim];
    }
 
    // Optional bounding-box computation: return false to default to a standard bbox computation loop.
    //   Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again.
    //   Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds)
    template <class BBOX>
    bool kdtree_get_bbox(BBOX& /*bb*/) const
    {
        return false;
    }
 
};
 
inline std::pair<std::vector<Eigen::Vector2f>, std::vector<int>> find_occluding_edge_correspondences(
    const core::Mesh& mesh, const morphablemodel::EdgeTopology& edge_topology,
    const fitting::RenderingParameters& rendering_parameters, const std::vector<Eigen::Vector2f>& image_edges,
    float distance_threshold = 64.0f, bool perform_self_occlusion_check = true)
{
    assert(rendering_parameters.get_camera_type() == fitting::CameraType::Orthographic);
    using Eigen::Vector2f;
    using std::vector;
 
    // If there are no image_edges given, there's no point in computing anything:
    if (image_edges.empty())
    {
        return {};
    }
 
    // Compute vertices that lye on occluding boundaries:
    const render::ProjectionType projection_type = [&rendering_parameters]() {
        if (rendering_parameters.get_camera_type() == CameraType::Orthographic)
        {
            return render::ProjectionType::Orthographic;
        } else
        {
            return render::ProjectionType::Perspective;
        }
    }();
    const auto occluding_vertices = occluding_boundary_vertices(
        mesh, edge_topology, rendering_parameters.get_rotation().normalized().toRotationMatrix(),
        projection_type, perform_self_occlusion_check);
 
    // Project these occluding boundary vertices from 3D to 2D:
    vector<Vector2f> model_edges_projected;
    for (const auto& v : occluding_vertices)
    {
        const auto p = render::project(
            mesh.vertices[v], rendering_parameters.get_modelview(), rendering_parameters.get_projection(),
            fitting::get_opencv_viewport(rendering_parameters.get_screen_width(),
                                         rendering_parameters.get_screen_height()));
        model_edges_projected.push_back({p.x(), p.y()});
    }
 
    // Find edge correspondences:
    // Build a kd-tree and use nearest neighbour search:
    using kd_tree_t = KDTreeVectorOfVectorsAdaptor<vector<Vector2f>, float, 2>;
    kd_tree_t tree(2, image_edges); // dim, samples, max_leaf
    tree.index->buildIndex();
 
    vector<std::pair<std::size_t, double>>
        idx_d; // will contain [ index to the 2D edge in 'image_edges', distance (L2^2) ]
    for (const auto& e : model_edges_projected)
    {
        std::size_t idx; // contains the indices into the original 'image_edges' vector
        double dist_sq;  // squared distances
        nanoflann::KNNResultSet<double> resultSet(1);
        resultSet.init(&idx, &dist_sq);
        tree.index->findNeighbors(resultSet, e.data(), nanoflann::SearchParams(10));
        idx_d.push_back({idx, dist_sq});
    }
    // Filter edge matches:
    // We filter below by discarding all correspondence that are a certain distance apart.
    // We could also (or in addition to) discard the worst 5% of the distances or something like that.
 
    // Filter and store the image (edge) points with their corresponding vertex id:
    vector<int> vertex_indices;
    vector<Vector2f> image_points;
    assert(occluding_vertices.size() == idx_d.size());
    for (int i = 0; i < occluding_vertices.size(); ++i)
    {
        const auto ortho_scale =
            rendering_parameters.get_screen_width() /
            rendering_parameters.get_frustum().r; // This might be a bit of a hack - we recover the "real"
                                                  // scaling from the SOP estimate
        if (idx_d[i].second <= distance_threshold * ortho_scale) // I think multiplying by the scale is good
                                                                 // here and gives us invariance w.r.t. the
                                                                 // image resolution and face size.
        {
            const auto edge_point = image_edges[idx_d[i].first];
            // Store the found 2D edge point, and the associated vertex id:
            vertex_indices.push_back(occluding_vertices[i]);
            image_points.push_back(edge_point);
        }
    }
    return {image_points, vertex_indices};
};
 
} /* namespace fitting */
} /* namespace eos */
 
#endif /* EOS_CLOSEST_EDGE_FITTING_HPP */