C++ vcl_vector::push_back方法代码示例

bool rgrsn_ldp::local_dynamic_programming(const vnl_matrix<double> & probMap, int nNeighborBin,
                                          vcl_vector<int> & optimalBins)
{
    const int N    = probMap.rows();
    const int nBin = probMap.cols();
    
    // dynamic programming
    vnl_matrix<double> accumulatedProbMap = vnl_matrix<double>(N, nBin);
    accumulatedProbMap.fill(0.0);
    vnl_matrix<int> lookbackTable = vnl_matrix<int>(N, nBin);
    lookbackTable.fill(0);
    // copy first row
    for (int c = 0; c<probMap.cols(); c++) {
        accumulatedProbMap[0][c] = probMap[0][c];
        lookbackTable[0][c] = c;
    }
    
    for (int r = 1; r <N; r++) {
        for (int c = 0; c<probMap.cols(); c++) {
            // lookup all possible place in the window
            double max_val = -1;
            int max_index  = -1;
            for (int w = -nNeighborBin; w <= nNeighborBin; w++) {
                if (c + w <0 || c + w >= probMap.cols()) {
                    continue;
                }
                double val = probMap[r][c] + accumulatedProbMap[r-1][c+w];
                if (val > max_val) {
                    max_val = val;
                    max_index = c + w; // most probable path from the [r-1] row, in column c + w
                }
            }
            assert(max_index != -1);
            accumulatedProbMap[r][c] = max_val;
            lookbackTable[r][c]      = max_index;
        }
    }
    
    // lookback the table
    double max_prob    = -1.0;
    int max_prob_index = -1;
    for (int c = 0; c<accumulatedProbMap.cols(); c++) {
        if (accumulatedProbMap[N-1][c] > max_prob) {
            max_prob = accumulatedProbMap[N-1][c];
            max_prob_index = c;
        }
    }
    
    // back track
    optimalBins.push_back(max_prob_index);
    for (int r = N-1; r > 0; r--) {
        int bin = lookbackTable[r][optimalBins.back()];
        optimalBins.push_back(bin);
    }
    assert(optimalBins.size() == N);
    
  //  vcl_reverse(optimalBins.begin(), optimalBins.end());
    return true;
}

void VilBaplSIFT::getMatchingLocations(const vcl_vector<bapl_key_match> & matches, vcl_vector<vgl_point_2d<double> > & pts1, vcl_vector<vgl_point_2d<double> > & pts2)
{
    for (int i = 0; i<matches.size(); i++)
    {
        bapl_lowe_keypoint_sptr s1 = dynamic_cast<bapl_lowe_keypoint*>(matches[i].first.as_pointer());
        bapl_lowe_keypoint_sptr s2 = dynamic_cast<bapl_lowe_keypoint*>(matches[i].second.as_pointer());
        vgl_point_2d<double> p1(s1->location_i(), s1->location_j());
        vgl_point_2d<double> p2(s2->location_i(), s2->location_j());
        pts1.push_back(p1);
        pts2.push_back(p2);
    }
    
}

bool VxlMvlPlus::fundamental_RANSAC(vcl_vector< vgl_point_2d< double > > const & positions1,
                                    vcl_vector< vgl_point_2d< double > > const & positions2,
                                    vcl_vector< vcl_pair<int, int> > const & initialMatchedIndices,  // matched index first --> second
                                    double error_threshold,
                                    // output
                                    vcl_vector< vcl_pair<int, int> > & finalMatchedIndices,
                                    vpgl_fundamental_matrix<double> & F)
{
    bool isFind = false;
    
    // extract point pair from initial matches
    vcl_vector<vgl_point_2d<double> > pts1_matched;
    vcl_vector<vgl_point_2d<double> > pts2_matched;
    for (int i = 0; i<initialMatchedIndices.size(); i++) {
        pts1_matched.push_back(positions1[initialMatchedIndices[i].first]);
        pts2_matched.push_back(positions2[initialMatchedIndices[i].second]);
    }
    
    vcl_vector<bool> inliers;
    isFind = VxlMvlPlus::fundamental_RANSAC(pts1_matched, pts2_matched, inliers, error_threshold, F);
    assert(inliers.size() == initialMatchedIndices.size());
    
    if (isFind) {
        for (int i = 0; i<inliers.size(); i++) {
            if (inliers[i]) {
                finalMatchedIndices.push_back(initialMatchedIndices[i]);
            }
        }
    }
    
    printf("fundamental matrix RANSAC find %lu from %lu initial matchings\n", finalMatchedIndices.size(), initialMatchedIndices.size());
    return  isFind;
}

void VilBaplSIFT::getSiftDescription(const vil_image_view<vxl_byte> &image, vcl_vector<bapl_lowe_keypoint_sptr> &descriptions)
{
    assert(image.nplanes() == 1 || image.nplanes() == 3);
    
    vil_image_view<vxl_byte> grey_img;
    if (image.nplanes() == 3) {
        vil_convert_planes_to_grey(image, grey_img);
    }
    else
    {
        grey_img.deep_copy(image);
    }
    
    vcl_vector<bapl_keypoint_sptr> sift_keypoints;
    vil_image_resource_sptr image_sptr = vil_new_image_resource_of_view(grey_img);
    
    bapl_keypoint_extractor(image_sptr, sift_keypoints);
    
    for (vcl_vector<bapl_keypoint_sptr>::iterator itr = sift_keypoints.begin();
         itr != sift_keypoints.end(); itr++) {
        bapl_lowe_keypoint_sptr sift_lowe_keypoint = dynamic_cast<bapl_lowe_keypoint*>((*itr).as_pointer());
        assert(sift_lowe_keypoint);
        descriptions.push_back(sift_lowe_keypoint);
    }
}

bool VilBaplSIFT::readSIFT_keypoint(const char *file, vcl_vector< bapl_keypoint_sptr > & keypoints)
{
    FILE *pf = fopen(file, "r");
    if (!pf) {
        printf("Error: canot open %s\n", file);
        return false;
    }
    int n = 0;
    int ret = fscanf(pf, "%d ", &n);
    assert(ret == 1);
    
    for (int i = 0; i < n; i++) {
        double loc_x = 0;
        double loc_y = 0;
        double scale = 0;
        double orientation = 0;
        ret = fscanf(pf, "%lf %lf %lf %lf ", &loc_x, &loc_y, &scale, &orientation);
        assert(ret == 4);
        
        vnl_vector_fixed<double, 128> desc;
        desc.fill(0);
        
        bapl_lowe_pyramid_set_sptr py;
        bapl_lowe_keypoint_sptr kp = new bapl_lowe_keypoint(py, loc_x, loc_y, scale, orientation, desc);
        keypoints.push_back(kp);
    }
    fclose(pf);
    printf("read %d keypoints.\n", n);
    return true;
}

void VilBaplSIFT::getSiftPositions(const vil_image_view<vxl_byte> &image, vcl_vector<vgl_point_2d<double> > &points, double curve_ratio)
{
    assert(image.nplanes() == 1 || image.nplanes() == 3);
    assert(points.size() == 0);
    
    vil_image_view<vxl_byte> grey_img;
    if (image.nplanes() == 3) {
        vil_convert_planes_to_grey(image, grey_img);
    }
    else
    {
        grey_img.deep_copy(image);
    }
    
    vcl_vector<bapl_keypoint_sptr> sift_keypoints;
    vil_image_resource_sptr image_sptr = vil_new_image_resource_of_view(grey_img);
    
    //  float curve_ratio = 2.0f;
    bapl_keypoint_extractor(image_sptr, sift_keypoints, curve_ratio);
    
    
    vcl_vector<bapl_keypoint_sptr>::iterator keypoint_itr, keypoint_end;
    keypoint_end = sift_keypoints.end();
    
    
    for (keypoint_itr = sift_keypoints.begin(); keypoint_itr != keypoint_end; ++keypoint_itr)
    {
        bapl_lowe_keypoint_sptr sift_lowe_keypoint = dynamic_cast<bapl_lowe_keypoint*>((*keypoint_itr).as_pointer());
        double x = sift_lowe_keypoint->location_i();
        double y = sift_lowe_keypoint->location_j();
        
        points.push_back(vgl_point_2d<double>(x, y));
    }
}

void VilBaplSIFT::getSIFTLocations(const vcl_vector<bapl_keypoint_sptr> & keypoints, vcl_vector<vgl_point_2d<double> > & pts)
{
    for (int i = 0; i<keypoints.size(); i++) {
        bapl_lowe_keypoint_sptr sift = dynamic_cast<bapl_lowe_keypoint*>(keypoints[i].as_pointer());
        vgl_point_2d<double> p(sift->location_i(), sift->location_j());
        pts.push_back(p);
    }
}

bool VxlMvlPlus::three_view_six_points_calib(const vcl_vector< vcl_vector<vgl_point_2d<double> > > & pointsVec,
                                             vcl_vector< vnl_matrix_fixed<double, 3, 4> > & Pmatrix,
                                             vgl_homg_point_3d<double> & Q)
{
    assert(pointsVec.size() == 3);
    for (int i = 0; i<3; i++) {
        assert(pointsVec[i].size() == 6);
    }
    
    mvl_three_view_six_point_structure mvl36;
    
    // set 6 (x, y)
    for (int i = 0; i<3; i++) {
        for (int j = 0; j<6; j++) {
            mvl36.u(i, j) = pointsVec[i][j].x();
            mvl36.v(i, j) = pointsVec[i][j].y();
        }
    }
    
    bool isFind = mvl36.compute();
    if (!isFind) {
        return false;
    }
    
    int validNum = 0;
    mvl_three_view_six_point_structure::solution_t slu; // only one solution can be used
    for (int i = 0; i<3; i++) {
        validNum += (mvl36.solution[i].valid == true ? 1:0);
        if (mvl36.solution[i].valid) {
            slu = mvl36.solution[i];
        }
    }
    
    // no ambiguity
    if (validNum != 1) {
        printf("%d ambituity solutions\n", validNum);
        for (int i = 0; i<3; i++) {
            if (mvl36.solution[i].valid)
            {
                for (int j = 0; j<3; j++) {
                    vcl_cout<<"P is: \n"<<mvl36.solution[i].P[j]<<vcl_endl;
                }
            }
            
            vcl_cout<<"Q is "<<vgl_point_3d<double>(mvl36.solution[i].Q[0], mvl36.solution[i].Q[1], mvl36.solution[i].Q[2])<<vcl_endl;
        }
        return false;
    }
    for (int i = 0; i<3; i++) {
        vcl_cout<<"P is: \n"<<slu.P[i]<<vcl_endl;
        Pmatrix.push_back(slu.P[i]);
    }
    vcl_cout<<"Q is: \n"<<slu.Q<<vcl_endl;
    Q = vgl_homg_point_3d<double>(slu.Q[0], slu.Q[1], slu.Q[2], slu.Q[3]);    
    return true;
}

// Creates a perspective camera looking at pt, and adds the camera and
// the projection of GOAL to the list.
static void add_pt_and_cam(
    vgl_homg_point_3d<double> pt,
    vgl_vector_3d<double> trans,
    vcl_vector<vgl_point_2d<double> > &points,
    vcl_vector<vpgl_perspective_camera<double> > &cameras)
{
    vpgl_perspective_camera<double> cam;
    cam.look_at(pt);
    cam.set_translation(trans);

    cameras.push_back(cam);

    double x,y;
    cam.project(GOAL.x(), GOAL.y(), GOAL.z(), x, y);
    
    std::cout << "x: " << x << " y: " << y << std::endl;

    points.push_back(vgl_point_2d<double>(x, y));
}

int VglPlus::sampleElliseInImage(const vgl_ellipse_2d<double> & ellipse, int sampleNum,
                                 int imageW, int imageH, vcl_vector<vgl_point_2d<double> > & pts)
{
    vgl_polygon<double> poly = ellipse.polygon(sampleNum);
    assert(poly.num_sheets() == 1);
    const vcl_vector< vgl_point_2d< double > > sheet = poly[0];
    for (int i = 0; i<sheet.size(); i++) {
        if (VglPlus::vgl_inside_image(sheet[i], imageW, imageH)) {
            pts.push_back(sheet[i]);
        }
    }
    return (int)pts.size();
}

void VilBaplSIFT::getSiftFromDesignatedPositions(const vil_image_view<vxl_byte> & image, vcl_vector<vnl_vector_fixed<double, 4> > & loc_sca_ori,
                                             vcl_vector<bapl_keypoint_sptr> & sifts)
{
   // assert(image.nplanes() == 3);
    assert(sifts.size() == 0);
    
    vil_image_view<vxl_byte> source_grey;
    if (image.nplanes() == 3) {
        vil_convert_planes_to_grey(image, source_grey);
    }
    else
    {
        source_grey = image;
    }
    
    bapl_lowe_pyramid_set_sptr source_pyramid_set = new bapl_lowe_pyramid_set(vil_new_image_resource_of_view(source_grey));
    
    for (int i = 0; i<loc_sca_ori.size(); i++) {
        bapl_lowe_keypoint *pKeypoint = new bapl_lowe_keypoint(source_pyramid_set, loc_sca_ori[i][0], loc_sca_ori[i][1], loc_sca_ori[i][2], loc_sca_ori[i][3]);
        sifts.push_back(bapl_keypoint_sptr(pKeypoint));
    }
    assert(loc_sca_ori.size() == sifts.size());
}

bool rgrsn_ldp::local_dynamic_programming_log(const vnl_matrix<double> & probMap, int nNeighborBin,
                                              vcl_vector<int> & optimalBins)
{
    // find minimum path
    const int N    = probMap.rows();
    const int nBin = probMap.cols();
    const double epsilon = 0.01;
    
    vnl_matrix<double> negLogProbMap(N, nBin);
    for (int r = 0; r<N; r++) {
        for (int c = 0; c <nBin; c++) {
            negLogProbMap(r, c) = -log(probMap(r, c) + epsilon);
        }
    }
    
    // dynamic programming
    vnl_matrix<double> accumulatedMap = vnl_matrix<double>(N, nBin);
    accumulatedMap.fill(0.0);
    vnl_matrix<int> lookbackTable = vnl_matrix<int>(N, nBin);
    lookbackTable.fill(0);
    // copy first row
    for (int c = 0; c<negLogProbMap.cols(); c++) {
        accumulatedMap[0][c] = negLogProbMap[0][c];
    }
    
    for (int r = 1; r <N; r++) {
        for (int c = 0; c<negLogProbMap.cols(); c++) {
            // lookup all possible place in the window
            double min_val = INT_MAX;
            int index      = -1;
            for (int w = -nNeighborBin; w <= nNeighborBin; w++) {
                if (c + w <0 || c + w >= negLogProbMap.cols()) {
                    continue;
                }
                double val = negLogProbMap[r][c] + accumulatedMap[r-1][c+w];
                if (val < min_val) {
                    min_val = val;
                    index = c + w;
                }
            }
            assert(index != -1);
            accumulatedMap[r][c] = min_val;
            lookbackTable[r][c]  = index;
        }
    }
    
    // lookback the table
    double min_val = INT_MAX;
    int initIndex  = -1;
    for (int c = 0; c<accumulatedMap.cols(); c++) {
        if (accumulatedMap[N-1][c] < min_val) {
            min_val = accumulatedMap[N-1][c];
            initIndex = c;
        }
    }
    
    // back track
    optimalBins.push_back(initIndex);
    for (int r = N-1; r > 0; r--) {
        int bin = lookbackTable[r][optimalBins.back()];
        optimalBins.push_back(bin);
    }
    assert(optimalBins.size() == N);
    
    vcl_reverse(optimalBins.begin(), optimalBins.end());
    return true;
}

bool rgrsn_ldp::viterbi(const vnl_matrix<double> & prob_map, const vnl_vector<double> & transition,
                        vcl_vector<int> & optimal_bins)
{
    
    const int N    = prob_map.rows();
    const int nBin = prob_map.cols();
    const int nNeighborBin = transition.size()/2;
    const double epsilon = 0.01;
    
    // dynamic programming
    vnl_matrix<double> log_accumulatedProbMap = vnl_matrix<double>(N, nBin);
    log_accumulatedProbMap.fill(0.0);
    vnl_matrix<int> lookbackTable = vnl_matrix<int>(N, nBin);
    lookbackTable.fill(0);
    // copy first row
    for (int c = 0; c<prob_map.cols(); c++) {
        log_accumulatedProbMap[0][c] = log(prob_map[0][c] + epsilon);
        lookbackTable[0][c] = c;
    }
    vnl_vector<double> log_transition = vnl_vector<double>(transition.size(), 0);
    
    for (int i = 0; i<transition.size(); i++) {
        log_transition[i] = log(transition[i] + epsilon);
    }
    
    for (int r = 1; r <N; r++) {
        for (int c = 0; c<prob_map.cols(); c++) {
            // lookup all possible place in the window
            double max_val = vcl_numeric_limits<int>::min();
            int max_index  = -1;
            for (int w = -nNeighborBin; w <= nNeighborBin; w++) {
                if (c + w < 0 || c + w >= prob_map.cols()) {
                    continue;
                }
                assert(w + nNeighborBin >= 0 && w + nNeighborBin < transition.size());
                double val = log_accumulatedProbMap[r-1][c+w] + log_transition[w + nNeighborBin];
                if (val > max_val) {
                    max_val = val;
                    max_index = c + w; // most probable path from the [r-1] row, in column c + w
                }
            }
            assert(max_index != -1);
            log_accumulatedProbMap[r][c] = max_val + log(prob_map[r][c] + epsilon);
            lookbackTable[r][c]          = max_index;
        }
    }
    
    // lookback the table
    double max_prob    = vcl_numeric_limits<int>::min();
    int max_prob_index = -1;
    for (int c = 0; c<log_accumulatedProbMap.cols(); c++) {
        if (log_accumulatedProbMap[N-1][c] > max_prob) {
            max_prob = log_accumulatedProbMap[N-1][c];
            max_prob_index = c;
        }
    }
    
    // back track
    optimal_bins.push_back(max_prob_index);
    for (int r = N-1; r > 0; r--) {
        int bin = lookbackTable[r][optimal_bins.back()];
        optimal_bins.push_back(bin);
    }
    assert(optimal_bins.size() == N);
    vcl_reverse(optimal_bins.begin(), optimal_bins.end());
    return true;
}

bool VglPlus::mergeEllipse(const vcl_vector<vgl_ellipse_2d<double> > & ellipses, const vcl_vector< vcl_vector<vgl_point_2d<double> > > & ellipse_pts,
                           const double distance_threshold,
                           vcl_vector<vgl_ellipse_2d<double> > & merged_ellipse, vcl_vector<vcl_vector<vgl_point_2d<double> > > & merged_points)
{
    assert(ellipses.size() == ellipse_pts.size());
    
    const double inlier_ratio_threshold = 0.5; //
    
    // order ellipse by pixels numbers
    vcl_vector<EllipsePoints> conics;
    for (int i = 0; i<ellipses.size(); i++) {
        EllipsePoints elli;
        elli.pts_ = ellipse_pts[i];
        elli.valid_ = true;
        elli.conic_ = vgl_conic<double>(ellipses[i]);
        conics.push_back(elli);
    }
    vcl_sort(conics.begin(), conics.end(), vcl_greater<EllipsePoints>());
    
    // greedily merge
    for (int i = 0; i<conics.size(); i++) {
        for (int j = i+1; j<conics.size(); j++) {
            // merge j to i
            if (conics[i].valid_ && conics[j].valid_) {
                vgl_conic<double> cur_conic = conics[i].conic_;
                vgl_box_2d<double> cur_box  = vgl_homg_operators_2d<double>::compute_bounding_box(cur_conic);
                cur_box.expand_about_centroid(10); // expand bounding box.
                vcl_vector<vgl_point_2d<double> > cur_pts = conics[j].pts_;
                vcl_vector<vgl_point_2d<double> > inlier_pts;
                for (int k = 0; k<cur_pts.size(); k++) {
                    vgl_point_2d<double> p = cur_pts[k];
                    if (cur_box.contains(p)) {
                        double dis = vgl_homg_operators_2d<double>::distance_squared(cur_conic, vgl_homg_point_2d<double>(p));
                        if (dis < distance_threshold * distance_threshold) {
                            inlier_pts.push_back(p);
                        }
                    }
                }
                // merge j to i
                if (1.0 * inlier_pts.size()/cur_pts.size() > inlier_ratio_threshold) {
                    conics[i].pts_.insert(conics[i].pts_.end(), inlier_pts.begin(), inlier_pts.end());
                    conics[j].valid_ = false;
                    // update conic equation
                    vgl_ellipse_2d<double> elli = vgl_fit_ellipse_2d_DLT(conics[i].pts_);
                    conics[i].conic_ = vgl_conic<double>(elli);
                }
            }
        }
    }
    
    vcl_sort(conics.begin(), conics.end(), vcl_greater<EllipsePoints>());
    
    // return valid conic
    for (int i = 0; i<conics.size(); i++) {
        if (conics[i].valid_) {
            merged_ellipse.push_back(vgl_ellipse_2d<double>(conics[i].conic_));
            merged_points.push_back(conics[i].pts_);
        }
    }
    return true;
}

bool VglPlus::mergeLinesegments(const vcl_vector<vgl_line_segment_2d<double> > & segments,
                                const vcl_vector<vcl_vector<vgl_point_2d<double> > > & segment_pts,
                                const double distance_threshold,
                                vcl_vector<vgl_line_segment_2d<double> > & merged_segments,
                                vcl_vector<vcl_vector<vgl_point_2d<double> > > & merged_pts)
{
    assert(segments.size() == segment_pts.size());
    
    const double inlier_ratio_threshold = 0.7; //
    vcl_vector<LinePoints> line_pts;
    for (int i = 0; i<segments.size(); i++) {
        vgl_line_2d<double> line(segments[i].point1(), segments[i].point2());
        LinePoints lp;
        lp.line_ = line;
        lp.pts_ = segment_pts[i];
        lp.seg_ = segments[i];
        line_pts.push_back(lp);
    }
    
    vcl_sort(line_pts.begin(), line_pts.end(), vcl_greater<LinePoints>::greater());
    
    // greedy mergy
    for (int i = 0; i<line_pts.size(); i++) {
        for (int j = i+1; j<line_pts.size(); j++) {
            if (line_pts[i].valid_ && line_pts[j].valid_) {
                vgl_line_2d<double> cur_line = line_pts[i].line_;
                vcl_vector<vgl_point_2d<double> > cur_pts = line_pts[j].pts_; // points to be merged
                vcl_vector<vgl_point_2d<double> > inlier_pts;
                for (int k = 0; k<cur_pts.size(); k++) {
                    double dis = vgl_distance(cur_pts[k], cur_line);
                    if (dis < distance_threshold) {
                        inlier_pts.push_back(cur_pts[k]);
                    }
                }
                if (1.0 * inlier_pts.size()/cur_pts.size() > inlier_ratio_threshold) {
                    vcl_vector<vgl_point_2d<double> > all_pts;
                    all_pts.insert(all_pts.end(), inlier_pts.begin(), inlier_pts.end());
                    all_pts.insert(all_pts.end(), line_pts[i].pts_.begin(), line_pts[i].pts_.end());
                    
                    vgl_line_2d<double> estimated_line = vgl_fit_line_2d(line_pts[i].pts_);
                    // project the two line segment to the line and merge
                    vgl_point_2d<double> p1 = vgl_closest_point(estimated_line, line_pts[i].seg_.point1());
                    vgl_point_2d<double> p2 = vgl_closest_point(estimated_line, line_pts[i].seg_.point2());
                    vgl_point_2d<double> p3 = vgl_closest_point(estimated_line, line_pts[j].seg_.point1());
                    vgl_point_2d<double> p4 = vgl_closest_point(estimated_line, line_pts[j].seg_.point2());
                    vgl_line_segment_2d<double> merged_seg;
                    bool isMerged = VglPlus::mergeTwolineSegmentOnALine(vgl_line_segment_2d<double>(p1, p2),
                                                                        vgl_line_segment_2d<double>(p3, p4),
                                                                        merged_seg);
                    if (isMerged) {
                        line_pts[i].pts_.insert(line_pts[i].pts_.end(), inlier_pts.begin(), inlier_pts.end());
                        line_pts[i].line_ = estimated_line;
                        line_pts[i].seg_  = merged_seg;
                        line_pts[j].valid_ = false;
                    }
                }
            }
        }
    }
    
    vcl_sort(line_pts.begin(), line_pts.end(), vcl_greater<LinePoints>::greater());
    
    // output
    for (int i = 0; i<line_pts.size(); i++) {
        if (line_pts[i].valid_) {
            merged_segments.push_back(line_pts[i].seg_);
            merged_pts.push_back(line_pts[i].pts_);
        }
    }
    return true;
}