Регистрация icp с ограничением степени свободы (RX, RY, TX, TY, TZ) ⇐ C++

[Anonymous]

У меня есть 2 набора облаков точек для регистрации, общие результаты icp — для 6dof. Можно ли ограничить регистрацию до 5dof(RX, RY, TX, TY, TZ)?
Я пытался реализовать мой собственный warp_point_rigid_RxRyT.h, который является модификацией
Модификация следующая:
void setParam(const VectorX& p) override
{
assert(p.rows() == this- >getDimension());

Код: Выделить всё

   // Copy the rotation and translation components
transform_matrix_.setZero();
transform_matrix_(0, 3) = p[0];
transform_matrix_(1, 3) = p[1];
transform_matrix_(2, 3) = p[2];
transform_matrix_(3, 3) = 1;

// Compute w from the unit quaternion
Eigen::Quaternion q(0, p[3], p[4], 0);
q.w() = static_cast(std::sqrt(1 - q.dot(q)));
q.normalize();
transform_matrix_.template topLeftCorner() = q.toRotationMatrix();

Это не работает, результат icp по-прежнему равен 6dof.

Подробнее здесь: https://stackoverflow.com/questions/792 ... y-tx-ty-tz