void TwoFrameModel::WriteSparse(FILE *f)
{
// WriteCameraPose(f, m_camera0);
// WriteCameraPose(f, m_camera1);
/* Compute the camera pose of camera1 relative to camera0 */
double pos0[3], pos1[3];
// matrix_transpose_product(3, 3, 3, 1, m_camera0.R, m_camera0.t, pos0);
// matrix_transpose_product(3, 3, 3, 1, m_camera1.R, m_camera1.t, pos1);
memcpy(pos0, m_camera0.t, 3 * sizeof(double));
memcpy(pos1, m_camera1.t, 3 * sizeof(double));
// matrix_scale(3, 1, pos0, -1.0, pos0);
// matrix_scale(3, 1, pos1, -1.0, pos1);
double diff[3];
matrix_diff(3, 1, 3, 1, pos1, pos0, diff);
double R1[9], tr[3];
matrix_transpose_product2(3, 3, 3, 3, m_camera0.R, m_camera1.R, R1);
// matrix_transpose_product(3, 3, 3, 3, m_camera1.R, m_camera0.R, R1);
matrix_product(3, 3, 3, 1, m_camera0.R, diff, tr);
double norm = matrix_norm(3, 1, tr);
matrix_scale(3, 1, tr, 1.0 / norm, tr);
double viewdir[3] = { -R1[2], -R1[5], -R1[8] };
double twist_angle = GetTwist(R1);
/* Compute the distance to the scene */
double z_avg = 0.0;
for (int p = 0; p < m_num_points; p++) {
v3_t &pt = m_points[p];
double diff1[3], diff2[3];
matrix_diff(3, 1, 3, 1, pt.p, pos0, diff1);
matrix_diff(3, 1, 3, 1, pt.p, pos1, diff2);
double dist1 = matrix_norm(3, 1, diff1);
double dist2 = matrix_norm(3, 1, diff2);
z_avg += 0.5 * (dist1 + dist2) / norm;
}
z_avg /= m_num_points;
WriteVector(f, 9, R1);
/* Write the viewing direction */
// WriteVector(f, 3, viewdir);
/* Write the twist angle */
// fprintf(f, "%0.8f\n", twist_angle);
/* Write the translation */
WriteVector(f, 3, tr);
fprintf(f, "%0.6f\n", z_avg);
}
/* Returns true (and stores the endpoints in t and u) if the
* epipolar swath e1, e2 intersects this segment */
bool LineSegment2D::IntersectsEpipolarSwath(double e1[3], double e2[3],
double &t, double &u)
{
/* Find the line between the two endpoints */
double p[3] = { m_p1[0], m_p1[1], 1.0 };
double q[3] = { m_p2[0], m_p2[1], 1.0 };
double l[3];
matrix_cross(p, q, l);
/* Intersect the line with the two epipolar lines */
double i1[3], i2[3];
matrix_cross(l, e1, i1);
matrix_cross(l, e2, i2);
double inv_i12 = 1.0 / i1[2];
i1[0] *= inv_i12;
i1[1] *= inv_i12;
double inv_i22 = 1.0 / i2[2];
i2[0] *= inv_i22;
i2[1] *= inv_i22;
double qp[3], i1p[3], i2p[3];
matrix_diff(2, 1, 2, 1, q, p, qp);
matrix_diff(2, 1, 2, 1, i1, p, i1p);
matrix_diff(2, 1, 2, 1, i2, p, i2p);
double dot1, dot2;
matrix_product(1, 2, 2, 1, i1p, qp, &dot1);
matrix_product(1, 2, 2, 1, i2p, qp, &dot2);
int sign1 = SGN(dot1);
int sign2 = SGN(dot2);
double inv_norm = 1.0 / matrix_norm(2, 1, qp);
double mag1 = matrix_norm(2, 1, i1p) * inv_norm; // matrix_norm(2, 1, qp);
double mag2 = matrix_norm(2, 1, i2p) * inv_norm; // matrix_norm(2, 1, qp);
t = sign1 * mag1;
u = sign2 * mag2;
/* Intersection check */
if ((t < 0.0 && u < 0.0) || (t > 1.0 && u > 1.0))
return false;
else
return true;
}
void PlaneData::Transform(const double *M)
{
double p[4] = { m_normal[0], m_normal[1], m_normal[2], m_dist };
double Minv[16];
matrix_invert(4, (double *)M, Minv);
double pNew[4];
matrix_transpose_product(4, 4, 4, 1, Minv, p, pNew);
double len = matrix_norm(3, 1, pNew);
m_normal[0] = pNew[0] / len;
m_normal[1] = pNew[1] / len;
m_normal[2] = pNew[2] / len;
m_dist = pNew[3] / len;
#if 0
double origin[4] = { m_origin[0], m_origin[1], m_origin[2], 1.0 };
double Morigin[4];
matrix_product(4, 4, 4, 1, (double *) M, origin, Morigin);
double dot0, dot1;
matrix_product(1, 4, 4, 1, p, origin, &dot0);
matrix_product(1, 4, 4, 1, pNew, Morigin, &dot1);
printf("dot0 = %0.3f\n", dot0);
printf("dot1 = %0.3f\n", dot1);
#endif
}
double TwoFrameModel::AverageDistanceToPoints() const {
double dist_sum = 0.0;
const double *pos1 = m_camera0.t;
const double *pos2 = m_camera1.t;
for (int i = 0; i < m_num_points; i++) {
double diff1[3], diff2[3];
matrix_diff(3, 1, 3, 1, m_points[i].p, (double *) pos1, diff1);
matrix_diff(3, 1, 3, 1, m_points[i].p, (double *) pos2, diff2);
double norm1 = matrix_norm(3, 1, diff1);
double norm2 = matrix_norm(3, 1, diff2);
dist_sum += (norm1 + norm2);
}
return dist_sum / (2 * m_num_points);
}
/* Return the homogeneous form of the line */
void LineSegment2D::Homogeneous(double *l)
{
double p1[3] = { m_p1[0], m_p1[1], 1.0 };
double p2[3] = { m_p2[0], m_p2[1], 1.0 };
matrix_cross(p1, p2, l);
double norm = matrix_norm(3, 1, l);
matrix_scale(3, 1, l, 1.0 / norm, l);
}
int values(double *A, int n, double *Q1, double *Q2, double *v, double eps){
double norm = matrix_norm(n,A);
double qe = fabs(eps*norm);
// printf("need %e\n", qe);
if (n > 2){
TDiag(A,n);
}
double* A1;
double sk;
double t = fabs(A[(n-1)*n + (n-2)]);
int r = n;
int c = 0;
while (r > 2)
{
sk = A[(r-1)*n + (r-1)];
for (int i = 0; i < r; i++){
A[i*n + i] -= sk;
}
A1 = new double [r*r];
for (int i = 0; i < r; i++){
for (int j = 0; j < r; j++){
A1[i*r+j] = A[i*n + j];
}
}
if (QR(A1,r,Q1,Q2)==-1){
delete [] A1;
return -1;
}
for (int i = 0; i < r; i++){
for (int j = 0; j < r; j++){
A[i*n+j] = A1[i*r+j];
}
}
delete [] A1;
for (int i = 0; i < r; i++){
A[i*n + i] += sk;
}
if ((fabs(A[(r-1)*n + (r-2)])-t)<EPS){
c++;
}
t = fabs(A[(r-1)*n + (r-2)]);
if (t < qe){
v[r-1] = A[(r-1)*n + (r-1)];
r--;
}
if (c > 10){
return 3;
}
}
double D = (A[0]+A[n+1])*(A[0]+A[n+1]) - 4.*(A[0]*A[n+1] - A[1]*A[n]);
v[1] = (A[0]+A[n+1] - sqrt(D))/2.;
v[0] = (A[0]+A[n+1] + sqrt(D))/2.;
return 1;
}
void infomax(gsl_matrix *x_white, gsl_matrix *weights, gsl_matrix *S, int verbose){
/*Computes ICA infomax in whitened data
Decomposes x_white as x_white=AS
*Input
x_white: whitened data (Use PCAwhiten)
*Output
A : mixing matrix
S : source matrix
*/
// int verbose = 1; //true
size_t NCOMP = x_white->size1;
size_t NVOX = x_white->size2;
//getting permutation vector
const gsl_rng_type * T;
gsl_rng * r;
gsl_permutation * p = gsl_permutation_alloc (NVOX);
// gsl_rng_env_setup();
T = gsl_rng_default;
r = gsl_rng_alloc (T);
gsl_permutation_init (p);
gsl_matrix *old_weights = gsl_matrix_alloc(NCOMP,NCOMP);
gsl_matrix *bias = gsl_matrix_calloc(NCOMP, 1);
gsl_matrix *d_weights = gsl_matrix_calloc(NCOMP,NCOMP);
gsl_matrix *temp_change = gsl_matrix_alloc(NCOMP,NCOMP);
gsl_matrix *old_d_weights = gsl_matrix_calloc(NCOMP,NCOMP);
gsl_matrix *shuffled_x_white = gsl_matrix_calloc(NCOMP,x_white->size2);
gsl_matrix_memcpy(shuffled_x_white, x_white);
gsl_matrix_set_identity(weights);
gsl_matrix_set_identity(old_weights);
double lrate = 0.005/log((double)NCOMP);
double change=1;
double angle_delta =0;
size_t step = 1;
int error = 0;
while( (step < MAX_STEP) && (change > W_STOP)){
error = w_update(weights, x_white, bias,
shuffled_x_white, p, r, lrate);
if (error==1 || error==2){
// It blowed up! RESTART!
step = 1;
// change = 1;
error = 0;
lrate *= ANNEAL;
gsl_matrix_set_identity(weights);
gsl_matrix_set_identity(old_weights);
gsl_matrix_set_zero(d_weights);
gsl_matrix_set_zero(old_d_weights);
gsl_matrix_set_zero(bias);
if (lrate > MIN_LRATE){
printf("\nLowering learning rate to %g and starting again.\n",lrate);
}
else{
printf("\nMatrix may not be invertible");
}
}
else if (error==0){
gsl_matrix_memcpy(d_weights, weights);
gsl_matrix_sub(d_weights, old_weights);
change = matrix_norm(d_weights);
if (step > 2){
// Compute angle delta
gsl_matrix_memcpy(temp_change, d_weights);
gsl_matrix_mul_elements(temp_change, old_d_weights);
angle_delta = acos(matrix_sum(temp_change) / sqrt(matrix_norm(d_weights)*(matrix_norm(old_d_weights))));
angle_delta *= (180.0 / M_PI);
}
gsl_matrix_memcpy(old_weights, weights);
if (angle_delta > 60){
lrate *= ANNEAL;
gsl_matrix_memcpy(old_d_weights, d_weights);
} else if (step==1) {
gsl_matrix_memcpy(old_d_weights, d_weights);
}
if ((verbose && (step % 10)== 0) || change < W_STOP){
printf("\nStep %zu: Lrate %.1e, Wchange %.1e, Angle %.2f",
step, lrate, change, angle_delta);
}
step ++;
}
}
matrix_mmul(weights, x_white, S);
gsl_matrix_free(old_d_weights);
gsl_matrix_free(old_weights);
gsl_matrix_free(bias);
gsl_matrix_free(d_weights);
gsl_matrix_free(shuffled_x_white);
gsl_rng_free (r);
gsl_permutation_free (p);
}
void LineSegment3D::Render(const CameraInfo &camera, double max_width,
int stroke_texture, ParameterBound stroke_bounds)
{
#ifndef __BUNDLER__
#ifndef __DEMO__
#if 1
/* Project the line into the camera */
double p1[4] = { m_p1[0], m_p1[1], m_p1[2], 1.0 };
double p2[4] = { m_p2[0], m_p2[1], m_p2[2], 1.0 };
double proj1[3], proj2[3];
matrix_product(3, 4, 4, 1, (double *) camera.m_Pmatrix, p1, proj1);
matrix_product(3, 4, 4, 1, (double *) camera.m_Pmatrix, p2, proj2);
if (proj1[2] >= 0.0 || proj2[2] >= 0.0)
return;
double width = CLAMP(5.0 / (-proj1[2] - proj2[2]), 0.5, max_width);
proj1[0] /= -proj1[2];
proj1[1] /= -proj1[2];
proj2[0] /= -proj2[2];
proj2[1] /= -proj2[2];
#endif
#if 0
bool in_front1 = camera.Project(m_p1, proj1);
bool in_front2 = camera.Project(m_p2, proj2);
if (!in_front1 || !in_front2)
return;
#endif
/* Draw a line segment whose width is proportional to the distance
* from the camera */
#if 0
double pos[3];
camera.GetPosition(pos);
double disp[3];
matrix_diff(3, 1, 3, 1, pos, m_p1, disp);
double dist = matrix_norm(3, 1, disp);
#endif
if (stroke_texture != -1) {
glEnable(GL_TEXTURE_2D);
glBindTexture(GL_TEXTURE_2D, stroke_texture);
}
#if 0
#define LINE_WIDTH_SIGMA 1.0
double width =
max_width * exp(-dist * dist / (LINE_WIDTH_SIGMA * LINE_WIDTH_SIGMA));
#endif
/* Create a stroke */
Stroke s;
s.radius() = 0.5 * width;
s.cap() = false;
s.depth() = 1.0;
if (stroke_texture == -1) {
s.useTexture() = false;
s.color() = makeColor(0, 0, 0, 0.9f);
} else {
GLubyte b = 0x0;
s.useTexture() = true;
s.color() = makeColor(b, b, b, 0.9f);
}
s.addControlPoint(proj1[0], proj1[1]);
s.addControlPoint(proj2[0], proj2[1]);
s.render();
if (stroke_texture != -1) {
glDisable(GL_TEXTURE_2D);
}
#endif /* __DEMO__ */
#endif /* __BUNDLER__ */
}
/* Align two sets of points with a 3D similarity transform */
int align_horn_3D_ransac(int n, v3_t *r_pts, v3_t *l_pts,
int num_ransac_rounds, double ransac_thresh,
double *Tret)
{
int round;
#define MIN_SUPPORT 3
v3_t *l_inliers, *r_inliers;
double *Vp, *TVp;
int num_inliers, max_inliers = 0;
double Tbest[16], TbestT[16];
int i;
double *ptr;
double ransac_threshsq = ransac_thresh * ransac_thresh;
if (n < MIN_SUPPORT) {
printf("[align_horn_3D_ransac] Error: need at least %d points!\n",
MIN_SUPPORT);
return 0;
}
l_inliers = (v3_t *) malloc(sizeof(v3_t) * n);
r_inliers = (v3_t *) malloc(sizeof(v3_t) * n);
Vp = (double *) malloc(sizeof(double) * 4 * n);
TVp = (double *) malloc(sizeof(double) * 4 * n);
for (i = 0; i < n; i++) {
memcpy(Vp + 4 * i, l_pts[i].p, 3 * sizeof(double));
Vp[4 * i + 3] = 1.0;
}
for (round = 0; round < num_ransac_rounds; round++) {
int support[MIN_SUPPORT];
int i, j;
v3_t r_pts_small[MIN_SUPPORT], l_pts_small[MIN_SUPPORT];
double Tout[16], ToutT[16];
int nan = 0;
for (i = 0; i < MIN_SUPPORT; i++) {
/* Select an index from 0 to n-1 */
int idx, reselect;
do {
reselect = 0;
idx = rand() % n;
for (j = 0; j < i; j++) {
if (support[j] == idx) {
reselect = 1;
break;
}
}
} while (reselect);
support[i] = idx;
r_pts_small[i] = r_pts[idx];
l_pts_small[i] = l_pts[idx];
}
align_horn_3D_2(MIN_SUPPORT, r_pts_small, l_pts_small, 1, Tout);
#if 1
for (i = 0; i < 16; i++) {
if (isnan(Tout[i]) || Tout[i] != Tout[i]) {
nan = 1;
break;
}
}
if (nan == 1)
continue;
#endif
/* Count inliers */
num_inliers = 0;
#if 0
for (i = 0; i < n; i++) {
double Tp[4];
double diff[3];
double dist;
double p[4] = { l_pts[i].p[0], l_pts[i].p[1], l_pts[i].p[2], 1.0 };
matrix_product(4, 4, 4, 1, Tout, p, Tp);
matrix_diff(3, 1, 3, 1, Tp, r_pts[i].p, diff);
dist = matrix_norm(3, 1, diff);
if (dist < ransac_thresh) {
num_inliers++;
}
}
#else
matrix_transpose(4, 4, Tout, ToutT);
matrix_product_old(n, 4, 4, 4, Vp, ToutT, TVp);
ptr = TVp;
for (i = 0; i < n; i++) {
// double diff[3], dist;
double dx, dy, dz, dist;
// matrix_diff(3, 1, 3, 1, TVp + 4 * i, r_pts[i].p, diff);
dx = ptr[0] - r_pts[i].p[0];
dy = ptr[1] - r_pts[i].p[1];
dz = ptr[2] - r_pts[i].p[2];
dist = dx * dx + dy * dy + dz * dz; // matrix_normsq(3, 1, diff);
if (dist < ransac_threshsq)
num_inliers++;
ptr += 4;
}
#endif
if (num_inliers > max_inliers) {
max_inliers = num_inliers;
memcpy(Tbest, Tout, sizeof(double) * 16);
}
}
/* Reestimate using all inliers */
#if 0
matrix_transpose(4, 4, Tbest, TbestT);
matrix_product(n, 4, 4, 4, Vp, TbestT, TVp);
num_inliers = 0;
for (i = 0; i < n; i++) {
// double Tp[4];
double diff[3];
double dist;
matrix_diff(3, 1, 3, 1, TVp + 4 * i, r_pts[i].p, diff);
// double p[4] = { l_pts[i].p[0], l_pts[i].p[1], l_pts[i].p[2], 1.0 };
// matrix_product(4, 4, 4, 1, Tbest, p, Tp);
// matrix_diff(3, 1, 3, 1, Tp, r_pts[i].p, diff);
dist = matrix_normsq(3, 1, diff);
if (dist < ransac_threshsq) {
r_inliers[num_inliers] = r_pts[i];
l_inliers[num_inliers] = l_pts[i];
num_inliers++;
}
}
align_horn_3D_2(num_inliers, r_inliers, l_inliers, 1, Tret);
// memcpy(Tret, Tbest, 16 * sizeof(double));
if (isnan(Tret[0]) || Tret[0] != Tret[0]) {
printf("[align_horn_3D_ransac] nan at end [num_inliers: %d], "
"restoring old matrix\n", num_inliers);
memcpy(Tret, Tbest, sizeof(double) * 16);
}
#else
memcpy(Tret, Tbest, sizeof(double) * 16);
#endif
free(r_inliers);
free(l_inliers);
free(Vp);
free(TVp);
return max_inliers;
#undef MIN_SUPPORT
}
/* Align two sets of points with a 2D similarity transform */
int align_horn_ransac(int n, v3_t *r_pts, v3_t *l_pts,
int num_ransac_rounds, double ransac_thresh,
double *Tret)
{
int round;
#define MIN_SUPPORT 3
v3_t *l_inliers, *r_inliers;
int num_inliers, max_inliers = 0;
double Tbest[9], R[9], T[9], scale;
int i;
if (n < 3) {
printf("[align_horn_ransac] Error: need at least 3 points!\n");
return 0;
}
l_inliers = (v3_t *) malloc(sizeof(v3_t) * n);
r_inliers = (v3_t *) malloc(sizeof(v3_t) * n);
for (round = 0; round < num_ransac_rounds; round++) {
int support[MIN_SUPPORT];
int i, j;
v3_t r_pts_small[MIN_SUPPORT], l_pts_small[MIN_SUPPORT];
double Rtmp[9], Ttmp[9], Tout[9], scale_tmp;
for (i = 0; i < MIN_SUPPORT; i++) {
/* Select an index from 0 to n-1 */
int idx, reselect;
do {
reselect = 0;
idx = rand() % n;
for (j = 0; j < i; j++) {
if (support[j] == idx) {
reselect = 1;
break;
}
}
} while (reselect);
support[i] = idx;
r_pts_small[i] = r_pts[idx];
l_pts_small[i] = l_pts[idx];
}
align_horn(MIN_SUPPORT, r_pts_small, l_pts_small, Rtmp, Ttmp,
Tout, &scale_tmp, NULL);
/* Count inliers */
num_inliers = 0;
for (i = 0; i < n; i++) {
double Tp[3];
double diff[3];
double dist;
matrix_product(3, 3, 3, 1, Tout, l_pts[i].p, Tp);
matrix_diff(3, 1, 3, 1, Tp, r_pts[i].p, diff);
dist = matrix_norm(3, 1, diff);
if (dist < ransac_thresh) {
num_inliers++;
}
if (num_inliers > max_inliers) {
max_inliers = num_inliers;
memcpy(Tbest, Tout, sizeof(double) * 9);
// printf(" inliers_new: %d\n", num_inliers);
}
}
}
#if 0
/* Reestimate using all inliers */
num_inliers = 0;
for (i = 0; i < n; i++) {
double Tp[3];
double diff[3];
double dist;
matrix_product(3, 3, 3, 1, Tbest, l_pts[i].p, Tp);
matrix_diff(3, 1, 3, 1, Tp, r_pts[i].p, diff);
dist = matrix_norm(3, 1, diff);
if (dist < ransac_thresh) {
r_inliers[num_inliers] = r_pts[i];
l_inliers[num_inliers] = l_pts[i];
num_inliers++;
}
}
// printf(" inliers: %d\n", num_inliers);
align_horn(num_inliers, r_inliers, l_inliers, R, T, Tret, &scale, NULL);
#else
memcpy(Tret, Tbest, 9 * sizeof(double));
#endif
free(r_inliers);
free(l_inliers);
return num_inliers;
#undef MIN_SUPPORT
}
/* Align two sets of points with a 2D similarity transform */
int align_2D_ransac(int n, v3_t *r_pts, v3_t *l_pts,
int num_ransac_rounds, double ransac_thresh,
double *Tret)
{
int round;
#define MIN_SUPPORT 2
v3_t *l_inliers, *r_inliers;
int num_inliers, max_inliers = 0;
double Tbest[9], R[9], T[9], scale;
int i;
if (n < 3) {
printf("[align_2D_ransac] Error: need at least 3 points!\n");
return 0;
}
l_inliers = (v3_t *) malloc(sizeof(v3_t) * n);
r_inliers = (v3_t *) malloc(sizeof(v3_t) * n);
for (round = 0; round < num_ransac_rounds; round++) {
int support[MIN_SUPPORT];
int i, j;
v3_t r_mean, l_mean, r0, l0;
v3_t r_pts_small[MIN_SUPPORT], l_pts_small[MIN_SUPPORT];
double Rtmp[9], T1tmp[9], T2tmp[9], tmp[9], Tout[9];
double a, b;
for (i = 0; i < MIN_SUPPORT; i++) {
/* Select an index from 0 to n-1 */
int idx, reselect;
do {
reselect = 0;
idx = rand() % n;
for (j = 0; j < i; j++) {
if (support[j] == idx) {
reselect = 1;
break;
}
}
} while (reselect);
support[i] = idx;
r_pts_small[i] = r_pts[idx];
l_pts_small[i] = l_pts[idx];
}
r_mean = v3_scale(0.5, v3_add(r_pts_small[0], r_pts_small[1]));
l_mean = v3_scale(0.5, v3_add(l_pts_small[0], l_pts_small[1]));
r0 = v3_sub(r_pts_small[0], r_mean);
// v3_t r1 = v3_sub(r_pts_small[1], mean);
l0 = v3_sub(l_pts_small[0], l_mean);
// v3_t l1 = v3_sub(l_pts_small[1], mean);
a = (Vy(r0) + Vx(r0) * Vx(l0) / Vy(l0)) /
(Vy(l0) + Vx(l0) * Vx(l0) / Vy(l0));
b = (Vx(r0) - a * Vx(l0)) / Vy(l0);
Rtmp[0] = a; Rtmp[1] = b; Rtmp[2] = 0.0;
Rtmp[3] = -b; Rtmp[4] = a; Rtmp[5] = 0.0;
Rtmp[6] = 0; Rtmp[7] = 0; Rtmp[8] = 1.0;
matrix_ident(3, T1tmp);
T1tmp[2] = -Vx(l_mean);
T1tmp[5] = -Vy(l_mean);
matrix_ident(3, T2tmp);
T2tmp[2] = Vx(r_mean);
T2tmp[5] = Vy(r_mean);
matrix_product(3, 3, 3, 3, Rtmp, T1tmp, tmp);
matrix_product(3, 3, 3, 3, T2tmp, tmp, Tout);
/* Count inliers */
num_inliers = 0;
for (i = 0; i < n; i++) {
double Tp[3];
double diff[3];
double dist;
matrix_product(3, 3, 3, 1, Tout, l_pts[i].p, Tp);
matrix_diff(3, 1, 3, 1, Tp, r_pts[i].p, diff);
dist = matrix_norm(3, 1, diff);
if (dist < ransac_thresh) {
num_inliers++;
}
if (num_inliers > max_inliers) {
max_inliers = num_inliers;
memcpy(Tbest, Tout, sizeof(double) * 9);
// printf(" inliers_new: %d\n", num_inliers);
}
}
}
#if 0
/* Reestimate using all inliers */
num_inliers = 0;
for (i = 0; i < n; i++) {
double Tp[3];
double diff[3];
double dist;
matrix_product(3, 3, 3, 1, Tbest, l_pts[i].p, Tp);
matrix_diff(3, 1, 3, 1, Tp, r_pts[i].p, diff);
dist = matrix_norm(3, 1, diff);
if (dist < ransac_thresh) {
r_inliers[num_inliers] = r_pts[i];
l_inliers[num_inliers] = l_pts[i];
num_inliers++;
}
}
// align_horn(num_inliers, r_inliers, l_inliers, R, T, Tret, &scale, NULL);
align_2D(num_inliers, r_inliers, l_inliers, R, T, Tret, &scale, NULL);
#else
memcpy(Tret, Tbest, 9 * sizeof(double));
#endif
free(r_inliers);
free(l_inliers);
return num_inliers;
#undef MIN_SUPPORT
}
/* Align two sets of points with a 3D rotation */
double align_3D_rotation(int n, v3_t *r_pts, v3_t *l_pts, double *R)
{
double A[9];
double U[9], S[3], V[9], VT[9], RT[9];
int i;
double error;
#if 0
if (n > 3) {
printf("A:\n");
for (i = 0; i < n; i++) {
printf("%0.6f %0.6f %0.6f\n",
Vx(r_pts[i]), Vy(r_pts[i]), Vz(r_pts[i]));
}
printf("B:\n");
for (i = 0; i < n; i++) {
printf("%0.6f %0.6f %0.6f\n",
Vx(l_pts[i]), Vy(l_pts[i]), Vz(l_pts[i]));
}
}
#endif
for (i = 0; i < 9; i++)
A[i] = 0.0;
for (i = 0; i < n; i++) {
double *a = l_pts[i].p, *b = r_pts[i].p;
// matrix_product(3, 1, 1, 3, l_pts[i].p, r_pts[i].p, tensor);
A[0] += a[0] * b[0];
A[1] += a[0] * b[1];
A[2] += a[0] * b[2];
A[3] += a[1] * b[0];
A[4] += a[1] * b[1];
A[5] += a[1] * b[2];
A[6] += a[2] * b[0];
A[7] += a[2] * b[1];
A[8] += a[2] * b[2];
}
// dgesvd_driver(3, 3, A, U, S, VT);
// printf("svd:\n");
// matrix_print(3, 3, A);
svd(3, 3, 1, 1, 1.0e-12, 1.0e-12, A, S, U, V, VT);
// printf("U:\n");
// matrix_print(3, 3, U);
// printf("VT:\n");
// matrix_print(3, 3, VT);
// printf("S:\n");
// matrix_print(3, 3, S);
matrix_product33(U, VT, RT);
matrix_transpose(3, 3, RT, R);
// printf("R:\n");
// matrix_print(3, 3, R);
if (matrix_determinant3(R) < 0.0) {
/* We're dealing with a reflection */
double tmp[9];
double reflectZ[9] = { 1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, -1.0 };
matrix_product33(U, reflectZ, tmp);
matrix_product33(tmp, VT, RT);
matrix_transpose(3, 3, RT, R);
}
/* Compute error */
error = 0.0;
for (i = 0; i < n; i++) {
double rot[3];
double diff[3];
double dist;
matrix_product331(R, l_pts[i].p, rot);
matrix_diff(3, 1, 3, 1, rot, r_pts[i].p, diff);
dist = matrix_norm(3, 1, diff);
// printf("d[%d] = %0.6f\n", i, dist);
error += dist;
}
return error / n;
}
/* Align two sets of points with a 3D rotation */
int align_3D_rotation_ransac(int n, v3_t *r_pts, v3_t *l_pts,
int num_ransac_rounds, double ransac_thresh,
double *R)
{
int round;
double error = 0.0;
#define MIN_SUPPORT 3
// const int min_support = 3;
v3_t *l_inliers, *r_inliers;
int num_inliers, max_inliers = 0;
double Rbest[9];
int i;
if (n < 3) {
printf("[align_3D_rotation_ransac] Error: need at least 3 points!\n");
return 0;
}
l_inliers = (v3_t *) malloc(sizeof(v3_t) * n);
r_inliers = (v3_t *) malloc(sizeof(v3_t) * n);
for (round = 0; round < num_ransac_rounds; round++) {
int support[MIN_SUPPORT];
int i, j;
v3_t r_pts_small[MIN_SUPPORT], l_pts_small[MIN_SUPPORT];
double Rtmp[9];
for (i = 0; i < MIN_SUPPORT; i++) {
/* Select an index from 0 to n-1 */
int idx, reselect;
do {
reselect = 0;
idx = rand() % n;
for (j = 0; j < i; j++) {
if (support[j] == idx) {
reselect = 1;
break;
}
}
} while (reselect);
support[i] = idx;
r_pts_small[i] = r_pts[idx];
l_pts_small[i] = l_pts[idx];
}
align_3D_rotation(MIN_SUPPORT, r_pts_small, l_pts_small, Rtmp);
/* Count inliers */
num_inliers = 0;
for (i = 0; i < n; i++) {
double rot[3];
double diff[3];
double dist;
matrix_product(3, 3, 3, 1, Rtmp, l_pts[i].p, rot);
matrix_diff(3, 1, 3, 1, rot, r_pts[i].p, diff);
dist = matrix_norm(3, 1, diff);
if (dist < ransac_thresh) {
num_inliers++;
}
if (num_inliers > max_inliers) {
max_inliers = num_inliers;
memcpy(Rbest, Rtmp, sizeof(double) * 9);
}
}
}
#if 0
/* Reestimate using all inliers */
num_inliers = 0;
for (i = 0; i < n; i++) {
double rot[3];
double diff[3];
double dist;
matrix_product(3, 3, 3, 1, Rbest, l_pts[i].p, rot);
matrix_diff(3, 1, 3, 1, rot, r_pts[i].p, diff);
dist = matrix_norm(3, 1, diff);
if (dist < ransac_thresh) {
r_inliers[num_inliers] = r_pts[i];
l_inliers[num_inliers] = l_pts[i];
num_inliers++;
}
}
error = align_3D_rotation(num_inliers, r_inliers, l_inliers, R);
printf("[align_3D_rotation] Error: %0.3f\n", error);
free(r_inliers);
free(l_inliers);
return num_inliers;
#else
memcpy(R, Rbest, 9 * sizeof(double));
free(r_inliers);
free(l_inliers);
return max_inliers;
#endif
#undef MIN_SUPPORT
}
