//============================================================================
void AAM_CAM::ShapeTexture2Combined(const CvMat* Shape, const CvMat* Texture,
CvMat* Appearance)
{
__shape.CalcParams(Shape, __pq);
CvMat mat1, mat2;
cvGetCols(__pq, &mat1, 4, 4+__shape.nModes());
cvGetCols(Appearance, &mat2, 0, __shape.nModes());
cvCopy(&mat1, &mat2);
cvConvertScale(&mat2, &mat2, __WeightsS2T);
cvGetCols(Appearance, &mat2, __shape.nModes(), __shape.nModes()+__texture.nModes());
__texture.CalcParams(Texture, &mat2);
}
void ann_check_data_and_get_predictors( CvMLData* _data, CvMat* _inputs )
{
const CvMat* values = _data->get_values();
const CvMat* var_idx = _data->get_var_idx();
if( var_idx->cols + var_idx->rows != values->cols )
CV_Error( CV_StsBadArg, "var_idx is not supported" );
if( _data->get_missing() )
CV_Error( CV_StsBadArg, "missing values are not supported" );
int resp_idx = _data->get_response_idx();
if( resp_idx == 0)
cvGetCols( values, _inputs, 1, values->cols );
else if( resp_idx == values->cols - 1 )
cvGetCols( values, _inputs, 0, values->cols - 1 );
else
CV_Error( CV_StsBadArg, "outputs must be in the first or last column; other cases are not supported" );
}
void calculateHOG_block(CvRect block, CvMat* hog_block,
IplImage** integrals,CvSize cell, int normalization)
{
int cell_start_x, cell_start_y;
CvMat vector_cell;
int startcol = 0;
for (cell_start_y = block.y; cell_start_y <=
block.y + block.height - cell.height;
cell_start_y += cell.height)
{
for (cell_start_x = block.x; cell_start_x <=
block.x + block.width - cell.width;
cell_start_x += cell.width)
{
cvGetCols(hog_block, &vector_cell, startcol,
startcol + 9);
calculateHOG_rect(cvRect(cell_start_x,
cell_start_y, cell.width, cell.height),
&vector_cell, integrals, -1);
startcol += 9;
}
}
if (normalization != -1)
cvNormalize(hog_block, hog_block, 1, 0,
normalization);
}
Mat AAM::getTextureParamsFromC(Mat c)
{
Mat ap = this->appearancePCA.backProject(c);
CvMat apMat=ap;
CvMat textureParams;
cvGetCols(&apMat, &textureParams, shapeSet.cols, shapeSet.cols+this->b_g.cols);
Mat texture=cvarrToMat(&textureParams);
return texture;
}
void MainWindow::displayOutput(){
CvSize imageSize = vision->getImageSize();
if(!imageRectifiedPair) imageRectifiedPair = cvCreateMat( imageSize.height, imageSize.width*2,CV_8UC3 );
vision->stereoProcess(camera.getFramesGray(0), camera.getFramesGray(1));
CvMat part;
cvGetCols( imageRectifiedPair, &part, 0, imageSize.width );
cvCvtColor( vision->imagesRectified[0], &part, CV_GRAY2BGR );
cvGetCols( imageRectifiedPair, &part, imageSize.width,imageSize.width*2 );
cvCvtColor( vision->imagesRectified[1], &part, CV_GRAY2BGR );
for(int j = 0; j < imageSize.height; j += 16 )
cvLine( imageRectifiedPair, cvPoint(0,j),cvPoint(imageSize.width*2,j),CV_RGB((j%3)?0:255,((j+1)%3)?0:255,((j+2)%3)?0:255));
cvShowImage( "rectified", imageRectifiedPair );
cvShowImage( "depth", vision->imageDepthNormalized );
}
Mat AAM::getShapeParamsFromC(Mat c)
{
Mat ap = this->appearancePCA.backProject(c);
CvMat apMat=ap;
CvMat shapeParams;
cvGetCols(&apMat, &shapeParams, 0, shapeSet.cols);
Mat shape=cvarrToMat(&shapeParams);
shape=shape/this->w;
return shape;
}
double compute_reprojection_error( const CvMat* object_points,
const CvMat* rot_vects, const CvMat* trans_vects,
const CvMat* camera_matrix, const CvMat* dist_coeffs,
const CvMat* image_points, const CvMat* point_counts,
CvMat* per_view_errors )
{
CvMat* image_points2 = cvCreateMat( image_points->rows,
image_points->cols, image_points->type );
int i, image_count = rot_vects->rows, points_so_far = 0;
double total_err = 0, err;
for( i = 0; i < image_count; i++ )
{
CvMat object_points_i, image_points_i, image_points2_i;
int point_count = point_counts->data.i[i];
CvMat rot_vect, trans_vect;
cvGetCols( object_points, &object_points_i,
points_so_far, points_so_far + point_count );
cvGetCols( image_points, &image_points_i,
points_so_far, points_so_far + point_count );
cvGetCols( image_points2, &image_points2_i,
points_so_far, points_so_far + point_count );
points_so_far += point_count;
cvGetRow( rot_vects, &rot_vect, i );
cvGetRow( trans_vects, &trans_vect, i );
cvProjectPoints2( &object_points_i, &rot_vect, &trans_vect,
camera_matrix, dist_coeffs, &image_points2_i,
0, 0, 0, 0, 0 );
err = cvNorm( &image_points_i, &image_points2_i, CV_L1 );
if( per_view_errors )
per_view_errors->data.db[i] = err/point_count;
total_err += err;
}
cvReleaseMat( &image_points2 );
return total_err/points_so_far;
}
/*
This function takes in a window(64x128 pixels,
but can be easily modified for other window sizes)
and calculated the hog features for the window. It
can be used to calculate the feature vector for a
64x128 pixel image as well. This window/image is the
training/detection window which is used for training
or on which the final detection is done. The hog features
are computed by dividing the window into overlapping blocks,
calculating the hog vectors for each block using calculateHOG_block(...)
and concatenating the so obtained vectors to obtain the
hog feature vector for the window
*/
CvMat* HoGProcessor::calculateHOG_window(IplImage **integrals, CvRect window, int normalization){
/*
A cell size of 8x8 pixels is considered and each block
is divided into 2x2 such cells(i.e. the block is 16x16 pixels).
So a 64x128 pixels window would be divided into 7x15 overlapping blocks
*/
int block_start_x, block_start_y;
/* The length of the feature vector for a cell is 9
(since no. of bins is 9), for block it would be
9*(no. of cells in the block) = 9*4 = 36. And the lenght
of the feature vector for a window would be 36*(no. of
blocks in the window)
*/
int lenghth_feature_of_block = 9 * m_block.height * m_block.width;
int num_overlap_block_of_width = ((window.width - m_cell.width * m_block.width) / (m_cell.width * m_fStepOverlap)) + 1;
int num_overlap_block_of_height = ((window.height - m_cell.height * m_block.height)/ (m_cell.height * m_fStepOverlap)) + 1;
int total_block_of_window = num_overlap_block_of_width * num_overlap_block_of_height ;
CvMat* window_feature_vector = cvCreateMat(1
, total_block_of_window * lenghth_feature_of_block
, CV_32FC1);
CvMat vector_block;
int startcol = 0;
int widthBlockInPixels = m_cell.width * m_block.width;
int heightBlockInPixels = m_cell.height * m_block.height;
for(block_start_y = window.y;
block_start_y <= window.y + window.height - m_cell.height * m_block.height;
block_start_y += m_cell.height * m_fStepOverlap)
{//overllap
for(block_start_x = window.x;
block_start_x <= window.x + window.width - m_cell.width * m_block.width;
block_start_x += m_cell.width * m_fStepOverlap)
{//overllap
cvGetCols(window_feature_vector, &vector_block, startcol, startcol+36);
calculateHOG_block(
cvRect(block_start_x, block_start_y, widthBlockInPixels , heightBlockInPixels)
, &vector_block, integrals, m_cell
, normalization);
startcol+= lenghth_feature_of_block;
}
}
return window_feature_vector;
}
//============================================================================
double AAM_Basic::EstResidual(const IplImage* image, const CvMat* c_q,
CvMat* s, CvMat* t_m,
CvMat* t_s, CvMat* deltat)
{
CvMat c, q;
cvGetCols(c_q, &q, 0, 4);
cvGetCols(c_q, &c, 4, 4+__cam.nModes());
// generate model texture
__cam.CalcTexture(t_m, &c);
// generate model shape
__cam.CalcShape(s, c_q);
// generate warped texture
AAM_Common::CheckShape(s, image->width, image->height);
__cam.__paw.CalcWarpTexture(s, image, t_s);
__cam.__texture.NormalizeTexture(__cam.__MeanG, t_s);
// calculate pixel difference
cvSub(t_m, t_s, deltat);
return cvNorm(deltat);
}
CvMat* calculateHOG_window(IplImage** integrals,
CvRect window, int normalization)
{
/*A cell size of 8x8 pixels is considered and each
block is divided into 2x2 such cells (i.e. the block
is 16x16 pixels). So a 64x128 pixels window would be
divided into 7x15 overlapping blocks*/
int block_start_x, block_start_y, cell_width = 8;
int cell_height = 8;
int block_width = 2, block_height = 2;
/* The length of the feature vector for a cell is
9(since no. of bins is 9), for block it would be
9*(no. of cells in the block) = 9*4 = 36. And the
length of the feature vector for a window would be
36*(no. of blocks in the window */
CvMat* window_feature_vector = cvCreateMat(1,
((((window.width - cell_width * block_width)
/ cell_width) + 1) * (((window.height -
cell_height * block_height) / cell_height)
+ 1)) * 36, CV_32FC1);
CvMat vector_block;
int startcol = 0;
for (block_start_y = window.y; block_start_y
<= window.y + window.height - cell_height
* block_height; block_start_y += cell_height)
{
for (block_start_x = window.x; block_start_x
<= window.x + window.width - cell_width
* block_width; block_start_x += cell_width)
{
cvGetCols(window_feature_vector, &vector_block,
startcol, startcol + 36);
calculateHOG_block(cvRect(block_start_x,
block_start_y, cell_width * block_width, cell_height
* block_height), &vector_block, integrals, cvSize(
cell_width, cell_height), normalization);
startcol += 36;
}
}
return (window_feature_vector);
}
// Ham tinh vector dac trung cho cua so HOG
CvMat* HOGProcessor::calcHOGWindow(IplImage** integrals, CvRect window, int normalization)
{
int blockStartX, blockStartY;
// Kich thuoc vector dac trung cho moi cell la 9
// Kich thuoc vector dac trung cho moi block se duoc tinh bang:
// 9 * (so cells trong moi block)
// Vi du so cells/ 1 block = 4 => 9*4 = 36
// => Kich thuoc vector dac trung cho moi cua so se la: 36 * (so block trong cua so)
int windowFeatureSize = getWindowFeatureVectorSize(cvSize(window.width, window.height));
int blockFeatureSize = NumOfBins * m_block.height * m_block.width;
CvMat* windowFeatureVector = cvCreateMat(1, windowFeatureSize, CV_32FC1);
CvMat vectorBlock;
int startcol = 0;
// Khoi tao cac tham so phu cho viect tinh toan
int widthBlockInPixels = m_cell.width * m_block.width;
int heightBlockInPixels = m_cell.height * m_block.height;
int heightStep = heightBlockInPixels - m_cell.height * m_fStepOverlap;
int widthStep = widthBlockInPixels - m_cell.width * m_fStepOverlap;
// Bat dau qua trinh tinh vector dac trung cho cua so
for(blockStartY = window.y;
blockStartY <= window.y + window.height - heightBlockInPixels;
blockStartY += heightStep)
{//overllap
for(blockStartX = window.x;
blockStartX <= window.x + window.width - widthBlockInPixels;
blockStartX += widthStep)
{//overllap
cvGetCols(windowFeatureVector, &vectorBlock, startcol, startcol + blockFeatureSize);
calcHOGForBlock(
cvRect(blockStartX, blockStartY, widthBlockInPixels , heightBlockInPixels)
, &vectorBlock, integrals, m_cell
, normalization);
startcol += blockFeatureSize;
}
}
return windowFeatureVector;
}
//============================================================================
void AAM_TDM::DoPCA(const CvMat* AllTextures, double percentage)
{
LOGD("Doing PCA of textures datas...");
int nSamples = AllTextures->rows;
int nPixels = AllTextures->cols;
int nEigenAtMost = MIN(nSamples, nPixels);
CvMat* tmpEigenValues = cvCreateMat(1, nEigenAtMost, CV_64FC1);
CvMat* tmpEigenVectors = cvCreateMat(nEigenAtMost, nPixels, CV_64FC1);
__MeanTexture = cvCreateMat(1, nPixels, CV_64FC1 );
cvCalcPCA(AllTextures, __MeanTexture,
tmpEigenValues, tmpEigenVectors, CV_PCA_DATA_AS_ROW);
double allSum = cvSum(tmpEigenValues).val[0];
double partSum = 0.0;
int nTruncated = 0;
double largesteigval = cvmGet(tmpEigenValues, 0, 0);
for(int i = 0; i < nEigenAtMost; i++)
{
double thiseigval = cvmGet(tmpEigenValues, 0, i);
if(thiseigval / largesteigval < 0.0001) break; // firstly check(remove small values)
partSum += thiseigval;
++ nTruncated;
if(partSum/allSum >= percentage) break; //secondly check
}
__TextureEigenValues = cvCreateMat(1, nTruncated, CV_64FC1);
__TextureEigenVectors = cvCreateMat(nTruncated, nPixels, CV_64FC1);
CvMat G;
cvGetCols(tmpEigenValues, &G, 0, nTruncated);
cvCopy(&G, __TextureEigenValues);
cvGetRows(tmpEigenVectors, &G, 0, nTruncated);
cvCopy(&G, __TextureEigenVectors);
cvReleaseMat(&tmpEigenVectors);
cvReleaseMat(&tmpEigenValues);
LOGD("Done (%d/%d)\n", nTruncated, nEigenAtMost);
}
// Ham tinh dac trung HOG tai moi block
void HOGProcessor::calcHOGForBlock(CvRect block, CvMat* hogBlock, IplImage** integrals,CvSize cell, int normalization)
{
int cellStartX, cellStartY;
CvMat vectorCell;
int startcol = 0;
for(cellStartY = block.y; cellStartY <= block.y + block.height - cell.height; cellStartY += cell.height)
{
for(cellStartX = block.x; cellStartX <= block.x + block.width - cell.width; cellStartX += cell.width)
{
cvGetCols(hogBlock, &vectorCell, startcol, startcol + NumOfBins);
calcHOGForCell(cvRect(cellStartX, cellStartY, cell.width, cell.height), &vectorCell, integrals, -1);
startcol += NumOfBins;
}
}
if(normalization != -1){
cvNormalize(hogBlock, hogBlock, 1, 0, normalization);
}
}
void FacePredict::CalcMeanShapeParams(const std::vector<AAM_Shape> &GroupShapes, int group)
{
int nSamples = GroupShapes.size();
CvMat mParams;
cvGetRow(__ShapeParamGroups, &mParams, group);
CvMat* p = cvCreateMat(1, __nShapeModes, CV_64FC1);
CvMat* pq = cvCreateMat(1, 4+__nShapeModes, CV_64FC1);
for (int i = 0; i < nSamples; i++) {
__shape.CalcParams(GroupShapes[i], pq);
cvGetCols(pq, p, 4, 4+__nShapeModes);
cvAdd(&mParams, p, &mParams);
}
CvMat * size = cvCreateMat(1, __nShapeModes, CV_64FC1);
for (int i = 0; i < __nShapeModes; i++)
cvmSet(size, 0, i, nSamples);
cvDiv(&mParams, size, &mParams);
cvReleaseMat(&p);
cvReleaseMat(&pq);
cvReleaseMat(&size);
}
//============================================================================
void AAM_IC::Fit(const IplImage* image, AAM_Shape& Shape,
int max_iter /* = 30 */, bool showprocess /* = false */)
{
//initialize some stuff
double t = gettime;
const CvMat* A0 = __texture.GetMean();
CvMat p; cvGetCols(__search_pq, &p, 4, 4+__shape.nModes());
Shape.Point2Mat(__current_s);
SetAllParamsZero();
__shape.CalcParams(__current_s, __search_pq);
IplImage* Drawimg = 0;
for(int iter = 0; iter < max_iter; iter++)
{
if(showprocess)
{
if(Drawimg == 0) Drawimg = cvCloneImage(image);
else cvCopy(image, Drawimg);
Shape.Mat2Point(__current_s);
Draw(Drawimg, Shape, 2);
mkdir("result");
char filename[100];
sprintf(filename, "result/Iter-%02d.jpg", iter);
cvSaveImage(filename, Drawimg);
}
//check the current shape
AAM_Common::CheckShape(__current_s, image->width, image->height);
//warp image to mesh shape mesh
__paw.CalcWarpTexture(__current_s, image, __warp_t);
AAM_TDM::NormalizeTexture(A0, __warp_t);
cvSub(__warp_t, A0, __error_t);
//calculate updates (and scale to account for linear lighting gain)
cvGEMM(__error_t, __G, 1, NULL, 1, __delta_pq, CV_GEMM_B_T);
//check for parameter convergence
if(cvNorm(__delta_pq) < 1e-6) break;
//apply inverse compositional algorithm to update parameters
InverseCompose(__delta_pq, __current_s, __update_s);
//smooth shape
cvAddWeighted(__current_s, 0.4, __update_s, 0.6, 0, __update_s);
//update parameters
__shape.CalcParams(__update_s, __search_pq);
//calculate constrained new shape
__shape.CalcShape(__search_pq, __update_s);
//check for shape convergence
if(cvNorm(__current_s, __update_s, CV_L2) < 0.001) break;
else cvCopy(__update_s, __current_s);
}
Shape.Mat2Point(__current_s);
t = gettime-t;
printf("AAM IC Fitting time cost %.3f millisec\n", t);
cvReleaseImage(&Drawimg);
}
bool
cvFindExtrinsicCameraParams3( const CvMat* obj_points,
const CvMat* img_points, const CvMat* A,
const CvMat* dist_coeffs,
CvMat* r_vec, CvMat* t_vec )
{
bool fGood = true;
const int max_iter = 20;
CvMat *_M = 0, *_Mxy = 0, *_m = 0, *_mn = 0, *_L = 0, *_J = 0;
CV_FUNCNAME( "cvFindExtrinsicCameraParams3" );
__BEGIN__;
int i, j, count;
double a[9], k[4] = { 0, 0, 0, 0 }, R[9], ifx, ify, cx, cy;
double Mc[3] = {0, 0, 0}, MM[9], U[9], V[9], W[3];
double JtJ[6*6], JtErr[6], JtJW[6], JtJV[6*6], delta[6], param[6];
CvPoint3D64f* M = 0;
CvPoint2D64f *m = 0, *mn = 0;
CvMat _a = cvMat( 3, 3, CV_64F, a );
CvMat _R = cvMat( 3, 3, CV_64F, R );
CvMat _r = cvMat( 3, 1, CV_64F, param );
CvMat _t = cvMat( 3, 1, CV_64F, param + 3 );
CvMat _Mc = cvMat( 1, 3, CV_64F, Mc );
CvMat _MM = cvMat( 3, 3, CV_64F, MM );
CvMat _U = cvMat( 3, 3, CV_64F, U );
CvMat _V = cvMat( 3, 3, CV_64F, V );
CvMat _W = cvMat( 3, 1, CV_64F, W );
CvMat _JtJ = cvMat( 6, 6, CV_64F, JtJ );
CvMat _JtErr = cvMat( 6, 1, CV_64F, JtErr );
CvMat _JtJW = cvMat( 6, 1, CV_64F, JtJW );
CvMat _JtJV = cvMat( 6, 6, CV_64F, JtJV );
CvMat _delta = cvMat( 6, 1, CV_64F, delta );
CvMat _param = cvMat( 6, 1, CV_64F, param );
CvMat _dpdr, _dpdt;
if( !CV_IS_MAT(obj_points) || !CV_IS_MAT(img_points) ||
!CV_IS_MAT(A) || !CV_IS_MAT(r_vec) || !CV_IS_MAT(t_vec) )
CV_ERROR( CV_StsBadArg, "One of required arguments is not a valid matrix" );
count = MAX(obj_points->cols, obj_points->rows);
CV_CALL( _M = cvCreateMat( 1, count, CV_64FC3 ));
CV_CALL( _Mxy = cvCreateMat( 1, count, CV_64FC2 ));
CV_CALL( _m = cvCreateMat( 1, count, CV_64FC2 ));
CV_CALL( _mn = cvCreateMat( 1, count, CV_64FC2 ));
M = (CvPoint3D64f*)_M->data.db;
m = (CvPoint2D64f*)_m->data.db;
mn = (CvPoint2D64f*)_mn->data.db;
CV_CALL( cvConvertPointsHomogenious( obj_points, _M ));
CV_CALL( cvConvertPointsHomogenious( img_points, _m ));
CV_CALL( cvConvert( A, &_a ));
if( dist_coeffs )
{
CvMat _k;
if( !CV_IS_MAT(dist_coeffs) ||
CV_MAT_DEPTH(dist_coeffs->type) != CV_64F &&
CV_MAT_DEPTH(dist_coeffs->type) != CV_32F ||
dist_coeffs->rows != 1 && dist_coeffs->cols != 1 ||
dist_coeffs->rows*dist_coeffs->cols*CV_MAT_CN(dist_coeffs->type) != 4 )
CV_ERROR( CV_StsBadArg,
"Distortion coefficients must be 1x4 or 4x1 floating-point vector" );
_k = cvMat( dist_coeffs->rows, dist_coeffs->cols,
CV_MAKETYPE(CV_64F,CV_MAT_CN(dist_coeffs->type)), k );
CV_CALL( cvConvert( dist_coeffs, &_k ));
}
if( CV_MAT_DEPTH(r_vec->type) != CV_64F && CV_MAT_DEPTH(r_vec->type) != CV_32F ||
r_vec->rows != 1 && r_vec->cols != 1 ||
r_vec->rows*r_vec->cols*CV_MAT_CN(r_vec->type) != 3 )
CV_ERROR( CV_StsBadArg, "Rotation vector must be 1x3 or 3x1 floating-point vector" );
if( CV_MAT_DEPTH(t_vec->type) != CV_64F && CV_MAT_DEPTH(t_vec->type) != CV_32F ||
t_vec->rows != 1 && t_vec->cols != 1 ||
t_vec->rows*t_vec->cols*CV_MAT_CN(t_vec->type) != 3 )
CV_ERROR( CV_StsBadArg,
"Translation vector must be 1x3 or 3x1 floating-point vector" );
ifx = 1./a[0]; ify = 1./a[4];
cx = a[2]; cy = a[5];
// normalize image points
// (unapply the intrinsic matrix transformation and distortion)
for( i = 0; i < count; i++ )
{
double x = (m[i].x - cx)*ifx, y = (m[i].y - cy)*ify, x0 = x, y0 = y;
// compensate distortion iteratively
if( dist_coeffs )
for( j = 0; j < 5; j++ )
{
double r2 = x*x + y*y;
double icdist = 1./(1 + k[0]*r2 + k[1]*r2*r2);
double delta_x = 2*k[2]*x*y + k[3]*(r2 + 2*x*x);
double delta_y = k[2]*(r2 + 2*y*y) + 2*k[3]*x*y;
x = (x0 - delta_x)*icdist;
y = (y0 - delta_y)*icdist;
}
mn[i].x = x; mn[i].y = y;
// calc mean(M)
Mc[0] += M[i].x;
Mc[1] += M[i].y;
Mc[2] += M[i].z;
}
Mc[0] /= count;
Mc[1] /= count;
Mc[2] /= count;
cvReshape( _M, _M, 1, count );
cvMulTransposed( _M, &_MM, 1, &_Mc );
cvSVD( &_MM, &_W, 0, &_V, CV_SVD_MODIFY_A + CV_SVD_V_T );
// initialize extrinsic parameters
if( W[2]/W[1] < 1e-3 || count < 4 )
{
// a planar structure case (all M's lie in the same plane)
double tt[3], h[9], h1_norm, h2_norm;
CvMat* R_transform = &_V;
CvMat T_transform = cvMat( 3, 1, CV_64F, tt );
CvMat _H = cvMat( 3, 3, CV_64F, h );
CvMat _h1, _h2, _h3;
if( V[2]*V[2] + V[5]*V[5] < 1e-10 )
cvSetIdentity( R_transform );
if( cvDet(R_transform) < 0 )
cvScale( R_transform, R_transform, -1 );
cvGEMM( R_transform, &_Mc, -1, 0, 0, &T_transform, CV_GEMM_B_T );
for( i = 0; i < count; i++ )
{
const double* Rp = R_transform->data.db;
const double* Tp = T_transform.data.db;
const double* src = _M->data.db + i*3;
double* dst = _Mxy->data.db + i*2;
dst[0] = Rp[0]*src[0] + Rp[1]*src[1] + Rp[2]*src[2] + Tp[0];
dst[1] = Rp[3]*src[0] + Rp[4]*src[1] + Rp[5]*src[2] + Tp[1];
}
cvFindHomography( _Mxy, _mn, &_H );
cvGetCol( &_H, &_h1, 0 );
_h2 = _h1; _h2.data.db++;
_h3 = _h2; _h3.data.db++;
h1_norm = sqrt(h[0]*h[0] + h[3]*h[3] + h[6]*h[6]);
h2_norm = sqrt(h[1]*h[1] + h[4]*h[4] + h[7]*h[7]);
cvScale( &_h1, &_h1, 1./h1_norm );
cvScale( &_h2, &_h2, 1./h2_norm );
cvScale( &_h3, &_t, 2./(h1_norm + h2_norm));
cvCrossProduct( &_h1, &_h2, &_h3 );
cvRodrigues2( &_H, &_r );
cvRodrigues2( &_r, &_H );
cvMatMulAdd( &_H, &T_transform, &_t, &_t );
cvMatMul( &_H, R_transform, &_R );
cvRodrigues2( &_R, &_r );
}
else
{
// non-planar structure. Use DLT method
double* L;
double LL[12*12], LW[12], LV[12*12], sc;
CvMat _LL = cvMat( 12, 12, CV_64F, LL );
CvMat _LW = cvMat( 12, 1, CV_64F, LW );
CvMat _LV = cvMat( 12, 12, CV_64F, LV );
CvMat _RRt, _RR, _tt;
CV_CALL( _L = cvCreateMat( 2*count, 12, CV_64F ));
L = _L->data.db;
for( i = 0; i < count; i++, L += 24 )
{
double x = -mn[i].x, y = -mn[i].y;
L[0] = L[16] = M[i].x;
L[1] = L[17] = M[i].y;
L[2] = L[18] = M[i].z;
L[3] = L[19] = 1.;
L[4] = L[5] = L[6] = L[7] = 0.;
L[12] = L[13] = L[14] = L[15] = 0.;
L[8] = x*M[i].x;
L[9] = x*M[i].y;
L[10] = x*M[i].z;
L[11] = x;
L[20] = y*M[i].x;
L[21] = y*M[i].y;
L[22] = y*M[i].z;
L[23] = y;
}
cvMulTransposed( _L, &_LL, 1 );
cvSVD( &_LL, &_LW, 0, &_LV, CV_SVD_MODIFY_A + CV_SVD_V_T );
_RRt = cvMat( 3, 4, CV_64F, LV + 11*12 );
cvGetCols( &_RRt, &_RR, 0, 3 );
cvGetCol( &_RRt, &_tt, 3 );
if( cvDet(&_RR) < 0 )
cvScale( &_RRt, &_RRt, -1 );
sc = cvNorm(&_RR);
cvSVD( &_RR, &_W, &_U, &_V, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T );
cvGEMM( &_U, &_V, 1, 0, 0, &_R, CV_GEMM_A_T );
cvScale( &_tt, &_t, cvNorm(&_R)/sc );
cvRodrigues2( &_R, &_r );
cvReleaseMat( &_L );
}
//
// Check if new r and t are good
//
if ( cvGetReal1D( r_vec, 0 ) ||
cvGetReal1D( r_vec, 1 ) ||
cvGetReal1D( r_vec, 2 ) ||
cvGetReal1D( t_vec, 0 ) ||
cvGetReal1D( t_vec, 1 ) ||
cvGetReal1D( t_vec, 2 ) )
{
//
// perfom this only when the old r and t exist.
//
CvMat * R_inv = cvCreateMat( 3, 3, CV_64FC1 );
CvMat * r_old = cvCreateMat( 3, 1, CV_64FC1 );
CvMat * R_old = cvCreateMat( 3, 3, CV_64FC1 );
CvMat * t_old = cvCreateMat( 3, 1, CV_64FC1 );
// get new center
cvInvert( &_R, R_inv );
double new_center[3];
CvMat newCenter = cvMat( 3, 1, CV_64FC1, new_center );
cvMatMul( R_inv, &_t, &newCenter );
cvScale( &newCenter, &newCenter, -1 );
// get old center
cvConvert( r_vec, r_old );
cvConvert( t_vec, t_old );
cvRodrigues2( r_old, R_old );
cvInvert( R_old, R_inv );
double old_center[3];
CvMat oldCenter = cvMat( 3, 1, CV_64FC1, old_center );
cvMatMul( R_inv, t_old, &oldCenter );
cvScale( &oldCenter, &oldCenter, -1 );
// get difference
double diff_center = 0;
for ( int i = 0; i < 3 ; i ++ )
diff_center += pow( new_center[i] - old_center[i], 2 );
diff_center = sqrt( diff_center );
if ( diff_center > 300 )
{
printf("diff_center = %.2f --> set initial r and t as same as the previous\n", diff_center);
cvConvert(r_vec, &_r);
cvConvert(t_vec, &_t);
fGood = false;
}
// else printf("diff_center = %.2f\n", diff_center );
cvReleaseMat( &R_inv );
cvReleaseMat( &r_old );
cvReleaseMat( &R_old );
cvReleaseMat( &t_old );
}
if ( fGood )
{
CV_CALL( _J = cvCreateMat( 2*count, 6, CV_64FC1 ));
cvGetCols( _J, &_dpdr, 0, 3 );
cvGetCols( _J, &_dpdt, 3, 6 );
// refine extrinsic parameters using iterative algorithm
for( i = 0; i < max_iter; i++ )
{
double n1, n2;
cvReshape( _mn, _mn, 2, 1 );
cvProjectPoints2( _M, &_r, &_t, &_a, dist_coeffs,
_mn, &_dpdr, &_dpdt, 0, 0, 0 );
cvSub( _m, _mn, _mn );
cvReshape( _mn, _mn, 1, 2*count );
cvMulTransposed( _J, &_JtJ, 1 );
cvGEMM( _J, _mn, 1, 0, 0, &_JtErr, CV_GEMM_A_T );
cvSVD( &_JtJ, &_JtJW, 0, &_JtJV, CV_SVD_MODIFY_A + CV_SVD_V_T );
if( JtJW[5]/JtJW[0] < 1e-12 )
break;
cvSVBkSb( &_JtJW, &_JtJV, &_JtJV, &_JtErr,
&_delta, CV_SVD_U_T + CV_SVD_V_T );
cvAdd( &_delta, &_param, &_param );
n1 = cvNorm( &_delta );
n2 = cvNorm( &_param );
if( n1/n2 < 1e-10 )
break;
}
_r = cvMat( r_vec->rows, r_vec->cols,
CV_MAKETYPE(CV_64F,CV_MAT_CN(r_vec->type)), param );
_t = cvMat( t_vec->rows, t_vec->cols,
CV_MAKETYPE(CV_64F,CV_MAT_CN(t_vec->type)), param + 3 );
cvConvert( &_r, r_vec );
cvConvert( &_t, t_vec );
}
__END__;
cvReleaseMat( &_M );
cvReleaseMat( &_Mxy );
cvReleaseMat( &_m );
cvReleaseMat( &_mn );
cvReleaseMat( &_L );
cvReleaseMat( &_J );
return fGood;
}
CvMat* cveGetCols(CvArr* arr, CvMat* submat, int startCol, int endCol)
{
return cvGetCols(arr, submat, startCol, endCol);
}
vector<Point> AAM::findPoints(Mat image)
{
float k=10;
int i=0;
Mat convHull = this->createConvexHull(this->meanPoints);
Mat points;
cout<<"mean shape"<<this->meanShape<<endl;
this->meanShape.copyTo(points);
cout<<"initial shape"<<points<<endl;
Mat deltaG;
double normE0;
Mat c;
do
{
cout<<"iteration "<<i<<endl;
cout<<"start Points: "<<points<<endl;
k=1;
//this->drawPoints(image, points, QString::number(i).toStdString());
Mat textureSampled=this->getTetureInShape(image, points);
textureSampled.convertTo(textureSampled, CV_32S);
this->meanTexture.convertTo(meanTexture, CV_32S);
Mat deltaT = textureSampled - this->meanTexture;
points.convertTo(points, meanShape.type());
Mat deltaPoint = points-meanShape;
// cout<<"deltaPoint: "<<deltaPoint<<endl;
deltaPoint=deltaPoint*this->w;
// cout<<"deltaPoint: "<<deltaPoint<<endl;
Mat b=this->textureVariationPCA.project(deltaT);
// cout<<"b for texture: "<<b<<endl;
Mat ap;
deltaPoint.convertTo(deltaPoint, b.type());
hconcat(deltaPoint, b, ap);
// cout<<"appearance: "<<ap<<endl;
c= this->appearancePCA.project(ap);
// cout<<"c: "<<c<<endl;
Mat modelAp=this->appearancePCA.backProject(c);
// cout<<"model appearance: "<<modelAp<<endl;
CvMat bModelMat;
CvMat modelApMat=modelAp;
cvGetCols(&modelApMat, &bModelMat, this->shapeSet.cols, this->shapeSet.cols + this->b_g.cols);
Mat bModel=cvarrToMat(&bModelMat);
bModel.convertTo(bModel, CV_32F);
// cout<<"b model"<<bModel<<endl;
Mat deltaGModel=this->textureVariationPCA.backProject(bModel);
// cout<<"delta g model: "<<deltaGModel<<endl;
deltaGModel.convertTo(deltaGModel, this->meanTexture.type());
Mat textureModel=this->meanTexture + deltaGModel;
textureModel.convertTo(textureModel, CV_8U);
textureModel.convertTo(textureModel, CV_32S);
// cout<<"texture model: "<<textureModel<<endl;
Mat modelShape=this->getShapeParamsFromC(c);
modelShape.convertTo(modelShape, this->meanShape.type());
modelShape=meanShape + modelShape;
modelShape=this->cutToRange(modelShape, 0, this->ImageHeight);
// cout<<"model shape: "<<modelShape<<endl;
Mat realTexture=this->getTetureInShape(image, modelShape);
realTexture.convertTo(realTexture, textureModel.type());
// cout<<"real texture"<<realTexture<<endl;
Mat deltaI=realTexture-textureModel;
// cout<<"deltaI: "<<deltaI<<endl;
deltaI.convertTo(deltaI, CV_32F);
Mat deltaC=deltaI*this->A;
cout<<"delta c"<<deltaC<<endl;
c.convertTo(c, deltaC.type());
normE0=this->euqlidesNorm(deltaI);
double normE=normE0;
Mat newC;
while(normE0<=normE && k>0.0001) //sprawdzić, czy deltaG jest mniejsze niż deltaG0
{
// cout<<"k "<<k<<endl;
newC=c+k*deltaC;
cout<<"newc: "<<newC<<endl;
Mat shapeParams=this->getShapeParamsFromC(newC);
Mat textureParams=this->getTextureParamsFromC(newC);
Mat textureDelta=this->textureVariationPCA.backProject(textureParams);
textureDelta.convertTo(textureDelta, meanTexture.type());
Mat texture=meanTexture + textureDelta;
shapeParams.convertTo(shapeParams, meanShape.type());
points=meanShape + shapeParams;
points=this->cutToRange(points, 0, this->ImageHeight);
Mat realTexture=this->getTetureInShape(image, points);
texture.convertTo(texture, realTexture.type());
deltaI=realTexture - texture;
normE=this->euqlidesNorm(deltaI);
k=k/2;
}
i++;
newC.copyTo(c);
normE0=normE;
cout<<"points: "<<points<<endl;
cout<<"error: "<<normE0<<endl;
}
while(i<30); // sprwdzić, czy błąd deltaG jest mniejszy od maxymalnego błędu
this->drawPoints(image, points, "final points");
cout<<"final points: "<<points<<endl;
cout<<"mean: "<<this->meanShape<<endl;
return this->convertMatToPoints(points);
}
CV_IMPL void
cvDecomposeProjectionMatrix( const CvMat *projMatr, CvMat *calibMatr,
CvMat *rotMatr, CvMat *posVect,
CvMat *rotMatrX, CvMat *rotMatrY,
CvMat *rotMatrZ, CvPoint3D64f *eulerAngles)
{
CvMat *tmpProjMatr = 0;
CvMat *tmpMatrixD = 0;
CvMat *tmpMatrixV = 0;
CvMat *tmpMatrixM = 0;
CV_FUNCNAME("cvDecomposeProjectionMatrix");
__BEGIN__;
/* Validate parameters. */
if(projMatr == 0 || calibMatr == 0 || rotMatr == 0 || posVect == 0)
CV_ERROR(CV_StsNullPtr, "Some of parameters is a NULL pointer!");
if(!CV_IS_MAT(projMatr) || !CV_IS_MAT(calibMatr) || !CV_IS_MAT(rotMatr) || !CV_IS_MAT(posVect))
CV_ERROR(CV_StsUnsupportedFormat, "Input parameters must be a matrices!");
if(projMatr->cols != 4 || projMatr->rows != 3)
CV_ERROR(CV_StsUnmatchedSizes, "Size of projection matrix must be 3x4!");
if(calibMatr->cols != 3 || calibMatr->rows != 3 || rotMatr->cols != 3 || rotMatr->rows != 3)
CV_ERROR(CV_StsUnmatchedSizes, "Size of calibration and rotation matrices must be 3x3!");
if(posVect->cols != 1 || posVect->rows != 4)
CV_ERROR(CV_StsUnmatchedSizes, "Size of position vector must be 4x1!");
CV_CALL(tmpProjMatr = cvCreateMat(4, 4, CV_64F));
CV_CALL(tmpMatrixD = cvCreateMat(4, 4, CV_64F));
CV_CALL(tmpMatrixV = cvCreateMat(4, 4, CV_64F));
CV_CALL(tmpMatrixM = cvCreateMat(3, 3, CV_64F));
/* Compute position vector. */
cvSetZero(tmpProjMatr); // Add zero row to make matrix square.
int i, k;
for(i = 0; i < 3; i++)
for(k = 0; k < 4; k++)
cvmSet(tmpProjMatr, i, k, cvmGet(projMatr, i, k));
CV_CALL(cvSVD(tmpProjMatr, tmpMatrixD, NULL, tmpMatrixV, CV_SVD_MODIFY_A + CV_SVD_V_T));
/* Save position vector. */
for(i = 0; i < 4; i++)
cvmSet(posVect, i, 0, cvmGet(tmpMatrixV, 3, i)); // Solution is last row of V.
/* Compute calibration and rotation matrices via RQ decomposition. */
cvGetCols(projMatr, tmpMatrixM, 0, 3); // M is first square matrix of P.
assert(cvDet(tmpMatrixM) != 0.0); // So far only finite cameras could be decomposed, so M has to be nonsingular [det(M) != 0].
CV_CALL(cvRQDecomp3x3(tmpMatrixM, calibMatr, rotMatr, rotMatrX, rotMatrY, rotMatrZ, eulerAngles));
__END__;
cvReleaseMat(&tmpProjMatr);
cvReleaseMat(&tmpMatrixD);
cvReleaseMat(&tmpMatrixV);
cvReleaseMat(&tmpMatrixM);
}
//============================================================================
void AAM_Basic::CalcJacobianMatrix(const file_lists& pts_files,
const file_lists& img_files,
double disp_scale /* = 0.2 */,
double disp_angle /* = 20 */,
double disp_trans /* = 5.0 */,
double disp_std /* = 1.0 */,
int nExp /* = 30 */)
{
CvMat* J = cvCreateMat(__cam.nModes()+4, __cam.__texture.nPixels(), CV_64FC1);
CvMat* d = cvCreateMat(1, __cam.nModes()+4, CV_64FC1);
CvMat* o = cvCreateMat(1, __cam.nModes()+4, CV_64FC1);
CvMat* oo = cvCreateMat(1, __cam.nModes()+4, CV_64FC1);
CvMat* t = cvCreateMat(1, __cam.__texture.nPixels(), CV_64FC1);
CvMat* t_m = cvCreateMat(1, __cam.__texture.nPixels(), CV_64FC1);
CvMat* t_s = cvCreateMat(1, __cam.__texture.nPixels(), CV_64FC1);
CvMat* t1 = cvCreateMat(1, __cam.__texture.nPixels(), CV_64FC1);
CvMat* t2 = cvCreateMat(1, __cam.__texture.nPixels(), CV_64FC1);
CvMat* u = cvCreateMat(1, __cam.nModes()+4, CV_64FC1);
CvMat* c = cvCreateMat(1, __cam.nModes(), CV_64FC1);
CvMat* s = cvCreateMat(1, __cam.__shape.nPoints()*2, CV_64FC1);
CvMat* q = cvCreateMat(1, 4, CV_64FC1);
CvMat* p = cvCreateMat(1, __cam.__shape.nModes(),CV_64FC1);
CvMat* lamda = cvCreateMat(1, __cam.__texture.nModes(), CV_64FC1);
double theta = disp_angle * CV_PI / 180;
double aa = MAX(fabs(disp_scale*cos(theta)), fabs(disp_scale*sin(theta)));
cvmSet(d,0,0,aa); cvmSet(d,0,1,aa); cvmSet(d,0,2,disp_trans); cvmSet(d,0,3,disp_trans);
for(int nmode = 0; nmode < __cam.nModes(); nmode++)
cvmSet(d,0,4+nmode,disp_std*sqrt(__cam.Var(nmode)));
srand(unsigned(time(0)));
cvSetZero(u);cvSetZero(J);
for(int i = 0; i < pts_files.size(); i++)
{
IplImage* image = cvLoadImage(img_files[i].c_str(), -1);
AAM_Shape Shape;
if(!Shape.ReadAnnotations(pts_files[i]))
Shape.ScaleXY(image->width, image->height);
Shape.Point2Mat(s);
//calculate current texture vector
__cam.__paw.CalcWarpTexture(s, image, t);
__cam.__texture.NormalizeTexture(__cam.__MeanG, t);
//calculate appearance parameters
__cam.__shape.CalcParams(s, p, q);
__cam.__texture.CalcParams(t, lamda);
__cam.CalcParams(c, p, lamda);
//update appearance and pose parameters
CvMat subo;
cvGetCols(o, &subo, 0, 4); cvCopy(q, &subo);
cvGetCols(o, &subo, 4, 4+__cam.nModes()); cvCopy(c, &subo);
//get optimal EstResidual
EstResidual(image, o, s, t_m, t_s, t1);
for(int j = 0; j < nExp; j++)
{
printf("Pertubing (%d/%d) for image (%d/%d)...\r", j, nExp, i, pts_files.size());
for(int l = 0; l < 4+__cam.nModes(); l++)
{
double D = cvmGet(d,0,l);
double v = rand_in_between(-D, D);
cvCopy(o, oo); CV_MAT_ELEM(*oo,double,0,l) += v;
EstResidual(image, oo, s, t_m, t_s, t2);
cvSub(t1, t2, t2);
cvConvertScale(t2, t2, 1.0/v);
//accumulate into l-th row
CvMat Jl; cvGetRow(J, &Jl, l);
cvAdd(&Jl, t2, &Jl);
CV_MAT_ELEM(*u, double, 0, l) += 1.0;
}
}
cvReleaseImage(&image);
}
//normalize
for(int l = 0; l < __cam.nModes()+4; l++)
{
CvMat Jl; cvGetRow(J, &Jl, l);
cvConvertScale(&Jl, &Jl, 1.0/cvmGet(u,0,l));
}
CvMat* JtJ = cvCreateMat(__cam.nModes()+4, __cam.nModes()+4, CV_64FC1);
CvMat* InvJtJ = cvCreateMat(__cam.nModes()+4, __cam.nModes()+4, CV_64FC1);
cvGEMM(J, J, 1, NULL, 0, JtJ, CV_GEMM_B_T);
cvInvert(JtJ, InvJtJ, CV_SVD);
cvMatMul(InvJtJ, J, __G);
cvReleaseMat(&J); cvReleaseMat(&d); cvReleaseMat(&o);
cvReleaseMat(&oo); cvReleaseMat(&t); cvReleaseMat(&t_s);
cvReleaseMat(&t_m); cvReleaseMat(&t1); cvReleaseMat(&t2);
cvReleaseMat(&u); cvReleaseMat(&c); cvReleaseMat(&s);
cvReleaseMat(&q); cvReleaseMat(&p); cvReleaseMat(&lamda);
cvReleaseMat(&JtJ); cvReleaseMat(&InvJtJ);
}
int icvIPPSepFilter( const CvMat* src, CvMat* dst, const CvMat* kernelX,
const CvMat* kernelY, CvPoint anchor )
{
int result = 0;
CvMat* top_bottom = 0;
CvMat* vout_hin = 0;
CvMat* dst_buf = 0;
CV_FUNCNAME( "icvIPPSepFilter" );
__BEGIN__;
CvSize ksize;
CvPoint el_anchor;
CvSize size;
int type, depth, pix_size;
int i, x, y, dy = 0, prev_dy = 0, max_dy;
CvMat vout;
CvCopyNonConstBorderFunc copy_border_func;
CvIPPSepFilterFunc x_func = 0, y_func = 0;
int src_step, top_bottom_step;
float *kx, *ky;
int align, stripe_size;
if( !icvFilterRow_8u_C1R_p )
EXIT;
if( !CV_ARE_TYPES_EQ( src, dst ) || !CV_ARE_SIZES_EQ( src, dst ) ||
!CV_IS_MAT_CONT(kernelX->type & kernelY->type) ||
CV_MAT_TYPE(kernelX->type) != CV_32FC1 ||
CV_MAT_TYPE(kernelY->type) != CV_32FC1 ||
kernelX->cols != 1 && kernelX->rows != 1 ||
kernelY->cols != 1 && kernelY->rows != 1 ||
(unsigned)anchor.x >= (unsigned)(kernelX->cols + kernelX->rows - 1) ||
(unsigned)anchor.y >= (unsigned)(kernelY->cols + kernelY->rows - 1) )
CV_ERROR( CV_StsError, "Internal Error: incorrect parameters" );
ksize.width = kernelX->cols + kernelX->rows - 1;
ksize.height = kernelY->cols + kernelY->rows - 1;
/*if( ksize.width <= 5 && ksize.height <= 5 )
{
float* ker = (float*)cvStackAlloc( ksize.width*ksize.height*sizeof(ker[0]));
CvMat kernel = cvMat( ksize.height, ksize.width, CV_32F, ker );
for( y = 0, i = 0; y < ksize.height; y++ )
for( x = 0; x < ksize.width; x++, i++ )
ker[i] = kernelY->data.fl[y]*kernelX->data.fl[x];
CV_CALL( cvFilter2D( src, dst, &kernel, anchor ));
EXIT;
}*/
type = CV_MAT_TYPE(src->type);
depth = CV_MAT_DEPTH(type);
pix_size = CV_ELEM_SIZE(type);
if( type == CV_8UC1 )
x_func = icvFilterRow_8u_C1R_p, y_func = icvFilterColumn_8u_C1R_p;
else if( type == CV_8UC3 )
x_func = icvFilterRow_8u_C3R_p, y_func = icvFilterColumn_8u_C3R_p;
else if( type == CV_8UC4 )
x_func = icvFilterRow_8u_C4R_p, y_func = icvFilterColumn_8u_C4R_p;
else if( type == CV_16SC1 )
x_func = icvFilterRow_16s_C1R_p, y_func = icvFilterColumn_16s_C1R_p;
else if( type == CV_16SC3 )
x_func = icvFilterRow_16s_C3R_p, y_func = icvFilterColumn_16s_C3R_p;
else if( type == CV_16SC4 )
x_func = icvFilterRow_16s_C4R_p, y_func = icvFilterColumn_16s_C4R_p;
else if( type == CV_32FC1 )
x_func = icvFilterRow_32f_C1R_p, y_func = icvFilterColumn_32f_C1R_p;
else if( type == CV_32FC3 )
x_func = icvFilterRow_32f_C3R_p, y_func = icvFilterColumn_32f_C3R_p;
else if( type == CV_32FC4 )
x_func = icvFilterRow_32f_C4R_p, y_func = icvFilterColumn_32f_C4R_p;
else
EXIT;
size = cvGetMatSize(src);
stripe_size = src->data.ptr == dst->data.ptr ? 1 << 15 : 1 << 16;
max_dy = MAX( ksize.height - 1, stripe_size/(size.width + ksize.width - 1));
max_dy = MIN( max_dy, size.height + ksize.height - 1 );
align = 8/CV_ELEM_SIZE(depth);
CV_CALL( top_bottom = cvCreateMat( ksize.height*2, cvAlign(size.width,align), type ));
CV_CALL( vout_hin = cvCreateMat( max_dy + ksize.height,
cvAlign(size.width + ksize.width - 1, align), type ));
if( src->data.ptr == dst->data.ptr && size.height )
CV_CALL( dst_buf = cvCreateMat( max_dy + ksize.height,
cvAlign(size.width, align), type ));
kx = (float*)cvStackAlloc( ksize.width*sizeof(kx[0]) );
ky = (float*)cvStackAlloc( ksize.height*sizeof(ky[0]) );
// mirror the kernels
for( i = 0; i < ksize.width; i++ )
kx[i] = kernelX->data.fl[ksize.width - i - 1];
for( i = 0; i < ksize.height; i++ )
ky[i] = kernelY->data.fl[ksize.height - i - 1];
el_anchor = cvPoint( ksize.width - anchor.x - 1, ksize.height - anchor.y - 1 );
cvGetCols( vout_hin, &vout, anchor.x, anchor.x + size.width );
copy_border_func = icvGetCopyNonConstBorderFunc( pix_size, IPL_BORDER_REPLICATE );
src_step = src->step ? src->step : CV_STUB_STEP;
top_bottom_step = top_bottom->step ? top_bottom->step : CV_STUB_STEP;
vout.step = vout.step ? vout.step : CV_STUB_STEP;
for( y = 0; y < size.height; y += dy )
{
const CvMat *vin = src, *hout = dst;
int src_y = y, dst_y = y;
dy = MIN( max_dy, size.height - (ksize.height - anchor.y - 1) - y );
if( y < anchor.y || dy < anchor.y )
{
int ay = anchor.y;
CvSize src_stripe_size = size;
if( y < anchor.y )
{
src_y = 0;
dy = MIN( anchor.y, size.height );
src_stripe_size.height = MIN( dy + ksize.height - anchor.y - 1, size.height );
}
else
{
src_y = MAX( y - anchor.y, 0 );
dy = size.height - y;
src_stripe_size.height = MIN( dy + anchor.y, size.height );
ay = MAX( anchor.y - y, 0 );
}
copy_border_func( src->data.ptr + src_y*src_step, src_step, src_stripe_size,
top_bottom->data.ptr, top_bottom_step,
cvSize(size.width, dy + ksize.height - 1),
ay, 0 );
vin = top_bottom;
src_y = anchor.y;
}
// do vertical convolution
IPPI_CALL( y_func( vin->data.ptr + src_y*vin->step, vin->step ? vin->step : CV_STUB_STEP,
vout.data.ptr, vout.step, cvSize(size.width, dy),
ky, ksize.height, el_anchor.y ));
// now it's time to copy the previously processed stripe to the input/output image
if( src->data.ptr == dst->data.ptr )
{
for( i = 0; i < prev_dy; i++ )
memcpy( dst->data.ptr + (y - prev_dy + i)*dst->step,
dst_buf->data.ptr + i*dst_buf->step, size.width*pix_size );
if( y + dy < size.height )
{
hout = dst_buf;
dst_y = 0;
}
}
// create a border for every line by replicating the left-most/right-most elements
for( i = 0; i < dy; i++ )
{
uchar* ptr = vout.data.ptr + i*vout.step;
for( x = -1; x >= -anchor.x*pix_size; x-- )
ptr[x] = ptr[x + pix_size];
for( x = size.width*pix_size; x < (size.width+ksize.width-anchor.x-1)*pix_size; x++ )
ptr[x] = ptr[x - pix_size];
}
// do horizontal convolution
IPPI_CALL( x_func( vout.data.ptr, vout.step, hout->data.ptr + dst_y*hout->step,
hout->step ? hout->step : CV_STUB_STEP,
cvSize(size.width, dy), kx, ksize.width, el_anchor.x ));
prev_dy = dy;
}
result = 1;
__END__;
cvReleaseMat( &vout_hin );
cvReleaseMat( &dst_buf );
cvReleaseMat( &top_bottom );
return result;
}
void AdjustBMParam::tryParam(){
int preFilterSz = this->ui->preFilterSz->value();
int preFilterCap = this->ui->preFilterCap->value();
int sadWndSz = this->ui->SADWndSz->value();
int minDisp = this->ui->minDisp->value();
int numDisp = 16*(this->ui->numDisp->value());
int texture = this->ui->textureth->value();
int uniquness = this->ui->uniq->value();
int spkWndSz = this->ui->speckleWndSz->value();
int spkRange = this->ui->specklerange->value();
if(sadWndSz%2==0)
sadWndSz++;
if(sadWndSz<5)
sadWndSz = 5;
if(preFilterSz%2==0)
preFilterSz++;
if(preFilterSz<5)
preFilterSz = 5;
CvStereoBMState *BMState = cvCreateStereoBMState();
assert(BMState != 0);
BMState->preFilterSize=preFilterSz;
BMState->preFilterCap=preFilterCap;
BMState->SADWindowSize=sadWndSz;
BMState->minDisparity= minDisp;
BMState->numberOfDisparities=numDisp;
BMState->textureThreshold=texture; //reduce noise
BMState->uniquenessRatio=uniquness;
BMState->speckleWindowSize = spkWndSz;
BMState->speckleRange = spkRange;
stereoCalibration->setBMState(BMState);
stereoCalibration->computeDisparity(leftImage, rightImage);
CvMat* pair = cvCreateMat(leftImage->height, leftImage->width*2, CV_8UC3 );
CvMat part;
cvGetCols( pair, &part, 0, leftImage->width );
cvCvtColor( stereoCalibration->leftImageRectified, &part, CV_GRAY2BGR );
cvGetCols( pair, &part, leftImage->width,leftImage->width*2 );
cvCvtColor( stereoCalibration->rightImageRectified, &part, CV_GRAY2BGR);
for(int j = 0; j < leftImage->height; j += 25 )
cvLine( pair, cvPoint(0,j),cvPoint(leftImage->width*2,j),CV_RGB(255,0,0));
cvReleaseImage(&dispartiyImageColor);
dispartiyImageColor = cvCreateImage (cvGetSize(leftImage),IPL_DEPTH_8U, 3);
if (dispartiyImageColor == NULL)
{
fprintf(stderr,"ERROR: Could not create pseudocolor image.\n");
exit(2);
}
IplImage *img, img_header;
img = cvGetImage(stereoCalibration->imageDisparityNormalized, &img_header);
disparityMapColor->g2cd_gray_to_chromadepth(img, this->dispartiyImageColor, this->g2cd_data);
cvShowImage("disparity coloured", dispartiyImageColor);
cvNamedWindow( "rectified", 1 );
cvShowImage( "rectified", pair );
// cvReleaseStereoBMState(&BMState);
//cvReleaseImage(&img);
}
// @Note: Ham nay se duoc dieu chinh lai sau
// Under reconstruction....
CvMat* HOGProcessor::calcHOGWindow(IplImage *img, IplImage** integrals, CvRect window, int normalization)
{
int blockStartX, blockStartY;
int cellWidth = m_cell.width, cellHeight = m_cell.height;
int blockWidth = m_block.width, blockHeight = m_block.height;
// Kich thuoc vector dac trung cho moi cua so
// = blockFeatureSize * numOfBlocks
int blockFeatureSize = NumOfBins * m_block.height * m_block.width;
int num_overlap_block_of_width = ((window.width - cellWidth * blockWidth) / (cellWidth * m_fStepOverlap)) + 1;
int num_overlap_block_of_height = ((window.height - cellHeight * blockHeight)/ (cellHeight * m_fStepOverlap)) + 1;
int totalBlockOfWindow = num_overlap_block_of_width * num_overlap_block_of_height ;
CvMat* windowFeatureVector = cvCreateMat(1, totalBlockOfWindow * blockFeatureSize, CV_32FC1);
CvMat vector_block;
int startcol = 0;
cvNamedWindow("aaa");
IplImage *imgHog = cvCreateImage(cvSize(9,9), img->depth, 1);
IplImage *imgHogShow = cvCreateImage(cvSize(81,81), img->depth, 1);
for(blockStartY = window.y;
blockStartY <= window.y + window.height - cellHeight * blockHeight;
blockStartY += cellHeight * m_fStepOverlap)
{//overllap
for(blockStartX = window.x;
blockStartX <= window.x + window.width - cellWidth * blockWidth;
blockStartX += cellWidth * m_fStepOverlap)
{//overllap
cvGetCols(windowFeatureVector, &vector_block, startcol, startcol+36);
calcHOGForBlock(cvRect(blockStartX,
blockStartY, cellWidth * blockWidth, cellHeight
* blockHeight), &vector_block, integrals, cvSize(
cellWidth, cellHeight), normalization);
startcol += blockFeatureSize;
int row=0;
int col=0;
for(row = 0; row < 9; row++)
{
for(col = 0; col < 9; col++)
{
float cell1 = CV_MAT_ELEM(vector_block, float, 0, (9 * row + col));
CV_IMAGE_ELEM(imgHog, uchar, row, col) = cell1 * 255;
}
}
cvResize(imgHog, imgHogShow);
cvShowImage("aaa", imgHogShow);
cvWaitKey();
}
}
cvReleaseImage(&imgHog);
cvReleaseImage(&imgHogShow);
return windowFeatureVector;
}
//============================================================================
void AAM_CAM::Train(const file_lists& pts_files,
const file_lists& img_files,
double scale /* = 1.0 */,
double shape_percentage /* = 0.975 */,
double texture_percentage /* = 0.975 */,
double appearance_percentage /* = 0.975 */)
{
//building shape and texture distribution model
std::vector<AAM_Shape> AllShapes;
for(int ii = 0; ii < pts_files.size(); ii++)
{
AAM_Shape Shape;
bool flag = Shape.ReadAnnotations(pts_files[ii]);
if(!flag)
{
IplImage* image = cvLoadImage(img_files[ii].c_str(), -1);
Shape.ScaleXY(image->width, image->height);
cvReleaseImage(&image);
}
AllShapes.push_back(Shape);
}
printf("Build point distribution model...\n");
__shape.Train(AllShapes, scale, shape_percentage);
printf("Build warp information of mean shape mesh...");
__Points = cvCreateMat (1, __shape.nPoints(), CV_32FC2);
__Storage = cvCreateMemStorage(0);
AAM_Shape refShape = __shape.__AAMRefShape/* * scale */;
//if(refShape.GetWidth() > 50)
// refShape.Scale(50/refShape.GetWidth());
__paw.Train(refShape, __Points, __Storage);
printf("[%d by %d, %d triangles, %d*3 pixels]\n",
__paw.Width(), __paw.Height(), __paw.nTri(), __paw.nPix());
printf("Build texture distribution model...\n");
__texture.Train(pts_files, img_files, __paw, texture_percentage, true);
__pq = cvCreateMat(1, __shape.nModes()+4, CV_64FC1);
printf("Build combined appearance model...\n");
int nsamples = pts_files.size();
int npointsby2 = __shape.nPoints()*2;
int npixels = __texture.nPixels();
int nfeatures = __shape.nModes() + __texture.nModes();
CvMat* AllAppearances = cvCreateMat(nsamples, nfeatures, CV_64FC1);
CvMat* s = cvCreateMat(1, npointsby2, CV_64FC1);
CvMat* t = cvCreateMat(1, npixels, CV_64FC1);
__MeanS = cvCreateMat(1, npointsby2, CV_64FC1);
__MeanG = cvCreateMat(1, npixels, CV_64FC1);
cvCopy(__shape.GetMean(), __MeanS);
cvCopy(__texture.GetMean(), __MeanG);
//calculate ratio of shape to appearance
CvScalar Sum1 = cvSum(__shape.__ShapesEigenValues);
CvScalar Sum2 = cvSum(__texture.__TextureEigenValues);
__WeightsS2T = sqrt(Sum2.val[0] / Sum1.val[0]);
printf("Combine shape and texture parameters...\n");
for(int i = 0; i < nsamples; i++)
{
//Get Shape and Texture respectively
IplImage* image = cvLoadImage(img_files[i].c_str(), -1);
AAM_Shape Shape;
if(!Shape.ReadAnnotations(pts_files[i]))
Shape.ScaleXY(image->width, image->height);
Shape.Point2Mat(s);
AAM_Common::CheckShape(s, image->width, image->height);
__paw.CalcWarpTexture(s, image, t);
__texture.NormalizeTexture(__MeanG, t);
//combine shape and texture parameters
CvMat OneAppearance;
cvGetRow(AllAppearances, &OneAppearance, i);
ShapeTexture2Combined(s, t, &OneAppearance);
cvReleaseImage(&image);
}
//Do PCA of appearances
DoPCA(AllAppearances, appearance_percentage);
int np = __AppearanceEigenVectors->rows;
printf("Extracting the shape and texture part of the combined eigen vectors..\n");
// extract the shape part of the combined eigen vectors
CvMat Ps;
cvGetCols(__AppearanceEigenVectors, &Ps, 0, __shape.nModes());
__Qs = cvCreateMat(np, npointsby2, CV_64FC1);
cvMatMul(&Ps, __shape.GetBases(), __Qs);
cvConvertScale(__Qs, __Qs, 1.0/__WeightsS2T);
// extract the texture part of the combined eigen vectors
CvMat Pg;
cvGetCols(__AppearanceEigenVectors, &Pg, __shape.nModes(), nfeatures);
__Qg = cvCreateMat(np, npixels, CV_64FC1);
cvMatMul(&Pg, __texture.GetBases(), __Qg);
__a = cvCreateMat(1, __AppearanceEigenVectors->cols, CV_64FC1);
}
int RandomTrees::train(const char* samples_filename, const char* model_filename, const double ratio, double &train_error, double &test_error)
{
CvMat* data = 0;
CvMat* responses = 0;
CvMat* var_type = 0;
CvMat* sample_idx = 0;
this->tree_parameters_.nactive_vars = (int)sqrt(this->number_of_features_);
int ok = read_num_class_data( samples_filename, this->number_of_features_, &data, &responses );
int nsamples_all = 0, ntrain_samples = 0;
int i = 0;
double train_hr = 0, test_hr = 0;
CvRTrees forest;
CvMat* var_importance = 0;
if( !ok )
{
cout << "Could not read the sample in" << samples_filename << endl;;
return -1;
}
cout << "The sample file " << samples_filename << " is loaded." << endl;
nsamples_all = data->rows;
ntrain_samples = (int)(nsamples_all * ratio);
// create classifier by using <data> and <responses>
cout << "Training the classifier ..." << endl;
// 1. create type mask
var_type = cvCreateMat( data->cols + 1, 1, CV_8U );
cvSet( var_type, cvScalarAll(CV_VAR_ORDERED) );
cvSetReal1D( var_type, data->cols, CV_VAR_CATEGORICAL );
// 2. create sample_idx
sample_idx = cvCreateMat( 1, nsamples_all, CV_8UC1 );
{
CvMat mat;
cvGetCols( sample_idx, &mat, 0, ntrain_samples );
cvSet( &mat, cvRealScalar(1) );
cvGetCols( sample_idx, &mat, ntrain_samples, nsamples_all );
cvSetZero( &mat );
}
// 3. train classifier
forest.train( data, CV_ROW_SAMPLE, responses, 0, sample_idx, var_type, 0, this->tree_parameters_);
cout << endl;
// compute prediction error on train and test data
for( i = 0; i < nsamples_all; i++ )
{
double r;
CvMat sample;
cvGetRow( data, &sample, i );
r = forest.predict( &sample );
r = fabs((double)r - responses->data.fl[i]) <= FLT_EPSILON ? 1 : 0;
if( i < ntrain_samples )
train_hr += r;
else
test_hr += r;
}
test_hr /= (double)(nsamples_all-ntrain_samples);
train_hr /= (double)ntrain_samples;
train_error = 1 - train_hr;
test_error = 1 - test_hr;
cout << "Recognition rate: train = " << train_hr*100 << ", test = " << test_hr*100 << endl;
cout << "Number of trees: " << forest.get_tree_count() << endl;
// Print variable importance
var_importance = (CvMat*)forest.get_var_importance();
if( var_importance )
{
double rt_imp_sum = cvSum( var_importance ).val[0];
printf("var#\timportance (in %%):\n");
for( i = 0; i < var_importance->cols; i++ )
printf( "%-2d\t%-4.1f\n", i,100.f*var_importance->data.fl[i]/rt_imp_sum);
}
// Save Random Trees classifier to file if needed
if( model_filename )
forest.save( model_filename );
//cvReleaseMat( &var_importance ); //causes a segmentation fault
cvReleaseMat( &sample_idx );
cvReleaseMat( &var_type );
cvReleaseMat( &data );
cvReleaseMat( &responses );
return 0;
}
IplImage* FacePredict::predict(const AAM_Shape& Shape, const IplImage& curImage,
const AAM_Shape& ShapeF, const IplImage& ImageF, double RatioF,
const AAM_Shape& ShapeM, const IplImage& ImageM, double RatioM,
int curAgeG, int newAgeG, bool save)
{
if (newAgeG > NGROUPS || curAgeG > NGROUPS)
{
fprintf(stderr, "ERROE(%s, %d): Age group larger than %d\n",
__FILE__, __LINE__, NGROUPS);
exit(0);
}
if(curImage.nChannels != 3 || curImage.depth != 8)
{
fprintf(stderr, "ERROR(%s: %d): The image channels must be 3, "
"and the depth must be 8!\n", __FILE__, __LINE__);
exit(0);
}
/*get the current shape parameters*/
AAM_Shape curShape = Shape;
curShape.Centralize();
double thisfacewidth = curShape.GetWidth();
if(stdwidth < thisfacewidth)
curShape.Scale(stdwidth / thisfacewidth);
curShape.AlignTo(__AAMRefShape);
CvMat* p = cvCreateMat(1, __nShapeModes, CV_64FC1);
CvMat* pq = cvCreateMat(1, 4+__nShapeModes, CV_64FC1);
__shape.CalcParams(curShape, pq);
cvGetCols(pq, p, 4, 4+__nShapeModes);
/*get the current texture parameters*/
CvMat* curTexture = cvCreateMat(1, __paw.nPix() * 3, CV_64FC1);
__paw.FasterGetWarpTextureFromShape(Shape, &curImage, curTexture, false);
__texture.AlignTextureToRef(__MeanT, curTexture);
CvMat* lamda = cvCreateMat(1, __nTextureModes, CV_64FC1);
__texture.CalcParams(curTexture, lamda);
//father
CvMat* pF = cvCreateMat(1, __nShapeModes, CV_64FC1);
CvMat* lamdaF = cvCreateMat(1, __nTextureModes, CV_64FC1);
if (RatioF == 0) {
cvZero(pF);
cvZero(lamdaF);
}
else {
AAM_Shape shapeF = ShapeF;
shapeF.Centralize();
thisfacewidth = ShapeF.GetWidth();
if(stdwidth < thisfacewidth)
shapeF.Scale(stdwidth / thisfacewidth);
shapeF.AlignTo(__AAMRefShape);
CvMat* pqF = cvCreateMat(1, 4+__nShapeModes, CV_64FC1);
__shape.CalcParams(shapeF, pqF);
cvGetCols(pqF, pF, 4, 4+__nShapeModes);
CvMat* TextureF = cvCreateMat(1, __paw.nPix() * 3, CV_64FC1);
__paw.FasterGetWarpTextureFromShape(ShapeF, &ImageF, TextureF, false);
__texture.AlignTextureToRef(__MeanT, TextureF);
__texture.CalcParams(TextureF, lamdaF);
}
//mother
CvMat* pM = cvCreateMat(1, __nShapeModes, CV_64FC1);
CvMat* lamdaM = cvCreateMat(1, __nTextureModes, CV_64FC1);
if (RatioM == 0) {
cvZero(pM);
cvZero(lamdaM);
}
else {
AAM_Shape shapeM = ShapeM;
shapeM.Centralize();
thisfacewidth = ShapeM.GetWidth();
if(stdwidth < thisfacewidth)
shapeM.Scale(stdwidth / thisfacewidth);
shapeM.AlignTo(__AAMRefShape);
CvMat* pqM = cvCreateMat(1, 4+__nShapeModes, CV_64FC1);
__shape.CalcParams(shapeM, pqM);
cvGetCols(pqM, pM, 4, 4+__nShapeModes);
CvMat* TextureM = cvCreateMat(1, __paw.nPix() * 3, CV_64FC1);
__paw.FasterGetWarpTextureFromShape(ShapeM, &ImageM, TextureM, false);
__texture.AlignTextureToRef(__MeanT, TextureM);
__texture.CalcParams(TextureM, lamdaM);
}
/*caculate new shape and texture parameters*/
CvMat newShapeParams;
CvMat* newpq = cvCreateMat(1, 4+__nShapeModes, CV_64FC1);
cvmSet(newpq, 0, 0, cvmGet(pq, 0, 0)); cvmSet(newpq, 0, 1, cvmGet(pq, 0, 1));
cvmSet(newpq, 0, 2, cvmGet(pq, 0, 2)); cvmSet(newpq, 0, 3, cvmGet(pq, 0, 3));
cvGetCols(newpq, &newShapeParams, 4, 4+__nShapeModes);
CvMat* newSP = cvCreateMat(1, __nShapeModes, CV_64FC1);
FacePredict::CalcNewShapeParams(p, newSP, curAgeG, newAgeG);
FacePredict::CalcParamsByRatio(newSP, pF, RatioF, pM, RatioM, &newShapeParams);
CvMat* newTP = cvCreateMat(1, __nTextureModes, CV_64FC1);
FacePredict::CalcNewTextureParams(lamda, newTP, curAgeG, newAgeG);
CvMat* newTextureParams = cvCreateMat(1, __nTextureModes, CV_64FC1);
FacePredict::CalcParamsByRatio(newTP, lamdaF, RatioF, lamdaM, RatioM, newTextureParams);
/*calculate the new shape and texture*/
AAM_Shape newShape;
__shape.CalcShape(newpq, newShape);
CvMat* newTexture = cvCreateMat(1, __paw.nPix() * 3, CV_64FC1);
__texture.CalcTexture(newTextureParams, newTexture);
/*systhetize the shape and texture*/
IplImage* newImage = cvCreateImage(cvSize(stdwidth, stdwidth / newShape.GetWidth() * newShape.GetHeight()), IPL_DEPTH_8U, 3);
FacePredict::FaceSynthesis(newShape, newTexture, newImage);
if(save)
cvSaveImage("facial prediction.jpg", newImage);
cvReleaseMat(&p);
cvReleaseMat(&pq);
cvReleaseMat(&curTexture);
cvReleaseMat(&lamda);
cvReleaseMat(&newTextureParams);
cvReleaseMat(&newpq);
cvReleaseMat(&newTexture);
return newImage;
}
int CybCamCalibration::calibration(CvSeq* image_points_seq, CvSize img_size, CvSize board_size,
float square_size, float aspect_ratio, int flags, CvMat* camera_matrix,
CvMat* dist_coeffs, CvMat** extr_params, CvMat** reproj_errs,
double* avg_reproj_err) {
int code;
int image_count = image_points_seq->total;
int point_count = board_size.width*board_size.height;
CvMat* image_points = cvCreateMat( 1, image_count*point_count, CV_32FC2);
CvMat* object_points = cvCreateMat( 1, image_count*point_count, CV_32FC3);
CvMat* point_counts = cvCreateMat( 1, image_count, CV_32SC1);
CvMat rot_vects, trans_vects;
int i, j, k;
CvSeqReader reader;
cvStartReadSeq(image_points_seq, &reader);
// initialize arrays of points
for (i = 0; i < image_count; i++) {
CvPoint2D32f* src_img_pt = (CvPoint2D32f*)reader.ptr;
CvPoint2D32f* dst_img_pt = ((CvPoint2D32f*)image_points->data.fl) + i
*point_count;
CvPoint3D32f* obj_pt = ((CvPoint3D32f*)object_points->data.fl) + i
*point_count;
for (j = 0; j < board_size.height; j++)
for (k = 0; k < board_size.width; k++) {
*obj_pt++ = cvPoint3D32f(j*square_size, k*square_size, 0);
*dst_img_pt++ = *src_img_pt++;
}
CV_NEXT_SEQ_ELEM( image_points_seq->elem_size, reader );
}
cvSet(point_counts, cvScalar(point_count) );
*extr_params = cvCreateMat(image_count, 6, CV_32FC1);
cvGetCols( *extr_params, &rot_vects, 0, 3);
cvGetCols( *extr_params, &trans_vects, 3, 6);
cvZero( camera_matrix );
cvZero( dist_coeffs );
if (flags & CV_CALIB_FIX_ASPECT_RATIO) {
camera_matrix->data.db[0] = aspect_ratio;
camera_matrix->data.db[4] = 1.;
}
cvCalibrateCamera2(object_points, image_points, point_counts, img_size,
camera_matrix, dist_coeffs, &rot_vects, &trans_vects, flags);
code = cvCheckArr(camera_matrix, CV_CHECK_QUIET) && cvCheckArr(dist_coeffs,
CV_CHECK_QUIET) && cvCheckArr( *extr_params, CV_CHECK_QUIET);
*reproj_errs = cvCreateMat( 1, image_count, CV_64FC1);
*avg_reproj_err = compute_error(object_points, &rot_vects,
&trans_vects, camera_matrix, dist_coeffs, image_points,
point_counts, *reproj_errs);
cvReleaseMat( &object_points);
cvReleaseMat( &image_points);
cvReleaseMat( &point_counts);
return code;
}
void AAM::countA()
{
Mat deltaC;
Mat deltaI;
for(int i=0; i<this->shapeSet.rows; i++)
{
for(int j=0; j<100; j++)
{
Mat ds=randomPointsMat(40, this->shapeSet.cols);
cout<<"random ds: "<<ds<<endl;
ds.convertTo(ds, this->shapeSet.type());
Mat newShape=ds+this->shapeSet.row(i);
newShape.convertTo(newShape, meanShape.type());
Mat newShapeParams=newShape-meanShape;
newShapeParams=newShapeParams*this->w;
Mat image_grey;
cvtColor(this->images[i], image_grey, COLOR_BGR2GRAY);
Mat newTexture=this->getTetureInShape(image_grey, newShape);
newTexture.convertTo(newTexture, meanTexture.type());
Mat newDeltaTexture=newTexture - meanTexture;
Mat newTextureParams=this->textureVariationPCA.project(newDeltaTexture);
Mat newAp;
newShapeParams.convertTo(newShapeParams, newTextureParams.type());
hconcat(newShapeParams, newTextureParams, newAp);
cout<<"new appearance: " <<newAp<<endl;
Mat c=this->appearancePCA.project(newAp);
cout<<"new c: "<<c<<endl;
c.convertTo(c, cSet.type());
Mat dc=this->cSet.row(i)-c;
cout<<"dc: "<<dc<<endl;
Mat ap = this->appearancePCA.backProject(c);
CvMat apMat=ap;
cout<<"appearance: "<<ap<<endl;
CvMat shapeParams;
cvGetCols(&apMat, &shapeParams, 0, shapeSet.cols);
cout<<"shape: "<<endl;
Mat shape=cvarrToMat(&shapeParams);
cout<<"shapeParams: "<<shape<<endl;
shape=shape/this->w;
cout<<"shapeParams scaled: "<<shape<<endl;
CvMat textureParams;
cvGetCols(&apMat, &textureParams, shapeSet.cols, shapeSet.cols+this->b_g.cols);
Mat texture=cvarrToMat(&textureParams);
cout<<"texture Params: "<<texture<<endl;
shape.convertTo(shape, meanShape.type());
Mat modelShape=this->meanShape + shape;
modelShape=this->cutToRange(modelShape, 0, this->ImageHeight);
cout<<"modelShape: "<<modelShape<<endl;
Mat backProjectShape=this->textureVariationPCA.backProject(texture);
backProjectShape.convertTo(backProjectShape, meanTexture.type());
Mat modelTexture=this->meanTexture + backProjectShape;
modelTexture.convertTo(modelTexture, CV_8U);
modelTexture.convertTo(modelTexture, CV_32S);
//cout<<"modelTexture: "<<modelTexture<<endl;
Mat warped=this->warpImage(this->images[i], modelShape);
cvtColor(warped, image_grey, COLOR_BGR2GRAY);
Mat convHull = this->createConvexHull(this->meanPoints);
Mat realTexture=this->getTextureInsideHull(image_grey, convHull);
Mat realTextureSampled=this->sampleMat(realTexture, sampling);
normalize(realTextureSampled, realTextureSampled, 0, 255, NORM_MINMAX);
cout<<realTexture.cols<<" "<<realTextureSampled.cols<<" "<<modelTexture.cols<<endl;
realTextureSampled.convertTo(realTextureSampled, modelTexture.type());
Mat dI=realTextureSampled-modelTexture;
deltaC.push_back(dc);
deltaI.push_back(dI);
}
}
cout<<"deltaI : " <<deltaI<<endl;
cout<<"deltaC: "<<deltaC<<endl;
this->A=this->findLinearAproximation(deltaI, deltaC);
Mat deltaItest;
deltaI.row(0).convertTo(deltaItest, A.type());
cout<<"delta I row: "<<deltaItest.row(0)<<endl;
cout<<"delta c row: "<<deltaC.row(0)<<endl;
//cout<<this->A.type()<<endl;
//cout<<deltaI.type()<<endl;
Mat result=deltaItest.row(0)*A;
cout<<result<<endl;
}
static
int build_rtrees_classifier( char* data_filename,
char* filename_to_save, char* filename_to_load )
{
CvMat* data = 0;
CvMat* responses = 0;
CvMat* var_type = 0;
CvMat* sample_idx = 0;
int ok = read_num_class_data( data_filename, 16, &data, &responses );
int nsamples_all = 0, ntrain_samples = 0;
int i = 0;
double train_hr = 0, test_hr = 0;
CvRTrees forest;
CvMat* var_importance = 0;
if( !ok )
{
printf( "Could not read the database %s\n", data_filename );
return -1;
}
printf( "The database %s is loaded.\n", data_filename );
nsamples_all = data->rows;
ntrain_samples = (int)(nsamples_all*0.8);
// Create or load Random Trees classifier
if( filename_to_load )
{
// load classifier from the specified file
forest.load( filename_to_load );
ntrain_samples = 0;
if( forest.get_tree_count() == 0 )
{
printf( "Could not read the classifier %s\n", filename_to_load );
return -1;
}
printf( "The classifier %s is loaded.\n", data_filename );
}
else
{
// create classifier by using <data> and <responses>
printf( "Training the classifier ...\n");
// 1. create type mask
var_type = cvCreateMat( data->cols + 1, 1, CV_8U );
cvSet( var_type, cvScalarAll(CV_VAR_ORDERED) );
cvSetReal1D( var_type, data->cols, CV_VAR_CATEGORICAL );
// 2. create sample_idx
sample_idx = cvCreateMat( 1, nsamples_all, CV_8UC1 );
{
CvMat mat;
cvGetCols( sample_idx, &mat, 0, ntrain_samples );
cvSet( &mat, cvRealScalar(1) );
cvGetCols( sample_idx, &mat, ntrain_samples, nsamples_all );
cvSetZero( &mat );
}
// 3. train classifier
forest.train( data, CV_ROW_SAMPLE, responses, 0, sample_idx, var_type, 0,
CvRTParams(10,10,0,false,15,0,true,4,100,0.01f,CV_TERMCRIT_ITER));
printf( "\n");
}
// compute prediction error on train and test data
for( i = 0; i < nsamples_all; i++ )
{
double r;
CvMat sample;
cvGetRow( data, &sample, i );
r = forest.predict( &sample );
r = fabs((double)r - responses->data.fl[i]) <= FLT_EPSILON ? 1 : 0;
if( i < ntrain_samples )
train_hr += r;
else
test_hr += r;
}
test_hr /= (double)(nsamples_all-ntrain_samples);
train_hr /= (double)ntrain_samples;
printf( "Recognition rate: train = %.1f%%, test = %.1f%%\n",
train_hr*100., test_hr*100. );
printf( "Number of trees: %d\n", forest.get_tree_count() );
// Print variable importance
var_importance = (CvMat*)forest.get_var_importance();
if( var_importance )
{
double rt_imp_sum = cvSum( var_importance ).val[0];
printf("var#\timportance (in %%):\n");
for( i = 0; i < var_importance->cols; i++ )
printf( "%-2d\t%-4.1f\n", i,
100.f*var_importance->data.fl[i]/rt_imp_sum);
}
//Print some proximitites
printf( "Proximities between some samples corresponding to the letter 'T':\n" );
{
CvMat sample1, sample2;
const int pairs[][2] = {{0,103}, {0,106}, {106,103}, {-1,-1}};
for( i = 0; pairs[i][0] >= 0; i++ )
{
cvGetRow( data, &sample1, pairs[i][0] );
cvGetRow( data, &sample2, pairs[i][1] );
printf( "proximity(%d,%d) = %.1f%%\n", pairs[i][0], pairs[i][1],
forest.get_proximity( &sample1, &sample2 )*100. );
}
}
// Save Random Trees classifier to file if needed
if( filename_to_save )
forest.save( filename_to_save );
cvReleaseMat( &sample_idx );
cvReleaseMat( &var_type );
cvReleaseMat( &data );
cvReleaseMat( &responses );
return 0;
}
//============================================================================
void AAM_Basic::Fit(const IplImage* image, AAM_Shape& Shape,
int max_iter /* = 30 */,bool showprocess /* = false */)
{
//intial some stuff
double t = gettime;
double e1, e2;
const int np = 5;
double k_values[np] = {1, 0.5, 0.25, 0.125, 0.0625};
int k;
IplImage* Drawimg = 0;
Shape.Point2Mat(__s);
InitParams(image);
CvMat subcq;
cvGetCols(__current_c_q, &subcq, 0, 4); cvCopy(__q, &subcq);
cvGetCols(__current_c_q, &subcq, 4, 4+__cam.nModes()); cvCopy(__c, &subcq);
//calculate error
e1 = EstResidual(image, __current_c_q, __s, __t_m, __t_s, __delta_t);
//do a number of iteration until convergence
for(int iter = 0; iter <max_iter; iter++)
{
if(showprocess)
{
if(Drawimg == 0) Drawimg = cvCloneImage(image);
else cvCopy(image, Drawimg);
__cam.CalcShape(__s, __current_c_q);
Shape.Mat2Point(__s);
Draw(Drawimg, Shape, 2);
#ifdef TARGET_WIN32
mkdir("result");
#else
mkdir("result", S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);
#endif
char filename[100];
sprintf(filename, "result/ter%d.bmp", iter);
cvSaveImage(filename, Drawimg);
}
// predict parameter update
cvGEMM(__delta_t, __G, 1, NULL, 0, __delta_c_q, CV_GEMM_B_T);
//force first iteration
if(iter == 0)
{
cvAdd(__current_c_q, __delta_c_q, __current_c_q);
CvMat c; cvGetCols(__current_c_q, &c, 4, 4+__cam.nModes());
//constrain parameters
__cam.Clamp(&c);
e1 = EstResidual(image, __current_c_q, __s, __t_m, __t_s, __delta_t);
}
//find largest step size which reduces texture EstResidual
else
{
for(k = 0; k < np; k++)
{
cvScaleAdd(__delta_c_q, cvScalar(k_values[k]), __current_c_q, __update_c_q);
//constrain parameters
CvMat c; cvGetCols(__update_c_q, &c, 4, 4+__cam.nModes());
__cam.Clamp(&c);
e2 = EstResidual(image, __update_c_q, __s, __t_m, __t_s, __delta_t);
if(e2 <= e1) break;
}
}
//check for convergence
if(iter > 0)
{
if(k == np)
{
e1 = e2;
cvCopy(__update_c_q, __current_c_q);
}
else if(fabs(e2-e1)<0.001*e1) break;
else if (cvNorm(__delta_c_q)<0.001) break;
else
{
cvCopy(__update_c_q, __current_c_q);
e1 = e2;
}
}
}
cvReleaseImage(&Drawimg);
__cam.CalcShape(__s, __current_c_q);
Shape.Mat2Point(__s);
t = gettime - t;
printf("AAM-Basic Fitting time cost: %.3f millisec\n", t);
}