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);
}
void ThresholdTwists(int num_images, ModelMap &models,
std::vector<ImageData> &image_data, bool panos_only)
{
int *num_large_twists = new int[num_images];
int *degree = new int[num_images];
for (int i = 0; i < num_images; i++) {
num_large_twists[i] = 0;
degree[i] = 0;
}
for (int i = 0; i < num_images; i++) {
ModelTable::iterator iter;
for (iter = models.Begin(i); iter != models.End(i); iter++) {
unsigned int j = iter->first; // iter->m_index;
if (i >= j)
continue;
MatchIndex idx = GetMatchIndex(i, j);
if (models.Contains(idx)) {
TwoFrameModel &m = models.GetModel(idx);
/* Compute the twist */
double Rp_i[9], Rp_j[9];
matrix_transpose(3, 3, m.m_camera0.R, Rp_i);
matrix_transpose(3, 3, m.m_camera1.R, Rp_j);
double Rp_ij[9];
matrix_transpose_product(3, 3, 3, 3, Rp_i, Rp_j, Rp_ij);
double twist_angle = GetTwist(Rp_ij);
if (fabs(RAD2DEG(twist_angle)) >= 12.0) {
num_large_twists[i]++;
num_large_twists[j]++;
}
degree[i]++;
degree[j]++;
}
}
}
for (int i = 0; i < num_images; i++) {
if (degree[i] == 0)
continue;
double perc_large_twists = (double) num_large_twists[i] / degree[i];
int w = image_data[i].GetWidth();
int h = image_data[i].GetHeight();
double ratio = (double) w / h;
if ((panos_only || perc_large_twists < 0.4) &&
ratio > 0.4 && ratio < 2.5) {
continue;
}
printf("[ThresholdTwists] Removing image %d with score %0.3f, %0.3f\n",
i, perc_large_twists, ratio);
std::list<unsigned int> nbrs = models.GetNeighbors(i);
std::list<unsigned int>::iterator iter;
for (iter = nbrs.begin(); iter != nbrs.end(); iter++) {
unsigned int j = *iter; // iter->m_index;
if (i < j) {
MatchIndex idx = GetMatchIndex(i, j);
if (models.Contains(idx)) {
models.RemoveModel(idx);
}
} else {
MatchIndex idx = GetMatchIndex(j, i);
if (models.Contains(idx)) {
models.RemoveModel(idx);
}
}
}
}
}
float jankyXboxJoystick::GetRightTwist()
{
float rightTwist = GetTwist();
return rightTwist;
}
float jankyXboxJoystick::GetLeftTwist()
{
float leftTwist = GetTwist();
return leftTwist;
}