void opticalFlow::occMatpEst( Mat_<Vec2f> &flow12, Mat_<Vec2f> &flow21, Mat_<uchar>&occMap) { int iy, ix; const float FLOW_PIXEL_THRESH = 2; occMap.setTo(255); for (iy=0; iy<height1; ++iy) { for (ix=0; ix<width1; ++ix) { Vec2f fFlow = flow12[iy][ix]; int ny, nx; ny = floor(iy+fFlow[1]+0.5); nx = floor(ix+fFlow[0]+0.5); if (ny>=0 && ny<height1 && nx>=0 && nx<width1) { cv::Vec2f bFlow = flow21[ny][nx]; if (fabs(bFlow[1]+ny-iy)<FLOW_PIXEL_THRESH && fabs(bFlow[0]+nx-ix)<FLOW_PIXEL_THRESH) { continue; } } occMap[iy][ix] = 0; } } Mat bw = occMap; Mat labelImage(occMap.size(), CV_32S); int nLabels = connectedComponents(bw, labelImage, 8); occMap[iy][ix] = 0; vector<int> hist(nLabels,0); for (iy=0; iy<height1; ++iy) for (ix=0; ix<width1; ++ix) hist[labelImage.at<int>(iy,ix)]++; vector<int> rmv_list; rmv_list.reserve(20); for (int i=0;i<nLabels;++i){ if (hist[i]<50) rmv_list.push_back(i); } for (iy=0; iy<height1; ++iy) { for (ix=0; ix<width1; ++ix) { for (int r=0; r<rmv_list.size(); ++r) if(labelImage.at<int>(iy,ix) == rmv_list[r]) occMap[iy][ix] = 0; } } }
/* Assigns Ix, Iy, It gradient values computed from imgA and imgB. */ void compute_derivatives(const Mat_<float>& imgA, const Mat_<float>& imgB, Mat_<float>& Ix, Mat_<float>& Iy, Mat_<float>& It) { int channels = 1; //assume grayscale images int nRows = imgA.rows; //assume same size of imgA and imgB int nCols = imgA.cols * channels; Ix = Mat::zeros(imgA.size(), CV_32F); Iy = Mat::zeros(imgA.size(), CV_32F); It = Mat::zeros(imgA.size(), CV_32F); float *p_a1, *p_a2, *p_b1, *p_b2; float *p_ix, *p_iy, *p_it; p_a1 = (float*)imgA.ptr<float>(0); //cast is needed (const uchar* to uchar*) p_b1 = (float*)imgB.ptr<float>(0); p_a2 = p_a1 + nCols; p_b2 = p_b1 + nCols; p_ix = Ix.ptr<float>(0); p_iy = Iy.ptr<float>(0); p_it = It.ptr<float>(0); for ( int j = 0; j < nCols*(nRows-1); ++j) { //calculate spatiotemporal averages like Horn & Schunck //up at right-hand side x-boundary) p_ix[j] = 0.25 * (p_a1[j+1] - p_a1[j]+p_a2[j+1]-p_a2[j] +p_b1[j+1]-p_b1[j]+p_b2[j+1]-p_b2[j]); p_iy[j] = 0.25 * (p_a2[j]-p_a1[j]+p_a2[j+1]-p_a1[j+1] +p_b2[j]-p_b1[j]+p_b2[j+1]-p_b1[j+1]); p_it[j] = 0.25 * (p_b1[j] - p_a1[j]+p_b1[j+1]-p_a1[j+1] +p_b2[j]-p_a2[j]+p_b2[j+1]-p_a2[j+1]); } }
LaplacianBlending::LaplacianBlending(const Mat_<Vec3f>& _left, const Mat_<Vec3f>& _right, int _levels)://construct function, used in LaplacianBlending lb(l,r,m,4); left(_left),right(_right),levels(_levels) { assert(_left.size() == _right.size()); buildPyramids(); //construct Laplacian Pyramid and Gaussian Pyramid blendLapPyrs(); //blend left & right Pyramids into one Pyramid };
LaplacianBlending::LaplacianBlending(const Mat_<Vec3f>& _left, const Mat_<Vec3f>& _right, const Mat_<float>& _leftMask, const Mat_<float>& _rightMask, int _levels): left(_left),right(_right),rightMask(_rightMask),leftMask(_leftMask),levels(_levels) { assert(_left.size() == _right.size()); assert(_left.size() == _leftMask.size()); assert(_rightMask.size() == _leftMask.size()); buildPyramids(); blendLapPyrs(); };
SLAM::LineFeature::LineFeature(const Mat_<imtype> & im, const CameraState & state, const Point2f & pt2d, int rx, int ry) :descriptor(im(Range(max(0, iround(pt2d.y-ry)), min(im.size().height, iround(pt2d.y+ry+1))), Range(max(0, iround(pt2d.x-rx)), min(im.size().width, iround(pt2d.x+rx+1)))).clone()), cone(state.getLocalCoordinatesPoint(pt2d), state.t, 3.f, state.f, 5, 100, 20, 3) /*TODO: cone parameters*/, timeSinceLastSeen(1) { }
void regularize(Mat_<uchar> &gray, BBox &bbox, Pose &P, Mat_<double> &shape, Mat_<uchar> &lEye, Mat_<uchar> &rEye) { double w = bbox.w; double h = bbox.h; double scale = 1.5; double minx = max(0.0, bbox.x - (scale - 1)*w / 2); double miny = max(0.0, bbox.y - (scale - 1)*h / 2); double maxx = min(gray.cols - 1.0, minx + scale*w); double maxy = min(gray.rows - 1.0, miny + scale*h); w = maxx - minx; h = maxy - miny; BBox oldbox(minx, miny, w, h); BBox newbox(0, 0, w, h); Mat_<uchar> faceImg = gray(Rect(minx, miny, w, h)); Mat_<uchar> regFace = Mat_<uchar>::zeros(faceImg.size()); Mat rotMat = getRotationMatrix2D(Point2f(w / 2.0, h / 2.0), P.roll, 1.0); warpAffine(faceImg, regFace, rotMat, faceImg.size()); double cosa = rotMat.at<double>(0, 0); double sina = rotMat.at<double>(0, 1); Mat_<double> projShape = projectShape(shape, oldbox); Mat_<double> newShape = Mat_<double>::zeros(shape.size()); for (int i = 0; i < shape.rows; i++) { newShape(i, 0) = cosa*projShape(i, 0) + sina*projShape(i, 1); newShape(i, 1) = -sina*projShape(i, 0) + cosa*projShape(i, 1); } newShape = reprojectShape(newShape, newbox); // get eye images & data Rect lRect = findEyeRect(regFace, newShape, LEFT_EYE); Rect rRect = findEyeRect(regFace, newShape, RIGHT_EYE); vector<Point> lCont = findEyeCont(lRect, newShape, LEFT_EYE); vector<Point> rCont = findEyeCont(rRect, newShape, RIGHT_EYE); lEye = regFace(lRect); rEye = regFace(rRect); // set mask and crop Mat_<uchar> lMask = findEyeMask(lEye, lCont); Mat_<uchar> rMask = findEyeMask(rEye, rCont); //lEye.setTo(0, ~lMask); //rEye.setTo(0, ~rMask); normalize(lEye, lEye, 255, 0, NORM_MINMAX); normalize(rEye, rEye, 255, 0, NORM_MINMAX); // resize to proper size resize(lEye, lEye, Size(64, 32)); resize(rEye, rEye, Size(64, 32)); }
Mat_< float > Saliency::assignFilter( const Mat_< Vec3b >& im, const Mat_< int >& seg, const vector< SuperpixelStatistic >& stat, const std::vector< float >& sal ) const { std::vector< float > source_features( seg.size().area()*5 ), target_features( im.size().area()*5 ); Mat_< Vec2f > data( seg.size() ); // There is a type on the paper: alpha and beta are actually squared, or directly applied to the values const float a = settings_.alpha_, b = settings_.beta_; const int D = 5; // Create the source features for( int j=0,k=0; j<seg.rows; j++ ) for( int i=0; i<seg.cols; i++, k++ ) { int id = seg(j,i); data(j,i) = Vec2f( sal[id], 1 ); source_features[D*k+0] = a * i; source_features[D*k+1] = a * j; if (D == 5) { source_features[D*k+2] = b * stat[id].mean_rgb_[0]; source_features[D*k+3] = b * stat[id].mean_rgb_[1]; source_features[D*k+4] = b * stat[id].mean_rgb_[2]; } } // Create the source features for( int j=0,k=0; j<im.rows; j++ ) for( int i=0; i<im.cols; i++, k++ ) { target_features[D*k+0] = a * i; target_features[D*k+1] = a * j; if (D == 5) { target_features[D*k+2] = b * im(j,i)[0]; target_features[D*k+3] = b * im(j,i)[1]; target_features[D*k+4] = b * im(j,i)[2]; } } // Do the filtering [Filtering using the target features twice works slightly better, as the method described in our paper] if (settings_.use_spix_color_) { Filter filter( source_features.data(), seg.cols*seg.rows, target_features.data(), im.cols*im.rows, D ); filter.filter( data.ptr<float>(), data.ptr<float>(), 2 ); } else { Filter filter( target_features.data(), im.cols*im.rows, D ); filter.filter( data.ptr<float>(), data.ptr<float>(), 2 ); } Mat_<float> r( im.size() ); for( int j=0; j<im.rows; j++ ) for( int i=0; i<im.cols; i++ ) r(j,i) = data(j,i)[0] / (data(j,i)[1] + 1e-10); return r; }
Mat_<uchar> transformModifiedLUX(Mat_<Vec3b> image_BGR) { Mat_<uchar> Ucap(image_BGR.size()); int B = 0, G = 0, R = 0, u_cap_int = 0; double u_cap = 0.0; for(int i = 0; i < image_BGR.rows; ++i) { for(int j = 0; j < image_BGR.cols; ++j) { B = image_BGR(i, j)[0]; G = image_BGR(i, j)[1]; R = image_BGR(i, j)[2]; if(R > G) { u_cap = (256*G) / R; u_cap_int = round(u_cap); Ucap.at<uchar>(i, j) = u_cap_int; } else Ucap.at<uchar>(i, j) = 255; } } return Ucap; }
Mat_<uchar> transformLUX(Mat_<Vec3b> image_BGR) { Mat_<uchar> U(image_BGR.size()); int B = 0, G = 0, R = 0, L_int = 0, u_int = 0; double L = 0.0, u = 0.0; for(int i = 0; i < image_BGR.rows; ++i) { for(int j = 0; j < image_BGR.cols; ++j) { B = image_BGR(i, j)[0]; G = image_BGR(i, j)[1]; R = image_BGR(i, j)[2]; L = (pow(R+1, 0.3) * pow(G+1, 0.6) * pow(B+1, 0.1)) - 1; L_int = round(L); if(R > L_int) { u = (256 * (L_int+1)) / (R + 1); u_int = round(u); U.at<uchar>(i, j) = u_int; } else U.at<uchar>(i, j) = 255; } } return U; }
void DetectionTool::getMove(const Mat_<Point2f>& flow, Mat& dst, float minmotion, int color) { dst.create(flow.size(), CV_8UC1); dst.setTo(Scalar::all(0)); //Permet de ne pas calculer la racine carrée pour chaque longueur de vecteur de mouvement. //Si minmotion <= 0, on veut sélectionner tous les mouvements. if(minmotion > 0) minmotion *= minmotion; //Parcours de la matrice des mouvements for (int y = 0; y < flow.rows; ++y) { for (int x = 0; x < flow.cols; ++x) { Point2f u = flow(y, x); if (isFlowCorrect(u)) { //la comparaison se fait sans la mise sous racine car on a élevé au carré minmotion if(u.x * u.x + u.y * u.y > minmotion) { dst.at<char>(y, x) = color; } } } } }
void add_feature(const Mat &image, const char *name) { assert (prediction.size() == image.size()); int found = 0; string _name = string(name); cout << "Got feature " << name << endl; for (int wi = 0; wi < weak_learners.size(); wi++) { if (weak_learners[wi].feature_name == _name) { found = 1; float *score_ptr = prediction.ptr<float>(0); const float *feature_ptr = image.ptr<float>(0); float thresh = weak_learners[wi].threshold; float left_val = weak_learners[wi].left_val; float right_val = weak_learners[wi].right_val; for (int i = 0; i < prediction.total(); i++, feature_ptr++, score_ptr++) *score_ptr += ((*feature_ptr <= thresh) ? left_val : right_val); } } if (! found) cout << "Didn't find any uses of feature " << name << endl; // remove old weak learners from consideration weak_learners.erase(remove(weak_learners.begin(), weak_learners.end(), name), weak_learners.end()); if (should_save(name)) write_feature(h5f, image, name); }
void iterative_computation(Mat_<float>& u, Mat_<float>& v, const Mat_<float>& Ix, const Mat_<float>& Iy, const Mat_<float>& It) { if (METHOD == 0) { // Jacobi Methd Mat_<float> f = (Mat_<float>(3,3) << 1.0/12.0, 1.0/6.0, 1.0/12.0, 1.0/6.0, 0.0, 1.0/6.0, 1.0/12.0, 1.0/6.0, 1.0/12.0); Mat_<float> avg_u, avg_v; filter2D(u, avg_u, -1 , f, Point(-1, -1), 0, BORDER_DEFAULT ); filter2D(v, avg_v, -1 , f, Point(-1, -1), 0, BORDER_DEFAULT ); Mat_<float> d1 = Ix.mul(avg_u) + Iy.mul(avg_v) + It; Mat_<float> d2 = Mat::ones(u.size(), CV_32F) * ALPHA * ALPHA + Ix.mul(Ix) + Iy.mul(Iy); Mat_<float> r = d1.mul(1 / d2); u = avg_u - Ix.mul(r); v = avg_v - Iy.mul(r); } else if (METHOD == 1) { // Gauss-Seidel method for (int i = 1; i < u.rows - 1; i ++) { for (int j = 1; j < u.cols - 1; j ++) { float avg_u = 1.f / 6.f * (u.at<float>(i - 1, j) + u.at<float>(i, j + 1) + u.at<float>(i + 1, j) + u.at<float>(i, j - 1)) + 1.f / 12.f * (u.at<float>(i - 1, j -1) + u.at<float>(i - 1, j + 1) + u.at<float>(i + 1, j - 1) + u.at<float>(i + 1, j + 1)); float avg_v = 1.f / 6.f * (v.at<float>(i - 1, j) + v.at<float>(i, j + 1) + v.at<float>(i + 1, j) + v.at<float>(i, j - 1)) + 1.f / 12.f * (v.at<float>(i - 1, j -1) + v.at<float>(i - 1, j + 1) + v.at<float>(i + 1, j - 1) + v.at<float>(i + 1, j + 1)); float ix = Ix.at<float>(i, j); float iy = Iy.at<float>(i, j); float it = It.at<float>(i, j); float r = (ix * avg_u + iy * avg_v + it) / (ALPHA * ALPHA + ix * ix + iy * iy); u.at<float>(i, j) = avg_u - ix * r; v.at<float>(i, j) = avg_v - iy * r; } } } else if (METHOD == 2) { // Successive overrelaxation method for (int i = 1; i < u.rows - 1; i ++) { for (int j = 1; j < u.cols - 1; j ++) { float avg_u = 1.f / 6.f * (u.at<float>(i - 1, j) + u.at<float>(i, j + 1) + u.at<float>(i + 1, j) + u.at<float>(i, j - 1)) + 1.f / 12.f * (u.at<float>(i - 1, j -1) + u.at<float>(i - 1, j + 1) + u.at<float>(i + 1, j - 1) + u.at<float>(i + 1, j + 1)); float avg_v = 1.f / 6.f * (v.at<float>(i - 1, j) + v.at<float>(i, j + 1) + v.at<float>(i + 1, j) + v.at<float>(i, j - 1)) + 1.f / 12.f * (v.at<float>(i - 1, j -1) + v.at<float>(i - 1, j + 1) + v.at<float>(i + 1, j - 1) + v.at<float>(i + 1, j + 1)); float ix = Ix.at<float>(i, j); float iy = Iy.at<float>(i, j); float it = It.at<float>(i, j); float r = (ix * avg_u + iy * avg_v + it) / (ALPHA * ALPHA + ix * ix + iy * iy); u.at<float>(i, j) = (1 - WEIGHT) * u.at<float>(i, j) + WEIGHT * (avg_u - ix * r); v.at<float>(i, j) = (1 - WEIGHT) * v.at<float>(i, j) + WEIGHT * (avg_v - iy * r); } } } }
void fft2(Mat_<float> src) { int x = getOptimalDFTSize(2* src.rows ); int y = getOptimalDFTSize(2* src.cols ); copyMakeBorder(src, src, 0, (x - src.rows), 0, (y - src.cols), BORDER_CONSTANT, Scalar::all(0)); // Get padded image size const int wx = src.cols, wy = src.rows; const int cx = wx/2, cy = wy/2; //--------------------------------// // DFT - performing // cv::Mat_<float> imgs[] = {src.clone(), Mat::zeros(src.size(), CV_32F)}; cv::Mat_<cv::Vec2f> img_dft; merge(imgs,2,img_dft); dft(img_dft, img_dft); split(img_dft,imgs); cv::Mat_<float> magnitude, phase; cartToPolar(imgs[0],imgs[1],magnitude,phase); dftshift(magnitude); magnitude = magnitude + 1.0f; log(magnitude,magnitude); normalize(magnitude,magnitude,0,1,CV_MINMAX); namedWindow("img_dft",WINDOW_NORMAL); imshow("img_dft",magnitude); waitKey(0); cout << "out" << endl; }
int copyMakeBorder(/*const*/ Mat_<_Tp, chs>& src, Mat_<_Tp, chs>& dst, int top, int bottom, int left, int right, int borderType, const Scalar& value = Scalar()) { FBC_Assert(top >= 0 && bottom >= 0 && left >= 0 && right >= 0); if (src.isSubmatrix() && (borderType & BORDER_ISOLATED) == 0) { Size wholeSize; Point ofs; src.locateROI(wholeSize, ofs); int dtop = std::min(ofs.y, top); int dbottom = std::min(wholeSize.height - src.rows - ofs.y, bottom); int dleft = std::min(ofs.x, left); int dright = std::min(wholeSize.width - src.cols - ofs.x, right); src.adjustROI(dtop, dbottom, dleft, dright); top -= dtop; left -= dleft; bottom -= dbottom; right -= dright; } if (dst.empty() || dst.rows != (src.rows + top + bottom) || dst.cols != (src.cols + left + right)) { dst.release(); dst = Mat_<_Tp, chs>(src.rows + top + bottom, src.cols + left + right); } if (top == 0 && left == 0 && bottom == 0 && right == 0) { if (src.data != dst.data || src.step != dst.step) src.copyTo(dst); return 0; } borderType &= ~BORDER_ISOLATED; if (borderType != BORDER_CONSTANT) { copyMakeBorder_8u(src.ptr(), src.step, src.size(), dst.ptr(), dst.step, dst.size(), top, left, src.elemSize(), borderType); } else { int cn = src.channels, cn1 = cn; AutoBuffer<double> buf(cn); scalarToRawData<_Tp, chs>(value, buf, cn); copyMakeConstBorder_8u(src.ptr(), src.step, src.size(), dst.ptr(), dst.step, dst.size(), top, left, (int)src.elemSize(), (uchar*)(double*)buf); } return 0; }
Mat_< Vec3f > Superpixel::visualizeRandom( const Mat_< int >& segmentation ) const { srand( segmentation.size().area() ); int n_label = nLabels( segmentation ); std::vector< Vec3f > colors; for( int k=0; k<n_label; k++ ) colors.push_back( Vec3f( 1.0*rand()/RAND_MAX, 1.0*rand()/RAND_MAX, 1.0*rand()/RAND_MAX ) ); return assign( colors, segmentation ); }
Mat_< float > Saliency::saliency( const Mat_< Vec3b >& im ) const { // Convert the image to the lab space Mat_<Vec3f> rgbim, labim; im.convertTo( rgbim, CV_32F, 1.0/255. ); cvtColor( rgbim, labim, CV_BGR2Lab ); // Superpixel superpixel_( 300, 50.0 ); // Do the abstraction Mat_<int> segmentation = superpixel_.segment( labim ); std::vector< SuperpixelStatistic > stat = superpixel_.stat( labim, im, segmentation ); // Compute the uniqueness std::vector<float> unique( stat.size(), 1 ); if (settings_.uniqueness_) { if (settings_.filter_uniqueness_) unique = uniquenessFilter( stat ); else unique = uniqueness( stat ); } // Compute the distribution std::vector<float> dist( stat.size(), 0 ); if (settings_.distribution_) { if (settings_.filter_distribution_) dist = distributionFilter( stat ); else dist = distribution( stat ); } // Combine the two measures std::vector<float> sp_saliency( stat.size() ); for( int i=0; i<stat.size(); i++ ) sp_saliency[i] = unique[i] * exp( - settings_.k_ * dist[i] ); // Upsampling Mat_<float> r; if (settings_.upsample_) r = assignFilter( im, segmentation, stat, sp_saliency ); else r = assign( segmentation, sp_saliency ); // Rescale the saliency to [0..1] double mn, mx; minMaxLoc( r, &mn, & mx ); r = (r - mn) / (mx - mn); // Increase the saliency value until we are below the minimal threshold double m_sal = settings_.min_saliency_ * r.size().area(); for( float sm = sum( r )[0]; sm < m_sal; sm = sum( r )[0] ) r = min( r*m_sal/sm, 1.0f ); return r; }
// Extract the pseudo-hue plane Mat_<uchar> transformPseudoHue(Mat_<Vec3b> image) { Mat_<double> pseudo_hue(image.size()); Mat_<uchar> pseudo_hue_norm(image.size()); int B = 0, G = 0, R = 0; for(int i = 0; i < image.rows; ++i) { for(int j = 0; j < image.cols; ++j) { B = image(i, j)[0]; G = image(i, j)[1]; R = image(i, j)[2]; if(R == 0) pseudo_hue.at<double>(i, j) = 0.0; else pseudo_hue.at<double>(i, j) = (double)R / (R + G); } } std::pair<double, double> statistics = Stats(pseudo_hue); double Hmin = statistics.first; double Hmax = statistics.second; double Hrange = (Hmax - Hmin); for(int i = 0; i < image.rows; ++i) { for(int j = 0; j < image.cols; ++j) { double temp = (pseudo_hue.at<double>(i, j) - Hmin) / Hrange; pseudo_hue_norm.at<uchar>(i, j) = round(temp * 255); } } return pseudo_hue_norm; }
/* Given a sensor grid to draw on and a map co-ordinate locating the hit, draws the patch centered at the proper grid index if ADDITIVE is #defined, the gaussian patch is added to the occupancy grid if ADDITIVE isnt #defined, the occupancy grid is set to the value in the patch unless it already has a higher value */ void drawHit(Mat_<uchar>& grid, Point2f hit) { static int radius = static_cast<int>(fattening / gridRes * (1 << fixedPoints)); static Mat_<uchar> patch = patchInit(); Point center = pointToGridPoint(hit); //roi doesn't support subpixel precision Rect roi_ = Rect(center.x-radius/2.0,center.y-radius/2.0, radius,radius); //rect bounding patch and centered on hit Rect roi = roi_ & gridMatBounds; //intersection of roi and map boundary if(roi.size() != roi_.size()) return; //do not draw patches if they don't entirely fit on the map because it is inconvenient to do so Mat_<uchar> t(grid,roi); if(t.size()!= patch.size()) return; //do not draw patches if they don't entirely fit on the map because it is inconvenient to do so //ROS_INFO("drawing a hit"); //don't print this out because it is slow if(ADDITIVE) t+=patch; else t= max(t, patch); }
/* * Function: scan * ---------------------------- * Scannt einen UPC Code, gibt die Zahl zurueck * * img: Bild mit dem Barcode * * returns: Barcode als Zahl */ deque<int> BarcodeScannerEAN::scan(Mat_<uchar> img) { Point currPos(0, img.size().height / 2); int unitWidth = scanFirstGuard(img, currPos); deque<int> codeNumbers; bool oddParity[6] = {0, 0, 0, 0, 0, 0}; //Den linken Bereich auslesen for (int i = 0; i < DIGIT_COUNT; ++i) { int num = readDigit(img, currPos, unitWidth, LEFT); if (num <= 10) { oddParity[i] = 1; } codeNumbers.push_back(num % 10); correctReaderPos(img, currPos, SPACE, BAR); } //Linken berech auf Kodierung ueberpruefen for (int i = 0; i < 10; i++) { if (matchingParityPattern(oddParity, i)) { codeNumbers.push_front(i); } } //Den mittleren Bereich ueberspringen for (int i = 0; i < 5; ++i) { skipPart(img, currPos); } //Den rechten Bereich auslesen for (int i = 0; i < DIGIT_COUNT; ++i) { int num = readDigit(img, currPos, unitWidth, RIGHT); codeNumbers.push_back(num); correctReaderPos(img, currPos, BAR, SPACE); } return codeNumbers; }
Mat_<uchar> binaryThresholding(const Mat_<uchar>& image, const std::pair<double, double>& stats) { Mat_<uchar> image_binary(image.size()); double Z = 0.9; double threshold = stats.first + (Z * stats.second); for(int i = 0; i < image.rows; ++i) { for(int j = 0; j < image.cols; ++j) { if(image.at<uchar>(i, j) >= threshold + std::numeric_limits<double>::epsilon()) image_binary.at<uchar>(i, j) = 255; else image_binary.at<uchar>(i, j) = 0; } } return image_binary; }
static Mat endpointError( const Mat_<Point2f>& flow1, const Mat_<Point2f>& flow2 ) { Mat result(flow1.size(), CV_32FC1); for ( int i = 0; i < flow1.rows; ++i ) { for ( int j = 0; j < flow1.cols; ++j ) { const Point2f u1 = flow1(i, j); const Point2f u2 = flow2(i, j); if ( isFlowCorrect(u1) && isFlowCorrect(u2) ) { const Point2f diff = u1 - u2; result.at<float>(i, j) = sqrt((float)diff.ddot(diff)); //distance } else result.at<float>(i, j) = std::numeric_limits<float>::quiet_NaN(); } } return result; }
static Mat angularError( const Mat_<Point2f>& flow1, const Mat_<Point2f>& flow2 ) { Mat result(flow1.size(), CV_32FC1); for ( int i = 0; i < flow1.rows; ++i ) { for ( int j = 0; j < flow1.cols; ++j ) { const Point2f u1_2d = flow1(i, j); const Point2f u2_2d = flow2(i, j); const Point3f u1(u1_2d.x, u1_2d.y, 1); const Point3f u2(u2_2d.x, u2_2d.y, 1); if ( isFlowCorrect(u1) && isFlowCorrect(u2) ) result.at<float>(i, j) = acos((float)(u1.ddot(u2) / norm(u1) * norm(u2))); else result.at<float>(i, j) = std::numeric_limits<float>::quiet_NaN(); } } return result; }
bool intersects(const vector<Point> &contour, const Mat_<uchar> &mask) { Mat_<uchar> c = Mat_<uchar>::zeros(mask.size()); fillConvexPoly(c, contour.data(), contour.size(), 255); /* Below is: bitwise_and(mask, c, c); return countNonZero(c); optimized. */ auto it_m = mask.begin(); auto it_c = c.begin(); while (it_m != mask.end()) { if (*it_m && *it_c) return true; ++it_m, ++it_c; } return false; }
void getLaplacianPyramid(const Mat_<Vec3f>& image, vector<Mat_<Vec3f> >& laplacianPyramid, int numLevels) { //laplacianPyramid.push_back(image); // Construct each level of the pyramid Mat_<Vec3f> curImage; //image.convertTo(curImage, CV_32FC3); curImage = image; for (int level = 0; level < numLevels; level++) { if (level < numLevels - 1) { Mat_<Vec3f> newLevel; Mat_<Vec3f> down_tmp, up_tmp; pyrDown(curImage, down_tmp); pyrUp(down_tmp, up_tmp, curImage.size()); newLevel = curImage - up_tmp; laplacianPyramid.push_back(newLevel); curImage = down_tmp; } else { // Top level of laplacian pyramid is just a Gaussian-blurred // image (not difference of gaussian/laplacian) laplacianPyramid.push_back(curImage); } } }
Mat optical_flow(const Mat_<float>& ImgA, const Mat_<float>& ImgB, int num_it, float threshold) { // Compute Gaussin Pyramid int nl = 5; float ds = 0.5; stack<pair<Mat, Mat> > gp = compute_gaussian_pyramids(ImgA, ImgB, nl, ds); Mat_<float> u = Mat::zeros(ImgA.size(), CV_32F); Mat_<float> v = Mat::zeros(ImgA.size(), CV_32F); while (!gp.empty()) { Mat imgA = (gp.top()).first; Mat imgB = (gp.top()).second; // Warp the first image. Mat_<float> imgBw = imgB.clone(); /* Compute warping here from u and v. */ imgBw = compute_warp(imgB, u, v, ds); /* Compute the derivatives. */ Mat_<float> Ix, Iy, It; compute_derivatives(imgBw, imgA, Ix, Iy, It); // papers Mat_<float> du = Mat::zeros(imgA.size(), CV_32F); Mat_<float> dv = Mat::zeros(imgA.size(), CV_32F); for (int i = 0; i < 500; i ++) iterative_computation(du, dv, Ix, Iy, It); u = u - du; v = v - dv; gp.pop(); } Mat Mflow = color_map(u, v); return Mflow; }
static void drawOpticalFlow(const Mat_<Point2f>& flow, Mat& dst, float maxmotion = -1) { dst.create(flow.size(), CV_8UC3); dst.setTo(Scalar::all(0)); // determine motion range: float maxrad = maxmotion; if (maxmotion <= 0) { maxrad = 1; for (int y = 0; y < flow.rows; ++y) { for (int x = 0; x < flow.cols; ++x) { Point2f u = flow(y, x); if (!isFlowCorrect(u)) continue; maxrad = max(maxrad, sqrt(u.x * u.x + u.y * u.y)); } } } for (int y = 0; y < flow.rows; ++y) { for (int x = 0; x < flow.cols; ++x) { Point2f u = flow(y, x); if (isFlowCorrect(u)) dst.at<Vec3b>(y, x) = computeColor(u.x / maxrad, u.y / maxrad); } } }
// We are implementing SLIC here. I'm too lazy to enfoce the connectivity Mat_< int > Superpixel::slic( const Mat_< Vec3f >& im ) const { // Compute the spacing and grid size of the superpixels double sp_area = 1.0 * im.cols * im.rows / K_; int Kx = 0.5 + im.cols / sqrt( sp_area ), Ky = 0.5 + im.rows / sqrt( sp_area ); int K = Kx*Ky; int win_sz = 1.0 * sqrt(sp_area) + 1; // Initialize the seeds on a regular grid std::vector< int64_t > cnt( K ); std::vector< Point2d > seedsd( K ); std::vector< Point > seeds( K ); for( int i=0,k=0; i<Kx; i++ ) for( int j=0; j<Ky; j++, k++ ) seeds[k] = Point( (i+0.5)*(im.cols-1)/Kx, (j+0.5)*(im.rows-1)/Ky ); // Run k-means Mat_<float> dist( im.size() ); Mat_<int> label( im.size() ); for( int it=0; it<n_iter_; it++ ) { // Assignment step dist = std::numeric_limits<float>::max(); label = -1; for( int k=0; k<K; k++ ) { Vec3f c = im( seeds[k] ); for( int j=std::max(0,seeds[k].y-win_sz); j<im.rows && j<=seeds[k].y+win_sz; j++ ) for( int i=std::max(0,seeds[k].x-win_sz); i<im.cols && i<=seeds[k].x+win_sz; i++ ){ double d = (i-seeds[k].x) * (i-seeds[k].x) + (j-seeds[k].y) * (j-seeds[k].y); double cd = ( im( j, i ) - c ).dot( im( j, i ) - c ); d += col_w_ * col_w_ * cd; if( d < dist( j, i ) ) { dist( j, i ) = d; label( j, i ) = k; } } } // Update for( int k=0; k<K; k++ ) { seedsd[k] = Point2d(0,0); cnt[k] = 0; } for( int j=0; j<im.rows; j++ ) for( int i=0; i<im.cols; i++ ) { // Fix all the pixels we messed up! if ( label( j, i ) < 0 ) { // printf("Oops that wasn't very slick :(\n"); for( int k=0; k<K; k++ ){ Vec3f c = im( seeds[k] ); double d = (i-seeds[k].x) * (i-seeds[k].x) + (j-seeds[k].y) * (j-seeds[k].y); double cd = ( im( j, i ) - c ).dot( im( j, i ) - c ); d += col_w_ * col_w_ * cd; if( d < dist( j, i ) ) { dist( j, i ) = d; label( j, i ) = k; } } } seedsd[ label( j, i ) ] += Point2d( i, j ); cnt[ label( j, i ) ] += 1; } for( int k=0; k<K; k++ ) if (cnt[k] > 0) seeds[k] = Point( 0.5 + seedsd[k].x / cnt[k], 0.5 + seedsd[k].y / cnt[k] ); } return label; }
Mat_< int > Superpixel::geodesicSegmentation( const Mat_< Vec3f >& im ) const { //std::tr1::uniform_int<int> distribution(-2, 2); //std::tr1::mt19937 engine; // Mersenne twister MT19937 //auto randint = std::tr1::bind(distribution, engine); int s; srand((unsigned)time(NULL)); s=rand()%4-2; // Compute the spacing and grid size of the superpixels double sp_area = 1.0 * im.cols * im.rows / K_; int Kx = 0.5 + im.cols / sqrt( sp_area ), Ky = 0.5 + im.rows / sqrt( sp_area ); int K = Kx*Ky; int win_sz = 1.0 * sqrt(sp_area) + 1; // Initialize the seeds on a regular grid std::vector< int64_t > cnt( K ); std::vector< Point2d > seedsd( K ); std::vector< Point > seeds( K ); for( int i=0,k=0; i<Kx; i++ ) for( int j=0; j<Ky; j++, k++ ) seeds[k] = Point( (i+0.5)*(im.cols-1)/Kx, (j+0.5)*(im.rows-1)/Ky ) + Point(rand()%4-2,rand()%4-2);/*Point( randint(), randint() );*/ Mat_<float> dx( im.size() ), dy( im.size() ); for( int j=0; j<im.rows; j++ ) for( int i=0; i<im.cols; i++ ) { if (i) dx(j,i-1) = col_w_*sqrt( (im(j,i)-im(j,i-1)).dot(im(j,i)-im(j,i-1)) ) + 1; if (j) dy(j-1,i) = col_w_*sqrt( (im(j-1,i)-im(j,i)).dot(im(j-1,i)-im(j,i)) ) + 1; } // Run k-means Mat_<float> dist( im.size() ); Mat_<int> label( im.size() ); for( int it=0; it<n_iter_; it++ ) { // Assignment step dist = std::numeric_limits<float>::max(); label = -1; for( int k=0; k<K; k++ ) { dist( seeds[k] ) = 0; label( seeds[k] ) = k; } for( int IT=0; IT<2; IT++ ){ for( int j=0; j<im.rows; j++ ) for( int i=0; i<im.cols; i++ ) { if (i && dist(j,i-1) + dx(j,i-1) < dist(j,i)) { dist(j,i) = dist(j,i-1) + dx(j,i-1); label(j,i) = label(j,i-1); } if (j && dist(j-1,i) + dx(j-1,i) < dist(j,i)) { dist(j,i) = dist(j-1,i) + dy(j-1,i); label(j,i) = label(j-1,i); } } for( int j=im.rows-1; j>=0; j-- ) for( int i=im.cols-1; i>=0; i-- ) { if (i && dist(j,i) + dx(j,i-1) < dist(j,i-1)) { dist(j,i-1) = dist(j,i) + dx(j,i-1); label(j,i-1) = label(j,i); } if (j && dist(j,i) + dx(j-1,i) < dist(j-1,i)) { dist(j-1,i) = dist(j,i) + dy(j-1,i); label(j-1,i) = label(j,i); } } } for( int k=0; k<K; k++ ) { Vec3f c = im( seeds[k] ); for( int j=std::max(0,seeds[k].y-win_sz); j<im.rows && j<=seeds[k].y+win_sz; j++ ) for( int i=std::max(0,seeds[k].x-win_sz); i<im.cols && i<=seeds[k].x+win_sz; i++ ){ double d = (i-seeds[k].x) * (i-seeds[k].x) + (j-seeds[k].y) * (j-seeds[k].y); double cd = ( im( j, i ) - c ).dot( im( j, i ) - c ); d += col_w_ * col_w_ * cd; if( d < dist( j, i ) ) { dist( j, i ) = d; label( j, i ) = k; } } } // Update for( int k=0; k<K; k++ ) { seedsd[k] = Point2d(0,0); cnt[k] = 0; } for( int j=0; j<im.rows; j++ ) for( int i=0; i<im.cols; i++ ) { // Fix all the pixels we messed up! if ( label( j, i ) < 0 ) { // printf("Oops that wasn't very slick :(\n"); for( int k=0; k<K; k++ ){ Vec3f c = im( seeds[k] ); double d = (i-seeds[k].x) * (i-seeds[k].x) + (j-seeds[k].y) * (j-seeds[k].y); double cd = ( im( j, i ) - c ).dot( im( j, i ) - c ); d += col_w_ * col_w_ * cd; if( d < dist( j, i ) ) { dist( j, i ) = d; label( j, i ) = k; } } } seedsd[ label( j, i ) ] += Point2d( i, j ); cnt[ label( j, i ) ] += 1; } for( int k=0; k<K; k++ ) if (cnt[k] > 0) seeds[k] = Point( 0.5 + seedsd[k].x / cnt[k], 0.5 + seedsd[k].y / cnt[k] ); } return label; }
int main(int argc, char** argv) { /*Polynomial2 poly2; poly2.kuu = -1; poly2.kuv = 1; poly2.kvv= -1; poly2.ku = 0.25; poly2.kv = 0.25; poly2.k1 = 5; CurveRasterizer<Polynomial2> raster(1, 1, -100, 100, poly2); CurveRasterizer2<Polynomial2> raster2(1, 1, -100, 100, poly2); auto tr0 = clock(); int x1 = 0; int x2 = 0; for (int i = 0; i < 10000000; i++) { raster.step(); x1 += raster.x; } auto tr1 = clock(); for (int i = 0; i < 10000000; i++) { raster2.step(); x2 += raster2.x; } auto tr2 = clock(); cout << "optimized " << double(tr1 - tr0) / CLOCKS_PER_SEC << endl; cout << "simple " << double(tr2 - tr1) / CLOCKS_PER_SEC << endl; cout << x1 << " " << x2 << endl; return 0;*/ ifstream paramFile(argv[1]); if (not paramFile.is_open()) { cout << argv[1] << " : ERROR, file is not found" << endl; return 0; } array<double, 6> params; cout << "EU Camera model parameters :" << endl; for (auto & p: params) { paramFile >> p; cout << setw(15) << p; } cout << endl; paramFile.ignore(); array<double, 6> cameraPose; cout << "Camera pose wrt the robot :" << endl; for (auto & e: cameraPose) { paramFile >> e; cout << setw(15) << e; } cout << endl; paramFile.ignore(); Transformation<double> TbaseCamera(cameraPose.data()); array<double, 6> planePose; cout << "Plane pose :" << endl; for (auto & e: cameraPose) { paramFile >> e; cout << setw(15) << e; } cout << endl; paramFile.ignore(); Transformation<double> TbasePlane(cameraPose.data()); StereoParameters stereoParams; paramFile >> stereoParams.u0; paramFile >> stereoParams.v0; paramFile >> stereoParams.disparityMax; paramFile >> stereoParams.blockSize; paramFile.ignore(); string imageDir; getline(paramFile, imageDir); string imageInfo, imageName; array<double, 6> robotPose1, robotPose2; getline(paramFile, imageInfo); istringstream imageStream(imageInfo); imageStream >> imageName; for (auto & x : robotPose1) imageStream >> x; Mat8 img1 = imread(imageDir + imageName, 0); int counter = 2; while (getline(paramFile, imageInfo)) { istringstream imageStream(imageInfo); imageStream >> imageName; for (auto & x : robotPose2) imageStream >> x; Transformation<double> T01(robotPose1.data()), T02(robotPose2.data()); Transformation<double> TleftRight = T01.compose(TbaseCamera).inverseCompose(T02.compose(TbaseCamera)); Mat8 img2 = imread(imageDir + imageName, 0); EnhancedStereo stereo(TleftRight, img1.cols, img1.rows, params.data(), params.data(), stereoParams); cv::Mat_<uint8_t> res; auto t2 = clock(); stereo.comuteStereo(img1, img2, res); auto t3 = clock(); // cout << double(t3 - t2) / CLOCKS_PER_SEC << endl; Mat_<float> distMat; Mat_<float> planeMat; stereo.computeDistance(distMat); Transformation<double> T0Camera = T01.compose(TbaseCamera); stereo.generatePlane(T0Camera.inverseCompose(TbasePlane), planeMat, vector<Vector3d>{Vector3d(-0.1, -0.1, 0), Vector3d(-0.1 + 3 * 0.45, -0.1, 0), Vector3d(-0.1 + 3 * 0.45, 0.5, 0), Vector3d(-0.1, 0.5, 0) } ); imshow("dist" + to_string(counter) , distMat); imshow("plane" , planeMat); imwrite("/home/bogdan/projects/plane.png", planeMat); double err = 0; double err2 = 0; double dist = 0; int N = 0; int Nmax = 0; Mat_<float> inlierMat(planeMat.size()); inlierMat.setTo(0); for (int u = 0; u < distMat.cols; u++) { for (int v = 0; v < distMat.rows; v++) { if (planeMat(v, u) == 0) continue; Nmax++; dist += planeMat(v, u); inlierMat(v, u) = 1; if (distMat(v, u) == 0 or distMat(v, u) != distMat(v, u) or planeMat(v, u) != planeMat(v, u)) continue; if (abs(distMat(v, u) - planeMat(v, u)) > 0.10) continue; inlierMat(v, u) = 0; err += distMat(v, u) - planeMat(v, u); err2 += pow(distMat(v, u) - planeMat(v, u), 2); N++; } } // cout << (counter - 1) * 7 << " & " << dist/ Nmax * 1000 << " & " << err / N *1000 << " & " << sqrt(err2 / N)*1000 // << " & " << 100 * N / double(Nmax) << "\\\\" << endl << "\\hline" << endl; cout << "avg err : " << err / N *1000 << " avg err2 : " << sqrt(err2 / N)*1000 << " number of inliers : " << 100 * N / double(Nmax) << endl; imshow("diff" + to_string(counter), abs(planeMat - distMat)); imshow("inliers" + to_string(counter), inlierMat); counter++; } waitKey(); return 0; }
bool c_FourierTransfrom::ifftw_complex_3d(const Mat_<Vec6d> &_input, Mat_<Vec6d> &_output) { size_t height = _input.rows; size_t width = _input.cols; size_t n_channels = _input.channels() / 2; size_t n_pixels = height * width; size_t n_data = n_pixels * n_channels; fftw_complex *in, *out; fftw_plan p; in = (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * n_data); out = (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * n_data); p = fftw_plan_dft_3d(height, width, n_channels, in, out, FFTW_BACKWARD, FFTW_ESTIMATE); /*!< prepare the data */ for (size_t i_row = 0; i_row < height; ++i_row) { const Vec3d *p = _input.ptr<Vec3d>(i_row); for (size_t i_col = 0; i_col < width; ++i_col) { size_t index = i_row * width + i_col; for (size_t k = 0; k < n_channels; ++k) { in[n_pixels * k + index][0] = p[i_col][k]; in[n_pixels * k + index][1] = p[i_col][k + n_channels]; } #if 0 in[index][0] = p[i_col][4]; in[index][1] = p[i_col][5]; in[n_pixels + index][0] = p[i_col][2]; in[n_pixels + index][1] = p[i_col][3]; in[n_pixels * 2 + index][0] = p[i_col][0]; in[n_pixels * 2 + index][1] = p[i_col][1]; #endif } } fftw_execute(p); /*!< write back data */ _output = Mat_<Vec6d>::zeros(_input.size()); for (size_t i_row = 0; i_row < height; ++i_row) { Vec6d *p = _output.ptr<Vec6d>(i_row); for (size_t i_col = 0; i_col < width; ++i_col) { size_t index = i_row * width + i_col; for (size_t k = 0; k < n_channels; ++k) { p[i_col][k] = out[n_pixels * k + index][0]; p[i_col][k + n_channels] = out[n_pixels * k + index][1]; } #if 0 p[i_col][0] = out[n_pixels * 2 + index][0]; p[i_col][1] = out[n_pixels + index][0]; p[i_col][2] = out[index][0]; p[i_col][3] = out[n_pixels * 2 + index][1]; p[i_col][4] = out[n_pixels + index][1]; p[i_col][5] = out[index][1]; #endif } } _output /= n_data; fftw_destroy_plan(p); fftw_free(in); fftw_free(out); return true; }