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;
}
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;
}
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);
}
}
}
}
/**
* 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;
}
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);
}
}
/*!
*
*/
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;
}
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);
}
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;
}
/* 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
}
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();
}
}
}
}
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);
}