We read every piece of feedback, and take your input very seriously.
To see all available qualifiers, see our documentation.
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
for (size_t i = 0; i < m_max_iter; i++) { m_loss_func(m_x, shared_data); if (!shared_data.valid) break; H.setZero(); b.setZero(); delta = m_x - predict_x; M21D J = M21D::Identity(); J.block<3, 3>(0, 0) = JrInv(delta.segment<3>(0)); J.block<3, 3>(6, 6) = JrInv(delta.segment<3>(6)); H += J.transpose() * m_P.inverse() * J; b += J.transpose() * m_P.inverse() * delta; H.block<12, 12>(0, 0) += shared_data.H; b.block<12, 1>(0, 0) += shared_data.b; delta = -H.inverse() * b; m_x += delta; shared_data.iter_num += 1; if (m_stop_func(delta)) break; } M21D L = M21D::Identity(); // L.block<3, 3>(0, 0) = Jr(delta.segment<3>(0)); // L.block<3, 3>(6, 6) = Jr(delta.segment<3>(6)); L.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity() - 0.5 * Sophus::SO3d::hat(delta.segment<3>(0)); L.block<3, 3>(6, 6) = Eigen::Matrix3d::Identity() - 0.5 * Sophus::SO3d::hat(delta.segment<3>(6)); m_P = L * H.inverse() * L.transpose();
m_loss_func(m_x, shared_data);在这里我们利用雷达点构建了点面残差,做了误差状态量中关于位移以及旋转的残差;
M21D J = M21D::Identity(); J.block<3, 3>(0, 0) = JrInv(delta.segment<3>(0)); J.block<3, 3>(6, 6) = JrInv(delta.segment<3>(6)); H += J.transpose() * m_P.inverse() * J; b += J.transpose() * m_P.inverse() * delta; H.block<12, 12>(0, 0) += shared_data.H; b.block<12, 1>(0, 0) += shared_data.b; 在这里构建的雅阁比J矩阵请问是什么含义呢,为什么又利用这个雅阁比与误差状态量的矩阵构建残差? 很困解的是这里的J矩阵推导公式是怎么来的,可以麻烦解释一下吗,或者有什么资料可以参考吗 ~~~~~~~~希望可以回答^_^
The text was updated successfully, but these errors were encountered:
No branches or pull requests
m_loss_func(m_x, shared_data);在这里我们利用雷达点构建了点面残差,做了误差状态量中关于位移以及旋转的残差;
The text was updated successfully, but these errors were encountered: