I search for non iterative, closed form, algorithm to find Least squares solution for point closest to the set of 3d lines. It is similar to 3d point triangulation (to minimize re-projections) but seems to be be simpler and faster?
Lines can be described in any form, 2 points, point and unit direction or similar.
Let the i th line be given by point ai and unit direction vector di. We need to find the single point that minimizes the sum of squared point to line distances. This is where the gradient is the zero vector:
Expanding the gradient,
Algebra yields a canonical 3x3 linear system,
where the k'th row (a 3-element row vector) of matrix M is
with vector ek the respective unit basis vector, and
It's not hard to turn this into code. I borrowed (and fixed a small bug in) a Gaussian elimination function from Rosettacode to solve the system. Thanks to the author!
#include <stdio.h>
#include <math.h>
typedef double VEC[3];
typedef VEC MAT[3];
void solve(double *a, double *b, double *x, int n); // linear solver
double dot(VEC a, VEC b) { return a[0]*b[0] + a[1]*b[1] + a[2]*b[2]; }
void find_nearest_point(VEC p, VEC a[], VEC d[], int n) {
MAT m = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
VEC b = {0, 0, 0};
for (int i = 0; i < n; ++i) {
double d2 = dot(d[i], d[i]), da = dot(d[i], a[i]);
for (int ii = 0; ii < 3; ++ii) {
for (int jj = 0; jj < 3; ++jj) m[ii][jj] += d[i][ii] * d[i][jj];
m[ii][ii] -= d2;
b[ii] += d[i][ii] * da - a[i][ii] * d2;
}
}
solve(&m[0][0], b, p, 3);
}
// Debug printing.
void pp(VEC v, char *l, char *r) {
printf("%s%.3lf, %.3lf, %.3lf%s", l, v[0], v[1], v[2], r);
}
void pv(VEC v) { pp(v, "(", ")"); }
void pm(MAT m) { for (int i = 0; i < 3; ++i) pp(m[i], "\n[", "]"); }
// A simple verifier.
double dist2(VEC p, VEC a, VEC d) {
VEC pa = { a[0]-p[0], a[1]-p[1], a[2]-p[2] };
double dpa = dot(d, pa);
return dot(d, d) * dot(pa, pa) - dpa * dpa;
}
double sum_dist2(VEC p, VEC a[], VEC d[], int n) {
double sum = 0;
for (int i = 0; i < n; ++i) sum += dist2(p, a[i], d[i]);
return sum;
}
// Check 26 nearby points and verify the provided one is nearest.
int is_nearest(VEC p, VEC a[], VEC d[], int n) {
double min_d2 = 1e100;
int ii = 2, jj = 2, kk = 2;
#define D 0.01
for (int i = -1; i <= 1; ++i)
for (int j = -1; j <= 1; ++j)
for (int k = -1; k <= 1; ++k) {
VEC pp = { p[0] + D * i, p[1] + D * j, p[2] + D * k };
double d2 = sum_dist2(pp, a, d, n);
// Prefer provided point among equals.
if (d2 < min_d2 || i == 0 && j == 0 && k == 0 && d2 == min_d2) {
min_d2 = d2;
ii = i; jj = j; kk = k;
}
}
return ii == 0 && jj == 0 && kk == 0;
}
void normalize(VEC v) {
double len = sqrt(dot(v, v));
v[0] /= len;
v[1] /= len;
v[2] /= len;
}
int main(void) {
VEC a[] = {{-14.2, 17, -1}, {1, 1, 1}, {2.3, 4.1, 9.8}, {1,2,3}};
VEC d[] = {{1.3, 1.3, -10}, {12.1, -17.2, 1.1}, {19.2, 31.8, 3.5}, {4,5,6}};
int n = 4;
for (int i = 0; i < n; ++i) normalize(d[i]);
VEC p;
find_nearest_point(p, a, d, n);
pv(p);
printf("\n");
if (!is_nearest(p, a, d, n)) printf("Woops. Not nearest.\n");
return 0;
}
// A linear solver from rosettacode (with bug fix: added a missing fabs())
#define mat_elem(a, y, x, n) (a + ((y) * (n) + (x)))
void swap_row(double *a, double *b, int r1, int r2, int n)
{
double tmp, *p1, *p2;
int i;
if (r1 == r2) return;
for (i = 0; i < n; i++) {
p1 = mat_elem(a, r1, i, n);
p2 = mat_elem(a, r2, i, n);
tmp = *p1, *p1 = *p2, *p2 = tmp;
}
tmp = b[r1], b[r1] = b[r2], b[r2] = tmp;
}
void solve(double *a, double *b, double *x, int n)
{
#define A(y, x) (*mat_elem(a, y, x, n))
int i, j, col, row, max_row, dia;
double max, tmp;
for (dia = 0; dia < n; dia++) {
max_row = dia, max = fabs(A(dia, dia));
for (row = dia + 1; row < n; row++)
if ((tmp = fabs(A(row, dia))) > max) max_row = row, max = tmp;
swap_row(a, b, dia, max_row, n);
for (row = dia + 1; row < n; row++) {
tmp = A(row, dia) / A(dia, dia);
for (col = dia+1; col < n; col++)
A(row, col) -= tmp * A(dia, col);
A(row, dia) = 0;
b[row] -= tmp * b[dia];
}
}
for (row = n - 1; row >= 0; row--) {
tmp = b[row];
for (j = n - 1; j > row; j--) tmp -= x[j] * A(row, j);
x[row] = tmp / A(row, row);
}
#undef A
}
This isn't extensively tested, but seems to be working fine.
Let base point of line is p and unit direction vector is d.
Then distance from point v to this line might be calculated using cross product
SquaredDist = ((v - p) x d)^2
Using Maple packet symbolic calculation, we can get
d := <dx, dy, dz>;
v := <vx, vy, vz>;
p := <px, py, pz>;
w := v - p;
cp := CrossProduct(d, w);
nrm := BilinearForm(cp, cp, conjugate=false); //squared dist
nr := expand(nrm);
//now partial derivatives
nrx := diff(nr, vx);
//results:
nrx := -2*dz^2*px-2*dy^2*px+2*dz^2*vx+2*dy^2*vx
+2*dx*py*dy-2*dx*vy*dy+2*dz*dx*pz-2*dz*dx*vz
nry := -2*dx^2*py-2*dz^2*py-2*dy*vz*dz+2*dx^2*vy
+2*dz^2*vy+2*dy*pz*dz+2*dx*dy*px-2*dx*dy*vx
nrz := -2*dy^2*pz+2*dy^2*vz-2*dy*dz*vy+2*dx^2*vz
-2*dx^2*pz-2*dz*vx*dx+2*dy*dz*py+2*dz*px*dx
To minimize sum of squared distances, we have to make system of linear equations for zero partial derivatives like this:
vx*2*(Sum(dz^2)+Sum(dy^2)) + vy * (-2*Sum(dx*dy)) + vz *(-2*Sum(dz*dx)) =
2*Sum(dz^2*px)-2*Sum(dy^2*px) -2*Sum(dx*py*dy)-2*Sum(dz*dx*pz)
where
Sum(dz^2) = Sum{over all i in line indexes} {dz[i] * dz[i]}
and solve it for unknowns vx, vy, vz
Edit: Old erroneous answer for planes instead of lines, left for reference
If we use general equation of line
A * x + B * y + C * z + D = 0
then distance from point (x, y, z) to this line is
Dist = Abs(A * x + B * y + C * z + D) / Sqrt(A^2 + B^2 + C^2)
To simplify - just normalize all line equations dividing by Norm's
Norm = Sqrt(A^2 + B^2 + C^2)
a = A / Norm
b = B / Norm
c = C / Norm
d = D / Norm
now equation is
a * x + b * y + c * z + d = 0
and distance
Dist = Abs(a * x + b * y + c * z + d)
and we can use squared distances like LS method (ai, bi, ci, di are coefficients for i-th line)
F = Sum(ai*x + bi*y + ci * z + d)^2 =
Sum(ai^2*x^2 + bi^2*y^2 + ci^2*z^2 + d^2 +
2 * (ai*bi*x*y + ai*ci*x*z + bi*y*ci*z + ai*x*di + bi*y*di + ci*z*di))
partial derivatives
dF/dx = 2*Sum(ai^2*x + ai*bi*y + ai*ci*z + ai*di) = 0
dF/dy = 2*Sum(bi^2*y + ai*bi*x + bi*ci*z + bi*di) = 0
dF/dz = 2*Sum(ci^2*z + ai*ci*x + bi*ci*y + ci*di) = 0
so we have system of linear equation
x * Sum(ai^2) + y * Sum(ai*bi) + z * Sum(ai*ci)= - Sum(ai*di)
y * Sum(bi^2) + x * Sum(ai*bi) + z * Sum(bi*ci)= - Sum(bi*di)
z * Sum(ci^2) + x * Sum(ai*ci) + y * Sum(bi*ci)= - Sum(ci*di)
x * Saa + y * Sab + z * Sac = - Sad
x * Sab + y * Sbb + z * Sbc = - Sbd
x * Sac + y * Sbc + z * Scc = - Scd
where S** are corresponding sums
and can solve it for unknowns x, y, z
I needed this for a sketch in Processing, so I ported Gene's answer. Works great and thought it might save someone else a little time. Unfortunately PVector/PMatrix don't have array accessors for vectors or matrices so I had to add these as local functions.
float getv(PVector v, int i) {
if(i == 0) return v.x;
if(i == 1) return v.y;
return v.z;
}
void setv(PVector v, int i, float value) {
if (i == 0) v.x = value;
else if (i == 1) v.y = value;
else v.z = value;
}
void incv(PVector v, int i, float value) {
setv(v,i,getv(v,i) + value);
}
float getm(float[] mm, int r, int c) { return mm[c + r*4]; }
void setm(float[] mm, int r, int c, float value) { mm[c + r*4] = value; }
void incm(float[] mm, int r, int c, float value) { mm[c + r*4] += value; }
PVector findNearestPoint(PVector a[], PVector d[]) {
var mm = new float[16];
var b = new PVector();
var n = a.length;
for (int i = 0; i < n; ++i) {
var d2 = d[i].dot(d[i]);
var da = d[i].dot(a[i]);
for (int ii = 0; ii < 3; ++ii) {
for (int jj = 0; jj < 3; ++jj) {
incm(mm,ii,jj, getv(d[i],ii) * getv(d[i],jj));
}
incm(mm, ii,ii, -d2);
incv(b, ii, getv(d[i], ii) * da - getv(a[i], ii) * d2);
}
}
var p = solve(mm, new float[] {b.x, b.y, b.z});
return new PVector(p[0],p[1],p[2]);
}
// Verifier
float dist2(PVector p, PVector a, PVector d) {
PVector pa = new PVector( a.x-p.x, a.y-p.y, a.z-p.z );
float dpa = d.dot(pa);
return d.dot(d) * pa.dot(pa) - dpa * dpa;
}
//double sum_dist2(VEC p, VEC a[], VEC d[], int n) {
float sum_dist2(PVector p, PVector a[], PVector d[]) {
int n = a.length;
float sum = 0;
for (int i = 0; i < n; ++i) {
sum += dist2(p, a[i], d[i]);
}
return sum;
}
// Check 26 nearby points and verify the provided one is nearest.
boolean isNearest(PVector p, PVector a[], PVector d[]) {
float min_d2 = 3.4028235E38;
int ii = 2, jj = 2, kk = 2;
final float D = 0.1f;
for (int i = -1; i <= 1; ++i)
for (int j = -1; j <= 1; ++j)
for (int k = -1; k <= 1; ++k) {
PVector pp = new PVector( p.x + D * i, p.y + D * j, p.z + D * k );
float d2 = sum_dist2(pp, a, d);
// Prefer provided point among equals.
if (d2 < min_d2 || i == 0 && j == 0 && k == 0 && d2 == min_d2) {
min_d2 = d2;
ii = i; jj = j; kk = k;
}
}
return ii == 0 && jj == 0 && kk == 0;
}
void setup() {
PVector a[] = {
new PVector(-14.2, 17, -1),
new PVector(1, 1, 1),
new PVector(2.3, 4.1, 9.8),
new PVector(1,2,3)
};
PVector d[] = {
new PVector(1.3, 1.3, -10),
new PVector(12.1, -17.2, 1.1),
new PVector(19.2, 31.8, 3.5),
new PVector(4,5,6)
};
int n = 4;
for (int i = 0; i < n; ++i)
d[i].normalize();
PVector p = findNearestPoint(a, d);
println(p);
if (!isNearest(p, a, d))
println("Woops. Not nearest.\n");
}
// From rosettacode (with bug fix: added a missing fabs())
int mat_elem(int y, int x) { return y*4+x; }
void swap_row(float[] a, float[] b, int r1, int r2, int n)
{
float tmp;
int p1, p2;
int i;
if (r1 == r2) return;
for (i = 0; i < n; i++) {
p1 = mat_elem(r1, i);
p2 = mat_elem(r2, i);
tmp = a[p1];
a[p1] = a[p2];
a[p2] = tmp;
}
tmp = b[r1];
b[r1] = b[r2];
b[r2] = tmp;
}
float[] solve(float[] a, float[] b)
{
float[] x = new float[] {0,0,0};
int n = x.length;
int i, j, col, row, max_row, dia;
float max, tmp;
for (dia = 0; dia < n; dia++) {
max_row = dia;
max = abs(getm(a, dia, dia));
for (row = dia + 1; row < n; row++) {
if ((tmp = abs(getm(a, row, dia))) > max) {
max_row = row;
max = tmp;
}
}
swap_row(a, b, dia, max_row, n);
for (row = dia + 1; row < n; row++) {
tmp = getm(a, row, dia) / getm(a, dia, dia);
for (col = dia+1; col < n; col++) {
incm(a, row, col, -tmp * getm(a, dia, col));
}
setm(a,row,dia, 0);
b[row] -= tmp * b[dia];
}
}
for (row = n - 1; row >= 0; row--) {
tmp = b[row];
for (j = n - 1; j > row; j--) {
tmp -= x[j] * getm(a, row, j);
}
x[row] = tmp / getm(a, row, row);
}
return x;
}
In the draft section 7.2.1.3 of The art of computer programming, generating all combinations, Knuth introduced Algorithm C for generating Chase's sequence.
He also mentioned a similar algorithm (based on the following equation) working with index-list without source code (exercise 45 of the draft).
I finally worked out a c++ version which I think is quite ugly. To generate all C_n^m combination, the memory complexity is about 3 (m+1) and the time complexity is bounded by O(m n^m)
class chase_generator_t{
public:
using size_type = ptrdiff_t;
enum class GET : char{ VALUE, INDEX };
chase_generator_t(size_type _n) : n(_n){}
void choose(size_type _m){
m = _m;
++_m;
index.resize(_m);
threshold.resize(_m + 1);
tag.resize(_m);
for (size_type i = 0, j = n - m; i != _m; ++i){
index[i] = j + i;
tag[i] = tag_t::DECREASE;
using std::max;
threshold[i] = max(i - 1, (index[i] - 3) | 1);
}
threshold[_m] = n;
}
bool get(size_type &x, size_type &y, GET const which){
if (which == GET::VALUE) return __get<false>(x, y);
return __get<true>(x, y);
}
size_type get_n() const{
return n;
}
size_type get_m() const{
return m;
}
size_type operator[](size_t const i) const{
return index[i];
}
private:
enum class tag_t : char{ DECREASE, INCREASE };
size_type n, m;
std::vector<size_type> index, threshold;
std::vector<tag_t> tag;
template<bool GetIndex>
bool __get(size_type &x, size_type &y){
using std::max;
size_type p = 0, i, q;
find:
q = p + 1;
if (index[p] == threshold[q]){
if (q >= m) return false;
p = q;
goto find;
}
x = GetIndex ? p : index[p];
if (tag[p] == tag_t::INCREASE){
using std::min;
increase:
index[p] = min(index[p] + 2, threshold[q]);
threshold[p] = index[p] - 1;
}
else if (index[p] && (i = (index[p] - 1) & ~1) >= p){
index[p] = i;
threshold[p] = max(p - 1, (index[p] - 3) | 1);
}
else{
tag[p] = tag_t::INCREASE;
i = p | 1;
if (index[p] == i) goto increase;
index[p] = i;
threshold[p] = index[p] - 1;
}
y = index[p];
for (q = 0; q != p; ++q){
tag[q] = tag_t::DECREASE;
threshold[q] = max(q - 1, (index[q] - 3) | 1);
}
return true;
}
};
Does any one has a better implementation, i.e. run faster with the same memory or use less memory with the same speed?
I think that the C code below is closer to what Knuth had in mind. Undoubtedly there are ways to make it more elegant (in particular, I'm leaving some scaffolding in case it helps with experimentation), though I'm skeptical that the array w can be disposed of. If storage is really important for some reason, then steal the sign bit from the a array.
#include <stdbool.h>
#include <stdio.h>
enum {
N = 10,
T = 5
};
static void next(int a[], bool w[], int *r) {
bool found_r = false;
int j;
for (j = *r; !w[j]; j++) {
int b = a[j] + 1;
int n = a[j + 1];
if (b < (w[j + 1] ? n - (2 - (n & 1)) : n)) {
if ((b & 1) == 0 && b + 1 < n) b++;
a[j] = b;
if (!found_r) *r = j > 1 ? j - 1 : 0;
return;
}
w[j] = a[j] - 1 >= j;
if (w[j] && !found_r) {
*r = j;
found_r = true;
}
}
int b = a[j] - 1;
if ((b & 1) != 0 && b - 1 >= j) b--;
a[j] = b;
w[j] = b - 1 >= j;
if (!found_r) *r = j;
}
int main(void) {
typedef char t_less_than_n[T < N ? 1 : -1];
int a[T + 1];
bool w[T + 1];
for (int j = 0; j < T + 1; j++) {
a[j] = N - (T - j);
w[j] = true;
}
int r = 0;
do {
for (int j = T - 1; j > -1; j--) printf("%x", a[j]);
putchar('\n');
if (false) {
for (int j = T - 1; j > -1; j--) printf("%d", w[j]);
putchar('\n');
}
next(a, w, &r);
} while (a[T] == N);
}
This is programming puzzle. We have two arrays A and B. Both contains 0's and 1's only.
We have to two indices i, j such that
a[i] + a[i+1] + .... a[j] = b[i] + b[i+1] + ... b[j].
Also we have to maximize this difference between i and j. Looking for O(n) solution.
I found O(n^2) solution but not getting O(n).
Best solution is O(n)
First let c[i] = a[i] - b[i], then question become find i, j, which sum(c[i], c[i+1], ..., c[j]) = 0, and max j - i.
Second let d[0] = 0, d[i + 1] = d[i] + c[i], i >= 0, then question become find i, j, which d[j + 1] == d[i], and max j - i.
The value of d is in range [-n, n], so we can use following code to find the answer
answer = 0, answer_i = 0, answer_j = 0
sumHash[2n + 1] set to -1
for (x <- 0 to n) {
if (sumHash[d[x]] == -1) {
sumHash[d[x]] = x
} else {
y = sumHash[d[x]]
// find one answer (y, x), compare to current best
if (x - y > answer) {
answer = x - y
answer_i = y
answer_j = y
}
}
}
Here is an O(n) solution.
I use the fact that sum[i..j] = sum[j] - sum[i - 1].
I keep the leftmost position of each found sum.
int convertToPositiveIndex(int index) {
return index + N;
}
int mostLeft[2 * N + 1];
memset(mostLeft, -1, sizeof(mostLeft));
int bestLen = 0, bestStart = -1, bestEnd = -1;
int sumA = 0, sumB = 0;
for (int i = 0; i < N; i++) {
sumA += A[i];
sumB += B[i];
int diff = sumA - sumB;
int diffIndex = convertToPositiveIndex(diff);
if (mostLeft[diffIndex] != -1) {
//we have found the sequence mostLeft[diffIndex] + 1 ... i
//now just compare it with the best one found so far
int currentLen = i - mostLeft[diffIndex];
if (currentLen > bestLen) {
bestLen = currentLen;
bestStart = mostLeft[diffIndex] + 1;
bestEnd = i;
}
}
if (mostLeft[diffIndex] == -1) {
mostLeft[diffIndex] = i;
}
}
cout << bestStart << " " << bestEnd << " " << bestLen << endl;
P.S. mostLeft array is 2 * N + 1, because of the negatives.
This is a fairly straightforward O(N) solution:
let sa = [s1, s2, s3.. sn] where si = sum(a[0:i]) and similar for sb
then sum(a[i:j]) = sa[j]-sa[i]
and sum(b[i:j]) = sb[j] - sb[i]
Note that because the sums only increase by 1 each time, we know 0 <= sb[N], sa[N] <=N
difference_array = [d1, d2, .. dn] where di = sb[i] - sa[i] <= N
note if di = dj, then sb[i] - sa[i] = sb[j] - sa[j] which means they have the same sum (rearrange to get sum(b[i:j]) and sum(a[i:j]) from above).
Now for each difference we need its max position occurrence and min position occurrence
Now for each difference di, the difference between max - min, is an i-j section of equal sum. Find the maximum max-min value and you're done.
sample code that should work:
a = []
b = []
sa = [0]
sb = [0]
for i in a:
sa.append(sa[-1] + i)
for i in b:
sb.append(sb[-1] + i)
diff = [sai-sbi for sai, sbi in zip(sa, sb)]
min_diff_pos = {}
max_diff_pos = {}
for pos, d in enumerate(diff):
if d in min_diff_pos:
max_diff_pos[d] = pos
else:
min_diff_pos[d] = pos
ans = min(max_diff_pos[d] - min_diff_pos[d] for d in diff)
Basically, my solution goes like this.
Take a variable to take care of the difference since the beginning.
int current = 0;
for index from 0 to length
if a[i] == 0 && b[i] == 1
current--;
else if a[i] == 1 && b[i] == 0
current++;
else
// nothing;
Find the positions where the variable has the same value, which indicates that there are equal 1s and 0s in between.
Pseudo Code:
Here is my primary solution:
int length = min (a.length, b.length);
int start[] = {-1 ... -1}; // from -length to length
start[0] = -1;
int count[] = {0 ... 0}; // from -length to length
int current = 0;
for (int i = 0; i < length; i++) {
if (a[i] == 0 && b[i] == 1)
current--;
else if (a[i] == 1 && b[i] == 0)
current++;
else
; // nothing
if (start[current] == -1) // index can go negative here, take care
start[current] = current;
else
count[current] = i - start[current];
}
return max_in(count[]);