void
approx_relpose_generalized
(
const Eigen::Matrix<double,6,6> &w1,
const Eigen::Matrix<double,6,6> &w2,
const Eigen::Matrix<double,6,6> &w3,
const Eigen::Matrix<double,6,6> &w4,
const Eigen::Matrix<double,6,6> &w5,
const Eigen::Matrix<double,6,6> &w6,
std::vector<Eigen::Vector3d> &rsolns
)
{
const Eigen::Matrix<double,15,35> A = computeA(w1,w2,w3,w4,w5,w6);
Eigen::Matrix<double,15,35> gbA;
gbA << A.col(0),A.col(1),A.col(2),A.col(3),A.col(4),A.col(5),A.col(7),A.col(9),A.col(11),A.col(15),A.col(18),A.col(21),A.col(24),A.col(28),A.col(13),A.col(6),A.col(8),A.col(10),A.col(12),A.col(16),A.col(19),A.col(22),A.col(25),A.col(29),A.col(14),A.col(17),A.col(20),A.col(23),A.col(26),A.col(30),A.col(32),A.col(27),A.col(31),A.col(33),A.col(34);
const Eigen::Matrix<double,15,20> G = gbA.block<15,15>(0,0).lu().solve(gbA.block<15,20>(0,15));
Eigen::Matrix<double,20,20> M = Eigen::Matrix<double,20,20>::Zero();
M.block<10,20>(0,0) = -G.block<10,20>(5,0);
M(10,4) = 1;
M(11,5) = 1;
M(12,6) = 1;
M(13,7) = 1;
M(14,8) = 1;
M(15,9) = 1;
M(16,13) = 1;
M(17,14) = 1;
M(18,15) = 1;
M(19,18) = 1;
const Eigen::EigenSolver< Eigen::Matrix<double,20,20> > eigensolver(M,true);
const Eigen::EigenSolver< Eigen::Matrix<double,20,20> >::EigenvalueType evalues = eigensolver.eigenvalues();
const Eigen::EigenSolver< Eigen::Matrix<double,20,20> >::EigenvectorsType evecs = eigensolver.eigenvectors();
rsolns.clear();
rsolns.reserve(evalues.size());
for ( size_t i = 0; i < evalues.size(); i++ )
{
if ( evalues[i].imag() != 0 ) continue;
const double zsoln = evalues(i).real();
const double xsoln = evecs(16,i).real()/evecs(19,i).real();
const double ysoln = evecs(17,i).real()/evecs(19,i).real();
Eigen::Vector3d rsoln;
rsoln << xsoln, ysoln, zsoln;
rsolns.push_back(rsoln);
}
}
void eigen2vector_vector( const Eigen::Matrix<T_eig,-1, -1> &matrixE, std::vector< std::vector<T_vec> > &data )
{
// Conceive from the needs to change Structure matrix to Vector
int num_cols = matrixE.cols();
int num_rows = matrixE.rows();
data.clear();
data.resize(num_cols);
for (register int ft = 0; ft < num_cols ; ++ft)
{
data[ft].resize(num_rows);
for (register int x = 0; x < num_rows; ++x)
{
data[ft][x] = T_vec( matrixE(x,ft) );
}
}
};
void mrpt::math::ransac_detect_3D_planes(
const Eigen::Matrix<NUMTYPE,Eigen::Dynamic,1> &x,
const Eigen::Matrix<NUMTYPE,Eigen::Dynamic,1> &y,
const Eigen::Matrix<NUMTYPE,Eigen::Dynamic,1> &z,
vector<pair<size_t,TPlane> > &out_detected_planes,
const double threshold,
const size_t min_inliers_for_valid_plane
)
{
MRPT_START
ASSERT_(x.size()==y.size() && x.size()==z.size())
out_detected_planes.clear();
if (x.empty())
return;
// The running lists of remaining points after each plane, as a matrix:
CMatrixTemplateNumeric<NUMTYPE> remainingPoints( 3, x.size() );
remainingPoints.insertRow(0,x);
remainingPoints.insertRow(1,y);
remainingPoints.insertRow(2,z);
// ---------------------------------------------
// For each plane:
// ---------------------------------------------
for (;;)
{
mrpt::vector_size_t this_best_inliers;
CMatrixTemplateNumeric<NUMTYPE> this_best_model;
math::RANSAC_Template<NUMTYPE>::execute(
remainingPoints,
ransac3Dplane_fit,
ransac3Dplane_distance,
ransac3Dplane_degenerate,
threshold,
3, // Minimum set of points
this_best_inliers,
this_best_model,
true, // Verbose
0.999 // Prob. of good result
);
// Is this plane good enough?
if (this_best_inliers.size()>=min_inliers_for_valid_plane)
{
// Add this plane to the output list:
out_detected_planes.push_back(
std::make_pair<size_t,TPlane>(
this_best_inliers.size(),
TPlane( this_best_model(0,0), this_best_model(0,1),this_best_model(0,2),this_best_model(0,3) )
) );
out_detected_planes.rbegin()->second.unitarize();
// Discard the selected points so they are not used again for finding subsequent planes:
remainingPoints.removeColumns(this_best_inliers);
}
else
{
break; // Do not search for more planes.
}
}
MRPT_END
}