void ResizeController::updatePosition()
{
if (isValid()) {
QRectF boundingRect = m_data->formEditorItem->qmlItemNode().instanceBoundingRect();
QPointF topLeftPointInLayerSpace(m_data->formEditorItem->mapToItem(m_data->layerItem.data(),
boundingRect.topLeft()));
QPointF topRightPointInLayerSpace(m_data->formEditorItem->mapToItem(m_data->layerItem.data(),
boundingRect.topRight()));
QPointF bottomLeftPointInLayerSpace(m_data->formEditorItem->mapToItem(m_data->layerItem.data(),
boundingRect.bottomLeft()));
QPointF bottomRightPointInLayerSpace(m_data->formEditorItem->mapToItem(m_data->layerItem.data(),
boundingRect.bottomRight()));
QPointF topPointInLayerSpace(m_data->formEditorItem->mapToItem(m_data->layerItem.data(),
topCenter(boundingRect)));
QPointF leftPointInLayerSpace(m_data->formEditorItem->mapToItem(m_data->layerItem.data(),
leftCenter(boundingRect)));
QPointF rightPointInLayerSpace(m_data->formEditorItem->mapToItem(m_data->layerItem.data(),
rightCenter(boundingRect)));
QPointF bottomPointInLayerSpace(m_data->formEditorItem->mapToItem(m_data->layerItem.data(),
bottomCenter(boundingRect)));
m_data->topRightItem->setHandlePosition(topRightPointInLayerSpace, boundingRect.topRight());
m_data->topLeftItem->setHandlePosition(topLeftPointInLayerSpace, boundingRect.topLeft());
m_data->bottomLeftItem->setHandlePosition(bottomLeftPointInLayerSpace, boundingRect.bottomLeft());
m_data->bottomRightItem->setHandlePosition(bottomRightPointInLayerSpace, boundingRect.bottomRight());
m_data->topItem->setHandlePosition(topPointInLayerSpace, topCenter(boundingRect));
m_data->leftItem->setHandlePosition(leftPointInLayerSpace, leftCenter(boundingRect));
m_data->rightItem->setHandlePosition(rightPointInLayerSpace, rightCenter(boundingRect));
m_data->bottomItem->setHandlePosition(bottomPointInLayerSpace, bottomCenter(boundingRect));
}
}
geometry_msgs::Pose2D findTarget(cv::Mat img) {
cv::Mat cdst, mdst;
mdst = img - img;
std::vector<cv::Vec4i>lines;
cv::Point center_point;
center_point.x = 0, center_point.y = 0;
double center_angle = 0.0;
cdst = img;
cv::Canny(img, cdst, 25, 75, 3);
cv::HoughLinesP(cdst, lines, 1, CV_PI / 180, 25, 15, 5);
int image_halfy=0;
cv::Point top(0,0), bottom(0,0);
int c=0;
/*for(int i=0;i<img.rows;i++)
{
for(int j=0;j<img.cols;j++)
{
if(img.at<uchar>(i,j)>200)
{
image_halfy=i/2;
}
}
}*/
image_halfy=img.rows-image_halfy;
for(int i=0/*image_halfy*/;i<img.rows;i++)
{
for(int j=0;j<img.cols;j++)
{
if(img.at<uchar>(i,j)>200 /*&& img.at<uchar>(i,j)[1]>200 && img.at<uchar>(i,j)[2]>200*/)
{
c++;
top.x+=j;
top.y+=i;
}
if(c==20)
break;
}
if(c==20)
break;
}
top.x/=20;
top.y/=20;
top.y-=img.rows;
c=0;
for(int i=img.rows-1;i>=0/*image_halfy*/;i--)
{
for(int j=0;j<img.cols;j++)
{
if(img.at<uchar>(i,j)>200 /*&& img.at<uchar>(i,j)[1]>200 && img.at<uchar>(i,j)[2]>200*/)
{
c++;
bottom.x+=j;
bottom.y+=i;
}
if(c==20)
break;
}
if(c==20)
break;
}
bottom.x/=20;
bottom.y/=200;
bottom.y-=img.rows;
for (int i = 0; i < lines.size(); i++) {
cv::Vec4i l = lines[i];
cv::line(mdst, cv::Point(l[0], l[1]), cv::Point(l[2], l[3]), cv::Scalar(255, 255, 255), 3, CV_AA);
}
for (int i = 0; i < lines.size(); i++) {
cv::Vec4i p = lines[i];
/*if(((p[1]+p[3])/2)>image_halfy)
continue;*/
center_point.x += (p[0] + p[2]) / 2;
center_point.y += (p[1] + p[3]) / 2;
if ((p[1] - p[3]) != 0) {
double theta = std::atan2((p[1] - p[3]), (p[0] - p[2]));
if (theta < 0)
theta = 3.14 + theta;
center_angle += (theta);
}
else
{
center_angle+=1.57;
}
}
if (debug)std::cout << "Total Angle" << center_angle << std::endl;
if (lines.size() != 0) {
center_angle = center_angle / lines.size();
center_point.x = center_point.x / lines.size();
center_point.y = center_point.y / lines.size();
if (debug)std::cout << center_point.x << " " << center_point.y << std::endl;
}
else
{
center_angle= 1.57;
center_point.x= bot_x;
center_point.y= bot_y;
}
double m = tan(center_angle);
if (m == HUGE_VAL) {
m = 10000;
}
cv::Point leftCenter(0, 0), rightCenter(0, 0);
double leftSlope = 0.0, rightSlope = 0.0, leftCount = 0, rightCount = 0;
cv::Point proj,target;
geometry_msgs::Pose2D target_pose;
if((bottom.x-top.x)>90 || (top.x-bottom.x)>90)
{
if(bottom.x>top.x)
{
bottom.x-=40;
top.x-=40;
if(top.x<0)
top.x=0;
proj.x = (bot_x + m * (bot_y - center_point.y) + m * m * center_point.x) / (1 + m * m); // Verified
proj.y = (center_point.y + m * (bot_x - center_point.x) + m * m * bot_y) / (1 + m * m); // Verify it
target.x += proj.x + cos(center_angle) * step_move -40;
if(target.x<0)
target.x=0;
target.y = m*(top.x-bottom.x)+bottom.y;
}
if(bottom.x<top.x)
{
bottom.x+=40;
top.x+=40;
if(top.x>img.cols)
top.x=img.cols;
proj.x = (bot_x + m * (bot_y - center_point.y) + m * m * center_point.x) / (1 + m * m); // Verified
proj.y = (center_point.y + m * (bot_x - center_point.x) + m * m * bot_y) / (1 + m * m); // Verify it
target.x += proj.x + cos(center_angle) * step_move +40;
if(target.x>img.cols)
target.x=img.cols;
target.y = m*(top.x-bottom.x)+bottom.y;
}
center_angle = -1 * center_angle * 180 / CV_PI;
//std::cout<<"new\n";
target_pose.x = target.x;
target_pose.y = (-1 * target.y + origin.y);
target_pose.theta = center_angle;
}
else{
for (int i = 0; i < lines.size(); i++)
{
cv::Vec4i p = lines[i];
cv::Point midPoint = cv::Point((p[0] + p[2]) / 2, (p[1] + p[3]) / 2);
/*if(midPoint.y>image_halfy)
continue;*/
if(m==0)
continue;
double L11 = (0 - center_point.y) / m - (0 - center_point.x);
double L22 = (midPoint.y - center_point.y) / m - (midPoint.x - center_point.x + 0.0);
if (L11 * L22 > 0) {
leftCenter.x += midPoint.x;
leftCenter.y += midPoint.y;
if ((lines[i][0] - lines[i][2]) != 0) {
leftSlope += -(lines[i][1] - lines[i][3]) / (lines[i][0] - lines[i][2]);
}
leftCount++;
}
else {
rightCenter.x += midPoint.x;
rightCenter.y += midPoint.y;
if ((lines[i][0] - lines[i][2]) != 0) {
rightSlope += -(lines[i][1] - lines[i][3]) / (lines[i][0] - lines[i][2]);
}
rightCount++;
}
}
if(leftCount!=0 && rightCount!=0){
leftCenter.x /= leftCount; //Load when count==0
leftCenter.y /= leftCount;
leftSlope /= leftCount;
rightCenter.x /= rightCount;
rightCenter.y /= rightCount;
rightSlope /= rightCount;
if ((center_point.x - leftCenter.x) < 50 || (leftCenter.x - center_point.x) < 50) {
center_point.x += 150; //Target must not lie on the lane
target.x = 150;
}
if ((rightCenter.x - center_point.x) < 50 || (center_point.x - rightCenter.x) < 50) {
center_point.x = center_point.x - 150;
target.x -= 150;
}
}
proj.x = (bot_x + m * (bot_y - center_point.y) + m * m * center_point.x) / (1 + m * m); // Verified
proj.y = (center_point.y + m * (bot_x - center_point.x) + m * m * bot_y) / (1 + m * m); // Verify it
target.x += proj.x + cos(center_angle) * step_move;
target.y = proj.y + sin(center_angle) * step_move;
cv::Point point2;
point2.x = center_point.x - 100;
point2.y = center_point.y - m * 100;
center_angle = -1 * center_angle * 180 / CV_PI;
target_pose.x = target.x;
target_pose.y = (-1 * target.y + origin.y);
target_pose.theta = center_angle;
//cv::Point a(target_pose.x,target_pose.y);
//cv::circle(img,a,2,cv::Scalar(0,0,255),1,8,0);
//cv::namedWindow("new",CV_WINDOW_AUTOSIZE);
//imshow("new!",img);
}
/*if(old_target.x!=0 || old_target.y!=0 || old_target.theta!=0)
{
target_pose.x=0.8*float(target_pose.x) + 0.2*float(old_target.x);
target_pose.theta=0.8*float(target_pose.theta) + 0.2*float(old_target.theta);
std::cout<<"in";
target.x=target_pose.x;
target.y=origin.y - target_pose.y;
if(target.y<60 || target.y>700)
target.y=230;
}*/
double valx=target_pose.x;
double valy=target_pose.y;
int valtheta=target_pose.theta;
for(int i=0;i<counter;i++)
{
valx+=(i+2)*(temp[i]).x;
valy+=(i+2)*(temp[i]).y;
valtheta+=(i+2)*(temp[i]).theta;
}
target_pose.x=int(valx/15);
target_pose.y=int(valy/15);
target_pose.theta=int(valtheta/15);
target.x=target_pose.x;
target.y=origin.y - target_pose.y;
if(target.y<60 || target.y>700)
{ target.y=230;
target_pose.y=origin.y - target.y;
}
if (debug)
{
std::cout << "target.x: " << target.x << " target.y: " << target.y << std::endl;
std::cout << "proj.x: " << proj.x << " " << "proj.y: " << proj.y << std::endl;
//cv::line(mdst, proj, target, cv::Scalar(150),2,8);
cv::line(mdst, cv::Point(bot_x, bot_y), target, cv::Scalar(255), 2, 8);
cv::namedWindow("Center_path", cv::WINDOW_NORMAL);
cv::imshow("Center_path", mdst);
// cv::waitKey(0);
}
return target_pose;
}
geometry_msgs::Pose2D findTarget(cv::Mat img) {
cv::Mat cdst, mdst;
mdst = img - img;
std::vector<cv::Vec4i>lines;
cv::Point center_point;
center_point.x = 0, center_point.y = 0;
double center_angle = 0.0;
cdst = img;
cv::HoughLinesP(cdst, lines, 1, CV_PI / 180, 25, 15, 5);
int image_halfy=0;
cv::Point top(0,0), bottom(0,0);
int c=0;
for (int i = 0; i < lines.size(); i++) {
cv::Vec4i l = lines[i];
cv::line(mdst, cv::Point(l[0], l[1]), cv::Point(l[2], l[3]), cv::Scalar(255, 255, 255), 3, CV_AA);
}
for (int i = 0; i < lines.size(); i++) {
cv::Vec4i p = lines[i];
center_point.x += (p[0] + p[2]) / 2;
center_point.y += (p[1] + p[3]) / 2;
if ((p[1] - p[3]) != 0) {
double theta = std::atan2((p[1] - p[3]), (p[0] - p[2]));
if (theta < 0)
theta = 3.14 + theta;
center_angle += (theta);
}
else
{
center_angle+=1.57;
}
}
if (lines.size() != 0) {
center_angle = center_angle / lines.size();
center_point.x = center_point.x / lines.size();
center_point.y = center_point.y / lines.size();
}
else
{
center_angle= 1.57;
center_point.x= bot_x;
center_point.y= bot_y;
}
double m = tan(center_angle);
if (m == HUGE_VAL) {
m = 10000;
}
cv::Point leftCenter(0, 0), rightCenter(0, 0);
double leftSlope = 0.0, rightSlope = 0.0, leftCount = 0, rightCount = 0;
cv::Point proj,target;
geometry_msgs::Pose2D target_pose;
for (int i = 0; i < lines.size(); i++)
{
cv::Vec4i p = lines[i];
cv::Point midPoint = cv::Point((p[0] + p[2]) / 2, (p[1] + p[3]) / 2);
if(m==0)
continue;
double L11 = (0 - center_point.y) / m - (0 - center_point.x);
double L22 = (midPoint.y - center_point.y) / m - (midPoint.x - center_point.x + 0.0);
if (L11 * L22 > 0) {
leftCenter.x += midPoint.x;
leftCenter.y += midPoint.y;
if ((lines[i][0] - lines[i][2]) != 0) {
leftSlope += -(lines[i][1] - lines[i][3]) / (lines[i][0] - lines[i][2]);
}
leftCount++;
}
else {
rightCenter.x += midPoint.x;
rightCenter.y += midPoint.y;
if ((lines[i][0] - lines[i][2]) != 0) {
rightSlope += -(lines[i][1] - lines[i][3]) / (lines[i][0] - lines[i][2]);
}
rightCount++;
}
}
if(leftCount!=0 && rightCount!=0){
leftCenter.x /= leftCount;
leftCenter.y /= leftCount;
leftSlope /= leftCount;
rightCenter.x /= rightCount;
rightCenter.y /= rightCount;
rightSlope /= rightCount;
if ((center_point.x - leftCenter.x) < 50 || (leftCenter.x - center_point.x) < 50) {
center_point.x += 150; //Target must not lie on the lane
target.x = 150;
}
if ((rightCenter.x - center_point.x) < 50 || (center_point.x - rightCenter.x) < 50) {
center_point.x = center_point.x - 150;
target.x -= 150;
}
}
proj.x = (bot_x + m * (bot_y - center_point.y) + m * m * center_point.x) / (1 + m * m); // Verified
proj.y = (center_point.y + m * (bot_x - center_point.x) + m * m * bot_y) / (1 + m * m); // Verify it
target.x += proj.x + cos(center_angle) * step_move;
target.y = proj.y + sin(center_angle) * step_move;
center_angle = -1 * center_angle * 180 / CV_PI;
target_pose.x = target.x;
target_pose.y = (-1 * target.y + origin.y);
target_pose.theta = center_angle;
if(target_pose.x>img.cols)
target_pose.x=img.cols;
if(target_pose.y>img.rows)
target_pose.y=img.rows;
if(target_pose.y<0)
target_pose.y=0;
std::cout << "target.x: " << target_pose.x << " target.y: " << target_pose.y << std::endl;
//std::cout << "proj.x: " << proj.x << " " << "proj.y: " << proj.y << std::endl;
//cv::line(mdst, cv::Point(bot_x, bot_y), target, cv::Scalar(255), 2, 8);
//cv::namedWindow("Center_path", cv::WINDOW_NORMAL);
//cv::imshow("Center_path", mdst);
//cv::waitKey(0);
return target_pose;
}
