本文整理汇总了C++中eigen::MatrixXd::jacobiSvd方法的典型用法代码示例。如果您正苦于以下问题：C++ MatrixXd::jacobiSvd方法的具体用法？C++ MatrixXd::jacobiSvd怎么用？C++ MatrixXd::jacobiSvd使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类eigen::MatrixXd的用法示例。
在下文中一共展示了MatrixXd::jacobiSvd方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: estimateTransform
Types::Transform PointOnPlaneCalibration::estimateTransform(const std::vector<PointPlanePair> & pair_vector)
{
const int size = pair_vector.size();
// Point-Plane Constraints
// Initial system (Eq. 10)
Eigen::MatrixXd system(size, 13);
for (int i = 0; i < size; ++i)
{
const double d = pair_vector[i].plane_.offset();
const Eigen::Vector3d n = pair_vector[i].plane_.normal();
const Types::Point3 & x = pair_vector[i].point_;
system.row(i) << d + n[0] * x[0] + n[1] * x[1] + n[2] * x[2], // q_0^2
2 * n[2] * x[1] - 2 * n[1] * x[2], // q_0 * q_1
2 * n[0] * x[2] - 2 * n[2] * x[0], // q_0 * q_2
2 * n[1] * x[0] - 2 * n[0] * x[1], // q_0 * q_3
d + n[0] * x[0] - n[1] * x[1] - n[2] * x[2], // q_1^2
2 * n[0] * x[1] + 2 * n[1] * x[0], // q_1 * q_2
2 * n[0] * x[2] + 2 * n[2] * x[0], // q_1 * q_3
d - n[0] * x[0] + n[1] * x[1] - n[2] * x[2], // q_2^2
2 * n[1] * x[2] + 2 * n[2] * x[1], // q_2 * q_3
d - n[0] * x[0] - n[1] * x[1] + n[2] * x[2], // q_3^2
n[0], n[1], n[2]; // q'*q*t
}
// Gaussian reduction
for (int k = 0; k < 3; ++k)
for (int i = k + 1; i < size; ++i)
system.row(i) = system.row(i) - system.row(k) * system.row(i)[10 + k] / system.row(k)[10 + k];
// Quaternion q
Eigen::Vector4d q;
// Transform to inhomogeneous (Eq. 13)
bool P_is_ok(false);
while (not P_is_ok)
{
Eigen::Matrix4d P(Eigen::Matrix4d::Random());
while (P.determinant() < 1e-5)
P = Eigen::Matrix4d::Random();
Eigen::MatrixXd reduced_system(size - 3, 10);
for (int i = 3; i < size; ++i)
{
const Eigen::VectorXd & row = system.row(i);
Eigen::Matrix4d Mi_tilde;
Mi_tilde << row[0] , row[1] / 2, row[2] / 2, row[3] / 2,
row[1] / 2, row[4] , row[5] / 2, row[6] / 2,
row[2] / 2, row[5] / 2, row[7] , row[8] / 2,
row[3] / 2, row[6] / 2, row[8] / 2, row[9] ;
Eigen::Matrix4d Mi_bar(P.transpose() * Mi_tilde * P);
reduced_system.row(i - 3) << Mi_bar(0, 0),
Mi_bar(0, 1) + Mi_bar(1, 0),
Mi_bar(0, 2) + Mi_bar(2, 0),
Mi_bar(0, 3) + Mi_bar(3, 0),
Mi_bar(1, 1),
Mi_bar(1, 2) + Mi_bar(2, 1),
Mi_bar(1, 3) + Mi_bar(3, 1),
Mi_bar(2, 2),
Mi_bar(2, 3) + Mi_bar(3, 2),
Mi_bar(3, 3);
}
// Solve A m* = b
Eigen::MatrixXd A = reduced_system.rightCols<9>();
Eigen::VectorXd b = - reduced_system.leftCols<1>();
Eigen::VectorXd m_star = A.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV).solve(b);
Eigen::Vector4d q_bar(1, m_star[0], m_star[1], m_star[2]);
Eigen::VectorXd err(6);
err << q_bar[1] * q_bar[1],
q_bar[1] * q_bar[2],
q_bar[1] * q_bar[3],
q_bar[2] * q_bar[2],
q_bar[2] * q_bar[3],
q_bar[3] * q_bar[3];
err -= m_star.tail<6>();
//if (err.norm() < 0.1) // P is not ok?
P_is_ok = true;
//std::cout << m_star.transpose() << std::endl;
//std::cout << q_bar[1] * q_bar[1] << " " << q_bar[1] * q_bar[2] << " " << q_bar[1] * q_bar[3] << " "
//<< q_bar[2] * q_bar[2] << " " << q_bar[2] * q_bar[3] << " " << q_bar[3] * q_bar[3] << std::endl;
q = P * q_bar;
}
// We want q.w > 0 (Why?)
//if (q[0] < 0)
// q = -q;
//.........这里部分代码省略.........