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_aamshape2d.cpp,v 1.1.1.1 2006-09-03 17:49:04 JIA Pei Exp $ 

#include <string>
#include <iostream>
#include <fstream>
#include "lv_aamshape2d.h"

using namespace std;




/**

    @author     JIA Pei

    @version    2006-04-13

    @brief      Copy constructor.  

    @param      s	The input shape.

    @return     Nothing.

*/

lv_aamshape2d::lv_aamshape2d( const lv_aamshape2d &s )
{
    this->CopyData (s);
}


lv_aamshape2d& lv_aamshape2d::operator=(const lv_aamshape2d &s)
{
    this->CopyData (s);
    return *this;
}


lv_aamshape2d& lv_aamshape2d::operator=(double value)
{
    if (this->m_vPoint.size())
    { 
        for (unsigned int i = 0; i < this->m_vPoint.size(); i++)
        {
            this->m_vPoint[i].m_CVxy.x = value;
            this->m_vPoint[i].m_CVxy.y = value;
        }   
    } 
    return *this;
   
}


lv_aamshape2d lv_aamshape2d::operator+(const lv_aamshape2d &s)
{    
    lv_aamshape2d res(*this);

    for (unsigned int i = 0; i < res.m_vPoint.size(); i++)
    {
        res.m_vPoint[i].m_CVxy.x += s.m_vPoint[i].m_CVxy.x;
        res.m_vPoint[i].m_CVxy.y += s.m_vPoint[i].m_CVxy.y;
    }
    
    return res;
}


lv_aamshape2d& lv_aamshape2d::operator+=(const lv_aamshape2d &s)
{
    for (unsigned int i = 0; i < this->m_vPoint.size(); i++)
    {
        this->m_vPoint[i].m_CVxy.x += s.m_vPoint[i].m_CVxy.x;
        this->m_vPoint[i].m_CVxy.y += s.m_vPoint[i].m_CVxy.y;
    }

    return *this;
}


lv_aamshape2d lv_aamshape2d::operator-(const lv_aamshape2d &s)
{    
    lv_aamshape2d res(*this);

    for (unsigned int i = 0; i < res.m_vPoint.size(); i++)
    {
        res.m_vPoint[i].m_CVxy.x -= s.m_vPoint[i].m_CVxy.x;
        res.m_vPoint[i].m_CVxy.y -= s.m_vPoint[i].m_CVxy.y;
    }

    return res;
}


lv_aamshape2d& lv_aamshape2d::operator-=(const lv_aamshape2d &s)
{
    for (unsigned int i = 0; i < this->m_vPoint.size(); i++)
    {
        this->m_vPoint[i].m_CVxy.x -= s.m_vPoint[i].m_CVxy.x;
        this->m_vPoint[i].m_CVxy.y -= s.m_vPoint[i].m_CVxy.y;
    }

    return *this;
}


lv_aamshape2d lv_aamshape2d::operator*(double value)
{
    lv_aamshape2d res(*this);

    for (unsigned int i = 0; i < this->m_vPoint.size(); i++)
    {
        res.m_vPoint[i].m_CVxy.x *= value;
        res.m_vPoint[i].m_CVxy.y *= value;
    }

    return res;
}


lv_aamshape2d& lv_aamshape2d::operator*=(double value)
{
    for (unsigned int i = 0; i < this->m_vPoint.size(); i++)
    {
        this->m_vPoint[i].m_CVxy.x *= value;
        this->m_vPoint[i].m_CVxy.y *= value;
    }

    return *this;
}


double lv_aamshape2d::operator*(const lv_aamshape2d &s)
{
    double result = 0.;

    for (unsigned int i = 0; i < this->m_vPoint.size(); i++)
    {
        result += this->m_vPoint[i].m_CVxy.x * s.m_vPoint[i].m_CVxy.x;
        result += this->m_vPoint[i].m_CVxy.y * s.m_vPoint[i].m_CVxy.y;
    }

    return result;
}


lv_aamshape2d lv_aamshape2d::operator/(double value)
{
    assert (value != 0);

    lv_aamshape2d res(*this);

    for (unsigned int i = 0; i < this->m_vPoint.size(); i++)
    {
        res.m_vPoint[i].m_CVxy.x /= value;
        res.m_vPoint[i].m_CVxy.y /= value;
    }

    return res;
}


lv_aamshape2d& lv_aamshape2d::operator/=(double value)
{
    assert (value != 0);

    for (unsigned int i = 0; i < this->m_vPoint.size(); i++)
    {
        this->m_vPoint[i].m_CVxy.x /= value;
        this->m_vPoint[i].m_CVxy.y /= value;
    }

    return *this;
}


void lv_aamshape2d::CopyData( const lv_aamshape2d &s )
{
    // copy vector data (i.e. the point coordinates)
    this->m_vPoint.resize( s.m_vPoint.size() );

    // copy class data members
    this->m_vPoint                  = s.m_vPoint;
    this->m_sHostImage              = s.m_sHostImage;
}


void lv_aamshape2d::resize(int length)
{ 	
    this->m_vPoint.resize(length);
}


/**

    @author     JIA Pei

    @version    2006-09-01

    @brief      Reads an .asf into relative coordinates.

    @see        WriteASF

    @param      filename	Input filename.

    @return     true on success, false on errors

*/
void lv_aamshape2d::ReadASF( const string &filename )
{
    fstream fp;
    fp.open(filename.c_str (), ios::in);

    string temp;

    getline( fp, temp );
    getline( fp, temp );
    getline( fp, temp );
    getline( fp, temp );
    getline( fp, temp );
    getline( fp, temp );
    getline( fp, temp );
    getline( fp, temp );
    getline( fp, temp );
    getline( fp, temp );

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

    getline( fp, temp );
    getline( fp, temp );
    getline( fp, temp );
    getline( fp, temp );
    getline( fp, temp );
    getline( fp, temp );

    lv_aampoint2d temppoint;

    for (unsigned int i = 0; i < NbOfPoints; i++)
    {
        getline(fp, temp, ' ');
        temppoint.m_ipath = atoi(temp.c_str ());
        getline(fp, temp, ' ');
        temppoint.m_itype = atoi(temp.c_str ());  
        getline(fp, temp, ' ');
        // In DTU IMM , x means rows from left to right
        temppoint.m_CVxy.x = atof(temp.c_str ());
        getline(fp, temp, ' ');
        // In DTU IMM , y means cols from top to bottom
        temppoint.m_CVxy.y = atof(temp.c_str ());
        getline(fp, temp, ' ');
        temppoint.m_iindex = atoi(temp.c_str ());
        getline(fp, temp, ' ');
        temppoint.m_ifrom = atoi(temp.c_str ());
        getline(fp, temp);
        temppoint.m_ito = atoi(temp.c_str ());
        // In sum, topleft is (0,0), right bottom is (640,480)
        this->m_vPoint.push_back (temppoint);
    }

    getline(fp, temp);
    getline(fp, temp);
    getline(fp, temp);
    getline(fp, temp);
    getline(fp, temp);

    this->m_sHostImage = temp;

    fp.close ();

}



/**

    @author     JIA Pei

    @version    2006-09-02

    @brief		Calculates the Center Of Gravity of the shape
                (actually it's the center of the centroid).

    @param      x	X Center Of Gravity output.

    @param      y	X Center Of Gravity output.

    @return     Nothing.

*/
void lv_aamshape2d::CenterOfGravity( double &x, double &y )
{
    double xSum, ySum;

    xSum = ySum = 0.0;

    for(int i=0; i<this->m_vPoint.size(); i++)
    {
        xSum += this->m_vPoint[i].m_CVxy.x;
        ySum += this->m_vPoint[i].m_CVxy.y;
    }

    x = xSum/this->m_vPoint.size();
    y = ySum/this->m_vPoint.size();
}




/**

    @author     JIA Pei

    @version    2006-09-02

    @brief      Translates the shape.

    @param      x	X-translation.

    @param      y	Y-translation.

    @return     Nothing.

*/
void lv_aamshape2d::Translate( double x, double y )
{
    for(unsigned int i = 0; i < this->m_vPoint.size(); i++)
    {
        this->m_vPoint[i].m_CVxy.x += x;
        this->m_vPoint[i].m_CVxy.y += y;
    }
}



/**

    @author     JIA Pei

    @version    2006-09-02

    @brief		Scales the shape.

    @param      s			Scale factor.

    @return     Nothing.

*/
void lv_aamshape2d::Scale( double s)
{
    for(unsigned int i = 0; i < this->m_vPoint.size(); i++)
    {
        this->m_vPoint[i].m_CVxy.x *= s;
        this->m_vPoint[i].m_CVxy.y *= s;
    }
}


void lv_aamshape2d::ScaleXY( double sx, double sy)
{
    for(unsigned int i = 0; i < this->m_vPoint.size(); i++)
    {
        this->m_vPoint[i].m_CVxy.x *= sx;
        this->m_vPoint[i].m_CVxy.y *= sy;
    }
}



/**

    @author     JIA Pei

    @version    2006-09-02

    @brief		Rotates the shape 'theta' radians.

    @param      theta		Rotation angle in radians.

    @return     Nothing.

*/
void lv_aamshape2d::Rotate( double theta)
{
    double x, y, cx, cy;

    // set up rotation matrix
    double c00 =  cos( theta );
    double c01 = -sin( theta );
    double c10 =  sin( theta );
    double c11 =  cos( theta );

    for(unsigned int i=0;i<this->m_vPoint.size();i++)
    {
        x = this->m_vPoint[i].m_CVxy.x;
        y = this->m_vPoint[i].m_CVxy.y;
        this->m_vPoint[i].m_CVxy.x = c00*x+c01*y;
        this->m_vPoint[i].m_CVxy.y = c10*x+c11*y;
    }
}



/**

    @author	    JIA Pei

    @version	2006-09-02

    @brief		Normalize the shape by translating to its Center Of Gravity
                scale by the reciprocal of the 2-norm.	

    @return	    The 2-norm of the shape seen as a nbPoint vector after the
                translation to origin.
*/

double lv_aamshape2d::Normalize()
{
    double x,y;

    this->CenterOfGravity( x, y );

    this->Translate( -x, -y );

    double norm = this->GetShapeNorm();

    this->Scale( 1./norm );

    return norm;
}



/**

    @author   JIA Pei

    @version  2006-04-13

    @brief    Returns the 2-norm of this shape centralized.

    @return   The 2-norm.

*/
double lv_aamshape2d::ShapeSize()
{
    lv_aamshape2d tmp(*this);

    return tmp.Normalize();
}


/**

    @author     JIA Pei

    @version    2006-09-02

    @brief      Get the norm of the shape.

    @return     double.

*/
double lv_aamshape2d::GetShapeNorm()
{
    double norm = 0.0;

    // Normalize the vector to unit length, using the 2-norm.
    for (int i = 0; i < this->m_vPoint.size(); i++)
    {
        norm += pow(this->m_vPoint[i].m_CVxy.x, 2);
        norm += pow(this->m_vPoint[i].m_CVxy.y, 2);
    }

    norm = sqrt(norm);

    return norm;
}



/**

    @author	    JIA Pei

    @version	2006-09-02

    @brief		Returns the rotation between ref and this (in radians).
                Get the rotation between two shapes by minimizing the sum 
                of squared point distances, as described by Goodall (and 
                Bookstein) using Singular Value Decomposition (SVD).

                Note that both shapes must be normalized with respect to 
                scale and position beforehand. This could be done by using
                lv_aamshape2d::Normalize(). 

    @return	    The estimated angle, theta, between the two shapes.

*/
double lv_aamshape2d::GetRotation( lv_aamshape2d &ref )
{
    assert( ref.m_vPoint.size() == this->m_vPoint.size() );

    // Note by JIA Pei, make sure the precision must be CV_64FC1 instead of CV_32FC1
    CvMat* mRef            =   cvCreateMat (this->m_vPoint.size(), 2, CV_64FC1);
    CvMat* mRefTranspose   =   cvCreateMat (2, this->m_vPoint.size(), CV_64FC1);
    CvMat* mS              =   cvCreateMat (this->m_vPoint.size(), 2, CV_64FC1);
    CvMat* res             =   cvCreateMat (2, 2, CV_64FC1);
    const int X = 0;
    const int Y = 1;

    // get data as matrices ( nbPoints x 2 columns )
    for(int i=0; i < this->m_vPoint.size(); i++)
    {
        ((double*)(mRef->data.ptr + mRef->step * i))[X]  = ref.m_vPoint[i].m_CVxy.x;
        ((double*)(mRef->data.ptr + mRef->step * i))[Y]  = ref.m_vPoint[i].m_CVxy.y;

        ((double*)(mS->data.ptr + mS->step * i))[X]  = this->m_vPoint[i].m_CVxy.x;
        ((double*)(mS->data.ptr + mS->step * i))[Y]  = this->m_vPoint[i].m_CVxy.y;
    }

    // calculate the rotation by minimizing the sum of squared point distances
    cvTranspose( mRef, mRefTranspose );

    cvMatMul( mRefTranspose, mS, res );

    CvMat* S   =   cvCreateMat (2, 1, CV_64FC1);
    CvMat* UT  =   cvCreateMat (2, 2, CV_64FC1);
    CvMat* V   =   cvCreateMat (2, 2, CV_64FC1);

    // SVD Definition in OpenCV
    // A = U*W*V^T = U^T*W*V
    // cvSVD( CvArr* A, CvArr* W, CvArr* U=NULL, CvArr* V=NULL, int flags=0 );
    cvSVD( res, S, UT, V, CV_SVD_U_T );

    cvMatMul( V, UT, res );

    // res holds now a normal 2x2 rotation matrix
    double angle;
    double cos_theta = ((double*)(res->data.ptr + res->step * 0))[0];
    double sin_theta = ((double*)(res->data.ptr + res->step * 1))[0];
    const double epsilon = 1e-12;

    // cos_theta should be less than 1.0; but here, cos_theta is calculated by 
    // matrix computation, in stead of by acos() function, so cos_theta might
    // have some special values like cos_theta >= 1.0.
    if ( ( fabs(1.0-cos_theta) < epsilon ) || cos_theta >= 1.0 )
    {
        // cos_theta = 1  =>  shapes are already aligned
        angle = 0;
        return angle;
    }
    if ( fabs(cos_theta) < epsilon )
    {
        // cos_theta = 0  =>  90 degrees rotation
        return M_PI/2;
    }
    // cos_theta should be bigger than -1.0; but here, cos_theta is calculated by 
    // matrix computation, in stead of by acos() function, so cos_theta might
    // have some special values like cos_theta <= -1.0.
    if ( ( fabs(1.0+cos_theta) < epsilon ) || cos_theta <= -1.0 )
    {
        // cos_theta=-1  =>  180 degrees rotation
        angle = M_PI;
    }
    else
    {
        // get the rotation in radians
        double a_cos = acos( cos_theta );
        double a_sin = asin( sin_theta );

        // The following is correct. 
        // Explained by JIA Pei, please refer to
        // http://en.wikipedia.org/wiki/Trigonometric_function
        if (a_sin<0)
        {		
            // lower half of the unit circle
            angle = -a_cos;
        }
        else
        {
            // upper half of the unit circle
            angle = a_cos;
        }
    }

    cvReleaseMat(&mRef);
    cvReleaseMat(&mRefTranspose);
    cvReleaseMat(&mS);
    cvReleaseMat(&res);

    cvReleaseMat(&S);
    cvReleaseMat(&UT);
    cvReleaseMat(&V);

    return angle;
}




/**

    @author	    JIA Pei

    @version	2006-09-02

    @brief		Aligns this to 'ref' with respect to pose.

    @param	    ref			The reference shape.

    @param	    pTheta		Optional pointer to return the rotation carried out on this.

    @return	    none.
*/
void lv_aamshape2d::AlignTo( const lv_aamshape2d &ref, double *pTheta )
{
    // make a copy of 'ref'
    lv_aamshape2d refCpy( ref );
    double x,y;

    // move this and refCpy to origin
    this->CenterOfGravity( x, y );
    this->Translate( -x, -y );
    refCpy.CenterOfGravity( x, y );
    refCpy.Translate( -x, -y ); // remember now, (x, y) are for ref (refCpy) !!

    // normalize scale, using the 2-norm
    double this_size = this->GetShapeNorm();
    double ref_size = refCpy.GetShapeNorm();
    this->Scale( ref_size/this_size );

    // align rotation between this and refCpy
    double theta;
    theta = this->GetRotation( refCpy );
    this->Rotate( -theta );

    if (pTheta)
    {
        *pTheta = -theta;
    }

    // translate this to ref origin
    this->Translate( x, y );

}




/**

    @author	    JIA Pei

    @version	10-06-2006

    @brief		Returns the transformation that aligns this to 'ref' with respect to pose.

    @param	    ref	The reference shape.

    @return	    Nothing.

*/
void lv_aamshape2d::AlignTransformation( const lv_aamshape2d &ref, double &scale, double &theta, CvPoint2D32f &t )
{
    lv_aamshape2d refCpy( ref );
    lv_aamshape2d thisCpy( *this );
    double x,y;

    // move thisCpy and refCpy to origin
    thisCpy.CenterOfGravity( x, y );
    t.x = x;
    t.y = y;
    thisCpy.Translate( -t.x, -t.y );
    refCpy.CenterOfGravity( x, y );
    refCpy.Translate( -x, -y );
    t.x = x - t.x;
    t.y = y - t.y;

    // normalize scale, using the 2-norm
    double this_size = thisCpy.Normalize();
    double ref_size = refCpy.Normalize();
    scale = refCpy.Normalize()/thisCpy.Normalize();
    thisCpy.Scale( scale );	

    // align rotation between thisCpy and refCpy		
    theta = -thisCpy.GetRotation( refCpy );
}



/**

    @author     JIA Pei

    @version    10-15-2006

    @brief      Displaces the shape around it's center of gravity using a displacement vector.

    @see        PoseToParam

    @param      poseVec		The input pose vector.

    @return     Nothing.

*/
void lv_aamshape2d::Displace(double scale, double theta, double tx, double ty)
{
    double cx, cy;

    // translate to origin
    this->CenterOfGravity( cx, cy );
    this->Translate( -cx, -cy );

    // scale, rotate and translate
    this->Scale( scale );
    this->Rotate( theta );

    // translate the specified amount plus back to c.o.g.
    this->Translate( cx+tx, cy+ty );
}




double lv_aamshape2d::MinX()
{	
    double val, min = 1.7E+308;

    for(int i=0;i<this->m_vPoint.size();i++)
    {	
        val = this->m_vPoint[i].m_CVxy.x;
        min = val<min ? val : min;
    }
    return min;
}


double lv_aamshape2d::MinY()
{	
    double val, min = 1.7E+308;

    for(int i=0;i<this->m_vPoint.size();i++)
    {	
        val = this->m_vPoint[i].m_CVxy.y;
        min = val<min ? val : min;
    }
    return min;
}


double lv_aamshape2d::MaxX()
{	
    double val, max = -1.7E+308;

    for(int i=0;i<this->m_vPoint.size();i++)
    {
        val = this->m_vPoint[i].m_CVxy.x;
        max = val>max ? val : max;
    }
    return max;
}


double lv_aamshape2d::MaxY()
{	
    double val, max = -1.7E+308;

    for(int i=0;i<this->m_vPoint.size();i++)
    {
        val = this->m_vPoint[i].m_CVxy.y;
        max = val>max ? val : max;
    }
    return max;
}



void lv_aamshape2d::Point2CvMat(CvMat* res) const
{
    for (unsigned int i = 0; i < this->m_vPoint.size() ; i++)
    {
        CV_MAT_ELEM( *res, double, 0 , 2 * i) = this->m_vPoint[i].m_CVxy.x;
        CV_MAT_ELEM( *res, double, 0 , 2 * i + 1) = this->m_vPoint[i].m_CVxy.y;
    }
}