void mitk::UndistortCameraImage::UndistortImage(IplImage *src, IplImage *dst)
{
   // init intrinsic camera matrix [fx 0 cx; 0 fy cy; 0 0 1].
  m_intrinsicMatrixData[0] = (double)m_fcX;
  m_intrinsicMatrixData[1] = 0.0;
  m_intrinsicMatrixData[2] = (double)m_ccX;
  m_intrinsicMatrixData[3] = 0.0;
  m_intrinsicMatrixData[4] = (double)m_fcY;
  m_intrinsicMatrixData[5] = (double)m_ccY;
  m_intrinsicMatrixData[6] = 0.0;
  m_intrinsicMatrixData[7] = 0.0;
  m_intrinsicMatrixData[8] = 1.0;
  m_intrinsicMatrix        = cvMat(3, 3, CV_32FC1, m_intrinsicMatrixData);

  // init distortion matrix
  m_distortionMatrix       = cvMat(1, 4, CV_32F, m_distortionMatrixData);

  // undistort
  cvUndistort2(src,dst, &m_intrinsicMatrix, &m_distortionMatrix);
}

int  cvHoughLinesP( CvArr* image, double rho,
                    double theta, int threshold,
                    int lineLength, int lineGap,
                    int* lines, int linesNumber )
{
    CvMat linesMat = cvMat( 1, linesNumber, CV_32SC4, lines );
    cvHoughLines2( image, &linesMat, CV_HOUGH_PROBABILISTIC,
                   rho, theta, threshold, lineLength, lineGap );

    return linesMat.cols;
}

void COpenCVMFCView::OnRotation30()
{
	// TODO: Add your command handler code here

	int angle = 30;                         //  Rotate 30 degree
	int opt = 0;                            //  1: with resize   0: just rotate
	double factor;                          //  resize factor
	IplImage *pImage;
	IplImage *pImgRotation = NULL;

	pImage = workImg;
	pImgRotation = cvCloneImage(workImg);

	angle = -angle;

	//  Create M Matrix
	float m[6];
	//      Matrix m looks like:
	//      [ m0  m1  m2 ] ----> [ a11  a12  b1 ]
	//      [ m3  m4  m5 ] ----> [ a21  a22  b2 ]

	CvMat M = cvMat(2,3,CV_32F,m);
	int w = workImg->width;
	int h = workImg->height;

	if (opt)
		factor = (cos(angle*CV_PI/180.)+1.0)*2;
	else 
		factor = 1;

	m[0] = (float)(factor*cos(-angle*CV_PI/180.));
	m[1] = (float)(factor*sin(-angle*CV_PI/180.));
	m[3] = -m[1];
	m[4] =  m[0];
	//  Make rotation center to image center
	m[2] = w*0.5f;
	m[5] = h*0.5f;

	//---------------------------------------------------------
	//  dst(x,y) = A * src(x,y) + b
	cvZero(pImgRotation);
	cvGetQuadrangleSubPix(pImage,pImgRotation,&M);
	//---------------------------------------------------------

	cvNamedWindow("Rotation Image");
	cvFlip(pImgRotation);
	cvShowImage("Rotation Image",pImgRotation);

	cvReleaseImage(&pImgRotation);

	cvWaitKey(0);

	cvDestroyWindow("Rotation Image");
}

void  cvbFastArctan( const float* y, const float* x, float* angle, int len )
{
    CvMat mx = cvMat( 1, len, CV_32F, (void*)x );
    CvMat my = mx;
    CvMat ma = mx;

    my.data.fl = (float*)y;
    ma.data.fl = (float*)angle;

    cvCartToPolar( &mx, &my, NULL, &ma, 1 );
}

/*!
*  \brief      Convert cv::Mat to integer Eigen matrix
*  \author     Sascha Kaden
*  \param[in]  image
*  \param[out] Eigen matrix
*  \date       2016-12-18
*/
cv::Mat eigenToCV(Eigen::MatrixXi eigenMat) {
    cv::Mat cvMat(eigenMat.rows(), eigenMat.cols(), CV_32SC1, eigenMat.data());

    if (Eigen::RowMajorBit)
        cv::transpose(cvMat, cvMat);

    cv::Mat dst;
    cvMat.convertTo(dst, CV_8UC1);
    cv::cvtColor(dst, dst, CV_GRAY2BGR);

    return dst;
}

void  cvKMeans( int num_clusters, float** samples,
                int num_samples, int vec_size,
                CvTermCriteria termcrit, int* cluster_idx )
{
    CvMat* samples_mat = cvCreateMat( num_samples, vec_size, CV_32FC1 );
    CvMat cluster_idx_mat = cvMat( num_samples, 1, CV_32SC1, cluster_idx );
    int i;
    for( i = 0; i < num_samples; i++ )
        memcpy( samples_mat->data.fl + i*vec_size, samples[i], vec_size*sizeof(float));
    cvKMeans2( samples_mat, num_clusters, &cluster_idx_mat, termcrit, 1, 0, 0, 0, 0 );
    cvReleaseMat( &samples_mat );
}

CvMat* FeatureManager::getGaborFeatureByImage(IplImage* pImage)//the feature is row vector;the returned mat need to be released by user.
{
    double* pGridFeature=getGridMbrmFeature(pImage);

    const int MBRMDIM=1280;

    CvMat tmp = cvMat(1,MBRMDIM,CV_64FC1,pGridFeature);
    CvMat* pFeatureMat=cvCreateMat(1,MBRMDIM,CV_64FC1);
    cvCopy(&tmp,pFeatureMat);
    delete [] pGridFeature;
    return pFeatureMat;
}

CV_IMPL IplConvKernel *
cvCreateStructuringElementEx( int cols, int rows,
                              int anchorX, int anchorY,
                              int shape, int *values )
{
    IplConvKernel *element = 0;
    int i, size = rows * cols;
    int element_size = sizeof(*element) + size*sizeof(element->values[0]);

    CV_FUNCNAME( "cvCreateStructuringElementEx" );

    __BEGIN__;

    if( !values && shape == CV_SHAPE_CUSTOM )
        CV_ERROR_FROM_STATUS( CV_NULLPTR_ERR );

    if( cols <= 0 || rows <= 0 ||
        (unsigned) anchorX >= (unsigned) cols ||
        (unsigned) anchorY >= (unsigned) rows )
        CV_ERROR_FROM_STATUS( CV_BADSIZE_ERR );

    CV_CALL( element = (IplConvKernel *)cvAlloc(element_size + 32));
    if( !element )
        CV_ERROR_FROM_STATUS( CV_OUTOFMEM_ERR );

    element->nCols = cols;
    element->nRows = rows;
    element->anchorX = anchorX;
    element->anchorY = anchorY;
    element->nShiftR = shape < CV_SHAPE_ELLIPSE ? shape : CV_SHAPE_CUSTOM;
    element->values = (int*)(element + 1);

    if( shape == CV_SHAPE_CUSTOM )
    {
        if( !values )
            CV_ERROR( CV_StsNullPtr, "Null pointer to the custom element mask" );
        for( i = 0; i < size; i++ )
            element->values[i] = values[i];
    }
    else
    {
        CvMat el_hdr = cvMat( rows, cols, CV_32SC1, element->values );
        CV_CALL( CvMorphology::init_binary_element(&el_hdr,
                        shape, cvPoint(anchorX,anchorY)));
    }

    __END__;

    if( cvGetErrStatus() < 0 )
        cvReleaseStructuringElement( &element );

    return element;
}

IplImage *derivateX(const IplImage *src) {
    CvMat matrix;
    matrix = cvMat(1, 5, CV_32F, mat);

//    IplImage *img = get_gray(src);
    IplImage *dst = cvCloneImage(src);
    cvFilter2D(src, dst, &matrix);

//    cvReleaseImage(&img);

    return dst;
}

//! Find homography between matched points and translate src_corners to dst_corners
int translateCorners(IpPairVec &matches, const CvPoint src_corners[4], CvPoint dst_corners[4])
{
#ifndef LINUX
  double h[9];
  CvMat _h = cvMat(3, 3, CV_64F, h);
  std::vector<CvPoint2D32f> pt1, pt2;
  CvMat _pt1, _pt2;

  int n = (int)matches.size();
  if( n < 4 ) return 0;

  // Set vectors to correct size
  pt1.resize(n);
  pt2.resize(n);

  // Copy Ipoints from match vector into cvPoint vectors
  for(int i = 0; i < n; i++ )
  {
    pt1[i] = cvPoint2D32f(matches[i].second.x, matches[i].second.y);
    pt2[i] = cvPoint2D32f(matches[i].first.x, matches[i].first.y);
  }
  _pt1 = cvMat(1, n, CV_32FC2, &pt1[0] );
  _pt2 = cvMat(1, n, CV_32FC2, &pt2[0] );

  // Find the homography (transformation) between the two sets of points
  if(!cvFindHomography(&_pt1, &_pt2, &_h, CV_RANSAC, 5))  // this line requires opencv 1.1
    return 0;

  // Translate src_corners to dst_corners using homography
  for(int i = 0; i < 4; i++ )
  {
    double x = src_corners[i].x, y = src_corners[i].y;
    double Z = 1./(h[6]*x + h[7]*y + h[8]);
    double X = (h[0]*x + h[1]*y + h[2])*Z;
    double Y = (h[3]*x + h[4]*y + h[5])*Z;
    dst_corners[i] = cvPoint(cvRound(X), cvRound(Y));
  }
#endif
  return 1;
}

// y := alpha * A * X + beta * y
inline void __dgemv(
	char trans, 
	int m, 
	int n, 
	double alpha, 
	double *A, // n * m
	int lda, 
	double *X, // m('T')
	int incx, 
	double beta, 
	double *y, // n('T')
	int incy
) {
	assert(incx==1 && incy==1);
	if(trans=='T') {
		CvMat A_mat= cvMat(n, m, CV_64FC1, A);
		CvMat X_mat= cvMat(m, 1, CV_64FC1, X);
		CvMat y_mat= cvMat(n, 1, CV_64FC1, y);
		cvGEMM(&A_mat, &X_mat, alpha, &y_mat, beta, &y_mat, 0);
	} else if(trans=='N') {
		CvMat A_mat= cvMat(n, m, CV_64FC1, A);
		CvMat X_mat= cvMat(n, 1, CV_64FC1, X);
		CvMat y_mat= cvMat(m, 1, CV_64FC1, y);
		cvGEMM(&A_mat, &X_mat, alpha, &y_mat, beta, &y_mat, CV_GEMM_A_T);
	} else {
		printf("error in function __dgemv");
		exit(-1);
	}
}

void GLReprojection::updateMesh()
{
    if ( m_lorigin == NULL )
        return ;

    const Dim &dim = m_ldisp.dim();

    m_mesh.resize(dim);
    m_mesh.lock();

#if 1
    const float *disp_c = m_ldisp.cpuMem()->data.fl;
    const float _1_255 = 1.0f/255.0f;

    m_box.clear();

    for (size_t r=0; r<dim.height(); r++)
    {
        const float *dispPtr = disp_c + r*dim.width();
        const uchar *colorPtr = m_lorigin->data.ptr + r*m_lorigin->step;


        for (size_t c=0; c<dim.width(); c++)
        {
            const uchar *colorBase = colorPtr + c*3;

            const ud::Vec3f color(colorBase[0]*_1_255,
                                  colorBase[1]*_1_255,
                                  colorBase[2]*_1_255);
            const ud::Vec3f vert(
                m_lrepr->reproject(
                    c, r, *dispPtr, dim));

            m_box.add(vert);

            m_mesh.setPoint(
                c, r, vert,
                color);

            dispPtr++;
        }
    }
#else
    RectificationCV *cv = dynamic_cast<RectificationCV*>(m_lrepr);
    CvMat *Q = cvMat(4, 4, CV_32F, cv->reprojectionMatrix());

#endif

    m_mesh.unlock();

    m_lorigin = NULL;
}

static double luck_pixel(int x, int y, const CvMat *hom1, const CvMat *hom2)
{
	CvMat *invhom1 = cvCreateMat(3, 3, CV_32FC1);
	cvInvert(hom1, invhom1, CV_LU);
	//inhom1到上一帧的映射变换
	
	CvPoint2D32f src = cvPoint2D32f(x, y);
	CvPoint2D32f d1, d2;
	CvMat pt_src = cvMat(1, 1, CV_32FC2, &src);
	CvMat pt_dst = cvMat(1, 1, CV_32FC2, &d1);
	cvPerspectiveTransform(&pt_src, &pt_dst, invhom1);
	//透视转换成上一帧
	pt_dst = cvMat(1, 1, CV_32FC2, &d2);
	cvPerspectiveTransform(&pt_src, &pt_dst, hom2);
	//透视转换为下一帧
	//得到d1和d2,为前后一帧相对应的点
	double dis = (src.x-d1.x)*(src.x-d1.x)+(src.y-d1.y)*(src.y-d1.y);
	dis += (src.x-d2.x)*(src.x-d2.x)+(src.y-d2.y)*(src.y-d2.y);
	double luck = exp(-dis/(2*SIGMA_L*SIGMA_L));
	cvReleaseMat(&invhom1);
	return luck;
}

// 以点center为旋转中心，对src旋转angle度并缩放factor倍。
void RotateImage(IplImage *src, IplImage *dst, CvPoint center, float angle,
		float factor) {
	float m[6];
	CvMat mat = cvMat(2, 3, CV_32FC1, m);
	m[0] = (float) (factor * cos(-angle * CV_PI / 180.));
	m[1] = (float) (factor * sin(-angle * CV_PI / 180.));
	m[2] = center.x;
	m[3] = -m[1];
	m[4] = m[0];
	m[5] = center.y;
	cvSetZero(dst);
	cvGetQuadrangleSubPix(src, dst, &mat);
}

void  cvbCartToPolar( const float* y, const float* x, float* magnitude, float* angle, int len )
{
    CvMat mx = cvMat( 1, len, CV_32F, (void*)x );
    CvMat my = mx;
    CvMat mm = mx;
    CvMat ma = mx;

    my.data.fl = (float*)y;
    mm.data.fl = (float*)magnitude;
    ma.data.fl = (float*)angle;

    cvCartToPolar( &mx, &my, &mm, angle ? &ma : NULL, 1 );
}

IplImage* lowPassFilter(IplImage *image){
	IplImage* filteredImage = cvCreateImage(cvSize(image->width, image->height), image->depth, image->nChannels);
	double K[] = { 1, 1, 1, 1, 1, 1, 1, 1, 1 };
	float t = 0;
	for (int i = 0; i< (3 * 3); ++i)
		t = t + K[i];
	for (int i = 0; i< (3 * 3); ++i)
		K[i] = K[i] / t;
	CvMat Kernel = cvMat(3, 3, CV_64FC1, K);
	cvFilter2D(image, filteredImage, &Kernel);
	
	return filteredImage;
}

	// this should be replaced by c++ 2.0 api style code once available
	vector<cv::Vec4i> convexityDefects(const vector<cv::Point>& contour) {
		vector<int> hullIndices;
		convexHull(Mat(contour), hullIndices, false, false);
		vector<cv::Vec4i> convexityDefects;
		if(hullIndices.size() > 0 && contour.size() > 0) {		
			CvMat contourMat = cvMat(1, contour.size(), CV_32SC2, (void*) &contour[0]);
			CvMat hullMat = cvMat(1, hullIndices.size(), CV_32SC1, (void*) &hullIndices[0]);
			CvMemStorage* storage = cvCreateMemStorage(0);
			CvSeq* defects = cvConvexityDefects(&contourMat, &hullMat, storage);
			for(int i = 0; i < defects->total; i++){
				CvConvexityDefect* cur = (CvConvexityDefect*) cvGetSeqElem(defects, i);
				cv::Vec4i defect;
				defect[0] = cur->depth_point->x;
				defect[1] = cur->depth_point->y;
				defect[2] = (cur->start->x + cur->end->x) / 2;
				defect[3] = (cur->start->y + cur->end->y) / 2;
				convexityDefects.push_back(defect);
			}
			cvReleaseMemStorage(&storage);
		}
		return convexityDefects;
	}

/*
    This method will calculate the convex hull of source landmarks
    and populate the pointsInsideHull vector with these points coordinates
*/
void PAW::populatePointsInsideHull(){
    //calc scrLandmarks convex hull
    CvPoint* pointsHull = (CvPoint*)malloc( nLandmarks * sizeof(pointsHull[0]));
    int* hull = (int*)malloc( nLandmarks * sizeof(hull[0]));
    CvMat pointMat = cvMat( 1, nLandmarks, CV_32SC2, pointsHull );
    CvMat hullMat = cvMat( 1, nLandmarks, CV_32SC1, hull );
        

    for(int i = 0; i < nLandmarks; i++ )
    {           
        pointsHull[i] = cvPoint(srcLandmarks.at<int>(i,0),srcLandmarks.at<int>(i,1));
    }
    cvConvexHull2( &pointMat, &hullMat, CV_CLOCKWISE, 0 );
    int hullcount = hullMat.cols;
        
    CvPoint* pointsHullFinal = (CvPoint*)malloc( hullcount * sizeof(pointsHullFinal[0]));        
        
    
    for(int i = 0; i < hullcount; i++ ){
        int ptIndex = hull[i];
        CvPoint pt = cvPoint(    srcLandmarks.at<int>(ptIndex,0),
                                srcLandmarks.at<int>(ptIndex,1));

        pointsHullFinal[i] = pt;
    }

    CvMat hullMatPoints = cvMat( 1, hullcount, CV_32SC2, pointsHullFinal);

    //check if point belongs
    for (int j=0;j<baseImageHeight;j++){            
        for(int i=0;i< baseImageWidth;i++){
            
            double distance = cvPointPolygonTest(&hullMatPoints,cvPoint2D32f(i,j),1);
            if(distance >=0){                    
                pointsInsideHull.push_back(cvPoint(i,j));        
            }
        }
    }
}

void ImageOverlayer::OverlayEstimatedRobotPose(double x, double y, double z, double thetaRob, CvScalar color, IplImage* baseImage)
{
   
   vector<CvPoint3D32f> Robot_PositionsToReProject;
   Robot_PositionsToReProject.resize(totPntsPerPos);
   
   CvMat _Robot_PositionsToReProject = cvMat(1, totPntsPerPos, CV_32FC3, &Robot_PositionsToReProject[0]);    
  
   
  for(float j=0; j<=robHeight; j+=0.01)
   {
    
    Robot_PositionsToReProject[0] = cvPoint3D32f(x,y,j);
    for(int pts = 0; pts < circlePointsPerPosition; pts++) //circlePointsPerPosition points out of totPntsPerPos for circle
      {
	float theta = -M_PI + (float)pts*2*M_PI/(circlePointsPerPosition);  
	Robot_PositionsToReProject[1 + pts] = cvPoint3D32f( x + robRadius*cosf(theta), y + robRadius*sinf(theta), j);
      }
     
   for(int pts = 0; pts < arrowPointsPerPosition /*&& j==robHeight*/; pts++) //arrowPointsPerPosition points out of totPntsPerPos for th arrow
    {
      Robot_PositionsToReProject[1 + circlePointsPerPosition + pts] = cvPoint3D32f( x + (float)pts*(robRadius/(float)arrowPointsPerPosition)*cosf(thetaRob), y + (float)pts*(robRadius/(float)arrowPointsPerPosition)*sinf(thetaRob), robHeight);
    }
   
    vector<CvPoint2D32f> reprojectedPoints_Robot;
    reprojectedPoints_Robot.resize(totPntsPerPos);
    
    CvMat _imageReprojectedPoints_RobotRight = cvMat(1, totPntsPerPos, CV_32FC2, &reprojectedPoints_Robot[0]);
    
    cvProjectPoints2(&_Robot_PositionsToReProject, Rvec_right_n, Tvec_right, &_M2, &_D2, &_imageReprojectedPoints_RobotRight, NULL, NULL, NULL, NULL, NULL); 
   
    for(int pts = 0; pts < totPntsPerPos; pts++)
      {    
	CvPoint robot_PointToBeShownRight = cvPoint(reprojectedPoints_Robot[pts].x,reprojectedPoints_Robot[pts].y);
	cvCircle(baseImage, robot_PointToBeShownRight, 0, color, 2, 8, 0);       	
      }
   }
}

int KitechSurfObjectRecognitionComp::LocatePlanarObject( const CvSeq* objectKeypoints, const CvSeq* objectDescriptors, const CvSeq* imageKeypoints, const CvSeq* imageDescriptors, const CvPoint src_corners[4], CvPoint dst_corners[4] )
{
    double h[9];
    CvMat _h = cvMat(3, 3, CV_64F, h);
    vector<int> ptpairs;
    vector<CvPoint2D32f> pt1, pt2;
    CvMat _pt1, _pt2;
    int i, n;

    FindPairs( objectKeypoints, objectDescriptors, imageKeypoints, imageDescriptors, ptpairs );
    n = ptpairs.size()/2;
    if( n < 4 )
        return 0;

    pt1.resize(n);
    pt2.resize(n);
    for( i = 0; i < n; i++ )
    {
        pt1[i] = ((CvSURFPoint*)cvGetSeqElem(objectKeypoints,ptpairs[i*2]))->pt;
        pt2[i] = ((CvSURFPoint*)cvGetSeqElem(imageKeypoints,ptpairs[i*2+1]))->pt;
    }

    _pt1 = cvMat(1, n, CV_32FC2, &pt1[0] );
    _pt2 = cvMat(1, n, CV_32FC2, &pt2[0] );
    if( !cvFindHomography( &_pt1, &_pt2, &_h, CV_RANSAC, 5 ))
        return 0;

    for( i = 0; i < 4; i++ )
    {
        double x = src_corners[i].x, y = src_corners[i].y;
        double Z = 1./(h[6]*x + h[7]*y + h[8]);
        double X = (h[0]*x + h[1]*y + h[2])*Z;
        double Y = (h[3]*x + h[4]*y + h[5])*Z;
        dst_corners[i] = cvPoint(cvRound(X), cvRound(Y));
    }

    return 1;
}