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
}
/* Return the product x**T A x */
double matrix_double_product(int n, double *A, double *x) {
double *tmp = malloc(sizeof(double) * n);
double result;
matrix_product(n, n, n, 1, A, x, tmp);
matrix_product(1, n, n, 1, x, tmp, &result);
free(tmp);
return result;
}
void TwoFrameModel::
ComputeTransformedCovariance(bool flip, double *S, double *C) const
{
double tmp[9];
if (flip) {
matrix_product(3, 3, 3, 3, S, m_C1, tmp);
matrix_transpose_product2(3, 3, 3, 3, tmp, S, C);
} else {
matrix_product(3, 3, 3, 3, S, m_C0, tmp);
matrix_transpose_product2(3, 3, 3, 3, tmp, S, C);
}
}
/* 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;
}
int main(int argc, char** argv)
{
printf("--------verify--------\n");
verify_matrix();
printf("----------------------\n");
// autorelease関数をbeginとendにはさまないで使うのはNG
// 全てのallocされた関数は
// (1) free
// (2) release
// (3) begin-endの中でautorelease
// のいずれかを行う必要がある。
// 擬似逆行列演算がこれくらい簡単に記述できる。
/* 3*2行列aを確保、成分ごとの代入 */
matrix_t* a = matrix_alloc(3, 2);
ELEMENT(a, 0, 0) = 1; ELEMENT(a, 0, 1) = 4;
ELEMENT(a, 1, 0) = 2; ELEMENT(a, 1, 1) = 5;
ELEMENT(a, 2, 0) = 3; ELEMENT(a, 2, 1) = 6;
/* 擬似逆行列の演算 */
// auto releaseモードに入る
matrix_begin();
// 擬似逆行列を求める。一行だけ！
matrix_t* inva = matrix_product(matrix_inverse(matrix_product(matrix_transpose(a), a)), matrix_transpose(a));
// 擬似逆行列の表示 (invaはmatrix_endで解放されるので、begin-end内で。)
matrix_print(inva);
// release poolを開放する
matrix_end();
/* ちなみに擬似逆行列を求める関数は内部に組んだので、それを使うこともできる。 */
// auto releaseモードに入る
matrix_begin();
// 擬似逆行列を求める関数を呼ぶ。
matrix_t* inva_simple = matrix_pseudo_inverse(a);
// 擬似逆行列の表示 (invaはmatrix_endで解放されるので、begin-end内で。)
matrix_print(inva_simple);
// release poolを開放する
matrix_end();
// これはautorelease対象でないので、しっかり自分でfree。
// リテインカウントを実装してあるので、理解できればreleaseの方が高性能。
//matrix_free(a);
matrix_release(a);
return 0;
}
void DistortPoint(const double x, const double y,
const fisheye_params_t &p, double *R,
double &x_out, double &y_out)
{
double xn = x; // - m_fCx;
double yn = y; // - m_fCy;
double ray[3] = { xn, yn, p.fFocal }, ray_rot[3];
matrix_product(3, 3, 3, 1, R, ray, ray_rot);
if (ray_rot[2] <= 0.0) {
xn = -DBL_MAX;
yn = -DBL_MAX;
return;
} else {
xn = ray_rot[0] * p.fFocal / ray_rot[2];
yn = ray_rot[1] * p.fFocal / ray_rot[2];
}
double r = sqrt(xn * xn + yn * yn);
double angle = RAD2DEG(atan(r / p.fFocal));
double rnew = p.fRad * angle / (0.5 * p.fAngle);
x_out = xn * (rnew / r) + p.fCx;
y_out = yn * (rnew / r) + p.fCy;
}
double PlaneData::ProjectV(double *p, double *p_proj)
{
double dot;
matrix_product(1, 3, 3, 1, p, m_vaxis, &dot);
matrix_scale(3, 1, m_vaxis, dot, p_proj);
return dot;
}
void EKF::_predictState(double *state, double *state_new) const
{
// copy old to new
matrix_scale(1, 3 * _num_feats_init_structure + _num_motion_states, state, 1.0, state_new);
double LinVel[3];
double AngVel[3];
LinVel[0] = state[3 * _num_feats_init_structure + 0];
LinVel[1] = state[3 * _num_feats_init_structure + 1];
LinVel[2] = state[3 * _num_feats_init_structure + 2];
AngVel[0] = state[3 * _num_feats_init_structure + 3];
AngVel[1] = state[3 * _num_feats_init_structure + 4];
AngVel[2] = state[3 * _num_feats_init_structure + 5];
double curr[3], new_pos[3];
double R[3 * 3];
matrix_scale(1, 3, LinVel, _dt, LinVel);
matrix_scale(1, 3, AngVel, _dt, AngVel);
RODR(AngVel, R);
for (int i = 0; i < _num_feats_init_structure; i++)
{
curr[0] = state[3 * i + 0];
curr[1] = state[3 * i + 1];
curr[2] = state[3 * i + 2];
matrix_product(3, 3, 3, 1, R, curr, new_pos);
matrix_sum(3, 1, 3, 1, new_pos, LinVel, new_pos);
//new_pos = LinVel*dt + ublas::prod(RODR(AngVel*dt),curr);
state_new[3 * i + 0] = new_pos[0];
state_new[3 * i + 1] = new_pos[1];
state_new[3 * i + 2] = new_pos[2];
}
}
/* Estimate an E-matrix from a given set of point matches */
std::vector<int> EstimateEMatrix(const std::vector<Keypoint> &k1,
const std::vector<Keypoint> &k2,
std::vector<KeypointMatch> matches,
int num_trials, double threshold,
double f1, double f2,
double *E, double *F)
{
int num_keys1 = k1.size();
int num_keys2 = k2.size();
std::vector<Keypoint> k1_norm, k2_norm;
k1_norm.resize(num_keys1);
k2_norm.resize(num_keys2);
for (int i = 0; i < num_keys1; i++) {
Keypoint k;
k.m_x = k1[i].m_x / f1;
k.m_y = k1[i].m_y / f1;
k1_norm[i] = k;
}
for (int i = 0; i < num_keys2; i++) {
Keypoint k;
k.m_x = k2[i].m_x / f2;
k.m_y = k2[i].m_y / f2;
k2_norm[i] = k;
}
double scale = 0.5 * (f1 + f2);
std::vector<int> inliers =
EstimateFMatrix(k1_norm, k2_norm, matches, num_trials,
threshold / (scale * scale), E, true);
double K1_inv[9] = { 1.0 / f1, 0.0, 0.0,
0.0, 1.0 / f1, 0.0,
0.0, 0.0, 1.0 };
double K2_inv[9] = { 1.0 / f2, 0.0, 0.0,
0.0, 1.0 / f2, 0.0,
0.0, 0.0, 1.0 };
double tmp[9];
matrix_product(3, 3, 3, 3, K1_inv, E, tmp);
matrix_product(3, 3, 3, 3, K2_inv, tmp, F);
return inliers;
}
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);
}
/* Intersect a ray with the plane */
double PlaneData::IntersectRay(double *pos, double *ray, double *pt) const
{
double pos_dot, ray_dot;
matrix_product(1, 3, 3, 1, (double *) m_normal, pos, &pos_dot);
matrix_product(1, 3, 3, 1, (double *) m_normal, ray, &ray_dot);
if (ray_dot==0.0) {
return -DBL_MAX;
}
double t = (-m_dist - pos_dot) / ray_dot;
pt[0] = pos[0] + t * ray[0];
pt[1] = pos[1] + t * ray[1];
pt[2] = pos[2] + t * ray[2];
return t;
}
static void
homography_resids(int *m, int *n, double *x, double *fvec, int *iflag)
{
int i;
double resids = 0.0;
double H[9];
memcpy(H, x, 8 * sizeof(double));
H[8] = 1.0;
if (*iflag == 0 && global_num_pts > 4)
{
printf("[Round %d]\n", global_round);
printf(
" H=(%0.5f, %0.5f, %0.5f, %0.5f, %0.5f, %0.5f, %0.5f, %0.5f, %0.1f)\n",
H[0], H[1], H[2], H[3], H[4], H[5], H[6], H[7], H[8]);
global_round++;
}
for (i = 0; i < global_num_pts; i++)
{
double p[3], q[3];
p[0] = Vx(global_l_pts[i]);
p[1] = Vy(global_l_pts[i]);
p[2] = Vz(global_l_pts[i]);
if (*iflag == 0 && global_num_pts > 4)
printf(" p=(%0.3f, %0.3f, %0.3f)\n", p[0], p[1], p[2]);
matrix_product(3, 3, 3, 1, H, p, q);
if (*iflag == 0 && global_num_pts > 4)
printf(" q=(%0.3f, %0.3f, %0.3f)\n", q[0], q[1], q[2]);
q[0] /= q[2];
q[1] /= q[2];
fvec[2 * i + 0] = q[0] - Vx(global_r_pts[i]);
fvec[2 * i + 1] = q[1] - Vy(global_r_pts[i]);
if (*iflag == 0 && global_num_pts > 4)
printf(" (%0.3f, %0.3f) ==> (%0.3f, %0.3f)\n", q[0], q[1],
Vx(global_r_pts[i]), Vy(global_r_pts[i]));
}
for (i = 0; i < 2 * global_num_pts; i++)
{
resids += fvec[i] * fvec[i];
}
if (*iflag == 0 && global_num_pts > 4)
printf("resids = %0.3f\n", resids);
}
bool PlaneData::SetCorners(double *R, double *t, double f, int w, int h, int ymin, int ymax, int xmin, int xmax)
{
if(xmax==-1) xmax=w;
if(ymax==-1) ymax=h;
if(xmax>w || ymax>h ||xmax<-1 || ymax<-1) printf("Error in bounding box data w=%d h=%d xmin=%d xmax=%d ymin=%d ymax=%d\n",w,h,xmin,xmax,ymin,ymax);
//printf("xmin=%d xmax=%d ymin=%d ymax=%d\n",xmin,xmax,ymin,ymax);
double Rt[9];
matrix_transpose(3, 3, R, Rt);
double center[3];
matrix_product(3, 3, 3, 1, Rt, t, center);
matrix_scale(3, 1, center, -1.0, center);
/* Create rays for the four corners */
//uncomment the follwing code to use the 4 image corners
//double ray1[3] = { -0.5 * w, -0.5 * h, -f };
//double ray2[3] = { 0.5 * w, -0.5 * h, -f };
//double ray3[3] = { 0.5 * w, 0.5 * h, -f };
//double ray4[3] = { -0.5 * w, 0.5 * h, -f };
double ray1[3] = { xmin-0.5 * w, ymin-0.5 * h, -f };
double ray2[3] = { xmax-0.5 * w, ymin-0.5 * h, -f };
double ray3[3] = { xmax -0.5 * w, ymax-0.5 * h, -f };
double ray4[3] = {xmin -0.5 * w, ymax - 0.5 * h, -f };
double ray_world[18];
matrix_product(3, 3, 3, 1, Rt, ray1, ray_world + 0);
matrix_product(3, 3, 3, 1, Rt, ray2, ray_world + 3);
matrix_product(3, 3, 3, 1, Rt, ray3, ray_world + 6);
matrix_product(3, 3, 3, 1, Rt, ray4, ray_world + 9);
double t0 = IntersectRay(center, ray_world + 0, m_corners + 0);
double t1 = IntersectRay(center, ray_world + 3, m_corners + 3);
double t2 = IntersectRay(center, ray_world + 6, m_corners + 6);
double t3 = IntersectRay(center, ray_world + 9, m_corners + 9);
if (t0 > 0.0 && t1 > 0.0 && t2 > 0.0 && t3 > 0.0)
return true;
else
return false;
}
// autorelease function
matrix_t* matrix_pseudo_inverse(matrix_t* m)
{
if (m->rows == m->cols) {
return matrix_inverse(m);
} else if (m->rows > m->cols) {
matrix_begin();
// (A^T A)^{-1} A^T
matrix_t* ret = matrix_product(matrix_inverse(matrix_product(matrix_transpose(m), m)),matrix_transpose(m));
matrix_retain(ret);
matrix_end();
return matrix_autorelease(ret);
} else {
matrix_begin();
// A^T (A A^T)^{-1}
matrix_t* ret = matrix_product(matrix_transpose(m), matrix_inverse(matrix_product(m, matrix_transpose(m))));
matrix_retain(ret);
matrix_end();
return matrix_autorelease(ret);
}
}
/* Compute the mean of a set of vectors */
void v3_svd(int n, const v3_t *v, double *U, double *S, double *VT) {
double A[9] = { 0.0, 0.0, 0.0,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0 };
int i;
for (i = 0; i < n; i++) {
double tensor[9];
matrix_product(3, 1, 1, 3, v[i].p, v[i].p, tensor);
matrix_sum(3, 3, 3, 3, A, tensor, A);
}
dgesvd_driver(3, 3, A, U, S, VT);
}
/* Compute the covariance of a set of (zero-mean) vectors */
void v3_covariance_zm(int n, const v3_t *v, double *cov) {
int i;
for (i = 0; i < 9; i++)
cov[i] = 0.0;
for (i = 0; i < n; i++) {
double tmp[9];
matrix_product(3, 1, 1, 3, v[i].p, v[i].p, tmp);
matrix_sum(3, 3, 3, 3, cov, tmp, cov);
}
matrix_scale(3, 3, cov, 1.0 / n, cov);
}
/* Decompose a square matrix into an orthogonal matrix and a symmetric
* positive semidefinite matrix */
void matrix_polar_decomposition(int n, double *A, double *Q, double *S)
{
double *U, *diag, *VT;
double *diag_full, *tmp;
int i;
U = (double *) malloc(sizeof(double) * n * n);
diag = (double *) malloc(sizeof(double) * n);
VT = (double *) malloc(sizeof(double) * n * n);
/* Compute SVD */
dgesvd_driver(n, n, A, U, diag, VT);
/* Compute Q */
matrix_product(n, n, n, n, U, VT, Q);
/* Compute S */
diag_full = (double *) malloc(sizeof(double) * n * n);
for (i = 0; i < n * n; i++) {
diag_full[i] = 0.0;
}
for (i = 0; i < n; i++) {
diag_full[i * n + i] = diag[i];
}
tmp = (double *) malloc(sizeof(double) * n * n);
matrix_transpose_product(n, n, n, n, VT, diag_full, tmp);
matrix_product(n, n, n, n, tmp, VT, S);
free(U);
free(diag);
free(VT);
free(diag_full);
free(tmp);
}
void slerp(double *v1, double *v2, double t, double *v3)
{
double dot, angle, sin1t, sint, sina;
matrix_product(1, 3, 3, 1, v1, v2, &dot);
angle = acos(CLAMP(dot, -1.0 + 1.0e-8, 1.0 - 1.0e-8));
sin1t = sin((1.0-t) * angle);
sint = sin(t * angle);
sina = sin(angle);
v3[0] = (sin1t / sina) * v1[0] + (sint / sina) * v2[0];
v3[1] = (sin1t / sina) * v1[1] + (sint / sina) * v2[1];
v3[2] = (sin1t / sina) * v1[2] + (sint / sina) * v2[2];
}
v3_t BundlerApp::GeneratePointAtInfinity(const ImageKeyVector &views,
int *added_order,
camera_params_t *cameras,
double &error,
bool explicit_camera_centers)
{
camera_params_t *cam = NULL;
int camera_idx = views[0].first;
int image_idx = added_order[camera_idx];
int key_idx = views[0].second;
Keypoint &key = GetKey(image_idx, key_idx);
cam = cameras + camera_idx;
double p3[3] = { key.m_x, key.m_y, 1.0 };
if (m_optimize_for_fisheye) {
/* Undistort the point */
double x = p3[0], y = p3[1];
m_image_data[image_idx].UndistortPoint(x, y, p3[0], p3[1]);
}
double K[9], Kinv[9];
GetIntrinsics(cameras[camera_idx], K);
matrix_invert(3, K, Kinv);
double ray[3];
matrix_product(3, 3, 3, 1, Kinv, p3, ray);
/* We now have a ray, put it at infinity */
double ray_world[3];
matrix_transpose_product(3, 3, 3, 1, cam->R, ray, ray_world);
double pos[3] = { 0.0, 0.0, 0.0 };
double pt_inf[3] = { 0.0, 0.0, 0.0 };
if (!explicit_camera_centers) {
} else {
memcpy(pos, cam->t, 3 * sizeof(double));
double ray_extend[3];
matrix_scale(3, 1, ray, 100.0, ray_extend);
matrix_sum(3, 1, 3, 1, pos, ray, pt_inf);
}
return v3_new(pt_inf[0], pt_inf[1], pt_inf[2]);
}
int highest_product(int len, int matrix[MY][MX]) {
int highest = 0;
int product;
int y; for(y = 0; y < MY; y++) {
int x; for(x = 0; x < MX; x++) {
int dir; for(dir = 0; dir < 4; dir++) {
product = matrix_product(x, y, len, dir, matrix);
if(product > highest) highest = product;
}
}
}
return highest;
}
/* Compute the power of a matrix */
void matrix_power(int n, double *A, int pow, double *R)
{
int i;
/* Slow way */
double *curr = (double *) malloc(sizeof(double) * n * n);
double *tmp = (double *) malloc(sizeof(double) * n * n);
matrix_ident(n, curr);
for (i = 0; i < pow; i++) {
matrix_product(n, n, n, n, curr, A, tmp);
memcpy(curr, tmp, sizeof(double) * n * n);
}
memcpy(R, curr, sizeof(double) * n * n);
free(curr);
free(tmp);
}
/* Create a planar patch for this point */
void PointData::CreatePlanarPatch(double size, PlaneData &plane)
{
memcpy(plane.m_normal, m_norm, 3 * sizeof(double));
double dist;
matrix_product(1, 3, 3, 1, m_norm, m_pos, &dist);
plane.m_dist = -dist;
memcpy(plane.m_origin, m_pos, 3 * sizeof(double));
// u-axis should be perp to the y-axis and to normal
double y[3] = { 0.0, 1.0, 0.0 };
matrix_cross(m_norm, y, plane.m_uaxis);
// v-axis should be perp to normal and uaxis
matrix_cross(m_norm, plane.m_uaxis, plane.m_vaxis);
plane.m_u_extent = plane.m_v_extent = size;
}
/* Project a point onto the plane */
void PlaneData::Project(double *p, double *p_proj)
{
/* Subtract the distance vector */
double vec[3];
matrix_scale(3, 1, m_normal, m_dist, vec);
double p_norm[3];
matrix_diff(3, 1, 3, 1, p, vec, p_norm);
double dot;
matrix_product(1, 3, 3, 1, m_normal, p_norm, &dot);
double p_par[3];
matrix_scale(3, 1, m_normal, dot, p_par);
double p_perp[3];
matrix_diff(3, 1, 3, 1, p_norm, p_par, p_perp);
matrix_sum(3, 1, 3, 1, p_perp, vec, p_proj);
}
static int CountInliers(const std::vector<Keypoint> &k1,
const std::vector<Keypoint> &k2,
std::vector<KeypointMatch> matches,
double *M, double thresh, std::vector<int> &inliers)
{
inliers.clear();
int count = 0;
for (unsigned int i = 0; i < matches.size(); i++) {
/* Determine if the ith feature in f1, when transformed by M,
* is within RANSACthresh of its match in f2 (if one exists)
*
* if so, increment count and append i to inliers */
double p[3];
p[0] = k1[matches[i].m_idx1].m_x;
p[1] = k1[matches[i].m_idx1].m_y;
p[2] = 1.0;
double q[3];
matrix_product(3, 3, 3, 1, M, p, q);
double qx = q[0] / q[2];
double qy = q[1] / q[2];
double dx = qx - k2[matches[i].m_idx2].m_x;
double dy = qy - k2[matches[i].m_idx2].m_y;
double dist = sqrt(dx * dx + dy * dy);
if (dist <= thresh) {
count++;
inliers.push_back(i);
}
}
return count;
}
void EKF::_getA(double *state, double *A) const
{
double LinVel[3];
double AngVel[3];
matrix_zeroes(3 * _num_feats_init_structure + _num_motion_states, 3 * _num_feats_init_structure + _num_motion_states,
A);
LinVel[0] = state[3 * _num_feats_init_structure + 0];
LinVel[1] = state[3 * _num_feats_init_structure + 1];
LinVel[2] = state[3 * _num_feats_init_structure + 2];
AngVel[0] = state[3 * _num_feats_init_structure + 3];
AngVel[1] = state[3 * _num_feats_init_structure + 4];
AngVel[2] = state[3 * _num_feats_init_structure + 5];
matrix_scale(1, 3, LinVel, _dt, LinVel);
matrix_scale(1, 3, AngVel, _dt, AngVel);
double DR[9 * 3];
double DRx[3 * 3], DRy[3 * 3], DRz[3 * 3];
double I3[3 * 3];
matrix_ident(3, I3);
RODR_derivative(AngVel, DR);
DRx[0 * 3 + 0] = DR[0 * 3 + 0];
DRx[0 * 3 + 1] = DR[0 * 3 + 1];
DRx[0 * 3 + 2] = DR[0 * 3 + 2];
DRx[1 * 3 + 0] = DR[1 * 3 + 0];
DRx[1 * 3 + 1] = DR[1 * 3 + 1];
DRx[1 * 3 + 2] = DR[1 * 3 + 2];
DRx[2 * 3 + 0] = DR[2 * 3 + 0];
DRx[2 * 3 + 1] = DR[2 * 3 + 1];
DRx[2 * 3 + 2] = DR[2 * 3 + 2];
DRy[0 * 3 + 0] = DR[3 * 3 + 0];
DRy[0 * 3 + 1] = DR[3 * 3 + 1];
DRy[0 * 3 + 2] = DR[3 * 3 + 2];
DRy[1 * 3 + 0] = DR[4 * 3 + 0];
DRy[1 * 3 + 1] = DR[4 * 3 + 1];
DRy[1 * 3 + 2] = DR[4 * 3 + 2];
DRy[2 * 3 + 0] = DR[5 * 3 + 0];
DRy[2 * 3 + 1] = DR[5 * 3 + 1];
DRy[2 * 3 + 2] = DR[5 * 3 + 2];
DRz[0 * 3 + 0] = DR[6 * 3 + 0];
DRz[0 * 3 + 1] = DR[6 * 3 + 1];
DRz[0 * 3 + 2] = DR[6 * 3 + 2];
DRz[1 * 3 + 0] = DR[7 * 3 + 0];
DRz[1 * 3 + 1] = DR[7 * 3 + 1];
DRz[1 * 3 + 2] = DR[7 * 3 + 2];
DRz[2 * 3 + 0] = DR[8 * 3 + 0];
DRz[2 * 3 + 1] = DR[8 * 3 + 1];
DRz[2 * 3 + 2] = DR[8 * 3 + 2];
double R[3 * 3];
RODR(AngVel, R);
double P[3];
double dfdxyz[3 * 3];
double dfdAVx[3];
double dfdAVy[3];
double dfdAVz[3];
//feature with respect to itself
//ublas::matrix<double> dfdxyz = ublas::prod(RODR(AngVel*dt),ublas::identity_matrix<double>(3));
matrix_product(3, 3, 3, 3, R, I3, dfdxyz);
//diff features wrt. features
for (int i = 0; i < _num_feats_init_structure; i++)
{
P[0] = state[3 * i + 0];
P[1] = state[3 * i + 1];
P[2] = state[3 * i + 2];
A[(3 * i + 0) * (3 * _num_feats_init_structure + _num_motion_states) + 3 * i + 0] = dfdxyz[0 * 3 + 0];
A[(3 * i + 0) * (3 * _num_feats_init_structure + _num_motion_states) + 3 * i + 1] = dfdxyz[0 * 3 + 1];
A[(3 * i + 0) * (3 * _num_feats_init_structure + _num_motion_states) + 3 * i + 2] = dfdxyz[0 * 3 + 2];
A[(3 * i + 1) * (3 * _num_feats_init_structure + _num_motion_states) + 3 * i + 0] = dfdxyz[1 * 3 + 0];
A[(3 * i + 1) * (3 * _num_feats_init_structure + _num_motion_states) + 3 * i + 1] = dfdxyz[1 * 3 + 1];
A[(3 * i + 1) * (3 * _num_feats_init_structure + _num_motion_states) + 3 * i + 2] = dfdxyz[1 * 3 + 2];
A[(3 * i + 2) * (3 * _num_feats_init_structure + _num_motion_states) + 3 * i + 0] = dfdxyz[2 * 3 + 0];
A[(3 * i + 2) * (3 * _num_feats_init_structure + _num_motion_states) + 3 * i + 1] = dfdxyz[2 * 3 + 1];
A[(3 * i + 2) * (3 * _num_feats_init_structure + _num_motion_states) + 3 * i + 2] = dfdxyz[2 * 3 + 2];
//feature with respect to the 9 last variables (linear/angular velocities and translation of center of rotation)
//feature with respect to linear velocities
A[(3 * i + 0) * (3 * _num_feats_init_structure + _num_motion_states) + 3 * _num_feats_init_structure] = _dt;
A[(3 * i + 0) * (3 * _num_feats_init_structure + _num_motion_states) + 3 * _num_feats_init_structure + 1] = 0;
A[(3 * i + 0) * (3 * _num_feats_init_structure + _num_motion_states) + 3 * _num_feats_init_structure + 2] = 0;
A[(3 * i + 1) * (3 * _num_feats_init_structure + _num_motion_states) + 3 * _num_feats_init_structure] = 0;
A[(3 * i + 1) * (3 * _num_feats_init_structure + _num_motion_states) + 3 * _num_feats_init_structure + 1] = _dt;
A[(3 * i + 1) * (3 * _num_feats_init_structure + _num_motion_states) + 3 * _num_feats_init_structure + 2] = 0;
A[(3 * i + 2) * (3 * _num_feats_init_structure + _num_motion_states) + 3 * _num_feats_init_structure] = 0;
A[(3 * i + 2) * (3 * _num_feats_init_structure + _num_motion_states) + 3 * _num_feats_init_structure + 1] = 0;
A[(3 * i + 2) * (3 * _num_feats_init_structure + _num_motion_states) + 3 * _num_feats_init_structure + 2] = _dt;
//feature with respect to angular velocities
//ublas::vector<double> dfdAVx = ublas::prod(ublas::prod(DRx, ublas::identity_matrix<double>(3)),ublas::trans(P));
matrix_product(3, 3, 3, 1, DRx, P, dfdAVx);
//ublas::vector<double> dfdAVy = ublas::prod(ublas::prod(DRy, ublas::identity_matrix<double>(3)),ublas::trans(P));
matrix_product(3, 3, 3, 1, DRy, P, dfdAVy);
//ublas::vector<double> dfdAVz = ublas::prod(ublas::prod(DRz, ublas::identity_matrix<double>(3)),ublas::trans(P));
matrix_product(3, 3, 3, 1, DRz, P, dfdAVz);
A[(3 * i + 0) * (3 * _num_feats_init_structure + _num_motion_states) + 3 * _num_feats_init_structure + 3]
= dfdAVx[0];
A[(3 * i + 0) * (3 * _num_feats_init_structure + _num_motion_states) + 3 * _num_feats_init_structure + 4]
= dfdAVy[0];
A[(3 * i + 0) * (3 * _num_feats_init_structure + _num_motion_states) + 3 * _num_feats_init_structure + 5]
= dfdAVz[0];
A[(3 * i + 1) * (3 * _num_feats_init_structure + _num_motion_states) + 3 * _num_feats_init_structure + 3]
= dfdAVx[1];
A[(3 * i + 1) * (3 * _num_feats_init_structure + _num_motion_states) + 3 * _num_feats_init_structure + 4]
= dfdAVy[1];
A[(3 * i + 1) * (3 * _num_feats_init_structure + _num_motion_states) + 3 * _num_feats_init_structure + 5]
= dfdAVz[1];
A[(3 * i + 2) * (3 * _num_feats_init_structure + _num_motion_states) + 3 * _num_feats_init_structure + 3]
= dfdAVx[2];
A[(3 * i + 2) * (3 * _num_feats_init_structure + _num_motion_states) + 3 * _num_feats_init_structure + 4]
= dfdAVy[2];
A[(3 * i + 2) * (3 * _num_feats_init_structure + _num_motion_states) + 3 * _num_feats_init_structure + 5]
= dfdAVz[2];
}
//the last 9 variables with respect to themselves
//dLinVel/dLinVel
A[(3 * _num_feats_init_structure + 0) * (3 * _num_feats_init_structure + _num_motion_states) + 3
* _num_feats_init_structure + 0] = 1;
A[(3 * _num_feats_init_structure + 0) * (3 * _num_feats_init_structure + _num_motion_states) + 3
* _num_feats_init_structure + 1] = 0;
A[(3 * _num_feats_init_structure + 0) * (3 * _num_feats_init_structure + _num_motion_states) + 3
* _num_feats_init_structure + 2] = 0;
A[(3 * _num_feats_init_structure + 1) * (3 * _num_feats_init_structure + _num_motion_states) + 3
* _num_feats_init_structure + 0] = 0;
A[(3 * _num_feats_init_structure + 1) * (3 * _num_feats_init_structure + _num_motion_states) + 3
* _num_feats_init_structure + 1] = 1;
A[(3 * _num_feats_init_structure + 1) * (3 * _num_feats_init_structure + _num_motion_states) + 3
* _num_feats_init_structure + 2] = 0;
A[(3 * _num_feats_init_structure + 2) * (3 * _num_feats_init_structure + _num_motion_states) + 3
* _num_feats_init_structure + 0] = 0;
A[(3 * _num_feats_init_structure + 2) * (3 * _num_feats_init_structure + _num_motion_states) + 3
* _num_feats_init_structure + 1] = 0;
A[(3 * _num_feats_init_structure + 2) * (3 * _num_feats_init_structure + _num_motion_states) + 3
* _num_feats_init_structure + 2] = 1;
//dAngVel/dAngVel
A[(3 * _num_feats_init_structure + 3) * (3 * _num_feats_init_structure + _num_motion_states) + 3
* _num_feats_init_structure + 3] = 1;
A[(3 * _num_feats_init_structure + 3) * (3 * _num_feats_init_structure + _num_motion_states) + 3
* _num_feats_init_structure + 4] = 0;
A[(3 * _num_feats_init_structure + 3) * (3 * _num_feats_init_structure + _num_motion_states) + 3
* _num_feats_init_structure + 5] = 0;
A[(3 * _num_feats_init_structure + 4) * (3 * _num_feats_init_structure + _num_motion_states) + 3
* _num_feats_init_structure + 3] = 0;
A[(3 * _num_feats_init_structure + 4) * (3 * _num_feats_init_structure + _num_motion_states) + 3
* _num_feats_init_structure + 4] = 1;
A[(3 * _num_feats_init_structure + 4) * (3 * _num_feats_init_structure + _num_motion_states) + 3
* _num_feats_init_structure + 5] = 0;
A[(3 * _num_feats_init_structure + 5) * (3 * _num_feats_init_structure + _num_motion_states) + 3
* _num_feats_init_structure + 3] = 0;
A[(3 * _num_feats_init_structure + 5) * (3 * _num_feats_init_structure + _num_motion_states) + 3
* _num_feats_init_structure + 4] = 0;
A[(3 * _num_feats_init_structure + 5) * (3 * _num_feats_init_structure + _num_motion_states) + 3
* _num_feats_init_structure + 5] = 1;
}
double EKF::_step(const std::vector<vision::FeaturePtr> &y)
{
double mean_innovation;
if (y.size() != _num_feats_init_structure)
{
ROS_ERROR(
"[EKF::_step] The size of the measurement y differs from the expected number of tracked and matched Features.\nSize of y: %d Expected size (number of matched Features): %d",
y.size(), _num_feats_init_structure);
throw std::string(
"FATAL ERROR [EKF::_step]: The size of the measurement y differs from the expected number of tracked and matched Features.");
}
// Prediction step
_getA(_updated_state, _A_ekf);
_predictState(_updated_state, _predicted_state);
// predCovar=A*upCovar*A'+WQW;
matrix_product(3 * _num_feats_init_structure + _num_motion_states,
3 * _num_feats_init_structure + _num_motion_states,
3 * _num_feats_init_structure + _num_motion_states,
3 * _num_feats_init_structure + _num_motion_states, _A_ekf, _updated_covar, _A_ekf_covar);
matrix_transpose_product2(3 * _num_feats_init_structure + _num_motion_states,
3 * _num_feats_init_structure + _num_motion_states,
3 * _num_feats_init_structure + _num_motion_states,
3 * _num_feats_init_structure + _num_motion_states, _A_ekf_covar, _A_ekf, _predicted_covar);
matrix_sum(3 * _num_feats_init_structure + _num_motion_states, 3 * _num_feats_init_structure + _num_motion_states,
3 * _num_feats_init_structure + _num_motion_states, 3 * _num_feats_init_structure + _num_motion_states,
_predicted_covar, _W_Q_W_ekf_trans, _predicted_covar);
//update step
_computeH(_predicted_state, _H_ekf);
// K=predCovar*H'/(H*predCovar*H'+R);
// H'
matrix_transpose(_num_feats_init_structure * 2, 3 * _num_feats_init_structure + _num_motion_states, _H_ekf,
_H_ekf_trans);
// predCovar*H'
matrix_product(3 * _num_feats_init_structure + _num_motion_states,
3 * _num_feats_init_structure + _num_motion_states,
3 * _num_feats_init_structure + _num_motion_states, _num_feats_init_structure * 2, _predicted_covar,
_H_ekf_trans, _covar_H_ekf);
// H*predCovar
matrix_product(_num_feats_init_structure * 2, 3 * _num_feats_init_structure + _num_motion_states,
3 * _num_feats_init_structure + _num_motion_states,
3 * _num_feats_init_structure + _num_motion_states, _H_ekf, _predicted_covar, _H_ekf_covar);
// H*predCovar*H'
matrix_product(_num_feats_init_structure * 2, 3 * _num_feats_init_structure + _num_motion_states,
3 * _num_feats_init_structure + _num_motion_states, _num_feats_init_structure * 2, _H_ekf_covar,
_H_ekf_trans, _H_covar_H_ekf);
// (H*predCovar*H'+R)
matrix_sum(_num_feats_init_structure * 2, _num_feats_init_structure * 2, _num_feats_init_structure * 2,
_num_feats_init_structure * 2, _H_covar_H_ekf, _R_ekf, _H_covar_H_ekf);
// inv(H*predCovar*H'+R)
matrix_invert(_num_feats_init_structure * 2, _H_covar_H_ekf, _H_Q_H_ekf);
//K=predCovar*H'*inv(H*predCovar*H'+R);
matrix_product(3 * _num_feats_init_structure + _num_motion_states, _num_feats_init_structure * 2,
_num_feats_init_structure * 2, _num_feats_init_structure * 2, _covar_H_ekf, _H_Q_H_ekf, _K_ekf);
// predict
_predictObservation(_predicted_state, _estimated_z);
// measurement
for (int i = 0; i < _num_feats_init_structure; i++)
{
_z[i * 2 + 0] = y[i]->getX();
_z[i * 2 + 1] = y[i]->getY();
}
//innovation = z-z_estimate;
matrix_diff(1, _num_feats_init_structure * 2, 1, _num_feats_init_structure * 2, _z, _estimated_z, _innovation);
// upState=predState+K*(innovation);
matrix_product(3 * _num_feats_init_structure + _num_motion_states, _num_feats_init_structure * 2,
_num_feats_init_structure * 2, 1, _K_ekf, _innovation, _updated_state);
matrix_sum(1, 3 * _num_feats_init_structure + _num_motion_states, 1,
3 * _num_feats_init_structure + _num_motion_states, _updated_state, _predicted_state, _updated_state);
// upCovar=(eye(3*numFeatures+6)-K*H)*predCovar;
matrix_product(3 * _num_feats_init_structure + _num_motion_states, _num_feats_init_structure * 2,
_num_feats_init_structure * 2, 3 * _num_feats_init_structure + _num_motion_states, _K_ekf, _H_ekf,
_K_H_ekf);
//I_KH = (I - KH);
matrix_diff(_num_feats_init_structure * 3 + _num_motion_states, _num_feats_init_structure * 3 + _num_motion_states,
_num_feats_init_structure * 3 + _num_motion_states, _num_feats_init_structure * 3 + _num_motion_states,
_I_ekf, _K_H_ekf, _I_K_H_ekf);
matrix_product(_num_feats_init_structure * 3 + _num_motion_states,
_num_feats_init_structure * 3 + _num_motion_states,
_num_feats_init_structure * 3 + _num_motion_states,
_num_feats_init_structure * 3 + _num_motion_states, _I_K_H_ekf, _predicted_covar, _updated_covar);
// calculate error after correction
_predictObservation(_updated_state, _estimated_z);
//innovation = z-z_estimate;
matrix_diff(1, _num_feats_init_structure * 2, 1, _num_feats_init_structure * 2, _z, _estimated_z, _innovation);
double dist = 0;
for (int i = 0; i < _num_feats_init_structure; i++)
{
dist += sqrt(pow(_innovation[i * 2 + 0], 2) + pow(_innovation[i * 2 + 1], 2));
}
mean_innovation = dist / ((double)_num_feats_init_structure);
return mean_innovation;
}
void EKF::_initVariables()
{
_updated_covar = new double[(_num_feats_init_structure * 3 + _num_motion_states) * (_num_feats_init_structure * 3
+ _num_motion_states)];
matrix_ident(_num_feats_init_structure * 3 + _num_motion_states, _updated_covar);
matrix_scale(_num_feats_init_structure * 3 + _num_motion_states, _num_feats_init_structure * 3 + _num_motion_states,
_updated_covar, 0.00001, _updated_covar);
double Qrot = 1;
double Qtrans = 1;
_updated_covar[(_num_feats_init_structure * 3 + _num_motion_states) * (3 * _num_feats_init_structure + 0) + 3
* _num_feats_init_structure + 0] = Qtrans;
_updated_covar[(_num_feats_init_structure * 3 + _num_motion_states) * (3 * _num_feats_init_structure + 1) + 3
* _num_feats_init_structure + 1] = Qtrans;
_updated_covar[(_num_feats_init_structure * 3 + _num_motion_states) * (3 * _num_feats_init_structure + 2) + 3
* _num_feats_init_structure + 2] = Qtrans;
_updated_covar[(_num_feats_init_structure * 3 + _num_motion_states) * (3 * _num_feats_init_structure + 3) + 3
* _num_feats_init_structure + 3] = Qrot;
_updated_covar[(_num_feats_init_structure * 3 + _num_motion_states) * (3 * _num_feats_init_structure + 4) + 3
* _num_feats_init_structure + 4] = Qrot;
_updated_covar[(_num_feats_init_structure * 3 + _num_motion_states) * (3 * _num_feats_init_structure + 5) + 3
* _num_feats_init_structure + 5] = Qrot;
_R_ekf = new double[(_num_feats_init_structure * 2) * (_num_feats_init_structure * 2)];
matrix_ident(_num_feats_init_structure * 2, _R_ekf);
matrix_scale(_num_feats_init_structure * 2, _num_feats_init_structure * 2, _R_ekf, 0.01, _R_ekf);
_Q_ekf = new double[_num_motion_states * _num_motion_states];
matrix_ident(_num_motion_states, _Q_ekf);
_Q_ekf[_num_motion_states * 0 + 0] = 1;
_Q_ekf[_num_motion_states * 1 + 1] = 1;
_Q_ekf[_num_motion_states * 2 + 2] = 1; // linear
_Q_ekf[_num_motion_states * 3 + 3] = 3;
_Q_ekf[_num_motion_states * 4 + 4] = 3;
_Q_ekf[_num_motion_states * 5 + 5] = 3; // rotation
matrix_scale(_num_motion_states, _num_motion_states, _Q_ekf, 0.01, _Q_ekf);
_A_ekf = new double[(3 * _num_feats_init_structure + _num_motion_states) * (3 * _num_feats_init_structure
+ _num_motion_states)];
matrix_zeroes(3 * _num_feats_init_structure + _num_motion_states, 3 * _num_feats_init_structure + _num_motion_states,
_A_ekf);
// _W_ekf doesn't depend on values of the state only the size matters
_W_ekf = new double[(3 * _num_feats_init_structure + _num_motion_states) * _num_motion_states];
_getW(_updated_state, _W_ekf);
_W_ekf_trans = new double[(3 * _num_feats_init_structure + _num_motion_states) * _num_motion_states];
matrix_transpose(3 * _num_feats_init_structure + _num_motion_states, _num_motion_states, _W_ekf, _W_ekf_trans);
_W_Q_ekf = new double[(3 * _num_feats_init_structure + _num_motion_states) * _num_motion_states];
matrix_product(3 * _num_feats_init_structure + _num_motion_states, _num_motion_states, _num_motion_states,
_num_motion_states, _W_ekf, _Q_ekf, _W_Q_ekf);
//_W_Q_W_ekf_trans = ublas::prod(W_Q, W_trans);
_W_Q_W_ekf_trans = new double[(3 * _num_feats_init_structure + _num_motion_states) * (3 * _num_feats_init_structure
+ _num_motion_states)];
matrix_product((3 * _num_feats_init_structure + _num_motion_states), _num_motion_states, _num_motion_states,
(3 * _num_feats_init_structure + _num_motion_states), _W_Q_ekf, _W_ekf_trans, _W_Q_W_ekf_trans);
_predicted_state = new double[_num_feats_init_structure * 3 + _num_motion_states];
matrix_zeroes(1, _num_feats_init_structure * 3 + _num_motion_states, _predicted_state);
_A_ekf_covar = new double[(3 * _num_feats_init_structure + _num_motion_states) * (3 * _num_feats_init_structure
+ _num_motion_states)];
_predicted_covar = new double[(3 * _num_feats_init_structure + _num_motion_states) * (3 * _num_feats_init_structure
+ _num_motion_states)];
// z is the observation vector
_z = new double[_num_feats_init_structure * 2];
_H_ekf = new double[(_num_feats_init_structure * 2) * (3 * _num_feats_init_structure + _num_motion_states)];
matrix_zeroes((_num_feats_init_structure * 2), (3 * _num_feats_init_structure + _num_motion_states), _H_ekf);
_H_ekf_trans = new double[(3 * _num_feats_init_structure + _num_motion_states) * (_num_feats_init_structure * 2)];
//H_covar = ublas::prod(H,predCovar);
_H_ekf_covar = new double[(_num_feats_init_structure * 2) * (3 * _num_feats_init_structure + _num_motion_states)];
//covarH = ublas::prod(predCovar, H_trans);
_covar_H_ekf = new double[(3 * _num_feats_init_structure + _num_motion_states) * (_num_feats_init_structure * 2)];
//HcovarH = ublas::prod(H_covar, H_trans) + R;
_H_covar_H_ekf = new double[(_num_feats_init_structure * 2) * (_num_feats_init_structure * 2)];
_K_ekf = new double[(3 * _num_feats_init_structure + _num_motion_states) * (_num_feats_init_structure * 2)];
_estimated_z = new double[_num_feats_init_structure * 2];
_innovation = new double[_num_feats_init_structure * 2];
_I_ekf = new double[(_num_feats_init_structure * 3 + _num_motion_states) * (_num_feats_init_structure * 3
+ _num_motion_states)];
matrix_ident(_num_feats_init_structure * 3 + _num_motion_states, _I_ekf);
//KH = ublas::prod(K,H);
_K_H_ekf = new double[(_num_feats_init_structure * 3 + _num_motion_states) * (_num_feats_init_structure * 3
+ _num_motion_states)];
//I_KH = (I - KH);
_I_K_H_ekf = new double[(_num_feats_init_structure * 3 + _num_motion_states) * (_num_feats_init_structure * 3
+ _num_motion_states)];
//HQH = ublas::matrix<double>(2*numFeatures, 2*numFeatures);
_H_Q_H_ekf = new double[(2 * _num_feats_init_structure) * (2 * _num_feats_init_structure)];
}
/* Estimate an F-matrix from a given set of point matches */
std::vector<int> EstimateFMatrix(const std::vector<Keypoint> &k1,
const std::vector<Keypoint> &k2,
std::vector<KeypointMatch> matches,
int num_trials, double threshold,
double *F, bool essential)
{
int num_pts = (int) matches.size();
/* num_pts should be greater than a threshold */
if (num_pts < 20) {
std::vector<int> inliers;
return inliers;
}
v3_t *k1_pts = new v3_t[num_pts];
v3_t *k2_pts = new v3_t[num_pts];
v3_t *k1_pts_in = new v3_t[num_pts];
v3_t *k2_pts_in = new v3_t[num_pts];
for (int i = 0; i < num_pts; i++) {
int idx1 = matches[i].m_idx1;
int idx2 = matches[i].m_idx2;
if(idx1 >= k1.size()) {
fprintf(stderr, "Error: %d >= %d\n", idx1, k1.size());
fprintf(stderr, "idx1/idx2: %d %d\n", idx1, idx2);
fprintf(stderr, "Fix me 1\n");
continue;
}
if(idx2 >= k2.size()) {
fprintf(stderr, "Error: %d >= %d\n", idx2, k2.size());
fprintf(stderr, "idx1/idx2: %d %d\n", idx1, idx2);
fprintf(stderr, "Fix me 2\n");
continue;
}
assert(idx1 < (int) k1.size());
assert(idx2 < (int) k2.size());
k1_pts[i] = v3_new(k1[idx1].m_x, k1[idx1].m_y, 1.0);
k2_pts[i] = v3_new(k2[idx2].m_x, k2[idx2].m_y, 1.0);
}
estimate_fmatrix_ransac_matches(num_pts, k2_pts, k1_pts,
num_trials, threshold, 0.99,
(essential ? 1 : 0), F);
/* Find the inliers */
std::vector<int> inliers;
for (int i = 0; i < num_pts; i++) {
double dist = fmatrix_compute_residual(F, k2_pts[i], k1_pts[i]);
if (dist < threshold) {
inliers.push_back(i);
}
}
/* Re-estimate using inliers */
int num_inliers = (int) inliers.size();
for (int i = 0; i < num_inliers; i++) {
k1_pts_in[i] = k1_pts[inliers[i]]; // v3_new(k1[idx1]->m_x, k1[idx1]->m_y, 1.0);
k2_pts_in[i] = k2_pts[inliers[i]]; // v3_new(k2[idx2]->m_x, k2[idx2]->m_y, 1.0);
}
// printf("[1] num_inliers = %d\n", num_inliers);
#if 0
double F0[9];
double e1[3], e2[3];
estimate_fmatrix_linear(num_inliers, k2_pts_in, k1_pts_in, F0, e1, e2);
inliers.clear();
for (int i = 0; i < num_pts; i++) {
double dist = fmatrix_compute_residual(F0, k2_pts[i], k1_pts[i]);
if (dist < threshold) {
inliers.push_back(i);
}
}
num_inliers = inliers.size();
// printf("[2] num_inliers = %d\n", num_inliers);
// matrix_print(3, 3, F0);
#else
double F0[9];
memcpy(F0, F, sizeof(double) * 9);
#endif
if (!essential) {
/* Refine using NLLS */
for (int i = 0; i < num_inliers; i++) {
k1_pts_in[i] = k1_pts[inliers[i]];
k2_pts_in[i] = k2_pts[inliers[i]];
}
refine_fmatrix_nonlinear_matches(num_inliers, k2_pts_in, k1_pts_in,
F0, F);
} else {
memcpy(F, F0, sizeof(double) * 9);
}
#if 0
if (essential) {
/* Compute the SVD of F */
double U[9], S[3], VT[9];
dgesvd_driver(3, 3, F, U, S, VT);
double E0[9] = { 1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 0.0 };
double tmp[9];
matrix_product(3, 3, 3, 3, U, E0, tmp);
matrix_product(3, 3, 3, 3, tmp, VT, F);
}
#endif
inliers.clear();
for (int i = 0; i < num_pts; i++) {
double dist = fmatrix_compute_residual(F, k2_pts[i], k1_pts[i]);
if (dist < threshold) {
inliers.push_back(i);
}
}
num_inliers = (int) inliers.size();
delete [] k1_pts;
delete [] k2_pts;
delete [] k1_pts_in;
delete [] k2_pts_in;
return inliers;
}
/* Triangulate a subtrack */
v3_t BundlerApp::TriangulateNViews(const ImageKeyVector &views,
int *added_order,
camera_params_t *cameras,
double &error,
bool explicit_camera_centers)
{
int num_views = (int) views.size();
v2_t *pv = new v2_t[num_views];
double *Rs = new double[9 * num_views];
double *ts = new double[3 * num_views];
for (int i = 0; i < num_views; i++) {
camera_params_t *cam = NULL;
int camera_idx = views[i].first;
int image_idx = added_order[camera_idx];
int key_idx = views[i].second;
Keypoint &key = GetKey(image_idx, key_idx);
double p3[3] = { key.m_x, key.m_y, 1.0 };
if (m_optimize_for_fisheye) {
/* Undistort the point */
double x = p3[0], y = p3[1];
m_image_data[image_idx].UndistortPoint(x, y, p3[0], p3[1]);
}
double K[9], Kinv[9];
GetIntrinsics(cameras[camera_idx], K);
matrix_invert(3, K, Kinv);
double p_n[3];
matrix_product(3, 3, 3, 1, Kinv, p3, p_n);
// EDIT!!!
pv[i] = v2_new(-p_n[0], -p_n[1]);
pv[i] = UndistortNormalizedPoint(pv[i], cameras[camera_idx]);
cam = cameras + camera_idx;
memcpy(Rs + 9 * i, cam->R, 9 * sizeof(double));
if (!explicit_camera_centers) {
memcpy(ts + 3 * i, cam->t, 3 * sizeof(double));
} else {
matrix_product(3, 3, 3, 1, cam->R, cam->t, ts + 3 * i);
matrix_scale(3, 1, ts + 3 * i, -1.0, ts + 3 * i);
}
}
v3_t pt = triangulate_n(num_views, pv, Rs, ts, &error);
error = 0.0;
for (int i = 0; i < num_views; i++) {
int camera_idx = views[i].first;
int image_idx = added_order[camera_idx];
int key_idx = views[i].second;
Keypoint &key = GetKey(image_idx, key_idx);
v2_t pr = sfm_project_final(cameras + camera_idx, pt,
explicit_camera_centers ? 1 : 0,
m_estimate_distortion ? 1 : 0);
if (m_optimize_for_fisheye) {
double x = Vx(pr), y = Vy(pr);
m_image_data[image_idx].DistortPoint(x, y, Vx(pr), Vy(pr));
}
double dx = Vx(pr) - key.m_x;
double dy = Vy(pr) - key.m_y;
error += dx * dx + dy * dy;
}
error = sqrt(error / num_views);
delete [] pv;
delete [] Rs;
delete [] ts;
return pt;
}
/* Triangulate two points */
v3_t Triangulate(v2_t p, v2_t q,
camera_params_t c1, camera_params_t c2,
double &proj_error, bool &in_front, double &angle,
bool explicit_camera_centers)
{
double K1[9], K2[9];
double K1inv[9], K2inv[9];
GetIntrinsics(c1, K1);
GetIntrinsics(c2, K2);
matrix_invert(3, K1, K1inv);
matrix_invert(3, K2, K2inv);
/* Set up the 3D point */
// EDIT!!!
double proj1[3] = { Vx(p), Vy(p), -1.0 };
double proj2[3] = { Vx(q), Vy(q), -1.0 };
double proj1_norm[3], proj2_norm[3];
matrix_product(3, 3, 3, 1, K1inv, proj1, proj1_norm);
matrix_product(3, 3, 3, 1, K2inv, proj2, proj2_norm);
v2_t p_norm = v2_new(proj1_norm[0] / proj1_norm[2],
proj1_norm[1] / proj1_norm[2]);
v2_t q_norm = v2_new(proj2_norm[0] / proj2_norm[2],
proj2_norm[1] / proj2_norm[2]);
/* Undo radial distortion */
p_norm = UndistortNormalizedPoint(p_norm, c1);
q_norm = UndistortNormalizedPoint(q_norm, c2);
/* Compute the angle between the rays */
angle = ComputeRayAngle(p, q, c1, c2);
/* Triangulate the point */
v3_t pt;
if (!explicit_camera_centers) {
pt = triangulate(p_norm, q_norm, c1.R, c1.t, c2.R, c2.t, &proj_error);
} else {
double t1[3];
double t2[3];
/* Put the translation in standard form */
matrix_product(3, 3, 3, 1, c1.R, c1.t, t1);
matrix_scale(3, 1, t1, -1.0, t1);
matrix_product(3, 3, 3, 1, c2.R, c2.t, t2);
matrix_scale(3, 1, t2, -1.0, t2);
pt = triangulate(p_norm, q_norm, c1.R, t1, c2.R, t2, &proj_error);
}
proj_error = (c1.f + c2.f) * 0.5 * sqrt(proj_error * 0.5);
/* Check cheirality */
bool cc1 = CheckCheirality(pt, c1);
bool cc2 = CheckCheirality(pt, c2);
in_front = (cc1 && cc2);
return pt;
}
