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

distance between a line segment and a point in a map

asked 2012-08-23 21:49:33 -0500

Astronaut gravatar image

updated 2012-08-24 01:08:40 -0500

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)),
                                ros::Time(0), "base_link");
        tf::Stamped<tf::Pose> corner2(
            tf::Pose(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.30, 0.45, 0.0)),
                                ros::Time(0), "base_link");
        tf::Stamped<tf::Pose> corner3(
            tf::Pose(tf::Quaternion(0, 0, 0, 1), tf::Vector3(-0.30, -0.45, 0.0)), 
                                ros::Time(0), "base_link");
        tf::Stamped<tf::Pose> corner4(
            tf::Pose(tf::Quaternion(0, 0, 0, 1), tf::Vector3(-0.30, 0.45, 0.0)), 
                                ros::Time(0), "base_link");

    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 flag offensive close merge delete

Comments

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

Martin Günther gravatar image Martin Günther  ( 2012-08-26 23:39:54 -0500 )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?

Astronaut gravatar image Astronaut  ( 2012-08-27 01:30:45 -0500 )edit

1 Answer

Sort by » oldest newest most voted
0

answered 2012-08-27 02:06:59 -0500

updated 2012-08-27 02:08:43 -0500

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?

edit flag offensive delete link more

Comments

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.

Astronaut gravatar image Astronaut  ( 2012-08-27 02:41:45 -0500 )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?

Astronaut gravatar image Astronaut  ( 2012-08-27 02:43:18 -0500 )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()??

Astronaut gravatar image Astronaut  ( 2012-08-27 02:58:21 -0500 )edit

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

Yes.

Martin Günther gravatar image Martin Günther  ( 2012-08-27 04:05:30 -0500 )edit

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

Astronaut gravatar image Astronaut  ( 2012-08-27 04:09:54 -0500 )edit

Question Tools

Stats

Asked: 2012-08-23 21:49:33 -0500

Seen: 1,577 times

Last updated: Aug 27 '12