ceres_nonlinear.hpp

/*
 * eos - A 3D Morphable Model fitting library written in modern C++11/14.
 *
 * File: include/eos/fitting/ceres_nonlinear.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_CERES_NONLINEAR_HPP
#define EOS_CERES_NONLINEAR_HPP
 
#include "eos/core/Image.hpp"
#include "eos/morphablemodel/MorphableModel.hpp"
#include "eos/morphablemodel/Blendshape.hpp"
#include "eos/render/matrix_projection.hpp"
 
#include "Eigen/Core"
 
#include "ceres/ceres.h"
#include "ceres/cubic_interpolation.h"
 
#include <vector>
 
namespace eos {
namespace fitting {
 
// Forward declarations:
template <typename T>
Eigen::Vector3<T> get_shape_at_point(const eos::morphablemodel::PcaModel& shape_model,
                                     const eos::morphablemodel::Blendshapes& blendshapes, int vertex_id,
                                     Eigen::Map<const Eigen::VectorX<T>> shape_coeffs,
                                     Eigen::Map<const Eigen::VectorX<T>> blendshape_coeffs);
 
template <typename T>
Eigen::Vector3<T> get_vertex_color_at_point(const eos::morphablemodel::PcaModel& color_model, int vertex_id,
                                            Eigen::Map<const Eigen::VectorX<T>> color_coeffs);
 
struct NormCost
{
 
    NormCost(int num_parameters) : num_parameters(num_parameters){};
 
    template <typename T>
    bool operator()(const T* const x, T* residual) const
    {
        for (int i = 0; i < num_parameters; ++i)
        {
            residual[i] = x[i];
        }
        return true;
    };
 
    template <int num_parameters>
    static ceres::CostFunction* Create()
    {
        return (new ceres::AutoDiffCostFunction<NormCost, num_parameters /* num residuals */,
                                                num_parameters /* num parameters */>(
            new NormCost(num_parameters)));
    }
 
private:
    int num_parameters;
};
 
struct PerspectiveProjectionLandmarkCost
{
 
    PerspectiveProjectionLandmarkCost(const morphablemodel::PcaModel& shape_model,
                                      const std::vector<morphablemodel::Blendshape>& blendshapes,
                                      int num_shape_coeffs, int num_blendshape_coeffs,
                                      Eigen::Vector2f observed_landmark, int vertex_id, int image_width,
                                      int image_height)
        : shape_model(shape_model), blendshapes(blendshapes), num_shape_coeffs(num_shape_coeffs),
          num_blendshape_coeffs(num_blendshape_coeffs), observed_landmark(observed_landmark),
          vertex_id(vertex_id), image_width(image_width), image_height(image_height),
          aspect_ratio(static_cast<double>(image_width) / image_height){};
 
    template <typename T>
    bool operator()(const T* const camera_rotation, const T* const camera_translation,
                    const T* const camera_intrinsics, const T* const shape_coeffs,
                    const T* const blendshape_coeffs, T* residual) const
    {
        // Generate shape instance (of only one vertex id) using current coefficients:
        Eigen::Map<const Eigen::VectorX<T>> shape_coeffs_mapped(shape_coeffs, num_shape_coeffs);
        Eigen::Map<const Eigen::VectorX<T>> blendshape_coeffs_mapped(blendshape_coeffs,
                                                                     num_blendshape_coeffs);
 
        const Eigen::Vector3<T> point_3d = get_shape_at_point(shape_model, blendshapes, vertex_id,
                                                              shape_coeffs_mapped, blendshape_coeffs_mapped);
        // Project the point to 2D:
        // I think the quaternion is always normalised because we run Ceres with QuaternionParameterization
        // Previously, we used glm::tquat, which expects w, x, y, z, and called it with [0, 1, 2, 3].
        // Eigen stores it as x, y, z, w. So... are we sure this is right?
        Eigen::Map<const Eigen::Quaternion<T>> rotation(camera_rotation);
        Eigen::Map<const Eigen::Vector3<T>> translation(camera_translation);
 
        Eigen::Matrix4<T> model_view_mtx = Eigen::Matrix4<T>::Identity();
        model_view_mtx.template block<3, 3>(0, 0) = rotation.toRotationMatrix(); // dependent name
        model_view_mtx.col(3).template head<3>() = translation;                  // dependent name
 
        // Todo: use get_opencv_viewport() from nonlin_cam_esti.hpp.
        const Eigen::Vector4<T> viewport(T(0), T(image_height), T(image_width),
                                         T(-image_height)); // OpenCV convention
 
        const T& fov = camera_intrinsics[0];
        const auto projection_mtx = render::perspective(fov, T(aspect_ratio), T(0.1), T(1000.0));
 
        Eigen::Vector3<T> projected_point =
            render::project(point_3d, model_view_mtx, projection_mtx, viewport);
 
        // Residual: Projected point minus the observed 2D landmark point
        residual[0] = projected_point.x() - T(observed_landmark[0]);
        residual[1] = projected_point.y() - T(observed_landmark[1]);
        return true;
    };
 
    template <int num_shape_coeffs, int num_blendshape_coeffs>
    static ceres::CostFunction* Create(const eos::morphablemodel::PcaModel& shape_model,
                                       const eos::morphablemodel::Blendshapes& blendshapes,
                                       Eigen::Vector2f observed_landmark, int vertex_id, int image_width,
                                       int image_height)
    {
        // Template arguments: 2 residuals, 4 (camera rotation), 3 (camera translation), 1 (camera
        // intrinsics), n (shape coeffs), m (blendshape coeffs)
        return (new ceres::AutoDiffCostFunction<PerspectiveProjectionLandmarkCost, 2, 4, 3, 1,
                                                num_shape_coeffs, num_blendshape_coeffs>(
            new PerspectiveProjectionLandmarkCost(shape_model, blendshapes, num_shape_coeffs,
                                                  num_blendshape_coeffs, observed_landmark, vertex_id,
                                                  image_width, image_height)));
    };
 
private:
    const morphablemodel::PcaModel&
        shape_model; // Or store as pointer (non-owning) or std::reference_wrapper?
    const std::vector<morphablemodel::Blendshape>& blendshapes;
    const int num_shape_coeffs;
    const int num_blendshape_coeffs;
    const Eigen::Vector2f observed_landmark;
    const int vertex_id;
    const int image_width;
    const int image_height;
    const double aspect_ratio;
};
 
struct VertexColorCost
{
    VertexColorCost(const eos::morphablemodel::PcaModel& shape_model,
                    const eos::morphablemodel::Blendshapes& blendshapes,
                    const eos::morphablemodel::PcaModel& color_model, int num_shape_coeffs,
                    int num_blendshape_coeffs, int num_color_coeffs, int vertex_id,
                    const eos::core::Image3u& image)
        : shape_model(shape_model), blendshapes(blendshapes), color_model(color_model),
          num_shape_coeffs(num_shape_coeffs), num_blendshape_coeffs(num_blendshape_coeffs),
          num_color_coeffs(num_color_coeffs), vertex_id(vertex_id), image(image){};
 
    template <typename T>
    bool operator()(const T* const camera_rotation, const T* const camera_translation,
                    const T* const camera_intrinsics, const T* const shape_coeffs,
                    const T* const blendshape_coeffs, const T* const color_coeffs, T* residual) const
    {
        // The following code blocks are identical to PerspectiveProjectionLandmarkCost:
 
        // Generate shape instance (of only one vertex id) using current coefficients:
        Eigen::Map<const Eigen::VectorX<T>> shape_coeffs_mapped(shape_coeffs, num_shape_coeffs);
        Eigen::Map<const Eigen::VectorX<T>> blendshape_coeffs_mapped(blendshape_coeffs,
                                                                     num_blendshape_coeffs);
 
        const Eigen::Vector3<T> point_3d = get_shape_at_point(shape_model, blendshapes, vertex_id,
                                                              shape_coeffs_mapped, blendshape_coeffs_mapped);
        // Project the point to 2D:
        // I think the quaternion is always normalised because we run Ceres with QuaternionParameterization
        // Previously, we used glm::tquat, which expects w, x, y, z, and called it with [0, 1, 2, 3].
        // Eigen stores it as x, y, z, w. So... are we sure this is right?
        Eigen::Map<const Eigen::Quaternion<T>> rotation(camera_rotation);
        Eigen::Map<const Eigen::Vector3<T>> translation(camera_translation);
 
        Eigen::Matrix4<T> model_view_mtx = Eigen::Matrix4<T>::Identity();
        model_view_mtx.template block<3, 3>(0, 0) = rotation.toRotationMatrix(); // dependent name
        model_view_mtx.col(3).template head<3>() = translation;                  // dependent name
 
        // Todo: use get_opencv_viewport() from nonlin_cam_esti.hpp.
        const Eigen::Vector4<T> viewport(T(0), T(image.height()), T(image.width()),
                                         T(-image.height())); // OpenCV convention
 
        const T& fov = camera_intrinsics[0];
        const double aspect_ratio = static_cast<double>(image.width()) / image.height();
        const auto projection_mtx = render::perspective(fov, T(aspect_ratio), T(0.1), T(1000.0));
 
        Eigen::Vector3<T> projected_point =
            render::project(point_3d, model_view_mtx, projection_mtx, viewport);
        // End of identical block with PerspectiveProjectionLandmarkCost.
 
        // Access the image colour value at the projected pixel location, if inside the image - otherwise
        // Ceres uses the value from the grid edge.
        // Note: We could store the BiCubicInterpolator as member variable.
        // The default template arguments for Grid2D are <T, kDataDim=1, kRowMajor=true,
        // kInterleaved=true> and (except for the dimension), they're the right ones for us.
        ceres::Grid2D<unsigned char, 3> grid(&image(0, 0).data()[0], 0, image.height(), 0, image.width());
        ceres::BiCubicInterpolator<ceres::Grid2D<unsigned char, 3>> interpolator(grid);
        T observed_colour[3];
        interpolator.Evaluate(projected_point.y(), projected_point.x(), &observed_colour[0]);
 
        // Get the vertex's colour value:
        Eigen::Map<const Eigen::VectorX<T>> color_coeffs_mapped(color_coeffs, num_color_coeffs);
        const auto model_color = get_vertex_color_at_point(color_model, vertex_id, color_coeffs_mapped);
        // Returns RGB, between [0, 1].
 
        // Residual: Vertex colour of model point minus the observed colour in the 2D image
        // observed_colour is RGB, model_colour is RGB. Residual will be RGB.
        residual[0] = model_color[0] * 255.0 - T(observed_colour[0]); // r
        residual[1] = model_color[1] * 255.0 - T(observed_colour[1]); // g
        residual[2] = model_color[2] * 255.0 - T(observed_colour[2]); // b
 
        return true;
    };
 
    template <int num_shape_coeffs, int num_blendshape_coeffs, int num_color_coeffs>
    static ceres::CostFunction* Create(const eos::morphablemodel::PcaModel& shape_model,
                                       const eos::morphablemodel::Blendshapes& blendshapes,
                                       const eos::morphablemodel::PcaModel& color_model, int vertex_id,
                                       const eos::core::Image3u& image)
    {
        // Template arguments: 3 residuals (RGB), 4 (camera rotation), 3 (camera translation), 1 (camera
        // intrinsics), x (shape coeffs), x (expr coeffs), x (colour coeffs)
        return (new ceres::AutoDiffCostFunction<VertexColorCost, 3, 4, 3, 1, num_shape_coeffs,
                                                num_blendshape_coeffs, num_color_coeffs>(
            new VertexColorCost(shape_model, blendshapes, color_model, num_shape_coeffs,
                                num_blendshape_coeffs, num_color_coeffs, vertex_id, image)));
    };
 
private:
    const eos::morphablemodel::PcaModel&
        shape_model; // Or store as pointer (non-owning) or std::reference_wrapper?
    const eos::morphablemodel::Blendshapes& blendshapes;
    const eos::morphablemodel::PcaModel& color_model;
    const int num_shape_coeffs;
    const int num_blendshape_coeffs;
    const int num_color_coeffs;
    const int vertex_id;
    const eos::core::Image3u& image; // the observed image
};
 
template <typename T>
Eigen::Vector3<T> get_shape_at_point(const eos::morphablemodel::PcaModel& shape_model,
                                     const eos::morphablemodel::Blendshapes& blendshapes, int vertex_id,
                                     Eigen::Map<const Eigen::VectorX<T>> shape_coeffs,
                                     Eigen::Map<const Eigen::VectorX<T>> blendshape_coeffs)
{
    // Computing Shape = mean + shape_basis*shape_coeffs + blendshapes*blendshape_coeffs:
    const Eigen::Vector3f mean = shape_model.get_mean_at_point(vertex_id);
    // Note: We seem to have a dependent name here, so we need 'template', to help the compiler that 'cast<T>'
    // is a template.
    const Eigen::Vector3<T> shape_vector = shape_model.get_rescaled_pca_basis_at_point(vertex_id)
                                               .leftCols(shape_coeffs.size())
                                               .template cast<T>() *
                                           shape_coeffs;
    Eigen::Vector3<T> expression_vector(T(0.0), T(0.0), T(0.0));
    for (std::size_t i = 0; i < blendshape_coeffs.size(); i++)
    {
        expression_vector.x() += T(blendshapes[i].deformation(vertex_id * 3 + 0)) * blendshape_coeffs(i);
        expression_vector.y() += T(blendshapes[i].deformation(vertex_id * 3 + 1)) * blendshape_coeffs(i);
        expression_vector.z() += T(blendshapes[i].deformation(vertex_id * 3 + 2)) * blendshape_coeffs(i);
    }
 
    return Eigen::Vector3<T>(mean.cast<T>() + shape_vector + expression_vector);
};
 
template <typename T>
Eigen::Vector3<T> get_vertex_color_at_point(const eos::morphablemodel::PcaModel& color_model, int vertex_id,
                                            Eigen::Map<const Eigen::VectorX<T>> color_coeffs)
{
    const Eigen::Vector3f mean = color_model.get_mean_at_point(vertex_id);
    // Note: We seem to have a dependent name here, so we need 'template', to help the compiler that 'cast<T>'
    // is a template.
    const Eigen::Vector3<T> color_vector = color_model.get_rescaled_pca_basis_at_point(vertex_id)
                                               .leftCols(color_coeffs.size())
                                               .template cast<T>() *
                                           color_coeffs;
 
    return Eigen::Vector3<T>(mean.cast<T>() + color_vector);
};
 
} /* namespace fitting */
} /* namespace eos */
 
#endif /* EOS_CERES_NONLINEAR_HPP */