IGL_INLINE void igl::normalize_row_sums(
const Eigen::MatrixBase<DerivedA>& A,
Eigen::MatrixBase<DerivedB> & B)
{
#ifndef NDEBUG
// loop over rows
for(int i = 0; i < A.rows();i++)
{
typename DerivedB::Scalar sum = A.row(i).sum();
assert(sum != 0);
}
#endif
B = (A.array().colwise() / A.rowwise().sum().array()).eval();
}
Eigen::MatrixBase<Eigen::Matrix<typename matrix::Scalar, matrix::Rows + 1, matrix::Rows + 1>> get_rigid_transform(
const Eigen::MatrixBase<matrix>& a,
const Eigen::MatrixBase<matrix>& b)
{
Eigen::Matrix<typename matrix::Scalar, matrix::Rows + 1, matrix::Rows + 1> transform =
Eigen::Matrix<typename matrix::Scalar, matrix::Rows + 1, matrix::Rows + 1>::Identity();
auto centroid_a = a.colwise() - a.rowwise().mean();
auto centroid_b = b.colwise() - b.rowwise().mean();
auto svd = (centered_a * centered_b.transpose()).eval().jacobiSVD(Eigen::ComputeFullU | Eigen::ComputeFullV);
auto U = svd.matrixU();
auto V = svd.matrixV();
auto R = (V * U.transpose()).eval();
auto C = Eigen::Matrix<typename matrix::Scalar, matrix::Rows, matrix::Rows>::Identity().eval();
C(matrix::Rows - 1, matrix::Rows - 1) = R.determinant();
R = V * C * U.transpose();
auto T = -R * centroid_a + centroid_b;
transform.block<matrix::Rows, matrix::Rows>(0, 0) = R;
transform.block<matrix::Rows, 1>(0, matrix::Rows) = T;
return transform;
}