Permalink
Switch branches/tags
Nothing to show
Find file Copy path
Fetching contributors…
Cannot retrieve contributors at this time
342 lines (291 sloc) 9.69 KB
/* This file is part of Polyora, a multi-target tracking library.
Copyright (C) 2010 Julien Pilet
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License along
with this program. If not, see <http://www.gnu.org/licenses/>.
To contact the author of this program, please send an e-mail to:
julien.pilet(at)calodox.org
*/
#include "fvec4.h"
#include <assert.h>
//#define DEBUG_CMPHOMO
#include <iostream>
std::ostream &operator<<(std::ostream &s, const fvec4& a) {
s << "[";
for (int i=0; i<fvec4::size; i++) {
s.width(12);
s.precision(5);
s << a[i];
}
s << "]";
return s;
}
/*! computes the 4 homographies sending [0,0] , [0,1], [1,1] and [1,0]
* to x,y,z and w.
*/
void homography4_from_4pt(const fvec4 x[2], const fvec4 y[2], const fvec4 z[2], const fvec4 w[2], fvec4 cgret[9])
{
fvec4 t1 = x[0];
fvec4 t2 = z[0];
fvec4 t4 = y[1];
fvec4 t5 = t1 * t2 * t4;
fvec4 t6 = w[1];
fvec4 t7 = t1 * t6;
fvec4 t8 = t2 * t7;
fvec4 t9 = z[1];
fvec4 t10 = t1 * t9;
fvec4 t11 = y[0];
fvec4 t14 = x[1];
fvec4 t15 = w[0];
fvec4 t16 = t14 * t15;
fvec4 t18 = t16 * t11;
fvec4 t20 = t15 * t11 * t9;
fvec4 t21 = t15 * t4;
fvec4 t24 = t15 * t9;
fvec4 t25 = t2 * t4;
fvec4 t26 = t6 * t2;
fvec4 t27 = t6 * t11;
fvec4 t28 = t9 * t11;
fvec4 t30 = fvec4(1)/(t21 -t24 - t25 + t26 - t27 + t28);
fvec4 t32 = t1 * t15;
fvec4 t35 = t14 * t11;
fvec4 t41 = t4 * t1;
fvec4 t42 = t6 * t41;
fvec4 t43 = t14 * t2;
fvec4 t46 = t16 * t9;
fvec4 t48 = t14 * t9 * t11;
fvec4 t51 = t4 * t6 * t2;
fvec4 t55 = t6 * t14;
cgret[0] = -(t8 -t5 + t10 * t11 - t11 * t7 - t16 * t2 + t18 - t20 + t21 * t2) * t30;
cgret[1] = (t5 - t8 - t32 * t4 + t32 * t9 + t18 - t2 * t35 + t27 * t2 - t20) * t30;
cgret[2] = t1;
cgret[3] = (-t9 * t7 + t42 + t43 * t4 - t16 * t4 + t46 - t48 + t27 * t9 - t51) * t30;
cgret[4] = (-t42 + t41 * t9 - t55 * t2 + t46 - t48 + t55 * t11 + t51 - t21 * t9) * t30;
cgret[5] = t14;
cgret[6] = (-t10 + t41 + t43 - t35 + t24 - t21 - t26 + t27) * t30;
cgret[7] = (-t7 + t10 + t16 - t43 + t27 - t28 - t21 + t25) * t30;
cgret[8] = fvec4(1);
}
inline void homography_transform(const float a[2], const float H[3][3], float r[2])
{
float z = 1.0f/(H[2][0]*a[0] + H[2][1]*a[1] + H[2][2]);
r[0] = (H[0][0]*a[0] + H[0][1]*a[1] + H[0][2])*z;
r[1] = (H[1][0]*a[0] + H[1][1]*a[1] + H[1][2])*z;
}
inline void homography4_transform(const fvec4 a[2], const fvec4 H[3][3], fvec4 r[2])
{
fvec4 z = fvec4(1)/(H[2][0]*a[0] + H[2][1]*a[1] + H[2][2]);
r[0] = (H[0][0]*a[0] + H[0][1]*a[1] + H[0][2])*z;
r[1] = (H[1][0]*a[0] + H[1][1]*a[1] + H[1][2])*z;
}
#ifdef DEBUG_CMPHOMO
#include <iostream>
static bool eps_cmp2(const fvec4 a[2], const fvec4 b[2])
{
float eps = 1e-1;
fvec4 dx = a[0]-b[0], dy =a[1]-b[1];
fvec4 dx2 = dx*dx;
fvec4 dy2 = dy*dy;
for (int i=0; i<fvec4::size; i++) {
if (finite(dx2[i]) && finite(dy2[i]) &&
dx2[i] > eps || dy2[i]>eps) {
std::cout << "(" << a[0][i] << "," << a[1][i] << ") should be at (" << b[0][i] << "," << b[1][i] << ")\n";
return false;
}
}
return true;
//return (dx*dx <eps && dy*dy<eps);
}
#endif
void homography4_from_4corresp(
const fvec4 a[2], const fvec4 b[2], const fvec4 c[2], const fvec4 d[2],
const fvec4 x[2], const fvec4 y[2], const fvec4 z[2], const fvec4 w[2], fvec4 R[3][3])
{
fvec4 Hr[3][3], Hl[3][3];
homography4_from_4pt(a,b,c,d,&Hr[0][0]);
homography4_from_4pt(x,y,z,w,&Hl[0][0]);
// the following code computes R = Hl * inverse Hr
fvec4 t2 = Hr[1][1]-Hr[2][1]*Hr[1][2];
fvec4 t4 = Hr[0][0]*Hr[1][1];
fvec4 t5 = Hr[0][0]*Hr[1][2];
fvec4 t7 = Hr[1][0]*Hr[0][1];
fvec4 t8 = Hr[0][2]*Hr[1][0];
fvec4 t10 = Hr[0][1]*Hr[2][0];
fvec4 t12 = Hr[0][2]*Hr[2][0];
fvec4 t15 = fvec4(1)/(t4-t5*Hr[2][1]-t7+t8*Hr[2][1]+t10*Hr[1][2]-t12*Hr[1][1]);
fvec4 t18 = -Hr[1][0]+Hr[1][2]*Hr[2][0];
fvec4 t23 = -Hr[1][0]*Hr[2][1]+Hr[1][1]*Hr[2][0];
fvec4 t28 = -Hr[0][1]+Hr[0][2]*Hr[2][1];
fvec4 t31 = Hr[0][0]-t12;
fvec4 t35 = Hr[0][0]*Hr[2][1]-t10;
fvec4 t41 = -Hr[0][1]*Hr[1][2]+Hr[0][2]*Hr[1][1];
fvec4 t44 = t5-t8;
fvec4 t47 = t4-t7;
fvec4 t48 = t2*t15;
fvec4 t49 = t28*t15;
fvec4 t50 = t41*t15;
R[0][0] = Hl[0][0]*t48+Hl[0][1]*(t18*t15)-Hl[0][2]*(t23*t15);
R[0][1] = Hl[0][0]*t49+Hl[0][1]*(t31*t15)-Hl[0][2]*(t35*t15);
R[0][2] = -Hl[0][0]*t50-Hl[0][1]*(t44*t15)+Hl[0][2]*(t47*t15);
R[1][0] = Hl[1][0]*t48+Hl[1][1]*(t18*t15)-Hl[1][2]*(t23*t15);
R[1][1] = Hl[1][0]*t49+Hl[1][1]*(t31*t15)-Hl[1][2]*(t35*t15);
R[1][2] = -Hl[1][0]*t50-Hl[1][1]*(t44*t15)+Hl[1][2]*(t47*t15);
R[2][0] = Hl[2][0]*t48+Hl[2][1]*(t18*t15)-t23*t15;
R[2][1] = Hl[2][0]*t49+Hl[2][1]*(t31*t15)-t35*t15;
R[2][2] = -Hl[2][0]*t50-Hl[2][1]*(t44*t15)+t47*t15;
#ifdef DEBUG_CMPHOMO
// sanity check
fvec4 uv[2];
homography4_transform(a, R, uv);
assert(eps_cmp2(uv,x));
homography4_transform(b, R, uv);
assert(eps_cmp2(uv,y));
homography4_transform(c, R, uv);
assert(eps_cmp2(uv,z));
homography4_transform(d, R, uv);
assert(eps_cmp2(uv,w));
#endif
}
static inline float rand_range(unsigned long n) {
// not smart at all.
int rnd = rand();
float r = (float)floor(double(n)*double(rnd)/(double(RAND_MAX)+1.0));
return r;
}
static inline fvec4 rand_range4(int n) {
return fvec4( rand_range(n), rand_range(n), rand_range(n), rand_range(n) );
}
static inline const float *row(int row, const float *array, int stride)
{
return (const float *)(((char*)array)+row*stride);
}
static inline fvec4 dist2(const fvec4 a[2], const fvec4 b[2]) { fvec4 dx(a[0]-b[0]); fvec4 dy(a[1]-b[1]); return dx*dx + dy*dy; }
static inline float dist2(const float a[2], const float b[2]) { float dx(a[0]-b[0]); float dy(a[1]-b[1]); return dx*dx + dy*dy; }
int ransac_h4(const float *uv1, int stride1, const float *uv2, int stride2, int n,
int maxiter, float dist_threshold, int stop_support,
float result[3][3], char *inliers_mask, float *inliers1, float *inliers2)
{
fvec4 bestH[3][3];
fvec4 best_support(0);
fvec4 threshold(dist_threshold*dist_threshold);
if (n<5) return 0;
for (int iter=0; iter<maxiter; ++iter) {
// draw 4 random correspondences
fvec4 corresp[4];
//std::cout << "Raw random:\n";
for (int i=0;i<4;i++) {
corresp[i] = rand_range4(std::min(6 + iter, n) -i);
//std::cout << corresp[i] << "\n";
for (int j=0; j<i; j++) {
corresp[i] += ((corresp[j] <= corresp[i]) & fvec4(1));
}
// hack to sort the array
if (i==1) {
fvec4 cmin = min(corresp[0],corresp[1]);
fvec4 cmax = max(corresp[0],corresp[1]);
corresp[0] = cmin;
corresp[1] = cmax;
}
if (i==2) {
fvec4 min01 = corresp[0]; // already sorted
fvec4 max01 = corresp[1];
fvec4 min12 = min(corresp[1],corresp[2]);
fvec4 min02 = min(corresp[0],corresp[2]);
corresp[0] = min(min01,corresp[2]);
corresp[1] = max(max(min01,min12), min02);
corresp[2]= max(max01,corresp[2]);
}
}
if (0) {
std::cout << "Iteration: " << iter << ", correspondences after sorting:\n";
for (int i=0;i<4;i++) {
std::cout << corresp[i] << "\n";
}
}
// fetch the 4 corresp
fvec4 pts1[4][2];
fvec4 pts2[4][2];
for (int i=0; i<4; i++) {
for (int j=0; j<fvec4::size; j++) {
int r = (int)floorf(corresp[i][j]);
assert (r>=0);
assert (r<n);
pts1[i][0][j] = row(r, uv1, stride1)[0];
pts1[i][1][j] = row(r, uv1, stride1)[1];
pts2[i][0][j] = row(r, uv2, stride2)[0];
pts2[i][1][j] = row(r, uv2, stride2)[1];
}
}
// compute the homography
fvec4 H[3][3];
homography4_from_4corresp(
pts1[0], pts1[1], pts1[2], pts1[3],
pts2[0], pts2[1], pts2[2], pts2[3],
H);
// evaluate support
fvec4 support(0);
for (int i=0; i<n; i++) {
fvec4 p[2], g[2];
p[0] = fvec4( row(i,uv1,stride1)[0] );
p[1] = fvec4( row(i,uv1,stride1)[1] );
g[0] = fvec4( row(i,uv2,stride2)[0] );
g[1] = fvec4( row(i,uv2,stride2)[1] );
fvec4 t[2];
homography4_transform(p, H, t);
fvec4 d = dist2(t,g);
support += (d < threshold) & fvec4(1 - .1 * (d / threshold));
}
// remember the best solution
fvec4 replace = support > best_support;
fvec4 keep = replace ^ (fvec4(0)<fvec4(1));
best_support = (support & replace) | (best_support & keep);
/*
std::cout << "Support : " << support << " ";
std::cout << "Best Sup: " << best_support << std::endl;
for (int i=0; i<3; i++)
for (int j=0;j<3; j++)
std::cout << bestH[i][j] << " -> " << ((H[i][j] & replace) | (bestH[i][j] & keep))<< std::endl;
*/
for (int i=0; i<3; i++)
for (int j=0;j<3; j++)
bestH[i][j] = (H[i][j] & replace) | (bestH[i][j] & keep);
// early termination ?
if (best_support.horizontal_max() >= stop_support) break;
}
int s = best_support.horizontal_max_index();
for (int i=0; i<3; i++)
for (int j=0;j<3; j++)
result[i][j] = bestH[i][j][s];
int final_support = 0;
float t2 = threshold[0];
// TODO: use SIMD
float *in1 = inliers1;
float *in2 = inliers2;
for (int i=0; i<n; i++) {
float t[2];
homography_transform(row(i,uv1,stride1), result, t);
bool inlier = dist2(t, row(i,uv2,stride2)) < t2;
if (inlier) {
++final_support;
}
if (inliers_mask) inliers_mask[i] = ( inlier ? 0xFF : 0);
if (inlier) {
if (inliers1) {
*in1++ = row(i,uv1,stride1)[0];
*in1++ = row(i,uv1,stride1)[1];
}
if (inliers2) {
*in2++ = row(i,uv2,stride2)[0];
*in2++ = row(i,uv2,stride2)[1];
}
}
}
return final_support;
}