C++ VO_Shape::GetNbOfPoints方法代码示例

/**
* @brief    Calculate face fitting effect
* @param    refShape    - input     reference shape
* @param    fittedShape - input     fitting result
* @param    deviation   - output    what is the deviation from refShape to fittedShape
* @param    ptErrorFreq - output    point error frequency
* @param    nb          - input     how many evaluation levels that is to be used
* @return   whether the fitting is acceptable
*/
void CRecognitionAlgs::CalcShapeFittingEffect(	const VO_Shape& refShape,
												const VO_Shape& fittedShape,
												float& deviation,
												vector<float>& ptErrorFreq,
												int nb,
												vector<float>* ptErrPerPoint)
{
    assert(refShape.GetNbOfDim() == fittedShape.GetNbOfDim());
	assert(refShape.GetNbOfPoints() == fittedShape.GetNbOfPoints());
    unsigned int NbOfShapeDim   = refShape.GetNbOfDim();
    unsigned int NbOfPoints     = refShape.GetNbOfPoints();
	ptErrorFreq.resize(nb);

	vector<float> ptDists(NbOfPoints, 0.0f);
	
	for(unsigned int i = 0; i < NbOfPoints; i++)
	{
		ptDists[i] = 0.0f;
		for(unsigned int j = 0; j < NbOfShapeDim; j++)
		{
			ptDists[i] += pow(refShape.GetAShape(j*NbOfPoints+i) - fittedShape.GetAShape(j*NbOfPoints+i), 2.0f);
		}
		ptDists[i] = sqrt(ptDists[i]);
	}
	
	ptErrorFreq.resize(nb);
	for(int i = 0; i < nb; i++)
	{
		for (unsigned int j = 0; j < NbOfPoints; j++)
		{
			if (ptDists[j] < i)
			{
				ptErrorFreq[i]++;
			}
		}
		ptErrorFreq[i] /= static_cast<float>(NbOfPoints);
	}
	float sumPtDist = 0.0;
	for(unsigned int i = 0; i<NbOfPoints;++i){
		sumPtDist += ptDists[i];
	}
	printf("Avg ptDists = %f\n",sumPtDist/NbOfPoints);

    deviation = CRecognitionAlgs::ShapeDistance(refShape, fittedShape);
	if(ptErrPerPoint != 0){
		(*ptErrPerPoint) = ptDists;
	}
}

/**
 * @author     JIA Pei
 * @version    2010-06-07
 * @brief      Constrain all points respetively
 * @param      ioShape     	Input and Output - the input and output shape
*/
void VO_Point2DDistributionModel::VO_ConstrainAllPoints(VO_Shape& ioShape)
{
    unsigned int NbOfPoints = ioShape.GetNbOfPoints();
    Point2f pt;

    for(unsigned int i = 0; i < NbOfPoints; i++)
    {
        pt = ioShape.GetA2DPoint(i);
        VO_Point2DDistributionModel::VO_ConstrainSinglePoint( pt, this->m_VONormalizedEllipses[i] );
        ioShape.SetA2DPoint(pt, i);
    }
}

/**
 * @param	fd					- input		folder name
 * @param	fnIdx				- input		fitting result
 * @param	deviation			- input		what is the deviation from refShape to fittedShape
 * @param	ptErrorFreq			- input		for curve to display frequency -- point distance
 * @param	fittedShape			- input		fitting result
 * @return	whether the fitting is acceptable
 */
void CRecognitionAlgs::SaveShapeResults(		const string& fd,
												const string& fnIdx,
												float deviation,
												vector<float>& ptDists,
												vector<float>& ptErrorFreq,
												const VO_Shape& fittedShape)
{
    string fn;
    fn = fd + "/" + fnIdx + ".res";
    
    fstream fp;
    fp.open(fn.c_str (), ios::out);

	fp << "Error per point -- Distance from ground truth" << endl;
	for(unsigned int i = 0; i < ptDists.size(); ++i){
		fp << ptDists[i] << endl;
	}
	fp << endl;

	fp << "Total landmark error" << endl;
	float errSum = std::accumulate(ptDists.begin(),ptDists.end(),0.0f);
	fp << errSum << endl;
	fp <<"Average landmark distance" << endl;
	fp << errSum / ptDists.size() << endl;
	fp << endl;

    fp << "Total Deviation" << endl << deviation << endl;				// deviation
    fp << "Point Error -- Frequency" << endl;
    for(unsigned int i = 0; i < ptErrorFreq.size(); i++)
    {
        fp << ptErrorFreq[i] << " ";
    }
	fp << endl;
	fp << endl;
	fp << "Fitted points" << endl;
	//output actual points along with error frequency
	unsigned int NbOfShapeDim   = fittedShape.GetNbOfDim();
	unsigned int NbOfPoints     = fittedShape.GetNbOfPoints();
	for(unsigned int i = 0; i < NbOfPoints; i++)
	{
		for(unsigned int j = 0; j < NbOfShapeDim; j++)
		{
			fp << fittedShape.GetAShape(j*NbOfPoints+i) << " ";
		}
		fp << endl;
	}
    fp << endl;
	
    fp.close();fp.clear();
}

/**
 * @author      JIA Pei
 * @version     2010-02-07
 * @brief       Write all annotation data in VO_Shape to a file
 * @param       filename    output parameter, which .pts annotation file to write
 * @param       iAAMShape   input parameter, save annotation data from AAM shape data structure
*/
void CAnnotationDBIO::WritePTS( const std::string &filename,
                                const VO_Shape& iAAMShape)
{
    std::fstream fp;
    fp.open(filename.c_str (), std::ios::out);

    std::string temp, oneLine;
    std::stringstream ss;
    float tempFloat = 0.0f;
    unsigned int NbOfPoints = iAAMShape.GetNbOfPoints();

    fp << "version: 1" << std::endl
    << "n_points: " << NbOfPoints << std::endl
    << "{" << std::endl;

    for (unsigned int i = 0; i < NbOfPoints; i++)
    {
        fp << iAAMShape.GetA2DPoint(i).x << " " << iAAMShape.GetA2DPoint(i).y << std::endl;
    }

    fp << "}" << std::endl << std::endl;

    fp.close ();
}

// Estimate face absolute orientations
vector<float> CRecognitionAlgs::CalcAbsoluteOrientations(
    const VO_Shape& iShape2D,
    const VO_Shape& iShape3D,
    VO_Shape& oShape2D)
{
    assert (iShape2D.GetNbOfPoints() == iShape3D.GetNbOfPoints() );
    unsigned int NbOfPoints = iShape3D.GetNbOfPoints();
    Point3f pt3d;
    Point2f pt2d;
    float height1 = iShape2D.GetHeight();
    float height2 = iShape3D.GetHeight();
    VO_Shape tempShape2D = iShape2D;
    tempShape2D.Scale(height2/height1);

    //Create the model points
    std::vector<CvPoint3D32f> modelPoints;
    for(unsigned int i = 0; i < NbOfPoints; ++i)
    {
        pt3d = iShape3D.GetA3DPoint(i);
        modelPoints.push_back(cvPoint3D32f(pt3d.x, pt3d.y, pt3d.z));
    }

    //Create the image points
    std::vector<CvPoint2D32f> srcImagePoints;
    for(unsigned int i = 0; i < NbOfPoints; ++i)
    {
        pt2d = tempShape2D.GetA2DPoint(i);
        srcImagePoints.push_back(cvPoint2D32f(pt2d.x, pt2d.y));
    }

    //Create the POSIT object with the model points
    CvPOSITObject *positObject = cvCreatePOSITObject( &modelPoints[0], NbOfPoints );

    //Estimate the pose
    CvMatr32f rotation_matrix = new float[9];
    CvVect32f translation_vector = new float[3];
    CvTermCriteria criteria = cvTermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 100, 1.0e-4f);
    cvPOSIT( positObject, &srcImagePoints[0], FOCAL_LENGTH, criteria, rotation_matrix, translation_vector );

    //rotation_matrix to Euler angles, refer to VO_Shape::GetRotation
    float sin_beta  = -rotation_matrix[0 * 3 + 2];
    float tan_alpha = rotation_matrix[1 * 3 + 2] / rotation_matrix[2 * 3 + 2];
    float tan_gamma = rotation_matrix[0 * 3 + 1] / rotation_matrix[0 * 3 + 0];

    //Project the model points with the estimated pose
    oShape2D = tempShape2D;
    for ( unsigned int i=0; i < NbOfPoints; ++i )
    {
        pt3d.x = rotation_matrix[0] * modelPoints[i].x +
            rotation_matrix[1] * modelPoints[i].y +
            rotation_matrix[2] * modelPoints[i].z +
            translation_vector[0];
        pt3d.y = rotation_matrix[3] * modelPoints[i].x +
            rotation_matrix[4] * modelPoints[i].y +
            rotation_matrix[5] * modelPoints[i].z +
            translation_vector[1];
        pt3d.z = rotation_matrix[6] * modelPoints[i].x +
            rotation_matrix[7] * modelPoints[i].y +
            rotation_matrix[8] * modelPoints[i].z +
            translation_vector[2];
        if ( pt3d.z != 0 )
        {
            pt2d.x = FOCAL_LENGTH * pt3d.x / pt3d.z;
            pt2d.y = FOCAL_LENGTH * pt3d.y / pt3d.z;
        }
        oShape2D.SetA2DPoint(pt2d, i);
    }

    //return Euler angles
    vector<float> pos(3);
    pos[0] = atan(tan_alpha);    // yaw
    pos[1] = asin(sin_beta);     // pitch
    pos[2] = atan(tan_gamma);    // roll
    return pos;
}

/**
 * @param	fd					- input		folder name
 * @param	fnIdx				- input		fitting result
 * @param	deviation			- input		what is the deviation from refShape to fittedShape
 * @param	ptErrorFreq			- input		for curve to display frequency -- point distance
 * @param	fittedShape			- input		fitting result
 * @param	gt_cp				- input		ground truth canidate points
 * @param	t_cp				- input		tested canidate points (l eye, r eye, mouth)
 * @return	whether the fitting is acceptable
 */
void CRecognitionAlgs::SaveFittingResults(		const string& fd,
												const string& fnIdx,
												float deviation,
												vector<float>& ptDists,
												vector<float>& ptErrorFreq,
												const VO_Shape& fittedShape,
												cv::Point2f* gt_cP,
												cv::Point2f* t_cP,
												float fitTime)
{
    string fn;
    fn = fd + "/" + fnIdx + ".res";
    
    fstream fp;
    fp.open(fn.c_str (), ios::out);

	fp << "Error per point -- Distance from ground truth" << endl;
	for(unsigned int i = 0; i < ptDists.size(); ++i){
		fp << ptDists[i] << endl;
	}
	fp << endl;

	fp << "Total landmark error" << endl;
	float errSum = std::accumulate(ptDists.begin(),ptDists.end(),0.0f);
	fp << errSum << endl;
	fp << "Average landmark distance" << endl;
	fp << errSum / ptDists.size() << endl;
	fp << "Candidate point error (Left eye, Right eye, Mouth)" << endl;
	//messy distance, too lazy
	float le_dist = sqrt(pow(gt_cP[0].x - t_cP[0].x,2) + pow(gt_cP[0].y - t_cP[0].y,2));
	float re_dist = sqrt(pow(gt_cP[1].x - t_cP[1].x,2) + pow(gt_cP[1].y - t_cP[1].y,2));
	float m_dist = sqrt(pow(gt_cP[2].x - t_cP[2].x,2) + pow(gt_cP[2].y - t_cP[2].y,2));

	fp << le_dist << endl;
	fp << re_dist << endl;
	fp << m_dist << endl;
	fp << endl;
	fp << "Fitting time" << endl;
	fp << fitTime << endl;
	fp << endl;

    fp << "Total deviation" << endl << deviation << endl;				// deviation
    fp << "Point error -- Frequency" << endl;
    for(unsigned int i = 0; i < ptErrorFreq.size(); i++)
    {
        fp << ptErrorFreq[i] << " ";
    }
	fp << endl;
	fp << endl;
	fp << "Canidate points" << endl;
	fp << t_cP[0].x << " " << t_cP[0].y << endl;
	fp << t_cP[1].x << " " << t_cP[1].y << endl;
	fp << t_cP[2].x << " " << t_cP[2].y << endl;
	fp << "Fitted points" << endl;
	//output actual points along with error frequency
	unsigned int NbOfShapeDim   = fittedShape.GetNbOfDim();
	unsigned int NbOfPoints     = fittedShape.GetNbOfPoints();
	for(unsigned int i = 0; i < NbOfPoints; i++)
	{
		for(unsigned int j = 0; j < NbOfShapeDim; j++)
		{
			fp << fittedShape.GetAShape(j*NbOfPoints+i) << " ";
		}
		fp << endl;
	}
    fp << endl;
	
    fp.close();fp.clear();
}

/**
 * @author      YAO Wei, JIA Pei
 * @version     2010-05-20
 * @brief       Find the best offset for one point
 * @param       asmmodel        Input - the ASM model
 * @param       iImg            Input - image to be fitted
 * @param       ioShape         Input and output - the input and output shape
 * @param       iShapeInfo      Input - the shape information
 * @param       iMean           Input - mean profile
 * @param       iCovInverse     Input - covariance inverse
 * @param       Lev             Input - current pyramid level
 * @param       offSetTolerance Input - offset tolerance, which is used to determine whether this point is convergede or not
 * @param       profdim         Input - specify the dimension that is going to be used when updating shape.
 *                              Sometimes, the trained data is of 4D profiles, but the user may only use 1D to test.
 * @note        Refer to "AAM Revisited, page 34, figure 13", particularly, those steps.
*/
int VO_FittingASMNDProfiles::UpdateShape(   const VO_ASMNDProfiles* asmmodel,
                                            const cv::Mat& iImg,
                                            VO_Shape& ioShape,
                                            const std::vector<VO_Shape2DInfo>& iShapeInfo,
                                            const std::vector< VO_Profile >& iMean,
                                            const std::vector< std::vector< cv::Mat_<float> > >& iCovInverse,
                                            unsigned int offSetTolerance,
                                            unsigned int profdim)
{
    int nGoodLandmarks = 0;
    std::vector<int> nBestOffset(profdim, 0);
    unsigned int NbOfPoints     = ioShape.GetNbOfPoints();
    unsigned int NbOfShapeDim   = ioShape.GetNbOfDim();
    unsigned int ProfileLength    = iMean[0].GetProfileLength();
    //std::vector<float> dists(NbOfPoints, 0.0f);
    cv::Point2f pt;

    // Take care of the 1st direction first.
    for (unsigned int i = 0; i < NbOfPoints; i++)
    {
        /////////////////////////////////////////////////////////////////////////////
        ///Calculate profile norm direction//////////////////////////////////////////
        /** Here, this is not compatible with 3D */
        cv::Point2f PrevPoint = ioShape.GetA2DPoint ( iShapeInfo[i].GetFrom() );
        cv::Point2f ThisPoint = ioShape.GetA2DPoint ( i );
        cv::Point2f NextPoint = ioShape.GetA2DPoint ( iShapeInfo[i].GetTo() );

        float deltaX, deltaY;
        float normX, normY;
        float sqrtsum;
        float bestXOffset, bestYOffset;

        // left side (connected from side)
        deltaX = ThisPoint.x - PrevPoint.x;
        deltaY = ThisPoint.y - PrevPoint.y;
        sqrtsum = sqrt ( deltaX*deltaX + deltaY*deltaY );
        if ( sqrtsum < FLT_EPSILON ) sqrtsum = 1.0f;
        deltaX /= sqrtsum; deltaY /= sqrtsum;         // Normalize
        // Firstly, normX normY record left side norm.
        normX = -deltaY;
        normY = deltaX;

        // right side (connected to side)
        deltaX = NextPoint.x - ThisPoint.x;
        deltaY = NextPoint.y - ThisPoint.y;
        sqrtsum = sqrt ( deltaX*deltaX + deltaY*deltaY );
        if ( sqrtsum < FLT_EPSILON ) sqrtsum = 1.0f;
        deltaX /= sqrtsum; deltaY /= sqrtsum;         // Normalize
        // Secondly, normX normY will average both left side and right side norm.
        normX += -deltaY;
        normY += deltaX;

        // Average left right side
        sqrtsum = sqrt ( normX*normX + normY*normY );
        if ( sqrtsum < FLT_EPSILON ) sqrtsum = 1.0f;
        normX /= sqrtsum;
        normY /= sqrtsum;                             // Final Normalize
        /////////////////////////////////////////////////////////////////////////////

        nBestOffset[0] = VO_FittingASMNDProfiles::VO_FindBestMatchingProfile1D( iImg,
                                                                                ThisPoint,
                                                                                iMean[i].Get1DimProfile(0),
                                                                                iCovInverse[i][0],
                                                                                ProfileLength,
                                                                                offSetTolerance,
                                                                                normX,
                                                                                normY);

        // set OutShape(iPoint) to best offset from current position
        // one dimensional profile: must move point along the whisker
        bestXOffset = nBestOffset[0] * normX;
        bestYOffset = nBestOffset[0] * normY;
        pt.x = ThisPoint.x + bestXOffset;
        pt.y = ThisPoint.y + bestYOffset;
        ioShape.SetA2DPoint(pt, i);
        //dists[i] = sqrt( pow( (double)bestXOffset, 2.0) + pow( (double)bestYOffset, 2.0) );

        //if (abs(nBestOffset[0]) <= offSetTolerance/2)
        if(profdim == 1)
        {
            if (abs(nBestOffset[0]) <= 1)
                nGoodLandmarks++;
        }
    }
//.........这里部分代码省略.........

/**
 * @brief       Calculate some key points on the face
 * @param       oPoint      output  point list
 * @param       iShape      input   shape
 * @param       iFaceParts  inut    faceparts
 * @param       ptType      input   point type
 * @return      void
 */
void VO_KeyPoint::CalcFaceKeyPoint( cv::Point2f& oPoint,
                                    const VO_Shape& iShape,
                                    const VO_FaceParts& iFaceParts,
                                    unsigned int ptType)
{
    std::vector<unsigned int> facePartsPoints;
    VO_Shape subiShape;
    // Very very very very important.
    // Explained by JIA Pei.
    // "resize()" is just for resize;
    // it doesn't always set what's already inside the the std::vector to "0"
    // Therefore, clear() is a must before resize().

    switch(ptType)
    {
    case CENTEROFGRAVITY:
        if (iShape.GetNbOfPoints() > 0)
            oPoint = iShape.GetA2DPoint( VO_Shape::CENTER);
        break;
    case LEFTEYELEFTCORNER:
        {
            facePartsPoints = iFaceParts.VO_GetOneFacePart(VO_FacePart::LEFTEYE).GetIndexes();
            if (facePartsPoints.size() > 0)
            {
                subiShape = iShape.GetSubShape(facePartsPoints);
                oPoint = subiShape.GetA2DPoint(VO_Shape::LEFTMOST);
            }
        }
        break;
    case LEFTEYERIGHTCORNER:
        {
            facePartsPoints = iFaceParts.VO_GetOneFacePart(VO_FacePart::LEFTEYE).GetIndexes();
            if (facePartsPoints.size() > 0)
            {
                subiShape = iShape.GetSubShape(facePartsPoints);
                oPoint = subiShape.GetA2DPoint(VO_Shape::RIGHTMOST);
            }
        }
        break;
    case LEFTEYECENTER:
        {
            facePartsPoints = iFaceParts.VO_GetOneFacePart(VO_FacePart::LEFTEYE).GetIndexes();
            if (facePartsPoints.size() > 0)
            {
                subiShape = iShape.GetSubShape(facePartsPoints);
                oPoint = subiShape.GetA2DPoint( VO_Shape::CENTER);
            }
        }
        break;
    case RIGHTEYELEFTCORNER:
        {
            facePartsPoints = iFaceParts.VO_GetOneFacePart(VO_FacePart::RIGHTEYE).GetIndexes();
            if (facePartsPoints.size() > 0)
            {
                subiShape = iShape.GetSubShape(facePartsPoints);
                oPoint = subiShape.GetA2DPoint(VO_Shape::LEFTMOST);
            }
        }
        break;
    case RIGHTEYERIGHTCORNER:
        {
            facePartsPoints = iFaceParts.VO_GetOneFacePart(VO_FacePart::RIGHTEYE).GetIndexes();
            if (facePartsPoints.size() > 0)
            {
                subiShape = iShape.GetSubShape(facePartsPoints);
                oPoint = subiShape.GetA2DPoint(VO_Shape::RIGHTMOST);
            }
        }
        break;
    case RIGHTEYECENTER:
        {
            facePartsPoints = iFaceParts.VO_GetOneFacePart(VO_FacePart::RIGHTEYE).GetIndexes();
            if (facePartsPoints.size() > 0)
            {
                subiShape = iShape.GetSubShape(facePartsPoints);
                oPoint = subiShape.GetA2DPoint( VO_Shape::CENTER);
            }
        }
        break;
    case NOSETIPKEY:
        {
            facePartsPoints = iFaceParts.VO_GetOneFacePart(VO_FacePart::NOSETIP).GetIndexes();    // Just one point
            if (facePartsPoints.size() == 1)
                oPoint = iShape.GetA2DPoint(facePartsPoints[0]);
        }
        break;
    case NOSTRILLEFT:
        {
            facePartsPoints = iFaceParts.VO_GetOneFacePart(VO_FacePart::NOSTRIL).GetIndexes();
            if (facePartsPoints.size() > 0)
            {
                subiShape = iShape.GetSubShape(facePartsPoints);
//.........这里部分代码省略.........