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;
}
//Wrapper de C a CPP
void HandDetection::findConvexityDefects(vector<Point>& contour, vector<int>& hull, vector<CvConvexityDefect>& convexDefects){
if(hull.size() > 0 && contour.size() > 0){
//Conversion de objetos CPP a C
CvSeq* contourPoints;
CvSeq* defects;
CvMemStorage* storage;
CvMemStorage* strDefects;
CvMemStorage* contourStr;
CvConvexityDefect *defectArray = 0;
strDefects = cvCreateMemStorage();
defects = cvCreateSeq( CV_SEQ_KIND_GENERIC|CV_32SC2, sizeof(CvSeq),sizeof(CvPoint), strDefects );
//Empezamos con los contornos: los pasamos a un array de objetos Seq
contourStr = cvCreateMemStorage();
contourPoints = cvCreateSeq(CV_SEQ_KIND_GENERIC|CV_32SC2, sizeof(CvSeq), sizeof(CvPoint), contourStr);
for(int i=0; i<(int)contour.size(); i++) {
CvPoint cp = {contour[i].x, contour[i].y};
cvSeqPush(contourPoints, &cp);
}
//Ahora con los puntos del poligono convexo
int count = (int)hull.size();
//int hullK[count];
int* hullK = (int*)malloc(count*sizeof(int));
for(int i=0; i<count; i++){hullK[i] = hull.at(i);}
CvMat hullMat = cvMat(1, count, CV_32SC1, hullK);
//Inicializamos la salida
storage = cvCreateMemStorage(0);
defects = cvConvexityDefects(contourPoints, &hullMat, storage);
defectArray = (CvConvexityDefect*)malloc(sizeof(CvConvexityDefect)*defects->total);
cvCvtSeqToArray(defects, defectArray, CV_WHOLE_SEQ);
//Conversion de C a CPP
//float cutoff = lower - (lower - upper) * 0.3f;
for(int i = 0; i<defects->total; i++){
convexDefects.push_back(defectArray[i]);
}
//Liberar vectores con cvReleaseMemStorage
//No olvides hacer un free
//free(hullK);
cvReleaseMemStorage(&contourStr);
cvReleaseMemStorage(&strDefects);
cvReleaseMemStorage(&storage);
//Devolvemos el vector de puntos con los defectos de convexidad
//return defects;
}
//return defectArray;
}
void cvMinAreaRect( CvPoint* points, int n, int, int, int, int,
CvPoint2D32f* anchor, CvPoint2D32f* vect1, CvPoint2D32f* vect2 )
{
CvMat mat = cvMat( 1, n, CV_32SC2, points );
CvBox2D box = cvMinAreaRect2( &mat, 0 );
CvPoint2D32f pt[4];
cvBoxPoints( box, pt );
*anchor = pt[0];
vect1->x = pt[1].x - pt[0].x;
vect1->y = pt[1].y - pt[0].y;
vect2->x = pt[3].x - pt[0].x;
vect2->y = pt[3].y - pt[0].y;
}
// called by the loop to actually grab the frames
int capture_image(int fd,CvFont *font, int *set_quality, IplImage* frame,CvMat *cvmat,char *capture_title,v4l2_buffer *buf, uint32_t *start_time, uint32_t *image_count) {
// request a new frame
if(-1 == xioctl(fd, VIDIOC_QBUF, buf)) {
perror("Query Buffer");
return 1;
}
// wait up to 2 sec for a new frame to arive
fd_set fds;
FD_ZERO(&fds);
FD_SET(fd, &fds);
struct timeval tv = {0};
tv.tv_sec = 2;
int r = select(fd+1, &fds, NULL, NULL, &tv);
if(-1 == r) {
perror("Waiting for Frame");
return 1;
}
// read it
if(-1 == xioctl(fd, VIDIOC_DQBUF, buf)) {
perror("Retrieving Frame");
return 1;
}
// convert v4l2 buffer to opencv image
*cvmat = cvMat(height, width, CV_8UC3, (void*)buffer);
frame = cvDecodeImage(cvmat, 1);
// add title, reused tv from select-wait
gettimeofday(&tv, NULL);
time_t secs = time(0);
struct tm *local = localtime(&secs);
sprintf(capture_title, CAPTURE_PROTO, local->tm_hour, local->tm_min, local->tm_sec, (int)((unsigned long long)(tv.tv_usec) / 1000)%1000);
(*image_count)++;
printf("%s @ %2.2f fps\n",capture_title, round((float)(*image_count)*100/(time(0)-(*start_time)))/100 );
cvPutText(frame, capture_title, cvPoint(22, 22), font, cvScalar(0,0,0,0));
cvPutText(frame, capture_title, cvPoint(24, 24), font, cvScalar(200,200,200,0));
// save to disk ... well RAM
cvSaveImage("/dev/shm/mjpeg/cam_full.part.jpg", frame, set_quality);
rename("/dev/shm/mjpeg/cam_full.part.jpg","/dev/shm/mjpeg/cam_full.jpg");
// important to avoid mem leakage
cvReleaseImage(&frame);
return 0;
}
void Animation::load(string animationDirectory) {
ofxDirList dir;
int n = dir.listDir(animationDirectory);
for(int i = 0; i < n; i++) {
ofImage* cur = new ofImage();
cur->loadImage(dir.getPath(i));
images.push_back(cur);
ofImage* curAlpha = new ofImage();
curAlpha->allocate(cur->getWidth(), cur->getHeight(), OF_IMAGE_GRAYSCALE);
// use opencv to extract the alpha channel from cur and put it in curAlpha
int rows = cur->getHeight();
int cols = cur->getWidth();
CvMat curMat = cvMat(rows, cols, CV_8UC4, cur->getPixels());
CvMat curAlphaMat = cvMat(rows, cols, CV_8UC1, curAlpha->getPixels());
cvSplit(&curMat, NULL, NULL, NULL, &curAlphaMat);
curAlpha->update();
alpha.push_back(curAlpha);
}
}
LDARec::LDARec(int class_num,vector<string>& IDs)
{
cnum = class_num;
for(int i=0;i<cnum;++i)
this->StIDs.push_back(IDs[i]);
char str[100];
int width,height;
FILE *fin;
std::printf("read LDAWcross\n");
sprintf(str,"%s/LDAWcross",settingpath);
fin = fopen(str,"r");
ReadDoubleMatrix(fin,w,width,height);
fclose(fin);
this->Ws = cvMat(height,width,CV_64FC1,(void*)(&(w[0])) );
printf("Ws %d,%d,%f,%f\n",width,height,cvmGet(&Ws, 0, 0),cvmGet(&Ws, 0, 1));
std::printf("read LDAMeancross\n");
sprintf(str,"%s/LDAMEANcross",settingpath);
fin = fopen(str,"r");
ReadDoubleMatrix(fin,m1,width,height);
fclose(fin);
this->mean1d = cvMat(height,width,CV_64FC1,(void*)(&(m1[0])) );
printf("mean1d %d,%d,%f,%f\n",width,height,cvmGet(&mean1d, 0, 0),cvmGet(&mean1d, 0, 1));
ydata = new double[Ws.rows];
y = cvMat(1,Ws.rows,CV_64FC1,ydata);
}
void ConvexityClassifier::findConvexityDefects(vector<Point>& contour, vector<int>& hull, vector<Point>& convexDefects){
if(hull.size() > 0 && contour.size() > 0){
CvSeq* contourPoints;
CvSeq* defects;
CvMemStorage* storage;
CvMemStorage* strDefects;
CvMemStorage* contourStr;
CvConvexityDefect *defectArray = 0;
strDefects = cvCreateMemStorage();
defects = cvCreateSeq( CV_SEQ_KIND_GENERIC|CV_32SC2, sizeof(CvSeq),sizeof(CvPoint), strDefects );
//We transform our vector<Point> into a CvSeq* object of CvPoint.
contourStr = cvCreateMemStorage();
contourPoints = cvCreateSeq(CV_SEQ_KIND_GENERIC|CV_32SC2, sizeof(CvSeq), sizeof(CvPoint), contourStr);
for(int i=0; i<(int)contour.size(); i++) {
CvPoint cp = {contour[i].x, contour[i].y};
cvSeqPush(contourPoints, &cp);
}
//Now, we do the same thing with the hull index
int count = (int)hull.size();
//int hullK[count];
int* hullK = (int*)malloc(count*sizeof(int));
for(int i=0; i<count; i++){hullK[i] = hull.at(i);}
CvMat hullMat = cvMat(1, count, CV_32SC1, hullK);
//We calculate convexity defects
storage = cvCreateMemStorage(0);
defects = cvConvexityDefects(contourPoints, &hullMat, storage);
defectArray = (CvConvexityDefect*)malloc(sizeof(CvConvexityDefect)*defects->total);
cvCvtSeqToArray(defects, defectArray, CV_WHOLE_SEQ);
//printf("DefectArray %i %i\n",defectArray->end->x, defectArray->end->y);
//We store defects points in the convexDefects parameter.
for(int i = 0; i<defects->total; i++){
CvPoint ptf;
ptf.x = defectArray[i].depth_point->x;
ptf.y = defectArray[i].depth_point->y;
convexDefects.push_back(ptf);
}
//We release memory
cvReleaseMemStorage(&contourStr);
cvReleaseMemStorage(&strDefects);
cvReleaseMemStorage(&storage);
}
}
//计算图2的四个角经矩阵H变换后的坐标
void SiftMatch::CalcFourCorner()
{
//计算图2的四个角经矩阵H变换后的坐标
double v2[]={0,0,1};//左上角
double v1[3];//变换后的坐标值
CvMat V2 = cvMat(3,1,CV_64FC1,v2);
CvMat V1 = cvMat(3,1,CV_64FC1,v1);
cvGEMM(H,&V2,1,0,1,&V1);//矩阵乘法
leftTop.x = cvRound(v1[0]/v1[2]);
leftTop.y = cvRound(v1[1]/v1[2]);
//cvCircle(xformed,leftTop,7,CV_RGB(255,0,0),2);
//将v2中数据设为左下角坐标
v2[0] = 0;
v2[1] = img2->height;
V2 = cvMat(3,1,CV_64FC1,v2);
V1 = cvMat(3,1,CV_64FC1,v1);
cvGEMM(H,&V2,1,0,1,&V1);
leftBottom.x = cvRound(v1[0]/v1[2]);
leftBottom.y = cvRound(v1[1]/v1[2]);
//cvCircle(xformed,leftBottom,7,CV_RGB(255,0,0),2);
//将v2中数据设为右上角坐标
v2[0] = img2->width;
v2[1] = 0;
V2 = cvMat(3,1,CV_64FC1,v2);
V1 = cvMat(3,1,CV_64FC1,v1);
cvGEMM(H,&V2,1,0,1,&V1);
rightTop.x = cvRound(v1[0]/v1[2]);
rightTop.y = cvRound(v1[1]/v1[2]);
//cvCircle(xformed,rightTop,7,CV_RGB(255,0,0),2);
//将v2中数据设为右下角坐标
v2[0] = img2->width;
v2[1] = img2->height;
V2 = cvMat(3,1,CV_64FC1,v2);
V1 = cvMat(3,1,CV_64FC1,v1);
cvGEMM(H,&V2,1,0,1,&V1);
rightBottom.x = cvRound(v1[0]/v1[2]);
rightBottom.y = cvRound(v1[1]/v1[2]);
//cvCircle(xformed,rightBottom,7,CV_RGB(255,0,0),2);
}
CvMat* myCvGetRotationMatrix(CvPoint2D32f center, CvMat* matrix)
{
double m[2][3];
CvMat M =cvMat(2,3,CV_64FC1, m);
double alpha,beta;
double angle=CV_PI;
alpha= cos(angle);
beta= sin(angle);
m[0][0]=alpha;
m[0][1]=beta;
m[0][2]=(1-alpha)*center.x - beta*center.y;
m[1][0]=-beta;
m[1][1]=alpha;
m[1][2]=beta*center.x + (1-alpha)* center.y;
cvConvert(&M,matrix);
return matrix;
}
void saveFramePoses(const string& dirname, vector<FramePose>& framePoses) {
// TODO: for now, turn poses into a CvMat of numOfKeyFrames x 7 (index, rod[3], shift[3])
double _poses[framePoses.size()*7];
CvMat _framePoses = cvMat(framePoses.size(), 7, CV_64FC1, _poses);
int i=0;
for (vector<FramePose>::const_iterator iter= framePoses.begin(); iter!=framePoses.end(); iter++,i++) {
_poses[i*7 + 0] = iter->mIndex;
_poses[i*7 + 1] = iter->mRod.x;
_poses[i*7 + 2] = iter->mRod.y;
_poses[i*7 + 3] = iter->mRod.z;
_poses[i*7 + 4] = iter->mShift.x;
_poses[i*7 + 5] = iter->mShift.y;
_poses[i*7 + 6] = iter->mShift.z;
}
if (i>0) {
string framePosesFilename("framePoses.xml");
cvSave((dirname+framePosesFilename).c_str(), &_framePoses, "index-rod3-shift3", "indices, rodrigues and shifts w.r.t. starting frame");
}
}
CvMat* FeatureManager::getMbrmFeatureByImage(IplImage* pImage, int rowNumber, int colNumber)//the feature is row vector;the returned mat need to be released by user.
{
const int MBRMDIM=17;
int x=0,y=0;//
int rectWidth=pImage->width/colNumber;
int rectHeight=pImage->height/rowNumber;
double* pFeature=new double[rowNumber*colNumber*MBRMDIM];
CvRect roi=cvRect(x, y, rectWidth, rectHeight);
for (int row=0; row!=rowNumber; ++row)
{
for (int col=0; col!=colNumber; ++col)
{
IplImage* pGridImage=::cvCreateImage(cvSize(roi.width,roi.height),pImage->depth,pImage->nChannels);
roi.x=col*rectWidth;
roi.y=row*rectHeight;
// cvGetSubImage(pImage, pGridImage, roi);
cvSetImageROI(pImage,roi);
// result=cvCreateImage(cvSize(roi.width,roi.height),image->depth,image->nChannels);
cvCopy(pImage,pGridImage);
cvResetImageROI(pImage);
double* pGridFeature=getGridMbrmFeature(pGridImage);
for (int i=0;i!=MBRMDIM;++i)
{
pFeature[MBRMDIM*(row*colNumber+col)+i]=pGridFeature[i];
}
delete [] pGridFeature;
cvReleaseImage(&pGridImage);
}
}
CvMat tmp = cvMat(rowNumber*colNumber,MBRMDIM,CV_64FC1,pFeature);
CvMat* pFeatureMat=cvCreateMat(rowNumber*colNumber,MBRMDIM,CV_64FC1);
cvCopy(&tmp,pFeatureMat);
delete [] pFeature;
return pFeatureMat;
}