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)),
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);
}
So, what's your question? Also, your source code is not complete (some variables neither defined nor explained...). Unable to answer question.
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?