<|start_filename|>C++/scope/utils/Setup.h<|end_filename|>
#pragma once

#include <cnpy/cnpy.h>
#include <glog/logging.h>
#include <scope/factor/Factors.h>
#include <scope/initializer/factor/Factors.h>
#include <scope/utils/InitialInfo.h>
#include <scope/utils/KeyPointInfo.h>

namespace scope {
template <typename M1, typename M2>
int projectToSO3(const Eigen::MatrixBase<M1> &M, Eigen::MatrixBase<M2> &R) {
  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M1, 3, 3)
  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M2, 3, 3)

  Eigen::JacobiSVD<Matrix3> svd(M, Eigen::ComputeFullU | Eigen::ComputeFullV);

  Scalar detU = svd.matrixU().determinant();
  Scalar detV = svd.matrixV().determinant();

  if (detU * detV > 0) {
    R.noalias() = svd.matrixU() * svd.matrixV().transpose();
  } else {
    Matrix3 Uprime = svd.matrixU();
    Uprime.col(Uprime.cols() - 1) *= -1;
    R.noalias() = Uprime * svd.matrixV().transpose();
  }

  return 0;
}

template <int K, int P, int N>
int loadModel(const std::string &file, MatrixX &JDirs, VectorX &J,
              MatrixX &RelJDirs, VectorX &RelJ, MatrixX &VDirs, VectorX &V) {
  cnpy::npz_t model_data = cnpy::npz_load(file);

  cnpy::NpyArray subdata;

  // load JDirs
  subdata = model_data.at("JDirs");
  assert(subdata.shape[0] == P - 1 && subdata.shape[1] == 3 * K + 3);

  if (subdata.shape[0] != P - 1 && subdata.shape[1] != 3 * K + 3) {
    LOG(ERROR) << "Bad input file format." << std::endl;

    exit(-1);
  }

  JDirs.setZero(3 * K + 3, P);
  JDirs.leftCols<P - 1>() =
      Eigen::Map<Eigen::MatrixXd>(subdata.data<double>(), subdata.shape[1],
                                  subdata.shape[0])
          .cast<Scalar>();

  // load J
  subdata = model_data.at("J");
  assert(subdata.shape[0] == 1 && subdata.shape[1] == 3 * K + 3);

  if (subdata.shape[0] != 1 && subdata.shape[1] != 3 * K + 3) {
    LOG(ERROR) << "Bad input file format." << std::endl;

    exit(-1);
  }

  J = Eigen::Map<Eigen::VectorXd>(subdata.data<double>(), subdata.shape[1],
                                  subdata.shape[0]);

  // load RelJDirs
  subdata = model_data.at("RelJDirs");
  assert(subdata.shape[0] == P - 1 && subdata.shape[1] == 3 * K + 3);

  if (subdata.shape[0] != P - 1 && subdata.shape[1] != 3 * K + 3) {
    LOG(ERROR) << "Bad input file format." << std::endl;

    exit(-1);
  }

  RelJDirs.setZero(3 * K, P);
  RelJDirs.leftCols<P - 1>() =
      Eigen::Map<Eigen::MatrixXd>(subdata.data<double>(), subdata.shape[1],
                                  subdata.shape[0])
          .bottomRows<3 * K>();

  // load RelJ
  subdata = model_data.at("RelJ");
  assert(subdata.shape[0] == 1 && subdata.shape[1] == 3 * K + 3);

  if (subdata.shape[0] != 1 && subdata.shape[1] != 3 * K + 3) {
    LOG(ERROR) << "Bad input file format." << std::endl;

    exit(-1);
  }

  RelJ = Eigen::Map<Eigen::VectorXd>(subdata.data<double>(), subdata.shape[1],
                                     subdata.shape[0])
             .tail<3 * K>();

  // load vDirs
  subdata = model_data.at("vDirs");
  assert(subdata.shape[0] == P - 1 && subdata.shape[1] == 3 * N);

  if (subdata.shape[0] != P && subdata.shape[1] != 3 * N) {
    LOG(ERROR) << "Bad input file format." << std::endl;

    exit(-1);
  }

  VDirs.setZero(3 * N, P);
  VDirs.leftCols<P - 1>() = Eigen::Map<Eigen::MatrixXd>(
      subdata.data<double>(), subdata.shape[1], subdata.shape[0]);

  // load v
  subdata = model_data.at("v");
  assert(subdata.shape[0] == 3 * N);

  if (subdata.shape[0] != 3 * N) {
    LOG(ERROR) << "Bad input file format." << std::endl;

    exit(-1);
  }

  V = Eigen::Map<Eigen::VectorXd>(subdata.data<double>(), subdata.shape[0], 1);

  return 0;
}

int loadSMPLKeyPoints(const MatrixX &JDirs, const VectorX &J,
                      const MatrixX &VDirs, const VectorX &V,
                      MatrixX &KeyPointDirs, VectorX &KeyPoints);

int load3DPWAnnotation(const std::string &file, int &gender, VectorX &betas,
                       VectorX &poses, VectorX &cam);

int loadKeyPoint2DMeasurements(const std::string &file, Matrix3X &Measurements);

int loadKeyPoint3DMeasurements(const std::string &file, Matrix3X &Measurements);

int loadKeyPoint2Dand3DMeasurements(const std::string &file, Vector4 & CamParam,
                                    Matrix3X &Measurements2D,
                                    Matrix3X &Measurements3D);

int loadPOFMeasurements(const Matrix3X &KeyPoints3D, Matrix3X &Measurements);

int loadJointLimitNeuralNetwork(const std::string &file,
                                AlignedVector<Matrix6> &weight,
                                AlignedVector<Vector6> &bias, VectorX &PRelua,
                                VectorX &PRelub);

int loadJointLimitPrior(const std::string &file,
                        AlignedVector<AlignedVector<Matrix6>> &weight,
                        AlignedVector<AlignedVector<Vector6>> &bias,
                        AlignedVector<VectorX> &PRelua,
                        AlignedVector<VectorX> &PRelub);

int createInitialSMPLSkeleton(const Matrix3X &Measurements3D, VectorX &J);

int createInitialPinholeCameraFactors(
    const std::vector<std::array<int, 3>> &Info,
    const std::vector<std::array<int, 2>> &Index, const VectorX &KeyPoints,
    Scalar gm_est_sigma, Scalar gm_est_eps, const Matrix3X &Measurements,
    const VectorX &ConfidenceThreshold2D,
    std::vector<std::shared_ptr<Initializer::PinholeCameraFactor>> &factors);

int createInitialPOFFactors(
    const AlignedVector<std::tuple<int, int, scope::Vector3, Scalar>> &Info,
    Scalar gm_est_sigma, Scalar gm_est_eps, const Matrix3X &Measurements,
    std::vector<std::shared_ptr<Initializer::POFFactor>> &factors);

int createInitialDepthCameraFactors(
    const std::vector<std::tuple<int, int, int, Scalar>> &Info,
    const std::vector<std::array<int, 2>> &Index, const VectorX &KeyPoints,
    Scalar gm_est_sigma, Scalar gm_est_eps, const Matrix3X &Measurements,
    const Vector3 &root,
    std::vector<std::shared_ptr<Initializer::DepthCameraFactor>> &factors);

int createInitialJointLimitFactors(
    const std::vector<std::array<int, 2>> &Index,
    const AlignedVector<AlignedVector<scope::Matrix6>> &weight,
    const AlignedVector<AlignedVector<Vector6>> &bias,
    const AlignedVector<VectorX> &PRelua, const AlignedVector<VectorX> &PRelub,
    const VectorX &scale,
    std::vector<std::shared_ptr<Initializer::JointLimitFactor>> &factors);

int createInitialPoseConstFactors(
    const AlignedVector<std::tuple<int, Vector6, Vector6, Vector6>>
        &PoseConstInfo,
    const AlignedVector<Pose> &PoseRef,
    std::vector<std::shared_ptr<Initializer::PoseConstFactor>> &factors);

int createInitialJointConstFactors(
    const AlignedVector<std::tuple<int, Vector3, Matrix3, Vector3, Vector3>>
        &JointConstInfo,
    std::vector<std::shared_ptr<Initializer::JointConstFactor>> &factors);

int createInitialEulerAngleConstFactors(
    const AlignedVector<std::tuple<int, Vector3, Matrix3, Vector3, Vector3>>
        &JointConstInfo,
    std::vector<std::shared_ptr<Initializer::EulerAngleConstFactor>> &factors);

int createInitialRootPinholeCameraFactors(
    const std::vector<std::array<int, 2>> &JointPinholeInfo,
    const std::vector<std::array<int, 3>> &VertexPinholeInfo,
    const AlignedVector<Pose> &Poses, const VectorX &Param,
    const MatrixX &JDirs, const VectorX &J, const MatrixX &KeyPointDirs,
    const VectorX &KeyPoints, Scalar gm_est_sigma, Scalar gm_est_eps,
    const Matrix3X &Measurements, const VectorX &ConfidenceThreshold2D,
    std::vector<std::shared_ptr<Initializer::PinholeCameraFactor>> &factors);

int createInitialRootPOFFactors(
    const AlignedVector<std::tuple<int, int, scope::Vector3, Scalar>>
        &UnitPOFInfo,
    const std::vector<std::tuple<int, int, int, int, Scalar>> &ScaledPOFInfo,
    const std::vector<std::tuple<int, int, int, int, int, Scalar>> &RelPOFInfo,
    const AlignedVector<Pose> &Poses, const VectorX &Param,
    const MatrixX &JDirs, const VectorX &J, const MatrixX &KeyPointDirs,
    const VectorX &KeyPoints, const std::vector<int> &kintree,
    Scalar gm_est_sigma, Scalar gm_est_eps, const Matrix3X &Measurements,
    std::vector<std::shared_ptr<Initializer::POFFactor>> &factors);

int createJointPinholeCameraFactors(
    const std::vector<std::array<int, 2>> &Info, Scalar gm_est_sigma,
    Scalar gm_est_eps, const Matrix3X &Measurements,
    const VectorX &ConfidenceThreshold2D,
    std::vector<std::shared_ptr<JointPinholeCameraFactor>> &factors);

int createVertexPinholeCameraFactors(
    const std::vector<std::array<int, 3>> &Info, int VertexParam,
    const MatrixX &JDirs, const VectorX &J, const MatrixX &KeyPointDirs,
    const VectorX &KeyPoints, Scalar gm_est_sigma, Scalar gm_est_eps,
    const Matrix3X &Measurements, const VectorX &ConfidenceThreshold2D,
    std::vector<std::shared_ptr<VertexPinholeCameraFactor>> &factors);

int createUnitPOFFactors(
    const AlignedVector<std::tuple<int, int, scope::Vector3, Scalar>> &Info,
    Scalar gm_est_sigma, Scalar gm_est_eps, const Matrix3X &Measurements,
    std::vector<std::shared_ptr<UnitPOFFactor>> &factors);

int createScaledPOFFactors(
    const std::vector<std::tuple<int, int, int, int, Scalar>> &Info,
    int VertexParam, const MatrixX &KeyPointDirs, const VectorX &KeyPoints,
    Scalar gm_est_sigma, Scalar gm_est_eps, const Matrix3X &Measurements,
    std::vector<std::shared_ptr<ScaledPOFFactor>> &factors);

int createRelPOFFactors(
    const std::vector<std::tuple<int, int, int, int, int, Scalar>> &Info,
    int VertexParam, const MatrixX &JDirs, const VectorX &J,
    const MatrixX &KeyPointDirs, const VectorX &KeyPoints,
    const std::vector<int> &kintree, Scalar gm_est_sigma, Scalar gm_est_eps,
    const Matrix3X &Measurements,
    std::vector<std::shared_ptr<RelPOFFactor>> &factors);

int createJointDepthCameraFactors(
    const std::vector<std::tuple<int, int, Scalar>> &Info, Scalar gm_est_sigma,
    Scalar gm_est_eps, const Matrix3X &Measurements, const Vector3 &root,
    std::vector<std::shared_ptr<JointDepthCameraFactor>> &factors);

int createVertexDepthCameraFactors(
    const std::vector<std::tuple<int, int, int, Scalar>> &Info, int VertexParam,
    const MatrixX &JDirs, const VectorX &J, const MatrixX &KeyPointDirs,
    const VectorX &KeyPoints, Scalar gm_est_sigma, Scalar gm_est_eps,
    const Matrix3X &Measurements, const Vector3 &root,
    std::vector<std::shared_ptr<VertexDepthCameraFactor>> &factors);

int createJointConstFactors(
    const AlignedVector<std::tuple<int, Vector3, Matrix3, Vector3, Vector3>>
        &JointConstInfo,
    std::vector<std::shared_ptr<JointConstFactor>> &factors);

int createEulerAngleConstFactors(
    const AlignedVector<std::tuple<int, Vector3, Matrix3, Vector3, Vector3>>
        &EulerAngleConstInfo,
    std::vector<std::shared_ptr<EulerAngleConstFactor>> &factors);

int createJointLimitFactors(
    const AlignedVector<AlignedVector<scope::Matrix6>> &weight,
    const AlignedVector<AlignedVector<Vector6>> &bias,
    const AlignedVector<VectorX> &PRelua, const AlignedVector<VectorX> &PRelub,
    const VectorX &scale,
    std::vector<std::shared_ptr<JointLimitFactor>> &factors);

int solveReducedEPnP(const std::vector<int> &KeyPoint3Dto2D,
                     const Matrix3X &Measurements3D,
                     const Matrix3X &Measurements2D,
                     const VectorX &ConfidenceThreshold2D, Vector3 &t);

int solvePose(
    const std::vector<std::tuple<int, int, int, Scalar>> &DepthCamInfo,
    const Vector3 &Root, const VectorX &KeyPoints,
    const Matrix3X &Measurements3D, Pose &pose);

int solveShape(
    const std::vector<std::tuple<std::array<int, 4>, Vector3>> &RelShapeInfo,
    const std::vector<std::tuple<std::array<int, 4>, Vector3>>
        &RelShapePairInfo,
    const MatrixX &ShapeSqrtCov, Scalar weight, const MatrixX &KeyPointDirs,
    const VectorX &KeyPoints, const Matrix3X &Measurements3D, VectorX &betas,
    Scalar &s);

int getKeyPointOrigin3D(const Pose &Root, const MatrixX &JDirs,
                        const VectorX &J, const MatrixX &KeyPointDirs,
                        const VectorX &KeyPoints, const VectorX &Param,
                        int OriginIndex, Vector3 &keypoint);
}  // namespace scope


<|start_filename|>C++/scope/initializer/ArmInitializer.cpp<|end_filename|>
#include <glog/logging.h>

#include <memory>
#include <scope/initializer/ArmInitializer.h>

namespace scope {
namespace Initializer {
ArmInitializer::ArmInitializer(const Options &options,
                               const std::array<Vector3, 2> &RelJointLocations,
                               Arm LR)
    : Initializer(2, options), mRelJointLocations(RelJointLocations), mArm(LR),
      mElbowLowerBnd(LR == Arm::Left ? -0.85 * M_PI : 0),
      mElbowUpperBnd(LR == Arm::Left ? 0 : 0.85 * M_PI) {}

int ArmInitializer::initialize(const Pose &T0,
                               const AlignedVector<Matrix3> &joints) const {
  assert(joints.size() == mNumJoints);

  if (joints.size() != mNumJoints) {
    LOG(ERROR) << "There should be " << mNumJoints << " joint states."
               << std::endl;

    exit(-1);
  }

  Vector3 xi;
  Matrix3 R;

  // Scale eblow joint to feasbile range
  math::SO3::log(joints[1], xi);
  xi[1] = std::max(xi[1], mElbowLowerBnd);
  xi[1] = std::min(xi[1], mElbowUpperBnd);
  xi[0] = xi[2] = 0;
  math::SO3::exp(xi, R);

  mElbowJoint[0] = mElbowJoint[1] = xi[1];

  Initializer::initialize(T0, {joints[0], R});

  return 0;
}

int ArmInitializer::FKintree(int n) const {
  auto &poses = mvPoses[n];
  auto &joints = mvJoints[n];

  poses[1].R.noalias() = poses[0].R * joints[0];
  poses[1].t = poses[0].t;
  poses[1].t.noalias() += poses[0].R * mRelJointLocations[0];

  poses[2].R.noalias() = poses[1].R * joints[1];
  poses[2].t = poses[1].t;
  poses[2].t.noalias() += poses[1].R * mRelJointLocations[1];

  return 0;
}

int ArmInitializer::DFKintree() const {
  const auto &poses = mvPoses[0];

  mB0.topRows<3>() = poses[0].R;
  mB0.block<3, 1>(3, 0).noalias() = poses[1].t.cross(poses[0].R.col(0));
  mB0.block<3, 1>(3, 1).noalias() = poses[1].t.cross(poses[0].R.col(1));
  mB0.block<3, 1>(3, 2).noalias() = poses[1].t.cross(poses[0].R.col(2));

  mB1.head<3>() = poses[1].R.col(1);
  mB1.tail<3>().noalias() = poses[2].t.cross(poses[1].R.col(1));

  return 0;
}

int ArmInitializer::updateGaussNewton() const {
  mH.resize(4, 4);

  Vector6 temp1;
  Matrix6 temp2;
  Matrix63 temp3;

  temp1.noalias() = mvMxx[2] * mB1;
  mH.topRightCorner<3, 1>().noalias() = mB0.transpose() * temp1;
  mH(3, 3) = mB1.dot(temp1);
  mH(3, 3) += mvMuu[1](1, 1);

  temp2 = mvMxx[1] + mvMxx[2];
  temp3.noalias() = temp2 * mB0;
  mH.topLeftCorner<3, 3>().noalias() = mB0.transpose() * temp3;
  mH.topLeftCorner<3, 3>().noalias() += mvMuu[0];

  mH.bottomLeftCorner<1, 3>().noalias() = mH.topRightCorner<3, 1>().transpose();

  mh.resize(4);

  mh[3] = mB1.dot(mvmx[2]);
  mh[3] += mvmu[1][1];

  temp1 = mvmx[1] + mvmx[2];
  mh.head<3>().noalias() = mB0.transpose() * temp1;
  mh.head<3>() += mvmu[0];

  return 0;
}

int ArmInitializer::solveGaussNewton() const {
  mLambda = mH.diagonal() * (mDLambda - 1);
  mLambda.array() += mOptions.delta;
  mH.diagonal() += mLambda;

  mHchol.compute(mH.topLeftCorner<3, 3>());

  mS.noalias() = -mHchol.solve(mH.block<3, 1>(0, 3));
  ms.noalias() = -mHchol.solve(mh.head<3>());

  mA = mH(3, 3) + mH.block<3, 1>(0, 3).dot(mS);
  ma = mh[3] + mH.block<3, 1>(0, 3).dot(ms);

  mhGN.resize(4);

  // Guarantee mKneeJoint in [0, 0.78 * M_PI]
  mhGN[3] = -ma / mA;
  mhGN[3] = std::max(mhGN[3], mElbowLowerBnd - mElbowJoint[0]);
  mhGN[3] = std::min(mhGN[3], mElbowUpperBnd - mElbowJoint[0]);
  mhGN.head<3>() = ms;
  mhGN.head<3>().noalias() += mS * mhGN[3];

  mSquaredError = mh;
  mSquaredError += 0.5 * mH * mhGN;

  mE = mhGN.dot(mSquaredError);
  mSquaredError = mhGN.cwiseAbs2();
  mE -= 0.5 * mLambda.dot(mSquaredError);

  return 0;
}

int ArmInitializer::update(Scalar stepsize) const {
  assert(stepsize > 0);

  mDJointChange = mhGN.head<3>() * stepsize;
  math::SO3::exp(mDJointChange, mJointChange);
  mvJoints[1][0].noalias() = mJointChange * mvJoints[0][0];

  mElbowJoint[1] = mElbowJoint[0] + stepsize * mhGN[3];

  mvJoints[1][1].setIdentity();
  mvJoints[1][1](0, 0) = std::cos(mElbowJoint[1]);
  mvJoints[1][1](2, 2) = mvJoints[1][1](0, 0);
  mvJoints[1][1](0, 2) = std::sin(mElbowJoint[1]);
  mvJoints[1][1](2, 0) = -mvJoints[1][1](0, 2);

  FKintree(1);

  return 0;
}

int ArmInitializer::accept() const {
  mElbowJoint[0] = mElbowJoint[1];

  return Initializer::accept();
}
} // namespace Initializer
} // namespace scope


<|start_filename|>C++/scope/initializer/factor/Factors.h<|end_filename|>
#pragma once

#include <scope/initializer/factor/DepthCameraFactor.h>
#include <scope/initializer/factor/EulerAngleConstFactor.h>
#include <scope/initializer/factor/JointConstFactor.h>
#include <scope/initializer/factor/JointFactor.h>
#include <scope/initializer/factor/JointLimitFactor.h>
#include <scope/initializer/factor/POFFactor.h>
#include <scope/initializer/factor/PinholeCameraFactor.h>
#include <scope/initializer/factor/PoseConstFactor.h>
#include <scope/initializer/factor/PoseFactor.h>


<|start_filename|>C++/scope/math/SO3.h<|end_filename|>
#pragma once

#include <scope/base/Types.h>
#include <Eigen/Dense>

namespace scope {
namespace math {
struct SO3 {
  typedef Matrix3 LieGroup;
  typedef Vector3 LieAlgebra;

  template <typename T1, typename T2>
  static void exp(Eigen::MatrixBase<T1> const &w, Eigen::MatrixBase<T2> &res) {
    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(T1, 3)
    EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(T2, 3, 3)

    Scalar const th = w.stableNorm();
    LieAlgebra s = w;

    if (th < 5e-2) {
      Scalar c, d, th2, th4, th6, d2, thd2;
      th2 = th * th;
      th4 = th2 * th2;
      th6 = th4 * th2;
      d = 1 + th2 / 12 + th4 / 120 + th6 / 1185.88235294117647058823529412;

      d2 = d * d;
      thd2 = th2 * d2;
      c = 2 / (4 + thd2);

      s *= c * d2;

      res.noalias() = s * w.transpose();
      res.diagonal().array() += 1 - c * thd2;

      s *= 2 / d;
    } else {
      // use exponential map

      Scalar sth, cth;

      fsincos(th, &sth, &cth);

      s /= th;

      res.noalias() = (1 - cth) * s * s.transpose();
      res.diagonal().array() += cth;

      s *= sth;
    }

    res(0, 1) -= s[2];
    res(1, 0) += s[2];
    res(0, 2) += s[1];
    res(2, 0) -= s[1];
    res(1, 2) -= s[0];
    res(2, 1) += s[0];
  }

  template <typename T1, typename T2>
  static void log(Eigen::MatrixBase<T1> const &R, Eigen::MatrixBase<T2> &res) {
    EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(T1, 3, 3)
    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(T2, 3)

    Scalar const tr = R.trace() + 1;

    if (fabs(tr) < 1e-9) {
      // theta = pi and use eigenvalue decomposition

      if (R(0, 0) > R(1, 1)) {
        if (R(0, 0) > R(2, 2)) {
          res[0] = std::sqrt((R(0, 0) + 1));

          res[1] = R(1, 0) / res[0];
          res[2] = R(2, 0) / res[0];
        } else {
          res[2] = std::sqrt((R(2, 2) + 1));

          res[0] = R(0, 2) / res[2];
          res[1] = R(1, 2) / res[2];
        }
      } else {
        if (R(1, 1) > R(2, 2)) {
          res[1] = std::sqrt((R(1, 1) + 1));

          res[0] = R(0, 1) / res[1];
          res[2] = R(2, 1) / res[1];
        } else {
          res[2] = std::sqrt((R(2, 2) + 1));

          res[0] = R(0, 2) / res[2];
          res[1] = R(1, 2) / res[2];
        }
      }

      res *= 2.221441469079183;  // multiplied by  pi/sqrt(2)

    } else {
      res(0) = R(2, 1) - R(1, 2);
      res(1) = R(0, 2) - R(2, 0);
      res(2) = R(1, 0) - R(0, 1);

      if (tr > INVERSE_OF_RETRACT_TOL) {
        // use cayley map when th is small for fast approximation
        // res *= 2 / (1.0 + tr);
        res *= (3.0666666666666669 * tr - 5.8666666666666663 +
                6.4000000000000004 / tr) /
               (tr * tr);
      } else {
        // use exponential map
        Scalar const th = acos((tr - 2) / 2);
        res *= 0.5 * th / sin(th);
      }
    }
  }

  template <typename T1, typename T2>
  static void dexp(Eigen::MatrixBase<T1> const &w, Eigen::MatrixBase<T2> &res) {
    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(T1, 3)
    EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(T2, 3, 3)

    Scalar const th = w.stableNorm();

    LieAlgebra s = w;

    if (th < 5e-2) {
      // use second-order taylor expansion when th is small for fast
      // approximation relative approx. error <= 1e-6 for DRETRACT_TOL = 0.1 rad

      Scalar th2 = th * th;
      Scalar c = 0.166666666666667 - th2 / 120;

      res.noalias() = c * w * w.transpose();
      res.diagonal().array() += 1 - c * th2;

      s *= 0.5 - th2 / 24.0;
    } else {
      // use exponential map
      Scalar sth, cth, th2, th3;

      fsincos(th, &sth, &cth);

      th2 = th * th;
      th3 = th2 * th;

      res.noalias() = (th - sth) / th3 * s *
                      s.transpose();  // (th-sth)/th^3 -> 1/6 as th -> 0
      res.diagonal().array() += sth / th;

      s *= (1 - cth) / th2;  // (1-cth)/th^2 -> 1/2 as th -> 0
    }

    res(0, 1) -= s[2];
    res(1, 0) += s[2];
    res(0, 2) += s[1];
    res(2, 0) -= s[1];
    res(1, 2) -= s[0];
    res(2, 1) += s[0];
  }

  template <typename T1, typename T2>
  static void dexpinv(Eigen::MatrixBase<T1> const &w,
                      Eigen::MatrixBase<T2> &res) {
    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(T1, 3)
    EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(T2, 3, 3)

    Scalar const th = w.stableNorm();

    LieAlgebra s = w;

    if (th < 5e-2) {
      // use cayley map when th is small for fast approximation
      Scalar th2 = th * th;
      Scalar c = 0.333333333333333 + th2 / 180;

      s *= -0.5;

      res.noalias() = c * s * s.transpose();
      res.diagonal().array() += 1 - 0.25 * c * th2;
    } else {
      // use exponential map
      Scalar sth, cth;

      fsincos(th, &sth, &cth);

      sth *= th;
      cth = 2 * cth - 2;

      res.noalias() = (sth + cth) / (th * th * cth) * s * s.transpose();
      res.diagonal().array() -= sth / cth;

      s *= -0.5;
    }

    res(0, 1) -= s[2];
    res(1, 0) += s[2];
    res(0, 2) += s[1];
    res(2, 0) -= s[1];
    res(1, 2) -= s[0];
    res(2, 1) += s[0];
  }

  template <typename T1, typename T2>
  static void cay(Eigen::MatrixBase<T1> const &w, Eigen::MatrixBase<T2> &res) {
    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(T1, 3)
    EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(T2, 3, 3)

    Scalar const th = w.stableNorm();
    LieAlgebra s = w;

    // using cayley map
    s *= 2 / (4 + th * th);

    res.noalias() = s * w.transpose();
    res.diagonal().array() += 1 - s.dot(w);

    s *= 2;

    res(0, 1) -= s[2];
    res(1, 0) += s[2];
    res(0, 2) += s[1];
    res(2, 0) -= s[1];
    res(1, 2) -= s[0];
    res(2, 1) += s[0];
  }

  template <typename T1, typename T2>
  static void cayinv(Eigen::MatrixBase<T1> const &R,
                     Eigen::MatrixBase<T2> &res) {
    EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(T1, 3, 3)
    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(T2, 3)

    Scalar const tr = R.trace();

    res(0) = R(2, 1) - R(1, 2);
    res(1) = R(0, 2) - R(2, 0);
    res(2) = R(1, 0) - R(0, 1);

    res /= 2 / (1.0 + tr);
  }

  template <typename T1, typename T2>
  static void dcay(Eigen::MatrixBase<T1> const &w, Eigen::MatrixBase<T2> &res) {
    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(T1, 3)
    EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(T2, 3, 3)

    Scalar const th = w.stableNorm();

    LieAlgebra s = w;

    Scalar c = 2 / (4 + th * th);

    s *= c;

    res.diagonal().array() = 2 * c;

    res(0, 1) = -s[2];
    res(1, 0) = s[2];
    res(0, 2) = s[1];
    res(2, 0) = -s[1];
    res(1, 2) = -s[0];
    res(2, 1) = s[0];
  }

  template <typename T1, typename T2>
  static void dcayinv(Eigen::MatrixBase<T1> const &w,
                      Eigen::MatrixBase<T2> &res) {
    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(T1, 3)
    EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(T2, 3, 3)

    Scalar const th = w.stableNorm();

    LieAlgebra s = w;

    s *= -0.5;

    res.noalias() = s * s.transpose();
    res.diagonal().array() += 1 - 0.25 * th;

    res(0, 1) -= s[2];
    res(1, 0) += s[2];
    res(0, 2) += s[1];
    res(2, 0) -= s[1];
    res(1, 2) -= s[0];
    res(2, 1) += s[0];
  }

  template <typename T1, typename T2>
  static void Rot2XYZ(Eigen::MatrixBase<T1> const &R,
                      Eigen::MatrixBase<T2> &res) {
    EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(T1, 3, 3)
    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(T2, 3)

    if (1 - fabs(R(0, 2)) >= 5e-11) {
      res[0] = atan2(-R(1, 2), R(2, 2));
      res[1] = asin(R(0, 2));
      res[2] = atan2(-R(0, 1), R(0, 0));
    } else {
      res[0] = atan2(R(1,0), R(1,1));
      res[1] = R(0, 2) > 0 ? 0.5 * M_PI : -0.5 * M_PI;
      res[2] = 0;
    }
  }

  static void fsincos(double x, double *sin, double *cos) {
    sincos(x, sin, cos);
  }

  static void fsincos(float x, float *sin, float *cos) { sincosf(x, sin, cos); }

  static void fsincos(long double x, long double *sin, long double *cos) {
    sincosl(x, sin, cos);
  }
};
}  // namespace math
}  // namespace scope


<|start_filename|>C++/scope/initializer/factor/EulerAngleConstFactor.h<|end_filename|>
#pragma once

//TODO: test evaluate() and linearize()

#include <scope/initializer/factor/JointConstFactor.h>

namespace scope {
namespace Initializer {
class EulerAngleConstFactor : public JointConstFactor {
 public:
  struct Linearization : public JointConstFactor::Linearization {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    Matrix3 H;
    Matrix3 D;
  };

 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  SCOAT_INITIAL_EVALUATION_NEW
  SCOAT_INITIAL_LINEARIZATION_NEW

  EulerAngleConstFactor(int joint, const Vector3 &weight, const Matrix3 &mean,
                        const Vector3 &lbnd, const Vector3 &ubnd,
                        const std::string &name = "", int index = -1);

  virtual int evaluate(const AlignedVector<Pose> &poses,
                       const AlignedVector<Matrix3> &joints,
                       Factor::Evaluation &base_eval) const override;

  virtual int linearize(const AlignedVector<Pose> &poses,
                        const AlignedVector<Matrix3> &joints,
                        const Factor::Evaluation &base_eval,
                        Factor::Linearization &base_lin) const override;
};
}  // namespace Initializer
}  // namespace scope


<|start_filename|>C++/scope/utils/JointConstInfo.h<|end_filename|>
#pragma once

#include <array>
#include <vector>

#include <scope/base/Types.h>

namespace scope {
namespace JointConstInfo {
namespace SMPL {
const AlignedVector<std::tuple<int, Vector3, Matrix3, Vector3, Vector3>>
    JointConstInfo{
        //[0]: joint index
        //[1]: weight
        //[2]: lower bound
        //[3]: upper bound
        {3, // left knee
         {5, 3, 3},
         (Matrix3() << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0).finished(),
         {-0.00 * M_PI, -0.05 * M_PI, -0.03 * M_PI},
         {+0.85 * M_PI, +0.05 * M_PI, +0.03 * M_PI}},
        {4, // right knee
         {5, 3, 3},
         (Matrix3() << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0).finished(),
         {-0.00 * M_PI, -0.05 * M_PI, -0.03 * M_PI},
         {+0.85 * M_PI, +0.05 * M_PI, +0.03 * M_PI}},
        {15,
         {3, 3, 3},
         (Matrix3() << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0).finished(),
         {-0.50 * M_PI, -0.75 * M_PI, -0.25 * M_PI},
         {+0.25 * M_PI, +0.00 * M_PI, +0.25 * M_PI}},
        {16,
         {3, 3, 3},
         (Matrix3() << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0).finished(),
         {-0.50 * M_PI, -0.00 * M_PI, -0.25 * M_PI},
         {+0.25 * M_PI, +0.75 * M_PI, +0.25 * M_PI}}};

const AlignedVector<std::tuple<int, Vector3, Matrix3, Vector3, Vector3>>
    EulerAngleConstInfo{
        //[0]: joint index
        //[1]: weight
        //[2]: lower bound
        //[3]: upper bound
        {0, // left hip
         {10, 3, 3},
         (Matrix3() << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0).finished(),
         {-0.67 * M_PI, -0.25 * M_PI, -0.17 * M_PI},
         {+0.50 * M_PI, +0.48 * M_PI, +0.25 * M_PI}},
        {1, // right hip
         {10, 3, 3},
         (Matrix3() << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0).finished(),
         {-0.67 * M_PI, -0.48 * M_PI, -0.25 * M_PI},
         {+0.50 * M_PI, +0.25 * M_PI, +0.17 * M_PI}},
        //{15,  // left upper arm
        //{3, 3, 3},
        //(Matrix3() << 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0).finished(),
        //{-0.50 * M_PI, -0.33 * M_PI, -0.60 * M_PI},
        //{+0.46 * M_PI, +0.48 * M_PI, +0.40 * M_PI}},
        //{16,  // right upper arm
        //{3, 3, 3},
        //(Matrix3() << 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0).finished(),
        //{-0.46 * M_PI, -0.33 * M_PI, -0.40 * M_PI},
        //{+0.50 * M_PI, +0.48 * M_PI, +0.60 * M_PI}},
        //{17,  // left upper arm
        //{3, 3, 3},
        //(Matrix3() << 0.0, 1.0, 0.0, -1.0, 0.0, 0.0, 0.0,
        // 0.0, 1.0).finished(),
        //{-0.00 * M_PI, -0.33 * M_PI, -0.15 * M_PI},
        //{+0.75 * M_PI, +0.33 * M_PI, +0.15 * M_PI}},
        //{18,  // right upper arm
        //{3, 3, 3},
        //(Matrix3() << 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0).finished(),
        //{-0.75 * M_PI, -0.33 * M_PI, -0.15 * M_PI},
        //{+0.00 * M_PI, +0.33 * M_PI, +0.15 * M_PI}}
    };
} // namespace SMPL

namespace SMPLH {
const AlignedVector<std::tuple<int, Vector3, Vector3, Vector3>> JointConstInfo{
    //[0]: joint index
    //[1]: weight
    //[2]: lower bound
    //[3]: upper bound
    {0, // left hip
     {10, 3, 3},
     {-0.67 * M_PI, -0.10 * M_PI, -0.20 * M_PI},
     {+0.50 * M_PI, +0.33 * M_PI, +0.33 * M_PI}},
    {1, // right hip
     {10, 3, 3},
     {-0.67 * M_PI, -0.33 * M_PI, -0.33 * M_PI},
     {+0.50 * M_PI, +0.10 * M_PI, +0.20 * M_PI}},
    {3, // left knee
     {5, 5, 5},
     {-0.00 * M_PI, -0.05 * M_PI, -0.03 * M_PI},
     {+0.80 * M_PI, +0.05 * M_PI, +0.03 * M_PI}},
    {4, // right knee
     {5, 5, 5},
     {-0.00 * M_PI, -0.05 * M_PI, -0.03 * M_PI},
     {+0.80 * M_PI, +0.05 * M_PI, +0.03 * M_PI}},
    {15,
     {3, 3, 3},
     {-0.50 * M_PI, -0.75 * M_PI, -0.25 * M_PI},
     {+0.25 * M_PI, +0.05 * M_PI, +0.25 * M_PI}},
    {16,
     {3, 3, 3},
     {-0.50 * M_PI, -0.05 * M_PI, -0.25 * M_PI},
     {+0.25 * M_PI, +0.75 * M_PI, +0.25 * M_PI}}};
}
} // namespace JointConstInfo
} // namespace scope


<|start_filename|>C++/scope/factor/Factors.h<|end_filename|>
#pragma once

#include <scope/factor/Factor.h>
#include <scope/factor/EulerAngleConstFactor.h>
#include <scope/factor/FullJointExtraPinholeCameraFactor.h>
#include <scope/factor/FullJointPinholeCameraFactor.h>
#include <scope/factor/FullVertexExtraPinholeCameraFactor.h>
#include <scope/factor/FullVertexPinholeCameraFactor.h>
#include <scope/factor/JointConstFactor.h>
#include <scope/factor/JointDepthCameraFactor.h>
#include <scope/factor/JointFactor.h>
#include <scope/factor/JointLimitFactor.h>
#include <scope/factor/JointPinholeCameraFactor.h>
#include <scope/factor/ParameterFactor.h>
#include <scope/factor/PoseConstFactor.h>
#include <scope/factor/PoseFactor.h>
#include <scope/factor/RelPOFFactor.h>
#include <scope/factor/ScaledPOFFactor.h>
#include <scope/factor/UnitPOFFactor.h>
#include <scope/factor/VertexDepthCameraFactor.h>
#include <scope/factor/VertexExtraPinholeCameraFactor.h>
#include <scope/factor/VertexPinholeCameraFactor.h>


<|start_filename|>C++/scope/optimizer/Optimizer.cpp<|end_filename|>
#include <glog/logging.h>

#include <memory>

#include <scope/optimizer/Optimizer.h>

namespace scope {
namespace Optimizer {
template <int K, int P, int N, bool CamOpt>
Optimizer<K, P, N, CamOpt>::Optimizer(
    const std::shared_ptr<const Model<K, P, N>> &model, const Options &options)
    : mModel(model),
      mOptions(options),
      mStatus(Status::Uninitialized),
      mRawB(6, K * (P + 3)),
      mB(mRawB.data()),
      mRawM(DSize, NumPoses * DSize),
      mM(mRawM.data()),
      mRawPoseGN(6, NumPoses),
      mPoseGN(mRawPoseGN.data()),
      mRawJointGN(3, NumJoints),
      mJointGN(mRawJointGN.data()),
      mRawParamGN(DParamSize),
      mParamGN(mRawParamGN.data()),
      mRawhGN(6 + DParamSize),
      mhGN(mRawhGN.data()),
      mRawH(DSize, NumJoints * DSize),
      mH(mRawH.data()),
      mRawHxB(DSize, NumJoints * (P + 3)),
      mHxB(mRawHxB.data()),
      mRawKuxp(3 * NumJoints, 6 + DParamSize),
      mKuxp(mRawKuxp.data()),
      mRawH0(6 + DParamSize, 6 + DParamSize),
      mH0(mRawH0.data()),
      mRawku(3 * K),
      mku(mRawku.data()),
      mRawh0(6 + DParamSize),
      mh0(mRawh0.data()),
      mRawRelJointLocations{VectorX(3 * K), VectorX(3 * K)},
      mRelJointLocations{
          Eigen::Map<Vector<3 * K>>(mRawRelJointLocations[0].data()),
          Eigen::Map<Vector<3 * K>>(mRawRelJointLocations[1].data())},
      mRawLambda(6 + DParamSize + 3 * NumJoints),
      mLambda(mRawLambda.data()),
      mRawSquaredError(6 + DParamSize + 3 * NumJoints),
      mSquaredError(mRawSquaredError.data()) {}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::initialize(bool refine) const {
  // TODO: initialize mvShapes

  mvPoses[0][0] = Pose::Identity();
  mvPoses[0][0].R(1, 1) = -1;
  mvPoses[0][0].R(2, 2) = -1;
  mvPoses[0][0].t[2] = 3;  // an guess for the depth
  mvJoints[0].assign(NumJoints, scope::Matrix3::Identity());

  for (int i = 0; i < NumParams; i++) {
    mvParams[0][i].setZero(ParamSizes[i]);
  }

  return initialize(mvPoses[0][0], mvJoints[0], mvParams[0], refine);
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::initialize(const Pose &T0,
                                           const AlignedVector<Matrix3> &joints,
                                           const AlignedVector<VectorX> &params,
                                           bool refine) const {
  assert(joints.size() == NumJoints);

  if (joints.size() != NumJoints) {
    LOG(ERROR) << "There should be " << NumJoints << " joint states."
               << std::endl;

    exit(-1);
  }

  assert(params.size() == NumParams);

  if (params.size() != NumParams) {
    LOG(ERROR) << "There should be " << NumParams << " parameters."
               << std::endl;

    exit(-1);
  }

  for (int i = 0; i < NumParams; i++) {
    assert(params[i].size() == ParamSizes[i]);

    if (params[i].size() != ParamSizes[i]) {
      LOG(ERROR) << "The size of params[" << i << "] should be "
                 << ParamSizes[i] << "." << std::endl;

      exit(-1);
    }
  }

  mvPoses[0][0] = T0;
  mvJoints[0] = joints;
  mvParams[0] = params;

  if (refine) {
    if (evaluateInitialFactors(0)) {
      mStatus = Status::Failed;

      exit(-1);
    }

    mDLambda = 1.0;
    mStepsize = 1.0;

    for (int i = 0; i < 20; i++) {
      initialOptimize();

      if (-mE / mfobj[0] < 1e-4) {
        break;
      }
    }
  }

  const auto &model = mModel;
  model->FK(mvPoses[0], mRelJointLocations[0], mvPoses[0][0], mvJoints[0],
            mvParams[0][0]);

  if (evaluateFactors(0)) {
    mStatus = Status::Failed;

    exit(-1);
  }

  updateGaussNewtonInfo();

  mResults = Results();
  mResults.fobjs.push_back(mfobj[0]);

  mDLambda = 1.5;
  mStepsize = 1.0;

  mStatus = Status::Initialized;

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::optimize() const {
  if (mStatus == Status::Uninitialized) {
    LOG(WARNING) << "The optimization must be initialized." << std::endl;

    exit(-1);
  }

  if (mStatus == Status::Failed) {
    LOG(ERROR) << "The optimization failed." << std::endl;

    exit(-1);
  }

  // mModel->DFK(mvB, mvPoses[0]);

  // for (int pose = 0; pose < NumPoses; pose++) {
  // linearizeFactors(pose);
  //}

  if (linearize()) {
    exit(-1);
  }

  solveGaussNewton();

  updateCostReduction();

  const auto &options = mOptions;

  Scalar stepsize;
  auto &results = mResults;

  const auto &fobj = mfobj;

  mStatus = Status::Aborted;

  Scalar expected_rel_cost_reduction = -mE / fobj[0];
  Scalar rel_cost_reduction = 0;

  if (options.method == Method::LM) {
    stepsize = 1.0;

    update(stepsize);

    if (evaluateFactors(1)) {
      exit(-1);
    }

    if (fobj[1] < fobj[0]) {
      rel_cost_reduction = (fobj[0] - fobj[1]) / fobj[0];

      Scalar rho = (fobj[1] - fobj[0]) / mE;

      if (rho > 0.70) {
        mDLambda = std::max(mDLambda * mOptions.lm_lambda_decrease, Scalar(1));
      } else if (rho < 0.25) {
        mDLambda = std::min(mDLambda * mOptions.lm_lambda_increase,
                            mOptions.lm_lambda_max);
      }

      accept();

      mStepsize = std::max(0.2, mStepsize);

      mStatus = Status::Accepted;
    } else {
      stepsize = std::min(mStepsize, mOptions.stepsize_decrease_ratio);

      for (int k = 1; k < mOptions.max_inner_iters; k++) {
        update(stepsize);

        if (evaluateFactors(1)) {
          exit(-1);
        }

        if (fobj[1] < fobj[0]) {
          rel_cost_reduction = (fobj[0] - fobj[1]) / fobj[0];

          accept();
          mStatus = Status::Accepted;

          mStepsize = std::min((Scalar)1.0,
                               stepsize * mOptions.stepsize_increase_ratio);

          break;
        }

        stepsize *= mOptions.stepsize_decrease_ratio;
      }

      mDLambda = std::min(mDLambda * mOptions.lm_lambda_increase,
                          mOptions.lm_lambda_max);
    }
  } else {
    stepsize = mStepsize;

    for (int k = 0; k < mOptions.max_inner_iters; k++) {
      update(stepsize);

      if (evaluateFactors(1)) {
        exit(-1);
      }

      if (fobj[1] < fobj[0]) {
        rel_cost_reduction = (fobj[0] - fobj[1]) / fobj[0];

        accept();
        mStatus = Status::Accepted;

        mStepsize =
            std::min((Scalar)1.0, stepsize * mOptions.stepsize_increase_ratio);

        break;
      }

      stepsize *= mOptions.stepsize_decrease_ratio;
    }
  }

  results.fobjs.push_back(fobj[0]);
  results.expected_rel_cost_reduction.push_back(expected_rel_cost_reduction);
  results.rel_cost_reduction.push_back(rel_cost_reduction);
  results.stepsizes.push_back(stepsize);

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::evaluate() const {
  if (mStatus == Status::Uninitialized) {
    LOG(WARNING) << "The optimizer is uninitialized." << std::endl;

    return -1;
  }

  const auto &model = mModel;
  model->FK(mvPoses[0], mRelJointLocations[0], mvPoses[0][0], mvJoints[0],
            mvParams[0][0]);

  if (evaluateFactors(0)) {
    mStatus = Status::Failed;

    exit(-1);
  }

  updateGaussNewtonInfo();

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::updateFactorWeights(
    const Eigen::Matrix<Scalar, NumFactors, 1> &weights) {
  mOptions.weights = weights;

  mfobj[0] = 0.5 * mOptions.weights.dot(mvFobj[0]);

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::reset() {
  mnvFactors.clear();
  mnFactors.clear();

  mvFactors.clear();
  mFactors.clear();

  mnvFactorEvals[0].clear();
  mvFactorEvals[0].clear();

  mvFactorEvals[0].clear();
  mFactorEvals[0].clear();

  mnvFactorEvals[1].clear();
  mvFactorEvals[1].clear();

  mvFactorEvals[1].clear();
  mFactorEvals[1].clear();

  mnvFactorLins.clear();
  mvFactorLins.clear();

  mvFactorLins.clear();
  mFactorLins.clear();

  setupFactors();

  mvFobj[0].setZero();
  mvFobj[1].setZero();

  mfobj[0] = 0;
  mfobj[1] = 0;

  mStatus = Status::Uninitialized;

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::addJointPinholeCameraFactor(
    std::shared_ptr<JointPinholeCameraFactor> factor) {
  if (checkFactor(factor)) {
    exit(-1);
  }

  const auto &pose = factor->getPoses()[0];

  assert(pose >= 0 && pose < NumPoses);

  if (pose < 0 || pose >= NumPoses) {
    LOG(ERROR) << "The pose must be valid." << std::endl;

    exit(-1);
  }

  int index;

  const auto &params = factor->getParams();

  if (params.size() == 0) {
    index = FactorIndex::JointPinholeCamera;
  } else {
    assert(params[0] == CamParamIndex);

    if (params[0] != CamParamIndex) {
      LOG(ERROR) << "The camera parameter must be valid." << std::endl;

      exit(-1);
    }

    index = FactorIndex::FullJointPinholeCamera;
  }

  std::array<std::shared_ptr<Factor::Evaluation>, 2> evals;
  std::shared_ptr<Factor::Linearization> lin;

  if (addFactor(index, factor, evals, lin)) {
    exit(-1);
  }

  mnvFactors[pose][index].push_back(factor);
  mnvFactorEvals[0][pose][index].push_back(evals[0]);
  mnvFactorEvals[1][pose][index].push_back(evals[1]);
  mnvFactorLins[pose][index].push_back(lin);

  mnFactors[pose].push_back(factor);
  mnFactorEvals[0][pose].push_back(evals[0]);
  mnFactorEvals[1][pose].push_back(evals[1]);
  mnFactorLins[pose].push_back(lin);

  return factor->getID();
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::addVertexPinholeCameraFactor(
    std::shared_ptr<VertexPinholeCameraFactor> factor) {
  if (checkFactor(factor)) {
    exit(-1);
  }

  const auto &pose = factor->getPoses()[0];

  assert(pose >= 0 && pose < NumPoses);

  if (pose < 0 || pose >= NumPoses) {
    LOG(ERROR) << "The pose must be valid." << std::endl;

    exit(-1);
  }

  const auto &params = factor->getParams();

  assert(params[0] == VertexParamIndex);

  if (params[0] != VertexParamIndex) {
    LOG(ERROR) << "The vertex parameter must be valid." << std::endl;

    exit(-1);
  }

  int index;

  std::shared_ptr<VertexPinholeCameraFactor> newFactor;

  if (params.size() == 1) {
    index = FactorIndex::VertexPinholeCamera;
  } else {
    assert(params[1] == CamParamIndex);

    if (params[1] != CamParamIndex) {
      LOG(ERROR) << "The camera parameter must be valid." << std::endl;

      exit(-1);
    }

    index = FactorIndex::FullVertexPinholeCamera;
  }

  std::array<std::shared_ptr<Factor::Evaluation>, 2> evals;
  std::shared_ptr<Factor::Linearization> lin;

  if (addFactor(index, factor, evals, lin)) {
    exit(-1);
  }

  mnvFactors[pose][index].push_back(factor);
  mnvFactorEvals[0][pose][index].push_back(evals[0]);
  mnvFactorEvals[1][pose][index].push_back(evals[1]);
  mnvFactorLins[pose][index].push_back(lin);

  mnFactors[pose].push_back(factor);
  mnFactorEvals[0][pose].push_back(evals[0]);
  mnFactorEvals[1][pose].push_back(evals[1]);
  mnFactorLins[pose].push_back(lin);

  return factor->getID();
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::addPOFFactor(
    std::shared_ptr<POFFactor> factor) {
  if (checkFactor(factor)) {
    exit(-1);
  }

  const auto &pose = factor->getPoses()[0];

  assert(pose >= 0 && pose < NumPoses);

  if (pose < 0 || pose >= NumPoses) {
    LOG(ERROR) << "The pose must be valid." << std::endl;

    exit(-1);
  }

  int index;

  const auto &params = factor->getParams();
  const auto &joints = factor->getJoints();

  std::shared_ptr<POFFactor> newFactor;

  if (params.size() == 0) {
    index = FactorIndex::UnitPOF;
  } else if (joints.size() == 0) {
    assert(params[0] == VertexParamIndex);

    if (params[0] != VertexParamIndex) {
      LOG(ERROR) << "The vertex parameter must be valid." << std::endl;

      exit(-1);
    }

    index = FactorIndex::ScaledPOF;
  } else {
    assert(params[0] == VertexParamIndex);

    if (params[0] != VertexParamIndex) {
      LOG(ERROR) << "The vertex parameter must be valid." << std::endl;

      exit(-1);
    }

    const auto &joint = factor->getJoints()[0];

    assert(joint == pose - 1 && joint >= 0);

    if (joint < 0 || joint != pose - 1) {
      LOG(ERROR) << "The joint must be valid." << std::endl;

      exit(-1);
    }

    index = FactorIndex::RelPOF;
  }

  std::array<std::shared_ptr<Factor::Evaluation>, 2> evals;
  std::shared_ptr<Factor::Linearization> lin;

  if (addFactor(index, factor, evals, lin)) {
    exit(-1);
  }

  mnvFactors[pose][index].push_back(factor);
  mnvFactorEvals[0][pose][index].push_back(evals[0]);
  mnvFactorEvals[1][pose][index].push_back(evals[1]);
  mnvFactorLins[pose][index].push_back(lin);

  mnFactors[pose].push_back(factor);
  mnFactorEvals[0][pose].push_back(evals[0]);
  mnFactorEvals[1][pose].push_back(evals[1]);
  mnFactorLins[pose].push_back(lin);

  return factor->getID();
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::addDepthCameraFactor(
    std::shared_ptr<DepthCameraFactor> factor) {
  if (checkFactor(factor)) {
    exit(-1);
  }

  const auto &pose = factor->getPoses()[0];

  assert(pose >= 0 && pose < NumPoses);

  if (pose < 0 || pose >= NumPoses) {
    LOG(ERROR) << "The pose must be valid." << std::endl;

    exit(-1);
  }

  int index;

  const auto &params = factor->getParams();

  std::shared_ptr<DepthCameraFactor> newFactor;

  if (params.size() == 0) {
    index = FactorIndex::JointDepthCamera;
  } else {
    assert(params[0] == VertexParamIndex);

    if (params[0] != VertexParamIndex) {
      LOG(ERROR) << "The vertex parameter must be valid." << std::endl;

      exit(-1);
    }

    index = FactorIndex::VertexDepthCamera;
  }

  std::array<std::shared_ptr<Factor::Evaluation>, 2> evals;
  std::shared_ptr<Factor::Linearization> lin;

  if (addFactor(index, factor, evals, lin)) {
    exit(-1);
  }

  mnvFactors[pose][index].push_back(factor);
  mnvFactorEvals[0][pose][index].push_back(evals[0]);
  mnvFactorEvals[1][pose][index].push_back(evals[1]);
  mnvFactorLins[pose][index].push_back(lin);

  mnFactors[pose].push_back(factor);
  mnFactorEvals[0][pose].push_back(evals[0]);
  mnFactorEvals[1][pose].push_back(evals[1]);
  mnFactorLins[pose].push_back(lin);

  return factor->getID();
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::addJointLimitFactor(
    std::shared_ptr<JointLimitFactor> factor) {
  if (checkFactor(factor)) {
    exit(-1);
  }

  const auto &joint = factor->getJoints()[0];

  assert(joint >= 0 && joint < NumJoints);

  if (joint < 0 || joint >= NumJoints) {
    LOG(ERROR) << "The joint must be valid." << std::endl;

    exit(-1);
  }

  const int pose = joint + 1;
  const int index = FactorIndex::JointLimit;

  std::array<std::shared_ptr<Factor::Evaluation>, 2> evals;
  std::shared_ptr<Factor::Linearization> lin;

  if (addFactor(index, factor, evals, lin)) {
    exit(-1);
  }

  mnvFactors[pose][index].push_back(factor);
  mnvFactorEvals[0][pose][index].push_back(evals[0]);
  mnvFactorEvals[1][pose][index].push_back(evals[1]);
  mnvFactorLins[pose][index].push_back(lin);

  mnFactors[pose].push_back(factor);
  mnFactorEvals[0][pose].push_back(evals[0]);
  mnFactorEvals[1][pose].push_back(evals[1]);
  mnFactorLins[pose].push_back(lin);

  return factor->getID();
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::addPoseConstFactor(
    std::shared_ptr<PoseConstFactor> factor) {
  if (checkFactor(factor)) {
    exit(-1);
  }

  const auto &pose = factor->getPoses()[0];

  assert(pose >= 0 && pose < NumPoses);

  if (pose < 0 || pose >= NumPoses) {
    LOG(ERROR) << "The pose must be valid." << std::endl;

    exit(-1);
  }

  const int index = FactorIndex::PoseConst;

  std::array<std::shared_ptr<Factor::Evaluation>, 2> evals;
  std::shared_ptr<Factor::Linearization> lin;

  if (addFactor(index, factor, evals, lin)) {
    exit(-1);
  }

  mnvFactors[pose][index].push_back(factor);
  mnvFactorEvals[0][pose][index].push_back(evals[0]);
  mnvFactorEvals[1][pose][index].push_back(evals[1]);
  mnvFactorLins[pose][index].push_back(lin);

  mnFactors[pose].push_back(factor);
  mnFactorEvals[0][pose].push_back(evals[0]);
  mnFactorEvals[1][pose].push_back(evals[1]);
  mnFactorLins[pose].push_back(lin);

  return factor->getID();
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::addJointConstFactor(
    std::shared_ptr<JointConstFactor> factor) {
  if (checkFactor(factor)) {
    exit(-1);
  }

  const auto &joint = factor->getJoints()[0];

  assert(joint >= 0 && joint < NumJoints);

  if (joint < 0 || joint >= NumJoints) {
    LOG(ERROR) << "The joint must be valid." << std::endl;

    exit(-1);
  }

  const int pose = joint + 1;
  const int index = FactorIndex::JointConst;

  std::array<std::shared_ptr<Factor::Evaluation>, 2> evals;
  std::shared_ptr<Factor::Linearization> lin;

  if (addFactor(index, factor, evals, lin)) {
    exit(-1);
  }

  mnvFactors[pose][index].push_back(factor);
  mnvFactorEvals[0][pose][index].push_back(evals[0]);
  mnvFactorEvals[1][pose][index].push_back(evals[1]);
  mnvFactorLins[pose][index].push_back(lin);

  mnFactors[pose].push_back(factor);
  mnFactorEvals[0][pose].push_back(evals[0]);
  mnFactorEvals[1][pose].push_back(evals[1]);
  mnFactorLins[pose].push_back(lin);

  return factor->getID();
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::addPoseFactor(
    std::shared_ptr<PoseFactor> factor) {
  if (checkFactor(factor)) {
    exit(-1);
  }

  const auto &pose = factor->getPoses()[0];

  assert(pose >= 0 && pose < NumPoses);

  if (pose < 0 || pose >= NumPoses) {
    LOG(ERROR) << "The pose must be valid." << std::endl;

    exit(-1);
  }

  const int index = FactorIndex::Pose;

  std::array<std::shared_ptr<Factor::Evaluation>, 2> evals;
  std::shared_ptr<Factor::Linearization> lin;

  if (addFactor(index, factor, evals, lin)) {
    exit(-1);
  }

  mnvFactors[pose][index].push_back(factor);
  mnvFactorEvals[0][pose][index].push_back(evals[0]);
  mnvFactorEvals[1][pose][index].push_back(evals[1]);
  mnvFactorLins[pose][index].push_back(lin);

  mnFactors[pose].push_back(factor);
  mnFactorEvals[0][pose].push_back(evals[0]);
  mnFactorEvals[1][pose].push_back(evals[1]);
  mnFactorLins[pose].push_back(lin);

  return factor->getID();
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::addJointFactor(
    std::shared_ptr<JointFactor> factor) {
  if (checkFactor(factor)) {
    exit(-1);
  }

  const auto &joint = factor->getJoints()[0];

  assert(joint >= 0 && joint < NumJoints);

  if (joint < 0 || joint >= NumJoints) {
    LOG(ERROR) << "The joint must be valid." << std::endl;

    exit(-1);
  }

  const int pose = joint + 1;
  const int index = FactorIndex::Joint;

  std::array<std::shared_ptr<Factor::Evaluation>, 2> evals;
  std::shared_ptr<Factor::Linearization> lin;

  if (addFactor(index, factor, evals, lin)) {
    exit(-1);
  }

  mnvFactors[pose][index].push_back(factor);
  mnvFactorEvals[0][pose][index].push_back(evals[0]);
  mnvFactorEvals[1][pose][index].push_back(evals[1]);
  mnvFactorLins[pose][index].push_back(lin);

  mnFactors[pose].push_back(factor);
  mnFactorEvals[0][pose].push_back(evals[0]);
  mnFactorEvals[1][pose].push_back(evals[1]);
  mnFactorLins[pose].push_back(lin);

  return factor->getID();
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::addParameterFactor(
    std::shared_ptr<ParameterFactor> factor) {
  if (checkFactor(factor)) {
    exit(-1);
  }

  const auto &param = factor->getParams()[0];

  assert(param >= 0 && param < NumParams);

  if (param < 0 || param >= NumParams) {
    LOG(ERROR) << "The parameter must be valid." << std::endl;

    exit(-1);
  }

  const int pose = 0;
  const int index = FactorIndex::Parameter;

  std::array<std::shared_ptr<Factor::Evaluation>, 2> evals;
  std::shared_ptr<Factor::Linearization> lin;

  if (addFactor(index, factor, evals, lin)) {
    exit(-1);
  }

  mnvFactors[pose][index].push_back(factor);
  mnvFactorEvals[0][pose][index].push_back(evals[0]);
  mnvFactorEvals[1][pose][index].push_back(evals[1]);
  mnvFactorLins[pose][index].push_back(lin);

  mnFactors[pose].push_back(factor);
  mnFactorEvals[0][pose].push_back(evals[0]);
  mnFactorEvals[1][pose].push_back(evals[1]);
  mnFactorLins[pose].push_back(lin);

  return factor->getID();
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::getNumPoses() const {
  return NumPoses;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::getNumShapes() const {
  return NumShapes;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::getNumJoints() const {
  return NumJoints;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::getNumParameters() const {
  return NumParams;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::setupModelInfo() {
  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::setupFactors() {
  mnvFactors.resize(
      NumPoses,
      std::vector<std::vector<std::shared_ptr<const Factor>>>(NumFactors));
  mnFactors.resize(NumPoses);
  mvFactors.resize(NumFactors);

  mnvFactorEvals[0].resize(
      NumPoses,
      std::vector<std::vector<std::shared_ptr<const Factor::Evaluation>>>(
          NumFactors));
  mnFactorEvals[0].resize(NumPoses);
  mvFactorEvals[0].resize(NumFactors);

  mnvFactorEvals[1].resize(
      NumPoses,
      std::vector<std::vector<std::shared_ptr<const Factor::Evaluation>>>(
          NumFactors));
  mnFactorEvals[1].resize(NumPoses);
  mvFactorEvals[1].resize(NumFactors);

  mnvFactorLins.resize(
      NumPoses,
      std::vector<std::vector<std::shared_ptr<const Factor::Linearization>>>(
          NumFactors));
  mnFactorLins.resize(NumPoses);
  mvFactorLins.resize(NumFactors);

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::getFaceParameterIndex() const {
  exit(-1);
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::getVertexParameterIndex() const {
  return VertexParamIndex;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::getCameraParameterIndex() const {
  return CamParamIndex;
}

template <int K, int P, int N, bool CamOpt>
const Results &Optimizer<K, P, N, CamOpt>::getResults() const {
  return mResults;
}

template <int K, int P, int N, bool CamOpt>
Scalar Optimizer<K, P, N, CamOpt>::getFobj() const {
  return mfobj[0];
}

template <int K, int P, int N, bool CamOpt>
AlignedVector<Pose> const &Optimizer<K, P, N, CamOpt>::getPoses() const {
  return mvPoses[0];
}

template <int K, int P, int N, bool CamOpt>
AlignedVector<VectorX> const &Optimizer<K, P, N, CamOpt>::getShapes() const {
  return mvShapes[0];
}

template <int K, int P, int N, bool CamOpt>
AlignedVector<Matrix3> const &Optimizer<K, P, N, CamOpt>::getJoints() const {
  return mvJoints[0];
}

template <int K, int P, int N, bool CamOpt>
AlignedVector<VectorX> const &Optimizer<K, P, N, CamOpt>::getParameters()
    const {
  return mvParams[0];
}

template <int K, int P, int N, bool CamOpt>
std::vector<std::vector<std::shared_ptr<const Factor::Evaluation>>> const &
Optimizer<K, P, N, CamOpt>::getEvaluations() const {
  return mvFactorEvals[0];
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::setupOptimization() {
  mvPoses[0].resize(NumPoses);
  mvPoses[1].resize(NumPoses);

  mvShapes[0].resize(NumShapes);
  mvShapes[1].resize(NumShapes);

  mvJoints[0].resize(NumJoints);
  mvJoints[1].resize(NumJoints);

  mvParams[0].resize(NumParams);
  mvParams[1].resize(NumParams);

  mvJointChange.resize(NumJoints);
  mvDJointChange.resize(NumJoints);

  mvmx.resize(NumPoses);
  mvmp.resize(NumPoses);
  mvmu.resize(NumPoses);

  mvPoseDG.resize(NumPoses);
  mvJointDG.resize(NumJoints);
  mvParamDG.resize(NumJoints);

  mPoseGN.resize(6, NumPoses);
  mJointGN.resize(3, NumJoints);

  mvPoseGN.reserve(NumPoses);
  mvJointGN.reserve(NumJoints);

  for (int i = 0; i < NumPoses; i++) {
    mvPoseGN.push_back(Eigen::Map<Vector6>(mPoseGN.data() + 6 * i));
  }

  for (int ii = 0; ii < NumJoints; ii++) {
    mvJointGN.push_back(Eigen::Map<Vector3>(mJointGN.data() + 3 * ii));
  }

  mvHuuInv.resize(NumJoints);

  mNumCollisions = 0;

  if (mOptions.check_collisions) {
    mCollisionParamSize = MaxNumCollisions;
  } else {
    mCollisionParamSize = 0;
  }

  mDCollisionParamOffset = 0;

  mH.setZero();
  mM.setZero();
  mHxB.setZero();
  mKuxp.setZero();
  mB.setZero();
  mku.setZero();

  int InnerSize[4] = {MaxDSize * MaxDSize, MaxDSize * (3 + VertexParamSize),
                      3 * DJointOffset, 6 * (3 + VertexParamSize)};

  mvH.reserve(NumJoints);
  mvM.reserve(NumPoses);
  mvHxB.reserve(NumJoints);
  mvKuxp.reserve(NumJoints);
  mvB.reserve(NumJoints);
  mvBp.reserve(NumJoints);
  mvBu.reserve(NumJoints);
  mvku.reserve(NumJoints);

  for (int ii = 0; ii < NumJoints; ii++) {
    mvH.push_back(
        Eigen::Map<Matrix<DSize, DSize>>(mH.data() + ii * InnerSize[0]));

    mvM.push_back(
        Eigen::Map<Matrix<DSize, DSize>>(mM.data() + ii * InnerSize[0]));

    mvHxB.push_back(
        Eigen::Map<Matrix<DSize, 3 + P>>(mHxB.data() + ii * InnerSize[1]));

    mvKuxp.push_back(Eigen::Map<RowMajorMatrix<3, 6 + ParamSize>>(
        mKuxp.data() + ii * InnerSize[2]));

    mvB.push_back(Eigen::Map<Matrix<6, 3 + P>>(mB.data() + ii * InnerSize[3]));

    mvBp.push_back(Eigen::Map<Matrix<3, P>, 0, Eigen::Stride<6, 1>>(
        mB.data() + 3 + ii * InnerSize[3]));

    mvBu.push_back(Eigen::Map<Matrix63>(mB.data() + 6 * P + ii * InnerSize[3]));

    mvku.push_back(Eigen::Map<Vector3>(mku.data() + ii * 3));
  }

  mvM.push_back(
      Eigen::Map<Matrix<DSize, DSize>>(mM.data() + NumJoints * InnerSize[0]));

  mvhx.resize(NumJoints);
  mvhp.resize(NumJoints);
  mvhu.resize(NumJoints);

  mvE.resize(NumJoints);

  mvDCostDx.resize(NumPoses);
  mvDCostDu.resize(NumPoses);
  mvDCostDp.resize(NumPoses);

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::init() const {
  scope::Matrix3 M = scope::Matrix3::Zero();
  scope::Vector3 m = scope::Vector3::Zero();
  scope::Matrix3 Minv;
  scope::Vector3 dt;

  if (linearize()) {
    exit(-1);
  }

  for (int i = 0; i < NumPoses; i++) {
    M += mvM[i].template block<3, 3>(3, 3);
    m += mvmx[i].template tail<3>();
  }

  M.diagonal().array() *= mDLambda;
  M.diagonal().array() += mOptions.delta;
  Minv.noalias() = M.inverse();

  dt.noalias() = -Minv * m;

  mE = 0.5 * dt.dot(m);
  M.diagonal().array() -= mOptions.delta;
  mE -= (1 - 1 / mDLambda) * M.diagonal().dot(dt.cwiseAbs2());

  mvPoses[1] = mvPoses[0];
  mvShapes[1] = mvShapes[0];
  mvJoints[1] = mvJoints[0];
  mvParams[1] = mvParams[0];

  Scalar stepsize = 1.0;

  const auto &fobj = mfobj;

  for (int iter = 0; iter < mOptions.max_inner_iters; iter++) {
    for (int i = 0; i < NumPoses; i++) {
      mvPoses[1][i].t = mvPoses[0][i].t + stepsize * dt;
    }

    if (evaluateFactors(1)) {
      exit(-1);
    }

    if (fobj[1] < fobj[0]) {
      Scalar rho = (fobj[1] - fobj[0]) / mE;

      if (rho > 0.75) {
        mDLambda = std::max(mDLambda * mOptions.lm_lambda_decrease, Scalar(1));
      } else if (rho < 0.25) {
        mDLambda = std::min(mDLambda * mOptions.lm_lambda_increase,
                            mOptions.lm_lambda_max);
      }

      accept();

      mStatus = Status::Accepted;

      return 0;
    }

    stepsize *= mOptions.stepsize_decrease_ratio;
  }

  mDLambda = std::max(mDLambda * mOptions.lm_lambda_decrease, Scalar(1));

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::initialOptimize() const {
  const auto &model = mModel;

  if (linearizeFactors(0)) {
    exit(-1);
  }

  mH0 = mvM[0]
            .template topLeftCorner<DPoseSize + DParamSize,
                                    DPoseSize + DParamSize>();
  mH0.template topRightCorner<DPoseSize, DParamSize>() =
      mH0.template bottomLeftCorner<DParamSize, DPoseSize>().transpose();

  if (mOptions.method == Method::LM) {
    mH0.diagonal().array() *= mDLambda;
  }

  mH0.diagonal().array() += mOptions.delta;

  mh0.template head<DPoseSize>() = mvmx[0];
  mh0.template tail<DParamSize>() = mvmp[0];

  mhGN.setZero();
  // mHchol0.compute(mRawH0);
  // mhGN = -mHchol0.solve(mh0);
  scope::Matrix6 Hinv = mH0.template topLeftCorner<6, 6>().inverse();
  mhGN.template head<6>().noalias() = -Hinv * mvmx[0];

  mvPoseGN[0] = mhGN.template head<DPoseSize>();
  mParamGN = mhGN.template tail<DParamSize>();

  mE = 0.5 * mh0.dot(mhGN);
  mH0.diagonal().array() -= mOptions.delta;
  mE -= (0.5 - 0.5 / mDLambda) * mH0.diagonal().dot(mhGN.cwiseAbs2());

  Scalar stepsize = 1.0;

  const auto &fobj = mfobj;

  for (int iter = 0; iter < mOptions.max_inner_iters; iter++) {
    mvParams[1] = mvParams[0];

    for (int i = 0; i < NumParams; i++) {
      mvParams[1][i] +=
          stepsize * mParamGN.segment(DParamOffsets[i], ParamSizes[i]);
    }

    mDRootPoseChange = mvPoseGN[0] * stepsize;
    Pose::exp(mDRootPoseChange, mRootPoseChange);
    mvPoses[1][0].R.noalias() = mRootPoseChange.R * mvPoses[0][0].R;
    mvPoses[1][0].t = mRootPoseChange.t;
    mvPoses[1][0].t.noalias() += mRootPoseChange.R * mvPoses[0][0].t;

    if (evaluateInitialFactors(1)) {
      exit(-1);
    }

    if (fobj[1] < fobj[0]) {
      Scalar rho = (fobj[1] - fobj[0]) / mE;

      if (rho > 0.75 && iter == 0) {
        mDLambda = std::max(mDLambda * mOptions.lm_lambda_decrease, Scalar(1));
      } else if (rho < 0.25 && iter > 0) {
        mDLambda = std::min(mDLambda * mOptions.lm_lambda_increase,
                            mOptions.lm_lambda_max);
      }

      mfobj[0] = mfobj[1];

      mvPoses[0][0] = mvPoses[1][0];
      mvParams[0].swap(mvParams[1]);

      mvFobj[0].swap(mvFobj[1]);

      mnvFactorEvals[0].swap(mnvFactorEvals[1]);
      mnFactorEvals[0].swap(mnFactorEvals[1]);

      mvFactorEvals[0].swap(mvFactorEvals[1]);
      mFactorEvals[0].swap(mFactorEvals[1]);

      mStatus = Status::Accepted;

      return 0;
    }

    stepsize *= mOptions.stepsize_decrease_ratio;
  }

  mDLambda = std::max(mDLambda * mOptions.lm_lambda_decrease, Scalar(1));

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::update(Scalar stepsize) const {
  assert(stepsize > 0);

  const auto &model = mModel;

  mvParams[1] = mvParams[0];

  for (int i = 0; i < NumParams; i++) {
    mvParams[1][i] +=
        stepsize * mParamGN.segment(DParamOffsets[i], ParamSizes[i]);
  }

  mDRootPoseChange = mvPoseGN[0] * stepsize;
  Pose::exp(mDRootPoseChange, mRootPoseChange);
  mvPoses[1][0].R.noalias() = mRootPoseChange.R * mvPoses[0][0].R;
  mvPoses[1][0].t = mRootPoseChange.t;
  mvPoses[1][0].t.noalias() += mRootPoseChange.R * mvPoses[0][0].t;

  for (int ii = 0; ii < NumJoints; ii++) {
    mvDJointChange[ii] = mvJointGN[ii] * stepsize;
    math::SO3::exp(mvDJointChange[ii], mvJointChange[ii]);
    mvJoints[1][ii].noalias() = mvJointChange[ii] * mvJoints[0][ii];
  }

  model->FK(mvPoses[1], mRelJointLocations[1], mvPoses[1][0], mvJoints[1],
            mvParams[1][0]);

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::evaluateFactors(int n) const {
  assert(n >= 0 && n <= 1);

  const auto &poses = mvPoses[n];
  const auto &shapes = mvShapes[n];
  const auto &joints = mvJoints[n];
  const auto &params = mvParams[n];

  auto &fobj = mfobj[n];
  auto &Fobj = mvFobj[n];
  auto &Evals = mFactorEvals[n];
  auto &FactorEvals = mvFactorEvals[n];

  Fobj.setZero();

  const auto &model = mModel;

#pragma omp parallel for
  for (int i = 0; i < mFactors.size(); i++) {
    const auto &factor = mFactors[i];
    auto &eval = *Evals[i];

    factor->evaluate(poses, shapes, joints, params, eval);
  }

  for (int k = 0; k < NumFactors; k++) {
    const auto &factors = mvFactors[k];
    const auto &evals = FactorEvals[k];

    for (int i = 0; i < factors.size(); i++) {
      const auto &factor = factors[i];

      auto &eval = *evals[i];

      assert(eval.status == Factor::Status::VALID);

      if (eval.status != Factor::Status::VALID) {
        LOG(ERROR) << "Failed to evaluate the factor." << std::endl;

        mStatus = Status::Failed;

        exit(-1);
      }

      Fobj[k] += eval.f;
    }
  }

  fobj = 0.5 * mOptions.weights.dot(Fobj);

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::evaluateInitialFactors(int n) const {
  assert(n >= 0 && n <= 1);

  const auto &poses = mvPoses[n];
  const auto &shapes = mvShapes[n];
  const auto &joints = mvJoints[n];
  const auto &params = mvParams[n];

  auto &fobj = mfobj[n];
  auto &Fobj = mvFobj[n];

  const auto &factors = mnFactors[0];

  auto &Evals = mnFactorEvals[n][0];
  auto &FactorEvals = mnvFactorEvals[n][0];

  Fobj.setZero();

  const auto &model = mModel;

  for (int i = 0; i < factors.size(); i++) {
    const auto &factor = factors[i];
    auto &eval = *Evals[i];

    factor->evaluate(poses, shapes, joints, params, eval);
  }

  for (int k = 0; k < NumFactors; k++) {
    const auto &factors = mnvFactors[0][k];
    const auto &evals = FactorEvals[k];

    for (int i = 0; i < factors.size(); i++) {
      const auto &factor = factors[i];

      auto &eval = *evals[i];

      assert(eval.status == Factor::Status::VALID);

      if (eval.status != Factor::Status::VALID) {
        LOG(ERROR) << "Failed to evaluate the factor." << std::endl;

        mStatus = Status::Failed;

        exit(-1);
      }

      Fobj[k] += eval.f;
    }
  }

  fobj = 0.5 * mOptions.weights.dot(Fobj);

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::updateGaussNewtonInfo() const {
  if (!mOptions.check_collisions) {
    return 0;
  }

  int CollisionParamSize = mCollisionParamSize;

  mNumCollisions = 0;

  const auto &factors = mvFactors[FactorIndex::Collision];
  const auto &evals = mvFactorEvals[0][FactorIndex::Collision];

  for (int i = 0; i < factors.size(); i++) {
    const auto &factor = factors[i];

    const auto &eval = *evals[i];

    if (eval.f > 0) {
      mNumCollisions++;
    }
  }

  mCollisionParamSize = std::min(mNumCollisions, MaxNumCollisions);

  if (mCollisionParamSize == CollisionParamSize) {
    return 0;
  }

#if 0
  mDCollisionParamOffset = DParamOffset;

  mDCamParamOffset = mDCollisionParamOffset + mCollisionParamSize;
  mDVertexParamOffset = mDCamParamOffset + mCamParamSize;
  mDFaceParamOffset = mDVertexParamOffset + mVertexParamSize;

  mDParamSize = mParamSize + mCollisionParamSize;

  mDJointOffset = mDParamOffset + mDParamSize;

  mDSize = mDJointOffset + mDJointSize;

  for (int ii = 0; ii < NumJoints; ii++) {
    new (&mvH[ii]) Eigen::Map<scope::MatrixX>(mvH[ii].data(), mDSize, mDSize);
    new (&mvM[ii]) Eigen::Map<scope::MatrixX>(mvM[ii].data(), mDSize, mDSize);
    new (&mvHxB[ii])
        Eigen::Map<scope::MatrixX>(mvHxB[ii].data(), mDSize, mvH[ii].cols());
    new (&mvKuxp[ii]) Eigen::Map<scope::RowMajorMatrix3X>(mvKuxp[ii].data(), 3,
                                                          mDJointOffset);
  }

  new (&mvM[NumJoints])
      Eigen::Map<scope::MatrixX>(mvM[NumJoints].data(), mDSize, mDSize);
#endif

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::accept() const {
  mfobj[0] = mfobj[1];

  mvPoses[0].swap(mvPoses[1]);
  mvShapes[0].swap(mvShapes[1]);
  mvJoints[0].swap(mvJoints[1]);
  mvParams[0].swap(mvParams[1]);
  mRelJointLocations[0].swap(mRelJointLocations[1]);

  mvFobj[0].swap(mvFobj[1]);

  mnvFactorEvals[0].swap(mnvFactorEvals[1]);
  mnFactorEvals[0].swap(mnFactorEvals[1]);

  mvFactorEvals[0].swap(mvFactorEvals[1]);
  mFactorEvals[0].swap(mFactorEvals[1]);

  updateGaussNewtonInfo();

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::linearize() const {
  const auto &poses = mvPoses[0];
  const auto &shapes = mvShapes[0];
  const auto &joints = mvJoints[0];
  const auto &params = mvParams[0];

  // linearize forward kinematics
  const auto &model = mModel;
  model->DFK(mvB, poses);

  // linearize factors
  const auto &Evals = mFactorEvals[0];
  auto &Lins = mFactorLins;

#pragma omp parallel for
  for (int i = 0; i < mFactors.size(); i++) {
    const auto &factor = mFactors[i];
    const auto &eval = *Evals[i];
    auto &lin = *Lins[i];

    assert(eval.status == Factor::Status::VALID);

    factor->linearize(poses, shapes, joints, params, eval, lin);

    assert(lin.status == Factor::Status::VALID);
  }

  for (const auto &lin : Lins) {
    if (lin->status != Factor::Status::VALID) {
      LOG(ERROR) << "Failed to linearize the factor." << std::endl;

      mStatus = Status::Failed;

      exit(-1);
    }
  }

  mM.setZero();

  mvmx.assign(NumPoses, Vector6::Zero());
  mvmp.assign(NumPoses, Vector<DParamSize>::Zero());
  mvmu.assign(NumPoses, Vector3::Zero());

  updateJointPinholeCameraFactorGaussNewton();
  updateVertexPinholeCameraFactorGaussNewton();
  updateFullJointPinholeCameraFactorGaussNewton();
  updateFullVertexPinholeCameraFactorGaussNewton();
  updateUnitPOFFactorGaussNewton();
  updateScaledPOFFactorGaussNewton();
  updateRelPOFFactorGaussNewton();
  updateJointDepthCameraFactorGaussNewton();
  updateVertexDepthCameraFactorGaussNewton();
  updateJointLimitFactorGaussNewton();
  updateCollisionFactorGaussNewton();
  updatePoseConstFactorGaussNewton();
  updateJointConstFactorGaussNewton();
  updatePoseFactorGaussNewton();
  updateShapeFactorGaussNewton();
  updateJointFactorGaussNewton();
  updateParamFactorGaussNewton();

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::linearizeFactors(int pose) const {
  assert(pose >= 0 && pose < NumPoses);

  if (pose < 0 || pose >= NumPoses) {
    LOG(ERROR) << "The pose must be valid." << std::endl;

    exit(-1);
  }

  const auto &poses = mvPoses[0];
  const auto &shapes = mvShapes[0];
  const auto &joints = mvJoints[0];
  const auto &params = mvParams[0];

  const auto &factors = mnFactors[pose];
  const auto &evals = mnFactorEvals[0][pose];
  auto &lins = mnFactorLins[pose];

  // linearize factors
  for (int i = 0; i < factors.size(); i++) {
    const auto &factor = factors[i];
    const auto &eval = *evals[i];
    auto &lin = *lins[i];

    assert(eval.status == Factor::Status::VALID);

    factor->linearize(poses, shapes, joints, params, eval, lin);

    assert(lin.status == Factor::Status::VALID);
  }

  mvM[pose].setZero();

  mvmx[pose].setZero();
  mvmp[pose].setZero();
  mvmu[pose].setZero();

  updateJointPinholeCameraFactorGaussNewton(pose);
  updateVertexPinholeCameraFactorGaussNewton(pose);
  updateFullJointPinholeCameraFactorGaussNewton(pose);
  updateFullVertexPinholeCameraFactorGaussNewton(pose);
  updateUnitPOFFactorGaussNewton(pose);
  updateScaledPOFFactorGaussNewton(pose);
  updateRelPOFFactorGaussNewton(pose);
  updateJointDepthCameraFactorGaussNewton(pose);
  updateVertexDepthCameraFactorGaussNewton(pose);
  updateJointLimitFactorGaussNewton(pose);
  updateCollisionFactorGaussNewton(pose);
  updatePoseConstFactorGaussNewton(pose);
  updateJointConstFactorGaussNewton(pose);
  updatePoseFactorGaussNewton(pose);
  updateShapeFactorGaussNewton(pose);
  updateJointFactorGaussNewton(pose);
  updateParamFactorGaussNewton(pose);

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::updateJointPinholeCameraFactorGaussNewton()
    const {
  const auto &factors = mvFactors[FactorIndex::JointPinholeCamera];
  const auto &evals = mvFactorEvals[0][FactorIndex::JointPinholeCamera];
  const auto &lins = mvFactorLins[FactorIndex::JointPinholeCamera];

  const auto &weight = mOptions.weights[FactorIndex::JointPinholeCamera];

  for (int i = 0; i < factors.size(); i++) {
    const auto &factor = factors[i];

    const auto &lin = std::dynamic_pointer_cast<
        const JointPinholeCameraFactor::Linearization>(lins[i]);

    const auto &pose = factor->getPoses()[0];

    const auto &JPose =
        Eigen::Map<const Matrix<2, 6>>(lin->jacobians[0][0].data());

    const auto &gPose = lin->gPose;

    auto &M = mvM[pose];
    auto &mx = mvmx[pose];

    M.template topLeftCorner<DPoseSize, DPoseSize>().noalias() +=
        weight * JPose.transpose() * JPose;
    mx += weight * gPose;
  }

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::updateVertexPinholeCameraFactorGaussNewton()
    const {
  const auto &options = mOptions;

  const auto &factors = mvFactors[FactorIndex::VertexPinholeCamera];
  const auto &evals = mvFactorEvals[0][FactorIndex::VertexPinholeCamera];
  const auto &lins = mvFactorLins[FactorIndex::VertexPinholeCamera];

  const auto &weight = options.weights[FactorIndex::VertexPinholeCamera];

  for (int i = 0; i < factors.size(); i++) {
    const auto &factor = factors[i];

    const auto &lin = std::dynamic_pointer_cast<
        const VertexPinholeCameraFactor::Linearization>(lins[i]);

    const auto &pose = factor->getPoses()[0];

    const auto &JPose =
        Eigen::Map<const Matrix<2, 6>>(lin->jacobians[0][0].data());
    const auto &JVertex =
        Eigen::Map<const Matrix<2, P>>(lin->jacobians[3][0].data());

    const auto &gPose = lin->gPose;
    const auto &gVertex = Eigen::Map<const Vector<P>>(lin->gVertex.data());

    auto &M = mvM[pose];

    auto &mx = mvmx[pose];
    auto &mp = mvmp[pose];

    M.template topLeftCorner<DPoseSize, DPoseSize>().noalias() +=
        weight * JPose.transpose() * JPose;

    M.template block<VertexParamSize, DPoseSize>(DVertexParamOffset,
                                                 DPoseOffset)
        .noalias() += weight * JVertex.transpose() * JPose;

    M.template block<VertexParamSize, VertexParamSize>(DVertexParamOffset,
                                                       DVertexParamOffset)
        .noalias() += weight * JVertex.transpose() * JVertex;

    mx += weight * gPose;
    mp.template segment<VertexParamSize>(DVertexParamOffset - DParamOffset) +=
        weight * gVertex;
  }

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::updateFullJointPinholeCameraFactorGaussNewton()
    const {
  const auto &options = mOptions;

  const auto &factors = mvFactors[FactorIndex::FullJointPinholeCamera];
  const auto &evals = mvFactorEvals[0][FactorIndex::FullJointPinholeCamera];
  const auto &lins = mvFactorLins[FactorIndex::FullJointPinholeCamera];

  const auto &weight = options.weights[FactorIndex::FullJointPinholeCamera];

  for (int i = 0; i < factors.size(); i++) {
    const auto &factor = factors[i];

    const auto &lin = std::dynamic_pointer_cast<
        const FullJointPinholeCameraFactor::Linearization>(lins[i]);

    const auto &pose = factor->getPoses()[0];
    const auto &cam = factor->getParams()[0];

    const auto &JPose =
        Eigen::Map<const Matrix<2, 6>>(lin->jacobians[0][0].data());
    const auto &JCam =
        Eigen::Map<const Matrix<2, 4>>(lin->jacobians[3][0].data());

    const auto &gPose = lin->gPose;
    const auto &gCam = lin->gCam;

    auto &M = mvM[pose];

    auto &mx = mvmx[pose];
    auto &mp = mvmp[pose];

    M.template topLeftCorner<DPoseSize, DPoseSize>().noalias() +=
        weight * JPose.transpose() * JPose;
    M.template block<4, DPoseSize>(DCamParamOffset, DPoseOffset).noalias() +=
        weight * JCam.transpose() * JPose;
    M.template block<4, 4>(DCamParamOffset, DCamParamOffset) =
        weight * JCam.transpose() * JCam;

    mx += weight * gPose;
    mp.template segment<4>(DCamParamOffset - DParamOffset) += weight * gCam;
  }

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::updateFullVertexPinholeCameraFactorGaussNewton()
    const {
  const auto &options = mOptions;

  const auto &factors = mvFactors[FactorIndex::FullVertexPinholeCamera];
  const auto &evals = mvFactorEvals[0][FactorIndex::FullVertexPinholeCamera];
  const auto &lins = mvFactorLins[FactorIndex::FullVertexPinholeCamera];

  const auto &weight = options.weights[FactorIndex::FullVertexPinholeCamera];

  for (int i = 0; i < factors.size(); i++) {
    const auto &factor = factors[i];

    const auto &lin = std::dynamic_pointer_cast<
        const FullVertexPinholeCameraFactor::Linearization>(lins[i]);

    const auto &pose = factor->getPoses()[0];
    const auto &cam = factor->getParams()[0];

    const auto &JPose =
        Eigen::Map<const Matrix<2, 6>>(lin->jacobians[0][0].data());
    const auto &JVertex =
        Eigen::Map<const Matrix<2, P>>(lin->jacobians[3][0].data());
    const auto &JCam =
        Eigen::Map<const Matrix<2, 4>>(lin->jacobians[3][1].data());

    const auto &gPose = lin->gPose;
    const auto &gVertex = Eigen::Map<const Vector<P>>(lin->gVertex.data());
    const auto &gCam = lin->gCam;

    auto &M = mvM[pose];

    auto &mx = mvmx[pose];
    auto &mp = mvmp[pose];

    M.template topLeftCorner<DPoseSize, DPoseSize>().noalias() +=
        weight * JPose.transpose() * JPose;
    M.template block<VertexParamSize, DPoseSize>(DVertexParamOffset,
                                                 DPoseOffset)
        .noalias() += weight * JVertex.transpose() * JPose;
    M.template block<4, DPoseSize>(DCamParamOffset, DPoseOffset).noalias() +=
        weight * JCam.transpose() * JPose;
    M.template block<VertexParamSize, VertexParamSize>(DVertexParamOffset,
                                                       DVertexParamOffset) =
        weight * JVertex.transpose() * JVertex;
    M.template block<VertexParamSize, 4>(DVertexParamOffset, DCamParamOffset) =
        weight * JVertex.transpose() * JCam;
    M.template block<4, VertexParamSize>(DCamParamOffset, DVertexParamOffset) =
        M.template block<VertexParamSize, 4>(DVertexParamOffset,
                                             DCamParamOffset)
            .transpose();
    M.template block<4, 4>(DCamParamOffset, DCamParamOffset) =
        weight * JCam.transpose() * JCam;

    mx += weight * gPose;
    mp.template segment<VertexParamSize>(DVertexParamOffset - DParamOffset) +=
        weight * gVertex;
    mp.template segment<4>(DCamParamOffset - DParamOffset) += weight * gCam;
  }

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::updateUnitPOFFactorGaussNewton() const {
  const auto &factors = mvFactors[FactorIndex::UnitPOF];
  const auto &evals = mvFactorEvals[0][FactorIndex::UnitPOF];
  const auto &lins = mvFactorLins[FactorIndex::UnitPOF];

  const auto &weight = mOptions.weights[FactorIndex::UnitPOF];

  for (int i = 0; i < factors.size(); i++) {
    const auto &factor = factors[i];

    const auto &lin =
        std::dynamic_pointer_cast<const UnitPOFFactor::Linearization>(lins[i]);

    const auto &pose = factor->getPoses()[0];

    assert(pose >= 0 && pose < NumPoses);

    const auto &JR =
        Eigen::Map<const Matrix<3, 3>>(lin->jacobians[0][0].data());

    const auto &gR = Eigen::Map<const Vector3>(lin->gPose.data());

    auto &M = mvM[pose];
    auto &mx = mvmx[pose];

    M.template topLeftCorner<3, 3>().noalias() += weight * JR.transpose() * JR;

    mx.template head<3>() += weight * gR;
  }

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::updateScaledPOFFactorGaussNewton() const {
  const auto &factors = mvFactors[FactorIndex::ScaledPOF];
  const auto &evals = mvFactorEvals[0][FactorIndex::ScaledPOF];
  const auto &lins = mvFactorLins[FactorIndex::ScaledPOF];

  const auto &weight = mOptions.weights[FactorIndex::ScaledPOF];

  for (int i = 0; i < factors.size(); i++) {
    const auto &factor = factors[i];

    const auto &lin =
        std::dynamic_pointer_cast<const ScaledPOFFactor::Linearization>(
            lins[i]);

    const auto &pose = factor->getPoses()[0];

    assert(pose >= 0 && pose < NumPoses);

    const auto &JR =
        Eigen::Map<const Matrix<3, 3>>(lin->jacobians[0][0].data());
    const auto &JVertex =
        Eigen::Map<const Matrix<3, P>>(lin->jacobians[3][0].data());

    const auto &gR = Eigen::Map<const Vector3>(lin->gPose.data());
    const auto &gVertex = Eigen::Map<const Vector<P>>(lin->gVertex.data());

    auto &M = mvM[pose];
    auto &mx = mvmx[pose];
    auto &mp = mvmp[pose];

    M.template topLeftCorner<3, 3>().noalias() += weight * JR.transpose() * JR;

    M.template block<VertexParamSize, 3>(DVertexParamOffset, DPoseOffset)
        .noalias() += weight * JVertex.transpose() * JR;

    M.template block<VertexParamSize, VertexParamSize>(DVertexParamOffset,
                                                       DVertexParamOffset)
        .noalias() += weight * JVertex.transpose() * JVertex;

    mx.template head<3>() += weight * gR;

    mp.template segment<VertexParamSize>(DVertexParamOffset - DParamOffset) +=
        weight * gVertex;
  }

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::updateRelPOFFactorGaussNewton() const {
  const auto &factors = mvFactors[FactorIndex::RelPOF];
  const auto &evals = mvFactorEvals[0][FactorIndex::RelPOF];
  const auto &lins = mvFactorLins[FactorIndex::RelPOF];

  const auto &weight = mOptions.weights[FactorIndex::RelPOF];

  for (int i = 0; i < factors.size(); i++) {
    const auto &factor = factors[i];

    const auto &lin =
        std::dynamic_pointer_cast<const RelPOFFactor::Linearization>(lins[i]);

    const auto &pose = factor->getPoses()[0];

    assert(pose > 0 && pose < NumPoses);

    const auto &JR =
        Eigen::Map<const Matrix<3, 3>>(lin->jacobians[0][0].data());
    const auto &JVertex =
        Eigen::Map<const Matrix<3, P>>(lin->jacobians[3][0].data());
    const auto &JJoint =
        Eigen::Map<const Matrix<3, 3>>(lin->jacobians[2][0].data());

    const auto &gR = Eigen::Map<const Vector3>(lin->gPose.data());
    const auto &gVertex = Eigen::Map<const Vector<P>>(lin->gVertex.data());
    const auto &gJoint = lin->gJoint;

    auto &M = mvM[pose];
    auto &mx = mvmx[pose];
    auto &mu = mvmu[pose];
    auto &mp = mvmp[pose];

    M.template topLeftCorner<3, 3>().noalias() += weight * JR.transpose() * JR;

    M.template block<VertexParamSize, 3>(DVertexParamOffset, DPoseOffset)
        .noalias() += weight * JVertex.transpose() * JR;

    M.template block<DJointSize, 3>(DJointOffset, DPoseOffset).noalias() +=
        weight * JJoint.transpose() * JR;

    M.template block<VertexParamSize, VertexParamSize>(DVertexParamOffset,
                                                       DVertexParamOffset)
        .noalias() += weight * JVertex.transpose() * JVertex;

    M.template block<DJointSize, VertexParamSize>(DJointOffset,
                                                  DVertexParamOffset)
        .noalias() += weight * JJoint.transpose() * JVertex;

    M.template block<DJointSize, DJointSize>(DJointOffset, DJointOffset)
        .noalias() += weight * JJoint.transpose() * JJoint;

    mx.template head<3>() += weight * gR;

    mp.template segment<VertexParamSize>(DVertexParamOffset - DParamOffset) +=
        weight * gVertex;

    mu.noalias() += weight * gJoint;
  }

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::updateJointDepthCameraFactorGaussNewton()
    const {
  const auto &factors = mvFactors[FactorIndex::JointDepthCamera];
  const auto &evals = mvFactorEvals[0][FactorIndex::JointDepthCamera];
  const auto &lins = mvFactorLins[FactorIndex::JointDepthCamera];

  const auto &weight = mOptions.weights[FactorIndex::JointDepthCamera];

  for (int i = 0; i < factors.size(); i++) {
    const auto &factor = factors[i];

    const auto &lin =
        std::dynamic_pointer_cast<const JointDepthCameraFactor::Linearization>(
            lins[i]);

    const auto &pose = factor->getPoses()[0];

    const auto &JPose =
        Eigen::Map<const Matrix<3, 6>>(lin->jacobians[0][0].data());

    const auto &gPose = lin->gPose;

    auto &M = mvM[pose];
    auto &mx = mvmx[pose];

    M.template topLeftCorner<DPoseSize, DPoseSize>().noalias() +=
        weight * JPose.transpose() * JPose;
    mx += weight * gPose;
  }

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::updateVertexDepthCameraFactorGaussNewton()
    const {
  const auto &options = mOptions;

  const auto &factors = mvFactors[FactorIndex::VertexDepthCamera];
  const auto &evals = mvFactorEvals[0][FactorIndex::VertexDepthCamera];
  const auto &lins = mvFactorLins[FactorIndex::VertexDepthCamera];

  const auto &weight = options.weights[FactorIndex::VertexDepthCamera];

  for (int i = 0; i < factors.size(); i++) {
    const auto &factor = factors[i];

    const auto &lin =
        std::dynamic_pointer_cast<const VertexDepthCameraFactor::Linearization>(
            lins[i]);

    const auto &pose = factor->getPoses()[0];

    const auto &JPose =
        Eigen::Map<const Matrix<3, 6>>(lin->jacobians[0][0].data());
    const auto &JVertex =
        Eigen::Map<const Matrix<3, P>>(lin->jacobians[3][0].data());

    const auto &gPose = lin->gPose;
    const auto &gVertex = Eigen::Map<const Vector<P>>(lin->gVertex.data());

    auto &M = mvM[pose];

    auto &mx = mvmx[pose];
    auto &mp = mvmp[pose];

    M.template topLeftCorner<DPoseSize, DPoseSize>().noalias() +=
        weight * JPose.transpose() * JPose;

    M.template block<VertexParamSize, DPoseSize>(DVertexParamOffset,
                                                 DPoseOffset)
        .noalias() += weight * JVertex.transpose() * JPose;

    M.template block<VertexParamSize, VertexParamSize>(DVertexParamOffset,
                                                       DVertexParamOffset)
        .noalias() += weight * JVertex.transpose() * JVertex;

    mx += weight * gPose;
    mp.template segment<VertexParamSize>(DVertexParamOffset - DParamOffset) +=
        weight * gVertex;
  }

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::updateJointLimitFactorGaussNewton() const {
  const auto &options = mOptions;

  const auto &factors = mvFactors[FactorIndex::JointLimit];
  const auto &evals = mvFactorEvals[0][FactorIndex::JointLimit];
  const auto &lins = mvFactorLins[FactorIndex::JointLimit];

  const auto &weight = options.weights[FactorIndex::JointLimit];

  for (int i = 0; i < factors.size(); i++) {
    const auto &factor = factors[i];

    const auto &lin = lins[i];
    const auto &eval = evals[i];

    const auto &pose = factor->getJoints()[0] + 1;

    const auto &JJoint =
        Eigen::Map<const Matrix<6, DJointSize>>(lin->jacobians[2][0].data());
    const auto &Error = eval->error;

    auto &M = mvM[pose];
    auto &mu = mvmu[pose];

    M.template bottomRightCorner<DJointSize, DJointSize>().noalias() +=
        weight * JJoint.transpose() * JJoint;
    mu.noalias() += weight * JJoint.transpose() * Error;
  }

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::updateCollisionFactorGaussNewton() const {
  // TODO: add collision factors
  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::updatePoseConstFactorGaussNewton() const {
  const auto &options = mOptions;

  const auto &factors = mvFactors[FactorIndex::PoseConst];
  const auto &evals = mvFactorEvals[0][FactorIndex::PoseConst];
  const auto &lins = mvFactorLins[FactorIndex::PoseConst];

  const auto &weight = options.weights[FactorIndex::PoseConst];

  for (int i = 0; i < factors.size(); i++) {
    const auto &factor = factors[i];

    const auto &lin = lins[i];
    const auto &eval = evals[i];

    const auto &pose = factor->getPoses()[0];

    const auto &JPose = Eigen::Map<const Matrix6>(lin->jacobians[0][0].data());
    const auto &Error = Eigen::Map<const Vector6>(eval->error.data());

    auto &M = mvM[pose];
    auto &mx = mvmx[pose];

    M.template topLeftCorner<DPoseSize, DPoseSize>().noalias() +=
        weight * JPose.transpose() * JPose;
    mx.noalias() += weight * JPose.transpose() * Error;
  }

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::updateJointConstFactorGaussNewton() const {
  const auto &options = mOptions;

  const auto &factors = mvFactors[FactorIndex::JointConst];
  const auto &evals = mvFactorEvals[0][FactorIndex::JointConst];
  const auto &lins = mvFactorLins[FactorIndex::JointConst];

  const auto &weight = options.weights[FactorIndex::JointConst];

  for (int i = 0; i < factors.size(); i++) {
    const auto &factor = factors[i];

    const auto &lin = lins[i];
    const auto &eval = evals[i];

    const auto &pose = factor->getJoints()[0] + 1;

    const auto &JJoint = Eigen::Map<const Matrix3>(lin->jacobians[2][0].data());
    const auto &Error = Eigen::Map<const Vector3>(eval->error.data());

    auto &M = mvM[pose];
    auto &mu = mvmu[pose];

    M.template bottomRightCorner<DJointSize, DJointSize>().noalias() +=
        weight * JJoint.transpose() * JJoint;
    mu.noalias() += weight * JJoint.transpose() * Error;
  }

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::updatePoseFactorGaussNewton() const {
  const auto &options = mOptions;

  const auto &factors = mvFactors[FactorIndex::Pose];
  const auto &evals = mvFactorEvals[0][FactorIndex::Pose];
  const auto &lins = mvFactorLins[FactorIndex::Pose];

  const auto &weight = options.weights[FactorIndex::Pose];

  for (int i = 0; i < factors.size(); i++) {
    const auto &factor = factors[i];

    const auto &lin = lins[i];
    const auto &eval = evals[i];

    const auto &pose = factor->getPoses()[0];

    const auto &JPose = Eigen::Map<const Matrix6>(lin->jacobians[0][0].data());
    const auto &Error = Eigen::Map<const Vector6>(eval->error.data());

    auto &M = mvM[pose];
    auto &mx = mvmx[pose];

    M.template topLeftCorner<DPoseSize, DPoseSize>().noalias() +=
        weight * JPose.transpose() * JPose;
    mx.noalias() += weight * JPose.transpose() * Error;
  }

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::updateShapeFactorGaussNewton() const {
  // TODO: add shape factors

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::updateJointFactorGaussNewton() const {
  const auto &options = mOptions;

  const auto &factors = mvFactors[FactorIndex::Joint];
  const auto &evals = mvFactorEvals[0][FactorIndex::Joint];
  const auto &lins = mvFactorLins[FactorIndex::Joint];

  const auto &weight = options.weights[FactorIndex::Joint];

  for (int i = 0; i < factors.size(); i++) {
    const auto &factor = factors[i];

    const auto &lin = lins[i];
    const auto &eval = evals[i];

    const auto &pose = factor->getJoints()[0] + 1;

    const auto &JJoint = Eigen::Map<const Matrix3>(lin->jacobians[2][0].data());
    const auto &Error = Eigen::Map<const Vector3>(eval->error.data());

    auto &M = mvM[pose];
    auto &mu = mvmu[pose];

    M.template bottomRightCorner<DJointSize, DJointSize>().noalias() +=
        weight * JJoint.transpose() * JJoint;
    mu.noalias() += weight * JJoint.transpose() * Error;
  }

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::updateParamFactorGaussNewton() const {
  const auto &options = mOptions;

  const auto &factors = mvFactors[FactorIndex::Parameter];
  const auto &evals = mvFactorEvals[0][FactorIndex::Parameter];
  const auto &lins = mvFactorLins[FactorIndex::Parameter];

  const auto &weight = options.weights[FactorIndex::Parameter];

  auto &M = mvM[0];
  auto &mp = mvmp[0];

  for (int i = 0; i < factors.size(); i++) {
    const auto &factor = factors[i];

    const auto &lin = lins[i];
    const auto &eval = evals[i];

    const auto &param = factor->getParams()[0];

    switch (param) {
      case VertexParamIndex: {
        const auto &JParam =
            Eigen::Map<const Matrix<P, P>>(lin->jacobians[3][0].data());
        const auto &Error = Eigen::Map<const Vector<P>>(eval->error.data());

        M.template block<VertexParamSize, VertexParamSize>(DVertexParamOffset,
                                                           DVertexParamOffset)
            .noalias() += weight * JParam.transpose() * JParam;
        mp.template segment<VertexParamSize>(DVertexParamOffset -
                                             DParamOffset) +=
            weight * JParam.transpose() * Error;

        break;
      }

      case CamParamIndex: {
        const auto &JParam =
            Eigen::Map<const Matrix<4, 4>>(lin->jacobians[3][0].data());
        const auto &Error = Eigen::Map<const Vector<4>>(eval->error.data());

        M.template block<4, 4>(DCamParamOffset, DCamParamOffset).noalias() +=
            weight * JParam.transpose() * JParam;
        mp.template segment<4>(DCamParamOffset - DParamOffset) +=
            weight * JParam.transpose() * Error;

        break;
      }

      case FaceParamIndex: {
        // TODO: add face parameters
        break;
      }
    }
  }

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::updateJointPinholeCameraFactorGaussNewton(
    int pose) const {
  static const int index = FactorIndex::JointPinholeCamera;

  assert(pose >= 0 && pose < NumPoses);

  if (pose < 0 || pose >= NumPoses) {
    LOG(ERROR) << "The pose must be valid." << std::endl;

    exit(-1);
  }

  const auto &factors = mnvFactors[pose][index];

  if (factors.empty()) {
    return 0;
  }

  const auto &evals = mnvFactorEvals[0][pose][index];
  const auto &lins = mnvFactorLins[pose][index];

  const auto &weight = mOptions.weights[index];

  auto &M = mvM[pose];
  auto &mx = mvmx[pose];

  for (int i = 0; i < factors.size(); i++) {
    const auto &factor = factors[i];

    assert(pose == factor->getPoses()[0]);

    const auto &lin = std::dynamic_pointer_cast<
        const JointPinholeCameraFactor::Linearization>(lins[i]);

    const auto &JPose =
        Eigen::Map<const Matrix<2, 6>>(lin->jacobians[0][0].data());

    const auto &gPose = lin->gPose;

    M.template topLeftCorner<DPoseSize, DPoseSize>().noalias() +=
        weight * JPose.transpose() * JPose;
    mx += weight * gPose;
  }

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::updateFullJointPinholeCameraFactorGaussNewton(
    int pose) const {
  static const int index = FactorIndex::FullJointPinholeCamera;

  assert(pose >= 0 && pose < NumPoses);

  if (pose < 0 || pose >= NumPoses) {
    LOG(ERROR) << "The pose must be valid." << std::endl;

    exit(-1);
  }

  const auto &factors = mnvFactors[pose][index];

  if (factors.empty()) {
    return 0;
  }

  const auto &evals = mnvFactorEvals[0][pose][index];
  const auto &lins = mnvFactorLins[pose][index];

  const auto &weight = mOptions.weights[index];

  auto &M = mvM[pose];
  auto &mx = mvmx[pose];
  auto &mp = mvmp[pose];

  for (int i = 0; i < factors.size(); i++) {
    const auto &factor = factors[i];

    assert(pose == factor->getPoses()[0]);

    const auto &lin = std::dynamic_pointer_cast<
        const FullJointPinholeCameraFactor::Linearization>(lins[i]);

    const auto &JPose =
        Eigen::Map<const Matrix<2, 6>>(lin->jacobians[0][0].data());
    const auto &JCam =
        Eigen::Map<const Matrix<2, 4>>(lin->jacobians[3][0].data());

    const auto &gPose = lin->gPose;
    const auto &gCam = lin->gCam;

    M.template topLeftCorner<DPoseSize, DPoseSize>().noalias() +=
        weight * JPose.transpose() * JPose;
    M.template block<4, DPoseSize>(DCamParamOffset, DPoseOffset).noalias() +=
        weight * JCam.transpose() * JPose;
    M.template block<4, 4>(DCamParamOffset, DCamParamOffset) =
        weight * JCam.transpose() * JCam;

    mx += weight * gPose;
    mp.template segment<4>(DCamParamOffset) += weight * gCam;
  }

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::updateVertexPinholeCameraFactorGaussNewton(
    int pose) const {
  static const int index = FactorIndex::VertexPinholeCamera;

  assert(pose >= 0 && pose < NumPoses);

  if (pose < 0 || pose >= NumPoses) {
    LOG(ERROR) << "The pose must be valid." << std::endl;

    exit(-1);
  }

  const auto &factors = mnvFactors[pose][index];

  if (factors.empty()) {
    return 0;
  }

  const auto &evals = mnvFactorEvals[0][pose][index];
  const auto &lins = mnvFactorLins[pose][index];

  const auto &weight = mOptions.weights[index];

  auto &M = mvM[pose];
  auto &mx = mvmx[pose];
  auto &mp = mvmp[pose];

  for (int i = 0; i < factors.size(); i++) {
    const auto &factor = factors[i];

    assert(pose == factor->getPoses()[0]);

    const auto &lin = std::dynamic_pointer_cast<
        const VertexPinholeCameraFactor::Linearization>(lins[i]);

    const auto &JPose =
        Eigen::Map<const Matrix<2, 6>>(lin->jacobians[0][0].data());
    const auto &JVertex =
        Eigen::Map<const Matrix<2, P>>(lin->jacobians[3][0].data());

    const auto &gPose = lin->gPose;
    const auto &gVertex = Eigen::Map<const Vector<P>>(lin->gVertex.data());

    M.template topLeftCorner<DPoseSize, DPoseSize>().noalias() +=
        weight * JPose.transpose() * JPose;

    M.template block<VertexParamSize, DPoseSize>(DVertexParamOffset,
                                                 DPoseOffset)
        .noalias() += weight * JVertex.transpose() * JPose;

    M.template block<VertexParamSize, VertexParamSize>(DVertexParamOffset,
                                                       DVertexParamOffset)
        .noalias() += weight * JVertex.transpose() * JVertex;

    mx += weight * gPose;
    mp.template segment<VertexParamSize>(DVertexParamOffset - DParamOffset) +=
        weight * gVertex;
  }

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::updateFullVertexPinholeCameraFactorGaussNewton(
    int pose) const {
  static const int index = FactorIndex::FullVertexPinholeCamera;

  assert(pose >= 0 && pose < NumPoses);

  if (pose < 0 || pose >= NumPoses) {
    LOG(ERROR) << "The pose must be valid." << std::endl;

    exit(-1);
  }

  const auto &factors = mnvFactors[pose][index];

  if (factors.empty()) {
    return 0;
  }

  const auto &evals = mnvFactorEvals[0][pose][index];
  const auto &lins = mnvFactorLins[pose][index];

  const auto &weight = mOptions.weights[index];

  auto &M = mvM[pose];
  auto &mx = mvmx[pose];
  auto &mp = mvmp[pose];

  for (int i = 0; i < factors.size(); i++) {
    const auto &factor = factors[i];

    assert(pose == factor->getPoses()[0]);

    const auto &lin = std::dynamic_pointer_cast<
        const FullVertexPinholeCameraFactor::Linearization>(lins[i]);

    const auto &JPose =
        Eigen::Map<const Matrix<2, 6>>(lin->jacobians[0][0].data());
    const auto &JVertex =
        Eigen::Map<const Matrix<2, P>>(lin->jacobians[3][0].data());
    const auto &JCam =
        Eigen::Map<const Matrix<2, 4>>(lin->jacobians[3][1].data());

    const auto &gPose = lin->gPose;
    const auto &gVertex = Eigen::Map<const Vector<P>>(lin->gVertex.data());
    const auto &gCam = lin->gCam;

    M.template topLeftCorner<DPoseSize, DPoseSize>().noalias() +=
        weight * JPose.transpose() * JPose;
    M.template block<VertexParamSize, DPoseSize>(DVertexParamOffset,
                                                 DPoseOffset)
        .noalias() += weight * JVertex.transpose() * JPose;
    M.template block<4, DPoseSize>(DCamParamOffset, DPoseOffset).noalias() +=
        weight * JCam.transpose() * JPose;
    M.template block<VertexParamSize, VertexParamSize>(DVertexParamOffset,
                                                       DVertexParamOffset) =
        weight * JVertex.transpose() * JVertex;
    M.template block<VertexParamSize, 4>(DVertexParamOffset, DCamParamOffset) =
        weight * JVertex.transpose() * JCam;
    M.template block<4, VertexParamSize>(DCamParamOffset, DVertexParamOffset) =
        M.template block<VertexParamSize, 4>(DVertexParamOffset,
                                             DCamParamOffset)
            .transpose();
    M.template block<4, 4>(DCamParamOffset, DCamParamOffset) =
        weight * JCam.transpose() * JCam;

    mx += weight * gPose;
    mp.template segment<VertexParamSize>(DVertexParamOffset - DParamOffset) +=
        weight * gVertex;
    mp.template segment<4>(DCamParamOffset - DParamOffset) += weight * gCam;
  }

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::updateUnitPOFFactorGaussNewton(int pose) const {
  static const int index = FactorIndex::UnitPOF;

  assert(pose >= 0 && pose < NumPoses);

  if (pose < 0 || pose >= NumPoses) {
    LOG(ERROR) << "The pose must be valid." << std::endl;

    exit(-1);
  }

  const auto &factors = mnvFactors[pose][index];

  if (factors.empty()) {
    return 0;
  }

  const auto &evals = mnvFactorEvals[0][pose][index];
  const auto &lins = mnvFactorLins[pose][index];

  const auto &weight = mOptions.weights[index];

  auto &M = mvM[pose];
  auto &mx = mvmx[pose];

  for (int i = 0; i < factors.size(); i++) {
    const auto &factor = factors[i];

    assert(pose == factor->getPoses()[0]);

    const auto &lin =
        std::dynamic_pointer_cast<const UnitPOFFactor::Linearization>(lins[i]);

    const auto &JR =
        Eigen::Map<const Matrix<3, 3>>(lin->jacobians[0][0].data());

    const auto &gR = Eigen::Map<const Vector3>(lin->gPose.data());

    M.template topLeftCorner<3, 3>().noalias() += weight * JR.transpose() * JR;

    mx.template head<3>() += weight * gR;
  }

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::updateScaledPOFFactorGaussNewton(
    int pose) const {
  static const int index = FactorIndex::ScaledPOF;

  assert(pose >= 0 && pose < NumPoses);

  if (pose < 0 || pose >= NumPoses) {
    LOG(ERROR) << "The pose must be valid." << std::endl;

    exit(-1);
  }

  const auto &factors = mnvFactors[pose][index];

  if (factors.empty()) {
    return 0;
  }

  const auto &evals = mnvFactorEvals[0][pose][index];
  const auto &lins = mnvFactorLins[pose][index];

  const auto &weight = mOptions.weights[index];

  auto &M = mvM[pose];
  auto &mx = mvmx[pose];
  auto &mp = mvmp[pose];

  for (int i = 0; i < factors.size(); i++) {
    const auto &factor = factors[i];

    assert(pose == factor->getPoses()[0]);

    const auto &lin =
        std::dynamic_pointer_cast<const ScaledPOFFactor::Linearization>(
            lins[i]);

    const auto &JR =
        Eigen::Map<const Matrix<3, 3>>(lin->jacobians[0][0].data());
    const auto &JVertex =
        Eigen::Map<const Matrix<3, P>>(lin->jacobians[3][0].data());

    const auto &gR = Eigen::Map<const Vector3>(lin->gPose.data());
    const auto &gVertex = Eigen::Map<const Vector<P>>(lin->gVertex.data());

    M.template topLeftCorner<3, 3>().noalias() += weight * JR.transpose() * JR;

    M.template block<VertexParamSize, 3>(DVertexParamOffset, DPoseOffset)
        .noalias() += weight * JVertex.transpose() * JR;

    M.template block<VertexParamSize, VertexParamSize>(DVertexParamOffset,
                                                       DVertexParamOffset)
        .noalias() += weight * JVertex.transpose() * JVertex;

    mx.template head<3>() += weight * gR;

    mp.template segment<VertexParamSize>(DVertexParamOffset - DParamOffset) +=
        weight * gVertex;
  }

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::updateRelPOFFactorGaussNewton(int pose) const {
  static const int index = FactorIndex::RelPOF;

  assert(pose >= 0 && pose < NumPoses);

  if (pose < 0 || pose >= NumPoses) {
    LOG(ERROR) << "The pose must be valid." << std::endl;

    exit(-1);
  }

  const auto &factors = mnvFactors[pose][index];

  if (factors.empty()) {
    return 0;
  }

  assert(pose > 0 && pose < NumPoses);

  const auto &evals = mnvFactorEvals[0][pose][index];
  const auto &lins = mnvFactorLins[pose][index];

  const auto &weight = mOptions.weights[index];

  auto &M = mvM[pose];
  auto &mx = mvmx[pose];
  auto &mu = mvmu[pose];
  auto &mp = mvmp[pose];

  for (int i = 0; i < factors.size(); i++) {
    const auto &factor = factors[i];

    assert(pose == factor->getPoses()[0]);

    const auto &lin =
        std::dynamic_pointer_cast<const RelPOFFactor::Linearization>(lins[i]);

    const auto &JR =
        Eigen::Map<const Matrix<3, 3>>(lin->jacobians[0][0].data());
    const auto &JVertex =
        Eigen::Map<const Matrix<3, P>>(lin->jacobians[3][0].data());
    const auto &JJoint =
        Eigen::Map<const Matrix<3, 3>>(lin->jacobians[2][0].data());

    const auto &gR = Eigen::Map<const Vector3>(lin->gPose.data());
    const auto &gVertex = Eigen::Map<const Vector<P>>(lin->gVertex.data());
    const auto &gJoint = lin->gJoint;

    M.template topLeftCorner<3, 3>().noalias() += weight * JR.transpose() * JR;

    M.template block<VertexParamSize, 3>(DVertexParamOffset, DPoseOffset)
        .noalias() += weight * JVertex.transpose() * JR;

    M.template block<DJointSize, 3>(DJointOffset, DPoseOffset).noalias() +=
        weight * JJoint.transpose() * JR;

    M.template block<VertexParamSize, VertexParamSize>(DVertexParamOffset,
                                                       DVertexParamOffset)
        .noalias() += weight * JVertex.transpose() * JVertex;

    M.template block<DJointSize, VertexParamSize>(DJointOffset,
                                                  DVertexParamOffset)
        .noalias() += weight * JJoint.transpose() * JVertex;

    M.template block<DJointSize, DJointSize>(DJointOffset, DJointOffset)
        .noalias() += weight * JJoint.transpose() * JJoint;

    mx.template head<3>() += weight * gR;

    mp.template segment<VertexParamSize>(DVertexParamOffset - DParamOffset) +=
        weight * gVertex;

    mu += weight * gJoint;
  }

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::updateJointDepthCameraFactorGaussNewton(
    int pose) const {
  static const int index = FactorIndex::JointDepthCamera;

  assert(pose >= 0 && pose < NumPoses);

  if (pose < 0 || pose >= NumPoses) {
    LOG(ERROR) << "The pose must be valid." << std::endl;

    exit(-1);
  }

  const auto &factors = mnvFactors[pose][index];

  if (factors.empty()) {
    return 0;
  }

  const auto &evals = mnvFactorEvals[0][pose][index];
  const auto &lins = mnvFactorLins[pose][index];

  const auto &weight = mOptions.weights[index];

  auto &M = mvM[pose];
  auto &mx = mvmx[pose];

  for (int i = 0; i < factors.size(); i++) {
    const auto &factor = factors[i];

    assert(pose == factor->getPoses()[0]);

    const auto &lin =
        std::dynamic_pointer_cast<const JointDepthCameraFactor::Linearization>(
            lins[i]);

    const auto &JPose =
        Eigen::Map<const Matrix<3, 6>>(lin->jacobians[0][0].data());

    const auto &gPose = lin->gPose;

    M.template topLeftCorner<DPoseSize, DPoseSize>().noalias() +=
        weight * JPose.transpose() * JPose;
    mx += weight * gPose;
  }

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::updateVertexDepthCameraFactorGaussNewton(
    int pose) const {
  static const int index = FactorIndex::VertexDepthCamera;

  assert(pose >= 0 && pose < NumPoses);

  if (pose < 0 || pose >= NumPoses) {
    LOG(ERROR) << "The pose must be valid." << std::endl;

    exit(-1);
  }

  const auto &factors = mnvFactors[pose][index];

  if (factors.empty()) {
    return 0;
  }

  const auto &evals = mnvFactorEvals[0][pose][index];
  const auto &lins = mnvFactorLins[pose][index];

  const auto &weight = mOptions.weights[index];

  auto &M = mvM[pose];
  auto &mx = mvmx[pose];
  auto &mp = mvmp[pose];

  for (int i = 0; i < factors.size(); i++) {
    const auto &factor = factors[i];

    assert(pose == factor->getPoses()[0]);

    const auto &lin =
        std::dynamic_pointer_cast<const VertexDepthCameraFactor::Linearization>(
            lins[i]);

    const auto &JPose =
        Eigen::Map<const Matrix<3, 6>>(lin->jacobians[0][0].data());
    const auto &JVertex =
        Eigen::Map<const Matrix<3, P>>(lin->jacobians[3][0].data());

    const auto &gPose = lin->gPose;
    const auto &gVertex = Eigen::Map<const Vector<P>>(lin->gVertex.data());

    M.template topLeftCorner<DPoseSize, DPoseSize>().noalias() +=
        weight * JPose.transpose() * JPose;

    M.template block<VertexParamSize, DPoseSize>(DVertexParamOffset,
                                                 DPoseOffset)
        .noalias() += weight * JVertex.transpose() * JPose;

    M.template block<VertexParamSize, VertexParamSize>(DVertexParamOffset,
                                                       DVertexParamOffset)
        .noalias() += weight * JVertex.transpose() * JVertex;

    mx += weight * gPose;
    mp.template segment<VertexParamSize>(DVertexParamOffset - DParamOffset) +=
        weight * gVertex;
  }

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::updateJointLimitFactorGaussNewton(
    int pose) const {
  static const int index = FactorIndex::JointLimit;

  assert(pose >= 0 && pose < NumPoses);

  if (pose < 0 || pose >= NumPoses) {
    LOG(ERROR) << "The pose must be valid." << std::endl;

    exit(-1);
  }

  const auto &factors = mnvFactors[pose][index];

  if (factors.empty()) {
    return 0;
  }

  const auto &evals = mnvFactorEvals[0][pose][index];
  const auto &lins = mnvFactorLins[pose][index];

  const auto &weight = mOptions.weights[index];

  auto &M = mvM[pose];
  auto &mu = mvmu[pose];

  for (int i = 0; i < factors.size(); i++) {
    const auto &factor = factors[i];

    assert(pose == factor->getJoints()[0] + 1);

    const auto &lin = lins[i];
    const auto &eval = evals[i];

    const auto &JJoint =
        Eigen::Map<const Matrix<6, DJointSize>>(lin->jacobians[2][0].data());
    const auto &Error = eval->error;

    M.template bottomRightCorner<DJointSize, DJointSize>().noalias() +=
        weight * JJoint.transpose() * JJoint;
    mu.noalias() += weight * JJoint.transpose() * Error;
  }

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::updateCollisionFactorGaussNewton(
    int pose) const {
  // TODO: add collision factors
  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::updatePoseConstFactorGaussNewton(
    int pose) const {
  static const int index = FactorIndex::PoseConst;

  assert(pose >= 0 && pose < NumPoses);

  if (pose < 0 || pose >= NumPoses) {
    LOG(ERROR) << "The pose must be valid." << std::endl;

    exit(-1);
  }

  const auto &factors = mnvFactors[pose][index];

  if (factors.empty()) {
    return 0;
  }

  const auto &evals = mnvFactorEvals[0][pose][index];
  const auto &lins = mnvFactorLins[pose][index];

  const auto &weight = mOptions.weights[index];

  auto &M = mvM[pose];
  auto &mx = mvmx[pose];

  for (int i = 0; i < factors.size(); i++) {
    const auto &factor = factors[i];

    assert(pose == factor->getPoses()[0]);

    const auto &lin = lins[i];
    const auto &eval = evals[i];

    const auto &JPose = Eigen::Map<const Matrix6>(lin->jacobians[0][0].data());
    const auto &Error = Eigen::Map<const Vector6>(eval->error.data());

    M.template topLeftCorner<DPoseSize, DPoseSize>().noalias() +=
        weight * JPose.transpose() * JPose;
    mx.noalias() += weight * JPose.transpose() * Error;
  }

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::updateJointConstFactorGaussNewton(
    int pose) const {
  static const int index = FactorIndex::JointConst;

  assert(pose >= 0 && pose < NumPoses);

  if (pose < 0 || pose >= NumPoses) {
    LOG(ERROR) << "The pose must be valid." << std::endl;

    exit(-1);
  }

  const auto &factors = mnvFactors[pose][index];

  if (factors.empty()) {
    return 0;
  }

  const auto &evals = mnvFactorEvals[0][pose][index];
  const auto &lins = mnvFactorLins[pose][index];

  const auto &weight = mOptions.weights[index];

  auto &M = mvM[pose];
  auto &mu = mvmu[pose];

  for (int i = 0; i < factors.size(); i++) {
    const auto &factor = factors[i];

    assert(pose == factor->getJoints()[0] + 1);

    const auto &lin = lins[i];
    const auto &eval = evals[i];

    const auto &JJoint = Eigen::Map<const Matrix3>(lin->jacobians[2][0].data());
    const auto &Error = Eigen::Map<const Vector3>(eval->error.data());

    M.template bottomRightCorner<DJointSize, DJointSize>().noalias() +=
        weight * JJoint.transpose() * JJoint;
    mu.noalias() += weight * JJoint.transpose() * Error;
  }

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::updatePoseFactorGaussNewton(int pose) const {
  static const int index = FactorIndex::Pose;

  assert(pose >= 0 && pose < NumPoses);

  if (pose < 0 || pose >= NumPoses) {
    LOG(ERROR) << "The pose must be valid." << std::endl;

    exit(-1);
  }

  const auto &factors = mnvFactors[pose][index];

  if (factors.empty()) {
    return 0;
  }

  const auto &evals = mnvFactorEvals[0][pose][index];
  const auto &lins = mnvFactorLins[pose][index];

  const auto &weight = mOptions.weights[index];

  auto &M = mvM[pose];
  auto &mx = mvmx[pose];

  for (int i = 0; i < factors.size(); i++) {
    const auto &factor = factors[i];

    assert(pose == factor->getPoses()[0]);

    const auto &lin = lins[i];
    const auto &eval = evals[i];

    const auto &JPose = Eigen::Map<const Matrix6>(lin->jacobians[0][0].data());
    const auto &Error = Eigen::Map<const Vector6>(eval->error.data());

    M.template topLeftCorner<DPoseSize, DPoseSize>().noalias() +=
        weight * JPose.transpose() * JPose;
    mx.noalias() += weight * JPose.transpose() * Error;
  }

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::updateShapeFactorGaussNewton(int pose) const {
  // TODO: add shape factors

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::updateJointFactorGaussNewton(int pose) const {
  static const int index = FactorIndex::Joint;

  assert(pose >= 0 && pose < NumPoses);

  if (pose < 0 || pose >= NumPoses) {
    LOG(ERROR) << "The pose must be valid." << std::endl;

    exit(-1);
  }

  const auto &factors = mnvFactors[pose][index];

  if (factors.empty()) {
    return 0;
  }

  const auto &evals = mnvFactorEvals[0][pose][index];
  const auto &lins = mnvFactorLins[pose][index];

  const auto &weight = mOptions.weights[index];

  auto &M = mvM[pose];
  auto &mu = mvmu[pose];

  for (int i = 0; i < factors.size(); i++) {
    const auto &factor = factors[i];

    assert(pose == factor->getJoints()[0] + 1);

    const auto &lin = lins[i];
    const auto &eval = evals[i];

    const auto &JJoint = Eigen::Map<const Matrix3>(lin->jacobians[2][0].data());
    const auto &Error = Eigen::Map<const Vector3>(eval->error.data());

    M.template bottomRightCorner<DJointSize, DJointSize>().noalias() +=
        weight * JJoint.transpose() * JJoint;
    mu.noalias() += weight * JJoint.transpose() * Error;
  }

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::updateParamFactorGaussNewton(int pose) const {
  static const int index = FactorIndex::Parameter;

  assert(pose >= 0 && pose < NumPoses);

  if (pose < 0 || pose >= NumPoses) {
    LOG(ERROR) << "The pose must be valid." << std::endl;

    exit(-1);
  }

  const auto &factors = mnvFactors[pose][index];

  if (factors.empty()) {
    return 0;
  }

  assert(pose == 0);

  const auto &evals = mnvFactorEvals[0][pose][index];
  const auto &lins = mnvFactorLins[pose][index];

  const auto &weight = mOptions.weights[index];

  auto &M = mvM[pose];
  auto &mp = mvmp[pose];

  for (int i = 0; i < factors.size(); i++) {
    const auto &factor = factors[i];

    const auto &lin = lins[i];
    const auto &eval = evals[i];

    const auto &param = factor->getParams()[0];

    switch (param) {
      case VertexParamIndex: {
        const auto &JParam =
            Eigen::Map<const Matrix<P, P>>(lin->jacobians[3][0].data());
        const auto &Error = Eigen::Map<const Vector<P>>(eval->error.data());

        M.template block<VertexParamSize, VertexParamSize>(DVertexParamOffset,
                                                           DVertexParamOffset)
            .noalias() += weight * JParam.transpose() * JParam;
        mp.template segment<VertexParamSize>(DVertexParamOffset -
                                             DParamOffset) +=
            weight * JParam.transpose() * Error;

        break;
      }

      case CamParamIndex: {
        const auto &JParam =
            Eigen::Map<const Matrix<4, 4>>(lin->jacobians[3][0].data());
        const auto &Error = Eigen::Map<const Vector<4>>(eval->error.data());

        M.template block<4, 4>(DCamParamOffset, DCamParamOffset).noalias() +=
            weight * JParam.transpose() * JParam;
        mp.template segment<4>(DCamParamOffset - DParamOffset) +=
            weight * JParam.transpose() * Error;

        break;
      }

      case FaceParamIndex: {
        // TODO: add face parameters
        break;
      }
    }
  }

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::computeGradient() const {
  for (int i = NumPoses - 1; i > 0; i--) {
    backwardDG(i);
  }

  const auto &model = mModel;

  const auto &links = model->getLinks();
  const auto &root = links[0];

  mvPoseDG[0] = mvmx[0];
  mParamDG = mvmp[0];

  for (const auto &j : root.children()) {
    const auto &jj = links[j].joint();

    mvPoseDG[0] += mvPoseDG[j];
    mParamDG += mvParamDG[jj];
  }

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::solveGaussNewton() const {
  for (int i = NumPoses - 1; i > 0; i--) {
    factorize(i);
    backward(i);
  }

  factorizeN();
  backwardN();

  for (int i = 1; i < NumPoses; i++) {
    forward(i);
  }

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::updateDCost() const {
  mDCost.setZero();

  for (int i = 1, ii = 0; i < NumPoses; i++, ii++) {
    mvDCostDx[i].noalias() =
        0.5 * mvM[i].template topLeftCorner<DPoseSize, DPoseSize>() *
        mvPoseGN[i];
    mDCost[0] += mvPoseGN[i].dot(mvDCostDx[i]);

    mvDCostDp[i].noalias() = mvM[i].template block<ParamSize, DPoseSize>(
                                 DCamParamOffset, DPoseOffset) *
                             mvPoseGN[i];
    mvDCostDp[i].noalias() +=
        0.5 *
        mvM[i].template block<ParamSize, ParamSize>(
            DCamParamOffset, DCamParamOffset, ParamSize, ParamSize) *
        mParamGN.template head<ParamSize>();
    mDCost[0] += mParamGN.template head<ParamSize>().dot(mvDCostDp[i]);

    mvDCostDu[i].noalias() = mvM[i].template block<DJointSize, DPoseSize>(
                                 DJointOffset, DPoseOffset) *
                             mvPoseGN[i];
    mvDCostDu[i].noalias() += mvM[i].template block<DJointSize, ParamSize>(
                                  DJointOffset, DCamParamOffset) *
                              mParamGN.template head<ParamSize>();
    mvDCostDu[i].noalias() +=
        0.5 * mvM[i].template bottomRightCorner<DJointSize, DJointSize>() *
        mvJointGN[ii];
    mDCost[0] += mvJointGN[ii].dot(mvDCostDu[i]);

    mDCost[1] += mvPoseGN[i].dot(mvmx[i]);
    mDCost[1] += mvJointGN[ii].dot(mvmu[i]);
    mDCost[1] += mParamGN.template head<ParamSize>().dot(
        mvmp[i].template head<ParamSize>());
  }

  mvDCostDx[0].noalias() =
      0.5 * mvM[0].template topLeftCorner<DPoseSize, DPoseSize>() * mvPoseGN[0];
  mDCost[0] += mvPoseGN[0].dot(mvDCostDx[0]);

  mvDCostDp[0].noalias() = mvM[0].template block<ParamSize, DPoseSize>(
                               DCamParamOffset, DPoseOffset) *
                           mvPoseGN[0];
  mvDCostDp[0].noalias() += 0.5 *
                            mvM[0].template block<ParamSize, ParamSize>(
                                DCamParamOffset, DCamParamOffset) *
                            mParamGN.template head<ParamSize>();
  mDCost[0] += mParamGN.template head<ParamSize>().dot(mvDCostDp[0]);

  mDCost[1] += mvPoseGN[0].dot(mvmx[0]);
  mDCost[1] += mParamGN.template head<ParamSize>().dot(
      mvmp[0].template head<ParamSize>());

  if (mOptions.check_collisions) {
    // TODO: handle collisions
  }

  return 0;
}

template <int K, int P, int N, bool CamOpt>
inline int Optimizer<K, P, N, CamOpt>::factorize(int i) const {
  assert(i < NumPoses && i > 0);

  const auto &model = mModel;

  const auto &links = model->getLinks();

  const auto &link = links[i];

  assert(i == link.id());

  const int ii = link.joint();

  const auto &children = link.children();

  mvH[ii] = mvM[i];

  for (const auto &j : children) {
    const auto &jj = links[j].joint();

    mvH[ii]
        .template topLeftCorner<DPoseSize + DParamSize,
                                DPoseSize + DParamSize>() +=
        mvH[jj]
            .template topLeftCorner<DPoseSize + DParamSize,
                                    DPoseSize + DParamSize>();
  }

  mvHxB[ii].noalias() = mvH[ii].template leftCols<6>() * mvB[ii];

  // update Hpx and Hpp
  mvH[ii].template block<VertexParamSize, DPoseSize + DParamSize>(
      DVertexParamOffset, DPoseOffset) +=
      mvHxB[ii]
          .template block<DPoseSize + DParamSize, VertexParamSize>(DPoseOffset,
                                                                   0)
          .transpose();

  // update Hux, Hup, Huu
  mvH[ii].template bottomRows<DJointSize>() +=
      mvHxB[ii].template rightCols<DJointSize>().transpose();

  // update Hpp and Hup
  mvH[ii].template block<DParamSize + DJointSize, VertexParamSize>(
      DParamOffset, DVertexParamOffset) +=
      mvHxB[ii].template block<DParamSize + DJointSize, VertexParamSize>(
          DParamOffset, 0);

  // update Hpp and Hup
  mvH[ii]
      .template block<VertexParamSize + 3, VertexParamSize>(DVertexParamOffset,
                                                            DVertexParamOffset)
      .noalias() += mvHxB[ii].template middleRows<3>(3).transpose() * mvBp[ii];

  // update Huu
  mvH[ii].template block<DJointSize, DJointSize>(DPoseSize + DParamSize,
                                                 DPoseSize + DParamSize) +=
      mvHxB[ii].template block<DJointSize, DJointSize>(DJointOffset,
                                                       VertexParamSize);
  mvH[ii]
      .template block<DJointSize, DJointSize>(DJointOffset, DJointOffset)
      .noalias() +=
      mvBu[ii].transpose() * mvHxB[ii].template block<DPoseSize, DJointSize>(
                                 DPoseOffset, VertexParamSize);

  if (mOptions.method == Method::LM) {
    mvH[ii].diagonal().template segment<DJointSize>(DJointOffset).array() *=
        mDLambda;
    mLambda.template segment<DJointSize>(6 + DParamSize + 3 * ii) =
        mvH[ii].diagonal().template segment<DJointSize>(DJointOffset);
  }

  mvH[ii].diagonal().template segment<DJointSize>(DJointOffset).array() +=
      mOptions.delta;

  mvHuuInv[ii] =
      mvH[ii].template bottomRightCorner<DJointSize, DJointSize>().inverse();

  mvKuxp[ii].template leftCols<DPoseSize + DParamSize>().noalias() =
      -mvHuuInv[ii] *
      mvH[ii].template bottomLeftCorner<DJointSize, DPoseSize + DParamSize>();

  // update Hxx
  mvH[ii]
      .template topLeftCorner<DPoseSize + DParamSize, DPoseSize + DParamSize>()
      .noalias() +=
      mvH[ii]
          .template bottomLeftCorner<DJointSize, DPoseSize + DParamSize>()
          .transpose() *
      mvKuxp[ii];

  return 0;
}

template <int K, int P, int N, bool CamOpt>
inline int Optimizer<K, P, N, CamOpt>::backward(int i) const {
  assert(i < NumPoses && i > 0);

  const auto &model = mModel;

  const auto &links = model->getLinks();

  const auto &link = links[i];

  assert(i == link.id());

  const auto &ii = link.joint();
  const auto &children = link.children();

  mvhx[ii] = mvmx[i];
  mvhp[ii] = mvmp[i];
  mvhu[ii] = mvmu[i];
  mvE[ii] = 0;

  for (const auto &j : children) {
    const auto &jj = links[j].joint();

    mvhx[ii] += mvhx[jj];
    mvhp[ii] += mvhp[jj];
    mvE[ii] += mvE[jj];
  }

  mvhp[ii].template segment<VertexParamSize>(VertexParamOffset).noalias() +=
      mvBp[ii].transpose() * mvhx[ii].template bottomRows<3>();
  mvhu[ii].noalias() += mvBu[ii].transpose() * mvhx[ii];

  mvku[ii].noalias() = -mvHuuInv[ii] * mvhu[ii];

  mvE[ii] += 0.5 * mvhu[ii].dot(mvku[ii]);

  mvhx[ii].noalias() +=
      mvH[ii]
          .template block<DJointSize, DPoseSize>(DJointOffset, DPoseOffset)
          .transpose() *
      mvku[ii];
  mvhp[ii].noalias() +=
      mvH[ii]
          .template block<DJointSize, DParamSize>(DJointOffset, DParamOffset)
          .transpose() *
      mvku[ii];

  return 0;
}

template <int K, int P, int N, bool CamOpt>
inline int Optimizer<K, P, N, CamOpt>::forward(int i) const {
  assert(i < NumPoses && i > 0);

  const auto &model = mModel;

  const auto &link = model->getLinks()[i];

  const auto &parent = link.parent();
  const auto &ii = link.joint();

  mvJointGN[ii] = mvku[ii];

  mvJointGN[ii].noalias() +=
      mvKuxp[ii].template leftCols<6>() * mvPoseGN[parent];

  mvPoseGN[i] = mvPoseGN[parent];
  mvPoseGN[i].noalias() += mvBu[ii] * mvJointGN[ii];
  mvPoseGN[i].template tail<3>().noalias() +=
      mvBp[ii] * mParamGN.template segment<VertexParamSize>(DVertexParamOffset -
                                                            DParamOffset);

  return 0;
}

template <int K, int P, int N, bool CamOpt>
inline int Optimizer<K, P, N, CamOpt>::factorizeN() const {
  const auto &model = mModel;

  const auto &links = model->getLinks();
  const auto &root = links[0];

  mH0 = mvM[0].template topLeftCorner<DJointOffset, DJointOffset>();

  for (const auto &j : root.children()) {
    const auto &jj = links[j].joint();

    mH0 += mvH[jj]
               .template topLeftCorner<DPoseSize + DParamSize,
                                       DPoseSize + DParamSize>();
  }

  mH0.template block<DPoseSize, DParamSize>(DPoseOffset, DParamOffset) =
      mH0.template block<DParamSize, DPoseSize>(DParamOffset, DPoseOffset)
          .transpose();

  if (mOptions.method == Method::LM) {
    mH0.diagonal().array() *= mDLambda;
    mLambda.template head<6 + DParamSize>() = mH0.diagonal();
  }

  mH0.diagonal().array() += mOptions.delta;

  if (mOptions.extra_hip_parameter) {
    assert(mvParams[0].size() == 1);

    mHchol0.compute(
        mRawH0.template topLeftCorner<DPoseSize + DParamSize - 1,
                                      DPoseSize + DParamSize - 1>());
  } else {
    mHchol0.compute(mRawH0);
  }

  return 0;
}

template <int K, int P, int N, bool CamOpt>
inline int Optimizer<K, P, N, CamOpt>::backwardN() const {
  const auto &model = mModel;

  const auto &links = model->getLinks();
  const auto &root = links[0];

  mh0.template head<DPoseSize>() = mvmx[0];
  mh0.template tail<DParamSize>() = mvmp[0];
  mE = 0;

  for (const auto &j : root.children()) {
    const auto &jj = links[j].joint();

    mh0.template head<DPoseSize>() += mvhx[jj];
    mh0.template tail<DParamSize>() += mvhp[jj];
    mE += mvE[jj];
  }

  if (mOptions.extra_hip_parameter) {
    assert(mvParams[0].size() == 1);

    mhGN.template head<DPoseSize + DParamSize - 1>().noalias() =
        -mHchol0.solve(mh0.template head<DPoseSize + DParamSize - 1>());

    mE += 0.5 * mhGN.template head<DPoseSize + DParamSize - 1>().dot(
                    mh0.template head<DPoseSize + DParamSize - 1>());

    Scalar LowerBnd =
        mOptions.extra_hip_parameter_bnd[0] - mvParams[0][0][DParamSize - 1];
    Scalar UpperBnd =
        mOptions.extra_hip_parameter_bnd[1] - mvParams[0][0][DParamSize - 1];

    Scalar H, b;

    mS.noalias() = mHchol0.solve(
        mH0.template topRightCorner<DPoseSize + DParamSize - 1, 1>());

    H = mH0(DPoseSize + DParamSize - 1, DPoseSize + DParamSize - 1) -
        mS.dot(mH0.template topRightCorner<DPoseSize + DParamSize - 1, 1>());
    b = mS.dot(mh0.template head<DPoseSize + DParamSize - 1>()) -
        mh0[DPoseSize + DParamSize - 1];

    mhGN[DPoseSize + DParamSize - 1] = b / H;
    mhGN[DPoseSize + DParamSize - 1] =
        std::max(mhGN[DPoseSize + DParamSize - 1], LowerBnd);
    mhGN[DPoseSize + DParamSize - 1] =
        std::min(mhGN[DPoseSize + DParamSize - 1], UpperBnd);

    mE += 0.5 * H * mhGN[DPoseSize + DParamSize - 1] *
              mhGN[DPoseSize + DParamSize - 1] -
          b * mhGN[DPoseSize + DParamSize - 1];

    mhGN.template head<DPoseSize + DParamSize - 1>() -=
        mhGN[DPoseSize + DParamSize - 1] * mS;
  } else {
    mhGN = -mHchol0.solve(mh0);

    mE += 0.5 * mh0.dot(mhGN);
  }

  mvPoseGN[0] = mhGN.template head<DPoseSize>();
  mParamGN = mhGN.template tail<DParamSize>();

  mku.noalias() += mKuxp.template rightCols<DParamSize>() * mParamGN;

  return 0;
}

template <int K, int P, int N, bool CamOpt>
inline int Optimizer<K, P, N, CamOpt>::backwardPass(
    const std::vector<int> &subtree) const {
  for (auto it = subtree.crbegin(); it != subtree.crend(); it++) {
    factorize(*it);
    backward(*it);
  }

  return 0;
}

template <int K, int P, int N, bool CamOpt>
inline int Optimizer<K, P, N, CamOpt>::forwardPass(
    const std::vector<int> &subtree) const {
  for (auto it = subtree.cbegin(); it != subtree.cend(); it++) {
    forward(*it);
  }

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::updateCostReduction() const {
  if (mOptions.method == Method::LM) {
    mLambda *= 0.5 - 0.5 / mDLambda;
    mLambda.array() += 0.5 * mOptions.delta;
    mSquaredError.template head<6 + DParamSize>() = mhGN.cwiseAbs2();
    mSquaredError.template tail<3 * K>() =
        Eigen::Map<const scope::Vector<3 * K>>(mJointGN.data()).cwiseAbs2();
    mExtraCost = mLambda.dot(mSquaredError);
  } else {
    mExtraCost =
        0.5 * mOptions.delta * (mhGN.squaredNorm() + mJointGN.squaredNorm());
  }

  mE -= mExtraCost;

  if (mOptions.check_collisions) {
    // TODO: handle collisions
  }

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::backwardDG(int i) const {
  assert(i < NumPoses && i > 0);

  const auto &model = mModel;

  const auto &links = model->getLinks();

  const auto &link = links[i];

  assert(i == link.id());

  const auto &ii = link.joint();
  const auto &children = link.children();

  mvPoseDG[i] = mvmx[i];
  mvJointDG[ii] = mvmu[i];
  mvParamDG[ii] = mvmp[i];

  for (const auto &j : children) {
    const auto &jj = links[j].joint();

    mvPoseDG[i] += mvPoseDG[j];
    mvParamDG[ii] += mvParamDG[jj];
  }

  mvParamDG[ii]
      .template segment<VertexParamSize>(DVertexParamOffset - DParamOffset)
      .noalias() += mvBp[ii].transpose() * mvPoseDG[i].template bottomRows<3>();
  mvJointDG[ii].noalias() += mvBu[ii].transpose() * mvPoseDG[i];

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::checkFactor(
    std::shared_ptr<const Factor> factor) {
  if (factor == nullptr) {
    LOG(WARNING) << "The factor should not be null." << std::endl;

    exit(-1);
  }

  return 0;
}

template <int K, int P, int N, bool CamOpt>
int Optimizer<K, P, N, CamOpt>::addFactor(
    int index, std::shared_ptr<Factor> factor,
    std::array<std::shared_ptr<Factor::Evaluation>, 2> &evals,
    std::shared_ptr<Factor::Linearization> &lin) {
  assert(index >= 0 && index < NumFactors);

  if (index < 0 || index >= NumFactors) {
    LOG(ERROR) << "The factor index must be valid." << std::endl;

    exit(-1);
  }

  if (checkFactor(factor)) {
    exit(-1);
  }

  evals[0] = factor->newEvaluation();
  evals[1] = factor->newEvaluation();
  lin = factor->newLinearization();

  mFactors.push_back(factor);
  mFactorEvals[0].push_back(evals[0]);
  mFactorEvals[1].push_back(evals[1]);
  mFactorLins.push_back(lin);

  mvFactors[index].push_back(factor);
  mvFactorEvals[0][index].push_back(evals[0]);
  mvFactorEvals[1][index].push_back(evals[1]);
  mvFactorLins[index].push_back(lin);

  if (mStatus != Status::Uninitialized) {
    auto &eval = *evals[0];

    factor->evaluate(mvPoses[0], mvShapes[0], mvJoints[0], mvParams[0], eval);

    if (eval.status != Factor::Status::VALID) {
      LOG(ERROR) << "Failed to evaluate the factor." << std::endl;

      mvFactors[index].pop_back();
      mvFactorEvals[0][index].pop_back();
      mvFactorEvals[1][index].pop_back();
      mvFactorLins[index].pop_back();

      exit(-1);
    }

    mvFobj[0][index] += eval.f;
    mfobj[0] += 0.5 * mOptions.weights[index] * eval.f;
  }

  factor->setID(mFactors.size() - 1);

  assert(factor.get() == mFactors.back().get());
  assert(factor->isActive() == true);

  return 0;
}

template <int K, int P, int N, bool CamOpt>
const int Optimizer<K, P, N, CamOpt>::NumPoses;

template <int K, int P, int N, bool CamOpt>
const int Optimizer<K, P, N, CamOpt>::NumShapes;

template <int K, int P, int N, bool CamOpt>
const int Optimizer<K, P, N, CamOpt>::NumJoints;

template <int K, int P, int N, bool CamOpt>
const int Optimizer<K, P, N, CamOpt>::NumParams;

template <int K, int P, int N, bool CamOpt>
const int Optimizer<K, P, N, CamOpt>::MaxNumCollisions;

template <int K, int P, int N, bool CamOpt>
const int Optimizer<K, P, N, CamOpt>::CamParamIndex;

template <int K, int P, int N, bool CamOpt>
const int Optimizer<K, P, N, CamOpt>::VertexParamIndex;

template <int K, int P, int N, bool CamOpt>
const int Optimizer<K, P, N, CamOpt>::FaceParamIndex;

template <int K, int P, int N, bool CamOpt>
const int Optimizer<K, P, N, CamOpt>::CamParamOffset;

template <int K, int P, int N, bool CamOpt>
const int Optimizer<K, P, N, CamOpt>::VertexParamOffset;

template <int K, int P, int N, bool CamOpt>
const int Optimizer<K, P, N, CamOpt>::FaceParamOffset;

template <int K, int P, int N, bool CamOpt>
const int Optimizer<K, P, N, CamOpt>::CamParamSize;

template <int K, int P, int N, bool CamOpt>
const int Optimizer<K, P, N, CamOpt>::VertexParamSize;

template <int K, int P, int N, bool CamOpt>
const int Optimizer<K, P, N, CamOpt>::FaceParamSize;

template <int K, int P, int N, bool CamOpt>
const int Optimizer<K, P, N, CamOpt>::ParamSize;

template <int K, int P, int N, bool CamOpt>
const int Optimizer<K, P, N, CamOpt>::Size;

template <int K, int P, int N, bool CamOpt>
const int Optimizer<K, P, N, CamOpt>::MaxDSize;

template <int K, int P, int N, bool CamOpt>
const int Optimizer<K, P, N, CamOpt>::DPoseSize;

template <int K, int P, int N, bool CamOpt>
const int Optimizer<K, P, N, CamOpt>::DJointSize;

template <int K, int P, int N, bool CamOpt>
const int Optimizer<K, P, N, CamOpt>::DParamSize;

template <int K, int P, int N, bool CamOpt>
const int Optimizer<K, P, N, CamOpt>::DSize;

template <int K, int P, int N, bool CamOpt>
const int Optimizer<K, P, N, CamOpt>::DPoseOffset;

template <int K, int P, int N, bool CamOpt>
const int Optimizer<K, P, N, CamOpt>::DParamOffset;

template <int K, int P, int N, bool CamOpt>
const int Optimizer<K, P, N, CamOpt>::DJointOffset;

template <int K, int P, int N, bool CamOpt>
const int Optimizer<K, P, N, CamOpt>::DCamParamOffset;

template <int K, int P, int N, bool CamOpt>
const int Optimizer<K, P, N, CamOpt>::DVertexParamOffset;

template <int K, int P, int N, bool CamOpt>
const int Optimizer<K, P, N, CamOpt>::DFaceParamOffset;

template <int K, int P, int N, bool CamOpt>
const std::vector<int> Optimizer<K, P, N, CamOpt>::DParamOffsets =
    CamOpt ? std::vector<int>{0, 4} : std::vector<int>{0};

template <int K, int P, int N, bool CamOpt>
const std::vector<int> Optimizer<K, P, N, CamOpt>::ParamSizes =
    CamOpt ? std::vector<int>{4, P} : std::vector<int>{P};

template class Optimizer<2, 2, 20, false>;
template class Optimizer<23, 10, 6890, false>;
template class Optimizer<51, 10, 6890, false>;
template class Optimizer<23, 11, 6890, false>;
template class Optimizer<51, 11, 6890, false>;
}  // namespace Optimizer
}  // namespace scope


<|start_filename|>C++/scope/math/operation.h<|end_filename|>
#pragma once

namespace scope {
namespace math {
enum class OPS { EQL, ADD, SUB };
}
}  // namespace scope


<|start_filename|>C++/scope/utils/Initialization.cpp<|end_filename|>
#include <scope/utils/Initialization.h>

#include <memory>

namespace scope {
namespace Initialization {
namespace SMPL {
int initialize(const MatrixX &ShapeSqrtCov, Scalar FocalLength,
               // KeyPoint2D setup
               const Matrix3X &Measurements2D, Scalar KeyPoint2DGMSigma,
               Scalar KeyPoint2DGMEps,
               const VectorX &KeyPoint2DConfidenceThreshold,
               // POF setup
               const Matrix3X &POFMeasurements, Scalar POFGMSigma,
               Scalar POFGMEps,
               // KeyPoint3D setup
               const Matrix3X &Measurements3D, Scalar KeyPoint3DGMSigma,
               Scalar KeyPoint3DGMEps,
               // joint limit
               const AlignedVector<AlignedVector<Matrix6>> &JointLimitWeight,
               const AlignedVector<AlignedVector<Vector6>> &JointLimitBias,
               const AlignedVector<VectorX> &JointLimitPRelua,
               const AlignedVector<VectorX> &JointLimitPRelub,
               const Vector<23> &JointLimitScale,
               // initial estimates
               Pose &TorsoPose,      // torso pose
               Matrix3 &SpineJoint,  // spine joint
               AlignedVector<Matrix3> &LeftArmJoint,
               AlignedVector<Matrix3> &RightArmJoint,
               AlignedVector<Matrix3> &LeftLegJoint,
               AlignedVector<Matrix3> &RightLegJoint, VectorX &betas) {
  //------------------------------------------------------------------------------
  // Optimizer weights
  //------------------------------------------------------------------------------
  Initializer::Options options;
  options.rel_lin_func_decrease_tol = 4e-3;
  options.delta = 2;
  options.weights[Initializer::FactorIndex::PinholeCamera] =
      FocalLength * FocalLength;
  options.weights[Initializer::FactorIndex::POF] = 100000;
  options.weights[Initializer::FactorIndex::DepthCamera] = 1000;
  options.weights[Initializer::FactorIndex::JointLimit] = 1000;
  options.weights[Initializer::FactorIndex::JointConst] = 1000;
  options.weights[Initializer::FactorIndex::Pose] = 100;

  //------------------------------------------------------------------------------
  // Create skeleton
  //------------------------------------------------------------------------------
  VectorX KeyPoints;
  createInitialSMPLSkeleton(Measurements3D, KeyPoints);

  //------------------------------------------------------------------------------
  // Torso Initializer
  //------------------------------------------------------------------------------
  const auto &TorsoIndex = InitialInfo::SMPL::TorsoIndex[1];
  const auto &TorsoDepthCamSInfo =
      scope::InitialInfo::SMPL::TorsoDepthCameraFactorInfo[1];

  solvePose(TorsoDepthCamSInfo, KeyPoints.segment<3>(3 * TorsoIndex[0][1]),
            KeyPoints, Measurements3D, TorsoPose);

  Vector3 t;
  solveReducedEPnP(KeyPointInfo::SMPL::KeyPoint3Dto2D, Measurements3D,
                   Measurements2D, KeyPoint2DConfidenceThreshold, t);
  TorsoPose.t = t;

  Scalar SpineAngle = acos(POFMeasurements.col(6).dot(POFMeasurements.col(7)));

  // create factors
  const auto &ExtTorsoIndex = InitialInfo::SMPL::ExtTorsoIndex[1];
  const auto &ExtTorsoPinholeCamInfo =
      InitialInfo::SMPL::ExtTorsoPinholeCameraFactorInfo[1];
  const auto &ExtTorsoPOFInfo = InitialInfo::SMPL::ExtTorsoPOFFactorInfo[1];
  const auto &ExtTorsoDepthCamInfo =
      InitialInfo::SMPL::ExtTorsoDepthCameraFactorInfo[1];

  // Pinhole camera factors
  std::vector<std::shared_ptr<Initializer::PinholeCameraFactor>>
      ExtTorsoPinholeCamFactors;
  createInitialPinholeCameraFactors(
      ExtTorsoPinholeCamInfo, ExtTorsoIndex, KeyPoints, KeyPoint2DGMSigma,
      KeyPoint2DGMEps, Measurements2D, KeyPoint2DConfidenceThreshold,
      ExtTorsoPinholeCamFactors);

  // POF factors
  std::vector<std::shared_ptr<Initializer::POFFactor>> ExtTorsoPOFFactors;
  createInitialPOFFactors(ExtTorsoPOFInfo, POFGMSigma, POFGMEps,
                          POFMeasurements, ExtTorsoPOFFactors);

  // Pose factors
  Matrix6 ExtTorsoPoseSqrtCov = Matrix6::Zero();
  ExtTorsoPoseSqrtCov.diagonal().tail<3>().array() = 5;
  std::shared_ptr<Initializer::PoseFactor> ExtTorsoPoseFactor =
      std::make_shared<Initializer::PoseFactor>(0, ExtTorsoPoseSqrtCov,
                                                TorsoPose);

  // add factors
  Initializer::ExtTorsoInitializer ExtTorsoInitializer(
      options, KeyPoints.segment<3>(3 * ExtTorsoIndex[1][1]));

  for (auto &factor : ExtTorsoPinholeCamFactors) {
    ExtTorsoInitializer.addPinholeCameraFactor(factor);
  }

  for (auto &factor : ExtTorsoPOFFactors) {
    ExtTorsoInitializer.addPOFFactor(factor);
  }

  ExtTorsoInitializer.addPoseFactor(ExtTorsoPoseFactor);

  ExtTorsoInitializer.initialize(TorsoPose, SpineAngle);
  ExtTorsoInitializer.solve();

  TorsoPose = ExtTorsoInitializer.getPoses()[0];
  SpineAngle = ExtTorsoInitializer.getSpineAngle();

  // Torso depth camera factors
  std::vector<std::shared_ptr<Initializer::DepthCameraFactor>>
      ExtTorsoDepthCamFactors;
  createInitialDepthCameraFactors(
      ExtTorsoDepthCamInfo, ExtTorsoIndex, KeyPoints, KeyPoint3DGMSigma,
      KeyPoint3DGMEps, Measurements3D, TorsoPose.t, ExtTorsoDepthCamFactors);

  for (auto &factor : ExtTorsoDepthCamFactors) {
    ExtTorsoInitializer.addDepthCameraFactor(factor);
  }

  ExtTorsoInitializer.initialize(TorsoPose, SpineAngle);
  ExtTorsoInitializer.solve();

  TorsoPose = ExtTorsoInitializer.getPoses()[0];
  const auto &ChestPose = ExtTorsoInitializer.getPoses()[1];

  SpineJoint = ExtTorsoInitializer.getJoints()[0];

  //------------------------------------------------------------------------------
  // Left Leg Initializer
  //------------------------------------------------------------------------------
  LeftLegJoint.resize(2);
  Matrix3 LeftLegMat = Matrix3::Identity();
  LeftLegMat.col(1) -= 4000 * TorsoPose.R.transpose() * POFMeasurements.col(1);
  projectToSO3(LeftLegMat, LeftLegJoint[0]);

  Scalar LeftKneeAngle =
      acos(POFMeasurements.col(1).dot(POFMeasurements.col(2)));
  LeftLegJoint[1].setIdentity();
  LeftLegJoint[1](1, 1) = cos(LeftKneeAngle);
  LeftLegJoint[1](2, 1) = sin(LeftKneeAngle);
  LeftLegJoint[1](2, 2) = LeftLegJoint[1](1, 1);
  LeftLegJoint[1](1, 2) = -LeftLegJoint[1](2, 1);

  // create factors
  const auto &LeftLegIndex = InitialInfo::SMPL::LeftLegIndex[1];
  const auto &LeftLegKintree = InitialInfo::SMPL::LeftLegKinematicsTree;
  const auto &LeftLegPinholeCamInfo =
      InitialInfo::SMPL::LeftLegPinholeCameraFactorInfo[1];
  const auto &LeftLegPOFInfo = InitialInfo::SMPL::LeftLegPOFFactorInfo[1];
  const auto &LeftLegDepthCamInfo =
      InitialInfo::SMPL::LeftLegDepthCameraFactorInfo[1];
  const auto &LeftLegEulerAngleConstInfo =
      InitialInfo::SMPL::LeftLegEulerAngleConstInfo;

  // pinhole camera factors
  std::vector<std::shared_ptr<Initializer::PinholeCameraFactor>>
      LeftLegPinholeCamFactors;
  createInitialPinholeCameraFactors(
      LeftLegPinholeCamInfo, LeftLegIndex, KeyPoints, KeyPoint2DGMSigma,
      KeyPoint2DGMEps, Measurements2D, KeyPoint2DConfidenceThreshold,
      LeftLegPinholeCamFactors);

  // POF factors
  std::vector<std::shared_ptr<Initializer::POFFactor>> LeftLegPOFFactors;
  createInitialPOFFactors(LeftLegPOFInfo, POFGMSigma, POFGMEps, POFMeasurements,
                          LeftLegPOFFactors);

  // depth camera factors
  std::vector<std::shared_ptr<Initializer::DepthCameraFactor>>
      LeftLegDepthCamFactors;
  createInitialDepthCameraFactors(
      LeftLegDepthCamInfo, LeftLegIndex, KeyPoints, KeyPoint3DGMSigma,
      KeyPoint3DGMEps, Measurements3D, TorsoPose.t, LeftLegDepthCamFactors);

  // pose factors
  Matrix6 LeftLegPoseSqrtCov = Matrix6::Zero();
  LeftLegPoseSqrtCov.diagonal().array() = 20;
  std::shared_ptr<Initializer::PoseFactor> LeftLegPoseFactor =
      std::make_shared<Initializer::PoseFactor>(0, LeftLegPoseSqrtCov,
                                                TorsoPose);

  // joint constraint factors
  std::vector<std::shared_ptr<Initializer::EulerAngleConstFactor>>
      LeftLegEulerAngleConstFactors;

  createInitialEulerAngleConstFactors(LeftLegEulerAngleConstInfo,
                                      LeftLegEulerAngleConstFactors);

  // left leg initializer
  std::array<Vector3, 2> LeftLegRelJointLocations;

  for (int i = 0; i < LeftLegKintree.size(); i++) {
    LeftLegRelJointLocations[i] =
        KeyPoints.segment<3>(3 * LeftLegIndex[LeftLegKintree[i][1]][1]) -
        KeyPoints.segment<3>(3 * LeftLegIndex[LeftLegKintree[i][0]][1]);
  }

  Initializer::LegInitializer LeftLegInitializer(options,
                                                 LeftLegRelJointLocations);

  for (auto &factor : LeftLegPinholeCamFactors) {
    LeftLegInitializer.addPinholeCameraFactor(factor);
  }

  for (auto &factor : LeftLegPOFFactors) {
    LeftLegInitializer.addPOFFactor(factor);
  }

  for (auto &factor : LeftLegDepthCamFactors) {
    LeftLegInitializer.addDepthCameraFactor(factor);
  }

  LeftLegInitializer.addPoseFactor(LeftLegPoseFactor);

  for (auto &factor : LeftLegEulerAngleConstFactors) {
    LeftLegInitializer.addJointConstFactor(factor);
  }

  LeftLegInitializer.updateFactorWeights(options.weights);
  LeftLegInitializer.initialize(TorsoPose, LeftLegJoint);
  LeftLegInitializer.solve();

  LeftLegJoint = LeftLegInitializer.getJoints();

  //------------------------------------------------------------------------------
  // Right Leg Initializer
  //------------------------------------------------------------------------------
  RightLegJoint.resize(2);
  Matrix3 RightLegMat = Matrix3::Identity();
  RightLegMat.col(1) -= 4000 * TorsoPose.R.transpose() * POFMeasurements.col(4);
  projectToSO3(RightLegMat, RightLegJoint[0]);

  Scalar RightKneeAngle =
      acos(POFMeasurements.col(4).dot(POFMeasurements.col(5)));
  RightLegJoint[1].setIdentity();
  RightLegJoint[1](1, 1) = cos(RightKneeAngle);
  RightLegJoint[1](2, 1) = sin(RightKneeAngle);
  RightLegJoint[1](2, 2) = RightLegJoint[1](1, 1);
  RightLegJoint[1](1, 2) = -RightLegJoint[1](2, 1);

  // create factors
  const auto &RightLegIndex = InitialInfo::SMPL::RightLegIndex[1];
  const auto &RightLegKintree = InitialInfo::SMPL::RightLegKinematicsTree;
  const auto &RightLegPinholeCamInfo =
      InitialInfo::SMPL::RightLegPinholeCameraFactorInfo[1];
  const auto &RightLegPOFInfo = InitialInfo::SMPL::RightLegPOFFactorInfo[1];
  const auto &RightLegDepthCamInfo =
      InitialInfo::SMPL::RightLegDepthCameraFactorInfo[1];
  const auto &RightLegEulerAngleConstInfo =
      InitialInfo::SMPL::RightLegEulerAngleConstInfo;

  // pinhole camera factors
  std::vector<std::shared_ptr<Initializer::PinholeCameraFactor>>
      RightLegPinholeCamFactors;
  createInitialPinholeCameraFactors(
      RightLegPinholeCamInfo, RightLegIndex, KeyPoints, KeyPoint2DGMSigma,
      KeyPoint2DGMEps, Measurements2D, KeyPoint2DConfidenceThreshold,
      RightLegPinholeCamFactors);

  // POF factors
  std::vector<std::shared_ptr<Initializer::POFFactor>> RightLegPOFFactors;
  createInitialPOFFactors(RightLegPOFInfo, POFGMSigma, POFGMEps,
                          POFMeasurements, RightLegPOFFactors);

  // depth camera factors
  std::vector<std::shared_ptr<Initializer::DepthCameraFactor>>
      RightLegDepthCamFactors;
  createInitialDepthCameraFactors(
      RightLegDepthCamInfo, RightLegIndex, KeyPoints, KeyPoint3DGMSigma,
      KeyPoint3DGMEps, Measurements3D, TorsoPose.t, RightLegDepthCamFactors);

  // pose factors
  Matrix6 RightLegPoseSqrtCov = Matrix6::Zero();
  RightLegPoseSqrtCov.diagonal().array() = 20;
  std::shared_ptr<Initializer::PoseFactor> RightLegPoseFactor =
      std::make_shared<Initializer::PoseFactor>(0, RightLegPoseSqrtCov,
                                                TorsoPose);

  // joint constraint factors
  std::vector<std::shared_ptr<Initializer::EulerAngleConstFactor>>
      RightLegEulerAngleConstFactors;

  createInitialEulerAngleConstFactors(RightLegEulerAngleConstInfo,
                                      RightLegEulerAngleConstFactors);

  // right leg initializer
  std::array<Vector3, 2> RightLegRelJointLocations;

  for (int i = 0; i < RightLegKintree.size(); i++) {
    RightLegRelJointLocations[i] =
        KeyPoints.segment<3>(3 * RightLegIndex[RightLegKintree[i][1]][1]) -
        KeyPoints.segment<3>(3 * RightLegIndex[RightLegKintree[i][0]][1]);
  }

  Initializer::LegInitializer RightLegInitializer(options,
                                                  RightLegRelJointLocations);

  for (auto &factor : RightLegPinholeCamFactors) {
    RightLegInitializer.addPinholeCameraFactor(factor);
  }

  for (auto &factor : RightLegPOFFactors) {
    RightLegInitializer.addPOFFactor(factor);
  }

  for (auto &factor : RightLegDepthCamFactors) {
    RightLegInitializer.addDepthCameraFactor(factor);
  }

  RightLegInitializer.addPoseFactor(RightLegPoseFactor);

  for (auto &factor : RightLegEulerAngleConstFactors) {
    RightLegInitializer.addJointConstFactor(factor);
  }

  RightLegInitializer.updateFactorWeights(options.weights);
  RightLegInitializer.initialize(TorsoPose, RightLegJoint);
  RightLegInitializer.solve();

  RightLegJoint = RightLegInitializer.getJoints();

  //------------------------------------------------------------------------------
  // Left Arm Initializer
  //------------------------------------------------------------------------------
  LeftArmJoint.resize(2);
  Matrix3 LeftArmMat = Matrix3::Identity();
  LeftArmMat.col(0) += 4000 * ChestPose.R.transpose() * POFMeasurements.col(11);
  projectToSO3(LeftArmMat, LeftArmJoint[0]);

  Scalar LeftElbowAngle =
      -acos(POFMeasurements.col(11).dot(POFMeasurements.col(12)));
  LeftArmJoint[1].setIdentity();
  LeftArmJoint[1](0, 0) = cos(LeftElbowAngle);
  LeftArmJoint[1](0, 2) = sin(LeftElbowAngle);
  LeftArmJoint[1](2, 2) = LeftArmJoint[1](0, 0);
  LeftArmJoint[1](2, 0) = -LeftArmJoint[1](0, 2);

  // create factors
  const auto &LeftArmIndex = InitialInfo::SMPL::LeftArmIndex[1];
  const auto &LeftArmKintree = InitialInfo::SMPL::LeftArmKinematicsTree;
  const auto &LeftArmPinholeCamInfo =
      InitialInfo::SMPL::LeftArmPinholeCameraFactorInfo[1];
  const auto &LeftArmPOFInfo = InitialInfo::SMPL::LeftArmPOFFactorInfo[1];
  const auto &LeftArmDepthCamInfo =
      InitialInfo::SMPL::LeftArmDepthCameraFactorInfo[1];
  const auto &LeftArmEulerAngleConstInfo =
      InitialInfo::SMPL::LeftArmEulerAngleConstInfo;

  // pinhole camera factors
  std::vector<std::shared_ptr<Initializer::PinholeCameraFactor>>
      LeftArmPinholeCamFactors;
  createInitialPinholeCameraFactors(
      LeftArmPinholeCamInfo, LeftArmIndex, KeyPoints, KeyPoint2DGMSigma,
      KeyPoint2DGMEps, Measurements2D, KeyPoint2DConfidenceThreshold,
      LeftArmPinholeCamFactors);

  // POF factors
  std::vector<std::shared_ptr<Initializer::POFFactor>> LeftArmPOFFactors;
  createInitialPOFFactors(LeftArmPOFInfo, POFGMSigma, POFGMEps, POFMeasurements,
                          LeftArmPOFFactors);

  // depth camera factors
  std::vector<std::shared_ptr<Initializer::DepthCameraFactor>>
      LeftArmDepthCamFactors;
  createInitialDepthCameraFactors(
      LeftArmDepthCamInfo, LeftArmIndex, KeyPoints, KeyPoint3DGMSigma,
      KeyPoint3DGMEps, Measurements3D, TorsoPose.t, LeftArmDepthCamFactors);

  // joint constraint factors
  std::vector<std::shared_ptr<Initializer::EulerAngleConstFactor>>
      LeftArmEulerAngleConstFactors;

  createInitialEulerAngleConstFactors(LeftArmEulerAngleConstInfo,
                                      LeftArmEulerAngleConstFactors);

  // left arm joint limit factors
  std::vector<std::shared_ptr<Initializer::JointLimitFactor>>
      LeftArmJointLimitFactors;

  createInitialJointLimitFactors(LeftArmIndex, JointLimitWeight, JointLimitBias,
                                 JointLimitPRelua, JointLimitPRelub,
                                 JointLimitScale, LeftArmJointLimitFactors);

  // left arm initializer
  std::array<Vector3, 2> LeftArmRelJointLocations;

  for (int i = 0; i < LeftArmKintree.size(); i++) {
    LeftArmRelJointLocations[i] =
        KeyPoints.segment<3>(3 * LeftArmIndex[LeftArmKintree[i][1]][1]) -
        KeyPoints.segment<3>(3 * LeftArmIndex[LeftArmKintree[i][0]][1]);
  }

  Initializer::ArmInitializer LeftArmInitializer(
      options, LeftArmRelJointLocations,
      Initializer::ArmInitializer::Arm::Left);

  for (auto &factor : LeftArmPinholeCamFactors) {
    LeftArmInitializer.addPinholeCameraFactor(factor);
  }

  for (auto &factor : LeftArmPOFFactors) {
    LeftArmInitializer.addPOFFactor(factor);
  }

  for (auto &factor : LeftArmDepthCamFactors) {
    LeftArmInitializer.addDepthCameraFactor(factor);
  }

  for (auto &factor : LeftArmEulerAngleConstFactors) {
    LeftArmInitializer.addJointConstFactor(factor);
  }

  options.weights[Initializer::FactorIndex::JointLimit] = 1000;
  LeftArmInitializer.updateFactorWeights(options.weights);

  LeftArmInitializer.initialize(ChestPose, LeftArmJoint);
  LeftArmInitializer.solve();

  options.weights[Initializer::FactorIndex::JointLimit] = 250;
  LeftArmInitializer.updateFactorWeights(options.weights);
  LeftArmInitializer.solve();

  LeftArmJoint = LeftArmInitializer.getJoints();

  //------------------------------------------------------------------------------
  // Right Arm Initializer
  //------------------------------------------------------------------------------
  RightArmJoint.resize(2);
  Matrix3 RightArmMat = Matrix3::Identity();
  RightArmMat.col(0) -=
      4000 * ChestPose.R.transpose() * POFMeasurements.col(14);
  projectToSO3(RightArmMat, RightArmJoint[0]);

  Scalar RightElbowAngle =
      acos(POFMeasurements.col(14).dot(POFMeasurements.col(15)));
  RightArmJoint[1].setIdentity();
  RightArmJoint[1](0, 0) = cos(RightElbowAngle);
  RightArmJoint[1](0, 2) = sin(RightElbowAngle);
  RightArmJoint[1](2, 2) = RightArmJoint[1](0, 0);
  RightArmJoint[1](2, 0) = -RightArmJoint[1](0, 2);

  // create factors
  const auto &RightArmIndex = InitialInfo::SMPL::RightArmIndex[1];
  const auto &RightArmKintree = InitialInfo::SMPL::RightArmKinematicsTree;
  const auto &RightArmPinholeCamInfo =
      InitialInfo::SMPL::RightArmPinholeCameraFactorInfo[1];
  const auto &RightArmPOFInfo = InitialInfo::SMPL::RightArmPOFFactorInfo[1];
  const auto &RightArmDepthCamInfo =
      InitialInfo::SMPL::RightArmDepthCameraFactorInfo[1];
  const auto &RightArmEulerAngleConstInfo =
      InitialInfo::SMPL::RightArmEulerAngleConstInfo;

  // pinhole camera factors
  std::vector<std::shared_ptr<Initializer::PinholeCameraFactor>>
      RightArmPinholeCamFactors;
  createInitialPinholeCameraFactors(
      RightArmPinholeCamInfo, RightArmIndex, KeyPoints, KeyPoint2DGMSigma,
      KeyPoint2DGMEps, Measurements2D, KeyPoint2DConfidenceThreshold,
      RightArmPinholeCamFactors);

  // POF factors
  std::vector<std::shared_ptr<Initializer::POFFactor>> RightArmPOFFactors;
  createInitialPOFFactors(RightArmPOFInfo, POFGMSigma, POFGMEps,
                          POFMeasurements, RightArmPOFFactors);

  // depth camera factors
  std::vector<std::shared_ptr<Initializer::DepthCameraFactor>>
      RightArmDepthCamFactors;
  createInitialDepthCameraFactors(
      RightArmDepthCamInfo, RightArmIndex, KeyPoints, KeyPoint3DGMSigma,
      KeyPoint3DGMEps, Measurements3D, TorsoPose.t, RightArmDepthCamFactors);

  // joint constraint factors
  std::vector<std::shared_ptr<Initializer::EulerAngleConstFactor>>
      RightArmEulerAngleConstFactors;

  createInitialEulerAngleConstFactors(RightArmEulerAngleConstInfo,
                                      RightArmEulerAngleConstFactors);

  // left arm joint limit factors
  std::vector<std::shared_ptr<Initializer::JointLimitFactor>>
      RightArmJointLimitFactors;

  createInitialJointLimitFactors(
      RightArmIndex, JointLimitWeight, JointLimitBias, JointLimitPRelua,
      JointLimitPRelub, JointLimitScale, RightArmJointLimitFactors);

  // right arm initializer
  std::array<Vector3, 2> RightArmRelJointLocations;

  for (int i = 0; i < RightArmKintree.size(); i++) {
    RightArmRelJointLocations[i] =
        KeyPoints.segment<3>(3 * RightArmIndex[RightArmKintree[i][1]][1]) -
        KeyPoints.segment<3>(3 * RightArmIndex[RightArmKintree[i][0]][1]);
  }

  Initializer::ArmInitializer RightArmInitializer(
      options, RightArmRelJointLocations,
      Initializer::ArmInitializer::Arm::Right);

  for (auto &factor : RightArmPinholeCamFactors) {
    RightArmInitializer.addPinholeCameraFactor(factor);
  }

  for (auto &factor : RightArmPOFFactors) {
    RightArmInitializer.addPOFFactor(factor);
  }

  for (auto &factor : RightArmDepthCamFactors) {
    RightArmInitializer.addDepthCameraFactor(factor);
  }

  for (auto &factor : RightArmEulerAngleConstFactors) {
    RightArmInitializer.addJointConstFactor(factor);
  }

  options.weights[Initializer::FactorIndex::JointLimit] = 0;
  RightArmInitializer.updateFactorWeights(options.weights);

  RightArmInitializer.initialize(ChestPose, RightArmJoint);
  RightArmInitializer.solve();

  options.weights[Initializer::FactorIndex::JointLimit] = 0;
  RightArmInitializer.solve();

  RightArmJoint = RightArmInitializer.getJoints();

  // leg knee joints correction
  const static Matrix3 KneeJointCorrection =
      (Matrix3() << 1, 0, 0, 0, cos(-0.075), -sin(-0.075), 0, sin(-0.075),
       cos(-0.075))
          .finished();
  LeftLegJoint[1] = LeftLegJoint[1] * KneeJointCorrection;
  RightLegJoint[1] = RightLegJoint[1] * KneeJointCorrection;

  return 0;
}

int initializeRootPose(const AlignedVector<Pose> &Poses, const VectorX &Param,
                       // SMPL model info
                       const std::vector<int> &kintree, const MatrixX &JDirs,
                       const VectorX &J, const MatrixX &KeyPointDirs,
                       const VectorX &KeyPoints,
                       // KeyPoint2D setup
                       Scalar FocalLength, const Matrix3X &Measurements2D,
                       Scalar KeyPoint2DGMSigma, Scalar KeyPoint2DGMEps,
                       const VectorX &KeyPoint2DConfidenceThreshold,
                       // POF setup
                       const Matrix3X &POFMeasurements, Scalar POFGMSigma,
                       Scalar POFGMEps,
                       // initial estimates
                       Pose &RootPose) {
  assert(Poses.size() == 24);
  assert(Param.size() == JDirs.cols());
  assert(Param.size() == KeyPointDirs.cols());

  if (Poses.size() != 24 || Param.size() != JDirs.cols() ||
      Param.size() != KeyPointDirs.cols()) {
    LOG(ERROR) << "Inconsistent keypoint information." << std::endl;
    exit(-1);
  }

  std::vector<std::shared_ptr<Initializer::PinholeCameraFactor>> RootCamFactors;

  createInitialRootPinholeCameraFactors(
      KeyPointInfo::SMPL::JointPinholeCameraFactorInfo,
      KeyPointInfo::SMPL::VertexPinholeCameraFactorInfo, Poses, Param, JDirs, J,
      KeyPointDirs, KeyPoints, KeyPoint2DGMSigma, KeyPoint2DGMEps,
      Measurements2D, KeyPoint2DConfidenceThreshold, RootCamFactors);

  std::vector<std::shared_ptr<Initializer::POFFactor>> RootPOFFactors;

  createInitialRootPOFFactors(KeyPointInfo::SMPL::UnitPOFFactorInfo,
                              KeyPointInfo::SMPL::ScaledPOFFactorInfo,
                              KeyPointInfo::SMPL::RelPOFFactorInfo, Poses,
                              Param, JDirs, J, KeyPointDirs, KeyPoints, kintree,
                              POFGMSigma, POFGMEps, POFMeasurements,
                              RootPOFFactors);

  Matrix6 RootPoseSqrtCov = Matrix6::Zero();
  RootPoseSqrtCov.diagonal().tail<3>().array() = 5;

  std::shared_ptr<Initializer::PoseFactor> TorsoPoseFactor =
      std::make_shared<Initializer::PoseFactor>(0, RootPoseSqrtCov,
                                                Pose::Identity());

  Initializer::Options options;

  options.rel_lin_func_decrease_tol = 4e-3;
  options.delta = 2;
  options.weights[Initializer::FactorIndex::PinholeCamera] =
      FocalLength * FocalLength;
  options.weights[Initializer::FactorIndex::POF] = 100000;
  options.weights[Initializer::FactorIndex::DepthCamera] = 1000;
  options.weights[Initializer::FactorIndex::JointLimit] = 1000;
  options.weights[Initializer::FactorIndex::JointConst] = 1000;
  options.weights[Initializer::FactorIndex::Pose] = 100;

  Initializer::TorsoInitializer RootInitializer(options);

  for (auto & factor: RootCamFactors) {
    RootInitializer.addPinholeCameraFactor(factor);
  }

  for (auto & factor: RootPOFFactors) {
    RootInitializer.addPOFFactor(factor);
  }

  RootInitializer.addPoseFactor(TorsoPoseFactor);

  RootInitializer.initialize(Pose::Identity(), {});
  RootInitializer.solve();

  RootPose.R.noalias() = RootInitializer.getPoses()[0].R * Poses[0].R;
  RootPose.t = RootInitializer.getPoses()[0].t;
  RootPose.t.noalias() += RootInitializer.getPoses()[0].R * Poses[0].t;

  return 0;
}
}  // namespace SMPL
}  // namespace Initialization
}  // namespace scope



<|start_filename|>C++/scope/initializer/TorsoInitializer.h<|end_filename|>
#pragma once

#include <memory>
#include <vector>

#include <scope/initializer/Initializer.h>

namespace scope {
namespace Initializer {

class TorsoInitializer : public Initializer {
protected:
  // intermediates used for update
  mutable Vector6 mDRootPoseChange;
  mutable Pose mRootPoseChange;

public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  TorsoInitializer(const Options &options);

  TorsoInitializer(const TorsoInitializer &) = delete;

  TorsoInitializer &operator=(const TorsoInitializer &) = delete;

protected:
  virtual int FKintree(int n) const override { return 0; }
  virtual int DFKintree() const override { return 0; }

  virtual int updateGaussNewton() const override;
  virtual int update(Scalar stepsize) const override;
};

} // namespace Initializer
} // namespace scope


<|start_filename|>C++/scope/model/Link.h<|end_filename|>
#pragma once

#include <scope/base/Types.h>
#include <memory>
#include <vector>

namespace scope {
class Link {
 public:
  std::string mName;

  int mId;
  int mParent;
  int mJoint;

  std::vector<int> mvChildren;

 public:
  Link() : mName(""), mId(-1), mParent(-1), mJoint(-1){};

  const std::string& name() const { return mName; };

  const int id() const { return mId; };

  const int parent() const { return mParent; }

  const int joint() const { return mJoint; }

  const std::vector<int>& children() const { return mvChildren; }
};

using LinkPtr = std::shared_ptr<Link>;
using LinkConstPtr = std::shared_ptr<const Link>;
}  // namespace scope


<|start_filename|>C++/scope/base/Types.h<|end_filename|>
#pragma once
//#define EIGEN_VECTORIZE_SSE4_2
#define EIGEN_VECTORIZE_AVX2

#include <Eigen/Dense>
#include <type_traits>
#include <vector>

namespace scope {
using Scalar = double;

template <int rows, int cols>
using Matrix = Eigen::Matrix<Scalar, rows, cols>;
template <int size>
using Vector = Eigen::Matrix<Scalar, size, 1>;
template <int size>
using Diagonal = Eigen::Diagonal<Scalar, size>;

using MatrixX = Matrix<Eigen::Dynamic, Eigen::Dynamic>;
using VectorX = Vector<Eigen::Dynamic>;
using Matrix2X = Matrix<2, Eigen::Dynamic>;
using Matrix3X = Matrix<3, Eigen::Dynamic>;
using MatrixX2 = Matrix<Eigen::Dynamic, 2>;
using MatrixX3 = Matrix<Eigen::Dynamic, 3>;
using Matrix2 = Matrix<2, 2>;
using Matrix3 = Matrix<3, 3>;
using Matrix4 = Matrix<4, 4>;
using Vector2 = Vector<2>;
using Vector3 = Vector<3>;
using Vector4 = Vector<4>;
using Matrix4X = Matrix<4, Eigen::Dynamic>;
using MatrixX4 = Matrix<Eigen::Dynamic, 4>;
using Matrix6X = Matrix<6, Eigen::Dynamic>;
using MatrixX6 = Matrix<Eigen::Dynamic, 6>;
using Matrix63 = Matrix<6, 3>;
using Matrix26 = Matrix<2, 6>;
using Matrix36 = Matrix<3, 6>;
using Matrix6 = Matrix<6, 6>;
using Vector6 = Matrix<6, 1>;

template <int rows, int cols>
using RowMajorMatrix = Eigen::Matrix<Scalar, rows, cols, Eigen::RowMajor>;

using RowMajorMatrixX = RowMajorMatrix<Eigen::Dynamic, Eigen::Dynamic>;
using RowMajorMatrix3X = RowMajorMatrix<3, Eigen::Dynamic>;
using RowMajorMatrix36 = RowMajorMatrix<3, 6>;

template <typename T>
using AlignedVector = std::vector<T, Eigen::aligned_allocator<T>>;

enum class HumanModel { SMPL };
}  // namespace scope


<|start_filename|>C++/scope/factor/ExtraPinholeCamera.cpp<|end_filename|>
#include <scope/factor/ExtraPinholeCamera.h>

namespace scope {
ExtraPinholeCamera::ExtraPinholeCamera(const Pose &T) : mExtraCamPose(T) {}

const Pose &ExtraPinholeCamera::getExtraCameraPose() const {
  return mExtraCamPose;
}

} // namespace scope


<|start_filename|>C++/scope/factor/JointConstFactor.h<|end_filename|>
#pragma once

#include <scope/factor/Factor.h>

namespace scope {
class JointConstFactor : public Factor {
 public:
  struct Evaluation : public Factor::Evaluation {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    Matrix3 errorR;
    Vector3 xi;

    Vector3 errorL;
    Vector3 errorU;

    Vector3 errorLInv;
    Vector3 errorUInv;

    Vector3 expL;
    Vector3 expU;
  };

  struct Linearization : public Factor::Linearization {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    Matrix3 DF;

    Vector3 Derror;
  };

 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  SCOAT_FACTOR_EVALUATION_NEW
  SCOAT_FACTOR_LINEARIZATION_NEW

  JointConstFactor(int joint, const Vector3 &weight, const Matrix3 &mean,
                   const Vector3 &lbnd, const Vector3 &ubnd,
                   const std::string &name = "", int index = -1,
                   bool active = true);

  virtual int evaluate(const AlignedVector<Pose> &poses,
                       const AlignedVector<VectorX> &shapes,
                       const AlignedVector<Matrix3> &joints,
                       const AlignedVector<VectorX> &params,
                       Factor::Evaluation &base_eval) const override;

  virtual int linearize(const AlignedVector<Pose> &poses,
                        const AlignedVector<VectorX> &shapes,
                        const AlignedVector<Matrix3> &joints,
                        const AlignedVector<VectorX> &params,
                        const Factor::Evaluation &base_eval,
                        Factor::Linearization &base_lin) const override;

 protected:
  Vector3 mWeight;
  Matrix3 mMean;

  Vector3 mLowerBnd;
  Vector3 mUpperBnd;

  const Scalar mScale = 2.5e-2;
  const Scalar mBnd = -1e-6;
};
}  // namespace scope


<|start_filename|>C++/scope/factor/JointDepthCameraFactor.cpp<|end_filename|>
#include <glog/logging.h>

#include <scope/factor/JointDepthCameraFactor.h>

namespace scope {
JointDepthCameraFactor::JointDepthCameraFactor(int pose, const Scalar &sigma,
                                               const Scalar &eps,
                                               const Vector3 &measurement,
                                               const Scalar &confidence,
                                               const std::string &name,
                                               int index, bool active)
    : DepthCameraFactor({pose}, {}, {}, {}, sigma, eps, measurement, confidence,
                        name, index, active) {}

int JointDepthCameraFactor::evaluate(const AlignedVector<Pose> &poses,
                                     const AlignedVector<VectorX> &shapes,
                                     const AlignedVector<Matrix3> &joints,
                                     const AlignedVector<VectorX> &params,
                                     Factor::Evaluation &base_eval) const {
  auto &eval = dynamic_cast<Evaluation &>(base_eval);
  eval.clear();

  const auto &i = mvPoses[0];
  assert(i >= 0 && i < poses.size());

  if (i < 0 || i >= poses.size()) {
    LOG(ERROR) << "The pose must be valid." << std::endl;

    exit(-1);
  }

  evaluate(poses[i], mMeasurement, eval);

  eval.status = Status::VALID;

  return 0;
}

int JointDepthCameraFactor::linearize(const AlignedVector<Pose> &poses,
                                      const AlignedVector<VectorX> &shapes,
                                      const AlignedVector<Matrix3> &joints,
                                      const AlignedVector<VectorX> &params,
                                      const Factor::Evaluation &base_eval,
                                      Factor::Linearization &base_lin) const {
  auto &eval = dynamic_cast<const Evaluation &>(base_eval);
  auto &lin = dynamic_cast<Linearization &>(base_lin);

  lin.clear();

  assert(eval.status == Status::VALID);

  if (eval.status != Status::VALID) {
    LOG(ERROR) << "The evaluation must be valid." << std::endl;

    exit(-1);
  }

  const auto &i = mvPoses[0];
  assert(i >= 0 && i < poses.size());

  if (i < 0 || i >= poses.size()) {
    LOG(ERROR) << "The pose must be valid." << std::endl;

    exit(-1);
  }

  lin.jacobians[0].resize(1);

  linearize(poses[i], mMeasurement, eval, lin);

  lin.status = Status::VALID;

  return 0;
}

int JointDepthCameraFactor::evaluate(const Pose &pose,
                                     const Vector3 &measurement,
                                     Evaluation &eval) const {
  eval.error = pose.t - measurement;
  eval.squaredErrorNorm = eval.error.squaredNorm();

#if not ROBUST_DEPTH
  eval.f = mCon * eval.squaredErrorNorm;
#else
  eval.f = mSigmaCon * eval.squaredErrorNorm / (mSigma + eval.squaredErrorNorm);
#endif

  return 0;
}

int JointDepthCameraFactor::linearize(const Pose &pose,
                                      const Vector3 &measurement,
                                      const Evaluation &eval,
                                      Linearization &lin) const {
  assert(lin.jacobians[0].size() == 1);

  auto &JPose = lin.jacobians[0][0];

  JPose.resize(3, 6);

  Eigen::Map<Matrix3> JR(JPose.data());
  Eigen::Map<Matrix3> Jt(JPose.data() + 9);

  const auto &t = pose.t;

  JR.setZero();

  JR(0, 1) = t[2];
  JR(0, 2) = -t[1];
  JR(1, 0) = -t[2];
  JR(1, 2) = t[0];
  JR(2, 0) = t[1];
  JR(2, 1) = -t[0];

  Jt.setIdentity();

  auto &gPose = lin.gPose;
  gPose.noalias() = JPose.transpose() * eval.error;

#if not ROBUST_DEPTH
  JPose *= mSqrtCon;
  gPose *= mCon;
#else
  preLinearizeRobustKernel(eval, lin);

  JPose.noalias() -= lin.scaledError * gPose.transpose();

  JPose *= lin.sqrtDrho;
  gPose *= lin.Drho;
#endif

  return 0;
}
}  // namespace scope


<|start_filename|>C++/scope/factor/JointFactor.cpp<|end_filename|>
#include <glog/logging.h>

#include <scope/factor/JointFactor.h>
#include <scope/math/SO3.h>

namespace scope {
JointFactor::JointFactor(int joint, const Matrix3 &sqrtCov, const Matrix3 &mean,
                         const std::string &name, int index, bool active)
    : Factor({}, {}, {joint}, {}, name, index, active),
      mSqrtCov(sqrtCov),
      mMean(mean) {
  assert((mean.col(0).cross(mean.col(1)) - mean.col(2)).norm() < 1e-5 &&
         "mean must be a rotational matrix");
}

int JointFactor::evaluate(const AlignedVector<Pose> &poses,
                          const AlignedVector<VectorX> &shapes,
                          const AlignedVector<Matrix3> &joints,
                          const AlignedVector<VectorX> &params,
                          Factor::Evaluation &base_eval) const {
  auto &eval = dynamic_cast<Evaluation &>(base_eval);
  eval.clear();

  const auto &i = mvJoints[0];
  assert(i >= 0 && i < joints.size());

  if (i >= joints.size()) {
    LOG(ERROR) << "The joint must be valid." << std::endl;

    exit(-1);
  }

  const auto &R = joints[i];

  eval.errorR.noalias() = mMean.transpose() * R;

  math::SO3::log(eval.errorR, eval.xi);
  eval.error.noalias() = mSqrtCov * eval.xi;

  eval.f = eval.error.squaredNorm();

  eval.status = Status::VALID;

  return 0;
}

int JointFactor::linearize(const AlignedVector<Pose> &poses,
                           const AlignedVector<VectorX> &shapes,
                           const AlignedVector<Matrix3> &joints,
                           const AlignedVector<VectorX> &params,
                           const Factor::Evaluation &base_eval,
                           Factor::Linearization &base_lin) const {
  auto &eval = dynamic_cast<const Evaluation &>(base_eval);
  auto &lin = dynamic_cast<Linearization &>(base_lin);

  lin.clear();

  assert(eval.status == Status::VALID);

  if (eval.status != Status::VALID) {
    LOG(ERROR) << "The evaluation must be valid." << std::endl;

    exit(-1);
  }

  const auto &i = mvJoints[0];
  assert(i >= 0 && i < joints.size());

  if (i < 0 || i >= joints.size()) {
    LOG(ERROR) << "The joint must be valid." << std::endl;

    exit(-1);
  }

  lin.jacobians[2].resize(1);
  lin.jacobians[2][0].resize(3, 3);
  Eigen::Map<Matrix3> J(lin.jacobians[2][0].data());

  math::SO3::dexpinv(eval.xi, lin.dexpinv);
  lin.dexpinvR.noalias() = lin.dexpinv * mMean.transpose();

  J.noalias() = mSqrtCov * lin.dexpinvR;

  lin.status = Status::VALID;

  return 0;
}
}  // namespace scope


<|start_filename|>C++/scope/initializer/Initializers.h<|end_filename|>
#pragma once

#include <scope/initializer/ArmInitializer.h>
#include <scope/initializer/ArmRefiner.h>
#include <scope/initializer/LegInitializer.h>
#include <scope/initializer/TorsoInitializer.h>
#include <scope/initializer/ExtTorsoInitializer.h>
#include <scope/initializer/HeadInitializer.h>


<|start_filename|>C++/scope/factor/JointFactor.h<|end_filename|>
#pragma once

#include <scope/factor/Factor.h>

namespace scope {
class JointFactor : public Factor {
 public:
  struct Evaluation : public Factor::Evaluation {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    Matrix3 errorR;
    Vector3 xi;
  };

  struct Linearization : public Factor::Linearization {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    Matrix3 dexpinv;
    Matrix3 dexpinvR;
  };

 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  SCOAT_FACTOR_EVALUATION_NEW
  SCOAT_FACTOR_LINEARIZATION_NEW

  JointFactor(int joint, const Matrix3 &sqrtCov, const Matrix3 &mean,
              const std::string &name = "", int index = -1, bool active = true);

  virtual int evaluate(const AlignedVector<Pose> &poses,
                       const AlignedVector<VectorX> &shapes,
                       const AlignedVector<Matrix3> &joints,
                       const AlignedVector<VectorX> &params,
                       Factor::Evaluation &base_eval) const override;

  virtual int linearize(const AlignedVector<Pose> &poses,
                        const AlignedVector<VectorX> &shapes,
                        const AlignedVector<Matrix3> &joints,
                        const AlignedVector<VectorX> &params,
                        const Factor::Evaluation &base_eval,
                        Factor::Linearization &base_lin) const override;

 protected:
  Matrix3 mSqrtCov;
  Matrix3 mMean;
};
}  // namespace scope


<|start_filename|>C++/scope/factor/ExtraPinholeCamera.h<|end_filename|>
#pragma once

#include <scope/base/Pose.h>
#include <scope/base/Types.h>

namespace scope {
class ExtraPinholeCamera {
public:
  struct Evaluation {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    // position in the extra camera frame
    Vector3 pExtraCam;
  };

  struct Linearization {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    Matrix<2, 3> JPoint;
  };

public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  ExtraPinholeCamera(const Pose &T);

  const Pose &getExtraCameraPose() const;

protected:
  // rigid body transformation from the main camera frame to the extra camera
  // frame
  Pose mExtraCamPose;
};
} // namespace scope


<|start_filename|>C++/scope/initializer/factor/PoseFactor.cpp<|end_filename|>
#include <glog/logging.h>

#include <scope/initializer/factor/PoseFactor.h>
#include <scope/math/SO3.h>

namespace scope {
namespace Initializer {
PoseFactor::PoseFactor(int pose, const Matrix6 &sqrtCov, const Pose &mean,
                       const std::string &name, int index)
    : Factor(name, index), mPose(pose), mSqrtCov(sqrtCov), mMean(mean) {
  assert((mean.R.col(0).cross(mean.R.col(1)) - mean.R.col(2)).norm() < 1e-5 &&
         "mean.R must be a rotational matrix");
}

int PoseFactor::evaluate(const AlignedVector<Pose> &poses,
                         const AlignedVector<Matrix3> &joints,
                         Factor::Evaluation &base_eval) const {
  auto &eval = dynamic_cast<Evaluation &>(base_eval);
  eval.clear();

  const auto &i = mPose;

  assert(i >= 0 && i < poses.size());

  if (i >= poses.size()) {
    LOG(ERROR) << "The pose must be valid." << std::endl;

    exit(-1);
  }

  auto &pose = poses[i];

  eval.errorR.noalias() = mMean.R.transpose() * pose.R;

  eval.error.resize(6);

  Eigen::Map<Vector3> omega(eval.xi.data());
  math::SO3::log(eval.errorR, omega);
  eval.xi.tail<3>() = pose.t - mMean.t;
  eval.error.noalias() = mSqrtCov * eval.xi;

  eval.f = eval.error.squaredNorm();

  eval.status = Status::VALID;

  return 0;
}

int PoseFactor::linearize(const AlignedVector<Pose> &poses,
                          const AlignedVector<Matrix3> &joints,
                          const Factor::Evaluation &base_eval,
                          Factor::Linearization &base_lin) const {
  auto &eval = dynamic_cast<const Evaluation &>(base_eval);
  auto &lin = dynamic_cast<Linearization &>(base_lin);

  lin.clear();

  const auto &i = mPose;

  assert(i >= 0 && i < poses.size());

  if (i >= poses.size()) {
    LOG(ERROR) << "The pose must be valid." << std::endl;

    exit(-1);
  }

  assert(eval.status == Status::VALID);

  if (eval.status != Status::VALID) {
    LOG(ERROR) << "The evaluation must be valid." << std::endl;

    exit(-1);
  }

  auto &pose = poses[i];

  auto &J = lin.jacobian;

  math::SO3::dexpinv(eval.xi.head<3>(), lin.dexpinv);
  lin.dexpinvR.noalias() = lin.dexpinv * mMean.R.transpose();

  J.topRightCorner<3, 3>().setZero();

  const auto &t = pose.t;

  Eigen::Map<Matrix63> JR(J.data());
  Eigen::Map<Matrix63> Jt(J.data() + 18);

  Jt = mSqrtCov.rightCols<3>();

  JR.row(0).noalias() = t.cross(Jt.row(0));
  JR.row(1).noalias() = t.cross(Jt.row(1));
  JR.row(2).noalias() = t.cross(Jt.row(2));
  JR.row(3).noalias() = t.cross(Jt.row(3));
  JR.row(4).noalias() = t.cross(Jt.row(4));
  JR.row(5).noalias() = t.cross(Jt.row(5));
  JR.noalias() += mSqrtCov.leftCols<3>() * lin.dexpinvR;

  lin.status = Status::VALID;

  return 0;
}
} // namespace Initializer
} // namespace scope


<|start_filename|>C++/scope/initializer/factor/JointConstFactor.h<|end_filename|>
#pragma once

#include <scope/initializer/factor/Factor.h>

namespace scope {
namespace Initializer {
class JointConstFactor : public Factor {
public:
  struct Evaluation : public Factor::Evaluation {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    Matrix3 errorR;
    Vector3 xi;

    Vector3 errorL;
    Vector3 errorU;

    Vector3 errorLInv;
    Vector3 errorUInv;

    Vector3 expL;
    Vector3 expU;
  };

  struct Linearization : public Factor::Linearization {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    Matrix3 jacobian;

    Matrix3 DF;

    Vector3 Derror;
  };

public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  SCOAT_INITIAL_EVALUATION_NEW
  SCOAT_INITIAL_LINEARIZATION_NEW

  JointConstFactor(int pose, const Vector3 &weight, const Matrix3 &mean,
                   const Vector3 &lbnd, const Vector3 &ubnd,
                   const std::string &name = "", int index = -1);

  virtual int evaluate(const AlignedVector<Pose> &poses,
                       const AlignedVector<Matrix3> &joints,
                       Factor::Evaluation &base_eval) const override;

  virtual int linearize(const AlignedVector<Pose> &poses,
                        const AlignedVector<Matrix3> &joints,
                        const Factor::Evaluation &base_eval,
                        Factor::Linearization &base_lin) const override;

  int getJoint() const { return mJoint; }

protected:
  int mJoint;

  Vector3 mWeight;
  Matrix3 mMean;

  Vector3 mLowerBnd;
  Vector3 mUpperBnd;

  const Scalar mScale = 2.5e-2;
  const Scalar mBnd = -1e-6;
};
} // namespace Initializer
} // namespace scope


<|start_filename|>C++/scope/utils/InitialInfo.h<|end_filename|>
#pragma once

#include <array>
#include <vector>

#include <scope/base/Types.h>

namespace scope {
namespace InitialInfo {
namespace SMPL {
//----------------------------------------------------
// Shape
//----------------------------------------------------
const std::vector<std::tuple<std::array<int, 4>, Vector3>> RelShapeInfo{
    //[0][0]: 2D keypoint tail
    //[0][1]: 2D keypoint head
    //[0][2]: 3D keypoint tail
    //[0][3]: 3D keypoint head
    //[1]: 3D direction
    {{13, 12, 0, 7}, {0, 1, 0}} // middle hip -> chest
};

const std::vector<std::tuple<std::array<int, 4>, Vector3>> RelShapePairInfo{
    //[0][0]: 2D keypoint tail
    //[0][1]: 2D keypoint head
    //[0][2]: 3D keypoint tail
    //[0][3]: 3D keypoint head
    //[1]: 3D direction
    {{13, 14, 0, 1}, {1, 0, 0}},    // middle hip -> left hip
    {{13, 15, 0, 4}, {-1, 0, 0}},   // middle hip -> right hip
    {{22, 25, 2, 3}, {0, -1, 0}},   // left knee -> left ankle
    {{23, 26, 5, 6}, {0, -1, 0}},   // right knee -> right ankle
    {{16, 36, 11, 12}, {1, 0, 0}},  // left shoulder ->  left elbow
    {{17, 37, 14, 15}, {-1, 0, 0}}, // right shoulder ->  right elbow
    {{36, 38, 12, 13}, {1, 0, 0}},  // left elbow ->  left wrist
    {{37, 39, 15, 16}, {-1, 0, 0}}  // right elbow ->  right wrist
};

//----------------------------------------------------
// Torso
//----------------------------------------------------
const std::vector<std::array<int, 2>> TorsoIndex[2] = {
    //[0]: body part index on SMPL/SMPLH
    //[1]: 3D keypoint index as joint origin
    {
        {0, 0} // 0: (torso, middle hip)
    },
    {
        {0, 0} // 0: (torso, middle hip)
    }};

const std::vector<std::array<int, 2>> TorsoKinematicsTree{
    //[0]: parent
    //[1]: child
};

const std::vector<std::array<int, 3>> TorsoPinholeCameraFactorInfo[2] = {
    {
        //[0]: 2D keypoint measurement index
        //[2]: body part index on SMPL/SMPLH
        //[3]: keypoint index
        {0, 0, 0},        // nose
        {1, 0, 1},        // left eye
        {2, 0, 2},        // right eye
        {3, 0, 3},        // left ear
        {4, 0, 4},        // right ear
        {5, 0, 16},       // left upper arm
        {6, 0, 17},       // right upper arm
        {11, 0, 14},      // left hip
        {12, 0, 15},      // right hip
        {17, 0, 11},      // head top
        {18, 0, 12 + 18}, // thorax
        {19, 0, 13},      // middle hip
        {26, 0, 12},      // chest
        {27, 0, 15 + 18}  // neck
    },
    {
        //[0]: 2D keypoint measurement index
        //[2]: body part index on initial skeleton
        //[3]: joint index on initial skeleton
        {5, 0, 11},  // left upper arm
        {6, 0, 14},  // right upper arm
        {11, 0, 1},  // left hip
        {12, 0, 4},  // right hip
        {17, 0, 10}, // head top
        {18, 0, 8},  // thorax
        {19, 0, 0},  // middle hip
        {26, 0, 7},  // chest
        {27, 0, 9}   // neck
    }};

const AlignedVector<std::tuple<int, int, Vector3, Scalar>>
    TorsoPOFFactorInfo[2] = {
        {
            //[0]: POF measurement index
            //[1]: body part index
            //[2]: unit directional vector
            //[3]: confidence
            {0, 0, {1, 0, 0}, 1.0},  // middle hip -> left hip
            {3, 0, {-1, 0, 0}, 1.0}, // middle hip -> right hip
            {6, 0, {0, 1, 0}, 0.4},  // middle hip -> chest
            {7, 0, {0, 1, 0}, 0.4},  // chest -> thorax
            {10, 0, {1, 0, 0}, 1.0}, // thorax -> left upper arm
            {13, 0, {-1, 0, 0}, 1.0} // thorax -> right upper arm
        },
        {
            //[0]: POF measurement index
            //[1]: body part index
            //[2]: unit directional vector
            //[3]: confidence
            {0, 0, {1, 0, 0}, 1.0},  // middle hip -> left hip
            {3, 0, {-1, 0, 0}, 1.0}, // middle hip -> right hip
            {6, 0, {0, 1, 0}, 0.4},  // middle hip -> chest
            {7, 0, {0, 1, 0}, 0.4},  // chest -> thorax
            {10, 0, {1, 0, 0}, 1.0}, // thorax -> left upper arm
            {13, 0, {-1, 0, 0}, 1.0} // thorax -> right upper arm
        }};

const std::vector<std::tuple<int, int, int, Scalar>>
    TorsoDepthCameraFactorInfo[2] = {
        {
            //[0]: 3D keypoint measurement index
            //[1]: rigid body index
            //[2]: keypoint index
            {0, 0, 13, 1.0},      // middle hip
            {1, 0, 14, 1.0},      // left hip
            {4, 0, 15, 1.0},      // right hip
            {7, 0, 12, 0.2},      // chest
            {8, 0, 12 + 18, 1.0}, // thorax
            {9, 0, 15 + 18, 0.2}, // neck
            {11, 0, 16, 1.0},     // left upper arm
            {14, 0, 17, 1.0}      // right upper arm
        },

        {
            //[0]: 3D keypoint measurement index
            //[1]: rigid body index
            //[2]: joint index on initial skeleton
            {0, 0, 0, 1.0},   // middle hip
            {1, 0, 1, 1.0},   // left hip
            {4, 0, 4, 1.0},   // right hip
            {7, 0, 7, 0.2},   // chest
            {8, 0, 8, 1.0},   // thorax
                              //{9, 0, 9, 0.2},    // neck
            {11, 0, 11, 1.0}, // left upper arm
            {14, 0, 14, 1.0}  // right upper arm
        }};

//----------------------------------------------------
// extended torso
//----------------------------------------------------
const std::vector<std::array<int, 2>> ExtTorsoIndex[2] = {
    //[0]: body part index on SMPL/SMPLH
    //[1]: 3D keypoint index as joint origin
    {
        {0, 13}, // 0: (torso, middle hip)
        {6, 12}  // 1: (spine, chest)
    },
    {
        {0, 0}, // 0: (torso, middle hip)
        {6, 7}  // 1: (spine, chest)
    }};

const std::vector<std::array<int, 2>> ExtTorsoKinematicsTree{
    //[0]: parent
    //[1]: child
    {0, 1}, // middle hip -> chest
};

const std::vector<std::array<int, 3>> ExtTorsoPinholeCameraFactorInfo[2] = {
    {
        //[0]: 2D keypoint measurement index
        //[2]: body part index on SMPL/SMPLH
        //[3]: keypoint index
        {0, 1, 0},        // nose
        {1, 1, 1},        // left eye
        {2, 1, 2},        // right eye
        {3, 1, 3},        // left ear
        {4, 1, 4},        // right ear
        {5, 1, 16},       // left upper arm
        {6, 1, 17},       // right upper arm
        {17, 1, 11},      // head top
        {18, 1, 12 + 18}, // thorax
        {27, 1, 15 + 18}, // neck
        {11, 0, 14},      // left hip
        {12, 0, 15},      // right hip
        {19, 0, 13},      // middle hip
        {26, 0, 12}       // chest
    },
    {
        //[0]: 2D keypoint measurement index
        //[2]: body part index on SMPL/SMPLH
        //[3]: keypoint index
        {5, 1, 11}, // left upper arm
        {6, 1, 14}, // right upper arm
        {18, 1, 8}, // thorax
        {11, 0, 1}, // left hip
        {12, 0, 4}, // right hip
        {19, 0, 0}, // middle hip
        {26, 0, 7}  // chest
    }};

const AlignedVector<std::tuple<int, int, Vector3, Scalar>>
    ExtTorsoPOFFactorInfo[2] = {
        {
            //[0]: POF measurement index
            //[1]: body part index
            //[2]: unit directional vector
            //[3]: confidence
            {0, 0, {1, 0, 0}, 1.0},      // middle hip -> left hip
            {3, 0, {-1, 0, 0}, 1.0},     // middle hip -> right hip
            {6, 0, {0, 1, 0}, 0.4},      // middle hip -> chest
            {7, 1, {0, 1, 0}, 0.4},      // chest -> thorax
            {10, 1, {1, -0.25, 0}, 1.0}, // thorax -> left upper arm
            {13, 1, {-1, -0.25, 0}, 1.0} // thorax -> right upper arm
        },
        {
            //[0]: POF measurement index
            //[1]: body part index
            //[2]: unit directional vector
            //[3]: confidence
            {0, 0, {1, 0, 0}, 1.0},  // middle hip -> left hip
            {3, 0, {-1, 0, 0}, 1.0}, // middle hip -> right hip
            {6, 0, {0, 1, 0}, 0.4},  // middle hip -> chest
            {7, 1, {0, 1, 0}, 0.4},  // chest -> thorax
            {10, 1, {1, 0, 0}, 1.0}, // thorax -> left upper arm
            {13, 1, {-1, 0, 0}, 1.0} // thorax -> right upper arm
        }};

const std::vector<std::tuple<int, int, int, Scalar>>
    ExtTorsoDepthCameraFactorInfo[2] = {
        {
            //[0]: 3D keypoint measurement index
            //[1]: rigid body index
            //[2]: keypoint index
            {0, 0, 13, 1.0},      // middle hip
            {1, 0, 14, 1.0},      // left hip
            {4, 0, 15, 1.0},      // right hip
            {7, 0, 12, 0.2},      // chest
            {8, 1, 12 + 18, 1.0}, // thorax
            {9, 1, 15 + 18, 0.2}, // neck
            {11, 1, 16, 1.0},     // left upper arm
            {14, 1, 17, 1.0}      // right upper arm
        },
        {
            //[0]: 3D keypoint measurement index
            //[1]: rigid body index
            //[2]: joint index on initial skeleton
            {0, 0, 0, 1.0},   // middle hip
            {1, 0, 1, 1.0},   // left hip
            {4, 0, 4, 1.0},   // right hip
            {7, 0, 7, 0.2},   // chest
            {8, 1, 8, 1.0},   // thorax
            {11, 1, 11, 1.0}, // left upper arm
            {14, 1, 14, 1.0}  // right upper arm
        }};

//----------------------------------------------------
// Head
//----------------------------------------------------
const std::vector<std::array<int, 2>> HeadIndex[2] = {
    //[0]: body part index on SMPL/SMPLH
    //[1]: 3D keypoint index as joint origin
    {
        {15, 12 + 18} // 0: (head, thorax)
    },
    {
        {15, 8} // 0: (head, thorax)
    }};

const std::vector<std::array<int, 2>> HeadKinematicsTree{
    //[0]: parent
    //[1]: child
};

const std::vector<std::array<int, 3>> HeadPinholeCameraFactorInfo[2] = {
    {
        //[0]: 2D keypoint measurement index
        //[2]: body part index on SMPL/SMPLH
        //[3]: keypoint index
        {0, 0, 0},        // nose
        {1, 0, 1},        // left eye
        {2, 0, 2},        // right eye
        {3, 0, 3},        // left ear
        {4, 0, 4},        // right ear
        {17, 0, 11},      // head top
        {18, 0, 12 + 18}, // thorax
    },
    {
        //[0]: 2D keypoint measurement index
        //[2]: body part index on initial skeleton
        //[3]: joint index on initial skeleton
        {17, 0, 10}, // head top
        {18, 0, 8},  // thorax
        {27, 0, 9}   // neck
    }};

const std::vector<std::tuple<int, int, int, Scalar>>
    HeadDepthCameraFactorInfo[2] = {{
                                        //[0]: 3D keypoint measurement index
                                        //[1]: rigid body index
                                        //[2]: keypoint index
                                        {8, 0, 12 + 18, 1.0}, // thorax
                                        {9, 0, 15 + 18, 0.2}, // neck
                                        {10, 0, 11, 0.5},     // head top
                                    },

                                    {
                                        //[0]: 3D keypoint measurement index
                                        //[1]: rigid body index
                                        //[2]: joint index on initial skeleton
                                        {8, 0, 8, 1.0},   // thorax
                                        {9, 0, 9, 0.5},   // neck
                                        {10, 0, 10, 0.5}, // head top
                                    }};

//----------------------------------------------------
// left arm
//----------------------------------------------------
const std::vector<std::array<int, 2>> LeftArmIndex[2] = {
    //[0]: body part index on SMPL/SMPLH
    //[1]: 2D keypoint index as joint origin on SMPL/SMPLH
    //[2]: 3D keypoint index as joint origin on SMPL/SMPLH
    {
        {0, 13},      // 0: (torso, middle hip)
        {16, 16},     // 1: (left upper arm, left shoulder)
        {18, 18 + 18} // 2: (left forearm, left elbow)
    },
    {
        {3, 7},   // 0: (torso, middle hip)
        {16, 11}, // 1: (left upper arm, left upper arm)
        {18, 12}  // 2: (left forearm, left elbow)
    }};

const std::vector<std::array<int, 2>> LeftArmKinematicsTree{
    //[0]: parent
    //[1]: child
    {0, 1}, // torso -> left upper arm
    {1, 2}  // left upper arm -> left forearm
};

const std::vector<std::array<int, 3>> LeftArmPinholeCameraFactorInfo[2] = {
    {
        //[0]: 2D keypoint measurement index
        //[1]: body part index on initial skeleton
        //[2]: joint index on initial skeleton
        {7, 1, 18 + 18}, // left elbow
        {9, 2, 20 + 18}  // left wrist
    },
    {
        //[0]: 2D keypoint measurement index
        //[1]: rigid body index
        //[2]: joint index on initial skeleton
        {7, 1, 12}, // left elbow
        {9, 2, 13}  // left wrist
    }};

const AlignedVector<std::tuple<int, int, Vector3, Scalar>>
    LeftArmPOFFactorInfo[2] = {
        {
            //[0]: POF measurement index
            //[1]: body part index
            //[2]: unit directional vector
            //[3]: confidence
            {11, 1, {1, 0, 0}, 5.0}, // left upper arm -> left elbow
            {12, 2, {1, 0, 0}, 5.0}  // left elbow -> left wrist
        },
        {
            //[0]: POF measurement index
            //[1]: body part index
            //[2]: unit directional vector
            //[3]: confidence
            {11, 1, {1, 0, 0}, 5.0}, // left upper arm -> left elbow
            {12, 2, {1, 0, 0}, 5.0}  // left elbow -> left wrist
        }};

const std::vector<std::tuple<int, int, int, Scalar>>
    LeftArmDepthCameraFactorInfo[2] = {
        {
            //[0]: 3D keypoint measurement index
            //[1]: rigid body index
            //[2]: joint index on initial skeleton
            {12, 1, 18 + 18, 1.0}, // left elbow
            {13, 2, 20 + 18, 1.0}  // left wrist
        },
        {
            //[0]: 3D keypoint measurement index
            //[1]: rigid body index
            //[2]: joint index on initial skeleton
            {12, 1, 12, 1.0}, // left elbow
            {13, 2, 13, 1.0}  // left wrist
        }};

const AlignedVector<std::tuple<int, Vector3, Matrix3, Vector3, Vector3>>
    LeftArmJointConstInfo{
        //[0]: joint index
        //[1]: weight
        //[2]: reference rotation
        //[3]: lower bound
        //[4]: upper bound
        {1,
         {3, 3, 3},
         (Matrix3() << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0).finished(),
         {-0.50 * M_PI, -0.75 * M_PI, -0.25 * M_PI},
         {+0.25 * M_PI, +0.00 * M_PI, +0.25 * M_PI}}};

const AlignedVector<std::tuple<int, Vector3, Matrix3, Vector3, Vector3>>
    LeftArmEulerAngleConstInfo{
        //[0]: joint index
        //[1]: weight
        //[2]: reference rotation
        //[3]: lower bound
        //[4]: upper bound
        {0,
         {3, 3, 3},
         (Matrix3() << 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0).finished(),
         {-0.67 * M_PI, -0.33 * M_PI, -0.60 * M_PI},
         {+0.46 * M_PI, +0.48 * M_PI, +0.40 * M_PI}}};

//----------------------------------------------------
// right arm
//----------------------------------------------------
const std::vector<std::array<int, 2>> RightArmIndex[2] = {
    //[0]: body part index on SMPL/SMPLH
    //[1]: 2D keypoint index as joint origin on SMPL/SMPLH
    //[2]: 3D keypoint index as joint origin on SMPL/SMPLH
    {
        {0, 13},      // 0: (torso, middle hip)
        {17, 17},     // 1: (right upper arm, right shoulder)
        {19, 19 + 18} // 2: (right forearm, right elbow)
    },
    {
        {3, 7},   // 0: (torso, middle hip)
        {17, 14}, // 1: (right upper arm, right upper arm)
        {19, 15}  // 2: (right forearm, right elbow)
    }};

const std::vector<std::array<int, 2>> RightArmKinematicsTree{
    //[0]: parent
    //[1]: child
    {0, 1}, // torso -> right upper arm
    {1, 2}  // right upper arm -> right forearm
};

const std::vector<std::array<int, 3>> RightArmPinholeCameraFactorInfo[2] = {
    {
        //[0]: 2D keypoint measurement index
        //[1]: rigid body index
        //[2]: keypoint index
        {8, 1, 19 + 18}, // right elbow
        {10, 2, 21 + 18} // right wrist
    },
    {
        //[0]: 2D keypoint measurement index
        //[1]: rigid body index
        //[2]: joint index on initial skeleton
        {8, 1, 15}, // right elbow
        {10, 2, 16} // right wrist
    }};

const AlignedVector<std::tuple<int, int, Vector3, Scalar>>
    RightArmPOFFactorInfo[2] = {
        {
            //[0]: POF measurement index
            //[1]: body part index
            //[2]: unit directional vector
            //[3]: confidence
            {14, 1, {-1, 0, 0}, 5.0}, // right upper arm -> right elbow
            {15, 2, {-1, 0, 0}, 5.0}  // right elbow -> right wrist
        },
        {
            //[0]: POF measurement index
            //[1]: body part index
            //[2]: unit directional vector
            //[3]: confidence
            {14, 1, {-1, 0, 0}, 5.0}, // right upper arm -> right elbow
            {15, 2, {-1, 0, 0}, 5.0}  // right elbow -> right wrist
        }};

const std::vector<std::tuple<int, int, int, Scalar>>
    RightArmDepthCameraFactorInfo[2] = {
        {
            //[0]: 3D keypoint measurement index
            //[1]: rigid body index
            //[2]: keypoint index
            {15, 1, 19 + 18, 1.0}, // right elbow
            {16, 2, 21 + 18, 1.0}  // right wrist
        },
        {
            //[0]: 3D keypoint measurement index
            //[1]: rigid body index
            //[2]: joint index on initial skeleton
            {15, 1, 15, 1.0}, // right elbow
            {16, 2, 16, 1.0}  // right wrist
        }};

const AlignedVector<std::tuple<int, Vector3, Matrix3, Vector3, Vector3>>
    RightArmJointConstInfo{
        //[0]: joint index
        //[1]: weight
        //[2]: reference rotation
        //[3]: lower bound
        //[4]: upper bound
        {1,
         {3, 3, 3},
         (Matrix3() << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0).finished(),
         {-0.50 * M_PI, -0.00 * M_PI, -0.25 * M_PI},
         {+0.25 * M_PI, +0.75 * M_PI, +0.25 * M_PI}}};

const AlignedVector<std::tuple<int, Vector3, Matrix3, Vector3, Vector3>>
    RightArmEulerAngleConstInfo{
        //[0]: joint index
        //[1]: weight
        //[2]: reference rotation
        //[3]: lower bound
        //[4]: upper bound
        {0,
         {3, 3, 3},
         (Matrix3() << 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0).finished(),
         {-0.46 * M_PI, -0.33 * M_PI, -0.40 * M_PI},
         {+0.67 * M_PI, +0.48 * M_PI, +0.60 * M_PI}}};

//----------------------------------------------------
// left leg
//----------------------------------------------------
const std::vector<std::array<int, 2>> LeftLegIndex[2] = {
    // body part index on SMPL/SMPLH
    {
        {0, 13},    // 0: (torso, middle hip, middle hip)
        {1, 14},    // 1: (left tigh, left hip, left hip)
        {4, 4 + 18} // 2: (left calf, left knee, left knee)
    },
    {
        {0, 0}, // 0: (torso, middle hip, middle hip)
        {1, 1}, // 1: (left tigh, left hip, left hip)
        {4, 2}  // 2: (left calf, left knee, left knee)
    }};

const std::vector<std::array<int, 2>> LeftLegKinematicsTree{
    //[0]: parent
    //[1]: child
    {0, 1}, // torso -> left tigh
    {1, 2}  // left tigh -> left calf
};

const std::vector<std::array<int, 3>> LeftLegPinholeCameraFactorInfo[2] = {
    {
        //[0]: 2D keypoint measurement index
        //[1]: rigid body index
        //[2]: keypoint index
        //------------------------------------
        {0, 0, 0},        // noise
        {1, 0, 1},        // left eye
        {2, 0, 2},        // right eye
        {3, 0, 3},        // left ear
        {4, 0, 4},        // right ear
        {5, 0, 16},       // left upper arm
        {6, 0, 17},       // right upper arm
        {11, 0, 14},      // left hip
        {12, 0, 15},      // right hip
        {17, 0, 11},      // head top
        {18, 0, 12 + 18}, // thorax
        {19, 0, 13},      // middle hip
        {26, 0, 12},      // chest
        {27, 0, 15 + 18}, // neck
                          //------------------------------------
        {13, 1, 4 + 18},  // left knee
        {15, 2, 7 + 18}   // left ankle
    },
    {
        //[0]: 2D keypoint measurement index
        //[1]: rigid body index
        //[2]: joint index on initial skeleton
        //------------------------------------
        //{5, 0, 11},   // left upper arm
        //{6, 0, 14},   // right upper arm
        //{17, 0, 10},  // head top
        //{18, 0, 8},   // thorax
        //{27, 0, 9},   // neck
        {11, 0, 1}, // left hip
        {12, 0, 4}, // right hip
        {19, 0, 0}, // middle hip
        {26, 0, 7}, // chest
                    //------------------------------------
        {13, 1, 2}, // left knee
        {15, 2, 3}  // left ankle
    }};

const AlignedVector<std::tuple<int, int, Vector3, Scalar>>
    LeftLegPOFFactorInfo[2] = {
        {
            //[0]: POF measurement index
            //[1]: body part index
            //[2]: unit directional vector
            //[3]: confidence
            //------------------------------------
            {10, 0, {1, 0, 0}, 1.0},  // thorax -> left upper arm
            {13, 0, {-1, 0, 0}, 1.0}, // thorax -> right upper arm
            {0, 0, {1, 0, 0}, 1.0},   // middle hip -> left hip
            {3, 0, {-1, 0, 0}, 1.0},  // middle hip -> right hip
                                      //------------------------------------
            {1, 1, {0, -1, 0}, 1},    // left hip -> left knee
            {2, 2, {0, -1, 0}, 1},    // left knee -> left ankle
        },
        {
            //[0]: POF measurement index
            //[1]: body part index
            //[2]: unit directional vector
            //[3]: confidence
            //------------------------------------
            //{10, 0, {1, 0, 0}, 1.0},   // thorax -> left upper arm
            //{13, 0, {-1, 0, 0}, 1.0},  // thorax -> right upper arm
            {0, 0, {1, 0, 0}, 1.0},  // middle hip -> left hip
            {3, 0, {-1, 0, 0}, 1.0}, // middle hip -> right hip
                                     //------------------------------------
            {1, 1, {0, -1, 0}, 1},   // left hip -> left knee
            {2, 2, {0, -1, 0}, 1},   // left knee -> left ankle
        }};

const std::vector<std::tuple<int, int, int, Scalar>>
    LeftLegDepthCameraFactorInfo[2] = {
        {
            //[0]: 3D keypoint measurement index
            //[1]: rigid body index
            //[2]: keypoint index
            //------------------------------------
            {0, 0, 13, 1.0},      // middle hip
            {1, 0, 14, 1.0},      // left hip
            {4, 0, 15, 1.0},      // right hip
            {7, 0, 12, 0.2},      // chest
            {8, 0, 12 + 18, 1.0}, // thorax
            {9, 0, 15 + 18, 0.2}, // neck
            {11, 0, 16, 1.0},     // left upper arm
            {14, 0, 17, 1.0},     // right upper arm
                                  //------------------------------------
            {2, 1, 4 + 18, 1.0},  // left knee
            {3, 2, 7 + 18, 1.0}   // left ankle
        },
        {
            //[0]: 3D keypoint measurement index
            //[1]: rigid body index
            //[2]: keypoint index
            //------------------------------------
            //{8, 0, 8, 1.0},    // thorax
            //{9, 0, 9, 0.2},    // neck
            //{11, 0, 11, 1.0},  // left upper arm
            //{14, 0, 14, 1.0},  // right upper arm
            {0, 0, 0, 1.0}, // middle hip
            {1, 0, 1, 1.0}, // left hip
            {4, 0, 4, 1.0}, // right hip
            {7, 0, 7, 0.2}, // chest
                            //------------------------------------
            {2, 1, 2, 1.0}, // left knee
            {3, 2, 3, 1.0}  // left ankle
        }};

const AlignedVector<std::tuple<int, Vector3, Matrix3, Vector3, Vector3>>
    LeftLegEulerAngleConstInfo{
        //[0]: joint index
        //[1]: weight
        //[2]: reference rotation
        //[3]: lower bound
        //[4]: upper bound
        {0,
         {10, 3, 3},
         (Matrix3() << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0).finished(),
         {-0.67 * M_PI, -0.25 * M_PI, -0.17 * M_PI},
         {+0.50 * M_PI, +0.48 * M_PI, +0.25 * M_PI}}};

//----------------------------------------------------
// right leg
//----------------------------------------------------
const std::vector<std::array<int, 2>> RightLegIndex[2] = {
    // body part index on SMPL/SMPLH
    {
        {0, 13},     // 0: (torso, middle hip, middle hip)
        {2, 15},     // 1: (right tigh, right hip, right hip)
        {5, 5 + 18}, // 2: (right calf, right knee, right knee)
    },
    {
        {0, 0}, // 0: (torso, middle hip, middle hip)
        {2, 4}, // 1: (right tigh, right hip, right hip)
        {5, 5}, // 2: (right calf, right knee, right knee)
    }};

const std::vector<std::array<int, 2>> RightLegKinematicsTree{
    //[0]: parent
    //[1]: child
    {0, 1}, // torso -> right tigh
    {1, 2}  // rightTigh -> right calf
};

const std::vector<std::array<int, 3>> RightLegPinholeCameraFactorInfo[2] = {
    {
        //[0]: 2D keypoint measurement index
        //[1]: rigid body index
        //[2]: keypoint index
        //------------------------------------
        {0, 0, 0},        // noise
        {1, 0, 1},        // left eye
        {2, 0, 2},        // right eye
        {3, 0, 3},        // left ear
        {4, 0, 4},        // right ear
        {5, 0, 16},       // left upper arm
        {6, 0, 17},       // right upper arm
        {11, 0, 14},      // left hip
        {12, 0, 15},      // right hip
        {17, 0, 11},      // head top
        {18, 0, 12 + 18}, // thorax
        {19, 0, 13},      // middle hip
        {26, 0, 12},      // chest
        {27, 0, 15 + 18}, // neck
                          //------------------------------------
        {14, 1, 5 + 18},  // right knee
        {16, 2, 8 + 18}   // right ankle
    },
    {
        //[0]: 2D keypoint measurement index
        //[1]: rigid body index
        //[2]: joint index on initial skeleton
        //------------------------------------
        //{5, 0, 11},   // left upper arm
        //{6, 0, 14},   // right upper arm
        //{17, 0, 10},  // head top
        //{18, 0, 8},   // thorax
        //{27, 0, 9},   // neck
        {11, 0, 1}, // left hip
        {12, 0, 4}, // right hip
        {19, 0, 0}, // middle hip
        {26, 0, 7}, // chest
                    //------------------------------------
        {14, 1, 5}, // right knee
        {16, 2, 6}  // right ankle
    }};

const AlignedVector<std::tuple<int, int, Vector3, Scalar>>
    RightLegPOFFactorInfo[2] = {
        {
            //[0]: POF measurement index
            //[1]: body part index
            //[2]: unit directional vector
            //[3]: confidence
            //------------------------------------
            {0, 0, {1, 0, 0}, 1.0},   // middle hip -> left hip
            {3, 0, {-1, 0, 0}, 1.0},  // middle hip -> right hip
            {10, 0, {1, 0, 0}, 1.0},  // thorax -> left upper arm
            {13, 0, {-1, 0, 0}, 1.0}, // thorax -> right upper arm
                                      //------------------------------------
            {4, 1, {0, -1, 0}, 1},    // right hip -> right knee
            {5, 2, {0, -1, 0}, 1},    // right knee -> right ankle
        },
        {
            //[0]: POF measurement index
            //[1]: body part index
            //[2]: unit directional vector
            //[3]: confidence
            //------------------------------------
            {0, 0, {1, 0, 0}, 1.0},  // middle hip -> left hip
            {3, 0, {-1, 0, 0}, 1.0}, // middle hip -> right hip
                                     //------------------------------------
            {4, 1, {0, -1, 0}, 1},   // right hip -> right knee
            {5, 2, {0, -1, 0}, 1},   // right knee -> right ankle
        }};

const std::vector<std::tuple<int, int, int, Scalar>>
    RightLegDepthCameraFactorInfo[2] = {
        {
            //[0]: 2D keypoint measurement index
            //[1]: rigid body index
            //[2]: keypoint index
            //------------------------------------
            {0, 0, 13, 1.0},      // middle hip
            {1, 0, 14, 1.0},      // left hip
            {4, 0, 15, 1.0},      // right hip
            {7, 0, 12, 0.2},      // chest
            {8, 0, 12 + 18, 1.0}, // thorax
            {9, 0, 15 + 18, 0.2}, // neck
            {11, 0, 16, 1.0},     // left upper arm
            {14, 0, 17, 1.0},     // right upper arm
                                  //------------------------------------
            {5, 1, 5 + 18, 1.0},  // right knee
            {6, 2, 8 + 18, 1.0}   // right ankle
        },
        {
            //[0]: 3D keypoint measurement index
            //[1]: rigid body index
            //[2]: keypoint index
            //------------------------------------
            //{8, 0, 8, 1.0},    // thorax
            //{9, 0, 9, 0.2},    // neck
            //{11, 0, 11, 1.0},  // left upper arm
            //{14, 0, 14, 1.0},  // right upper arm
            {0, 0, 0, 1.0}, // middle hip
            {1, 0, 1, 1.0}, // left hip
            {4, 0, 4, 1.0}, // right hip
            {7, 0, 7, 0.2}, // chest
                            //------------------------------------
            {5, 1, 5, 1.0}, // right knee
            {6, 2, 6, 1.0}  // right ankle
        }};

const AlignedVector<std::tuple<int, Vector3, Matrix3, Vector3, Vector3>>
    RightLegEulerAngleConstInfo{
        //[0]: joint index
        //[1]: weight
        //[2]: reference rotation
        //[3]: lower bound
        //[4]: upper bound
        {0,
         {10, 3, 3},
         (Matrix3() << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0).finished(),
         {-0.67 * M_PI, -0.48 * M_PI, -0.25 * M_PI},
         {+0.50 * M_PI, +0.25 * M_PI, +0.17 * M_PI}}};
} // namespace SMPL
} // namespace InitialInfo
} // namespace scope


<|start_filename|>C++/scope/factor/Factor.h<|end_filename|>
#pragma once

#include <memory>
#include <scope/base/Pose.h>
#include <vector>

#define SCOAT_FACTOR_EVALUATION_NEW                                            \
  virtual std::shared_ptr<Factor::Evaluation> newEvaluation() const override { \
    return std::make_shared<Evaluation>();                                     \
  }

#define SCOAT_FACTOR_LINEARIZATION_NEW                                         \
  virtual std::shared_ptr<Factor::Linearization> newLinearization()            \
      const override {                                                         \
    return std::make_shared<Linearization>();                                  \
  }

namespace scope {
class Factor : public std::enable_shared_from_this<Factor> {
protected:
  std::vector<int> mvPoses;
  std::vector<int> mvShapes;
  std::vector<int> mvJoints;
  std::vector<int> mvParams;

  std::string mName;

  int mIndex;

  bool mActive;

public:
  enum class Status { INVALID = -1, VALID = 0 };

  struct Evaluation : std::enable_shared_from_this<Evaluation> {
    Status status;

    VectorX error;

    Scalar f;

    Evaluation();

    virtual int reset();

    virtual int clear();
  };

  struct Linearization : std::enable_shared_from_this<Linearization> {
    Status status;

    // Jacobians[0]: linearization w.r.t. poses
    // Jacobians[1]: linearization w.r.t. shapes
    // Jacobians[2]: linearization w.r.t. joint states
    // Jacobians[3]: linearization w.r.t. implicit parameters
    AlignedVector<MatrixX> jacobians[4];

    Linearization();

    virtual int reset();

    virtual int clear();
  };

public:
  Factor(const std::vector<int> &poses, const std::vector<int> &shapes,
         const std::vector<int> &joints, const std::vector<int> &params,
         const std::string &name = "", int index = -1, bool active = true);

  virtual int evaluate(const AlignedVector<Pose> &poses,
                       const AlignedVector<VectorX> &shapes,
                       const AlignedVector<Matrix3> &joints,
                       const AlignedVector<VectorX> &params,
                       Evaluation &eval) const = 0;

  virtual int linearize(const AlignedVector<Pose> &poses,
                        const AlignedVector<VectorX> &shapes,
                        const AlignedVector<Matrix3> &joints,
                        const AlignedVector<VectorX> &params,
                        const Evaluation &eval, Linearization &lin) const = 0;

  virtual std::shared_ptr<Evaluation> newEvaluation() const {
    return std::make_shared<Evaluation>();
  }

  virtual std::shared_ptr<Linearization> newLinearization() const {
    return std::make_shared<Linearization>();
  }

  const std::vector<int> &getPoses() const { return mvPoses; }

  const std::vector<int> &getShapes() const { return mvShapes; }

  const std::vector<int> &getJoints() const { return mvJoints; }

  const std::vector<int> &getParams() const { return mvParams; }

  void setID(int index) { mIndex = index; }

  int getID() const { return mIndex; }

  const std::string &getName() const { return mName; }

  bool isActive() const { return mActive; }

  void activate() { mActive = true; }

  void deactivate() { mActive = false; }
};
} // namespace scope


<|start_filename|>C++/scope/initializer/ExtTorsoInitializer.cpp<|end_filename|>
#include <glog/logging.h>

#include <memory>
#include <scope/initializer/ExtTorsoInitializer.h>

namespace scope {
namespace Initializer {
ExtTorsoInitializer::ExtTorsoInitializer(const Options &options, const Vector3 &RelJointLocation)
    : Initializer(1, options), mRelJointLocation({RelJointLocation}) {}

int ExtTorsoInitializer::initialize(const Pose &T0,
                               const AlignedVector<Matrix3> &joints) const {
  assert(joints.size() == mNumJoints);

  if (joints.size() != mNumJoints) {
    LOG(ERROR) << "There should be " << mNumJoints << " joint states."
               << std::endl;

    exit(-1);
  }

  Vector3 xi;
  Matrix3 R;

  // Scale spine joint to feasbile range
  math::SO3::log(joints[0], xi);
  xi[0] = std::max(xi[0], SpineLowerBnd);
  xi[0] = std::min(xi[0], SpineUpperBnd);
  xi[1] = xi[2] = 0;
  math::SO3::exp(xi, R);

  mSpineAngle[0] = mSpineAngle[1] = xi[0];

  Initializer::initialize(T0, {R});

  return 0;
}

int ExtTorsoInitializer::initialize(const Pose &T0, const Scalar& SpineAngle) const {
  // Scale spine joint to feasbile range
  mSpineAngle[0] = SpineAngle;
  mSpineAngle[0] = std::max(mSpineAngle[0], SpineLowerBnd);
  mSpineAngle[0] = std::min(mSpineAngle[0], SpineUpperBnd);

  mSpineAngle[1] = mSpineAngle[0];

  Matrix3 R = Matrix3::Identity();
  R(1, 1) = cos(mSpineAngle[0]);
  R(2, 1) = sin(mSpineAngle[0]);
  R(2, 2) = R(1, 1);
  R(1, 2) = -R(2, 1);

  Initializer::initialize(T0, {R});

  return 0;
}

int ExtTorsoInitializer::FKintree(int n) const {
  auto &poses = mvPoses[n];
  auto &joints = mvJoints[n];

  poses[1].R.noalias() = poses[0].R * joints[0];
  poses[1].t = poses[0].t;
  poses[1].t.noalias() += poses[0].R * mRelJointLocation;

  return 0;
}

int ExtTorsoInitializer::DFKintree() const {
  const auto &poses = mvPoses[0];

  mB.head<3>() = poses[0].R.col(0);
  mB.tail<3>().noalias() = poses[1].t.cross(poses[0].R.col(0));

  return 0;
}

int ExtTorsoInitializer::updateGaussNewton() const {
  mH.resize(7, 7);

  mH.block<6, 1>(0, 6).noalias() = mvMxx[1] * mB;
  mH(6, 6) = mB.dot(mH.block<6, 1>(0, 6));
  mH(6, 6) += mvMuu[0](0, 0);

  mH.topLeftCorner<6, 6>() = mvMxx[0] + mvMxx[1];

  mH.block<1, 6>(6, 0).noalias() = mH.block<6, 1>(0, 6).transpose();

  mh.resize(7);

  mh[6] = mB.dot(mvmx[1]);
  mh[6] += mvmu[0][0];

  mh.head<6>() = mvmx[0] + mvmx[1];

  return 0;
}

int ExtTorsoInitializer::solveGaussNewton() const {
  mLambda = mH.diagonal() * (mDLambda - 1);
  mLambda.array() += mOptions.delta;
  mH.diagonal() += mLambda;

  mHchol.compute(mH.topLeftCorner<6, 6>());

  mS.noalias() = -mHchol.solve(mH.block<6, 1>(0, 6));
  ms.noalias() = -mHchol.solve(mh.head<6>());

  mA = mH(6, 6) + mH.block<6, 1>(0, 6).dot(mS);
  ma = mh[6] + mH.block<6, 1>(0, 6).dot(ms);

  mhGN.resize(7);

  // Guarantee mKneeJoint in [0, 0.78 * M_PI]
  mhGN[6] = -ma / mA;
  mhGN[6] = std::max(mhGN[6], SpineLowerBnd - mSpineAngle[0]);
  mhGN[6] = std::min(mhGN[6], SpineUpperBnd - mSpineAngle[0]);
  mhGN.head<6>() = ms;
  mhGN.head<6>().noalias() += mS * mhGN[6];

  mSquaredError = mh;
  mSquaredError += 0.5 * mH * mhGN;

  mE = mhGN.dot(mSquaredError);
  mSquaredError = mhGN.cwiseAbs2();
  mE -= 0.5 * mLambda.dot(mSquaredError);

  return 0;
}

int ExtTorsoInitializer::update(Scalar stepsize) const {
  assert(stepsize > 0);

  mDRootPoseChange = mhGN.head<6>() * stepsize;
  Pose::exp(mDRootPoseChange, mRootPoseChange);

  mvPoses[1][0].R.noalias() = mRootPoseChange.R * mvPoses[0][0].R;
  mvPoses[1][0].t = mRootPoseChange.t;
  mvPoses[1][0].t.noalias() += mRootPoseChange.R * mvPoses[0][0].t;

  mSpineAngle[1] = mSpineAngle[0] + stepsize * mhGN[6];

  mvJoints[1][0].setIdentity();
  mvJoints[1][0](1, 1) = std::cos(mSpineAngle[1]);
  mvJoints[1][0](2, 2) = mvJoints[1][0](1, 1);
  mvJoints[1][0](2, 1) = std::sin(mSpineAngle[1]);
  mvJoints[1][0](1, 2) = -mvJoints[1][0](2, 1);

  FKintree(1);

  return 0;
}

int ExtTorsoInitializer::accept() const {
  mSpineAngle[0] = mSpineAngle[1];

  return Initializer::accept();
}
} // namespace Initializer
} // namespace scope


<|start_filename|>C++/scope/initializer/Initializer.cpp<|end_filename|>
#include <glog/logging.h>

#include <scope/initializer/Initializer.h>
#include <memory>

namespace scope {
namespace Initializer {
Initializer::Initializer(int NumJoints, const Options &options)
    : mOptions(options),
      mStatus(Status::Uninitialized),
      mNumPoses(NumJoints + 1),
      mNumJoints(NumJoints) {
  assert(mNumJoints >= 0);

  mvFactors.resize(NumFactors);

  mvMxx.resize(mNumPoses);
  mvMuu.resize(mNumJoints);

  mvmx.resize(mNumPoses);
  mvmu.resize(mNumJoints);

  mvPoses[0].resize(mNumPoses);
  mvPoses[1].resize(mNumPoses);

  mvJoints[0].resize(mNumJoints);
  mvJoints[1].resize(mNumJoints);

  mvFactorEvals[0].resize(NumFactors);
  mvFactorEvals[1].resize(NumFactors);

  mvFactorLins.resize(NumFactors);

  mfobj[0] = 0;
  mfobj[1] = 0;

  mvFobj[0].resize(NumFactors);
  mvFobj[1].resize(NumFactors);

  mDLambda = 3;
}

int Initializer::initialize(const Pose &pose,
                            const AlignedVector<Matrix3> &joints) const {
  assert(joints.size() == mNumJoints);

  if (joints.size() != mNumJoints) {
    LOG(ERROR) << "There should be " << mNumJoints << " joint states."
               << std::endl;

    exit(-1);
  }

  mvPoses[0][0] = pose;
  mvJoints[0] = joints;

  FKintree(0);

  mvPoses[1] = mvPoses[0];
  mvJoints[1] = mvJoints[0];

  if (evaluate(0)) {
    mStatus = Status::Failed;

    exit(-1);
  }

  mResults = Results();
  mResults.fobjs.push_back(mfobj[0]);

  mStatus = Status::Initialized;

  return 0;
}

int Initializer::solve() const {
  mDLambda = 1.0;
  mStepsize = 1.0;

  for (int iter = 0; iter < mOptions.max_iterations; iter++) {
    optimize();

    if (mResults.expected_rel_cost_reduction.back() <
        mOptions.rel_lin_func_decrease_tol) {
      mStatus = Status::Converged;

      break;
    }
  }

  return 0;
}

int Initializer::updateFactorWeights(
    const Eigen::Matrix<Scalar, NumFactors, 1> &weights) {
  mOptions.weights = weights;

  if (mStatus != Status::Uninitialized) {
    mfobj[0] = 0.5 * mOptions.weights.dot(mvFobj[0]);
  }

  return 0;
}

int Initializer::optimize() const {
  if (mStatus == Status::Uninitialized) {
    LOG(WARNING) << "The optimization must be initialized." << std::endl;

    exit(-1);
  }

  if (mStatus == Status::Failed) {
    LOG(ERROR) << "The optimization failed." << std::endl;

    exit(-1);
  }

  if (linearize()) {
    exit(-1);
  }

  if (solveGaussNewton()) {
    exit(-1);
  }

  const auto &options = mOptions;

  auto &results = mResults;

  const auto &fobj = mfobj;

  mStatus = Status::Aborted;

  results.expected_rel_cost_reduction.push_back(-mE / fobj[0]);

  Scalar stepsize = 1.0;

  update(stepsize);

  if (evaluate(1)) {
    exit(-1);
  }

  if (fobj[1] < fobj[0]) {
    Scalar rho = (fobj[1] - fobj[0]) / mE;

    if (rho > 0.70) {
      mDLambda = std::max(mDLambda * mOptions.lm_lambda_decrease, Scalar(1));
    } else if (rho < 0.25) {
      mDLambda = std::min(mDLambda * mOptions.lm_lambda_increase,
                          mOptions.lm_lambda_max);
    }

    accept();

    mStepsize = std::max(0.2, mStepsize);

    mStatus = Status::Accepted;
  } else {
    stepsize = std::min(mStepsize, mOptions.stepsize_decrease_ratio);

    for (int k = 1; k < mOptions.max_inner_iters; k++) {
      update(stepsize);

      if (evaluate(1)) {
        exit(-1);
      }

      if (fobj[1] < fobj[0]) {
        accept();
        mStatus = Status::Accepted;

        mStepsize =
            std::min((Scalar)1.0, stepsize * mOptions.stepsize_increase_ratio);

        break;
      }

      stepsize *= mOptions.stepsize_decrease_ratio;
    }

    mDLambda = std::min(mDLambda * mOptions.lm_lambda_increase,
                        mOptions.lm_lambda_max);
  }

  return 0;
}

int Initializer::reset() {
  mvFactors.clear();
  mFactors.clear();

  mvFactorEvals[0].clear();
  mFactorEvals[0].clear();

  mvFactorEvals[1].clear();
  mFactorEvals[1].clear();

  mvFactorLins.clear();
  mFactorLins.clear();

  mvFactors.resize(NumFactors);

  mvFactorEvals[0].resize(NumFactors);
  mvFactorEvals[1].resize(NumFactors);
  mvFactorLins.resize(NumFactors);

  mvFobj[0].setZero();
  mvFobj[1].setZero();

  mfobj[0] = 0;
  mfobj[1] = 0;

  mStatus = Status::Uninitialized;

  return 0;
}

int Initializer::addPinholeCameraFactor(
    std::shared_ptr<PinholeCameraFactor> factor) {
  if (checkFactor(factor)) {
    exit(-1);
  }

  if (factor->getPose() >= mNumPoses) {
    LOG(ERROR) << "The pose must be valid." << std::endl;

    exit(-1);
  }

  const int index = FactorIndex::PinholeCamera;

  std::array<std::shared_ptr<Factor::Evaluation>, 2> evals;
  std::shared_ptr<Factor::Linearization> lin;

  if (addFactor(index, factor, evals, lin)) {
    exit(-1);
  }

  return factor->getID();
}

int Initializer::addPOFFactor(std::shared_ptr<POFFactor> factor) {
  if (checkFactor(factor)) {
    exit(-1);
  }

  if (factor->getPose() >= mNumPoses) {
    LOG(ERROR) << "The pose must be valid." << std::endl;

    exit(-1);
  }

  const int index = FactorIndex::POF;

  std::array<std::shared_ptr<Factor::Evaluation>, 2> evals;
  std::shared_ptr<Factor::Linearization> lin;

  if (addFactor(index, factor, evals, lin)) {
    exit(-1);
  }

  return factor->getID();
}

int Initializer::addDepthCameraFactor(
    std::shared_ptr<DepthCameraFactor> factor) {
  if (checkFactor(factor)) {
    exit(-1);
  }

  if (factor->getPose() >= mNumPoses) {
    LOG(ERROR) << "The pose must be valid." << std::endl;

    exit(-1);
  }

  const int index = FactorIndex::DepthCamera;

  std::array<std::shared_ptr<Factor::Evaluation>, 2> evals;
  std::shared_ptr<Factor::Linearization> lin;

  if (addFactor(index, factor, evals, lin)) {
    exit(-1);
  }

  return factor->getID();
}

int Initializer::addJointLimitFactor(std::shared_ptr<JointLimitFactor> factor) {
  if (checkFactor(factor)) {
    exit(-1);
  }

  if (factor->getJoint() >= mNumJoints) {
    LOG(ERROR) << "The joint must be valid." << std::endl;

    exit(-1);
  }

  const int index = FactorIndex::JointLimit;

  std::array<std::shared_ptr<Factor::Evaluation>, 2> evals;
  std::shared_ptr<Factor::Linearization> lin;

  if (addFactor(index, factor, evals, lin)) {
    exit(-1);
  }

  return factor->getID();
}

int Initializer::addPoseConstFactor(std::shared_ptr<PoseConstFactor> factor) {
  if (checkFactor(factor)) {
    exit(-1);
  }

  if (factor->getPose() >= mNumPoses) {
    LOG(ERROR) << "The pose must be valid." << std::endl;

    exit(-1);
  }

  const int index = FactorIndex::PoseConst;

  std::array<std::shared_ptr<Factor::Evaluation>, 2> evals;
  std::shared_ptr<Factor::Linearization> lin;

  if (addFactor(index, factor, evals, lin)) {
    exit(-1);
  }

  return factor->getID();
}

int Initializer::addJointConstFactor(std::shared_ptr<JointConstFactor> factor) {
  if (checkFactor(factor)) {
    exit(-1);
  }

  assert(factor->getJoint() < mNumJoints);

  if (factor->getJoint() >= mNumJoints) {
    LOG(ERROR) << "The joint must be valid." << std::endl;

    exit(-1);
  }

  const int index = FactorIndex::JointConst;

  std::array<std::shared_ptr<Factor::Evaluation>, 2> evals;
  std::shared_ptr<Factor::Linearization> lin;

  if (addFactor(index, factor, evals, lin)) {
    exit(-1);
  }

  return factor->getID();
}

int Initializer::addPoseFactor(std::shared_ptr<PoseFactor> factor) {
  if (checkFactor(factor)) {
    exit(-1);
  }

  if (factor->getPose() >= mNumPoses) {
    LOG(ERROR) << "The pose must be valid." << std::endl;

    exit(-1);
  }

  const int index = FactorIndex::Pose;

  std::array<std::shared_ptr<Factor::Evaluation>, 2> evals;
  std::shared_ptr<Factor::Linearization> lin;

  if (addFactor(index, factor, evals, lin)) {
    exit(-1);
  }

  return factor->getID();
}

int Initializer::addJointFactor(std::shared_ptr<JointFactor> factor) {
  if (checkFactor(factor)) {
    exit(-1);
  }

  assert(factor->getJoint() < mNumJoints);

  if (factor->getJoint() >= mNumJoints) {
    LOG(ERROR) << "The joint must be valid." << std::endl;

    exit(-1);
  }

  const int index = FactorIndex::Joint;

  std::array<std::shared_ptr<Factor::Evaluation>, 2> evals;
  std::shared_ptr<Factor::Linearization> lin;

  if (addFactor(index, factor, evals, lin)) {
    exit(-1);
  }

  return factor->getID();
}

int Initializer::accept() const {
  mfobj[0] = mfobj[1];

  mvPoses[0].swap(mvPoses[1]);
  mvJoints[0].swap(mvJoints[1]);

  mvFobj[0].swap(mvFobj[1]);

  mvFactorEvals[0].swap(mvFactorEvals[1]);
  mFactorEvals[0].swap(mFactorEvals[1]);

  return 0;
}

int Initializer::solveGaussNewton() const {
  mLambda = mH.diagonal() * (mDLambda - 1);
  mLambda.array() += mOptions.delta;
  mH.diagonal() += mLambda;

  mHchol.compute(mH);
  mhGN = -mHchol.solve(mh);

  mE = 0.5 * mhGN.dot(mh);
  mSquaredError = mhGN.cwiseAbs2();
  mE -= 0.5 * mLambda.dot(mSquaredError);

  return 0;
}

int Initializer::evaluate(int n) const {
  assert(n >= 0 && n <= 1);

  const auto &poses = mvPoses[n];
  const auto &joints = mvJoints[n];

  auto &fobj = mfobj[n];
  auto &Fobj = mvFobj[n];
  auto &Evals = mFactorEvals[n];
  auto &FactorEvals = mvFactorEvals[n];

  Fobj.setZero();

  for (int i = 0; i < mFactors.size(); i++) {
    const auto &factor = mFactors[i];
    auto &eval = *Evals[i];

    factor->evaluate(poses, joints, eval);
  }

  for (int k = 0; k < NumFactors; k++) {
    const auto &factors = mvFactors[k];
    const auto &evals = FactorEvals[k];

    for (int i = 0; i < factors.size(); i++) {
      const auto &factor = factors[i];

      auto &eval = *evals[i];

      assert(eval.status == Factor::Status::VALID);

      if (eval.status != Factor::Status::VALID) {
        LOG(ERROR) << "Failed to evaluate the factor." << std::endl;

        mStatus = Status::Failed;

        exit(-1);
      }

      Fobj[k] += eval.f;
    }
  }

  fobj = 0.5 * mOptions.weights.dot(Fobj);

  return 0;
}

int Initializer::linearize() const {
  const auto &poses = mvPoses[0];
  const auto &joints = mvJoints[0];

  DFKintree();

  const auto &Evals = mFactorEvals[0];
  auto &Lins = mFactorLins;

  for (int i = 0; i < mFactors.size(); i++) {
    const auto &factor = mFactors[i];
    const auto &eval = *Evals[i];
    auto &lin = *Lins[i];

    assert(eval.status == Factor::Status::VALID);

    factor->linearize(poses, joints, eval, lin);

    assert(lin.status == Factor::Status::VALID);
  }

  for (const auto &lin : Lins) {
    if (lin->status != Factor::Status::VALID) {
      LOG(ERROR) << "Failed to linearize the factor." << std::endl;

      mStatus = Status::Failed;

      exit(-1);
    }
  }

  mvMxx.assign(mNumPoses, Matrix6::Zero());
  mvMuu.assign(mNumJoints, Matrix3::Zero());

  mvmx.assign(mNumPoses, Vector6::Zero());
  mvmu.assign(mNumJoints, Vector3::Zero());

  updatePinholeCameraFactorGaussNewton();
  updatePOFFactorGaussNewton();
  updateDepthCameraFactorGaussNewton();
  updateJointLimitFactorGaussNewton();
  updatePoseConstFactorGaussNewton();
  updateJointConstFactorGaussNewton();
  updatePoseFactorGaussNewton();
  updateJointFactorGaussNewton();

  updateGaussNewton();

  return 0;
}

int Initializer::updatePinholeCameraFactorGaussNewton() const {
  const auto &factors = mvFactors[FactorIndex::PinholeCamera];
  const auto &evals = mvFactorEvals[0][FactorIndex::PinholeCamera];
  const auto &lins = mvFactorLins[FactorIndex::PinholeCamera];

  const auto &weight = mOptions.weights[FactorIndex::PinholeCamera];

  for (int i = 0; i < factors.size(); i++) {
    const auto &factor =
        std::dynamic_pointer_cast<const PinholeCameraFactor>(factors[i]);

    const auto &lin =
        std::dynamic_pointer_cast<const PinholeCameraFactor::Linearization>(
            lins[i]);

    const auto &J = lin->jacobian;
    const auto &g = lin->g;

    const int &pose = factor->getPose();

    mvMxx[pose].noalias() += weight * J.transpose() * J;
    mvmx[pose] += weight * g;
  }

  return 0;
}

int Initializer::updatePOFFactorGaussNewton() const {
  const auto &factors = mvFactors[FactorIndex::POF];
  const auto &evals = mvFactorEvals[0][FactorIndex::POF];
  const auto &lins = mvFactorLins[FactorIndex::POF];

  const auto &weight = mOptions.weights[FactorIndex::POF];

  for (int i = 0; i < factors.size(); i++) {
    const auto &factor = std::dynamic_pointer_cast<const POFFactor>(factors[i]);

    const auto &lin =
        std::dynamic_pointer_cast<const POFFactor::Linearization>(lins[i]);

    const auto &JR = lin->jacobian;

    const int &pose = factor->getPose();

    mvMxx[pose].topLeftCorner<3, 3>().noalias() += weight * JR.transpose() * JR;
    mvmx[pose].head<3>().noalias() +=
        weight * lin->scaledError * JR.transpose();
  }

  return 0;
}

int Initializer::updateDepthCameraFactorGaussNewton() const {
  const auto &factors = mvFactors[FactorIndex::DepthCamera];
  const auto &evals = mvFactorEvals[0][FactorIndex::DepthCamera];
  const auto &lins = mvFactorLins[FactorIndex::DepthCamera];

  const auto &weight = mOptions.weights[FactorIndex::DepthCamera];

  for (int i = 0; i < factors.size(); i++) {
    const auto &factor =
        std::dynamic_pointer_cast<const DepthCameraFactor>(factors[i]);

    const auto &lin =
        std::dynamic_pointer_cast<const DepthCameraFactor::Linearization>(
            lins[i]);

    const auto &J = lin->jacobian;
    const auto &g = lin->g;

    const int &pose = factor->getPose();

    mvMxx[pose].noalias() += weight * J.transpose() * J;
    mvmx[pose].noalias() += weight * lin->g;
  }

  return 0;
}

int Initializer::updateJointLimitFactorGaussNewton() const {
  const auto &factors = mvFactors[FactorIndex::JointLimit];
  const auto &evals = mvFactorEvals[0][FactorIndex::JointLimit];
  const auto &lins = mvFactorLins[FactorIndex::JointLimit];

  const auto &weight = mOptions.weights[FactorIndex::JointLimit];

  for (int i = 0; i < factors.size(); i++) {
    const auto &factor =
        std::dynamic_pointer_cast<const JointLimitFactor>(factors[i]);

    const auto &lin =
        std::dynamic_pointer_cast<const JointLimitFactor::Linearization>(
            lins[i]);
    const auto &eval = evals[i];

    const auto &JJoint = lin->jacobian;
    const auto &Error = Eigen::Map<const Vector6>(eval->error.data());

    const int &joint = factor->getJoint();

    mvMuu[joint].noalias() += weight * JJoint.transpose() * JJoint;
    mvmu[joint].noalias() += weight * JJoint.transpose() * Error;
  }

  return 0;
}

int Initializer::updatePoseConstFactorGaussNewton() const {
  const auto &factors = mvFactors[FactorIndex::PoseConst];
  const auto &evals = mvFactorEvals[0][FactorIndex::PoseConst];
  const auto &lins = mvFactorLins[FactorIndex::PoseConst];

  const auto &weight = mOptions.weights[FactorIndex::PoseConst];

  for (int i = 0; i < factors.size(); i++) {
    const auto &factor =
        std::dynamic_pointer_cast<const PoseConstFactor>(factors[i]);

    const auto &lin =
        std::dynamic_pointer_cast<const PoseConstFactor::Linearization>(
            lins[i]);
    const auto &eval = evals[i];

    const auto &JPose = lin->jacobian;
    const auto &Error = Eigen::Map<const Vector6>(eval->error.data());

    const int &pose = factor->getPose();

    mvMxx[pose].noalias() += weight * JPose.transpose() * JPose;
    mvmx[pose].noalias() += weight * JPose.transpose() * Error;
  }

  return 0;
}

int Initializer::updateJointConstFactorGaussNewton() const {
  const auto &factors = mvFactors[FactorIndex::JointConst];
  const auto &evals = mvFactorEvals[0][FactorIndex::JointConst];
  const auto &lins = mvFactorLins[FactorIndex::JointConst];

  const auto &weight = mOptions.weights[FactorIndex::JointConst];

  for (int i = 0; i < factors.size(); i++) {
    const auto &factor =
        std::dynamic_pointer_cast<const JointConstFactor>(factors[i]);

    const auto &lin =
        std::dynamic_pointer_cast<const JointConstFactor::Linearization>(
            lins[i]);
    const auto &eval = evals[i];

    const auto &JJoint = lin->jacobian;
    const auto &Error = Eigen::Map<const Vector3>(eval->error.data());

    const int &joint = factor->getJoint();

    mvMuu[joint].noalias() += weight * JJoint.transpose() * JJoint;
    mvmu[joint].noalias() += weight * JJoint.transpose() * Error;
  }

  return 0;
}

int Initializer::updatePoseFactorGaussNewton() const {
  const auto &factors = mvFactors[FactorIndex::Pose];
  const auto &evals = mvFactorEvals[0][FactorIndex::Pose];
  const auto &lins = mvFactorLins[FactorIndex::Pose];

  const auto &weight = mOptions.weights[FactorIndex::Pose];

  for (int i = 0; i < factors.size(); i++) {
    const auto &factor =
        std::dynamic_pointer_cast<const PoseFactor>(factors[i]);

    const auto &lin =
        std::dynamic_pointer_cast<const PoseFactor::Linearization>(lins[i]);
    const auto &eval = evals[i];

    const auto &JPose = lin->jacobian;
    const auto &Error = Eigen::Map<const Vector6>(eval->error.data());

    const int &pose = factor->getPose();

    mvMxx[pose].noalias() += weight * JPose.transpose() * JPose;
    mvmx[pose].noalias() += weight * JPose.transpose() * Error;
  }

  return 0;
}

int Initializer::updateJointFactorGaussNewton() const {
  const auto &factors = mvFactors[FactorIndex::Joint];
  const auto &evals = mvFactorEvals[0][FactorIndex::Joint];
  const auto &lins = mvFactorLins[FactorIndex::Joint];

  const auto &weight = mOptions.weights[FactorIndex::Joint];

  for (int i = 0; i < factors.size(); i++) {
    const auto &factor =
        std::dynamic_pointer_cast<const JointFactor>(factors[i]);

    const auto &lin =
        std::dynamic_pointer_cast<const JointFactor::Linearization>(lins[i]);
    const auto &eval = evals[i];

    const auto &JJoint = lin->jacobian;
    const auto &Error = Eigen::Map<const Vector3>(eval->error.data());

    const int &joint = factor->getJoint();

    mvMuu[joint].noalias() += weight * JJoint.transpose() * JJoint;
    mvmu[joint].noalias() += weight * JJoint.transpose() * Error;
  }

  return 0;
}

int Initializer::addFactor(
    int index, std::shared_ptr<Factor> factor,
    std::array<std::shared_ptr<Factor::Evaluation>, 2> &evals,
    std::shared_ptr<Factor::Linearization> &lin) {
  assert(index >= 0 && index < NumFactors);

  if (index < 0 || index >= NumFactors) {
    LOG(ERROR) << "The factor index must be valid." << std::endl;

    exit(-1);
  }

  if (checkFactor(factor)) {
    exit(-1);
  }

  evals[0] = factor->newEvaluation();
  evals[1] = factor->newEvaluation();
  lin = factor->newLinearization();

  mFactors.push_back(factor);
  mFactorEvals[0].push_back(evals[0]);
  mFactorEvals[1].push_back(evals[1]);
  mFactorLins.push_back(lin);

  mvFactors[index].push_back(factor);
  mvFactorEvals[0][index].push_back(evals[0]);
  mvFactorEvals[1][index].push_back(evals[1]);
  mvFactorLins[index].push_back(lin);

  if (mStatus != Status::Uninitialized) {
    auto &eval = *evals[0];

    factor->evaluate(mvPoses[0], mvJoints[0], eval);

    if (eval.status != Factor::Status::VALID) {
      LOG(ERROR) << "Failed to evaluate the factor." << std::endl;

      mvFactors[index].pop_back();
      mvFactorEvals[0][index].pop_back();
      mvFactorEvals[1][index].pop_back();
      mvFactorLins[index].pop_back();

      exit(-1);
    }

    mvFobj[0][index] += eval.f;
    mfobj[0] += 0.5 * mOptions.weights[index] * eval.f;
  }

  factor->setID(mFactors.size() - 1);

  assert(factor.get() == mFactors.back().get());
  assert(factor->getID() == mFactors.size() - 1);

  return 0;
}

int Initializer::checkFactor(std::shared_ptr<const Factor> factor) {
  if (factor == nullptr) {
    LOG(WARNING) << "The factor should not be null." << std::endl;

    exit(-1);
  }

  return 0;
}
}  // namespace Initializer
}  // namespace scope


<|start_filename|>C++/scope/optimizer/SMPLify.cpp<|end_filename|>
#include <memory>

#include <glog/logging.h>

#include <scope/factor/Factors.h>
#include <scope/optimizer/SMPLify.h>

#define FACTORIZE(i)                                                           \
  {                                                                            \
    const auto &link = links[i];                                               \
                                                                               \
    assert(i == link.id());                                                    \
                                                                               \
    const int ii = i - 1;                                                      \
                                                                               \
    assert(ii == link.joint());                                                \
                                                                               \
    const auto &children = link.children();                                    \
                                                                               \
    mvH[ii] = mvM[i];                                                          \
                                                                               \
    for (const auto &j : children) {                                           \
      const auto &jj = links[j].joint();                                       \
                                                                               \
      mvH[ii]                                                                  \
          .template topLeftCorner<DPoseSize + DParamSize,                      \
                                  DPoseSize + DParamSize>() +=                 \
          mvH[jj]                                                              \
              .template topLeftCorner<DPoseSize + DParamSize,                  \
                                      DPoseSize + DParamSize>();               \
    }                                                                          \
                                                                               \
    mvHxB[ii].noalias() = mvH[ii].template leftCols<6>() * mvB[ii];            \
                                                                               \
    mvH[ii].template block<VertexParamSize, DPoseSize + DParamSize>(           \
        DVertexParamOffset, DPoseOffset) +=                                    \
        mvHxB[ii]                                                              \
            .template block<DPoseSize + DParamSize, VertexParamSize>(          \
                DPoseOffset, 0)                                                \
            .transpose();                                                      \
                                                                               \
    mvH[ii].template bottomRows<DJointSize>() +=                               \
        mvHxB[ii].template rightCols<DJointSize>().transpose();                \
                                                                               \
    mvH[ii].template block<DParamSize + DJointSize, VertexParamSize>(          \
        DParamOffset, DVertexParamOffset) +=                                   \
        mvHxB[ii].template block<DParamSize + DJointSize, VertexParamSize>(    \
            DParamOffset, 0);                                                  \
                                                                               \
    mvH[ii]                                                                    \
        .template block<VertexParamSize + 3, VertexParamSize>(                 \
            DVertexParamOffset, DVertexParamOffset)                            \
        .noalias() +=                                                          \
        mvHxB[ii].template middleRows<3>(3).transpose() * mvBp[ii];            \
                                                                               \
    mvH[ii].template block<DJointSize, DJointSize>(DPoseSize + DParamSize,     \
                                                   DPoseSize + DParamSize) +=  \
        mvHxB[ii].template block<DJointSize, DJointSize>(DJointOffset,         \
                                                         VertexParamSize);     \
    mvH[ii]                                                                    \
        .template block<DJointSize, DJointSize>(DJointOffset, DJointOffset)    \
        .noalias() += mvBu[ii].transpose() *                                   \
                      mvHxB[ii].template block<DPoseSize, DJointSize>(         \
                          DPoseOffset, VertexParamSize);                       \
                                                                               \
    if (mOptions.method == Method::LM) {                                       \
      mvH[ii].diagonal().template segment<DJointSize>(DJointOffset).array() *= \
          mDLambda;                                                            \
      mLambda.template segment<DJointSize>(6 + DParamSize + 3 * ii) =          \
          mvH[ii].diagonal().template segment<DJointSize>(DJointOffset);       \
    }                                                                          \
                                                                               \
    mvH[ii].diagonal().template segment<DJointSize>(DJointOffset).array() +=   \
        mOptions.delta;                                                        \
                                                                               \
    mvHuuInv[ii] = mvH[ii]                                                     \
                       .template bottomRightCorner<DJointSize, DJointSize>()   \
                       .inverse();                                             \
                                                                               \
    mvKuxp[ii].template leftCols<DPoseSize + DParamSize>().noalias() =         \
        -mvHuuInv[ii] *                                                        \
        mvH[ii]                                                                \
            .template bottomLeftCorner<DJointSize, DPoseSize + DParamSize>();  \
                                                                               \
    mvH[ii]                                                                    \
        .template topLeftCorner<DPoseSize + DParamSize,                        \
                                DPoseSize + DParamSize>()                      \
        .noalias() +=                                                          \
        mvH[ii]                                                                \
            .template bottomLeftCorner<DJointSize, DPoseSize + DParamSize>()   \
            .transpose() *                                                     \
        mvKuxp[ii];                                                            \
  }

#define BACKWARD(i)                                                            \
  {                                                                            \
    const auto &link = links[i];                                               \
                                                                               \
    assert(i == link.id());                                                    \
                                                                               \
    const int ii = i - 1;                                                      \
                                                                               \
    assert(ii == link.joint());                                                \
                                                                               \
    const auto &children = link.children();                                    \
                                                                               \
    mvhx[ii] = mvmx[i];                                                        \
    mvhp[ii] = mvmp[i];                                                        \
    mvhu[ii] = mvmu[i];                                                        \
    mvE[ii] = 0;                                                               \
                                                                               \
    for (const auto &j : children) {                                           \
      const auto &jj = links[j].joint();                                       \
                                                                               \
      mvhx[ii] += mvhx[jj];                                                    \
      mvhp[ii] += mvhp[jj];                                                    \
      mvE[ii] += mvE[jj];                                                      \
    }                                                                          \
                                                                               \
    mvhp[ii].template segment<VertexParamSize>(VertexParamOffset).noalias() += \
        mvBp[ii].transpose() * mvhx[ii].template bottomRows<3>();              \
    mvhu[ii].noalias() += mvBu[ii].transpose() * mvhx[ii];                     \
                                                                               \
    mvku[ii].noalias() = -mvHuuInv[ii] * mvhu[ii];                             \
                                                                               \
    mvE[ii] += 0.5 * mvhu[ii].dot(mvku[ii]);                                   \
                                                                               \
    mvhx[ii].noalias() +=                                                      \
        mvH[ii]                                                                \
            .template block<DJointSize, DPoseSize>(DJointOffset, DPoseOffset)  \
            .transpose() *                                                     \
        mvku[ii];                                                              \
    mvhp[ii].noalias() += mvH[ii]                                              \
                              .template block<DJointSize, DParamSize>(         \
                                  DJointOffset, DParamOffset)                  \
                              .transpose() *                                   \
                          mvku[ii];                                            \
  }

#define BACKWARD_PASS(i)                                                       \
  FACTORIZE(i)                                                                 \
  BACKWARD(i)

#define FORWARD_PASS(i)                                                        \
  {                                                                            \
    const auto &link = links[i];                                               \
                                                                               \
    const auto &parent = link.parent();                                        \
    const auto &ii = i - 1;                                                    \
                                                                               \
    mvJointGN[ii] = mvku[ii];                                                  \
                                                                               \
    mvJointGN[ii].noalias() +=                                                 \
        mvKuxp[ii].template leftCols<6>() * mvPoseGN[parent];                  \
                                                                               \
    mvPoseGN[i] = mvPoseGN[parent];                                            \
    mvPoseGN[i].noalias() += mvBu[ii] * mvJointGN[ii];                         \
    mvPoseGN[i].template tail<3>().noalias() +=                                \
        mvBp[ii] * mParamGN.template segment<VertexParamSize>(                 \
                       DVertexParamOffset - DParamOffset);                     \
  }

namespace scope {
namespace Optimizer {
template <int P, bool CamOpt>
SMPLify<P, CamOpt>::SMPLify(const Model &smpl, const Options &options)
    : Optimization(std::make_shared<Model>(smpl), options) {
  mSMPL = std::dynamic_pointer_cast<const Model>(this->mModel);

  this->setupModelInfo();

  assert(Optimization::NumPoses == Optimization::NumJoints + 1);

  setupParameterInfo();
  this->setupFactors();
  this->setupOptimization();
}

template <int P, bool CamOpt> int SMPLify<P, CamOpt>::setupParameterInfo() {
  return 0;
}

template <int P, bool CamOpt> int SMPLify<P, CamOpt>::solveGaussNewton() const {
#if 0
#pragma omp parallel
  {
#pragma omp single nowait
    {
#pragma omp task
      { this->backwardPass(mSMPL->mvLeftArm); }

#pragma omp task
      { this->backwardPass(mSMPL->mvRightArm); }

#pragma omp task
      { this->backwardPass(mSMPL->mvHead); }

#pragma omp task
      { this->backwardPass(mSMPL->mvLeftLeg); }

#pragma omp task
      { this->backwardPass(mSMPL->mvRightLeg); }
    }
#pragma omp taskwait
  }

  this->backwardPass(mSMPL->mvBack);
  factorizeN();
  this->backwardN();

  this->forwardPass(mSMPL->mvBack);
#pragma omp parallel
  {
#pragma omp single nowait
    {
#pragma omp task
      { this->forwardPass(mSMPL->mvLeftArm); }

#pragma omp task
      { this->forwardPass(mSMPL->mvRightArm); }

#pragma omp task
      { this->forwardPass(mSMPL->mvHead); }

#pragma omp task
      { this->forwardPass(mSMPL->mvLeftLeg); }

#pragma omp task
      { this->forwardPass(mSMPL->mvRightLeg); }
    }
#pragma omp taskwait
  }
#else
#if 0
  const auto& links = mSMPL->getLinks();

#pragma omp parallel sections
  {
#pragma omp section
    {
      // head
      BACKWARD_PASS(15);
      BACKWARD_PASS(12);
    }

#pragma omp section
    {
      // left arm
      BACKWARD_PASS(22);
      BACKWARD_PASS(20);
      BACKWARD_PASS(18);
      BACKWARD_PASS(16);
      BACKWARD_PASS(13);
    }

#pragma omp section
    {
      // right arm
      BACKWARD_PASS(23);
      BACKWARD_PASS(21);
      BACKWARD_PASS(19);
      BACKWARD_PASS(17);
      BACKWARD_PASS(14);
    }
  }

#pragma omp parallel sections
  {
#pragma omp section
    {  // left leg
      BACKWARD_PASS(10);
      BACKWARD_PASS(7);
      BACKWARD_PASS(4);
      BACKWARD_PASS(1);
    }

#pragma omp section
    {  // right leg
      BACKWARD_PASS(11);
      BACKWARD_PASS(8);
      BACKWARD_PASS(5);
      BACKWARD_PASS(2);
    }

#pragma omp section
    {
      // torso
      BACKWARD_PASS(9);
      BACKWARD_PASS(6);
      BACKWARD_PASS(3);
    }
  }

  //this->linearize(0);
  this->factorizeN();
  this->backwardN();

#pragma omp parallel sections
  {
#pragma omp section
    {
      // torso
      FORWARD_PASS(3);
      FORWARD_PASS(6);
      FORWARD_PASS(9);
    }

#pragma omp section
    {  // left leg
      FORWARD_PASS(1);
      FORWARD_PASS(4);
      FORWARD_PASS(7);
      FORWARD_PASS(10);
    }

#pragma omp section
    {  // right leg
      FORWARD_PASS(2);
      FORWARD_PASS(5);
      FORWARD_PASS(8);
      FORWARD_PASS(11);
    }
  }

#pragma omp parallel sections
  {
#pragma omp section
    {
      // head
      FORWARD_PASS(12);
      FORWARD_PASS(15);
    }

#pragma omp section
    {
      // left arm
      FORWARD_PASS(13);
      FORWARD_PASS(16);
      FORWARD_PASS(18);
      FORWARD_PASS(20);
      FORWARD_PASS(22);
    }

#pragma omp section
    {
      // right arm
      FORWARD_PASS(14);
      FORWARD_PASS(17);
      FORWARD_PASS(19);
      FORWARD_PASS(21);
      FORWARD_PASS(23);
    }
  }
#else
#pragma omp parallel sections
  {
#pragma omp section
    { this->backwardPass(mSMPL->mvHead); }

#pragma omp section
    { this->backwardPass(mSMPL->mvLeftArm); }

#pragma omp section
    { this->backwardPass(mSMPL->mvRightArm); }
  }

#pragma omp parallel sections
  {
#pragma omp section
    { this->backwardPass(mSMPL->mvLeftLeg); }

#pragma omp section
    { this->backwardPass(mSMPL->mvRightLeg); }

#pragma omp section
    { this->backwardPass(mSMPL->mvBack); }
  }

  this->factorizeN();
  this->backwardN();

#pragma omp parallel sections
  {
#pragma omp section
    { this->forwardPass(mSMPL->mvLeftLeg); }

#pragma omp section
    { this->forwardPass(mSMPL->mvRightLeg); }

#pragma omp section
    { this->forwardPass(mSMPL->mvBack); }
  }

#pragma omp parallel sections
  {
#pragma omp section
    { this->forwardPass(mSMPL->mvHead); }

#pragma omp section
    { this->forwardPass(mSMPL->mvLeftArm); }

#pragma omp section
    { this->forwardPass(mSMPL->mvRightArm); }
  }
#endif
#endif

  return 0;
}

template class SMPLify<10, false>;
template class SMPLify<11, false>;
} // namespace Optimizer
} // namespace scope


<|start_filename|>C++/scope/initializer/factor/JointFactor.h<|end_filename|>
#pragma once

#include <scope/initializer/factor/Factor.h>

namespace scope {
namespace Initializer {
class JointFactor : public Factor {
public:
  struct Evaluation : public Factor::Evaluation {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    Matrix3 errorR;
    Vector3 xi;
  };

  struct Linearization : public Factor::Linearization {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    Matrix3 jacobian;
    Matrix3 dexpinv;
    Matrix3 dexpinvR;
  };

public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  SCOAT_INITIAL_EVALUATION_NEW
  SCOAT_INITIAL_LINEARIZATION_NEW

  JointFactor(int pose, const Matrix3 &sqrtCov, const Matrix3 &mean,
              const std::string &name = "", int index = -1);

  virtual int evaluate(const AlignedVector<Pose> &poses,
                       const AlignedVector<Matrix3> &joints,
                       Factor::Evaluation &base_eval) const override;

  virtual int linearize(const AlignedVector<Pose> &poses,
                        const AlignedVector<Matrix3> &joints,
                        const Factor::Evaluation &base_eval,
                        Factor::Linearization &base_lin) const override;

  int getJoint() const { return mJoint; }

protected:
  int mJoint;

  Matrix3 mSqrtCov;
  Matrix3 mMean;
};
} // namespace Initializer
} // namespace scope


<|start_filename|>C++/scope/initializer/factor/Factor.h<|end_filename|>
#pragma once

#include <memory>
#include <scope/base/Pose.h>
#include <vector>

#define SCOAT_INITIAL_EVALUATION_NEW                                           \
  virtual std::shared_ptr<Initializer::Factor::Evaluation> newEvaluation()     \
      const override {                                                         \
    return std::make_shared<Evaluation>();                                     \
  }

#define SCOAT_INITIAL_LINEARIZATION_NEW                                        \
  virtual std::shared_ptr<Initializer::Factor::Linearization>                  \
  newLinearization() const override {                                          \
    return std::make_shared<Linearization>();                                  \
  }

namespace scope {
namespace Initializer {
class Factor : public std::enable_shared_from_this<Factor> {
protected:
  std::string mName;

  int mIndex;

public:
  enum class Status { INVALID = -1, VALID = 0 };

  struct Evaluation : std::enable_shared_from_this<Evaluation> {
    Status status;

    VectorX error;

    Scalar f;

    Evaluation();

    virtual int reset();

    virtual int clear();
  };

  struct Linearization : std::enable_shared_from_this<Linearization> {
    Status status;

    Linearization();

    virtual int reset();

    virtual int clear();
  };

public:
  Factor(const std::string &name = "", int index = -1);

  virtual int evaluate(const AlignedVector<Pose> &poses,
                       const AlignedVector<Matrix3> &joints,
                       Evaluation &eval) const = 0;

  virtual int linearize(const AlignedVector<Pose> &poses,
                        const AlignedVector<Matrix3> &joints,
                        const Evaluation &eval, Linearization &lin) const = 0;

  virtual std::shared_ptr<Evaluation> newEvaluation() const {
    return std::make_shared<Evaluation>();
  }

  virtual std::shared_ptr<Linearization> newLinearization() const {
    return std::make_shared<Linearization>();
  }

  void setID(int index) { mIndex = index; }

  int getID() const { return mIndex; }

  const std::string &getName() const { return mName; }
};
} // namespace Initializer
} // namespace scope


<|start_filename|>C++/scope/factor/VertexExtraPinholeCameraFactor.cpp<|end_filename|>
#include <glog/logging.h>

#include <scope/factor/VertexExtraPinholeCameraFactor.h>

namespace scope {
VertexExtraPinholeCameraFactor::VertexExtraPinholeCameraFactor(
    int pose, int vParam, const Matrix3X &vDirs, const Vector3 &v,
    const Scalar &sigma, const Scalar &eps, const Pose &extraCamPose,
    const Vector2 &measurement, const Scalar &confidence,
    const std::string &name, int index, bool active)
    : VertexPinholeCameraFactor(pose, vParam, vDirs, v, sigma, eps, measurement,
                                confidence, name, index, active),
      ExtraPinholeCamera(extraCamPose) {}

int VertexExtraPinholeCameraFactor::evaluate(
    const AlignedVector<Pose> &poses, const AlignedVector<VectorX> &shapes,
    const AlignedVector<Matrix3> &joints, const AlignedVector<VectorX> &params,
    Factor::Evaluation &base_eval) const {
  auto &eval = dynamic_cast<Evaluation &>(base_eval);
  eval.clear();

  const auto &pose = mvPoses[0];
  assert(pose >= 0 && pose < poses.size());

  if (pose < 0 || pose >= poses.size()) {
    LOG(ERROR) << "The pose must be valid." << std::endl;

    exit(-1);
  }

  const auto &vertexParam = mvParams[0];
  assert(vertexParam >= 0 && vertexParam < params.size());

  if (vertexParam < 0 || vertexParam > params.size()) {
    LOG(ERROR) << "The parameter must be valid." << std::endl;

    exit(-1);
  }

  evaluate(poses[pose], params[vertexParam], mMeasurement, eval);

  eval.status = Status::VALID;

  return 0;
}

int VertexExtraPinholeCameraFactor::linearize(
    const AlignedVector<Pose> &poses, const AlignedVector<VectorX> &shapes,
    const AlignedVector<Matrix3> &joints, const AlignedVector<VectorX> &params,
    const Factor::Evaluation &base_eval,
    Factor::Linearization &base_lin) const {
  auto &eval = dynamic_cast<const Evaluation &>(base_eval);
  auto &lin = dynamic_cast<Linearization &>(base_lin);

  lin.clear();

  assert(eval.status == Status::VALID);

  if (eval.status != Status::VALID) {
    LOG(ERROR) << "The evaluation must be valid." << std::endl;

    exit(-1);
  }

  const auto &pose = mvPoses[0];
  assert(pose >= 0 && pose < poses.size());

  if (pose < 0 || pose >= poses.size()) {
    LOG(ERROR) << "The pose must be valid." << std::endl;

    exit(-1);
  }

  const auto &vertexParam = mvParams[0];
  assert(vertexParam >= 0 && vertexParam < params.size());

  if (vertexParam < 0 || vertexParam > params.size()) {
    LOG(ERROR) << "The parameter must be valid." << std::endl;

    exit(-1);
  }

  lin.jacobians[0].resize(1);
  lin.jacobians[3].resize(1);

  linearize(poses[pose], params[vertexParam], mMeasurement, eval, lin);

  lin.status = Status::VALID;

  return 0;
}

int VertexExtraPinholeCameraFactor::evaluate(const Pose &pose,
                                             const VectorX &vertexParam,
                                             const Vector2 &measurement,
                                             Evaluation &eval) const {
  eval.vertex = mv;
  eval.vertex.noalias() += mvDirs * vertexParam;

  eval.pCam = pose.t;
  eval.pCam.noalias() += pose.R * eval.vertex;

  eval.pExtraCam = mExtraCamPose.t;
  eval.pExtraCam.noalias() += mExtraCamPose.R * eval.pCam;

  eval.point2D = eval.pExtraCam.head<2>() / eval.pExtraCam[2];
  eval.error = eval.point2D - measurement;
  eval.squaredErrorNorm = eval.error.squaredNorm();

#if not ROBUST_PINHOLE
  eval.f = mCon * eval.squaredErrorNorm;
#else
  eval.f = mSigmaCon * eval.squaredErrorNorm / (mSigma + eval.squaredErrorNorm);
#endif

  return 0;
}

int VertexExtraPinholeCameraFactor::linearize(const Pose &pose,
                                              const VectorX &vertexParam,
                                              const Vector2 &measurement,
                                              const Evaluation &eval,
                                              Linearization &lin) const {
  assert(lin.jacobians[0].size() == 1);
  assert(lin.jacobians[3].size() >= 1);

  Scalar zinv = 1.0 / eval.pExtraCam[2];

  lin.JPoint(0, 0) = zinv;
  lin.JPoint(0, 1) = 0;
  lin.JPoint(0, 2) = -eval.point2D[0] * zinv;
  lin.JPoint(1, 0) = 0;
  lin.JPoint(1, 1) = zinv;
  lin.JPoint(1, 2) = -eval.point2D[1] * zinv;

  auto &JPose = lin.jacobians[0][0];

  JPose.resize(2, 6);

  Eigen::Map<Matrix<2, 3>> JR(JPose.data());
  Eigen::Map<Matrix<2, 3>> Jt(JPose.data() + 6);

  Jt.noalias() = lin.JPoint * mExtraCamPose.R;

  JR.row(0) = eval.pCam.cross(Jt.row(0));
  JR.row(1) = eval.pCam.cross(Jt.row(1));

  auto &JVertex = lin.jacobians[3][0];

  lin.D.noalias() = Jt * pose.R;
  JVertex.noalias() = lin.D * mvDirs;

  auto &gPose = lin.gPose;
  auto &gVertex = lin.gVertex;

  gPose.noalias() = JPose.transpose() * eval.error;
  gVertex.noalias() = JVertex.transpose() * eval.error;

#if not ROBUST_PINHOLE
  JPose *= mSqrtCon;
  JVertex *= mSqrtCon;

  gPose *= mCon;
  gVertex *= mCon;
#else
  preLinearizeRobustKernel(eval, lin);

  JPose.noalias() -= lin.scaledError * gPose.transpose();
  JVertex.noalias() -= lin.scaledError * gVertex.transpose();

  JPose *= lin.sqrtDrho;
  JVertex *= lin.sqrtDrho;

  gPose *= lin.Drho;
  gVertex *= lin.Drho;
#endif

  return 0;
}
} // namespace scope


<|start_filename|>C++/scope/initializer/ArmRefiner.h<|end_filename|>
#pragma once

#include <memory>
#include <vector>

#include <scope/initializer/Initializer.h>

namespace scope {
namespace Initializer {

class ArmRefiner : public Initializer {
public:
  std::array<Vector3, 2> mRelJointLocations;

  mutable Vector6 mDJointChange;
  mutable std::array<Matrix3, 2> mvJointChange;

  mutable std::array<Matrix63, 2> mvB;

public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  ArmRefiner(const Options &options,
             const std::array<Vector3, 2> &RelJointLocations);

  ArmRefiner(const ArmRefiner &) = delete;

  ArmRefiner &operator=(const ArmRefiner &) = delete;

public:
  virtual int FKintree(int n) const override;
  virtual int DFKintree() const override;

  virtual int updateGaussNewton() const override;
  virtual int update(Scalar stepsize) const override;
};

} // namespace Initializer
} // namespace scope


<|start_filename|>C++/scope/initializer/factor/PoseConstFactor.cpp<|end_filename|>
#include <glog/logging.h>

#include <scope/initializer/factor/PoseConstFactor.h>
#include <scope/math/SO3.h>

namespace scope {
namespace Initializer {
PoseConstFactor::PoseConstFactor(int pose, const Vector6 &weight,
                                 const Pose &mean, const Vector6 &lbnd,
                                 const Vector6 &ubnd, const std::string &name,
                                 int index)
    : Factor(name, index), mPose(pose), mWeight(weight), mMean(mean),
      mLowerBnd(lbnd), mUpperBnd(ubnd) {
  assert((mean.R.col(0).cross(mean.R.col(1)) - mean.R.col(2)).norm() < 1e-5 &&
         "mean.R must be a rotational matrix");
  assert((lbnd.cwiseMin(ubnd) - lbnd).norm() < 1e-7);
  assert((ubnd.cwiseMax(lbnd) - ubnd).norm() < 1e-7);
}

int PoseConstFactor::evaluate(const AlignedVector<Pose> &poses,
                              const AlignedVector<Matrix3> &joints,
                              Factor::Evaluation &base_eval) const {
  auto &eval = dynamic_cast<Evaluation &>(base_eval);
  eval.clear();

  const auto &i = mPose;

  assert(i >= 0 && i < poses.size());

  if (i >= poses.size()) {
    LOG(ERROR) << "The pose must be valid." << std::endl;

    exit(-1);
  }

  auto &pose = poses[i];

  eval.errorR.noalias() = mMean.R.transpose() * pose.R;

  eval.error.resize(6);

  Eigen::Map<Vector3> omega(eval.xi.data());
  math::SO3::log(eval.errorR, omega);
  eval.xi.tail<3>() = pose.t - mMean.t;

  eval.errorL = eval.xi - mLowerBnd;
  eval.errorL = eval.errorL.cwiseMin(mBnd);

  eval.errorU = mUpperBnd - eval.xi;
  eval.errorU = eval.errorU.cwiseMin(mBnd);

  eval.errorLInv = mScale * eval.errorL.cwiseInverse();
  eval.errorUInv = mScale * eval.errorU.cwiseInverse();

  eval.expL = eval.errorLInv.array().exp();
  eval.expU = eval.errorUInv.array().exp();

  eval.error = eval.errorL.cwiseProduct(eval.expL);
  eval.error += eval.errorU.cwiseProduct(eval.expU);

  eval.error.noalias() = mWeight.asDiagonal() * eval.error;

  eval.f = eval.error.squaredNorm();

  eval.status = Status::VALID;

  return 0;
}

int PoseConstFactor::linearize(const AlignedVector<Pose> &poses,
                               const AlignedVector<Matrix3> &joints,
                               const Factor::Evaluation &base_eval,
                               Factor::Linearization &base_lin) const {
  auto &eval = dynamic_cast<const Evaluation &>(base_eval);
  auto &lin = dynamic_cast<Linearization &>(base_lin);

  lin.clear();

  const auto &i = mPose;

  assert(i >= 0 && i < poses.size());

  if (i >= poses.size()) {
    LOG(ERROR) << "The pose must be valid." << std::endl;

    exit(-1);
  }

  assert(eval.status == Status::VALID);

  if (eval.status != Status::VALID) {
    LOG(ERROR) << "The evaluation must be valid." << std::endl;

    exit(-1);
  }

  auto &pose = poses[i];

  auto &J = lin.jacobian;

  math::SO3::dexpinv(eval.xi.head<3>(), lin.dexpinv);

  lin.Derror = eval.expL;
  lin.Derror -= eval.expL.cwiseProduct(eval.errorLInv);
  lin.Derror -= eval.expU;
  lin.Derror += eval.expU.cwiseProduct(eval.errorUInv);

  J.topRightCorner<3, 3>().setZero();

  const auto &t = poses[i].t;

  Eigen::Map<Matrix63> JR(J.data());
  Eigen::Map<Matrix63> Jt(J.data() + 18);

  JR.topRows<3>().noalias() = lin.dexpinv * mMean.R.transpose();
  JR(3, 0) = 0;
  JR(3, 1) = t[2];
  JR(3, 2) = -t[1];
  JR(4, 0) = -t[2];
  JR(4, 1) = 0;
  JR(4, 2) = t[0];
  JR(5, 0) = t[1];
  JR(5, 1) = -t[0];
  JR(5, 2) = 0;
  JR.noalias() = lin.Derror.asDiagonal() * JR;
  JR.noalias() = mWeight.asDiagonal() * JR;

  Jt.topRows<3>().setZero();
  Jt.bottomRows<3>() = lin.Derror.tail<3>().asDiagonal();
  Jt.bottomRows<3>() =
      mWeight.tail<3>().cwiseProduct(lin.Derror.tail<3>()).asDiagonal();

  lin.status = Status::VALID;

  return 0;
}
} // namespace Initializer
} // namespace scope


<|start_filename|>C++/scope/factor/EulerAngleConstFactor.cpp<|end_filename|>
#include <glog/logging.h>

#include <scope/factor/EulerAngleConstFactor.h>
#include <scope/math/SO3.h>

namespace scope {
EulerAngleConstFactor::EulerAngleConstFactor(
    int joint, const Vector3 &weight, const Matrix3 &mean, const Vector3 &lbnd,
    const Vector3 &ubnd, const std::string &name, int index, bool active)
    : JointConstFactor(joint, weight, mean, lbnd, ubnd, name, index, active) {}

int EulerAngleConstFactor::evaluate(const AlignedVector<Pose> &poses,
                                    const AlignedVector<VectorX> &shapes,
                                    const AlignedVector<Matrix3> &joints,
                                    const AlignedVector<VectorX> &params,
                                    Factor::Evaluation &base_eval) const {
  auto &eval = dynamic_cast<Evaluation &>(base_eval);
  eval.clear();

  const auto &i = mvJoints[0];
  assert(i >= 0 && i < joints.size());

  if (i >= joints.size()) {
    LOG(ERROR) << "The joint must be valid." << std::endl;

    exit(-1);
  }

  const auto &R = joints[i];

  eval.errorR.noalias() = mMean.transpose() * R;

  math::SO3::Rot2XYZ(eval.errorR, eval.xi);

  eval.errorL = eval.xi - mLowerBnd;
  eval.errorL = eval.errorL.cwiseMin(mBnd);

  eval.errorU = mUpperBnd - eval.xi;
  eval.errorU = eval.errorU.cwiseMin(mBnd);

  eval.errorLInv = mScale * eval.errorL.cwiseInverse();
  eval.errorUInv = mScale * eval.errorU.cwiseInverse();

  eval.expL = eval.errorLInv.array().exp();
  eval.expU = eval.errorUInv.array().exp();

  eval.error = eval.errorL.cwiseProduct(eval.expL);
  eval.error += eval.errorU.cwiseProduct(eval.expU);

  eval.error.noalias() = mWeight.asDiagonal() * eval.error;

  eval.f = eval.error.squaredNorm();

  eval.status = Status::VALID;

  return 0;
}

int EulerAngleConstFactor::linearize(const AlignedVector<Pose> &poses,
                                     const AlignedVector<VectorX> &shapes,
                                     const AlignedVector<Matrix3> &joints,
                                     const AlignedVector<VectorX> &params,
                                     const Factor::Evaluation &base_eval,
                                     Factor::Linearization &base_lin) const {
  auto &eval = dynamic_cast<const Evaluation &>(base_eval);
  auto &lin = dynamic_cast<Linearization &>(base_lin);

  lin.clear();

  assert(eval.status == Status::VALID);

  if (eval.status != Status::VALID) {
    LOG(ERROR) << "The evaluation must be valid." << std::endl;

    exit(-1);
  }

  const auto &i = mvJoints[0];
  assert(i >= 0 && i < joints.size());

  if (i < 0 || i >= joints.size()) {
    LOG(ERROR) << "The joint must be valid." << std::endl;

    exit(-1);
  }

  lin.jacobians[2].resize(1);
  lin.jacobians[2][0].resize(3, 3);
  Eigen::Map<Matrix3> J(lin.jacobians[2][0].data());

  lin.DF.setIdentity();
  lin.DF(1, 1) = cos(eval.xi[0]);
  lin.DF(2, 1) = sin(eval.xi[0]);
  lin.DF.col(2) = eval.errorR.col(2);

  lin.H.noalias() = lin.DF.transpose() * lin.DF;
  lin.H.diagonal().array() += 1e-4;

  lin.D.noalias() = lin.H.inverse() * lin.DF.transpose();

  J.noalias() = lin.D * mMean.transpose();

  lin.Derror = eval.expL;
  lin.Derror -= eval.expL.cwiseProduct(eval.errorLInv);
  lin.Derror -= eval.expU;
  lin.Derror += eval.expU.cwiseProduct(eval.errorUInv);

  J.noalias() = lin.Derror.asDiagonal() * J;
  J.noalias() = mWeight.asDiagonal() * J;

  lin.status = Status::VALID;

  return 0;
}
}  // namespace scope


<|start_filename|>C++/scope/utils/Stopwatch.h<|end_filename|>
/** This small file provides a pair of convenience functions that are useful for
 * measuring elapsed computation times.
 *
 * Copyright (C) 2017 - 2018 by <NAME> (<EMAIL>)
 */

#pragma once

#include <chrono>

namespace scope {
namespace Stopwatch {

/** This function returns a chrono::time_point struct encoding the time at which
 * it is called.*/
inline std::chrono::time_point<std::chrono::high_resolution_clock> tick() {
  return std::chrono::high_resolution_clock::now();
}

/** When this function is called with a chrono::time_point struct returned by
 * tick(), it returns the elapsed time (in seconds) between the calls to tick()
 * and tock().*/
inline double
tock(const std::chrono::time_point<std::chrono::high_resolution_clock>
         &tick_time) {
  auto counter = std::chrono::high_resolution_clock::now() - tick_time;
  return std::chrono::duration_cast<std::chrono::nanoseconds>(counter).count() /
         1000000000.0;
}
} // namespace Stopwatch
} // namespace scope


<|start_filename|>C++/scope/factor/ScaledPOFFactor.h<|end_filename|>
#pragma once

#include <scope/factor/POFFactor.h>

namespace scope {
class ScaledPOFFactor : public POFFactor {
 public:
  struct Evaluation : public POFFactor::Evaluation {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    // vertex positions
    Vector3 vertex;
    Vector3 D;
    Scalar DNorm;
  };

  struct Linearization : public POFFactor::Linearization {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    Matrix3 DF;
    Matrix3 DFR;

    VectorX gVertex;
  };

 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  SCOAT_FACTOR_EVALUATION_NEW
  SCOAT_FACTOR_LINEARIZATION_NEW

  ScaledPOFFactor(int pose, int vParam, const std::array<Matrix3X, 2> &VDirs,
                  const std::array<Vector3, 2> &V, const Scalar &sigma,
                  const Scalar &eps, const Vector3 &measurement,
                  const Scalar &confidence = 1.0, const std::string &name = "",
                  int index = -1, bool active = true);

  virtual int evaluate(const AlignedVector<Pose> &poses,
                       const AlignedVector<VectorX> &shapes,
                       const AlignedVector<Matrix3> &joints,
                       const AlignedVector<VectorX> &params,
                       Factor::Evaluation &base_eval) const override;

  virtual int linearize(const AlignedVector<Pose> &poses,
                        const AlignedVector<VectorX> &shapes,
                        const AlignedVector<Matrix3> &joints,
                        const AlignedVector<VectorX> &params,
                        const Factor::Evaluation &base_eval,
                        Factor::Linearization &base_lin) const override;

 protected:
  Matrix3X mVDirs;
  Vector3 mV;

 protected:
  virtual int evaluate(const Pose &pose, const VectorX &vertexParam,
                       const Vector3 &measurement, Evaluation &eval) const;

  virtual int linearize(const Pose &pos, const VectorX &vertexParam,
                        const Vector3 &measurement, const Evaluation &eval,
                        Linearization &lin) const;
};
}  // namespace scope


<|start_filename|>C++/scope/factor/Factor.cpp<|end_filename|>
#include <scope/factor/Factor.h>

namespace scope {
Factor::Evaluation::Evaluation() : status(Status::INVALID) {}

int Factor::Evaluation::reset() {
  status = Status::INVALID;

  return 0;
}

int Factor::Evaluation::clear() {
  reset();

  error.resize(0);

  return 0;
}

Factor::Linearization::Linearization() : status(Status::INVALID) {}

int Factor::Linearization::reset() {
  status = Status::INVALID;

  jacobians[0].clear();
  jacobians[1].clear();
  jacobians[2].clear();
  jacobians[3].clear();

  return 0;
}

int Factor::Linearization::clear() {
  reset();

  jacobians[0].clear();
  jacobians[1].clear();
  jacobians[2].clear();
  jacobians[3].clear();

  return 0;
}

Factor::Factor(const std::vector<int> &poses, const std::vector<int> &shapes,
               const std::vector<int> &joints, const std::vector<int> &params,
               const std::string &name, int index, bool active)
    : mvPoses(poses), mvShapes(shapes), mvJoints(joints), mvParams(params),
      mIndex(index), mName(name), mActive(active) {}

} // namespace scope


<|start_filename|>C++/scope/initializer/ArmRefiner.cpp<|end_filename|>
#include <glog/logging.h>

#include <memory>
#include <scope/initializer/ArmRefiner.h>

namespace scope {
namespace Initializer {
ArmRefiner::ArmRefiner(const Options &options,
                       const std::array<Vector3, 2> &RelJointLocations)
    : Initializer(2, options), mRelJointLocations(RelJointLocations) {}

int ArmRefiner::FKintree(int n) const {
  auto &poses = mvPoses[n];
  auto &joints = mvJoints[n];

  poses[1].R.noalias() = poses[0].R * joints[0];
  poses[1].t = poses[0].t;
  poses[1].t.noalias() += poses[0].R * mRelJointLocations[0];

  poses[2].R.noalias() = poses[1].R * joints[1];
  poses[2].t = poses[1].t;
  poses[2].t.noalias() += poses[1].R * mRelJointLocations[1];

  return 0;
}

int ArmRefiner::DFKintree() const {
  const auto &poses = mvPoses[0];

  mvB[0].topRows<3>() = poses[0].R;
  mvB[0].block<3, 1>(3, 0) = poses[1].t.cross(poses[0].R.col(0));
  mvB[0].block<3, 1>(3, 1) = poses[1].t.cross(poses[0].R.col(1));
  mvB[0].block<3, 1>(3, 2) = poses[1].t.cross(poses[0].R.col(2));

  mvB[1].topRows<3>() = poses[1].R;
  mvB[1].block<3, 1>(3, 0) = poses[2].t.cross(poses[1].R.col(0));
  mvB[1].block<3, 1>(3, 1) = poses[2].t.cross(poses[1].R.col(1));
  mvB[1].block<3, 1>(3, 2) = poses[2].t.cross(poses[1].R.col(2));

  return 0;
}

int ArmRefiner::updateGaussNewton() const {
  Matrix6 temp1;
  Matrix63 temp2;

  temp1 = mvMxx[1] + mvMxx[2];
  temp2.noalias() = temp1 * mvB[0];

  mH.resize(6, 6);
  mH.topLeftCorner<3, 3>().noalias() = mvB[0].transpose() * temp2;

  temp2.noalias() = mvMxx[2] * mvB[1];

  mH.topRightCorner<3, 3>().noalias() = mvB[0].transpose() * temp2;
  mH.bottomLeftCorner<3, 3>().noalias() = mH.topRightCorner<3, 3>().transpose();

  mH.bottomRightCorner<3, 3>().noalias() = mvB[1].transpose() * temp2;

  mH.topLeftCorner<3, 3>().noalias() += mvMuu[0];
  mH.bottomRightCorner<3, 3>().noalias() += mvMuu[1];

  Vector6 temp3;
  temp3 = mvmx[1] + mvmx[2];

  mh.resize(6);
  mh.head<3>() = mvmu[0];
  mh.tail<3>() = mvmu[1];

  mh.head<3>().noalias() += mvB[0].transpose() * temp3;
  mh.tail<3>().noalias() += mvB[1].transpose() * mvmx[2];

  return 0;
}

int ArmRefiner::update(Scalar stepsize) const {
  assert(stepsize > 0);

  mDJointChange = mhGN * stepsize;

  math::SO3::exp(mDJointChange.head<3>(), mvJointChange[0]);
  math::SO3::exp(mDJointChange.tail<3>(), mvJointChange[1]);

  mvJoints[1][0].noalias() = mvJointChange[0] * mvJoints[0][0];
  mvJoints[1][1].noalias() = mvJointChange[1] * mvJoints[0][1];

  FKintree(1);

  return 0;
}

} // namespace Initializer
} // namespace scope


<|start_filename|>C++/scope/initializer/factor/JointFactor.cpp<|end_filename|>
#include <glog/logging.h>

#include <scope/initializer/factor/JointFactor.h>
#include <scope/math/SO3.h>

namespace scope {
namespace Initializer {
JointFactor::JointFactor(int joint, const Matrix3 &sqrtCov, const Matrix3 &mean,
                         const std::string &name, int index)
    : Factor(name, index), mJoint(joint), mSqrtCov(sqrtCov), mMean(mean) {
  assert((mMean.col(0).cross(mMean.col(1)) - mMean.col(2)).norm() < 1e-5 &&
         "mean.R must be a rotational matrix");
}

int JointFactor::evaluate(const AlignedVector<Pose> &poses,
                          const AlignedVector<Matrix3> &joints,
                          Factor::Evaluation &base_eval) const {
  auto &eval = dynamic_cast<Evaluation &>(base_eval);
  eval.clear();

  const auto &i = mJoint;

  assert(i >= 0 && i < joints.size());

  if (i >= joints.size()) {
    LOG(ERROR) << "The pose must be valid." << std::endl;

    exit(-1);
  }

  const auto &R = joints[i];

  eval.errorR.noalias() = mMean.transpose() * R;

  math::SO3::log(eval.errorR, eval.xi);
  eval.error.noalias() = mSqrtCov * eval.xi;

  eval.f = eval.error.squaredNorm();

  eval.status = Status::VALID;

  return 0;
}

int JointFactor::linearize(const AlignedVector<Pose> &poses,
                           const AlignedVector<Matrix3> &joints,
                           const Factor::Evaluation &base_eval,
                           Factor::Linearization &base_lin) const {
  auto &eval = dynamic_cast<const Evaluation &>(base_eval);
  auto &lin = dynamic_cast<Linearization &>(base_lin);

  lin.clear();

  const auto &i = mJoint;

  assert(i >= 0 && i < joints.size());

  if (i >= joints.size()) {
    LOG(ERROR) << "The pose must be valid." << std::endl;

    exit(-1);
  }

  assert(eval.status == Status::VALID);

  if (eval.status != Status::VALID) {
    LOG(ERROR) << "The evaluation must be valid." << std::endl;

    exit(-1);
  }

  auto &J = lin.jacobian;

  math::SO3::dexpinv(eval.xi, lin.dexpinv);
  lin.dexpinvR.noalias() = lin.dexpinv * mMean.transpose();

  J.noalias() = mSqrtCov * lin.dexpinvR;

  lin.status = Status::VALID;

  return 0;
}
}  // namespace Initializer
}  // namespace scope


<|start_filename|>C++/scope/model/SMPL.cpp<|end_filename|>
#include <scope/model/SMPL.h>

namespace scope {
template <int P>
SMPL<P>::SMPL(const MatrixX& RelJDirs, const VectorX& RelJ,
              const MatrixX& vDirs, const VectorX& v)
    : Model<23, P, 6890>(HumanModel::SMPL, RelJDirs, RelJ, vDirs, v) {
  setKinematicsTree();
}

template <int P>
const std::vector<int>& SMPL<P>::getKinematicsTree() const {
  static const std::vector<int> kintree = {
      -1,  // 0
      0,   // 1
      0,   // 2
      0,   // 3
      1,   // 4
      2,   // 5
      3,   // 6
      4,   // 7
      5,   // 8
      6,   // 9
      7,   // 10
      8,   // 11
      9,   // 12
      9,   // 13
      9,   // 14
      12,  // 15
      13,  // 16
      14,  // 17
      16,  // 18
      17,  // 19
      18,  // 20
      19,  // 21
      20,  // 22
      21   // 23
  };

  return kintree;
}

template <int P>
int SMPL<P>::setKinematicsTree() {
  auto& links = this->mvLinks;

  links.resize(this->NumJoints + 1);

  static const std::string names[] = {
      "pelvis",         // 0
      "leftThigh",      // 1
      "rightThigh",     // 2
      "spine",          // 3
      "leftCalf",       // 4
      "rightCalf",      // 5
      "spine1",         // 6
      "leftFoot",       // 7
      "rightFoot",      // 8
      "spine2",         // 9
      "leftToes",       // 10
      "rightToes",      // 11
      "neck",           // 12
      "leftShoulder",   // 13
      "rightShoulder",  // 14
      "head",           // 15
      "leftUpperArm",   // 16
      "rightUpperArm",  // 17
      "leftForeArm",    // 18
      "rightForeArm",   // 19
      "leftHand",       // 20
      "rightHand",      // 21
      "leftFingers",    // 22
      "rightFingers"    // 23
  };

  const auto& parents = getKinematicsTree();

  for (int i = 0; i <= this->NumJoints; i++) {
    assert(parents[i] < i);

    links[i].mId = i;
    links[i].mName = names[i];
    links[i].mParent = parents[i];
    links[i].mJoint = i - 1;
  }

  for (int i = 1; i <= this->NumJoints; i++) {
    links[links[i].mParent].mvChildren.push_back(i);
  }

  return 0;
}

template <int P>
const std::vector<int> SMPL<P>::mvLeftLeg = std::vector<int>{1, 4, 7, 10};
template <int P>
const std::vector<int> SMPL<P>::mvRightLeg = std::vector<int>{2, 5, 8, 11};
template <int P>
const std::vector<int> SMPL<P>::mvHead = std::vector<int>{12, 15};
template <int P>
const std::vector<int> SMPL<P>::mvLeftArm =
    std::vector<int>{13, 16, 18, 20, 22};
template <int P>
const std::vector<int> SMPL<P>::mvRightArm =
    std::vector<int>{14, 17, 19, 21, 23};
template <int P>
const std::vector<int> SMPL<P>::mvBack = std::vector<int>{3, 6, 9};

// const std::vector<int> SMPL::mvLeftLeg = std::vector<int>{10, 7, 4, 1};
// const std::vector<int> SMPL::mvRightLeg = std::vector<int>{11, 8, 5, 2};
// const std::vector<int> SMPL::mvHead = std::vector<int>{15, 12};
// const std::vector<int> SMPL::mvLeftArm = std::vector<int>{22, 20, 18, 16,
// 13}; const std::vector<int> SMPL::mvRightArm = std::vector<int>{23, 21, 19,
// 17, 14}; const std::vector<int> SMPL::mvBack = std::vector<int>{9, 6, 3};

template class SMPL<10>;
template class SMPL<11>;
}  // namespace scope


<|start_filename|>C++/scope/factor/FullJointPinholeCameraFactor.h<|end_filename|>
#pragma once

#include <memory>

#include <scope/factor/JointPinholeCameraFactor.h>

namespace scope {
class FullJointPinholeCameraFactor : public JointPinholeCameraFactor {
public:
  struct Evaluation : public JointPinholeCameraFactor::Evaluation {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    Vector2 camMeasurement;
  };

  struct Linearization : public JointPinholeCameraFactor::Linearization {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    Vector4 gCam;
  };

public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  SCOAT_FACTOR_EVALUATION_NEW
  SCOAT_FACTOR_LINEARIZATION_NEW

  FullJointPinholeCameraFactor(int pose, int camParam, const Scalar &sigma,
                               const Scalar &eps, const Vector2 &measurement,
                               const Scalar &confidence = 1.0,
                               const std::string &name = "", int index = -1,
                               bool active = true);

  virtual int evaluate(const AlignedVector<Pose> &poses,
                       const AlignedVector<VectorX> &shapes,
                       const AlignedVector<Matrix3> &joints,
                       const AlignedVector<VectorX> &params,
                       Factor::Evaluation &base_eval) const override;

  virtual int linearize(const AlignedVector<Pose> &poses,
                        const AlignedVector<VectorX> &shapes,
                        const AlignedVector<Matrix3> &joints,
                        const AlignedVector<VectorX> &params,
                        const Factor::Evaluation &base_eval,
                        Factor::Linearization &base_lin) const override;

protected:
  // camParam: the inverse of the camera intrinsic parameters
  virtual int evaluate(const Pose &pose, const VectorX &camParam,
                       const Vector2 &measurement, Evaluation &eval) const;

  virtual int linearize(const Pose &pose, const VectorX &camParam,
                        const Vector2 &measurement, const Evaluation &eval,
                        Linearization &lin) const;
};

} // namespace scope


<|start_filename|>C++/scope/factor/VertexExtraPinholeCameraFactor.h<|end_filename|>
#pragma once

#include <scope/factor/ExtraPinholeCamera.h>
#include <scope/factor/VertexPinholeCameraFactor.h>

namespace scope {
class VertexExtraPinholeCameraFactor : public VertexPinholeCameraFactor,
                                       public ExtraPinholeCamera {
public:
  struct Evaluation : public VertexPinholeCameraFactor::Evaluation,
                      public ExtraPinholeCamera::Evaluation {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  };

  struct Linearization : public VertexPinholeCameraFactor::Linearization,
                         public ExtraPinholeCamera::Linearization {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    Vector3 gPoseR;
    Vector3 gJoint;
    VectorX gParam;
  };

public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  SCOAT_FACTOR_EVALUATION_NEW
  SCOAT_FACTOR_LINEARIZATION_NEW

  VertexExtraPinholeCameraFactor(int pose, int vParam, const Matrix3X &vDirs,
                                 const Vector3 &v, const Scalar &sigma,
                                 const Scalar &eps, const Pose &extraCamPose,
                                 const Vector2 &measurement,
                                 const Scalar &confidence = 1.0,
                                 const std::string &name = "", int index = -1,
                                 bool active = true);

  virtual int evaluate(const AlignedVector<Pose> &poses,
                       const AlignedVector<VectorX> &shapes,
                       const AlignedVector<Matrix3> &joints,
                       const AlignedVector<VectorX> &params,
                       Factor::Evaluation &base_eval) const override;

  virtual int linearize(const AlignedVector<Pose> &poses,
                        const AlignedVector<VectorX> &shapes,
                        const AlignedVector<Matrix3> &joints,
                        const AlignedVector<VectorX> &params,
                        const Factor::Evaluation &base_eval,
                        Factor::Linearization &base_lin) const override;

protected:
  virtual int evaluate(const Pose &pose, const VectorX &vertexParam,
                       const Vector2 &measurement, Evaluation &eval) const;

  virtual int linearize(const Pose &pose, const VectorX &vertexParam,
                        const Vector2 &measurement, const Evaluation &eval,
                        Linearization &lin) const;
};
} // namespace scope


<|start_filename|>C++/scope/factor/POFFactor.cpp<|end_filename|>
#include <scope/factor/POFFactor.h>

namespace scope {

POFFactor::POFFactor(const std::vector<int> &poses,
                     const std::vector<int> &shapes,
                     const std::vector<int> &joints,
                     const std::vector<int> &params, const Scalar &sigma,
                     const Scalar &eps, const Vector3 &measurement,
                     const Scalar &confidence, const std::string &name,
                     int index, bool active)
    : Factor(poses, shapes, joints, params, name, index, active),
      mMeasurement(measurement),
      mSigma(sigma),
      mCon(confidence),
      mSqrtCon(std::sqrt(confidence)),
      mSigmaCon(confidence * sigma) {
  assert(eps > 1e-5 && eps <= 1.0);
  assert(sigma > 0);
  assert(confidence >= 0);

  mMaxGMScale = 1 - eps;
  mGMThreshold = (1 - eps * eps) / (3 + eps * eps) * sigma;

  mMeasurement.stableNormalize();
}

int POFFactor::preLinearizeRobustKernel(const Evaluation &eval,
                                        Linearization &lin) const {
  // scaled jacobians and graidents related with GM kernel
  lin.sqrtDrho = mSigma / (mSigma + eval.squaredErrorNorm);
  lin.Drho = lin.sqrtDrho * lin.sqrtDrho;
  lin.alpha = eval.squaredErrorNorm > mGMThreshold
                  ? mMaxGMScale
                  : 1 - std::sqrt(1 - 4 * eval.squaredErrorNorm /
                                          (mSigma + eval.squaredErrorNorm));

  if (lin.alpha > 1e-5) {
    lin.scaledError = lin.alpha * eval.error / eval.squaredErrorNorm;
  } else {
    lin.scaledError.setZero();
  }

  lin.sqrtDrho *= mSqrtCon;
  lin.Drho *= mCon;

  return 0;
}

}  // namespace scope


<|start_filename|>C++/scope/initializer/ExtTorsoInitializer.h<|end_filename|>
#pragma once

#include <memory>
#include <vector>

#include <scope/initializer/Initializer.h>

namespace scope {
namespace Initializer {
class ExtTorsoInitializer : public Initializer {
protected:
  Vector3 mRelJointLocation;

  mutable Vector6 mDRootPoseChange;
  mutable Pose mRootPoseChange;

  mutable Scalar mSpineAngle[2];

  mutable Vector6 mB;

  mutable Vector<6> mS;
  mutable Vector<6> ms;
  mutable Scalar mA;
  mutable Scalar ma;

public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  ExtTorsoInitializer(const Options &options, const Vector3 &RelJointLocation);

  ExtTorsoInitializer(const ExtTorsoInitializer &) = delete;

  ExtTorsoInitializer &operator=(const ExtTorsoInitializer &) = delete;

  virtual int initialize(const Pose &T0,
                         const AlignedVector<Matrix3> &joints) const override;

  int initialize(const Pose& T0, const Scalar& SpineAngle) const;

  Scalar getSpineAngle() const { return mSpineAngle[0]; };

public:
  virtual int FKintree(int n) const override;
  virtual int DFKintree() const override;

  virtual int updateGaussNewton() const override;
  virtual int solveGaussNewton() const override;

  virtual int update(Scalar stepsize) const override;
  virtual int accept() const override;

protected:
  const Scalar SpineLowerBnd = 0;
  const Scalar SpineUpperBnd = 0.3 * M_PI;
};

} // namespace Initializer
} // namespace scope


<|start_filename|>C++/scope/initializer/ArmInitializer.h<|end_filename|>
#pragma once

#include <memory>
#include <vector>

#include <scope/initializer/Initializer.h>

namespace scope {
namespace Initializer {
class ArmInitializer : public Initializer {
public:
  enum class Arm { Left, Right };

protected:
  std::array<Vector3, 2> mRelJointLocations;

  mutable Vector3 mDJointChange;
  mutable Matrix3 mJointChange;

  mutable Scalar mElbowJoint[2];

  mutable Matrix63 mB0;
  mutable Vector6 mB1;

  mutable Vector<3> mS;
  mutable Vector<3> ms;
  mutable Scalar mA;
  mutable Scalar ma;

public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  ArmInitializer(const Options &options,
                 const std::array<Vector3, 2> &RelJointLocations, Arm LR);

  ArmInitializer(const ArmInitializer &) = delete;

  ArmInitializer &operator=(const ArmInitializer &) = delete;

  virtual int initialize(const Pose &T0,
                         const AlignedVector<Matrix3> &joints) const override;

public:
  virtual int FKintree(int n) const override;
  virtual int DFKintree() const override;

  virtual int updateGaussNewton() const override;
  virtual int solveGaussNewton() const override;

  virtual int update(Scalar stepsize) const override;
  virtual int accept() const override;

protected:
  Arm mArm;

  const Scalar mElbowLowerBnd;
  const Scalar mElbowUpperBnd;
};

} // namespace Initializer
} // namespace scope


<|start_filename|>C++/scope/factor/UnitPOFFactor.h<|end_filename|>
#pragma once

#include <scope/factor/POFFactor.h>

namespace scope {
class UnitPOFFactor : public POFFactor {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  SCOAT_FACTOR_EVALUATION_NEW
  SCOAT_FACTOR_LINEARIZATION_NEW

  UnitPOFFactor(int pose, const Vector3 &s, const Scalar &sigma,
                const Scalar &eps, const Vector3 &measurement,
                const Scalar &confidence = 1.0, const std::string &name = "",
                int index = -1, bool active = true);

  virtual int evaluate(const AlignedVector<Pose> &poses,
                       const AlignedVector<VectorX> &shapes,
                       const AlignedVector<Matrix3> &joints,
                       const AlignedVector<VectorX> &params,
                       Factor::Evaluation &base_eval) const override;

  virtual int linearize(const AlignedVector<Pose> &poses,
                        const AlignedVector<VectorX> &shapes,
                        const AlignedVector<Matrix3> &joints,
                        const AlignedVector<VectorX> &params,
                        const Factor::Evaluation &base_eval,
                        Factor::Linearization &base_lin) const override;

protected:
  Vector3 mS; // unit directional vector
};
} // namespace scope


<|start_filename|>C++/scope/base/Pose.h<|end_filename|>
#pragma once

#include <scope/base/Types.h>
#include <scope/math/SO3.h>

namespace scope {
class Pose {
 public:
  Pose() : R(matrix.data()), t(matrix.data() + 9) {
    R.setIdentity();
    t.setZero();
  }

  Pose(const Pose& T)
      : matrix(T.matrix), R(matrix.data()), t(matrix.data() + 9) {}

  Pose(const Matrix3& R, const Vector3& t): Pose() {
    this->R = R;
    this->t = t;
  }

  Pose& operator=(const Pose& T) {
    matrix = T.matrix;

    return *this;
  }

  Pose(Pose&& T) : matrix(T.matrix), R(std::move(T.R)), t(std::move(T.t)) {}

  Pose& operator=(Pose&& T) {
    matrix = std::move(T.matrix);
    R = std::move(T.R);
    t = std::move(T.t);

    return *this;
  }

  static const Pose& Identity() {
    static Pose identity;

    return identity;
  }

  template <typename T>
  static int exp(Eigen::MatrixBase<T> const& xi, Pose& res) {
    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(T, 6)
    const auto& w = xi.template head<3>();
    const auto& v = xi.template tail<3>();

    // TODO: Optimize the code
    const Scalar th = w.stableNorm();
    Vector3 s = w;

    if (th < 1e-2) {
      Scalar c, d, th2, th4, th6, d2, thd2;
      th2 = th * th;
      th4 = th2 * th2;
      th6 = th4 * th2;
      d = 1 + th2 / 12 + th4 / 120 + th6 / 1185.88235294117647058823529412;

      d2 = d * d;
      thd2 = th2 * d2;
      c = 2 / (4 + thd2);

      s *= c * d2;

      res.R.noalias() = s * w.transpose();
      res.R.diagonal().array() += 1 - c * thd2;

      s *= 2 / d;

      res.t = v;
      res.t += 0.1666666666666666666666666666667 * w.dot(v) * w;
      res.t += 0.5 * w.cross(v);
    } else {
      Scalar sth, cth;

      math::SO3::fsincos(th, &sth, &cth);

      s /= th;

      res.R.noalias() = (1 - cth) * s * s.transpose();
      res.R.diagonal().array() += cth;

      s *= sth;

      res.t = sth * v;
      res.t += (th - sth) * w.dot(v) * w;
      res.t += (1 - cth) * w.cross(v);
      res.t /= th;
    }

    res.R(0, 1) -= s[2];
    res.R(1, 0) += s[2];
    res.R(0, 2) += s[1];
    res.R(2, 0) -= s[1];
    res.R(1, 2) -= s[0];
    res.R(2, 1) += s[0];

    return 0;
  }

 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  Eigen::Matrix<Scalar, 3, 4> matrix;
  Eigen::Map<Matrix3> R;  // rotational part
  Eigen::Map<Vector3> t;  // translational part
};
}  // namespace scope


<|start_filename|>C++/scope/initializer/LegInitializer.cpp<|end_filename|>
#include <glog/logging.h>

#include <memory>
#include <scope/initializer/LegInitializer.h>

namespace scope {
namespace Initializer {
LegInitializer::LegInitializer(const Options &options,
                               const std::array<Vector3, 2> &RelJointLocations)
    : Initializer(2, options), mRelJointLocations(RelJointLocations) {
  if (mRelJointLocations.size() != mNumJoints) {
    LOG(ERROR) << "Inconsistent relative joint locations." << std::endl;
    exit(-1);
  }
}

int LegInitializer::initialize(const Pose &T0,
                               const AlignedVector<Matrix3> &joints) const {
  assert(joints.size() == mNumJoints);

  if (joints.size() != mNumJoints) {
    LOG(ERROR) << "There should be " << mNumJoints << " joint states."
               << std::endl;

    exit(-1);
  }

  Vector3 xi;
  Matrix3 R;

  // Scale knee joint to feasbile range
  math::SO3::log(joints[1], xi);
  xi[0] = std::max(xi[0], KneeLowerBnd);
  xi[0] = std::min(xi[0], KneeUpperBnd);
  xi.tail<2>().setZero();
  math::SO3::exp(xi, R);

  mKneeJoint[0] = mKneeJoint[1] = xi[0];

  Initializer::initialize(T0, {joints[0], R});

  return 0;
}

int LegInitializer::FKintree(int n) const {
  auto &poses = mvPoses[n];
  auto &joints = mvJoints[n];

  poses[1].R.noalias() = poses[0].R * joints[0];
  poses[1].t = poses[0].t;
  poses[1].t.noalias() += poses[0].R * mRelJointLocations[0];

  poses[2].R.noalias() = poses[1].R * joints[1];
  poses[2].t = poses[1].t;
  poses[2].t.noalias() += poses[1].R * mRelJointLocations[1];

  return 0;
}

int LegInitializer::DFKintree() const {
  const auto &poses = mvPoses[0];

  mB0.topRows<3>() = poses[0].R;
  mB0.block<3, 1>(3, 0).noalias() = poses[1].t.cross(poses[0].R.col(0));
  mB0.block<3, 1>(3, 1).noalias() = poses[1].t.cross(poses[0].R.col(1));
  mB0.block<3, 1>(3, 2).noalias() = poses[1].t.cross(poses[0].R.col(2));

  mB1.head<3>() = poses[1].R.col(0);
  mB1.tail<3>().noalias() = poses[2].t.cross(poses[1].R.col(0));

  return 0;
}

int LegInitializer::updateGaussNewton() const {
  mH.setZero(10, 10);

  mH.topLeftCorner<6, 6>() = mvMxx[2];
  mH.block<6, 1>(0, 9).noalias() = mH.topLeftCorner<6, 6>() * mB1;
  mH.block<3, 1>(6, 9).noalias() = mB0.transpose() * mH.block<6, 1>(0, 9);
  mH(9, 9) = mB1.dot(mH.block<6, 1>(0, 9));
  mH(9, 9) += mvMuu[1](0, 0);

  mH.topLeftCorner<6, 6>() += mvMxx[1];
  mH.block<6, 3>(0, 6).noalias() = mH.topLeftCorner<6, 6>() * mB0;
  mH.block<3, 3>(6, 6).noalias() = mB0.transpose() * (mH.block<6, 3>(0, 6));
  mH.block<3, 3>(6, 6) += mvMuu[0];

  mH.topLeftCorner<6, 6>() += mvMxx[0];

  mH.block<3, 6>(6, 0) = mH.block<6, 3>(0, 6).transpose();
  mH.block<1, 9>(9, 0) = mH.block<9, 1>(0, 9).transpose();

  mh.resize(10);

  mh.head<6>() = mvmx[2];
  mh[9] = mB1.dot(mh.head<6>());
  mh[9] += mvmu[1][0];

  mh.head<6>() += mvmx[1];
  mh.segment<3>(6) = mB0.transpose() * mh.head<6>();
  mh.segment<3>(6) += mvmu[0];

  mh.head<6>() += mvmx[0];

  return 0;
}

int LegInitializer::solveGaussNewton() const {
  mLambda = mH.diagonal() * (mDLambda - 1);
  mLambda.array() += mOptions.delta;
  mH.diagonal() += mLambda;

  mHchol.compute(mH.topLeftCorner<9, 9>());

  mS.noalias() = -mHchol.solve(mH.block<9, 1>(0, 9));
  ms.noalias() = -mHchol.solve(mh.head<9>());

  mA = mH(9, 9) + mH.block<9, 1>(0, 9).dot(mS);
  ma = mh[9] + mH.block<9, 1>(0, 9).dot(ms);

  mhGN.resize(10);

  // Guarantee mKneeJoint in [0, 0.78 * M_PI]
  mhGN[9] = -ma / mA;
  mhGN[9] = std::max(mhGN[9], KneeLowerBnd - mKneeJoint[0]);
  mhGN[9] = std::min(mhGN[9], KneeUpperBnd - mKneeJoint[0]);
  mhGN.head<9>() = ms;
  mhGN.head<9>().noalias() += mS * mhGN[9];

  mSquaredError = mh;
  mSquaredError += 0.5 * mH * mhGN;

  mE = mhGN.dot(mSquaredError);
  mSquaredError = mhGN.cwiseAbs2();
  mE -= 0.5 * mLambda.dot(mSquaredError);

  return 0;
}

int LegInitializer::update(Scalar stepsize) const {
  assert(stepsize > 0);

  mDRootPoseChange = mhGN.head<6>() * stepsize;
  Pose::exp(mDRootPoseChange, mRootPoseChange);

  mvPoses[1][0].R.noalias() = mRootPoseChange.R * mvPoses[0][0].R;
  mvPoses[1][0].t = mRootPoseChange.t;
  mvPoses[1][0].t.noalias() += mRootPoseChange.R * mvPoses[0][0].t;

  mDJointChange = mhGN.segment<3>(6) * stepsize;
  math::SO3::exp(mDJointChange, mJointChange);
  mvJoints[1][0].noalias() = mJointChange * mvJoints[0][0];

  mKneeJoint[1] = mKneeJoint[0] + stepsize * mhGN[9];

  mvJoints[1][1].setIdentity();
  mvJoints[1][1](1, 1) = std::cos(mKneeJoint[1]);
  mvJoints[1][1](2, 2) = mvJoints[1][1](1, 1);
  mvJoints[1][1](2, 1) = std::sin(mKneeJoint[1]);
  mvJoints[1][1](1, 2) = -mvJoints[1][1](2, 1);

  FKintree(1);

  return 0;
}

int LegInitializer::accept() const {
  mKneeJoint[0] = mKneeJoint[1];

  return Initializer::accept();
}
} // namespace Initializer
} // namespace scope


<|start_filename|>C++/scope/factor/FullVertexExtraPinholeCameraFactor.h<|end_filename|>
#pragma once

#include <scope/factor/VertexExtraPinholeCameraFactor.h>

namespace scope {
class FullVertexExtraPinholeCameraFactor
    : public VertexExtraPinholeCameraFactor {
public:
  struct Evaluation : public VertexExtraPinholeCameraFactor::Evaluation {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    Vector2 camMeasurement;
  };

  struct Linearization : public VertexExtraPinholeCameraFactor::Linearization {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    Vector4 gCam;
  };

public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  SCOAT_FACTOR_EVALUATION_NEW
  SCOAT_FACTOR_LINEARIZATION_NEW

  FullVertexExtraPinholeCameraFactor(int pose, int vParams, int camParams,
                                     const Matrix3X &vDirs, const Vector3 &v,
                                     const Scalar &sigma, const Scalar &eps,
                                     const Pose &extraCamPose,
                                     const Vector2 &measurement,
                                     const Scalar &confidence = 1.0,
                                     const std::string &name = "",
                                     int index = -1, bool active = true);

  virtual int evaluate(const AlignedVector<Pose> &poses,
                       const AlignedVector<VectorX> &shapes,
                       const AlignedVector<Matrix3> &joints,
                       const AlignedVector<VectorX> &params,
                       Factor::Evaluation &base_eval) const override;

  virtual int linearize(const AlignedVector<Pose> &poses,
                        const AlignedVector<VectorX> &shapes,
                        const AlignedVector<Matrix3> &joints,
                        const AlignedVector<VectorX> &params,
                        const Factor::Evaluation &base_eval,
                        Factor::Linearization &base_lin) const override;

protected:
  virtual int evaluate(const Pose &pose, const VectorX &vertexParam,
                       const VectorX &camParam, const Vector2 &measurement,
                       Evaluation &eval) const;

  virtual int linearize(const Pose &pose, const VectorX &vertexParam,
                        const VectorX &camParam, const Vector2 &measurement,
                        const Evaluation &eval, Linearization &lin) const;
};
} // namespace scope


<|start_filename|>C++/scope/initializer/Initializer.h<|end_filename|>
#pragma once

#include <memory>
#include <vector>

#include <scope/initializer/factor/Factors.h>

namespace scope {
namespace Initializer {
struct FactorIndex {
  static const int PinholeCamera = 0;
  static const int POF = 1;
  static const int DepthCamera = 2;
  static const int JointLimit = 3;
  static const int PoseConst = 4;
  static const int JointConst = 5;
  static const int Pose = 6;
  static const int Joint = 7;
};

static const int NumFactors = FactorIndex::Joint + 1;

enum class Status {
  Uninitialized,
  Initialized,
  Accepted,
  Failed,
  Aborted,
  Converged
};

struct Options {
  /** A small positive number used to stablize the linear system */
  Scalar delta = 1e-1;

  /** Stopping criterion based upon the relative decrease in function value */
  Scalar rel_func_decrease_tol = 5e-3;

  /** Stopping tolerance for the relative decrease in linearized approximation
   */
  Scalar rel_lin_func_decrease_tol = 5e-3;

  /** Maximum elapsed computation time (in seconds) */
  double max_total_computation_time = 30;

  /** Stopping criterion based upon the norm of an accepted update step */
  Scalar stepsize_tol = 1e-3;

  /** Maximum permitted number of iterations */
  int max_iterations = 1000;

  /** Weights for each factor */
  scope::Vector<NumFactors> weights =
      100 * Eigen::Matrix<Scalar, NumFactors, 1>::Ones();

  /** Maximum number of inner iterations */
  int max_inner_iters = 20;

  /** Parameter to increase the stepsize */
  Scalar stepsize_increase_ratio = 1.2;

  /** Parameter to reduce the stepsize */
  Scalar stepsize_decrease_ratio = 0.5;

  /** Parameters for LM method */
  Scalar lm_lambda_increase = 1.5;
  Scalar lm_lambda_decrease = 0.8;
  Scalar lm_lambda_max = 1e3;
};

struct Results {
  /** Status */
  Status status = Status::Initialized;

  /** The objective values */
  std::vector<Scalar> fobjs;

  /** The stepsizes */
  std::vector<Scalar> stepsizes;

  /** The expected relative cost reduction */
  std::vector<Scalar> expected_rel_cost_reduction;

  /** The total elapsed computation time */
  double total_computation_time;

  /** The elapsed optimization time for each iteration*/
  std::vector<double> elapsed_optimization_times;
};

class Initializer {
 public:
  // the number of poses and joints
  const int mNumPoses;
  const int mNumJoints;

  // factors
  std::vector<std::vector<std::shared_ptr<const Factor>>> mvFactors;
  std::vector<std::shared_ptr<const Factor>> mFactors;

  // optimizer options
  Options mOptions;
  mutable Results mResults;

  // status
  mutable Status mStatus;

  // stepsize
  mutable Scalar mStepsize;

  mutable AlignedVector<Pose> mvPoses[2];
  mutable AlignedVector<Matrix3> mvJoints[2];

  mutable AlignedVector<Matrix6> mvMxx;
  mutable AlignedVector<Matrix3> mvMuu;

  mutable AlignedVector<Vector6> mvmx;
  mutable AlignedVector<Vector3> mvmu;

  // the objective value
  mutable Scalar mfobj[2];

  // objective values for each factor
  mutable scope::Vector<NumFactors> mvFobj[2];

  mutable std::vector<std::vector<std::shared_ptr<const Factor::Evaluation>>>
      mvFactorEvals[2];
  mutable std::vector<std::shared_ptr<Factor::Evaluation>> mFactorEvals[2];

  // linearizations
  std::vector<std::vector<std::shared_ptr<const Factor::Linearization>>>
      mvFactorLins;
  mutable std::vector<std::shared_ptr<Factor::Linearization>> mFactorLins;

  mutable Eigen::LDLT<MatrixX> mHchol;

  mutable MatrixX mH;
  mutable VectorX mh;
  mutable VectorX mhGN;

  mutable Scalar mE;
  mutable VectorX mSquaredError;

  mutable Scalar mDLambda;
  mutable VectorX mLambda;

 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  Initializer(int NumJoints, const Options &options);

  Initializer(const Initializer &) = delete;

  Initializer &operator=(const Initializer &) = delete;

  virtual int initialize(const Pose &T0,
                         const AlignedVector<Matrix3> &joints) const;
  int solve() const;

  int updateFactorWeights(const Eigen::Matrix<Scalar, NumFactors, 1> &weights);

  int reset();

  int addPinholeCameraFactor(std::shared_ptr<PinholeCameraFactor> factor);
  int addJointLimitFactor(std::shared_ptr<JointLimitFactor> factor);
  int addPOFFactor(std::shared_ptr<POFFactor> factor);
  int addDepthCameraFactor(std::shared_ptr<DepthCameraFactor> factor);
  int addPoseConstFactor(std::shared_ptr<PoseConstFactor> factor);
  int addJointConstFactor(std::shared_ptr<JointConstFactor> factor);
  int addPoseFactor(std::shared_ptr<PoseFactor> factor);
  int addJointFactor(std::shared_ptr<JointFactor> factor);

  const Results &getResults() const { return mResults; }

  const AlignedVector<Pose> &getPoses() const { return mvPoses[0]; }

  const AlignedVector<Matrix3> &getJoints() const { return mvJoints[0]; }

  Scalar getFobj() const { return mfobj[0]; }

  const std::vector<std::vector<std::shared_ptr<const Factor::Evaluation>>>
      &getEvaluations() const {
    return mvFactorEvals[0];
  }

 public:
  int checkFactor(std::shared_ptr<const Factor> factor);

  int updatePinholeCameraFactorGaussNewton() const;
  int updateJointLimitFactorGaussNewton() const;
  int updatePOFFactorGaussNewton() const;
  int updateDepthCameraFactorGaussNewton() const;
  int updatePoseConstFactorGaussNewton() const;
  int updateJointConstFactorGaussNewton() const;
  int updatePoseFactorGaussNewton() const;
  int updateJointFactorGaussNewton() const;

  int optimize() const;
  virtual int accept() const;
  virtual int solveGaussNewton() const;

  int evaluate(int n) const;
  int linearize() const;

  int addFactor(int index, std::shared_ptr<Factor> factor,
                std::array<std::shared_ptr<Factor::Evaluation>, 2> &evals,
                std::shared_ptr<Factor::Linearization> &lin);

  virtual int FKintree(int n) const = 0;
  virtual int DFKintree() const = 0;

  virtual int updateGaussNewton() const = 0;

  virtual int update(Scalar stepsize) const = 0;
};
}  // namespace Initializer
}  // namespace scope


<|start_filename|>C++/scope/model/SMPL.h<|end_filename|>
#pragma once

#include <scope/model/Model.h>

namespace scope {
template <int P>
class SMPL : public Model<23, P, 6890> {
 public:
  const static std::vector<int> mvLeftLeg, mvRightLeg, mvLeftArm, mvRightArm,
      mvHead, mvBack;

  SMPL(const MatrixX& RelJDirs, const VectorX& RelJ,
       const MatrixX& VDirs, const VectorX& V);

  virtual const std::vector<int> & getKinematicsTree() const override;

 protected:
  virtual int setKinematicsTree() override;
};

extern template class SMPL<10>;
extern template class SMPL<11>;
}  // namespace scope


<|start_filename|>C++/scope/optimizer/Optimizer.h<|end_filename|>
#pragma once

#include <memory>
#include <vector>

#include <scope/factor/Factors.h>
#include <scope/model/Model.h>

// TODO: how to deal with collisions

namespace scope {
namespace Optimizer {
struct FactorIndex {
  static const int JointPinholeCamera = 0;
  static const int FullJointPinholeCamera = 1;
  static const int VertexPinholeCamera = 2;
  static const int FullVertexPinholeCamera = 3;
  static const int UnitPOF = 4;
  static const int ScaledPOF = 5;
  static const int RelPOF = 6;
  static const int JointDepthCamera = 7;
  static const int VertexDepthCamera = 8;
  static const int JointLimit = 9;
  static const int Collision = 10;
  static const int JointConst = 11;
  static const int PoseConst = 12;
  static const int Pose = 13;
  static const int Shape = 14;
  static const int Joint = 15;
  static const int Parameter = 16;
};

static const int NumFactors = FactorIndex::Parameter + 1;

enum class Status {
  Converged,
  Failed,
  Accepted,
  Aborted,
  Initialized,
  Uninitialized
};

enum class Method { GaussNewton, LM };

struct Options {
  /** Method */
  Method method = Method::LM;

  /** A small positive number used to stablize the linear system */
  Scalar delta = 1e-3;

  /** Stopping criterion based upon the relative decrease in function value */
  Scalar rel_func_decrease_tol = 1e-4;

  /** Stopping tolerance for the relative decrease in linearized approximation
   */
  Scalar rel_lin_func_decrease_tol = 1e-3;

  /** Maximum elapsed computation time (in seconds) */
  double max_computation_time = 30;

  /** Stopping criterion based upon the norm of an accepted update step */
  Scalar stepsize_tol = 1e-3;

  /** Maximum permitted number of iterations */
  int max_iterations = 1000;

  /** Check collisions or not */
  bool check_collisions = false;

  /** Weights for each factor */
  scope::Vector<NumFactors> weights =
      100 * Eigen::Matrix<Scalar, NumFactors, 1>::Ones();

  /** Threshold for collision checking */
  Scalar collision_threshold = 0;

  /** Maximum number of inner iterations */
  int max_inner_iters = 20;

  /** Parameter to increase the stepsize */
  Scalar stepsize_increase_ratio = 1.2;

  /** Parameter to reduce the stepsize */
  Scalar stepsize_decrease_ratio = 0.5;

  /** Parameters for LM method */
  Scalar lm_lambda_increase = 1.5;
  Scalar lm_lambda_decrease = 0.8;
  Scalar lm_lambda_max = 1e3;

  /** Extra parameter for hip */
  bool extra_hip_parameter = true;
  double extra_hip_parameter_bnd[2] = {-0.1, 0.1};
};

struct Results {
  /** Status */
  Status status = Status::Initialized;

  /** The objective values */
  std::vector<Scalar> fobjs;

  /** The stepsizes */
  std::vector<Scalar> stepsizes;

  /** The expected relative cost reduction */
  std::vector<Scalar> expected_rel_cost_reduction;

  /** The actual relative cost reduction */
  std::vector<Scalar> rel_cost_reduction;

  /** The total elapsed computation time */
  double total_computation_time;

  /** The elapsed optimization time for each iteration*/
  std::vector<double> elapsed_optimization_times;
};

template <int K, int P, int N, bool CamOpt>
class Optimizer {
 public:
 protected:
  // optimizer options
  Options mOptions;
  mutable Results mResults;

  // human model used
  std::shared_ptr<const Model<K, P, N>> mModel;

  // the number of poses
  static const int NumPoses = K + 1;
  // the number of shapes
  static const int NumShapes = 0;
  // the number of joints
  static const int NumJoints = K;
  // the number of implicit parameters
  static const int NumParams = 1 + CamOpt;

  /** Maximum number of collisions considered when computing a descent
   * direction */
  static const int MaxNumCollisions = 0;

  static const int CamParamIndex = CamOpt ? 0 : -1;
  static const int VertexParamIndex = CamOpt ? 1 : 0;
  static const int FaceParamIndex = -2;

  static const int CamParamOffset = 0;
  static const int VertexParamOffset = CamOpt ? 4 : 0;
  static const int FaceParamOffset = VertexParamOffset + P;

  static const int CamParamSize = CamOpt ? 4 : 0;
  static const int VertexParamSize = P;
  static const int FaceParamSize = 0;

  static const std::vector<int> ParamSizes;

  // factors
  std::vector<std::vector<std::vector<std::shared_ptr<const Factor>>>>
      mnvFactors;
  std::vector<std::vector<std::shared_ptr<const Factor>>> mnFactors;

  std::vector<std::vector<std::shared_ptr<const Factor>>> mvFactors;
  std::vector<std::shared_ptr<const Factor>> mFactors;

  // the size of parameters
  static const int ParamSize = CamOpt ? P + 4 : P;
  // the dimension of optimization variables
  static const int Size = 6 + 3 + ParamSize;
  // maximum DSize
  static const int MaxDSize = Size;

  // dimension of 3D Pose
  static const int DPoseSize = 6;
  // dimension of each joint state
  static const int DJointSize = 3;
  // the number of implicit parameters used in descent direction computation,
  // which depends on the number of collisions
  static const int DParamSize = ParamSize;
  // the size of the Hessian matrix
  static const int DSize = Size;

  static const int DPoseOffset = 0;
  static const int DParamOffset = 6;
  static const int DJointOffset = DParamOffset + DParamSize;

  static const int DCamParamOffset = DParamOffset + CamParamOffset;
  static const int DVertexParamOffset = DParamOffset + VertexParamOffset;
  static const int DFaceParamOffset = DParamOffset + FaceParamOffset;

  static const std::vector<int> DParamOffsets;

  // status
  mutable Status mStatus;

  // stepsize
  mutable Scalar mStepsize;

  // graident norm
  mutable Scalar mGradientNorm;

  mutable int mCollisionParamSize;
  mutable int mDCollisionParamOffset;

  // the objective value
  mutable Scalar mfobj[2];
  // poses of body parts
  mutable AlignedVector<Pose> mvPoses[2];
  // shape parameters
  mutable AlignedVector<VectorX> mvShapes[2];
  // joint states
  mutable AlignedVector<Matrix3> mvJoints[2];
  // model parameters and camera intrinsics
  mutable AlignedVector<VectorX> mvParams[2];
  // relative joint locations
  VectorX mRawRelJointLocations[2];
  mutable Eigen::Map<Vector<3 * K>> mRelJointLocations[2];

  // objective values for each factor
  mutable scope::Vector<NumFactors> mvFobj[2];

  // number of collisions detected
  mutable int mNumCollisions;

  // evaluations
  mutable std::vector<
      std::vector<std::vector<std::shared_ptr<const Factor::Evaluation>>>>
      mnvFactorEvals[2];
  mutable std::vector<std::vector<std::shared_ptr<Factor::Evaluation>>>
      mnFactorEvals[2];

  mutable std::vector<std::vector<std::shared_ptr<const Factor::Evaluation>>>
      mvFactorEvals[2];
  mutable std::vector<std::shared_ptr<Factor::Evaluation>> mFactorEvals[2];

  // intermediates used for update
  mutable Pose mRootPoseChange;
  mutable Vector6 mDRootPoseChange;
  mutable AlignedVector<Matrix3> mvJointChange;
  mutable AlignedVector<Vector3> mvDJointChange;

  // linearizations
  std::vector<
      std::vector<std::vector<std::shared_ptr<const Factor::Linearization>>>>
      mnvFactorLins;
  mutable std::vector<std::vector<std::shared_ptr<Factor::Linearization>>>
      mnFactorLins;

  std::vector<std::vector<std::shared_ptr<const Factor::Linearization>>>
      mvFactorLins;
  mutable std::vector<std::shared_ptr<Factor::Linearization>> mFactorLins;

  Matrix6X mRawB;
  Eigen::Map<Matrix<6, K *(P + 3)>> mB;
  mutable AlignedVector<Eigen::Map<Matrix<6, P + 3>>> mvB;
  AlignedVector<Eigen::Map<Matrix<3, P>, 0, Eigen::Stride<6, 1>>> mvBp;
  AlignedVector<Eigen::Map<Matrix63>> mvBu;

  // [Mxx  *   *
  //  Mpx Mpp  *
  //  Mux Mup Muu]
  Matrix<DSize, Eigen::Dynamic> mRawM;
  mutable Eigen::Map<Matrix<DSize, NumPoses * DSize>> mM;
  mutable AlignedVector<Eigen::Map<scope::Matrix<DSize, DSize>>> mvM;

  mutable AlignedVector<Vector6> mvmx;
  mutable AlignedVector<Vector<DParamSize>> mvmp;
  mutable AlignedVector<Vector3> mvmu;

  // Gradient
  mutable AlignedVector<Vector6> mvPoseDG;
  mutable AlignedVector<Vector3> mvJointDG;
  mutable AlignedVector<Vector<P>> mvParamDG;
  mutable VectorX mParamDG;

  // Gauss-Newton direction
  Matrix6X mRawPoseGN;
  Matrix3X mRawJointGN;
  Eigen::Map<Matrix<6, NumPoses>> mPoseGN;
  Eigen::Map<Matrix<3, NumJoints>> mJointGN;
  mutable AlignedVector<Eigen::Map<Vector6>> mvPoseGN;
  mutable AlignedVector<Eigen::Map<Vector3>> mvJointGN;

  VectorX mRawParamGN;
  mutable Eigen::Map<Vector<DParamSize>> mParamGN;
  VectorX mRawhGN;
  mutable Eigen::Map<Vector<6 + DParamSize>> mhGN;

  mutable VectorX mS;

  // [Hxx  *   *
  //  Hpx Hpp  *
  //  Hux Hup Huu]
  Matrix<DSize, Eigen::Dynamic> mRawH;
  mutable Eigen::Map<Matrix<DSize, NumJoints * DSize>> mH;
  mutable AlignedVector<Eigen::Map<Matrix<DSize, DSize>>> mvH;

  Matrix<DSize, Eigen::Dynamic> mRawHxB;
  Eigen::Map<Matrix<DSize, NumJoints *(P + 3)>> mHxB;
  mutable AlignedVector<Eigen::Map<Matrix<DSize, P + 3>>> mvHxB;

  RowMajorMatrix<3 * NumJoints, Eigen::Dynamic> mRawKuxp;
  Eigen::Map<RowMajorMatrix<3 * NumJoints, 6 + DParamSize>> mKuxp;
  mutable AlignedVector<Eigen::Map<RowMajorMatrix<3, 6 + DParamSize>>> mvKuxp;

  Matrix<6 + DParamSize, Eigen::Dynamic> mRawH0;
  mutable Eigen::Map<Matrix<6 + DParamSize, 6 + DParamSize>> mH0;

  mutable AlignedVector<Matrix3> mvHuuInv;
  mutable Eigen::LDLT<MatrixX> mHchol0;

  mutable AlignedVector<Vector6> mvhx;
  mutable AlignedVector<Vector<P>> mvhp;
  mutable AlignedVector<Vector3> mvhu;

  VectorX mRawku;
  mutable Eigen::Map<Vector<3 * K>> mku;
  mutable AlignedVector<Eigen::Map<Vector3>> mvku;

  mutable scope::Vector<K> mvE;

  VectorX mRawh0;
  mutable Eigen::Map<Vector<6 + DParamSize>> mh0;

  // expected cost reduction
  mutable Scalar mExtraCost;
  mutable Scalar mE;

  // cost reduction
  mutable Vector2 mDCost;

  mutable AlignedVector<Vector6> mvDCostDx;
  mutable AlignedVector<Vector<ParamSize>> mvDCostDp;
  mutable AlignedVector<Vector3> mvDCostDu;

  // LM parameters
  mutable Scalar mDLambda;

  scope::VectorX mRawLambda;
  mutable Eigen::Map<Vector<6 + DParamSize + 3 * K>> mLambda;

  scope::VectorX mRawSquaredError;
  mutable Eigen::Map<Vector<6 + DParamSize + 3 * K>> mSquaredError;

 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  Optimizer(const std::shared_ptr<const Model<K, P, N>> &model,
            const Options &options);

  Optimizer(const Optimizer &) = delete;

  Optimizer &operator=(const Optimizer &) = delete;

  int initialize(bool refine = true) const;

  int initialize(const Pose &T0, const AlignedVector<Matrix3> &joints,
                 const AlignedVector<VectorX> &params,
                 bool refine = true) const;

  int optimize() const;

  int evaluate() const;

  int updateFactorWeights(const Eigen::Matrix<Scalar, NumFactors, 1> &weights);

  int reset();

  int addJointPinholeCameraFactor(
      std::shared_ptr<JointPinholeCameraFactor> factor);
  int addVertexPinholeCameraFactor(
      std::shared_ptr<VertexPinholeCameraFactor> factor);
  int addPOFFactor(std::shared_ptr<POFFactor> factor);
  int addDepthCameraFactor(std::shared_ptr<DepthCameraFactor> factor);
  int addJointLimitFactor(std::shared_ptr<JointLimitFactor> factor);
  int addPoseConstFactor(std::shared_ptr<PoseConstFactor> factor);
  int addJointConstFactor(std::shared_ptr<JointConstFactor> factor);
  int addPoseFactor(std::shared_ptr<PoseFactor> factor);
  int addJointFactor(std::shared_ptr<JointFactor> factor);
  int addParameterFactor(std::shared_ptr<ParameterFactor> factor);

  int getNumPoses() const;
  int getNumShapes() const;
  int getNumJoints() const;
  int getNumParameters() const;

  int getFaceParameterIndex() const;
  int getVertexParameterIndex() const;
  int getCameraParameterIndex() const;

  const Results &getResults() const;

  Scalar getFobj() const;

  AlignedVector<Pose> const &getPoses() const;
  AlignedVector<VectorX> const &getShapes() const;
  AlignedVector<Matrix3> const &getJoints() const;
  AlignedVector<VectorX> const &getParameters() const;

  std::vector<std::vector<std::shared_ptr<const Factor::Evaluation>>> const &
  getEvaluations() const;

 protected:
  // Gauss-Newton directions;
  AlignedVector<Vector6> const &getPoseDG() { return mvPoseDG; }

  AlignedVector<Vector3> const &getJointDG() { return mvJointDG; }

  VectorX const &getParamDG() { return mParamDG; }

  // setup
  virtual int setupModelInfo();
  virtual int setupParameterInfo() = 0;
  virtual int setupFactors();
  virtual int setupOptimization();

  // initialization
  virtual int init() const;
  virtual int initialOptimize() const;

  // evaluation
  virtual int update(Scalar stepsize) const;
  virtual int evaluateFactors(int n) const;
  virtual int evaluateInitialFactors(int n) const;

  virtual int updateGaussNewtonInfo() const;
  virtual int accept() const;

  // linearization
  virtual int linearize() const;
  virtual int linearizeFactors(int pose) const;

  // update hessians and gradients
  virtual int updateJointPinholeCameraFactorGaussNewton() const;
  virtual int updateFullJointPinholeCameraFactorGaussNewton() const;
  virtual int updateVertexPinholeCameraFactorGaussNewton() const;
  virtual int updateFullVertexPinholeCameraFactorGaussNewton() const;
  virtual int updateUnitPOFFactorGaussNewton() const;
  virtual int updateScaledPOFFactorGaussNewton() const;
  virtual int updateRelPOFFactorGaussNewton() const;
  virtual int updateJointDepthCameraFactorGaussNewton() const;
  virtual int updateVertexDepthCameraFactorGaussNewton() const;
  virtual int updateJointLimitFactorGaussNewton() const;
  virtual int updateCollisionFactorGaussNewton() const;
  virtual int updatePoseConstFactorGaussNewton() const;
  virtual int updateJointConstFactorGaussNewton() const;
  virtual int updatePoseFactorGaussNewton() const;
  virtual int updateShapeFactorGaussNewton() const;
  virtual int updateJointFactorGaussNewton() const;
  virtual int updateParamFactorGaussNewton() const;

  virtual int updateJointPinholeCameraFactorGaussNewton(int pose) const;
  virtual int updateFullJointPinholeCameraFactorGaussNewton(int pose) const;
  virtual int updateVertexPinholeCameraFactorGaussNewton(int pose) const;
  virtual int updateFullVertexPinholeCameraFactorGaussNewton(int pose) const;
  virtual int updateUnitPOFFactorGaussNewton(int pose) const;
  virtual int updateScaledPOFFactorGaussNewton(int pose) const;
  virtual int updateRelPOFFactorGaussNewton(int pose) const;
  virtual int updateJointDepthCameraFactorGaussNewton(int pose) const;
  virtual int updateVertexDepthCameraFactorGaussNewton(int pose) const;
  virtual int updateJointLimitFactorGaussNewton(int pose) const;
  virtual int updateCollisionFactorGaussNewton(int pose) const;
  virtual int updatePoseConstFactorGaussNewton(int pose) const;
  virtual int updateJointConstFactorGaussNewton(int pose) const;
  virtual int updatePoseFactorGaussNewton(int pose) const;
  virtual int updateShapeFactorGaussNewton(int pose) const;
  virtual int updateJointFactorGaussNewton(int pose) const;
  virtual int updateParamFactorGaussNewton(int pose) const;

  // compute gradient
  virtual int computeGradient() const;

  // solve Gauss Newton direction
  virtual int solveGaussNewton() const;

  /** compute DCost */
  virtual int updateDCost() const;

  /** Gauss-Newton direction */
  // factorize non-root link
  virtual int factorize(int i) const;

  // backward pass for non-root link
  virtual int backward(int i) const;

  // forward pass for non-root link
  virtual int forward(int i) const;

  // factorize root link
  virtual int factorizeN() const;

  // backward pass for root link
  virtual int backwardN() const;

  // backward pass through a subtree
  virtual int backwardPass(const std::vector<int> &subtree) const;

  // forkward pass through a subtree
  virtual int forwardPass(const std::vector<int> &subtree) const;

  // update cost reduction
  virtual int updateCostReduction() const;

  // backward paoss for gradient evaluation
  virtual int backwardDG(int i) const;

  // add a factor
  int addFactor(int index, std::shared_ptr<Factor> factor,
                std::array<std::shared_ptr<Factor::Evaluation>, 2> &evals,
                std::shared_ptr<Factor::Linearization> &lin);

  // check if a factor is valid
  int checkFactor(std::shared_ptr<const Factor> factor);
};
}  // namespace Optimizer
}  // namespace scope


<|start_filename|>C++/scope/factor/RelPOFFactor.cpp<|end_filename|>
#include <glog/logging.h>

#include <scope/factor/RelPOFFactor.h>

namespace scope {
RelPOFFactor::RelPOFFactor(int pose, int vParam,
                           const std::vector<int> &parents,
                           const std::array<Matrix3X, 2> &VDirs,
                           const std::array<Vector3, 2> &V, const Scalar &sigma,
                           const Scalar &eps, const Vector3 &measurement,
                           const Scalar &confidence, const std::string &name,
                           int index, bool active)
    : POFFactor({pose}, {}, {pose - 1}, {vParam}, sigma, eps, measurement,
                confidence, name, index, active) {
  assert(pose > 0 && pose < parents.size());
  assert(VDirs[0].cols() == VDirs[1].cols());

  if (pose <= 0 || pose >= parents.size()) {
    LOG(ERROR) << "The pose must be valid." << std::endl;

    mvPoses[0] = -1;
    mvParams[0] = -1;
  }

  if (VDirs[0].cols() != VDirs[1].cols()) {
    LOG(ERROR) << "Inconsistent shape directions." << std::endl;

    mvPoses[0] = -1;
    mvParams[0] = -1;
  }

  mParent = parents[pose];

  mVDirs.resize(6, VDirs[0].cols());
  mVDirs.topRows<3>() = VDirs[1];
  mVDirs.bottomRows<3>() = VDirs[0];

  mV.head<3>() = V[1];
  mV.tail<3>() = V[0];
}

int RelPOFFactor::evaluate(const AlignedVector<Pose> &poses,
                           const AlignedVector<VectorX> &shapes,
                           const AlignedVector<Matrix3> &joints,
                           const AlignedVector<VectorX> &params,
                           Factor::Evaluation &base_eval) const {
  auto &eval = dynamic_cast<Evaluation &>(base_eval);
  eval.clear();

  const auto &i = mvPoses[0];
  const auto &j = mParent;

  assert(i >= 1 && i < poses.size());
  assert(j >= 0 && j < poses.size());

  if (i < 1 || i >= poses.size() || j < 0 || j >= poses.size()) {
    LOG(ERROR) << "The pose must be valid." << std::endl;

    exit(-1);
  }

  const auto &vertexParam = mvParams[0];
  assert(vertexParam >= 0 && vertexParam < params.size());

  if (vertexParam < 0 || vertexParam > params.size()) {
    LOG(ERROR) << "The parameter must be valid." << std::endl;

    exit(-1);
  }

  evaluate(poses[i], poses[j], params[vertexParam], mMeasurement, eval);

  eval.status = Status::VALID;

  return 0;
}

int RelPOFFactor::linearize(const AlignedVector<Pose> &poses,
                            const AlignedVector<VectorX> &shapes,
                            const AlignedVector<Matrix3> &joints,
                            const AlignedVector<VectorX> &params,
                            const Factor::Evaluation &base_eval,
                            Factor::Linearization &base_lin) const {
  auto &eval = dynamic_cast<const Evaluation &>(base_eval);
  auto &lin = dynamic_cast<Linearization &>(base_lin);

  lin.clear();

  assert(eval.status == Status::VALID);

  if (eval.status != Status::VALID) {
    LOG(ERROR) << "The evaluation must be valid." << std::endl;

    exit(-1);
  }

  const auto &i = mvPoses[0];
  const auto &j = mParent;

  assert(i >= 1 && i < poses.size());
  assert(j >= 0 && j < poses.size());

  if (i < 1 || i >= poses.size() || j < 0 || j >= poses.size()) {
    LOG(ERROR) << "The pose must be valid." << std::endl;

    exit(-1);
  }

  const auto &vertexParam = mvParams[0];
  assert(vertexParam >= 0 && vertexParam < params.size());

  if (vertexParam < 0 || vertexParam > params.size()) {
    LOG(ERROR) << "The parameter must be valid." << std::endl;

    exit(-1);
  }

  lin.jacobians[0].resize(1);
  lin.jacobians[2].resize(1);
  lin.jacobians[3].resize(1);

  linearize(poses[i], poses[j], params[vertexParam], mMeasurement, eval, lin);

  lin.status = Status::VALID;

  return 0;
}

int RelPOFFactor::evaluate(const Pose &pose, const Pose &parent,
                           const VectorX &vertexParam,
                           const Vector3 &measurement, Evaluation &eval) const {
  eval.error.resize(1);

  eval.vertex = mV;
  eval.vertex.noalias() += mVDirs * vertexParam;

  eval.D.noalias() = pose.R * eval.vertex.head<3>();
  eval.D.noalias() -= parent.R * eval.vertex.tail<3>();

  eval.DNorm = eval.D.stableNorm();

  eval.d.setZero();

  if (eval.DNorm > 1e-10) {
    eval.d = eval.D / eval.DNorm;
  }

  eval.error = eval.d - mMeasurement;
  eval.squaredErrorNorm = eval.error.squaredNorm();

#if not ROBUST_POF
  eval.f = mCon * eval.squaredErrorNorm;
#else
  eval.f = mSigmaCon * eval.squaredErrorNorm / (mSigma + eval.squaredErrorNorm);
#endif

  return 0;
}

int RelPOFFactor::linearize(const Pose &pose, const Pose &parent,
                            const VectorX &vertexParam,
                            const Vector3 &measurement, const Evaluation &eval,
                            Linearization &lin) const {
  assert(lin.jacobians[0].size() == 1);
  assert(lin.jacobians[2].size() == 1);
  assert(lin.jacobians[3].size() == 1);

  lin.DF.noalias() = eval.d * eval.d.transpose();
  lin.DF.diagonal().array() -= 1;
  lin.DF /= -eval.DNorm;

  lin.jacobians[0][0].setZero(3, 6);
  Eigen::Map<Matrix3> JR(lin.jacobians[0][0].data());

  JR.row(0).noalias() = eval.D.cross(lin.DF.col(0));
  JR.row(1).noalias() = eval.D.cross(lin.DF.col(1));
  JR.row(2).noalias() = eval.D.cross(lin.DF.col(2));

  lin.DFR.middleCols(0, 3).noalias() = lin.DF * pose.R;
  lin.DFR.middleCols(3, 3).noalias() = -lin.DF * parent.R;

  lin.jacobians[2][0].setZero(3, 3);
  Eigen::Map<Matrix3> JJoint(lin.jacobians[2][0].data());

  JJoint.row(0).noalias() =
      -eval.vertex.tail<3>().cross(lin.DFR.block<1, 3>(0, 3));
  JJoint.row(1).noalias() =
      -eval.vertex.tail<3>().cross(lin.DFR.block<1, 3>(1, 3));
  JJoint.row(2).noalias() =
      -eval.vertex.tail<3>().cross(lin.DFR.block<1, 3>(2, 3));

  auto &JVertex = lin.jacobians[3][0];
  JVertex.noalias() = lin.DFR * mVDirs;

  lin.gPose.setZero();

  Eigen::Map<Vector3> gR(lin.gPose.data());
  auto& gJoint = lin.gJoint;
  auto& gVertex = lin.gVertex;

  gR.noalias() = JR.transpose() * eval.error;
  gJoint.noalias() = JJoint.transpose() * eval.error;
  gVertex.noalias() = JVertex.transpose() * eval.error;

#if not ROBUST_POF
  JR *= mSqrtCon;
  JJoint *= mSqrtCon;
  JVertex *= mSqrtCon;

  gR *= mCon;
  gJoint *= mCon;
  gVertex *= mCon;
#else
  preLinearizeRobustKernel(eval, lin);

  JR.noalias() -= lin.scaledError * gR.transpose();
  JJoint.noalias() -= lin.scaledError * gJoint.transpose();
  JVertex.noalias() -= lin.scaledError * gVertex.transpose();

  JR *= lin.sqrtDrho;
  JJoint *= lin.sqrtDrho;
  JVertex *= lin.sqrtDrho;

  gR *= lin.Drho;
  gJoint *= lin.Drho;
  gVertex *= lin.Drho;
#endif

  return 0;
}
}  // namespace scope


<|start_filename|>C++/scope/factor/ParameterFactor.h<|end_filename|>
#pragma once

#include <scope/factor/Factor.h>

namespace scope {
class ParameterFactor : public Factor {
public:
  struct Evaluation : public Factor::Evaluation {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    VectorX unscaledError;
  };

public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  SCOAT_FACTOR_EVALUATION_NEW
  SCOAT_FACTOR_LINEARIZATION_NEW

  ParameterFactor(int param, const MatrixX &sqrtCov, const VectorX &mean,
                  const std::string &name = "", int index = -1,
                  bool active = true);

  virtual int evaluate(const AlignedVector<Pose> &poses,
                       const AlignedVector<VectorX> &shapes,
                       const AlignedVector<Matrix3> &joints,
                       const AlignedVector<VectorX> &params,
                       Factor::Evaluation &base_eval) const override;

  virtual int linearize(const AlignedVector<Pose> &poses,
                        const AlignedVector<VectorX> &shapes,
                        const AlignedVector<Matrix3> &joints,
                        const AlignedVector<VectorX> &params,
                        const Factor::Evaluation &base_eval,
                        Factor::Linearization &base_lin) const override;

protected:
  MatrixX mSqrtCov;
  VectorX mMean;
};
} // namespace scope


<|start_filename|>C++/scope/factor/PoseConstFactor.h<|end_filename|>
#pragma once

#include <scope/factor/Factor.h>

namespace scope {
class PoseConstFactor : public Factor {
 public:
  struct Evaluation : public Factor::Evaluation {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    Matrix3 errorR;
    Vector6 xi;

    Vector6 errorL;
    Vector6 errorU;

    Vector6 errorLInv;
    Vector6 errorUInv;

    Vector6 expL;
    Vector6 expU;
  };

  struct Linearization : public Factor::Linearization {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    Matrix3 dexpinv;

    Vector6 Derror;
  };

 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  SCOAT_FACTOR_EVALUATION_NEW
  SCOAT_FACTOR_LINEARIZATION_NEW

  PoseConstFactor(int pose, const Vector6 &weight, const Pose &mean,
                  const Vector6 &lbnd, const Vector6 &ubnd,
                  const std::string &name = "", int index = -1,
                  bool active = true);

  virtual int evaluate(const AlignedVector<Pose> &poses,
                       const AlignedVector<VectorX> &shapes,
                       const AlignedVector<Matrix3> &joints,
                       const AlignedVector<VectorX> &params,
                       Factor::Evaluation &base_eval) const override;

  virtual int linearize(const AlignedVector<Pose> &poses,
                        const AlignedVector<VectorX> &shapes,
                        const AlignedVector<Matrix3> &joints,
                        const AlignedVector<VectorX> &params,
                        const Factor::Evaluation &base_eval,
                        Factor::Linearization &base_lin) const override;

 protected:
  Vector6 mWeight;
  Pose mMean;

  Vector6 mLowerBnd;
  Vector6 mUpperBnd;

  const Scalar mScale = 1e-2;
  const Scalar mBnd = -1e-6;
};
}  // namespace scope


<|start_filename|>C++/scope/factor/JointExtraPinholeCameraFactor.cpp<|end_filename|>
#include <glog/logging.h>

#include <scope/factor/JointExtraPinholeCameraFactor.h>

namespace scope {
JointExtraPinholeCameraFactor::JointExtraPinholeCameraFactor(
    int pose, const Scalar &sigma, const Scalar &eps, const Pose &extraCamPose,
    const Vector2 &measurement, const Scalar &confidence,
    const std::string &name, int index, bool active)
    : JointPinholeCameraFactor(pose, sigma, eps, measurement, confidence, name,
                               index, active),
      ExtraPinholeCamera(extraCamPose) {}

int JointExtraPinholeCameraFactor::evaluate(
    const AlignedVector<Pose> &poses, const AlignedVector<VectorX> &shapes,
    const AlignedVector<Matrix3> &joints, const AlignedVector<VectorX> &params,
    Factor::Evaluation &base_eval) const {
  auto &eval = dynamic_cast<Evaluation &>(base_eval);
  eval.clear();

  const auto &i = mvPoses[0];
  assert(i >= 0 && i < poses.size());

  if (i < 0 || i >= poses.size()) {
    LOG(ERROR) << "The pose must be valid." << std::endl;

    exit(-1);
  }

  evaluate(poses[i], mMeasurement, eval);

  eval.status = Status::VALID;

  return 0;
}

int JointExtraPinholeCameraFactor::linearize(
    const AlignedVector<Pose> &poses, const AlignedVector<VectorX> &shapes,
    const AlignedVector<Matrix3> &joints, const AlignedVector<VectorX> &params,
    const Factor::Evaluation &base_eval,
    Factor::Linearization &base_lin) const {
  auto &eval = dynamic_cast<const Evaluation &>(base_eval);
  auto &lin = dynamic_cast<Linearization &>(base_lin);

  lin.clear();

  assert(eval.status == Status::VALID);

  if (eval.status != Status::VALID) {
    LOG(ERROR) << "The evaluation must be valid." << std::endl;

    exit(-1);
  }

  const auto &i = mvPoses[0];
  assert(i >= 0 && i < poses.size());

  if (i < 0 || i >= poses.size()) {
    LOG(ERROR) << "The pose must be valid." << std::endl;

    exit(-1);
  }

  lin.jacobians[0].resize(1);

  linearize(poses[i], mMeasurement, eval, lin);

  lin.status = Status::VALID;

  return 0;
}

int JointExtraPinholeCameraFactor::evaluate(const Pose &pose,
                                            const Vector2 &measurement,
                                            Evaluation &eval) const {
  eval.pExtraCam = mExtraCamPose.t;
  eval.pExtraCam.noalias() += mExtraCamPose.R * pose.t;

  eval.point2D = eval.pExtraCam.head<2>() / eval.pExtraCam[2];
  eval.error = eval.point2D - measurement;
  eval.squaredErrorNorm = eval.error.squaredNorm();

#if not ROBUST_PINHOLE
  eval.f = mCon * eval.squaredErrorNorm;
#else
  eval.f = mSigmaCon * eval.squaredErrorNorm / (mSigma + eval.squaredErrorNorm);
#endif

  return 0;
}

int JointExtraPinholeCameraFactor::linearize(const Pose &pose,
                                             const Vector2 &measurement,
                                             const Evaluation &eval,
                                             Linearization &lin) const {
  assert(lin.jacobians[0].size() == 1);

  Scalar zinv = (Scalar)1.0 / eval.pExtraCam[2];

  lin.JPoint(0, 0) = zinv;
  lin.JPoint(0, 1) = 0;
  lin.JPoint(0, 2) = -eval.point2D[0] * zinv;
  lin.JPoint(1, 0) = 0;
  lin.JPoint(1, 1) = zinv;
  lin.JPoint(1, 2) = -eval.point2D[1] * zinv;

  auto &JPose = lin.jacobians[0][0];

  JPose.resize(2, 6);

  Eigen::Map<Matrix<2, 3>> JR(JPose.data());
  Eigen::Map<Matrix<2, 3>> Jt(JPose.data() + 6);

  Jt.noalias() = lin.JPoint * mExtraCamPose.R;

  JR.row(0) = pose.t.cross(Jt.row(0));
  JR.row(1) = pose.t.cross(Jt.row(1));

  auto &gPose = lin.gPose;
  gPose.noalias() = JPose.transpose() * eval.error;

#if not ROBUST_PINHOLE
  JPose *= mSqrtCon;
  gPose *= mCon;
#else
  preLinearizeRobustKernel(eval, lin);

  JPose.noalias() -= lin.scaledError * gPose.transpose();

  JPose *= lin.sqrtDrho;
  gPose *= lin.Drho;
#endif

  return 0;
}
} // namespace scope


<|start_filename|>C++/scope/model/Model.h<|end_filename|>
#pragma once

#include <scope/base/Pose.h>
#include <scope/model/Link.h>

namespace scope {
template <int K, int P, int N>
class Model {
 public:
  // number of joints
  const static int NumJoints = K;
  // number of implicit parameters
  const static int NumParams = P;
  // number of vertices
  const static int NumVertices = N;

 protected:
  // human model
  HumanModel mHuman;

  const MatrixX mRawRelJDirs;
  const VectorX mRawRelJ;
  const MatrixX mRawVDirs;
  const VectorX mRawV;

  // linear map from shape parameters to joint locations
  const Eigen::Map<const Matrix<3 * K, P>> mRelJDirs;
  const Eigen::Map<const Vector<3 * K>> mRelJ;

  AlignedVector<Matrix<3, P>> mvRelJDirs;
  AlignedVector<Vector3> mvRelJ;

  // linear map from shape parameters to mesh vertices
  const Eigen::Map<const Matrix<3 * N, P>> mVDirs;
  const Eigen::Map<const Vector<3 * N>> mV;

  AlignedVector<Matrix<3, P>> mvVDirs;
  AlignedVector<Vector3> mvV;

  // body parts
  std::vector<Link> mvLinks;

  // pairs of body parts to check collisions
  std::vector<std::array<int, 2>> mvCollisionPairs;

 protected:
  Model(const HumanModel& human, const MatrixX& RelJDirs, const VectorX& RelJ,
        const MatrixX& VDirs, const VectorX& v);

  Model(const Model<K, P, N>& model);
  Model(Model<K, P, N>&& model);
  Model& operator=(const Model<K, P, N>& model) = delete;
  Model& operator=(Model<K, P, N>&& model) = delete;

  virtual int setKinematicsTree() = 0;

  virtual int FKIterate(AlignedVector<Pose>& T, int i, const Pose& T0,
                        const AlignedVector<Matrix3>& Omega,
                        const Eigen::Map<Vector<3 * K>>& mu) const;

  virtual int DFKIterate(AlignedVector<Matrix<3, P>>& Bp,
                         AlignedVector<Matrix63>& Bu, int i,
                         const AlignedVector<Pose>& T) const;

  virtual int DFKIterate(AlignedVector<Eigen::Map<Matrix<6, P + 3>>>& B, int i,
                         const AlignedVector<Pose>& T) const;

 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  const HumanModel& getHumanModel() const;

  const Eigen::Map<const Matrix<3 * K, P>>& getRelJDirs() const;
  const Eigen::Map<const Vector<3 * K>>& getRelJ() const;

  const Eigen::Map<const Matrix<3 * N, P>>& getVDirs() const;
  const Eigen::Map<const Vector<3 * N>>& getV() const;

  const std::vector<Link>& getLinks() const;

  const std::vector<std::array<int, 2>>& getCollisionsPairs() const;

  virtual const std::vector<int> & getKinematicsTree() const = 0;

  // ---------------------------------------------------------
  // forward kinematics
  // ---------------------------------------------------------
  // T: body part poses
  // T0: the root pose
  // Omega: joint states
  // mu: joint locations
  virtual int FK(AlignedVector<Pose>& T, Eigen::Map<Vector<3 * K>>& mu,
                 const Pose& T0, const AlignedVector<Matrix3>& Omega,
                 const VectorX& beta) const;

  // ---------------------------------------------------------
  // linearization of forward kinematics
  // ---------------------------------------------------------
  virtual int DFK(AlignedVector<Matrix<3, P>>& Bp, AlignedVector<Matrix63>& Bu,
                  const AlignedVector<Pose>& T) const;

  virtual int DFK(AlignedVector<Eigen::Map<Matrix<6, P + 3>>>& B,
                  const AlignedVector<Pose>& T) const;
};

extern template class Model<2, 2, 20>;
extern template class Model<23, 10, 6890>;
extern template class Model<51, 10, 6890>;
}  // namespace scope


<|start_filename|>C++/scope/factor/JointLimitFactor.h<|end_filename|>
#pragma once

#include <scope/factor/Factor.h>

namespace scope {
class JointLimitFactor : public Factor {
public:
  struct Evaluation : public Factor::Evaluation {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    AlignedVector<Eigen::Array<Scalar, 6, 1>> outputs;
    AlignedVector<Eigen::Array<Scalar, 6, 1>> fin;
    AlignedVector<Eigen::Array<Scalar, 6, 1>> fexp[2];
    AlignedVector<Eigen::Array<Scalar, 6, 1>> flog;
    AlignedVector<Eigen::Array<bool, 6, 1>> selected;

    virtual int clear() override;
  };

  struct Linearization : public Factor::Linearization {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    AlignedVector<Matrix6> derivatives;
    AlignedVector<Vector6> slopes;

    virtual int clear() override;
  };

public:
  SCOAT_FACTOR_EVALUATION_NEW
  SCOAT_FACTOR_LINEARIZATION_NEW

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  JointLimitFactor(int joint, const AlignedVector<Matrix6> &NNWeight,
                   const AlignedVector<Vector6> &NNBias, const VectorX &a,
                   const VectorX &b, Scalar scale = 1.0,
                   const std::string &name = "", int index = -1,
                   bool active = true);

  virtual int evaluate(const AlignedVector<Pose> &poses,
                       const AlignedVector<VectorX> &shapes,
                       const AlignedVector<Matrix3> &joints,
                       const AlignedVector<VectorX> &params,
                       Factor::Evaluation &base_eval) const override;

  virtual int linearize(const AlignedVector<Pose> &poses,
                        const AlignedVector<VectorX> &shapes,
                        const AlignedVector<Matrix3> &joints,
                        const AlignedVector<VectorX> &params,
                        const Factor::Evaluation &base_eval,
                        Factor::Linearization &base_lin) const override;

protected:
  int mnLayers;
  int mnInnerLayers;

  // weights for linear transformation
  AlignedVector<Matrix6> NNWeight;
  // biases for linear transformation
  AlignedVector<Vector6> NNBias;
  // parameters for PRelu
  VectorX NNPReLUScale[2];
  VectorX NNPReLURate;

protected:
  virtual int evaluate(const Matrix3 &Omega, Evaluation &eval) const;

  virtual int linearize(const Matrix3 &Omega, const Evaluation &eval,
                        Linearization &lin) const;
};
} // namespace scope


<|start_filename|>C++/scope/factor/PoseFactor.h<|end_filename|>
#pragma once

#include <scope/factor/Factor.h>

namespace scope {
class PoseFactor : public Factor {
public:
  struct Evaluation : public Factor::Evaluation {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    Matrix3 errorR;
    Vector6 xi;
  };

  struct Linearization : public Factor::Linearization {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    Matrix3 dexpinv;
    Matrix3 dexpinvR;
  };

public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  SCOAT_FACTOR_EVALUATION_NEW
  SCOAT_FACTOR_LINEARIZATION_NEW

  PoseFactor(int pose, const Matrix6 &sqrtCov, const Pose &mean,
             const std::string &name = "", int index = -1, bool active = true);

  virtual int evaluate(const AlignedVector<Pose> &poses,
                       const AlignedVector<VectorX> &shapes,
                       const AlignedVector<Matrix3> &joints,
                       const AlignedVector<VectorX> &params,
                       Factor::Evaluation &base_eval) const override;

  virtual int linearize(const AlignedVector<Pose> &poses,
                        const AlignedVector<VectorX> &shapes,
                        const AlignedVector<Matrix3> &joints,
                        const AlignedVector<VectorX> &params,
                        const Factor::Evaluation &base_eval,
                        Factor::Linearization &base_lin) const override;

protected:
  Matrix6 mSqrtCov;
  Pose mMean;
};
} // namespace scope


<|start_filename|>C++/scope/model/Model.cpp<|end_filename|>
#include <scope/model/Model.h>

namespace scope {
template <int K, int P, int N>
Model<K, P, N>::Model(const HumanModel& human, const MatrixX& RelJDirs,
                      const VectorX& RelJ, const MatrixX& VDirs,
                      const VectorX& V)
    : mHuman(human),
      mRawRelJDirs(RelJDirs),
      mRawRelJ(RelJ),
      mRawVDirs(VDirs),
      mRawV(V),
      mRelJDirs(mRawRelJDirs.data()),
      mRelJ(mRawRelJ.data()),
      mVDirs(mRawVDirs.data()),
      mV(mRawV.data()) {
  assert(mRelJDirs.rows() == 3 * K && mRelJDirs.cols() == P);
  assert(mVDirs.rows() == 3 * N && mVDirs.cols() == P);
  assert(mRelJ.rows() == 3 * K);
  assert(mV.rows() == 3 * N);

  mvRelJDirs.resize(K);
  mvRelJ.resize(K);

  for (int i = 0; i < K; i++) {
    mvRelJDirs[i] = mRelJDirs.template middleRows<3>(3 * i);
    mvRelJ[i] = mRelJ.template segment<3>(3 * i);
  }

  mvVDirs.resize(N);
  mvV.resize(N);

  for (int i = 0; i < N; i++) {
    mvVDirs[i] = mVDirs.middleRows(3 * i, 3);
    mvV[i] = mV.segment(3 * i, 3);
  }
}

template <int K, int P, int N>
Model<K, P, N>::Model(const Model<K, P, N>& model)
    : mHuman(model.mHuman),
      mRawRelJDirs(model.mRawRelJDirs),
      mRawRelJ(model.mRawRelJ),
      mRawVDirs(model.mRawVDirs),
      mRawV(model.mRawV),
      mRelJDirs(mRawRelJDirs.data()),
      mRelJ(mRawRelJ.data()),
      mVDirs(mRawVDirs.data()),
      mV(mRawV.data()),
      mvRelJDirs(model.mvRelJDirs),
      mvRelJ(model.mvRelJ),
      mvVDirs(model.mvVDirs),
      mvV(model.mvV),
      mvLinks(model.mvLinks),
      mvCollisionPairs(model.mvCollisionPairs) {}

template <int K, int P, int N>
Model<K, P, N>::Model(Model<K, P, N>&& model)
    : mHuman(std::move(model.mHuman)),
      mRawRelJDirs(std::move(model.mRawRelJDirs)),
      mRawRelJ(std::move(model.mRawRelJ)),
      mRawVDirs(std::move(model.mRawVDirs)),
      mRawV(std::move(model.mRawV)),
      mRelJDirs(std::move(model.mRelJDirs)),
      mRelJ(std::move(model.mRelJ)),
      mVDirs(std::move(model.mVDirs)),
      mV(std::move(model.mV)),
      mvRelJDirs(std::move(model.mvRelJDirs)),
      mvRelJ(std::move(model.mvRelJ)),
      mvVDirs(std::move(model.mvVDirs)),
      mvV(std::move(model.mvV)),
      mvLinks(std::move(model.mvLinks)),
      mvCollisionPairs(std::move(model.mvCollisionPairs)) {}

template <int K, int P, int N>
const HumanModel& Model<K, P, N>::getHumanModel() const {
  return mHuman;
}

template <int K, int P, int N>
const Eigen::Map<const Matrix<3 * K, P>>& Model<K, P, N>::getRelJDirs() const {
  return mRelJDirs;
}

template <int K, int P, int N>
const Eigen::Map<const Vector<3 * K>>& Model<K, P, N>::getRelJ() const {
  return mRelJ;
}

template <int K, int P, int N>
const Eigen::Map<const Matrix<3 * N, P>>& Model<K, P, N>::getVDirs() const {
  return mVDirs;
}

template <int K, int P, int N>
const Eigen::Map<const Vector<3 * N>>& Model<K, P, N>::getV() const {
  return mV;
}

template <int K, int P, int N>
const std::vector<Link>& Model<K, P, N>::getLinks() const {
  return mvLinks;
}

template <int K, int P, int N>
const std::vector<std::array<int, 2>>& Model<K, P, N>::getCollisionsPairs()
    const {
  return mvCollisionPairs;
}

template <int K, int P, int N>
int Model<K, P, N>::FKIterate(AlignedVector<Pose>& T, int i, const Pose& T0,
                              const AlignedVector<Matrix3>& Omega,
                              const Eigen::Map<Vector<3 * K>>& mu) const {
  assert(T.size() == K + 1);
  assert(i >= 1 && i <= K);
  assert(Omega.size() == K);
  assert(mu.size() == 3 * K);

  const auto& link = mvLinks[i];

  assert(i == link.id());

  const auto& ii = link.joint();
  const auto& parent = link.parent();

  const auto& Tpar = T[parent];
  auto& Ti = T[i];

  Ti.R.noalias() = Tpar.R * Omega[ii];
  Ti.t = Tpar.t;
  Ti.t.noalias() += Tpar.R * mu.template segment<3>(3 * ii);

  return 0;
}

template <int K, int P, int N>
int Model<K, P, N>::DFKIterate(AlignedVector<Matrix<3, P>>& Bp,
                               AlignedVector<Matrix63>& Bu, int i,
                               const AlignedVector<Pose>& T) const {
  assert(Bp.size() == K);
  assert(Bu.size() == K);
  assert(i >= 1 && i <= K);
  assert(T.size() == K + 1);

  const auto& link = mvLinks[i];

  assert(i == link.id());

  const auto& ii = link.joint();
  const auto& parent = link.parent();

  const auto& Tpar = T[parent];
  const auto& Ti = T[i];

  Bp[ii].noalias() = Tpar.R * mvRelJDirs[ii];

  Bu[ii].template topRows<3>() = Tpar.R;
  Bu[ii].template block<3, 1>(3, 0).noalias() = Ti.t.cross(Tpar.R.col(0));
  Bu[ii].template block<3, 1>(3, 1).noalias() = Ti.t.cross(Tpar.R.col(1));
  Bu[ii].template block<3, 1>(3, 2).noalias() = Ti.t.cross(Tpar.R.col(2));

  return 0;
}

template <int K, int P, int N>
int Model<K, P, N>::DFKIterate(AlignedVector<Eigen::Map<Matrix<6, P + 3>>>& B,
                               int i, const AlignedVector<Pose>& T) const {
  assert(B.size() == K);
  assert(B.size() == K);
  assert(i >= 1 && i <= K);
  assert(T.size() == K + 1);

  const auto& link = mvLinks[i];

  assert(i == link.id());

  const auto& ii = link.joint();
  const auto& parent = link.parent();

  const auto& Tpar = T[parent];
  const auto& Ti = T[i];

  B[ii].template topLeftCorner<3, P>().setZero();
  B[ii].template bottomLeftCorner<3, P>().noalias() = Tpar.R * mvRelJDirs[ii];

  B[ii].template topRightCorner<3, 3>() = Tpar.R;
  B[ii].template block<3, 1>(3, P + 0).noalias() = Ti.t.cross(Tpar.R.col(0));
  B[ii].template block<3, 1>(3, P + 1).noalias() = Ti.t.cross(Tpar.R.col(1));
  B[ii].template block<3, 1>(3, P + 2).noalias() = Ti.t.cross(Tpar.R.col(2));

  return 0;
}

template <int K, int P, int N>
int Model<K, P, N>::FK(AlignedVector<Pose>& T, Eigen::Map<Vector<3 * K>>& mu,
                       const Pose& T0, const AlignedVector<Matrix3>& Omega,
                       const VectorX& beta) const {
  assert(Omega.size() == K);
  assert(beta.size() == P);

  T.resize(K + 1);

  mu = mRelJ;
  mu.noalias() += mRelJDirs * beta.head<P>();

  T[0] = T0;

  for (int i = 1; i <= K; i++) {
    FKIterate(T, i, T0, Omega, mu);
  }

  return 0;
}

template <int K, int P, int N>
int Model<K, P, N>::DFK(AlignedVector<Matrix<3, P>>& Bp,
                        AlignedVector<Matrix63>& Bu,
                        const AlignedVector<Pose>& T) const {
  assert(T.size() == K + 1);

  Bp.resize(K);
  Bu.resize(K);

  for (int i = 1; i <= K; i++) {
    DFKIterate(Bp, Bu, i, T);
  }

  return 0;
}

template <int K, int P, int N>
int Model<K, P, N>::DFK(AlignedVector<Eigen::Map<Matrix<6, P + 3>>>& B,
                        const AlignedVector<Pose>& T) const {
  assert(T.size() == K + 1);

  assert(B.size() == K);

  for (int i = 1; i <= K; i++) {
    DFKIterate(B, i, T);
  }

  return 0;
}


template <int K, int P, int N> const int Model<K, P, N>::NumJoints;
template <int K, int P, int N> const int Model<K, P, N>::NumParams;
template <int K, int P, int N> const int Model<K, P, N>::NumVertices;

template class Model<2, 2, 20>;
template class Model<23, 10, 6890>;
template class Model<51, 10, 6890>;
template class Model<23, 11, 6890>;
template class Model<51, 11, 6890>;
}  // namespace scope


<|start_filename|>C++/scope/initializer/factor/JointLimitFactor.cpp<|end_filename|>
#include <glog/logging.h>

#include <scope/initializer/factor/JointLimitFactor.h>
#include <scope/math/skew3.h>

namespace scope {
namespace Initializer {
int JointLimitFactor::Evaluation::clear() {
  Factor::Evaluation::clear();

  outputs.clear();
  fin.clear();
  fexp[0].clear();
  fexp[1].clear();
  flog.clear();
  selected.clear();

  return 0;
}

int JointLimitFactor::Linearization::clear() {
  Factor::Linearization::clear();

  derivatives.clear();
  slopes.clear();

  return 0;
}

JointLimitFactor::JointLimitFactor(int joint, const AlignedVector<Matrix6> &W,
                                   const AlignedVector<Vector6> &w,
                                   const VectorX &a, const VectorX &b,
                                   Scalar scale, const std::string &name,
                                   int index)
    : Factor(name, index), mJoint(joint), mnLayers(W.size()),
      mnInnerLayers(W.size() - 1), NNWeight(W), NNBias(w), NNPReLURate(b) {
  assert(NNWeight.size() == NNBias.size());
  assert(mnLayers > 0);
  assert(a.size() == mnInnerLayers);
  assert(b.size() == mnInnerLayers);

  NNPReLUScale[0] = (1 - a.array()) / b.array();
  NNPReLUScale[1] = a;

  JointLimitFactor::Evaluation eval;
  eval.outputs.resize(mnInnerLayers);
  eval.fin.resize(mnInnerLayers);
  eval.fexp[0].resize(mnInnerLayers);
  eval.fexp[1].resize(mnInnerLayers);
  eval.flog.resize(mnInnerLayers);
  eval.selected.resize(mnInnerLayers);

  Matrix3 Omega = Matrix3::Identity();
  evaluate(Omega, eval);
  Scalar fsqrt = std::max(std::sqrt(eval.f), 1e-4);

  NNBias[mnInnerLayers] *= scale;
  NNWeight[mnInnerLayers] *= scale;
  NNBias[mnInnerLayers] /= fsqrt;
  NNWeight[mnInnerLayers] /= fsqrt;
}

int JointLimitFactor::evaluate(const AlignedVector<Pose> &poses,
                               const AlignedVector<Matrix3> &joints,
                               Factor::Evaluation &base_eval) const {
  auto &eval = dynamic_cast<Evaluation &>(base_eval);
  eval.clear();

  const auto &i = mJoint;
  assert(i >= 0 && i < joints.size());

  if (i >= joints.size()) {
    LOG(ERROR) << "The joint must be valid." << std::endl;

    exit(-1);
  }

  assert(mnLayers >= 0);

  eval.outputs.resize(mnInnerLayers);
  eval.fin.resize(mnInnerLayers);
  eval.fexp[0].resize(mnInnerLayers);
  eval.fexp[1].resize(mnInnerLayers);
  eval.flog.resize(mnInnerLayers);
  eval.selected.resize(mnInnerLayers);

  evaluate(joints[i], eval);

  eval.status = Status::VALID;

  return 0;
}

int JointLimitFactor::linearize(const AlignedVector<Pose> &poses,
                                const AlignedVector<Matrix3> &joints,
                                const Factor::Evaluation &base_eval,
                                Factor::Linearization &base_lin) const {
  auto &eval = dynamic_cast<const Evaluation &>(base_eval);
  auto &lin = dynamic_cast<Linearization &>(base_lin);

  lin.clear();

  assert(eval.status == Status::VALID);

  if (eval.status != Status::VALID) {
    LOG(ERROR) << "The evaluation must be valid." << std::endl;

    exit(-1);
  }

  const auto &i = mJoint;
  assert(i >= 0 && i < joints.size());

  if (i < 0 || i >= joints.size()) {
    LOG(ERROR) << "The joint must be valid." << std::endl;

    exit(-1);
  }

  assert(mnLayers > 0);

  lin.derivatives.resize(mnLayers);
  lin.slopes.resize(mnInnerLayers);

  linearize(joints[i], eval, lin);

  lin.status = Status::VALID;

  return 0;
}

int JointLimitFactor::evaluate(const Matrix3 &Omega, Evaluation &eval) const {
  Eigen::Map<const Vector6> input(Omega.data(), 6);

  eval.outputs[0] = NNBias[0];
  eval.outputs[0].matrix().noalias() += NNWeight[0] * input;

  eval.fin[0] = eval.outputs[0] * NNPReLURate[0];
  eval.selected[0] = eval.fin[0] >= 10;

  eval.fexp[0][0] = eval.selected[0].select(1e15, eval.fin[0].exp());
  eval.fexp[1][0] = eval.fexp[0][0] + 1;
  eval.fexp[0][0] += NNPReLUScale[1][0];

  eval.flog[0] = eval.selected[0].select(eval.fin[0], eval.fexp[1][0].log());

  eval.outputs[0] =
      NNPReLUScale[0][0] * eval.flog[0] + NNPReLUScale[1][0] * eval.outputs[0];

  for (int i = 1, ii = 0; i < mnInnerLayers; i++, ii++) {
    eval.outputs[i] = NNBias[i];
    eval.outputs[i].matrix().noalias() +=
        NNWeight[i] * eval.outputs[ii].matrix();

    eval.fin[i] = eval.outputs[i] * NNPReLURate[i];
    eval.selected[i] = eval.fin[i] >= 10;

    eval.fexp[0][i] = eval.selected[i].select(1e15, eval.fin[i].exp());
    eval.fexp[1][i] = eval.fexp[0][i] + 1;
    eval.fexp[0][i] += NNPReLUScale[1][i];

    eval.flog[i] = eval.selected[i].select(eval.fin[i], eval.fexp[1][i].log());

    eval.outputs[i] = NNPReLUScale[0][i] * eval.flog[i] +
                      NNPReLUScale[1][i] * eval.outputs[i];
  }

  eval.error.resize(6);
  Eigen::Map<Vector6> error(eval.error.data());

  error = NNBias[mnInnerLayers];
  error.noalias() += NNWeight[mnInnerLayers] * eval.outputs.back().matrix();

  eval.f = eval.error.squaredNorm();

  return 0;
}

int JointLimitFactor::linearize(const Matrix3 &Omega, const Evaluation &eval,
                                Linearization &lin) const {
  lin.jacobian.setZero(6, 3);

  lin.derivatives[mnInnerLayers] = NNWeight[mnInnerLayers];

  for (int i = mnInnerLayers, ii = mnInnerLayers - 1; i > 0; i--, ii--) {
    lin.slopes[ii] = eval.fexp[0][ii] / eval.fexp[1][ii];
    lin.derivatives[i] *= lin.slopes[ii].asDiagonal();
    lin.derivatives[ii].noalias() = lin.derivatives[i] * NNWeight[ii];
  }

  math::skew3::multR<math::OPS::SUB>(
      Omega.col(0), lin.derivatives[0].leftCols<3>(), lin.jacobian);
  math::skew3::multR<math::OPS::SUB>(
      Omega.col(1), lin.derivatives[0].rightCols<3>(), lin.jacobian);

  return 0;
}
} // namespace Initializer
} // namespace scope


<|start_filename|>C++/scope/initializer/HeadInitializer.cpp<|end_filename|>
#include <glog/logging.h>

#include <memory>
#include <scope/initializer/HeadInitializer.h>

namespace scope {
namespace Initializer {
HeadInitializer::HeadInitializer(const Options &options)
    : Initializer(0, options) {}

int HeadInitializer::updateGaussNewton() const {
  mH = mvMxx[0];
  mh = mvmx[0];

  return 0;
}

int HeadInitializer::update(Scalar stepsize) const {
  assert(stepsize > 0);

  mDRootPoseChange = mhGN * stepsize;
  Pose::exp(mDRootPoseChange, mRootPoseChange);

  mvPoses[1][0].R.noalias() = mRootPoseChange.R * mvPoses[0][0].R;
  mvPoses[1][0].t = mRootPoseChange.t;
  mvPoses[1][0].t.noalias() += mRootPoseChange.R * mvPoses[0][0].t;

  return 0;
}

} // namespace Initializer
} // namespace scope


<|start_filename|>C++/scope/math/macro.h<|end_filename|>
#ifndef _MACRO_H
#define _MACRO_H

#define EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_ROWS(TYPE, ROWS) \
  EIGEN_STATIC_ASSERT(TYPE::RowsAtCompileTime == ROWS,       \
                      "THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_SPECIFIC_ROWS")

#define EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_COLS(TYPE, COLS) \
  EIGEN_STATIC_ASSERT(TYPE::ColsAtCompileTime == COLS,       \
                      "THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_SPECIFIC_COLS")

#endif


<|start_filename|>C++/scope/initializer/factor/Factor.cpp<|end_filename|>
#include <scope/initializer/factor/Factor.h>

namespace scope {
namespace Initializer {
Factor::Evaluation::Evaluation() : status(Status::INVALID) {}

int Factor::Evaluation::reset() {
  status = Status::INVALID;

  return 0;
}

int Factor::Evaluation::clear() {
  reset();

  error.resize(0);

  return 0;
}

Factor::Linearization::Linearization() : status(Status::INVALID) {}

int Factor::Linearization::reset() {
  status = Status::INVALID;

  return 0;
}

int Factor::Linearization::clear() {
  reset();

  return 0;
}

Factor::Factor(const std::string &name, int index)
    : mIndex(index), mName(name) {}
} // namespace Initializer
} // namespace scope


<|start_filename|>C++/scope/initializer/factor/POFFactor.cpp<|end_filename|>
#include <glog/logging.h>

#include <scope/initializer/factor/POFFactor.h>

namespace scope {
namespace Initializer {
POFFactor::POFFactor(int pose, const Vector3 &s, const Scalar &sigma,
                     const Scalar &eps, const Vector3 &measurement,
                     const Scalar &confidence, const std::string &name,
                     int index)
    : Factor(name, index), mPose(pose), mMeasurement(measurement),
      mSigma(sigma), mCon(confidence), mSqrtCon(std::sqrt(confidence)),
      mSigmaCon(confidence * sigma), mMinAlpha(eps), mS(s) {
  mGMThreshold = (1 - eps * eps) / (3 + eps * eps) * sigma;
  mMeasurement.stableNormalize();
  mS.stableNormalize();
}

int POFFactor::evaluate(const AlignedVector<Pose> &poses,
                        const AlignedVector<Matrix3> &joints,
                        Factor::Evaluation &base_eval) const {
  auto &eval = dynamic_cast<Evaluation &>(base_eval);
  eval.clear();

  const auto &i = mPose;

  assert(i >= 0 && i < poses.size());

  if (i >= poses.size()) {
    LOG(ERROR) << "The pose must be valid." << std::endl;

    exit(-1);
  }

  auto &pose = poses[i];

  eval.error.resize(1);

  eval.d.noalias() = pose.R * mS;
  eval.error[0] = 1 - mMeasurement.dot(eval.d);
  eval.squaredErrorNorm = eval.error.squaredNorm();

#if 0
  eval.f = mCon * eval.squaredErrorNorm;
#else
  eval.f = mSigmaCon * eval.squaredErrorNorm / (mSigma + eval.squaredErrorNorm);
#endif

  eval.status = Status::VALID;

  return 0;
}

int POFFactor::linearize(const AlignedVector<Pose> &poses,
                         const AlignedVector<Matrix3> &joints,
                         const Factor::Evaluation &base_eval,
                         Factor::Linearization &base_lin) const {
  auto &eval = dynamic_cast<const Evaluation &>(base_eval);
  auto &lin = dynamic_cast<Linearization &>(base_lin);

  lin.clear();

  const auto &i = mPose;

  assert(i >= 0 && i < poses.size());

  if (i >= poses.size()) {
    LOG(ERROR) << "The pose must be valid." << std::endl;

    exit(-1);
  }

  assert(eval.status == Status::VALID);

  if (eval.status != Status::VALID) {
    LOG(ERROR) << "The evaluation must be valid." << std::endl;

    exit(-1);
  }

  lin.jacobian.setZero();

  Eigen::Map<Vector3> JR(lin.jacobian.data());

  JR.noalias() = mMeasurement.cross(eval.d);

#if 0
  JR *= mSqrtCon;
  lin.scaledError = mSqrtCon * eval.error[0];
#else
  lin.sqrtDrho = mSigma / (mSigma + eval.squaredErrorNorm);
  lin.alpha = eval.squaredErrorNorm > mGMThreshold
                  ? mMinAlpha
                  : std::sqrt(1 - 4 * eval.squaredErrorNorm /
                                      (mSigma + eval.squaredErrorNorm));

  lin.sqrtDrho *= mSqrtCon;
  lin.scaledError = lin.sqrtDrho * eval.error[0] / lin.alpha;

  JR *= lin.sqrtDrho * lin.alpha;
#endif

  lin.status = Status::VALID;

  return 0;
}
} // namespace Initializer
} // namespace scope


<|start_filename|>C++/scope/factor/PinholeCameraFactor.h<|end_filename|>
#pragma once

#define ROBUST_PINHOLE 1

#include <scope/factor/Factor.h>

namespace scope {
class PinholeCameraFactor : public Factor {
public:
  struct Evaluation : public Factor::Evaluation {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    Vector2 point2D;

    Scalar squaredErrorNorm;
  };

  struct Linearization : public Factor::Linearization {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    Scalar Drho;
    Scalar sqrtDrho;
    Scalar alpha;

    Vector2 scaledError;

    Vector6 gPose;
  };

public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  SCOAT_FACTOR_EVALUATION_NEW
  SCOAT_FACTOR_LINEARIZATION_NEW

  PinholeCameraFactor(const std::vector<int> &poses,
                      const std::vector<int> &shapes,
                      const std::vector<int> &joints,
                      const std::vector<int> &params, const Scalar &sigma,
                      const Scalar &eps, const Vector2 &measurement,
                      const Scalar &confidence, const std::string &name = "",
                      int index = -1, bool active = true);

protected:
  virtual int preLinearizeRobustKernel(const Evaluation &eval,
                                       Linearization &lin) const;

protected:
  Vector2 mMeasurement;

  Scalar mCon;
  Scalar mSqrtCon;
  Scalar mSigmaCon;

  Scalar mSigma;
  Scalar mGMThreshold;
  Scalar mMaxGMScale;
};
} // namespace scope


<|start_filename|>C++/scope/factor/VertexDepthCameraFactor.h<|end_filename|>
#pragma once

#include <scope/factor/DepthCameraFactor.h>

namespace scope {
class VertexDepthCameraFactor : public DepthCameraFactor {
public:
  struct Evaluation : public DepthCameraFactor::Evaluation {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    // vertex position in the body frame
    Vector3 vertex;
    // vertex position in the camera frame
    Vector3 point3D;
  };

  struct Linearization : public DepthCameraFactor::Linearization {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    VectorX gVertex;
  };

public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  SCOAT_FACTOR_EVALUATION_NEW
  SCOAT_FACTOR_LINEARIZATION_NEW

  VertexDepthCameraFactor(int pose, int vParam, const Matrix3X &vDirs,
                            const Vector3 &v, const Scalar &sigma,
                            const Scalar &eps, const Vector3 &measurement,
                            const Scalar &confidence = 1.0,
                            const std::string &name = "", int index = -1,
                            bool active = true);

  virtual int evaluate(const AlignedVector<Pose> &poses,
                       const AlignedVector<VectorX> &shapes,
                       const AlignedVector<Matrix3> &joints,
                       const AlignedVector<VectorX> &params,
                       Factor::Evaluation &base_eval) const override;

  virtual int linearize(const AlignedVector<Pose> &poses,
                        const AlignedVector<VectorX> &shapes,
                        const AlignedVector<Matrix3> &joints,
                        const AlignedVector<VectorX> &params,
                        const Factor::Evaluation &base_eval,
                        Factor::Linearization &base_lin) const override;

protected:
  Matrix3X mvDirs;
  Vector3 mv;

protected:
  virtual int evaluate(const Pose &pose, const VectorX &vertexParam,
                       const Vector3 &measurement, Evaluation &eval) const;

  virtual int linearize(const Pose &pose, const VectorX &vertexParam,
                        const Vector3 &measurement, const Evaluation &eval,
                        Linearization &lin) const;
};
}


<|start_filename|>C++/scope/initializer/factor/POFFactor.h<|end_filename|>
#pragma once

#include <scope/initializer/factor/Factor.h>

namespace scope {
namespace Initializer {
class POFFactor : public Factor {
public:
  struct Evaluation : public Factor::Evaluation {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    Vector3 d; // direction

    Scalar squaredErrorNorm;
  };

  struct Linearization : public Factor::Linearization {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    Matrix<1, 3> jacobian;

    Scalar sqrtDrho;
    Scalar alpha;
    Scalar scaledError;
  };

public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  SCOAT_INITIAL_EVALUATION_NEW
  SCOAT_INITIAL_LINEARIZATION_NEW

  POFFactor(int pose, const Vector3 &S, const Scalar &sigma, const Scalar &eps,
            const Vector3 &measurement, const Scalar &confidence = 1.0,
            const std::string &name = "", int index = -1);

  virtual int evaluate(const AlignedVector<Pose> &poses,
                       const AlignedVector<Matrix3> &joints,
                       Factor::Evaluation &base_eval) const override;

  virtual int linearize(const AlignedVector<Pose> &poses,
                        const AlignedVector<Matrix3> &joints,
                        const Factor::Evaluation &base_eval,
                        Factor::Linearization &base_lin) const override;

  int getPose() const { return mPose; }

protected:
  int mPose;

  Vector3 mMeasurement;

  Scalar mCon;
  Scalar mSqrtCon;
  Scalar mSigmaCon;

  Scalar mSigma;
  Scalar mGMThreshold;
  Scalar mMinAlpha;

  Vector3 mS;
};
} // namespace Initializer
} // namespace scope


<|start_filename|>C++/scope/factor/EulerAngleConstFactor.h<|end_filename|>
#pragma once

#include <scope/factor/JointConstFactor.h>

namespace scope {
class EulerAngleConstFactor : public JointConstFactor {
 public:
  struct Linearization : public JointConstFactor::Linearization {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    Matrix3 H;
    Matrix3 D;
  };

 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  SCOAT_FACTOR_EVALUATION_NEW
  SCOAT_FACTOR_LINEARIZATION_NEW

  EulerAngleConstFactor(int joint, const Vector3 &weight, const Matrix3 &mean,
                        const Vector3 &lbnd, const Vector3 &ubnd,
                        const std::string &name = "", int index = -1,
                        bool active = true);

  virtual int evaluate(const AlignedVector<Pose> &poses,
                       const AlignedVector<VectorX> &shapes,
                       const AlignedVector<Matrix3> &joints,
                       const AlignedVector<VectorX> &params,
                       Factor::Evaluation &base_eval) const override;

  virtual int linearize(const AlignedVector<Pose> &poses,
                        const AlignedVector<VectorX> &shapes,
                        const AlignedVector<Matrix3> &joints,
                        const AlignedVector<VectorX> &params,
                        const Factor::Evaluation &base_eval,
                        Factor::Linearization &base_lin) const override;
};
}  // namespace scope


<|start_filename|>C++/scope/math/skew3.h<|end_filename|>
#pragma once

#include <Eigen/Dense>

#include <scope/math/macro.h>
#include <scope/math/operation.h>

namespace scope {
namespace math {
struct skew3 {
  template <math::OPS op, typename T1, typename T2>
  static Eigen::MatrixBase<T2>& set(Eigen::MatrixBase<T1> const& p,
                                    Eigen::MatrixBase<T2>& res) {
    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(T1, 3)
    EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(T2, 3, 3)

    switch (op) {
      case OPS::EQL:
        res(0, 0) = 0;
        res(0, 1) = -p[2];
        res(0, 2) = p[1];
        res(1, 0) = p[2];
        res(1, 1) = 0;
        res(1, 2) = -p[0];
        res(2, 0) = -p[1];
        res(2, 1) = p[0];
        res(2, 2) = 0;

        break;

      case OPS::ADD:
        res(0, 0) += 0;
        res(0, 1) += -p[2];
        res(0, 2) += p[1];
        res(1, 0) += p[2];
        res(1, 1) += 0;
        res(1, 2) += -p[0];
        res(2, 0) += -p[1];
        res(2, 1) += p[0];
        res(2, 2) += 0;

        break;

      case OPS::SUB:
        res(0, 0) -= 0;
        res(0, 1) -= -p[2];
        res(0, 2) -= p[1];
        res(1, 0) -= p[2];
        res(1, 1) -= 0;
        res(1, 2) -= -p[0];
        res(2, 0) -= -p[1];
        res(2, 1) -= p[0];
        res(2, 2) -= 0;

        break;
    }

    return res;
  }

  template <math::OPS op, typename T1, typename T2, typename T3>
  static Eigen::MatrixBase<T3>& multL(Eigen::MatrixBase<T1> const& p,
                                      Eigen::MatrixBase<T2> const& other,
                                      Eigen::MatrixBase<T3>& res) {
    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(T1, 3)
    EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_ROWS(T2, 3)
    EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_ROWS(T3, 3)
    eigen_assert(other.cols() == res.cols());

    int const n = other.cols();

    switch (op) {
      case OPS::EQL:
#ifdef USE_OPENMP
#pragma omp parallel for default(shared) if (n > 100)
#endif
        for (int i = 0; i < n; i++) {
          res.col(i).noalias() = p.cross(other.col(i));
        }

        break;

      case OPS::ADD:
#ifdef USE_OPENMP
#pragma omp parallel for default(shared) if (n > 100)
#endif
        for (int i = 0; i < n; i++) {
          res.col(i).noalias() += p.cross(other.col(i));
        }

        break;

      case OPS::SUB:
#ifdef USE_OPENMP
#pragma omp parallel for default(shared) if (n > 100)
#endif
        for (int i = 0; i < n; i++) {
          res.col(i).noalias() -= p.cross(other.col(i));
        }

        break;

      default:
        assert(false && "Wrong op requesed value.");
        break;
    }

    return res;
  }

  template <math::OPS op, typename T1, typename T2, typename T3>
  static Eigen::MatrixBase<T3>& multR(Eigen::MatrixBase<T1> const& p,
                                      Eigen::MatrixBase<T2> const& other,
                                      Eigen::MatrixBase<T3>& res) {
    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(T1, 3)
    EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_COLS(T2, 3)
    EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_COLS(T3, 3)
    eigen_assert(other.rows() == res.rows());

    int const n = other.rows();

    switch (op) {
      case OPS::EQL:
#ifdef USE_OPENMP
#pragma omp parallel for default(shared) if (n > 100)
#endif
        for (int i = 0; i < n; i++) {
          res.row(i).noalias() = -p.cross(other.row(i));
        }

        break;

      case OPS::ADD:
#ifdef USE_OPENMP
#pragma omp parallel for default(shared) if (n > 100)
#endif
        for (int i = 0; i < n; i++) {
          res.row(i).noalias() -= p.cross(other.row(i));
        }

        break;

      case OPS::SUB:
#ifdef USE_OPENMP
#pragma omp parallel for default(shared) if (n > 100)
#endif
        for (int i = 0; i < n; i++) {
          res.row(i).noalias() += p.cross(other.row(i));
        }

        break;

      default:
        assert(false && "Wrong op requesed value.");
        break;
    }

    return res;
  }

  template <math::OPS op, typename T1, typename T2, typename T3>
  static Eigen::MatrixBase<T3>& mult2(Eigen::MatrixBase<T1> const& p1,
                                      Eigen::MatrixBase<T2> const& p2,
                                      Eigen::MatrixBase<T3>& res) {
    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(T1, 3)
    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(T2, 3)
    EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(T3, 3, 3)

    switch (op) {
      case OPS::EQL:
        res.noalias() = p2 * p1.transpose();
        res.diagonal().array() -= p2.dot(p1);

        break;

      case OPS::ADD:
        res.noalias() += p2 * p1.transpose();
        res.diagonal().array() -= p2.dot(p1);

        break;

      case OPS::SUB:
        res.noalias() -= p2 * p1.transpose();
        res.diagonal().array() += p2.dot(p1);

        break;
    }

    return res;
  }
};
}  // namespace math
}  // namespace scope


<|start_filename|>C++/examples/.ipynb_checkpoints/SMPLify-checkpoint.cpp<|end_filename|>
#include <data/cnpy/cnpy.h>

#include <scoat/optimizer/SMPLify.h>
#include <scoat/utils/Initialization.h>
#include <scoat/utils/JointConstInfo.h>
#include <scoat/utils/KeyPointInfo.h>
#include <scoat/utils/Stopwatch.h>
#include <memory>

#include <ostream>

int main(int argc, char** argv) {
  if (argc <= 3) {
    std::cout << "There must be at least 3 inputs." << std::endl;
    return -1;
  }

  std::string model = std::string(argv[1]);
  std::string seq = std::string(argv[2]);
  int start = std::stoi(argv[3]);
  int count = std::stoi(argv[4]);
  int end = start + count;

  const int ParamSize = 11;

  using SMPL = scoat::SMPL<ParamSize>;
  using SMPLify = scoat::Optimizer::SMPLify<ParamSize, false>;

  const int NumJoints = SMPL::NumJoints;

  const std::string KeyPoint2DHeader =
      "/media/coldsky/Documents/Northwestern/Research/2021/SCO/measurements/"
      "H36M/yolo/KeyPoints2D/" +
      seq + "/";

  const std::string KeyPoint3DHeader =
      "/media/coldsky/Documents/Northwestern/Research/2021/SCO/measurements/"
      "H36M/yolo/KeyPoints3D/" +
      seq + "/";

  const std::string JointLimitHeader =
      "/media/coldsky/Documents/Northwestern/Research/2021/SCO/model/"
      "JointLimitNeuralNetworks/";

  const std::string ResultDir =
      "/media/coldsky/Documents/Northwestern/Research/2021/SCO/results/H36M/yolo/" +
      seq + "/";

  //------------------------------------------------------------------------------
  // Load model info
  //------------------------------------------------------------------------------
  scoat::MatrixX SMPLHJDirs, SMPLHRelJDirs;
  scoat::VectorX SMPLHJ, SMPLHRelJ;
  scoat::MatrixX JDirs, RelJDirs, VDirs;
  scoat::VectorX J, RelJ, V;

  scoat::loadModel<51, ParamSize, 6890>(model, SMPLHJDirs, SMPLHJ,
                                        SMPLHRelJDirs, SMPLHRelJ, VDirs, V);

  SMPLHJDirs.middleRows<3>(66) = SMPLHJDirs.middleRows<3>(75);
  SMPLHJDirs.middleRows<3>(69) = SMPLHJDirs.middleRows<3>(120);
  JDirs = SMPLHJDirs.topRows<72>();

  SMPLHJ.segment<3>(66) = SMPLHJ.segment<3>(75);
  SMPLHJ.segment<3>(69) = SMPLHJ.segment<3>(120);
  J = SMPLHJ.head<72>();

  SMPLHRelJDirs.middleRows<3>(63) = SMPLHRelJDirs.middleRows<3>(72);
  SMPLHRelJDirs.middleRows<3>(66) = SMPLHRelJDirs.middleRows<3>(117);
  RelJDirs = SMPLHRelJDirs.topRows<69>();

  SMPLHRelJ.segment<3>(63) = SMPLHRelJ.segment<3>(72);
  SMPLHRelJ.segment<3>(66) = SMPLHRelJ.segment<3>(117);
  RelJ = SMPLHRelJ.topRows<69>();

  scoat::MatrixX ShapeSqrtCov = scoat::MatrixX::Identity(ParamSize, ParamSize);
  ShapeSqrtCov.diagonal().segment<5>(5).array() = 0.8;
  ShapeSqrtCov.diagonal()[ParamSize - 1] = 0;

  //------------------------------------------------------------------------------
  // setup model
  //------------------------------------------------------------------------------
  SMPL smpl(RelJDirs, RelJ, VDirs, V);

  scoat::MatrixX KeyPointDirs;
  scoat::VectorX KeyPoints;

  scoat::loadSMPLKeyPoints(JDirs, J, VDirs, V, KeyPointDirs, KeyPoints);

  //------------------------------------------------------------------------------
  // Load joint limits neural networks info
  //------------------------------------------------------------------------------
  scoat::AlignedVector<scoat::AlignedVector<scoat::Matrix6>> JointLimitWeight(
      SMPL::NumJoints);
  scoat::AlignedVector<scoat::AlignedVector<scoat::Vector6>> JointLimitBias(
      SMPL::NumJoints);
  scoat::AlignedVector<scoat::VectorX> JointLimitPRelua(SMPL::NumJoints);
  scoat::AlignedVector<scoat::VectorX> JointLimitPRelub(SMPL::NumJoints);

  for (int n = 0; n < SMPL::NumJoints - 2; n++) {
    std::string file =
        JointLimitHeader + "joint_flow_" + std::to_string(n) + ".npz";
    scoat::loadJointLimitNeuralNetwork(file, JointLimitWeight[n],
                                       JointLimitBias[n], JointLimitPRelua[n],
                                       JointLimitPRelub[n]);
  }

  {
    int n = 21;

    std::string file =
        JointLimitHeader + "joint_flow_" + std::to_string(24) + ".npz";
    scoat::loadJointLimitNeuralNetwork(file, JointLimitWeight[n],
                                       JointLimitBias[n], JointLimitPRelua[n],
                                       JointLimitPRelub[n]);
  }

  {
    int n = 22;

    std::string file =
        JointLimitHeader + "joint_flow_" + std::to_string(39) + ".npz";
    scoat::loadJointLimitNeuralNetwork(file, JointLimitWeight[n],
                                       JointLimitBias[n], JointLimitPRelua[n],
                                       JointLimitPRelub[n]);
  }

  //------------------------------------------------------------------------------
  // Setup camera info
  //------------------------------------------------------------------------------
  std::map<std::string, scoat::Vector4> CamParamInfo = {
      {"54138969",
       {1145.04940458804, 1143.78109572365, 512.541504956548,
        515.451486977600}},
      {"55011271",
       {1149.67569986785, 1147.59161666764, 508.848621645943,
        508.064917088557}},
      {"58860488",
       {1149.14071676148, 1148.79896856760, 519.815837182153,
        501.402658888552}},
      {"60457274",
       {1145.51133842318, 1144.77392807652, 514.968197319863,
        501.882018537695}}};

  std::string cam = seq.substr(seq.length() - 8);
  scoat::Vector4 CamParams = CamParamInfo.at(cam);

  scoat::Scalar meanCamParam = CamParams.head<2>().mean();

  //------------------------------------------------------------------------------
  // Setup KeyPoint2D, POF, KeyPoint3D, JointLimit and Pose parameters
  //------------------------------------------------------------------------------
  scoat::Scalar KeyPoint2DGMSigma = 0.01;
  scoat::Scalar KeyPoint2DGMEps = 0.5;

  scoat::VectorX KeyPoint2DConfidenceThreshold = 0.3 * scoat::VectorX::Ones(28);
  KeyPoint2DConfidenceThreshold.head<3>().array() = 0.9;
  KeyPoint2DConfidenceThreshold.segment<2>(3).array() = 0.85;
  KeyPoint2DConfidenceThreshold.segment<6>(20).array() = 0.85;

  scoat::Scalar POFGMSigma = 0.1;
  scoat::Scalar POFGMEps = 0.8;

  scoat::Scalar KeyPoint3DGMSigma = 0.01;
  scoat::Scalar KeyPoint3DGMEps = 0.5;

  scoat::Scalar KeyPoint3DOriginGMSigma = 100;
  scoat::Scalar KeyPoint3DOriginGMEps = 0.5;

  scoat::Vector<SMPL::NumJoints> InitJointLimitScale;
  InitJointLimitScale.setOnes();
  InitJointLimitScale[0] = 0.05;
  InitJointLimitScale[1] = 0.05;
  InitJointLimitScale[2] = 1.0;
  InitJointLimitScale[3] = 0.05;
  InitJointLimitScale[4] = 0.05;
  InitJointLimitScale[5] = 1.0;
  InitJointLimitScale[11] = 0.2;
  InitJointLimitScale[14] = 0.2;
  InitJointLimitScale[15] = 1;
  InitJointLimitScale[16] = 1;

  scoat::Vector<SMPL::NumJoints> JointLimitScale;
  JointLimitScale.setOnes();
  JointLimitScale[0] = 0.05;
  JointLimitScale[1] = 0.05;
  JointLimitScale[2] = 1.0;
  JointLimitScale[3] = 0.2;
  JointLimitScale[4] = 0.2;
  JointLimitScale[5] = 1.0;
  JointLimitScale[8] = 1.0;
  JointLimitScale[11] = 0.1;
  JointLimitScale[14] = 0.1;
  JointLimitScale[15] = 1;
  JointLimitScale[16] = 1;

  scoat::Matrix6 SqrtCovPose = scoat::Matrix6::Identity();
  SqrtCovPose.diagonal().head<3>().array() = 0;

  //------------------------------------------------------------------------------
  // Setup joint constraint, parameter and joint factors
  //------------------------------------------------------------------------------
  // create joint constraint factors
  const auto& JointConstInfo = scoat::JointConstInfo::SMPL::JointConstInfo;
  std::vector<std::shared_ptr<scoat::JointConstFactor>> JointConstFactors(
      JointConstInfo.size());

  scoat::createJointConstFactors(JointConstInfo, JointConstFactors);

  // create Euler angle constraint factors
  const auto& EulerAngleConstInfo = scoat::JointConstInfo::SMPL::EulerAngleConstInfo;
  std::vector<std::shared_ptr<scoat::EulerAngleConstFactor>> EulerAngleConstFactors(
      EulerAngleConstInfo.size());

  scoat::createEulerAngleConstFactors(EulerAngleConstInfo, EulerAngleConstFactors);

  // create joint limits factors
  std::vector<std::shared_ptr<scoat::JointLimitFactor>> JointLimitFactors(
      SMPL::NumJoints);

  scoat::createJointLimitFactors(JointLimitWeight, JointLimitBias,
                                 JointLimitPRelua, JointLimitPRelub,
                                 JointLimitScale, JointLimitFactors);

  //------------------------------------------------------------------------------
  // Measurements
  //------------------------------------------------------------------------------
  const int KeyPoint2DSize = 28;
  scoat::Matrix3X Measurements2D(3, KeyPoint2DSize);

  const int KeyPoint3DSize = 17;
  scoat::Matrix3X Measurements3D(3, KeyPoint3DSize);

  const int POFSize = 16;
  scoat::Matrix3X POFMeasurements(3, POFSize);

  for (int index = start; index < end; index++) {
    std::ostringstream ss;
    ss << std::setw(6) << std::setfill('0') << 5 * index + 1;

    //------------------------------------------------------------------------------
    // Load 2D keypoint measurements
    //------------------------------------------------------------------------------
    std::string KeyPoint2DResult = KeyPoint2DHeader + ss.str() + ".npy";

    scoat::loadKeyPoint2DMeasurements(KeyPoint2DResult, Measurements2D);

    Measurements2D.topRows<2>().colwise() -= CamParams.tail<2>();
    Measurements2D.row(0) /= CamParams[0];
    Measurements2D.row(1) /= CamParams[1];

    //------------------------------------------------------------------------------
    // Load 3D keypoint measurements
    //------------------------------------------------------------------------------
    std::string KeyPoint3DResult = KeyPoint3DHeader + ss.str() + ".npy";

    scoat::loadKeyPoint3DMeasurements(KeyPoint3DResult, Measurements3D);

    //------------------------------------------------------------------------------
    // Load POF measurements
    //------------------------------------------------------------------------------
    scoat::loadPOFMeasurements(Measurements3D, POFMeasurements);

    //******************************************************************************
    // Initialization
    //******************************************************************************
    scoat::Pose InitialPose;
    scoat::Pose TorsoPose;
    scoat::AlignedVector<scoat::Matrix3> LeftArmJoints;
    scoat::AlignedVector<scoat::Matrix3> RightArmJoints;
    scoat::AlignedVector<scoat::Matrix3> LeftLegJoints;
    scoat::AlignedVector<scoat::Matrix3> RightLegJoints;

    scoat::VectorX betas;
    scoat::Matrix3 SpineJoint;

    auto InitStart = scoat::Stopwatch::tick();

    scoat::Initialization::SMPL::initialize(
        ShapeSqrtCov, meanCamParam, Measurements2D, KeyPoint2DGMSigma,
        KeyPoint2DGMEps, KeyPoint2DConfidenceThreshold, POFMeasurements,
        POFGMSigma, POFGMEps, Measurements3D, KeyPoint3DGMSigma, KeyPoint3DGMEps,
        JointLimitWeight, JointLimitBias, JointLimitPRelua, JointLimitPRelub,
        InitJointLimitScale, TorsoPose, SpineJoint, LeftArmJoints,
        RightArmJoints, LeftLegJoints, RightLegJoints, betas);

    auto InitTime = scoat::Stopwatch::tock(InitStart);

    const auto& ExtTorsoIndex = scoat::InitialInfo::SMPL::ExtTorsoIndex[1];
    const auto& LeftArmIndex = scoat::InitialInfo::SMPL::LeftArmIndex[1];
    const auto& RightArmIndex = scoat::InitialInfo::SMPL::RightArmIndex[1];
    const auto& LeftLegIndex = scoat::InitialInfo::SMPL::LeftLegIndex[1];
    const auto& RightLegIndex = scoat::InitialInfo::SMPL::RightLegIndex[1];

    // Initialize joints
    scoat::AlignedVector<scoat::Matrix3> joints(NumJoints,
                                                scoat::Matrix3::Identity());

    joints[ExtTorsoIndex[1][0] - 1] = SpineJoint;

    joints[LeftArmIndex[1][0] - 1] = LeftArmJoints[0];
    joints[LeftArmIndex[2][0] - 1] = LeftArmJoints[1];

    joints[RightArmIndex[1][0] - 1] = RightArmJoints[0];
    joints[RightArmIndex[2][0] - 1] = RightArmJoints[1];

    joints[LeftLegIndex[1][0] - 1] = LeftLegJoints[0];
    joints[LeftLegIndex[2][0] - 1] = LeftLegJoints[1];

    joints[RightLegIndex[1][0] - 1] = RightLegJoints[0];
    joints[RightLegIndex[2][0] - 1] = RightLegJoints[1];

    // Initialize params
    scoat::Vector<ParamSize> params = scoat::Vector<ParamSize>::Zero();

    // Initialize root params
    const auto&  KeyPointOriginIndex = scoat::KeyPointInfo::SMPL::KeyPointOriginIndex;
    auto root = TorsoPose;
    scoat::Vector3 origin;
    scoat::getKeyPointOrigin3D(TorsoPose, JDirs, J, KeyPointDirs, KeyPoints, params,
                         KeyPointOriginIndex, origin);
    root.t -= origin - TorsoPose.t;

    //******************************************************************************
    // Optimization
    //******************************************************************************
    auto SetupStart = scoat::Stopwatch::tick();

    //------------------------------------------------------------------------------
    // Setup JointPinholeCamera Factors
    //------------------------------------------------------------------------------
    const auto& JointPinholeCamInfo =
        scoat::KeyPointInfo::SMPL::JointPinholeCameraFactorInfo;
    std::vector<std::shared_ptr<scoat::JointPinholeCameraFactor>>
        JointPinholeCamFactors;

    scoat::createJointPinholeCameraFactors(
        JointPinholeCamInfo, KeyPoint2DGMSigma, KeyPoint2DGMEps, Measurements2D,
        KeyPoint2DConfidenceThreshold, JointPinholeCamFactors);

    //------------------------------------------------------------------------------
    // Setup VertexPinholeCamera Factors
    //------------------------------------------------------------------------------
    const auto& VertexPinholeCamInfo =
        scoat::KeyPointInfo::SMPL::VertexPinholeCameraFactorInfo;
    std::vector<std::shared_ptr<scoat::VertexPinholeCameraFactor>>
        VertexPinholeCamFactors;

    scoat::createVertexPinholeCameraFactors(
        VertexPinholeCamInfo, 0, JDirs, J, KeyPointDirs, KeyPoints,
        KeyPoint2DGMSigma, KeyPoint2DGMEps, Measurements2D,
        KeyPoint2DConfidenceThreshold, VertexPinholeCamFactors);

    //------------------------------------------------------------------------------
    // Setup UnitPOF Factors
    //------------------------------------------------------------------------------
    const auto& UnitPOFInfo = scoat::KeyPointInfo::SMPL::UnitPOFFactorInfo;
    std::vector<std::shared_ptr<scoat::UnitPOFFactor>> UnitPOFFactors;

    scoat::createUnitPOFFactors(UnitPOFInfo, POFGMSigma, POFGMEps,
                                POFMeasurements, UnitPOFFactors);

    //------------------------------------------------------------------------------
    // Setup ScaledPOF Factors
    //------------------------------------------------------------------------------
    const auto& ScaledPOFInfo = scoat::KeyPointInfo::SMPL::ScaledPOFFactorInfo;
    std::vector<std::shared_ptr<scoat::ScaledPOFFactor>> ScaledPOFFactors;

    scoat::createScaledPOFFactors(ScaledPOFInfo, 0, KeyPointDirs, KeyPoints,
                                  POFGMSigma, POFGMEps, POFMeasurements,
                                  ScaledPOFFactors);

    //------------------------------------------------------------------------------
    // Setup RelPOF Factors
    //------------------------------------------------------------------------------
    const auto& RelPOFInfo = scoat::KeyPointInfo::SMPL::RelPOFFactorInfo;
    const auto& kintree = smpl.getKinematicsTree();
    std::vector<std::shared_ptr<scoat::RelPOFFactor>> RelPOFFactors;

    scoat::createRelPOFFactors(RelPOFInfo, 0, JDirs, J, KeyPointDirs, KeyPoints,
                               kintree, POFGMSigma, POFGMEps, POFMeasurements,
                               RelPOFFactors);

    //------------------------------------------------------------------------------
    // Setup Joint Factors
    //------------------------------------------------------------------------------
    scoat::Matrix3 JointSqrtCov = scoat::Matrix3::Identity();

    std::vector<std::shared_ptr<scoat::JointFactor>> JointFactors;

    JointFactors.push_back(std::make_shared<scoat::JointFactor>(LeftLegIndex[2][0] - 1, JointSqrtCov, LeftLegJoints[1]));
    JointFactors.push_back(std::make_shared<scoat::JointFactor>(RightLegIndex[2][0] - 1, JointSqrtCov, RightLegJoints[1]));

    //------------------------------------------------------------------------------
    // Setup JointDepthCamera factors
    //------------------------------------------------------------------------------
    const auto& JointDepthCamInfo =
        scoat::KeyPointInfo::SMPL::JointDepthCameraFactorInfo;
    std::vector<std::shared_ptr<scoat::JointDepthCameraFactor>>
        JointDepthCamFactors;

    scoat::createJointDepthCameraFactors(JointDepthCamInfo, KeyPoint3DGMSigma,
                                         KeyPoint3DGMEps, Measurements3D,
                                         TorsoPose.t, JointDepthCamFactors);

    //------------------------------------------------------------------------------
    // Setup VertexDepthCamera factors
    //------------------------------------------------------------------------------
    const auto& VertexDepthCamInfo =
        scoat::KeyPointInfo::SMPL::VertexDepthCameraFactorInfo;
    std::vector<std::shared_ptr<scoat::VertexDepthCameraFactor>>
        VertexDepthCamFactors;

    scoat::createVertexDepthCameraFactors(
        VertexDepthCamInfo, 0, JDirs, J, KeyPointDirs, KeyPoints,
        KeyPoint3DGMSigma, KeyPoint3DGMEps, Measurements3D, TorsoPose.t,
        VertexDepthCamFactors);

    //------------------------------------------------------------------------------
    // Setup OriginDepthCamera factors
    //------------------------------------------------------------------------------
    const auto& OriginDepthCamInfo =
        scoat::KeyPointInfo::SMPL::OriginDepthCameraFactorInfo;
    std::vector<std::shared_ptr<scoat::VertexDepthCameraFactor>>
        OriginDepthCamFactors;

    scoat::createVertexDepthCameraFactors(
        OriginDepthCamInfo, 0, JDirs, J, KeyPointDirs, KeyPoints,
        KeyPoint3DOriginGMSigma, KeyPoint3DOriginGMEps, Measurements3D, TorsoPose.t,
        OriginDepthCamFactors);

    //------------------------------------------------------------------------------
    // Setup pose factor
    //------------------------------------------------------------------------------
    scoat::Matrix6 PoseSqrtCov = scoat::Matrix6::Identity();
    PoseSqrtCov.diagonal().head<3>().setConstant(2);

    std::shared_ptr<scoat::PoseFactor> PoseFactor =
        std::make_shared<scoat::PoseFactor>(0, PoseSqrtCov, root);

    //------------------------------------------------------------------------------
    // Setup parameter factor
    //------------------------------------------------------------------------------
    std::shared_ptr<scoat::ParameterFactor> ParameterFactor =
        std::make_shared<scoat::ParameterFactor>(0, ShapeSqrtCov, params);

    auto SetupTime = scoat::Stopwatch::tock(SetupStart);

    // ------------------------------------------------------------
    // Set up the optimizer
    // ------------------------------------------------------------
    scoat::Optimizer::Options options;

    options.method = scoat::Optimizer::Method::LM;
    options.delta = 1;

    SMPLify smplify(smpl, options);

    scoat::Pose RootPose;
    scoat::AlignedVector<scoat::Matrix3> Joints;
    scoat::Vector<ParamSize> Params;

    auto OptStart = scoat::Stopwatch::tick();

    // Reset optimizer
    smplify.reset();

    // Add factors
    for (const auto& factor : JointPinholeCamFactors) {
      smplify.addJointPinholeCameraFactor(factor);
    }

    for (const auto& factor : VertexPinholeCamFactors) {
      smplify.addVertexPinholeCameraFactor(factor);
    }

    for (const auto& factor : UnitPOFFactors) {
      smplify.addPOFFactor(factor);
    }

    for (const auto& factor : ScaledPOFFactors) {
      smplify.addPOFFactor(factor);
    }

    for (const auto& factor : RelPOFFactors) {
      smplify.addPOFFactor(factor);
    }

    for (const auto& factor : JointConstFactors) {
      smplify.addJointConstFactor(factor);
    }

    for (const auto& factor : EulerAngleConstFactors) {
      smplify.addJointConstFactor(factor);
    }

    for (int n = 0; n < NumJoints; n++) {
      smplify.addJointLimitFactor(JointLimitFactors[n]);
    }

    for (const auto& factor: JointFactors) {
      smplify.addJointFactor(factor);
    }

    smplify.addParameterFactor(ParameterFactor);

    smplify.addPoseFactor(PoseFactor);

    //---------------------------------------------------------------
    // Stage 0: Initialization
    //---------------------------------------------------------------
    options.weights.head<4>().array() = std::pow(meanCamParam, 2);
    options.weights[scoat::Optimizer::FactorIndex::UnitPOF] = 10000;
    options.weights[scoat::Optimizer::FactorIndex::ScaledPOF] = 10000;
    options.weights[scoat::Optimizer::FactorIndex::RelPOF] = 10000;
    options.weights[scoat::Optimizer::FactorIndex::JointDepthCamera] = 32000;
    options.weights[scoat::Optimizer::FactorIndex::VertexDepthCamera] = 32000;
    options.weights[scoat::Optimizer::FactorIndex::JointLimit] = 1000;
    options.weights[scoat::Optimizer::FactorIndex::JointConst] = 1000;
    options.weights[scoat::Optimizer::FactorIndex::Joint] = 10000;
    options.weights[scoat::Optimizer::FactorIndex::Pose] = 500;
    options.weights[scoat::Optimizer::FactorIndex::Parameter] = 1000;

    smplify.updateFactorWeights(options.weights);

    // Initialize joints
    // Initialize parameters
    smplify.initialize(root, joints, {params}, false);

    int NumIters = 0;
    //---------------------------------------------------------------
    // Stage 1
    //---------------------------------------------------------------
    for (int iter = 0; iter < 50; iter++) {
      smplify.optimize();

      if (smplify.getResults().expected_rel_cost_reduction.back() < 5e-2 ||
          smplify.getResults().rel_cost_reduction.back() < 1e-2) {
        break;
      }
    }

    //---------------------------------------------------------------
    // Stage 2
    //---------------------------------------------------------------
    NumIters += smplify.getResults().fobjs.size() - 1;

    RootPose = smplify.getPoses()[0];
    Joints = smplify.getJoints();
    Params = smplify.getParameters()[0];

    //------------------------------------------------------------------------------
    // Setup pose factor
    //------------------------------------------------------------------------------
    PoseSqrtCov = scoat::Matrix6::Identity();
    PoseSqrtCov.diagonal().head<3>().setConstant(2);

    PoseFactor = std::make_shared<scoat::PoseFactor>(0, PoseSqrtCov, RootPose);

    //------------------------------------------------------------------------------
    // Setup parameter factor
    //------------------------------------------------------------------------------
    ParameterFactor = std::make_shared<scoat::ParameterFactor>(0, ShapeSqrtCov, Params);

    //------------------------------------------------------------------------------
    // Get KeyPoint3D origin position
    //------------------------------------------------------------------------------
    scoat::getKeyPointOrigin3D(
        RootPose, JDirs, J, KeyPointDirs, KeyPoints, Params,
        scoat::KeyPointInfo::SMPL::KeyPointOriginIndex, origin);

    //------------------------------------------------------------------------------
    // Setup JointDepthCamera factors
    //------------------------------------------------------------------------------
    scoat::createJointDepthCameraFactors(JointDepthCamInfo, KeyPoint3DGMSigma,
                                         KeyPoint3DGMEps, Measurements3D,
                                         origin, JointDepthCamFactors);

    //------------------------------------------------------------------------------
    // Setup VertexDepthCamera factors
    //------------------------------------------------------------------------------
    scoat::createVertexDepthCameraFactors(
        VertexDepthCamInfo, 0, JDirs, J, KeyPointDirs, KeyPoints,
        KeyPoint3DGMSigma, KeyPoint3DGMEps, Measurements3D, origin,
        VertexDepthCamFactors);

    // Reset optimizer
    smplify.reset();

    // Add factors
    for (const auto& factor : JointPinholeCamFactors) {
      smplify.addJointPinholeCameraFactor(factor);
    }

    for (const auto& factor : VertexPinholeCamFactors) {
      smplify.addVertexPinholeCameraFactor(factor);
    }

    for (const auto& factor : UnitPOFFactors) {
      smplify.addPOFFactor(factor);
    }

    for (const auto& factor : ScaledPOFFactors) {
      smplify.addPOFFactor(factor);
    }

    for (const auto& factor : RelPOFFactors) {
      smplify.addPOFFactor(factor);
    }

    for (const auto& factor : JointDepthCamFactors) {
      smplify.addDepthCameraFactor(factor);
    }

    for (const auto& factor : VertexDepthCamFactors) {
      smplify.addDepthCameraFactor(factor);
    }

    for (const auto& factor : JointFactors) {
      smplify.addJointFactor(factor);
    }

    for (const auto& factor : JointConstFactors) {
      smplify.addJointConstFactor(factor);
    }

    for (const auto& factor : EulerAngleConstFactors) {
      smplify.addJointConstFactor(factor);
    }

    for (int n = 0; n < NumJoints; n++) {
      smplify.addJointLimitFactor(JointLimitFactors[n]);
    }

    smplify.addParameterFactor(ParameterFactor);

    smplify.addPoseFactor(PoseFactor);

    options.weights.tail<8>().array() = 10;
    options.weights[scoat::Optimizer::FactorIndex::UnitPOF] = 6000;
    options.weights[scoat::Optimizer::FactorIndex::ScaledPOF] = 6000;
    options.weights[scoat::Optimizer::FactorIndex::RelPOF] = 6000;
    options.weights[scoat::Optimizer::FactorIndex::JointDepthCamera] = 24000;
    options.weights[scoat::Optimizer::FactorIndex::VertexDepthCamera] = 24000;
    options.weights[scoat::Optimizer::FactorIndex::JointLimit] = 500;
    options.weights[scoat::Optimizer::FactorIndex::JointConst] = 1000;
    options.weights[scoat::Optimizer::FactorIndex::Pose] = 400;
    options.weights[scoat::Optimizer::FactorIndex::Joint] = 5000;
    options.weights[scoat::Optimizer::FactorIndex::Parameter] = 400;
    smplify.updateFactorWeights(options.weights);

    // initialize the optimizer
    smplify.initialize(RootPose, Joints, {Params}, false);

    for (int iter = 0; iter < 50; iter++) {
      smplify.optimize();

      if (smplify.getResults().expected_rel_cost_reduction.back() < 5e-2 ||
          smplify.getResults().rel_cost_reduction.back() < 1e-2) {
        break;
      }
    }

    //---------------------------------------------------------------
    // Stage 3
    //---------------------------------------------------------------
    NumIters += smplify.getResults().fobjs.size() - 1;

    RootPose = smplify.getPoses()[0];
    Joints = smplify.getJoints();
    Params = smplify.getParameters()[0];

    //------------------------------------------------------------------------------
    // Setup pose factor
    //------------------------------------------------------------------------------
    PoseSqrtCov = scoat::Matrix6::Identity();
    PoseSqrtCov.diagonal().head<3>().setConstant(2);

    PoseFactor = std::make_shared<scoat::PoseFactor>(0, PoseSqrtCov, RootPose);

    //------------------------------------------------------------------------------
    // Setup parameter factor
    //------------------------------------------------------------------------------
    ParameterFactor = std::make_shared<scoat::ParameterFactor>(0, ShapeSqrtCov, Params);

    //------------------------------------------------------------------------------
    // Get KeyPoint3D origin position
    //------------------------------------------------------------------------------
    scoat::getKeyPointOrigin3D(
        RootPose, JDirs, J, KeyPointDirs, KeyPoints, Params,
        scoat::KeyPointInfo::SMPL::KeyPointOriginIndex, origin);

    //------------------------------------------------------------------------------
    // Setup JointDepthCamera factors
    //------------------------------------------------------------------------------
    scoat::createJointDepthCameraFactors(JointDepthCamInfo, KeyPoint3DGMSigma,
                                         KeyPoint3DGMEps, Measurements3D,
                                         origin, JointDepthCamFactors);

    //------------------------------------------------------------------------------
    // Setup VertexDepthCamera factors
    //------------------------------------------------------------------------------
    scoat::createVertexDepthCameraFactors(
        VertexDepthCamInfo, 0, JDirs, J, KeyPointDirs, KeyPoints,
        KeyPoint3DGMSigma, KeyPoint3DGMEps, Measurements3D, origin,
        VertexDepthCamFactors);

    // Reset optimizer
    smplify.reset();

    // Add factors
    for (const auto& factor : JointPinholeCamFactors) {
      smplify.addJointPinholeCameraFactor(factor);
    }

    for (const auto& factor : VertexPinholeCamFactors) {
      smplify.addVertexPinholeCameraFactor(factor);
    }

    for (const auto& factor : UnitPOFFactors) {
      smplify.addPOFFactor(factor);
    }

    for (const auto& factor : ScaledPOFFactors) {
      smplify.addPOFFactor(factor);
    }

    for (const auto& factor : RelPOFFactors) {
      smplify.addPOFFactor(factor);
    }

    for (const auto& factor : JointDepthCamFactors) {
      smplify.addDepthCameraFactor(factor);
    }

    for (const auto& factor : VertexDepthCamFactors) {
      smplify.addDepthCameraFactor(factor);
    }

    for (const auto& factor : JointFactors) {
      smplify.addJointFactor(factor);
    }

    for (const auto& factor : JointConstFactors) {
      smplify.addJointConstFactor(factor);
    }

    for (const auto& factor : EulerAngleConstFactors) {
      smplify.addJointConstFactor(factor);
    }

    for (int n = 0; n < NumJoints; n++) {
      smplify.addJointLimitFactor(JointLimitFactors[n]);
    }

    smplify.addParameterFactor(ParameterFactor);

    smplify.addPoseFactor(PoseFactor);

    // Update weights
    options.weights.tail<8>() *= 0.1;
    options.weights[scoat::Optimizer::FactorIndex::UnitPOF] = 3000;
    options.weights[scoat::Optimizer::FactorIndex::ScaledPOF] = 3000;
    options.weights[scoat::Optimizer::FactorIndex::RelPOF] = 3000;
    options.weights[scoat::Optimizer::FactorIndex::JointDepthCamera] = 24000;
    options.weights[scoat::Optimizer::FactorIndex::VertexDepthCamera] = 24000;
    options.weights[scoat::Optimizer::FactorIndex::JointLimit] = 400;
    options.weights[scoat::Optimizer::FactorIndex::JointConst] = 1000;
    options.weights[scoat::Optimizer::FactorIndex::Pose] = 200;
    options.weights[scoat::Optimizer::FactorIndex::Joint] = 2500;
    options.weights[scoat::Optimizer::FactorIndex::Parameter] = 400;
    smplify.updateFactorWeights(options.weights);

    // initialize the optimizer
    smplify.initialize(RootPose, Joints, {Params}, false);

    for (int iter = 0; iter < 100; iter++) {
      smplify.optimize();

      if (smplify.getResults().expected_rel_cost_reduction.back() < 1e-2 ||
          smplify.getResults().rel_cost_reduction.back() < 2e-3) {
        break;
      }
    }

    //---------------------------------------------------------------
    // Stage 4
    //---------------------------------------------------------------
    options.weights[scoat::Optimizer::FactorIndex::UnitPOF] = 1500;
    options.weights[scoat::Optimizer::FactorIndex::ScaledPOF] = 1500;
    options.weights[scoat::Optimizer::FactorIndex::RelPOF] = 1500;
    options.weights[scoat::Optimizer::FactorIndex::JointDepthCamera] = 24000;
    options.weights[scoat::Optimizer::FactorIndex::VertexDepthCamera] = 24000;
    options.weights[scoat::Optimizer::FactorIndex::JointLimit] = 150;
    options.weights[scoat::Optimizer::FactorIndex::JointConst] = 1000;
    options.weights[scoat::Optimizer::FactorIndex::Pose] = 100;
    options.weights[scoat::Optimizer::FactorIndex::Joint] = 500;
    options.weights[scoat::Optimizer::FactorIndex::Parameter] = 100;
    smplify.updateFactorWeights(options.weights);

    for (int iter = 0; iter < 100; iter++) {
      smplify.optimize();

      if (smplify.getResults().expected_rel_cost_reduction.back() < 1e-2 ||
          smplify.getResults().rel_cost_reduction.back() < 2e-3) {
        break;
      }
    }

    //---------------------------------------------------------------
    // Stage 5
    //---------------------------------------------------------------
    options.weights[scoat::Optimizer::FactorIndex::UnitPOF] = 1500;
    options.weights[scoat::Optimizer::FactorIndex::ScaledPOF] = 1500;
    options.weights[scoat::Optimizer::FactorIndex::RelPOF] = 1500;
    options.weights[scoat::Optimizer::FactorIndex::JointDepthCamera] = 24000;
    options.weights[scoat::Optimizer::FactorIndex::VertexDepthCamera] = 24000;
    options.weights[scoat::Optimizer::FactorIndex::JointLimit] = 50;
    options.weights[scoat::Optimizer::FactorIndex::JointConst] = 1000;
    options.weights[scoat::Optimizer::FactorIndex::Pose] = 100;
    options.weights[scoat::Optimizer::FactorIndex::Joint] = 100;
    options.weights[scoat::Optimizer::FactorIndex::Parameter] = 20;
    smplify.updateFactorWeights(options.weights);

    for (int iter = 0; iter < 100; iter++) {
      smplify.optimize();

      if (smplify.getResults().expected_rel_cost_reduction.back() < 1e-3 ||
          smplify.getResults().rel_cost_reduction.back() < 2e-4) {
        break;
      }
    }

    //---------------------------------------------------------------
    // Stage 6
    //---------------------------------------------------------------
    NumIters += smplify.getResults().fobjs.size() - 1;

    RootPose = smplify.getPoses()[0];
    Joints = smplify.getJoints();
    Params = smplify.getParameters()[0];

    //------------------------------------------------------------------------------
    // Setup pose factor
    //------------------------------------------------------------------------------
    PoseSqrtCov = scoat::Matrix6::Identity();
    PoseSqrtCov.diagonal().head<3>().setConstant(2);
    PoseSqrtCov.diagonal().tail<3>().setConstant(2000);

    PoseFactor = std::make_shared<scoat::PoseFactor>(0, PoseSqrtCov, RootPose);

    //------------------------------------------------------------------------------
    // Setup parameter factor
    //------------------------------------------------------------------------------
    ParameterFactor = std::make_shared<scoat::ParameterFactor>(0, ShapeSqrtCov, Params);

    //------------------------------------------------------------------------------
    // Get KeyPoint3D origin position
    //------------------------------------------------------------------------------
    scoat::getKeyPointOrigin3D(
        RootPose, JDirs, J, KeyPointDirs, KeyPoints, Params,
        scoat::KeyPointInfo::SMPL::KeyPointOriginIndex, origin);

    //------------------------------------------------------------------------------
    // Setup JointDepthCamera factors
    //------------------------------------------------------------------------------
    scoat::createJointDepthCameraFactors(JointDepthCamInfo, KeyPoint3DGMSigma,
                                         KeyPoint3DGMEps, Measurements3D,
                                         origin, JointDepthCamFactors);

    //------------------------------------------------------------------------------
    // Setup VertexDepthCamera factors
    //------------------------------------------------------------------------------
    scoat::createVertexDepthCameraFactors(
        VertexDepthCamInfo, 0, JDirs, J, KeyPointDirs, KeyPoints,
        KeyPoint3DGMSigma, KeyPoint3DGMEps, Measurements3D, origin,
        VertexDepthCamFactors);

    // Reset optimizer
    smplify.reset();

    // Add factors
    for (const auto& factor : JointPinholeCamFactors) {
      smplify.addJointPinholeCameraFactor(factor);
    }

    for (const auto& factor : VertexPinholeCamFactors) {
      smplify.addVertexPinholeCameraFactor(factor);
    }

    for (const auto& factor : UnitPOFFactors) {
      smplify.addPOFFactor(factor);
    }

    for (const auto& factor : ScaledPOFFactors) {
      smplify.addPOFFactor(factor);
    }

    for (const auto& factor : RelPOFFactors) {
      smplify.addPOFFactor(factor);
    }

    for (const auto& factor : JointDepthCamFactors) {
      smplify.addDepthCameraFactor(factor);
    }

    for (const auto& factor : VertexDepthCamFactors) {
      smplify.addDepthCameraFactor(factor);
    }

    for (const auto& factor : JointFactors) {
      smplify.addJointFactor(factor);
    }

    for (const auto& factor : JointConstFactors) {
      smplify.addJointConstFactor(factor);
    }

    for (const auto& factor : EulerAngleConstFactors) {
      smplify.addJointConstFactor(factor);
    }

    for (int n = 0; n < NumJoints; n++) {
      smplify.addJointLimitFactor(JointLimitFactors[n]);
    }

    smplify.addParameterFactor(ParameterFactor);

    smplify.addPoseFactor(PoseFactor);

    // Update weights
    options.weights[scoat::Optimizer::FactorIndex::UnitPOF] = 2500;
    options.weights[scoat::Optimizer::FactorIndex::ScaledPOF] = 2500;
    options.weights[scoat::Optimizer::FactorIndex::RelPOF] = 2500;
    options.weights[scoat::Optimizer::FactorIndex::JointDepthCamera] = 24000;
    options.weights[scoat::Optimizer::FactorIndex::VertexDepthCamera] = 24000;
    options.weights[scoat::Optimizer::FactorIndex::JointLimit] = 50;
    options.weights[scoat::Optimizer::FactorIndex::JointConst] = 250;
    options.weights[scoat::Optimizer::FactorIndex::Pose] = 100;
    options.weights[scoat::Optimizer::FactorIndex::Joint] = 25;
    options.weights[scoat::Optimizer::FactorIndex::Parameter] = 100;
    smplify.updateFactorWeights(options.weights);

    // initialize the optimizer
    smplify.initialize(RootPose, Joints, {Params}, false);

    for (int iter = 0; iter < 100; iter++) {
      smplify.optimize();

      if (smplify.getResults().expected_rel_cost_reduction.back() < 1e-4 ||
          smplify.getResults().rel_cost_reduction.back() < 2e-5) {
        break;
      }
    }

    NumIters += smplify.getResults().fobjs.size() - 1;

    auto OptTime = scoat::Stopwatch::tock(OptStart);
    auto Fobj = smplify.getFobj();
    auto TotalTime = OptTime + SetupTime + InitTime;

    std::cout << "======================================================="
              << std::endl;
    std::cout << seq + "_" + ss.str() << std::endl;
    std::cout << "-------------------------------------------------------"
              << std::endl;
    std::cout << "objective value: " << Fobj << std::endl;
    std::cout << "init time: " << InitTime << std::endl;
    std::cout << "setup time: " << SetupTime << std::endl;
    std::cout << "optimization time: " << OptTime << std::endl;
    std::cout << "total time: " << TotalTime << std::endl;
    std::cout << "iterations: " << NumIters << std::endl;

    //---------------------------------------------------------------
    // Results
    //---------------------------------------------------------------
    const auto& Poses = smplify.getPoses();
    Params = smplify.getParameters()[0];
    RootPose = smplify.getPoses()[0];
    Joints = smplify.getJoints();

    // Process root pose and joints
    Eigen::Matrix3Xd Rots;
    Rots.setZero(3, 52);

    auto RootRot = Rots.col(0);
    scoat::math::SO3::log(RootPose.R, RootRot);

    for (int i = 0; i < SMPL::NumJoints; i++) {
      auto rot = Rots.col(i + 1);
      scoat::math::SO3::log(Joints[i], rot);
    }

    Eigen::Vector3d J0 = J.head<3>();
    J0.noalias() += JDirs.topRows<3>() * Params;

    // get 2D keypoints
    scoat::Matrix2X KeyPoints2D =
        scoat::Matrix2X::Zero(2, Measurements2D.cols());

    for (const auto& info : JointPinholeCamInfo) {
      const auto& m = info[0];
      const auto& i = info[1];

      KeyPoints2D.col(m) = Poses[i].t.head<2>() / Poses[i].t[2];
    }

    for (const auto& info : VertexPinholeCamInfo) {
      scoat::VertexPinholeCameraFactor::Evaluation eval;

      const auto& m = info[0];
      const auto& i = info[1];
      const auto& v = info[2];

      const auto& measurement = Measurements2D.col(m);

      scoat::Matrix3X KeyPointDir =
          KeyPointDirs.middleRows<3>(3 * v) - JDirs.middleRows<3>(3 * i);
      scoat::Vector3 KeyPoint =
          KeyPoints.segment<3>(3 * v) - J.segment<3>(3 * i);

      std::shared_ptr<scoat::VertexPinholeCameraFactor> factor =
          std::make_shared<scoat::VertexPinholeCameraFactor>(
              i, 0, KeyPointDir, KeyPoint, 1, 0.5, measurement.head<2>(),
              measurement[2]);

      factor->evaluate(Poses, {}, Joints, {Params}, eval);

      KeyPoints2D.col(m) = eval.point2D;
    }

    // get 3D keypoints
    scoat::Matrix3X KeyPoints3D =
        scoat::Matrix3X::Zero(3, Measurements3D.cols());

    for (const auto& info : JointDepthCamInfo) {
      const auto& m = std::get<0>(info);
      const auto& i = std::get<1>(info);

      KeyPoints3D.col(m) = Poses[i].t;
    }

    for (const auto& info :
         scoat::KeyPointInfo::SMPL::VertexDepthCameraFactorInfo) {
      scoat::VertexDepthCameraFactor::Evaluation eval;

      const auto& m = std::get<0>(info);
      const auto& i = std::get<1>(info);
      const auto& v = std::get<2>(info);
      const auto& confidence = std::get<3>(info);

      const scoat::Vector3 measurement = Measurements3D.col(m) + Poses[0].t;

      scoat::Matrix3X KeyPointDir =
          KeyPointDirs.middleRows<3>(3 * v) - JDirs.middleRows<3>(3 * i);
      scoat::Vector3 KeyPoint =
          KeyPoints.segment<3>(3 * v) - J.segment<3>(3 * i);

      std::shared_ptr<scoat::VertexDepthCameraFactor> factor =
          std::make_shared<scoat::VertexDepthCameraFactor>(
              i, 0, KeyPointDir, KeyPoint, 1, 0.5, measurement, confidence);

      factor->evaluate(Poses, {}, Joints, {Params}, eval);

      KeyPoints3D.col(m) = eval.point3D;
    }

    // save results
    std::string ResultFile = ResultDir + ss.str() + ".npz";

    cnpy::npz_save(ResultFile, "betas", Params.data(), {(size_t)Params.size() - 1},
                   "w");
    cnpy::npz_save(ResultFile, "poses", Rots.data(), {(size_t)(3 * 52)}, "a");
    cnpy::npz_save(ResultFile, "trans", RootPose.t.data(), {(size_t)(3)}, "a");
    cnpy::npz_save(ResultFile, "measurements", Measurements2D.data(),
                   {size_t(Measurements2D.cols()), size_t(3)}, "a");
    cnpy::npz_save(ResultFile, "camera", CamParams.data(), {(size_t)(4)}, "a");
    cnpy::npz_save(ResultFile, "J0", J0.data(), {(size_t)(3)}, "a");
    cnpy::npz_save(ResultFile, "kpts2D", KeyPoints2D.data(),
                   {size_t(KeyPoints2D.cols()), 2}, "a");
    cnpy::npz_save(ResultFile, "kpts3D", KeyPoints3D.data(),
                   {size_t(KeyPoints3D.cols()), 3}, "a");
    cnpy::npz_save(ResultFile, "opt_time", &OptTime, {1}, "a");
    cnpy::npz_save(ResultFile, "setup_time", &SetupTime, {1}, "a");
    cnpy::npz_save(ResultFile, "init_time", &InitTime, {1}, "a");
    cnpy::npz_save(ResultFile, "total_time", &TotalTime, {1}, "a");
    cnpy::npz_save(ResultFile, "objective", &Fobj, {1}, "a");
    cnpy::npz_save(ResultFile, "iterations", &NumIters, {1}, "a");
  }

  return 0;
}

<|start_filename|>C++/scope/initializer/factor/DepthCameraFactor.h<|end_filename|>
#pragma once

#include <scope/initializer/factor/Factor.h>

namespace scope {
namespace Initializer {
class DepthCameraFactor : public Factor {
 public:
  struct Evaluation : public Factor::Evaluation {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    Vector3 point3D;
    Scalar squaredErrorNorm;
  };

  struct Linearization : public Factor::Linearization {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    Matrix36 jacobian;

    Scalar Drho;
    Scalar sqrtDrho;
    Scalar alpha;

    Vector3 scaledError;

    Vector6 g;
  };

 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  SCOAT_INITIAL_EVALUATION_NEW
  SCOAT_INITIAL_LINEARIZATION_NEW

  DepthCameraFactor(int pose, const Vector3 &point, const Scalar &sigma,
                    const Scalar &eps, const Vector3 &measurement,
                    const Scalar &confidence = 1.0,
                    const std::string &name = "", int index = -1);

  virtual int evaluate(const AlignedVector<Pose> &poses,
                       const AlignedVector<Matrix3> &joints,
                       Factor::Evaluation &base_eval) const override;

  virtual int linearize(const AlignedVector<Pose> &poses,
                        const AlignedVector<Matrix3> &joints,
                        const Factor::Evaluation &base_eval,
                        Factor::Linearization &base_lin) const override;

  int getPose() const { return mPose; }

  const Vector3 &point() const { return mPoint; }

  const Vector3 &measurement() const { return mMeasurement; }

 protected:
  int mPose;

  Vector3 mPoint;
  Vector3 mMeasurement;

  Scalar mCon;
  Scalar mSqrtCon;
  Scalar mSigmaCon;

  Scalar mSigma;
  Scalar mGMThreshold;
  Scalar mMaxGMScale;
};
}  // namespace Initializer
}  // namespace scope


<|start_filename|>C++/scope/utils/Initialization.h<|end_filename|>
#pragma once

#include <scope/initializer/Initializers.h>
#include <scope/model/SMPL.h>
#include <scope/utils/Setup.h>

namespace scope {
namespace Initialization {
namespace SMPL {
int initialize(const MatrixX &ShapeSqrtCov, Scalar FocalLength,
               // KeyPoint2D setup
               const Matrix3X &Measurements2D, Scalar KeyPoint2DGMSigma,
               Scalar KeyPoint2DGMEps,
               const VectorX &KeyPoint2DConfidenceThreshold,
               // POF setup
               const Matrix3X &POFMeasurements, Scalar POFGMSigma,
               Scalar POFGMEps,
               // KeyPoint3D setup
               const Matrix3X &Measurements3D, Scalar KeyPoint3DGMSigma,
               Scalar KeyPoint3DGMEps,
               // joint limit
               const AlignedVector<AlignedVector<Matrix6>> &JointLimitWeight,
               const AlignedVector<AlignedVector<Vector6>> &JointLimitBias,
               const AlignedVector<VectorX> &JointLimitPRelua,
               const AlignedVector<VectorX> &JointLimitPRelub,
               const Vector<23> &JointLimitScale,
               // initial estimates
               Pose &TorsoPose,      // torso pose
               Matrix3 &SpineJoint,  // spine joint
               AlignedVector<Matrix3> &LeftArmJoint,
               AlignedVector<Matrix3> &RightArmJoint,
               AlignedVector<Matrix3> &LeftLegJoint,
               AlignedVector<Matrix3> &RightLegJoint, VectorX &betas);

int initializeRootPose(const AlignedVector<Pose> &Poses, const VectorX &Param,
                       // SMPL model info
                       const std::vector<int> &kintree, const MatrixX &JDirs,
                       const VectorX &J, const MatrixX &KeyPointDirs,
                       const VectorX &KeyPoints,
                       // KeyPoint2D setup
                       Scalar FocalLength, const Matrix3X &Measurements2D,
                       Scalar KeyPoint2DGMSigma, Scalar KeyPoint2DGMEps,
                       const VectorX &KeyPoint2DConfidenceThreshold,
                       // POF setup
                       const Matrix3X &POFMeasurements, Scalar POFGMSigma,
                       Scalar POFGMEps,
                       // initial estimates
                       Pose &RootPose);
}  // namespace SMPL
}  // namespace Initialization
}  // namespace scope


<|start_filename|>C++/scope/factor/UnitPOFFactor.cpp<|end_filename|>
#include <glog/logging.h>

#include <scope/factor/UnitPOFFactor.h>

namespace scope {
UnitPOFFactor::UnitPOFFactor(int pose, const Vector3 &s, const Scalar &sigma,
                             const Scalar &eps, const Vector3 &measurement,
                             const Scalar &confidence, const std::string &name,
                             int index, bool active)
    : POFFactor({pose}, {}, {}, {}, sigma, eps, measurement, confidence, name,
                index, active),
      mS(s) {
  mS.stableNormalize();
}

int UnitPOFFactor::evaluate(const AlignedVector<Pose> &poses,
                            const AlignedVector<VectorX> &shapes,
                            const AlignedVector<Matrix3> &joints,
                            const AlignedVector<VectorX> &params,
                            Factor::Evaluation &base_eval) const {
  auto &eval = dynamic_cast<Evaluation &>(base_eval);
  eval.clear();

  const auto &i = mvPoses[0];
  assert(i >= 0 && i < poses.size());

  if (i >= poses.size()) {
    LOG(ERROR) << "The pose must be valid." << std::endl;

    exit(-1);
  }

  auto &pose = poses[i];
  eval.error.resize(3);

  eval.d.noalias() = pose.R * mS;
  eval.error = eval.d - mMeasurement;
  eval.squaredErrorNorm = eval.error.squaredNorm();

#if not ROBUST_POF
  eval.f = mCon * eval.squaredErrorNorm;
#else
  eval.f = mSigmaCon * eval.squaredErrorNorm / (mSigma + eval.squaredErrorNorm);
#endif

  eval.status = Status::VALID;

  return 0;
}

int UnitPOFFactor::linearize(const AlignedVector<Pose> &poses,
                             const AlignedVector<VectorX> &shapes,
                             const AlignedVector<Matrix3> &joints,
                             const AlignedVector<VectorX> &params,
                             const Factor::Evaluation &base_eval,
                             Factor::Linearization &base_lin) const {
  auto &eval = dynamic_cast<const Evaluation &>(base_eval);
  auto &lin = dynamic_cast<Linearization &>(base_lin);

  lin.clear();

  assert(eval.status == Status::VALID);

  if (eval.status != Status::VALID) {
    LOG(ERROR) << "The evaluation must be valid." << std::endl;

    exit(-1);
  }

  const auto &i = mvPoses[0];
  assert(i >= 0 && i < poses.size());

  if (i < 0 || i >= poses.size()) {
    LOG(ERROR) << "The joint must be valid." << std::endl;

    exit(-1);
  }

  lin.jacobians[0].resize(1);
  lin.jacobians[0][0].setZero(3, 6);

  Eigen::Map<Matrix3> JR(lin.jacobians[0][0].data());

  JR(0, 1) = eval.d[2];
  JR(0, 2) = -eval.d[1];
  JR(1, 0) = -eval.d[2];
  JR(1, 2) = eval.d[0];
  JR(2, 0) = eval.d[1];
  JR(2, 1) = -eval.d[0];

  lin.gPose.setZero();

  Eigen::Map<Vector3> gR(lin.gPose.data());

  gR.noalias() = JR.transpose() * eval.error;

#if not ROBUST_POF
  JR *= mSqrtCon;
  gR *= mCon;
#else
  preLinearizeRobustKernel(eval, lin);

  JR.noalias() -= lin.scaledError * gR.transpose();

  JR *= lin.sqrtDrho;
  gR *= lin.Drho;
#endif

  lin.status = Status::VALID;

  return 0;
}
}  // namespace scope


<|start_filename|>C++/scope/factor/JointPinholeCameraFactor.h<|end_filename|>
#pragma once

#include <scope/factor/PinholeCameraFactor.h>

namespace scope {
class JointPinholeCameraFactor : public PinholeCameraFactor {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  SCOAT_FACTOR_EVALUATION_NEW
  SCOAT_FACTOR_LINEARIZATION_NEW

  JointPinholeCameraFactor(int pose, const Scalar &sigma, const Scalar &eps,
                           const Vector2 &measurement,
                           const Scalar &confidence = 1.0,
                           const std::string &name = "", int index = -1,
                           bool active = true);

  virtual int evaluate(const AlignedVector<Pose> &poses,
                       const AlignedVector<VectorX> &shapes,
                       const AlignedVector<Matrix3> &joints,
                       const AlignedVector<VectorX> &params,
                       Factor::Evaluation &base_eval) const override;

  virtual int linearize(const AlignedVector<Pose> &poses,
                        const AlignedVector<VectorX> &shapes,
                        const AlignedVector<Matrix3> &joints,
                        const AlignedVector<VectorX> &params,
                        const Factor::Evaluation &base_eval,
                        Factor::Linearization &base_lin) const override;

protected:
  virtual int evaluate(const Pose &pose, const Vector2 &measurement,
                       Evaluation &eval) const;

  virtual int linearize(const Pose &pose, const Vector2 &measurement,
                        const Evaluation &eval, Linearization &lin) const;
};

} // namespace scope


<|start_filename|>C++/scope/initializer/factor/PinholeCameraFactor.cpp<|end_filename|>
#include <glog/logging.h>

#include <scope/initializer/factor/PinholeCameraFactor.h>

namespace scope {
namespace Initializer {
PinholeCameraFactor::PinholeCameraFactor(int pose, const Vector3 &point,
                                         const Scalar &sigma, const Scalar &eps,
                                         const Vector2 &measurement,
                                         const Scalar &confidence,
                                         const std::string &name, int index)
    : Factor(name, index), mPose(pose), mPoint(point),
      mMeasurement(measurement), mSigma(sigma), mCon(confidence),
      mSqrtCon(std::sqrt(confidence)), mSigmaCon(confidence * sigma) {
  assert(eps > 1e-5 && eps < 1.0);
  assert(sigma > 0);
  assert(confidence >= 0);

  mMaxGMScale = 1 - eps;
  mGMThreshold = (1 - eps * eps) / (3 + eps * eps) * sigma;
}

int PinholeCameraFactor::evaluate(const AlignedVector<Pose> &poses,
                                  const AlignedVector<Matrix3> &joints,
                                  Factor::Evaluation &base_eval) const {
  auto &eval = dynamic_cast<Evaluation &>(base_eval);
  eval.clear();

  const auto &i = mPose;

  assert(i >= 0 && i < poses.size());

  if (i >= poses.size()) {
    LOG(ERROR) << "The pose must be valid." << std::endl;

    exit(-1);
  }

  auto &pose = poses[i];

  eval.pCam = pose.t;
  eval.pCam.noalias() += pose.R * mPoint;

  eval.point2D = eval.pCam.head<2>() / eval.pCam[2];
  eval.error = eval.point2D - mMeasurement;
  eval.squaredErrorNorm = eval.error.squaredNorm();

#if 0
  eval.f = mCon * eval.squaredErrorNorm;
#else
  eval.f = mSigmaCon * eval.squaredErrorNorm / (mSigma + eval.squaredErrorNorm);
#endif

  eval.status = Status::VALID;

  return 0;
}

int PinholeCameraFactor::linearize(const AlignedVector<Pose> &poses,
                                   const AlignedVector<Matrix3> &joints,
                                   const Factor::Evaluation &base_eval,
                                   Factor::Linearization &base_lin) const {
  auto &eval = dynamic_cast<const Evaluation &>(base_eval);
  auto &lin = dynamic_cast<Linearization &>(base_lin);

  lin.clear();

  const auto &i = mPose;

  assert(i >= 0 && i < poses.size());

  if (i >= poses.size()) {
    LOG(ERROR) << "The pose must be valid." << std::endl;

    exit(-1);
  }

  assert(eval.status == Status::VALID);

  if (eval.status != Status::VALID) {
    LOG(ERROR) << "The evaluation must be valid." << std::endl;

    exit(-1);
  }

  auto &J = lin.jacobian;

  J.resize(2, 6);

  Eigen::Map<Matrix<2, 3>> JR(J.data());
  Eigen::Map<Matrix<2, 3>> Jt(J.data() + 6);

  Scalar zinv = 1.0 / eval.pCam[2];

  Jt(0, 0) = zinv;
  Jt(0, 1) = 0;
  Jt(0, 2) = -eval.point2D[0] * zinv;
  Jt(1, 0) = 0;
  Jt(1, 1) = zinv;
  Jt(1, 2) = -eval.point2D[1] * zinv;

  JR.row(0) = eval.pCam.cross(Jt.row(0));
  JR.row(1) = eval.pCam.cross(Jt.row(1));

  auto &g = lin.g;

  g.noalias() = J.transpose() * eval.error;

#if 0
  J *= mSqrtCon;
  g *= mCon;
#else
  lin.sqrtDrho = mSigma / (mSigma + eval.squaredErrorNorm);
  lin.Drho = lin.sqrtDrho * lin.sqrtDrho;
  lin.alpha = eval.squaredErrorNorm > mGMThreshold
                  ? mMaxGMScale
                  : 1 - std::sqrt(1 - 4 * eval.squaredErrorNorm /
                                          (mSigma + eval.squaredErrorNorm));

  if (lin.alpha > 1e-5) {
    lin.scaledError = lin.alpha * eval.error / eval.squaredErrorNorm;
  } else {
    lin.scaledError.setZero();
  }

  lin.sqrtDrho *= mSqrtCon;
  lin.Drho *= mCon;

  J.noalias() -= lin.scaledError * g.transpose();

  J *= lin.sqrtDrho;
  g *= lin.Drho;
#endif

  lin.status = Status::VALID;

  return 0;
}
} // namespace Initializer
} // namespace scope


<|start_filename|>C++/scope/factor/FullJointExtraPinholeCameraFactor.cpp<|end_filename|>
#include <glog/logging.h>

#include <scope/factor/FullJointExtraPinholeCameraFactor.h>

namespace scope {
FullJointExtraPinholeCameraFactor::FullJointExtraPinholeCameraFactor(
    int pose, int camParam, const Scalar &sigma, const Scalar &eps,
    const Pose &extraCamPose, const Vector2 &measurement,
    const Scalar &confidence, const std::string &name, int index, bool active)
    : JointExtraPinholeCameraFactor(pose, sigma, eps, extraCamPose, measurement,
                                    confidence, name, index, active) {
  mvParams = {camParam};
}

int FullJointExtraPinholeCameraFactor::evaluate(
    const AlignedVector<Pose> &poses, const AlignedVector<VectorX> &shapes,
    const AlignedVector<Matrix3> &joints, const AlignedVector<VectorX> &params,
    Factor::Evaluation &base_eval) const {
  auto &eval = dynamic_cast<Evaluation &>(base_eval);
  eval.clear();

  const auto &pose = mvPoses[0];
  assert(pose >= 0 && pose < poses.size());

  if (pose < 0 || pose >= poses.size()) {
    LOG(ERROR) << "The pose must be valid." << std::endl;

    exit(-1);
  }

  const auto &camParam = mvParams[0];
  assert(camParam >= 0 && camParam < params.size());

  if (camParam < 0 || camParam > params.size()) {
    LOG(ERROR) << "The parameter must be valid." << std::endl;

    exit(-1);
  }

  evaluate(poses[pose], params[camParam], mMeasurement, eval);

  eval.status = Status::VALID;

  return 0;
}

int FullJointExtraPinholeCameraFactor::linearize(
    const AlignedVector<Pose> &poses, const AlignedVector<VectorX> &shapes,
    const AlignedVector<Matrix3> &joints, const AlignedVector<VectorX> &params,
    const Factor::Evaluation &base_eval,
    Factor::Linearization &base_lin) const {
  auto &eval = dynamic_cast<const Evaluation &>(base_eval);
  auto &lin = dynamic_cast<Linearization &>(base_lin);

  lin.clear();

  assert(eval.status == Status::VALID);

  if (eval.status != Status::VALID) {
    LOG(ERROR) << "The evaluation must be valid." << std::endl;

    exit(-1);
  }

  const auto &pose = mvPoses[0];
  assert(pose >= 0 && pose < poses.size());

  if (pose < 0 || pose >= poses.size()) {
    LOG(ERROR) << "The pose must be valid." << std::endl;

    exit(-1);
  }

  const auto &camParam = mvParams[0];
  assert(camParam >= 0 && camParam < params.size());

  if (camParam < 0 || camParam > params.size()) {
    LOG(ERROR) << "The parameter must be valid." << std::endl;

    exit(-1);
  }

  lin.jacobians[0].resize(1);
  lin.jacobians[3].resize(1);

  linearize(poses[pose], params[camParam], mMeasurement, eval, lin);

  lin.status = Status::VALID;

  return 0;
}

int FullJointExtraPinholeCameraFactor::evaluate(const Pose &pose,
                                                const VectorX &camParam,
                                                const Vector2 &measurement,
                                                Evaluation &eval) const {
  assert(camParam.size() == 4);

  eval.camMeasurement = camParam.tail<2>();
  eval.camMeasurement[0] += camParam[0] * measurement[0];
  eval.camMeasurement[1] += camParam[1] * measurement[1];

  JointExtraPinholeCameraFactor::evaluate(pose, eval.camMeasurement, eval);

  return 0;
}

int FullJointExtraPinholeCameraFactor::linearize(const Pose &pose,
                                                 const VectorX &camParam,
                                                 const Vector2 &measurement,
                                                 const Evaluation &eval,
                                                 Linearization &lin) const {
  assert(camParam.size() == 4);

  assert(lin.jacobians[0].size() == 1);
  assert(lin.jacobians[3].size() == 1);

  JointExtraPinholeCameraFactor::linearize(pose, eval.camMeasurement, eval,
                                           lin);

  auto &JCam = lin.jacobians[3][0];

  JCam.resize(2, 4);

  JCam(0, 0) = -measurement[0];
  JCam(0, 1) = 0;
  JCam(0, 2) = -1;
  JCam(0, 3) = 0;

  JCam(1, 0) = 0;
  JCam(1, 1) = -measurement[1];
  JCam(1, 2) = 0;
  JCam(1, 3) = -1;

  auto &gCam = lin.gCam;
  gCam.noalias() = JCam.transpose() * eval.error;

#if not ROBUST_PINHOLE
  JCam *= mSqrtCon;
  gCam *= mCon;
#else
  JCam.noalias() -= lin.scaledError * gCam.transpose();

  JCam *= lin.sqrtDrho;
  gCam *= lin.Drho;
#endif

  return 0;
}
} // namespace scope


<|start_filename|>C++/scope/optimizer/SMPLify.h<|end_filename|>
#pragma once

#include <scope/model/SMPL.h>
#include <scope/optimizer/Optimizer.h>

namespace scope {
namespace Optimizer {
template <int P, bool CamOpt>
class SMPLify
    : public Optimizer<SMPL<P>::NumJoints, P, SMPL<P>::NumVertices, CamOpt> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  using Model = SMPL<P>;
  using Optimization =
      Optimizer<Model::NumJoints, P, Model::NumVertices, CamOpt>;

  using Optimization::computeGradient;
  using Optimization::linearize;
  using Optimization::solveGaussNewton;

  SMPLify(const Model &smpl, const Options &options);

  virtual int solveGaussNewton() const override;

protected:
  std::shared_ptr<const Model> mSMPL;

protected:
  virtual int setupParameterInfo() override;

private:
  using Optimization::CamParamIndex;
  using Optimization::CamParamOffset;
  using Optimization::CamParamSize;
  using Optimization::DCamParamOffset;
  using Optimization::DFaceParamOffset;
  using Optimization::DJointOffset;
  using Optimization::DJointSize;
  using Optimization::DParamOffset;
  using Optimization::DParamSize;
  using Optimization::DPoseOffset;
  using Optimization::DPoseSize;
  using Optimization::DSize;
  using Optimization::DVertexParamOffset;
  using Optimization::FaceParamIndex;
  using Optimization::FaceParamOffset;
  using Optimization::FaceParamSize;
  using Optimization::MaxDSize;
  using Optimization::MaxNumCollisions;
  using Optimization::NumJoints;
  using Optimization::NumParams;
  using Optimization::NumPoses;
  using Optimization::NumShapes;
  using Optimization::ParamSize;
  using Optimization::Size;
  using Optimization::VertexParamIndex;
  using Optimization::VertexParamOffset;
  using Optimization::VertexParamSize;

  using Optimization::mvB;
  using Optimization::mvBp;
  using Optimization::mvBu;
  using Optimization::mvH;
  using Optimization::mvHuuInv;
  using Optimization::mvHxB;
  using Optimization::mvKuxp;
  using Optimization::mvM;

  using Optimization::mvE;
  using Optimization::mvhp;
  using Optimization::mvhu;
  using Optimization::mvhx;
  using Optimization::mvku;
  using Optimization::mvmp;
  using Optimization::mvmu;
  using Optimization::mvmx;

  using Optimization::mParamGN;
  using Optimization::mvJointGN;
  using Optimization::mvPoseGN;

  using Optimization::mDLambda;
  using Optimization::mLambda;
  using Optimization::mOptions;
};
} // namespace Optimizer
} // namespace scope


<|start_filename|>C++/scope/initializer/factor/EulerAngleConstFactor.cpp<|end_filename|>
#include <glog/logging.h>

#include <scope/initializer/factor/EulerAngleConstFactor.h>
#include <scope/math/SO3.h>

namespace scope {
namespace Initializer {
EulerAngleConstFactor::EulerAngleConstFactor(int joint, const Vector3 &weight,
                                             const Matrix3 &mean,
                                             const Vector3 &lbnd,
                                             const Vector3 &ubnd,
                                             const std::string &name, int index)
    : JointConstFactor(joint, weight, mean, lbnd, ubnd, name, index){
}

int EulerAngleConstFactor::evaluate(const AlignedVector<Pose> &poses,
                                    const AlignedVector<Matrix3> &joints,
                                    Factor::Evaluation &base_eval) const {
  auto &eval = dynamic_cast<Evaluation &>(base_eval);
  eval.clear();

  const auto &i = mJoint;

  assert(i >= 0 && i < joints.size());

  if (i >= joints.size()) {
    LOG(ERROR) << "The pose must be valid." << std::endl;

    exit(-1);
  }

  const auto &R = joints[i];

  eval.errorR.noalias() = mMean.transpose() * R;

  math::SO3::Rot2XYZ(eval.errorR, eval.xi);

  eval.errorL = eval.xi - mLowerBnd;
  eval.errorL = eval.errorL.cwiseMin(mBnd);

  eval.errorU = mUpperBnd - eval.xi;
  eval.errorU = eval.errorU.cwiseMin(mBnd);

  eval.errorLInv = mScale * eval.errorL.cwiseInverse();
  eval.errorUInv = mScale * eval.errorU.cwiseInverse();

  eval.expL = eval.errorLInv.array().exp();
  eval.expU = eval.errorUInv.array().exp();

  eval.error = eval.errorL.cwiseProduct(eval.expL);
  eval.error += eval.errorU.cwiseProduct(eval.expU);

  eval.error.noalias() = mWeight.asDiagonal() * eval.error;

  eval.f = eval.error.squaredNorm();

  eval.status = Status::VALID;

  return 0;
}

int EulerAngleConstFactor::linearize(const AlignedVector<Pose> &poses,
                                     const AlignedVector<Matrix3> &joints,
                                     const Factor::Evaluation &base_eval,
                                     Factor::Linearization &base_lin) const {
  auto &eval = dynamic_cast<const Evaluation &>(base_eval);
  auto &lin = dynamic_cast<Linearization &>(base_lin);

  lin.clear();

  const auto &i = mJoint;

  assert(i >= 0 && i < joints.size());

  if (i >= joints.size()) {
    LOG(ERROR) << "The pose must be valid." << std::endl;

    exit(-1);
  }

  assert(eval.status == Status::VALID);

  if (eval.status != Status::VALID) {
    LOG(ERROR) << "The evaluation must be valid." << std::endl;

    exit(-1);
  }

  auto &J = lin.jacobian;

  lin.DF.setIdentity();
  lin.DF(1, 1) = cos(eval.xi[0]);
  lin.DF(2, 1) = sin(eval.xi[0]);
  lin.DF.col(2) = eval.errorR.col(2);

  lin.H.noalias() = lin.DF.transpose() * lin.DF;
  lin.H.diagonal().array() += 1e-4;

  lin.D.noalias() = lin.H.inverse() * lin.DF.transpose();

  J.noalias() = lin.D * mMean.transpose();

  lin.Derror = eval.expL;
  lin.Derror -= eval.expL.cwiseProduct(eval.errorLInv);
  lin.Derror -= eval.expU;
  lin.Derror += eval.expU.cwiseProduct(eval.errorUInv);

  J.noalias() = lin.Derror.asDiagonal() * J;
  J.noalias() = mWeight.asDiagonal() * J;

  lin.status = Status::VALID;

  return 0;
}
}  // namespace Initializer
}  // namespace scope


<|start_filename|>C++/scope/utils/KeyPointInfo.h<|end_filename|>
#pragma once

#include <array>
#include <scope/base/Types.h>

namespace scope {
namespace KeyPointInfo {
//----------------------------------------------
// 2D keypoint measurement index
//----------------------------------------------
// 0: nose
// 1: left eye
// 2: right eye
// 3: left ear
// 4: right ear
// 5: left upper arm
// 6: right upper arm
// 7: left elbow
// 8: right elow
// 9: left wrist
// 10: right wrist
// 11: left hip
// 12: right hip
// 13: left knee
// 14: right knee
// 15: left ankle
// 16: right ankle
// 17: head top
// 18: thorax
// 19: middle hip
// 20: left big toe
// 21: right big toe
// 22: left small toe
// 23: right small toe
// 24: left heel
// 25: right heel
// 26: chest
// 27: neck
//----------------------------------------------

//----------------------------------------------
// 3D keypoint measurement index
//----------------------------------------------
// 0: middile hip
// 1: left hip
// 2: left knee
// 3: left ankle
// 4: right hip
// 5: right knee
// 6: right ankle
// 7: chest
// 8: thorax
// 9: neck
// 10: head top
// 11: left upper arm
// 12: left elbow
// 13: left wrist
// 14: right upper arm
// 15: right elbow
// 16: right wrist
//----------------------------------------------

namespace SMPL {
const int KeyPointOriginIndex = 13;

const std::vector<std::array<int, 2>> KeyPointInfo{
    {15, 332},  // 0: nose
    {15, 2800}, // 1: left eye
    {15, 6260}, // 2: right eye
    {15, 583},  // 3: left ear
    {15, 4071}, // 4: right ear
    {10, 3216}, // 5: left big toe
    {11, 6617}, // 6: right big toe
    {10, 3226}, // 7: left small toe
    {11, 6624}, // 8: right small toe
    {7, 3387},  // 9: left heel
    {8, 6787},  // 10: right heel
    {15, 6890}, // 11: head top
    {3, 6891},  // 12: chest
    {0, 6892},  // 13: middile hip
    {0, 6893},  // 14: left hip
    {0, 6894},  // 15: right hip
    {9, 6911},  // 16: left upper arm
    {9, 6912},  // 17: right upper arm
    {0, 6895},  // 18: pelvis
    {1, 6896},  // 19: left hip
    {2, 6897},  // 20: right hip
    {3, 6898},  // 21: spine
    {4, 6899},  // 22: left knee
    {5, 6900},  // 23: right knee
    {6, 6901},  // 24: spine1
    {7, 6902},  // 25: left ankle
    {8, 6903},  // 26: right ankle
    {9, 6904},  // 27: spine2
    {10, 6905}, // 28: left toes
    {11, 6906}, // 29: right toes
    {12, 6907}, // 30: thorax
    {13, 6908}, // 31: left chest
    {14, 6909}, // 32: right chest
    {15, 6910}, // 33: neck
    {16, 6911}, // 34: left shoulder
    {17, 6912}, // 35: right shoulder
    {18, 6913}, // 36: left elbow
    {19, 6914}, // 37: right elbow
    {20, 6915}, // 38: left wrist
    {21, 6916}, // 39: right wrist
    {22, 6917}, // 40: left fingers
    {23, 6918}  // 41: right fingers
};

const std::vector<std::array<int, 3>> POFInfo{
    //[0]: 3D keypoint measurment index -- tail
    //[1]: 3D keypoint measurment index -- head
    {0, 1},   // 0: middle hip -> left hip
    {1, 2},   // 1: left hip -> left knee
    {2, 3},   // 2: left knee -> left ankle
    {0, 4},   // 3: middle hip -> right hip
    {4, 5},   // 4: right hip -> right knee
    {5, 6},   // 5: right knee -> right ankle
    {0, 7},   // 6: middle hip -> chest/spine1
    {7, 8},   // 7: chest -> thorax
    {8, 9},   // 8: thorax -> neck
    {9, 10},  // 9: neck -> head top
    {8, 11},  // 10: thorax -> left upper arm
    {11, 12}, // 11: left upper arm -> left elbow
    {12, 13}, // 12: left elbow -> left wrist
    {8, 14},  // 13: thorax -> right upper arm
    {14, 15}, // 14: right upper arm -> right elbow
    {15, 16}  // 15: right elbow -> right wrist
};

const std::vector<int> KeyPoint3Dto2D{
    // 2D keypoint measurement index
    19, // 0: middile hip
    11, // 1: left hip
    13, // 2: left knee
    15, // 3: left ankle
    12, // 4: right hip
    14, // 5: right knee
    16, // 6: right ankle
    26, // 7: chest
    18, // 8: thorax
    27, // 9: neck
    17, // 10: head top
    5,  // 11: left upper arm
    7,  // 12: left elbow
    9,  // 13: left wrist
    6,  // 14: right upper arm
    8,  // 15: right elbow
    10  // 16: right wrist
};

const std::vector<std::array<int, 2>> JointPinholeCameraFactorInfo{
    //[0]: 2D keypoint measurement index
    //[1]: SMPL body part index
    {7, 18},  // left elbow
    {8, 19},  // right elbow
    {9, 20},  // left wrist
    {10, 21}, // right wrist
    {13, 4},  // left knee
    {14, 5},  // right knee
    {15, 7},  // left ankle
    {16, 8},  // right ankle
    {18, 12}, // thorax
    {27, 15}  // neck
};

const std::vector<std::array<int, 3>> VertexPinholeCameraFactorInfo{
    //[0]: 2D keypoint measurement index
    //[1]: SMPL body part index
    //[2]: SMPL vertex index
    {0, 15, 0},   // nose
    {1, 15, 1},   // left eye
    {2, 15, 2},   // right eye
    {3, 15, 3},   // left ear
    {4, 15, 4},   // right ear
    {5, 9, 16},   // left upper arm
    {6, 9, 17},   // right upper arm
    {11, 0, 14},  // left hip
    {12, 0, 15},  // right hip
    {17, 15, 11}, // head top
    {19, 0, 13},  // middle hip
    {20, 10, 5},  // left big toe
    {21, 11, 6},  // right big toe
    {22, 10, 7},  // left small toe
    {23, 11, 8},  // right small toe
    {24, 7, 9},   // left heel
    {25, 8, 10},  // right heel
    {26, 3, 12}   // chest
};

const std::vector<std::array<int, 2>> UpperBodyJointPinholeCameraFactorInfo{
    //[0]: 2D keypoint measurement index
    //[1]: SMPL body part index
    {7, 18},  // left elbow
    {8, 19},  // right elbow
    {9, 20},  // left wrist
    {10, 21}, // right wrist
    {18, 12}, // thorax
    {27, 15}, // neck
};

const std::vector<std::array<int, 3>> UpperBodyVertexPinholeCameraFactorInfo{
    //[0]: 2D keypoint measurement index
    //[1]: SMPL body part index
    //[2]: SMPL vertex index
    {0, 15, 0},   // nose
    {1, 15, 1},   // left eye
    {2, 15, 2},   // right eye
    {3, 15, 3},   // left ear
    {4, 15, 4},   // right ear
    {5, 9, 15},   // left upper arm
    {6, 9, 16},   // right upper arm
    {11, 0, 13},  // left hip
    {12, 0, 14},  // right hip
    {17, 15, 11}, // head top
    {19, 0, 13},  // middle hip
    {26, 3, 12}   // chest
};

const std::vector<std::array<int, 2>> LowerBodyJointPinholeCameraFactorInfo{
    //[0]: 2D keypoint measurement index
    //[1]: SMPL body part index
    {13, 4}, // left knee
    {14, 5}, // right knee
    {15, 7}, // left ankle
    {16, 8}  // right ankle
};

const std::vector<std::array<int, 3>> LowerBodyVertexPinholeCameraFactorInfo{
    //[0]: 2D keypoint measurement index
    //[1]: SMPL body part index
    //[2]: SMPL vertex index
    {20, 10, 5}, // left big toe
    {21, 11, 6}, // right big toe
    {22, 10, 7}, // left small toe
    {23, 11, 8}, // right small toe
    {24, 7, 9},  // left heel
    {25, 8, 10}  // right heel
};

const std::vector<std::tuple<int, int, Scalar>> JointDepthCameraFactorInfo{
    //[0]: 3D keypoint measurment index
    //[1]: SMPL body part index
    //[2]: confidence
    {2, 4, 1.0},   // left knee
    {3, 7, 1.0},   // left ankle
    {5, 5, 1.0},   // right knee
    {6, 8, 1.0},   // right ankle
    {8, 12, 1.0},  // thorax
    {9, 15, 1.0},  // neck
    {12, 18, 1.0}, // left elbow
    {13, 20, 1.0}, // left wrist
    {15, 19, 1.0}, // right elbow
    {16, 21, 1.0}  // right wrist
};

const std::vector<std::tuple<int, int, int, Scalar>>
    VertexDepthCameraFactorInfo{
        //[0]: 3D keypoint measurment index
        //[1]: SMPL body part index
        //[2]: SMPL vertex index
        //[3]: confidence
        {0, 0, 13, 1.0},   // middle hip
        {1, 0, 14, 1.0},   // left hip
        {4, 0, 15, 1.0},   // right hip
        {7, 3, 12, 1.0},   // chest
        {10, 15, 11, 1.0}, // head top
        {11, 9, 16, 1.0},  // left upper arm
        {14, 9, 17, 1.0}   // right upper arm
    };

const std::vector<std::tuple<int, int, int, Scalar>>
    OriginDepthCameraFactorInfo{
        //[0]: 3D keypoint measurment index
        //[1]: SMPL body part index
        //[2]: SMPL vertex index
        //[3]: confidence
        {0, 0, 13, 50.0}, // middle hip
    };

const AlignedVector<std::tuple<int, int, scope::Vector3, Scalar>>
    UnitPOFFactorInfo{
        //[0]: POF measurement index
        //[1]: body part index
        //[2]: unit directional vector
        //[3]: confidence
        {0, 0, {1, 0, 0}, 1.0}, // middle hip -> left hip
        //{1, 1, {0, -1, 0}, 1.0},    // left hip -> left knee
        {2, 4, {0, -1, 0}, 1.0}, // left knee -> left ankle
        {3, 0, {-1, 0, 0}, 1.0}, // middle hip -> right hip
        //{4, 2, {0, -1, 0}, 1.0},    // right hip -> right knee
        {5, 5, {0, -1, 0}, 1.0},   // right knee -> right ankle
        {7, 9, {0, 1, 0}, 0.5},    // chest -> thorax
        {11, 16, {1, 0, 0}, 1.0},  // left upper arm -> left elbow
        {12, 18, {1, 0, 0}, 1.0},  // left elbow -> left wrist
        {14, 17, {-1, 0, 0}, 1.0}, // right upper arm -> right elbow
        {15, 19, {-1, 0, 0}, 1.0}  // right elbow -> right wrist
    };

const std::vector<std::tuple<int, int, int, int, Scalar>> ScaledPOFFactorInfo{
    //[0]: POF measurement index
    //[1]: body part index
    //[2]: vertex index -- tail
    //[3]: vertex index -- head
    //[4]: confidence
    {8, 12, 30, 33, 0.25}, // thorax -> neck
    {9, 15, 33, 11, 0.25}, // neck -> head top
    {10, 9, 30, 16, 0.5},  // thorax -> left upper arm
    {13, 9, 30, 17, 0.5}   // thorax -> right upper arm
};

const std::vector<std::tuple<int, int, int, int, int, Scalar>> RelPOFFactorInfo{
    //[0]: POF measurement index
    //[1]: body part index -- child
    //[2]: vertex index -- tail
    //[3]: vertex index -- head
    //[4]: inverse the POF measurement or not
    //[5]: confidence
    {6, 3, 13, 12, 1, 0.1}, // middle hip -> chest
    {1, 1, 14, 22, 1, 2.0}, // left hip -> left knee
    {4, 2, 15, 23, 1, 2.0}, // right hip -> right knee
};
} // namespace SMPL
} // namespace KeyPointInfo
} // namespace scope


<|start_filename|>C++/examples/run.cpp<|end_filename|>
#include <cnpy/cnpy.h>
#include <scope/optimizer/SMPLify.h>
#include <scope/utils/Initialization.h>
#include <scope/utils/JointConstInfo.h>
#include <scope/utils/KeyPointInfo.h>
#include <scope/utils/Stopwatch.h>

#include <boost/program_options.hpp>
#include <boost/program_options/parsers.hpp>
#include <boost/program_options/variables_map.hpp>
#include <fstream>
#include <iomanip>
#include <memory>
#include <nlohmann/json.hpp>

int main(int argc, char **argv) {
  namespace po = boost::program_options;

  po::options_description desc("Options and arguments");

  desc.add_options()
      // help
      ("help,h", "Print help and exit")
      // SMPL model
      ("model,m", po::value<std::string>(), "Path to the SMPL model")
      // joint limit priors
      ("prior,p", po::value<std::string>(), "Path to the joint limit priors")
      // 2D and 3D keypoints
      ("keypoint,k", po::value<std::string>(),
       "Path to the 2D and 3D keypoints")
      // results
      ("result,r", po::value<std::string>()->default_value("results.json"),
       "Path to save the results");

  po::variables_map vm;

  try {
    po::store(po::command_line_parser(argc, argv).options(desc).run(), vm);
  } catch (po::error &e) {
    LOG(ERROR) << e.what() << std::endl;
    std::cout << desc << std::endl;

    return -1;
  }
  po::notify(vm);

  if (vm.count("help")) {
    std::cout << desc << std::endl;
    return 0;
  }

  std::string model, prior, kpts;

  assert(vm.count("model") && vm.count("prior") && vm.count("keypoint"));

  if (vm.count("model")) {
    model = vm["model"].as<std::string>();
  } else {
    LOG(ERROR) << "No path to the SMPL model is assigned." << std::endl;
    return -1;
  }

  if (vm.count("prior")) {
    prior = vm["prior"].as<std::string>();
  } else {
    LOG(ERROR) << "No path to the joint limit priors is assigned." << std::endl;
    return -1;
  }

  if (vm.count("keypoint")) {
    kpts = vm["keypoint"].as<std::string>();
  } else {
    LOG(ERROR) << "No path to the 2D and 3D keypoints is assigned."
               << std::endl;
    return -1;
  }

  const int ParamSize = 11;

  using SMPL = scope::SMPL<ParamSize>;
  using SMPLify = scope::Optimizer::SMPLify<ParamSize, false>;

  const int NumJoints = SMPL::NumJoints;

  const std::string KeyPointFile = kpts;

  const std::string JointLimitFile = prior;

  const std::string ResultFile = vm["result"].as<std::string>();

  //------------------------------------------------------------------------------
  // Load model info
  //------------------------------------------------------------------------------
  scope::MatrixX SMPLJDirs, SMPLRelJDirs;
  scope::VectorX SMPLJ, SMPLRelJ;
  scope::MatrixX JDirs, RelJDirs, VDirs;
  scope::VectorX J, RelJ, V;

  scope::loadModel<23, ParamSize, 6890>(model, SMPLJDirs, SMPLJ, SMPLRelJDirs,
                                        SMPLRelJ, VDirs, V);

  JDirs = SMPLJDirs.topRows<72>();

  J = SMPLJ.head<72>();

  RelJDirs = SMPLRelJDirs.topRows<69>();

  RelJ = SMPLRelJ.topRows<69>();

  scope::MatrixX ShapeSqrtCov = scope::MatrixX::Identity(ParamSize, ParamSize);
  ShapeSqrtCov.diagonal().segment<5>(5).array() = 0.8;
  ShapeSqrtCov.diagonal()[ParamSize - 1] = 0;

  //------------------------------------------------------------------------------
  // setup model
  //------------------------------------------------------------------------------
  SMPL smpl(RelJDirs, RelJ, VDirs, V);

  scope::MatrixX KeyPointDirs;
  scope::VectorX KeyPoints;

  scope::loadSMPLKeyPoints(JDirs, J, VDirs, V, KeyPointDirs, KeyPoints);

  //------------------------------------------------------------------------------
  // Load joint limits neural networks info
  //------------------------------------------------------------------------------
  scope::AlignedVector<scope::AlignedVector<scope::Matrix6>> JointLimitWeight(
      SMPL::NumJoints);
  scope::AlignedVector<scope::AlignedVector<scope::Vector6>> JointLimitBias(
      SMPL::NumJoints);
  scope::AlignedVector<scope::VectorX> JointLimitPRelua(SMPL::NumJoints);
  scope::AlignedVector<scope::VectorX> JointLimitPRelub(SMPL::NumJoints);

  scope::loadJointLimitPrior(prior, JointLimitWeight, JointLimitBias,
                             JointLimitPRelua, JointLimitPRelub);

  //------------------------------------------------------------------------------
  // Setup KeyPoint2D, POF, KeyPoint3D, JointLimit and Pose parameters
  //------------------------------------------------------------------------------
  scope::Scalar KeyPoint2DGMSigma = 0.01;
  scope::Scalar KeyPoint2DGMEps = 0.5;

  scope::VectorX KeyPoint2DConfidenceThreshold = 0.3 * scope::VectorX::Ones(28);
  KeyPoint2DConfidenceThreshold.head<3>().array() = 0.9;
  KeyPoint2DConfidenceThreshold.segment<2>(3).array() = 0.85;
  KeyPoint2DConfidenceThreshold.segment<6>(20).array() = 0.85;

  scope::Scalar POFGMSigma = 0.1;
  scope::Scalar POFGMEps = 0.8;

  scope::Scalar KeyPoint3DGMSigma = 0.01;
  scope::Scalar KeyPoint3DGMEps = 0.5;

  scope::Scalar KeyPoint3DOriginGMSigma = 100;
  scope::Scalar KeyPoint3DOriginGMEps = 0.5;

  scope::Vector<SMPL::NumJoints> InitJointLimitScale;
  InitJointLimitScale.setOnes();
  InitJointLimitScale[0] = 0.05;
  InitJointLimitScale[1] = 0.05;
  InitJointLimitScale[2] = 1.0;
  InitJointLimitScale[3] = 0.05;
  InitJointLimitScale[4] = 0.05;
  InitJointLimitScale[5] = 1.0;
  InitJointLimitScale[11] = 0.2;
  InitJointLimitScale[14] = 0.2;
  InitJointLimitScale[15] = 1;
  InitJointLimitScale[16] = 1;

  scope::Vector<SMPL::NumJoints> JointLimitScale;
  JointLimitScale.setOnes();
  JointLimitScale[0] = 0.05;
  JointLimitScale[1] = 0.05;
  JointLimitScale[2] = 1.0;
  JointLimitScale[3] = 0.2;
  JointLimitScale[4] = 0.2;
  JointLimitScale[5] = 1.0;
  JointLimitScale[8] = 1.0;
  JointLimitScale[11] = 0.1;
  JointLimitScale[14] = 0.1;
  JointLimitScale[15] = 1;
  JointLimitScale[16] = 1;

  scope::Matrix6 SqrtCovPose = scope::Matrix6::Identity();
  SqrtCovPose.diagonal().head<3>().array() = 0;

  //------------------------------------------------------------------------------
  // Setup joint constraint, parameter and joint factors
  //------------------------------------------------------------------------------
  // create joint constraint factors
  const auto &JointConstInfo = scope::JointConstInfo::SMPL::JointConstInfo;
  std::vector<std::shared_ptr<scope::JointConstFactor>> JointConstFactors(
      JointConstInfo.size());

  scope::createJointConstFactors(JointConstInfo, JointConstFactors);

  // create Euler angle constraint factors
  const auto &EulerAngleConstInfo =
      scope::JointConstInfo::SMPL::EulerAngleConstInfo;
  std::vector<std::shared_ptr<scope::EulerAngleConstFactor>>
      EulerAngleConstFactors(EulerAngleConstInfo.size());

  scope::createEulerAngleConstFactors(EulerAngleConstInfo,
                                      EulerAngleConstFactors);

  // create joint limits factors
  std::vector<std::shared_ptr<scope::JointLimitFactor>> JointLimitFactors(
      SMPL::NumJoints);

  scope::createJointLimitFactors(JointLimitWeight, JointLimitBias,
                                 JointLimitPRelua, JointLimitPRelub,
                                 JointLimitScale, JointLimitFactors);

  //------------------------------------------------------------------------------
  // Measurements
  //------------------------------------------------------------------------------
  const int KeyPoint2DSize = 28;
  scope::Matrix3X Measurements2D(3, KeyPoint2DSize);

  const int KeyPoint3DSize = 17;
  scope::Matrix3X Measurements3D(3, KeyPoint3DSize);

  const int POFSize = 16;
  scope::Matrix3X POFMeasurements(3, POFSize);

  scope::Vector4 CamParams;

  //------------------------------------------------------------------------------
  // Load 2D and 3D keypoint measurements
  //------------------------------------------------------------------------------
  scope::loadKeyPoint2Dand3DMeasurements(KeyPointFile, CamParams,
                                         Measurements2D, Measurements3D);

  scope::Scalar meanCamParam = CamParams.head<2>().mean();

  Measurements2D.topRows<2>().colwise() -= CamParams.tail<2>();
  Measurements2D.row(0) /= CamParams[0];
  Measurements2D.row(1) /= CamParams[1];
  //------------------------------------------------------------------------------
  // Load POF measurements
  //------------------------------------------------------------------------------
  scope::loadPOFMeasurements(Measurements3D, POFMeasurements);

  //******************************************************************************
  // Initialization
  //******************************************************************************
  scope::Pose InitialPose;
  scope::Pose TorsoPose;
  scope::AlignedVector<scope::Matrix3> LeftArmJoints;
  scope::AlignedVector<scope::Matrix3> RightArmJoints;
  scope::AlignedVector<scope::Matrix3> LeftLegJoints;
  scope::AlignedVector<scope::Matrix3> RightLegJoints;

  scope::VectorX betas;
  scope::Matrix3 SpineJoint;

  auto InitStart = scope::Stopwatch::tick();

  scope::Initialization::SMPL::initialize(
      ShapeSqrtCov, meanCamParam, Measurements2D, KeyPoint2DGMSigma,
      KeyPoint2DGMEps, KeyPoint2DConfidenceThreshold, POFMeasurements,
      POFGMSigma, POFGMEps, Measurements3D, KeyPoint3DGMSigma, KeyPoint3DGMEps,
      JointLimitWeight, JointLimitBias, JointLimitPRelua, JointLimitPRelub,
      InitJointLimitScale, TorsoPose, SpineJoint, LeftArmJoints, RightArmJoints,
      LeftLegJoints, RightLegJoints, betas);

  auto InitTime = scope::Stopwatch::tock(InitStart);

  const auto &ExtTorsoIndex = scope::InitialInfo::SMPL::ExtTorsoIndex[1];
  const auto &LeftArmIndex = scope::InitialInfo::SMPL::LeftArmIndex[1];
  const auto &RightArmIndex = scope::InitialInfo::SMPL::RightArmIndex[1];
  const auto &LeftLegIndex = scope::InitialInfo::SMPL::LeftLegIndex[1];
  const auto &RightLegIndex = scope::InitialInfo::SMPL::RightLegIndex[1];

  // Initialize joints
  scope::AlignedVector<scope::Matrix3> joints(NumJoints,
                                              scope::Matrix3::Identity());

  joints[ExtTorsoIndex[1][0] - 1] = SpineJoint;

  joints[LeftArmIndex[1][0] - 1] = LeftArmJoints[0];
  joints[LeftArmIndex[2][0] - 1] = LeftArmJoints[1];

  joints[RightArmIndex[1][0] - 1] = RightArmJoints[0];
  joints[RightArmIndex[2][0] - 1] = RightArmJoints[1];

  joints[LeftLegIndex[1][0] - 1] = LeftLegJoints[0];
  joints[LeftLegIndex[2][0] - 1] = LeftLegJoints[1];

  joints[RightLegIndex[1][0] - 1] = RightLegJoints[0];
  joints[RightLegIndex[2][0] - 1] = RightLegJoints[1];

  // Initialize params
  scope::Vector<ParamSize> params = scope::Vector<ParamSize>::Zero();

  // Initialize root params
  const auto &KeyPointOriginIndex =
      scope::KeyPointInfo::SMPL::KeyPointOriginIndex;
  auto root = TorsoPose;
  scope::Vector3 origin;
  scope::getKeyPointOrigin3D(TorsoPose, JDirs, J, KeyPointDirs, KeyPoints,
                             params, KeyPointOriginIndex, origin);
  root.t -= origin - TorsoPose.t;

  // Initialize root
  {
    scope::AlignedVector<scope::Pose> poses;
    scope::VectorX JointLocationData(69);
    Eigen::Map<scope::Vector<69>> JointLocation(JointLocationData.data());

    smpl.FK(poses, JointLocation, root, joints, params);

    scope::Initialization::SMPL::initializeRootPose(
        poses, params, smpl.getKinematicsTree(), JDirs, J, KeyPointDirs,
        KeyPoints, meanCamParam, Measurements2D, KeyPoint2DGMSigma,
        KeyPoint2DGMEps, KeyPoint2DConfidenceThreshold, POFMeasurements,
        POFGMSigma, POFGMEps, root);
  }

  //******************************************************************************
  // Optimization
  //******************************************************************************
  auto SetupStart = scope::Stopwatch::tick();

  //------------------------------------------------------------------------------
  // Setup JointPinholeCamera Factors
  //------------------------------------------------------------------------------
  const auto &JointPinholeCamInfo =
      scope::KeyPointInfo::SMPL::JointPinholeCameraFactorInfo;
  std::vector<std::shared_ptr<scope::JointPinholeCameraFactor>>
      JointPinholeCamFactors;

  scope::createJointPinholeCameraFactors(
      JointPinholeCamInfo, KeyPoint2DGMSigma, KeyPoint2DGMEps, Measurements2D,
      KeyPoint2DConfidenceThreshold, JointPinholeCamFactors);

  //------------------------------------------------------------------------------
  // Setup VertexPinholeCamera Factors
  //------------------------------------------------------------------------------
  const auto &VertexPinholeCamInfo =
      scope::KeyPointInfo::SMPL::VertexPinholeCameraFactorInfo;
  std::vector<std::shared_ptr<scope::VertexPinholeCameraFactor>>
      VertexPinholeCamFactors;

  scope::createVertexPinholeCameraFactors(
      VertexPinholeCamInfo, 0, JDirs, J, KeyPointDirs, KeyPoints,
      KeyPoint2DGMSigma, KeyPoint2DGMEps, Measurements2D,
      KeyPoint2DConfidenceThreshold, VertexPinholeCamFactors);

  //------------------------------------------------------------------------------
  // Setup UnitPOF Factors
  //------------------------------------------------------------------------------
  const auto &UnitPOFInfo = scope::KeyPointInfo::SMPL::UnitPOFFactorInfo;
  std::vector<std::shared_ptr<scope::UnitPOFFactor>> UnitPOFFactors;

  scope::createUnitPOFFactors(UnitPOFInfo, POFGMSigma, POFGMEps,
                              POFMeasurements, UnitPOFFactors);

  //------------------------------------------------------------------------------
  // Setup ScaledPOF Factors
  //------------------------------------------------------------------------------
  const auto &ScaledPOFInfo = scope::KeyPointInfo::SMPL::ScaledPOFFactorInfo;
  std::vector<std::shared_ptr<scope::ScaledPOFFactor>> ScaledPOFFactors;

  scope::createScaledPOFFactors(ScaledPOFInfo, 0, KeyPointDirs, KeyPoints,
                                POFGMSigma, POFGMEps, POFMeasurements,
                                ScaledPOFFactors);

  //------------------------------------------------------------------------------
  // Setup RelPOF Factors
  //------------------------------------------------------------------------------
  const auto &RelPOFInfo = scope::KeyPointInfo::SMPL::RelPOFFactorInfo;
  const auto &kintree = smpl.getKinematicsTree();
  std::vector<std::shared_ptr<scope::RelPOFFactor>> RelPOFFactors;

  scope::createRelPOFFactors(RelPOFInfo, 0, JDirs, J, KeyPointDirs, KeyPoints,
                             kintree, POFGMSigma, POFGMEps, POFMeasurements,
                             RelPOFFactors);

  //------------------------------------------------------------------------------
  // Setup Joint Factors
  //------------------------------------------------------------------------------
  scope::Matrix3 JointSqrtCov = scope::Matrix3::Identity();

  std::vector<std::shared_ptr<scope::JointFactor>> JointFactors;

  JointFactors.push_back(std::make_shared<scope::JointFactor>(
      LeftLegIndex[2][0] - 1, JointSqrtCov, LeftLegJoints[1]));
  JointFactors.push_back(std::make_shared<scope::JointFactor>(
      RightLegIndex[2][0] - 1, JointSqrtCov, RightLegJoints[1]));

  //------------------------------------------------------------------------------
  // Setup JointDepthCamera factors
  //------------------------------------------------------------------------------
  const auto &JointDepthCamInfo =
      scope::KeyPointInfo::SMPL::JointDepthCameraFactorInfo;
  std::vector<std::shared_ptr<scope::JointDepthCameraFactor>>
      JointDepthCamFactors;

  scope::createJointDepthCameraFactors(JointDepthCamInfo, KeyPoint3DGMSigma,
                                       KeyPoint3DGMEps, Measurements3D,
                                       TorsoPose.t, JointDepthCamFactors);

  //------------------------------------------------------------------------------
  // Setup VertexDepthCamera factors
  //------------------------------------------------------------------------------
  const auto &VertexDepthCamInfo =
      scope::KeyPointInfo::SMPL::VertexDepthCameraFactorInfo;
  std::vector<std::shared_ptr<scope::VertexDepthCameraFactor>>
      VertexDepthCamFactors;

  scope::createVertexDepthCameraFactors(
      VertexDepthCamInfo, 0, JDirs, J, KeyPointDirs, KeyPoints,
      KeyPoint3DGMSigma, KeyPoint3DGMEps, Measurements3D, TorsoPose.t,
      VertexDepthCamFactors);

  //------------------------------------------------------------------------------
  // Setup OriginDepthCamera factors
  //------------------------------------------------------------------------------
  const auto &OriginDepthCamInfo =
      scope::KeyPointInfo::SMPL::OriginDepthCameraFactorInfo;
  std::vector<std::shared_ptr<scope::VertexDepthCameraFactor>>
      OriginDepthCamFactors;

  scope::createVertexDepthCameraFactors(
      OriginDepthCamInfo, 0, JDirs, J, KeyPointDirs, KeyPoints,
      KeyPoint3DOriginGMSigma, KeyPoint3DOriginGMEps, Measurements3D,
      TorsoPose.t, OriginDepthCamFactors);

  //------------------------------------------------------------------------------
  // Setup pose factor
  //------------------------------------------------------------------------------
  scope::Matrix6 PoseSqrtCov = scope::Matrix6::Identity();
  PoseSqrtCov.diagonal().head<3>().setConstant(2);

  std::shared_ptr<scope::PoseFactor> PoseFactor =
      std::make_shared<scope::PoseFactor>(0, PoseSqrtCov, root);

  //------------------------------------------------------------------------------
  // Setup parameter factor
  //------------------------------------------------------------------------------
  std::shared_ptr<scope::ParameterFactor> ParameterFactor =
      std::make_shared<scope::ParameterFactor>(0, ShapeSqrtCov, params);

  auto SetupTime = scope::Stopwatch::tock(SetupStart);

  // ------------------------------------------------------------
  // Set up the optimizer
  // ------------------------------------------------------------
  scope::Optimizer::Options options;

  options.method = scope::Optimizer::Method::LM;
  options.delta = 1;

  SMPLify smplify(smpl, options);

  scope::Pose RootPose;
  scope::AlignedVector<scope::Matrix3> Joints;
  scope::Vector<ParamSize> Params;

  auto OptStart = scope::Stopwatch::tick();

  // Reset optimizer
  smplify.reset();

  // Add factors
  for (const auto &factor : JointPinholeCamFactors) {
    smplify.addJointPinholeCameraFactor(factor);
  }

  for (const auto &factor : VertexPinholeCamFactors) {
    smplify.addVertexPinholeCameraFactor(factor);
  }

  for (const auto &factor : UnitPOFFactors) {
    smplify.addPOFFactor(factor);
  }

  for (const auto &factor : ScaledPOFFactors) {
    smplify.addPOFFactor(factor);
  }

  for (const auto &factor : RelPOFFactors) {
    smplify.addPOFFactor(factor);
  }

  for (const auto &factor : JointConstFactors) {
    smplify.addJointConstFactor(factor);
  }

  for (const auto &factor : EulerAngleConstFactors) {
    smplify.addJointConstFactor(factor);
  }

  for (int n = 0; n < NumJoints; n++) {
    smplify.addJointLimitFactor(JointLimitFactors[n]);
  }

  for (const auto &factor : JointFactors) {
    smplify.addJointFactor(factor);
  }

  smplify.addParameterFactor(ParameterFactor);

  smplify.addPoseFactor(PoseFactor);

  //---------------------------------------------------------------
  // Stage 0: Initialization
  //---------------------------------------------------------------
  options.weights.head<4>().array() = std::pow(meanCamParam, 2);
  options.weights[scope::Optimizer::FactorIndex::UnitPOF] = 10000;
  options.weights[scope::Optimizer::FactorIndex::ScaledPOF] = 10000;
  options.weights[scope::Optimizer::FactorIndex::RelPOF] = 10000;
  options.weights[scope::Optimizer::FactorIndex::JointDepthCamera] = 32000;
  options.weights[scope::Optimizer::FactorIndex::VertexDepthCamera] = 32000;
  options.weights[scope::Optimizer::FactorIndex::JointLimit] = 1000;
  options.weights[scope::Optimizer::FactorIndex::JointConst] = 1000;
  options.weights[scope::Optimizer::FactorIndex::Joint] = 10000;
  options.weights[scope::Optimizer::FactorIndex::Pose] = 500;
  options.weights[scope::Optimizer::FactorIndex::Parameter] = 1000;

  smplify.updateFactorWeights(options.weights);

  // Initialize joints
  // Initialize parameters
  smplify.initialize(root, joints, {params}, false);

  int NumIters = 0;
  //---------------------------------------------------------------
  // Stage 1
  //---------------------------------------------------------------
  for (int iter = 0; iter < 50; iter++) {
    smplify.optimize();

    if (smplify.getResults().expected_rel_cost_reduction.back() < 5e-2 ||
        smplify.getResults().rel_cost_reduction.back() < 1e-2) {
      break;
    }
  }

  //---------------------------------------------------------------
  // Stage 2
  //---------------------------------------------------------------
  NumIters += smplify.getResults().fobjs.size() - 1;

  RootPose = smplify.getPoses()[0];
  Joints = smplify.getJoints();
  Params = smplify.getParameters()[0];

  //------------------------------------------------------------------------------
  // Setup pose factor
  //------------------------------------------------------------------------------
  PoseSqrtCov = scope::Matrix6::Identity();
  PoseSqrtCov.diagonal().head<3>().setConstant(2);

  PoseFactor = std::make_shared<scope::PoseFactor>(0, PoseSqrtCov, RootPose);

  //------------------------------------------------------------------------------
  // Setup parameter factor
  //------------------------------------------------------------------------------
  ParameterFactor =
      std::make_shared<scope::ParameterFactor>(0, ShapeSqrtCov, Params);

  //------------------------------------------------------------------------------
  // Get KeyPoint3D origin position
  //------------------------------------------------------------------------------
  scope::getKeyPointOrigin3D(
      RootPose, JDirs, J, KeyPointDirs, KeyPoints, Params,
      scope::KeyPointInfo::SMPL::KeyPointOriginIndex, origin);

  //------------------------------------------------------------------------------
  // Setup JointDepthCamera factors
  //------------------------------------------------------------------------------
  scope::createJointDepthCameraFactors(JointDepthCamInfo, KeyPoint3DGMSigma,
                                       KeyPoint3DGMEps, Measurements3D, origin,
                                       JointDepthCamFactors);

  //------------------------------------------------------------------------------
  // Setup VertexDepthCamera factors
  //------------------------------------------------------------------------------
  scope::createVertexDepthCameraFactors(
      VertexDepthCamInfo, 0, JDirs, J, KeyPointDirs, KeyPoints,
      KeyPoint3DGMSigma, KeyPoint3DGMEps, Measurements3D, origin,
      VertexDepthCamFactors);

  // Reset optimizer
  smplify.reset();

  // Add factors
  for (const auto &factor : JointPinholeCamFactors) {
    smplify.addJointPinholeCameraFactor(factor);
  }

  for (const auto &factor : VertexPinholeCamFactors) {
    smplify.addVertexPinholeCameraFactor(factor);
  }

  for (const auto &factor : UnitPOFFactors) {
    smplify.addPOFFactor(factor);
  }

  for (const auto &factor : ScaledPOFFactors) {
    smplify.addPOFFactor(factor);
  }

  for (const auto &factor : RelPOFFactors) {
    smplify.addPOFFactor(factor);
  }

  for (const auto &factor : JointDepthCamFactors) {
    smplify.addDepthCameraFactor(factor);
  }

  for (const auto &factor : VertexDepthCamFactors) {
    smplify.addDepthCameraFactor(factor);
  }

  for (const auto &factor : JointFactors) {
    smplify.addJointFactor(factor);
  }

  for (const auto &factor : JointConstFactors) {
    smplify.addJointConstFactor(factor);
  }

  for (const auto &factor : EulerAngleConstFactors) {
    smplify.addJointConstFactor(factor);
  }

  for (int n = 0; n < NumJoints; n++) {
    smplify.addJointLimitFactor(JointLimitFactors[n]);
  }

  smplify.addParameterFactor(ParameterFactor);

  smplify.addPoseFactor(PoseFactor);

  options.weights.tail<8>().array() = 10;
  options.weights[scope::Optimizer::FactorIndex::UnitPOF] = 6000;
  options.weights[scope::Optimizer::FactorIndex::ScaledPOF] = 6000;
  options.weights[scope::Optimizer::FactorIndex::RelPOF] = 6000;
  options.weights[scope::Optimizer::FactorIndex::JointDepthCamera] = 24000;
  options.weights[scope::Optimizer::FactorIndex::VertexDepthCamera] = 24000;
  options.weights[scope::Optimizer::FactorIndex::JointLimit] = 500;
  options.weights[scope::Optimizer::FactorIndex::JointConst] = 1000;
  options.weights[scope::Optimizer::FactorIndex::Pose] = 400;
  options.weights[scope::Optimizer::FactorIndex::Joint] = 5000;
  options.weights[scope::Optimizer::FactorIndex::Parameter] = 400;
  smplify.updateFactorWeights(options.weights);

  // initialize the optimizer
  smplify.initialize(RootPose, Joints, {Params}, false);

  for (int iter = 0; iter < 50; iter++) {
    smplify.optimize();

    if (smplify.getResults().expected_rel_cost_reduction.back() < 5e-2 ||
        smplify.getResults().rel_cost_reduction.back() < 1e-2) {
      break;
    }
  }

  //---------------------------------------------------------------
  // Stage 3
  //---------------------------------------------------------------
  NumIters += smplify.getResults().fobjs.size() - 1;

  RootPose = smplify.getPoses()[0];
  Joints = smplify.getJoints();
  Params = smplify.getParameters()[0];

  //------------------------------------------------------------------------------
  // Setup pose factor
  //------------------------------------------------------------------------------
  PoseSqrtCov = scope::Matrix6::Identity();
  PoseSqrtCov.diagonal().head<3>().setConstant(2);

  PoseFactor = std::make_shared<scope::PoseFactor>(0, PoseSqrtCov, RootPose);

  //------------------------------------------------------------------------------
  // Setup parameter factor
  //------------------------------------------------------------------------------
  ParameterFactor =
      std::make_shared<scope::ParameterFactor>(0, ShapeSqrtCov, Params);

  //------------------------------------------------------------------------------
  // Get KeyPoint3D origin position
  //------------------------------------------------------------------------------
  scope::getKeyPointOrigin3D(
      RootPose, JDirs, J, KeyPointDirs, KeyPoints, Params,
      scope::KeyPointInfo::SMPL::KeyPointOriginIndex, origin);

  //------------------------------------------------------------------------------
  // Setup JointDepthCamera factors
  //------------------------------------------------------------------------------
  scope::createJointDepthCameraFactors(JointDepthCamInfo, KeyPoint3DGMSigma,
                                       KeyPoint3DGMEps, Measurements3D, origin,
                                       JointDepthCamFactors);

  //------------------------------------------------------------------------------
  // Setup VertexDepthCamera factors
  //------------------------------------------------------------------------------
  scope::createVertexDepthCameraFactors(
      VertexDepthCamInfo, 0, JDirs, J, KeyPointDirs, KeyPoints,
      KeyPoint3DGMSigma, KeyPoint3DGMEps, Measurements3D, origin,
      VertexDepthCamFactors);

  // Reset optimizer
  smplify.reset();

  // Add factors
  for (const auto &factor : JointPinholeCamFactors) {
    smplify.addJointPinholeCameraFactor(factor);
  }

  for (const auto &factor : VertexPinholeCamFactors) {
    smplify.addVertexPinholeCameraFactor(factor);
  }

  for (const auto &factor : UnitPOFFactors) {
    smplify.addPOFFactor(factor);
  }

  for (const auto &factor : ScaledPOFFactors) {
    smplify.addPOFFactor(factor);
  }

  for (const auto &factor : RelPOFFactors) {
    smplify.addPOFFactor(factor);
  }

  for (const auto &factor : JointDepthCamFactors) {
    smplify.addDepthCameraFactor(factor);
  }

  for (const auto &factor : VertexDepthCamFactors) {
    smplify.addDepthCameraFactor(factor);
  }

  for (const auto &factor : JointFactors) {
    smplify.addJointFactor(factor);
  }

  for (const auto &factor : JointConstFactors) {
    smplify.addJointConstFactor(factor);
  }

  for (const auto &factor : EulerAngleConstFactors) {
    smplify.addJointConstFactor(factor);
  }

  for (int n = 0; n < NumJoints; n++) {
    smplify.addJointLimitFactor(JointLimitFactors[n]);
  }

  smplify.addParameterFactor(ParameterFactor);

  smplify.addPoseFactor(PoseFactor);

  // Update weights
  options.weights.tail<8>() *= 0.1;
  options.weights[scope::Optimizer::FactorIndex::UnitPOF] = 3000;
  options.weights[scope::Optimizer::FactorIndex::ScaledPOF] = 3000;
  options.weights[scope::Optimizer::FactorIndex::RelPOF] = 3000;
  options.weights[scope::Optimizer::FactorIndex::JointDepthCamera] = 24000;
  options.weights[scope::Optimizer::FactorIndex::VertexDepthCamera] = 24000;
  options.weights[scope::Optimizer::FactorIndex::JointLimit] = 400;
  options.weights[scope::Optimizer::FactorIndex::JointConst] = 1000;
  options.weights[scope::Optimizer::FactorIndex::Pose] = 200;
  options.weights[scope::Optimizer::FactorIndex::Joint] = 2500;
  options.weights[scope::Optimizer::FactorIndex::Parameter] = 400;
  smplify.updateFactorWeights(options.weights);

  // initialize the optimizer
  smplify.initialize(RootPose, Joints, {Params}, false);

  for (int iter = 0; iter < 100; iter++) {
    smplify.optimize();

    if (smplify.getResults().expected_rel_cost_reduction.back() < 1e-2 ||
        smplify.getResults().rel_cost_reduction.back() < 2e-3) {
      break;
    }
  }

  //---------------------------------------------------------------
  // Stage 4
  //---------------------------------------------------------------
  options.weights[scope::Optimizer::FactorIndex::UnitPOF] = 1500;
  options.weights[scope::Optimizer::FactorIndex::ScaledPOF] = 1500;
  options.weights[scope::Optimizer::FactorIndex::RelPOF] = 1500;
  options.weights[scope::Optimizer::FactorIndex::JointDepthCamera] = 24000;
  options.weights[scope::Optimizer::FactorIndex::VertexDepthCamera] = 24000;
  options.weights[scope::Optimizer::FactorIndex::JointLimit] = 150;
  options.weights[scope::Optimizer::FactorIndex::JointConst] = 1000;
  options.weights[scope::Optimizer::FactorIndex::Pose] = 100;
  options.weights[scope::Optimizer::FactorIndex::Joint] = 500;
  options.weights[scope::Optimizer::FactorIndex::Parameter] = 100;
  smplify.updateFactorWeights(options.weights);

  for (int iter = 0; iter < 100; iter++) {
    smplify.optimize();

    if (smplify.getResults().expected_rel_cost_reduction.back() < 1e-2 ||
        smplify.getResults().rel_cost_reduction.back() < 2e-3) {
      break;
    }
  }

  //---------------------------------------------------------------
  // Stage 5
  //---------------------------------------------------------------
  options.weights[scope::Optimizer::FactorIndex::UnitPOF] = 1500;
  options.weights[scope::Optimizer::FactorIndex::ScaledPOF] = 1500;
  options.weights[scope::Optimizer::FactorIndex::RelPOF] = 1500;
  options.weights[scope::Optimizer::FactorIndex::JointDepthCamera] = 24000;
  options.weights[scope::Optimizer::FactorIndex::VertexDepthCamera] = 24000;
  options.weights[scope::Optimizer::FactorIndex::JointLimit] = 50;
  options.weights[scope::Optimizer::FactorIndex::JointConst] = 1000;
  options.weights[scope::Optimizer::FactorIndex::Pose] = 100;
  options.weights[scope::Optimizer::FactorIndex::Joint] = 100;
  options.weights[scope::Optimizer::FactorIndex::Parameter] = 20;
  smplify.updateFactorWeights(options.weights);

  for (int iter = 0; iter < 100; iter++) {
    smplify.optimize();

    if (smplify.getResults().expected_rel_cost_reduction.back() < 1e-3 ||
        smplify.getResults().rel_cost_reduction.back() < 2e-4) {
      break;
    }
  }

  //---------------------------------------------------------------
  // Stage 6
  //---------------------------------------------------------------
  NumIters += smplify.getResults().fobjs.size() - 1;

  RootPose = smplify.getPoses()[0];
  Joints = smplify.getJoints();
  Params = smplify.getParameters()[0];

  //------------------------------------------------------------------------------
  // Setup pose factor
  //------------------------------------------------------------------------------
  PoseSqrtCov = scope::Matrix6::Identity();
  PoseSqrtCov.diagonal().head<3>().setConstant(2);
  PoseSqrtCov.diagonal().tail<3>().setConstant(2000);

  PoseFactor = std::make_shared<scope::PoseFactor>(0, PoseSqrtCov, RootPose);

  //------------------------------------------------------------------------------
  // Setup parameter factor
  //------------------------------------------------------------------------------
  ParameterFactor =
      std::make_shared<scope::ParameterFactor>(0, ShapeSqrtCov, Params);

  //------------------------------------------------------------------------------
  // Get KeyPoint3D origin position
  //------------------------------------------------------------------------------
  scope::getKeyPointOrigin3D(
      RootPose, JDirs, J, KeyPointDirs, KeyPoints, Params,
      scope::KeyPointInfo::SMPL::KeyPointOriginIndex, origin);

  //------------------------------------------------------------------------------
  // Setup JointDepthCamera factors
  //------------------------------------------------------------------------------
  scope::createJointDepthCameraFactors(JointDepthCamInfo, KeyPoint3DGMSigma,
                                       KeyPoint3DGMEps, Measurements3D, origin,
                                       JointDepthCamFactors);

  //------------------------------------------------------------------------------
  // Setup VertexDepthCamera factors
  //------------------------------------------------------------------------------
  scope::createVertexDepthCameraFactors(
      VertexDepthCamInfo, 0, JDirs, J, KeyPointDirs, KeyPoints,
      KeyPoint3DGMSigma, KeyPoint3DGMEps, Measurements3D, origin,
      VertexDepthCamFactors);

  // Reset optimizer
  smplify.reset();

  // Add factors
  for (const auto &factor : JointPinholeCamFactors) {
    smplify.addJointPinholeCameraFactor(factor);
  }

  for (const auto &factor : VertexPinholeCamFactors) {
    smplify.addVertexPinholeCameraFactor(factor);
  }

  for (const auto &factor : UnitPOFFactors) {
    smplify.addPOFFactor(factor);
  }

  for (const auto &factor : ScaledPOFFactors) {
    smplify.addPOFFactor(factor);
  }

  for (const auto &factor : RelPOFFactors) {
    smplify.addPOFFactor(factor);
  }

  for (const auto &factor : JointDepthCamFactors) {
    smplify.addDepthCameraFactor(factor);
  }

  for (const auto &factor : VertexDepthCamFactors) {
    smplify.addDepthCameraFactor(factor);
  }

  for (const auto &factor : JointFactors) {
    smplify.addJointFactor(factor);
  }

  for (const auto &factor : JointConstFactors) {
    smplify.addJointConstFactor(factor);
  }

  for (const auto &factor : EulerAngleConstFactors) {
    smplify.addJointConstFactor(factor);
  }

  for (int n = 0; n < NumJoints; n++) {
    smplify.addJointLimitFactor(JointLimitFactors[n]);
  }

  smplify.addParameterFactor(ParameterFactor);

  smplify.addPoseFactor(PoseFactor);

  // Update weights
  options.weights[scope::Optimizer::FactorIndex::UnitPOF] = 2500;
  options.weights[scope::Optimizer::FactorIndex::ScaledPOF] = 2500;
  options.weights[scope::Optimizer::FactorIndex::RelPOF] = 2500;
  options.weights[scope::Optimizer::FactorIndex::JointDepthCamera] = 24000;
  options.weights[scope::Optimizer::FactorIndex::VertexDepthCamera] = 24000;
  options.weights[scope::Optimizer::FactorIndex::JointLimit] = 50;
  options.weights[scope::Optimizer::FactorIndex::JointConst] = 250;
  options.weights[scope::Optimizer::FactorIndex::Pose] = 100;
  options.weights[scope::Optimizer::FactorIndex::Joint] = 25;
  options.weights[scope::Optimizer::FactorIndex::Parameter] = 100;
  smplify.updateFactorWeights(options.weights);

  // initialize the optimizer
  smplify.initialize(RootPose, Joints, {Params}, false);

  for (int iter = 0; iter < 100; iter++) {
    smplify.optimize();

    if (smplify.getResults().expected_rel_cost_reduction.back() < 1e-4 ||
        smplify.getResults().rel_cost_reduction.back() < 2e-5) {
      break;
    }
  }

  NumIters += smplify.getResults().fobjs.size() - 1;

  auto OptTime = scope::Stopwatch::tock(OptStart);
  auto Fobj = smplify.getFobj();
  auto TotalTime = OptTime + SetupTime + InitTime;

  std::cout << "======================================================="
            << std::endl;
  std::cout << "File: " << KeyPointFile << std::endl;
  std::cout << "-------------------------------------------------------"
            << std::endl;
  std::cout << "objective value: " << Fobj << std::endl;
  std::cout << "init time: " << InitTime << std::endl;
  std::cout << "setup time: " << SetupTime << std::endl;
  std::cout << "optimization time: " << OptTime << std::endl;
  std::cout << "total time: " << TotalTime << std::endl;
  std::cout << "iterations: " << NumIters << std::endl;
  std::cout << "-------------------------------------------------------"
            << std::endl;

  //---------------------------------------------------------------
  // Results
  //---------------------------------------------------------------
  const auto &Poses = smplify.getPoses();
  Params = smplify.getParameters()[0];
  RootPose = smplify.getPoses()[0];
  Joints = smplify.getJoints();

  // Process root pose and joints
  Eigen::Matrix3Xd Rots;
  Rots.setZero(3, 24);

  auto RootRot = Rots.col(0);
  scope::math::SO3::log(RootPose.R, RootRot);

  for (int i = 0; i < SMPL::NumJoints; i++) {
    auto rot = Rots.col(i + 1);
    scope::math::SO3::log(Joints[i], rot);
  }

  nlohmann::json results;

  auto &trans = results["trans"];
  auto &rots = results["rotation"];
  auto &param = results["shape"];
  auto &cam = results["camera"];

  trans = {RootPose.t[0], RootPose.t[1], RootPose.t[2]};

  for (int i = 0; i <= SMPL::NumJoints; i++) {
    auto &rot = rots[i];

    for (int j = 0; j < 3; j++) rot[j] = Rots(j, i);
  }

  for (int i = 0; i < 10; i++) {
    param[i] = Params[i];
  }

  for (int i = 0; i < 4; i++) {
    cam[i] = CamParams[i];
  }

  std::ofstream resFile(ResultFile);

  if (resFile.fail()) {
    LOG(ERROR) << "Fail to open " << ResultFile << std::endl;
    return -1;
  }

  resFile << results.dump();
  resFile.close();

  std::cout << "Results saved in " << ResultFile << std::endl;
  std::cout << "-------------------------------------------------------"
            << std::endl;

  return 0;
}


<|start_filename|>C++/scope/utils/Setup.cpp<|end_filename|>
#include <scope/utils/Setup.h>

#include <fstream>
#include <nlohmann/json.hpp>

namespace scope {
int loadSMPLKeyPoints(const MatrixX &JDirs, const VectorX &J,
                      const MatrixX &VDirs, const VectorX &V,
                      MatrixX &KeyPointDirs, VectorX &KeyPoints) {
  const int NumPoses = 24;
  const int ParamSize = JDirs.cols();

  const auto &KeyPointInfo = KeyPointInfo::SMPL::KeyPointInfo;
  const auto &NumKeyPoints = KeyPointInfo.size();

  assert(JDirs.rows() == 3 * NumPoses);
  assert(J.rows() == 3 * NumPoses);
  assert(VDirs.rows() == 20670);
  assert(V.rows() == 20670);
  assert(JDirs.cols() == VDirs.cols());
  assert(JDirs.rightCols<1>().cwiseAbs().sum() < 1e-10);
  assert(VDirs.rightCols<1>().cwiseAbs().sum() < 1e-10);

  KeyPointDirs.setZero(3 * NumKeyPoints, ParamSize);
  KeyPoints.setZero(3 * NumKeyPoints);

  // setup vertex keypoints
  for (int i = 0; i < 11; i++) {
    assert(KeyPointInfo[i][1] < 6890);

    if (KeyPointInfo[i][1] >= 6890) {
      LOG(ERROR) << "Inconsistent keypoint information." << std::endl;
      exit(-1);
    }

    KeyPointDirs.middleRows<3>(3 * i) =
        VDirs.middleRows<3>(3 * KeyPointInfo[i][1]);
    KeyPoints.segment<3>(3 * i) = V.segment<3>(3 * KeyPointInfo[i][1]);
  }

  // setup head top
  const Scalar HeadTopRatio = 0.65;
  KeyPointDirs.middleRows<3>(3 * 11) =
      HeadTopRatio * VDirs.middleRows<3>(3 * 414) +
      (1 - HeadTopRatio) * JDirs.middleRows<3>(3 * 15);
  KeyPoints.segment<3>(3 * 11) = HeadTopRatio * V.segment<3>(3 * 414) +
                                 (1 - HeadTopRatio) * J.segment<3>(3 * 15);

  // setup chest keypoint
  const Scalar ChestRatio = 0.94;

  KeyPointDirs.middleRows<3>(3 * 12) = JDirs.middleRows<3>(3 * 6);
  KeyPointDirs.row(3 * 12 + 1) = ChestRatio * JDirs.row(3 * 6 + 1) +
                                 (1 - ChestRatio) * JDirs.row(3 * 3 + 1);

  KeyPoints.segment<3>(3 * 12) = J.segment<3>(3 * 6);
  KeyPoints[3 * 12 + 1] =
      ChestRatio * J[3 * 6 + 1] + (1 - ChestRatio) * J[3 * 3 + 1];

  // setup hip keypoints
  const Scalar HipRatio = 0.5;

  KeyPointDirs.middleRows<3>(3 * 13) = JDirs.middleRows<3>(0);
  KeyPointDirs.middleRows<3>(3 * 14) = JDirs.middleRows<3>(3);
  KeyPointDirs.middleRows<3>(3 * 15) = JDirs.middleRows<3>(6);

  Eigen::MatrixXd HDirs =
      JDirs.row(1) - 0.5 * JDirs.row(4) - 0.5 * JDirs.row(7);
  KeyPointDirs.row(3 * 13 + 1) -= (1 - HipRatio) * HDirs;
  KeyPointDirs.row(3 * 14 + 1) += HipRatio * HDirs;
  KeyPointDirs.row(3 * 15 + 1) += HipRatio * HDirs;

  KeyPointDirs.row(3 * 14) = JDirs.row(12);
  KeyPointDirs.row(3 * 15) = JDirs.row(15);

  KeyPointDirs.row(3 * 14 + 2) = JDirs.row(2);
  KeyPointDirs.row(3 * 15 + 2) = JDirs.row(2);

  KeyPoints.segment<3>(3 * 13) = J.segment<3>(0);
  KeyPoints.segment<3>(3 * 14) = J.segment<3>(3);
  KeyPoints.segment<3>(3 * 15) = J.segment<3>(6);

  Scalar H = J[1] - 0.5 * J[4] - 0.5 * J[7];
  KeyPoints[3 * 13 + 1] -= (1 - HipRatio) * H;
  KeyPoints[3 * 14 + 1] += HipRatio * H;
  KeyPoints[3 * 15 + 1] += HipRatio * H;

  KeyPoints[3 * 14] = J[12];
  KeyPoints[3 * 15] = J[15];

  KeyPoints[3 * 14 + 2] = J[2];
  KeyPoints[3 * 15 + 2] = J[2];

  KeyPointDirs(3 * 13 + 1, ParamSize - 1) = 10 * H;
  KeyPointDirs(3 * 14 + 1, ParamSize - 1) = 10 * H;
  KeyPointDirs(3 * 15 + 1, ParamSize - 1) = 10 * H;

  // setup upper arm keypoints
  const Scalar UpperArmRatio[] = {0.65, 0.5};
  KeyPointDirs.middleRows<3>(3 * 16) = JDirs.middleRows<3>(3 * 16);
  KeyPoints.segment<3>(3 * 16) = J.segment<3>(3 * 16);

  KeyPointDirs.middleRows<3>(3 * 17) = JDirs.middleRows<3>(3 * 17);
  KeyPoints.segment<3>(3 * 17) = J.segment<3>(3 * 17);

  KeyPointDirs.row(48) = (1 - UpperArmRatio[0]) * JDirs.row(48) +
                         UpperArmRatio[0] * KeyPointDirs.row(42);
  KeyPoints[48] =
      (1 - UpperArmRatio[0]) * J[48] + UpperArmRatio[0] * KeyPoints[42];

  KeyPointDirs.row(51) = (1 - UpperArmRatio[0]) * JDirs.row(51) +
                         UpperArmRatio[0] * KeyPointDirs.row(45);
  KeyPoints[51] =
      (1 - UpperArmRatio[0]) * J[51] + UpperArmRatio[0] * KeyPoints[45];

  KeyPointDirs.row(49) =
      (1 - UpperArmRatio[1]) * JDirs.row(49) + UpperArmRatio[1] * JDirs.row(37);
  KeyPoints[49] = (1 - UpperArmRatio[1]) * J[49] + UpperArmRatio[1] * J[37];

  KeyPointDirs.row(52) =
      (1 - UpperArmRatio[1]) * JDirs.row(52) + UpperArmRatio[1] * JDirs.row(37);
  KeyPoints[52] = (1 - UpperArmRatio[1]) * J[52] + UpperArmRatio[1] * J[37];

  // setup joint keypoints
  KeyPointDirs.bottomRows<3 * NumPoses>() = JDirs;
  KeyPoints.tail<3 * NumPoses>() = J;

  return 0;
}

int load3DPWAnnotation(const std::string &file, int &gender, VectorX &betas,
                       VectorX &poses, VectorX &cam) {
  cnpy::npz_t annot = cnpy::npz_load(file);

  cnpy::NpyArray subdata;

  subdata = annot.at("gender");
  assert(subdata.num_vals == 1);

  if (subdata.num_vals != 1) {
    LOG(ERROR) << "Bad input file format." << std::endl;

    exit(-1);
  }

  gender = *subdata.data<double>();

  subdata = annot.at("shape");
  assert(subdata.shape[0] == subdata.num_vals);

  if (subdata.shape[0] != subdata.num_vals) {
    LOG(ERROR) << "Bad input file format." << std::endl;

    exit(-1);
  }

  betas = Eigen::Map<VectorX>(subdata.data<double>(), subdata.num_vals);

  subdata = annot.at("pose");
  assert(subdata.shape[0] == subdata.num_vals);

  if (subdata.shape[0] != subdata.num_vals) {
    LOG(ERROR) << "Bad input file format." << std::endl;

    exit(-1);
  }

  poses = Eigen::Map<VectorX>(subdata.data<double>(), subdata.num_vals);

  subdata = annot.at("cam_params");
  assert(subdata.shape[0] == subdata.num_vals && subdata.num_vals == 4);

  if (subdata.num_vals != 4) {
    LOG(ERROR) << "Bad input file format." << std::endl;

    exit(-1);
  }

  cam = Eigen::Map<VectorX>(subdata.data<double>(), subdata.num_vals);

  return 0;
}

int loadKeyPoint2DMeasurements(const std::string &file,
                               Matrix3X &Measurements) {
  cnpy::NpyArray measurements = cnpy::npy_load(file);

  assert(measurements.shape[1] == 3);

  if (measurements.shape[1] != 3) {
    LOG(ERROR) << "Inconsistent measurement size." << std::endl;
    exit(-1);
  }

  Measurements = Eigen::Map<Eigen::Matrix3X<double>>(
                     measurements.data<double>(), measurements.shape[1],
                     measurements.shape[0])
                     .cast<Scalar>();

  Measurements.row(2) = Measurements.row(2).cwiseMin(1.0);

  return 0;
}

int loadKeyPoint3DMeasurements(const std::string &file,
                               Matrix3X &Measurements) {
  cnpy::NpyArray measurements = cnpy::npy_load(file);

  assert(measurements.shape[1] == 3);

  if (measurements.shape[1] != 3) {
    LOG(ERROR) << "Inconsistent measurement size." << std::endl;
    exit(-1);
  }

  Measurements = Eigen::Map<Eigen::Matrix3X<double>>(
                     measurements.data<double>(), measurements.shape[1],
                     measurements.shape[0])
                     .cast<Scalar>();

  Measurements.col(0).setZero();

  return 0;
}

int loadKeyPoint2Dand3DMeasurements(const std::string &file, Vector4 &CamParam,
                                    Matrix3X &Measurements2D,
                                    Matrix3X &Measurements3D) {
  std::ifstream input(file);

  if (input.fail()) {
    LOG(ERROR) << "Fail to open " << file << std::endl;
    exit(-1);
  }

  nlohmann::json measurements;

  input >> measurements;

  CamParam.setZero();
  Measurements2D.resize(3, 0);
  Measurements3D.resize(3, 0);

  assert(measurements.count("camera"));

  if (measurements.count("camera")) {
    const auto &camera = measurements["camera"];

    assert(camera.size() == 4);

    if (camera.size() != 4) {
      LOG(ERROR) << "Inconsistent camera parameter size." << std::endl;
      exit(-1);
    }

    for (int i = 0; i < 4; i++) {
      CamParam[i] = camera[i];
    }
  } else {
    LOG(ERROR) << "No 2D keypoints." << std::endl;
    exit(-1);
  }

  assert(measurements.count("keypoints2D"));

  if (measurements.count("keypoints2D")) {
    const auto &kpts2D = measurements["keypoints2D"];

    Measurements2D.resize(3, kpts2D.size());

    for (int i = 0; i < kpts2D.size(); i++) {
      const auto &kpt2D = kpts2D[i];

      assert(kpt2D.size() == 3);

      if (kpt2D.size() != 3) {
        LOG(ERROR) << "Inconsistent measurement size." << std::endl;
        exit(-1);
      }

      for (int j = 0; j < 3; j++) {
        Measurements2D(j, i) = kpt2D[j];
      }
    }
  } else {
    LOG(ERROR) << "No 2D keypoints." << std::endl;
    exit(-1);
  }

  assert(measurements.count("keypoints3D"));

  if (measurements.count("keypoints3D")) {
    const auto &kpts3D = measurements["keypoints3D"];

    Measurements3D.resize(3, kpts3D.size());

    for (int i = 0; i < kpts3D.size(); i++) {
      const auto &kpt3D = kpts3D[i];

      assert(kpt3D.size() == 3);

      if (kpt3D.size() != 3) {
        LOG(ERROR) << "Inconsistent measurement size." << std::endl;
        exit(-1);
      }

      for (int j = 0; j < 3; j++) {
        Measurements3D(j, i) = kpt3D[j];
      }
    }
  } else {
    LOG(ERROR) << "No 3D keypoints." << std::endl;
    exit(-1);
  }

  return 0;
}

int loadPOFMeasurements(const Matrix3X &KeyPoints3D, Matrix3X &Measurements) {
  const auto &POFInfo = KeyPointInfo::SMPL::POFInfo;

  Measurements.setZero(3, POFInfo.size());

  for (int i = 0; i < POFInfo.size(); i++) {
    assert(POFInfo[i][0] < KeyPoints3D.cols());
    assert(POFInfo[i][1] < KeyPoints3D.cols());

    if (POFInfo[i][0] >= KeyPoints3D.cols() ||
        POFInfo[i][1] >= KeyPoints3D.cols()) {
      LOG(ERROR) << "Inconsistent measurement size." << std::endl;
      exit(-1);
    }

    Measurements.col(i) =
        KeyPoints3D.col(POFInfo[i][1]) - KeyPoints3D.col(POFInfo[i][0]);
  }

  Measurements.colwise().normalize();

  return 0;
}

int loadJointLimitNeuralNetwork(const std::string &file,
                                AlignedVector<Matrix6> &weight,
                                AlignedVector<Vector6> &bias, VectorX &PRelua,
                                VectorX &PRelub) {
  cnpy::npz_t nn = cnpy::npz_load(file);

  const int size = nn.size();

  assert(size % 4 == 2);

  if (size % 4 != 2) {
    LOG(ERROR) << "Bad input file format." << std::endl;

    weight.clear();
    bias.clear();
    PRelua.resize(0);
    PRelub.resize(0);

    exit(-1);
  }

  const int num_layers = size / 4;

  weight.resize(num_layers + 1);
  bias.resize(num_layers + 1);
  PRelua.resize(num_layers);
  PRelub.resize(num_layers);

  cnpy::NpyArray w, b, a, s;

  for (int n = 0; n < num_layers; n++) {
    w = nn.at("weight" + std::to_string(n));
    b = nn.at("bias" + std::to_string(n));
    a = nn.at("alpha" + std::to_string(n));
    s = nn.at("beta" + std::to_string(n));

    weight[n] = Eigen::Map<Eigen::Matrix<double, 6, 6>>(w.data<double>(),
                                                        w.shape[1], w.shape[0])
                    .transpose()
                    .cast<Scalar>();
    bias[n] =
        Eigen::Map<Eigen::Matrix<double, 6, 1>>(b.data<double>(), b.num_vals)
            .cast<Scalar>();
    PRelua[n] = a.data<double>()[0];
    PRelub[n] = s.data<double>()[0];
  }

  w = nn.at("weight" + std::to_string(num_layers));
  b = nn.at("bias" + std::to_string(num_layers));

  weight[num_layers] = Eigen::Map<Eigen::Matrix<double, 6, 6>>(
                           w.data<double>(), w.shape[1], w.shape[0])
                           .transpose()
                           .cast<Scalar>();
  bias[num_layers] =
      Eigen::Map<Eigen::Matrix<double, 6, 1>>(b.data<double>(), b.num_vals)
          .cast<Scalar>();

  return 0;
}

int loadJointLimitPrior(const std::string &file,
                        AlignedVector<AlignedVector<Matrix6>> &weight,
                        AlignedVector<AlignedVector<Vector6>> &bias,
                        AlignedVector<VectorX> &PRelua,
                        AlignedVector<VectorX> &PRelub) {
  std::ifstream input(file);

  if (input.fail()) {
    LOG(ERROR) << "Fail to open " << file << std::endl;
    exit(-1);
  }

  nlohmann::json joints;

  input >> joints;

  int K = joints.size();

  weight.resize(K);
  bias.resize(K);
  PRelua.resize(K);
  PRelub.resize(K);

  for (int k = 0; k < K; k++) {
    const auto &joint = joints[k];

    int num_layers = joint.size() / 4;

    weight[k].resize(num_layers + 1);
    bias[k].resize(num_layers + 1);
    PRelua[k].resize(num_layers);
    PRelub[k].resize(num_layers);

    for (int n = 0; n < num_layers; n++) {
      const auto &w = joint["weight" + std::to_string(n)];
      const auto &b = joint["bias" + std::to_string(n)];
      const auto &a = joint["alpha" + std::to_string(n)];
      const auto &s = joint["beta" + std::to_string(n)];

      weight[k][n].setZero();
      bias[k][n].setZero();

      for (int i = 0; i < 6; i++) {
        for (int j = 0; j < 6; j++) {
          weight[k][n](i, j) = w[i][j].get<double>();
        }

        bias[k][n][i] = b[i].get<double>();
      }

      PRelua[k][n] = a.get<double>();
      PRelub[k][n] = s.get<double>();
    }

    const auto &w = joint["weight" + std::to_string(num_layers)];
    const auto &b = joint["bias" + std::to_string(num_layers)];

    for (int i = 0; i < 6; i++) {
      for (int j = 0; j < 6; j++) {
        weight[k][num_layers](i, j) = w[i][j].get<double>();
      }

      bias[k][num_layers][i] = b[i].get<double>();
    }
  }

  return 0;
}

int createInitialSMPLSkeleton(const Matrix3X &Measurements3D, VectorX &J) {
  J.setZero(Measurements3D.size());

  Eigen::Map<Matrix3X> Joints(J.data(), 3, Measurements3D.cols());

  // hip setup
  {
    Scalar L =
        0.5 * (Measurements3D.col(1) - Measurements3D.col(0)).stableNorm() +
        0.5 * (Measurements3D.col(4) - Measurements3D.col(0)).stableNorm();
    Joints.col(1) = Joints.col(0);
    Joints(0, 1) += L;
    Joints.col(4) = Joints.col(0);
    Joints(0, 4) -= L;
  }

  // knee setup
  {
    Scalar L =
        0.5 * (Measurements3D.col(2) - Measurements3D.col(1)).stableNorm() +
        0.5 * (Measurements3D.col(5) - Measurements3D.col(4)).stableNorm();
    Joints.col(2) = Joints.col(1);
    Joints(1, 2) -= L;
    Joints.col(5) = Joints.col(4);
    Joints(1, 5) -= L;
  }

  // ankle setup
  {
    Scalar L =
        0.5 * (Measurements3D.col(3) - Measurements3D.col(2)).stableNorm() +
        0.5 * (Measurements3D.col(6) - Measurements3D.col(5)).stableNorm();
    Joints.col(3) = Joints.col(2);
    Joints(1, 3) -= L;
    Joints.col(6) = Joints.col(5);
    Joints(1, 6) -= L;
  }

  // chest setup
  {
    Scalar L = (Measurements3D.col(7) - Measurements3D.col(0)).stableNorm();
    Joints.col(7) = Joints.col(0);
    Joints(1, 7) += L;
  }

  // thorax setup
  {
    Scalar L = (Measurements3D.col(8) - Measurements3D.col(7)).stableNorm();
    Joints.col(8) = Joints.col(7);
    Joints(1, 8) += L;
  }

  // neck setup
  {
    Vector3 Thorax = Measurements3D.col(8) - Measurements3D.col(7);
    Vector3 Neck = Measurements3D.col(9) - Measurements3D.col(8);

    Scalar LThorax = Thorax.stableNorm();
    Scalar LNeck = Neck.stableNorm();

    Scalar theta = M_PI / 4;

    Joints.col(9) = Joints.col(8);
    Joints(1, 9) += LNeck * sin(theta);
    Joints(2, 9) += LNeck * cos(theta);
  }

  // head top setup
  {
    Vector3 Neck = Measurements3D.col(9) - Measurements3D.col(8);
    Vector3 Head = Measurements3D.col(10) - Measurements3D.col(9);

    Scalar LNeck = Neck.stableNorm();
    Scalar LHead = Head.stableNorm();

    Scalar theta = acos(Neck.dot(Head) / (LNeck * LHead)) + M_PI / 4;

    Joints.col(10) = Joints.col(9);
    Joints(1, 10) += LHead * sin(theta);
    Joints(2, 10) += LHead * cos(theta);
  }

  // shoulder setup
  {
    Vector3 LShoulder = Measurements3D.col(11) - Measurements3D.col(8);
    Vector3 RShoulder = Measurements3D.col(14) - Measurements3D.col(8);
    Scalar LLShoulder = LShoulder.stableNorm();
    Scalar LRShoulder = RShoulder.stableNorm();

    Scalar theta =
        0.5 * acos(LShoulder.dot(RShoulder) / (LLShoulder * LRShoulder));

    Scalar L = 0.5 * LLShoulder + 0.5 * LRShoulder;
    Scalar X = L * sin(theta);
    Scalar Y = L * cos(theta);

    Joints.col(11) = Joints.col(8);
    Joints(0, 11) += X;
    Joints(1, 11) -= Y;
    Joints.col(14) = Joints.col(8);
    Joints(0, 14) -= X;
    Joints(1, 14) -= Y;
  }

  // elbow setup
  {
    Scalar L =
        0.5 * (Measurements3D.col(12) - Measurements3D.col(11)).stableNorm() +
        0.5 * (Measurements3D.col(15) - Measurements3D.col(14)).stableNorm();

    Joints.col(12) = Joints.col(11);
    Joints(0, 12) += L;
    Joints.col(15) = Joints.col(14);
    Joints(0, 15) -= L;
  }

  // wrist setup
  {
    Scalar L =
        0.5 * (Measurements3D.col(13) - Measurements3D.col(12)).stableNorm() +
        0.5 * (Measurements3D.col(16) - Measurements3D.col(15)).stableNorm();

    Joints.col(13) = Joints.col(12);
    Joints(0, 13) += L;
    Joints.col(16) = Joints.col(15);
    Joints(0, 16) -= L;
  }

  return 0;
}

int createInitialPinholeCameraFactors(
    const std::vector<std::array<int, 3>> &Info,
    const std::vector<std::array<int, 2>> &Index, const VectorX &KeyPoints,
    Scalar gm_est_sigma, Scalar gm_est_eps, const Matrix3X &Measurements,
    const VectorX &ConfidenceThreshold2D,
    std::vector<std::shared_ptr<Initializer::PinholeCameraFactor>> &factors) {
  factors.clear();

  assert(KeyPoints.size() % 3 == 0);
  assert(Measurements.cols() == ConfidenceThreshold2D.size());

  if (KeyPoints.size() % 3 ||
      Measurements.cols() != ConfidenceThreshold2D.size()) {
    LOG(ERROR) << "Inconsistent keypoints or joint locations." << std::endl;

    exit(-1);
  }

  const int M = Measurements.cols();
  const int I = Index.size();
  const int K = KeyPoints.size() / 3;

  factors.reserve(Info.size());

  for (const auto &info : Info) {
    const auto &m = info[0];
    const auto &i = info[1];
    const auto &v = info[2];

    assert(m < M);
    assert(i < I);
    assert(v < K);

    const int &n = Index[i][1];

    assert(n < K);

    if (m >= M || i >= I || v >= K || n >= K) {
      LOG(ERROR) << "Inconsistent factor information or measurements."
                 << std::endl;

      factors.clear();

      exit(-1);
    }

    const auto &measurement = Measurements.col(m);
    const auto &confidence_threshold = ConfidenceThreshold2D(m);

    if (measurement[2] < confidence_threshold) {
      continue;
    }

    Vector3 kpt = KeyPoints.segment<3>(3 * v) - KeyPoints.segment<3>(3 * n);
    factors.push_back(std::make_shared<Initializer::PinholeCameraFactor>(
        i, kpt, gm_est_sigma, gm_est_eps, measurement.head<2>(),
        measurement[2]));
  }

  return 0;
}

int createInitialPOFFactors(
    const AlignedVector<std::tuple<int, int, scope::Vector3, Scalar>> &Info,
    Scalar gm_est_sigma, Scalar gm_est_eps, const Matrix3X &Measurements,
    std::vector<std::shared_ptr<Initializer::POFFactor>> &factors) {
  factors.clear();

  const int M = Measurements.cols();

  factors.reserve(Info.size());

  for (const auto &info : Info) {
    const auto &m = std::get<0>(info);
    const auto &i = std::get<1>(info);
    const auto &d = std::get<2>(info);
    const auto &confidence = std::get<3>(info);

    assert(m < M);

    if (m >= M) {
      LOG(ERROR) << "Inconsistent factor information or measurements."
                 << std::endl;

      factors.clear();

      exit(-1);
    }

    const auto &measurement = Measurements.col(m);

    if (measurement.stableNorm() < 1e-8) {
      continue;
    }

    factors.push_back(std::make_shared<Initializer::POFFactor>(
        i, d, gm_est_sigma, gm_est_eps, measurement, confidence));
  }

  return 0;
}

int createInitialDepthCameraFactors(
    const std::vector<std::tuple<int, int, int, Scalar>> &Info,
    const std::vector<std::array<int, 2>> &Index, const VectorX &KeyPoints,
    Scalar gm_est_sigma, Scalar gm_est_eps, const Matrix3X &Measurements,
    const Vector3 &root,
    std::vector<std::shared_ptr<Initializer::DepthCameraFactor>> &factors) {
  factors.clear();

  assert(KeyPoints.size() % 3 == 0);

  if (KeyPoints.size() % 3) {
    LOG(ERROR) << "Inconsistent keypoints or joint locations." << std::endl;

    exit(-1);
  }

  const int M = Measurements.cols();
  const int I = Index.size();
  const int K = KeyPoints.size() / 3;

  factors.reserve(Info.size());

  for (const auto &info : Info) {
    const auto &m = std::get<0>(info);
    const auto &i = std::get<1>(info);
    const auto &v = std::get<2>(info);
    const auto &confidence = std::get<3>(info);

    assert(m < M);
    assert(i < I);
    assert(v < K);

    const int &n = Index[i][1];

    assert(n < K);

    if (m >= M || i >= I || v >= K || n >= K) {
      LOG(ERROR) << "Inconsistent factor information or measurements."
                 << std::endl;

      factors.clear();

      exit(-1);
    }

    const Vector3 measurement = Measurements.col(m) + root;

    Vector3 kpt = KeyPoints.segment<3>(3 * v) - KeyPoints.segment<3>(3 * n);

    factors.push_back(std::make_shared<Initializer::DepthCameraFactor>(
        i, kpt, gm_est_sigma, gm_est_eps, measurement, confidence));
  }

  return 0;
}

int createInitialJointLimitFactors(
    const std::vector<std::array<int, 2>> &Index,
    const AlignedVector<AlignedVector<scope::Matrix6>> &weight,
    const AlignedVector<AlignedVector<Vector6>> &bias,
    const AlignedVector<VectorX> &PRelua, const AlignedVector<VectorX> &PRelub,
    const VectorX &scale,
    std::vector<std::shared_ptr<Initializer::JointLimitFactor>> &factors) {
  factors.clear();

  const int N = weight.size();

  assert(bias.size() == N);
  assert(PRelua.size() == N);
  assert(PRelub.size() == N);
  assert(scale.size() == N);

  if (bias.size() != N || PRelua.size() != N || PRelub.size() != N ||
      scale.size() != N) {
    LOG(ERROR) << "Inconsistent factor information or measurements."
               << std::endl;

    exit(-1);
  }

  for (int n = 1; n < Index.size(); n++) {
    const int &j = Index[n][0] - 1;

    assert(j < N && j >= 0);

    if (j >= N || j < 0) {
      LOG(ERROR) << "Inconsistent factor information or measurements."
                 << std::endl;

      factors.clear();

      exit(-1);
    }

    factors.push_back(std::make_shared<Initializer::JointLimitFactor>(
        n - 1, weight[j], bias[j], PRelua[j], PRelub[j], scale[j]));
  }

  return 0;
}

int createInitialPoseConstFactors(
    const AlignedVector<std::tuple<int, Vector6, Vector6, Vector6>>
        &PoseConstInfo,
    const AlignedVector<Pose> &PoseRef,
    std::vector<std::shared_ptr<Initializer::PoseConstFactor>> &factors) {
  factors.clear();

  const int N = PoseRef.size();
  assert(PoseConstInfo.size() == N);

  if (PoseConstInfo.size() != N) {
    LOG(ERROR) << "Inconsistent factor information." << std::endl;

    exit(-1);
  }

  for (int n = 0; n < N; n++) {
    const auto &info = PoseConstInfo[n];
    const auto &pose = std::get<0>(info);

    assert(pose >= 0);

    if (pose < 0) {
      LOG(ERROR) << "Inconsistent factor information." << std::endl;

      exit(-1);
    }

    const auto &weight = std::get<1>(info);
    const auto &lbnd = std::get<2>(info);
    const auto &ubnd = std::get<3>(info);

    factors.push_back(std::make_shared<Initializer::PoseConstFactor>(
        pose, weight, PoseRef[n], lbnd, ubnd));
  }

  return 0;
}

int createInitialJointConstFactors(
    const AlignedVector<std::tuple<int, Vector3, Matrix3, Vector3, Vector3>>
        &JointConstInfo,
    std::vector<std::shared_ptr<Initializer::JointConstFactor>> &factors) {
  factors.clear();

  const int N = JointConstInfo.size();

  for (int n = 0; n < N; n++) {
    const auto &info = JointConstInfo[n];
    const auto &joint = std::get<0>(info);

    assert(joint >= 0);

    if (joint < 0) {
      LOG(ERROR) << "Inconsistent factor information." << std::endl;

      exit(-1);
    }

    const auto &weight = std::get<1>(info);
    const auto &ref = std::get<2>(info);
    const auto &lbnd = std::get<3>(info);
    const auto &ubnd = std::get<4>(info);

    factors.push_back(std::make_shared<Initializer::JointConstFactor>(
        joint, weight, ref, lbnd, ubnd));
  }

  return 0;
}

int createInitialEulerAngleConstFactors(
    const AlignedVector<std::tuple<int, Vector3, Matrix3, Vector3, Vector3>>
        &JointConstInfo,
    std::vector<std::shared_ptr<Initializer::EulerAngleConstFactor>> &factors) {
  factors.clear();

  const int N = JointConstInfo.size();

  for (int n = 0; n < N; n++) {
    const auto &info = JointConstInfo[n];
    const auto &joint = std::get<0>(info);

    assert(joint >= 0);

    if (joint < 0) {
      LOG(ERROR) << "Inconsistent factor information." << std::endl;

      exit(-1);
    }

    const auto &weight = std::get<1>(info);
    const auto &ref = std::get<2>(info);
    const auto &lbnd = std::get<3>(info);
    const auto &ubnd = std::get<4>(info);

    factors.push_back(std::make_shared<Initializer::EulerAngleConstFactor>(
        joint, weight, ref, lbnd, ubnd));
  }

  return 0;
}

int createInitialRootPinholeCameraFactors(
    const std::vector<std::array<int, 2>> &JointPinholeInfo,
    const std::vector<std::array<int, 3>> &VertexPinholeInfo,
    const AlignedVector<Pose> &poses, const VectorX &Param,
    const MatrixX &JDirs, const VectorX &J, const MatrixX &KeyPointDirs,
    const VectorX &KeyPoints, Scalar gm_est_sigma, Scalar gm_est_eps,
    const Matrix3X &Measurements, const VectorX &ConfidenceThreshold2D,
    std::vector<std::shared_ptr<Initializer::PinholeCameraFactor>> &factors) {
  assert(KeyPoints.size() % 3 == 0);
  assert(J.size() % 3 == 0);
  assert(KeyPointDirs.rows() == KeyPoints.size());
  assert(JDirs.rows() == J.size());
  assert(Measurements.cols() == ConfidenceThreshold2D.size());

  factors.clear();

  if (KeyPoints.size() % 3 || J.size() % 3 ||
      KeyPointDirs.rows() != KeyPoints.size() || JDirs.rows() != J.size() ||
      Measurements.cols() != ConfidenceThreshold2D.size()) {
    LOG(ERROR) << "Inconsistent keypoints or joint locations." << std::endl;

    exit(-1);
  }

  const int M = Measurements.cols();
  const int N = poses.size();
  const int K = KeyPoints.size() / 3;

  for (const auto &info : JointPinholeInfo) {
    const auto &m = info[0];
    const auto &n = info[1];

    assert(m < M);
    assert(n < N);

    if (m >= M || n >= N) {
      LOG(ERROR) << "Inconsistent factor information or measurements."
                 << std::endl;

      factors.clear();

      exit(-1);
    }

    const auto &measurement = Measurements.col(m);
    const auto &confidence_threshold = ConfidenceThreshold2D(m);

    if (measurement[2] < confidence_threshold) {
      continue;
    }

    factors.push_back(std::make_shared<Initializer::PinholeCameraFactor>(
        0, poses[n].t, gm_est_sigma, gm_est_eps, measurement.head<2>(),
        measurement[2]));
  }

  Matrix3X KptDirs;
  Vector3 vertex;
  Vector3 p;

  for (const auto &info : VertexPinholeInfo) {
    const auto &m = info[0];
    const auto &n = info[1];
    const auto &k = info[2];

    assert(m < M);
    assert(n < N);
    assert(k < K);

    if (m >= M || n >= N || k >= K) {
      LOG(ERROR) << "Inconsistent factor information or measurements."
                 << std::endl;

      factors.clear();

      exit(-1);
    }

    const auto &measurement = Measurements.col(m);
    const auto &confidence_threshold = ConfidenceThreshold2D(m);

    if (measurement[2] < confidence_threshold) {
      continue;
    }

    KptDirs = KeyPointDirs.middleRows<3>(3 * k) - JDirs.middleRows<3>(3 * n);
    vertex = KeyPoints.segment<3>(3 * k) - J.segment<3>(3 * n);
    vertex.noalias() += KptDirs * Param;

    p = poses[n].t;
    p.noalias() += poses[n].R * vertex;

    factors.push_back(std::make_shared<Initializer::PinholeCameraFactor>(
        0, p, gm_est_sigma, gm_est_eps, measurement.head<2>(), measurement[2]));
  }

  return 0;
}

int createInitialRootPOFFactors(
    const AlignedVector<std::tuple<int, int, scope::Vector3, Scalar>>
        &UnitPOFInfo,
    const std::vector<std::tuple<int, int, int, int, Scalar>> &ScaledPOFInfo,
    const std::vector<std::tuple<int, int, int, int, int, Scalar>> &RelPOFInfo,
    const AlignedVector<Pose> &poses, const VectorX &Param,
    const MatrixX &JDirs, const VectorX &J, const MatrixX &KeyPointDirs,
    const VectorX &KeyPoints, const std::vector<int> &kintree,
    Scalar gm_est_sigma, Scalar gm_est_eps, const Matrix3X &Measurements,
    std::vector<std::shared_ptr<Initializer::POFFactor>> &factors) {
  assert(KeyPoints.size() % 3 == 0);
  assert(J.size() % 3 == 0);
  assert(KeyPointDirs.rows() == KeyPoints.size());
  assert(JDirs.rows() == J.size());

  factors.clear();

  if (KeyPoints.size() % 3 || J.size() % 3 ||
      KeyPointDirs.rows() != KeyPoints.size() || JDirs.rows() != J.size()) {
    LOG(ERROR) << "Inconsistent keypoints or joint locations." << std::endl;

    exit(-1);
  }

  const int M = Measurements.cols();
  const int N = poses.size();
  const int K = KeyPoints.size() / 3;

  Vector3 d;

  for (const auto &info : UnitPOFInfo) {
    const auto &m = std::get<0>(info);
    const auto &n = std::get<1>(info);
    const auto &S = std::get<2>(info);
    const auto &confidence = std::get<3>(info);

    assert(m < M);
    assert(n < N);

    if (m >= M || n >= N) {
      LOG(ERROR) << "Inconsistent factor information or measurements."
                 << std::endl;

      factors.clear();

      exit(-1);
    }

    const auto &measurement = Measurements.col(m);

    if (measurement.stableNorm() < 1e-8) {
      continue;
    }

    d.noalias() = poses[n].R * S;

    factors.push_back(std::make_shared<Initializer::POFFactor>(
        0, d, gm_est_sigma, gm_est_eps, measurement, confidence));
  }

  Matrix3X Dirs;
  Vector3 vertex;

  for (const auto &info : ScaledPOFInfo) {
    const auto &m = std::get<0>(info);
    const auto &n = std::get<1>(info);
    const auto &v0 = std::get<2>(info);
    const auto &v1 = std::get<3>(info);
    const auto &confidence = std::get<4>(info);

    assert(m < M);
    assert(n < N);
    assert(v0 < K);
    assert(v1 < K);

    if (m >= M || n >= N || v0 >= K || v1 >= K) {
      LOG(ERROR) << "Inconsistent factor information or measurements."
                 << std::endl;

      factors.clear();

      exit(-1);
    }

    const auto &measurement = Measurements.col(m);

    if (measurement.stableNorm() < 1e-8) {
      continue;
    }

    Dirs =
        KeyPointDirs.middleRows<3>(3 * v1) - KeyPointDirs.middleRows<3>(3 * v0);
    vertex = KeyPoints.segment<3>(3 * v1) - KeyPoints.segment<3>(3 * v0);
    vertex.noalias() += Dirs * Param;

    d.noalias() = poses[n].R * vertex;

    factors.push_back(std::make_shared<Initializer::POFFactor>(
        0, d, gm_est_sigma, gm_est_eps, measurement, confidence));
  }

  for (const auto &info : RelPOFInfo) {
    const auto &m = std::get<0>(info);
    const auto &n = std::get<1>(info);
    const auto &v0 = std::get<2>(info);
    const auto &v1 = std::get<3>(info);
    const auto &confidence = std::get<5>(info);

    assert(m < M);
    assert(n < N);
    assert(kintree[n] < n);
    assert(v0 < K);
    assert(v1 < K);

    if (m >= M || n > N || kintree[n] >= n || v0 >= K || v1 >= K) {
      LOG(ERROR) << "Inconsistent factor information or measurements."
                 << std::endl;

      factors.clear();

      exit(-1);
    }

    Vector3 measurement = Measurements.col(m) * std::get<4>(info);

    if (measurement.stableNorm() < 1e-8) {
      continue;
    }

    Dirs = KeyPointDirs.middleRows<3>(3 * v1) - JDirs.middleRows<3>(3 * n);
    vertex = KeyPoints.segment<3>(3 * v1) - J.segment<3>(3 * n);
    vertex.noalias() += Dirs * Param;
    d.noalias() = poses[n].R * vertex;

    Dirs = KeyPointDirs.middleRows<3>(3 * v0) - JDirs.middleRows<3>(3 * n);
    vertex = KeyPoints.segment<3>(3 * v0) - J.segment<3>(3 * n);
    vertex.noalias() += Dirs * Param;
    d.noalias() -= poses[kintree[n]].R * vertex;

    if (d.stableNorm() < 1e-8) {
      continue;
    }

    factors.push_back(std::make_shared<Initializer::POFFactor>(
        0, d, gm_est_sigma, gm_est_eps, measurement, confidence));
  }

  return 0;
}

int createJointPinholeCameraFactors(
    const std::vector<std::array<int, 2>> &Info, Scalar gm_est_sigma,
    Scalar gm_est_eps, const Matrix3X &Measurements,
    const VectorX &ConfidenceThreshold2D,
    std::vector<std::shared_ptr<JointPinholeCameraFactor>> &factors) {
  factors.clear();

  assert(Measurements.cols() == ConfidenceThreshold2D.size());

  if (Measurements.cols() != ConfidenceThreshold2D.size()) {
    LOG(ERROR) << "Inconsistent keypoints or joint locations." << std::endl;

    exit(-1);
  }

  const int M = Measurements.cols();

  factors.reserve(Info.size());

  for (const auto &info : Info) {
    assert(info[0] < M);

    if (info[0] >= M) {
      LOG(ERROR) << "Inconsistent factor information or measurements."
                 << std::endl;

      factors.clear();

      exit(-1);
    }

    const auto &measurement = Measurements.col(info[0]);
    const auto &confidence_threshold = ConfidenceThreshold2D(info[0]);

    if (measurement[2] < confidence_threshold) {
      continue;
    }

    factors.push_back(std::make_shared<JointPinholeCameraFactor>(
        info[1], gm_est_sigma, gm_est_eps, measurement.head<2>(),
        measurement[2]));
  }

  return 0;
}

int createVertexPinholeCameraFactors(
    const std::vector<std::array<int, 3>> &Info, int VertexParam,
    const MatrixX &JDirs, const VectorX &J, const MatrixX &KeyPointDirs,
    const VectorX &KeyPoints, Scalar gm_est_sigma, Scalar gm_est_eps,
    const Matrix3X &Measurements, const VectorX &ConfidenceThreshold2D,
    std::vector<std::shared_ptr<VertexPinholeCameraFactor>> &factors) {
  factors.clear();

  assert(KeyPoints.size() % 3 == 0);
  assert(J.size() % 3 == 0);
  assert(KeyPointDirs.rows() == KeyPoints.size());
  assert(JDirs.rows() == J.size());
  assert(Measurements.cols() == ConfidenceThreshold2D.size());

  if (KeyPoints.size() % 3 || J.size() % 3 ||
      KeyPointDirs.rows() != KeyPoints.size() || JDirs.rows() != J.size() ||
      Measurements.cols() != ConfidenceThreshold2D.size()) {
    LOG(ERROR) << "Inconsistent keypoints or joint locations." << std::endl;

    exit(-1);
  }

  const int M = Measurements.cols();
  const int N = J.size() / 3;
  const int K = KeyPoints.size() / 3;

  factors.reserve(Info.size());

  Matrix3X KptDirs;
  Vector3 Kpt;

  for (const auto &info : Info) {
    assert(info[0] < M);
    assert(info[1] < N);
    assert(info[2] < K);

    if (info[0] >= M || info[1] >= N || info[2] >= K) {
      LOG(ERROR) << "Inconsistent factor information or measurements."
                 << std::endl;

      factors.clear();

      exit(-1);
    }

    const auto &measurement = Measurements.col(info[0]);
    const auto &confidence_threshold = ConfidenceThreshold2D(info[0]);

    if (measurement[2] < confidence_threshold) {
      continue;
    }

    KptDirs = KeyPointDirs.middleRows<3>(3 * info[2]) -
              JDirs.middleRows<3>(3 * info[1]);
    Kpt = KeyPoints.segment<3>(3 * info[2]) - J.segment<3>(3 * info[1]);

    factors.push_back(std::make_shared<VertexPinholeCameraFactor>(
        info[1], VertexParam, KptDirs, Kpt, gm_est_sigma, gm_est_eps,
        measurement.head<2>(), measurement[2]));
  }

  return 0;
}

int createUnitPOFFactors(
    const AlignedVector<std::tuple<int, int, scope::Vector3, Scalar>> &Info,
    Scalar gm_est_sigma, Scalar gm_est_eps, const Matrix3X &Measurements,
    std::vector<std::shared_ptr<UnitPOFFactor>> &factors) {
  factors.clear();

  const int M = Measurements.cols();

  for (const auto &info : Info) {
    const auto &m = std::get<0>(info);

    assert(m < M);

    if (m >= M) {
      LOG(ERROR) << "Inconsistent factor information or measurements."
                 << std::endl;

      factors.clear();

      exit(-1);
    }

    const auto &measurement = Measurements.col(m);

    if (measurement.stableNorm() < 1e-8) {
      continue;
    }

    const auto &i = std::get<1>(info);
    const auto &S = std::get<2>(info);
    const auto &confidence = std::get<3>(info);

    factors.push_back(std::make_shared<UnitPOFFactor>(
        i, S, gm_est_sigma, gm_est_eps, measurement, confidence));
  }

  return 0;
}

int createScaledPOFFactors(
    const std::vector<std::tuple<int, int, int, int, Scalar>> &Info,
    int VertexParam, const MatrixX &KeyPointDirs, const VectorX &KeyPoints,
    Scalar gm_est_sigma, Scalar gm_est_eps, const Matrix3X &Measurements,
    std::vector<std::shared_ptr<ScaledPOFFactor>> &factors) {
  factors.clear();

  const int M = Measurements.cols();
  const int K = KeyPoints.size() / 3;

  assert(KeyPoints.size() % 3 == 0);
  assert(KeyPointDirs.rows() == KeyPoints.size());

  if (KeyPoints.size() % 3 || KeyPointDirs.rows() != KeyPoints.size()) {
    LOG(ERROR) << "Inconsistent keypoints." << std::endl;

    exit(-1);
  }

  std::array<Matrix3X, 2> UDirs;
  std::array<Vector3, 2> U;
  Vector3 measurement;

  for (const auto &info : Info) {
    const auto &m = std::get<0>(info);
    const auto &i = std::get<1>(info);
    const auto &v0 = std::get<2>(info);
    const auto &v1 = std::get<3>(info);
    const auto &confidence = std::get<4>(info);

    assert(m < M);
    assert(i >= 0);
    assert(v0 < K);
    assert(v1 < K);

    if (m >= M || i < 0 || v0 >= K || v1 >= K) {
      LOG(ERROR) << "Inconsistent factor information or measurements."
                 << std::endl;

      factors.clear();

      exit(-1);
    }

    measurement = Measurements.col(m);

    if (measurement.stableNorm() < 1e-8) {
      continue;
    }

    UDirs[0] = KeyPointDirs.middleRows<3>(3 * v0);
    UDirs[1] = KeyPointDirs.middleRows<3>(3 * v1);
    U[0] = KeyPoints.segment<3>(3 * v0);
    U[1] = KeyPoints.segment<3>(3 * v1);

    factors.push_back(std::make_shared<ScaledPOFFactor>(
        i, VertexParam, UDirs, U, gm_est_sigma, gm_est_eps, measurement,
        confidence));
  }

  return 0;
}

int createRelPOFFactors(
    const std::vector<std::tuple<int, int, int, int, int, Scalar>> &Info,
    int VertexParam, const MatrixX &JDirs, const VectorX &J,
    const MatrixX &KeyPointDirs, const VectorX &KeyPoints,
    const std::vector<int> &kintree, Scalar gm_est_sigma, Scalar gm_est_eps,
    const Matrix3X &Measurements,
    std::vector<std::shared_ptr<RelPOFFactor>> &factors) {
  factors.clear();

  const int M = Measurements.cols();
  const int K = KeyPoints.size() / 3;
  const int N = J.size() / 3;

  assert(KeyPoints.size() % 3 == 0);
  assert(J.size() % 3 == 0);
  assert(KeyPointDirs.rows() == KeyPoints.size());
  assert(JDirs.rows() == J.size());
  assert(kintree.size() == N);

  if (KeyPoints.size() % 3 || J.size() % 3 ||
      KeyPointDirs.rows() != KeyPoints.size() || JDirs.rows() != J.size() ||
      kintree.size() != N) {
    LOG(ERROR) << "Inconsistent keypoints or joint locations." << std::endl;

    exit(-1);
  }

  std::array<Matrix3X, 2> UDirs;
  std::array<Vector3, 2> U;
  Vector3 measurement;

  for (const auto &info : Info) {
    const auto &m = std::get<0>(info);
    const auto &i = std::get<1>(info);
    const auto &v0 = std::get<2>(info);
    const auto &v1 = std::get<3>(info);
    const auto &confidence = std::get<5>(info);

    assert(m < M);
    assert(i < N);
    assert(kintree[i] < i);
    assert(i > 0);
    assert(v0 < K);
    assert(v1 < K);

    if (m >= M || i > N || kintree[i] >= i || i <= 0 || v0 >= K || v1 >= K) {
      LOG(ERROR) << "Inconsistent factor information or measurements."
                 << std::endl;

      factors.clear();

      exit(-1);
    }

    measurement = Measurements.col(m) * std::get<4>(info);

    if (measurement.stableNorm() < 1e-8) {
      continue;
    }

    UDirs[0] = KeyPointDirs.middleRows<3>(3 * v0) - JDirs.middleRows<3>(3 * i);
    UDirs[1] = KeyPointDirs.middleRows<3>(3 * v1) - JDirs.middleRows<3>(3 * i);
    U[0] = KeyPoints.segment<3>(3 * v0) - J.segment<3>(3 * i);
    U[1] = KeyPoints.segment<3>(3 * v1) - J.segment<3>(3 * i);

    factors.push_back(std::make_shared<RelPOFFactor>(
        i, VertexParam, kintree, UDirs, U, gm_est_sigma, gm_est_eps,
        measurement, confidence));
  }

  return 0;
}

int createJointDepthCameraFactors(
    const std::vector<std::tuple<int, int, Scalar>> &Info, Scalar gm_est_sigma,
    Scalar gm_est_eps, const Matrix3X &Measurements, const Vector3 &root,
    std::vector<std::shared_ptr<JointDepthCameraFactor>> &factors) {
  factors.clear();

  const int M = Measurements.cols();

  factors.reserve(Info.size());

  for (const auto &info : Info) {
    const auto &m = std::get<0>(info);
    const auto &i = std::get<1>(info);
    const auto &confidence = std::get<2>(info);

    assert(m < M);

    if (m >= M) {
      LOG(ERROR) << "Inconsistent factor information or measurements."
                 << std::endl;

      factors.clear();

      exit(-1);
    }

    const Vector3 measurement = Measurements.col(m) + root;

    factors.push_back(std::make_shared<JointDepthCameraFactor>(
        i, gm_est_sigma, gm_est_eps, measurement, confidence));
  }

  return 0;
}

int createVertexDepthCameraFactors(
    const std::vector<std::tuple<int, int, int, Scalar>> &Info, int VertexParam,
    const MatrixX &JDirs, const VectorX &J, const MatrixX &KeyPointDirs,
    const VectorX &KeyPoints, Scalar gm_est_sigma, Scalar gm_est_eps,
    const Matrix3X &Measurements, const Vector3 &root,
    std::vector<std::shared_ptr<VertexDepthCameraFactor>> &factors) {
  factors.clear();

  assert(KeyPoints.size() % 3 == 0);
  assert(J.size() % 3 == 0);
  assert(KeyPointDirs.rows() == KeyPoints.size());
  assert(JDirs.rows() == J.size());

  if (KeyPoints.size() % 3 || J.size() % 3 ||
      KeyPointDirs.rows() != KeyPoints.size() || JDirs.rows() != J.size()) {
    LOG(ERROR) << "Inconsistent keypoints or joint locations." << std::endl;

    exit(-1);
  }

  const int M = Measurements.cols();
  const int N = J.size() / 3;
  const int K = KeyPoints.size() / 3;

  factors.reserve(Info.size());

  Matrix3X KptDirs;
  Vector3 Kpt;

  for (const auto &info : Info) {
    const auto &m = std::get<0>(info);
    const auto &i = std::get<1>(info);
    const auto &v = std::get<2>(info);
    const auto &confidence = std::get<3>(info);

    if (m >= M || i >= N || v >= K) {
      LOG(ERROR) << "Inconsistent factor information or measurements."
                 << std::endl;

      factors.clear();

      exit(-1);
    }

    const Vector3 measurement = Measurements.col(m) + root;

    KptDirs = KeyPointDirs.middleRows<3>(3 * v) - JDirs.middleRows<3>(3 * i);
    Kpt = KeyPoints.segment<3>(3 * v) - J.segment<3>(3 * i);

    factors.push_back(std::make_shared<VertexDepthCameraFactor>(
        i, VertexParam, KptDirs, Kpt, gm_est_sigma, gm_est_eps, measurement,
        confidence));
  }

  return 0;
}

int createJointConstFactors(
    const AlignedVector<std::tuple<int, Vector3, Matrix3, Vector3, Vector3>>
        &JointConstInfo,
    std::vector<std::shared_ptr<JointConstFactor>> &factors) {
  factors.clear();

  const int N = JointConstInfo.size();

  for (int n = 0; n < N; n++) {
    const auto &info = JointConstInfo[n];
    const auto &joint = std::get<0>(info);

    assert(joint >= 0);

    if (joint < 0) {
      LOG(ERROR) << "Inconsistent factor information." << std::endl;

      exit(-1);
    }

    const auto &weight = std::get<1>(info);
    const auto &ref = std::get<2>(info);
    const auto &lbnd = std::get<3>(info);
    const auto &ubnd = std::get<4>(info);

    factors.push_back(
        std::make_shared<JointConstFactor>(joint, weight, ref, lbnd, ubnd));
  }

  return 0;
}

int createEulerAngleConstFactors(
    const AlignedVector<std::tuple<int, Vector3, Matrix3, Vector3, Vector3>>
        &EulerAngleConstInfo,
    std::vector<std::shared_ptr<EulerAngleConstFactor>> &factors) {
  factors.clear();

  const int N = EulerAngleConstInfo.size();

  for (int n = 0; n < N; n++) {
    const auto &info = EulerAngleConstInfo[n];
    const auto &joint = std::get<0>(info);

    assert(joint >= 0);

    if (joint < 0) {
      LOG(ERROR) << "Inconsistent factor information." << std::endl;

      exit(-1);
    }

    const auto &weight = std::get<1>(info);
    const auto &ref = std::get<2>(info);
    const auto &lbnd = std::get<3>(info);
    const auto &ubnd = std::get<4>(info);

    factors.push_back(std::make_shared<EulerAngleConstFactor>(joint, weight,
                                                              ref, lbnd, ubnd));
  }

  return 0;
}

int createJointLimitFactors(
    const AlignedVector<AlignedVector<scope::Matrix6>> &weight,
    const AlignedVector<AlignedVector<Vector6>> &bias,
    const AlignedVector<VectorX> &PRelua, const AlignedVector<VectorX> &PRelub,
    const VectorX &scale,
    std::vector<std::shared_ptr<JointLimitFactor>> &factors) {
  factors.clear();

  const int N = weight.size();

  assert(bias.size() == N);
  assert(PRelua.size() == N);
  assert(PRelub.size() == N);
  assert(scale.size() == N);

  if (bias.size() != N || PRelua.size() != N || PRelub.size() != N ||
      scale.size() != N) {
    LOG(ERROR) << "Inconsistent factor information or measurements."
               << std::endl;

    exit(-1);
  }

  for (int n = 0; n < N; n++) {
    factors.push_back(std::make_shared<JointLimitFactor>(
        n, weight[n], bias[n], PRelua[n], PRelub[n], scale[n]));
  }

  return 0;
}

int solveReducedEPnP(const std::vector<int> &KeyPoint3Dto2D,
                     const Matrix3X &Measurements3D,
                     const Matrix3X &Measurements2D,
                     const VectorX &ConfidenceThreshold2D, Vector3 &t) {
  assert(Measurements3D.cols() == KeyPoint3Dto2D.size());
  assert(Measurements2D.cols() == ConfidenceThreshold2D.size());

  if (Measurements3D.cols() != KeyPoint3Dto2D.size() ||
      Measurements2D.cols() != ConfidenceThreshold2D.size()) {
    LOG(ERROR) << "Inconsistent keypoint information." << std::endl;

    exit(-1);
  }

  const auto &N = KeyPoint3Dto2D.size();
  const auto &M = ConfidenceThreshold2D.size();

  std::vector<int> sel;

  for (int n = 0; n < N; n++) {
    const auto &m = KeyPoint3Dto2D[n];

    assert(m < M);

    if (m >= M) {
      LOG(ERROR) << "Inconsistent keypoint information." << std::endl;

      exit(-1);
    }

    const auto &measurement2D = Measurements2D.col(m);

    if (measurement2D[2] < ConfidenceThreshold2D[m]) {
      continue;
    }

    sel.push_back(n);
  }

  if (sel.size() >= 2) {
    MatrixX3 A = MatrixX3::Zero(2 * sel.size(), 3);
    VectorX a = VectorX::Zero(2 * sel.size());

    for (int i = 0; i < sel.size(); i++) {
      const auto &n = sel[i];
      const auto &m = KeyPoint3Dto2D[n];

      const auto &measurement3D = Measurements3D.col(n);
      const auto &measurement2D = Measurements2D.col(m);

      A.block<2, 2>(2 * i, 0).setIdentity();
      A.block<2, 1>(2 * i, 2) = -measurement2D.head<2>();

      a.segment<2>(2 * i) =
          measurement2D.head<2>() * measurement3D[2] - measurement3D.head<2>();

      A.middleRows<2>(2 * i) *= measurement2D[2];
      a.segment<2>(2 * i) *= measurement2D[2];
    }

    t.noalias() = A.colPivHouseholderQr().solve(a);

    return sel.size();
  } else {
    t.head<2>().setZero();

    for (const auto &n : sel) {
      const auto &m = KeyPoint3Dto2D[n];
      t.head<2>() += Measurements2D.col(m).head<2>();
    }

    t.head<2>() /= std::max(1, (int)sel.size());

    t.head<2>() *= 2;
    t[2] = 2;

    return -1;
  }

  return 0;
}

int solvePose(
    const std::vector<std::tuple<int, int, int, Scalar>> &DepthCamInfo,
    const Vector3 &Root, const VectorX &KeyPoints,
    const Matrix3X &Measurements3D, Pose &pose) {
  Matrix3X P(3, DepthCamInfo.size());
  Matrix3X Q(3, DepthCamInfo.size());

  for (int n = 0; n < DepthCamInfo.size(); n++) {
    const auto &info = DepthCamInfo[n];

    const auto &m = std::get<0>(info);
    const auto &v = std::get<2>(info);

    assert(m < Measurements3D.cols());
    assert(3 * v + 2 < KeyPoints.size());

    if (m >= Measurements3D.cols() || 3 * v + 2 >= KeyPoints.size()) {
      LOG(ERROR) << "Inconsistent keypoint or measurement information"
                 << std::endl;

      exit(-1);
    }

    P.col(n) = KeyPoints.segment<3>(3 * v) - Root;
    Q.col(n) = Measurements3D.col(m);
  }

  Vector3 Pmean = P.rowwise().mean();
  Vector3 Qmean = Q.rowwise().mean();

  P.colwise() -= Pmean;
  Q.colwise() -= Qmean;

  Matrix3 M;

  M.noalias() = Q * P.transpose();

  projectToSO3(M, pose.R);
  pose.t = Qmean;
  pose.t.noalias() -= pose.R * Pmean;

  return 0;
}

int solveShape(
    const std::vector<std::tuple<std::array<int, 4>, Vector3>> &RelShapeInfo,
    const std::vector<std::tuple<std::array<int, 4>, Vector3>>
        &RelShapePairInfo,
    const MatrixX &ShapeSqrtCov, Scalar weight, const MatrixX &KeyPointDirs,
    const VectorX &KeyPoints, const Matrix3X &Measurements3D, VectorX &betas,
    Scalar &s) {
  assert(RelShapePairInfo.size() % 2 == 0);
  assert(KeyPointDirs.cols() == ShapeSqrtCov.cols());

  if (RelShapePairInfo.size() % 2 != 0 ||
      KeyPointDirs.cols() != ShapeSqrtCov.cols()) {
    LOG(ERROR) << "Inconsistent relative shape information" << std::endl;

    exit(-1);
  }

  const int N = RelShapeInfo.size() + RelShapePairInfo.size();
  const int P = KeyPointDirs.cols();

  MatrixX A(3 * N, P);
  VectorX a(3 * N);
  VectorX b(3 * N);

  Scalar L0, L1;

  int n = 0;
  for (; n < RelShapePairInfo.size(); n += 2) {
    const auto &index0 = std::get<0>(RelShapePairInfo[n]);
    const auto &index1 = std::get<0>(RelShapePairInfo[n + 1]);

    Scalar L = (Measurements3D.col(index0[3]) - Measurements3D.col(index0[2]))
                   .stableNorm() +
               (Measurements3D.col(index1[3]) - Measurements3D.col(index1[2]))
                   .stableNorm();
    L *= 0.5;

    A.middleRows<3>(3 * n) = KeyPointDirs.middleRows<3>(3 * index0[1]) -
                             KeyPointDirs.middleRows<3>(3 * index0[0]);
    a.segment<3>(3 * n) = KeyPoints.segment<3>(3 * index0[1]) -
                          KeyPoints.segment<3>(3 * index0[0]);
    b.segment<3>(3 * n) = L * std::get<1>(RelShapePairInfo[n]);
    L0 += a.segment<3>(3 * n).stableNorm();
    L1 += L;

    A.middleRows<3>(3 * n + 3) = KeyPointDirs.middleRows<3>(3 * index1[1]) -
                                 KeyPointDirs.middleRows<3>(3 * index1[0]);
    a.segment<3>(3 * n + 3) = KeyPoints.segment<3>(3 * index1[1]) -
                              KeyPoints.segment<3>(3 * index1[0]);
    b.segment<3>(3 * n + 3) = L * std::get<1>(RelShapePairInfo[n + 1]);
    L0 += a.segment<3>(3 * n + 3).stableNorm();
    L1 += L;
  }

  for (const auto &info : RelShapeInfo) {
    const auto &index = std::get<0>(info);

    Scalar L = (Measurements3D.col(index[3]) - Measurements3D.col(index[2]))
                   .stableNorm();

    A.middleRows<3>(3 * n) = KeyPointDirs.middleRows<3>(3 * index[1]) -
                             KeyPointDirs.middleRows<3>(3 * index[0]);
    a.segment<3>(3 * n) =
        KeyPoints.segment<3>(3 * index[1]) - KeyPoints.segment<3>(3 * index[0]);
    b.segment<3>(3 * n) = L * std::get<1>(info);
    L0 += a.segment<3>(3 * n).stableNorm();
    L1 += L;

    n++;
  }

  s = L1 / L0;

  VectorX c = b - s * a;

  for (int n = 2; n < A.rows(); n += 3) {
    A.row(n) *= 0.4;
    c[n] *= 0.4;
  }

  MatrixX M;
  VectorX m;

  M.noalias() = weight * ShapeSqrtCov.transpose() * ShapeSqrtCov;
  M.noalias() += A.transpose() * A;
  M.diagonal().array() += 1e-12;
  m.noalias() = A.transpose() * c;

  betas.noalias() = M.ldlt().solve(m);

  return 0;
}

int getKeyPointOrigin3D(const Pose &Root, const MatrixX &JDirs,
                        const VectorX &J, const MatrixX &KeyPointDirs,
                        const VectorX &KeyPoints, const VectorX &Param,
                        int OriginIndex, Vector3 &keypoint) {
  assert(JDirs.cols() == Param.size());
  assert(KeyPointDirs.cols() == Param.size());
  assert(JDirs.cols() == KeyPointDirs.cols());

  Matrix3X vDirs =
      KeyPointDirs.middleRows<3>(3 * OriginIndex) - JDirs.middleRows<3>(0);
  Vector3 vertex = KeyPoints.segment<3>(3 * OriginIndex) - J.segment<3>(0);
  vertex.noalias() += vDirs * Param;

  keypoint = Root.t;
  keypoint += Root.R * vertex;

  return 0;
}
}  // namespace scope


<|start_filename|>C++/scope/factor/ParameterFactor.cpp<|end_filename|>
#include <glog/logging.h>

#include <scope/factor/ParameterFactor.h>

namespace scope {
ParameterFactor::ParameterFactor(int param, const MatrixX &sigma,
                                 const VectorX &mean, const std::string &name,
                                 int index, bool active)
    : Factor({}, {}, {}, {param}, name, index, active), mSqrtCov(sigma),
      mMean(mean) {}

int ParameterFactor::evaluate(const AlignedVector<Pose> &poses,
                              const AlignedVector<VectorX> &shapes,
                              const AlignedVector<Matrix3> &joints,
                              const AlignedVector<VectorX> &params,
                              Factor::Evaluation &base_eval) const {
  auto &eval = dynamic_cast<Evaluation &>(base_eval);
  eval.clear();

  const auto &s = mvParams[0];
  assert(s >= 0 && s < params.size());

  if (s < 0 || s >= params.size()) {
    LOG(ERROR) << "The parameter must be valid." << std::endl;

    exit(-1);
  }

  eval.unscaledError = params[s] - mMean;

  eval.error.noalias() = mSqrtCov * eval.unscaledError;
  eval.f = eval.error.squaredNorm();

  eval.status = Status::VALID;

  return 0;
}

int ParameterFactor::linearize(const AlignedVector<Pose> &poses,
                               const AlignedVector<VectorX> &shapes,
                               const AlignedVector<Matrix3> &joints,
                               const AlignedVector<VectorX> &params,
                               const Factor::Evaluation &base_eval,
                               Factor::Linearization &base_lin) const {
  assert(base_eval.status == Status::VALID);

  auto &eval = base_eval;
  auto &lin = base_lin;

  lin.clear();

  if (eval.status != Status::VALID) {
    LOG(ERROR) << "The evaluation must be valid." << std::endl;

    exit(-1);
  }

  const auto &s = mvParams[0];
  assert(s >= 0 && s < params.size());

  if (s >= params.size()) {
    LOG(ERROR) << "The parameter must be valid." << std::endl;

    exit(-1);
  }

  lin.jacobians[3].resize(mvParams.size());

  lin.jacobians[3][0] = mSqrtCov;

  lin.status = Status::VALID;

  return 0;
}

} // namespace scope
