I am working with ROS and its map_server node.
I dont understand what the origin metadata info of a map means (conceptually). According to the official documetation:
origin : The 2-D pose of the lower-left pixel in the map, as (x, y,
yaw), with yaw as counterclockwise rotation (yaw=0 means no rotation).
Many parts of the system currently ignore yaw.
It is not the initial pose of the robot? But It establish some area of interest of the occupance grid?
Why this value is so important for the Navigation Stack?
Can you give me a simple example of the same map with different origins?
Origin is generally the position of the robot at the beginning of the program. So yes the initial pose of the robot. When I've used it, it could be used as the original position of the robot. Generally, when using origin you create a deep copy of the current position.
def initPose(self):
origin = copy.deepcopy(self._current)
q = [origin.orientation.x,
origin.orientation.y,
origin.orientation.z,
origin.orientation.w] # quaternion nonsense
(roll, pitch, yaw) = euler_from_quaternion(q)
return (self._current.position.x, self._current.position.y, yaw)
# self._odom_list.waitForTransform('YOUR_STRING_HERE', 'YOUR_STRING_HERE', rospy.Time(0), rospy.Duration(1.0))
But I've also used origin to be the origin of a function.
def navToPose(self, goal):
# self._odom_list.waitForTransform('map', 'base_footprint', rospy.Time(0), rospy.Duration(1.0))
# transGoal = self._odom_list.transformPose('base_footprint', goal) # transform the nav goal from the global coordinate system to the robot's coordinate system
origin = copy.deepcopy(self._current)
q = [origin.orientation.x,
origin.orientation.y,
origin.orientation.z,
origin.orientation.w] # quaternion nonsense
(roll, pitch, yaw) = euler_from_quaternion(q)
qc = [self._current.orientation.x,
self._current.orientation.y,
self._current.orientation.z,
self._current.orientation.w]
(rollc, pitchc, yawc) = euler_from_quaternion(qc)
x = goal.pose.position.x
y = goal.pose.position.y
cx = origin.position.x
cy = self._current.position.y
print('current', cx, cy)
print(x, y)
theta = math.atan2(y-cy, x-cx)
print ('angle is ', theta)
self.rotate(theta)
distance = (((x - cx) ** 2) + ((y - cy) ** 2)) ** .5
print ('distance is ', distance)
self.driveStraight(0.5, distance)
So generally, I've used it more as another variable.
Depending on how the Occupancy grid is done. Sometime the origin will refer to where it started on the grid. Allowing the program to know if it still on the map. This can create issues shown here: https://answers.ros.org/question/285602/static-map-corner-at-origin-for-navigation-stack/ (at least from what I've experienced)
For more information on the nav stack go: http://wiki.ros.org/navigation
and here: https://www.dis.uniroma1.it/~nardi/Didattica/CAI/matdid/robot-programming-ROS-introduction-to-navigation.pdf
From the ros message definition (http://docs.ros.org/api/nav_msgs/html/msg/MapMetaData.html) :
# The origin of the map [m, m, rad]. This is the real-world pose of the
# cell (0,0) in the map.
That is the coordinate of the lower left corner of your map in the reference frame.
The robot and its position have no relationship with the map origin.
There is some details and nice illustration here : https://answers.ros.org/question/205521/robot-coordinates-in-map/
Related
PLEASE do not just post a solution to my problem. For me, this is all about understanding how to do this and be able to explain to myself and others how this and that makes it all work!
I have a dart board I created with turtle. I can post it if someone really wants to see it.
Now, I need to create a function that will create a random spot on the board to hit, then incorporate the point value for that spot. The random point is simple. But is there a way that I can assign the correct value to an AREA without having to name EVERY coordinate one by one?
Say your dart board is centered at (x0, y0), and you have a dart at (x, y). You need to translate your dart into polar coordinates (phi, r):
r = sqrt((x - x0) ** 2, (y - y0) ** 2)
phi = math.atan2(y, x)
Then figure out whether r makes your dart is in center, inner, mid or outer ring, and in which section of the circle your phi lies.
What you need is a reverse transform, one that given x,y coordinates can identify which area it belongs to.
The best way to deal with this problem is to think in terms of coordinate systems. The area of a dart board is specified by its angle and radius. You must convert your x,y coordinates to an angle and radius, then determining the area it falls within will be simple.
Determining the angle is best done with an arctan2 function, which can directly convert an x,y offset into an angle. The radius is a simple sqrt(x**2 + y**2) once you have subtracted the center point from x,y.
good ol' Pythagoras
Let the point (x1, y1) be the center of the dart board that is used from the constructor. and let (x2, y2) be the random point on the board you find.
Use this formula to find d the distance from the center. Then you just need a few if statements
if 0 <= d || d <= 2:
# area 0
elif d < 2 || 4 <= d:
# area 1
elif d < 4 || 6 <= d:
# area 2
I have two points (x1, y1) and (x2,y2) which represent the location of two entities in my space. I calculate the Euclidian distance between them using Pythagoras' theorem and everything is wonderful. However, if my space becomes finite, I want to define a new shortest distance between the points that "wraps around" the seams of the map. For example, if I have point A as (10, 10) and point B as (90,10), and my map is 100 units wide, I'd like to calculate the distance between A and B as 20 (out the right edge of the map and back into the left edge), instead of 80, which is the normal Euclidian distance.
I think my issue is that I'm using a coordinate system that isn't quite right for what I'm trying to do, and that really my flat square map is more of a seamless doughnut shape. Any suggestions for how to implement a system of this nature and convert back and forth from Cartesian coordinates would be appreciated too!
Toroidal plane? Okay, I'll bite.
var raw_dx = Math.abs(x2 - x1);
var raw_dy = Math.abs(y2 - y1);
var dx = (raw_dx < (xmax / 2)) ? raw_dx : xmax - raw_dx;
var dy = (raw_dy < (ymax / 2)) ? raw_dy : ymax - raw_dy;
var l2dist = Math.sqrt((dx * dx) + (dy * dy));
There's a correspondence here between the rollover behavior of your x and y coordinates and the rollover behavior of signed integers represented using the base's complement representation in the method of complements.
If your coordinate bounds map exactly to the bounds of a binary integer type supported by your language, you can take advantage of the two's complement representation used by nearly all current machines by simply performing the subtraction directly, ignoring overflow and reinterpreting the result as a signed value of the same size as the original coordinate. In the general case, you're not going to be that lucky, so the above dance with abs, compare and subtract is required.
Although the context of this question is about making a 2d/3d game, the problem i have boils down to some math.
Although its a 2.5D world, lets pretend its just 2d for this question.
// xa: x-accent, the x coordinate of the projection
// mapP: a coordinate on a map which need to be projected
// _Dist_ values are constants for the projection, choosing them correctly will result in i.e. an isometric projection
xa = mapP.x * xDistX + mapP.y * xDistY;
ya = mapP.x * yDistX + mapP.y * yDistY;
xDistX and yDistX determine the angle of the x-axis, and xDistY and yDistY determine the angle of the y-axis on the projection (and also the size of the grid, but lets assume this is 1-pixel for simplicity).
x-axis-angle = atan(yDistX/xDistX)
y-axis-angle = atan(yDistY/yDistY)
a "normal" coordinate system like this
--------------- x
|
|
|
|
|
y
has values like this:
xDistX = 1;
yDistX = 0;
xDistY = 0;
YDistY = 1;
So every step in x direction will result on the projection to 1 pixel to the right end 0 pixels down. Every step in the y direction of the projection will result in 0 steps to the right and 1 pixel down.
When choosing the correct xDistX, yDistX, xDistY, yDistY, you can project any trimetric or dimetric system (which is why i chose this).
So far so good, when this is drawn everything turns out okay. If "my system" and mindset are clear, lets move on to perspective.
I wanted to add some perspective to this grid so i added some extra's like this:
camera = new MapPoint(60, 60);
dx = mapP.x - camera.x; // delta x
dy = mapP.y - camera.y; // delta y
dist = Math.sqrt(dx * dx + dy * dy); // dist is the distance to the camera, Pythagoras etc.. all objects must be in front of the camera
fac = 1 - dist / 100; // this formula determines the amount of perspective
xa = fac * (mapP.x * xDistX + mapP.y * xDistY) ;
ya = fac * (mapP.x * yDistX + mapP.y * yDistY );
Now the real hard part... what if you got a (xa,ya) point on the projection and want to calculate the original point (x,y).
For the first case (without perspective) i did find the inverse function, but how can this be done for the formula with the perspective. May math skills are not quite up to the challenge to solve this.
( I vaguely remember from a long time ago mathematica could create inverse function for some special cases... could it solve this problem? Could someone maybe try?)
The function you've defined doesn't have an inverse. Just as an example, as user207422 already pointed out anything that's 100 units away from the camera will get mapped to (xa,ya)=(0,0), so the inverse isn't uniquely defined.
More importantly, that's not how you calculate perspective. Generally the perspective scaling factor is defined to be viewdist/zdist where zdist is the perpendicular distance from the camera to the object and viewdist is a constant which is the distance from the camera to the hypothetical screen onto which everything is being projected. (See the diagram here, but feel free to ignore everything else on that page.) The scaling factor you're using in your example doesn't have the same behaviour.
Here's a stab at trying to convert your code into a correct perspective calculation (note I'm not simplifying to 2D; perspective is about projecting three dimensions to two, trying to simplify the problem to 2D is kind of pointless):
camera = new MapPoint(60, 60, 10);
camera_z = camera.x*zDistX + camera.y*zDistY + camera.z*zDistz;
// viewdist is the distance from the viewer's eye to the screen in
// "world units". You'll have to fiddle with this, probably.
viewdist = 10.0;
xa = mapP.x*xDistX + mapP.y*xDistY + mapP.z*xDistZ;
ya = mapP.x*yDistX + mapP.y*yDistY + mapP.z*yDistZ;
za = mapP.x*zDistX + mapP.y*zDistY + mapP.z*zDistZ;
zdist = camera_z - za;
scaling_factor = viewdist / zdist;
xa *= scaling_factor;
ya *= scaling_factor;
You're only going to return xa and ya from this function; za is just for the perspective calculation. I'm assuming the the "za-direction" points out of the screen, so if the pre-projection x-axis points towards the viewer then zDistX should be positive and vice-versa, and similarly for zDistY. For a trimetric projection you would probably have xDistZ==0, yDistZ<0, and zDistZ==0. This would make the pre-projection z-axis point straight up post-projection.
Now the bad news: this function doesn't have an inverse either. Any point (xa,ya) is the image of an infinite number of points (x,y,z). But! If you assume that z=0, then you can solve for x and y, which is possibly good enough.
To do that you'll have to do some linear algebra. Compute camera_x and camera_y similar to camera_z. That's the post-transformation coordinates of the camera. The point on the screen has post-tranformation coordinates (xa,ya,camera_z-viewdist). Draw a line through those two points, and calculate where in intersects the plane spanned by the vectors (xDistX, yDistX, zDistX) and (xDistY, yDistY, zDistY). In other words, you need to solve the equations:
x*xDistX + y*xDistY == s*camera_x + (1-s)*xa
x*yDistX + y*yDistY == s*camera_y + (1-s)*ya
x*zDistX + y*zDistY == s*camera_z + (1-s)*(camera_z - viewdist)
It's not pretty, but it will work.
I think that with your post i can solve the problem. Still, to clarify some questions:
Solving the problem in 2d is useless indeed, but this was only done to make the problem easier to grasp (for me and for the readers here). My program actually give's a perfect 3d projection (i checked it with 3d images rendered with blender). I did left something out about the inverse function though. The inverse function is only for coordinates between 0..camera.x * 0.5 and 0.. camera.y*0.5. So in my example between 0 and 30. But even then i have doubt's about my function.
In my projection the z-axis is always straight up, so to calculate the height of an object i only used the vieuwingangle. But since you cant actually fly or jumpt into the sky everything has only a 2d point. This also means that when you try to solve the x and y, the z really is 0.
I know not every funcion has an inverse, and some functions do, but only for a particular domain. My basic thought in this all was... if i can draw a grid using a function... every point on that grid maps to exactly one map-point. I can read the x and y coordinate so if i just had the correct function i would be able to calculate the inverse.
But there is no better replacement then some good solid math, and im very glad you took the time to give a very helpfull responce :).
My problem:
How can I take two 3D points and lock them to a single axis? For instance, so that both their z-axes are 0.
What I'm trying to do:
I have a set of 3D coordinates in a scene, representing a a box with a pyramid on it. I also have a camera, represented by another 3D coordinate. I subtract the camera coordinate from the scene coordinate and normalize it, returning a vector that points to the camera. I then do ray-plane intersection with a plane that is behind the camera point.
O + tD
Where O (origin) is the camera position, D is the direction from the scene point to the camera and t is time it takes for the ray to intersect the plane from the camera point.
If that doesn't make sense, here's a crude drawing:
I've searched far and wide, and as far as I can tell, this is called using a "pinhole camera".
The problem is not my camera rotation, I've eliminated that. The trouble is in translating the intersection point to barycentric (uv) coordinates.
The translation on the x-axis looks like this:
uaxis.x = -a_PlaneNormal.y;
uaxis.y = a_PlaneNormal.x;
uaxis.z = a_PlaneNormal.z;
point vaxis = uaxis.CopyCrossProduct(a_PlaneNormal);
point2d.x = intersection.DotProduct(uaxis);
point2d.y = intersection.DotProduct(vaxis);
return point2d;
While the translation on the z-axis looks like this:
uaxis.x = -a_PlaneNormal.z;
uaxis.y = a_PlaneNormal.y;
uaxis.z = a_PlaneNormal.x;
point vaxis = uaxis.CopyCrossProduct(a_PlaneNormal);
point2d.x = intersection.DotProduct(uaxis);
point2d.y = intersection.DotProduct(vaxis);
return point2d;
My question is: how can I turn a ray plane intersection point to barycentric coordinates on both the x and the z axis?
The usual formula for points (p) on a line, starting at (p0) with vector direction (v) is:
p = p0 + t*v
The criterion for a point (p) on a plane containing (p1) and with normal (n) is:
(p - p1).n = 0
So, plug&chug:
(p0 + t*v - p1).n = (p0-p1).n + t*(v.n) = 0
-> t = (p1-p0).n / v.n
-> p = p0 + ((p1-p0).n / v.n)*v
To check:
(p - p1).n = (p0-p1).n + ((p1-p0).n / v.n)*(v.n)
= (p0-p1).n + (p1-p0).n
= 0
If you want to fix the Z coordinate at a particular value, you need to choose a normal along the Z axis (which will define a plane parallel to XY plane).
Then, you have:
n = (0,0,1)
-> p = p0 + ((p1.z-p0.z)/v.z) * v
-> x and y offsets from p0 = ((p1.z-p0.z)/v.z) * (v.x,v.y)
Finally, if you're trying to build a virtual "camera" for 3D computer graphics, the standard way to do this kind of thing is homogeneous coordinates. Ultimately, working with homogeneous coordinates is simpler (and usually faster) than the kind of ad hoc 3D vector algebra I have written above.
I need to calculate the 2 angles (yaw and pitch) for a 3D object to face an arbitrary 3D point. These rotations are known as "Euler" rotations simply because after the first rotation, (lets say Z, based on the picture below) the Y axis also rotates with the object.
This is the code I'm using but its not working fully. When on the ground plane (Y = 0) the object correctly rotates to face the point, but as soon as I move the point upwards in Y, the rotations don't look correct.
// x, y, z represent a fractional value between -[1] and [1]
// a "unit vector" of the point I need to rotate towards
yaw = Math.atan2( y, x )
pitch = Math.atan2( z, Math.sqrt( x * x + y * y ) )
Do you know how to calculate the 2 Euler angles given a point?
The picture below shows the way I rotate. These are the angles I need to calculate.
(The only difference is I'm rotating the object in the order X,Y,Z and not Z,Y,X)
This is my system.
coordinate system is x = to the right, y = downwards, z = further back
an object is by default at (0,0,1) which is facing backward
rotations are in the order X, Y, Z where rotation upon X is pitch, Y is yaw and Z is roll
Here are my working assumptions:
The coordinate system (x,y,z) is such that positive x is to the right, positive y is down, and z is the remaining direction. In particular, y=0 is the ground plane.
An object at (0,0,0) currently facing towards (0,0,1) is being turned to face towards (x,y,z).
In order to accomplish this, there will be a rotation about the x-axis followed by one around the y-axis. Finally, there is a rotation about the z-axis in order to have things upright.
(The terminology yaw, pitch, and roll can be confusing, so I'd like to avoid using it, but roughly speaking the correspondence is x=pitch, y=yaw, z=roll.)
Here is my attempt to solve your problem given this setup:
rotx = Math.atan2( y, z )
roty = Math.atan2( x * Math.cos(rotx), z )
rotz = Math.atan2( Math.cos(rotx), Math.sin(rotx) * Math.sin(roty) )
Hopefully this is correct up to signs. I think the easiest way to fix the signs is by trial and error. Indeed, you appear to have gotten the signs on rotx and roty correct -- including a subtle issue with regards to z -- so you only need to fix the sign on rotz.
I expect this to be nontrivial (possibly depending on which octant you're in), but please try a few possibilities before saying it's wrong. Good luck!
Here is the code that finally worked for me.
I noticed a "flip" effect that occurred when the object moved from any front quadrant (positive Z) to any back quadrant. In the front quadrants the front of the object would always face the point. In the back quadrants the back of the object always faces the point.
This code corrects the flip effect so the front of the object always faces the point. I encountered it through trial-and-error so I don't really know what's happening!
rotx = Math.atan2( y, z );
if (z >= 0) {
roty = -Math.atan2( x * Math.cos(rotx), z );
}else{
roty = Math.atan2( x * Math.cos(rotx), -z );
}
Rich Seller's answer shows you how to rotate a point from one 3-D coordinate system to another system, given a set of Euler angles describing the rotation between the two coordinate systems.
But it sounds like you're asking for something different:
You have: 3-D coordinates of a single point
You want: a set of Euler angles
If that's what you're asking for, you don't have enough information. To find the Euler angles,
you'd need coordinates of at least two points, in both coordinate systems, to determine the rotation from one coordinate system into the other.
You should also be aware that Euler angles can be ambiguous: Rich's answer assumes the
rotations are applied to Z, then X', then Z', but that's not standardized. If you have to interoperate with some other code using Euler angles, you need to make sure you're using the same convention.
You might want to consider using rotation matrices or quaternions instead of Euler angles.
This series of rotations will give you what you're asking for:
About X: 0
About Y: atan2(z, x)
About Z: atan2(y, sqrt(x*x + z*z))
I cannot tell you what these are in terms of "roll", "pitch" and "yaw" unless you first define how you are using these terms. You are not using them in the standard way.
EDIT:
All right, then try this:
About X: -atan2(y, z)
About Y: atan2(x, sqrt(y*y + z*z))
About Z: 0
Talking about the rotation of axes, I think step 3 should have been the rotation of X'-, Y''-, and Z'-axes about the Y''-axis.