VOSM Code

/****************************************************************************
* 
* Copyright (c) 2006 by JIA Pei, all rights reserved.
* Copyright (c) 2006 by Vision Open: http://www.visionopen.com/
*
* Author:      	JIA Pei
* Contact:     	jp4work@gmail.com
* URL:		http://www.visionopen.com/members/jiapei.php
* The author administrates Vision Open -- http://www.visionopen.com
*
* This program is free software; you can redistribute it and/or modify it 
* under the terms of the GNU General Public License as published by the 
* Free Software Foundation.
*
* This program is distributed in the hope that it will be useful, but 
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY 
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License 
* for more details.
* 
* You should have received a copy of the GNU General Public License along 
* with this program; if not, write to the Free Software Foundation, Inc., 
* 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
* 
* This software is partly based on the following open source: 
*  
*		- Boost 
*		- OpenCV 
* 
* This software is using IMM Face Database, which can be downloaded from
* http://www2.imm.dtu.dk/~aam/.
*
* M. B. Stegmann, B. K. Ersb{\o}ll, and R. Larsen. FAME - a flexible appearance 
* modelling environment. IEEE Trans. on Medical Imaging, 22(10):1319-1331, 2003
*
****************************************************************************/

// $Id: lv_aambuilding.cpp,v 1.1.1.1 2006-09-03 17:49:04 JIA Pei Exp $ 


#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <set>
#include <algorithm>

#include "lv_aambuilding.h"
#include "lv_aamshape2d.h"
#include "lv_io.h"
#include "lv_aamedge.h"
#include "lv_aamtriangle2d.h"


using namespace std;


extern const double TruncatePercentage;
extern const double TemplateMarginX;
extern const double TemplateMarginY;


lv_aambuilding::lv_aambuilding()
{
    this->m_CVMShape                                = NULL;    
    this->m_CVMAlignedShapes                        = NULL;
    this->m_CVMMeanShape                            = NULL;
    this->m_CVMAlignedShapesEigenVectors            = NULL;
    this->m_CVMAlignedShapesEigenValues             = NULL;
    this->m_CVMTruncatedAlignedShapesEigenVectors   = NULL;
    this->m_CVMTruncatedAlignedShapesEigenValues    = NULL;
    this->m_CVMPoints                               = NULL;
    this->m_CVMConvexHull                           = NULL;
    this->m_CVSubdiv                                = NULL;
    this->m_CVMTexture                              = NULL;
    this->m_CVMMeanTexture                          = NULL;
    this->m_CVMTexturesEigenVectors                 = NULL;
    this->m_CVMTexturesEigenValues                  = NULL;
    this->m_CVMTruncatedTexturesEigenVectors        = NULL;
    this->m_CVMTruncatedTexturesEigenValues         = NULL;    
    this->m_CVMWeightsScale2Texture                 = NULL;
    this->m_CVMConcatenated                         = NULL;
    this->m_CVMMeanConcatenated                     = NULL;
    this->m_CVMConcatenatedEigenVectors             = NULL;
    this->m_CVMConcatenatedEigenValues              = NULL;
    this->m_CVMTruncatedConcatenatedEigenVectors    = NULL;
    this->m_CVMTruncatedConcatenatedEigenValues     = NULL;
    this->m_CVMQs                                   = NULL;
    this->m_CVMQg                                   = NULL;
    this->m_CVDeltaC                                = NULL;
    this->m_CVDeltaG                                = NULL;
    this->m_CVR                                     = NULL;
    this->m_R_s                                     = NULL;
    this->m_R_g                                     = NULL;
    this->FaceTemplate                              = NULL;
    this->GradTPartialWPB                           = NULL;
    this->GradTPartialWPG                           = NULL;
    this->GradTPartialWPR                           = NULL;
    this->m_dAverageSize                            = 0;
    this->m_iNbOfTextures                           = 0;
    this->m_iNbOfSamples                            = 0;
    this->m_iNbOfPoints                             = 0;
}


lv_aambuilding::lv_aambuilding(const vector<string> &asfFiles,
                               const vector<string> &imgFiles,
                               string build2File)
{
    this->LV_BuildAAM(asfFiles, imgFiles, build2File);
}


lv_aambuilding::~lv_aambuilding()
{
    cvReleaseMat (&this->m_CVMShape);    
    cvReleaseMat (&this->m_CVMAlignedShapes);
    cvReleaseMat (&this->m_CVMMeanShape);
    cvReleaseMat (&this->m_CVMAlignedShapesEigenVectors);
    cvReleaseMat (&this->m_CVMAlignedShapesEigenValues);
    cvReleaseMat (&this->m_CVMTruncatedAlignedShapesEigenVectors);
    cvReleaseMat (&this->m_CVMTruncatedAlignedShapesEigenValues);
    cvReleaseMat (&this->m_CVMPoints);
    cvReleaseMat (&this->m_CVMConvexHull);
    // How to release CvSubdiv2D* m_CVSubdiv is still a problem.
    // cvReleaseMat (&this->m_CVSubdiv);
    cvReleaseMat (&this->m_CVMTexture);
    cvReleaseMat (&this->m_CVMMeanTexture);
    cvReleaseMat (&this->m_CVMTexturesEigenVectors);
    cvReleaseMat (&this->m_CVMTexturesEigenValues);
    cvReleaseMat (&this->m_CVMTruncatedTexturesEigenVectors);
    cvReleaseMat (&this->m_CVMTruncatedTexturesEigenValues);    
    cvReleaseMat (&this->m_CVMWeightsScale2Texture);
    cvReleaseMat (&this->m_CVMConcatenated);
    cvReleaseMat (&this->m_CVMMeanConcatenated);
    cvReleaseMat (&this->m_CVMConcatenatedEigenVectors);
    cvReleaseMat (&this->m_CVMConcatenatedEigenValues);
    cvReleaseMat (&this->m_CVMTruncatedConcatenatedEigenVectors);
    cvReleaseMat (&this->m_CVMTruncatedConcatenatedEigenValues);
    cvReleaseMat (&this->m_CVMQs);
    cvReleaseMat (&this->m_CVMQg);
    cvReleaseMat (&this->m_CVDeltaC);
    cvReleaseMat (&this->m_CVDeltaG);
    cvReleaseMat (&this->m_CVR);
    cvReleaseMat (&this->m_R_s);
    cvReleaseMat (&this->m_R_g);
    cvReleaseMat (&this->GradTPartialWPB);
    cvReleaseMat (&this->GradTPartialWPG);
    cvReleaseMat (&this->GradTPartialWPR);
    cvReleaseImage (&this->FaceTemplate);

    for (unsigned int i = 0; i <this->m_vImages.size (); i++ )
    {
        if (this->m_vImages[i])
        {
            cvReleaseImage (&this->m_vImages[i]);
        }
    }

}



//build AAM
void lv_aambuilding::LV_BuildAAM(const vector<string> &asfFiles, 
                                 const vector<string> &imgFiles, 
                                 string build2File)
{
    cvNamedWindow( "Reference", 1 );
    cvNamedWindow( "Mean", 1 );

    this->m_vasfFiles = asfFiles;
    this->m_vimgFiles = imgFiles;


   this->LV_BuildAAMShapeModel();

   this->LV_BuildAAMTextureModel();


    cvShowImage ("Reference", FaceTemplate);

    

    this->LV_WriteParameters2File (build2File);

    cvWaitKey(0);

}




/**

    @author     JIA Pei

    @version    2006-09-01

    @brief      build AAM shape model

*/
void lv_aambuilding::LV_BuildAAMShapeModel()
{

    this->LV_LoadShapes2Absolute( this->m_vasfFiles);

    this->LV_AlignShapes();

    int NbOfEigenAtMost = MIN(this->m_iNbOfSamples, this->m_CVMAlignedShapes->cols);

    this->m_CVMAlignedShapesEigenValues = cvCreateMat( 1, NbOfEigenAtMost, CV_64FC1 );

    this->m_CVMAlignedShapesEigenVectors = cvCreateMat( NbOfEigenAtMost, this->m_iNbOfPoints * 2, CV_64FC1 );

    this->m_CVMMeanShape = cvCreateMat( 1, this->m_iNbOfPoints * 2, CV_64FC1 );

    cvCalcPCA( this->m_CVMAlignedShapes, this->m_CVMMeanShape, 
        this->m_CVMAlignedShapesEigenValues, this->m_CVMAlignedShapesEigenVectors,
        CV_PCA_DATA_AS_ROW );

    CvScalar SumOfEigenValues = cvSum( this->m_CVMAlignedShapesEigenValues );

    double limit =.0001 * SumOfEigenValues.val[0];

    int i = 0;
    for(i = 0; i < this->m_CVMAlignedShapesEigenValues->cols; i++)
    { 
        if ( CV_MAT_ELEM( *this->m_CVMAlignedShapesEigenValues, double, 0, i ) < limit) break;
    }

    // NonZero EigenValues
    int NbOfNonZeroEigenValues = i;

    for(i = NbOfNonZeroEigenValues; i < this->m_CVMAlignedShapesEigenValues->cols; i++)
    {
        CV_MAT_ELEM( *this->m_CVMAlignedShapesEigenValues, double, 0, i ) = 0.;
    }

    SumOfEigenValues = cvSum( this->m_CVMAlignedShapesEigenValues );
    double ps = .0;
    int np =0;

    for(unsigned int i = 0; i < NbOfNonZeroEigenValues; i++)
    {
        ps += CV_MAT_ELEM( *this->m_CVMAlignedShapesEigenValues, double, 0, i );
        ++np;
        if( ps/SumOfEigenValues.val[0] >= TruncatePercentage) break;
    }

    this->m_CVMTruncatedAlignedShapesEigenValues = cvCreateMat( 1, np, CV_64FC1);
    this->m_CVMTruncatedAlignedShapesEigenVectors = cvCreateMat( np, this->m_iNbOfPoints * 2, CV_64FC1);

    for(unsigned int i = 0; i < np; i++)
    {
        CV_MAT_ELEM( *this->m_CVMTruncatedAlignedShapesEigenValues, double, 0, i )
            = CV_MAT_ELEM( *this->m_CVMAlignedShapesEigenValues, double, 0, i );
    }

    for(unsigned int i = 0; i < np; i++)
    {
        for (unsigned int j = 0; j < this->m_CVMTruncatedAlignedShapesEigenVectors->cols; j++)
        {
            CV_MAT_ELEM( *this->m_CVMTruncatedAlignedShapesEigenVectors, double, i, j )
                = CV_MAT_ELEM( *this->m_CVMAlignedShapesEigenVectors, double, i, j );
        }
    }

}



/**

    @author     JIA Pei

    @version    2006-09-01

    @brief      load shapes from asfFiles to m_CVMShape

    @param      asfFiles	Vector of "asf" filenames.

    @output	    "m_vAAMAlignedShapes", "m_CVMShape".

    @see        "How to access matrix elements?" in OpenCV FAQ!

*/
void lv_aambuilding::LV_LoadShapes2Absolute(const vector<string> &asfFiles)
{
    this->m_iNbOfSamples = asfFiles.size ();

    lv_aamshape2d tempAAMShape;

    // Read all ASF file data into "m_vAAMAlignedShapes" and scale back to the original
    // size, and load all corresponding images
    for(unsigned int i=0; i < this->m_iNbOfSamples; i++)
    {
        tempAAMShape.ReadASF( asfFiles[i] );

        this->m_vAAMAlignedShapes.push_back(tempAAMShape);

        this->m_vImages.push_back (cvLoadImage ( this->m_vimgFiles[i].c_str (), CV_LOAD_IMAGE_COLOR  ));

        this->m_vAAMAlignedShapes[i].ScaleXY (this->m_vImages[i]->width, this->m_vImages[i]->height);

        tempAAMShape.Clear ();

    }
    // Get the number of points in the shape
    this->m_iNbOfPoints = this->m_vAAMAlignedShapes[0].PointsInShape ();

    this->m_CVMShape = cvCreateMat (this->m_iNbOfSamples, this->m_iNbOfPoints * 2, CV_64FC1);

    // Assign all points' positions from "vector<lv_aamshape2d> m_vAAMAlignedShapes" to "CvMat* m_CVMShape"
    for(unsigned int i=0; i < this->m_iNbOfSamples; i++)
    {
        for(unsigned int j = 0; j < this->m_iNbOfPoints; j++)
        {
            CV_MAT_ELEM( *this->m_CVMShape, double, i, 2*j ) = (double)(this->m_vAAMAlignedShapes[i].m_vPoint[j].m_CVxy.x);
            CV_MAT_ELEM( *this->m_CVMShape, double, i, 2*j+1 ) = (double)(this->m_vAAMAlignedShapes[i].m_vPoint[j].m_CVxy.y);
        }
    }
}



/**

    @author     JIA Pei

    @version    2006-09-01

    @brief      align shapes to the original parameters

    @output	    "m_CVMAlignedShapes", "m_CVMMeanShape"

*/
void lv_aambuilding::LV_AlignShapes()
{
    // allocate storage for "m_CVMAlignedShapes" and "m_CVMMeanShape"
    // Here, "m_CVMAlignedShapes" is allocated beforehand.
    this->m_CVMAlignedShapes = cvCreateMat( this->m_CVMShape->rows, this->m_CVMShape->cols, CV_64FC1 );

    // Note by JIA Pei, cvCopy() will release the
    cvCopy(this->m_CVMShape, this->m_CVMAlignedShapes);

    // calculate the average shape size according to the 2-norm
    this->m_dAverageSize = .0;
    for(int i = 0; i < this->m_iNbOfSamples; i++)
    {
        this->m_dAverageSize += this->m_vAAMAlignedShapes[i].Normalize(); // move to origin and scale to unit size
    }
    this->m_dAverageSize /= this->m_iNbOfSamples;

    this->m_oAAMMeanShape.resize (this->m_iNbOfPoints);
    // Initializing "m_oAAMMeanShape" to the first shape is no good. 
    // It's better to initialize "m_oAAMMeanShape" to MeanShape instead of AlignedShapes[0]
    // this->m_oAAMMeanShape = this->m_vAAMAlignedShapes[0];
    this->LV_MeanShape( this->m_oAAMMeanShape );
    this->m_oAAMMeanShape.m_sHostImage = "";

    // setup
    bool forceMeanOrientation = true;

    // do a number of alignment iterations until the mean shape estimate is stable
    double diff, diff_max = 0.001;              // diff must be less than 0.1%
    int max_iter = 30, iter = 1;
    lv_aamshape2d tempMeanShape;
    double theta;
    
    CvMat * rot = cvCreateMat (1, this->m_iNbOfSamples, CV_64FC1);

    do
    {
        // normalize and align all other shapes to the mean shape estimate
        for(int i=0;i < this->m_iNbOfSamples;i++)
        {
            // align the i-th shape to the estimate of the mean shape
            this->m_vAAMAlignedShapes[i].AlignTo( this->m_oAAMMeanShape, &theta );
            CV_MAT_ELEM( *rot, double, 0, i )  = theta;  // record the rotation

            // re-scale to unit size to avoid the so-called 'shrinking effect'
            // i.e. the alignment error goes towards zero, when the shapes are downscaled
            this->m_vAAMAlignedShapes[i].Scale( 1.0/this->m_vAAMAlignedShapes[i].GetShapeNorm () );
        }
        tempMeanShape = this->m_oAAMMeanShape;

        // estimate the new mean shape
        this->LV_MeanShape( this->m_oAAMMeanShape );

        // if this is the first iteration, correct the orientation of the mean shape, so that 
        // rotation of the training set to fit the mean shape is -- on average -- 
        // zero or put more clearly: "make the meanshape have a mean orientation"
        if (forceMeanOrientation && iter==1)
        {
            CvScalar rotMean = cvAvg( rot );
            this->m_oAAMMeanShape.Rotate( -rotMean.val[0] );
        }

        diff = (tempMeanShape-this->m_oAAMMeanShape).ShapeSize();
        
        ++iter;

    }while( fabs(diff)/this->m_oAAMMeanShape.ShapeSize() > diff_max && iter < max_iter );

    cvReleaseMat (&rot);

    // Explained by JIA Pei, 2006-09-15. Actually, it's projecting into 
    // m_oAAMMeanShape direction to recalculate the respective shape scale.
    double ts;

    // Asked by JIA Pei, 2006-09-15. It's not actually a projection as in
    // http://elearning.stut.edu.tw/mechanical/Statics/newpage71.htm
    // It's a kind of rescale, in former calculation, we do
    // this->m_vAAMAlignedShapes[i].Scale( 1.0/this->m_vAAMAlignedShapes[i].GetShapeNorm () )
    for(unsigned int i = 0; i < this->m_iNbOfSamples; i++)
    {
        // project into m_oAAMMeanShape direction, where m_oAAMMeanShape
        // has already been normalized to 1.
        ts = this->m_oAAMMeanShape * this->m_vAAMAlignedShapes[i];
        this->m_vAAMAlignedShapes[i].Scale( 1./ts );
    }

    // calculate the mean shape of the aligned shapes after project into tangent space 
    this->LV_MeanShape( this->m_oAAMMeanShape );

    for(unsigned int i=0; i < this->m_iNbOfSamples; i++)
    {
        for(unsigned int j = 0; j < this->m_iNbOfPoints; j++)
        {
            CV_MAT_ELEM( *this->m_CVMAlignedShapes, double, i, 2*j ) = (double)(this->m_vAAMAlignedShapes[i].m_vPoint[j].m_CVxy.x);
            CV_MAT_ELEM( *this->m_CVMAlignedShapes, double, i, 2*j+1 ) = (double)(this->m_vAAMAlignedShapes[i].m_vPoint[j].m_CVxy.y);
        }
    }

}


/**

    @author     JIA Pei

    @version    2006-09-03

    @brief      Mean shape

    @output	    meanShape

*/

void lv_aambuilding::LV_MeanShape(lv_aamshape2d &meanShape)
{
    meanShape = 0;

    for(unsigned int i = 0; i < this->m_iNbOfSamples; i++)
    {
        meanShape += this->m_vAAMAlignedShapes[i];
    }

    meanShape = meanShape / this->m_iNbOfSamples;

}



/**

    @author     JIA Pei

    @version    2006-09-01

    @brief      build AAM texture model

*/
void lv_aambuilding::LV_BuildAAMTextureModel()
{
    this->LV_BuildTemplateMesh();

    this->LV_LoadTexture2Normalized();

    // Note by JIA Pei, in fact, for Lucas-Kanade, we don't need the normalized texture,
    // but just the m_oAAMReferenceTexture is ok to perform the L-K alignment
    this->LV_TemplateFaceGradient();
    this->LV_SteepestDescentImages();
    this->LV_HessianMatrix();
}



// Explained by JIA Pei, 2006-09-15
// calculate mask image
// For 58 points dataset, 19 edges on the convex hull.
// 133 inner edges in(side) the convex hull; 133+19=152 edges totally
// (133*2+19)/3 =95 triangles totally -- because every inner edge belongs to two triangles,
// that is, this edge is used twice.
void lv_aambuilding::LV_BuildTemplateMesh()
{
    this->m_oAAMReferenceShape = this->m_oAAMMeanShape * this->m_dAverageSize;
    CvSize SizeOfFaceTemplate;
    SizeOfFaceTemplate.width = (int)(this->m_oAAMReferenceShape.MaxX() - this->m_oAAMReferenceShape.MinX()) + 1;// + TemplateMarginX;
    SizeOfFaceTemplate.height = (int)(this->m_oAAMReferenceShape.MaxY() - this->m_oAAMReferenceShape.MinY()) + 1;// + TemplateMarginY;
    
    this->FaceTemplate = cvCreateImage (SizeOfFaceTemplate, IPL_DEPTH_8U, 3);
    cvZero(FaceTemplate);

    // move shape to original // Actually, this is moving the "m_oAAMReferenceShape"
    // to its top left most point!! It's ok that "m_oAAMReferenceShape" and
    // other shapes are not at the same center!!!!!!!!!!
    this->m_oAAMReferenceShape.Translate( -this->m_oAAMReferenceShape.MinX(), -this->m_oAAMReferenceShape.MinY() );

    this->m_CVMPoints = cvCreateMat (1, this->m_iNbOfPoints, CV_32FC2);
    this->m_CVMConvexHull = cvCreateMat (1, this->m_iNbOfPoints, CV_32FC2);

    for( unsigned int i = 0; i < this->m_iNbOfPoints; i++ )
    {
        CV_MAT_ELEM( *this->m_CVMPoints, CvPoint2D32f, 0, i ) = cvPoint2D32f(
            this->m_oAAMReferenceShape.m_vPoint[i].m_CVxy.x, 
            this->m_oAAMReferenceShape.m_vPoint[i].m_CVxy.y);
    }

    cvConvexHull2(this->m_CVMPoints, this->m_CVMConvexHull, CV_CLOCKWISE, 0 );
  
    CvRect rect = cvBoundingRect( this->m_CVMConvexHull, 0 );

    CvMemStorage* DelaunayStorage = cvCreateMemStorage(0);

    // By JIA Pei, 2006-09-20. How to release this storage?
    this->m_CVSubdiv = cvCreateSubdivDelaunay2D( rect, DelaunayStorage );

    for( unsigned int i = 0; i < this->m_iNbOfPoints; i++ )
    {
        cvSubdivDelaunay2DInsert( this->m_CVSubdiv, CV_MAT_ELEM( *this->m_CVMPoints, CvPoint2D32f, 0, i ));
    }

    this->LV_BuildEdges ();

    this->LV_BuildTriangles ();

    this->LV_TextureTriangles ();

    cvReleaseMemStorage( &DelaunayStorage );

}





void lv_aambuilding::LV_BuildEdges()
{
    // The following display is correct!!
    CvSeqReader  reader;

    cvStartReadSeq( (CvSeq*)(this->m_CVSubdiv->edges), &reader, 0 );

    for( unsigned int i = 0; i < this->m_CVSubdiv->edges->total; i++ )
    {
        CvQuadEdge2D* edge = (CvQuadEdge2D*)(reader.ptr);

        if( CV_IS_SET_ELEM( edge ))
        {
            CvPoint2D32f org;
            CvPoint2D32f dst;

            CvSubdiv2DPoint* org_pt = cvSubdiv2DEdgeOrg((CvSubdiv2DEdge)edge);
            CvSubdiv2DPoint* dst_pt = cvSubdiv2DEdgeDst((CvSubdiv2DEdge)edge);

            if( org_pt && dst_pt )
            {
                org = org_pt->pt;
                dst = dst_pt->pt;

                // The following LV_IsPointInConvexHull is of no use, just to make sure
                // the point is in the convex hull. Actually, org and dst must be in the convex hull
                if ( this->LV_IsPointInConvexHull(org, this->m_CVMConvexHull, true) && 
                    this->LV_IsPointInConvexHull(dst, this->m_CVMConvexHull, true) )
                {
                    for (unsigned int j = 0; j < this->m_iNbOfPoints; j++)
                    {
                        // Explained by JIA Pei, actually, shouldn't use "==" for float data, 
                        // should use fabs(a-b)<1e-6 or something alike, here, I'm lazy.
                        if ( (org.x == CV_MAT_ELEM( *this->m_CVMPoints, CvPoint2D32f, 0, j ).x )
                            && (org.y == CV_MAT_ELEM( *this->m_CVMPoints, CvPoint2D32f, 0, j ).y ) )
                        {
                            for (unsigned int k = 0; k < this->m_iNbOfPoints; k++)
                            {
                                if ( (dst.x == CV_MAT_ELEM( *this->m_CVMPoints, CvPoint2D32f, 0, k ).x )
                                    && (dst.y == CV_MAT_ELEM( *this->m_CVMPoints, CvPoint2D32f, 0, k ).y ) )
                                {
                                    // Already tested, this->m_vAAMEdge is definitely correct!
                                    this->m_vAAMEdge.push_back (lv_aamedge(j,k));
                                }
                            }
                        }
                    }
                }
            }
        }

        CV_NEXT_SEQ_ELEM( this->m_CVSubdiv->edges->elem_size, reader );
    }
}



bool lv_aambuilding::LV_IsPointInConvexHull(CvPoint2D32f pt, CvMat* ch, bool includinghull)
{
    if(!includinghull)
    {
        if (cvPointPolygonTest( ch, pt, 0 ) > 0)
            return true;
    }
    else
    {
        if (cvPointPolygonTest( ch, pt, 0 ) >= 0)
            return true;
    }
    return false;
}



void lv_aambuilding::LV_BuildTriangles()
{
    for (unsigned int i = 0; i < this->m_vAAMEdge.size (); i++)
    {
        int ind1 = this->m_vAAMEdge[i].index1;
        int ind2 = this->m_vAAMEdge[i].index2;

        for (unsigned int j = 0; j < this->m_iNbOfPoints; j++)
        {
            // At most, there are only 2 triangles that can be added 
            if( this->LV_IsCurrentEdgeAlreadyInAAM(ind1, j) && this->LV_IsCurrentEdgeAlreadyInAAM(ind2, j) )
            {
                vector<CvPoint2D32f> vVertexes;
                vector<int> iVertexes;
                vVertexes.resize (3);
                iVertexes.resize (3);
                vVertexes[0] = CV_MAT_ELEM( *this->m_CVMPoints, CvPoint2D32f, 0, ind1 );
                vVertexes[1] = CV_MAT_ELEM( *this->m_CVMPoints, CvPoint2D32f, 0, ind2 );
                vVertexes[2] = CV_MAT_ELEM( *this->m_CVMPoints, CvPoint2D32f, 0, j );
                iVertexes[0] = ind1;
                iVertexes[1] = ind2;
                iVertexes[2] = j;
                if (this->LV_VectorIsNotInFormerTriangles(iVertexes) )
                {
                    this->m_vAAMTriangle2D.push_back (lv_aamtriangle2d(vVertexes, iVertexes));
                }
            }
        }
    }
}


bool lv_aambuilding::LV_IsCurrentEdgeAlreadyInAAM(int ind1, int ind2)
{
    for (unsigned int i = 0; i < this->m_vAAMEdge.size (); i++)
    {
        if ( ( (this->m_vAAMEdge[i].index1 == ind1) && (this->m_vAAMEdge[i].index2 == ind2) )
            || ( (this->m_vAAMEdge[i].index1 == ind2) && (this->m_vAAMEdge[i].index2 == ind1) ) )
        {
            return true;
        }
    }

    return false;
}




bool lv_aambuilding::LV_VectorIsNotInFormerTriangles(const vector<int> v)
{
    set<int> tTriangle;
    set<int> sTriangle;

    for (unsigned int i = 0; i < this->m_vAAMTriangle2D.size (); i ++)
    {
        // These two clear() are very important, cannot be displaced by empty().
        tTriangle.clear ();
        sTriangle.clear ();
        for (unsigned int j = 0; j < 3; j++ )
        {
            tTriangle.insert (this->m_vAAMTriangle2D[i].m_iIndexes[j]);
            sTriangle.insert (v[j]);
        }
        if (tTriangle == sTriangle)
        {
            return false;
        }
    }

    return true;
}


// Explained by JIA Pei, 2006-09-15
// 31461 points (so, if taking R, G, B into consideration, there should be 
// 31461*3 = 94383 features totally.) are respectively in 95 triangles.
void lv_aambuilding::LV_TextureTriangles()
{
    this->m_iNbOfTextures = 0;

    CvRect rect = cvBoundingRect( this->m_CVMConvexHull, 0 );

    for (unsigned int i = 0; i < rect.height; i++)
    {
        for (unsigned int j = 0; j < rect.width; j++)
        {
            CvPoint2D32f pt;
            // JIA Pei. 2006-11-25. You will see the following (int) is very important
            // without (int), the result here is not correct at all!!
            pt.x = (int)j;
            pt.y = (int)i;
            if( this->LV_IsPointInConvexHull(pt, this->m_CVMConvexHull, false) )
            {
                CvMat* tempVert = cvCreateMat (1, 3, CV_32FC2);
                for (unsigned int k = 0; k < this->m_vAAMTriangle2D.size (); k++)
                {
                    CV_MAT_ELEM( *tempVert, CvPoint2D32f, 0, 0 ) = this->m_vAAMTriangle2D[k].m_vVertexes[0];
                    CV_MAT_ELEM( *tempVert, CvPoint2D32f, 0, 1 ) = this->m_vAAMTriangle2D[k].m_vVertexes[1];
                    CV_MAT_ELEM( *tempVert, CvPoint2D32f, 0, 2 ) = this->m_vAAMTriangle2D[k].m_vVertexes[2];
                    
                    // Here, we can actually give out all the local variables except the BGR, 
                    // the BGR is not shape based and will be given by function LV_LoadTexture2Normalized()
                    if( cvPointPolygonTest( tempVert, pt, 0 ) >= 0)
                    {
                        vector<int> Index;
                        Index.resize(3);
                        Index[0] = this->m_vAAMTriangle2D[k].m_iIndexes[0];
                        Index[1] = this->m_vAAMTriangle2D[k].m_iIndexes[1];
                        Index[2] = this->m_vAAMTriangle2D[k].m_iIndexes[2];

                        lv_aamwarping temptextri;
                        temptextri.m_CVPosition = pt;
                        temptextri.m_iTriangleIndex = k;
                        temptextri.m_iPointIndex = this->m_iNbOfTextures;
                        temptextri.SetVertIndexes(tempVert, Index);

                        // for a certain temptextri.m_CVPosition, the Jacobian[i] are different
                        temptextri.CalcJacobianOne();
                        
                        temptextri.m_JacobianMatrix.resize (2);
                        temptextri.m_JacobianMatrix[0].resize(this->m_CVMTruncatedAlignedShapesEigenVectors->rows);
                        temptextri.m_JacobianMatrix[1].resize(this->m_CVMTruncatedAlignedShapesEigenVectors->rows);

                        for (unsigned int np = 0; np < this->m_CVMTruncatedAlignedShapesEigenVectors->rows; np++)
                        {
                            temptextri.m_JacobianMatrix[0][np] = temptextri.m_Jacobian_One[0] 
                                                                * CV_MAT_ELEM( *this->m_CVMTruncatedAlignedShapesEigenVectors, 
                                                                    double, np, temptextri.m_oAAMTriangle2d.m_iIndexes[0] * 2)
                                                                + temptextri.m_Jacobian_One[1] 
                                                                * CV_MAT_ELEM( *this->m_CVMTruncatedAlignedShapesEigenVectors, 
                                                                    double, np, temptextri.m_oAAMTriangle2d.m_iIndexes[1] * 2)
                                                                + temptextri.m_Jacobian_One[2] 
                                                                * CV_MAT_ELEM( *this->m_CVMTruncatedAlignedShapesEigenVectors, 
                                                                    double, np, temptextri.m_oAAMTriangle2d.m_iIndexes[2] * 2);
                            temptextri.m_JacobianMatrix[1][np] = temptextri.m_Jacobian_One[0] 
                                                                * CV_MAT_ELEM( *this->m_CVMTruncatedAlignedShapesEigenVectors, 
                                                                    double, np, temptextri.m_oAAMTriangle2d.m_iIndexes[0] * 2 + 1)
                                                                + temptextri.m_Jacobian_One[1] 
                                                                * CV_MAT_ELEM( *this->m_CVMTruncatedAlignedShapesEigenVectors, 
                                                                    double, np, temptextri.m_oAAMTriangle2d.m_iIndexes[1] * 2 + 1)
                                                                + temptextri.m_Jacobian_One[2] 
                                                                * CV_MAT_ELEM( *this->m_CVMTruncatedAlignedShapesEigenVectors, 
                                                                    double, np, temptextri.m_oAAMTriangle2d.m_iIndexes[2] * 2 + 1);
                        }

                        // Very important
                        // Note by JIA Pei, push_back cannot perform on 2-D vectors
                        this->m_vTextureTriangle.push_back (temptextri);
                    }
                }
                this->m_iNbOfTextures ++;
                cvReleaseMat (&tempVert);
            }
        }
    }

     this->m_iNbOfTextures *= 3;
}



void lv_aambuilding::LV_LoadTexture2Normalized()
{
    CvPoint2D32f* src = new CvPoint2D32f[3];
    CvPoint2D32f* dst = new CvPoint2D32f[3];
    CvMat* map_matrix = cvCreateMat (2, 3, CV_64FC1);

    CvMat* MappedPixel = cvCreateMat (2, 1, CV_64FC1);
    CvMat* TemplatePixel= cvCreateMat (3, 1, CV_64FC1);

    this->m_vAAMTextures.resize (this->m_iNbOfSamples);

    for(unsigned int i = 0; i < this->m_iNbOfSamples; i++)
    {
        for (unsigned int j = 0; j < this->m_vTextureTriangle.size (); j++ )
        {
            // Get the affine transformation for each triangle pair.
            src[0] = this->m_vAAMTriangle2D[this->m_vTextureTriangle[j].m_iTriangleIndex].m_vVertexes[0];
            src[1] = this->m_vAAMTriangle2D[this->m_vTextureTriangle[j].m_iTriangleIndex].m_vVertexes[1];
            src[2] = this->m_vAAMTriangle2D[this->m_vTextureTriangle[j].m_iTriangleIndex].m_vVertexes[2];

            dst[0].x = (float) CV_MAT_ELEM( *this->m_CVMShape, double, i, 
                2 * this->m_vAAMTriangle2D[this->m_vTextureTriangle[j].m_iTriangleIndex].m_iIndexes[0] );
            dst[0].y = (float) CV_MAT_ELEM( *this->m_CVMShape, double, i, 
                2 * this->m_vAAMTriangle2D[this->m_vTextureTriangle[j].m_iTriangleIndex].m_iIndexes[0] + 1);
            dst[1].x = (float) CV_MAT_ELEM( *this->m_CVMShape, double, i, 
                2 * this->m_vAAMTriangle2D[this->m_vTextureTriangle[j].m_iTriangleIndex].m_iIndexes[1] );
            dst[1].y = (float) CV_MAT_ELEM( *this->m_CVMShape, double, i, 
                2 * this->m_vAAMTriangle2D[this->m_vTextureTriangle[j].m_iTriangleIndex].m_iIndexes[1] + 1);
            dst[2].x = (float) CV_MAT_ELEM( *this->m_CVMShape, double, i, 
                2 * this->m_vAAMTriangle2D[this->m_vTextureTriangle[j].m_iTriangleIndex].m_iIndexes[2] );
            dst[2].y = (float) CV_MAT_ELEM( *this->m_CVMShape, double, i, 
                2 * this->m_vAAMTriangle2D[this->m_vTextureTriangle[j].m_iTriangleIndex].m_iIndexes[2] + 1);

            cvGetAffineTransform( src, dst, map_matrix );

            // warp from m_vAAMTriangle2D[j] (template pixel in the specific triangle) to each shape/image
            CV_MAT_ELEM( *TemplatePixel, double, 0, 0) = this->m_vTextureTriangle[j].m_CVPosition.x;
            CV_MAT_ELEM( *TemplatePixel, double, 1, 0) = this->m_vTextureTriangle[j].m_CVPosition.y;
            CV_MAT_ELEM( *TemplatePixel, double, 2, 0) = 1.0;

            cvMatMul( map_matrix, TemplatePixel, MappedPixel );

            // extract the B,G,R on each shape/image respectively into textures
            // CV_MAT_ELEM( *MappedPixel, double, 0, 0) is for x while
            // CV_MAT_ELEM( *MappedPixel, double, 1, 0) is for y. But, 
            // x determines cols, while y determines rows.
            vector<double> OnePixelBGR = this->LV_GetSubPixelInto3Channels (
                CV_MAT_ELEM( *MappedPixel, double, 0, 0), 
                CV_MAT_ELEM( *MappedPixel, double, 1, 0), this->m_vImages[i]);

            this->m_vAAMTextures[i].m_vTextures.push_back(OnePixelBGR[0]);
            this->m_vAAMTextures[i].m_vTextures.push_back(OnePixelBGR[1]);
            this->m_vAAMTextures[i].m_vTextures.push_back(OnePixelBGR[2]);

        }
    }

    // Till now it's correct.
    cvReleaseMat (&map_matrix);
    cvReleaseMat (&MappedPixel);
    cvReleaseMat (&TemplatePixel);
    delete src;
    delete dst;

    this->LV_NormalizeTexture ();

}





// Noted by JIA Pei, 2006-09-15. Make sure the x, y relations!
// In the pictures, x represents cols, y represents rows!
// while in IplImage* image, x represents cols, y represents rows as well!!!
vector<double> lv_aambuilding::LV_GetSubPixelInto3Channels(double x, double y, const IplImage* image)
{
    vector<double> result;

    // Normally, the nChannels == 3!!!
    result.resize ( 3 );

    register int Y = (int)y;
    register int X = (int)x;

    register double s=x-X;
    register double t=y-Y;

    // BILINEAR interpolation
    vector<unsigned char> LeftTop        =   lv_aambuilding::LV_GetPixelInto3Channels(X, Y, image);
    vector<unsigned char> LeftBottom     =   lv_aambuilding::LV_GetPixelInto3Channels(X , Y + 1, image);
    vector<unsigned char> RightTop       =   lv_aambuilding::LV_GetPixelInto3Channels(X + 1, Y, image);
    vector<unsigned char> RightBottom    =   lv_aambuilding::LV_GetPixelInto3Channels(X + 1, Y + 1, image);

    vector<double> tempA, tempB;
    tempA.resize (3);
    tempB.resize (3);

    for (int i = 0; i < 3; i++)
    {
        tempA[i] += (1-t) * LeftTop[i] + t * LeftBottom[i];
        tempB[i] += (1-t) * RightTop[i] + t * RightBottom[i];
    }

    result[0] = tempA[0] * (1.-s) + tempB[0] * s;
    result[1] = tempA[1] * (1.-s) + tempB[1] * s;
    result[2] = tempA[2] * (1.-s) + tempB[2] * s;

    return result;
}


// Explained by JIA Pei, always remember OpenCV gets into image pixel like this.

// I(x,y)blue ~ ((uchar*)(img->imageData + img->widthStep*y))[x*3]
// I(x,y)green ~ ((uchar*)(img->imageData + img->widthStep*y))[x*3+1]
// I(x,y)red ~ ((uchar*)(img->imageData + img->widthStep*y))[x*3+2]

// OpenCV image is starting origin from topleft.


vector<unsigned char> lv_aambuilding::LV_GetPixelInto3Channels(int x, int y, const IplImage* image)
{
    vector<unsigned char> result;

    // Normally, the nChannels == 3!!!
    if (image->nChannels == 3)
    {
        result.push_back ( ((uchar*)(image->imageData + image->widthStep*y))[x*3] );
        result.push_back ( ((uchar*)(image->imageData + image->widthStep*y))[x*3 + 1] );
        result.push_back ( ((uchar*)(image->imageData + image->widthStep*y))[x*3 + 2] );
    }
    // if nChannels == 1, copy 3 times! So, that is to say, we can solve gray-level images as well!!
    else if (image->nChannels == 1)
    {
        result.push_back ( ((uchar*)(image->imageData + image->widthStep*y))[x*3] );
        result.push_back ( ((uchar*)(image->imageData + image->widthStep*y))[x*3] );
        result.push_back ( ((uchar*)(image->imageData + image->widthStep*y))[x*3] );
    }

    return result;
}



// This function is called only once in the aam building project.
void lv_aambuilding::LV_NormalizeTexture()
{
    this->LV_MeanTexture( this->m_oAAMMeanTexture );

    // Get the image FaceTemplate, for alignment!
    int PixelsInFaceTemplate = 0;
    for (unsigned int y = 0; y < this->FaceTemplate->height; y++)
    {
        for (unsigned int x = 0; x < this->FaceTemplate->width; x++)
        {
            CvPoint2D32f pt = cvPoint2D32f(x , y);

            if( lv_aambuilding::LV_IsPointInConvexHull(pt, this->m_CVMConvexHull, true) )
            {
                CV_IMAGE_ELEM(this->FaceTemplate, unsigned char, y, x * 3 + 0) = 
                    this->m_oAAMMeanTexture.m_vTextures[PixelsInFaceTemplate++];
                CV_IMAGE_ELEM(this->FaceTemplate, unsigned char, y, x * 3 + 1) = 
                    this->m_oAAMMeanTexture.m_vTextures[PixelsInFaceTemplate++];
                CV_IMAGE_ELEM(this->FaceTemplate, unsigned char, y, x * 3 + 2) = 
                    this->m_oAAMMeanTexture.m_vTextures[PixelsInFaceTemplate++];
            }
        }
    }
    

    // We can just remember the mean texture of the 1st time, so that it's convenient 
    // for us to roughly back project from the normalized textures.
    // Actually, the 1st time mean texture is just the mean texture of the 
    // training dataset. It's cool!!
    this->m_oAAMReferenceTexture = this->m_oAAMMeanTexture;

    cvSaveImage( "Reference.jpg", FaceTemplate );

}



void lv_aambuilding::LV_ZeroMeanUnitLengthOneTexture(lv_aamtexture2d3c &onetexture)
{
    double mean = 0.;
    double sqsum = 0.;

    for (unsigned int i = 0; i < onetexture.m_vTextures.size(); i++)
    {
        mean    += onetexture.m_vTextures[i];
    }
    mean    /=  onetexture.m_vTextures.size();

    for (unsigned int i = 0; i < onetexture.m_vTextures.size(); i++)
    {
        onetexture.m_vTextures[i]  -= mean;
        sqsum += onetexture.m_vTextures[i] * onetexture.m_vTextures[i];
    }

    double a = 1./sqrt(sqsum);
    assert(a != 0.);

    for (unsigned int i = 0; i < onetexture.m_vTextures.size(); i++)
    {
        onetexture.m_vTextures[i] *= a;
    }
}


void lv_aambuilding::LV_ZeroMeanUnitLengthOneTexture(CvMat* onetexture)
{
    double mean = 0.;
    double sqsum = 0.;

    for (unsigned int i = 0; i < onetexture->cols; i++)
    {
        mean += CV_MAT_ELEM( *onetexture, double, 0, i );
    }
    mean    /=  onetexture->cols;

    for (unsigned int i = 0; i < onetexture->cols; i++)
    {
        CV_MAT_ELEM( *onetexture, double, 0, i ) -= mean;
        sqsum += CV_MAT_ELEM( *onetexture, double, 0, i ) * CV_MAT_ELEM( *onetexture, double, 0, i );
    }

    double a = 1./sqrt(sqsum);
    assert(a!=0.);

    for (unsigned int i = 0; i < onetexture->cols; i++)
    {
        CV_MAT_ELEM( *onetexture, double, 0, i ) *= a;
    }
}



void lv_aambuilding::LV_MeanTexture(lv_aamtexture2d3c &meanTexture)
{
    vector<double> sum;
    sum.resize(this->m_iNbOfTextures);  // initialize to zeros already

    meanTexture.m_vTextures.resize (this->m_iNbOfTextures);

    for (unsigned int i = 0; i < this->m_iNbOfSamples; i++)
    {
        for (unsigned int j = 0; j < this->m_iNbOfTextures; j++)
        {
            sum[j] += this->m_vAAMTextures[i].m_vTextures[j];
        }
    }

    for (unsigned int i = 0; i < this->m_iNbOfTextures; i++)
    {
        meanTexture.m_vTextures[i] = sum[i] / this->m_iNbOfSamples;
    }
}



// In the pictures, x represents cols, y represents rows!
// while in IplImage* image, x represents cols, y represents rows as well!!!
// Refer to LV_LoadTexture2Normalized ()
// vector<unsigned char> LeftTop        =   lv_aambuilding::LV_GetPixelInto3Channels(X, Y, image);
// vector<unsigned char> LeftBottom     =   lv_aambuilding::LV_GetPixelInto3Channels(X , Y + 1, image);
// vector<unsigned char> RightTop       =   lv_aambuilding::LV_GetPixelInto3Channels(X + 1, Y, image);
// vector<unsigned char> RightBottom    =   lv_aambuilding::LV_GetPixelInto3Channels(X + 1, Y + 1, image);

void lv_aambuilding::LV_TemplateFaceGradient ()
{
    vector<double> RenferenceGrayTexture;
    RenferenceGrayTexture.resize ( this->m_vTextureTriangle.size () );

    // Calculate the Gray Level for each pixel first.
    for (unsigned int i = 0; i < this->m_vTextureTriangle.size (); i++)     // 31461
    {
        double blue     = this->m_oAAMReferenceTexture.m_vTextures[i * 3 + 0];
        double green    = this->m_oAAMReferenceTexture.m_vTextures[i * 3 + 1];
        double red      = this->m_oAAMReferenceTexture.m_vTextures[i * 3 + 2];

        // Refer to OpenCV documentation cvCvtColor
        // RGB[A]->Gray: Y <-   0.299*R + 0.587*G + 0.114*B
        RenferenceGrayTexture[i] = 0.299 * red + 0.587 * green + 0.114 * blue;
    }

    // Note by JIA Pei, in fact, for Lucas-Kanade, we don't need the normalized texture,
    // but just the m_oAAMReferenceTexture is ok to perform the L-K alignment
    // Here, we use a classical search method to do the gradient calculation
    for (unsigned int i = 0; i < this->m_vTextureTriangle.size (); i++)     // 31461
    {
        double dis = cvPointPolygonTest( this->m_CVMConvexHull, this->m_vTextureTriangle[i].m_CVPosition, 1 );
        
        // Here simply use 2^0.5 to justify whether we need to calculate the 
        // x/y gradients for this point. Or else, leave it as "0"
        if (fabs(dis) - 1.4142135623731 >= 1e-15 )
        {
            // x refer to column; while y refer to row.
            double xPos        = this->m_vTextureTriangle[i].m_CVPosition.x;
            double yPos        = this->m_vTextureTriangle[i].m_CVPosition.y;

            // Start searching for the top bottom left right point index in this->m_vTextureTriangle
            vector<int>     tblr;
            tblr.resize (4);
            for (unsigned int j = 0; j < this->m_vTextureTriangle.size (); j++)     // 31461
            {
                // top
                if ( (this->m_vTextureTriangle[j].m_CVPosition.x == xPos) &&
                    (this->m_vTextureTriangle[j].m_CVPosition.y == yPos - 1) )
                {
                    tblr[0] = j;
                }
                // bottom
                if ( (this->m_vTextureTriangle[j].m_CVPosition.x == xPos) &&
                    (this->m_vTextureTriangle[j].m_CVPosition.y == yPos + 1) )
                {
                    tblr[1] = j;
                }
                // left
                if ( (this->m_vTextureTriangle[j].m_CVPosition.x == xPos - 1) &&
                    (this->m_vTextureTriangle[j].m_CVPosition.y == yPos) )
                {
                    tblr[2] = j;
                }
                // right
                if ( (this->m_vTextureTriangle[j].m_CVPosition.x == xPos + 1) &&
                    (this->m_vTextureTriangle[j].m_CVPosition.y == yPos) )
                {
                    tblr[3] = j;
                }
            }

            // Calculate the gradient
            this->m_vTextureTriangle[i].m_Gradients[0][0] = 
                (this->m_oAAMReferenceTexture.m_vTextures[tblr[3] * 3 + 0] 
                - this->m_oAAMReferenceTexture.m_vTextures[tblr[2] * 3 + 0])/2;    // for x
            this->m_vTextureTriangle[i].m_Gradients[0][1] = 
                (this->m_oAAMReferenceTexture.m_vTextures[tblr[1] * 3 + 0] 
                - this->m_oAAMReferenceTexture.m_vTextures[tblr[0] * 3 + 0])/2;    // for y

            this->m_vTextureTriangle[i].m_Gradients[1][0] = 
                (this->m_oAAMReferenceTexture.m_vTextures[tblr[3] * 3 + 1] 
                - this->m_oAAMReferenceTexture.m_vTextures[tblr[2] * 3 + 1])/2;    // for x
            this->m_vTextureTriangle[i].m_Gradients[1][1] = 
                (this->m_oAAMReferenceTexture.m_vTextures[tblr[1] * 3 + 1] 
                - this->m_oAAMReferenceTexture.m_vTextures[tblr[0] * 3 + 1])/2;    // for y

            this->m_vTextureTriangle[i].m_Gradients[2][0] = 
                (this->m_oAAMReferenceTexture.m_vTextures[tblr[3] * 3 + 2] 
                - this->m_oAAMReferenceTexture.m_vTextures[tblr[2] * 3 + 2])/2;    // for x
            this->m_vTextureTriangle[i].m_Gradients[2][1] = 
                (this->m_oAAMReferenceTexture.m_vTextures[tblr[1] * 3 + 2] 
                - this->m_oAAMReferenceTexture.m_vTextures[tblr[0] * 3 + 2])/2;    // for y

            this->m_vTextureTriangle[i].m_Gradients[3][0] = 
                (RenferenceGrayTexture[tblr[3]] - RenferenceGrayTexture[tblr[2]])/2;    // for x
            this->m_vTextureTriangle[i].m_Gradients[3][1] = 
                (RenferenceGrayTexture[tblr[1]] - RenferenceGrayTexture[tblr[0]])/2;    // for y
        }
    }
}


void lv_aambuilding::LV_SteepestDescentImages()
{
    for (unsigned int i = 0; i < this->m_vTextureTriangle.size (); i++)
    {
        this->m_vTextureTriangle[i].CalcSteepestDescentImages ();
    }

    this->m_SteepestDescentImages4All.resize (4);   // for (blue, green ,red, gray)

    for (unsigned int i = 0; i < this->m_SteepestDescentImages4All.size (); i++)
    {
        this->m_SteepestDescentImages4All[i].resize (this->m_vTextureTriangle.size ());
        for (unsigned int j = 0; j < this->m_SteepestDescentImages4All[i].size (); j++)
        {
            this->m_SteepestDescentImages4All[i][j].resize (this->m_vTextureTriangle[0].m_SteepestDescentImages[0].size () );
        }
    }

    for (unsigned int i = 0; i < this->m_vTextureTriangle.size (); i++)
    {
        for (unsigned int j = 0; j < this->m_SteepestDescentImages4All[0][0].size (); j++)
        {
            this->m_SteepestDescentImages4All[0][i][j] = this->m_vTextureTriangle[i].m_SteepestDescentImages[0][j];
            this->m_SteepestDescentImages4All[1][i][j] = this->m_vTextureTriangle[i].m_SteepestDescentImages[1][j];
            this->m_SteepestDescentImages4All[2][i][j] = this->m_vTextureTriangle[i].m_SteepestDescentImages[2][j];
            this->m_SteepestDescentImages4All[3][i][j] = this->m_vTextureTriangle[i].m_SteepestDescentImages[3][j];
        }
    }
}


void lv_aambuilding::LV_HessianMatrix()
{
    // HessianMatrix to zeros
    this->m_HessianMatrixInverse.resize(4);

    for (unsigned int i = 0; i < this->m_HessianMatrixInverse.size (); i++)
    {
        this->m_HessianMatrixInverse[i].resize (this->m_SteepestDescentImages4All[0][0].size ());
        for (unsigned int j = 0; j < this->m_HessianMatrixInverse[i].size (); j++)
        {            
            this->m_HessianMatrixInverse[i][j].resize (this->m_HessianMatrixInverse[i].size () );
        }
    }

    CvMat* HessianMatrix_Blue   = cvCreateMat ( this->m_HessianMatrixInverse[0].size(),
                                                this->m_HessianMatrixInverse[0].size(), CV_64FC1);
    CvMat* HessianMatrix_Green  = cvCreateMat ( this->m_HessianMatrixInverse[0].size(),
                                                this->m_HessianMatrixInverse[0].size(), CV_64FC1);
    CvMat* HessianMatrix_Red    = cvCreateMat ( this->m_HessianMatrixInverse[0].size(),
                                                this->m_HessianMatrixInverse[0].size(), CV_64FC1);
    CvMat* HessianMatrix_Gray   = cvCreateMat ( this->m_HessianMatrixInverse[0].size(),
                                                this->m_HessianMatrixInverse[0].size(), CV_64FC1);

    CvMat* HessianMatrix_Blue_Inv   = cvCreateMat ( this->m_HessianMatrixInverse[0].size(),
        this->m_HessianMatrixInverse[0].size(), CV_64FC1);
    CvMat* HessianMatrix_Green_Inv  = cvCreateMat ( this->m_HessianMatrixInverse[0].size(),
        this->m_HessianMatrixInverse[0].size(), CV_64FC1);
    CvMat* HessianMatrix_Red_Inv    = cvCreateMat ( this->m_HessianMatrixInverse[0].size(),
        this->m_HessianMatrixInverse[0].size(), CV_64FC1);
    CvMat* HessianMatrix_Gray_Inv   = cvCreateMat ( this->m_HessianMatrixInverse[0].size(),
        this->m_HessianMatrixInverse[0].size(), CV_64FC1);


    int HMSize = this->m_HessianMatrixInverse[0].size ();
    cvZero(HessianMatrix_Blue);
    cvZero(HessianMatrix_Green);
    cvZero(HessianMatrix_Red);
    cvZero(HessianMatrix_Gray);

    for (unsigned int i = 0; i < this->m_vTextureTriangle.size (); i++)
    {
        for (unsigned int j = 0; j < HMSize; j++)
        {
            for (unsigned int k = 0; k < HMSize; k++)
            {
                CV_MAT_ELEM(*HessianMatrix_Blue, double, j, k) += 
                    this->m_SteepestDescentImages4All[0][i][j] *
                    this->m_SteepestDescentImages4All[0][i][k];
                CV_MAT_ELEM(*HessianMatrix_Green, double, j, k) += 
                    this->m_SteepestDescentImages4All[1][i][j] *
                    this->m_SteepestDescentImages4All[1][i][k];
                CV_MAT_ELEM(*HessianMatrix_Red, double, j, k) += 
                    this->m_SteepestDescentImages4All[2][i][j] *
                    this->m_SteepestDescentImages4All[2][i][k];
                CV_MAT_ELEM(*HessianMatrix_Gray, double, j, k) += 
                    this->m_SteepestDescentImages4All[3][i][j] *
                    this->m_SteepestDescentImages4All[3][i][k];
            }
        }
    }
    vector <vector<vector < double> > > test;
    test.resize(4);
    for (unsigned int i = 0; i < test.size (); i++)
    {
        test[i].resize (HessianMatrix_Blue->rows);
        for (unsigned int j = 0; j < HessianMatrix_Blue->rows; j++)
        {
            test[i][j].resize (HessianMatrix_Blue->rows);
        }
    }

    for (unsigned int i = 0; i < HessianMatrix_Blue->rows; i++)
    {
        for (unsigned int j = 0; j < HessianMatrix_Blue->cols; j++)
        {
            test[0][i][j] = CV_MAT_ELEM(*HessianMatrix_Blue, double, i, j);
            test[1][i][j] = CV_MAT_ELEM(*HessianMatrix_Green, double, i, j);
            test[2][i][j] = CV_MAT_ELEM(*HessianMatrix_Red, double, i, j);
            test[3][i][j] = CV_MAT_ELEM(*HessianMatrix_Gray, double, i, j);
        }
    }
    
    cvInvert( HessianMatrix_Blue, HessianMatrix_Blue_Inv, CV_SVD );
    cvInvert( HessianMatrix_Green, HessianMatrix_Green_Inv, CV_SVD );
    cvInvert( HessianMatrix_Red, HessianMatrix_Red_Inv, CV_SVD );
    cvInvert( HessianMatrix_Gray, HessianMatrix_Gray_Inv, CV_SVD );

    for (unsigned int i = 0; i < HMSize; i++)
    {
        for (unsigned int j = 0; j < HMSize; j++)
        {            
            this->m_HessianMatrixInverse[0][i][j] = CV_MAT_ELEM(*HessianMatrix_Blue_Inv, double, i, j);
            this->m_HessianMatrixInverse[1][i][j] = CV_MAT_ELEM(*HessianMatrix_Green_Inv, double, i, j);
            this->m_HessianMatrixInverse[2][i][j] = CV_MAT_ELEM(*HessianMatrix_Red_Inv, double, i, j);
            this->m_HessianMatrixInverse[3][i][j] = CV_MAT_ELEM(*HessianMatrix_Gray_Inv, double, i, j);
        }
    }

    cvReleaseMat (&HessianMatrix_Blue);
    cvReleaseMat (&HessianMatrix_Green);
    cvReleaseMat (&HessianMatrix_Red);
    cvReleaseMat (&HessianMatrix_Gray);
    cvReleaseMat (&HessianMatrix_Blue_Inv);
    cvReleaseMat (&HessianMatrix_Green_Inv);
    cvReleaseMat (&HessianMatrix_Red_Inv);
    cvReleaseMat (&HessianMatrix_Gray_Inv);

}


/**
    @author	    JIA Pei

    @version	2006-10-01

    @brief		Performs PCA on the Concatenated AAM.

    @return	    The b-parameters for all training examples. 
                The format is a vector of b-parameter vectors.
*/
void lv_aambuilding::LV_BuildConcatenatedAAM()
{
    vector<double> b;

    this->m_CVMWeightsScale2Texture = cvCreateMat (
                            this->m_CVMTruncatedAlignedShapesEigenVectors->rows, 
                            this->m_CVMTruncatedAlignedShapesEigenVectors->rows, 
                            CV_64FC1);

    CvScalar SumOfEigenValues_Shape     = cvSum( this->m_CVMTruncatedAlignedShapesEigenValues );
    CvScalar SumOfEigenValues_Texture   = cvSum( this->m_CVMTruncatedTexturesEigenValues );

    double val = SumOfEigenValues_Texture.val[0] / SumOfEigenValues_Shape.val[0];
    
    cvSetIdentity( this->m_CVMWeightsScale2Texture, cvRealScalar(val) );

    // generate b-vectors for all elements in the training set
    for(int i = 0; i < this->m_iNbOfSamples; i++)
    {
        b.clear ();

        // calculate parameters
        this->LV_ShapeTexture2Concatenated( this->m_vAAMAlignedShapes[i], this->m_vAAMTextures[i], b );

        // add
        this->m_vAAMConcatenated.push_back (b);
    }

    this->m_CVMConcatenated = cvCreateMat(  this->m_vAAMConcatenated.size (), 
                                            this->m_vAAMConcatenated[0].size (), 
                                            CV_64FC1 );

    for(unsigned int i=0; i < this->m_vAAMConcatenated.size (); i++)
    {
        for(unsigned int j = 0; j < this->m_vAAMConcatenated[0].size (); j++)
        {
            CV_MAT_ELEM( *this->m_CVMConcatenated, double, i, j ) = this->m_vAAMConcatenated[i][j];
        }
    }

    int NbOfEigenAtMost = MIN(this->m_vAAMConcatenated.size (), this->m_vAAMConcatenated[0].size ());

    this->m_CVMConcatenatedEigenValues = cvCreateMat( 1, NbOfEigenAtMost, CV_64FC1 );
    this->m_CVMConcatenatedEigenVectors = cvCreateMat( NbOfEigenAtMost, this->m_vAAMConcatenated[0].size (), CV_64FC1 );
    this->m_CVMMeanConcatenated = cvCreateMat( 1, this->m_vAAMConcatenated[0].size (), CV_64FC1 );

    // do normal PCA
    cvCalcPCA( this->m_CVMConcatenated, this->m_CVMMeanConcatenated, 
        this->m_CVMConcatenatedEigenValues, this->m_CVMConcatenatedEigenVectors,
        CV_PCA_DATA_AS_ROW );

    CvScalar SumOfEigenValues = cvSum( this->m_CVMConcatenatedEigenValues );

    double limit =.0001 * SumOfEigenValues.val[0];

    int i = 0;
    for(i = 0; i < this->m_CVMConcatenatedEigenValues->cols; i++)
    {
        if ( CV_MAT_ELEM( *this->m_CVMConcatenatedEigenValues, double, 0, i ) < limit) break;
    }

    // NonZero EigenValues
    int NbOfNonZeroEigenValues = i;

    for(i = NbOfNonZeroEigenValues; i < this->m_CVMConcatenatedEigenValues->cols; i++)
    {
        CV_MAT_ELEM( *this->m_CVMConcatenatedEigenValues, double, 0, i ) = 0.;
    }

    SumOfEigenValues = cvSum( this->m_CVMConcatenatedEigenValues );
    double ps = .0;
    int np =0;

    for(unsigned int i = 0; i < NbOfNonZeroEigenValues; i++)
    {
        ps += CV_MAT_ELEM( *this->m_CVMConcatenatedEigenValues, double, 0, i );
        ++np;
        if( ps/SumOfEigenValues.val[0] >= TruncatePercentage) break;
    }

    this->m_CVMTruncatedConcatenatedEigenValues = cvCreateMat( 1, np, CV_64FC1);
    this->m_CVMTruncatedConcatenatedEigenVectors = cvCreateMat( np, b.size (), CV_64FC1);

    for(unsigned int i = 0; i < np; i++)
    {
        CV_MAT_ELEM( *this->m_CVMTruncatedConcatenatedEigenValues, double, 0, i )
            = CV_MAT_ELEM( *this->m_CVMConcatenatedEigenValues, double, 0, i );
    }

    for(unsigned int i = 0; i < np; i++)
    {
        for (unsigned int j = 0; j < this->m_CVMTruncatedConcatenatedEigenVectors->cols; j++)
        {
            CV_MAT_ELEM( *this->m_CVMTruncatedConcatenatedEigenVectors, double, i, j )
                = CV_MAT_ELEM( *this->m_CVMConcatenatedEigenVectors, double, i, j );
        }
    }

    // extract the shape part of the combined eigen vectors
    CvRect rects = cvRect( 0, 0,
                            this->m_CVMTruncatedAlignedShapesEigenValues->cols,
                            this->m_CVMTruncatedConcatenatedEigenVectors->rows);
    this->m_CVMQs = cvCreateMat (this->m_CVMTruncatedConcatenatedEigenVectors->rows,
                                this->m_CVMTruncatedAlignedShapesEigenValues->cols,
                                CV_64FC1);
    cvGetSubRect( this->m_CVMTruncatedConcatenatedEigenVectors, this->m_CVMQs, rects );

    // extract the texture part of the combined eigen vectors
    CvRect rectg = cvRect( 0, 0,
                            this->m_CVMTruncatedTexturesEigenValues->cols,
                            this->m_CVMTruncatedConcatenatedEigenVectors->rows);
    this->m_CVMQg = cvCreateMat (this->m_CVMTruncatedConcatenatedEigenVectors->rows,
                                this->m_CVMTruncatedTexturesEigenValues->cols,
                                 CV_64FC1);
    cvGetSubRect( this->m_CVMTruncatedConcatenatedEigenVectors, this->m_CVMQg, rectg );

}



void lv_aambuilding::LV_ShapeTexture2Concatenated( lv_aamshape2d iShape, lv_aamtexture2d3c iTexture, vector<double>& b )
{
    CvMat* b_s_temp = cvCreateMat (1, this->m_CVMTruncatedAlignedShapesEigenValues->cols, CV_64FC1);
    CvMat* b_s = cvCreateMat (1, this->m_CVMTruncatedAlignedShapesEigenValues->cols, CV_64FC1);
    CvMat* b_t = cvCreateMat (1, this->m_CVMTruncatedTexturesEigenValues->cols, CV_64FC1);

    this->LV_ShapeProject2TruncatedEigenSpace(iShape, b_s_temp);    // 1*116->1*20

    vector<double> v_b_s_temp;
    v_b_s_temp.resize(b_s_temp->cols);
    for (unsigned int i = 0; i < v_b_s_temp.size(); i++)
    {
        v_b_s_temp[i] =  CV_MAT_ELEM( *b_s_temp, double, 0, i );
    }

    double ttttt = CV_MAT_ELEM( *this->m_CVMWeightsScale2Texture, double, 0, 0 );

    cvMatMul( b_s_temp, this->m_CVMWeightsScale2Texture, b_s );     // 1*20->1*20, just for rescaling

    vector<double> v_b_s;
    v_b_s.resize(b_s->cols);
    for (unsigned int i = 0; i < v_b_s.size(); i++)
    {
        v_b_s[i] =  CV_MAT_ELEM( *b_s, double, 0, i );
    }

    this->LV_TextureProject2TruncatedEigenSpace(iTexture, b_t);     // 1*94383->1*116

    b.clear();

    for (unsigned int i = 0; i < b_s->cols; i++)
    {
        b.push_back ( CV_MAT_ELEM( *b_s, double, 0, i ) );
    }

    for (unsigned int i = 0; i < b_t->cols; i++)
    {
        b.push_back ( CV_MAT_ELEM( *b_t, double, 0, i ) );
    }

    // b is of 1*136
    cvReleaseMat ( &b_s_temp );
    cvReleaseMat ( &b_s );
    cvReleaseMat ( &b_t );

}



void lv_aambuilding::LV_ShapeProject2TruncatedEigenSpace(const lv_aamshape2d& iShape, CvMat* projectedTruncatedShape)
{
    // allocation
    CvMat* currentshape = cvCreateMat (1, iShape.m_vPoint.size() * 2, CV_64FC1);

    // vector to CvMat
    iShape.Point2CvMat(currentshape);

    // project current input texture to the projected truncated texture space
    cvProjectPCA( currentshape, this->m_CVMMeanShape, this->m_CVMAlignedShapesEigenVectors, projectedTruncatedShape );

    cvReleaseMat (&currentshape);

}




void lv_aambuilding::LV_TextureProject2TruncatedEigenSpace(const lv_aamtexture2d3c& iTexture, CvMat* projectedTruncatedTexture)
{
    // allocation
    CvMat* currenttexture = cvCreateMat (1, iTexture.m_vTextures.size(), CV_64FC1);

    // vector to CvMat
    iTexture.Vector2CvMat(currenttexture);

    // project current input texture to the projected truncated texture space
    cvProjectPCA( currenttexture, this->m_CVMMeanTexture, this->m_CVMTexturesEigenVectors, projectedTruncatedTexture );

    cvReleaseMat (&currenttexture);
}



// We don't actually need to build the synthetic images.

void lv_aambuilding::BuildRegressionMatrices()
{
    for (unsigned int i = 0; i <this->m_vImages.size (); i++ )
    {
        if (this->m_vImages[i])
        {
            cvReleaseImage (&this->m_vImages[i]);
        }
    }

    // convert b to c parameters // Can use cvProjectPCA() as well
    // Tested, the following function is correct!!!
    vector<vector<double> > cVectors = this->LV_Concatenated2Parameters( );

    // Explained by JIA Pei. 2006-10-20
    // We cannot simply use cvInvert() to get the multivariate linear regression matrix,
    // because it needs too big a memory to calculate. 
    // We have to follow the reduced rank multivariate linear regression way.
    // Actually, we transform the huge matrix from 240*94383 to 240*15
    this->m_CVDeltaC = cvCreateMat (cVectors.size (), cVectors[0].size (), CV_64FC1);
    for(unsigned int i = 0; i < cVectors.size (); i++)
    {
        for (unsigned int j = 0; j < cVectors[0].size (); j ++)
        {
            CV_MAT_ELEM( *this->m_CVDeltaC, double, i , j) = cVectors[i][j];
        }
    }

    // 15*240
    CvMat* DeltaCTranspose = cvCreateMat (this->m_CVDeltaC->cols, this->m_CVDeltaC->rows, CV_64FC1);
    cvTranspose(this->m_CVDeltaC, DeltaCTranspose);

    // 240*94383
    this->m_CVDeltaG = cvCreateMat (this->m_iNbOfSamples, this->m_iNbOfTextures, CV_64FC1);

    for(unsigned int i = 0; i < this->m_CVDeltaG->rows; i++)
    {
        for (unsigned int j = 0; j < this->m_CVDeltaG->cols; j ++)
        {
            CV_MAT_ELEM( *this->m_CVDeltaG, double, i , j) = 
                CV_MAT_ELEM( *this->m_CVMTexture, double, i , j) - 
                CV_MAT_ELEM( *this->m_CVMMeanTexture, double, 0 , j);
        }
    }

    // 94383*240
    CvMat* DeltaGTranspose = cvCreateMat (this->m_iNbOfTextures, this->m_iNbOfSamples, CV_64FC1);
    cvTranspose(this->m_CVDeltaG, DeltaGTranspose);

    // 240*240
    CvMat* DeltaGTransposeDeltaG = cvCreateMat (this->m_iNbOfSamples, this->m_iNbOfSamples, CV_64FC1);
    cvMatMul(this->m_CVDeltaG, DeltaGTranspose, DeltaGTransposeDeltaG);

    // 240*240
    CvMat* Fi = cvCreateMat(this->m_iNbOfSamples, this->m_iNbOfSamples, CV_64FC1);
    // 1*240
    CvMat* Lamda_temp = cvCreateMat(1, this->m_iNbOfSamples, CV_64FC1);
    cvEigenVV( DeltaGTransposeDeltaG, Fi, Lamda_temp, DBL_EPSILON );

    CvScalar SumOfEigenValues = cvSum( Lamda_temp );

    double limit =.000001 * SumOfEigenValues.val[0];

    int i = 0;
    for(i = 0; i < Lamda_temp->cols; i++)
    {
        if ( CV_MAT_ELEM( *Lamda_temp, double, 0, i ) < limit) break;
    }

    // NonZero EigenValues
    int K = i;

    for(i = K; i < Lamda_temp->cols; i++)
    {
        CV_MAT_ELEM( *Lamda_temp, double, 0, i ) = 0.;
    }
    
    // K*240
    CvMat* Fi_K = cvCreateMat( K, this->m_iNbOfSamples, CV_64FC1);
    // 1*K
    CvMat* Lamda_K_temp = cvCreateMat( 1, K, CV_64FC1);

    for(unsigned int i = 0; i < K; i++)
    {
        CV_MAT_ELEM( *Lamda_K_temp, double, 0, i ) = CV_MAT_ELEM( *Lamda_temp, double, 0, i );
    }
    
    // K*K
    CvMat* Lamda_K = cvCreateMat(K, K, CV_64FC1);
    cvZero(Lamda_K);
    for(unsigned int i = 0; i < K; i++)
    {
        CV_MAT_ELEM( *Lamda_K, double, i , i) = CV_MAT_ELEM( *Lamda_K_temp, double, 0 , i);
    }

    for(unsigned int i = 0; i < K; i++)
    {
        for (unsigned int j = 0; j < this->m_iNbOfSamples; j++)
        {
            CV_MAT_ELEM( *Fi_K, double, i, j ) = CV_MAT_ELEM( *Fi, double, i, j );
        }
    }

    cvReleaseMat(&Fi);
    cvReleaseMat(&Lamda_temp);
    cvReleaseMat(&Lamda_K_temp);
    
    // 240*K
    CvMat* Fi_K_Transpose = cvCreateMat(Fi_K->cols, Fi_K->rows, CV_64FC1);
    cvTranspose (Fi_K, Fi_K_Transpose);

    // According to http://www2.imm.dtu.dk/~aam/main/node16.html
    // t<=k<=s. Here, for simplicity, let k = s!!
    // However, if there is a eigenvalue too close to 0, the calculation later on
    // will have a big deviation! So, ignore all eigenvalues too close to 0!
    // But, we don't actually need to choose exactly 0.95 eigenvalues out like the former PCA

    CvMat* Lamda_K_Inverse = cvCreateMat(K, K, CV_64FC1);
    cvZero(Lamda_K_Inverse);
    for(unsigned int i = 0; i < Lamda_K->cols; i++)
    {
        CV_MAT_ELEM( *Lamda_K_Inverse, double, i , i) = 1.0 / CV_MAT_ELEM( *Lamda_K, double, i , i);
    }

    vector<vector<double> > vLamda_K_Inverse;
    vLamda_K_Inverse.resize(Lamda_K_Inverse->rows);
    for (unsigned int i = 0; i < vLamda_K_Inverse.size() ; i++)
    {
        vLamda_K_Inverse[i].resize(Lamda_K_Inverse->cols);
    }

    for (unsigned int i = 0; i < vLamda_K_Inverse.size(); i++)
    {
        for (unsigned int j = 0; j < vLamda_K_Inverse[0].size(); j++)
        {
             vLamda_K_Inverse[i][j] = CV_MAT_ELEM( *Lamda_K_Inverse, double, i , j);
        }
    }

    // 240*K
    CvMat* Fi_K_Transpose_Lamda_K_Inverse = cvCreateMat (this->m_iNbOfSamples, K, CV_64FC1);
    cvMatMul(Fi_K_Transpose, Lamda_K_Inverse, Fi_K_Transpose_Lamda_K_Inverse);

    vector<vector<double> > vFi_K_Transpose_Lamda_K_Inverse;
    vFi_K_Transpose_Lamda_K_Inverse.resize(Fi_K_Transpose_Lamda_K_Inverse->rows);
    for (unsigned int i = 0; i < vFi_K_Transpose_Lamda_K_Inverse.size() ; i++)
    {
        vFi_K_Transpose_Lamda_K_Inverse[i].resize(Fi_K_Transpose_Lamda_K_Inverse->cols);
    }

    for (unsigned int i = 0; i < vFi_K_Transpose_Lamda_K_Inverse.size(); i++)
    {
        for (unsigned int j = 0; j < vFi_K_Transpose_Lamda_K_Inverse[0].size(); j++)
        {
             vFi_K_Transpose_Lamda_K_Inverse[i][j] = CV_MAT_ELEM( *Fi_K_Transpose_Lamda_K_Inverse, double, i , j);
        }
    }

    CvMat* Fi_K_DeltaG = cvCreateMat (K, this->m_iNbOfTextures, CV_64FC1);
    cvMatMul(Fi_K, this->m_CVDeltaG, Fi_K_DeltaG);

    vector<vector<double> > vFi_K_DeltaG;
    vFi_K_DeltaG.resize(Fi_K_DeltaG->rows);
    for (unsigned int i = 0; i < vFi_K_DeltaG.size() ; i++)
    {
        vFi_K_DeltaG[i].resize(Fi_K_DeltaG->cols);
    }

    for (unsigned int i = 0; i < vFi_K_DeltaG.size(); i++)
    {
        for (unsigned int j = 0; j < vFi_K_DeltaG[0].size(); j++)
        {
             vFi_K_DeltaG[i][j] = CV_MAT_ELEM( *Fi_K_DeltaG, double, i , j);
        }
    }

    CvMat* temp1 = cvCreateMat (this->m_iNbOfSamples, this->m_iNbOfTextures, CV_64FC1);
    cvMatMul(Fi_K_Transpose_Lamda_K_Inverse, Fi_K_DeltaG, temp1);

    vector<vector<double> > vtemp1;
    vtemp1.resize(temp1->rows);
    for (unsigned int i = 0; i < vtemp1.size() ; i++)
    {
        vtemp1[i].resize(temp1->cols);
    }

    for (unsigned int i = 0; i < vtemp1.size(); i++)
    {
        for (unsigned int j = 0; j < vtemp1[0].size(); j++)
        {
             vtemp1[i][j] = CV_MAT_ELEM( *temp1, double, i , j);
        }
    }

    this->m_CVR = cvCreateMat (DeltaCTranspose->rows, this->m_iNbOfTextures, CV_64FC1);
    cvMatMul(DeltaCTranspose, temp1, this->m_CVR);

    // store negative versions to avoid any sign change at each iteration
    for (unsigned int i = 0; i < this->m_CVR->rows; i++)
    {
        for (unsigned int j = 0; j < this->m_CVR->cols; j++)
        {
             CV_MAT_ELEM( *this->m_CVR, double, i , j) *= -1;
        }
    }

 vector<vector<double> > vm_CVR;
    vm_CVR.resize(m_CVR->rows);
    for (unsigned int i = 0; i < vm_CVR.size() ; i++)
    {
        vm_CVR[i].resize(m_CVR->cols);
    }

    for (unsigned int i = 0; i < vm_CVR.size(); i++)
    {
        for (unsigned int j = 0; j < vm_CVR[0].size(); j++)
        {
             vm_CVR[i][j] = CV_MAT_ELEM( *m_CVR, double, i , j);
        }
    }

    // Explained by JIA Pei, 2006-10-21. temp2 is used for demonstrated the 
    // calculation is correct or not. We can see that temp2 is equal to 
    // cVectors, which shows our calculation is correct!! 
//     CvMat* temp2 = cvCreateMat (DeltaCTranspose->rows, this->m_iNbOfSamples, CV_64FC1);
//     cvMatMul( this->m_CVR, DeltaGTranspose, temp2);
// 
//     vector<vector<double> > vtemp2;
//     vtemp2.resize(temp2->rows);
//     for (unsigned int i = 0; i < vtemp2.size() ; i++)
//     {
//         vtemp2[i].resize(temp2->cols);
//     }
// 
//     for (unsigned int i = 0; i < vtemp2.size(); i++)
//     {
//         for (unsigned int j = 0; j < vtemp2[0].size(); j++)
//         {
//              vtemp2[i][j] = CV_MAT_ELEM( *temp2, double, i , j);
//         }
//     }
//     cvReleaseMat(&temp2);

    cvReleaseMat(&DeltaCTranspose);
    cvReleaseMat(&DeltaGTranspose);
    cvReleaseMat(&DeltaGTransposeDeltaG);
    cvReleaseMat(&Fi_K);
    cvReleaseMat(&Lamda_K);
    cvReleaseMat(&Fi_K_Transpose);
    cvReleaseMat(&Lamda_K_Inverse);
    cvReleaseMat(&Fi_K_Transpose_Lamda_K_Inverse);
    cvReleaseMat(&Fi_K_DeltaG);
    cvReleaseMat(&temp1);
    

}




vector<vector<double> > lv_aambuilding::LV_Concatenated2Parameters( )
{
    vector<double> c;
    vector<vector<double> > cVecVec;

    cVecVec.clear();

    for(unsigned int i = 0 ;i < this->m_vAAMConcatenated.size(); i++)
    {
        c = this->LV_ConcatenatedProject2TruncatedEigenSpace( this->m_vAAMConcatenated[i]);
        cVecVec.push_back( c );
    }

    return cVecVec;
}



vector<double> lv_aambuilding::LV_ConcatenatedProject2TruncatedEigenSpace(const vector<double> iConcatenated)
{
    vector<double> result;
    CvMat* res = cvCreateMat (this->m_CVMTruncatedConcatenatedEigenVectors->rows, 1, CV_64FC1); // 15*1

    CvMat* temp = cvCreateMat (iConcatenated.size (), 1, CV_64FC1);  // 136*1

    for (unsigned int i = 0; i < iConcatenated.size (); i++)
    {
        CV_MAT_ELEM( *temp, double, i , 0) = iConcatenated[i] - CV_MAT_ELEM( *this->m_CVMMeanConcatenated, double, 0 , i);
    }

    cvMatMul( this->m_CVMTruncatedConcatenatedEigenVectors, temp, res );   // (15*136)*(136*1) = (15*1)

    for (unsigned int i = 0; i < res->rows; i++)
    {
        result.push_back( CV_MAT_ELEM( *res, double, i, 0) );
    }

    cvReleaseMat (&res);
    cvReleaseMat (&temp);

    return result;
}






// write all needed parameters to file.
void lv_aambuilding::LV_WriteParameters2File(string fn)
{
    fstream fp;

    fp.open(fn.c_str (), ios::out);

    fp << "Shape Model" << endl;

    fp << "Number of Points: " << endl;
    fp << this->m_iNbOfPoints << endl;

    fp << "Average Shape Size: " << endl;
    fp << this->m_dAverageSize << endl;

    fp << "Normalized Reference Shape: " << endl;

    for (unsigned int i = 0; i < this->m_iNbOfPoints; i++)
    {
        fp << this->m_oAAMMeanShape.m_vPoint[i].m_CVxy.x << " " 
            << this->m_oAAMMeanShape.m_vPoint[i].m_CVxy.y << endl;
    }

    fp << "Number of Truncated AlignedShapes Eigens: " << endl;
    fp << this->m_CVMTruncatedAlignedShapesEigenValues->cols << endl;

    fp << "Shape Model Truncated EigenValues:" << endl;
    for (unsigned int i = 0; i < this->m_CVMTruncatedAlignedShapesEigenValues->cols; i++)
    {
        fp << CV_MAT_ELEM( *this->m_CVMTruncatedAlignedShapesEigenValues, double, 0, i) << " " ;
    }

    fp << endl;

    fp << "Shape Model Average Vector:" << endl;
    for (unsigned int i = 0; i < this->m_CVMMeanShape->cols; i++)
    {
        fp << CV_MAT_ELEM( *this->m_CVMMeanShape, double, 0, i) << " " ;
    }

    fp << endl;

    fp << "Shape Model Truncated EigenVectors:" << endl;
    for (unsigned int i = 0; i < this->m_CVMTruncatedAlignedShapesEigenVectors->rows; i++)
    {
        for (unsigned int j = 0; j < this->m_CVMTruncatedAlignedShapesEigenVectors->cols; j++)
        {
            fp << CV_MAT_ELEM( *this->m_CVMTruncatedAlignedShapesEigenVectors, double, i, j) << " " ;
        }
        fp << endl;
    }

    fp << "Number of Pixels:" << endl;
    fp << this->m_vTextureTriangle.size () << endl;

    fp << "Number of Edges:" << endl;
    fp << this->m_vAAMEdge.size () << endl;

    fp << "All Mesh Edges:" << endl;

    for (unsigned int i = 0; i < this->m_vAAMEdge.size (); i++)
    {
        fp << this->m_vAAMEdge[i].index1 << " " << this->m_vAAMEdge[i].index2 << endl; 
    }

    fp << "Number of Triangles:" << endl;
    fp << this->m_vAAMTriangle2D.size () << endl;

    fp << "All Mesh Triangles:" << endl;

    for (unsigned int i = 0; i < this->m_vAAMTriangle2D.size (); i++)
    {
        fp  << this->m_vAAMTriangle2D[i].m_iIndexes[0] << " " 
            << this->m_vAAMTriangle2D[i].m_vVertexes[0].x << " "
            << this->m_vAAMTriangle2D[i].m_vVertexes[0].y << " "
            << this->m_vAAMTriangle2D[i].m_iIndexes[1] << " " 
            << this->m_vAAMTriangle2D[i].m_vVertexes[1].x << " "
            << this->m_vAAMTriangle2D[i].m_vVertexes[1].y << " "
            << this->m_vAAMTriangle2D[i].m_iIndexes[2] << " "
            << this->m_vAAMTriangle2D[i].m_vVertexes[2].x << " "
            << this->m_vAAMTriangle2D[i].m_vVertexes[2].y<< endl; 
    }

    fp << "Reference Shape:" << endl;

    for (unsigned int i = 0; i < this->m_iNbOfPoints; i++)
    {
        fp << this->m_oAAMReferenceShape.m_vPoint[i].m_CVxy.x << " "
            << this->m_oAAMReferenceShape.m_vPoint[i].m_CVxy.y << endl; 
    }

    fp << "Convex Hull:" << endl;

    fp << this->m_CVMConvexHull->cols << endl;

    for (unsigned int i = 0; i < this->m_CVMConvexHull->cols; i++)
    {
        fp << CV_MAT_ELEM( *this->m_CVMConvexHull, CvPoint2D32f, 0, i).x << " "
            << CV_MAT_ELEM( *this->m_CVMConvexHull, CvPoint2D32f, 0, i).y << endl; 
    }

    fp << "Texture Model" << endl;

    fp << "Number of Pixels in the Template Face: " << endl;
    fp << this->m_iNbOfTextures/3 << endl;

    fp << "Reference Texture: " << endl;

    for (unsigned int i = 0; i < this->m_oAAMReferenceTexture.m_vTextures.size (); i++)
    {
        fp << this->m_oAAMReferenceTexture.m_vTextures[i] << " ";
    }

    fp << endl;

    // For Lucas-Kanade, we need to remember all the following information
    fp << "Pixel Triangle Relation: " << endl;

    // 31461
    for (unsigned int i = 0; i < this->m_vTextureTriangle.size (); i++)
    {
        fp << this->m_vTextureTriangle[i].m_iPointIndex << " "
           << this->m_vTextureTriangle[i].m_CVPosition.x << " "
           << this->m_vTextureTriangle[i].m_CVPosition.y << " "
           << this->m_vTextureTriangle[i].m_iTriangleIndex << " ";
        for (unsigned int j = 0; j < this->m_vTextureTriangle[i].m_SteepestDescentImages[0].size (); j++)
        {
            fp  << this->m_vTextureTriangle[i].m_SteepestDescentImages[0][j] << " "
                << this->m_vTextureTriangle[i].m_SteepestDescentImages[1][j] << " "
                << this->m_vTextureTriangle[i].m_SteepestDescentImages[2][j] << " "
                << this->m_vTextureTriangle[i].m_SteepestDescentImages[3][j] << " ";
        }
        
        fp << endl;
    }

    fp << "Hessian Matrix Size: " << endl;
    fp << this->m_HessianMatrixInverse[0].size () << endl;

    fp << "Hessian Matrix Inverse: " << endl; 
    for (unsigned int i = 0; i < this->m_HessianMatrixInverse[0].size (); i++)
    {
        for (unsigned int j = 0; j < this->m_HessianMatrixInverse[0].size (); j++)
        {
            fp << this->m_HessianMatrixInverse[0][i][j] << " ";
            fp << this->m_HessianMatrixInverse[1][i][j] << " ";
            fp << this->m_HessianMatrixInverse[2][i][j] << " ";
            fp << this->m_HessianMatrixInverse[3][i][j] << " ";
        }
        fp << endl;
    }

    fp.close ();
}



// load all needed parameters from file.
void lv_aambuilding::LV_LoadParamtersFromFile(string fn)
{
    fstream fp;
    fp.open(fn.c_str (), ios::in);

    string temp;

    getline( fp, temp );    // fp << "Shape Model" << endl;
    getline( fp, temp );    // fp << "Number of Points: " << endl;
    getline( fp, temp );    // fp << this->m_iNbOfPoints << endl;

    this->m_iNbOfPoints = atoi(temp.c_str ());

    getline( fp, temp );    // fp << "Average Shape Size: " << endl;

    getline( fp, temp ); 
    this->m_dAverageSize = atof(temp.c_str ());

    getline( fp, temp );    // fp << "Normalized Reference Shape: " << endl;

    // read standardized mean shape.
    this->m_oAAMMeanShape.m_vPoint.resize (this->m_iNbOfPoints);

    for (unsigned int i = 0; i < this->m_iNbOfPoints; i++)
    {
        getline(fp, temp, ' ');
        this->m_oAAMMeanShape.m_vPoint[i].m_CVxy.x = atof(temp.c_str ());
        getline(fp, temp);
        this->m_oAAMMeanShape.m_vPoint[i].m_CVxy.y = atof(temp.c_str ());
    }

    getline( fp, temp );    // fp << "Number of Truncated AlignedShapes Eigens:" << endl;
    getline(fp, temp);      // 20

    int NbOfTruncatedAlignedShapesEigenValues = atoi(temp.c_str ());

    this->m_CVMTruncatedAlignedShapesEigenValues = cvCreateMat (1, NbOfTruncatedAlignedShapesEigenValues, CV_64FC1);
    this->m_CVMTruncatedAlignedShapesEigenVectors = cvCreateMat (
        this->m_CVMTruncatedAlignedShapesEigenValues->cols, this->m_iNbOfPoints * 2, CV_64FC1);

    getline( fp, temp );    // fp << "Shape Model Truncated EigenValues:" << endl;

    for (unsigned int i = 0; i < NbOfTruncatedAlignedShapesEigenValues; i++)
    {
        getline(fp, temp, ' ');
        CV_MAT_ELEM( *this->m_CVMTruncatedAlignedShapesEigenValues, double, 0, i) = atof(temp.c_str ());
    }

    getline( fp, temp );    // For /c/r
    getline( fp, temp );    // fp << "Shape Model Average Vector:" << endl;

    this->m_CVMMeanShape = cvCreateMat (1, this->m_iNbOfPoints * 2, CV_64FC1);

    for (unsigned int i = 0; i < this->m_iNbOfPoints * 2; i++)
    {
        getline(fp, temp, ' ');
        CV_MAT_ELEM( *this->m_CVMMeanShape, double, 0, i) = atof(temp.c_str ());
    }

    getline( fp, temp );    // For /c/r
    getline( fp, temp );    // fp << "Shape Model Truncated EigenVectors:" << endl;

    for (unsigned int i = 0; i < this->m_CVMTruncatedAlignedShapesEigenVectors->rows; i++)
    {
        for (unsigned int j = 0; j < this->m_CVMTruncatedAlignedShapesEigenVectors->cols; j++)
        {
            getline(fp, temp, ' ');
            CV_MAT_ELEM( *this->m_CVMTruncatedAlignedShapesEigenVectors, double, i, j) = atof(temp.c_str ());
        }
    }

    getline( fp, temp );    // For /c/r
    getline( fp, temp );    // fp << "Number of Pixels:" << endl;

    getline( fp, temp );

    this->m_vTextureTriangle.resize (atoi(temp.c_str ()));

    getline( fp, temp );    // fp << "Number of Edges:" << endl;

    getline( fp, temp );

    this->m_vAAMEdge.resize (atoi(temp.c_str ()));

    getline( fp, temp );    // fp << "All Mesh Edges:" << endl;

    for (unsigned int i = 0; i < this->m_vAAMEdge.size (); i++)
    {
        getline(fp, temp, ' ');
        this->m_vAAMEdge[i].index1 = atof(temp.c_str ());
        getline(fp, temp);
        this->m_vAAMEdge[i].index2 = atof(temp.c_str ());
    }

    getline( fp, temp );    // fp << "Number of Triangles:" << endl;

    getline( fp, temp );

    this->m_vAAMTriangle2D.resize (atoi(temp.c_str ()));

    getline( fp, temp );    // fp << "All Mesh Triangles:" << endl;

    for (unsigned int i = 0; i < this->m_vAAMTriangle2D.size (); i++)
    {
        getline(fp, temp, ' ');
        this->m_vAAMTriangle2D[i].m_iIndexes[0] = atoi(temp.c_str ());
        getline(fp, temp, ' ');
        this->m_vAAMTriangle2D[i].m_vVertexes[0].x = atof(temp.c_str ());
        getline(fp, temp, ' ');
        this->m_vAAMTriangle2D[i].m_vVertexes[0].y = atof(temp.c_str ());
        getline(fp, temp, ' ');
        this->m_vAAMTriangle2D[i].m_iIndexes[1] = atoi(temp.c_str ());
        getline(fp, temp, ' ');
        this->m_vAAMTriangle2D[i].m_vVertexes[1].x = atof(temp.c_str ());
        getline(fp, temp, ' ');
        this->m_vAAMTriangle2D[i].m_vVertexes[1].y = atof(temp.c_str ());
        getline(fp, temp, ' ');
        this->m_vAAMTriangle2D[i].m_iIndexes[2] = atoi(temp.c_str ());
        getline(fp, temp, ' ');
        this->m_vAAMTriangle2D[i].m_vVertexes[2].x = atof(temp.c_str ());
        getline(fp, temp);
        this->m_vAAMTriangle2D[i].m_vVertexes[2].y = atof(temp.c_str ());
    }

    getline( fp, temp );    // fp << "Reference Shape:" << endl;

    this->m_oAAMReferenceShape.m_vPoint.resize (this->m_iNbOfPoints);

    for (unsigned int i = 0; i < this->m_iNbOfPoints; i++)
    {
        getline( fp, temp, ' ' );
        this->m_oAAMReferenceShape.m_vPoint[i].m_CVxy.x = atof(temp.c_str ());
        getline( fp, temp );
        this->m_oAAMReferenceShape.m_vPoint[i].m_CVxy.y = atof(temp.c_str ());
    }

    getline( fp, temp );    // fp << "Convex Hull:" << endl;

    getline( fp, temp );

    int ConvexNumber = atoi(temp.c_str ());

    this->m_CVMConvexHull = cvCreateMat (1, ConvexNumber, CV_32FC2);

    for (unsigned int i = 0; i < ConvexNumber; i++)
    {
        getline( fp, temp, ' ' );
        CV_MAT_ELEM( *this->m_CVMConvexHull, CvPoint2D32f, 0, i ).x = atof(temp.c_str ());
        getline( fp, temp );
        CV_MAT_ELEM( *this->m_CVMConvexHull, CvPoint2D32f, 0, i ).y = atof(temp.c_str ());
    }

    getline( fp, temp );    // fp << "Texture Model" << endl;

    getline( fp, temp );    // fp << "Number of Textures: " << endl;

    getline( fp, temp );    // 31461

    this->m_iNbOfTextures = atoi(temp.c_str ()) * 3;    // 31461 * 3

    this->m_oAAMReferenceTexture.m_vTextures.resize (this->m_iNbOfTextures);
    this->m_oAAMMeanTexture.m_vTextures.resize (this->m_iNbOfTextures);

    getline( fp, temp );    // fp << "Reference Texture: " << endl;

    for (unsigned int i = 0; i < this->m_iNbOfTextures; i++)
    {
        getline(fp, temp, ' ');
        this->m_oAAMReferenceTexture.m_vTextures[i] = atof(temp.c_str ());
    }

    getline( fp, temp );    // /c/r

    getline (fp, temp);     // Pixel Triangle Relation: 

    this->m_vTextureTriangle.resize ( this->m_iNbOfTextures/3 );

    // 31461
    for (unsigned int i = 0; i < this->m_vTextureTriangle.size (); i++)
    {
        getline (fp, temp, ' ');
        this->m_vTextureTriangle[i].m_iPointIndex = atoi(temp.c_str ());

        getline (fp, temp, ' ');
        this->m_vTextureTriangle[i].m_CVPosition.x = atof(temp.c_str ());

        getline (fp, temp, ' ');
        this->m_vTextureTriangle[i].m_CVPosition.y = atof(temp.c_str ());

        getline (fp, temp, ' ');
        this->m_vTextureTriangle[i].m_iTriangleIndex = atoi(temp.c_str ());

        this->m_vTextureTriangle[i].m_SteepestDescentImages[0].resize (NbOfTruncatedAlignedShapesEigenValues);
        this->m_vTextureTriangle[i].m_SteepestDescentImages[1].resize (NbOfTruncatedAlignedShapesEigenValues);
        this->m_vTextureTriangle[i].m_SteepestDescentImages[2].resize (NbOfTruncatedAlignedShapesEigenValues);
        this->m_vTextureTriangle[i].m_SteepestDescentImages[3].resize (NbOfTruncatedAlignedShapesEigenValues);

        for (unsigned int j = 0; j < NbOfTruncatedAlignedShapesEigenValues; j++)
        {
            getline (fp, temp, ' ');
            this->m_vTextureTriangle[i].m_SteepestDescentImages[0][j] = atof(temp.c_str ());

            getline (fp, temp, ' ');
            this->m_vTextureTriangle[i].m_SteepestDescentImages[1][j] = atof(temp.c_str ());

            getline (fp, temp, ' ');
            this->m_vTextureTriangle[i].m_SteepestDescentImages[2][j] = atof(temp.c_str ());

            getline (fp, temp, ' ');
            this->m_vTextureTriangle[i].m_SteepestDescentImages[3][j] = atof(temp.c_str ());
        }

        getline( fp, temp );    // /c/r
    }

    getline( fp, temp );        // Hessian Matrix Size:

    getline( fp, temp ); 

    int HMSize = atoi(temp.c_str ());

    this->m_HessianMatrixInverse.resize (4);

    for (unsigned int i = 0; i < 4; i++)
    {
        this->m_HessianMatrixInverse[i].resize( HMSize );
        for (unsigned int j = 0; j < HMSize; j++)
        {
            this->m_HessianMatrixInverse[i][j].resize( HMSize );

        }
    }
    

    getline( fp, temp );        // Hessian Matrix Inverse:

    for (unsigned int i = 0; i < HMSize; i++)
    {
        for (unsigned int j = 0; j < HMSize; j++)
        {
            getline( fp, temp, ' ' );
            this->m_HessianMatrixInverse[0][i][j] = atof(temp.c_str ());
            getline( fp, temp, ' ' );
            this->m_HessianMatrixInverse[1][i][j] = atof(temp.c_str ());
            getline( fp, temp, ' ' );
            this->m_HessianMatrixInverse[2][i][j] = atof(temp.c_str ());
            getline( fp, temp, ' ' );
            this->m_HessianMatrixInverse[3][i][j] = atof(temp.c_str ());
        }
        getline( fp, temp );    // for /c/r
    }

    // In order to draw the face texture, we need to know which point is inside the convex!
    this->m_CVMPoints = cvCreateMat (1, this->m_iNbOfPoints, CV_32FC2);

    fp.close ();
}