C++ Vector4d::head方法代码示例

Eigen::Vector3d intersectRayPlane(const Eigen::Vector3d ray,
  const Eigen::Vector3d ray_origin, const Eigen::Vector4d plane)
{
    // Plane defined as ax + by + cz + d = 0
    // Ray: P = P0 + tV
    // Plane: P.N + d = 0, where P is intersection point
    // t = -(P0.N + d)/(V.N) , P = P0 + tV
    float t = -(ray_origin.dot(plane.head(3)) + plane[3]) / (ray.dot(plane.head(3)));
    Eigen::Vector3d P = ray_origin + t*ray;
    return P;
}

const CPoint3DCAMERA CMiniVisionToolbox::getPointStereoLinearTriangulationSVDDLT( const cv::Point2d& p_ptPointLEFT, const cv::Point2d& p_ptPointRIGHT, const Eigen::Matrix< double, 3, 4 >& p_matProjectionLEFT, const Eigen::Matrix< double, 3, 4 >& p_matProjectionRIGHT )
{
    //ds A matrix for system: A*X=0
    Eigen::Matrix4d matAHomogeneous;

    matAHomogeneous.row(0) = p_ptPointLEFT.x*p_matProjectionLEFT.row(2)-p_matProjectionLEFT.row(0);
    matAHomogeneous.row(1) = p_ptPointLEFT.y*p_matProjectionLEFT.row(2)-p_matProjectionLEFT.row(1);
    matAHomogeneous.row(2) = p_ptPointRIGHT.x*p_matProjectionRIGHT.row(2)-p_matProjectionRIGHT.row(0);
    matAHomogeneous.row(3) = p_ptPointRIGHT.y*p_matProjectionRIGHT.row(2)-p_matProjectionRIGHT.row(1);

    //ds solve system subject to ||A*x||=0 ||x||=1
    const Eigen::JacobiSVD< Eigen::Matrix4d > matSVD( matAHomogeneous, Eigen::ComputeFullU | Eigen::ComputeFullV );

    //ds solution x is the last column of V
    const Eigen::Vector4d vecX( matSVD.matrixV( ).col( 3 ) );

    return vecX.head( 3 )/vecX(3);
}

void LinearSystem::_BuildSystem(
        LinearSystem*       pLS,
        const unsigned int& StartU,
        const unsigned int& EndU,
        const unsigned int& StartV,
        const unsigned int& EndV
        )
{
    // Jacobian
    Eigen::Matrix<double, 1, 6> BigJ;
    Eigen::Matrix<double, 1, 6> J;

    BigJ.setZero();
    J.setZero();

	// Errors
	double		 e;
	double		 SSD = 0;
    double       Error    = 0;
    unsigned int ErrorPts = 0;

    for( int ii = StartV; ii < EndV; ii++ ) {
        for( int jj = StartU; jj < EndU; jj++ ) {
            // variables
            Eigen::Vector2d Ur;        // pixel position
            Eigen::Vector3d Pr, Pv;    // 3d point
            Eigen::Vector4d Ph;        // homogenized point

            // check if pixel is contained in our model (i.e. has depth)
            if( pLS->m_vVirtDepth[ii * pLS->m_nImgWidth + jj] == 0 ) {
                continue;
            }

            // --------------------- first term 1x2
            // evaluate 'a' = L[ Trv * Linv( Uv ) ]
            // back project to virtual camera's reference frame
            // this already brings points to robotics reference frame
            Pv = pLS->_BackProject( jj, ii, pLS->m_vVirtDepth[ii * pLS->m_nImgWidth + jj] );

            // convert to homogeneous coordinate
            Ph << Pv, 1;

            // transform point to reference camera's frame
            // Pr = Trv * Pv
            Ph = pLS->m_dTrv * Ph;
            Pr = Ph.head( 3 );

            // project onto reference camera
            Eigen::Vector3d Lr;

            Lr = pLS->_Project( Pr );
            Ur = Lr.head( 2 );
            Ur = Ur / Lr( 2 );

            // check if point falls in camera's field of view
            if( (Ur( 0 ) <= 1) || (Ur( 0 ) >= pLS->m_nImgWidth - 2) || (Ur( 1 ) <= 1)
                    || (Ur( 1 ) >= pLS->m_nImgHeight - 2) ) {
                continue;
            }

            // finite differences
            float                       TopPix   = pLS->_Interpolate( Ur( 0 ), Ur( 1 ) - 1, pLS->m_vRefImg );
            float                       BotPix   = pLS->_Interpolate( Ur( 0 ), Ur( 1 ) + 1, pLS->m_vRefImg );
            float                       LeftPix  = pLS->_Interpolate( Ur( 0 ) - 1, Ur( 1 ), pLS->m_vRefImg );
            float                       RightPix = pLS->_Interpolate( Ur( 0 ) + 1, Ur( 1 ), pLS->m_vRefImg );
            Eigen::Matrix<double, 1, 2> Term1;

            Term1( 0 ) = (RightPix - LeftPix) / 2.0;
            Term1( 1 ) = (BotPix - TopPix) / 2.0;

            // --------------------- second term 2x3
            // evaluate 'b' = Trv * Linv( Uv )
            // this was already calculated for Term1
            // fill matrix
            // 1/c      0       -a/c^2
            // 0       1/c     -b/c^2
            Eigen::Matrix<double, 2, 3> Term2;
            double                      PowC = Lr( 2 ) * Lr( 2 );

            Term2( 0, 0 ) = 1.0 / Lr( 2 );
            Term2( 0, 1 ) = 0;
            Term2( 0, 2 ) = -(Lr( 0 )) / PowC;
            Term2( 1, 0 ) = 0;
            Term2( 1, 1 ) = 1.0 / Lr( 2 );
            Term2( 1, 2 ) = -(Lr( 1 )) / PowC;
            Term2         = Term2 * pLS->m_Kr;

            // --------------------- third term 3x1
            // we need Pv in homogenous coordinates
            Ph << Pv, 1;

            Eigen::Vector4d Term3i;

            // last row of Term3 is truncated since it is always 0
            Eigen::Vector3d Term3;

            // fill Jacobian with T generators
            Term3i    = pLS->m_dTrv * pLS->m_Gen[0] * Ph;
            Term3     = Term3i.head( 3 );
            J( 0, 0 ) = Term1 * Term2 * Term3;
//.........这里部分代码省略.........

std::vector<int> PlaneFitter::ransacFit(std::vector<Eigen::Vector3d> points3d, std::vector<int> inputIndices){

	Eigen::Matrix<double,4,Eigen::Dynamic> D(4,points3d.size());
	this->vecToEigenMat(points3d,D);

	int N = D.cols();
	//cout << N << endl;

	// RANSAC variables
	float p = 0.99;
	float testRansiter = 0;
	int bestInlNum = 0;
	int best_it=-1;
	int inl_num;
	srand (time(NULL));
	Eigen::Vector4d *testplane = new Eigen::Vector4d();


	//keep the indices of the final inliers
	std::vector<int> bestInlierIds;

	// RANSAC loop
	for (int iter=0;iter<2000;iter++){

		//sample 3 different random points
		std::vector<int> samplevec;
		Eigen::Matrix<double,3,4> D_ransac_sample;
		int x;
		for (int s=0; s<3; s++){
			do{
				x = rand() % N;
			}while (std::find(samplevec.begin(), samplevec.end(), x) != samplevec.end());
			samplevec.push_back(x);
			D_ransac_sample.block<1,4>(s,0) = D.block<4,1>(0,x).transpose();//<sizeRows,sizeCols>(beginRow,beginCol)
		}

		//fit model
		this->fitPlane(D_ransac_sample,testplane);

		std::vector<int> inl_id;
		//inlier test
		inl_num = 0;
		for (int i=0;i<N;i++){
			if ( abs(D.block<4,1>(0,i).dot(*testplane)/testplane->head(3).norm()) < this->RANSAC_THRESH ){
				inl_num++;
				inl_id.push_back(i);
			}
		}

		//cout << "iter,inliers: " << iter <<","<<inl_num<< endl;

		//test (adaptive ransac)
		testRansiter = log(1-p)/log(1-pow((float)inl_num/N,3));
		if (inl_num > bestInlNum){
			bestInlNum = inl_num;
			best_it = iter;
			bestInlierIds.swap(inl_id);
		}

	}

	//bestplane contains the fitted plane
	cout << "best iter,inliers: " << best_it <<","<<bestInlNum<< endl;

	//Refine best plane
	delete this->Dinliers;
	delete this->fittedplane;

	this->numberOfInliers = bestInlNum;
	this->Dinliers = new Eigen::Matrix<double,4,Eigen::Dynamic>(4,bestInlNum);

	this->vecToEigenMat(points3d,*this->Dinliers,bestInlierIds);

	//get the indices of the inlier points for the output
	vector<int> outputIndices;
	for (size_t i=0; i< bestInlierIds.size(); i++){
		outputIndices.push_back(inputIndices[bestInlierIds[i]]);
	}

	this->fitPlane(Dinliers->transpose(),testplane);

	this->fittedplane = new Eigen::Vector4d();
	this->fittedplane = testplane;
	return outputIndices;
}