point Align::nearestPoint(struct point intersection, struct model line, double dist){
point p1 = point();
point p2 = point();
p1.x = (1.0/(sqrt(1.0 + line.m * line.m))) * dist + intersection.x;
p1.y = line.m/(sqrt(1.0 + line.m * line.m)) * dist + intersection.y;
p2.x = -1.0/(sqrt(1.0 + line.m * line.m)) * dist + intersection.x;
p2.y = -line.m/(sqrt(1.0 + line.m * line.m)) * dist + intersection.y;
point origin = point();
origin.x = 0.0;
origin.y = 0.0;
if(distancePoint(p1, origin) >= distancePoint(p2, origin)){
return p2;
}else{
return p1;
}
}
//kk89 eq 16
// second derivate after y_m
qreal Graph::del2_E__del_y2m(Vertex *m)
{
qreal result = 0;
//qDebug() << "begin del2_E__del_y2m" << result;
QPointF mp = m->nodePos();
for(QMap<uint,Vertex*>::const_iterator i = m_vertices.constBegin();
i != m_vertices.constEnd(); ++i )
{
if( *i == m )
continue;
QPointF ip = (*i)->nodePos();
qreal top = kij( m, *i ) * pow( mp.x() - ip.x(), 2.0 );
qreal bot = pow( distancePoint( mp, ip ), 3.0 );
qreal gravity = 3*pow( mp.y() - ip.y(), 2.0 ) / pow( distancePoint( mp, ip ), 5.0 ) - 1 / pow( distancePoint( mp, ip ), 3.0 );
result += kij( m,*i ) * (1.0 - top/bot) + gravity;
//qDebug() << "del2_E__del_y2m" << result;
}
//qDebug() << "final del2_E__del_y2m" << result;
return result;
}
//kk89 eq 8
qreal Graph::del_E__del_ym(Vertex *m)
{
qreal result = 0;
//qDebug() << "begin del_E__del_ym" << result;
QPointF mp = m->nodePos();
for(QMap<uint,Vertex*>::const_iterator i = m_vertices.constBegin();
i != m_vertices.constEnd(); ++i )
{
if( *i == m )
continue;
QPointF ip = (*i)->nodePos();
qreal top = kij( m, *i ) * ( mp.y() - ip.y() );
qreal bot = distancePoint( mp, ip );
qreal gravity = -1* force * ( mp.y() - ip.y() ) / pow(distancePoint(mp, ip), 3.0);
//qDebug() << "\tkij( m,*i )" << kij( m,*i );
//qDebug() << "\t( mp.y() - ip.y() )" << ( mp.y() - ip.y() );
//qDebug() << "\ttop/bot" << top << bot << top/bot;
result += kij( m,*i ) * ( ( mp.y() - ip.y() ) - top / bot) + gravity;
//qDebug() << "del_E__del_ym" << result;
}
//qDebug() << "final del_E__del_ym" << result;
return result;
}
void Align::solve()
{
ROS_INFO("Calculating middle");
point intersection = point();
int w2 = 0;
point origin = point();
origin.x = 0.0;
origin.y = 0.0;
for(int i = 1; i < walls.size(); i++){
intersection.x = (walls.at(i).c - walls.at(0).c)/(walls.at(0).m - walls.at(i).m);
intersection.y = walls.at(0).m * intersection.x + walls.at(0).c;
if(distancePoint(intersection, origin) <= 1.5){
w2 = i;
break;
}
}
ROS_INFO("Intersection found X: %f Y: %f", intersection.x, intersection.y);
point diag1 = nearestPoint(intersection, walls.at(0), 0.78);
point diag2 = nearestPoint(intersection, walls.at(w2), 0.78);
point result = point();
ROS_INFO("Diag1 X: %f Y: %f", diag1.x, diag1.y);
ROS_INFO("Diag2 X: %f Y: %f", diag2.x, diag2.y);
result.x = diag1.x + (diag2.x - diag1.x)/2;
result.y = diag1.y + (diag2.y - diag1.y)/2;
ROS_INFO("RESULT X: %f Y: %f", result.x, result.y);
ROS_INFO("ANGLE: %f DIST: %f", atan2(result.y , result.x), sqrt(result.x * result.x + result.y * result.y));
double angle = atan2(result.y,result.x);
if(angle < M_PI/2 && angle > 0.0){
rotateRight(M_PI/2 - angle);
} else if(angle < M_PI){
rotateLeft(angle - M_PI/2);
} else if(angle < 2 * M_PI){
rotateRight((2 * M_PI - angle) + M_PI/2);
} else if(angle < 0.0){
rotateRight(-1.0 * angle + M_PI/2);
}
move(sqrt(result.x * result.x + result.y * result.y));
ROS_INFO("FINITO!!!");
}
void getEndPointOfLine(const Mat src,const vector<electronComponent> DeviceSet,vector<Cline> &circuitLines){
//binary
Mat binary;
double thre = 10;
int mode = CV_THRESH_BINARY_INV;
binaryImage(src, binary, thre, mode);
//排除元器件，将元器件涂黑
Mat srcB = src.clone();
for(int i = 0;i <DeviceSet.size();i++){
electronComponent DeviceInfo = DeviceSet[i];
vector<vector<Point>> tmp;
tmp.push_back(DeviceSet[i].contours);
drawContours(srcB, tmp, 0, Scalar(0),CV_FILLED);
}
//线的细化
Mat bin;
int intera = 8;
imageThin(binary,bin,intera);
//寻找曲线的端点
vector<vector<Point> > contours;
Mat copyBin;
bin.copyTo(copyBin);
findContours(copyBin, contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
vector<Cline> lines;
for (size_t i = 0; i < contours.size(); i++) {
if (contours[i].size() < 10) {
continue;
}
vector<Point> endPoint;
findEndPoint(bin,contours[i], endPoint);
if (endPoint.size() > 0) {
Cline line;
line.numPoint = (int)endPoint.size();
for (size_t k = 0;k < endPoint.size();k++) {
line.endPoint.push_back(endPoint[k]);
}
lines.push_back(line);
}
}
//找与线连接的半圆
Mat binaryB = Mat::zeros(srcB.rows, srcB.cols, CV_8UC1);
int value1 = 200;
int value2 = 20;
for (int i = 0; i < srcB.rows; i++) {
for (int j = 0; j < srcB.cols; j++) {
if (!(srcB.at<Vec3b>(i,j)[0] >= value1 && srcB.at<Vec3b>(i,j)[1] >= value1 &&srcB.at<Vec3b>(i,j)[2] >= value1 ) && !(srcB.at<Vec3b>(i,j)[0] <= value2 && srcB.at<Vec3b>(i,j)[1] <= value2 &&srcB.at<Vec3b>(i,j)[2] <= value2)) {
binaryB.at<uchar>(i,j) = 255;
}
}
}
medianBlur(binaryB, binaryB, 5);
vector<CSemicircle> semicircles;
//求半圆的半径 圆心
vector<vector<Point> > contoursb;
Mat copyBinaryb = binaryB.clone();
#ifdef _SHOW_
Mat binaryBrgb;
cvtColor(binaryB,binaryBrgb, CV_GRAY2BGR);
#endif
findContours(copyBinaryb, contoursb, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
for (int k = 0; k < contoursb.size(); k++) {
CSemicircle semi;
float radius;
Point2f center;
minEnclosingCircle(contoursb[k],center, radius);
#ifdef _SHOW_
circle(binaryBrgb, center, radius, Scalar(0,0,255));
#endif
semi.center = center;
semi.radius = radius;
semicircles.push_back(semi);
}
//连接曲线与半圆
//1 首先将连接的直线连接起来
vector<Cline> newLines;
float threRadius = 1.5;
for (int i = 0; i < lines.size(); i++) {
Cline line;
line.numPoint = lines[i].numPoint;
for (int j = 0 ; j < lines[i].numPoint; j++ ) {
for (int k = 0; k < semicircles.size(); k++) {
if (distancePoint(lines[i].endPoint[j], semicircles[k].center) < threRadius * semicircles[k].radius) {
line.endPoint.push_back(semicircles[k].center);
break;
}
}
}
newLines.push_back(line);
}
#ifdef _SHOW_
Mat rgb2 = src.clone();
for (int i = 0; i < newLines.size(); i++) {
for (int j = 0 ; j < newLines[i].numPoint; j++) {
circle(rgb2,newLines[i].endPoint[j], 3, Scalar(0,0,255),CV_FILLED);
}
}
imshow("rgb2", rgb2);
imshow("ConnectPart",binaryBrgb);
waitKey();
std::cout << "newLines: " << newLines.size() << std::endl;
#endif
//合并
for (int i = 0; i < newLines.size() ; i++) {
int flag = 0;
for (int j = i+1; j < newLines.size(); j++) {
for (int m = 0 ; m < newLines[i].numPoint; m++) {
for (int n = 0; n < newLines[j].numPoint; n++) {
if (newLines[i].endPoint[m] == newLines[j].endPoint[n]) {
vector<Point> newEndPoint;
for (int km = 0; km < newLines[i].numPoint; km++) {
if (km == m) {
continue;
}
newEndPoint.push_back(newLines[i].endPoint[km]);
}
for (int kn = 0; kn < newLines[j].numPoint; kn++) {
if (kn == n) {
continue;
}
newEndPoint.push_back(newLines[j].endPoint[kn]);
}
newLines[i].endPoint.clear();
for (int k = 0; k < newEndPoint.size(); k++) {
newLines[i].endPoint.push_back(newEndPoint[k]);
}
newLines[i].numPoint = (int)newLines[i].endPoint.size();
newLines[j].numPoint = 0;
newLines[j].endPoint.clear();
flag = 1;
break;
}
}
if (flag == 1) {
break;
}
}
flag = 0;
}
}
for (int i = 0; i < newLines.size(); i++) {
if (newLines[i].numPoint == 0) {
continue;
}
circuitLines.push_back(newLines[i]);
}
#ifdef _SHOW_
Mat rgb3 = src.clone();
for (int i = 0; i < circuitLines.size(); i++) {
for (int j = 0 ; j < circuitLines[i].numPoint; j++) {
circle(rgb3,circuitLines[i].endPoint[j], 3, Scalar(0,0,255),CV_FILLED);
}
imshow("rgb3", rgb3);
waitKey();
}
std::cout << "circuitLines: " << circuitLines.size() << std::endl;
#endif
}
inline qreal Graph::distanceVertex( Vertex *i, Vertex *j )
{
// sqrt( (i.x-j.x)^2 + (i.y - j.y)^2 )
return distancePoint( i->nodePos(), j->nodePos() );
}
void Align::ransac()
{
ROS_INFO("RANSAC TIME");
srand (time(NULL));
while(walls.size() < 4 && points.size() > 200){
int iterations = 0;
model bestfit;
int besterr = 0; ///Besterror is the amount of points fitting the model
ROS_INFO("RANSAC Nr %d", walls.size());
while(iterations < 10000){
point p1 = points.at(rand() % (points.size()-1) + 0);
point p2 = points.at(rand() % (points.size()-1) + 0);
model maybemodel;
int thiserr = 0;
maybemodel.m = (p2.y - p1.y)/(p2.x - p1.x);
maybemodel.c = -maybemodel.m * p1.x + p1.y;
for(int i = 0; i < points.size(); i++){
if(distance(points.at(i), maybemodel) < 0.02){
thiserr++;
}
}
if(thiserr > 200 && thiserr > besterr){
ROS_INFO("New Bestmodel! %d error: %d", iterations, thiserr);
bestfit = maybemodel;
besterr = thiserr;
}
iterations++;
//ROS_INFO("Model worked %d error: %d", iterations, thiserr);
}
if(besterr >= 200){
walls.push_back(bestfit);
counter = 0;
std::vector<struct point> newPoints;
for(int i = 0; i < points.size(); i++){
if(distance(points.at(i), bestfit) >= 0.02){
newPoints.push_back(point());
newPoints[counter].x = points.at(i).x;
newPoints[counter].y = points.at(i).y;
counter++;
}
}
points = newPoints;
ROS_INFO("bestfit: %f *x + %f error: %d >>>WALLS: %d", bestfit.m, bestfit.c, besterr, walls.size());
ROS_INFO("New points size: %d", points.size());
}else{
ROS_INFO("No acceptable bestfit found");
break;
}
//ros::Duration(7).sleep();
}
position = 5;
if(walls.size() > 1){
solve();
}else{
point reference = point();
//point reference2 = point();
reference.x = 100.0;
reference.y = 100.0;
point origin = point();
origin.x = 0.0;
origin.y = 0.0;
counter = 0;
for(int i = 0; i < points.size(); i++){
if(distancePoint(points.at(i), origin) < distancePoint(reference, origin)){
reference.x = points.at(i).x;
reference.y = points.at(i).y;
counter++;
}
}
model newLine = model();
newLine.m = -1/walls.at(0).m;
newLine.c = reference.y - newLine.m * reference.x;
walls.push_back(newLine);
solve();
}
}
