cv::Mat rotateImage(const cv::Mat& source, double angle){
cv::Point2f src_center(source.cols/2.0F, source.rows/2.0F);
cv::Mat rot_mat = cv::getRotationMatrix2D(src_center, angle, 1.0);
cv::Mat dst;
cv::warpAffine(source, dst, rot_mat, source.size());
return dst;
}
void VideoCorrect::rotateImage(Mat& source, double angle)
{
Point2f src_center(source.cols/2.0F, source.rows/2.0F);
Mat rot_mat = getRotationMatrix2D(src_center, angle, 1.0);
Mat dst;
warpAffine(source, source, rot_mat, source.size(), 1, 0, Scalar(255,255,255));
}
Mat Detector::rotateImage(const Mat& source, double angle)
{
Point2f src_center(source.cols/2.0F, source.rows/2.0F);
Mat rot_mat = getRotationMatrix2D(src_center, angle, 1.0);
Mat dst;
warpAffine(source, dst, rot_mat, source.size());
return dst;
}
//rotates about center
cv::Mat ModelMaker::rotateImage(const Mat& source, double anglerad)
{
qDebug()<<"Angle in rad"<<anglerad;
double angle = ((anglerad*180)/CV_PI);
qDebug()<<"Angle in deg"<<angle;
Point2f src_center(source.cols/2.0F, source.rows/2.0F);
Mat rot_mat = getRotationMatrix2D(src_center, angle, 1.0);
Mat dst;
warpAffine(source, dst, rot_mat, source.size());
return dst;
}
void myrotate(const Mat &src, Mat& dst, int angle)
{
Point2f src_center(0, 0);
Mat rot_mat = getRotationMatrix2D(src_center, angle, 1.0);
if ((angle / 90) % 2 == 0)
warpAffine(src, dst, rot_mat, src.size());
else
{
Size dst_size= Size(src.rows, src.cols);
warpAffine(src, dst, rot_mat, dst_size);
}
}
//////////////////////////////////////////////
// rotate picture (to align eyes-y)
//////////////////////////////////////////////
Mat rotate(Mat& image, double angle, CvPoint centre)
{
Point2f src_center(centre.x, centre.y);
// conversion en degre
angle = angle*180.0/3.14157;
//DEBUG printf("(D) rotate : rotating : %f° %d %d\n",angle, centre.x, centre.y);
Mat rot_matrix = getRotationMatrix2D(src_center, angle, 1.0);
Mat rotated_img(Size(image.size().height, image.size().width), image.type());
warpAffine(image, rotated_img, rot_matrix, image.size());
return (rotated_img);
}
void CMyImage::RotateImage(const void* src, void** dst, int angleDegrees)
{//???? replaced by cv::flip()
if (!src)
return;
cv::Mat* pmatSrc = (cv::Mat*)src;
//take the dimention of original image
cv::Size size = pmatSrc->size();
int w = size.width;
int h = size.height;
// Make a new image for the result
cv::Mat* pmatRotated = new cv::Mat(h, w, CV_8UC3);
if (!pmatRotated)
return;
cv::Point2f src_center( pmatSrc->cols/2.0F, pmatSrc->rows/2.0F);
cv::Mat M = cv::getRotationMatrix2D(src_center, angleDegrees, 1.0);
// Transform the image
cv::warpAffine(*pmatSrc, *pmatRotated, M, cv::Size(w, h));
*dst = pmatRotated;
return;
}
void ModelMaker::extractModel(cv::Mat origframe)
{
// qDebug()<<"Frame is"<<frame.empty();
for(int i = 0; i < centroids.size(); i++) {
Mat subtractedframe(origframe);
subtractedframe = subtractBack(origframe);
Mat polymask = subtractedframe.clone();
// cv::cvtColor(frameMat, frameMat, CV_BGR2BGRA);
// cv::cvtColor(polymask, polymask, CV_BGR2BGRA);
//cv::rectangle(mask, Point( 0, 0 ), Point( mask.cols, mask.rows), Scalar( 0, 255,0,255 ),-1, 8 ); //Fill all mask in
polymask.setTo(Scalar(0,0,0,0));
//Polgon Masking
polygon = paintCanvas->polygons.at(i);
Point poly_points[polygon.size()];
//Find point furthest from center
Point furthest = Point(paintCanvas->centerPoint.x()*xscale,paintCanvas->centerPoint.y()*yscale); //set to center
int scaledcenterx = paintCanvas->centerPoint.x()*xscale;
int scaledcentery = paintCanvas->centerPoint.y()*yscale;
int scaledheadx= paintCanvas->headPoint.x()*xscale;
int scaledheady=paintCanvas->headPoint.y()*yscale;
float biggestdistancesquared=0;
for(int j=0;j<polygon.size();j++)
{
poly_points[j]=Point(xscale*polygon.at(j).x(), yscale*polygon.at(j).y());
Point candidate = Point(xscale*polygon.at(j).x(), yscale*polygon.at(j).y());
float distancecandidatesquared;
//Find furthest
distancecandidatesquared= (candidate.x - scaledcenterx)*(candidate.x - scaledcenterx) + (candidate.y - scaledcentery)*(candidate.y - scaledcentery);
if(distancecandidatesquared>biggestdistancesquared){
biggestdistancesquared=distancecandidatesquared;
qDebug()<<"biggcandidate x "<<candidate.x <<" y "<<candidate.y << " distance ="<<biggestdistancesquared;
}
}
const Point* ppt[1] = { poly_points };
int npt[] = { polygon.size() };
fillPoly( polymask,
ppt,
npt,
1,
Scalar( 255, 255,255,255 ),
8,
0);
//Debug
// cv::circle(origframe,cv::Point(scaledcenterx,scaledcentery),1,Scalar(255,255,255,255),2);
// cv::circle(origframe,cv::Point(scaledheadx,scaledheady),1,Scalar(255,0,255, 254),2);
//cv::circle(polymask,cv::Point(scaledcenterx,scaledcentery),1,Scalar(255,255,255,255),2);
// cv::circle(polymask,cv::Point(scaledheadx,scaledheady),1,Scalar(255,0,255, 254),2);
// cv::circle(subtractedframe,cv::Point(scaledcenterx,scaledcentery),1,Scalar(255,255,255,255),2);
// cv::circle(subtractedframe,cv::Point(scaledheadx,scaledheady),1,Scalar(255,0,255, 254),2);
//background subtraction: take original image, apply background as a mask, save over original
//bitwise_and(subtractedframe, polymask, subtractedframe);
qDebug()<<"Roi "<<x1<<" "<<y1<<" "<<x2<<" "<<y2<<" ";
cv::cvtColor(polymask,polymask, CV_RGB2GRAY);
//Full alpha mask = polygon selection (a =200) + BG subtracted organism (a= 255) + Center Mark ( a = 250) + head mark (a = 240)
//Set Head to alpha=240
//Set Center to Alpha = 250
//Everything inside mask == alpha 200
//Everything outside alpha=100;
//BG subtracted ant = 255
Mat maskedsubtraction;
subtractedframe.copyTo(maskedsubtraction,polymask); // note that m.copyTo(m,mask) will have no masking effect
cvtColor(maskedsubtraction, maskedsubtraction,CV_BGR2GRAY);
polymask = polymask+155; //255 moves to 255, 0 moves to 155
polymask = polymask - 55; //255 moves to 200, 155 moves to 100
maskedsubtraction = polymask+maskedsubtraction;
cv::circle(maskedsubtraction,cv::Point(scaledcenterx,scaledcentery),1,Scalar(250),2); //Encode the Center
cv::circle(maskedsubtraction,cv::Point(scaledheadx,scaledheady),1,Scalar(240),2); //encode the head
Mat bgr;
bgr=origframe.clone();
Mat alpha;
maskedsubtraction.copyTo(alpha);
Mat bgra;
cvtColor(origframe, bgra,CV_BGR2BGRA); //Copy the origframe, we'll write over it next
// forming array of matrices is quite efficient operations,
// because the matrix data is not copied, only the headers
Mat in[] = { bgr, alpha };
// BGRa[0] -> bgr[0], BGRa[1] -> bgr[1],
// BGRa[2] -> bgr[2], BGRa[3] -> alpha[0]
int from_to[] = { 0,0, 1,1, 2,2, 3,3 };
mixChannels( in, 2, &bgra, 1, from_to, 4 ); // input array, number of files in input array, destination, number of files in destination, from-to array, number of pairs in from-to
QString ext = ".png";
// QString fullframe = savepath+paintCanvas->polyNames.at(i)+"_"+QString::number(centroids[i].x())+"_"+QString::number(centroids[i].y())+"_"+QString::number(currentFrame)+ext;
QString modelfilename = savepath+paintCanvas->polyNames.at(i)+"_f"+QString::number(currentFrame)+ext;
//DEBUG IMAGES
/*
imwrite(modelfilename.toStdString()+"_subtraction",subtractedframe);
imwrite(modelfilename.toStdString()+"_polymask",polymask);
imwrite(modelfilename.toStdString()+"_alpha",alpha);
*/
//save out Model
//Full Keyframe
// imwrite(modelfilename.toStdString()+"_keyframe",bgra); //Disabled for now
qDebug()<<"Saved out: "<<modelfilename;
//***Crop and Rotate ***//
qDebug()<<"crop centered on "<<scaledcenterx<<" "<<scaledcentery;
//crop the frame based on ROI
Point2f src_center(scaledcenterx, scaledcentery);
//To do this correctly use getRectSubPix instead of frameMat(MYROI) method
// getRectSubPix only works with certain image formats (this is undocumented in opencv : P
// so we have to do that cutting and mixing again!
getRectSubPix(bgr, cv::Size(sqrt(biggestdistancesquared)*2,sqrt(biggestdistancesquared)*2), src_center, bgr);
getRectSubPix(alpha, cv::Size(sqrt(biggestdistancesquared)*2,sqrt(biggestdistancesquared)*2), src_center, alpha);
Mat bgracropped;
cvtColor(bgr, bgracropped,CV_BGR2BGRA); //Copy the origframe, we'll write over it next
Mat inagain[] = { bgr, alpha };
int from_to2[] = { 0,0, 1,1, 2,2, 3,3 };
//Note: the height and width dimensions have to be the same for the inputs and outputs
mixChannels( inagain, 2, &bgracropped, 1, from_to2, 4 ); // input array, number of files in input array, destination, number of files in destination, from-to array, number of pairs in from-to
//rotate the cropped frame about the center of the cropped frame.
qDebug()<<"Rotate that image "<<angle;
bgracropped = rotateImage(bgracropped, angle);//Rotate full image about this center
//after I rotate clear the global angle
angle =0;
//debug
angle=-1;
// Save the Nicely Rotated and Cropped Model File
imwrite(modelfilename.toStdString(),bgracropped);
centroids.clear();
maxXY.clear();
minXY.clear();
paintCanvas->polyNames.clear();
paintCanvas->polygons.clear();
paintCanvas->masks.pop_back();
polyinfo = "Polygon cleared";
paintCanvas->temp.clear();
ui->statusBar->showMessage(polyinfo,2000);
paintCanvas->replyMask = replyNull;
capture.set(CV_CAP_PROP_POS_FRAMES,(double)currentFrame);
}
}
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);
}
