I have 3 robots and I know the starts and end coordinate of each of them. For example for Robot 1 I have Xr1, Yr1 and Xr2,Yr2 coordinates pair , same for Robot2 and 3. I have lateral cameras so can obtain the visual information. I would like to get the robot-to-robot relative position over time. The camera update is every 0.5 seconds. How can I approach this problem and any possible solution? Is the Hidden Markov Model chains would be a good approach? OR semantic Mapping can be helpful?
Related
I have an electromagnetic sensor and electromagnetic field emitter.
The sensor will read power from the emitter. I want to predict the position of the sensor using the reading.
Let me simplify the problem, suppose the sensor and the emitter are in 1 dimension world where there are only position X (not X,Y,Z) and the emitter emits power as a function of distance squared.
From the painted image below, you will see that the emitter is drawn as a circle and the sensor is drawn as a cross.
E.g. if the sensor is 5 meter away from the emitter, the reading you get on the sensor will be 5^2 = 25. So the correct position will be either 0 or 10, because the emitter is at position 5.
So, with one emitter, I cannot know the exact position of the sensor. I only know that there are 50% chance it's at 0, and 50% chance it's at 10.
So if I have two emitters like the following image:
I will get two readings. And I can know exactly where the sensor is. If the reading is 25 and 16, I know the sensor is at 10.
So from this fact, I want to use 2 emitters to locate the sensor.
Now that I've explained you the situation, my problems are like this:
The emitter has a more complicated function of the distance. It's
not just distance squared. And it also have noise. so I'm trying to
model it using machine learning.
Some of the areas, the emitter don't work so well. E.g. if you are
between 3 to 4 meters away, the emitter will always give you a fixed
reading of 9 instead of going from 9 to 16.
When I train the machine learning model with 2 inputs, the
prediction is very accurate. E.g. if the input is 25,36 and the
output will be position 0. But it means that after training, I
cannot move the emitters at all. If I move one of the emitters to be
further apart, the prediction will be broken immediately because the
reading will be something like 25,49 when the right emitter moves to
the right 1 meter. And the prediction can be anything because the
model has not seen this input pair before. And I cannot afford to
train the model on all possible distance of the 2 emitters.
The emitters can be slightly not identical. The difference will
be on the scale. E.g. one of the emitters can be giving 10% bigger
reading. But you can ignore this problem for now.
My question is How do I make the model work when the emitters are allowed to move? Give me some ideas.
Some of my ideas:
I think that I have to figure out the position of both
emitters relative to each other dynamically. But after knowing the
position of both emitters, how do I tell that to the model?
I have tried training each emitter separately instead of pairing
them as input. But that means there are many positions that cause
conflict like when you get reading=25, the model will predict the
average of 0 and 10 because both are valid position of reading=25.
You might suggest training to predict distance instead of position,
that's possible if there is no problem number 2. But because
there is problem number 2, the prediction between 3 to 4 meters away
will be wrong. The model will get input as 9, and the output will be
the average distance 3.5 meters or somewhere between 3 to 4 meters.
Use the model to predict position
probability density function instead of predicting the position.
E.g. when the reading is 9, the model should predict a uniform
density function from 3 to 4 meters. And then you can combine the 2
density functions from the 2 readings somehow. But I think it's not
going to be that accurate compared to modeling 2 emitters together
because the density function can be quite complicated. We cannot
assume normal distribution or even uniform distribution.
Use some kind of optimizer to predict the position separately for each
emitter based on the assumption that both predictions must be the same. If
the predictions are not the same, the optimizer must try to move the
predictions so that they are exactly at the same point. Maybe reinforcement
learning where the actions are "move left", "move right", etc.
I told you my ideas so that it might evoke some ideas in you. Because this is already my best but it's not solving the issue elegantly yet.
So ideally, I would want the end-to-end model that are fed 2 readings, and give me position even when the emitters are moved. How would I go about that?
PS. The emitters are only allowed to move before usage. During usage or prediction, the model can assume that the emitter will not be moved anymore. This allows you to have time to run emitters position calibration algorithm before usage. Maybe this will be a helpful thing for you to know.
You're confusing memoizing a function with training a model; the former is merely recalling previous results; the latter is the province of AI. To train with two emitters, you need to give the useful input data and appropriate labels (right answers), and design your model topology such that it can be trained to a useful functional response fro cases it has never seen.
Let the first emitter be at position 0 by definition. Your data then consists of the position of the second emitter and the two readings. The label is the sensor's position. Your given examples would look like this:
emit2 read1 read2 sensor
1 25 36 0
1 25 16 5
2 25 49 0
1.5 25 9 5 distance of 3 < d < 4 always reads as 3^2
Since you know that you have an squared relationship in the underlying physics, you need to include quadratic capability in your model. To handle noise, you'll want some dampener capability, such as an extra node or two in a hidden layer after the first. For more complex relationships, you'll need other topologies, non-linear activation functions, etc.
Can you take it from there?
I have four cameras which are at fixed position. So I can measure the distance (even rotation) among them using a ruler (physically). Camera one and two gives me a point cloud and camera three and four gives me another point cloud. I need to merge these point clouds.
As far I have understood that ICP and other such algorithms do a rigid transformation of one point cloud to match the other. My question is how can I use the extra knowledge (distance between the cameras in centimeter) to do this transformation.
I am quite new to such work so, please correct me if I misunderstood something. And thanks in advance.
First, what kind of accuracy are you looking for, and over what volume of space? Achieving 0.1 mm registration accuracy over a 0.5 m tabletop scene is a completely different problem (in terms of mechanical design and constraints) than a few mm over a floor tens of meters wide.
Generally speaking, with a well reconstructed and unambiguous object shape, ICP will always give you a better solution than manual measurements.
If the cameras are static, then what you have is really a calibration problem, and you need to calibrate your 4-camera rig only at setup and when its configuration changes for whatever reason.
I suggest using a calibration object of precisely known size and geometry, e.g. a machined polyhedron. You can generate and ICP-register point clouds for it, then fit the merged cloud to the known geometry, thus obtaining position and orientation of every individual point cloud with respect to the fixed object. From these you can work out the poses of the cameras w.r.t. each other.
I am working on bundle block adjustment for finding
X,Y,Z values of image points
Corrected values of camera characteristics(extrinsic parameters etc..)
Corrected values of measurements
In my opinion BB Adjustment process is done by following these steps(camera intrinsics are given):
Gather tie points( x,y for each image pair ) and ground control points( x,y and related X,Y,Z positions for each image )
Calculate initial extrinsic parameters( camera pose ) for each view
Calculate each tie point's initial real world position by using camera pose
Execute sparse bundle adjustment step by using all these initial values and other parameters as inputs
Use output of sparse bundle adjustment as accurate results of real world position, extrinsic characteristics and measurements.
One thing i want to ask is if that flow is correct. There are lots of methods for structure and motion estimation from views so i can not be so sure about that.
As i search through various resources i found that there are libraries that does each part of the block bundle adjustment operation. For each step:
Image processing libraries like OpenCV may be used for automatic tie point collection
cvFindExtrinsicCameraParams2 may be used for space resection ( but it requires 4 points, for block bundle adjustment it is mentioned that 3 Ground control points are enough for each view. Should i use another method like pose estimation from stereo views? )
By using triangulation and projection methods of OpenCV, real world positions may be calculated
SBA or SSBA is suitable for this operation
N/A
One another question is that, if previously mentioned flow is right, is matched libraries are enough for implementing entire flow?( May be better advises for each part )
I am newbie in this field, so i appreciate any help in this subject, Thanks...
You have described the default approach to stereo photogrammetry. Rather than using computer vision terms (extrinsic, intrinsic) I suggest you search using the terms interior- and exterior-orientation. This is a good approach if you have finite numbers of overlapping images and it has the benefit of some well defined error estimation methods.
Here is some basic math:
http://itee.uq.edu.au/~elec4600/elec4600_lectures/1perpage/uq1.pdf
http://itee.uq.edu.au/~elec4600/elec4600_lectures/1perpage/uq2.pdf
.2. cvFindExtrinsicCameraParams2 may be used for space resection ( but it
requires 4 points, for block bundle adjustment it is mentioned that 3
Ground control points are enough for each view.
The reason four control points are required by cvFindExtrinsicCameraParams2 is that the equations are under-determined with only three. If you don't have enough control, you might have to use an alternate approach (or sensor) to estimate the initial camera pose vector.
I have a field filled with obstacles, I know where they are located, and I know the robot's position. Using a path-finding algorithm, I calculate a path for the robot to follow.
Now my problem is, I am guiding the robot from grid to grid but this creates a not-so-smooth motion. I start at A, turn the nose to point B, move straight until I reach point B, rinse and repeat until the final point is reached.
So my question is: What kind of techniques are used for navigating in such an environment so that I get a smooth motion?
The robot has two wheels and two motors. I change the direction of the motor by turning the motors in reverse.
EDIT: I can vary the speed of the motors basically the robot is an arduino plus ardumoto, I can supply values between 0-255 to the motors on either direction.
You need feedback linearization for a differentially driven robot. This document explains it in Section 2.2. I've included relevant portions below:
The simulated robot required for the
project is a differential drive robot
with a bounded velocity. Since
the differential drive robots are
nonholonomic, the students are encouraged to use feedback linearization to
convert the kinematic control output
from their algorithms to control the
differential drive robots. The
transformation follows:
where v, ω, x, y are the linear,
angular, and kinematic velocities. L
is an offset length proportional to the
wheel base dimension of the robot.
One control algorithm I've had pretty good results with is pure pursuit. Basically, the robot attempts to move to a point along the path a fixed distance ahead of the robot. So as the robot moves along the path, the look ahead point also advances. The algorithm compensates for non-holonomic constraints by modeling possible paths as arcs.
Larger look ahead distances will create smoother movement. However, larger look ahead distances will cause the robot to cut corners, which may collide with obstacles. You can fix this problem by implementing ideas from a reactive control algorithm called Vector Field Histogram (VFH). VFH basically pushes the robot away from close walls. While this normally uses a range finding sensor of some sort, you can extrapolate the relative locations of the obstacles since you know the robot pose and the obstacle locations.
My initial thoughts on this(I'm at work so can't spend too much time):
It depends how tight you want or need your corners to be (which would depend on how much distance your path finder gives you from the obstacles)
Given the width of the robot you can calculate the turning radius given the speeds for each wheel. Assuming you want to go as fast as possible and that skidding isn't an issue, you will always keep the outside wheel at 255 and reduce the inside wheel down to the speed that gives you the required turning radius.
Given the angle for any particular turn on your path and the turning radius that you will use, you can work out the distance from that node where you will slow down the inside wheel.
An optimization approach is a very general way to handle this.
Use your calculated path as input to a generic non-linear optimization algorithm (your choice!) with a cost function made up of closeness of the answer trajectory to the input trajectory as well as adherence to non-holonomic constraints, and any other constraints you want to enforce (e.g. staying away from the obstacles). The optimization algorithm can also be initialised with a trajectory constructed from the original trajectory.
Marc Toussaint's robotics course notes are a good source for this type of approach. See in particular lecture 7:
http://userpage.fu-berlin.de/mtoussai/teaching/10-robotics/
I'm writing a simulation in which a creature object should be able to move towards some other arbitrary object in the environment, sliding around obstacles rather than doing any intelligent pathfinding. I'm not trying to have it plan a path -- just to move in one general direction, and bounce around obstacles.
It's a 2D environment (overhead view), and every object has a bounding rectangle for collision detection. There is no grid, and I am not looking for A* solution.
I haven't been able to find any tutorials on this kind of "dumb" collision-based pathfinding, so I might not be describing this using the most common terms.
Any recommendations on how to implement this (or links to tutorials)?
Expanding on what Guillaume said about obstacle avoidance, a technique that would work well for you is anti-gravity movement. You treat local obstacles as point sources of antigravity, the destination as gravity, and your computer controlled character will slip (like soap!) around the obstacles to get to the destination.
you can combine two steering algorithm :
seek : you apply a steering force in the direction which is the difference between the current velocity and the desired velocity towards the target
Obstacle Avoidance : you anticipates the vehicle's future using a box whose length is a constant time multiplied by the current velocity of the vehicle. Any obstacle that intersects this box is a potential collision threat. The nearest such threat is chosen for avoidance. To avoid an obstacle, a lateral steering force is applied opposite to the obstacle's center. In addition, a braking (deceleration) force is applied. These forces vary with urgency (the distance from the tip of the box to the point of potential collision). Steering varies linearly, braking varies quadratically.
You can find more on the website "Steering Behaviors For Autonomous Characters"
regards
Guillaume
PS : this assume you're using a point/velocity/acceleration method for the object's movement.
Maybe you could use Pledge's algorithm
Whenever your creature, travelling in vector direction v, collides with a wall whose direction is represented by vector w, the direction that you need to "slide" is given by the vector that is the projection of v onto w. This can be found using
v . w
--------- w
|w|*|w|
where . is the vector dot product and |w| is the magnitude of vector w ( = sqrt(w . w)). If w is a unit vector, this becomes simply
(v . w) w
Using the resulting vector as your creature's speed will mean your creature travels quickly when it just "grazes" the wall, and slowly when it hits the wall nearly dead-on. (This is how most first-person shooter games manage collisions for the human player.)
If instead you want your creature to always travel at full speed, you only need the sign of v . w -- you will always be travelling either in the direction the wall faces (w) or the opposite direction (-w).
The issue that you will have is when your creature hits the wall dead-on. In that case your projected vector will be (0, 0), and you need some other technique to decide which way (w or -w) to go. The usual approach here is A*, although this may be unnecessary if your environment possesses enough structure.
I posted a pathfinding algorithm in C# a while back
Here's the link
You can try and use it as a starting point, ie, you could modify the function that checks the next cell to see if it's valid to check for the obstacles, and you could feed it small intervals instead of the starting and end points, kinda like multiple mini-pahfinding routes.
(The text is in spanish, but you can download the application from the link at the top)