C++ Matrix::push_back方法代码示例

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 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
}

 void dims(Eigen::Matrix<T,Eigen::Dynamic,1> v, 
           std::vector<size_t> ds) {
   ds.push_back(v.size());
 }