Algorithm to control acceleration until a position is reached - algorithm

I have a point that moves (in one dimension), and I need it to move smoothly. So I think that it's velocity has to be a continuous function and I need to control the acceleration and then calculate it's velocity and position.
The algorithm doesn't seem something obvious to me, but I guess this must be a common problem, I just can't find the solution.
Notes:
The final destination of the object may change while it's moving and the movement needs to be smooth anyway.
I guess that a naive implementation would produce bouncing, and I need to avoid that.

This is a perfect candidate for using a "critically damped spring".
Conceptually you attach the point to the target point with a spring, or piece of elastic. The spring is damped so that you get no 'bouncing'. You can control how fast the system reacts by changing a constant called the "SpringConstant". This is essentially how strong the piece of elastic is.
Basically you apply two forces to the position, then integrate this over time. The first force is that applied by the spring, Fs = SpringConstant * DistanceToTarget. The second is the damping force, Fd = -CurrentVelocity * 2 * sqrt( SpringConstant ).
The CurrentVelocity forms part of the state of the system, and can be initialised to zero.
In each step, you multiply the sum of these two forces by the time step. This gives you the change of the value of the CurrentVelocity. Multiply this by the time step again and it will give you the displacement.
We add this to the actual position of the point.
In C++ code:
float CriticallyDampedSpring( float a_Target,
float a_Current,
float & a_Velocity,
float a_TimeStep )
{
float currentToTarget = a_Target - a_Current;
float springForce = currentToTarget * SPRING_CONSTANT;
float dampingForce = -a_Velocity * 2 * sqrt( SPRING_CONSTANT );
float force = springForce + dampingForce;
a_Velocity += force * a_TimeStep;
float displacement = a_Velocity * a_TimeStep;
return a_Current + displacement;
}
In systems I was working with a value of around 5 was a good point to start experimenting with the value of the spring constant. Set it too high will result in too fast a reaction, and too low the point will react too slowly.
Note, you might be best to make a class that keeps the velocity state rather than have to pass it into the function over and over.
I hope this is helpful, good luck :)
EDIT: In case it's useful for others, it's easy to apply this to 2 or 3 dimensions. In this case you can just apply the CriticallyDampedSpring independently once for each dimension. Depending on the motion you want you might find it better to work in polar coordinates (for 2D), or spherical coordinates (for 3D).

I'd do something like Alex Deem's answer for trajectory planning, but with limits on force and velocity:
In pseudocode:
xtarget: target position
vtarget: target velocity*
x: object position
v: object velocity
dt: timestep
F = Ki * (xtarget-x) + Kp * (vtarget-v);
F = clipMagnitude(F, Fmax);
v = v + F * dt;
v = clipMagnitude(v, vmax);
x = x + v * dt;
clipMagnitude(y, ymax):
r = magnitude(y) / ymax
if (r <= 1)
return y;
else
return y * (1/r);
where Ki and Kp are tuning constants, Fmax and vmax are maximum force and velocity. This should work for 1-D, 2-D, or 3-D situations (magnitude(y) = abs(y) in 1-D, otherwise use vector magnitude).

It's not quite clear exactly what you're after, but I'm going to assume the following:
There is some maximum acceleration;
You want the object to have stopped moving when it reaches the destination;
Unlike velocity, you do not require acceleration to be continuous.
Let A be the maximum acceleration (by which I mean the acceleration is always between -A and A).
The equation you want is
v_f^2 = v_i^2 + 2 a d
where v_f = 0 is the final velocity, v_i is the initial (current) velocity, and d is the distance to the destination (when you switch from acceleration A to acceleration -A -- that is, from speeding up to slowing down; here I'm assuming d is positive).
Solving:
d = v_i^2 / (2A)
is the distance. (The negatives cancel).
If the current distance remaining is greater than d, speed up as quickly as possible. Otherwise, begin slowing down.
Let's say you update the object's position every t_step seconds. Then:
new_position = old_position + old_velocity * t_step + (1/2)a(t_step)^2
new_velocity = old_velocity + a * t_step.
If the destination is between new_position and old_position (i.e., the object reached its destination in between updates), simply set new_position = destination.

You need an easing formula, which you would call at a set interval, passing in the time elapsed, start point, end point and duration you want the animation to be.
Doing time-based calculations will account for slow clients and other random hiccups. Since it calculates on time elapsed vs. the time in which it has to compkete, it will account for slow intervals between calls when returning how far along your point should be in the animation.
The jquery.easing plugin has a ton of easing functions you can look at:
http://gsgd.co.uk/sandbox/jquery/easing/
I've found it best to pass in 0 and 1 as my start and end point, since it will return a floating point between the two, you can easily apply it to the real value you are modifying using multiplication.

Related

Predict Collision Between 2 Uniform Circular Motion Objects

this is my first question on the forum and my algebra is rusty so please be indulgent ^^'
So my problem is that i want to predict collision between two uniform circular motion objects for which i know velocity (angular speed in radian), distance from the origin (radius), cartesian coordinate of the center of the circle.
I can get cartesian position for each object given for t time (timestamp) using :
Oa.x = ra X cos(wa X t)
Oa.y = ra X sin(wa X t)
Oa.x: Object A x coordinates
ra: radius of a Circle A
wa: velocity of object A (angular speed in radian)
t: time (timestamp)
Same goes for object b (Ob)
I want to find t such that ||Ca - Cb|| = (rOa + rOb)
rOa: radius of object a
Squaring both side and expanding give me this :
||Ca-Cb||^2 = (rOa+rOb)^2
(ra * cos (wa * t) - rb / cos (wb * t))^2 + (ra * sin (wa * t) - rb / sin (wb * t))^2 = (ra+rb)^2
From that i should get a quadratic polynomial that i can solve for t, but how can i find a condition that tell me if such a t exist ? And possibly, how to solve it for t ?
Your motion equations are missing some stuff I expect this instead:
a0(t) = omg0*t + ang0
x0(t) = cx0 + R0 * cos(a0(t))
y0(t) = cy0 + R0 * sin(a0(t))
a1(t) = omg1*t + ang1
x1(t) = cx1 + R1 * cos(a1(t))
y1(t) = cy1 + R1 * sin(a1(t))
where t is time in [sec], cx?,cy? is the center of rotation ang? is starting angle (t=0) in [rad] and omg? is angular speed in [rad/sec]. If the objects have radius r? then collision occurs when the distance is <= r0+r1
so You want to find smallest time where:
(x1-x0)^2 + (y1-y0)^2 <= (r0+r1)^2
This will most likely lead to transcendent equation so you need numeric approach to solve this. For stuff like this I usually use Approximation search so to solve this do:
loop t from 0 to some reasonable time limit
The collision will happen with constant frequency and the time between collisions will be divisible by periods of both motions so I would test up to lcm(2*PI/omg0,2*PI/omg1) time limit where lcm is least common multiple
Do not loop t through all possible times with brute force but use heuristic (like the approx search linked above) beware initial time step must be reasonable I would try dt = min(0.2*PI/omg0,0.2*PI/omg1) so you have at least 10 points along circle
solve t so the distance between objects is minimal
This however will find the time when the objects collide fully so their centers merge. So you need to substract some constant time (or search it again) that will get you to the start of collision. This time you can use even binary search as the distance will be monotonic.
next collision will appear after lcm(2*PI/omg0,2*PI/omg1)
so if you found first collision time tc0 then
tc(i) = tc0 + i*lcm(2*PI/omg0,2*PI/omg1)
i = 0,1,2,3,...

Multiliteration implementation with inaccurate distance data

I am trying to create an android smartphone application which uses Apples iBeacon technology to determine the current indoor location of itself. I already managed to get all available beacons and calculate the distance to them via the rssi signal.
Currently I face the problem, that I am not able to find any library or implementation of an algorithm, which calculates the estimated location in 2D by using 3 (or more) distances of fixed points with the condition, that these distances are not accurate (which means, that the three "trilateration-circles" do not intersect in one point).
I would be deeply grateful if anybody can post me a link or an implementation of that in any common programming language (Java, C++, Python, PHP, Javascript or whatever). I already read a lot on stackoverflow about that topic, but could not find any answer I were able to convert in code (only some mathematical approaches with matrices and inverting them, calculating with vectors or stuff like that).
EDIT
I thought about an own approach, which works quite well for me, but is not that efficient and scientific. I iterate over every meter (or like in my example 0.1 meter) of the location grid and calculate the possibility of that location to be the actual position of the handset by comparing the distance of that location to all beacons and the distance I calculate with the received rssi signal.
Code example:
public Location trilaterate(ArrayList<Beacon> beacons, double maxX, double maxY)
{
for (double x = 0; x <= maxX; x += .1)
{
for (double y = 0; y <= maxY; y += .1)
{
double currentLocationProbability = 0;
for (Beacon beacon : beacons)
{
// distance difference between calculated distance to beacon transmitter
// (rssi-calculated distance) and current location:
// |sqrt(dX^2 + dY^2) - distanceToTransmitter|
double distanceDifference = Math
.abs(Math.sqrt(Math.pow(beacon.getLocation().x - x, 2)
+ Math.pow(beacon.getLocation().y - y, 2))
- beacon.getCurrentDistanceToTransmitter());
// weight the distance difference with the beacon calculated rssi-distance. The
// smaller the calculated rssi-distance is, the more the distance difference
// will be weighted (it is assumed, that nearer beacons measure the distance
// more accurate)
distanceDifference /= Math.pow(beacon.getCurrentDistanceToTransmitter(), 0.9);
// sum up all weighted distance differences for every beacon in
// "currentLocationProbability"
currentLocationProbability += distanceDifference;
}
addToLocationMap(currentLocationProbability, x, y);
// the previous line is my approach, I create a Set of Locations with the 5 most probable locations in it to estimate the accuracy of the measurement afterwards. If that is not necessary, a simple variable assignment for the most probable location would do the job also
}
}
Location bestLocation = getLocationSet().first().location;
bestLocation.accuracy = calculateLocationAccuracy();
Log.w("TRILATERATION", "Location " + bestLocation + " best with accuracy "
+ bestLocation.accuracy);
return bestLocation;
}
Of course, the downside of that is, that I have on a 300m² floor 30.000 locations I had to iterate over and measure the distance to every single beacon I got a signal from (if that would be 5, I do 150.000 calculations only for determine a single location). That's a lot - so I will let the question open and hope for some further solutions or a good improvement of this existing solution in order to make it more efficient.
Of course it has not to be a Trilateration approach, like the original title of this question was, it is also good to have an algorithm which includes more than three beacons for the location determination (Multilateration).
If the current approach is fine except for being too slow, then you could speed it up by recursively subdividing the plane. This works sort of like finding nearest neighbors in a kd-tree. Suppose that we are given an axis-aligned box and wish to find the approximate best solution in the box. If the box is small enough, then return the center.
Otherwise, divide the box in half, either by x or by y depending on which side is longer. For both halves, compute a bound on the solution quality as follows. Since the objective function is additive, sum lower bounds for each beacon. The lower bound for a beacon is the distance of the circle to the box, times the scaling factor. Recursively find the best solution in the child with the lower lower bound. Examine the other child only if the best solution in the first child is worse than the other child's lower bound.
Most of the implementation work here is the box-to-circle distance computation. Since the box is axis-aligned, we can use interval arithmetic to determine the precise range of distances from box points to the circle center.
P.S.: Math.hypot is a nice function for computing 2D Euclidean distances.
Instead of taking confidence levels of individual beacons into account, I would instead try to assign an overall confidence level for your result after you make the best guess you can with the available data. I don't think the only available metric (perceived power) is a good indication of accuracy. With poor geometry or a misbehaving beacon, you could be trusting poor data highly. It might make better sense to come up with an overall confidence level based on how well the perceived distance to the beacons line up with the calculated point assuming you trust all beacons equally.
I wrote some Python below that comes up with a best guess based on the provided data in the 3-beacon case by calculating the two points of intersection of circles for the first two beacons and then choosing the point that best matches the third. It's meant to get started on the problem and is not a final solution. If beacons don't intersect, it slightly increases the radius of each up until they do meet or a threshold is met. Likewise, it makes sure the third beacon agrees within a settable threshold. For n-beacons, I would pick 3 or 4 of the strongest signals and use those. There are tons of optimizations that could be done and I think this is a trial-by-fire problem due to the unwieldy nature of beaconing.
import math
beacons = [[0.0,0.0,7.0],[0.0,10.0,7.0],[10.0,5.0,16.0]] # x, y, radius
def point_dist(x1,y1,x2,y2):
x = x2-x1
y = y2-y1
return math.sqrt((x*x)+(y*y))
# determines two points of intersection for two circles [x,y,radius]
# returns None if the circles do not intersect
def circle_intersection(beacon1,beacon2):
r1 = beacon1[2]
r2 = beacon2[2]
dist = point_dist(beacon1[0],beacon1[1],beacon2[0],beacon2[1])
heron_root = (dist+r1+r2)*(-dist+r1+r2)*(dist-r1+r2)*(dist+r1-r2)
if ( heron_root > 0 ):
heron = 0.25*math.sqrt(heron_root)
xbase = (0.5)*(beacon1[0]+beacon2[0]) + (0.5)*(beacon2[0]-beacon1[0])*(r1*r1-r2*r2)/(dist*dist)
xdiff = 2*(beacon2[1]-beacon1[1])*heron/(dist*dist)
ybase = (0.5)*(beacon1[1]+beacon2[1]) + (0.5)*(beacon2[1]-beacon1[1])*(r1*r1-r2*r2)/(dist*dist)
ydiff = 2*(beacon2[0]-beacon1[0])*heron/(dist*dist)
return (xbase+xdiff,ybase-ydiff),(xbase-xdiff,ybase+ydiff)
else:
# no intersection, need to pseudo-increase beacon power and try again
return None
# find the two points of intersection between beacon0 and beacon1
# will use beacon2 to determine the better of the two points
failing = True
power_increases = 0
while failing and power_increases < 10:
res = circle_intersection(beacons[0],beacons[1])
if ( res ):
intersection = res
else:
beacons[0][2] *= 1.001
beacons[1][2] *= 1.001
power_increases += 1
continue
failing = False
# make sure the best fit is within x% (10% of the total distance from the 3rd beacon in this case)
# otherwise the results are too far off
THRESHOLD = 0.1
if failing:
print 'Bad Beacon Data (Beacon0 & Beacon1 don\'t intersection after many "power increases")'
else:
# finding best point between beacon1 and beacon2
dist1 = point_dist(beacons[2][0],beacons[2][1],intersection[0][0],intersection[0][1])
dist2 = point_dist(beacons[2][0],beacons[2][1],intersection[1][0],intersection[1][1])
if ( math.fabs(dist1-beacons[2][2]) < math.fabs(dist2-beacons[2][2]) ):
best_point = intersection[0]
best_dist = dist1
else:
best_point = intersection[1]
best_dist = dist2
best_dist_diff = math.fabs(best_dist-beacons[2][2])
if best_dist_diff < THRESHOLD*best_dist:
print best_point
else:
print 'Bad Beacon Data (Beacon2 distance to best point not within threshold)'
If you want to trust closer beacons more, you may want to calculate the intersection points between the two closest beacons and then use the farther beacon to tie-break. Keep in mind that almost anything you do with "confidence levels" for the individual measurements will be a hack at best. Since you will always be working with very bad data, you will defintiely need to loosen up the power_increases limit and threshold percentage.
You have 3 points : A(xA,yA,zA), B(xB,yB,zB) and C(xC,yC,zC), which respectively are approximately at dA, dB and dC from you goal point G(xG,yG,zG).
Let's say cA, cB and cC are the confidence rate ( 0 < cX <= 1 ) of each point.
Basically, you might take something really close to 1, like {0.95,0.97,0.99}.
If you don't know, try different coefficient depending of distance avg. If distance is really big, you're likely to be not very confident about it.
Here is the way i'll do it :
var sum = (cA*dA) + (cB*dB) + (cC*dC);
dA = cA*dA/sum;
dB = cB*dB/sum;
dC = cC*dC/sum;
xG = (xA*dA) + (xB*dB) + (xC*dC);
yG = (yA*dA) + (yB*dB) + (yC*dC);
xG = (zA*dA) + (zB*dB) + (zC*dC);
Basic, and not really smart but will do the job for some simple tasks.
EDIT
You can take any confidence coef you want in [0,inf[, but IMHO, restraining at [0,1] is a good idea to keep a realistic result.

Finding initial speed and angle to hit a known position (parabolic trajectory)

I am currently doing a small turn based cannon game with XNA 4.0. The game is very simple: the player chooses the speed and angle at which he desires to shoot his rocket in order to hit another player. There is also a randomly generated wind vector that affects the X trajectory of the rocket. I would like to add an AI so that the player could play against the computer in a single player mode.
The way I would like to implement the AI is very simple: find the velocity and angle that would make the rocket hit the player directly, and add a random modifier to those fields so that the AI doesn't hit another player each time.
This is the code I use in order to update the position and speed of the rocket:
Vector2 gravity = new Vector2(0, (float)400); // 400 is the sweet spot value that i have found works best for the gravity
Vector2 totalAcceleration = gravity + _terrain.WindDirection;
float deltaT = (float)gameTime.ElapsedGameTime.TotalSeconds; // Elapsed time since last update() call
foreach (Rocket rocket in _instantiatedRocketList)
{
rocket.RocketSpeed += Vector2.Multiply(gravity, deltaT); // Only changes the Y component
rocket.RocketSpeed += Vector2.Multiply(_terrain.WindDirection, deltaT); // Only changes the X component
rocket.RocketPosition += Vector2.Multiply(rocket.RocketSpeed, deltaT) + Vector2.Multiply(totalAcceleration, (float)0.5) * deltaT * deltaT;
// We update the angle of the rocket accordingly
rocket.RocketAngle = (float)Math.Atan2(rocket.RocketSpeed.X, -rocket.RocketSpeed.Y);
rocket.CreateSmokeParticles(3);
}
I know that the basic equations to find the final X and Y coordinates are:
X = V0 * cos(theta) * totalFlightTime
Y = V0 * sin(theta) * totalFlightTime - 0.5 * g * totalFlightTime^2
where X and Y are the coordinates of the player I want to hit, V0 the initial speed, theta the angle at witch the rocket is shot, totalFlightTime is, like the name says, the total flight time of the rocket until it reaches (X, Y) and g is the gravity (400 in my game).
Questions:
What I am having problems with, is knowing where to add the wind in those formulas (is it just adding "+ windDirection * totalFlightTime" in the X = equation?), and also what to do with those equations in order to do what I want to do (finding the initial speed and theta angle) since there are 3 variables (V0, theta and totalFlightTime) and only 2 equations?
Thanks for your time.
You can do this as follows:
Assuming there is no specific limit to V0 (i.e. the robot can fire the rocket at any desired speed) and using the substitutions
T=totalFlightTime
Vx=V0cos(theta)
Vy=V0sin(theta)
Choose an arbitrary value for Vx. Now your first equation simplifies to
X=VxT so T=X/Vx
to solve for T. Now substitute the value of T into the second equation and solve for Vy
Y=VyT + gT^2/2 so Vy = (Y - gT^2/2)/T
Finally you can now solve for V0 and theta
V0 = Sqrt(Vx^2 + Vy^2) and Theta = aTan(Vy/Vx)
Note that your initial choice of Vx will determine the trajectory the missile will take - if Vx is large then T will be small and the trajectory will be almost a straight line (like a bullet fired at a nearby target) - if Vx is small then T will be large and the trajectory will be an arc (like a mortar round's path). You dis start with three variables (V0, totalFlightTime, and theta) but they are dependent variables so choosing any one (or in this case Vx) plus the two equations solves for the other two. You could also pre-determine flight time and solve for Vx, Vy, theta and V0, or predetermine theta (although this would be tricky as some theta wouldn't provide a real solution.

Finding translation and scale on two sets of points to get least square error in their distance?

I have two sets of 3D points (original and reconstructed) and correspondence information about pairs - which point from one set represents the second one. I need to find 3D translation and scaling factor which transforms reconstruct set so the sum of square distances would be least (rotation would be nice too, but points are rotated similarly, so this is not main priority and might be omitted in sake of simplicity and speed). And so my question is - is this solved and available somewhere on the Internet? Personally, I would use least square method, but I don't have much time (and although I'm somewhat good at math, I don't use it often, so it would be better for me to avoid it), so I would like to use other's solution if it exists. I prefer solution in C++, for example using OpenCV, but algorithm alone is good enough.
If there is no such solution, I will calculate it by myself, I don't want to bother you so much.
SOLUTION: (from your answers)
For me it's Kabsch alhorithm;
Base info: http://en.wikipedia.org/wiki/Kabsch_algorithm
General solution: http://nghiaho.com/?page_id=671
STILL NOT SOLVED:
I also need scale. Scale values from SVD are not understandable for me; when I need scale about 1-4 for all axises (estimated by me), SVD scale is about [2000, 200, 20], which is not helping at all.
Since you are already using Kabsch algorithm, just have a look at Umeyama's paper which extends it to get scale. All you need to do is to get the standard deviation of your points and calculate scale as:
(1/sigma^2)*trace(D*S)
where D is the diagonal matrix in SVD decomposition in the rotation estimation and S is either identity matrix or [1 1 -1] diagonal matrix, depending on the sign of determinant of UV (which Kabsch uses to correct reflections into proper rotations). So if you have [2000, 200, 20], multiply the last element by +-1 (depending on the sign of determinant of UV), sum them and divide by the standard deviation of your points to get scale.
You can recycle the following code, which is using the Eigen library:
typedef Eigen::Matrix<double, 3, 1, Eigen::DontAlign> Vector3d_U; // microsoft's 32-bit compiler can't put Eigen::Vector3d inside a std::vector. for other compilers or for 64-bit, feel free to replace this by Eigen::Vector3d
/**
* #brief rigidly aligns two sets of poses
*
* This calculates such a relative pose <tt>R, t</tt>, such that:
*
* #code
* _TyVector v_pose = R * r_vertices[i] + t;
* double f_error = (r_tar_vertices[i] - v_pose).squaredNorm();
* #endcode
*
* The sum of squared errors in <tt>f_error</tt> for each <tt>i</tt> is minimized.
*
* #param[in] r_vertices is a set of vertices to be aligned
* #param[in] r_tar_vertices is a set of vertices to align to
*
* #return Returns a relative pose that rigidly aligns the two given sets of poses.
*
* #note This requires the two sets of poses to have the corresponding vertices stored under the same index.
*/
static std::pair<Eigen::Matrix3d, Eigen::Vector3d> t_Align_Points(
const std::vector<Vector3d_U> &r_vertices, const std::vector<Vector3d_U> &r_tar_vertices)
{
_ASSERTE(r_tar_vertices.size() == r_vertices.size());
const size_t n = r_vertices.size();
Eigen::Vector3d v_center_tar3 = Eigen::Vector3d::Zero(), v_center3 = Eigen::Vector3d::Zero();
for(size_t i = 0; i < n; ++ i) {
v_center_tar3 += r_tar_vertices[i];
v_center3 += r_vertices[i];
}
v_center_tar3 /= double(n);
v_center3 /= double(n);
// calculate centers of positions, potentially extend to 3D
double f_sd2_tar = 0, f_sd2 = 0; // only one of those is really needed
Eigen::Matrix3d t_cov = Eigen::Matrix3d::Zero();
for(size_t i = 0; i < n; ++ i) {
Eigen::Vector3d v_vert_i_tar = r_tar_vertices[i] - v_center_tar3;
Eigen::Vector3d v_vert_i = r_vertices[i] - v_center3;
// get both vertices
f_sd2 += v_vert_i.squaredNorm();
f_sd2_tar += v_vert_i_tar.squaredNorm();
// accumulate squared standard deviation (only one of those is really needed)
t_cov.noalias() += v_vert_i * v_vert_i_tar.transpose();
// accumulate covariance
}
// calculate the covariance matrix
Eigen::JacobiSVD<Eigen::Matrix3d> svd(t_cov, Eigen::ComputeFullU | Eigen::ComputeFullV);
// calculate the SVD
Eigen::Matrix3d R = svd.matrixV() * svd.matrixU().transpose();
// compute the rotation
double f_det = R.determinant();
Eigen::Vector3d e(1, 1, (f_det < 0)? -1 : 1);
// calculate determinant of V*U^T to disambiguate rotation sign
if(f_det < 0)
R.noalias() = svd.matrixV() * e.asDiagonal() * svd.matrixU().transpose();
// recompute the rotation part if the determinant was negative
R = Eigen::Quaterniond(R).normalized().toRotationMatrix();
// renormalize the rotation (not needed but gives slightly more orthogonal transformations)
double f_scale = svd.singularValues().dot(e) / f_sd2_tar;
double f_inv_scale = svd.singularValues().dot(e) / f_sd2; // only one of those is needed
// calculate the scale
R *= f_inv_scale;
// apply scale
Eigen::Vector3d t = v_center_tar3 - (R * v_center3); // R needs to contain scale here, otherwise the translation is wrong
// want to align center with ground truth
return std::make_pair(R, t); // or put it in a single 4x4 matrix if you like
}
For 3D points the problem is known as the Absolute Orientation problem. A c++ implementation is available from Eigen http://eigen.tuxfamily.org/dox/group__Geometry__Module.html#gab3f5a82a24490b936f8694cf8fef8e60 and paper http://web.stanford.edu/class/cs273/refs/umeyama.pdf
you can use it via opencv by converting the matrices to eigen with cv::cv2eigen() calls.
Start with translation of both sets of points. So that their centroid coincides with the origin of the coordinate system. Translation vector is just the difference between these centroids.
Now we have two sets of coordinates represented as matrices P and Q. One set of points may be obtained from other one by applying some linear operator (which performs both scaling and rotation). This operator is represented by 3x3 matrix X:
P * X = Q
To find proper scale/rotation we just need to solve this matrix equation, find X, then decompose it into several matrices, each representing some scaling or rotation.
A simple (but probably not numerically stable) way to solve it is to multiply both parts of the equation to the transposed matrix P (to get rid of non-square matrices), then multiply both parts of the equation to the inverted PT * P:
PT * P * X = PT * Q
X = (PT * P)-1 * PT * Q
Applying Singular value decomposition to matrix X gives two rotation matrices and a matrix with scale factors:
X = U * S * V
Here S is a diagonal matrix with scale factors (one scale for each coordinate), U and V are rotation matrices, one properly rotates the points so that they may be scaled along the coordinate axes, other one rotates them once more to align their orientation to second set of points.
Example (2D points are used for simplicity):
P = 1 2 Q = 7.5391 4.3455
2 3 12.9796 5.8897
-2 1 -4.5847 5.3159
-1 -6 -15.9340 -15.5511
After solving the equation:
X = 3.3417 -1.2573
2.0987 2.8014
After SVD decomposition:
U = -0.7317 -0.6816
-0.6816 0.7317
S = 4 0
0 3
V = -0.9689 -0.2474
-0.2474 0.9689
Here SVD has properly reconstructed all manipulations I performed on matrix P to get matrix Q: rotate by the angle 0.75, scale X axis by 4, scale Y axis by 3, rotate by the angle -0.25.
If sets of points are scaled uniformly (scale factor is equal by each axis), this procedure may be significantly simplified.
Just use Kabsch algorithm to get translation/rotation values. Then perform these translation and rotation (centroids should coincide with the origin of the coordinate system). Then for each pair of points (and for each coordinate) estimate Linear regression. Linear regression coefficient is exactly the scale factor.
A good explanation Finding optimal rotation and translation between corresponding 3D points
The code is in matlab but it's trivial to convert to opengl using the cv::SVD function
You might want to try ICP (Iterative closest point).
Given two sets of 3d points, it will tell you the transformation (rotation + translation) to go from the first set to the second one.
If you're interested in a c++ lightweight implementation, try libicp.
Good luck!
The general transformation, as well the scale can be retrieved via Procrustes Analysis. It works by superimposing the objects on top of each other and tries to estimate the transformation from that setting. It has been used in the context of ICP, many times. In fact, your preference, Kabash algorithm is a special case of this.
Moreover, Horn's alignment algorithm (based on quaternions) also finds a very good solution, while being quite efficient. A Matlab implementation is also available.
Scale can be inferred without SVD, if your points are uniformly scaled in all directions (I could not make sense of SVD-s scale matrix either). Here is how I solved the same problem:
Measure distances of each point to other points in the point cloud to get a 2d table of distances, where entry at (i,j) is norm(point_i-point_j). Do the same thing for the other point cloud, so you get two tables -- one for original and the other for reconstructed points.
Divide all values in one table by the corresponding values in the other table. Because the points correspond to each other, the distances do too. Ideally, the resulting table has all values being equal to each other, and this is the scale.
The median value of the divisions should be pretty close to the scale you are looking for. The mean value is also close, but I chose median just to exclude outliers.
Now you can use the scale value to scale all the reconstructed points and then proceed to estimating the rotation.
Tip: If there are too many points in the point clouds to find distances between all of them, then a smaller subset of distances will work, too, as long as it is the same subset for both point clouds. Ideally, just one distance pair would work if there is no measurement noise, e.g when one point cloud is directly derived from the other by just rotating it.
you can also use ScaleRatio ICP proposed by BaoweiLin
The code can be found in github

AI algorithm to "shoot" at a target in a 2d game

in my 2d game I would like to create an intelligent bot who can "shoot" to the player.
Suppose I can pass to my bot:
actual xEnemy, yEnemy
also enemy speed and angle direction
How can I calculate "where to shoot" considering that Bot must rotate its gun to the right direction?
This is a really big problem for me, because.. I'm absolutely not good in math!
Thanks in advance for your precious help!
Notation: I write vectors in capital letters, scalars in lower case, and ∠V for the angle that the vector V makes with the x-axis. (Which you can compute with the function atan2 in many languages.)
The simplest case is a stationary shooter which can rotate instantly.
Let the target be at the position A and moving with velocity VA, and the shooter be stationary at the position B and can fire bullets with speed s. Let the shooter fire at time 0. The bullet hits at time t such that |A − B + t VA| = t s. This is a straightforward quadratic equation in t, which you should be easily able to solve (or determine that there is no solution). Having determined t, you can now work out the firing angle, which is just ∠(A − B + t VA).
Now suppose that the shooter is not stationary but has constant velocity VB. (I'm supposing Newtonian relativity here, i.e. the bullet velocity is added to the shooter's velocity.)
It's still a straightforward quadratic equation to work out the time to hit: |A − B + t(VA − VB)| = t s. In this case the firing angle is ∠(A − B + t (VA − VB)).
What if the shooter waits until time u before firing? Then the bullet hits the target when |A − B + t(VA − VB)| = (t − u) s. The firing angle is still ∠(A − B + t(VA − VB)).
Now for your problem. Suppose that the shooter can complete a half rotation in time r. Then it can certainly fire at time r. (Basically: work out the necessary firing angle, if any, for a shot at time r, as described above, rotate to that angle, stop, wait until time r, then fire.)
But you probably want to know the earliest time at which the shooter can fire. Here's where you probably want to use successive approximation to find it. (Sketch of algorithm: Can you fire at time 0? No. Can you fire at time r? Yes. Can you fire at time ½ r? No. etc.)
What you're asking is non-trivial. I'll start with the simplest solution and expand on that.
First, assume that both you and your enemy are stationary.
You need to compute the angle between you and your enemy, rotate your weapon to point at the enemy, and then fire. Use your favorite search engine to find a description of how to find the angle between two points on a plane (you did say 2D).
Once you write code that can do the above, move on to:
Your enemy is moving in a constant direction at a constant velocity. You are still stationary. This is a surprisingly difficult problem. To simplify, we'll assume that you can aim your weapon instantaneously.
If you know where you and the enemy are, and the speed and direction of the enemy, then you can determine the enemy's position (and, consequently, his distance and direction from you) at any time.
You know how fast your projectile can travel. So if you draw a line from your current position to intercept any position on the enemy's expected path of travel, you can determine how long it will take your projectile to hit the enemy. The key, then, is to find the point on the enemy's path that, were you to fire the projectile immediately, it would intersect the enemy at the proper time. This typically requires successive approximations.
If you can't rotate your weapon instantaneously, then the problem becomes more difficult because the time it takes to rotate your weapon to point at the enemy depends on how fast and in what direction the enemy is traveling. More approximations are required.
Things get even more involved when you and the enemy are both moving, although it's possible to construct the math so that you "hold yourself still". That is, do a transformation on the enemy's velocity and trajectory to reflect how the enemy is moving in relation to you. The math then becomes identical to the case of you being stationary.
The math itself is at most elementary trigonometry. You need to know how to compute the distance between two points, the distance between a line and a point, the angle between two points, compute points on a line given a starting point and direction, and how to rotate about an arbitrary point. All of those are well-known problems that have many good examples online. You'll have to do a little research to find them, though.
Probably your best bet is to find a good computer graphics tutorial.
The solution to this problem for the "simple" case, where the gun does NOT need to rotate has been asked and answered several times. I found the best answer to be in this post (Jeffrey Hantin has a good explanation).
As Jim Mischel said, the answer to the rotating case is not even a little trivial. This is because the amount of rotation you have to do is a function of the intercept position of the target (where the collision happens). I do not know if there is a clean closed form solution to this (seems unlikely given the formulas), but I was able to solve it by working backwards and iterating to a solution.
The basic idea works like this:
Assume your "entity" will rotate from its current facing position to face the intercept position and then fire immediately.
Pick a time of impact.
Calculate the final position of the target at that time given it is moving at constant velocity.
Calculate how long it would have taken the projectile to travel to that position.
Calculate how long it would take your entity to rotate to face the intercept position.
Calculate the difference of the impact time and the rotation/travel times.
Adjust the time of impact up/down so that the difference gets smaller each time (e.g. binary search).
When you get close enough, you are done. Otherwise, start the calculations again.
In the simple case, you could know from the discriminant of the quadratic if you had 0, 1, or 2 solutions and pick the best one. I don't think you can guarantee that here, but you can bound the time range you are willing to search over and how many iterations you will search. This works very well in practice.
Since I could not find the solution to this on the web, I am going to post mine. I wrote a function to handle this problem specifically. I have a blog entry on the entire calculation here. There is also a nice video showing it here.
The Code:
/* Calculate the future position of a moving target so that
* a turret can turn to face the position and fire a projectile.
*
* This algorithm works by "guessing" an intial time of impact
* for the projectile 0.5*(tMin + tMax). It then calculates
* the position of the target at that time and computes what the
* time for the turret to rotate to that position (tRot0) and
* the flight time of the projectile (tFlight). The algorithms
* drives the difference between tImpact and (tFlight + tRot) to
* zero using a binary search.
*
* The "solution" returned by the algorithm is the impact
* location. The shooter should rotate towards this
* position and fire immediately.
*
* The algorithm will fail (and return false) under the
* following conditions:
* 1. The target is out of range. It is possible that the
* target is out of range only for a short time but in
* range the rest of the time, but this seems like an
* unnecessary edge case. The turret is assumed to
* "react" by checking range first, then plot to shoot.
* 2. The target is heading away from the shooter too fast
* for the projectile to reach it before tMax.
* 3. The solution cannot be reached in the number of steps
* allocated to the algorithm. This seems very unlikely
* since the default value is 40 steps.
*
* This algorithm uses a call to sqrt and atan2, so it
* should NOT be run continuously.
*
* On the other hand, nominal runs show convergence usually
* in about 7 steps, so this may be a good 'do a step per
* frame' calculation target.
*
*/
bool CalculateInterceptShotPosition(const Vec2& pShooter,
const Vec2& vShooter,
const Vec2& pSFacing0,
const Vec2& pTarget0,
const Vec2& vTarget,
float64 sProjectile,
float64 wShooter,
float64 maxDist,
Vec2& solution,
float64 tMax = 4.0,
float64 tMin = 0.0
)
{
cout << "----------------------------------------------" << endl;
cout << " Starting Calculation [" << tMin << "," << tMax << "]" << endl;
cout << "----------------------------------------------" << endl;
float64 tImpact = (tMin + tMax)/2;
float64 tImpactLast = tImpact;
// Tolerance in seconds
float64 SOLUTION_TOLERANCE_SECONDS = 0.01;
const int MAX_STEPS = 40;
for(int idx = 0; idx < MAX_STEPS; idx++)
{
// Calculate the position of the target at time tImpact.
Vec2 pTarget = pTarget0 + tImpact*vTarget;
// Calulate the angle between the shooter and the target
// when the impact occurs.
Vec2 toTarget = pTarget - pShooter;
float64 dist = toTarget.Length();
Vec2 pSFacing = (pTarget - pShooter);
float64 pShootRots = pSFacing.AngleRads();
float64 tRot = fabs(pShootRots)/wShooter;
float64 tFlight = dist/sProjectile;
float64 tShot = tImpact - (tRot + tFlight);
cout << "Iteration: " << idx
<< " tMin: " << tMin
<< " tMax: " << tMax
<< " tShot: " << tShot
<< " tImpact: " << tImpact
<< " tRot: " << tRot
<< " tFlight: " << tFlight
<< " Impact: " << pTarget.ToString()
<< endl;
if(dist >= maxDist)
{
cout << "FAIL: TARGET OUT OF RANGE (" << dist << "m >= " << maxDist << "m)" << endl;
return false;
}
tImpactLast = tImpact;
if(tShot > 0.0)
{
tMax = tImpact;
tImpact = (tMin + tMax)/2;
}
else
{
tMin = tImpact;
tImpact = (tMin + tMax)/2;
}
if(fabs(tImpact - tImpactLast) < SOLUTION_TOLERANCE_SECONDS)
{ // WE HAVE A WINNER!!!
solution = pTarget;
return true;
}
}
return false;
}

Resources