예제 #1
0
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);
}
예제 #2
0
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);
                }
            }
        }
    }
}
예제 #3
0
float jankyXboxJoystick::GetRightTwist()
{
	float rightTwist = GetTwist();
	return rightTwist;
}
예제 #4
0
float jankyXboxJoystick::GetLeftTwist()
{
	float leftTwist = GetTwist();
	return leftTwist;
}