//== INCLUDES =================================================================


#include "Energy_term_pca_landmarks_skinned_twoscans.h"
#include <graphene/surface_mesh/algorithms/templatefitting/utility/my_helper.h>

#include "../settings.h"


//== NAMESPACE ================================================================


namespace graphene {
namespace surface_mesh {


//== IMPLEMENTATION ===========================================================

#if 0

Energy_term_pca_landmarks_skinned_twoscans::
Energy_term_pca_landmarks_skinned_twoscans(const Energy_term_data& energy_term_data, double weight)
: Energy_term(energy_term_data, weight, "PCA Landmarks Skinned Two Scans")
{
}


//-----------------------------------------------------------------------------


Energy_term_pca_landmarks_skinned_twoscans::
~Energy_term_pca_landmarks_skinned_twoscans()
{}


//-----------------------------------------------------------------------------


void
Energy_term_pca_landmarks_skinned_twoscans::
pre_minimize_action()
{
    if (!energy_term_data_.character)
    {
        std::cerr << "[ERROR] - Energy_term_pca_landmarks_skinned_twoscans::!energy_term_data_.character - EXIT!" << std::endl;
        exit(1);
    }

    Surface_mesh::Vertex_property<Vec4f> vdepends = energy_term_data_.template_mesh->get_vertex_property<Vec4f>("v:skin_depend");
    Surface_mesh::Vertex_property<Vec4f> vweights = energy_term_data_.template_mesh->get_vertex_property<Vec4f>("v:skin_weight");
    Surface_mesh::Vertex_property<Vec4f> vdepends2 = energy_term_data_.template_mesh->get_vertex_property<Vec4f>("v:skin_depend2");
    Surface_mesh::Vertex_property<Vec4f> vweights2 = energy_term_data_.template_mesh->get_vertex_property<Vec4f>("v:skin_weight2");

    if (!(vdepends && vweights && vdepends2 && vweights2))
    {
        std::cerr << "[ERROR] - Energy_term_pca_landmarks_skinned_twoscans::pre_minimize_action() - EXIT!" << std::endl;
        exit(1);
    }

    const PCA& pca = energy_term_data_.pca;


    //
    // front_body
    //

    const std::vector<Mat4f>* pose_vector_front_body = energy_term_data_.pose_vector_front_body;
    surface_mesh::set_pose_vector(*pose_vector_front_body, energy_term_data_.character->skeleton());

    const std::vector<Mat4f>& skinning_tpose_to_scanpose_front_body = energy_term_data_.character->skeleton().skinning_matrices_;
    if (skinning_tpose_to_scanpose_front_body.size() == 0)
    {
        std::cerr << "[ERROR] - Energy_term_pca_landmarks_skinned_twoscans::pre_minimize_action() - skinning_tpose_to_scanpose_front_body.size(): " << skinning_tpose_to_scanpose_front_body.size() << std::endl;
        exit(1);
    }

    // compute T_j*H_i for a convenient access (for all joints j we store a (3*#verts X d)-matrix)
    // T_j are skinning-matrices
    // H_i are the three rows corresponding to vertex i
    Tj_times_Hi_front_body_.clear();
    for (unsigned int i = 0; i < skinning_tpose_to_scanpose_front_body.size(); ++i)
    {
        const Mat4f& current_skinning_mat = skinning_tpose_to_scanpose_front_body[i]; // current 4x4 skinning matrix

        Eigen::MatrixXd temp_mat(pca.P_pca_.rows(), pca.P_pca_.cols());

        for (unsigned int j = 0; j < energy_term_data_.template_mesh->n_vertices(); ++j)
        {
            // compute and set sub-matrix
            for (unsigned int k = 0; k < pca.P_pca_.cols(); ++k)
            {
                Vec3f p(pca.P_pca_(3*j+0, k), pca.P_pca_(3*j+1, k), pca.P_pca_(3*j+2, k));

                // current skinning_mat * sub-matrix
                Vec3f trans_p = affine_transform(current_skinning_mat, p);

                temp_mat(3*j+0, k) = trans_p[0];
                temp_mat(3*j+1, k) = trans_p[1];
                temp_mat(3*j+2, k) = trans_p[2];
            }
        }

        Tj_times_Hi_front_body_.push_back(temp_mat);
    }

    // also compute T_j*pca_mean for a convenient access (for all joints j we store a (3*#verts)-vector)
    Tj_times_pcamean_front_body_.clear();
    for (unsigned int i = 0; i < skinning_tpose_to_scanpose_front_body.size(); ++i)
    {
        const Mat4f& current_skinning_mat = skinning_tpose_to_scanpose_front_body[i]; // current 4x4 skinning matrix

        Eigen::VectorXd temp_vec(pca.m_pca_.rows());

        for (unsigned int j = 0; j < energy_term_data_.template_mesh->n_vertices(); ++j)
        {
            Vec3f p(pca.m_pca_(3*j+0), pca.m_pca_(3*j+1), pca.m_pca_(3*j+2));

            // current skinning_mat * sub-matrix
            Vec3f trans_p = affine_transform(current_skinning_mat, p);

            temp_vec(3*j+0) = trans_p[0];
            temp_vec(3*j+1) = trans_p[1];
            temp_vec(3*j+2) = trans_p[2];
        }

        Tj_times_pcamean_front_body_.push_back(temp_vec);
    }


    //
    // back_body
    //

    const std::vector<Mat4f>* pose_vector_back_body = energy_term_data_.pose_vector_back_body;
    surface_mesh::set_pose_vector(*pose_vector_back_body, energy_term_data_.character->skeleton());

    const std::vector<Mat4f>& skinning_tpose_to_scanpose_back_body = energy_term_data_.character->skeleton().skinning_matrices_;
    if (skinning_tpose_to_scanpose_back_body.size() == 0)
    {
        std::cerr << "[ERROR] - Energy_term_pca_landmarks_skinned_twoscans::pre_minimize_action() - skinning_tpose_to_scanpose_back_body.size(): " << skinning_tpose_to_scanpose_back_body.size() << std::endl;
        exit(1);
    }

    // compute T_j*H_i for a convenient access (for all joints j we store a (3*#verts X d)-matrix)
    // T_j are skinning-matrices
    // H_i are the three rows corresponding to vertex i
    Tj_times_Hi_back_body_.clear();
    for (unsigned int i = 0; i < skinning_tpose_to_scanpose_back_body.size(); ++i)
    {
        const Mat4f& current_skinning_mat = skinning_tpose_to_scanpose_back_body[i]; // current 4x4 skinning matrix

        Eigen::MatrixXd temp_mat(pca.P_pca_.rows(), pca.P_pca_.cols());

        for (unsigned int j = 0; j < energy_term_data_.template_mesh->n_vertices(); ++j)
        {
            // compute and set sub-matrix
            for (unsigned int k = 0; k < pca.P_pca_.cols(); ++k)
            {
                Vec3f p(pca.P_pca_(3*j+0, k), pca.P_pca_(3*j+1, k), pca.P_pca_(3*j+2, k));

                // current skinning_mat * sub-matrix
                Vec3f trans_p = affine_transform(current_skinning_mat, p);

                temp_mat(3*j+0, k) = trans_p[0];
                temp_mat(3*j+1, k) = trans_p[1];
                temp_mat(3*j+2, k) = trans_p[2];
            }
        }

        Tj_times_Hi_back_body_.push_back(temp_mat);
    }

    // also compute T_j*pca_mean for a convenient access (for all joints j we store a (3*#verts)-vector)
    Tj_times_pcamean_back_body_.clear();
    for (unsigned int i = 0; i < skinning_tpose_to_scanpose_back_body.size(); ++i)
    {
        const Mat4f& current_skinning_mat = skinning_tpose_to_scanpose_back_body[i]; // current 4x4 skinning matrix

        Eigen::VectorXd temp_vec(pca.m_pca_.rows());

        for (unsigned int j = 0; j < energy_term_data_.template_mesh->n_vertices(); ++j)
        {
            Vec3f p(pca.m_pca_(3*j+0), pca.m_pca_(3*j+1), pca.m_pca_(3*j+2));

            // current skinning_mat * sub-matrix
            Vec3f trans_p = affine_transform(current_skinning_mat, p);

            temp_vec(3*j+0) = trans_p[0];
            temp_vec(3*j+1) = trans_p[1];
            temp_vec(3*j+2) = trans_p[2];
        }

        Tj_times_pcamean_back_body_.push_back(temp_vec);
    }
}


//-----------------------------------------------------------------------------


double
Energy_term_pca_landmarks_skinned_twoscans::
evaluate()
{
    const Landmarks_manager& landmarks_manager = energy_term_data_.landmarks_manager;
    const PCA& pca = energy_term_data_.pca;
    const Point_set* point_set_front_body = energy_term_data_.point_set;
    const Point_set* point_set_back_body = energy_term_data_.point_set_back_body;
    const Surface_mesh* template_mesh = energy_term_data_.template_mesh;

    Surface_mesh::Vertex_property<Vec4f> vdepends = template_mesh->get_vertex_property<Vec4f>("v:skin_depend");
    Surface_mesh::Vertex_property<Vec4f> vweights = template_mesh->get_vertex_property<Vec4f>("v:skin_weight");
    Surface_mesh::Vertex_property<Vec4f> vdepends2 = template_mesh->get_vertex_property<Vec4f>("v:skin_depend2");
    Surface_mesh::Vertex_property<Vec4f> vweights2 = template_mesh->get_vertex_property<Vec4f>("v:skin_weight2");
    if (!(vdepends && vweights && vdepends2 && vweights2) ||
        Tj_times_Hi_front_body_.size() == 0 || Tj_times_pcamean_front_body_.size() == 0 ||
        Tj_times_Hi_back_body_.size() == 0 || Tj_times_pcamean_back_body_.size() == 0)
    {
        std::cerr << "DEBUG - Tj_times_Hi_front_body_.size(): " << Tj_times_Hi_front_body_.size() << std::endl;
        std::cerr << "DEBUG - Tj_times_pcamean_front_body_.size(): " << Tj_times_pcamean_front_body_.size() << std::endl;
        std::cerr << "DEBUG - Tj_times_Hi_back_body_.size(): " << Tj_times_Hi_back_body_.size() << std::endl;
        std::cerr << "DEBUG - Tj_times_pcamean_back_body_.size(): " << Tj_times_pcamean_back_body_.size() << std::endl;
        std::cerr << "[ERROR] - Energy_term_pca_landmarks_skinned_twoscans::evaluate() - EXIT!" << std::endl;
        exit(1);
    }

    std::vector<Point> lm_ps_front_body;
    lm_ps_front_body.resize(point_set_front_body->landmarks_.size());
    unsigned int i = 0;
    for (auto v : point_set_front_body->landmarks_)
    {
        lm_ps_front_body[i] = point_set_front_body->points_[v];
        ++i;
    }

    std::vector<Point> lm_ps_back_body;
    lm_ps_back_body.resize(point_set_back_body->landmarks_.size());
    i = 0;
    for (auto v : point_set_back_body->landmarks_)
    {
        lm_ps_back_body[i] = point_set_back_body->points_[v];
        ++i;
    }

    Surface_mesh::Mesh_property< std::vector<Surface_mesh::Vertex> > lm_prop_front_body = template_mesh->get_mesh_property< std::vector<Surface_mesh::Vertex> >("m:landmarks_front_body");
    if (!lm_prop_front_body)
    {
        std::cerr << "Energy_term_pca_landmarks_skinned_twoscans::evaluate(): [ERROR] !lm_prop_front_body." << std::endl;
        return DBL_MAX;
    }
    const std::vector<Surface_mesh::Vertex>& tm_landmarks_front_body = lm_prop_front_body[0];
    Surface_mesh::Vertex_property<double> local_landmark_weight_front_body = template_mesh->get_vertex_property<double>("v:landmark_weight_front_body");

    Surface_mesh::Mesh_property< std::vector<Surface_mesh::Vertex> > lm_prop_back_body = template_mesh->get_mesh_property< std::vector<Surface_mesh::Vertex> >("m:landmarks_back_body");
    if (!lm_prop_back_body)
    {
        std::cerr << "Energy_term_pca_landmarks_skinned_twoscans::evaluate(): [ERROR] !lm_prop_back_body." << std::endl;
        return DBL_MAX;
    }
    const std::vector<Surface_mesh::Vertex>& tm_landmarks_back_body = lm_prop_back_body[0];
    Surface_mesh::Vertex_property<double> local_landmark_weight_back_body = template_mesh->get_vertex_property<double>("v:landmark_weight_back_body");

    // check dimensions
    if (tm_landmarks_front_body.size() != lm_ps_front_body.size() ||
        tm_landmarks_back_body.size()  != lm_ps_back_body.size())
    {
        std::cerr << "Energy_term_pca_landmarks_skinned_twoscans::evaluate(): [ERROR] Number of landmarks do not match for front_body and/or back_body." << std::endl;
        return DBL_MAX;
    }

    double error_reg_pca_ff = 0.0;

    const unsigned int L_front(lm_ps_front_body.size());
    const unsigned int L_back(lm_ps_back_body.size());
    const unsigned int L_total = L_front + L_back;

    for (unsigned int i = 0; i < L_front; ++i)
    {
        double local_weight = 1.0;
        if (local_landmark_weight_front_body)
        {
            local_weight = local_landmark_weight_front_body[tm_landmarks_front_body[i]];
            if (local_weight < 1e-5)
                continue;
            local_weight = sqrt(local_weight);
        }

        const int idx = tm_landmarks_front_body[i].idx();

        Surface_mesh::Vertex v_idx(idx);

        int dep0 = (int)vdepends[v_idx][0];
        float w0 = vweights[v_idx][0];
        int dep1 = (int)vdepends[v_idx][1];
        float w1 = vweights[v_idx][1];
        int dep2 = (int)vdepends[v_idx][2];
        float w2 = vweights[v_idx][2];
        int dep3 = (int)vdepends[v_idx][3];
        float w3 = vweights[v_idx][3];
        int dep4 = (int)vdepends2[v_idx][0];
        float w4 = vweights2[v_idx][0];
        int dep5 = (int)vdepends2[v_idx][1];
        float w5 = vweights2[v_idx][1];
        int dep6 = (int)vdepends2[v_idx][2];
        float w6 = vweights2[v_idx][2];
        int dep7 = (int)vdepends2[v_idx][3];
        float w7 = vweights2[v_idx][3];

        Eigen::MatrixXd part_mat(3, pca.P_pca_.cols());
        part_mat.setZero();
        for (unsigned int j = 0; j < pca.P_pca_.cols(); ++j)
        {
            part_mat(0, j)  = w0*Tj_times_Hi_front_body_[dep0](3*idx+0, j);
            part_mat(1, j)  = w0*Tj_times_Hi_front_body_[dep0](3*idx+1, j);
            part_mat(2, j)  = w0*Tj_times_Hi_front_body_[dep0](3*idx+2, j);

            part_mat(0, j) += w1*Tj_times_Hi_front_body_[dep1](3*idx+0, j);
            part_mat(1, j) += w1*Tj_times_Hi_front_body_[dep1](3*idx+1, j);
            part_mat(2, j) += w1*Tj_times_Hi_front_body_[dep1](3*idx+2, j);

            part_mat(0, j) += w2*Tj_times_Hi_front_body_[dep2](3*idx+0, j);
            part_mat(1, j) += w2*Tj_times_Hi_front_body_[dep2](3*idx+1, j);
            part_mat(2, j) += w2*Tj_times_Hi_front_body_[dep2](3*idx+2, j);

            part_mat(0, j) += w3*Tj_times_Hi_front_body_[dep3](3*idx+0, j);
            part_mat(1, j) += w3*Tj_times_Hi_front_body_[dep3](3*idx+1, j);
            part_mat(2, j) += w3*Tj_times_Hi_front_body_[dep3](3*idx+2, j);

            part_mat(0, j) += w4*Tj_times_Hi_front_body_[dep4](3*idx+0, j);
            part_mat(1, j) += w4*Tj_times_Hi_front_body_[dep4](3*idx+1, j);
            part_mat(2, j) += w4*Tj_times_Hi_front_body_[dep4](3*idx+2, j);

            part_mat(0, j) += w5*Tj_times_Hi_front_body_[dep5](3*idx+0, j);
            part_mat(1, j) += w5*Tj_times_Hi_front_body_[dep5](3*idx+1, j);
            part_mat(2, j) += w5*Tj_times_Hi_front_body_[dep5](3*idx+2, j);

            part_mat(0, j) += w6*Tj_times_Hi_front_body_[dep6](3*idx+0, j);
            part_mat(1, j) += w6*Tj_times_Hi_front_body_[dep6](3*idx+1, j);
            part_mat(2, j) += w6*Tj_times_Hi_front_body_[dep6](3*idx+2, j);

            part_mat(0, j) += w7*Tj_times_Hi_front_body_[dep7](3*idx+0, j);
            part_mat(1, j) += w7*Tj_times_Hi_front_body_[dep7](3*idx+1, j);
            part_mat(2, j) += w7*Tj_times_Hi_front_body_[dep7](3*idx+2, j);
        }

        Vec3f part_vec(0);
        part_vec[0]  = w0*Tj_times_pcamean_front_body_[dep0](3*idx+0);
        part_vec[1]  = w0*Tj_times_pcamean_front_body_[dep0](3*idx+1);
        part_vec[2]  = w0*Tj_times_pcamean_front_body_[dep0](3*idx+2);
        part_vec[0] += w1*Tj_times_pcamean_front_body_[dep1](3*idx+0);
        part_vec[1] += w1*Tj_times_pcamean_front_body_[dep1](3*idx+1);
        part_vec[2] += w1*Tj_times_pcamean_front_body_[dep1](3*idx+2);
        part_vec[0] += w2*Tj_times_pcamean_front_body_[dep2](3*idx+0);
        part_vec[1] += w2*Tj_times_pcamean_front_body_[dep2](3*idx+1);
        part_vec[2] += w2*Tj_times_pcamean_front_body_[dep2](3*idx+2);
        part_vec[0] += w3*Tj_times_pcamean_front_body_[dep3](3*idx+0);
        part_vec[1] += w3*Tj_times_pcamean_front_body_[dep3](3*idx+1);
        part_vec[2] += w3*Tj_times_pcamean_front_body_[dep3](3*idx+2);
        part_vec[0] += w4*Tj_times_pcamean_front_body_[dep4](3*idx+0);
        part_vec[1] += w4*Tj_times_pcamean_front_body_[dep4](3*idx+1);
        part_vec[2] += w4*Tj_times_pcamean_front_body_[dep4](3*idx+2);
        part_vec[0] += w5*Tj_times_pcamean_front_body_[dep5](3*idx+0);
        part_vec[1] += w5*Tj_times_pcamean_front_body_[dep5](3*idx+1);
        part_vec[2] += w5*Tj_times_pcamean_front_body_[dep5](3*idx+2);
        part_vec[0] += w6*Tj_times_pcamean_front_body_[dep6](3*idx+0);
        part_vec[1] += w6*Tj_times_pcamean_front_body_[dep6](3*idx+1);
        part_vec[2] += w6*Tj_times_pcamean_front_body_[dep6](3*idx+2);
        part_vec[0] += w7*Tj_times_pcamean_front_body_[dep7](3*idx+0);
        part_vec[1] += w7*Tj_times_pcamean_front_body_[dep7](3*idx+1);
        part_vec[2] += w7*Tj_times_pcamean_front_body_[dep7](3*idx+2);

        Point the_point(0.0, 0.0, 0.0);
        for (unsigned int j = 0; j < 3; ++j)
        {
            for (unsigned int dcount = 0; dcount < pca.dim_pca_model_; ++dcount)
            {
                the_point[j] += part_mat(j, dcount) * pca.d_pca_(dcount) + part_vec[j];
            }
        }

        double u = norm( the_point - lm_ps_front_body[i] );
        if ( robustness_parameters_.solver_type == SOLVER_TYPE_L2 )
        {
            error_reg_pca_ff += u * u * local_weight;
        }
        else if ( robustness_parameters_.solver_type == SOLVER_TYPE_L1 )
        {
            error_reg_pca_ff += std::pow( u, robustness_parameters_.parameter) * local_weight;
        }
        else if ( robustness_parameters_.solver_type == SOLVER_TYPE_HUBER )
        {
            double huber_thr = robustness_parameters_.parameter;
            if ( u > huber_thr )
            {
                error_reg_pca_ff += ( huber_thr*u - huber_thr*huber_thr/2.0 ) * local_weight;
            }
            else
            {
                error_reg_pca_ff += ( u*u/2.0 ) * local_weight;
            }
        }
        else if ( robustness_parameters_.solver_type == SOLVER_TYPE_GEMANMCCLURE )
        {
            double scale_parameter = robustness_parameters_.parameter;
//                error_reg_pca_ff += ( u*u / (scale_parameter*scale_parameter + u*u) ); // VERSION 2
              error_reg_pca_ff += ( ( u*u / ( 2.0 * ( 1.0 + (u*u/(scale_parameter*scale_parameter)) ) ) ) ) * local_weight; // VERSION 3
        }
        else // default to L2
        {
            error_reg_pca_ff += u * u * local_weight;
        }
    }


    for (unsigned int i = 0; i < L_back; ++i)
    {
        double local_weight = 1.0;
        if (local_landmark_weight_back_body)
        {
            local_weight = local_landmark_weight_back_body[tm_landmarks_back_body[i]];
            if (local_weight < 1e-5)
                continue;
            local_weight = sqrt(local_weight);
        }

        const int idx = tm_landmarks_back_body[i].idx();

        Surface_mesh::Vertex v_idx(idx);

        int dep0 = (int)vdepends[v_idx][0];
        float w0 = vweights[v_idx][0];
        int dep1 = (int)vdepends[v_idx][1];
        float w1 = vweights[v_idx][1];
        int dep2 = (int)vdepends[v_idx][2];
        float w2 = vweights[v_idx][2];
        int dep3 = (int)vdepends[v_idx][3];
        float w3 = vweights[v_idx][3];
        int dep4 = (int)vdepends2[v_idx][0];
        float w4 = vweights2[v_idx][0];
        int dep5 = (int)vdepends2[v_idx][1];
        float w5 = vweights2[v_idx][1];
        int dep6 = (int)vdepends2[v_idx][2];
        float w6 = vweights2[v_idx][2];
        int dep7 = (int)vdepends2[v_idx][3];
        float w7 = vweights2[v_idx][3];

        Eigen::MatrixXd part_mat(3, pca.P_pca_.cols());
        part_mat.setZero();
        for (unsigned int j = 0; j < pca.P_pca_.cols(); ++j)
        {
            part_mat(0, j)  = w0*Tj_times_Hi_back_body_[dep0](3*idx+0, j);
            part_mat(1, j)  = w0*Tj_times_Hi_back_body_[dep0](3*idx+1, j);
            part_mat(2, j)  = w0*Tj_times_Hi_back_body_[dep0](3*idx+2, j);

            part_mat(0, j) += w1*Tj_times_Hi_back_body_[dep1](3*idx+0, j);
            part_mat(1, j) += w1*Tj_times_Hi_back_body_[dep1](3*idx+1, j);
            part_mat(2, j) += w1*Tj_times_Hi_back_body_[dep1](3*idx+2, j);

            part_mat(0, j) += w2*Tj_times_Hi_back_body_[dep2](3*idx+0, j);
            part_mat(1, j) += w2*Tj_times_Hi_back_body_[dep2](3*idx+1, j);
            part_mat(2, j) += w2*Tj_times_Hi_back_body_[dep2](3*idx+2, j);

            part_mat(0, j) += w3*Tj_times_Hi_back_body_[dep3](3*idx+0, j);
            part_mat(1, j) += w3*Tj_times_Hi_back_body_[dep3](3*idx+1, j);
            part_mat(2, j) += w3*Tj_times_Hi_back_body_[dep3](3*idx+2, j);

            part_mat(0, j) += w4*Tj_times_Hi_back_body_[dep4](3*idx+0, j);
            part_mat(1, j) += w4*Tj_times_Hi_back_body_[dep4](3*idx+1, j);
            part_mat(2, j) += w4*Tj_times_Hi_back_body_[dep4](3*idx+2, j);

            part_mat(0, j) += w5*Tj_times_Hi_back_body_[dep5](3*idx+0, j);
            part_mat(1, j) += w5*Tj_times_Hi_back_body_[dep5](3*idx+1, j);
            part_mat(2, j) += w5*Tj_times_Hi_back_body_[dep5](3*idx+2, j);

            part_mat(0, j) += w6*Tj_times_Hi_back_body_[dep6](3*idx+0, j);
            part_mat(1, j) += w6*Tj_times_Hi_back_body_[dep6](3*idx+1, j);
            part_mat(2, j) += w6*Tj_times_Hi_back_body_[dep6](3*idx+2, j);

            part_mat(0, j) += w7*Tj_times_Hi_back_body_[dep7](3*idx+0, j);
            part_mat(1, j) += w7*Tj_times_Hi_back_body_[dep7](3*idx+1, j);
            part_mat(2, j) += w7*Tj_times_Hi_back_body_[dep7](3*idx+2, j);
        }

        Vec3f part_vec(0);
        part_vec[0]  = w0*Tj_times_pcamean_back_body_[dep0](3*idx+0);
        part_vec[1]  = w0*Tj_times_pcamean_back_body_[dep0](3*idx+1);
        part_vec[2]  = w0*Tj_times_pcamean_back_body_[dep0](3*idx+2);
        part_vec[0] += w1*Tj_times_pcamean_back_body_[dep1](3*idx+0);
        part_vec[1] += w1*Tj_times_pcamean_back_body_[dep1](3*idx+1);
        part_vec[2] += w1*Tj_times_pcamean_back_body_[dep1](3*idx+2);
        part_vec[0] += w2*Tj_times_pcamean_back_body_[dep2](3*idx+0);
        part_vec[1] += w2*Tj_times_pcamean_back_body_[dep2](3*idx+1);
        part_vec[2] += w2*Tj_times_pcamean_back_body_[dep2](3*idx+2);
        part_vec[0] += w3*Tj_times_pcamean_back_body_[dep3](3*idx+0);
        part_vec[1] += w3*Tj_times_pcamean_back_body_[dep3](3*idx+1);
        part_vec[2] += w3*Tj_times_pcamean_back_body_[dep3](3*idx+2);
        part_vec[0] += w4*Tj_times_pcamean_back_body_[dep4](3*idx+0);
        part_vec[1] += w4*Tj_times_pcamean_back_body_[dep4](3*idx+1);
        part_vec[2] += w4*Tj_times_pcamean_back_body_[dep4](3*idx+2);
        part_vec[0] += w5*Tj_times_pcamean_back_body_[dep5](3*idx+0);
        part_vec[1] += w5*Tj_times_pcamean_back_body_[dep5](3*idx+1);
        part_vec[2] += w5*Tj_times_pcamean_back_body_[dep5](3*idx+2);
        part_vec[0] += w6*Tj_times_pcamean_back_body_[dep6](3*idx+0);
        part_vec[1] += w6*Tj_times_pcamean_back_body_[dep6](3*idx+1);
        part_vec[2] += w6*Tj_times_pcamean_back_body_[dep6](3*idx+2);
        part_vec[0] += w7*Tj_times_pcamean_back_body_[dep7](3*idx+0);
        part_vec[1] += w7*Tj_times_pcamean_back_body_[dep7](3*idx+1);
        part_vec[2] += w7*Tj_times_pcamean_back_body_[dep7](3*idx+2);

        Point the_point(0.0, 0.0, 0.0);
        for (unsigned int j = 0; j < 3; ++j)
        {
            for (unsigned int dcount = 0; dcount < pca.dim_pca_model_; ++dcount)
            {
                the_point[j] += part_mat(j, dcount) * pca.d_pca_(dcount) + part_vec[j];
            }
        }

        double u = norm( the_point - lm_ps_back_body[i] );
        if ( robustness_parameters_.solver_type == SOLVER_TYPE_L2 )
        {
            error_reg_pca_ff += u * u * local_weight;
        }
        else if ( robustness_parameters_.solver_type == SOLVER_TYPE_L1 )
        {
            error_reg_pca_ff += std::pow( u, robustness_parameters_.parameter) * local_weight;
        }
        else if ( robustness_parameters_.solver_type == SOLVER_TYPE_HUBER )
        {
            double huber_thr = robustness_parameters_.parameter;
            if ( u > huber_thr )
            {
                error_reg_pca_ff += ( huber_thr*u - huber_thr*huber_thr/2.0 ) * local_weight;
            }
            else
            {
                error_reg_pca_ff += ( u*u/2.0 ) * local_weight;
            }
        }
        else if ( robustness_parameters_.solver_type == SOLVER_TYPE_GEMANMCCLURE )
        {
            double scale_parameter = robustness_parameters_.parameter;
//                error_reg_pca_ff += ( u*u / (scale_parameter*scale_parameter + u*u) ); // VERSION 2
              error_reg_pca_ff += ( ( u*u / ( 2.0 * ( 1.0 + (u*u/(scale_parameter*scale_parameter)) ) ) ) ) * local_weight; // VERSION 3
        }
        else // default to L2
        {
            error_reg_pca_ff += u * u * local_weight;
        }
    }


    error_reg_pca_ff *= (weight_ / L_total);

    return error_reg_pca_ff;
}


//-----------------------------------------------------------------------------


void
Energy_term_pca_landmarks_skinned_twoscans::
add_rows_big_lse( std::vector<Tripl>& coeffs,
                  Eigen::VectorXd& b,
                  unsigned int& r )
{
    //std::cout << "[INFO] Energy_term_pca_landmarks_skinned_twoscans::add_rows_big_lse(): weight_: " << weight_ << std::endl;

    const Column_offsets& column_offsets = energy_term_data_.column_offsets;
    const unsigned int offset_nrd_w_pca = column_offsets.offset_nrd_w_pca;
    const PCA& pca = energy_term_data_.pca;
    const Landmarks_manager& landmarks_manager = energy_term_data_.landmarks_manager;
    const Point_set* point_set_front_body = energy_term_data_.point_set;
    const Point_set* point_set_back_body = energy_term_data_.point_set_back_body;
    const Surface_mesh* template_mesh = energy_term_data_.template_mesh;

    Surface_mesh::Vertex_property<Vec4f> vdepends = template_mesh->get_vertex_property<Vec4f>("v:skin_depend");
    Surface_mesh::Vertex_property<Vec4f> vweights = template_mesh->get_vertex_property<Vec4f>("v:skin_weight");
    Surface_mesh::Vertex_property<Vec4f> vdepends2 = template_mesh->get_vertex_property<Vec4f>("v:skin_depend2");
    Surface_mesh::Vertex_property<Vec4f> vweights2 = template_mesh->get_vertex_property<Vec4f>("v:skin_weight2");
    if (!(vdepends && vweights && vdepends2 && vweights2) ||
        Tj_times_Hi_front_body_.size() == 0 || Tj_times_pcamean_front_body_.size() == 0 ||
        Tj_times_Hi_back_body_.size() == 0 || Tj_times_pcamean_back_body_.size() == 0)
    {
        std::cerr << "DEBUG - Tj_times_Hi_front_body_.size(): " << Tj_times_Hi_front_body_.size() << std::endl;
        std::cerr << "DEBUG - Tj_times_pcamean_front_body_.size(): " << Tj_times_pcamean_front_body_.size() << std::endl;
        std::cerr << "DEBUG - Tj_times_Hi_back_body_.size(): " << Tj_times_Hi_back_body_.size() << std::endl;
        std::cerr << "DEBUG - Tj_times_pcamean_back_body_.size(): " << Tj_times_pcamean_back_body_.size() << std::endl;
        std::cerr << "[ERROR] - Energy_term_pca_landmarks_skinned_twoscans::add_rows_big_lse() - EXIT!" << std::endl;
        exit(1);
    }

    std::vector<Point> lm_ps_front_body;
    lm_ps_front_body.resize(point_set_front_body->landmarks_.size());
    unsigned int i = 0;
    for (auto v : point_set_front_body->landmarks_)
    {
        lm_ps_front_body[i] = point_set_front_body->points_[v];
        ++i;
    }

    std::vector<Point> lm_ps_back_body;
    lm_ps_back_body.resize(point_set_back_body->landmarks_.size());
    i = 0;
    for (auto v : point_set_back_body->landmarks_)
    {
        lm_ps_back_body[i] = point_set_back_body->points_[v];
        ++i;
    }

    Surface_mesh::Mesh_property< std::vector<Surface_mesh::Vertex> > lm_prop_front_body = template_mesh->get_mesh_property< std::vector<Surface_mesh::Vertex> >("m:landmarks_front_body");
    if (!lm_prop_front_body)
    {
        std::cerr << "Energy_term_pca_landmarks_skinned_twoscans::add_rows_big_lse(): [ERROR] !lm_prop_front_body." << std::endl;
        return;
    }
    const std::vector<Surface_mesh::Vertex>& tm_landmarks_front_body = lm_prop_front_body[0];
    Surface_mesh::Vertex_property<double> local_landmark_weight_front_body = template_mesh->get_vertex_property<double>("v:landmark_weight_front_body");

    Surface_mesh::Mesh_property< std::vector<Surface_mesh::Vertex> > lm_prop_back_body = template_mesh->get_mesh_property< std::vector<Surface_mesh::Vertex> >("m:landmarks_back_body");
    if (!lm_prop_back_body)
    {
        std::cerr << "Energy_term_pca_landmarks_skinned_twoscans::add_rows_big_lse(): [ERROR] !lm_prop_back_body." << std::endl;
        return;
    }
    const std::vector<Surface_mesh::Vertex>& tm_landmarks_back_body = lm_prop_back_body[0];
    Surface_mesh::Vertex_property<double> local_landmark_weight_back_body = template_mesh->get_vertex_property<double>("v:landmark_weight_back_body");

    // check dimensions
    if (tm_landmarks_front_body.size() != lm_ps_front_body.size() ||
        tm_landmarks_back_body.size()  != lm_ps_back_body.size())
    {
        std::cerr << "Energy_term_pca_landmarks_skinned_twoscans::add_rows_big_lse(): [ERROR] Number of landmarks do not match for front_body and/or back_body." << std::endl;
        return;
    }

    const unsigned int L_front(lm_ps_front_body.size());
    const unsigned int L_back(lm_ps_back_body.size());
    const unsigned int L_total = L_front + L_back;

    const double weight_model = sqrt(weight_ / L_total);
    //std::cout << "[INFO] Energy_term_pca_landmarks_skinned_twoscans::add_rows_big_lse(): weight_model: " << weight_model << std::endl;


    for (unsigned int i = 0; i < L_front; ++i)
    {
        int idx = tm_landmarks_front_body[i].idx();

        double local_weight = 1.0;
        if (local_landmark_weight_front_body)
        {
            local_weight = local_landmark_weight_front_body[tm_landmarks_front_body[i]];
            if (local_weight < 1e-5)
            {
                continue;
            }
            local_weight = sqrt(local_weight);
        }

        Surface_mesh::Vertex v_idx(idx);
        int dep0 = (int)vdepends[v_idx][0];
        float w0 = vweights[v_idx][0];
        int dep1 = (int)vdepends[v_idx][1];
        float w1 = vweights[v_idx][1];
        int dep2 = (int)vdepends[v_idx][2];
        float w2 = vweights[v_idx][2];
        int dep3 = (int)vdepends[v_idx][3];
        float w3 = vweights[v_idx][3];
        int dep4 = (int)vdepends2[v_idx][0];
        float w4 = vweights2[v_idx][0];
        int dep5 = (int)vdepends2[v_idx][1];
        float w5 = vweights2[v_idx][1];
        int dep6 = (int)vdepends2[v_idx][2];
        float w6 = vweights2[v_idx][2];
        int dep7 = (int)vdepends2[v_idx][3];
        float w7 = vweights2[v_idx][3];

        // compute RHS part
        Vec3f rhs_part(0);
        rhs_part[0]  = w0*Tj_times_pcamean_front_body_[dep0](3*idx+0);
        rhs_part[1]  = w0*Tj_times_pcamean_front_body_[dep0](3*idx+1);
        rhs_part[2]  = w0*Tj_times_pcamean_front_body_[dep0](3*idx+2);
        rhs_part[0] += w1*Tj_times_pcamean_front_body_[dep1](3*idx+0);
        rhs_part[1] += w1*Tj_times_pcamean_front_body_[dep1](3*idx+1);
        rhs_part[2] += w1*Tj_times_pcamean_front_body_[dep1](3*idx+2);
        rhs_part[0] += w2*Tj_times_pcamean_front_body_[dep2](3*idx+0);
        rhs_part[1] += w2*Tj_times_pcamean_front_body_[dep2](3*idx+1);
        rhs_part[2] += w2*Tj_times_pcamean_front_body_[dep2](3*idx+2);
        rhs_part[0] += w3*Tj_times_pcamean_front_body_[dep3](3*idx+0);
        rhs_part[1] += w3*Tj_times_pcamean_front_body_[dep3](3*idx+1);
        rhs_part[2] += w3*Tj_times_pcamean_front_body_[dep3](3*idx+2);
        rhs_part[0] += w4*Tj_times_pcamean_front_body_[dep4](3*idx+0);
        rhs_part[1] += w4*Tj_times_pcamean_front_body_[dep4](3*idx+1);
        rhs_part[2] += w4*Tj_times_pcamean_front_body_[dep4](3*idx+2);
        rhs_part[0] += w5*Tj_times_pcamean_front_body_[dep5](3*idx+0);
        rhs_part[1] += w5*Tj_times_pcamean_front_body_[dep5](3*idx+1);
        rhs_part[2] += w5*Tj_times_pcamean_front_body_[dep5](3*idx+2);
        rhs_part[0] += w6*Tj_times_pcamean_front_body_[dep6](3*idx+0);
        rhs_part[1] += w6*Tj_times_pcamean_front_body_[dep6](3*idx+1);
        rhs_part[2] += w6*Tj_times_pcamean_front_body_[dep6](3*idx+2);
        rhs_part[0] += w7*Tj_times_pcamean_front_body_[dep7](3*idx+0);
        rhs_part[1] += w7*Tj_times_pcamean_front_body_[dep7](3*idx+1);
        rhs_part[2] += w7*Tj_times_pcamean_front_body_[dep7](3*idx+2);

        // compute LHS part
        Eigen::MatrixXd lhs_part(3, pca.P_pca_.cols());
        lhs_part.setZero();
        for (unsigned int j = 0; j < pca.P_pca_.cols(); ++j)
        {
            lhs_part(0, j)  = w0*Tj_times_Hi_front_body_[dep0](3*idx+0, j);
            lhs_part(1, j)  = w0*Tj_times_Hi_front_body_[dep0](3*idx+1, j);
            lhs_part(2, j)  = w0*Tj_times_Hi_front_body_[dep0](3*idx+2, j);

            lhs_part(0, j) += w1*Tj_times_Hi_front_body_[dep1](3*idx+0, j);
            lhs_part(1, j) += w1*Tj_times_Hi_front_body_[dep1](3*idx+1, j);
            lhs_part(2, j) += w1*Tj_times_Hi_front_body_[dep1](3*idx+2, j);

            lhs_part(0, j) += w2*Tj_times_Hi_front_body_[dep2](3*idx+0, j);
            lhs_part(1, j) += w2*Tj_times_Hi_front_body_[dep2](3*idx+1, j);
            lhs_part(2, j) += w2*Tj_times_Hi_front_body_[dep2](3*idx+2, j);

            lhs_part(0, j) += w3*Tj_times_Hi_front_body_[dep3](3*idx+0, j);
            lhs_part(1, j) += w3*Tj_times_Hi_front_body_[dep3](3*idx+1, j);
            lhs_part(2, j) += w3*Tj_times_Hi_front_body_[dep3](3*idx+2, j);

            lhs_part(0, j) += w4*Tj_times_Hi_front_body_[dep4](3*idx+0, j);
            lhs_part(1, j) += w4*Tj_times_Hi_front_body_[dep4](3*idx+1, j);
            lhs_part(2, j) += w4*Tj_times_Hi_front_body_[dep4](3*idx+2, j);

            lhs_part(0, j) += w5*Tj_times_Hi_front_body_[dep5](3*idx+0, j);
            lhs_part(1, j) += w5*Tj_times_Hi_front_body_[dep5](3*idx+1, j);
            lhs_part(2, j) += w5*Tj_times_Hi_front_body_[dep5](3*idx+2, j);

            lhs_part(0, j) += w6*Tj_times_Hi_front_body_[dep6](3*idx+0, j);
            lhs_part(1, j) += w6*Tj_times_Hi_front_body_[dep6](3*idx+1, j);
            lhs_part(2, j) += w6*Tj_times_Hi_front_body_[dep6](3*idx+2, j);

            lhs_part(0, j) += w7*Tj_times_Hi_front_body_[dep7](3*idx+0, j);
            lhs_part(1, j) += w7*Tj_times_Hi_front_body_[dep7](3*idx+1, j);
            lhs_part(2, j) += w7*Tj_times_Hi_front_body_[dep7](3*idx+2, j);
        }

        // right hand side
        b(r) = local_weight * weight_model * ( lm_ps_front_body[i][0] - rhs_part[0] );

        // begin_row
        for (unsigned int dcount = 0; dcount < pca.dim_pca_model_; ++dcount)
        {
            coeffs.push_back( Tripl(r, offset_nrd_w_pca + dcount, lhs_part(0, dcount) * local_weight * weight_model) ); // d's
        }
        ++r;

        // right hand side
        b(r) = local_weight * weight_model * ( lm_ps_front_body[i][1] - rhs_part[1] );

        // begin_row
        for (unsigned int dcount = 0; dcount < pca.dim_pca_model_; ++dcount)
        {
            coeffs.push_back( Tripl(r, offset_nrd_w_pca + dcount, lhs_part(1, dcount) * local_weight * weight_model) ); // d's
        }
        ++r;

        // right hand side
        b(r) = local_weight * weight_model * ( lm_ps_front_body[i][2] - rhs_part[2] );

        // begin_row
        for (unsigned int dcount = 0; dcount < pca.dim_pca_model_; ++dcount)
        {
            coeffs.push_back( Tripl(r, offset_nrd_w_pca + dcount, lhs_part(2, dcount) * local_weight * weight_model) ); // d's
        }
        ++r;
    }


    for (unsigned int i = 0; i < L_back; ++i)
    {
        int idx = tm_landmarks_back_body[i].idx();

        double local_weight = 1.0;
        if (local_landmark_weight_back_body)
        {
            local_weight = local_landmark_weight_back_body[tm_landmarks_back_body[i]];
            if (local_weight < 1e-5)
            {
                continue;
            }
            local_weight = sqrt(local_weight);
        }

        Surface_mesh::Vertex v_idx(idx);
        int dep0 = (int)vdepends[v_idx][0];
        float w0 = vweights[v_idx][0];
        int dep1 = (int)vdepends[v_idx][1];
        float w1 = vweights[v_idx][1];
        int dep2 = (int)vdepends[v_idx][2];
        float w2 = vweights[v_idx][2];
        int dep3 = (int)vdepends[v_idx][3];
        float w3 = vweights[v_idx][3];
        int dep4 = (int)vdepends2[v_idx][0];
        float w4 = vweights2[v_idx][0];
        int dep5 = (int)vdepends2[v_idx][1];
        float w5 = vweights2[v_idx][1];
        int dep6 = (int)vdepends2[v_idx][2];
        float w6 = vweights2[v_idx][2];
        int dep7 = (int)vdepends2[v_idx][3];
        float w7 = vweights2[v_idx][3];

        // compute RHS part
        Vec3f rhs_part(0);
        rhs_part[0]  = w0*Tj_times_pcamean_back_body_[dep0](3*idx+0);
        rhs_part[1]  = w0*Tj_times_pcamean_back_body_[dep0](3*idx+1);
        rhs_part[2]  = w0*Tj_times_pcamean_back_body_[dep0](3*idx+2);
        rhs_part[0] += w1*Tj_times_pcamean_back_body_[dep1](3*idx+0);
        rhs_part[1] += w1*Tj_times_pcamean_back_body_[dep1](3*idx+1);
        rhs_part[2] += w1*Tj_times_pcamean_back_body_[dep1](3*idx+2);
        rhs_part[0] += w2*Tj_times_pcamean_back_body_[dep2](3*idx+0);
        rhs_part[1] += w2*Tj_times_pcamean_back_body_[dep2](3*idx+1);
        rhs_part[2] += w2*Tj_times_pcamean_back_body_[dep2](3*idx+2);
        rhs_part[0] += w3*Tj_times_pcamean_back_body_[dep3](3*idx+0);
        rhs_part[1] += w3*Tj_times_pcamean_back_body_[dep3](3*idx+1);
        rhs_part[2] += w3*Tj_times_pcamean_back_body_[dep3](3*idx+2);
        rhs_part[0] += w4*Tj_times_pcamean_back_body_[dep4](3*idx+0);
        rhs_part[1] += w4*Tj_times_pcamean_back_body_[dep4](3*idx+1);
        rhs_part[2] += w4*Tj_times_pcamean_back_body_[dep4](3*idx+2);
        rhs_part[0] += w5*Tj_times_pcamean_back_body_[dep5](3*idx+0);
        rhs_part[1] += w5*Tj_times_pcamean_back_body_[dep5](3*idx+1);
        rhs_part[2] += w5*Tj_times_pcamean_back_body_[dep5](3*idx+2);
        rhs_part[0] += w6*Tj_times_pcamean_back_body_[dep6](3*idx+0);
        rhs_part[1] += w6*Tj_times_pcamean_back_body_[dep6](3*idx+1);
        rhs_part[2] += w6*Tj_times_pcamean_back_body_[dep6](3*idx+2);
        rhs_part[0] += w7*Tj_times_pcamean_back_body_[dep7](3*idx+0);
        rhs_part[1] += w7*Tj_times_pcamean_back_body_[dep7](3*idx+1);
        rhs_part[2] += w7*Tj_times_pcamean_back_body_[dep7](3*idx+2);

        // compute LHS part
        Eigen::MatrixXd lhs_part(3, pca.P_pca_.cols());
        lhs_part.setZero();
        for (unsigned int j = 0; j < pca.P_pca_.cols(); ++j)
        {
            lhs_part(0, j)  = w0*Tj_times_Hi_back_body_[dep0](3*idx+0, j);
            lhs_part(1, j)  = w0*Tj_times_Hi_back_body_[dep0](3*idx+1, j);
            lhs_part(2, j)  = w0*Tj_times_Hi_back_body_[dep0](3*idx+2, j);

            lhs_part(0, j) += w1*Tj_times_Hi_back_body_[dep1](3*idx+0, j);
            lhs_part(1, j) += w1*Tj_times_Hi_back_body_[dep1](3*idx+1, j);
            lhs_part(2, j) += w1*Tj_times_Hi_back_body_[dep1](3*idx+2, j);

            lhs_part(0, j) += w2*Tj_times_Hi_back_body_[dep2](3*idx+0, j);
            lhs_part(1, j) += w2*Tj_times_Hi_back_body_[dep2](3*idx+1, j);
            lhs_part(2, j) += w2*Tj_times_Hi_back_body_[dep2](3*idx+2, j);

            lhs_part(0, j) += w3*Tj_times_Hi_back_body_[dep3](3*idx+0, j);
            lhs_part(1, j) += w3*Tj_times_Hi_back_body_[dep3](3*idx+1, j);
            lhs_part(2, j) += w3*Tj_times_Hi_back_body_[dep3](3*idx+2, j);

            lhs_part(0, j) += w4*Tj_times_Hi_back_body_[dep4](3*idx+0, j);
            lhs_part(1, j) += w4*Tj_times_Hi_back_body_[dep4](3*idx+1, j);
            lhs_part(2, j) += w4*Tj_times_Hi_back_body_[dep4](3*idx+2, j);

            lhs_part(0, j) += w5*Tj_times_Hi_back_body_[dep5](3*idx+0, j);
            lhs_part(1, j) += w5*Tj_times_Hi_back_body_[dep5](3*idx+1, j);
            lhs_part(2, j) += w5*Tj_times_Hi_back_body_[dep5](3*idx+2, j);

            lhs_part(0, j) += w6*Tj_times_Hi_back_body_[dep6](3*idx+0, j);
            lhs_part(1, j) += w6*Tj_times_Hi_back_body_[dep6](3*idx+1, j);
            lhs_part(2, j) += w6*Tj_times_Hi_back_body_[dep6](3*idx+2, j);

            lhs_part(0, j) += w7*Tj_times_Hi_back_body_[dep7](3*idx+0, j);
            lhs_part(1, j) += w7*Tj_times_Hi_back_body_[dep7](3*idx+1, j);
            lhs_part(2, j) += w7*Tj_times_Hi_back_body_[dep7](3*idx+2, j);
        }

        // right hand side
        b(r) = local_weight * weight_model * ( lm_ps_back_body[i][0] - rhs_part[0] );

        // begin_row
        for (unsigned int dcount = 0; dcount < pca.dim_pca_model_; ++dcount)
        {
            coeffs.push_back( Tripl(r, offset_nrd_w_pca + dcount, lhs_part(0, dcount) * local_weight * weight_model) ); // d's
        }
        ++r;

        // right hand side
        b(r) = local_weight * weight_model * ( lm_ps_back_body[i][1] - rhs_part[1] );

        // begin_row
        for (unsigned int dcount = 0; dcount < pca.dim_pca_model_; ++dcount)
        {
            coeffs.push_back( Tripl(r, offset_nrd_w_pca + dcount, lhs_part(1, dcount) * local_weight * weight_model) ); // d's
        }
        ++r;

        // right hand side
        b(r) = local_weight * weight_model * ( lm_ps_back_body[i][2] - rhs_part[2] );

        // begin_row
        for (unsigned int dcount = 0; dcount < pca.dim_pca_model_; ++dcount)
        {
            coeffs.push_back( Tripl(r, offset_nrd_w_pca + dcount, lhs_part(2, dcount) * local_weight * weight_model) ); // d's
        }
        ++r;
    }
}

//-----------------------------------------------------------------------------


unsigned int
Energy_term_pca_landmarks_skinned_twoscans::
n_rows_big_sle()
{
    //std::cout << "[INFO] Energy_term_pca_landmarks_skinned_twoscans::n_rows_big_sle()" << std::endl;

    unsigned int c = 0;

    const Surface_mesh* template_mesh = energy_term_data_.template_mesh;

    //
    // front_body
    //
    Surface_mesh::Mesh_property< std::vector<Surface_mesh::Vertex> > lm_prop_front_body = template_mesh->get_mesh_property< std::vector<Surface_mesh::Vertex> >("m:landmarks_front_body");
    if (lm_prop_front_body)
    {
        const std::vector<Surface_mesh::Vertex>& tm_landmarks = lm_prop_front_body[0];
        Surface_mesh::Vertex_property<double> local_landmark_weight = template_mesh->get_vertex_property<double>("v:landmark_weight_front_body");

        if (local_landmark_weight)
        {
            for (size_t i=0; i < tm_landmarks.size(); ++i)
            {
                if (local_landmark_weight[tm_landmarks[i]] >= 1e-5)
                {
                    ++c;
                }
                else
                {
                    //std::cout << "landmark ignored " << tm_landmarks[i] << std::endl;
                }
            }
        }
        else
        {
            c += (unsigned int) tm_landmarks.size();
        }
    }

    //
    // back_body
    //
    Surface_mesh::Mesh_property< std::vector<Surface_mesh::Vertex> > lm_prop_back_body = template_mesh->get_mesh_property< std::vector<Surface_mesh::Vertex> >("m:landmarks_back_body");
    if (lm_prop_back_body)
    {
        const std::vector<Surface_mesh::Vertex>& tm_landmarks = lm_prop_back_body[0];
        Surface_mesh::Vertex_property<double> local_landmark_weight = template_mesh->get_vertex_property<double>("v:landmark_weight_back_body");

        if (local_landmark_weight)
        {
            for (size_t i=0; i < tm_landmarks.size(); ++i)
            {
                if (local_landmark_weight[tm_landmarks[i]] >= 1e-5)
                {
                    ++c;
                }
                else
                {
                    //std::cout << "landmark ignored " << tm_landmarks[i] << std::endl;
                }
            }
        }
        else
        {
            c += (unsigned int) tm_landmarks.size();
        }
    }

    return c*3;
}

#endif

//=============================================================================
} // namespace surface_mesh
} // namespace graphene
//=============================================================================
