orthographic_camera_estimation_linear.hpp

/*
 * eos - A 3D Morphable Model fitting library written in modern C++11/14.
 *
 * File: include/eos/fitting/orthographic_camera_estimation_linear.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_ORTHOGRAPHIC_CAMERA_ESTIMATION_LINEAR_HPP
#define EOS_ORTHOGRAPHIC_CAMERA_ESTIMATION_LINEAR_HPP
 
#include "eos/cpp17/optional.hpp"
 
#include "Eigen/Core"
#include "Eigen/Dense"
#include "Eigen/SVD"
#include "Eigen/Dense"
 
#include <vector>
#include <cassert>
 
namespace eos {
namespace fitting {
 
struct ScaledOrthoProjectionParameters
{
    Eigen::Matrix3f R; 
    float tx, ty; 
    float s;      
};
 
inline ScaledOrthoProjectionParameters estimate_orthographic_projection_linear(
    std::vector<Eigen::Vector2f> image_points, std::vector<Eigen::Vector4f> model_points,
    bool is_viewport_upsidedown, cpp17::optional<int> viewport_height = cpp17::nullopt)
{
    using Eigen::Matrix;
    assert(image_points.size() == model_points.size());
    assert(image_points.size() >= 4); // Number of correspondence points given needs to be equal to or larger than 4
 
    const int num_correspondences = static_cast<int>(image_points.size());
 
    if (is_viewport_upsidedown)
    {
        if (viewport_height == cpp17::nullopt)
        {
            throw std::runtime_error(
                "Error: If is_viewport_upsidedown is set to true, viewport_height needs to be given.");
        }
        for (auto&& ip : image_points)
        {
            ip[1] = viewport_height.value() - ip[1];
        }
    }
 
    Matrix<float, Eigen::Dynamic, 8> A = Matrix<float, Eigen::Dynamic, 8>::Zero(2 * num_correspondences, 8);
    for (int i = 0; i < model_points.size(); ++i)
    {
        A.block<1, 4>(2 * i, 0) = model_points[i];       // even row - copy to left side (first row is row 0)
        A.block<1, 4>((2 * i) + 1, 4) = model_points[i]; // odd row - copy to right side
    } // 4th coord (homogeneous) is already 1
 
    Matrix<float, Eigen::Dynamic, 1> b(2 * num_correspondences, 1);
    for (int i = 0; i < image_points.size(); ++i)
    {
        b.segment<2>(2 * i) = image_points[i];
    }
 
    // The original implementation used SVD here to solve the linear system, but
    // QR seems to do the job fine too.
    const Matrix<float, 8, 1> k = A.colPivHouseholderQr().solve(b); // resulting affine matrix (8x1)
 
    // Extract all values from the estimated affine parameters k:
    const Eigen::Vector3f R1 = k.segment<3>(0);
    const Eigen::Vector3f R2 = k.segment<3>(4);
    const float sTx = k(3);
    const float sTy = k(7);
    const auto s = (R1.norm() + R2.norm()) / 2.0f;
    Eigen::Matrix3f R;
    Eigen::Vector3f r1 = R1.normalized(); // Not sure why R1.normalize() (in-place) produces a compiler error.
    Eigen::Vector3f r2 = R2.normalized();
    R.block<1, 3>(0, 0) = r1;
    R.block<1, 3>(1, 0) = r2;
    R.block<1, 3>(2, 0) = r1.cross(r2);
 
    // Set R to the closest orthonormal matrix to the estimated affine transform:
    Eigen::JacobiSVD<Eigen::Matrix3f, Eigen::NoQRPreconditioner> svd(R, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::Matrix3f U = svd.matrixU();
    const Eigen::Matrix3f V = svd.matrixV();
    Eigen::Matrix3f R_ortho = U * V.transpose();
 
    // The determinant of R must be 1 for it to be a valid rotation matrix
    if (R_ortho.determinant() < 0)
    {
        U.block<1, 3>(2, 0) = -U.block<1, 3>(2, 0); // not tested
        R_ortho = U * V.transpose();
    }
 
    // Remove the scale from the translations:
    const auto t1 = sTx / s;
    const auto t2 = sTy / s;
 
    return ScaledOrthoProjectionParameters{R_ortho, t1, t2, s};
};
 
} /* namespace fitting */
} /* namespace eos */
 
#endif /* EOS_ORTHOGRAPHIC_CAMERA_ESTIMATION_LINEAR_HPP */