582 Matrix4 transformation_matrix = gicp_->base_transformation_;
583 gicp_->applyState(transformation_matrix, x);
584 const Eigen::Matrix4f transformation_matrix_float =
585 transformation_matrix.template cast<float>();
586 const Eigen::Matrix4f base_transformation_float =
587 gicp_->base_transformation_.template cast<float>();
592 Eigen::Matrix3d dR_dPhi;
593 Eigen::Matrix3d dR_dTheta;
594 Eigen::Matrix3d dR_dPsi;
595 gicp_->getRDerivatives(x[3], x[4], x[5], dR_dPhi, dR_dTheta, dR_dPsi);
596 Eigen::Matrix3d ddR_dPhi_dPhi;
597 Eigen::Matrix3d ddR_dPhi_dTheta;
598 Eigen::Matrix3d ddR_dPhi_dPsi;
599 Eigen::Matrix3d ddR_dTheta_dTheta;
600 Eigen::Matrix3d ddR_dTheta_dPsi;
601 Eigen::Matrix3d ddR_dPsi_dPsi;
602 gicp_->getR2ndDerivatives(x[3],
611 Eigen::Matrix3d dCost_dR_T = Eigen::Matrix3d::Zero();
612 Eigen::Matrix3d dCost_dR_T1 = Eigen::Matrix3d::Zero();
613 Eigen::Matrix3d dCost_dR_T2 = Eigen::Matrix3d::Zero();
614 Eigen::Matrix3d dCost_dR_T3 = Eigen::Matrix3d::Zero();
615 Eigen::Matrix3d dCost_dR_T1b = Eigen::Matrix3d::Zero();
616 Eigen::Matrix3d dCost_dR_T2b = Eigen::Matrix3d::Zero();
617 Eigen::Matrix3d dCost_dR_T3b = Eigen::Matrix3d::Zero();
618 Eigen::Matrix3d hessian_rot_phi = Eigen::Matrix3d::Zero();
619 Eigen::Matrix3d hessian_rot_theta = Eigen::Matrix3d::Zero();
620 Eigen::Matrix3d hessian_rot_psi = Eigen::Matrix3d::Zero();
621 Eigen::Matrix<double, 9, 6> hessian_rot_tmp = Eigen::Matrix<double, 9, 6>::Zero();
623 int m =
static_cast<int>(gicp_->tmp_idx_src_->size());
624 for (
int i = 0; i < m; ++i) {
626 const auto& src_idx = (*gicp_->tmp_idx_src_)[i];
630 (*gicp_->tmp_tgt_)[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap();
631 Eigen::Vector4f p_trans_src(transformation_matrix_float * p_src);
634 const Eigen::Vector3d d(p_trans_src[0] - p_tgt[0],
635 p_trans_src[1] - p_tgt[1],
636 p_trans_src[2] - p_tgt[2]);
637 const Eigen::Matrix3d& M = gicp_->mahalanobis(src_idx);
638 const Eigen::Vector3d Md(M * d);
639 gradient.head<3>() += Md;
640 hessian.block<3, 3>(0, 0) += M;
641 p_trans_src = base_transformation_float * p_src;
642 const Eigen::Vector3d p_base_src(p_trans_src[0], p_trans_src[1], p_trans_src[2]);
643 dCost_dR_T.noalias() += p_base_src * Md.transpose();
644 dCost_dR_T1b += p_base_src[0] * M;
645 dCost_dR_T2b += p_base_src[1] * M;
646 dCost_dR_T3b += p_base_src[2] * M;
647 hessian_rot_tmp.noalias() +=
648 Eigen::Map<const Eigen::Matrix<double, 9, 1>>{M.data()} *
649 (Eigen::Matrix<double, 1, 6>() << p_base_src[0] * p_base_src[0],
650 p_base_src[0] * p_base_src[1],
651 p_base_src[0] * p_base_src[2],
652 p_base_src[1] * p_base_src[1],
653 p_base_src[1] * p_base_src[2],
654 p_base_src[2] * p_base_src[2])
657 gradient.head<3>() *= 2.0 / m;
658 dCost_dR_T *= 2.0 / m;
659 gicp_->computeRDerivative(x, dCost_dR_T, gradient);
660 hessian.block<3, 3>(0, 0) *= 2.0 / m;
662 dCost_dR_T1.row(0) = dCost_dR_T1b.col(0);
663 dCost_dR_T1.row(1) = dCost_dR_T2b.col(0);
664 dCost_dR_T1.row(2) = dCost_dR_T3b.col(0);
665 dCost_dR_T2.row(0) = dCost_dR_T1b.col(1);
666 dCost_dR_T2.row(1) = dCost_dR_T2b.col(1);
667 dCost_dR_T2.row(2) = dCost_dR_T3b.col(1);
668 dCost_dR_T3.row(0) = dCost_dR_T1b.col(2);
669 dCost_dR_T3.row(1) = dCost_dR_T2b.col(2);
670 dCost_dR_T3.row(2) = dCost_dR_T3b.col(2);
671 dCost_dR_T1 *= 2.0 / m;
672 dCost_dR_T2 *= 2.0 / m;
673 dCost_dR_T3 *= 2.0 / m;
674 hessian(3, 0) = (dR_dPhi * dCost_dR_T1).trace();
675 hessian(4, 0) = (dR_dTheta * dCost_dR_T1).trace();
676 hessian(5, 0) = (dR_dPsi * dCost_dR_T1).trace();
677 hessian(3, 1) = (dR_dPhi * dCost_dR_T2).trace();
678 hessian(4, 1) = (dR_dTheta * dCost_dR_T2).trace();
679 hessian(5, 1) = (dR_dPsi * dCost_dR_T2).trace();
680 hessian(3, 2) = (dR_dPhi * dCost_dR_T3).trace();
681 hessian(4, 2) = (dR_dTheta * dCost_dR_T3).trace();
682 hessian(5, 2) = (dR_dPsi * dCost_dR_T3).trace();
683 hessian.block<3, 3>(0, 3) = hessian.block<3, 3>(3, 0).transpose();
685 int lookup[3][3] = {{0, 1, 2}, {1, 3, 4}, {2, 4, 5}};
686 for (
int l = 0; l < 3; ++l) {
687 for (
int i = 0; i < 3; ++i) {
688 double phi_tmp = 0.0, theta_tmp = 0.0, psi_tmp = 0.0;
689 for (
int j = 0; j < 3; ++j) {
690 for (
int k = 0; k < 3; ++k) {
691 phi_tmp += hessian_rot_tmp(3 * j + i, lookup[l][k]) * dR_dPhi(j, k);
692 theta_tmp += hessian_rot_tmp(3 * j + i, lookup[l][k]) * dR_dTheta(j, k);
693 psi_tmp += hessian_rot_tmp(3 * j + i, lookup[l][k]) * dR_dPsi(j, k);
696 hessian_rot_phi(i, l) = phi_tmp;
697 hessian_rot_theta(i, l) = theta_tmp;
698 hessian_rot_psi(i, l) = psi_tmp;
701 hessian_rot_phi *= 2.0 / m;
702 hessian_rot_theta *= 2.0 / m;
703 hessian_rot_psi *= 2.0 / m;
704 hessian(3, 3) = (dR_dPhi.transpose() * hessian_rot_phi).trace() +
705 (ddR_dPhi_dPhi * dCost_dR_T).trace();
706 hessian(3, 4) = (dR_dPhi.transpose() * hessian_rot_theta).trace() +
707 (ddR_dPhi_dTheta * dCost_dR_T).trace();
708 hessian(3, 5) = (dR_dPhi.transpose() * hessian_rot_psi).trace() +
709 (ddR_dPhi_dPsi * dCost_dR_T).trace();
710 hessian(4, 4) = (dR_dTheta.transpose() * hessian_rot_theta).trace() +
711 (ddR_dTheta_dTheta * dCost_dR_T).trace();
712 hessian(4, 5) = (dR_dTheta.transpose() * hessian_rot_psi).trace() +
713 (ddR_dTheta_dPsi * dCost_dR_T).trace();
714 hessian(5, 5) = (dR_dPsi.transpose() * hessian_rot_psi).trace() +
715 (ddR_dPsi_dPsi * dCost_dR_T).trace();
716 hessian(4, 3) = hessian(3, 4);
717 hessian(5, 3) = hessian(3, 5);
718 hessian(5, 4) = hessian(4, 5);