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; }