double mycalibrateCamera( InputArrayOfArrays _objectPoints, InputArrayOfArrays _imagePoints, Size imageSize, InputOutputArray _cameraMatrix, InputOutputArray _distCoeffs, OutputArrayOfArrays _rvecs, OutputArrayOfArrays _tvecs, int flags, TermCriteria criteria ) { int rtype = CV_64F; Mat cameraMatrix = _cameraMatrix.getMat(); cameraMatrix = prepareCameraMatrix(cameraMatrix, rtype); Mat distCoeffs = _distCoeffs.getMat(); distCoeffs = prepareDistCoeffs(distCoeffs, rtype); if( !(flags & CALIB_RATIONAL_MODEL) ) distCoeffs = distCoeffs.rows == 1 ? distCoeffs.colRange(0, 5) : distCoeffs.rowRange(0, 5); int i; size_t nimages = _objectPoints.total(); CV_Assert( nimages > 0 ); Mat objPt, imgPt, npoints, rvecM((int)nimages, 3, CV_64FC1), tvecM((int)nimages, 3, CV_64FC1); collectCalibrationData( _objectPoints, _imagePoints, noArray(), objPt, imgPt, 0, npoints ); CvMat c_objPt = objPt, c_imgPt = imgPt, c_npoints = npoints; CvMat c_cameraMatrix = cameraMatrix, c_distCoeffs = distCoeffs; CvMat c_rvecM = rvecM, c_tvecM = tvecM; double reprojErr = cvCalibrateCamera2(&c_objPt, &c_imgPt, &c_npoints, imageSize, &c_cameraMatrix, &c_distCoeffs, &c_rvecM, &c_tvecM, flags, criteria ); bool rvecs_needed = _rvecs.needed(), tvecs_needed = _tvecs.needed(); if( rvecs_needed ) _rvecs.create((int)nimages, 1, CV_64FC3); if( tvecs_needed ) _tvecs.create((int)nimages, 1, CV_64FC3); for( i = 0; i < (int)nimages; i++ ) { if( rvecs_needed ) { _rvecs.create(3, 1, CV_64F, i, true); Mat rv = _rvecs.getMat(i); memcpy(rv.data, rvecM.ptr<double>(i), 3*sizeof(double)); } if( tvecs_needed ) { _tvecs.create(3, 1, CV_64F, i, true); Mat tv = _tvecs.getMat(i); memcpy(tv.data, tvecM.ptr<double>(i), 3*sizeof(double)); } } cameraMatrix.copyTo(_cameraMatrix); distCoeffs.copyTo(_distCoeffs); return reprojErr; }
int cv::floodFill( InputOutputArray _image, InputOutputArray _mask, Point seedPoint, Scalar newVal, Rect* rect, Scalar loDiff, Scalar upDiff, int flags ) { CvConnectedComp ccomp; CvMat c_image = _image.getMat(), c_mask = _mask.getMat(); cvFloodFill(&c_image, seedPoint, newVal, loDiff, upDiff, &ccomp, flags, c_mask.data.ptr ? &c_mask : 0); if( rect ) *rect = ccomp.rect; return cvRound(ccomp.area); }
void RhoanaBlocksGainCompensator::apply(int index, Point /*corner*/, InputOutputArray _image, InputArray /*mask*/) { CV_INSTRUMENT_REGION() ///CV_Assert(image.type() == CV_8UC3); CV_Assert(_image.type() == CV_8UC1); UMat u_gain_map; if (gain_maps_[index].size() == _image.size()) u_gain_map = gain_maps_[index]; else resize(gain_maps_[index], u_gain_map, _image.size(), 0, 0, INTER_LINEAR); Mat_<float> gain_map = u_gain_map.getMat(ACCESS_READ); Mat image = _image.getMat(); for (int y = 0; y < image.rows; ++y) { const float* gain_row = gain_map.ptr<float>(y); ///Point3_<uchar>* row = image.ptr<Point3_<uchar> >(y); uchar* row = image.ptr<uchar>(y); for (int x = 0; x < image.cols; ++x) { ///row[x].x = saturate_cast<uchar>(row[x].x * gain_row[x]); ///row[x].y = saturate_cast<uchar>(row[x].y * gain_row[x]); ///row[x].z = saturate_cast<uchar>(row[x].z * gain_row[x]); row[x] = saturate_cast<uchar>(row[x] * gain_row[x]); } } }
void cv::updateMotionHistory( InputArray _silhouette, InputOutputArray _mhi, double timestamp, double duration ) { Mat silhouette = _silhouette.getMat(); CvMat c_silhouette = silhouette, c_mhi = _mhi.getMat(); cvUpdateMotionHistory( &c_silhouette, &c_mhi, timestamp, duration ); }
void Utility::drawSegmentBorder(InputOutputArray imgInputOutput, Mat_<int> segment, Scalar color /*= Scalar(255, 255, 255)*/) { Mat img = imgInputOutput.getMat(); Mat_<uchar> mask(img.rows, img.cols); // ��־SuperPixel�߽�ľ��� mask.setTo(0); int rows = img.rows; int cols = img.cols; for (int i = 1; i < rows; i++) { int* pCurrentPoint = segment.ptr<int>(i)+1; // ָ��ǰ�� int* pUpperPoint = segment.ptr<int>(i-1)+1; // ָ������ĵ� for (int j = 1; j < cols; j++) { int cPoint = *pCurrentPoint; // ��ǰ���SuperPixelID int lPoint = *(pCurrentPoint-1); // ��ߵ��SuperPixelID int uPoint = *pUpperPoint; // ������SuperPixelID if (cPoint != lPoint || cPoint != uPoint) { mask(i, j) = 1; } pCurrentPoint++; pUpperPoint++; } } add(img, color, img, mask); }
void reconstruct_(const T &input, OutputArray Rs, OutputArray Ts, InputOutputArray K, OutputArray points3d, const bool refinement=true) { // Initial reconstruction const int keyframe1 = 1, keyframe2 = 2; const int select_keyframes = 1; // enable automatic keyframes selection const int verbosity_level = -1; // mute libmv logs // Refinement parameters const int refine_intrinsics = ( !refinement ) ? 0 : SFM_REFINE_FOCAL_LENGTH | SFM_REFINE_PRINCIPAL_POINT | SFM_REFINE_RADIAL_DISTORTION_K1 | SFM_REFINE_RADIAL_DISTORTION_K2; // Camera data Matx33d Ka = K.getMat(); const double focal_length = Ka(0,0); const double principal_x = Ka(0,2), principal_y = Ka(1,2), k1 = 0, k2 = 0, k3 = 0; // Set reconstruction options libmv_ReconstructionOptions reconstruction_options(keyframe1, keyframe2, refine_intrinsics, select_keyframes, verbosity_level); libmv_CameraIntrinsicsOptions camera_instrinsic_options = libmv_CameraIntrinsicsOptions(SFM_DISTORTION_MODEL_POLYNOMIAL, focal_length, principal_x, principal_y, k1, k2, k3); //-- Instantiate reconstruction pipeline Ptr<BaseSFM> reconstruction = SFMLibmvEuclideanReconstruction::create(camera_instrinsic_options, reconstruction_options); //-- Run reconstruction pipeline reconstruction->run(input, K, Rs, Ts, points3d); }
void fillOcclusion(InputOutputArray src_, int invalidvalue, int disp_or_depth) { CV_Assert(src_.channels() == 1); Mat src = src_.getMat(); if (disp_or_depth == FILL_DEPTH) { if (src.depth() == CV_8U) { fillOcclusionInv_<uchar>(src, (uchar)invalidvalue, 0); } else if (src.depth() == CV_16S) { fillOcclusionInv_<short>(src, (short)invalidvalue, 0); } else if (src.depth() == CV_16U) { fillOcclusionInv_<ushort>(src, (ushort)invalidvalue, 0); } else if (src.depth() == CV_32S) { fillOcclusionInv_<int>(src, (int)invalidvalue, 0); } else if (src.depth() == CV_32F) { fillOcclusionInv_<float>(src, (float)invalidvalue, 0.f); } else if (src.depth() == CV_64F) { fillOcclusionInv_ <double> (src, (double)invalidvalue, 0.f); } } else { if (src.depth() == CV_8U) { fillOcclusion_<uchar>(src, (uchar)invalidvalue, 255); } else if (src.depth() == CV_16S) { fillOcclusion_<short>(src, (short)invalidvalue, SHRT_MAX); } else if (src.depth() == CV_16U) { fillOcclusion_<ushort>(src, (ushort)invalidvalue, USHRT_MAX); } else if (src.depth() == CV_32S) { fillOcclusion_<int>(src, (int)invalidvalue, INT_MAX); } else if (src.depth() == CV_32F) { fillOcclusion_<float>(src, (float)invalidvalue, FLT_MAX); } else if (src.depth() == CV_64F) { fillOcclusion_<double>(src, (double)invalidvalue, DBL_MAX); } } }
void cv::insertChannel(InputArray _src, InputOutputArray _dst, int coi) { Mat src = _src.getMat(), dst = _dst.getMat(); CV_Assert( src.size == dst.size && src.depth() == dst.depth() ); CV_Assert( 0 <= coi && coi < dst.channels() && src.channels() == 1 ); int ch[] = { 0, coi }; mixChannels(&src, 1, &dst, 1, ch, 1); }
double ConjGradSolverImpl::minimize(InputOutputArray x){ CV_Assert(_Function.empty()==false); dprintf(("termcrit:\n\ttype: %d\n\tmaxCount: %d\n\tEPS: %g\n",_termcrit.type,_termcrit.maxCount,_termcrit.epsilon)); Mat x_mat=x.getMat(); CV_Assert(MIN(x_mat.rows,x_mat.cols)==1); int ndim=MAX(x_mat.rows,x_mat.cols); CV_Assert(x_mat.type()==CV_64FC1); if(d.cols!=ndim){ d.create(1,ndim); r.create(1,ndim); r_old.create(1,ndim); minimizeOnTheLine_buf1.create(1,ndim); minimizeOnTheLine_buf2.create(1,ndim); } Mat_<double> proxy_x; if(x_mat.rows>1){ buf_x.create(1,ndim); Mat_<double> proxy(ndim,1,buf_x.ptr<double>()); x_mat.copyTo(proxy); proxy_x=buf_x; }else{ proxy_x=x_mat; } _Function->getGradient(proxy_x.ptr<double>(),d.ptr<double>()); d*=-1.0; d.copyTo(r); //here everything goes. check that everything is setted properly dprintf(("proxy_x\n"));print_matrix(proxy_x); dprintf(("d first time\n"));print_matrix(d); dprintf(("r\n"));print_matrix(r); for(int count=0;count<_termcrit.maxCount;count++){ minimizeOnTheLine(_Function,proxy_x,d,minimizeOnTheLine_buf1,minimizeOnTheLine_buf2); r.copyTo(r_old); _Function->getGradient(proxy_x.ptr<double>(),r.ptr<double>()); r*=-1.0; double r_norm_sq=norm(r); if(_termcrit.type==(TermCriteria::MAX_ITER+TermCriteria::EPS) && r_norm_sq<_termcrit.epsilon){ break; } r_norm_sq=r_norm_sq*r_norm_sq; double beta=MAX(0.0,(r_norm_sq-r.dot(r_old))/r_norm_sq); d=r+beta*d; } if(x_mat.rows>1){ Mat(ndim, 1, CV_64F, proxy_x.ptr<double>()).copyTo(x); } return _Function->calc(proxy_x.ptr<double>()); }
static void fftShift(InputOutputArray _out) { Mat out = _out.getMat(); if(out.rows == 1 && out.cols == 1) { // trivially shifted. return; } std::vector<Mat> planes; split(out, planes); int xMid = out.cols >> 1; int yMid = out.rows >> 1; bool is_1d = xMid == 0 || yMid == 0; if(is_1d) { xMid = xMid + yMid; for(size_t i = 0; i < planes.size(); i++) { Mat tmp; Mat half0(planes[i], Rect(0, 0, xMid, 1)); Mat half1(planes[i], Rect(xMid, 0, xMid, 1)); half0.copyTo(tmp); half1.copyTo(half0); tmp.copyTo(half1); } } else { for(size_t i = 0; i < planes.size(); i++) { // perform quadrant swaps... Mat tmp; Mat q0(planes[i], Rect(0, 0, xMid, yMid)); Mat q1(planes[i], Rect(xMid, 0, xMid, yMid)); Mat q2(planes[i], Rect(0, yMid, xMid, yMid)); Mat q3(planes[i], Rect(xMid, yMid, xMid, yMid)); q0.copyTo(tmp); q3.copyTo(q0); tmp.copyTo(q3); q1.copyTo(tmp); q2.copyTo(q1); tmp.copyTo(q2); } } merge(planes, out); }
void setDepthMaxValue(InputOutputArray src) { Mat s = src.getMat(); if (s.depth() == CV_8U)s.setTo(UCHAR_MAX); else if (s.depth() == CV_16U)s.setTo(USHRT_MAX); else if (s.depth() == CV_16S)s.setTo(SHRT_MAX); else if (s.depth() == CV_32S)s.setTo(INT_MAX); else if (s.depth() == CV_32F)s.setTo(FLT_MAX); else if (s.depth() == CV_64F)s.setTo(DBL_MAX); }
void extractLibmvReconstructionData(InputOutputArray K, OutputArray Rs, OutputArray Ts, OutputArray points3d) { getCameras(Rs, Ts); getPoints(points3d); getIntrinsics().copyTo(K.getMat()); }
void reconstruct(const std::vector<cv::String> images, OutputArray Ps, OutputArray points3d, InputOutputArray K, bool is_projective) { const int nviews = static_cast<int>(images.size()); CV_Assert( nviews >= 2 ); Matx33d Ka = K.getMat(); const int depth = Mat(Ka).depth(); // Projective reconstruction if ( is_projective ) { std::vector<Mat> Rs, Ts; reconstruct(images, Rs, Ts, Ka, points3d, is_projective); // From Rs and Ts, extract Ps const int nviews_est = Rs.size(); Ps.create(nviews_est, 1, depth); Matx34d P; for (size_t i = 0; i < nviews_est; ++i) { projectionFromKRt(Ka, Rs[i], Vec3d(Ts[i]), P); Mat(P).copyTo(Ps.getMatRef(i)); } Mat(Ka).copyTo(K.getMat()); } // Affine reconstruction else { // TODO: implement me CV_Error(Error::StsNotImplemented, "Affine reconstruction not yet implemented"); } }
void removeRows(InputOutputArray _src, cv::Range range) { CV_Assert( range.start >= 0 && range.end < _src.getMat().rows ); Mat src = _src.getMat(); int type = src.type(); // if(src.data != dst.data || src.step != dst.step) // { for(int i = range.start; i <= range.end; ++i) { // cout << i << endl; removeRow(src, src, range.start); // cout << "src: " << endl << src << endl; } src.copyTo(_src); // } return; }
static void _prepareImgAndDrawKeypoints( InputArray img1, const std::vector<KeyPoint>& keypoints1, InputArray img2, const std::vector<KeyPoint>& keypoints2, InputOutputArray _outImg, Mat& outImg1, Mat& outImg2, const Scalar& singlePointColor, DrawMatchesFlags flags ) { Mat outImg; Size img1size = img1.size(), img2size = img2.size(); Size size( img1size.width + img2size.width, MAX(img1size.height, img2size.height) ); if( !!(flags & DrawMatchesFlags::DRAW_OVER_OUTIMG) ) { outImg = _outImg.getMat(); if( size.width > outImg.cols || size.height > outImg.rows ) CV_Error( Error::StsBadSize, "outImg has size less than need to draw img1 and img2 together" ); outImg1 = outImg( Rect(0, 0, img1size.width, img1size.height) ); outImg2 = outImg( Rect(img1size.width, 0, img2size.width, img2size.height) ); } else { const int cn1 = img1.channels(), cn2 = img2.channels(); const int out_cn = std::max(3, std::max(cn1, cn2)); _outImg.create(size, CV_MAKETYPE(img1.depth(), out_cn)); outImg = _outImg.getMat(); outImg = Scalar::all(0); outImg1 = outImg( Rect(0, 0, img1size.width, img1size.height) ); outImg2 = outImg( Rect(img1size.width, 0, img2size.width, img2size.height) ); _prepareImage(img1, outImg1); _prepareImage(img2, outImg2); } // draw keypoints if( !(flags & DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS) ) { Mat _outImg1 = outImg( Rect(0, 0, img1size.width, img1size.height) ); drawKeypoints( _outImg1, keypoints1, _outImg1, singlePointColor, flags | DrawMatchesFlags::DRAW_OVER_OUTIMG ); Mat _outImg2 = outImg( Rect(img1size.width, 0, img2size.width, img2size.height) ); drawKeypoints( _outImg2, keypoints2, _outImg2, singlePointColor, flags | DrawMatchesFlags::DRAW_OVER_OUTIMG ); } }
void cv::cornerSubPix( InputArray _image, InputOutputArray _corners, Size winSize, Size zeroZone, TermCriteria criteria ) { Mat corners = _corners.getMat(); int ncorners = corners.checkVector(2); CV_Assert( ncorners >= 0 && corners.depth() == CV_32F ); Mat image = _image.getMat(); CvMat c_image = image; cvFindCornerSubPix( &c_image, (CvPoint2D32f*)corners.data, ncorners, winSize, zeroZone, criteria ); }
void CustomPattern::drawOrientation(InputOutputArray image, InputArray tvec, InputArray rvec, InputArray cameraMatrix, InputArray distCoeffs, double axis_length, double axis_width) { Point3f ptrCtr3d = Point3f((img_roi.cols * pxSize)/2, (img_roi.rows * pxSize)/2, 0); vector<Point3f> axis(4); axis[0] = ptrCtr3d; axis[1] = Point3f(axis_length * pxSize, 0, 0) + ptrCtr3d; axis[2] = Point3f(0, axis_length * pxSize, 0) + ptrCtr3d; axis[3] = Point3f(0, 0, -axis_length * pxSize) + ptrCtr3d; vector<Point2f> proj_axis; projectPoints(axis, rvec, tvec, cameraMatrix, distCoeffs, proj_axis); Mat img = image.getMat(); line(img, proj_axis[0], proj_axis[1], CV_RGB(255, 0, 0), axis_width); line(img, proj_axis[0], proj_axis[2], CV_RGB(0, 255, 0), axis_width); line(img, proj_axis[0], proj_axis[3], CV_RGB(0, 0, 255), axis_width); img.copyTo(image); }
void CustomPattern::drawOrientation(InputOutputArray image, InputArray tvec, InputArray rvec, InputArray cameraMatrix, InputArray distCoeffs, double axis_length, int axis_width) { Point3f ptrCtr3d = Point3f(float((img_roi.cols * pxSize)/2.0), float((img_roi.rows * pxSize)/2.0), 0); vector<Point3f> axis(4); float alen = float(axis_length * pxSize); axis[0] = ptrCtr3d; axis[1] = Point3f(alen, 0, 0) + ptrCtr3d; axis[2] = Point3f(0, alen, 0) + ptrCtr3d; axis[3] = Point3f(0, 0, -alen) + ptrCtr3d; vector<Point2f> proj_axis; projectPoints(axis, rvec, tvec, cameraMatrix, distCoeffs, proj_axis); Mat img = image.getMat(); line(img, proj_axis[0], proj_axis[1], Scalar(0, 0, 255), axis_width); // red line(img, proj_axis[0], proj_axis[2], Scalar(0, 255, 0), axis_width); // green line(img, proj_axis[0], proj_axis[3], Scalar(255, 0, 0), axis_width); // blue img.copyTo(image); }
void cv::watershed( InputArray _src, InputOutputArray _markers ) { const int IN_QUEUE = -2; const int WSHED = -1; const int NQ = 256; Mat src = _src.getMat(), dst = _markers.getMat(); Size size = src.size(); std::vector<WSNode> storage; int free_node = 0, node; WSQueue q[NQ]; int active_queue; int i, j; int db, dg, dr; int subs_tab[513]; // MAX(a,b) = b + MAX(a-b,0) #define ws_max(a,b) ((b) + subs_tab[(a)-(b)+NQ]) // MIN(a,b) = a - MAX(a-b,0) #define ws_min(a,b) ((a) - subs_tab[(a)-(b)+NQ]) #define ws_push(idx,mofs,iofs) \ { \ if( !free_node ) \ free_node = allocWSNodes( storage );\ node = free_node; \ free_node = storage[free_node].next;\ storage[node].next = 0; \ storage[node].mask_ofs = mofs; \ storage[node].img_ofs = iofs; \ if( q[idx].last ) \ storage[q[idx].last].next=node; \ else \ q[idx].first = node; \ q[idx].last = node; \ } #define ws_pop(idx,mofs,iofs) \ { \ node = q[idx].first; \ q[idx].first = storage[node].next; \ if( !storage[node].next ) \ q[idx].last = 0; \ storage[node].next = free_node; \ free_node = node; \ mofs = storage[node].mask_ofs; \ iofs = storage[node].img_ofs; \ } #define c_diff(ptr1,ptr2,diff) \ { \ db = std::abs((ptr1)[0] - (ptr2)[0]);\ dg = std::abs((ptr1)[1] - (ptr2)[1]);\ dr = std::abs((ptr1)[2] - (ptr2)[2]);\ diff = ws_max(db,dg); \ diff = ws_max(diff,dr); \ assert( 0 <= diff && diff <= 255 ); \ } CV_Assert( src.type() == CV_8UC3 && dst.type() == CV_32SC1 ); CV_Assert( src.size() == dst.size() ); const uchar* img = src.data; int istep = int(src.step/sizeof(img[0])); int* mask = dst.ptr<int>(); int mstep = int(dst.step / sizeof(mask[0])); for( i = 0; i < 256; i++ ) subs_tab[i] = 0; for( i = 256; i <= 512; i++ ) subs_tab[i] = i - 256; // draw a pixel-wide border of dummy "watershed" (i.e. boundary) pixels for( j = 0; j < size.width; j++ ) mask[j] = mask[j + mstep*(size.height-1)] = WSHED; // initial phase: put all the neighbor pixels of each marker to the ordered queue - // determine the initial boundaries of the basins for( i = 1; i < size.height-1; i++ ) { img += istep; mask += mstep; mask[0] = mask[size.width-1] = WSHED; for( j = 1; j < size.width-1; j++ ) { int* m = mask + j; if( m[0] < 0 ) m[0] = 0; if( m[0] == 0 && (m[-1] > 0 || m[1] > 0 || m[-mstep] > 0 || m[mstep] > 0) ) { const uchar* ptr = img + j*3; int idx = 256, t; if( m[-1] > 0 ) c_diff( ptr, ptr - 3, idx ); if( m[1] > 0 ) { c_diff( ptr, ptr + 3, t ); idx = ws_min( idx, t ); } if( m[-mstep] > 0 ) { c_diff( ptr, ptr - istep, t ); idx = ws_min( idx, t ); } if( m[mstep] > 0 ) { c_diff( ptr, ptr + istep, t ); idx = ws_min( idx, t ); } assert( 0 <= idx && idx <= 255 ); ws_push( idx, i*mstep + j, i*istep + j*3 ); m[0] = IN_QUEUE; } } } // find the first non-empty queue for( i = 0; i < NQ; i++ ) if( q[i].first ) break; // if there is no markers, exit immediately if( i == NQ ) return; active_queue = i; img = src.data; mask = dst.ptr<int>(); // recursively fill the basins for(;;) { int mofs, iofs; int lab = 0, t; int* m; const uchar* ptr; if( q[active_queue].first == 0 ) { for( i = active_queue+1; i < NQ; i++ ) if( q[i].first ) break; if( i == NQ ) break; active_queue = i; } ws_pop( active_queue, mofs, iofs ); m = mask + mofs; ptr = img + iofs; t = m[-1]; if( t > 0 ) lab = t; t = m[1]; if( t > 0 ) { if( lab == 0 ) lab = t; else if( t != lab ) lab = WSHED; } t = m[-mstep]; if( t > 0 ) { if( lab == 0 ) lab = t; else if( t != lab ) lab = WSHED; } t = m[mstep]; if( t > 0 ) { if( lab == 0 ) lab = t; else if( t != lab ) lab = WSHED; } assert( lab != 0 ); m[0] = lab; if( lab == WSHED ) continue; if( m[-1] == 0 ) { c_diff( ptr, ptr - 3, t ); ws_push( t, mofs - 1, iofs - 3 ); active_queue = ws_min( active_queue, t ); m[-1] = IN_QUEUE; } if( m[1] == 0 ) { c_diff( ptr, ptr + 3, t ); ws_push( t, mofs + 1, iofs + 3 ); active_queue = ws_min( active_queue, t ); m[1] = IN_QUEUE; } if( m[-mstep] == 0 ) { c_diff( ptr, ptr - istep, t ); ws_push( t, mofs - mstep, iofs - istep ); active_queue = ws_min( active_queue, t ); m[-mstep] = IN_QUEUE; } if( m[mstep] == 0 ) { c_diff( ptr, ptr + istep, t ); ws_push( t, mofs + mstep, iofs + istep ); active_queue = ws_min( active_queue, t ); m[mstep] = IN_QUEUE; } } }
void cv::watershed( InputArray _src, InputOutputArray _markers ) { // Labels for pixels const int IN_QUEUE = -2; // Pixel visited const int WSHED = -1; // Pixel belongs to watershed // possible bit values = 2^8 const int NQ = 256; Mat src = _src.getMat(), dst = _markers.getMat(); Size size = src.size(); // Vector of every created node std::vector<WSNode> storage; int free_node = 0, node; // Priority queue of queues of nodes // from high priority (0) to low priority (255) WSQueue q[NQ]; // Non-empty queue with highest priority int active_queue; int i, j; // Color differences int db, dg, dr; int subs_tab[513]; // MAX(a,b) = b + MAX(a-b,0) #define ws_max(a,b) ((b) + subs_tab[(a)-(b)+NQ]) // MIN(a,b) = a - MAX(a-b,0) #define ws_min(a,b) ((a) - subs_tab[(a)-(b)+NQ]) // Create a new node with offsets mofs and iofs in queue idx #define ws_push(idx,mofs,iofs) \ { \ if( !free_node ) \ free_node = allocWSNodes( storage );\ node = free_node; \ free_node = storage[free_node].next;\ storage[node].next = 0; \ storage[node].mask_ofs = mofs; \ storage[node].img_ofs = iofs; \ if( q[idx].last ) \ storage[q[idx].last].next=node; \ else \ q[idx].first = node; \ q[idx].last = node; \ } // Get next node from queue idx #define ws_pop(idx,mofs,iofs) \ { \ node = q[idx].first; \ q[idx].first = storage[node].next; \ if( !storage[node].next ) \ q[idx].last = 0; \ storage[node].next = free_node; \ free_node = node; \ mofs = storage[node].mask_ofs; \ iofs = storage[node].img_ofs; \ } // Get highest absolute channel difference in diff #define c_diff(ptr1,ptr2,diff) \ { \ db = std::abs((ptr1)[0] - (ptr2)[0]);\ dg = std::abs((ptr1)[1] - (ptr2)[1]);\ dr = std::abs((ptr1)[2] - (ptr2)[2]);\ diff = ws_max(db,dg); \ diff = ws_max(diff,dr); \ assert( 0 <= diff && diff <= 255 ); \ } CV_Assert( src.type() == CV_8UC3 && dst.type() == CV_32SC1 ); CV_Assert( src.size() == dst.size() ); // Current pixel in input image const uchar* img = src.ptr(); // Step size to next row in input image int istep = int(src.step/sizeof(img[0])); // Current pixel in mask image int* mask = dst.ptr<int>(); // Step size to next row in mask image int mstep = int(dst.step / sizeof(mask[0])); for( i = 0; i < 256; i++ ) subs_tab[i] = 0; for( i = 256; i <= 512; i++ ) subs_tab[i] = i - 256; // draw a pixel-wide border of dummy "watershed" (i.e. boundary) pixels for( j = 0; j < size.width; j++ ) mask[j] = mask[j + mstep*(size.height-1)] = WSHED; // initial phase: put all the neighbor pixels of each marker to the ordered queue - // determine the initial boundaries of the basins for( i = 1; i < size.height-1; i++ ) { img += istep; mask += mstep; mask[0] = mask[size.width-1] = WSHED; // boundary pixels for( j = 1; j < size.width-1; j++ ) { int* m = mask + j; if( m[0] < 0 ) m[0] = 0; if( m[0] == 0 && (m[-1] > 0 || m[1] > 0 || m[-mstep] > 0 || m[mstep] > 0) ) { // Find smallest difference to adjacent markers const uchar* ptr = img + j*3; int idx = 256, t; if( m[-1] > 0 ) c_diff( ptr, ptr - 3, idx ); if( m[1] > 0 ) { c_diff( ptr, ptr + 3, t ); idx = ws_min( idx, t ); } if( m[-mstep] > 0 ) { c_diff( ptr, ptr - istep, t ); idx = ws_min( idx, t ); } if( m[mstep] > 0 ) { c_diff( ptr, ptr + istep, t ); idx = ws_min( idx, t ); } // Add to according queue assert( 0 <= idx && idx <= 255 ); ws_push( idx, i*mstep + j, i*istep + j*3 ); m[0] = IN_QUEUE; } } } // find the first non-empty queue for( i = 0; i < NQ; i++ ) if( q[i].first ) break; // if there is no markers, exit immediately if( i == NQ ) return; active_queue = i; img = src.ptr(); mask = dst.ptr<int>(); // recursively fill the basins for(;;) { int mofs, iofs; int lab = 0, t; int* m; const uchar* ptr; // Get non-empty queue with highest priority // Exit condition: empty priority queue if( q[active_queue].first == 0 ) { for( i = active_queue+1; i < NQ; i++ ) if( q[i].first ) break; if( i == NQ ) break; active_queue = i; } // Get next node ws_pop( active_queue, mofs, iofs ); // Calculate pointer to current pixel in input and marker image m = mask + mofs; ptr = img + iofs; // Check surrounding pixels for labels // to determine label for current pixel t = m[-1]; // Left if( t > 0 ) lab = t; t = m[1]; // Right if( t > 0 ) { if( lab == 0 ) lab = t; else if( t != lab ) lab = WSHED; } t = m[-mstep]; // Top if( t > 0 ) { if( lab == 0 ) lab = t; else if( t != lab ) lab = WSHED; } t = m[mstep]; // Bottom if( t > 0 ) { if( lab == 0 ) lab = t; else if( t != lab ) lab = WSHED; } // Set label to current pixel in marker image assert( lab != 0 ); m[0] = lab; if( lab == WSHED ) continue; // Add adjacent, unlabeled pixels to corresponding queue if( m[-1] == 0 ) { c_diff( ptr, ptr - 3, t ); ws_push( t, mofs - 1, iofs - 3 ); active_queue = ws_min( active_queue, t ); m[-1] = IN_QUEUE; } if( m[1] == 0 ) { c_diff( ptr, ptr + 3, t ); ws_push( t, mofs + 1, iofs + 3 ); active_queue = ws_min( active_queue, t ); m[1] = IN_QUEUE; } if( m[-mstep] == 0 ) { c_diff( ptr, ptr - istep, t ); ws_push( t, mofs - mstep, iofs - istep ); active_queue = ws_min( active_queue, t ); m[-mstep] = IN_QUEUE; } if( m[mstep] == 0 ) { c_diff( ptr, ptr + istep, t ); ws_push( t, mofs + mstep, iofs + istep ); active_queue = ws_min( active_queue, t ); m[mstep] = IN_QUEUE; } } }
void removeRow(InputOutputArray _matIn, int row, int method = METHOD::CV_RECT) { CV_Assert( row >= 0 && row < _matIn.getMat().rows ); Mat matIn = _matIn.getMat(); cv::Size size = matIn.size(); Mat matOut( matIn.rows - 1, matIn.cols, matIn.type()); switch(method) { case(METHOD::STD_MEMCPY) : { int rowSizeInBytes = size.width * matIn.elemSize(); if ( row > 0 ) { int numRows = row; int numBytes = rowSizeInBytes * numRows; std::memcpy( matOut.data, matIn.data, numBytes ); } if ( row < size.height - 1 ) { int matOutOffset = rowSizeInBytes * row; int matInOffset = matOutOffset + rowSizeInBytes; int numRows = size.height - ( row + 1 ); int numBytes = rowSizeInBytes * numRows; std::memcpy( matOut.data + matOutOffset , matIn.data + matInOffset, numBytes ); } } break; case(METHOD::CV_RANGE) : { if(matIn.data != matOut.data || matIn.step != matOut.step) { if(row == 0) { matIn(Range(1, matIn.rows), Range(0, matIn.cols)).copyTo(matOut); } else if (row == matIn.rows - 1) { matIn(Range(0, row), Range(0, matIn.cols)).copyTo(matOut); } else { Mat a, b; matIn(Range(0, row), Range(0, matIn.cols)).copyTo(a); matIn(Range(row +1, matIn.rows), Range(0, matIn.cols)).copyTo(b); // cout << "dst: " << a << endl; // cout << "temp: " << b << endl; vconcat(a, b, matOut); } } } break; default : if ( row > 0 ) { cv::Rect rect( 0, 0, size.width, row ); matIn( rect ).copyTo( matOut( rect ) ); } if ( row < size.height - 1 ) { cv::Rect rect1( 0, row + 1, size.width, size.height - row - 1 ); cv::Rect rect2( 0, row, size.width, size.height - row - 1 ); matIn( rect1 ).copyTo( matOut( rect2 ) ); } break; } matOut.copyTo(_matIn); }
void removeCol(InputOutputArray _matIn, int col, int method = METHOD::CV_RECT) { CV_Assert( col >= 0 && col < _matIn.getMat().cols ); Mat matIn = _matIn.getMat(); cv::Size size = matIn.size(); Mat matOut( matIn.rows, matIn.cols - 1, matIn.type()); switch(method) { case(METHOD::STD_MEMCPY) : { int rowInInBytes = size.width * matIn.elemSize(); int rowOutInBytes = ( size.width - 1 ) * matIn.elemSize(); if ( col > 0 ) { int matInOffset = 0; int matOutOffset = 0; int numCols = col; int numBytes = numCols * matIn.elemSize(); for ( int y = 0; y < size.height; ++y ) { std::memcpy( matOut.data + matOutOffset, matIn.data + matInOffset, numBytes ); matInOffset += rowInInBytes; matOutOffset += rowOutInBytes; } } if ( col < size.width - 1 ) { int matInOffset = ( col + 1 ) * matIn.elemSize(); int matOutOffset = col * matIn.elemSize(); int numCols = size.width - ( col + 1 ); int numBytes = numCols * matIn.elemSize(); for ( int y = 0; y < size.height; ++y ) { std::memcpy( matOut.data + matOutOffset, matIn.data + matInOffset, numBytes ); matInOffset += rowInInBytes; matOutOffset += rowOutInBytes; } } } break; case(METHOD::CV_RANGE) : { if(matIn.data != matOut.data || matIn.step != matOut.step) { if(col == 0) { matIn(Range(0, matIn.rows), Range(1, matIn.cols)).copyTo(matOut); } else if (col == matIn.cols - 1) { matIn(Range(0, matIn.rows), Range(0, col)).copyTo(matOut); } else { Mat a, b; matIn(Range(0, matIn.rows), Range(0, col)).copyTo(a); matIn(Range(0, matIn.rows), Range(col + 1, matIn.cols)).copyTo(b); // cout << "dst: " << a << endl; // cout << "temp: " << b << endl; hconcat(a, b, matOut); } } } break; default : if ( col > 0 ) { cv::Rect rect( 0, 0, col, size.height ); matIn( rect ).copyTo( matOut( rect ) ); } if ( col < size.width - 1 ) { cv::Rect rect1( col + 1, 0, size.width - col - 1, size.height ); cv::Rect rect2( col, 0, size.width - col - 1, size.height ); matIn( rect1 ).copyTo( matOut( rect2 ) ); } break; } matOut.copyTo(_matIn); }
double cv::kmeans( InputArray _data, int K, InputOutputArray _bestLabels, TermCriteria criteria, int attempts, int flags, OutputArray _centers ) { const int SPP_TRIALS = 3; Mat data0 = _data.getMat(); bool isrow = data0.rows == 1; int N = isrow ? data0.cols : data0.rows; int dims = (isrow ? 1 : data0.cols)*data0.channels(); int type = data0.depth(); attempts = std::max(attempts, 1); CV_Assert( data0.dims <= 2 && type == CV_32F && K > 0 ); CV_Assert( N >= K ); Mat data(N, dims, CV_32F, data0.ptr(), isrow ? dims * sizeof(float) : static_cast<size_t>(data0.step)); _bestLabels.create(N, 1, CV_32S, -1, true); Mat _labels, best_labels = _bestLabels.getMat(); if( flags & CV_KMEANS_USE_INITIAL_LABELS ) { CV_Assert( (best_labels.cols == 1 || best_labels.rows == 1) && best_labels.cols*best_labels.rows == N && best_labels.type() == CV_32S && best_labels.isContinuous()); best_labels.copyTo(_labels); } else { if( !((best_labels.cols == 1 || best_labels.rows == 1) && best_labels.cols*best_labels.rows == N && best_labels.type() == CV_32S && best_labels.isContinuous())) best_labels.create(N, 1, CV_32S); _labels.create(best_labels.size(), best_labels.type()); } int* labels = _labels.ptr<int>(); Mat centers(K, dims, type), old_centers(K, dims, type), temp(1, dims, type); std::vector<int> counters(K); std::vector<Vec2f> _box(dims); Vec2f* box = &_box[0]; double best_compactness = DBL_MAX, compactness = 0; RNG& rng = theRNG(); int a, iter, i, j, k; if( criteria.type & TermCriteria::EPS ) criteria.epsilon = std::max(criteria.epsilon, 0.); else criteria.epsilon = FLT_EPSILON; criteria.epsilon *= criteria.epsilon; if( criteria.type & TermCriteria::COUNT ) criteria.maxCount = std::min(std::max(criteria.maxCount, 2), 100); else criteria.maxCount = 100; if( K == 1 ) { attempts = 1; criteria.maxCount = 2; } const float* sample = data.ptr<float>(0); for( j = 0; j < dims; j++ ) box[j] = Vec2f(sample[j], sample[j]); for( i = 1; i < N; i++ ) { sample = data.ptr<float>(i); for( j = 0; j < dims; j++ ) { float v = sample[j]; box[j][0] = std::min(box[j][0], v); box[j][1] = std::max(box[j][1], v); } } for( a = 0; a < attempts; a++ ) { double max_center_shift = DBL_MAX; for( iter = 0;; ) { swap(centers, old_centers); if( iter == 0 && (a > 0 || !(flags & KMEANS_USE_INITIAL_LABELS)) ) { if( flags & KMEANS_PP_CENTERS ) generateCentersPP(data, centers, K, rng, SPP_TRIALS); else { for( k = 0; k < K; k++ ) generateRandomCenter(_box, centers.ptr<float>(k), rng); } } else { if( iter == 0 && a == 0 && (flags & KMEANS_USE_INITIAL_LABELS) ) { for( i = 0; i < N; i++ ) CV_Assert( (unsigned)labels[i] < (unsigned)K ); } // compute centers centers = Scalar(0); for( k = 0; k < K; k++ ) counters[k] = 0; for( i = 0; i < N; i++ ) { sample = data.ptr<float>(i); k = labels[i]; float* center = centers.ptr<float>(k); j=0; #if CV_ENABLE_UNROLLED for(; j <= dims - 4; j += 4 ) { float t0 = center[j] + sample[j]; float t1 = center[j+1] + sample[j+1]; center[j] = t0; center[j+1] = t1; t0 = center[j+2] + sample[j+2]; t1 = center[j+3] + sample[j+3]; center[j+2] = t0; center[j+3] = t1; } #endif for( ; j < dims; j++ ) center[j] += sample[j]; counters[k]++; } if( iter > 0 ) max_center_shift = 0; for( k = 0; k < K; k++ ) { if( counters[k] != 0 ) continue; // if some cluster appeared to be empty then: // 1. find the biggest cluster // 2. find the farthest from the center point in the biggest cluster // 3. exclude the farthest point from the biggest cluster and form a new 1-point cluster. int max_k = 0; for( int k1 = 1; k1 < K; k1++ ) { if( counters[max_k] < counters[k1] ) max_k = k1; } double max_dist = 0; int farthest_i = -1; float* new_center = centers.ptr<float>(k); float* old_center = centers.ptr<float>(max_k); float* _old_center = temp.ptr<float>(); // normalized float scale = 1.f/counters[max_k]; for( j = 0; j < dims; j++ ) _old_center[j] = old_center[j]*scale; for( i = 0; i < N; i++ ) { if( labels[i] != max_k ) continue; sample = data.ptr<float>(i); double dist = normL2Sqr(sample, _old_center, dims); if( max_dist <= dist ) { max_dist = dist; farthest_i = i; } } counters[max_k]--; counters[k]++; labels[farthest_i] = k; sample = data.ptr<float>(farthest_i); for( j = 0; j < dims; j++ ) { old_center[j] -= sample[j]; new_center[j] += sample[j]; } } for( k = 0; k < K; k++ ) { float* center = centers.ptr<float>(k); CV_Assert( counters[k] != 0 ); float scale = 1.f/counters[k]; for( j = 0; j < dims; j++ ) center[j] *= scale; if( iter > 0 ) { double dist = 0; const float* old_center = old_centers.ptr<float>(k); for( j = 0; j < dims; j++ ) { double t = center[j] - old_center[j]; dist += t*t; } max_center_shift = std::max(max_center_shift, dist); } } } if( ++iter == MAX(criteria.maxCount, 2) || max_center_shift <= criteria.epsilon ) break; // assign labels Mat dists(1, N, CV_64F); double* dist = dists.ptr<double>(0); parallel_for_(Range(0, N), KMeansDistanceComputer(dist, labels, data, centers)); compactness = 0; for( i = 0; i < N; i++ ) { compactness += dist[i]; } } if( compactness < best_compactness ) { best_compactness = compactness; if( _centers.needed() ) centers.copyTo(_centers); _labels.copyTo(best_labels); } } return best_compactness; }
double cv::findTransformECC(InputArray templateImage, InputArray inputImage, InputOutputArray warpMatrix, int motionType, TermCriteria criteria) { Mat src = templateImage.getMat();//template iamge Mat dst = inputImage.getMat(); //input image (to be warped) Mat map = warpMatrix.getMat(); //warp (transformation) CV_Assert(!src.empty()); CV_Assert(!dst.empty()); if( ! (src.type()==dst.type())) CV_Error( CV_StsUnmatchedFormats, "Both input images must have the same data type" ); //accept only 1-channel images if( src.type() != CV_8UC1 && src.type()!= CV_32FC1) CV_Error( CV_StsUnsupportedFormat, "Images must have 8uC1 or 32fC1 type"); if( map.type() != CV_32FC1) CV_Error( CV_StsUnsupportedFormat, "warpMatrix must be single-channel floating-point matrix"); CV_Assert (map.cols == 3); CV_Assert (map.rows == 2 || map.rows ==3); CV_Assert (motionType == MOTION_AFFINE || motionType == MOTION_HOMOGRAPHY || motionType == MOTION_EUCLIDEAN || motionType == MOTION_TRANSLATION); if (motionType == MOTION_HOMOGRAPHY){ CV_Assert (map.rows ==3); } CV_Assert (criteria.type & TermCriteria::COUNT || criteria.type & TermCriteria::EPS); const int numberOfIterations = (criteria.type & TermCriteria::COUNT) ? criteria.maxCount : 200; const double termination_eps = (criteria.type & TermCriteria::EPS) ? criteria.epsilon : -1; int paramTemp = 6;//default: affine switch (motionType){ case MOTION_TRANSLATION: paramTemp = 2; break; case MOTION_EUCLIDEAN: paramTemp = 3; break; case MOTION_HOMOGRAPHY: paramTemp = 8; break; } const int numberOfParameters = paramTemp; const int ws = src.cols; const int hs = src.rows; const int wd = dst.cols; const int hd = dst.rows; Mat Xcoord = Mat(1, ws, CV_32F); Mat Ycoord = Mat(hs, 1, CV_32F); Mat Xgrid = Mat(hs, ws, CV_32F); Mat Ygrid = Mat(hs, ws, CV_32F); float* XcoPtr = Xcoord.ptr<float>(0); float* YcoPtr = Ycoord.ptr<float>(0); int j; for (j=0; j<ws; j++) XcoPtr[j] = (float) j; for (j=0; j<hs; j++) YcoPtr[j] = (float) j; repeat(Xcoord, hs, 1, Xgrid); repeat(Ycoord, 1, ws, Ygrid); Xcoord.release(); Ycoord.release(); Mat templateZM = Mat(hs, ws, CV_32F);// to store the (smoothed)zero-mean version of template Mat templateFloat = Mat(hs, ws, CV_32F);// to store the (smoothed) template Mat imageFloat = Mat(hd, wd, CV_32F);// to store the (smoothed) input image Mat imageWarped = Mat(hs, ws, CV_32F);// to store the warped zero-mean input image Mat allOnes = Mat::ones(hd, wd, CV_8U); //to use it for mask warping Mat imageMask = Mat(hs, ws, CV_8U); //to store the final mask //gaussian filtering is optional src.convertTo(templateFloat, templateFloat.type()); GaussianBlur(templateFloat, templateFloat, Size(5, 5), 0, 0);//is in-place filtering slower? dst.convertTo(imageFloat, imageFloat.type()); GaussianBlur(imageFloat, imageFloat, Size(5, 5), 0, 0); // needed matrices for gradients and warped gradients Mat gradientX = Mat::zeros(hd, wd, CV_32FC1); Mat gradientY = Mat::zeros(hd, wd, CV_32FC1); Mat gradientXWarped = Mat(hs, ws, CV_32FC1); Mat gradientYWarped = Mat(hs, ws, CV_32FC1); // calculate first order image derivatives Matx13f dx(-0.5f, 0.0f, 0.5f); filter2D(imageFloat, gradientX, -1, dx); filter2D(imageFloat, gradientY, -1, dx.t()); // matrices needed for solving linear equation system for maximizing ECC Mat jacobian = Mat(hs, ws*numberOfParameters, CV_32F); Mat hessian = Mat(numberOfParameters, numberOfParameters, CV_32F); Mat hessianInv = Mat(numberOfParameters, numberOfParameters, CV_32F); Mat imageProjection = Mat(numberOfParameters, 1, CV_32F); Mat templateProjection = Mat(numberOfParameters, 1, CV_32F); Mat imageProjectionHessian = Mat(numberOfParameters, 1, CV_32F); Mat errorProjection = Mat(numberOfParameters, 1, CV_32F); Mat deltaP = Mat(numberOfParameters, 1, CV_32F);//transformation parameter correction Mat error = Mat(hs, ws, CV_32F);//error as 2D matrix const int imageFlags = CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS+CV_WARP_INVERSE_MAP; const int maskFlags = CV_INTER_NN+CV_WARP_FILL_OUTLIERS+CV_WARP_INVERSE_MAP; // iteratively update map_matrix double rho = -1; double last_rho = - termination_eps; for (int i = 1; (i <= numberOfIterations) && (fabs(rho-last_rho)>= termination_eps); i++) { // warp-back portion of the inputImage and gradients to the coordinate space of the templateImage if (motionType != MOTION_HOMOGRAPHY) { warpAffine(imageFloat, imageWarped, map, imageWarped.size(), imageFlags); warpAffine(gradientX, gradientXWarped, map, gradientXWarped.size(), imageFlags); warpAffine(gradientY, gradientYWarped, map, gradientYWarped.size(), imageFlags); warpAffine(allOnes, imageMask, map, imageMask.size(), maskFlags); } else { warpPerspective(imageFloat, imageWarped, map, imageWarped.size(), imageFlags); warpPerspective(gradientX, gradientXWarped, map, gradientXWarped.size(), imageFlags); warpPerspective(gradientY, gradientYWarped, map, gradientYWarped.size(), imageFlags); warpPerspective(allOnes, imageMask, map, imageMask.size(), maskFlags); } Scalar imgMean, imgStd, tmpMean, tmpStd; meanStdDev(imageWarped, imgMean, imgStd, imageMask); meanStdDev(templateFloat, tmpMean, tmpStd, imageMask); subtract(imageWarped, imgMean, imageWarped, imageMask);//zero-mean input subtract(templateFloat, tmpMean, templateZM, imageMask);//zero-mean template const double tmpNorm = std::sqrt(countNonZero(imageMask)*(tmpStd.val[0])*(tmpStd.val[0])); const double imgNorm = std::sqrt(countNonZero(imageMask)*(imgStd.val[0])*(imgStd.val[0])); // calculate jacobian of image wrt parameters switch (motionType){ case MOTION_AFFINE: image_jacobian_affine_ECC(gradientXWarped, gradientYWarped, Xgrid, Ygrid, jacobian); break; case MOTION_HOMOGRAPHY: image_jacobian_homo_ECC(gradientXWarped, gradientYWarped, Xgrid, Ygrid, map, jacobian); break; case MOTION_TRANSLATION: image_jacobian_translation_ECC(gradientXWarped, gradientYWarped, jacobian); break; case MOTION_EUCLIDEAN: image_jacobian_euclidean_ECC(gradientXWarped, gradientYWarped, Xgrid, Ygrid, map, jacobian); break; } // calculate Hessian and its inverse project_onto_jacobian_ECC(jacobian, jacobian, hessian); hessianInv = hessian.inv(); const double correlation = templateZM.dot(imageWarped); // calculate enhanced correlation coefficiont (ECC)->rho last_rho = rho; rho = correlation/(imgNorm*tmpNorm); // project images into jacobian project_onto_jacobian_ECC( jacobian, imageWarped, imageProjection); project_onto_jacobian_ECC(jacobian, templateZM, templateProjection); // calculate the parameter lambda to account for illumination variation imageProjectionHessian = hessianInv*imageProjection; const double lambda_n = (imgNorm*imgNorm) - imageProjection.dot(imageProjectionHessian); const double lambda_d = correlation - templateProjection.dot(imageProjectionHessian); if (lambda_d <= 0.0) { rho = -1; CV_Error(CV_StsNoConv, "The algorithm stopped before its convergence. The correlation is going to be minimized. Images may be uncorrelated or non-overlapped"); } const double lambda = (lambda_n/lambda_d); // estimate the update step delta_p error = lambda*templateZM - imageWarped; project_onto_jacobian_ECC(jacobian, error, errorProjection); deltaP = hessianInv * errorProjection; // update warping matrix update_warping_matrix_ECC( map, deltaP, motionType); } // return final correlation coefficient return rho; }
int recoverPose( InputArray E, InputArray _points1, InputArray _points2, InputArray _cameraMatrix, OutputArray _R, OutputArray _t, InputOutputArray _mask) { Mat points1, points2, cameraMatrix; _points1.getMat().convertTo(points1, CV_64F); _points2.getMat().convertTo(points2, CV_64F); _cameraMatrix.getMat().convertTo(cameraMatrix, CV_64F); int npoints = points1.checkVector(2); CV_Assert( npoints >= 0 && points2.checkVector(2) == npoints && points1.type() == points2.type()); CV_Assert(cameraMatrix.rows == 3 && cameraMatrix.cols == 3 && cameraMatrix.channels() == 1); if (points1.channels() > 1) { points1 = points1.reshape(1, npoints); points2 = points2.reshape(1, npoints); } double fx = cameraMatrix.at<double>(0,0); double fy = cameraMatrix.at<double>(1,1); double cx = cameraMatrix.at<double>(0,2); double cy = cameraMatrix.at<double>(1,2); points1.col(0) = (points1.col(0) - cx) / fx; points2.col(0) = (points2.col(0) - cx) / fx; points1.col(1) = (points1.col(1) - cy) / fy; points2.col(1) = (points2.col(1) - cy) / fy; points1 = points1.t(); points2 = points2.t(); Mat R1, R2, t; decomposeEssentialMat(E, R1, R2, t); Mat P0 = Mat::eye(3, 4, R1.type()); Mat P1(3, 4, R1.type()), P2(3, 4, R1.type()), P3(3, 4, R1.type()), P4(3, 4, R1.type()); P1(Range::all(), Range(0, 3)) = R1 * 1.0; P1.col(3) = t * 1.0; P2(Range::all(), Range(0, 3)) = R2 * 1.0; P2.col(3) = t * 1.0; P3(Range::all(), Range(0, 3)) = R1 * 1.0; P3.col(3) = -t * 1.0; P4(Range::all(), Range(0, 3)) = R2 * 1.0; P4.col(3) = -t * 1.0; // Do the cheirality check. // Notice here a threshold dist is used to filter // out far away points (i.e. infinite points) since // there depth may vary between postive and negtive. double dist = 50.0; Mat Q; triangulatePoints(P0, P1, points1, points2, Q); Mat mask1 = Q.row(2).mul(Q.row(3)) > 0; Q.row(0) /= Q.row(3); Q.row(1) /= Q.row(3); Q.row(2) /= Q.row(3); Q.row(3) /= Q.row(3); mask1 = (Q.row(2) < dist) & mask1; Q = P1 * Q; mask1 = (Q.row(2) > 0) & mask1; mask1 = (Q.row(2) < dist) & mask1; triangulatePoints(P0, P2, points1, points2, Q); Mat mask2 = Q.row(2).mul(Q.row(3)) > 0; Q.row(0) /= Q.row(3); Q.row(1) /= Q.row(3); Q.row(2) /= Q.row(3); Q.row(3) /= Q.row(3); mask2 = (Q.row(2) < dist) & mask2; Q = P2 * Q; mask2 = (Q.row(2) > 0) & mask2; mask2 = (Q.row(2) < dist) & mask2; triangulatePoints(P0, P3, points1, points2, Q); Mat mask3 = Q.row(2).mul(Q.row(3)) > 0; Q.row(0) /= Q.row(3); Q.row(1) /= Q.row(3); Q.row(2) /= Q.row(3); Q.row(3) /= Q.row(3); mask3 = (Q.row(2) < dist) & mask3; Q = P3 * Q; mask3 = (Q.row(2) > 0) & mask3; mask3 = (Q.row(2) < dist) & mask3; triangulatePoints(P0, P4, points1, points2, Q); Mat mask4 = Q.row(2).mul(Q.row(3)) > 0; Q.row(0) /= Q.row(3); Q.row(1) /= Q.row(3); Q.row(2) /= Q.row(3); Q.row(3) /= Q.row(3); mask4 = (Q.row(2) < dist) & mask4; Q = P4 * Q; mask4 = (Q.row(2) > 0) & mask4; mask4 = (Q.row(2) < dist) & mask4; mask1 = mask1.t(); mask2 = mask2.t(); mask3 = mask3.t(); mask4 = mask4.t(); // If _mask is given, then use it to filter outliers. if (!_mask.empty()) { Mat mask = _mask.getMat(); CV_Assert(mask.size() == mask1.size()); bitwise_and(mask, mask1, mask1); bitwise_and(mask, mask2, mask2); bitwise_and(mask, mask3, mask3); bitwise_and(mask, mask4, mask4); } if (_mask.empty() && _mask.needed()) { _mask.create(mask1.size(), CV_8U); } CV_Assert(_R.needed() && _t.needed()); _R.create(3, 3, R1.type()); _t.create(3, 1, t.type()); int good1 = countNonZero(mask1); int good2 = countNonZero(mask2); int good3 = countNonZero(mask3); int good4 = countNonZero(mask4); if (good1 >= good2 && good1 >= good3 && good1 >= good4) { R1.copyTo(_R); t.copyTo(_t); if (_mask.needed()) mask1.copyTo(_mask); return good1; } else if (good2 >= good1 && good2 >= good3 && good2 >= good4) { R2.copyTo(_R); t.copyTo(_t); if (_mask.needed()) mask2.copyTo(_mask); return good2; } else if (good3 >= good1 && good3 >= good2 && good3 >= good4) { t = -t; R1.copyTo(_R); t.copyTo(_t); if (_mask.needed()) mask3.copyTo(_mask); return good3; } else { t = -t; R2.copyTo(_R); t.copyTo(_t); if (_mask.needed()) mask4.copyTo(_mask); return good4; } }
void cv::cornerSubPix( InputArray _image, InputOutputArray _corners, Size win, Size zeroZone, TermCriteria criteria ) { CV_INSTRUMENT_REGION() const int MAX_ITERS = 100; int win_w = win.width * 2 + 1, win_h = win.height * 2 + 1; int i, j, k; int max_iters = (criteria.type & CV_TERMCRIT_ITER) ? MIN(MAX(criteria.maxCount, 1), MAX_ITERS) : MAX_ITERS; double eps = (criteria.type & CV_TERMCRIT_EPS) ? MAX(criteria.epsilon, 0.) : 0; eps *= eps; // use square of error in comparsion operations cv::Mat src = _image.getMat(), cornersmat = _corners.getMat(); int count = cornersmat.checkVector(2, CV_32F); CV_Assert( count >= 0 ); Point2f* corners = cornersmat.ptr<Point2f>(); if( count == 0 ) return; CV_Assert( win.width > 0 && win.height > 0 ); CV_Assert( src.cols >= win.width*2 + 5 && src.rows >= win.height*2 + 5 ); CV_Assert( src.channels() == 1 ); Mat maskm(win_h, win_w, CV_32F), subpix_buf(win_h+2, win_w+2, CV_32F); float* mask = maskm.ptr<float>(); for( i = 0; i < win_h; i++ ) { float y = (float)(i - win.height)/win.height; float vy = std::exp(-y*y); for( j = 0; j < win_w; j++ ) { float x = (float)(j - win.width)/win.width; mask[i * win_w + j] = (float)(vy*std::exp(-x*x)); } } // make zero_zone if( zeroZone.width >= 0 && zeroZone.height >= 0 && zeroZone.width * 2 + 1 < win_w && zeroZone.height * 2 + 1 < win_h ) { for( i = win.height - zeroZone.height; i <= win.height + zeroZone.height; i++ ) { for( j = win.width - zeroZone.width; j <= win.width + zeroZone.width; j++ ) { mask[i * win_w + j] = 0; } } } // do optimization loop for all the points for( int pt_i = 0; pt_i < count; pt_i++ ) { Point2f cT = corners[pt_i], cI = cT; int iter = 0; double err = 0; do { Point2f cI2; double a = 0, b = 0, c = 0, bb1 = 0, bb2 = 0; getRectSubPix(src, Size(win_w+2, win_h+2), cI, subpix_buf, subpix_buf.type()); const float* subpix = &subpix_buf.at<float>(1,1); // process gradient for( i = 0, k = 0; i < win_h; i++, subpix += win_w + 2 ) { double py = i - win.height; for( j = 0; j < win_w; j++, k++ ) { double m = mask[k]; double tgx = subpix[j+1] - subpix[j-1]; double tgy = subpix[j+win_w+2] - subpix[j-win_w-2]; double gxx = tgx * tgx * m; double gxy = tgx * tgy * m; double gyy = tgy * tgy * m; double px = j - win.width; a += gxx; b += gxy; c += gyy; bb1 += gxx * px + gxy * py; bb2 += gxy * px + gyy * py; } } double det=a*c-b*b; if( fabs( det ) <= DBL_EPSILON*DBL_EPSILON ) break; // 2x2 matrix inversion double scale=1.0/det; cI2.x = (float)(cI.x + c*scale*bb1 - b*scale*bb2); cI2.y = (float)(cI.y - b*scale*bb1 + a*scale*bb2); err = (cI2.x - cI.x) * (cI2.x - cI.x) + (cI2.y - cI.y) * (cI2.y - cI.y); cI = cI2; if( cI.x < 0 || cI.x >= src.cols || cI.y < 0 || cI.y >= src.rows ) break; } while( ++iter < max_iters && err > eps ); // if new point is too far from initial, it means poor convergence. // leave initial point as the result if( fabs( cI.x - cT.x ) > win.width || fabs( cI.y - cT.y ) > win.height ) cI = cT; corners[pt_i] = cI; } }
void cv::updateMotionHistory( InputArray _silhouette, InputOutputArray _mhi, double timestamp, double duration ) { CV_Assert( _silhouette.type() == CV_8UC1 && _mhi.type() == CV_32FC1 ); CV_Assert( _silhouette.sameSize(_mhi) ); float ts = (float)timestamp; float delbound = (float)(timestamp - duration); CV_OCL_RUN(_mhi.isUMat() && _mhi.dims() <= 2, ocl_updateMotionHistory(_silhouette, _mhi, ts, delbound)) Mat silh = _silhouette.getMat(), mhi = _mhi.getMat(); Size size = silh.size(); if( silh.isContinuous() && mhi.isContinuous() ) { size.width *= size.height; size.height = 1; } #if CV_SSE2 volatile bool useSIMD = cv::checkHardwareSupport(CV_CPU_SSE2); #endif for(int y = 0; y < size.height; y++ ) { const uchar* silhData = silh.ptr<uchar>(y); float* mhiData = mhi.ptr<float>(y); int x = 0; #if CV_SSE2 if( useSIMD ) { __m128 ts4 = _mm_set1_ps(ts), db4 = _mm_set1_ps(delbound); for( ; x <= size.width - 8; x += 8 ) { __m128i z = _mm_setzero_si128(); __m128i s = _mm_unpacklo_epi8(_mm_loadl_epi64((const __m128i*)(silhData + x)), z); __m128 s0 = _mm_cvtepi32_ps(_mm_unpacklo_epi16(s, z)), s1 = _mm_cvtepi32_ps(_mm_unpackhi_epi16(s, z)); __m128 v0 = _mm_loadu_ps(mhiData + x), v1 = _mm_loadu_ps(mhiData + x + 4); __m128 fz = _mm_setzero_ps(); v0 = _mm_and_ps(v0, _mm_cmpge_ps(v0, db4)); v1 = _mm_and_ps(v1, _mm_cmpge_ps(v1, db4)); __m128 m0 = _mm_and_ps(_mm_xor_ps(v0, ts4), _mm_cmpneq_ps(s0, fz)); __m128 m1 = _mm_and_ps(_mm_xor_ps(v1, ts4), _mm_cmpneq_ps(s1, fz)); v0 = _mm_xor_ps(v0, m0); v1 = _mm_xor_ps(v1, m1); _mm_storeu_ps(mhiData + x, v0); _mm_storeu_ps(mhiData + x + 4, v1); } } #endif for( ; x < size.width; x++ ) { float val = mhiData[x]; val = silhData[x] ? ts : val < delbound ? 0 : val; mhiData[x] = val; } } }
bool cv::find4QuadCornerSubpix(InputArray _img, InputOutputArray _corners, Size region_size) { CV_INSTRUMENT_REGION(); Mat img = _img.getMat(), cornersM = _corners.getMat(); int ncorners = cornersM.checkVector(2, CV_32F); CV_Assert( ncorners >= 0 ); Point2f* corners = cornersM.ptr<Point2f>(); const int nbins = 256; float ranges[] = {0, 256}; const float* _ranges = ranges; Mat hist; Mat black_comp, white_comp; for(int i = 0; i < ncorners; i++) { int channels = 0; Rect roi(cvRound(corners[i].x - region_size.width), cvRound(corners[i].y - region_size.height), region_size.width*2 + 1, region_size.height*2 + 1); Mat img_roi = img(roi); calcHist(&img_roi, 1, &channels, Mat(), hist, 1, &nbins, &_ranges); int black_thresh = 0, white_thresh = 0; segment_hist_max(hist, black_thresh, white_thresh); threshold(img, black_comp, black_thresh, 255.0, THRESH_BINARY_INV); threshold(img, white_comp, white_thresh, 255.0, THRESH_BINARY); const int erode_count = 1; erode(black_comp, black_comp, Mat(), Point(-1, -1), erode_count); erode(white_comp, white_comp, Mat(), Point(-1, -1), erode_count); std::vector<std::vector<Point> > white_contours, black_contours; findContours(black_comp, black_contours, RETR_LIST, CHAIN_APPROX_SIMPLE); findContours(white_comp, white_contours, RETR_LIST, CHAIN_APPROX_SIMPLE); if(black_contours.size() < 5 || white_contours.size() < 5) continue; // find two white and black blobs that are close to the input point std::vector<std::pair<int, float> > white_order, black_order; orderContours(black_contours, corners[i], black_order); orderContours(white_contours, corners[i], white_order); const float max_dist = 10.0f; if(black_order[0].second > max_dist || black_order[1].second > max_dist || white_order[0].second > max_dist || white_order[1].second > max_dist) { continue; // there will be no improvement in this corner position } const std::vector<Point>* quads[4] = {&black_contours[black_order[0].first], &black_contours[black_order[1].first], &white_contours[white_order[0].first], &white_contours[white_order[1].first]}; std::vector<Point2f> quads_approx[4]; Point2f quad_corners[4]; for(int k = 0; k < 4; k++) { std::vector<Point2f> temp; for(size_t j = 0; j < quads[k]->size(); j++) temp.push_back((*quads[k])[j]); approxPolyDP(Mat(temp), quads_approx[k], 0.5, true); findCorner(quads_approx[k], corners[i], quad_corners[k]); quad_corners[k] += Point2f(0.5f, 0.5f); } // cross two lines Point2f origin1 = quad_corners[0]; Point2f dir1 = quad_corners[1] - quad_corners[0]; Point2f origin2 = quad_corners[2]; Point2f dir2 = quad_corners[3] - quad_corners[2]; double angle = acos(dir1.dot(dir2)/(norm(dir1)*norm(dir2))); if(cvIsNaN(angle) || cvIsInf(angle) || angle < 0.5 || angle > CV_PI - 0.5) continue; findLinesCrossPoint(origin1, dir1, origin2, dir2, corners[i]); } return true; }
// Reconstruction function for API void reconstruct(InputArrayOfArrays points2d, OutputArray Ps, OutputArray points3d, InputOutputArray K, bool is_projective) { const int nviews = points2d.total(); CV_Assert( nviews >= 2 ); // OpenCV data types std::vector<Mat> pts2d; points2d.getMatVector(pts2d); const int depth = pts2d[0].depth(); Matx33d Ka = K.getMat(); // Projective reconstruction if (is_projective) { if ( nviews == 2 ) { // Get Projection matrices Matx33d F; Matx34d P, Pp; normalizedEightPointSolver(pts2d[0], pts2d[1], F); projectionsFromFundamental(F, P, Pp); Ps.create(2, 1, depth); Mat(P).copyTo(Ps.getMatRef(0)); Mat(Pp).copyTo(Ps.getMatRef(1)); // Triangulate and find 3D points using inliers triangulatePoints(points2d, Ps, points3d); } else { std::vector<Mat> Rs, Ts; reconstruct(points2d, Rs, Ts, Ka, points3d, is_projective); // From Rs and Ts, extract Ps const int nviews = Rs.size(); Ps.create(nviews, 1, depth); Matx34d P; for (size_t i = 0; i < nviews; ++i) { projectionFromKRt(Ka, Rs[i], Vec3d(Ts[i]), P); Mat(P).copyTo(Ps.getMatRef(i)); } Mat(Ka).copyTo(K.getMat()); } } // Affine reconstruction else { // TODO: implement me } }