Exemple #1
0
void PBASFrameProcessor::process(cv:: Mat &frame, cv:: Mat &output)
{
		const int medFilterSize = 7;
		double meanGradMagn;
		std::vector<AmbiguousCandidate> candidates;
		cv::Mat subSamplingOffset = cv::Mat::zeros(frame.size(), CV_32F);

		if (m_iteration == 0)
		{
			m_lastResult = cv::Mat::zeros(frame.size(), CV_8U);
			m_lastResultPP = cv::Mat::zeros(frame.size(), CV_8U);
			m_noiseMap = cv::Mat::zeros(frame.size(), CV_32F);
			m_gradMagnMap.create(frame.size(), CV_8U);
			m_motionDetector.initialize(frame);
		}
		if (m_iteration != 0)
			m_motionDetector.updateMotionMap(frame);

		//updateGradMagnMap(frame);
		std::vector<PBASFeature> featureMap = PBASFeature::calcFeatureMap(frame);
		//meanGradMagn = PBASFeature::calcMeanGradMagn(featureMap, frame.rows, frame.cols);
		//PBASFeature::setColorWeight(0.8 - meanGradMagn / 255);
		
		m_pbas.process(featureMap, frame.rows, frame.cols, m_currentResult, m_motionDetector.getMotionMap());

		if (m_pbas.isInitialized())
		{
			m_motionDetector.updateCandidates(m_currentResultPP);
			m_motionDetector.classifyStaticCandidates(frame, m_pbas.drawBgSample(), m_currentResultPP);

			candidates = m_motionDetector.getStaticObjects();

			for (int k = 0; k < candidates.size(); ++k)
			{
				if (candidates[k].state == DETECTOR_GHOST_OBJECT)
				{
					subSamplingOffset(candidates[k].boundingBox) -= 100;
				}
				else if ((candidates[k].state == DETECTOR_STATIC_OBJECT))
				{
					subSamplingOffset(candidates[k].boundingBox) += 50;
				}
				
			}
			m_pbas.subSamplingOffset(subSamplingOffset);
			
			#ifndef _DATASET	
			MotionDetector::drawBoundingBoxesClassified(frame, m_outputWithBb, candidates);
			#endif 
		}



// !_DATASET

		// normalize gradient magnmap

		//parallelBackgroundAveraging(&rgbChannels, false, &m_currentResult);
		//###############################################
		//POST-PROCESSING HERE
		//for the final results in the changedetection-challenge a 9x9 median filter has been applied
		cv::medianBlur(m_currentResult, m_currentResultPP, medFilterSize);

		//###############################################
		m_currentResultPP.copyTo(output);
		updateNoiseMap();
		m_currentResult.copyTo(m_lastResult);
		++m_iteration;
}
Exemple #2
0
bool cv::RGBDOdometry( cv::Mat& Rt, const Mat& initRt,
                       const cv::Mat& image0, const cv::Mat& _depth0, const cv::Mat& validMask0,
                       const cv::Mat& image1, const cv::Mat& _depth1, const cv::Mat& validMask1,
                       const cv::Mat& cameraMatrix, float minDepth, float maxDepth, float maxDepthDiff,
                       const std::vector<int>& iterCounts, const std::vector<float>& minGradientMagnitudes,
                       int transformType )
{
    const int sobelSize = 3;
    const double sobelScale = 1./8;

    Mat depth0 = _depth0.clone(),
        depth1 = _depth1.clone();

    // check RGB-D input data
    CV_Assert( !image0.empty() );
    CV_Assert( image0.type() == CV_8UC1 );
    CV_Assert( depth0.type() == CV_32FC1 && depth0.size() == image0.size() );

    CV_Assert( image1.size() == image0.size() );
    CV_Assert( image1.type() == CV_8UC1 );
    CV_Assert( depth1.type() == CV_32FC1 && depth1.size() == image0.size() );

    // check masks
    CV_Assert( validMask0.empty() || (validMask0.type() == CV_8UC1 && validMask0.size() == image0.size()) );
    CV_Assert( validMask1.empty() || (validMask1.type() == CV_8UC1 && validMask1.size() == image0.size()) );

    // check camera params
    CV_Assert( cameraMatrix.type() == CV_32FC1 && cameraMatrix.size() == Size(3,3) );

    // other checks
    CV_Assert( iterCounts.empty() || minGradientMagnitudes.empty() ||
               minGradientMagnitudes.size() == iterCounts.size() );
    CV_Assert( initRt.empty() || (initRt.type()==CV_64FC1 && initRt.size()==Size(4,4) ) );

    std::vector<int> defaultIterCounts;
    std::vector<float> defaultMinGradMagnitudes;
    std::vector<int> const* iterCountsPtr = &iterCounts;
    std::vector<float> const* minGradientMagnitudesPtr = &minGradientMagnitudes;

    if( iterCounts.empty() || minGradientMagnitudes.empty() )
    {
        defaultIterCounts.resize(4);
        defaultIterCounts[0] = 7;
        defaultIterCounts[1] = 7;
        defaultIterCounts[2] = 7;
        defaultIterCounts[3] = 10;

        defaultMinGradMagnitudes.resize(4);
        defaultMinGradMagnitudes[0] = 12;
        defaultMinGradMagnitudes[1] = 5;
        defaultMinGradMagnitudes[2] = 3;
        defaultMinGradMagnitudes[3] = 1;

        iterCountsPtr = &defaultIterCounts;
        minGradientMagnitudesPtr = &defaultMinGradMagnitudes;
    }

    preprocessDepth( depth0, depth1, validMask0, validMask1, minDepth, maxDepth );

    std::vector<Mat> pyramidImage0, pyramidDepth0,
                pyramidImage1, pyramidDepth1, pyramid_dI_dx1, pyramid_dI_dy1, pyramidTexturedMask1,
                pyramidCameraMatrix;
    buildPyramids( image0, image1, depth0, depth1, cameraMatrix, sobelSize, sobelScale, *minGradientMagnitudesPtr,
                   pyramidImage0, pyramidDepth0, pyramidImage1, pyramidDepth1,
                   pyramid_dI_dx1, pyramid_dI_dy1, pyramidTexturedMask1, pyramidCameraMatrix );

    Mat resultRt = initRt.empty() ? Mat::eye(4,4,CV_64FC1) : initRt.clone();
    Mat currRt, ksi;
    for( int level = (int)iterCountsPtr->size() - 1; level >= 0; level-- )
    {
        const Mat& levelCameraMatrix = pyramidCameraMatrix[level];

        const Mat& levelImage0 = pyramidImage0[level];
        const Mat& levelDepth0 = pyramidDepth0[level];
        Mat levelCloud0;
        cvtDepth2Cloud( pyramidDepth0[level], levelCloud0, levelCameraMatrix );

        const Mat& levelImage1 = pyramidImage1[level];
        const Mat& levelDepth1 = pyramidDepth1[level];
        const Mat& level_dI_dx1 = pyramid_dI_dx1[level];
        const Mat& level_dI_dy1 = pyramid_dI_dy1[level];

        CV_Assert( level_dI_dx1.type() == CV_16S );
        CV_Assert( level_dI_dy1.type() == CV_16S );

        const double fx = levelCameraMatrix.at<double>(0,0);
        const double fy = levelCameraMatrix.at<double>(1,1);
        const double determinantThreshold = 1e-6;

        Mat corresps( levelImage0.size(), levelImage0.type() );

        // Run transformation search on current level iteratively.
        for( int iter = 0; iter < (*iterCountsPtr)[level]; iter ++ )
        {
            int correspsCount = computeCorresp( levelCameraMatrix, levelCameraMatrix.inv(), resultRt.inv(DECOMP_SVD),
                                                levelDepth0, levelDepth1, pyramidTexturedMask1[level], maxDepthDiff,
                                                corresps );

            if( correspsCount == 0 )
                break;

            bool solutionExist = computeKsi( transformType,
                                             levelImage0, levelCloud0,
                                             levelImage1, level_dI_dx1, level_dI_dy1,
                                             corresps, correspsCount,
                                             fx, fy, sobelScale, determinantThreshold,
                                             ksi );

            if( !solutionExist )
                break;

            computeProjectiveMatrix( ksi, currRt );

            resultRt = currRt * resultRt;

#if SHOW_DEBUG_IMAGES
            std::cout << "currRt " << currRt << std::endl;
            Mat warpedImage0;
            const Mat distCoeff(1,5,CV_32FC1,Scalar(0));
            warpImage<uchar>( levelImage0, levelDepth0, resultRt, levelCameraMatrix, distCoeff, warpedImage0 );

            imshow( "im0", levelImage0 );
            imshow( "wim0", warpedImage0 );
            imshow( "im1", levelImage1 );
            waitKey();
#endif
        }
    }

    Rt = resultRt;

    return !Rt.empty();
}
cv::Mat
Auvsi_Recognize::centerBinary( cv::Mat input )
{
	typedef cv::Vec<unsigned char, 1> VT_binary;

	cv::Mat buffered = cv::Mat( input.rows * 2, input.cols * 2, input.type() );
	cv::Mat retVal;

	int centerX, centerY;
	int minX, minY, maxX, maxY;
	int radiusX, radiusY;
	std::vector<int> xCoords;
	std::vector<int> yCoords;

	// Get centroid
	cv::Moments imMoments = cv::moments( input, true );

	centerX = (imMoments.m10 / imMoments.m00) - buffered.cols / 2;
	centerY = (imMoments.m01 / imMoments.m00) - buffered.rows / 2;

	// Get centered x and y coordinates
	cv::MatIterator_<VT_binary> inputIter = input.begin<VT_binary>();
	cv::MatIterator_<VT_binary> inputEnd = input.end<VT_binary>();

	for( ; inputIter != inputEnd; ++inputIter )
	{
		unsigned char value = (*inputIter)[0];
		if( value )
		{
			xCoords.push_back( inputIter.pos().x - centerX );
			yCoords.push_back( inputIter.pos().y - centerY );
		}
	}

	if( xCoords.size() <= 0 || yCoords.size() <= 0 ) // nothing in image
	{
		return input;
	}

	// Get min and max x and y coords (centered)
	minX = *std::min_element( xCoords.begin(), xCoords.end() );
	minY = *std::min_element( yCoords.begin(), yCoords.end() );
	maxX = *std::max_element( xCoords.begin(), xCoords.end() );
	maxY = *std::max_element( yCoords.begin(), yCoords.end() );	

	// Get new centroids
	centerX = getVectorMean<int>( xCoords );
	centerY = getVectorMean<int>( yCoords );

	// Get radius from center in each direction
	radiusX = std::max( abs(maxX - centerX), abs(centerX - minX) );
	radiusY = std::max( abs(maxY - centerY), abs(centerY - minY) );

	// Center image in temporary buffered array
	buffered = cvScalar(0);

	std::vector<int>::iterator iterX = xCoords.begin();
	std::vector<int>::iterator endX = xCoords.end();
	std::vector<int>::iterator iterY = yCoords.begin();

	for( ; iterX != endX; ++iterX, ++iterY )
	{
		buffered.at<VT_binary>( *iterY, *iterX ) = VT_binary(255);
	}

	// Center image
	buffered = buffered.colRange( centerX - radiusX, centerX + radiusX + 1 );
	buffered = buffered.rowRange( centerY - radiusY, centerY + radiusY + 1 );

	// Add extra padding to make square
	int outH, outW;
	outH = buffered.rows;
	outW = buffered.cols;
	
	if( outH < outW ) // pad height
		cv::copyMakeBorder( buffered, retVal, (outW-outH)/2, (outW-outH)/2, 0, 0, cv::BORDER_CONSTANT, cvScalar(0) );
	else // pad width
		cv::copyMakeBorder( buffered, retVal, 0, 0, (outH-outW)/2, (outH-outW)/2, cv::BORDER_CONSTANT, cvScalar(0) );

	// Make sure output is desired width
	cv::resize( retVal, buffered, input.size(), 0, 0, cv::INTER_NEAREST );
	
	return buffered;
}
Exemple #4
0
	cv::Mat orientation(cv::Mat inputImage)
	{
		cv::Mat orientationMat = cv::Mat::zeros(inputImage.size(), CV_8UC1);

		// compute gradients at each pixel
		cv::Mat grad_x, grad_y;
		cv::Sobel(inputImage, grad_x, CV_16SC1, 1, 0, 3, 1, 0, cv::BORDER_DEFAULT);
		cv::Sobel(inputImage, grad_y, CV_16SC1, 0, 1, 3, 1, 0, cv::BORDER_DEFAULT);

		cv::Mat Vx, Vy, theta, lowPassX, lowPassY;
		cv::Mat lowPassX2, lowPassY2;

		Vx = cv::Mat::zeros(inputImage.size(), inputImage.type());
		Vx.copyTo(Vy);
		Vx.copyTo(theta);
		Vx.copyTo(lowPassX);
		Vx.copyTo(lowPassY);
		Vx.copyTo(lowPassX2);
		Vx.copyTo(lowPassY2);

		// estimate the local orientation of each block
		int blockSize = 16;

		for (int i = blockSize / 2; i < inputImage.rows - blockSize / 2; i += blockSize)
		{
			for (int j = blockSize / 2; j < inputImage.cols - blockSize / 2; j += blockSize)
			{
				float sum1 = 0.0;
				float sum2 = 0.0;

				for (int u = i - blockSize / 2; u < i + blockSize / 2; u++)
				{
					for (int v = j - blockSize / 2; v < j + blockSize / 2; v++)
					{
						sum1 += grad_x.at<float>(u, v) * grad_y.at<float>(u, v);
						sum2 += (grad_x.at<float>(u, v)*grad_x.at<float>(u, v)) * (grad_y.at<float>(u, v)*grad_y.at<float>(u, v));
					}
				}

				Vx.at<float>(i, j) = sum1;
				Vy.at<float>(i, j) = sum2;

				double calc = 0.0;

				if (sum1 != 0 && sum2 != 0)
				{
					calc = 0.5 * atan(Vy.at<float>(i, j) / Vx.at<float>(i, j));
				}

				theta.at<float>(i, j) = calc;

				// Perform low-pass filtering
				float angle = 2 * calc;
				lowPassX.at<float>(i, j) = cos(angle * CV_PI / 180);
				lowPassY.at<float>(i, j) = sin(angle * CV_PI / 180);

				float sum3 = 0.0;
				float sum4 = 0.0;
				int lowPassSize;

				for (int u = -lowPassSize / 2; u < lowPassSize / 2; u++)
				{
					for (int v = -lowPassSize / 2; v < lowPassSize / 2; v++)
					{
						sum3 += inputImage.at<float>(u, v) * lowPassX.at<float>(i - u*lowPassSize, j - v * lowPassSize);
						sum4 += inputImage.at<float>(u, v) * lowPassY.at<float>(i - u*lowPassSize, j - v * lowPassSize);
					}
				}
				lowPassX2.at<float>(i, j) = sum3;
				lowPassY2.at<float>(i, j) = sum4;

				float calc2 = 0.0;

				if (sum3 != 0 && sum4 != 0)
				{
					calc2 = 0.5 * atan(lowPassY2.at<float>(i, j) / lowPassX2.at<float>(i, j)) * 180 / CV_PI;
				}
				orientationMat.at<float>(i, j) = calc2;

			}

		}
		return orientationMat;
	}
/*Function to calculate the integral histogram*/
std::vector<cv::Mat> calculateIntegralHOG(const cv::Mat& _in, const int _nbins) {
  /*Convert the input image to grayscale*/
//  cv::Mat img_gray;

//  cv::cvtColor(_in, img_gray, CV_BGR2GRAY);
//  cv::equalizeHist(img_gray, img_gray);

  /* Calculate the derivates of the grayscale image in the x and y
   directions using a sobel operator and obtain 2 gradient images
   for the x and y directions*/

  cv::Mat xsobel, ysobel;
  cv::Sobel(_in, xsobel, CV_32FC1, 1, 0);
  cv::Sobel(_in, ysobel, CV_32FC1, 0, 1);

  //Gradient magnitude
  cv::Mat gradient_magnitude;
  cv::magnitude(xsobel, ysobel, gradient_magnitude);
  //Gradient orientation
  cv::Mat gradient_orientation;
  bool angleInDegrees = true;
  cv::phase(xsobel, ysobel, gradient_orientation, angleInDegrees);

//  std::cout << "_in=" << _in.rows << "x" << _in.cols << std::endl;
//  std::cout << "gradient_magnitude=" << gradient_magnitude.rows << "x" << gradient_magnitude.cols << std::endl;
//  std::cout << "gradient_orientation=" << gradient_orientation.rows << "x" << gradient_orientation.cols << std::endl;
//
//  double min_angle, max_angle;
//  cv::minMaxLoc(gradient_orientation, &min_angle, &max_angle, NULL, NULL);
//  std::cout << "min_angle=" << min_angle << " ; max_angle=" << max_angle << std::endl;


  //TODO: useless ?
//  img_gray.release();

  /* Create an array of 9 images (9 because I assume bin size 20 degrees
   and unsigned gradient ( 180/20 = 9), one for each bin which will have
   zeroes for all pixels, except for the pixels in the original image
   for which the gradient values correspond to the particular bin.
   These will be referred to as bin images. These bin images will be then
   used to calculate the integral histogram, which will quicken
   the calculation of HOG descriptors */

  std::vector<cv::Mat> bins(_nbins);
  for (int i = 0; i < _nbins; i++) {
    bins[i] = cv::Mat::zeros(_in.size(), CV_32FC1);
  }

  /* Create an array of 9 images ( note the dimensions of the image,
   the cvIntegral() function requires the size to be that), to store
   the integral images calculated from the above bin images.
   These 9 integral images together constitute the integral histogram */

  std::vector<cv::Mat> integrals(_nbins);
  //IplImage** integrals = (IplImage**) malloc(9 * sizeof(IplImage*));
  for (int i = 0; i < _nbins; i++) {
    integrals[i] = cv::Mat(
        cv::Size(_in.size().width + 1, _in.size().height + 1), CV_64FC1);
  }

  /* Calculate the bin images. The magnitude and orientation of the gradient
   at each pixel is calculated using the xsobel and ysobel images.
   {Magnitude = sqrt(sq(xsobel) + sq(ysobel) ), gradient = itan (ysobel/xsobel) }.
   Then according to the orientation of the gradient, the value of the
   corresponding pixel in the corresponding image is set */

  int x, y;
  float temp_gradient, temp_magnitude;
  for (y = 0; y < _in.size().height; y++) {
    /* ptr1 and ptr2 point to beginning of the current row in the xsobel and ysobel images
     respectively.
     ptrs[i] point to the beginning of the current rows in the bin images */

#if 0
    float* xsobelRowPtr = (float*) (xsobel.row(y).data);
    float* ysobelRowPtr = (float*) (ysobel.row(y).data);
    float** binsRowPtrs = new float *[_nbins];

    for (int i = 0; i < _nbins; i++) {
      binsRowPtrs[i] = (float*) (bins[i].row(y).data);
    }
#else
    float* xsobelRowPtr = xsobel.ptr<float>(y);
    float* ysobelRowPtr = ysobel.ptr<float>(y);
    float** binsRowPtrs = new float *[_nbins];

    for (int i = 0; i < _nbins; i++) {
      binsRowPtrs[i] = bins[i].ptr<float>(y);
    }

    float* magnitudeRowPtr = gradient_magnitude.ptr<float>(y);
    float* orientationRowPtr = gradient_orientation.ptr<float>(y);
#endif

    /*For every pixel in a row gradient orientation and magnitude
     are calculated and corresponding values set for the bin images. */
    for (x = 0; x < _in.size().width; x++) {
#if 0
      /* if the xsobel derivative is zero for a pixel, a small value is
       added to it, to avoid division by zero. atan returns values in radians,
       which on being converted to degrees, correspond to values between -90 and 90 degrees.
       90 is added to each orientation, to shift the orientation values range from {-90-90} to {0-180}.
       This is just a matter of convention. {-90-90} values can also be used for the calculation. */
      if (xsobelRowPtr[x] == 0) {
        temp_gradient = ((atan(ysobelRowPtr[x] / (xsobelRowPtr[x] + 0.00001)))
            * (180 / M_PI)) + 90;
      } else {
        temp_gradient = ((atan(ysobelRowPtr[x] / xsobelRowPtr[x])) * (180 / M_PI))
            + 90;
      }
      temp_magnitude = sqrt(
          (xsobelRowPtr[x] * xsobelRowPtr[x])
              + (ysobelRowPtr[x] * ysobelRowPtr[x]));
#else
      temp_magnitude = magnitudeRowPtr[x];
      temp_gradient = orientationRowPtr[x] > 180 ? orientationRowPtr[x]-180 : orientationRowPtr[x];
#endif

      /*The bin image is selected according to the gradient values.
       The corresponding pixel value is made equal to the gradient
       magnitude at that pixel in the corresponding bin image */
      float binStep = 180 / _nbins;

      for (int i = 1; i <= _nbins; i++) {
        if (temp_gradient <= binStep * i) {
          binsRowPtrs[i - 1][x] = temp_magnitude;
          break;
        }
      }
    }

    //Delete binsRowPtrs
    delete[] binsRowPtrs;
  }

  //TODO: useless
//  xsobel.release();
//  ysobel.release();

  /*Integral images for each of the bin images are calculated*/
  for (int i = 0; i < _nbins; i++) {
    cv::integral(bins[i], integrals[i]);
  }

  //TODO: useless
//  for (int i = 0; i < _nbins; i++) {
//    bins[i].release();
//  }

  /*The function returns an array of 9 images which constitute the integral histogram*/
  return integrals;
}
void optimized_elec(const PointCloud<pcl::PointXYZRGB> &cloud, const cv::Mat& src_labels, float tolerance,
                    std::vector<std::vector<PointIndices> > &labeled_clusters,
                    unsigned int min_pts_per_cluster, unsigned int max_pts_per_cluster, unsigned int num_parts,
                    bool brute_force_border, float radius_scale)
{
    typedef unsigned char uchar;

    Size sz = src_labels.size();

    cv::Mat dst_labels(sz, CV_32S, Scalar(-1));
    cv::Mat wavefront(sz, CV_32SC2);
    cv::Mat region_sizes = Mat::zeros(sz, CV_32S);

    Point2i *wf = wavefront.ptr<Point2i>();
    int *rsizes = region_sizes.ptr<int>();

    int cc = -1;

    double squared_radius = tolerance * tolerance;

    for(int j = 0; j < sz.height; ++j)
    {
        for(int i = 0; i < sz.width; ++i)
        {
            if (src_labels.at<uchar>(j, i) >= num_parts || dst_labels.at<int>(j, i) != -1) // invalid label && has been labeled
                continue;

            Point2i* ws = wf; // initialize wavefront
            Point2i p(i, j);  // current pixel

            cc++;	// next label

            dst_labels.at<int>(j, i) = cc;
            int count = 0;	// current region size

            // wavefront propagation
            while( ws >= wf ) // wavefront not empty
            {
                // put neighbors onto wavefront
                const uchar* sl = &src_labels.at<uchar>(p.y, p.x);
                int*         dl = &dst_labels.at<  int>(p.y, p.x);
                const pcl::PointXYZRGB *sp = &cloud.at(p.x, p.y);

                //right
                if( p.x < sz.width-1 && dl[+1] == -1 && sl[+1] == sl[0])
                    if (sqnorm(sp[0], sp[+1]) <= squared_radius)
                    {
                        dl[+1] = cc;
                        *ws++ = Point2i(p.x+1, p.y);
                    }

                //left
                if( p.x > 0 && dl[-1] == -1 && sl[-1] == sl[0])
                    if (sqnorm(sp[0], sp[-1]) <= squared_radius)
                    {
                        dl[-1] = cc;
                        *ws++ = Point2i(p.x-1, p.y);
                    }

                //top
                if( p.y < sz.height-1 && dl[+sz.width] == -1 && sl[+sz.width] == sl[0])
                    if (sqnorm(sp[0], sp[+sz.width]) <= squared_radius)
                    {
                        dl[+sz.width] = cc;
                        *ws++ = Point2i(p.x, p.y+1);
                    }


                //top
                if( p.y > 0 && dl[-sz.width] == -1 && sl[-sz.width] == sl[0])
                    if (sqnorm(sp[0], sp[-sz.width]) <= squared_radius)
                    {
                        dl[-sz.width] = cc;
                        *ws++ = Point2i(p.x, p.y-1);
                    }

                // pop most recent and propagate
                p = *--ws;
                count++;
            }

            rsizes[cc] = count;
        } /* for(int i = 0; i < sz.width; ++i) */
    } /* for(int j = 0; j < sz.height; ++j) */

    Mat djset(sz, CV_32S);
    int* dj = djset.ptr<int>();
    yota(dj, dj + sz.area(), 0);


    if (brute_force_border)
    {
        pcl::ScopeTime time("border");

        SearchD search;
        search.setInputCloud(cloud.makeShared());

#define HT_USE_OMP

#ifdef HT_USE_OMP

        const int threads_num = 3;

        int range[threads_num + 1];
        for(int i = 0; i < threads_num+1; ++i)
            range[i] = i * num_parts/threads_num;

        #pragma omp parallel for num_threads(threads_num)
        for(int r = 0; r < threads_num; ++r)
        {
#endif
            for(int j = 1; j < sz.height-1; ++j)
            {
                int *dl = dst_labels.ptr<int>(j);

                for(int i = 1; i < sz.width-1; ++i)
                {
                    int cc = dl[i];

                    if (cc == -1)
                        continue;

#ifdef HT_USE_OMP
                    uchar lbl = src_labels.at<uchar>(j, i);
                    bool inRange =range[r] <= lbl && lbl < range[r+1];
                    if (!inRange)
                        continue;
#endif

                    if (cc == dl[i+1] && cc == dl[i-1] && cc == dl[i+sz.width] && cc == dl[i-sz.width])
                        continue; // inner point

                    int root = cc;
                    while(root != dj[root])
                        root = dj[root];

                    const pcl::PointXYZRGB& sp = cloud.at(i, j);
                    uchar sl = src_labels.at<uchar>(j, i);

                    if (!isFinite(sp))
                        continue;

                    unsigned left, right, top, bottom;
                    search.getProjectedRadiusSearchBox (sp, squared_radius, left, right, top, bottom);

                    if (radius_scale != 1.f)
                    {
                        left =  i - (i - left) * radius_scale;
                        right = i + (right - i) * radius_scale;
                        top =  j - (j - top) * radius_scale;
                        bottom = j + (bottom - j) * radius_scale;
                    }

                    for(int y = top; y < bottom + 1; ++y)
                    {
                        const uchar *sl1  = src_labels.ptr<uchar>(y);
                        const int   *dl1 = dst_labels.ptr<int>(y);
                        const pcl::PointXYZRGB* sp1 = &cloud.at(0, y);

                        for(int x = left; x < right + 1; ++x)
                        {
                            int cc1 = dl1[x];
                            // not the same cc, the same label, and in radius
                            if (cc1 != cc && sl1[x] == sl && sqnorm(sp, sp1[x]) <= squared_radius)
                            {
                                while(cc1 != dj[cc1])
                                    cc1 = dj[cc1];

                                dj[cc1] = root;
                            }
                        }
                    }
                }
            }
#ifdef HT_USE_OMP
        }
#endif

        for(int j = 0; j < sz.height; ++j)
        {
            int *dl = dst_labels.ptr<int>(j);

            for(int i = 0; i < sz.width; ++i)
            {
                int cc = dl[i];
                if (cc == -1)
                    continue;

                while(cc != dj[cc]) //disjoint set
                    cc = dj[cc];
                dl[i] = cc;
                rsizes[cc]++;
            }
        }

    } /* if (brute_force_border)*/

    //convert to output format
    labeled_clusters.clear();
    labeled_clusters.resize(num_parts);

    vector<int> remap(sz.area(), -1);

    for(int j = 0; j < sz.height; ++j)
    {
        const uchar *sl = src_labels.ptr<uchar>(j);
        const int   *dl = dst_labels.ptr<int>(j);

        for(int i = 0; i < sz.width; ++i)
        {
            int part = sl[i];
            int cc   = dl[i];

            if (cc == -1)
                continue;

            if (min_pts_per_cluster <= rsizes[cc] && rsizes[cc] <= max_pts_per_cluster)
            {

                int ccindex = remap[cc];
                if (ccindex == -1)
                {
                    ccindex = (int)labeled_clusters[part].size();
                    labeled_clusters[part].resize(ccindex + 1);
                    remap[cc] = ccindex;
                }

                labeled_clusters[part][ccindex].indices.push_back(j*sz.width + i);
            }
        }
    }
}
Exemple #7
0
/**
* read data
*/
bool KGDAL2CV::readData(cv::Mat img){
	// make sure the image is the proper size
	if (img.size().height != m_height){
		return false;
	}
	if (img.size().width != m_width){
		return false;
	}

	// make sure the raster is alive
	if (m_dataset == NULL || m_driver == NULL){
		return false;
	}

	// set the image to zero
	img = 0;

	// iterate over each raster band
	// note that OpenCV does bgr rather than rgb
	int nChannels = m_dataset->GetRasterCount();

	GDALColorTable* gdalColorTable = NULL;
	if (m_dataset->GetRasterBand(1)->GetColorTable() != NULL){
		gdalColorTable = m_dataset->GetRasterBand(1)->GetColorTable();
	}

	const GDALDataType gdalType = m_dataset->GetRasterBand(1)->GetRasterDataType();
	int nRows, nCols;

	//if (nChannels > img.channels()){
	//	nChannels = img.channels();
	//}

	for (int c = 0; c < img.channels(); c++){

		int realBandIndex = c;
		
		// get the GDAL Band
		GDALRasterBand* band = m_dataset->GetRasterBand(c + 1);

		//if (hasColorTable == false){
		if (GCI_RedBand == band->GetColorInterpretation()) realBandIndex = 2;
		if (GCI_GreenBand == band->GetColorInterpretation()) realBandIndex = 1;
		if (GCI_BlueBand == band->GetColorInterpretation()) realBandIndex = 0;
		//}
		if (hasColorTable && gdalColorTable->GetPaletteInterpretation() == GPI_RGB) c = img.channels() - 1;
		// make sure the image band has the same dimensions as the image
		if (band->GetXSize() != m_width || band->GetYSize() != m_height){ return false; }

		// grab the raster size
		nRows = band->GetYSize();
		nCols = band->GetXSize();

		// create a temporary scanline pointer to store data
		double* scanline = new double[nCols];

		// iterate over each row and column
		for (int y = 0; y<nRows; y++){

			// get the entire row
			band->RasterIO(GF_Read, 0, y, nCols, 1, scanline, nCols, 1, GDT_Float64, 0, 0);

			// set inside the image
			for (int x = 0; x<nCols; x++){

				// set depending on image types
				// given boost, I would use enable_if to speed up.  Avoid for now.
				if (hasColorTable == false){
					write_pixel(scanline[x], gdalType, nChannels, img, y, x, realBandIndex);
				}
				else{
					write_ctable_pixel(scanline[x], gdalType, gdalColorTable, img, y, x, c);
				}
			}
		}
		// delete our temp pointer
		delete[] scanline;
	}

	return true;
}
Exemple #8
0
void T2FMRF_UM::process(const cv::Mat &img_input, cv::Mat &img_output, cv::Mat &img_bgmodel)
{
  if(img_input.empty())
    return;

  loadConfig();

  if(firstTime)
    saveConfig();

  frame = new IplImage(img_input);
  
  if(firstTime)
    frame_data.ReleaseMemory(false);
  frame_data = frame;

  if(firstTime)
  {
    int width	= img_input.size().width;
    int height = img_input.size().height;

    lowThresholdMask = cvCreateImage(cvSize(width, height), IPL_DEPTH_8U, 1);
    lowThresholdMask.Ptr()->origin = IPL_ORIGIN_BL;

    highThresholdMask = cvCreateImage(cvSize(width, height), IPL_DEPTH_8U, 1);
    highThresholdMask.Ptr()->origin = IPL_ORIGIN_BL;

    params.SetFrameSize(width, height);
    params.LowThreshold() = threshold;
    params.HighThreshold() = 2*params.LowThreshold();
    params.Alpha() = alpha;
    params.MaxModes() = gaussians;
    params.Type() = TYPE_T2FMRF_UM;
    params.KM() = km; // Factor control for the T2FMRF-UM [0,3] default: 2
    params.KV() = kv; // Factor control for the T2FMRF-UV [0.3,1] default: 0.9

    bgs.Initalize(params);
    bgs.InitModel(frame_data);

    old_labeling = cvCreateImage(cvSize(width, height), IPL_DEPTH_8U, 1);
    old = cvCreateImage(cvSize(width, height), IPL_DEPTH_8U, 1);

    mrf.height = height;
	  mrf.width = width;
    mrf.Build_Classes_OldLabeling_InImage_LocalEnergy();

    firstTime = false;
  }

  bgs.Subtract(frameNumber, frame_data, lowThresholdMask, highThresholdMask);
  cvCopy(lowThresholdMask.Ptr(), old);

  /************************************************************************/
	/* the code for MRF, it can be noted when using other methods   */
	/************************************************************************/
	//the optimization process is done when the foreground detection is stable, 
	if(frameNumber >= 10)
	{
		gmm = bgs.gmm();
		hmm = bgs.hmm();
		mrf.background2 = frame_data.Ptr();
		mrf.in_image = lowThresholdMask.Ptr();
		mrf.out_image = lowThresholdMask.Ptr();
		mrf.InitEvidence2(gmm,hmm,old_labeling);
		mrf.ICM2();
		cvCopy(mrf.out_image, lowThresholdMask.Ptr());
	}

  cvCopy(old, old_labeling);

  lowThresholdMask.Clear();
  bgs.Update(frameNumber, frame_data, lowThresholdMask);
  
  cv::Mat foreground(highThresholdMask.Ptr());

  if(showOutput)
    cv::imshow("T2FMRF-UM", foreground);
  
  foreground.copyTo(img_output);

  delete frame;
  frameNumber++;
}
//buil the VOR once(weighted vor diagram?)
void CVT::vor(cv::Mat &  img)
{
	cv::Mat dist(img.size(), CV_32F, cv::Scalar::all(FLT_MAX)); //an image with infinity distance
	cv::Mat root(img.size(), CV_16U, cv::Scalar::all(USHRT_MAX)); //an image of root index
	cv::Mat visited(img.size(), CV_8U, cv::Scalar::all(0)); //all unvisited


	//init
	std::vector< std::pair<float, cv::Point> > open;
	ushort site_id = 0;
	for (auto& c : this->cells)
	{
		if (debug)
		{
			if (c.site.x<0 || c.site.x>img.size().height)
				std::cout << "! Warning: c.site.x=" << c.site.x << std::endl;

			if (c.site.y<0 || c.site.y>img.size().width)
				std::cout << "! Warning: c.site.y=" << c.site.y << std::endl;
		}


		cv::Point pix((int)c.site.x, (int)c.site.y);
		float d = color2dist(img, pix);
		//dist에 2D에서의 color value저장. dist 가 크다 <-> 흰색에 가깝다.
		dist.at<float>(pix.x, pix.y) = d;
		root.at<ushort>(pix.x, pix.y) = site_id++;
		open.push_back( std::make_pair(d, pix) );//push
		c.coverage.clear();
	}
	
	std::make_heap(open.begin(), open.end(), compareCell);

	//propagate
	while (open.empty() == false)
	{
		//
		std::pop_heap(open.begin(), open.end(), compareCell);//pop, 저장된 color value와 point pair를 꺼낸다.
		auto cell = open.back();
		auto& cpos = cell.second;
		open.pop_back();

		//check if the distance from this cell is already updated
		if (cell.first > dist.at<float>(cpos.x, cpos.y)) continue;
		if (visited.at<uchar>(cpos.x, cpos.y) > 0) continue; //visited
		visited.at<uchar>(cpos.x, cpos.y) = 1;

		//check the neighbors
		for (int dx =-1; dx <= 1; dx++) //x is row
		{
			int x = cpos.x + dx;
			if (x < 0 || x >= img.size().height) continue;
			for (int dy = -1; dy <= 1; dy++) //y is column
			{
				if (dx == 0 && dy == 0) continue; //itself...

				int y = cpos.y + dy;
				if (y < 0 || y >= img.size().width) continue;
				float newd = dist.at<float>(cpos.x, cpos.y) + color2dist(img, cv::Point(x, y));
				float oldd = dist.at<float>(x, y);

				if (newd < oldd)
				{
					dist.at<float>(x, y)=newd;
					root.at<ushort>(x, y) = root.at<ushort>(cpos.x, cpos.y);
					open.push_back(std::make_pair(newd, cv::Point(x, y)));
					std::push_heap(open.begin(), open.end(), compareCell);
				}
			}//end for dy
		}//end for dx
	}//end while

	//collect cells
	for (int x = 0; x < img.size().height; x++)
	{
		for (int y = 0; y < img.size().width; y++)
		{
			ushort rootid = root.at<ushort>(x, y);
			this->cells[rootid].coverage.push_back(cv::Point(x,y));
		}//end y
	}//end x

	//remove empty cells...CVT할때를 위해
	int cvt_size = this->cells.size();
	for (int i = 0; i < cvt_size; i++)
	{
		if (this->cells[i].coverage.empty())
		{
			this->cells[i] = this->cells.back();
			this->cells.pop_back();
			i--;
			cvt_size--;
		}
	}//end for i

	if (debug)
	{
		//this shows the progress...
		double min;
		double max;
		cv::minMaxIdx(dist, &min, &max);
		cv::Mat adjMap;
		cv::convertScaleAbs(dist, adjMap, 255 / max);
		//cv::applyColorMap(adjMap, adjMap, cv::COLORMAP_JET);
	
		for (auto& c : this->cells)
		{
			cv::circle(adjMap, cv::Point(c.site.y, c.site.x), 2, CV_RGB(0, 0, 255), -1);
		}
	
		cv::imshow("CVT", adjMap);
		cv::waitKey(5);
	}
}
Exemple #10
0
/*!
 *
 */
int main(int argc,char **argv)
{
  readArguments(argc,argv);

  aruco::BoardConfiguration boardConf;
  boardConf.readFromFile(boC->sval[0]);

  cv::VideoCapture vcapture;

  //read from camera or from  file
  if (strcmp(inp->sval[0], "live")==0)
  {
    vcapture.open(0);
    vcapture.set(CV_CAP_PROP_FRAME_WIDTH,  wid->ival[0]);
    vcapture.set(CV_CAP_PROP_FRAME_HEIGHT, hei->ival[0]);
    int val = CV_FOURCC('M', 'P', 'E', 'G');
    vcapture.set(CV_CAP_PROP_FOURCC, val);
  }
  else
    vcapture.open(inp->sval[0]);

  //check video is open
  if (!vcapture.isOpened())
  {
    std::cerr<<"Could not open video"<<std::endl;
    return -1;
  }

  //read first image to get the dimensions
  vcapture>>inpImg;

  //Open outputvideo
  cv::VideoWriter vWriter;
  if (out->count)
    vWriter.open(out->sval[0], CV_FOURCC('M','J','P','G'), fps->ival[0], inpImg.size());

  //read camera parameters if passed
  if (intr->count)
  {
    params.readFromXMLFile(intr->sval[0]);
    params.resize(inpImg.size());
  }

  //Create gui
  cv::namedWindow("thres", CV_GUI_NORMAL | CV_WINDOW_AUTOSIZE);
  cv::namedWindow("in", CV_GUI_NORMAL | CV_WINDOW_AUTOSIZE);
  cvMoveWindow("thres", 0, 0);
  cvMoveWindow("in", inpImg.cols + 5, 0);
  boardDetector.setParams(boardConf,params,msiz->dval[0]);
  boardDetector.getMarkerDetector().getThresholdParams( dThresParam1,dThresParam2);
//  boardDetector.getMarkerDetector().enableErosion(true);//for chessboards
  iThresParam1=dThresParam1;
  iThresParam2=dThresParam2;
  cv::createTrackbar("ThresParam1", "in",&iThresParam1, 13, cvTrackBarEvents);
  cv::createTrackbar("ThresParam2", "in",&iThresParam2, 13, cvTrackBarEvents);
  int index=0;

  ///////////////
  // Main Lopp //
  ///////////////
  while(appRunnin)
  {
    inpImg.copyTo(inpImgCpy);
    index++;                                    //number of images captured
    double tick = static_cast<double>(cv::getTickCount());//for checking the speed
    float probDetect=boardDetector.detect(inpImg); //Detection of the board
    tick=(static_cast<double>(cv::getTickCount())-tick)/cv::getTickFrequency();
    std::cout<<"Time detection="<<1000*tick<<" milliseconds"<<std::endl;
    //print marker borders
    for (unsigned int i=0; i<boardDetector.getDetectedMarkers().size(); i++)
      boardDetector.getDetectedMarkers()[i].draw(inpImgCpy,cv::Scalar(0,0,255),1);

    //print board
    if (params.isValid())
    {
      if ( probDetect>thre->dval[0])
      {
        aruco::CvDrawingUtils::draw3dAxis( inpImgCpy,boardDetector.getDetectedBoard(),params);
      }
    }
    //DONE! Easy, right?

    //show input with augmented information and the thresholded image
    cv::imshow("in",inpImgCpy);
    cv::imshow("thres",boardDetector.getMarkerDetector().getThresholdedImage());

    // write to video if desired
    if (out->count)
    {
      //create a beautiful composed image showing the thresholded
      //first create a small version of the thresholded image
      cv::Mat smallThres;
      cv::resize(boardDetector.getMarkerDetector().getThresholdedImage(),smallThres,
                 cv::Size(inpImgCpy.cols/3,inpImgCpy.rows/3));
      cv::Mat small3C;
      cv::cvtColor(smallThres,small3C,CV_GRAY2BGR);
      cv::Mat roi=inpImgCpy(cv::Rect(0,0,inpImgCpy.cols/3,inpImgCpy.rows/3));
      small3C.copyTo(roi);
      vWriter<<inpImgCpy;
    }

    processKey(cv::waitKey(waitTime));//wait for key to be pressed
    appRunnin &= vcapture.grab();
    vcapture.retrieve(inpImg);
  }

  arg_freetable(argtable,sizeof(argtable)/sizeof(argtable[0]));
  return EXIT_SUCCESS;
}
bool Segmentation::Ransac_for_buildings(float dem_spacing, double ransac_threshold, cv::Mat original_tiles_merged)
{   
	StepResource pStep("Computing RANSAC on all the identified buildings", "app", "a2beb9b8-218e-11e4-969b-b2227cce2b54");
	   
	ProgressResource pResource("ProgressBar");
	Progress *pProgress = pResource.get(); 
	pProgress-> setSettingAutoClose(true);
	pProgress->updateProgress("Computing RANSAC on all buildings", 0, NORMAL);
	Ransac_buildings = Ransac(Segmentation::path);
	cv::Mat roof_image = cv::Mat::zeros(original_tiles_merged.size(), CV_8UC3);
		
	buildingS.resize(blobs.size());
	buildingS_inliers.resize(blobs.size());
	buildingS_outliers.resize(blobs.size());
	buildingS_plane_coefficients.resize(blobs.size());
	buldingS_number_inliers.resize(blobs.size());

	std::ofstream building_file;
	std::ofstream cont_file;
	cont_file.open (std::string(path) + "/Results/Number_of_RANSAC_applications.txt"); 
	for(int i = 0; i < blobs.size(); i++)
	{// i index is the building (blob) index
		pProgress->updateProgress("Computing RANSAC on all buildings\nBuilding "+ StringUtilities::toDisplayString(i) + " on "+ StringUtilities::toDisplayString(blobs.size()), static_cast<double>(static_cast<double>(i)/blobs.size()*100), NORMAL);
		building_file.open (std::string(path) + "/Results/Building_" + StringUtilities::toDisplayString(i)+".txt");
		building_file << 'i' << '\t' << 'j' << '\t' << 'X' << '\t' << 'Y' << '\t' << 'Z' << '\n'; 
		buildingS[i].setConstant(blobs[i].size(), 3, 0.0);
		
		// the j loop retrieves the  X, Y, Z coordinate for each pixel of all the buildings
		for(int j = 0; j < blobs[i].size(); j++) 
		{// j index is the pixel index for the single building
		 // loop on all the pixel of the SINGLE building
		    int pixel_column = blobs[i][j].x;
            int pixel_row = blobs[i][j].y;			

			double x_building =  pixel_column * dem_spacing;// xMin + pixel_column * dem_spacing // object coordinate 
			double y_building =  pixel_row * dem_spacing;// yMin + pixel_row * dem_spacing // object coordinate
			double z_building = original_tiles_merged.at<float>(pixel_row, pixel_column);//object coordinate
			
			buildingS[i](j,0) = x_building;
			buildingS[i](j,1) = y_building;
			buildingS[i](j,2) = z_building;
			 
			building_file << pixel_row+1 <<  '\t' << pixel_column+1 <<  '\t' << buildingS[i](j,0) << '\t' << buildingS[i](j,1) << '\t' << buildingS[i](j,2) << '\n'; //+1 on the imae coordinates to verify with opticks' rasters (origin is 1,1)
		}

		building_file.close();

		std::ofstream inliers_file;
		std::ofstream parameters_file;
		inliers_file.open (std::string(path) + "/Results/Inliers_building_" + StringUtilities::toDisplayString(i)+".txt");
		parameters_file.open (std::string(path) + "/Results/plane_parameters_building_" + StringUtilities::toDisplayString(i)+".txt");
		//parameters_file << "a\tb\tc\td\tmean_dist\tstd_dist\n";
		int cont = 0;
		Ransac_buildings.ransac_msg += "\n____________Building number " + StringUtilities::toDisplayString(i) +"____________\n";
		Ransac_buildings.ransac_msg += "\nITERATION NUMBER " + StringUtilities::toDisplayString(cont) +"\n";
		Ransac_buildings.ComputeModel(buildingS[i], ransac_threshold);
		
		buldingS_number_inliers[i]= Ransac_buildings.n_best_inliers_count;
		buildingS_inliers[i] = Ransac_buildings.final_inliers;
		buildingS_outliers[i] = Ransac_buildings.final_outliers;
		buildingS_plane_coefficients[i] = Ransac_buildings.final_model_coefficients;
		double inliers_percentage = static_cast<double>( (Ransac_buildings.n_best_inliers_count) ) / static_cast<double> (buildingS[i].rows());
		int inliers_so_far = Ransac_buildings.n_best_inliers_count;
		std::vector<int> old_final_outliers = Ransac_buildings.final_outliers;
		 

		// DRAWS THE ROOFS yellow
		for (int k = 0; k < Ransac_buildings.n_best_inliers_count; k++)
		{
			int pixel_row = static_cast<int>(buildingS[i](Ransac_buildings.final_inliers[k], 1) /  dem_spacing);
            int pixel_column = static_cast<int>(buildingS[i](Ransac_buildings.final_inliers[k], 0) /  dem_spacing);

			unsigned char r = 255;// unsigned char(255 * (rand()/(1.0 + RAND_MAX)));
            unsigned char g = 255;// unsigned char(255 * (rand()/(1.0 + RAND_MAX)));
            unsigned char b = 0;//unsigned char(255 * (rand()/(1.0 + RAND_MAX)));
		
			roof_image.at<cv::Vec3b>(pixel_row, pixel_column)[0] = b;
            roof_image.at<cv::Vec3b>(pixel_row, pixel_column)[1] = g;
            roof_image.at<cv::Vec3b>(pixel_row, pixel_column)[2] = r;
		}

		while (inliers_percentage < 0.90)
		{
			cont ++;
			Ransac_buildings.ransac_msg += "\nITERATION NUMBER " + StringUtilities::toDisplayString(cont) +"\n";
			Eigen::Matrix<double,  Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> building_outliers;
			building_outliers.setConstant(buildingS[i].rows() - inliers_so_far, 3, 0.0);
			
		   	//* forse il metodo va già bene così, perchè riempio la matrice deglio outlier in maniera ordinata,
			//* solo che gli indici degli inlier/outlier non sono più indicativi rispetto alla matrice di building originale, ma rispetto alla matrice di innput
			//* devo riporatre gli ID degli indici alla loro posizione originale
			for (int w = 0; w <building_outliers.rows(); w++)
			{
				building_outliers(w, 0) = buildingS[i](old_final_outliers[w], 0);
			    building_outliers(w, 1) = buildingS[i](old_final_outliers[w], 1);
			    building_outliers(w, 2) = buildingS[i](old_final_outliers[w], 2);
				
				//Ransac_buildings.ransac_msg += "\n" + StringUtilities::toDisplayString(pixel_row+1) + "\t" + StringUtilities::toDisplayString(pixel_column+1) + "\t" + StringUtilities::toDisplayString(final_outliers[w]) + "\t" + StringUtilities::toDisplayString(building_outliers(w, 0))+ "\t"+ StringUtilities::toDisplayString(building_outliers(w, 1)) + "\t" + StringUtilities::toDisplayString(building_outliers(w, 2))+"\n"; // needed for tesing (test passed at first iteration)
			}
			

			Ransac_buildings.ransac_msg += "\n";
			//Ransac_buildings.ransac_msg += "\nprova "+ StringUtilities::toDisplayString(inliers_percentage*100)+"\n";
			Ransac_buildings.ComputeModel(building_outliers, ransac_threshold);
			//inliers_percentage = inliers_percentage + static_cast<double>( (n_best_inliers_count) ) / static_cast<double> (building_outliers.rows());
			inliers_percentage = inliers_percentage + static_cast<double>( (Ransac_buildings.n_best_inliers_count) ) / static_cast<double> (buildingS[i].rows());

			Ransac_buildings.ransac_msg += "\nINLIERS IN RELATION TO GLOBAL INDEX ("+ StringUtilities::toDisplayString(Ransac_buildings.n_best_inliers_count) + ")\n";
	        for(size_t i = 0; i < Ransac_buildings.n_best_inliers_count; i++)
	        {
		       Ransac_buildings.ransac_msg +=  StringUtilities::toDisplayString(old_final_outliers[Ransac_buildings.final_inliers[i]])+" ";
			   inliers_file << old_final_outliers[Ransac_buildings.final_inliers[i]] << "\t";
	        }
			Ransac_buildings.ransac_msg += "\n";
			inliers_file << "\n";

			//old_final_outliers.resize(building_outliers.rows() - Ransac_buildings.n_best_inliers_count);
			Ransac_buildings.ransac_msg += "\nOUTLIERS IN RELATION TO GLOBAL INDEX("+ StringUtilities::toDisplayString(building_outliers.rows() - Ransac_buildings.n_best_inliers_count) + ")\n";
			for(size_t i = 0; i < building_outliers.rows() - Ransac_buildings.n_best_inliers_count; i++)
			{
				Ransac_buildings.ransac_msg +=  StringUtilities::toDisplayString(old_final_outliers[Ransac_buildings.final_outliers[i]])+" ";
				old_final_outliers[i] = old_final_outliers[Ransac_buildings.final_outliers[i]];// in this way I refer the outliers indexes to the global indexes (those referred to the original eigen matrix)
			}
			
			//parameters_file << Ransac_buildings.final_model_coefficients[0] << "\t" << Ransac_buildings.final_model_coefficients[1] << "\t" << Ransac_buildings.final_model_coefficients[2] << "\t" << Ransac_buildings.final_model_coefficients[3] << "\t" << Ransac_buildings.mean_distances << "\t"<< Ransac_buildings.std_distances << "\n";
			parameters_file << Ransac_buildings.final_model_coefficients[0] << "\t" << Ransac_buildings.final_model_coefficients[1] << "\t" << Ransac_buildings.final_model_coefficients[2] << "\t" << Ransac_buildings.final_model_coefficients[3] << "\n";


			if (cont == 1)
			{
				// DRAWS THE ROOFS blue
			   for (int k = 0; k < Ransac_buildings.n_best_inliers_count; k++)
			   {
					int pixel_row = static_cast<int>(buildingS[i](old_final_outliers[Ransac_buildings.final_inliers[k]], 1) /  dem_spacing);
					int pixel_column = static_cast<int>(buildingS[i](old_final_outliers[Ransac_buildings.final_inliers[k]], 0) /  dem_spacing);

					unsigned char r = 0;// unsigned char(255 * (rand()/(1.0 + RAND_MAX)));
					unsigned char g = 0;// unsigned char(255 * (rand()/(1.0 + RAND_MAX)));
					unsigned char b = 255;//unsigned char(255 * (rand()/(1.0 + RAND_MAX)));
		
					roof_image.at<cv::Vec3b>(pixel_row, pixel_column)[0] = b;
					roof_image.at<cv::Vec3b>(pixel_row, pixel_column)[1] = g;
					roof_image.at<cv::Vec3b>(pixel_row, pixel_column)[2] = r;
				}
			}

			if (cont ==2)
			{
				// DRAWS THE ROOFS green
			   for (int k = 0; k < Ransac_buildings.n_best_inliers_count; k++)
			   {
					int pixel_row = static_cast<int>(buildingS[i](old_final_outliers[Ransac_buildings.final_inliers[k]], 1) /  dem_spacing);
					int pixel_column = static_cast<int>(buildingS[i](old_final_outliers[Ransac_buildings.final_inliers[k]], 0) /  dem_spacing);

					unsigned char r = 0;// unsigned char(255 * (rand()/(1.0 + RAND_MAX)));
					unsigned char g = 255;// unsigned char(255 * (rand()/(1.0 + RAND_MAX)));
					unsigned char b = 0;//unsigned char(255 * (rand()/(1.0 + RAND_MAX)));
		
					roof_image.at<cv::Vec3b>(pixel_row, pixel_column)[0] = b;
					roof_image.at<cv::Vec3b>(pixel_row, pixel_column)[1] = g;
					roof_image.at<cv::Vec3b>(pixel_row, pixel_column)[2] = r;
				}
			}

			if (cont ==3)
			{
				// DRAWS THE ROOFS brown
			   for (int k = 0; k < Ransac_buildings.n_best_inliers_count; k++)
			   {
					int pixel_row = static_cast<int>(buildingS[i](old_final_outliers[Ransac_buildings.final_inliers[k]], 1) /  dem_spacing);
					int pixel_column = static_cast<int>(buildingS[i](old_final_outliers[Ransac_buildings.final_inliers[k]], 0) /  dem_spacing);

					unsigned char r = 128;// unsigned char(255 * (rand()/(1.0 + RAND_MAX)));
					unsigned char g = 0;// unsigned char(255 * (rand()/(1.0 + RAND_MAX)));
					unsigned char b = 0;//unsigned char(255 * (rand()/(1.0 + RAND_MAX)));
		
					roof_image.at<cv::Vec3b>(pixel_row, pixel_column)[0] = b;
					roof_image.at<cv::Vec3b>(pixel_row, pixel_column)[1] = g;
					roof_image.at<cv::Vec3b>(pixel_row, pixel_column)[2] = r;
				}
			}

			//if (cont == 4)
			//{
			//	// DRAWS THE ROOFS white
			//   for (int k = 0; k < Ransac_buildings.n_best_inliers_count; k++)
			//   {
			//		int pixel_row = static_cast<int>(buildingS[i](old_final_outliers[Ransac_buildings.final_inliers[k]], 1) /  dem_spacing);
			//		int pixel_column = static_cast<int>(buildingS[i](old_final_outliers[Ransac_buildings.final_inliers[k]], 0) /  dem_spacing);

			//		unsigned char r = 255;// unsigned char(255 * (rand()/(1.0 + RAND_MAX)));
			//		unsigned char g = 255;// unsigned char(255 * (rand()/(1.0 + RAND_MAX)));
			//		unsigned char b = 255;//unsigned char(255 * (rand()/(1.0 + RAND_MAX)));
		
			//		roof_image.at<cv::Vec3b>(pixel_row, pixel_column)[0] = b;
			//		roof_image.at<cv::Vec3b>(pixel_row, pixel_column)[1] = g;
			//		roof_image.at<cv::Vec3b>(pixel_row, pixel_column)[2] = r;
			//	}
			//}


			Ransac_buildings.ransac_msg += "\n"; 
			inliers_so_far += Ransac_buildings.n_best_inliers_count;  
			
		    
		}// fine while
		Ransac_buildings.ransac_msg += "__________________________________________________________________\n";
		//boh_file.close();

		cont_file << i << "\t" << cont << "\n";
	}
	building_file.close();
	cont_file.close();
	cv::imshow("roofs", roof_image);
	
	cv::imwrite(path + "/Results/building_roofs.png", roof_image);
	cv::waitKey(0);
	
	pProgress->updateProgress("All buildings have been processed with RANSAC.", 100, NORMAL);
    pStep->finalize();
	return true;
}
Exemple #12
0
cv::Mat get_bboxes_(const cv::Mat & image, const cv::Mat & seg, int x1, int y1, int x2, int y2) {
	double max_id_;
	cv::minMaxIdx(seg, nullptr, &max_id_);
	int max_id = max_id_;

	std::vector<std::shared_ptr<Segment>> segments;
	segments.reserve(max_id);
	int nchannels = image.channels();
	cv::Size size = image.size();
	for (int i = 0; i <= max_id; i++) {
		segments.push_back(std::make_shared<UniformSegment>(i, size));
	}

	{
		AdjacencyMatrix adjacency(max_id + 1);
		for (int i = 0; i < image.rows; i++) {
			for (int j = 0; j < image.cols; j++) {
				cv::Point p(j, i);
				uint16_t id = seg.at<uint16_t>(p);
				segments[id]->addPoint(image, p);

				if (i < image.rows - 1) {
					uint16_t n = seg.at<uint16_t>(i+1, j);
					if (n != id && adjacency.get(id, n) == false) {
						adjacency.get(id, n) = true;
						segments[id]->addNeighbour(n);
						segments[n]->addNeighbour(id);
					}
				}

				if (j < image.cols - 1) {
					uint16_t n = seg.at<uint16_t>(i, j+1);
					if (n != id && adjacency.get(id, n) == false) {
						adjacency.get(id, n) = true;
						segments[id]->addNeighbour(n);
						segments[n]->addNeighbour(id);
					}
				}
			}
		}
	}

	cv::Mat bboxes_out;
	float similarity_sum = 0.f;
	for (auto & s: segments) {
		s->finalizeSetup();
	}

	//for (uint32_t i = 0; i < bboxes.rows; i++) {
		//cv::Rect truth(cv::Point(bboxes.at<int>(i, 0), bboxes.at<int>(i, 1)), cv::Point(bboxes.at<int>(i, 2), bboxes.at<int>(i, 3)));
		cv::Rect truth(cv::Point(x1, y1), cv::Point(x2, y2));
		cv::Mat mask = cv::Mat::zeros(image.size(), CV_8UC1);
		cv::rectangle(mask, truth, cv::Scalar(1), CV_FILLED);

		std::unordered_set<uint32_t> contained_segments;
		std::unordered_set<uint32_t> crossing_segments;
		for (const auto & s: segments) {
			cv::Mat masked;
			mask.copyTo(masked, s->mask);
			int count = cv::countNonZero(masked);
			if (count == s->size)
				contained_segments.insert(s->id);
			else if (count)
				crossing_segments.insert(s->id);
		}

		std::shared_ptr<Segment> s = std::make_shared<UniformSegment>(-1, image.size());
		if (contained_segments.size()) {
			for (const int & id: contained_segments) {
				s = s->merge(segments[id].get());
			}
		} else {
			s = s->merge(segments[seg.at<uint16_t>(truth.y + truth.height / 2, truth.x + truth.width)].get());
		}

		std::cout << "size: " << crossing_segments.size() << std::endl;

#ifdef DEBUG
		cv::Mat draw;
		s->mask.copyTo(draw);
		draw *= 255;
		/*for (const int & n: s->neighbours) {
			draw += segments[n]->mask * 127;
		}*/
		//cv::rectangle(draw, cv::Point(bboxes.at<int>(0, 0), bboxes.at<int>(0, 1)), cv::Point(bboxes.at<int>(0, 2), bboxes.at<int>(0, 3)), cv::Scalar(255));
		cv::namedWindow("Mask", cv::WINDOW_NORMAL);
		cv::imshow("Mask", draw);
		cv::waitKey();
#endif

		float max_sim = jaccardSimilarity(truth, cv::Rect(s->min_p, s->max_p));
#ifdef DEBUG
		std::cout << "max_sim: " << max_sim << std::endl;
#endif
		bool quit = false;
		while (quit == false) {
			quit = true;
			for (const int & n: s->neighbours) {
				std::shared_ptr<Segment> s2 = s->merge(segments[n].get());
				float sim = jaccardSimilarity(truth, cv::Rect(s2->min_p, s2->max_p));
				if (sim > max_sim) {
					s = s2;
					max_sim = sim;
					quit = false;
#ifdef DEBUG
					std::cout << "new max sim: " << max_sim << std::endl;
					cv::imshow("Mask", s->mask * 255);
					cv::waitKey();
#endif
					break;
				}
			}
		}

		cv::Mat bbox = cv::Mat(1, 4, CV_32SC1);
		bbox.at<int>(0) = s->min_p.x;
		bbox.at<int>(1) = s->min_p.y;
		bbox.at<int>(2) = s->max_p.x;
		bbox.at<int>(3) = s->max_p.y;
		if (bboxes_out.empty())
			bboxes_out = bbox;
		else
			cv::vconcat(bboxes_out, bbox, bboxes_out);
	//}

#ifdef DEBUG
	std::cout << "bboxes: " << bboxes_out << std::endl;
#endif

	return bboxes_out;
}
void MapperGradAffine::calculate(
    const cv::Mat& img1, const cv::Mat& image2, cv::Ptr<Map>& res) const
{
    Mat gradx, grady, imgDiff;
    Mat img2;

    CV_DbgAssert(img1.size() == image2.size());
    CV_DbgAssert(img1.channels() == image2.channels());
    CV_DbgAssert(img1.channels() == 1 || img1.channels() == 3);

    if(!res.empty()) {
        // We have initial values for the registration: we move img2 to that initial reference
        res->inverseWarp(image2, img2);
    } else {
        img2 = image2;
    }

    // Get gradient in all channels
    gradient(img1, img2, gradx, grady, imgDiff);

    // Matrices with reference frame coordinates
    Mat grid_r, grid_c;
    grid(img1, grid_r, grid_c);

    // Calculate parameters using least squares
    Matx<double, 6, 6> A;
    Vec<double, 6> b;
    // For each value in A, all the matrix elements are added and then the channels are also added,
    // so we have two calls to "sum". The result can be found in the first element of the final
    // Scalar object.
    Mat xIx = grid_c.mul(gradx);
    Mat xIy = grid_c.mul(grady);
    Mat yIx = grid_r.mul(gradx);
    Mat yIy = grid_r.mul(grady);
    Mat Ix2 = gradx.mul(gradx);
    Mat Iy2 = grady.mul(grady);
    Mat xy = grid_c.mul(grid_r);
    Mat IxIy = gradx.mul(grady);
    A(0, 0) = sum(sum(sqr(xIx)))[0];
    A(0, 1) = sum(sum(xy.mul(Ix2)))[0];
    A(0, 2) = sum(sum(grid_c.mul(Ix2)))[0];
    A(0, 3) = sum(sum(sqr(grid_c).mul(IxIy)))[0];
    A(0, 4) = sum(sum(xy.mul(IxIy)))[0];
    A(0, 5) = sum(sum(grid_c.mul(IxIy)))[0];
    A(1, 1) = sum(sum(sqr(yIx)))[0];
    A(1, 2) = sum(sum(grid_r.mul(Ix2)))[0];
    A(1, 3) = A(0, 4);
    A(1, 4) = sum(sum(sqr(grid_r).mul(IxIy)))[0];
    A(1, 5) = sum(sum(grid_r.mul(IxIy)))[0];
    A(2, 2) = sum(sum(Ix2))[0];
    A(2, 3) = A(0, 5);
    A(2, 4) = A(1, 5);
    A(2, 5) = sum(sum(IxIy))[0];
    A(3, 3) = sum(sum(sqr(xIy)))[0];
    A(3, 4) = sum(sum(xy.mul(Iy2)))[0];
    A(3, 5) = sum(sum(grid_c.mul(Iy2)))[0];
    A(4, 4) = sum(sum(sqr(yIy)))[0];
    A(4, 5) = sum(sum(grid_r.mul(Iy2)))[0];
    A(5, 5) = sum(sum(Iy2))[0];
    // Lower half values (A is symmetric)
    A(1, 0) = A(0, 1);
    A(2, 0) = A(0, 2);
    A(2, 1) = A(1, 2);
    A(3, 0) = A(0, 3);
    A(3, 1) = A(1, 3);
    A(3, 2) = A(2, 3);
    A(4, 0) = A(0, 4);
    A(4, 1) = A(1, 4);
    A(4, 2) = A(2, 4);
    A(4, 3) = A(3, 4);
    A(5, 0) = A(0, 5);
    A(5, 1) = A(1, 5);
    A(5, 2) = A(2, 5);
    A(5, 3) = A(3, 5);
    A(5, 4) = A(4, 5);

    // Calculation of b
    b(0) = -sum(sum(imgDiff.mul(xIx)))[0];
    b(1) = -sum(sum(imgDiff.mul(yIx)))[0];
    b(2) = -sum(sum(imgDiff.mul(gradx)))[0];
    b(3) = -sum(sum(imgDiff.mul(xIy)))[0];
    b(4) = -sum(sum(imgDiff.mul(yIy)))[0];
    b(5) = -sum(sum(imgDiff.mul(grady)))[0];

    // Calculate affine transformation. We use Cholesky decomposition, as A is symmetric.
    Vec<double, 6> k = A.inv(DECOMP_CHOLESKY)*b;

    Matx<double, 2, 2> linTr(k(0) + 1., k(1), k(3), k(4) + 1.);
    Vec<double, 2> shift(k(2), k(5));
    if(res.empty()) {
        res = Ptr<Map>(new MapAffine(linTr, shift));
    } else {
        MapAffine newTr(linTr, shift);
        res->compose(newTr);
   }
}
void cds::Binarize(cv::Mat const &anImage, cv::Mat &binaryResult)
{
  binaryResult.create(anImage.size(), anImage.type());
  cv::threshold(anImage, binaryResult, 128, 255, CV_THRESH_BINARY+CV_THRESH_OTSU);
}
cv::Mat ArmObjSegmentation::segment(cv::Mat& color_img, cv::Mat& depth_img, cv::Mat& self_mask,
                                    cv::Mat& table_mask, bool init_color_models)
{

  cv::Mat color_img_lab_uchar(color_img.size(), color_img.type());
  cv::Mat color_img_lab(color_img.size(), CV_32FC3);
  cv::cvtColor(color_img, color_img_lab_uchar, CV_BGR2HSV);
  // cv::cvtColor(color_img, color_img_lab_uchar, CV_BGR2Lab);
  color_img_lab_uchar.convertTo(color_img_lab, CV_32FC3, 1.0/255);
  cv::Mat tmp_bw(color_img.size(), CV_8UC1);
  cv::Mat bw_img(color_img.size(), CV_32FC1);
  // Convert to grayscale
  cv::cvtColor(color_img, tmp_bw, CV_BGR2GRAY);
  tmp_bw.convertTo(bw_img, CV_32FC1, 1.0/255);


#ifdef VISUALIZE_GRAPH_WEIGHTS
  cv::Mat fg_weights(color_img.size(), CV_32FC1, cv::Scalar(0.0));
  cv::Mat fg_tied_weights(color_img.size(), CV_32FC1, cv::Scalar(0.0));
  cv::Mat bg_tied_weights(color_img.size(), CV_32FC1, cv::Scalar(0.0));
  cv::Mat bg_weights(color_img.size(), CV_32FC1, cv::Scalar(0.0));
  cv::Mat wu_weights(color_img.size(), CV_32FC1, cv::Scalar(0.0));
  cv::Mat wl_weights(color_img.size(), CV_32FC1, cv::Scalar(0.0));
#endif // VISUALIZE_GRAPH_WEIGHTS

  // TODO: Clean this up once we have stuff working well
  cv::Mat inv_self_mask;
  cv::bitwise_not(self_mask, inv_self_mask);
  cv::Mat much_larger_mask(self_mask.size(), CV_8UC1, cv::Scalar(255));
  cv::Mat enlarge_element(bg_enlarge_size_, bg_enlarge_size_, CV_8UC1, cv::Scalar(255));
  cv::dilate(inv_self_mask, much_larger_mask, enlarge_element);
  cv::Mat larger_mask, known_arm_mask;
  cv::Mat arm_band = getArmBand(inv_self_mask, arm_enlarge_width_, arm_shrink_width_, false,
                                larger_mask, known_arm_mask);

  // Get known arm pixels
  cv::Mat known_arm_pixels;
  color_img_lab.copyTo(known_arm_pixels, known_arm_mask);
  cv::Mat known_bg_mask = much_larger_mask - larger_mask;

  // Get known object pixels
  cv::Mat known_bg_pixels;
  color_img_lab.copyTo(known_bg_pixels, known_bg_mask);

  // Get stats for building graph
  int num_nodes = 0;
  int num_edges = 0;

  tabletop_pushing::NodeTable nt;
  for (int r = 0; r < much_larger_mask.rows; ++r)
  {
    for (int c = 0; c < much_larger_mask.cols; ++c)
    {
      if (much_larger_mask.at<uchar>(r,c) > 0)
      {
        // Add this as another node
        int cur_idx = num_nodes++;
        nt.addNode(r, c, cur_idx);
        int test_idx = nt.getIdx(r,c);
        // Check for edges to add
        // left edge
        if (c > 0 && much_larger_mask.at<uchar>(r, c-1) > 0)
        {
          num_edges++;
        }
        if (r > 0)
        {
          // Up edge
          if(much_larger_mask.at<uchar>(r-1,c) > 0)
          {
            num_edges++;
          }
        }
      }
    }
  }
  if (num_nodes < 1)
  {
    cv::Mat empty(color_img.size(), CV_8UC1, cv::Scalar(0));
    return empty;
  }

  if (!have_arm_color_model_)
  {
    std::cout << "[arm_obj_segmentation]: Building arm color model." << std::endl;
    arm_color_model_ = getGMMColorModel(known_arm_pixels, known_arm_mask, 3);
    have_arm_color_model_ = true;
  }
  if(!have_bg_color_model_)
  {
    std::cout << "[arm_obj_segmentation]: Building bg color model." << std::endl;
    bg_color_model_ = getGMMColorModel(known_bg_pixels, known_bg_mask, 5);
    have_bg_color_model_ = true;
  }

#ifdef USE_CANNY_EDGES
  cv::Mat canny_edges_8bit;
  cv::Mat canny_edges;
  cv::Canny(tmp_bw, canny_edges_8bit, 120, 250);
  canny_edges_8bit.convertTo(canny_edges, CV_32FC1, 1.0/255);
#else
  cv::Mat Ix = getXImageDeriv(bw_img);
  cv::Mat Iy = getYImageDeriv(bw_img);
#endif
  cv::Mat Dx = getXImageDeriv(depth_img);
  cv::Mat Dy = getYImageDeriv(depth_img);
  tabletop_pushing::GraphType *g;
  g = new GraphType(num_nodes, num_edges);
  // Populate unary and binary edge weights
  for (int r = 0; r < much_larger_mask.rows; ++r)
  {
    for (int c = 0; c < much_larger_mask.cols; ++c)
    {
      if (much_larger_mask.at<uchar>(r,c) > 0)
      {
        int cur_idx = nt.getIdx(r,c);
        float fg_weight = 0.0;
        float bg_weight = 0.0;
        if (known_arm_mask.at<uchar>(r,c) > 0)
        {
          fg_weight = fg_tied_weight_;
          bg_weight = 0.0;
        }
        else if (known_bg_mask.at<uchar>(r,c) > 0 || table_mask.at<uchar>(r,c) > 0)
        {
          fg_weight = 0.0;
          bg_weight = bg_tied_weight_;
        }
        else
        {
          fg_weight = getUnaryWeight(color_img_lab.at<cv::Vec3f>(r,c), arm_color_model_);
          bg_weight = getUnaryWeight(color_img_lab.at<cv::Vec3f>(r,c), bg_color_model_)*0.5;
        }
        g->add_node();
        g->add_tweights(cur_idx, fg_weight, bg_weight);

#ifdef VISUALIZE_GRAPH_WEIGHTS
        fg_weights.at<float>(r,c) = fg_weight;
        bg_weights.at<float>(r,c) = bg_weight;
#endif // VISUALIZE_GRAPH_WEIGHTS

        // Add left link
        if (c > 0 && much_larger_mask.at<uchar>(r, c-1) > 0)
        {
          int other_idx = nt.getIdx(r, c-1);
#ifdef USE_CANNY_EDGES
          float w_l = getEdgeWeightBoundary(canny_edges.at<float>(r,c), Dx.at<float>(r,c),
                                            canny_edges.at<float>(r, c-1), Dx.at<float>(r, c-1));
#else // USE_CANNY_EDGES
          float w_l = getEdgeWeightBoundary(Ix.at<float>(r,c), Dx.at<float>(r,c),
                                            Ix.at<float>(r, c-1), Dx.at<float>(r, c-1));
#endif // USE_CANNY_EDGES
          g->add_edge(cur_idx, other_idx, /*capacities*/ w_l, w_l);

#ifdef VISUALIZE_GRAPH_WEIGHTS
          wl_weights.at<float>(r,c) = w_l;
#endif // VISUALIZE_GRAPH_WEIGHTS
        }
        if (r > 0)
        {
          // Add up link
          if(much_larger_mask.at<uchar>(r-1,c) > 0)
          {
            int other_idx = nt.getIdx(r-1, c);
#ifdef USE_CANNY_EDGES
            float w_u = getEdgeWeightBoundary(canny_edges.at<float>(r, c),   Dy.at<float>(r, c),
                                              canny_edges.at<float>(r-1, c), Dy.at<float>(r-1, c));
#else // USE_CANNY_EDGES
            float w_u = getEdgeWeightBoundary(Iy.at<float>(r,c), Dx.at<float>(r,c),
                                              Iy.at<float>(r, c-1), Dx.at<float>(r, c-1));
#endif // USE_CANNY_EDGES
            g->add_edge(cur_idx, other_idx, /*capacities*/ w_u, w_u);

#ifdef VISUALIZE_GRAPH_WEIGHTS
            wu_weights.at<float>(r,c) = w_u;
#endif // VISUALIZE_GRAPH_WEIGHTS
          }
        }
      }
    }
  }

  // Perform cut
  g->maxflow(false);
  // Convert output into image
  cv::Mat segs = convertFlowToMat(g, nt, color_img.rows, color_img.cols);
  // Cleanup
  delete g;

  cv::Mat graph_input;
  cv::Mat segment_arm;
  cv::Mat segment_bg;
  color_img.copyTo(graph_input, much_larger_mask);
  color_img.copyTo(segment_arm, segs);
  color_img.copyTo(segment_bg, (segs == 0));
  //cv::imshow("segments", segs);
  // cv::imshow("Graph input", graph_input);
  cv::imshow("Arm segment", segment_arm);
  cv::imshow("Background segment", segment_bg);
  cv::imshow("Table mask", table_mask);
  // cv::imshow("Known bg mask", known_bg_mask);

#ifdef VISUALIZE_GRAPH_WEIGHTS
  double min_val, max_val;
  cv::minMaxLoc(fg_weights, &min_val, &max_val);
  cv::imshow("fg weights", (fg_weights-min_val)/(max_val-min_val));
  // std::cout << "Max fg weight: " << max_val << std::endl;
  // std::cout << "Min fg weight: " << min_val << std::endl;

  cv::minMaxLoc(bg_weights, &min_val, &max_val);
  cv::imshow("bg weights", (bg_weights-min_val)/(max_val-min_val));
  // std::cout << "Max bg weight: " << max_val << std::endl;
  // std::cout << "Min bg weight: " << min_val << std::endl;

  // cv::minMaxLoc(wu_weights, &min_val, &max_val);
  // cv::imshow("up weights", wu_weights/max_val);
  // std::cout << "Max up weight: " << max_val << std::endl;
  // std::cout << "Min up weight: " << min_val << std::endl;

  // cv::minMaxLoc(wl_weights, &min_val, &max_val);
  // cv::imshow("left weights", wl_weights/max_val);
  // std::cout << "Max left weight: " << max_val << std::endl;
  // std::cout << "Min left weight: " << min_val << std::endl;
#endif // VISUALIZE_GRAPH_WEIGHTS
#ifdef USE_CANNY_EDGES
  cv::imshow("Canny", canny_edges);
#endif
  return segs;
}
void MultiLayerBGS::process(const cv::Mat &img_input, cv::Mat &img_output, cv::Mat &img_bgmodel)
{
  if (img_input.empty())
    return;

  loadConfig();

  CvSize img_size = cvSize(cvCeil((double)img_input.size().width), cvCeil((double)img_input.size().height));

  if (firstTime)
  {
    if (disableDetectMode)
      status = MLBGS_LEARN;

    if (status == MLBGS_LEARN)
      std::cout << "MultiLayerBGS in LEARN mode" << std::endl;

    if (status == MLBGS_DETECT)
      std::cout << "MultiLayerBGS in DETECT mode" << std::endl;

    org_img = new IplImage(img_input);

    fg_img = cvCreateImage(img_size, org_img->depth, org_img->nChannels);
    bg_img = cvCreateImage(img_size, org_img->depth, org_img->nChannels);
    fg_prob_img = cvCreateImage(img_size, org_img->depth, 1);
    fg_mask_img = cvCreateImage(img_size, org_img->depth, 1);
    fg_prob_img3 = cvCreateImage(img_size, org_img->depth, org_img->nChannels);
    merged_img = cvCreateImage(cvSize(img_size.width * 2, img_size.height * 2), org_img->depth, org_img->nChannels);

    BGS = new CMultiLayerBGS();
    BGS->Init(img_size.width, img_size.height);
    BGS->SetForegroundMaskImage(fg_mask_img);
    BGS->SetForegroundProbImage(fg_prob_img);

    if (bg_model_preload.empty() == false)
    {
      std::cout << "MultiLayerBGS loading background model: " << bg_model_preload << std::endl;
      BGS->Load(bg_model_preload.c_str());
    }

    if (status == MLBGS_DETECT)
    {
      BGS->m_disableLearning = disableLearning;

      if (disableLearning)
        std::cout << "MultiLayerBGS disabled learning in DETECT mode" << std::endl;
      else
        std::cout << "MultiLayerBGS enabled learning in DETECT mode" << std::endl;
    }

    if (loadDefaultParams)
    {
      std::cout << "MultiLayerBGS loading default params" << std::endl;

      max_mode_num = 5;
      weight_updating_constant = 5.0;
      texture_weight = 0.5;
      bg_mode_percent = 0.6f;
      pattern_neig_half_size = 4;
      pattern_neig_gaus_sigma = 3.0f;
      bg_prob_threshold = 0.2f;
      bg_prob_updating_threshold = 0.2f;
      robust_LBP_constant = 3;
      min_noised_angle = 10.0 / 180.0 * PI; //0,01768
      shadow_rate = 0.6f;
      highlight_rate = 1.2f;
      bilater_filter_sigma_s = 3.0f;
      bilater_filter_sigma_r = 0.1f;
    }
    else
      std::cout << "MultiLayerBGS loading config params" << std::endl;

    BGS->m_nMaxLBPModeNum = max_mode_num;
    BGS->m_fWeightUpdatingConstant = weight_updating_constant;
    BGS->m_fTextureWeight = texture_weight;
    BGS->m_fBackgroundModelPercent = bg_mode_percent;
    BGS->m_nPatternDistSmoothNeigHalfSize = pattern_neig_half_size;
    BGS->m_fPatternDistConvGaussianSigma = pattern_neig_gaus_sigma;
    BGS->m_fPatternColorDistBgThreshold = bg_prob_threshold;
    BGS->m_fPatternColorDistBgUpdatedThreshold = bg_prob_updating_threshold;
    BGS->m_fRobustColorOffset = robust_LBP_constant;
    BGS->m_fMinNoisedAngle = min_noised_angle;
    BGS->m_fRobustShadowRate = shadow_rate;
    BGS->m_fRobustHighlightRate = highlight_rate;
    BGS->m_fSigmaS = bilater_filter_sigma_s;
    BGS->m_fSigmaR = bilater_filter_sigma_r;

    if (loadDefaultParams)
    {
      //frame_duration = 1.0 / 30.0;
      //frame_duration = 1.0 / 25.0;
      frame_duration = 1.0f / 10.0f;
    }

    BGS->SetFrameRate(frame_duration);

    if (status == MLBGS_LEARN)
    {
      if (loadDefaultParams)
      {
        mode_learn_rate_per_second = 0.5;
        weight_learn_rate_per_second = 0.5;
        init_mode_weight = 0.05f;
      }
      else
      {
        mode_learn_rate_per_second = learn_mode_learn_rate_per_second;
        weight_learn_rate_per_second = learn_weight_learn_rate_per_second;
        init_mode_weight = learn_init_mode_weight;
      }
    }

    if (status == MLBGS_DETECT)
    {
      if (loadDefaultParams)
      {
        mode_learn_rate_per_second = 0.01f;
        weight_learn_rate_per_second = 0.01f;
        init_mode_weight = 0.001f;
      }
      else
      {
        mode_learn_rate_per_second = detect_mode_learn_rate_per_second;
        weight_learn_rate_per_second = detect_weight_learn_rate_per_second;
        init_mode_weight = detect_init_mode_weight;
      }
    }

    BGS->SetParameters(max_mode_num, mode_learn_rate_per_second, weight_learn_rate_per_second, init_mode_weight);

    saveConfig();

    delete org_img;
  }

  //IplImage* inputImage = new IplImage(img_input);
  //IplImage* img = cvCreateImage(img_size, IPL_DEPTH_8U, 3);
  //cvCopy(inputImage, img);
  //delete inputImage;

  if (detectAfter > 0 && detectAfter == frameNumber)
  {
    std::cout << "MultiLayerBGS in DETECT mode" << std::endl;

    status = MLBGS_DETECT;

    mode_learn_rate_per_second = 0.01f;
    weight_learn_rate_per_second = 0.01f;
    init_mode_weight = 0.001f;

    BGS->SetParameters(max_mode_num, mode_learn_rate_per_second, weight_learn_rate_per_second, init_mode_weight);

    BGS->m_disableLearning = disableLearning;

    if (disableLearning)
      std::cout << "MultiLayerBGS disabled learning in DETECT mode" << std::endl;
    else
      std::cout << "MultiLayerBGS enabled learning in DETECT mode" << std::endl;
  }

  IplImage* img = new IplImage(img_input);

  BGS->SetRGBInputImage(img);
  BGS->Process();

  BGS->GetBackgroundImage(bg_img);
  BGS->GetForegroundImage(fg_img);
  BGS->GetForegroundProbabilityImage(fg_prob_img3);
  BGS->GetForegroundMaskImage(fg_mask_img);
  BGS->MergeImages(4, img, bg_img, fg_prob_img3, fg_img, merged_img);

  img_merged = cv::Mat(merged_img);
  img_foreground = cv::Mat(fg_mask_img);
  img_background = cv::Mat(bg_img);

  if (showOutput)
  {
    //cv::imshow("MLBGS Layers", img_merged);
   // cv::imshow("MLBGS FG Mask", img_foreground);
  }

  img_foreground.copyTo(img_output);
  img_background.copyTo(img_bgmodel);

  delete img;
  //cvReleaseImage(&img);

  firstTime = false;
  frameNumber++;
}
  void detect_regions(cv::Mat img,
  		vector<Region> & regions_black, vector<Region> & regions_white){

  	cv::Mat grey, lab_img, gradient_magnitude, gradient_direction;
  	gradient_magnitude = cv::Mat_<double>(img.size());
  	gradient_direction = cv::Mat_<double>(img.size());
  	this->get_gradient_maps( grey, gradient_magnitude, gradient_direction);
  	cvtColor(img, grey, CV_BGR2GRAY);
  	cvtColor(img, lab_img, CV_BGR2Lab);

  	// parameters:
  	// bool eight: 			[false]
  	// int delta			[25], smaller will get more regions
  	// double minArea,		[0.000008]
  	// double maxArea,		[0.03]
  	// double maxVariation	[1], smaller will get more regions
  	// double minDiversity	[0.7]
  	vector<Region> regions;
  	// for black regions,
  	::MSER mser_green(false, 25, 0.000008, 1.0, 1, 0.7);
  	// for white regions,
  	::MSER mser_white(false, 20, 0.000008, 0.03, 1, 0.7);

  	// try to detect green blob
  	mser_green((uchar*)grey.data, grey.cols, grey.rows, regions);
  	for (int i=0; i<regions.size(); i++){
  		regions[i].er_fill(grey);
  		regions[i].extract_features(lab_img, grey, gradient_magnitude);

  		// lab color model
  		//

  		if(regions[i].color_mean_[1] > 110
  				|| regions[i].color_mean_[0] > 150 //||
  				//regions[i].color_mean_[0] > 140
  				//regions[i].color_mean_[2] < 100
  		){
  			continue;
  		}

  		cv::Rect r = regions[i].bbox_;
  		float asp_ratio = (float)r.width / (float)r.height;

  		if(r.height < 20){
  			continue;
  		}
  		if(asp_ratio > 4 || asp_ratio < 1.5){
  			continue;
  		}

  		ROS_INFO_STREAM("r " << regions[i].color_mean_[0]);
  		regions_black.push_back(regions[i]);
  	}

  	//  	// step = 1: detect black
  	//  	// step = 2: detect white
  	//  	for (int step =1; step<2; step++)
  	//  	{
  	//
  	//  		if (step == 1){
  	//  			mser_black((uchar*)grey.data, grey.cols, grey.rows, regions);
  	//  		}
  	//  		if (step == 2){
  	//  			grey = 255-grey;
  	//  			mser_white((uchar*)grey.data, grey.cols, grey.rows, regions);
  	//  		}
  	//
  	//  		for (int i=0; i<regions.size(); i++)
  	//  			regions[i].er_fill(grey);
  	//
  	//  		double max_stroke = 0;
  	//  		for (int i=regions.size()-1; i>=0; i--)
  	//  		{
  	//  			regions[i].extract_features(lab_img, grey, gradient_magnitude);
  	//
  	//  			if ( (regions.at(i).stroke_std_/regions.at(i).stroke_mean_ > 0.8)
  	//  					|| (regions.at(i).num_holes_>2)
  	//  					|| (regions.at(i).bbox_.width <=3)
  	//  					|| (regions.at(i).bbox_.height <=3)
  	//  					)
  	//  				regions.erase(regions.begin()+i);
  	//  			else
  	//  				max_stroke = max(max_stroke, regions[i].stroke_mean_);
  	//  		}
  	//
  	//  		// filter
  	//  		for (int i=0; i<regions.size(); i++){
  	//  			cv::Rect r = regions[i].bbox_;
  	//  			float asp_ratio = (float)r.width / (float)r.height;
  	//  			float area_ratio = (float)regions[i].area_ / (float)r.area();
  	//
  	//  			if(r.width < 10){
  	//  				continue;
  	//  			}

  	//
  	//  			if(asp_ratio > 2){
  	//  				continue;
  	//  			}
  	//  			if(asp_ratio < 0.5){
  	//  				continue;
  	//  			}
  	//
  	//
  	//  			// stroke shouldn't be too small
  	//  	//		if(regions_all[i].stroke_mean_ < this->erMinStrokeWidth){
  	//  	//			continue;
  	//  	//		}
  	//  	//		cout << "intensity std: " << regions_in[i].intensity_std_ << endl;
  	//
  	//
  	//  			// black
  	//  			if(step == 1){
  	//  				if(area_ratio < 0.3){
  	//  					continue;
  	//  				}
  	//  				regions_black.push_back(regions[i]);
  	//  			}
  	//
  	//  			// white
  	//  			if(step == 2){
  	//  				if(area_ratio < 0.4){
  	//  					continue;
  	//  				}
  	//
  	//  				//cout << "area ratio: " << area_ratio << endl;
  	//  				regions_white.push_back(regions[i]);
  	//  			}
  	//  		}
  	//
  	//  		regions.clear();
  	//  	}

  }
//===============================================
center_and_angle_t get_location_and_rotation(const cv::Mat &templ_img, const cv::Mat &test_img, double rotation_start, double rotation_end, double rotation_stride) {
    //read image

    Mat test_rotate_img;
    Mat result;
    
    Point best_loc;
    double best_angle = 0;
    double best_val = 0;
    int best_rows = 0;
    int best_cols = 0;
    double min_val;
    double max_val;
    Point min_loc;
    Point max_loc;
    Size osize = test_img.size();
    int test_cols = test_img.cols;
    int test_rows = test_img.rows;
    
    for (double angle = rotation_start; angle < rotation_end; angle += rotation_stride * 10) {
        
        result = cal_similarity_score(templ_img, test_img, angle);
        osize.width = abs(test_img.cols * cos(angle/180*PI)) + abs(test_img.rows * sin(angle/180*PI));
        osize.height = abs(test_img.cols * sin(angle/180*PI)) + abs(test_img.rows * cos(angle/180*PI));
        /// Localizing the best match with minMaxLoc
        /// max_loc here is the matching point
        minMaxLoc(result, &min_val, &max_val, &min_loc, &max_loc, Mat());

        if (max_val > best_val) {
            best_val = max_val;
            best_angle = angle;
            best_loc.x = max_loc.x + templ_img.cols / 2;
            best_loc.y = max_loc.y + templ_img.rows / 2;
            best_cols = osize.width;
            best_rows = osize.height;
        }
        
    }
    
    double rough_angle = best_angle;
    
    for (double angle = rough_angle - 5; angle < rough_angle + 6; angle += rotation_stride) {
        
        result = cal_similarity_score(templ_img, test_img, angle);
        osize.width = abs(test_img.cols * cos(angle/180*PI)) + abs(test_img.rows * sin(angle/180*PI));
        osize.height = abs(test_img.cols * sin(angle/180*PI)) + abs(test_img.rows * cos(angle/180*PI));
        
        /// Localizing the best match with minMaxLoc
        /// max_loc here is the matching point
        minMaxLoc(result, &min_val, &max_val, &min_loc, &max_loc, Mat());
        
        if (max_val > best_val) {
            best_val = max_val;
            best_angle = angle;
            best_loc.x = max_loc.x + templ_img.cols / 2;
            best_loc.y = max_loc.y + templ_img.rows / 2;
            best_cols = osize.width;
            best_rows = osize.height;
        }
        
    }
    
    return cal_original_coordinate(best_rows, best_cols,
                                   test_rows, test_cols,
                                   best_loc.y, best_loc.x,
                                   best_angle / 180 * PI);
    
}
Exemple #19
0
bool WarpCost::Evaluate(const double* const* params,
                        double* residuals,
                        double** jacobians) const {
  int num_params = warper_->numParams();
  int diameter = J_->rows;
  int radius = (diameter - 1) / 2;

  // Check that configuration is valid.
  if (!warper_->isValid(params[0], I_->size(), radius)) {
    return false;
  }

  int num_pixels = diameter * diameter;

  // Build matrix representing affine transformation.
  cv::Mat M = warper_->matrix(params[0]);
  // Sample image for x in regular grid.
  cv::Mat patch;
  samplePatchAffine(*I_, patch, M, diameter, false, interpolation_);

  // Compute residuals.
  cv::Mat error = cv::Mat_<double>(diameter, diameter, residuals);
  error = patch - *J_;
  // Weight by the mask.
  cv::multiply(error, *mask_, error);

  if (jacobians != NULL && jacobians[0] != NULL) {
    cv::Mat jac = cv::Mat_<double>(num_pixels, num_params, jacobians[0]);

    // f(x, p) = I(W(x, p)) - J(x)
    // df/dp(x, p) = dI/dx(W(x, p)) dW/dp(x, p)

    // g(x, p) = M(x) f(x, p)
    // dg/dp(x, p) = M(x) df/dp(x, p)

    // Sample whole patches of derivative image.
    // (for efficiency and hopefully correct downsampling)
    cv::Mat ddx_patch;
    cv::Mat ddy_patch;
    samplePatchAffine(*dIdx_, ddx_patch, M, diameter, false, interpolation_);
    samplePatchAffine(*dIdy_, ddy_patch, M, diameter, false, interpolation_);

    cv::Point2d center(radius, radius);

    for (int u = 0; u < diameter; u += 1) {
      for (int v = 0; v < diameter; v += 1) {
        // Get row-major order index.
        int i = v * diameter + u;

        // Get image derivative at each warped point.
        cv::Mat dIdx = (cv::Mat_<double>(1, 2) <<
            ddx_patch.at<double>(v, u), ddy_patch.at<double>(v, u));

        // Get derivative of warp function.
        double dWdp_data[2 * num_params];
        cv::Point2d position = cv::Point2d(u, v) - center;
        warper_->evaluate(position, params[0], dWdp_data);

        // Use chain rule.
        cv::Mat dWdp(2, num_params, cv::DataType<double>::type, dWdp_data);

        // Compute partial Jacobian.
        double m = mask_->at<double>(v, u);
        jac.row(i) = m * dIdx * dWdp;
      }
    }

    if (check_condition_) {
      // Check that Jacobian is well-conditioned.
      double condition = cond(jac);

      if (condition > max_condition_) {
        DLOG(INFO) << "Condition number of Jacobian too large (" <<
          condition << " > " << max_condition_ << ")";
        return false;
      }
    }
  }

  return true;
}
std::vector<cv::Rect> DetectMetalImg::destArea( const cv::Mat& testImg, cv::Mat& resutlImg )
{
	std::vector<std::vector<cv::Point> >contour;
	resutlImg = cv::Mat(testImg.size(),CV_8U,cv::Scalar(255));
	cv::findContours(testImg,                  // 输入图像为二值图像
					 contour,                  // 轮廓的数组
					 CV_RETR_CCOMP,             // 获取外轮廓
					 CV_CHAIN_APPROX_NONE);    // 获取每个轮廓的每个像素


	size_t maxPos = 0;
	for (size_t i = 0;i < contour.size();++i)
	{
		if (contour[i].size() > contour[maxPos].size())
		{
			maxPos = i;
		}
	}
	int maxLength = contour[maxPos].size();

	std::vector<std::vector<cv::Point> >:: const_iterator itc = contour.begin();
	while (itc != contour.end())
	{
		if (itc -> size() == maxLength)
		{
			itc = contour.erase(itc);
		}
		else if (itc -> size() < testImg.rows / 10)
		{
			itc = contour.erase(itc);
		}
		else
			itc++;
	}

	std::vector<cv::Rect> oilArea;
	cv::Rect flagArea ;
	std::vector<cv::Rect> metalBars;
	std::vector<int> collecHeight;
	for (int i = 0;i < contour.size();++i)
	{
		flagArea = cv::boundingRect(cv::Mat(contour[i]));
		metalBars.push_back(flagArea);
	}

	detectBars(metalBars);

	int centralBar_pos = 0;
	int tmpHeight = 0;
	for (int i = 0;i < metalBars.size();i++)
	{
		if (metalBars[i].height > tmpHeight)
		{
			centralBar_pos = i;
			tmpHeight = metalBars[i].height;
		}
	}

	oilArea.push_back(metalBars[centralBar_pos]);

	cv::drawContours(resutlImg,contour,
		-1,
		cv::Scalar(0),
		1);
	rectangle(resutlImg, oilArea[0], cv::Scalar( 0, 255, 0 ), 2, 8, 0 );
	return oilArea;
}
Exemple #21
0
/* Find cell soma */
bool findCellSoma( std::vector<cv::Point> nucleus_contour, 
                   cv::Mat cell_mask, 
                   cv::Mat *intersection, 
                   std::vector<cv::Point> *soma_contour ) {

    bool status = false;

    // Calculate radius and center of the nucleus
    cv::Moments mu = moments(nucleus_contour, true);
    cv::Point2f mc = cv::Point2f(   static_cast<float>(mu.m10/mu.m00), 
                                    static_cast<float>(mu.m01/mu.m00)   );
    // Nucleus' region of influence
    cv::Mat roi_mask = cv::Mat::zeros(cell_mask.size(), CV_8UC1);
    float roi_radius = (float) (SOMA_FACTOR * DAPI_MASK_RADIUS);
    cv::circle(roi_mask, mc, roi_radius, 255, -1, 8);
    cv::circle(roi_mask, mc, DAPI_MASK_RADIUS, 0, -1, 8);
    int circle_score = countNonZero(roi_mask);

    // Soma present in ROI
    bitwise_and(roi_mask, cell_mask, *intersection);
    int intersection_score = countNonZero(*intersection);

    // Add the dapi contour to intersection region
    cv::circle(*intersection, mc, DAPI_MASK_RADIUS, 255, -1, 8);

    // Add to the soma mask if coverage area exceeds a certain threshold
    float ratio = ((float) intersection_score) / circle_score;
    if (ratio >= COVERAGE_RATIO) {

        // Segment
        cv::Mat soma_segmented;
        std::vector<std::vector<cv::Point>> contours_soma;
        std::vector<cv::Vec4i> hierarchy_soma;
        std::vector<HierarchyType> soma_contour_mask;
        std::vector<double> soma_contour_area;
        contourCalc(    *intersection, 
                        ChannelType::DAPI, 
                        1.0, 
                        &soma_segmented, 
                        &contours_soma, 
                        &hierarchy_soma, 
                        &soma_contour_mask, 
                        &soma_contour_area
                   );

        double max_area  = 0.0;
        for (size_t i = 0; i < contours_soma.size(); i++) {
            if (soma_contour_mask[i] != HierarchyType::PARENT_CNTR) continue;
            if (contours_soma[i].size() < 5) continue;
            if (soma_contour_area[i] < MIN_SOMA_SIZE) continue;

            // Find the largest permissible contour
            if (soma_contour_area[i] > max_area) {
                max_area = soma_contour_area[i];
                *soma_contour = contours_soma[i];
                status = true;
            }
        }
    }
    return status;
}
int main(int argc, char *argv[]){

  // 1. read image file
  char *filename = (argc >= 2) ? argv[1] : (char*)"/home/denjo/opencv.build/opencv-2.4.11/samples/cpp/fruits.jpg";
  original_image = cv::imread(filename);
  if(original_image.empty()){
	printf("ERROR: image not found!\n");
    return 0;
  }

  //print hot keys
  printf( "Hot keys: \n"
	  "\tESC - quit the program\n"
	  "\ti or ENTER - run inpainting algorithm\n"
	  "\t\t(before running it, paint something on the image)\n");
  
  // 2. prepare window
  cv::namedWindow("image",1);
  
  // 3. prepare Mat objects for processing-mask and processed-image
  whiteLined_image = original_image.clone();
  whiteLined_image_backup = whiteLined_image.clone();
  inpainted = original_image.clone();
  bitwised = original_image.clone();
  inpaint_mask.create(original_image.size(), CV_8UC1);
  bitwise_mask.create(original_image.size(), CV_8UC1);
  bitwise_mask_backup = bitwise_mask.clone();
  
  inpaint_mask = cv::Scalar(0);
  bitwise_mask = cv::Scalar(0);
  bitwise_mask_backup = cv::Scalar(0);
  inpainted = cv::Scalar(0);
  /* hatena */
  
  // 4. show image to window for generating mask
  cv::imshow("image", whiteLined_image);
  
  // 5. set callback function for mouse operations
  cv::setMouseCallback("image", on_mouse, 0);
  
  bool loop_flag = true;
  while(loop_flag){
    
    // 6. wait for key input
    int c = cv::waitKey(0);
    
    // 7. process according to input
    switch(c){
    case 27://ESC
    case 'q':
        loop_flag = false;
        break;
	
    case 'r':
        inpaint_mask = cv::Scalar(0);
        original_image.copyTo(whiteLined_image);
        cv::imshow("image", whiteLined_image);
        break;
	
    case 'i':
        cv::namedWindow("inpainted image", 1);
	cv::inpaint(original_image, inpaint_mask, inpainted, 3.0, cv::INPAINT_TELEA);
        cv::imshow("inpainted image", inpainted);
        break;
	
    case 'b':
      cv::namedWindow("bitwise_not image", 1);
      cv::bitwise_not(original_image, bitwised, bitwise_mask);
      cv::imshow("bitwised image", bitwised);
      break;

    case 10://ENTER
      cv::namedWindow("inpainted and bitwised not image", 1);
      cv::inpaint(original_image, inpaint_mask, inpainted, 3.0, cv::INPAINT_TELEA);
      original_image.copyTo(bitwised);
      cv::bitwise_not(inpainted, bitwised, bitwise_mask);
      cv::imshow("inpainted and bitwised not image", bitwised);
      cv::imwrite("inpaintedBitwisedNot.jpg", bitwised);
    }
  }
  return 0;
}
void	DetectRegions::part2( const cv::Mat& input,
			      std::vector<img_Plate>& output,
			      cv::Mat& img_threshold,
			      const std::string& out_id )
{

  cv::Mat	my_input;
  input.copyTo(my_input);

  //Find contours of possibles plates
  std::vector< std::vector< cv::Point> > contours;
  findContours( img_threshold,
  		contours, // a vector of contours
  		CV_RETR_EXTERNAL, // retrieve the external contours
  		// CV_CHAIN_APPROX_NONE ); // all pixels of each contours
		CV_CHAIN_APPROX_SIMPLE );


  //Start to iterate to each contour founded
  std::vector< std::vector<cv::Point> >::iterator itc = contours.begin();
  std::vector<cv::RotatedRect> rects;



  cv::Mat	my_input_rect;
  input.copyTo(my_input_rect);

  //Remove patch that are no inside limits of aspect ratio and area.
  while (itc != contours.end())
    {

      //Create bounding rect of object
      cv::RotatedRect	mr = minAreaRect(cv::Mat(*itc));

      if (!verifySizes(mr))
	{
	  itc = contours.erase(itc);

	  // rotated rectangle drawing
	  cv::Point2f	rect_points[4];
	  mr.points( rect_points );
	  for (int j = 0; j < 4; ++j)
	    line( my_input_rect, rect_points[j], rect_points[ (j + 1) % 4 ],
		  cv::Scalar(255,0,0), 1, 8 );

	}
      else
  	{
  	  ++itc;
  	  rects.push_back(mr);

	  // rotated rectangle drawing
	  cv::Point2f	rect_points[4];
	  mr.points( rect_points );
	  for (int j = 0; j < 4; ++j)
	    line( my_input_rect, rect_points[j], rect_points[ (j + 1) % 4 ],
		  cv::Scalar(0,255,0), 2, 8 );

  	}

    }


  D_IMG_SAVE( my_input_rect, "img_" << out_id << "Rect.png" );


  // Draw blue contours on a white image
  cv::Mat result;
  input.copyTo(result);
  cv::drawContours( result, contours,
		    -1, // draw all contours
		    cv::Scalar(255,0,0), // in blue
		    1 ); // with a thickness of 1
		    // 3 ); // with a thickness of 1

  D_IMG_SAVE( result, "04_img_" << out_id << "Contours.png" );


  // std::cerr << "rects.size : " << rects.size() << std::endl;

  std::vector<cv::Mat>	Mats;

  for (unsigned int i = 0; i < rects.size(); ++i)
    {


      //For better rect cropping for each posible box
      //Make floodfill algorithm because the plate has white background
      //And then we can retrieve more clearly the contour box
      circle(result, rects[i].center, 3, cv::Scalar(0,255,0), -1);

      //get the min size between width and height
      // float minSize = ( (rects[i].size.width < rects[i].size.height)
      float	minSize = ( (rects[i].size.width > rects[i].size.height)
			    ? (rects[i].size.width)
			    : (rects[i].size.height) );
      minSize = minSize - minSize * 0.5;

      //initialize rand and get 5 points around center for floodfill algorithm
      srand ( time(NULL) );

      //Initialize floodfill parameters and variables
      cv::Mat mask;
      mask.create(input.rows + 2, input.cols + 2, CV_8UC1);
      mask = cv::Scalar::all(0);
      int loDiff = 30;
      int upDiff = 30;
      int connectivity = 4;
      int newMaskVal = 255;
      // int NumSeeds = 100;
      cv::Rect ccomp;
      int flags = connectivity + (newMaskVal << 8) + CV_FLOODFILL_FIXED_RANGE + CV_FLOODFILL_MASK_ONLY;


      int	max_size = rects[i].size.width * rects[i].size.height;


      cv::Rect	b_rect = rects[i].boundingRect();

      int	min_x = b_rect.x;
      int	min_y = b_rect.y;

      int	max_x = min_x + b_rect.width;
      int	max_y = min_y + b_rect.height;

      for (int local_y = min_y; local_y < max_y; local_y += 5)
	for (int local_x = min_x; local_x < max_x; local_x += 5)
	  {
	    cv::Point	seed;

	    seed.x = local_x;
	    seed.y = local_y;

	    if (Collision( contours[i], seed ))
	      {
		cv::Mat	tmp_mask;
		tmp_mask.create( input.rows + 2, input.cols + 2, CV_8UC1 );
		tmp_mask = cv::Scalar::all(0);

		int	area = floodFill( input, tmp_mask, seed,
	  				  cv::Scalar(255,0,0), &ccomp,
	  				  cv::Scalar(loDiff, loDiff, loDiff),
	  				  cv::Scalar(upDiff, upDiff, upDiff), flags );



		{
		  cv::Point	c( ccomp.x + ccomp.width / 2,
			   ccomp.y + ccomp.height / 2 );

		  cv::Size	s( ccomp.width, ccomp.height );

		  cv::RotatedRect	tmp_rect( c, s, 0 );

		  // rotated rectangle drawing
		  cv::Point2f	rect_points[4];
		  tmp_rect.points( rect_points );
		  for (int j = 0; j < 4; ++j)
		    line( my_input, rect_points[j], rect_points[ (j + 1) % 4 ],
			  cv::Scalar(0,255,255), 1, 8 );
		}


		bool	rect_invalid = ( ccomp.x < min_x || ccomp.x > max_x ||
					 ccomp.y < min_y || ccomp.y > max_y );


		cv::Point	left_top( min_x, min_y );
		cv::Point	right_top( max_x, min_y );

		cv::Point	left_bottom( min_x, max_y );
		cv::Point	right_bottom( max_x, max_y );

		if (area > max_size)
		  {
		    circle( result, seed, 1, cv::Scalar(255,0,0), -1 );
		    circle( my_input, seed, 1, cv::Scalar(255,0,0), -1 );
		  }

		else if (rect_invalid)
		  {
		    circle( result, seed, 1, cv::Scalar(255,0,0), -1 );
		    circle( my_input, seed, 1, cv::Scalar(255,0,0), -1 );
		  }

		else
		  {
		    circle( result, seed, 1, cv::Scalar(0,255,0), -1 );
		    circle( my_input, seed, 1, cv::Scalar(0,255,0), -1 );

		    floodFill( input, mask, seed,
			       cv::Scalar(255,0,0), &ccomp,
			       cv::Scalar(loDiff, loDiff, loDiff),
			       cv::Scalar(upDiff, upDiff, upDiff), flags );
		  }

	      }
	    else
	      {
		circle( result, seed, 1, cv::Scalar(255,0,0), -1 );
		circle( my_input, seed, 1, cv::Scalar(255,0,0), -1 );
	      }

	  } // for (int j = 0; j < NumSeeds; ++j)


      {


	// rotated rectangle drawing
	cv::Point2f	rect_points[4];
	rects[i].points( rect_points );
	for (int j = 0; j < 4; ++j)
	  line( my_input, rect_points[j], rect_points[ (j + 1) % 4 ],
		cv::Scalar(255,255,255), 2, 8 );


	D_IMG_SAVE( mask, "img_" << out_id << "" << i << "_01_Mask.png" );

      }

      //cvWaitKey(0);

      //Check new floodfill mask match for a correct patch.
      //Get all points detected for get Minimal rotated Rect
      std::vector<cv::Point>	pointsInterest;
      cv::Mat_<uchar>::iterator	itMask = mask.begin<uchar>();
      cv::Mat_<uchar>::iterator	end = mask.end<uchar>();
      for (; itMask != end; ++itMask)
	if (*itMask == 255)
	  pointsInterest.push_back(itMask.pos());

      if (pointsInterest.size() < 2)
	continue;

      cv::RotatedRect	minRect = minAreaRect(pointsInterest);

      if (verifySizes(minRect))
	{

	  // rotated rectangle drawing
	  cv::Point2f rect_points[4]; minRect.points( rect_points );
	  for( int j = 0; j < 4; j++ )
	    line( result, rect_points[j], rect_points[(j+1)%4], cv::Scalar(0,0,255), 1, 8 );

	  //Get rotation matrix
	  float r = (float)minRect.size.width / (float)minRect.size.height;
	  float angle=minRect.angle;

	  if (r < 1)
	    angle = 90 + angle;

	  cv::Mat rotmat = getRotationMatrix2D(minRect.center, angle,1);

	  //Create and rotate image
	  cv::Mat img_rotated;
	  warpAffine(input, img_rotated, rotmat, input.size(), CV_INTER_CUBIC);

	  //Crop image
	  cv::Size rect_size = minRect.size;

	  if (r < 1)
	    std::swap( rect_size.width, rect_size.height );

	  cv::Mat img_crop;
	  getRectSubPix(img_rotated, rect_size, minRect.center, img_crop);


	  D_IMG_SAVE( img_crop, "img_" << out_id << "" << i << "_02_crop.png" );

	  cv::Mat	resultResized;
	  resultResized.create(33,144, CV_8UC3);
	  resize(img_crop, resultResized, resultResized.size(), 0, 0, cv::INTER_CUBIC);


	  D_IMG_SAVE( resultResized, "img_" << out_id << "" << i << "_03_resultResized.png" );

	  output.push_back( img_Plate( resultResized, minRect.boundingRect() ) );

	  // //Equalize croped image
	  // cv::Mat grayResult;
	  // cvtColor(resultResized, grayResult, CV_BGR2GRAY);
	  // // blur(grayResult, grayResult, Size(3,3));
	  // grayResult = histeq(grayResult);


	  // D_IMG_SAVE( grayResult, "img_" << out_id << "" << i << "_04_grayResult.png" );

	  // output.push_back( Plate( grayResult, minRect.boundingRect() ) );

	} // if (verifySizes(minRect))

    } // for (int i = 0; i < rects.size(); ++i)

  D_IMG_SAVE( result, "10_img_" << out_id << "Contours.png" );

  D_IMG_SAVE( my_input, "11_img_" << out_id << "my_input.png" );
}
void TableObjectManagerBiBackground::compute(const cv::Mat& src, cv::Mat& human, std::vector<size_t>& put_objects, std::vector<size_t>& taken_objects){
#ifdef DEBUG_TABLE_OBJECT_MANAGER
	StopWatch timer;
#else
	cv::Mat bgs_image;
	cv::Mat labels;
	cv::Mat static_region_labels;
	cv::Mat no_touched_fg;
#endif

	bg_subtract(src,bgs_image);

#ifdef DEBUG_TABLE_OBJECT_MANAGER
	std::cerr << "BGS   :" << timer.lap() << std::endl;
	region_num = 
#endif
		_rl_algo->compute(src,bgs_image,labels);

#ifdef DEBUG_TABLE_OBJECT_MANAGER
	std::cerr << "LABEL :" <<timer.lap() << std::endl;
#endif

	std::list<size_t> human_region;
	assert(_hd_algo!=NULL);
	cv::Mat human_small = cv::Mat(labels.size(),CV_8UC1);
	human_region = _hd_algo->compute(src,labels,human_small);
#ifdef DEBUG_TABLE_OBJECT_MANAGER
	std::cerr << "HUMAN :" << timer.lap() << std::endl;
#endif

	static_region_labels = cv::Mat(human_small.size(),CV_16SC1);
	size_t object_cand_num = _srd_algo->compute(labels,255-human_small, static_region_labels);

#ifdef DEBUG_TABLE_OBJECT_MANAGER
	std::cerr << "STATIC:" << timer.lap() << std::endl;
#endif

	cv::Mat object_cand_small = cv::Mat(human_small.size(),CV_16SC1);
	object_cand_num = _trd_algo->compute(static_region_labels,human_small,object_cand_small);

	// mat type = CV_32FC1
	cv::Mat non_update_mask_small = getLabelDiff(labels,object_cand_small);
	// mat type = CV_32FC1
	cv::Mat no_touched_fg_small = getLabelDiff(static_region_labels,object_cand_small);

#ifdef DEBUG_TABLE_OBJECT_MANAGER
	std::cerr << "TOUCH:" << timer.lap() << std::endl;
#endif

	// set scale of label resolution to src image resolution;
	assert(0 == src.rows % labels.rows);
	int scale = src.rows / labels.rows;

	cv::Mat object_cand,non_update_mask;
	if(scale > 1){
		if(human.size()!=src.size()){
			human = cv::Mat::zeros(src.size(),human_small.type());
		}
		skl::resize_label<unsigned char>(human_small,scale,human);
		object_cand = skl::resize_label<short>(object_cand_small,scale);
		non_update_mask = skl::resize_label<float>(non_update_mask_small,scale);
		no_touched_fg = skl::resize_label<float>(no_touched_fg_small,scale);
	}
	else{
		human = human_small;
		object_cand = object_cand_small;
		non_update_mask = non_update_mask_small;
		no_touched_fg = no_touched_fg_small;
	}

#ifdef DEBUG_TABLE_OBJECT_MANAGER
	std::cerr << "RESIZE:" << timer.lap() << std::endl;
#endif

	_patch_model_ptr2->setObjectLabels(
			src,
			human,
			object_cand,
			object_cand_num,
			&put_objects,
			&taken_objects);

#ifdef DEBUG_TABLE_OBJECT_MANAGER
	std::cerr << "PATCH :" << timer.lap() << std::endl;
#endif

	cv::Mat mask = _patch_model->updated_mask();

	no_touched_fg = Patch::blur_mask(no_touched_fg,PATCH_DILATE);

	non_update_mask = Patch::blur_mask(non_update_mask,PATCH_DILATE);
	non_update_mask = cv::max(_patch_model->updated_mask(),non_update_mask);
	non_update_mask = cv::max(no_touched_fg,non_update_mask);
	non_update_mask *= _learning_rate;
	non_update_mask += 1.0 - _learning_rate;

#ifdef _DEBUG
	update_mask = cv::Mat(non_update_mask.size(),CV_8UC1);
	non_update_mask.convertTo(update_mask,CV_8UC1,255);
	update_mask = cv::Scalar(255.0) - update_mask;
#endif

	if(_bg2.size()!=src.size()){
		_bg2 = cv::Mat(src.size(),CV_8UC3);
	}
	blending<cv::Vec3b,float>(src,_patch_model_ptr2->latest_bg2(),no_touched_fg,_bg2);
	_bg = _patch_model->latest_bg();

	assert(non_update_mask.type()==no_touched_fg.type());
	assert(non_update_mask.size()==no_touched_fg.size());

#ifdef DEBUG_TABLE_OBJECT_MANAGER
	std::cerr << "MASK  :" << timer.lap() << std::endl;
#endif

	bg_update(src,non_update_mask,no_touched_fg);
#ifdef DEBUG_TABLE_OBJECT_MANAGER
	std::cerr << "UPDATE :" << timer.stop() << std::endl;
#endif
}
Exemple #25
0
template<> void momentsInTile<uchar, int, int>( const cv::Mat& img, double* moments )
{
    typedef uchar T;
    typedef int WT;
    typedef int MT;
    Size size = img.size();
    int y;
    MT mom[10] = {0,0,0,0,0,0,0,0,0,0};
    bool useSIMD = checkHardwareSupport(CV_CPU_SSE2);

    for( y = 0; y < size.height; y++ )
    {
        const T* ptr = img.ptr<T>(y);
        int x0 = 0, x1 = 0, x2 = 0, x3 = 0, x = 0;

        if( useSIMD )
        {
            __m128i qx_init = _mm_setr_epi16(0, 1, 2, 3, 4, 5, 6, 7);
            __m128i dx = _mm_set1_epi16(8);
            __m128i z = _mm_setzero_si128(), qx0 = z, qx1 = z, qx2 = z, qx3 = z, qx = qx_init;

            for( ; x <= size.width - 8; x += 8 )
            {
                __m128i p = _mm_unpacklo_epi8(_mm_loadl_epi64((const __m128i*)(ptr + x)), z);
                qx0 = _mm_add_epi32(qx0, _mm_sad_epu8(p, z));
                __m128i px = _mm_mullo_epi16(p, qx);
                __m128i sx = _mm_mullo_epi16(qx, qx);
                qx1 = _mm_add_epi32(qx1, _mm_madd_epi16(p, qx));
                qx2 = _mm_add_epi32(qx2, _mm_madd_epi16(p, sx));
                qx3 = _mm_add_epi32(qx3, _mm_madd_epi16(px, sx));

                qx = _mm_add_epi16(qx, dx);
            }
            int CV_DECL_ALIGNED(16) buf[4];
            _mm_store_si128((__m128i*)buf, qx0);
            x0 = buf[0] + buf[1] + buf[2] + buf[3];
            _mm_store_si128((__m128i*)buf, qx1);
            x1 = buf[0] + buf[1] + buf[2] + buf[3];
            _mm_store_si128((__m128i*)buf, qx2);
            x2 = buf[0] + buf[1] + buf[2] + buf[3];
            _mm_store_si128((__m128i*)buf, qx3);
            x3 = buf[0] + buf[1] + buf[2] + buf[3];
        }

        for( ; x < size.width; x++ )
        {
            WT p = ptr[x];
            WT xp = x * p, xxp;

            x0 += p;
            x1 += xp;
            xxp = xp * x;
            x2 += xxp;
            x3 += xxp * x;
        }

        WT py = y * x0, sy = y*y;

        mom[9] += ((MT)py) * sy;  // m03
        mom[8] += ((MT)x1) * sy;  // m12
        mom[7] += ((MT)x2) * y;  // m21
        mom[6] += x3;             // m30
        mom[5] += x0 * sy;        // m02
        mom[4] += x1 * y;         // m11
        mom[3] += x2;             // m20
        mom[2] += py;             // m01
        mom[1] += x1;             // m10
        mom[0] += x0;             // m00
    }

    for(int x = 0; x < 10; x++ )
        moments[x] = (double)mom[x];
}
void MarkerDetector::recognizeMarkers(const cv::Mat& grayscale, std::vector<Marker>& detectedMarkers)
{
    std::vector<Marker> goodMarkers;

    // Identify the markers
    for (size_t i=0;i<detectedMarkers.size();i++)
    {
        Marker& marker = detectedMarkers[i];

        // Find the perspective transformation that brings current marker to rectangular form
        cv::Mat markerTransform = cv::getPerspectiveTransform(marker.points, m_markerCorners2d);

        // Transform image to get a canonical marker image
        cv::warpPerspective(grayscale, canonicalMarkerImage,  markerTransform, markerSize);

#ifdef SHOW_DEBUG_IMAGES
        {
            cv::Mat markerImage = grayscale.clone();
            marker.drawContour(markerImage);
            cv::Mat markerSubImage = markerImage(cv::boundingRect(marker.points));

            cv::showAndSave("Source marker" + ToString(i),           markerSubImage);
            cv::showAndSave("Marker " + ToString(i) + " after warp", canonicalMarkerImage);
        }
#endif

        int nRotations;
        int id = Marker::getMarkerId(canonicalMarkerImage, nRotations);
        if (id !=- 1)
        {
            marker.id = id;
            //sort the points so that they are always in the same order no matter the camera orientation
            std::rotate(marker.points.begin(), marker.points.begin() + 4 - nRotations, marker.points.end());

            goodMarkers.push_back(marker);
        }
    }  

    // Refine marker corners using sub pixel accuracy
    if (goodMarkers.size() > 0)
    {
        std::vector<cv::Point2f> preciseCorners(4 * goodMarkers.size());

        for (size_t i=0; i<goodMarkers.size(); i++)
        {  
            const Marker& marker = goodMarkers[i];      

            for (int c = 0; c <4; c++)
            {
                preciseCorners[i*4 + c] = marker.points[c];
            }
        }

        cv::TermCriteria termCriteria = cv::TermCriteria(cv::TermCriteria::MAX_ITER | cv::TermCriteria::EPS, 30, 0.01);
        cv::cornerSubPix(grayscale, preciseCorners, cvSize(5,5), cvSize(-1,-1), termCriteria);

        // Copy refined corners position back to markers
        for (size_t i=0; i<goodMarkers.size(); i++)
        {
            Marker& marker = goodMarkers[i];      

            for (int c=0;c<4;c++) 
            {
                marker.points[c] = preciseCorners[i*4 + c];
            }      
        }
    }

#if SHOW_DEBUG_IMAGES
    {
        cv::Mat markerCornersMat(grayscale.size(), grayscale.type());
        markerCornersMat = cv::Scalar(0);

        for (size_t i=0; i<goodMarkers.size(); i++)
        {
            goodMarkers[i].drawContour(markerCornersMat, cv::Scalar(255));    
        }

        cv::showAndSave("Markers refined edges", grayscale * 0.5 + markerCornersMat);
    }
#endif

    detectedMarkers = goodMarkers;
}
void computeNormals(const cv::Mat &edges, cv::Mat &normals, cv::Mat &orientationIndices)
{
  //TODO: move up
  const int directionsCount = 60;
  LFLineFitter lineFitter;
  fitLines(edges, lineFitter);

  Mat linearMap(edges.size(), CV_8UC1, Scalar(0));
  Mat linearMapNormals(edges.size(), CV_32FC2, Scalar::all(std::numeric_limits<float>::quiet_NaN()));
  Mat linearMapOrientationIndices(edges.size(), CV_32SC1, Scalar(-1));
  for (int i = 0; i < lineFitter.rNLineSegments(); ++i)
  {
    cv::Point start(lineFitter.outEdgeMap_[i].sx_, lineFitter.outEdgeMap_[i].sy_);
    cv::Point end(lineFitter.outEdgeMap_[i].ex_, lineFitter.outEdgeMap_[i].ey_);

    LineIterator edgelsIterator(linearMap, start, end);
    for(int j = 0; j < edgelsIterator.count; ++j, ++edgelsIterator)
    {
      **edgelsIterator = 255;
      Vec2f normal(lineFitter.outEdgeMap_[i].normal_.x, lineFitter.outEdgeMap_[i].normal_.y);
      linearMapNormals.at<Vec2f>(edgelsIterator.pos()) = normal;
      linearMapOrientationIndices.at<int>(edgelsIterator.pos()) = theta2Index(lineFitter.outEdgeMap_[i].Theta(), directionsCount);
    }
  }
//  imshow("edges", edges);
//  imshow("linearMap", linearMap);
//  waitKey();

  Mat dt, labels;
  distanceTransform(~linearMap, dt, labels, CV_DIST_L2, CV_DIST_MASK_PRECISE, DIST_LABEL_PIXEL);

  CV_Assert(linearMap.type() == CV_8UC1);
  CV_Assert(labels.type() == CV_32SC1);
  std::map<int, cv::Point> label2position;
  for (int i = 0; i < linearMap.rows; ++i)
  {
    for (int j = 0; j < linearMap.cols; ++j)
    {
      if (linearMap.at<uchar>(i, j) != 0)
      {
        //TODO: singal error if the label already exists
        label2position[labels.at<int>(i, j)] = cv::Point(j, i);
      }
    }
  }

  orientationIndices.create(edges.size(), CV_32SC1);
  orientationIndices = -1;

  normals.create(edges.size(), CV_32FC2);
  normals = Scalar::all(std::numeric_limits<float>::quiet_NaN());
  for (int i = 0; i < edges.rows; ++i)
  {
    for (int j = 0; j < edges.cols; ++j)
    {
//      if (edges.at<uchar>(i, j) != 0)
      cv::Point nearestEdgelPosition = label2position[labels.at<int>(i, j)];
      normals.at<Vec2f>(i, j) = linearMapNormals.at<Vec2f>(nearestEdgelPosition);
      orientationIndices.at<int>(i, j) = linearMapOrientationIndices.at<int>(nearestEdgelPosition);
      CV_Assert(orientationIndices.at<int>(i, j) >= 0 && orientationIndices.at<int>(i, j) < directionsCount);
    }
  }

/*
  Mat vis(orientations.size(), CV_8UC1, Scalar(0));
  for (int i = 0; i < orientations.rows; ++i)
  {
    for (int j = 0; j < orientations.cols; ++j)
    {
      Vec2f elem = orientations.at<Vec2f>(i, j);
      vis.at<uchar>(i, j) = (cvIsNaN(elem[0]) || cvIsNaN(elem[1])) ? 0 : 255;
    }
  }
  imshow("final or", vis);
  waitKey();
*/
}
void ContourBasedFingerDetector::detectFingerTipsSuggestions(std::vector<cv::Point> &tips, const cv::Mat &patch)
{
    //track contour, calculate curvature at different scales...
    //find starting point:in

    vector<vector <Point> > contours;
    vector<Vec4i> hierarchy;
    Mat object = patch.clone();
    int mode = CV_RETR_CCOMP,method = CV_CHAIN_APPROX_NONE;

    _patchSize = patch.size();

    //find inner and outer contours
    findContours(object,contours,hierarchy,mode,method);
    int outerContour = -1;

    //now take outer contour
    for(int i=0; i< contours.size(); i++){
        if(hierarchy[i][3]<0) //no parent contour
        {
            outerContour = i; //assume only one outer contour
            break;
        }
    }

    if (outerContour >= 0){
        _contour = contours[outerContour];
        int scaleLength = _scalesCount, contourLength = _contour.size();
        int scales[scaleLength];

        queue<int> locmins[scaleLength];
        int first,start;
        int minidx;
        Point pp, pn;
        double pnorm,nnorm,cos,sin,minvalue,firstminvalue,thr = _cosThr;
        int pNi,nNi;

        //set up scales
        for(int i=0; i< scaleLength; i++){
            scales[i]=floor(0.01*contourLength+i*((0.03*contourLength)/scaleLength));
            if (scales[i]==0){
                scales[i]=1;
            }
        }

        for(int i=0; i< scaleLength; i++){
            pNi = contourLength - scales[i] + 1;
            nNi = scales[i];
            first = -1;
            start = -1;
            firstminvalue = -1;

            for(int j=0; j< contourLength; j++){
                pp = _contour[j] - _contour[pNi];
                pn = _contour[nNi] - _contour[j];

                pnorm = sqrt(pp.x*pp.x + pp.y*pp.y);
                nnorm = sqrt(pn.x*pn.x + pp.y*pp.y);

                cos = (pp.x*pn.x+pp.y*pn.y)/(pnorm*nnorm);
                sin = -(pp.x*pn.y - pp.y*pn.x);

                if(first >= 0 & sin > 0 & cos < thr){

                    //state is the same; search min
                    if(cos < minvalue){
                        minvalue = cos;
                        minidx = j;
                    }
                }

                if(first< 0 & sin > 0 & cos < thr){
                    //state change; start to search for local minimum
                    first = j;
                    minvalue = cos;
                    minidx = j;
                    if (start < 0){
                        start = j;
                    }
                }

                if(first >= 0 & (sin < 0 | cos > thr)){
                    //state change; save local minimum
                    if (locmins[i].empty()){
                        firstminvalue = minvalue;
                    }
                    locmins[i].push(minidx);
                    first = -1;
                    minidx = -1;
                    minvalue = 1; //cos - bigger then that it is imposible
                }
                pNi = (pNi + 1)%contourLength;
                nNi = (nNi + 1)%contourLength;
            }

            //process the last part
            if(first > 0 & start == 0){
                //continious interval throught the starting point
                if (minvalue < firstminvalue){
                    locmins[i].front() = minidx;
                }
            }
        }

        _ids.clear();

        //transform indices to actual points
        for(int i=0; i<scaleLength; i++){
            while(!locmins[i].empty()){
                _ids.push_back(locmins[i].front());
                tips.push_back(_contour[locmins[i].front()]);
                locmins[i].pop();
            }
        }
    }
}
Exemple #29
0
void ImageProcessor::connectedCompLabeling(cv::Mat input, cv::Mat &output)
{
	//Allocate output and set it to zero
	output = cv::Mat(input.size(), CV_16U);
	output.setTo(cv::Scalar(0));
	
	//1 channel pointer to input image
	cv::Mat_<uchar>& ptrInput = (cv::Mat_<uchar>&)input;
	//1 channel pointer to output image
	cv::Mat_<ushort>& ptrOutput = (cv::Mat_<ushort>&)output; 

	//disjoint set data structure to manage the labels 
	unsigned long int* parent = new unsigned long int[output.size().height * output.size().width];
	for(unsigned long int i = 0; i < output.size().height * output.size().width; i++) parent[i] = i;

	std::vector<int>  rank (output.size().height * output.size().width);
//	std::vector<int>  parent (output.size().height * output.size().width);
//	boost::disjoint_sets<int*,int*> ds(&rank[0], &parent[0]);
//	for(unsigned long int i = 0; i < output.size().height * output.size().width; i++) {ds.make_set(i);}

	//first pass: Initial labeling
	unsigned short int currentLabel = 0;
	for (int y = 0; y < input.size().height; y++)
	{
		for(int x = 0; x < input.size().width; x++)
		{
			if (y == 0)
			{
				if(x == 0)
				{
					//First pixel. Create first label.
					ptrOutput(y,x) = ++currentLabel;
				}
				else
				{
					//First row. Only check left pixel	
					if (ptrInput(y,x) == ptrInput(y, x - 1))
					{
						//same region as left pixel -> assign same label
						ptrOutput(y,x) = ptrOutput(y, x - 1);
					}
					else
					{
						//different region -> create new label
						ptrOutput(y,x) = ++currentLabel;
					}
				}
			}
			else
			{
				if(x == 0)
				{
					//First column. Only check top pixel	
					if (ptrInput(y,x) == ptrInput(y - 1, x))
					{
						//same region as top pixel -> assign same label
						ptrOutput(y,x) = ptrOutput(y - 1, x);
					}
					else
					{
						//different region -> create new label
						ptrOutput(y,x) = ++currentLabel;
					}
				}
				else
				{
					//Regular column. Check top and left pixel
					if (ptrInput(y,x) == ptrInput(y, x - 1) && ptrInput(y,x) == ptrInput(y - 1, x))
					{
						//same region as left and top pixel -> assign minimum label of both
						ptrOutput(y,x) = std::min(ptrOutput(y, x - 1), ptrOutput(y - 1, x));
						if (ptrOutput(y, x - 1) != ptrOutput(y - 1, x))
						{
							//mark labels as equivalent
							//we are using the union/find algorithm for disjoint sets
							ImageProcessor::unite(ImageProcessor::find(ptrOutput(y, x - 1), parent),
									      ImageProcessor::find(ptrOutput(y - 1, x), parent), parent);
//							ds.union_set(ptrOutput(y, x - 1), ptrOutput(y - 1, x));
						}
					}
					else
					if (ptrInput(y,x) == ptrInput(y, x - 1))
					{
						//same region as left pixel -> assign same label
						ptrOutput(y,x) = ptrOutput(y, x - 1);
					}
					else
					if (ptrInput(y,x) == ptrInput(y - 1, x))
					{
						//same region as top pixel -> assign same label
						ptrOutput(y,x) = ptrOutput(y - 1, x);
					}
					else
					{
						//different region -> create new label
						ptrOutput(y,x) = ++currentLabel;
					}
				}
			}
		}
	}

	//second pass: Merge equivalent labels
	for (int y = 0; y < output.size().height; y++)
	{
		for(int x = 0; x < output.size().width; x++)
		{
			//we are using the union/find algorithm for disjoint sets
			ptrOutput(y,x) = (unsigned short int) ImageProcessor::find(ptrOutput(y, x), parent);
//			ptrOutput(y,x) = ds.find_set(ptrOutput(y, x));
		}
	}

	delete[] parent;
}
void imageCallback(const ImageConstPtr& msg_left, 
        const ImageConstPtr& msg_right)
{
    static Point2f pt_left, pt_right;

    pt_left = find_marker(msg_left, "");
    pt_right = find_marker(msg_right, "");
    
    static cv::Mat image_left;
    static cv::Mat image_right;


    image_left = cv_bridge::toCvShare(msg_left, "mono8")->image;
    image_right = cv_bridge::toCvShare(msg_right, "mono8")->image;
        
    cv::Size size = image_left.size();
    const float side_offset = 5;
    cv::Mat image_out(size.height, size.width * 2 + side_offset, image_left.type(), Scalar(255,255,255));

    image_left.copyTo(image_out(Rect(0, 0, image_left.cols, image_left.rows)));
    image_right.copyTo(image_out(Rect(image_right.cols + side_offset, 0, image_right.cols, image_right.rows)));


    static Point2f left_closest_point;
    static Point2f right_closest_point;

    //corresponding coordinates from the other image
    static Point2f left_closest_point_right;
    static Point2f right_closest_point_left;
    
    Point2f center;
    center.x = size.width / 2;

    //draw a line representing image center
    line(image_out, Point(center.x, 0), 
            Point(center.x, size.height), Scalar(255));

    line(image_out, Point(center.x + size.width + side_offset, 0), 
            Point(center.x + size.width + side_offset, size.height), 
            Scalar(255));


    if (pt_left.x != 0.0f && pt_right.x != 0.0f ) {
        //got a marker in both images, keep track of the marker which is
        //closest to center for each camera
        if (fabsf(center.x - pt_left.x) < fabsf(center.x - left_closest_point.x)) {
            //got a new closest point
            left_closest_point = pt_left;
            left_closest_point_right = pt_right;
        }

        if (fabsf(center.x - pt_right.x) < fabsf(center.x - right_closest_point.x)) {
            //got a new closest point
            right_closest_point = pt_right;
            right_closest_point_left = pt_left;
        }
        //draw circles for the best matches
        circle(image_out, left_closest_point, 5, Scalar(255));
        circle(image_out, 
                Point(right_closest_point.x + size.width + side_offset, 
                    right_closest_point.y), 5, Scalar(255));
       
    }

    //calculate error for both cameras
    float error_left = fabsf(center.x - left_closest_point.x);
    float error_right = fabsf(center.x - right_closest_point.x);

    char text_left[64];
    char text_right[64];

    snprintf(text_left, 64, "Error left: %.2f", error_left);
    snprintf(text_right, 64, "Error right: %.2f", error_right);

    putText(image_out, std::string(text_left), Point(center.x, size.height - 20), 
            FONT_HERSHEY_PLAIN, 1.0, Scalar(255));

    putText(image_out, std::string(text_right), Point(center.x + size.width + side_offset, 
                size.height - 20), 
            FONT_HERSHEY_PLAIN, 1.0, Scalar(255));
    
    if (error_left < 1.0f) {
        float alpha = RAD_TO_DEG(atanf(N / D));
        float fov = (90 - alpha) / (0.5 - left_closest_point_right.x / size.width);
        ROS_INFO_THROTTLE(1, "\n========\n");

        ROS_INFO_THROTTLE(1, "P_L: (%.2f, %.2f), P_R: (%.2f, %.2f)\n", left_closest_point.x, left_closest_point.y, left_closest_point_right.x, left_closest_point_right.y);
        ROS_INFO_THROTTLE(1, "FOV: %.2f\n", fov);

    }

    imshow("view", image_out);
}