ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange

# distance between a line segment and a point in a map

I assume that my robot is an rectangle so want to know the position of each corner point of my rectangle. So as I understand to get the position and orientation I need these two tf transforms. I wont the have the position and orientation of all four corner point of the rectangle . My rectangle have dimension 60x90cm . The center of the base_link frame is the center of the rectangle.After that I create the 4 line segments . So to calculate the distance between the line segment and the point in the map I tried this code. Bit not sure that is ok. This is my node

Any help?

tf::Stamped<tf::Pose> corner1(
tf::Pose(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.30, -0.45, 0.0)),
tf::Stamped<tf::Pose> corner2(
tf::Pose(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.30, 0.45, 0.0)),
tf::Stamped<tf::Pose> corner3(
tf::Pose(tf::Quaternion(0, 0, 0, 1), tf::Vector3(-0.30, -0.45, 0.0)),
tf::Stamped<tf::Pose> corner4(
tf::Pose(tf::Quaternion(0, 0, 0, 1), tf::Vector3(-0.30, 0.45, 0.0)),

tf::Stamped<tf::Pose> transformed_corner_1;
transform_listener.transformPose("map", corner1, transformed_corner_1);
tf::Stamped<tf::Pose> transformed_corner_2;
transform_listener.transformPose("map", corner2, transformed_corner_2);
tf::Stamped<tf::Pose> transformed_corner_3;
transform_listener.transformPose("map", corner3, transformed_corner_3);
tf::Stamped<tf::Pose> transformed_corner_4;
transform_listener.transformPose("map", corner4, transformed_corner_4);

double x_vect_a = transformed_corner_1.getOrigin().x();
double y_vect_a = transformed_corner_1.getOrigin().y();

double x_vect_b = transformed_corner_2.getOrigin().x();
double y_vect_b = transformed_corner_2.getOrigin().y();

double x_vect_c = transformed_corner_3.getOrigin().x();
double y_vect_c = transformed_corner_3.getOrigin().y();

double x_vect_d = transformed_corner_4.getOrigin().x();
double y_vect_d = transformed_corner_4.getOrigin().y();

double door_x = transform.getOrigin.x();
double door_y = transform.getOrigin.y();

double r_numerator = (cx-ax)*(bx-ax) + (cy-ay)*(by-ay);
double r_denomenator = (bx-ax)*(bx-ax) + (by-ay)*(by-ay);
double r = r_numerator / r_denomenator;

double px = ax + r*(bx-ax);
double py = ay + r*(by-ay);

double s =  ((ay-cy)*(bx-ax)-(ax-cx)*(by-ay) ) / r_denomenator;

distanceLine = fabs(s)*sqrt(r_denomenator);


// (xx,yy) is the point on the lineSegment closest to (cx,cy)

double xx = px;
double yy = py;

if ( (r >= 0) && (r <= 1) )
{
distanceSegment = distanceLine;
}
else
{

double dist1 = (cx-ax)*(cx-ax) + (cy-ay)*(cy-ay);
double dist2 = (cx-bx)*(cx-bx) + (cy-by)*(cy-by);
if (dist1 < dist2)
{
xx = ax;
yy = ay;
distanceSegment = sqrt(dist1);
}
else
{
xx = bx;
yy = by;
distanceSegment = sqrt(dist2);
}

edit retag close merge delete

So, what's your question? Also, your source code is not complete (some variables neither defined nor explained...). Unable to answer question.

( 2012-08-26 23:39:54 -0600 )edit

So my question is. If transformed_corner_1.getOrigin() is the position vector in the map frame of the line segment (from the 4 corners of my robot rectangle), than how to get the minimum distance between the fixed point in the map and that line segment. Understand?

( 2012-08-27 01:30:45 -0600 )edit

Sort by » oldest newest most voted

If transformed_corner_1.getOrigin() is the position vector in the map frame of the line segment (from the 4 corners of my robot rectangle), than how to get the minimum distance between the fixed point in the map and that line segment. Understand?

Transform everything into the map frame, then calculate the point-line distance. Also calculate the distance to both end points and take the minimum of the three. Understand?

more

But Why to calculate the distance to the both end point. There is only one minimum distance from a line segment to a point. Understand?? So in my case I think have to calculate the 4 distances (from the 4 line segments) to the fixed point and take the minimum one.

( 2012-08-27 02:41:45 -0600 )edit

And ho to declarete the line segement. Is it transformed_corner_1.getOrigin()?? Or I should get thetransformed_corner_1.getOrigin(), x() and transformed_corner_1.getOrigin().y() the x,y coordinate of all 4 corners and than calculate the distance?

( 2012-08-27 02:43:18 -0600 )edit

So what are the coordinates (x,y) of my line segment in the map frame??? Is that transformed_corner_1.getOrigin(),x() and transformed_corner_1.getOrigin().y()??

( 2012-08-27 02:58:21 -0600 )edit

> Is that transformed_corner_1.getOrigin(),x() and transformed_corner_1.getOrigin().y()??

Yes.

( 2012-08-27 04:05:30 -0600 )edit

So just to get the distance of the line segment transformed_corner_1.getOrigin() and one point in the map. Wright?

( 2012-08-27 04:09:54 -0600 )edit