605 Matrix4 transformation_matrix = gicp_->base_transformation_;
606 gicp_->applyState(transformation_matrix, x);
607 const Eigen::Matrix4f transformation_matrix_float =
608 transformation_matrix.template cast<float>();
609 const Eigen::Matrix4f base_transformation_float =
610 gicp_->base_transformation_.template cast<float>();
615 Eigen::Matrix3d dR_dPhi;
616 Eigen::Matrix3d dR_dTheta;
617 Eigen::Matrix3d dR_dPsi;
618 gicp_->getRDerivatives(x[3], x[4], x[5], dR_dPhi, dR_dTheta, dR_dPsi);
619 Eigen::Matrix3d ddR_dPhi_dPhi;
620 Eigen::Matrix3d ddR_dPhi_dTheta;
621 Eigen::Matrix3d ddR_dPhi_dPsi;
622 Eigen::Matrix3d ddR_dTheta_dTheta;
623 Eigen::Matrix3d ddR_dTheta_dPsi;
624 Eigen::Matrix3d ddR_dPsi_dPsi;
625 gicp_->getR2ndDerivatives(x[3],
634 Eigen::Matrix3d dCost_dR_T = Eigen::Matrix3d::Zero();
635 Eigen::Matrix3d dCost_dR_T1 = Eigen::Matrix3d::Zero();
636 Eigen::Matrix3d dCost_dR_T2 = Eigen::Matrix3d::Zero();
637 Eigen::Matrix3d dCost_dR_T3 = Eigen::Matrix3d::Zero();
638 Eigen::Matrix3d dCost_dR_T1b = Eigen::Matrix3d::Zero();
639 Eigen::Matrix3d dCost_dR_T2b = Eigen::Matrix3d::Zero();
640 Eigen::Matrix3d dCost_dR_T3b = Eigen::Matrix3d::Zero();
641 Eigen::Matrix3d hessian_rot_phi = Eigen::Matrix3d::Zero();
642 Eigen::Matrix3d hessian_rot_theta = Eigen::Matrix3d::Zero();
643 Eigen::Matrix3d hessian_rot_psi = Eigen::Matrix3d::Zero();
644 Eigen::Matrix<double, 9, 6> hessian_rot_tmp = Eigen::Matrix<double, 9, 6>::Zero();
646 int m =
static_cast<int>(gicp_->tmp_idx_src_->size());
647 for (
int i = 0; i < m; ++i) {
649 const auto& src_idx = (*gicp_->tmp_idx_src_)[i];
653 (*gicp_->tmp_tgt_)[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap();
654 Eigen::Vector4f p_trans_src(transformation_matrix_float * p_src);
657 const Eigen::Vector3d d(p_trans_src[0] - p_tgt[0],
658 p_trans_src[1] - p_tgt[1],
659 p_trans_src[2] - p_tgt[2]);
660 const Eigen::Matrix3d& M = gicp_->mahalanobis(src_idx);
661 const Eigen::Vector3d Md(M * d);
662 gradient.head<3>() += Md;
663 hessian.topLeftCorner<3, 3>() += M;
664 p_trans_src = base_transformation_float * p_src;
665 const Eigen::Vector3d p_base_src(p_trans_src[0], p_trans_src[1], p_trans_src[2]);
666 dCost_dR_T.noalias() += p_base_src * Md.transpose();
667 dCost_dR_T1b += p_base_src[0] * M;
668 dCost_dR_T2b += p_base_src[1] * M;
669 dCost_dR_T3b += p_base_src[2] * M;
670 hessian_rot_tmp.noalias() +=
671 Eigen::Map<const Eigen::Matrix<double, 9, 1>>{M.data()} *
672 (Eigen::Matrix<double, 1, 6>() << p_base_src[0] * p_base_src[0],
673 p_base_src[0] * p_base_src[1],
674 p_base_src[0] * p_base_src[2],
675 p_base_src[1] * p_base_src[1],
676 p_base_src[1] * p_base_src[2],
677 p_base_src[2] * p_base_src[2])
680 gradient.head<3>() *= 2.0 / m;
681 dCost_dR_T *= 2.0 / m;
682 gicp_->computeRDerivative(x, dCost_dR_T, gradient);
683 hessian.topLeftCorner<3, 3>() *= 2.0 / m;
685 dCost_dR_T1.row(0) = dCost_dR_T1b.col(0);
686 dCost_dR_T1.row(1) = dCost_dR_T2b.col(0);
687 dCost_dR_T1.row(2) = dCost_dR_T3b.col(0);
688 dCost_dR_T2.row(0) = dCost_dR_T1b.col(1);
689 dCost_dR_T2.row(1) = dCost_dR_T2b.col(1);
690 dCost_dR_T2.row(2) = dCost_dR_T3b.col(1);
691 dCost_dR_T3.row(0) = dCost_dR_T1b.col(2);
692 dCost_dR_T3.row(1) = dCost_dR_T2b.col(2);
693 dCost_dR_T3.row(2) = dCost_dR_T3b.col(2);
694 dCost_dR_T1 *= 2.0 / m;
695 dCost_dR_T2 *= 2.0 / m;
696 dCost_dR_T3 *= 2.0 / m;
697 hessian(3, 0) = (dR_dPhi * dCost_dR_T1).trace();
698 hessian(4, 0) = (dR_dTheta * dCost_dR_T1).trace();
699 hessian(5, 0) = (dR_dPsi * dCost_dR_T1).trace();
700 hessian(3, 1) = (dR_dPhi * dCost_dR_T2).trace();
701 hessian(4, 1) = (dR_dTheta * dCost_dR_T2).trace();
702 hessian(5, 1) = (dR_dPsi * dCost_dR_T2).trace();
703 hessian(3, 2) = (dR_dPhi * dCost_dR_T3).trace();
704 hessian(4, 2) = (dR_dTheta * dCost_dR_T3).trace();
705 hessian(5, 2) = (dR_dPsi * dCost_dR_T3).trace();
706 hessian.block<3, 3>(0, 3) = hessian.block<3, 3>(3, 0).transpose();
708 int lookup[3][3] = {{0, 1, 2}, {1, 3, 4}, {2, 4, 5}};
709 for (
int l = 0; l < 3; ++l) {
710 for (
int i = 0; i < 3; ++i) {
711 double phi_tmp = 0.0, theta_tmp = 0.0, psi_tmp = 0.0;
712 for (
int j = 0; j < 3; ++j) {
713 for (
int k = 0; k < 3; ++k) {
714 phi_tmp += hessian_rot_tmp(3 * j + i, lookup[l][k]) * dR_dPhi(j, k);
715 theta_tmp += hessian_rot_tmp(3 * j + i, lookup[l][k]) * dR_dTheta(j, k);
716 psi_tmp += hessian_rot_tmp(3 * j + i, lookup[l][k]) * dR_dPsi(j, k);
719 hessian_rot_phi(i, l) = phi_tmp;
720 hessian_rot_theta(i, l) = theta_tmp;
721 hessian_rot_psi(i, l) = psi_tmp;
724 hessian_rot_phi *= 2.0 / m;
725 hessian_rot_theta *= 2.0 / m;
726 hessian_rot_psi *= 2.0 / m;
727 hessian(3, 3) = (dR_dPhi.transpose() * hessian_rot_phi).trace() +
728 (ddR_dPhi_dPhi * dCost_dR_T).trace();
729 hessian(3, 4) = (dR_dPhi.transpose() * hessian_rot_theta).trace() +
730 (ddR_dPhi_dTheta * dCost_dR_T).trace();
731 hessian(3, 5) = (dR_dPhi.transpose() * hessian_rot_psi).trace() +
732 (ddR_dPhi_dPsi * dCost_dR_T).trace();
733 hessian(4, 4) = (dR_dTheta.transpose() * hessian_rot_theta).trace() +
734 (ddR_dTheta_dTheta * dCost_dR_T).trace();
735 hessian(4, 5) = (dR_dTheta.transpose() * hessian_rot_psi).trace() +
736 (ddR_dTheta_dPsi * dCost_dR_T).trace();
737 hessian(5, 5) = (dR_dPsi.transpose() * hessian_rot_psi).trace() +
738 (ddR_dPsi_dPsi * dCost_dR_T).trace();
739 hessian(4, 3) = hessian(3, 4);
740 hessian(5, 3) = hessian(3, 5);
741 hessian(5, 4) = hessian(4, 5);