コード例 #1
0
    void SecondOrderOptimizeFusionMove::optimize(Depth &result, const int max_iter) const {
        vector<Depth> proposals;
        genProposal(proposals);
        //proposals.push_back(noisyDisp);

        char buffer[1024] = {};

        //initialize by random
        result.initialize(width, height, -1);
	    const int nLabel = model->nLabel;

        std::default_random_engine generator;
        std::uniform_int_distribution<int> distribution(0, nLabel - 1);
        for (auto i = 0; i < width * height; ++i) {
            //result.setDepthAtInd(i, (double) distribution(generator));
            //result.setDepthAtInd(i, noisyDisp[i]);
            result[i] = 0;
        }
        sprintf(buffer, "%s/temp/init_result.jpg", file_io.getDirectory().c_str());
        result.saveImage(buffer, 256.0 / (double)nLabel);

        list<double> diffE;
        double lastEnergy = evaluateEnergy(result);
        double initialEnergy = lastEnergy;
        int iter = 0;

        const double termination = 0.1;
        float timming = (float) getTickCount();
        const int smoothInterval = 5;
        while (true) {
            if (iter == max_iter)
                break;
            cout << "======================" << endl;

            Depth newProposal;

//            if (iter > 0 && iter % smoothInterval == 0) {
//                Depth orip1;
//                newProposal.initialize(width, height, -1);
//                orip1.initialize(width, height, -1);
//                for (auto i = 0; i < width * height; ++i) {
//                    orip1.setDepthAtInd(i, result[i]);
//                    newProposal.setDepthAtInd(i, result[i]);
//                }
//                int direction = iter / smoothInterval;
//                if (direction % 2 == 0) {
//                    //horizontally
//                    for (auto y = 0; y < height; ++y) {
//                        for (auto x = 1; x < width - 1; ++x)
//                            newProposal.setDepthAtInt(x, y, (orip1(x + 1, y) + orip1(x - 1, y)) / 2);
//                        newProposal.setDepthAtInt(width - 1, y, orip1(width - 1, y));
//                    }
//                } else {
//                    //vertically
//                    for (auto x = 0; x < width; ++x) {
//                        for (auto y = 1; y < height - 1; ++y)
//                            newProposal.setDepthAtInt(x, y, (orip1(x, y + 1) + orip1(x, y - 1)) / 2);
//                        newProposal.setDepthAtInt(x, height - 1, orip1(x, height - 1));
//                    }
//                }
//                cout << "Iteration " << iter << " using smoothing proposal " << endl;
//            } else {
//          }
            newProposal = proposals[iter % (proposals.size())];
            printf("Fusing with proposal %d\n", (int)(iter % proposals.size()));
            //after several iteration, smooth the dispartiy
            fusionMove(result, newProposal);
            double e = evaluateEnergy(result);

            double energyDiff = lastEnergy - e;

            if (diffE.size() >= average_over)
                diffE.pop_front();
            diffE.push_back(energyDiff);
            double average_diffe = std::accumulate(diffE.begin(), diffE.end(), 0.0) / (double) diffE.size();

            printf("Done. Final energy: %.5f, energy decrease: %.5f average decrease: %.5f\n", e, energyDiff,
                   average_diffe);
            lastEnergy = e;

            sprintf(buffer, "%s/temp/fusionmove_iter%05d.jpg", file_io.getDirectory().c_str(), iter);
            result.saveImage(buffer, 256.0 / (double) nLabel);

            if (iter > proposals.size() * 2 && average_diffe < termination) {
                cout << "Converge!" << endl;
                break;
            }

            iter++;
        }
        timming = ((float) getTickCount() - timming) / (float) getTickFrequency();
        printf("All done. Initial energy: %.5f, final energy: %.5f, time usage: %.2fs\n", initialEnergy, lastEnergy,
               timming);
    }
コード例 #2
0
	void DynamicConfidence::run(const int anchor, Depth &confidence) {
		const int framenum = file_io.getTotalNum();
		CHECK_LT(anchor, framenum);
		const int startid = anchor - max_tWindow / 2 >= 0 ? anchor - max_tWindow / 2 : 0;
		const int endid = anchor + max_tWindow / 2 < framenum ? anchor + max_tWindow / 2 : framenum - 1;

		char buffer[1024] = {};

		vector<FlowFrame> flow_forward((size_t) endid - startid + 1);
		vector<FlowFrame> flow_backward((size_t) endid - startid + 1);
		printf("=====================\nFrame: %d\n", anchor);
		cout << "Reading optical flow..." << endl;
		for (auto i = startid; i <= endid; ++i) {
			cout << '.' << flush;
			if (i < file_io.getTotalNum() - 1)
				flow_forward[i - startid].readFlowFile(file_io.getOpticalFlow_forward(i));
			if (i > 0)
				flow_backward[i - startid].readFlowFile(file_io.getOpticalFlow_backward(i));
		}
		cout << endl;
		const int width = (int)(flow_forward[0].width() / downsample);
		const int height = (int)(flow_forward[0].height() / downsample);
		const int widthFull = flow_forward[0].width();
		const int heightFull = flow_forward[0].height();

		Depth confidence_down(width, height, -1);
		confidence.initialize(widthFull, heightFull, 0);

		const int min_interval = 0;
		const double kth_ratio = 0.8;
		const size_t min_length = 5;
		double min_depth, max_depth;
		cout << "Computing min-max depth" << endl;
		computeMinMaxDepth(anchor, min_depth, max_depth);

		const int id = anchor - startid;
		const theia::Camera& refCam = reconstruction.View(orderedId[anchor].second)->Camera();

		cout << "Computing confidence..." << endl;
		const int unit = width * height / 10;
		const int testx = 1596 / downsample;
		const int testy = 472 / downsample;

		int startx = 0, endx = width-1, starty = 0, endy = height-1;
		if(testx >=0 && testy>=0){
			printf("Debug mode: %d, %d\n", testx, testy);
			startx = testx;
			endx = testx;
			starty = testy;
			endy = testy;
		}

		double max_line_length = 100;
		for (auto y = starty; y<= endy; ++y) {
			for (auto x = startx; x <= endx; ++x) {
				if((y*width+x) % unit == 0)
					cout << '.' << flush;
				vector<double> epiErr;
				Vector2d locL((double) x, (double) y);
				Vector3d ray = refCam.PixelToUnitDepthRay(locL * downsample);
				Vector3d minpt = refCam.GetPosition() + ray * min_depth;
				Vector3d maxpt = refCam.GetPosition() + ray * max_depth;
				if(testx >= 0 && testy >= 0){
					printf("min depth: %.3f, max depth: %.3f\n", min_depth, max_depth);
				}

				for (auto i = 0; i < flow_forward.size(); ++i) {
					const theia::Camera& cam2 = reconstruction.View(orderedId[i+startid].second)->Camera();
					if(i == id){
						Mat img = imread(file_io.getImage(i+startid));
						cv::circle(img, cv::Point(locL[0], locL[1]), 2, cv::Scalar(0,0,255), 2);
						sprintf(buffer, "%s/temp/conf_ref%05d_%05d.jpg", file_io.getDirectory().c_str(), anchor, i+startid);
						imwrite(buffer, img);
						continue;
					}
					if (std::abs(id - i) < min_interval)
						continue;

					Vector2d locR;
					if (id < i) {
						if (!flow_util::trackPoint(locL * downsample, flow_forward, id, i, locR))
							continue;
					} else {
						if (!flow_util::trackPoint(locL * downsample, flow_backward, id, i, locR))
							continue;
					}

					Vector2d spt, ept;
					cam2.ProjectPoint(minpt.homogeneous(), &spt);
					cam2.ProjectPoint(maxpt.homogeneous(), &ept);
//					Vector2d dir = spt - ept;
//					dir.normalize();
//					spt = ept + dir * max_line_length;

					if(x == testx && y == testy){
						Mat img = imread(file_io.getImage(i+startid));
						cv::circle(img, cv::Point(locR[0], locR[1]), 2, cv::Scalar(0,0,255), 2);
						printf("---------------------\nFrame %d, spt:(%.2f,%.2f), ept:(%.2f,%.2f)\n", i+startid, spt[0], spt[1], ept[0], ept[1]);
						cv::line(img, cv::Point(spt[0], spt[1]), cv::Point(ept[0], ept[1]), cv::Scalar(255,0,0), 2);
						sprintf(buffer, "%s/temp/conf_ref%05d_%05d.jpg", file_io.getDirectory().c_str(), anchor, i+startid);
						imwrite(buffer, img);

						theia::Matrix3x4d pMatrix;
						cam2.GetProjectionMatrix(&pMatrix);
						cout << "Projection matrix:" << endl << pMatrix << endl;
					}

					epiErr.push_back(geometry_util::distanceToLineSegment<2>(locR, spt, ept));
				}
				if (epiErr.size() < min_length) {
					confidence(x, y) =  0.0;
					continue;
				}
				const size_t kth = (size_t) (epiErr.size() * kth_ratio);
				nth_element(epiErr.begin(), epiErr.begin() + kth, epiErr.end());
				confidence_down.setDepthAtInt(x, y, epiErr[kth]);
			}
		}
		cout << endl;

		//upsample to original resolution
		for(auto x=0; x<widthFull-downsample; ++x){
			for(auto y=0; y<heightFull-downsample; ++y)
				confidence(x,y) = confidence_down.getDepthAt(Vector2d((double)x/downsample, (double)y/downsample));
		}

		confidence.updateStatics();
	}