void BlockEdgeDetectorT::testag(vector<cv::Point2f> contour)
{
vector<double> partk;
const int partspan = 100;
partk.push_back(p_block->UpLine.k);
for (int i = 0; i + partspan < contour.size(); i += partspan)
{
vector<cv::Point> points;
for (int j = i; j < i + partspan; j+= 10)
{
points.push_back(contour[j]);
}
cv::Vec4f line4f;
fitLine(cv::Mat(points), line4f, CV_DIST_L2, 0, 0.01, 0.01);
double dx = line4f[0];
double dy = line4f[1];
partk.push_back(dy / dx);
}
for (int i = 0; i < partk.size(); i++)
{
}
}
int fitLineIter(double* y,double* x,size_t size,double* a,double* b)
{
size_t i,nbok,oldnbok,nbiter,nbitermax;
double rms,mean,yy,norm;
nbok=size;
norm=1./(double)size;
nbiter=0;nbitermax=100;
while(++nbiter<=nbitermax)
{
oldnbok=nbok;
fitLine(y,x,size,a,b);
rms=mean=0.;
for(i=0;i<size;i++)
{
yy=y[i]-x[i]**a-*b;
mean+=yy;
rms+=yy*yy;
}
mean*=norm;
rms*=norm;
rms=sqrt(rms-mean*mean);
nbok=0;
for(i=0;i<size;i++)
{
yy=y[i]-x[i]**a-*b;
yy=(yy<0)?-yy:yy;
if(yy<=3*rms) nbok++;
}
if(nbok==oldnbok) break;
}
return 1;
}
vector<vector<float> > Recognition::realignPoints(vector<Point2f> mesh, int horizontalSize){
vector<vector<float> > fittedLines;
for(unsigned int i=0; i< mesh.size(); i+= horizontalSize+1){
vector<Point2f> line(mesh.begin()+i, mesh.begin()+ i+ horizontalSize);
//cout << line << endl;
vector<float> fittedLine;
fitLine(line, fittedLine, CV_DIST_L2, 0, .01, .01);
float angularCoefficient= fittedLine[1]/fittedLine[0];
//float perpendicularCoefficient= -1/angularCoefficient;
//cout << angularCoefficient << " " << perpendicularCoefficient << endl;
fittedLine.push_back(angularCoefficient);
fittedLines.push_back(fittedLine);
}
return fittedLines;
}
void EdgeDetector::FitLine(vector<vector<Point>> &Fit_contours, vector<Vec4f> &line_, vector<Vec4f> &line_roi, Vec4f &Fit_Line)
{
for (int i = 0; i < Fit_contours.size(); i++)
{
if (!Fit_contours[i].empty())
{
for (int j = 0; j < 10; j++)
{
vector<float> Distance;
fitLine(Mat(Fit_contours[i]), Fit_Line, CV_DIST_L2, 0, 1, 1);
//
DistanceDetector_set(Fit_contours[i], Fit_Line, Distance);
int Tabel = Distamce_MaxTabel(Distance);
if (Distance[Tabel]>5)
{
//
// circle(image, Fit_contours[i][Tabel], 20, CV_RGB(160, 0, 50 + i * 50), 1);
Fit_contours[i].erase(Fit_contours[i].begin() + Tabel); // 擦除奇异点
}
else
break;
}
line_roi.push_back(Fit_Line);
/////////////////////////////////////////////////////
/*DrawLine(i, ROI[i], Fit_Line, 200, 200, 100);*/
/////////////////////////////////////////////////////
int t1 = 0, t2 = 0;
switch (i)
{
case 0: t1 = xleft; t2 = yleft; break;
case 1: t1 = xdown; t2 = ydown; break;
case 2: t1 = xright; t2 = yright; break;
case 3: t1 = xup; t2 = yup; break;
}
Fit_Line[2] += t1;
Fit_Line[3] += t2;
DrawLine(i, src, Fit_Line, 200, 200, 100);
/*DrawLine(i, image1, Fit_Line, 200, 200, 100);*/
line_.push_back(Fit_Line);
}
}
}
void TextInputMenu::drawMenu(FWRenderer &r, bool top) {
const int x = _pos.x;
const int y = _pos.y;
int i, tx, ty, tw;
int line = 0, words = 0, cw = 0;
int space = 0, extraSpace = 0;
const bool isAmiga = (g_cine->getPlatform() == Common::kPlatformAmiga);
if (isAmiga)
r.drawTransparentBox(x, y, _width, 4);
else
r.drawPlainBox(x, y, _width, 4, r._messageBg);
tx = x + 4;
ty = _info[0] ? y - 5 : y + 4;
tw = _width - 8;
const int infoSize = _info.size();
// input box info message
for (i = 0; i < infoSize; i++, line--) {
// fit line of text
if (!line) {
line = fitLine(_info.c_str() + i, tw, words, cw);
if (i + line < infoSize && words) {
space = (tw - cw) / words;
extraSpace = (tw - cw) % words;
} else {
space = 5;
extraSpace = 0;
}
ty += 9;
if (isAmiga)
r.drawTransparentBox(x, ty, _width, 9);
else
r.drawPlainBox(x, ty, _width, 9, r._messageBg);
tx = x + 4;
}
// draw characters
if (_info[i] == ' ') {
tx += space + extraSpace;
if (extraSpace) {
extraSpace = 0;
}
} else {
tx = r.drawChar(_info[i], tx, ty);
}
}
// input area background
ty += 9;
if (isAmiga)
r.drawTransparentBox(x, ty, _width, 9);
else
r.drawPlainBox(x, ty, _width, 9, r._messageBg);
r.drawPlainBox(x + 16, ty - 1, _width - 32, 9, 0);
tx = x + 20;
// text in input area
const int inputSize = _input.size();
for (i = 0; i < inputSize; i++) {
tx = r.drawChar(_input[i], tx, ty);
if (_cursor == i + 2) {
r.drawLine(tx, ty - 1, 1, 9, 2);
}
}
if (_input.empty() || _cursor == 1) {
r.drawLine(x + 20, ty - 1, 1, 9, 2);
}
ty += 9;
if (isAmiga)
r.drawTransparentBox(x, ty, _width, 4);
else
r.drawPlainBox(x, ty, _width, 4, r._messageBg);
r.drawDoubleBorder(x, y, _width, ty - y + 4, isAmiga ? 18 : 2);
}
/**
* Draw message in a box
* @param str Message to draw
* @param x Top left message box corner coordinate
* @param y Top left message box corner coordinate
* @param width Message box width
* @param color Message box background color (Or if negative draws only the text)
* @note Negative colors are used in Operation Stealth's timed cutscenes
* (e.g. when first meeting The Movement for the Liberation of Santa Paragua).
*/
void FWRenderer::drawMessage(const char *str, int x, int y, int width, int color) {
int i, tx, ty, tw;
int line = 0, words = 0, cw = 0;
int space = 0, extraSpace = 0;
const bool isAmiga = (g_cine->getPlatform() == Common::kPlatformAmiga);
if (color >= 0) {
if (isAmiga)
drawTransparentBox(x, y, width, 4);
else
drawPlainBox(x, y, width, 4, color);
}
tx = x + 4;
ty = str[0] ? y - 5 : y + 4;
tw = width - 8;
for (i = 0; str[i]; i++, line--) {
// Fit line of text into textbox
if (!line) {
while (str[i] == ' ') i++;
line = fitLine(str + i, tw, words, cw);
if ( str[i + line] != '\0' && str[i + line] != 0x7C && words) {
space = (tw - cw) / words;
extraSpace = (tw - cw) % words;
} else {
space = 5;
extraSpace = 0;
}
ty += 9;
if (color >= 0) {
if (isAmiga)
drawTransparentBox(x, ty, width, 9);
else
drawPlainBox(x, ty, width, 9, color);
}
tx = x + 4;
}
// draw characters
if (str[i] == ' ') {
tx += space + extraSpace;
if (extraSpace) {
extraSpace = 0;
}
} else {
tx = drawChar(str[i], tx, ty);
}
}
ty += 9;
if (color >= 0) {
if (isAmiga)
drawTransparentBox(x, ty, width, 4);
else
drawPlainBox(x, ty, width, 4, color);
drawDoubleBorder(x, y, width, ty - y + 4, isAmiga ? 18 : 2);
}
}
void fitLine(const ofPolyline& polyline, ofVec2f& point, ofVec2f& direction) {
Vec4f line;
fitLine(Mat(toCv(polyline)), line, CV_DIST_L2, 0, .01, .01);
direction.set(line[0], line[1]);
point.set(line[2], line[3]);
}
// 二次曲線当てはめ
// 戻り値：二次曲線の分類
ConicType
fitConicAny(double retcoef[6], // 二次曲線係数
double* retError, // エラーコード
double sum[5][5], // 二次曲線当てはめの微分係数行列
int* point, // 当てはめる輪郭の点列
const int nPoint, // 当てはめる輪郭の点数
const int start, // 当てはめる輪郭点列の始点
const int end, // 当てはめる輪郭点列の終点
Parameters parameters, // 全パラメータ
int line_detect_flag, // 直線検出フラグ
double* offset) // 重心のオフセット
{
*retError = 0.0;
if (sum[0][0] <= 2.0)
{
return ConicType_Unknown; // 当てはまらなかったことにする
}
// 直線にあてはめてみる
double center[2];
double direction[2];
double error;
double coef[3][6];
int nConic;
double minError;
int c, minC;
if (line_detect_flag)
{
fitLine(center, direction, sum, offset);
if (checkConicInLine(&error, center, direction, point, nPoint, start, end,
parameters.feature2D.maxErrorofLineFit) == 1)
{
// 直線でも十分にあてはまってしまった
retcoef[0] = retcoef[1] = retcoef[2] = 0.0;
retcoef[3] = direction[1];
retcoef[4] = -direction[0];
retcoef[5] = direction[0] * center[1] - direction[1] * center[0];
*retError = error;
return ConicType_Line;
}
return ConicType_Unknown;
}
nConic = fitConic(sum, coef, offset);
if (nConic <= 0)
{
retcoef[0] = retcoef[1] = retcoef[2] = retcoef[3] = retcoef[4] = retcoef[5] = 0.0;
return ConicType_Unknown;
}
// 解の中から妥当なものを選択する
minError = DBL_MAX;
minC = -1;
for (c = 0; c < nConic; c++)
{
error = 0.0;
if (checkConicInConic2(&error, coef[c], point, nPoint, start, end, parameters) == 1)
{
if (minError > error)
{ // よりよくあてはまったものがみつかった
minC = c;
minError = error;
}
}
}
if (minC == -1)
{ // 誤差範囲内であてはまる曲線がなかった
retcoef[0] = retcoef[1] = retcoef[2] = retcoef[3] = retcoef[4] = retcoef[5] = 0.0;
return ConicType_Unknown;
}
else
{
memcpy(retcoef, coef[minC], sizeof(double) * 6);
*retError = minError;
return getConicType(retcoef);
}
}
void CannyFittingDetector::findROI(vecROI *rois)
{
cv::Mat currentFrame, grayscale, blur, canny, closure, findContour;
// Current frame
currentFrame = load("currentFrame");
// Grayscale
cv::cvtColor(currentFrame, grayscale, CV_BGR2GRAY);
// Gaussian blur
//cv::GaussianBlur(grayscale, blur, m_blurSize, m_sigmaX, m_sigmaY);
cv::adaptiveThreshold(grayscale, canny, 255, CV_ADAPTIVE_THRESH_GAUSSIAN_C, CV_THRESH_BINARY_INV, 11, 5);
// Canny
//cv::Canny(blur, canny, m_lowThreshold, m_highThreshold, m_sobelSize);
#ifdef EXPORT_FRAME
cv::Mat saveCanny = 255*canny;
save("Edge detection",saveCanny);
#endif
// Milgram closure
close(canny,ARAM_MilgramContourClosing);
#ifdef EXPORT_FRAME
cv::Mat saveClosure = 255*canny;
save("Contour closing",saveClosure);
#endif
// findContours out values
std::vector<std::vector<cv::Point> > contours;
std::vector<cv::Vec4i> hierarchy;
// compute all contour in binary frame
// use CV_RETR_EXTERNAL to avoid double contour finding
// use CV_CHAIN_APPROX_NONE to keep all contours points (we need every contours points in this case)
cv::findContours(canny, contours, hierarchy, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE, cv::Point(0,0));
#ifdef EXPORT_FRAME
cv::Mat drawingRois = cv::Mat::zeros(canny.size(), CV_8UC3);
cv::Mat drawingContour = cv::Mat::zeros(canny.size(), CV_8UC3);
for(unsigned int i=0; i<contours.size(); i++)
{
cv::Scalar color = cv::Scalar(0, 100, 200);
drawContours(drawingContour, contours, i, color, 2, 8, hierarchy, 0, cv::Point());
}
save("Edge linking",drawingContour);
#endif
// list of all valid approximated curves
std::vector<std::vector<cv::Point> > approxCurves;
// list of all valid intial curves (before approxPolyDP)
std::vector<std::vector<cv::Point> > initialCurves;
// approxPolyDP out values
std::vector<cv::Point> curve;
// for each contours found
for(unsigned int i=0; i<contours.size(); ++i)
{
double perim = cv::arcLength(contours[i],true);
// Perimeter filter
if(perim>m_minPerimeter)
{
double area = cv::contourArea(contours[i]);
if(area>m_minArea)
{
// approxPolyDP : Ramer–Douglas–Peucker algorithm
cv::approxPolyDP(contours[i], curve, double(contours[i].size())*m_epsilon, true);
// 4 corners = quadrangle
if(curve.size()==4)
{
// convexity filter
if(cv::isContourConvex(curve))
{
// add to valid lists
approxCurves.push_back(curve);
initialCurves.push_back(contours[i]);
}
}
}
}
}
// 4 lines
std::vector<std::vector<cv::Point> > lines;
// list of line after fitting
std::vector<Line> lineFitted;
lines.resize(4);
lineFitted.resize(4);
int line = 0;
// for each valid curve
for(unsigned int i=0; i<initialCurves.size(); i++)
{
lines.clear();
lineFitted.clear();
lines.resize(4);
lineFitted.resize(4);
line = 0;
// for every points in initial curve
for(unsigned int j=0; j<initialCurves[i].size(); j++)
{
// current point
cv::Point currPoint = initialCurves[i][j];
// add current point to current line
lines[line].push_back(currPoint);
// if current point is a corner
if(currPoint==approxCurves[i][0] || currPoint==approxCurves[i][1] || currPoint==approxCurves[i][2] || currPoint==approxCurves[i][3])
{
// use next line
line = (line+1)%4;
// add current point to next line
lines[line].push_back(currPoint);
}
}
// for every 4 lines
for(unsigned int l=0; l<lines.size(); l++)
{
fitLine(lines[l], lineFitted[l], m_distType, 0, m_reps, m_aeps);
}
// build ROI
ROI *r = new ROI;
for(unsigned int l=0; l<lineFitted.size(); l++)
{
Point2D inter = Intersect(lineFitted[l],lineFitted[(l+1)%4]);
r->corners(inter);
}
// add ROI to ROI list in detector
rois->push_back(r);
#ifdef EXPORT_FRAME
std::vector<Point2D> corners = r->corners();
for(unsigned int i=0; i<corners.size(); i++)
{
cv::Scalar color = cv::Scalar(50, 200, 255);
cv::line(drawingRois, corners[i], corners[(i+1)%4], color, 2);
}
#endif
}
#ifdef EXPORT_FRAME
save("Line fitting",drawingRois);
#endif
return;
}
bool WallFollowing::Iterate()
{
float angle = 0.0, coefficient_affichage = 45.0/*8.*/;
float distance = 0.0, taille_pointeur;
m_iterations++;
m_map = Mat::zeros(LARGEUR_MAPPING, HAUTEUR_MAPPING, CV_8UC3);
m_map = Scalar(255, 255, 255);
std::vector<Point2f> points_obstacles;
for(list<pair<float, float> >::const_iterator it = m_obstacles.begin() ; it != m_obstacles.end() ; it ++)
{
angle = it->first; // clef
distance = it->second; // valeur
//if(distance < 5.)
if(distance < 0.25 || distance > 2.)
continue;
float x_obstacle = 0;
float y_obstacle = 0;
y_obstacle -= distance * cos(angle * M_PI / 180.0);
x_obstacle += distance * sin(angle * M_PI / 180.0);
// Filtrage des angles
double angle_degre = MOOSRad2Deg(MOOS_ANGLE_WRAP(MOOSDeg2Rad(angle)));
if(angle_degre > -160. && angle_degre < -70.)
{
points_obstacles.push_back(Point2f(x_obstacle, y_obstacle));
x_obstacle *= -coefficient_affichage;
y_obstacle *= coefficient_affichage;
x_obstacle += LARGEUR_MAPPING / 2.0;
y_obstacle += HAUTEUR_MAPPING / 2.0;
// Pointeurs
taille_pointeur = 3;
line(m_map, Point(x_obstacle, y_obstacle - taille_pointeur), Point(x_obstacle, y_obstacle + taille_pointeur), Scalar(161, 149, 104), 1, 8, 0);
line(m_map, Point(x_obstacle - taille_pointeur, y_obstacle), Point(x_obstacle + taille_pointeur, y_obstacle), Scalar(161, 149, 104), 1, 8, 0);
}
}
int echelle_ligne = 150;
Mat m(points_obstacles);
if(!points_obstacles.empty())
{
Vec4f resultat_regression;
try
{
// Méthode 1
fitLine(m, resultat_regression, CV_DIST_L2, 0, 0.01, 0.01);
float x0 = resultat_regression[2];
float y0 = resultat_regression[3];
float vx = resultat_regression[0];
float vy = resultat_regression[1];
// Affichage de l'approximation
line(m_map,
Point((LARGEUR_MAPPING / 2.0) - (x0 * coefficient_affichage) + (vx * echelle_ligne),
(HAUTEUR_MAPPING / 2.0) + (y0 * coefficient_affichage) - (vy * echelle_ligne)),
Point((LARGEUR_MAPPING / 2.0) - (x0 * coefficient_affichage) - (vx * echelle_ligne),
(HAUTEUR_MAPPING / 2.0) + (y0 * coefficient_affichage) + (vy * echelle_ligne)),
Scalar(29, 133, 217), 1, 8, 0); // Orange
// Méthode 2
fitLine(m, resultat_regression, CV_DIST_L12, 0, 0.01, 0.01);
x0 = resultat_regression[2];
y0 = resultat_regression[3];
vx = resultat_regression[0];
vy = resultat_regression[1];
// Affichage de l'approximation
line(m_map,
Point((LARGEUR_MAPPING / 2.0) - (x0 * coefficient_affichage) + (vx * echelle_ligne),
(HAUTEUR_MAPPING / 2.0) + (y0 * coefficient_affichage) - (vy * echelle_ligne)),
Point((LARGEUR_MAPPING / 2.0) - (x0 * coefficient_affichage) - (vx * echelle_ligne),
(HAUTEUR_MAPPING / 2.0) + (y0 * coefficient_affichage) + (vy * echelle_ligne)),
Scalar(77, 130, 27), 1, 8, 0); // Vert
// Méthode 3
fitLine(m, resultat_regression, CV_DIST_L1, 0, 0.01, 0.01);
x0 = resultat_regression[2];
y0 = resultat_regression[3];
vx = resultat_regression[0];
vy = resultat_regression[1];
// Affichage de l'approximation
line(m_map,
Point((LARGEUR_MAPPING / 2.0) - (x0 * coefficient_affichage) + (vx * echelle_ligne),
(HAUTEUR_MAPPING / 2.0) + (y0 * coefficient_affichage) - (vy * echelle_ligne)),
Point((LARGEUR_MAPPING / 2.0) - (x0 * coefficient_affichage) - (vx * echelle_ligne),
(HAUTEUR_MAPPING / 2.0) + (y0 * coefficient_affichage) + (vy * echelle_ligne)),
Scalar(13, 13, 188), 1, 8, 0); // Rouge
// Affichage de l'origine
taille_pointeur = 6;
line(m_map,
Point((LARGEUR_MAPPING / 2.0) - (x0 * coefficient_affichage),
(HAUTEUR_MAPPING / 2.0) + (y0 * coefficient_affichage) - taille_pointeur),
Point((LARGEUR_MAPPING / 2.0) - (x0 * coefficient_affichage),
(HAUTEUR_MAPPING / 2.0) + (y0 * coefficient_affichage) + taille_pointeur),
Scalar(9, 0, 130), 2, 8, 0);
line(m_map,
Point((LARGEUR_MAPPING / 2.0) - (x0 * coefficient_affichage) - taille_pointeur,
(HAUTEUR_MAPPING / 2.0) + (y0 * coefficient_affichage)),
Point((LARGEUR_MAPPING / 2.0) - (x0 * coefficient_affichage) + taille_pointeur,
(HAUTEUR_MAPPING / 2.0) + (y0 * coefficient_affichage)),
Scalar(9, 0, 130), 2, 8, 0);
angle = atan2(vy, vx);
cout << "X0 : " << x0 << "\t\tY0 : " << y0 << endl;
distance = abs(-vy*x0 + vx*y0);
cout << "Angle : " << angle * 180.0 / M_PI << "\t\tDist : " << distance << endl;
m_Comms.Notify("DIST_MUR", distance);
if(m_regulate)
computeAndSendCommands(angle, distance);
}
catch(Exception e) { }
// Rotation
Point2f src_center(m_map.cols/2.0F, m_map.rows/2.0F);
Mat rot_mat = getRotationMatrix2D(src_center, 180.0, 1.0);
warpAffine(m_map, m_map, rot_mat, m_map.size());
}
// Affichage des échelles circulaires
char texte[50];
float taille_texte = 0.4;
Scalar couleur_echelles(220, 220, 220);
for(float j = 1.0 ; j < 30.0 ; j ++)
{
float rayon = coefficient_affichage * j;
circle(m_map, Point(LARGEUR_MAPPING / 2, HAUTEUR_MAPPING / 2), rayon, couleur_echelles, 1);
sprintf(texte, "%dm", (int)j);
rayon *= cos(M_PI / 4.0);
putText(m_map, string(texte), Point((LARGEUR_MAPPING / 2) + rayon, (HAUTEUR_MAPPING / 2) - rayon), FONT_HERSHEY_SIMPLEX, taille_texte, couleur_echelles);
}
// Affichage de l'origine
taille_pointeur = 20;
line(m_map, Point(LARGEUR_MAPPING / 2, HAUTEUR_MAPPING / 2 - taille_pointeur * 1.5), Point(LARGEUR_MAPPING / 2, HAUTEUR_MAPPING / 2 + taille_pointeur), Scalar(150, 150, 150), 1, 8, 0);
line(m_map, Point(LARGEUR_MAPPING / 2 - taille_pointeur, HAUTEUR_MAPPING / 2), Point(LARGEUR_MAPPING / 2 + taille_pointeur, HAUTEUR_MAPPING / 2), Scalar(150, 150, 150), 1, 8, 0);
// Localisation des points de données
line(m_map, Point(0, (HAUTEUR_MAPPING / 2) + HAUTEUR_MAPPING * sin(MOOSDeg2Rad(-70.))), Point(LARGEUR_MAPPING / 2, HAUTEUR_MAPPING / 2), Scalar(150, 150, 150), 1, 8, 0);
line(m_map, Point(0, (HAUTEUR_MAPPING / 2) - HAUTEUR_MAPPING * sin(MOOSDeg2Rad(-160.))), Point(LARGEUR_MAPPING / 2, HAUTEUR_MAPPING / 2), Scalar(150, 150, 150), 1, 8, 0);
// Affichage d'informations
if(!points_obstacles.empty())
{
sprintf(texte, "Dist = %.2fm Angle = %.2f", distance, angle);
putText(m_map, string(texte), Point(10, HAUTEUR_MAPPING - 10), FONT_HERSHEY_SIMPLEX, taille_texte, Scalar(50, 50, 50));
}
imshow("Mapping", m_map);
waitKey(1);
return(true);
}
long long CGraphFitting::fastFitting()
{
int num_pixels=pImg3D->foreNum+1;
int num_labels=getValidNum();
////////////////////////////////////////////////////////////
int *data = new int[num_pixels*num_labels];
int curIndex=0;
int slice=pImg3D->getS();
int width=pImg3D->getW();
int height=pImg3D->getH();
for(int z = 0;z<slice;++z)
for(int y=0;y<height;++y)
for(int x=0;x<width;++x)
{
bool vessel=pImg3D->isVessel(x,y,z);
if(vessel)
{
for (int l=0; l < num_labels; l++ )
{
if(l==num_labels-1)
data[curIndex*num_labels+l]=5000;//INFINIT;
else
data[curIndex*num_labels+l]=models[l].compEnergy(x,y,z,*pImg3D);
}
curIndex++;
}
}
for (int l=0; l < num_labels; l++ )
{
if(l==num_labels-1)
data[curIndex*num_labels+l]=0;
else
data[curIndex*num_labels+l]=INFINIT;
}
//////////////////////////////////////////////////////////////////
int *label = new int[num_labels];
for(int k=0;k<num_labels;++k) label[k]=LABELCOST;
/////////////////////////////////////////////////////////////////
int *smooth = new int[num_labels*num_labels];
memset(smooth,0,sizeof(int)*num_labels*num_labels);
for( int i=0; i<num_labels; i++ )
for ( int j=0; j<num_labels; j++ )
smooth[ i*num_labels+j ] = smooth[ i+j*num_labels ] =SMOOTHCOST* int(i!=j);
long long energy=0;
try{
GCoptimizationGeneralGraph *gc = new GCoptimizationGeneralGraph(num_pixels,num_labels);
gc->setDataCost(data);
gc->setLabelCost(label);
#ifdef USE_SMOOTHCOST
gc->setSmoothCost(smooth);
for(int z = 0;z<slice;++z)
for(int y=0;y<height;++y)
for(int x=0;x<width;++x)
{
if(!pImg3D->isVessel(x,y,z)) continue;
int numHoles=0;
//////////////////////////////////////////////////////////////////////////////////
if(inRange(Voxel(x+1,y,z)))//right neighbour
{
if(pImg3D->isVessel(x+1,y,z))
gc->setNeighbors(pImg3D->getRealPos(x,y,z),pImg3D->getRealPos(x+1,y,z));
else
++numHoles;
}
//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
if(inRange(Voxel(x,y+1,z)))//top neighbour
{
if(pImg3D->isVessel(x,y+1,z))
gc->setNeighbors(pImg3D->getRealPos(x,y,z),pImg3D->getRealPos(x,y+1,z));
else
++numHoles;
}
//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
if(inRange(Voxel(x,y,z+1)))//front neighbour
{
if(pImg3D->isVessel(x,y,z+1))
gc->setNeighbors(pImg3D->getRealPos(x,y,z),pImg3D->getRealPos(x,y,z+1));
else
++numHoles;
}
//////////////////////////////////////////////////////////////////////////////////
if(inRange(Voxel(x-1,y,z))&& !pImg3D->isVessel(x-1,y,z) ) ++numHoles;//left hole
if(inRange(Voxel(x,y-1,z))&& !pImg3D->isVessel(x,y-1,z) ) ++numHoles;//down hole
if(inRange(Voxel(x,y,z-1))&& !pImg3D->isVessel(x,y,z-1) ) ++numHoles;//back hole
if(numHoles>0)
gc->setNeighbors(pImg3D->getRealPos(x,y,z),num_pixels-1,numHoles);
}
#endif
//printf("\nBefore optimization energy is %d",gc->compute_energy());
//std::cout<<"\nBefore optimization energy is "<<gc->compute_energy();
energy=gc->compute_energy();
gc->expansion(2);// run expansion for 2 iterations. For swap use gc->swap(num_iterations);
//printf("\nAfter optimization energy is %d",gc->compute_energy());
//std::cout<<"\nAfter optimization energy is "<<gc->compute_energy();
gc->compute_energy();
for ( int i = 0; i < num_pixels; i++ )
{
int tag = gc->whatLabel(i);
models[tag].addSupport();
pLabels[i]=tag;
//if(result[i]!=num_labels-1) printf("%d ",result[i]);
}
////////////////////////////////////////////////////////////////////////////////////
for(int i=0;i<num_labels;++i)
{
int sp=models[i].getSupport();
models[i].setValid(sp>0);
if(i==num_labels-1)//last model ,must be valid
models[i].setValid(true);
}
///////////////////////////////////////////////
for(int i=0;i<num_labels-1;++i)
{
if(models[i].isValid())
{
fitLine(i,gc);
}
}
delete gc;
}
catch (GCException e){
e.Report();
}
delete [] smooth;
delete []label;
delete [] data;
return energy;
}
void DetectorBarcode::Ransac(std::vector<cv::RotatedRect> vecRectsIn, int min_inliers, double max_px_dist, std::vector< std::vector<int> > & vecVecInlierIdx, std::vector<cv::Vec4f> & vecLines, const cv::Mat image){
//cout << "cpRansac_barcode().." << endl;
//This is a custom Ransac function that extracts lotsa lines
RNG rng;
const bool debugransac = false;
const int minstart = 2; //Amount of randomly chosen points RANSAC will start with
const int numiter = 10000; //Amount of iterations
const double stripediff_min = 0.88; // 0.88=88%
const double stripediff_max = 1.12; // 1.12=12%
int numpts = vecRectsIn.size();
//This next array keeps track of which points we have already used
int incrnumsOK[numpts];
for (int i = 0; i < numpts; i++) {
incrnumsOK[i] = i;
}
for (int i = 0; i < numiter; i++) {
Mat img_debug;
if (debugransac) {
cout << "#i=" << i << endl;
image.copyTo(img_debug);
}
vector<RotatedRect> vecRectsCandidate;
vector<int> vecRectsCandidateIdx;
int inliers_now = minstart;
//Make the array [0:1:numpts]
//This array will keep track of the numbers in vecRectsIn that we use
int incrnums[numpts];
for (int ii = 0; ii < numpts; ii++) {
incrnums[ii] = ii;
}
//Count how many we have used already
int numcandidatesLeft = 0;
for (int ii = 0; ii < numpts; ii++){
if (incrnumsOK[ii] != -1) {
numcandidatesLeft++;
}
}
if (numcandidatesLeft < min_inliers) {
break;
}
//Select 'minstart' amount of points
int tries = 0;
for(int ii = 0; ii < minstart; ii++) {
tries++;
//cout<<" ii="<<ii<<endl;
int rn = floor(rng.uniform(0., 1.)*numpts);
//cout << " rn=" << rn << endl;
if ((incrnums[rn] == -1) || (incrnumsOK[rn] == -1)) { //Make sure the point we chose is unique
ii--; //If the point is not unique (already chosen) we select a new one
if (tries > 5000) {
break;
}
} else {
incrnums[rn]=-1;
vecRectsCandidate.push_back(vecRectsIn[rn]);
vecRectsCandidateIdx.push_back(rn);
if (debugransac) {
//cout <<" pt#"<<ii<<" (x,y)=("<<vecPtsIn[rn].x<<","<<vecPtsIn[rn].y<<")"<<endl;
circle(img_debug, vecRectsIn[rn].center, 12, Scalar(255,100,100), 2,CV_AA,0);
//namedWindow("win", 0);
//imshow( "win", img_debug );
//waitKey(10);
}
}
}
if (tries > 5000) {
break;
}
//We have now selected a few random candidates, compute the stripeheight from those
int stripeheight_now = 0;
for (int s = 0; s < vecRectsCandidate.size(); s++) {
stripeheight_now += max(vecRectsCandidate[s].size.width, vecRectsCandidate[s].size.height); //Sum it up (later divide for average)
}
stripeheight_now = stripeheight_now/vecRectsCandidate.size(); //This is the average
//Fit the line
Vec4f line;
Point2f pt1,pt2;
fitLine(cv::Mat(util::vecrotrect2vecpt(vecRectsCandidate)), line, CV_DIST_L2, 0, 0.01, 0.01);
//Now get two points on this line
double dd = sqrt(line[0] * line[0] + line[1] * line[1]);
line[0] /= dd;
line[1] /= dd;
double t = double(image.cols + image.rows);
pt1.x = round(line[2] - line[0] * t);
pt1.y = round(line[3] - line[1] * t);
pt2.x = round(line[2] + line[0] * t);
pt2.y = round(line[3] + line[1] * t);
/*
if (debugransac){
//Shows the line that passes the angle constrant
cv::line(img_debug, pt1, pt2, Scalar(0,255,255), 10, CV_AA, 0 );
namedWindow("win", WINDOW_NORMAL);
Mat img_tmp;
resize(img_debug, img_tmp, Size(), 0.05, 0.5);
imshow( "win", img_tmp );
waitKey(200);
//cout << line[0] << " "<< line[1] << " "<< line[2] << " "<< line[3] << endl;
}*/
//For every point not in maybe_inliers we iterate and add it to the set
for (int ii = 0; ii < numpts; ii++) {
if ((incrnums[ii] != -1) && (incrnumsOK[ii] != -1)) { //Dont chose points already in the model
Point2f pt0 = vecRectsIn[ii].center;
//Calculate the distance between this point and the line (model)
double dist = abs((pt2.x - pt1.x) * (pt1.y - pt0.y) - (pt1.x - pt0.x) * (pt2.y - pt1.y));
dist = dist / (sqrt(pow(double(pt2.x - pt1.x), 2) + pow(double(pt2.y - pt1.y), 2)));
double longest_side = max(vecRectsIn[ii].size.width, vecRectsIn[ii].size.height);
double length_diff = longest_side / stripeheight_now;
if (dist <= max_px_dist && (length_diff > stripediff_min && length_diff < stripediff_max )) {
incrnums[ii] = -1;//OK; Include it in the index
vecRectsCandidate.push_back(vecRectsIn[ii]);
vecRectsCandidateIdx.push_back(ii);
inliers_now++;
if (debugransac) {
circle(img_debug, vecRectsIn[ii].center, 10, Scalar(100,255,100), 3, CV_AA, 0);
}
}
} else {
continue; //Continue if point already included
}
}
if (inliers_now >= min_inliers) { //If we have found a succesful line
//cout << "RANSAC found a line.." << endl;
for (int ii = 0; ii < numpts; ii++) { //For all the points
if (incrnums[ii] == -1) { //If this number is included in our line now
incrnumsOK[ii] = -1; //Exclude the points from the total array so it cant be chosen next time
}
}
fitLine(cv::Mat( util::vecrotrect2vecpt(vecRectsCandidate)), line, CV_DIST_L2, 0, 0.01, 0.01);
vecLines.push_back(line);
//vector<Point> vecPtsNow;
vecVecInlierIdx.push_back(vecRectsCandidateIdx);
//circle(img_debug, Point(0,y), img_debug.rows/60, Scalar(100,200,100), 2,CV_AA,0);
if (debugransac) {
double m = 10000;
//cv::line(img_debug, Point(line[2]-m*line[0], line[3]-m*line[1]), Point(line[2]+m*line[0], line[3]+m*line[1]), Scalar(0,0,255), 6, CV_AA, 0 );
cv::line(img_debug, pt1, pt2, Scalar(0, 0, 255), 15, CV_AA, 0);
//namedWindow("win", WINDOW_NORMAL);
imwrite("/Users/tzaman/Desktop/bc/img_debug.tif", img_debug);
//exit(-1);
//Mat img_tmp;
//resize(img_debug, img_tmp, Size(), 0.05, 0.05);
//imshow( "win", img_tmp );
//waitKey(5000);
//break;
}
}
//cout << endl;
}
}
// ######################################################################
Image<PixRGB<byte> > SuperPixelRoadSegmenter::findRoad()
{
if(!itsSuperPixelMap.initialized()) return itsSuperPixelMap;
LINFO("hi13");
//debugWin(itsSuperPixelMap,"itsSuperPixelMap");
LINFO("hi14");
// display
int w = itsSuperPixelMap.getWidth();
int h = itsSuperPixelMap.getHeight();
//Dims dims = itsSuperPixelMap.getDims();
itsDispImg.resize(w*3, 3*h, ZEROS);
Image<PixRGB<byte> > roadFindingMap(w, h, ZEROS);
//LINFO("superpixel size1 w %d h %d",itsSuperPixelMap.getWidth(),itsSuperPixelMap.getHeight());
// Estimate Middle of Road
LINFO("hi15");
if(itsSuperPixelMap.initialized())
{
//LINFO("Find road color");
std::vector<Point2D<int> > points;
LINFO("hi16");
for(int y = 0 ; y < itsSuperPixelMap.getHeight() ; y ++)
{
int middle = 0, pixCount = 0;
for(int x = 0 ; x < itsSuperPixelMap.getWidth() ; x ++)
{
PixRGB<byte> s = itsSuperPixelMap.getVal(x,y);
//Find avg of middle pixel
if(s.red()==255 && s.green()==0 && s.blue()==0)
{
middle+=x;
pixCount++;
}
roadFindingMap.setVal(x,y,s);//copy pixel from itsSuperPixelMap
}
if(pixCount!=0)
{
int midx = (int)middle/pixCount;
roadFindingMap.setVal(midx,y,PixRGB<byte>(255,255,0));//draw yellow point in the middle line
drawCircle(roadFindingMap,Point2D<int>(midx,y),2,PixRGB<byte>(255,255,0),1);//Make line thicker
//Only use bottom 20 pixel for middle line,which is just right front the robot
if(y > h-21 && y < h-5)
points.push_back(Point2D<int>(midx,y));
if(y > h-5)
itsMiddlePoint[h-y] = midx;
}
}//end for
LINFO("hi17");
//debugWin(roadFindingMap,"finish copy sp");//ok
//LINFO("Do Middle Line finder, point size %d",(int)points.size());
//roadFindingMap = getGroundTruthData(roadFindingMap);//FIXXXX
// drawLine
// (roadFindingMap,Point2D<int>(w/2,0),
// Point2D<int>(w/2,h),PixRGB<byte>(0,255,0),1);//Green line
//debugWin(roadFindingMap,"Road Finding Map after green drawLine");
LINFO("hi18");
if(points.size() > 1)
{
Point2D<int> p1,p2;
fitLine(points,p1,p2);
//char buffer[20];
//LINFO("Compute Navigation Error");
//Compute Navigation Error
itsEstMiddlePoint = Point2D<int>((p1.i+p2.i)/2,h-8);
itsHighMiddlePoint = p1;
int currMd= itsEstMiddlePoint.i;
int rf = itsRoadColor.red();
int gf = itsRoadColor.green();
int bf = itsRoadColor.blue();
LINFO("Current SuperPixel Road Middle Point %d Color (%d,%d,%d)",currMd,rf,gf,bf);
drawLine(roadFindingMap,p1,p2,PixRGB<byte>(255,255,0),3);//Yellow Middle line
//debugWin(roadFindingMap,"Road Finding Map after yellow drawLine");
LINFO("hi19");
//LINFO("Compute Navigation Error Done");
}//extra,remove it later
LINFO("hi20");
inplacePaste(itsDispImg,roadFindingMap, Point2D<int>(w, 0));
}
LINFO("hi24");
//debugWin(roadFindingMap,"Road Finding Map");
LINFO("hi25");
return roadFindingMap;
}
MainApplication::MainApplication(QWidget *parent) : QWidget(parent)
{
const int textSize = 12;
const int toolBoxWidth = 160;
const int toolBoxWidgetsWidth = 140;
const int toolBoxSubWidgetsWidth = 120;
QSize textEditSize = QSize(40, 30);
int windowHeight = 500;
int windowWidth = 800;
/*---- Buttons ----*/
QPushButton *rangeQueryButton = new QPushButton(tr("Range"));
rangeQueryButton->setFont(QFont("Times", textSize, QFont::AnyStyle));
QPushButton *radiusQueryButton = new QPushButton(tr("Radius"));
radiusQueryButton->setFont(QFont("Times", textSize, QFont::AnyStyle));
QPushButton *loadButton = new QPushButton(tr("Load"));
loadButton->setFont(QFont("Times", textSize, QFont::AnyStyle));
QPushButton *nnQueryButton = new QPushButton(tr("NN-Query"));
nnQueryButton->setFont(QFont("Times", textSize, QFont::AnyStyle));
QPushButton *smoothingButton = new QPushButton(tr("Smooth"));
smoothingButton->setFont(QFont("Times", textSize, QFont::AnyStyle));
QPushButton *distanceColorMapButton = new QPushButton(tr("ColorbyDist"));
distanceColorMapButton->setFont(QFont("Times", textSize, QFont::AnyStyle));
QPushButton *thinningButton = new QPushButton(tr("Thinning"));
thinningButton->setFont(QFont("Times", textSize, QFont::AnyStyle));
QPushButton *lineFittingButton = new QPushButton(tr("fit Line"));
lineFittingButton->setFont(QFont("Times", textSize, QFont::AnyStyle));
QPushButton *planeFittingButton = new QPushButton(tr("fit Plane"));
planeFittingButton->setFont(QFont("Times", textSize, QFont::AnyStyle));
QPushButton *sphereFittingButton = new QPushButton(tr("fit Sphere"));
sphereFittingButton->setFont(QFont("Times", textSize, QFont::AnyStyle));
connect(loadButton, SIGNAL(clicked()), this, SLOT(loadPoints()));
connect(rangeQueryButton, SIGNAL(clicked()), this, SLOT(rangeQuery()));
connect(radiusQueryButton, SIGNAL(clicked()), this, SLOT(radiusQuery()));
connect(smoothingButton, SIGNAL(clicked()), this, SLOT(smoothPointCloud()));
connect(nnQueryButton, SIGNAL(clicked()), this, SLOT(nnQuery()));
connect(distanceColorMapButton, SIGNAL(clicked()), this, SLOT(colorPointsByDistance()));
connect(thinningButton, SIGNAL(clicked()), this, SLOT(applyThinning()));
connect(lineFittingButton, SIGNAL(clicked()), this, SLOT(fitLine()));
connect(planeFittingButton, SIGNAL(clicked()), this, SLOT(fitPlane()));
connect(sphereFittingButton, SIGNAL(clicked()), this, SLOT(fitSphere()));
/*---- Labels ----*/
labelCloudBounds = new QLabel("---", this);
labelCloudBounds->setMaximumHeight(60);
labelPoints = new QLabel("---", this);
labelPoints->setMaximumHeight(60);
labelTime = new QLabel("---", this);
labelTime->setMaximumHeight(60);
labelFitting = new QLabel("p: dir:", this);
labelFitting->setMaximumHeight(120);
/*---- Text Edits ----*/
QDoubleValidator *validDouble = new QDoubleValidator();
minXRange = new QLineEdit();
minXRange->setMaximumSize(textEditSize);
minXRange->setValidator(validDouble);
maxXRange = new QLineEdit();
maxXRange->setMaximumSize(textEditSize);
maxXRange->setValidator(validDouble);
minYRange = new QLineEdit();
minYRange->setMaximumSize(textEditSize);
minYRange->setValidator(validDouble);
maxYRange = new QLineEdit();
maxYRange->setMaximumSize(textEditSize);
maxYRange->setValidator(validDouble);
minZRange = new QLineEdit();
minZRange->setMaximumSize(textEditSize);
minZRange->setValidator(validDouble);
maxZRange = new QLineEdit();
maxZRange->setMaximumSize(textEditSize);
maxZRange->setValidator(validDouble);
xRadius = new QLineEdit();
xRadius->setMaximumSize(textEditSize);
xRadius->setValidator(validDouble);
yRadius = new QLineEdit();
yRadius->setMaximumSize(textEditSize);
yRadius->setValidator(validDouble);
zRadius = new QLineEdit();
zRadius->setMaximumSize(textEditSize);
zRadius->setValidator(validDouble);
rRadius = new QLineEdit();
rRadius->setMaximumSize(textEditSize);
rRadius->setValidator(validDouble);
xNeighbour = new QLineEdit();
xNeighbour->setMaximumSize(textEditSize);
xNeighbour->setValidator(validDouble);
yNeighbour = new QLineEdit();
yNeighbour->setMaximumSize(textEditSize);
yNeighbour->setValidator(validDouble);
zNeighbour = new QLineEdit();
zNeighbour->setMaximumSize(textEditSize);
zNeighbour->setValidator(validDouble);
rSmoothing = new QLineEdit();
rSmoothing->setMaximumSize(textEditSize);
rSmoothing->setMaximumWidth(toolBoxSubWidgetsWidth);
rSmoothing->setValidator(validDouble);
rThinning = new QLineEdit();
rThinning->setMaximumSize(textEditSize);
rThinning->setMaximumWidth(toolBoxSubWidgetsWidth);
rThinning->setValidator(validDouble);
/*---- Tool Box and Tool Box Widgets ----*/
QToolBox *toolBox = new QToolBox();
//Load
QVBoxLayout *layoutLoad = new QVBoxLayout();
layoutLoad->addWidget(loadButton);
QWidget* LoadWidget = new QWidget();
LoadWidget->setLayout(layoutLoad);
LoadWidget->setFixedWidth(toolBoxWidgetsWidth);
toolBox->addItem(LoadWidget, "Load Data");
// Range Query
QGridLayout *layoutRangeTextEdits = new QGridLayout();
layoutRangeTextEdits->addWidget(minXRange,0,0,0);
layoutRangeTextEdits->addWidget(maxXRange,0,1,0);
layoutRangeTextEdits->addWidget(minYRange,1,0,0);
layoutRangeTextEdits->addWidget(maxYRange,1,1,0);
layoutRangeTextEdits->addWidget(minZRange,2,0,0);
layoutRangeTextEdits->addWidget(maxZRange,2,1,0);
QWidget* RangeTextEditsWidget = new QWidget();
RangeTextEditsWidget->setLayout(layoutRangeTextEdits);
RangeTextEditsWidget->setFixedWidth(toolBoxSubWidgetsWidth);
QVBoxLayout *layoutRange = new QVBoxLayout();
layoutRange->addWidget(RangeTextEditsWidget);
layoutRange->addWidget(rangeQueryButton);
QWidget* RangeWidget = new QWidget();
RangeWidget->setLayout(layoutRange);
RangeWidget->setFixedWidth(toolBoxWidgetsWidth);
toolBox->addItem(RangeWidget, "Range Query");
// Radius Query
QGridLayout *layoutRadiusTextEdits = new QGridLayout();
layoutRadiusTextEdits->addWidget(xRadius, 0, 0, 0);
layoutRadiusTextEdits->addWidget(yRadius, 0, 1, 0);
layoutRadiusTextEdits->addWidget(zRadius, 0, 3, 0);
layoutRadiusTextEdits->addWidget(rRadius, 1, 1, 0);
QWidget* RadiusTextEditsWidget = new QWidget();
RadiusTextEditsWidget->setLayout(layoutRadiusTextEdits);
RadiusTextEditsWidget->setFixedWidth(toolBoxSubWidgetsWidth);
QVBoxLayout *layoutRadius = new QVBoxLayout();
layoutRadius->addWidget(RadiusTextEditsWidget);
layoutRadius->addWidget(radiusQueryButton);
QWidget* RadiusWidget = new QWidget();
RadiusWidget->setLayout(layoutRadius);
RadiusWidget->setFixedWidth(toolBoxWidgetsWidth);
toolBox->addItem(RadiusWidget, "Radius Query");
// NN Query
QGridLayout *layoutNNTextEdits = new QGridLayout();
layoutNNTextEdits->addWidget(xNeighbour, 0, 0, 0);
layoutNNTextEdits->addWidget(yNeighbour, 0, 1, 0);
layoutNNTextEdits->addWidget(zNeighbour, 0, 3, 0);
QWidget* NNTextEditsWidget = new QWidget();
NNTextEditsWidget->setLayout(layoutNNTextEdits);
NNTextEditsWidget->setFixedWidth(toolBoxSubWidgetsWidth);
QVBoxLayout *layoutNN = new QVBoxLayout();
layoutNN->addWidget(NNTextEditsWidget);
layoutNN->addWidget(nnQueryButton);
QWidget* NNWidget = new QWidget();
NNWidget->setLayout(layoutNN);
NNWidget->setFixedWidth(toolBoxWidgetsWidth);
toolBox->addItem(NNWidget, "Nearest Neighbour");
// Thinning
QVBoxLayout *layoutThinning = new QVBoxLayout();
layoutThinning->addWidget(rThinning);
layoutThinning->addWidget(thinningButton);
QWidget* ThinningWidget = new QWidget();
ThinningWidget->setLayout(layoutThinning);
ThinningWidget->setFixedWidth(toolBoxWidgetsWidth);
toolBox->addItem(ThinningWidget, "Thinning");
// Smoothing
QVBoxLayout *layoutSmoothing = new QVBoxLayout();
layoutSmoothing->addWidget(rSmoothing);
layoutSmoothing->addWidget(smoothingButton);
QWidget* SmoothingWidget = new QWidget();
SmoothingWidget->setLayout(layoutSmoothing);
SmoothingWidget->setFixedWidth(toolBoxWidgetsWidth);
toolBox->addItem(SmoothingWidget, "Smoothing");
// Fitting
QVBoxLayout *layoutFitting = new QVBoxLayout();
layoutFitting->addWidget(labelFitting);
layoutFitting->addWidget(planeFittingButton);
layoutFitting->addWidget(lineFittingButton);
layoutFitting->addWidget(sphereFittingButton);
QWidget* FittingWidget = new QWidget();
FittingWidget->setLayout(layoutFitting);
FittingWidget->setFixedWidth(toolBoxWidgetsWidth);
toolBox->addItem(FittingWidget, "Fitting");
// Color
QVBoxLayout *layoutColorByDist = new QVBoxLayout();
layoutColorByDist->addWidget(distanceColorMapButton);
QWidget* ColorByDistWidget = new QWidget();
ColorByDistWidget->setLayout(layoutColorByDist);
ColorByDistWidget->setFixedWidth(toolBoxWidgetsWidth);
toolBox->addItem(ColorByDistWidget, "Color by Distance");
/*---- Data Group Box ----*/
QGroupBox *dataBox = new QGroupBox(tr("Data"));
QVBoxLayout *layoutDataBox = new QVBoxLayout;
layoutDataBox->addWidget(labelPoints);
layoutDataBox->addWidget(labelCloudBounds);
dataBox->setLayout(layoutDataBox);
/*---- Side Bar ----*/
QVBoxLayout *layoutSideBar = new QVBoxLayout();
layoutSideBar->addWidget(dataBox);
layoutSideBar->addWidget(toolBox);
layoutSideBar->addWidget(labelTime);
QWidget* sideBarWidget = new QWidget();
sideBarWidget->setLayout(layoutSideBar);
sideBarWidget->setFixedWidth(toolBoxWidth);
/*---- Main Widget ----*/
glWidget = new MainGLWidget();
glWidget->resize(windowWidth, windowHeight);
glWidget->setMinimumWidth(windowWidth);
glWidget->setMinimumHeight(windowHeight);
QHBoxLayout *layoutMain = new QHBoxLayout();
layoutMain->addWidget(glWidget);
layoutMain->addWidget(sideBarWidget);
setLayout(layoutMain);
}
bool ContourModel::update(vector<vector<Point> > contours,vector<Point2d>& originalPoints, int image_w_half)
{
//step 1: find the larger contours to filter out some noise (area > thresh)
vector<vector<Point> > largeContours;
int areaThreshold = 130;
for(int i = 0;i < (int)contours.size();i++)
{
vector<Point> currCont = contours.at(i);
double area = contourArea(contours.at(i));
if(area > areaThreshold)
{
largeContours.push_back(currCont);
}
}
//step 2: for each larger contour: find the center of mass and the lane direction to group them
vector<Point2d> mass_centers;
vector<Point2d> line_directions;
for(int i = 0;i < (int)largeContours.size();i++)
{
//calculate the line direction for each contour
Vec4f lineParams;
fitLine(largeContours.at(i), lineParams, CV_DIST_L2, 0, 0.01, 0.01);
Point2d lineDirection(lineParams[0],lineParams[1]);
line_directions.push_back(lineDirection);
//calculate the mass center for each contour
vector<Moments> contourMoments;
Moments currMoments = moments(largeContours.at(i));
double x_cent = currMoments.m10 / currMoments.m00;
double y_cent = currMoments.m01 / currMoments.m00;
Point2d mass_cent(x_cent,y_cent);
mass_centers.push_back(mass_cent);
}
//assert these vectors have same length:
if(largeContours.size() != mass_centers.size())cout << "ERROR in ContourModel: massCenters.size != largeContours.size()" << endl;
if(largeContours.size() != line_directions.size())cout << "ERROR in ContourModel: massCenters.size != largeContours.size()" << endl;
//step 3: create the "mergeList": store for each contour weather it wants to merge with another one
vector<vector<int> > mergelist;
//merge contours based on center of mass and line direction
for(int i = 0;i < (int)largeContours.size();i++)
{
vector<int> mergeWishes;
Point2d currCenter = mass_centers.at(i);
Point2d currDirection = line_directions.at(i);
for(int j = i+1;j < (int)largeContours.size();j++)
{
Point2d compCenter = mass_centers.at(j);
Point2d compDirection = line_directions.at(j);
bool wantMerge = mergeContours(currCenter, currDirection, compCenter, compDirection);
if(wantMerge)mergeWishes.push_back(j);
}
mergelist.push_back(mergeWishes);
}
//step 4: use the mergeList to create the final_mergelist which looks as follows:
//[ [0,2,5] [3] [1] [4,6]] telling which contours should be merged together
vector<vector<int> > final_mergelist;
for(int i = 0;i < (int)largeContours.size();i++)
{
vector<int> temp;
temp.push_back(i);
final_mergelist.push_back(temp);
}
for(int i = 0;i < (int)largeContours.size();i++)
{
vector<int>* containerToPushTo = NULL;
//step 1: find the container the contour i is in - note that this will always succeed so containerToPushTo wont stay NULL
for(int j = 0;j < (int)final_mergelist.size();j++)
{
vector<int>* currContainer;
currContainer = &final_mergelist.at(j);
for(int k = 0;k < (int)final_mergelist.at(j).size();k++)
{
if(final_mergelist.at(j).at(k) == i)
{
containerToPushTo = currContainer;
}
}
}
//step2: for each element to push: make sure it appears in the container
for(int j = 0;j < (int)mergelist.at(i).size();j++)
{
int elemToMerge = mergelist.at(i).at(j);
//if elemToMerge already appears in containerToPushTo => do nothing
bool alreadyInContainer = false;
for(int k = 0;k < (int)containerToPushTo->size();k++)
{
if(containerToPushTo->at(k) == elemToMerge)
alreadyInContainer = true;
}
//not inside: push the element and delete it from the old vector it was in
if(!alreadyInContainer)
{
//delete it from the old container!!
for(int k = 0;k < (int)final_mergelist.size();k++)
{
for(int l = 0;l < (int)final_mergelist.at(k).size();l++)
{
//DEBUG IFS - ERASE LATER
if(k < 0 || k >= (int)final_mergelist.size())cout << "OVERFLOW IN 159::ContourModel" << endl;
if(l < 0 || l >= (int)final_mergelist.at(k).size())cout << "OVERFLOW IN 160::ContourModel" << endl;
if(final_mergelist.at(k).at(l) == elemToMerge)
{
//DEBUG IF- ERASE LATER
if(l < 0 || l >= (int)final_mergelist.at(k).size()) cout << "ERROR ContourModel 162" << endl;
final_mergelist.at(k).erase(final_mergelist.at(k).begin()+l);
}
}
}
//add it in the new container
containerToPushTo->push_back(elemToMerge);
}
}
}
//step 5: merge the contours together
vector< vector<vector<Point> > > mergedContours;
for(int i = 0;i < (int)final_mergelist.size();i++)
{
vector<vector<Point> > currGrouping;
for(int j = 0;j < (int)final_mergelist.at(i).size();j++)
{
vector<Point> currContour = largeContours.at(final_mergelist.at(i).at(j));
currGrouping.push_back(currContour);
}
if(currGrouping.size() > 0)mergedContours.push_back(currGrouping);
}
//TRY TO FIND THE MIDDLE LANE
vector<vector<Point> > singleContours;
vector<vector<vector<Point> > > multipleContours;
for(int i = 0;i < (int)mergedContours.size();i++)
{
vector<vector<Point> > currContGroup = mergedContours.at(i);
if(currContGroup.size() == 1) singleContours.push_back(currContGroup.at(0));
else if(currContGroup.size() > 1) multipleContours.push_back(currContGroup);
}
//in this situation there is actually a chance to apply the middle lane extraction, otherwise the old procedure is applied
if(multipleContours.size() == 1 && singleContours.size() <= 2 && singleContours.size() > 0)
{
//sort single contours by area
std::sort(singleContours.begin(),singleContours.end(),acompareCont);
vector<Point> largestSingleContour = singleContours.at(singleContours.size()-1);
double areaLargestSingle = contourArea(largestSingleContour);
vector<vector<Point> > middleContour = multipleContours.at(0);
double areaMiddle = 0;
bool validMid = true;
for(int i = 0;i < (int)middleContour.size();i++)
{
double areaCurr = contourArea(middleContour.at(i));
if(areaCurr > areaLargestSingle/2.0){
validMid = false;
}
areaMiddle += contourArea(middleContour.at(i));
}
//if both contours have a certain size
if(areaLargestSingle > 120 && areaMiddle > 120)
{
//MIDDLE LANE AND OTHER LANE FOUND => RETURN THE ESTIMATE
//first argument will be the middle lane
//second argument will be the other larger lane
vector<vector<Point2d> > nicelyGroupedPoints;
//1) --- MIDDLE LANE ---
vector<Point2d> temp_result;
for(int i = 0;i < (int)middleContour.size();i++)
{
vector<Point> currCont = middleContour.at(i);
Rect bound = boundingRect(currCont);
//visit every point in the bounding rect
for(int y = bound.y;y < bound.y+bound.height;y++)
{
for(int x = bound.x;x < bound.x+bound.width;x++)
{
if(pointPolygonTest(currCont, Point(x,y), false) >= 0)
{
temp_result.push_back(Point2d(x-image_w_half,y));
}
}
}
}
nicelyGroupedPoints.push_back(temp_result);
//2) --- OTHER LANE ---
vector<Point2d> temp_result2;
Rect bound = boundingRect(largestSingleContour);
//visit every point in the bounding rect
for(int y = bound.y;y < bound.y+bound.height;y++)
{
for(int x = bound.x;x < bound.x+bound.width;x++)
{
if(pointPolygonTest(largestSingleContour, Point(x,y), false) >= 0)
{
temp_result2.push_back(Point2d(x-image_w_half,y));
}
}
}
if(validMid)
{
nicelyGroupedPoints.push_back(temp_result2);
points = nicelyGroupedPoints;
return true; //middle lane estimate provided
}
}
}
//MIDDLE LANE WAS NOT FOUND
//step 6: get the final result: the grouped points matching the contours
//need to perform a inside contour check within the bounding rectangle of the contour for
//each point in the bounding rectangle
vector<vector<Point2d> > nicelyGroupedPoints;
for(int i = 0;i < (int)mergedContours.size();i++)
{
vector<Point2d> temp_result;
for(int j = 0;j < (int)mergedContours.at(i).size();j++)
{
vector<Point> currContour = mergedContours.at(i).at(j);
Rect bound = boundingRect(currContour);
//visit every point in the bounding rect
for(int y = bound.y;y < bound.y+bound.height;y++)
{
for(int x = bound.x;x < bound.x+bound.width;x++)
{
if(pointPolygonTest(currContour, Point(x,y), false) >= 0)
{
temp_result.push_back(Point2d(x-image_w_half,y));
}
}
}
}
if(temp_result.size() > 0)
{
nicelyGroupedPoints.push_back(temp_result);
}
}
/*
//step 6 (alternative): get the final result: the grouped points matching the contours
//need to perform a inside contour check for the input points if in boundary rectangle of the contour
vector<vector<Point2d> > nicelyGroupedPoints;
for(int i = 0;i < mergedContours.size();i++)
{
vector<Point2d> temp_result;
for(int j = 0;j < mergedContours.at(i).size();j++)
{
vector<Point> currContour = mergedContours.at(i).at(j);
Rect bound = boundingRect(currContour);
for(int k = 0;k < originalPoints.size();k++)
{
//check if within the contour:
if(pointPolygonTest(currContour, originalPoints.at(k), false) >= 0)
{
temp_result.push_back(Point2d(originalPoints.at(k).x-image_w_half, originalPoints.at(k).y));
}
}
}
if(temp_result.size() > 0)
{
nicelyGroupedPoints.push_back(temp_result);
}
}
*/
points = nicelyGroupedPoints;
return false; //everything as usual, no further information
}