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