CvMat *Morphining::GetAffineMatrix(vector<Coordinate> coord1,vector<Coordinate>coord2,bool debug)
{
//b_temp:affine transformation,b_temp_inv:inverse of affine transform
CvMat *b_temp = cvCreateMat(3, 3, CV_32FC1);
CvMat *b_temp_inv = cvCreateMat(3, 3, CV_32FC1);
CvMat *B = cvCreateMat(3, 3, CV_32FC1);
CvMat *X = cvCreateMat(3, 3, CV_32FC1);
CvMat *X_Inv = cvCreateMat(3, 3, CV_32FC1);
// x1' = w11 * x1 + w12 * y1 + w13
// y1' = w21 * x1 + w22 * y1 + w13
// x2' = w11 * x2 + w12 * y2 + w13
// y2' = w21 * x2 + w22 * y2 + w13
// x3' = w11 * x3 + w12 * y3 + w13
// y3' = w21 * x3 + w22 * y3 + w13
// Ax = b x:left or right b:front face
for(int i = 0;i < 3;i++)
{
cvSetReal2D(B, 0, i, coord1[i].x);
cvSetReal2D(B, 1, i, coord1[i].y);
cvSetReal2D(B, 2, i, 1);
}
for(int i = 0;i < 3;i++)
{
cvSetReal2D(X, 0 , i, coord2[i].x);
cvSetReal2D(X, 1 , i, coord2[i].y);
cvSetReal2D(X, 2 , i, 1);
}
cvInv(X, X_Inv);
cvmMul(B,X_Inv,b_temp);
cvInv(b_temp,b_temp_inv);
if(debug)
{
cout << "b_temp:\n";
PrintMatrix(b_temp,3,3);
cout << "b_temp_inv:\n";
PrintMatrix(b_temp_inv,3,3);
}
cvReleaseMat(&B);
cvReleaseMat(&X);
cvReleaseMat(&X_Inv);
cvReleaseMat(&b_temp);
//return b_temp;
return b_temp_inv;
}
CvMat* Panoramic::GetAffineMatrix(vector<Coordinate> adjustCoord,vector<Coordinate> adjustedCoord)
{
//affineMatrix:affine transformation,b_temp_inv:inverse of affine transform
// x1' = w11 * x1 + w12 * y1 + w13
// y1' = w21 * x1 + w22 * y1 + w13
// x2' = w11 * x2 + w12 * y2 + w13
// y2' = w21 * x2 + w22 * y2 + w13
// x3' = w11 * x3 + w12 * y3 + w13
// y3' = w21 * x3 + w22 * y3 + w13
// Ax = b x:left or right b:front face
m_affineMatrix = cvCreateMat(3, 3, CV_32FC1);
CvMat *invAffineMatrix = cvCreateMat(3, 3, CV_32FC1);
CvMat *B = cvCreateMat(3, 3, CV_32FC1);
CvMat *X = cvCreateMat(3, 3, CV_32FC1);
CvMat *X_Inv = cvCreateMat(3, 3, CV_32FC1);
for(int i = 0;i < 3;i++)
{
cvSetReal2D(B, 0, i, adjustCoord[i].x);
cvSetReal2D(B, 1, i, adjustCoord[i].y);
cvSetReal2D(B, 2, i, 1);
cvSetReal2D(X, 0 , i, adjustedCoord[i].x);
cvSetReal2D(X, 1 , i, adjustedCoord[i].y);
cvSetReal2D(X, 2 , i, 1);
}
cvInv(X, X_Inv);
cvmMul(B,X_Inv,m_affineMatrix);
cvInv(m_affineMatrix,invAffineMatrix);
if(m_debug)
{
cout << "inverse affine matrix\n";
PrintMatrix(invAffineMatrix,3,3);
}
cvReleaseMat(&B);
cvReleaseMat(&X);
cvReleaseMat(&X_Inv);
cvReleaseMat(&m_affineMatrix);
return invAffineMatrix;
}
void laser_visualization_callback(const sensor_msgs::LaserScan::ConstPtr &scan)
{
/*
sensor_msgs::LaserScan Laser;
Laser.header.seq = scan->header.seq;
Laser.header.stamp = scan->header.stamp;
Laser.header.frame_id = scan->header.frame_id;
Laser.angle_min = scan->angle_min;
Laser.angle_max = scan->angle_max;
Laser.angle_increment = scan->angle_increment;
Laser.time_increment = scan->time_increment;
Laser.scan_time = scan->scan_time;
Laser.range_min = scan->range_min;
Laser.range_max = scan->range_max;
Laser.ranges = scan->ranges;
*/
visualization_msgs::MarkerArray markerArray;
double laser_x = 7.75;
double laser_y = 5.55;
double laser_point_x;
double laser_point_y;
CvMat *m_Rotate = cvCreateMat(2, 2, CV_64F);
CvMat *Temp = cvCreateMat(2, 1, CV_64F);
CvMat *Temp2 = cvCreateMat(2, 1, CV_64F);
double theta;
cvmSet(m_Rotate, 0, 0, cos(-PI/2.0));
cvmSet(m_Rotate, 0, 1, -sin(-PI/2.0));
cvmSet(m_Rotate, 1, 0, sin(-PI/2.0));
cvmSet(m_Rotate, 1, 1, cos(-PI/2.0));
int id = 0;
for(unsigned int i=0; i < scan->ranges.size(); i++)
{
theta = scan->angle_increment * i + scan->angle_min;
cvmSet(Temp, 0, 0, scan->ranges[i] * cos(theta));
cvmSet(Temp, 1, 0, scan->ranges[i] * sin(theta));
cvmMul(m_Rotate, Temp, Temp2);
laser_point_x = cvmGet(Temp2, 0, 0);
laser_point_y = cvmGet(Temp2, 1, 0);
laser_point_x = laser_point_x + laser_x;
laser_point_y = laser_point_y + laser_y;
//add draw_line
visualization_msgs::Marker marker;
uint32_t laser_line = visualization_msgs::Marker::LINE_STRIP;
marker.header.frame_id = "/world";
marker.header.stamp = ros::Time::now();
marker.id = id; id++;
marker.type = laser_line;
marker.action = visualization_msgs::Marker::ADD;
marker.scale.x = 0.005;
marker.color.r = 0;
marker.color.g = 0;
marker.color.b = 1;
marker.color.a = 0.5;
geometry_msgs::Point ps;
ps.x = laser_x;
ps.y = laser_y;
ps.z = 1.0;
marker.points.push_back(ps);
ps.x = laser_point_x;
ps.y = laser_point_y;
ps.z = 1.0;
//std::cout << id << " " << laser_x << " " << laser_y << " " << laser_point_x << " " << laser_point_y << std::endl;
std::cout << scan->ranges.size() << std::endl;
marker.points.push_back(ps);
marker.lifetime = ros::Duration(0.02);
markerArray.markers.push_back(marker);
}
pub.publish(markerArray);
}
//coord1:校正用之點 coord2:被校正之點
//可執行四個以上的轉換
CvMat* Morphining::GetProjectionMatrix(vector<Coordinate> adjust,vector<Coordinate> adjusted,bool debug)
{
//spec://cvSetReal2D(cvMat,row,col);
if(m_numFeature < 4)
{
printf("Not enough tiles points to execute perspective transform!\n");
exit(0);
}
//Element of PerspectiveTransformation
CvMat *b = cvCreateMat( 8 , 1 , CV_32FC1);
CvMat *MUL_8_2N = cvCreateMat( 8 , 2*m_numFeature , CV_32FC1);
CvMat *b_temp = cvCreateMat( 3 , 3 , CV_32FC1);
CvMat *b_temp_inv = cvCreateMat( 3 , 3 , CV_32FC1);
//Size:2n*1
for(int i = 0;i < m_numFeature;i++)
{
cvSetReal2D(U, i, 0, adjusted[i].x);
cvSetReal2D(U, i + m_numFeature, 0, adjusted[i].y);
}
//Size:2n*8
for(int i = 0; i < m_numFeature;i++)
{
double col_6 = -1.0 * adjust[i].x * cvGetReal2D(U,i,0);
double col_7 = -1.0 * adjust[i].y * cvGetReal2D(U,i,0);
double col_6_pair = -1.0 * adjust[i].x * cvGetReal2D(U,i + m_numFeature,0);
double col_7_pair = -1.0 * adjust[i].y * cvGetReal2D(U,i + m_numFeature,0);
cvSetReal2D(W, i, 0, adjust[i].x);
cvSetReal2D(W, i, 1, adjust[i].y);
cvSetReal2D(W, i, 2, 1);
cvSetReal2D(W, i, 3, 0);
cvSetReal2D(W, i, 4, 0);
cvSetReal2D(W, i, 5, 0);
cvSetReal2D(W, i, 6, col_6);
cvSetReal2D(W, i, 7, col_7);
cvSetReal2D(W, i + m_numFeature, 0, 0);
cvSetReal2D(W, i + m_numFeature, 1, 0);
cvSetReal2D(W, i + m_numFeature, 2, 0);
cvSetReal2D(W, i + m_numFeature, 3, adjust[i].x);
cvSetReal2D(W, i + m_numFeature, 4, adjust[i].y);
cvSetReal2D(W, i + m_numFeature, 5, 1);
cvSetReal2D(W, i + m_numFeature, 6, col_6_pair);
cvSetReal2D(W, i + m_numFeature, 7, col_7_pair);
}
if(debug)
{
std::cout << "U:\n";
PrintMatrix(U, 2*m_numFeature,1,1);
std::cout << "W:\n";
PrintMatrix(W, 2*m_numFeature,8,1);
}
if(W->cols == W->rows )cvInv(W,MUL_8_2N);
else { //pseudo inverse
cvTranspose(W,T);
cvmMul(T,W,MulResult);
cvInv(MulResult,Inv);
cvmMul(Inv,T,MUL_8_2N);
/*printf("T\n");
PrintMatrix(T, 8,2*m_numFeature,1);*/
/*printf("MulResult\n");
PrintMatrix(MulResult, 8,2*m_numFeature,1);*/
/*printf("Inv\n");
PrintMatrix(Inv,2*m_numFeature, 8,1);*/
}
cvmMul(MUL_8_2N,U,b);
cvSetReal2D(b_temp, 0, 0, cvGetReal2D(b,0,0)); cvSetReal2D(b_temp, 0, 1, cvGetReal2D(b,1,0)); cvSetReal2D(b_temp, 0, 2, cvGetReal2D(b,2,0));
cvSetReal2D(b_temp, 1, 0, cvGetReal2D(b,3,0)); cvSetReal2D(b_temp, 1, 1, cvGetReal2D(b,4,0)); cvSetReal2D(b_temp, 1, 2, cvGetReal2D(b,5,0));
cvSetReal2D(b_temp, 2, 0, cvGetReal2D(b,6,0)); cvSetReal2D(b_temp, 2, 1, cvGetReal2D(b,7,0)); cvSetReal2D(b_temp, 2, 2, 1);
cvInv(b_temp,b_temp_inv);
if(debug)
{
std::cout << "b_temp\n";
PrintMatrix(b_temp,3, 3,1);
std::cout << "b_temp_inv\n";
PrintMatrix(b_temp_inv,3, 3,1);
std::cout << "......................................................................\n";
std::cout << "......................................................................\n";
}
cvReleaseMat(&b);
cvReleaseMat(&MUL_8_2N);
cvReleaseMat(&b_temp);
//return b_temp;
return b_temp_inv;
}
/* Optimization using Levenberg-Marquardt */
void cvLevenbergMarquardtOptimization(pointer_LMJac JacobianFunction,
pointer_LMFunc function,
/*pointer_Err error_function,*/
CvMat *X0,CvMat *observRes,CvMat *resultX,
int maxIter,double epsilon)
{
/* This is not sparse method */
/* Make optimization using */
/* func - function to compute */
/* uses function to compute jacobian */
/* Allocate memory */
CvMat *vectX = 0;
CvMat *vectNewX = 0;
CvMat *resFunc = 0;
CvMat *resNewFunc = 0;
CvMat *error = 0;
CvMat *errorNew = 0;
CvMat *Jac = 0;
CvMat *delta = 0;
CvMat *matrJtJ = 0;
CvMat *matrJtJN = 0;
CvMat *matrJt = 0;
CvMat *vectB = 0;
CV_FUNCNAME( "cvLevenbegrMarquardtOptimization" );
__BEGIN__;
if( JacobianFunction == 0 || function == 0 || X0 == 0 || observRes == 0 || resultX == 0 )
{
CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
}
if( !CV_IS_MAT(X0) || !CV_IS_MAT(observRes) || !CV_IS_MAT(resultX) )
{
CV_ERROR( CV_StsUnsupportedFormat, "Some of input parameters must be a matrices" );
}
int numVal;
int numFunc;
double valError;
double valNewError;
numVal = X0->rows;
numFunc = observRes->rows;
/* test input data */
if( X0->cols != 1 )
{
CV_ERROR( CV_StsUnmatchedSizes, "Number of colomn of vector X0 must be 1" );
}
if( observRes->cols != 1 )
{
CV_ERROR( CV_StsUnmatchedSizes, "Number of colomn of vector observed rusult must be 1" );
}
if( resultX->cols != 1 || resultX->rows != numVal )
{
CV_ERROR( CV_StsUnmatchedSizes, "Size of result vector X must be equals to X0" );
}
if( maxIter <= 0 )
{
CV_ERROR( CV_StsUnmatchedSizes, "Number of maximum iteration must be > 0" );
}
if( epsilon < 0 )
{
CV_ERROR( CV_StsUnmatchedSizes, "Epsilon must be >= 0" );
}
/* copy x0 to current value of x */
CV_CALL( vectX = cvCreateMat(numVal, 1, CV_64F) );
CV_CALL( vectNewX = cvCreateMat(numVal, 1, CV_64F) );
CV_CALL( resFunc = cvCreateMat(numFunc,1, CV_64F) );
CV_CALL( resNewFunc = cvCreateMat(numFunc,1, CV_64F) );
CV_CALL( error = cvCreateMat(numFunc,1, CV_64F) );
CV_CALL( errorNew = cvCreateMat(numFunc,1, CV_64F) );
CV_CALL( Jac = cvCreateMat(numFunc,numVal, CV_64F) );
CV_CALL( delta = cvCreateMat(numVal, 1, CV_64F) );
CV_CALL( matrJtJ = cvCreateMat(numVal, numVal, CV_64F) );
CV_CALL( matrJtJN = cvCreateMat(numVal, numVal, CV_64F) );
CV_CALL( matrJt = cvCreateMat(numVal, numFunc,CV_64F) );
CV_CALL( vectB = cvCreateMat(numVal, 1, CV_64F) );
cvCopy(X0,vectX);
/* ========== Main optimization loop ============ */
double change;
int currIter;
double alpha;
change = 1;
currIter = 0;
alpha = 0.001;
do {
/* Compute value of function */
function(vectX,resFunc);
/* Print result of function to file */
/* Compute error */
cvSub(observRes,resFunc,error);
//valError = error_function(observRes,resFunc);
/* Need to use new version of computing error (norm) */
valError = cvNorm(observRes,resFunc);
/* Compute Jacobian for given point vectX */
JacobianFunction(vectX,Jac);
/* Define optimal delta for J'*J*delta=J'*error */
/* compute J'J */
cvMulTransposed(Jac,matrJtJ,1);
cvCopy(matrJtJ,matrJtJN);
/* compute J'*error */
cvTranspose(Jac,matrJt);
cvmMul(matrJt,error,vectB);
/* Solve normal equation for given alpha and Jacobian */
do
{
/* Increase diagonal elements by alpha */
for( int i = 0; i < numVal; i++ )
{
double val;
val = cvmGet(matrJtJ,i,i);
cvmSet(matrJtJN,i,i,(1+alpha)*val);
}
/* Solve system to define delta */
cvSolve(matrJtJN,vectB,delta,CV_SVD);
/* We know delta and we can define new value of vector X */
cvAdd(vectX,delta,vectNewX);
/* Compute result of function for new vector X */
function(vectNewX,resNewFunc);
cvSub(observRes,resNewFunc,errorNew);
valNewError = cvNorm(observRes,resNewFunc);
currIter++;
if( valNewError < valError )
{/* accept new value */
valError = valNewError;
/* Compute relative change of required parameter vectorX. change = norm(curr-prev) / norm(curr) ) */
change = cvNorm(vectX, vectNewX, CV_RELATIVE_L2);
alpha /= 10;
cvCopy(vectNewX,vectX);
break;
}
else
{
alpha *= 10;
}
} while ( currIter < maxIter );
/* new value of X and alpha were accepted */
} while ( change > epsilon && currIter < maxIter );
/* result was computed */
cvCopy(vectX,resultX);
__END__;
cvReleaseMat(&vectX);
cvReleaseMat(&vectNewX);
cvReleaseMat(&resFunc);
cvReleaseMat(&resNewFunc);
cvReleaseMat(&error);
cvReleaseMat(&errorNew);
cvReleaseMat(&Jac);
cvReleaseMat(&delta);
cvReleaseMat(&matrJtJ);
cvReleaseMat(&matrJtJN);
cvReleaseMat(&matrJt);
cvReleaseMat(&vectB);
return;
}
bool FaceCrop::Crop(CvPoint LE , CvPoint RE , CvPoint No , CvPoint Mo){
CvPoint eyeVector;
double rotation=0.0;
IplImage* rotationImg=cvCreateImage(cvSize(srcImg->width,srcImg->height),IPL_DEPTH_8U,3);
double eyeDistance;
eyeVector.x=LE.x-RE.x;
eyeVector.y=LE.y-RE.y;
eyeDistance=sqrt((double)(eyeVector.x*eyeVector.x + eyeVector.y*eyeVector.y));
rotation=atan2((double)eyeVector.y , (double)eyeVector.x) * 180 / CV_PI+180;
CvMat *matrix=NULL;
if(rotation > maxRotationAngle){
matrix=cvCreateMat(2,3,CV_32FC1);
matrix=cv2DRotationMatrix(cvPoint2D32f(LE.x,LE.y),rotation,1,matrix);
cvWarpAffine( srcImg,rotationImg,matrix,CV_WARP_FILL_OUTLIERS,cvScalarAll(0) );
}
else{
cvCopy(srcImg,rotationImg);
}
eyeDistance=(int)eyeDistance;
int x=LE.x-(int)(a*eyeDistance);
int y=LE.y-(int)(b*eyeDistance);
int width= (int)(a*eyeDistance)+(int)(a*eyeDistance)+eyeDistance+1;
int height=(int)(b*eyeDistance)+(int)(c*eyeDistance)+1;
if(x<0)x=0;
if(y<0)y=0;
if(x+width>=rotationImg->width)width=rotationImg->width-x-1;
if(y+height>=rotationImg->height)height=rotationImg->height-y-1;
cvSetImageROI(rotationImg,cvRect(x , y , width , height));
cropImg=cvCreateImage(cvSize(width,height),IPL_DEPTH_8U,3);
cvCopy(rotationImg,cropImg);
cvResetImageROI(rotationImg);
normalizeImg=cvCreateImage(cvSize(normalizeWidth,normalizeHeight),IPL_DEPTH_8U,3);
cvResize(cropImg,normalizeImg);
if(matrix!=NULL){
matrix=cv2DRotationMatrix(cvPoint2D32f(LE.x,LE.y),-rotation,1,matrix);
CvMat *pointMatrix=cvCreateMat(3,1,CV_32FC1);
CvMat *rotatedPointMatrix=cvCreateMat(2,1,CV_32FC1);
cvmSet(pointMatrix,0,0,x);
cvmSet(pointMatrix,1,0,y);
cvmSet(pointMatrix,2,0,1);
cvmMul(matrix,pointMatrix,rotatedPointMatrix);
leftTop.x=cvmGet(rotatedPointMatrix,0,0);
leftTop.y=cvmGet(rotatedPointMatrix,1,0);
cvmSet(pointMatrix,0,0,x+width);
cvmSet(pointMatrix,1,0,y);
cvmMul(matrix,pointMatrix,rotatedPointMatrix);
rightTop.x=cvmGet(rotatedPointMatrix,0,0);
rightTop.y=cvmGet(rotatedPointMatrix,1,0);
cvmSet(pointMatrix,0,0,x);
cvmSet(pointMatrix,1,0,y+height);
cvmMul(matrix,pointMatrix,rotatedPointMatrix);
leftdown.x=cvmGet(rotatedPointMatrix,0,0);
leftdown.y=cvmGet(rotatedPointMatrix,1,0);
cvmSet(pointMatrix,0,0,x+width);
cvmSet(pointMatrix,1,0,y+height);
cvmMul(matrix,pointMatrix,rotatedPointMatrix);
rightdown.x=cvmGet(rotatedPointMatrix,0,0);
rightdown.y=cvmGet(rotatedPointMatrix,1,0);
}
else{
leftTop.x=x;
leftTop.y=y;
rightTop.x=x+width;
rightTop.y=y;
leftdown.x=x;
leftdown.y=y+height;
rightdown.x=x+width;
rightdown.y=y+height;
}
//cvSaveImage("result.jpg",cropImg);
//cvNamedWindow("Img",1);
//cvShowImage("Img",cropImg);
//cvWaitKey(0);
cvReleaseImage(&rotationImg);
cvReleaseImage(&cropImg);
return true;
}
