Society of Robots - Robot Forum

Software => Software => Topic started by: robvoi on October 02, 2013, 03:02:22 AM

Title: check which cells are covered by robot
Post by: robvoi on October 02, 2013, 03:02:22 AM
Hi,

for an occupancy grid (resolution 1cm) I want to calculate which cells are covered by the robots body - so I can set them as "not occupied".
The robot has the form of a rectangle.

What’s the mathematical approach on this?

Sonar and IR sensor modeling I did with simple trigonometry. But with this one I am a bit stuck.

Thanks
Robert
Title: Re: check which cells are covered by robot
Post by: Roman505 on October 02, 2013, 03:52:33 AM
Perhaps I have been enjoying myself too much this evening, but it is not clear to me what is your question :).

What data do you expect to have, on which we should calculate overlap or absence?
Title: Re: check which cells are covered by robot
Post by: robvoi on October 02, 2013, 04:17:00 AM
I have a rectangle with a given x/y pos and theta in an Cartesian coordinate system.
Given the position, length and width of the robot I want to know which coordinates are covered by the robots body so I can set them as known to be free of obstacles.

I have a solution now – but it’s not elegant:


Not the complete code but the approach should be visible:
Code: [Select]
void setRobotPosAsFree() {
  float[] LeftBottomCorner = new float[2];
  float[] temp = new float[2];

  for (float k = RobotWidth/2 ; k >= -RobotWidth/2 ; k=k-0.25) { 
    LeftBottomCorner = MapOffsetPosFloat(x_pos_MAP, y_pos_MAP, k, theta - radians(-90));
    LeftBottomCorner = MapOffsetPosFloat(LeftBottomCorner[0], LeftBottomCorner[1], RobotLength/2, theta - radians(-180));

    for (int x = 0; x <= RobotLength; x++) {
      temp = MapOffsetPosFloat(LeftBottomCorner[0], LeftBottomCorner[1], x, theta);
      map[int(temp[0])][int(temp[1])] = -127;
    }
  }
}


float[] MapOffsetPosFloat(float x, float y, float dist, float theta) {
  float[] temp = new float[2];
  float Xoff = dist * cos(theta); //x
  float Yoff = dist * sin(theta); //y
  temp[0] =  x + Xoff;
  temp[1] =  y - Yoff;
  return temp;
}

Title: Re: check which cells are covered by robot
Post by: Roman505 on October 02, 2013, 02:41:33 PM
Deleted, because I missed the case of a robot wholly covering a cell rather than merely intersecting with it.

I will stick with the first part of my original post which started "yes". :)
Title: Re: check which cells are covered by robot
Post by: jwatte on October 02, 2013, 07:59:45 PM
Your pose can be expressed as two orthonormal basis vectors Vx and Vy and two half-extents ex and ey.
You also need a center position PC.
In vector algebra, you want to set to 1 the cells where cell coordinate CC conforms to:
fabs(dot(CC-PC, Vx)) <= ex && fabs(dot(CC-PC, Vy)) <= ey
You can then scan the possible extent of the square around the center PC for intersection, and mark the appropriate cells.
The minimum X coordinate to test is PC.x - fabs(Vx.x) - fabs(Vy.x); the maximum X coordinate is PC.x + fabs(Vx.x) + fabs(Vy.x).
The minimum Y coordinate to test is PC.y - fabs(Vx.y) - fabs(Vy.y); the maximum Y coordinate is PC.y + fabs(Vx.y) + fabs(Vy.y).

Trying to rasterize in body space, like you're trying to do, is problematic because you will miss certain cells, and count others twice. You solve this by over-sampling by a factor of 4, which is quite wasteful.

Separately, the implementation you have has some inefficiencies, such as using "new" to create arrays of floats, which would be more efficient on the stack, and calling sin(theta) and cos(theta) once per cell instead of once per invocation. You also leak the things allocated with new[] at the end of your function.
Title: Re: check which cells are covered by robot
Post by: robvoi on October 04, 2013, 07:37:26 AM
Thanks a lot for the reply. Finally I have to get into vectors. So far I avoided it. :-) Therefore I didn’t completely understand your reply yet.

My approach indeed is wasteful. The over-sampling is needed with my approach – as you stated. In this case it is “only” wasting CPU power. IN other cases, where I calculate probability I had to keep track of already updated cells in a separate array to prevent double processing.

„You also leak the things allocated with new[] at the end of your function.”
I fully trust (well I don’t  :P ) on java’s garbage collection with this one.

Trying to dig into vectors now …

Robert
Title: Re: check which cells are covered by robot
Post by: jwatte on October 04, 2013, 12:30:04 PM
In Java the only cost of those additional allocs is higher than necessary garbage collector pressure.