Ejemplo n.º 1
0
int Hungarian(){
    int ret = 0;
    mxMatch = 0;
    ms(xy,-1);
    ms(yx,-1);
    initLabels();
    Augment();
    rep(x,N) ret += cost[x][xy[x]];
    return ret;
}
Ejemplo n.º 2
0
	void MinCost<FlowType, CostType>::Dijkstra(Node* start)
{
	assert(start->excess > 0);

	Node* i;
	Node* j;
	Arc* a;
	CostType d;
	Node* permanentNodes;

	int FLAG0 = ++ counter; // permanently labeled nodes
	int FLAG1 = ++ counter; // temporarily labeled nodes

	start->parent = NULL;
	start->flag = FLAG1;
	queue.Reset();
	queue.Add(start, 0);

	permanentNodes = NULL;

	while ( (i=queue.RemoveMin(d)) )
	{
		if (i->excess < 0)
		{
			FlowType delta = Augment(start, i);
			cost += delta*(d - i->pi + start->pi);
			for (i=permanentNodes; i; i=i->next_permanent) i->pi += d;
			break;
		}

		i->pi -= d;
		i->flag = FLAG0;
		i->next_permanent = permanentNodes;
		permanentNodes = i;

		for (a=i->firstNonsaturated; a; a=a->next)
		{
			j = a->head;
			if (j->flag == FLAG0) continue;
			d = a->GetRCost();
			if (j->flag == FLAG1)
			{
				if (d >= queue.GetKey(j)) continue;
				queue.DecreaseKey(j, d);
			}
			else
			{
				queue.Add(j, d);
				j->flag = FLAG1;
			}
			j->parent = a;
		}

	}
}
Ejemplo n.º 3
0
double* CLinearSystem::Solve()
{
    printf("system to solve: \n");
    PrettyPrint();
    printf("==== solving ==== \n");
    Normalize();
    Augment();
    Reduce();
    Augment();
    PrettyPrint();
    if(m_result != NULL)
    {
        delete m_result;
    }
    m_result = new double[m_n];
    for(int n = 0; n < m_n; n++)
    {
        m_result[n] = Get(n,m_m-1);
    }
    return m_result;
}
Ejemplo n.º 4
0
int CUKF::Doit()
{
//	VehicleModel();
	//StopWatch("start");
	AddControlNoise();
	//StopWatch("AddControlNoise");
 	Prediction();
	//StopWatch("Prediction");
// 	PrintCvMat(XX, "after prediction : XX");
// 	PrintCvMat(PX, "after prediction : PX");

 	static double dtsum=0;
	dtsum += DT_CONTROLS;
 	if(dtsum >= DT_OBSERVE)
 	{
//		TRACE("xtrue : %.4f %.4f %.4f\n", xtrue->data.db[0], xtrue->data.db[1], xtrue->data.db[2]);
 		dtsum = 0;
 		GetVisibleLandmark();
		//StopWatch("GetVisibleLandmark");
 		AddObservationNoise();
		//StopWatch("AddObservationNoise");
 		KnownDataAssociation();
		//StopWatch("KnownDataAssociation");

// 		PrintCvMat(XX, "before update : XX");
// 		PrintCvMat(PX, "before update : PX");

 		Update();
		//StopWatch("Update");

// 		PrintCvMat(XX, "after update : XX");
// 		PrintCvMat(PX, "after update : PX");

		Augment();
		//StopWatch("Augment");

 		//PrintCvMat(XX, "after augment : XX");
 		//PrintCvMat(PX, "after augment : PX");
 		//TRACE("XX dim : %d %d\n", XX->rows, XX->cols);
 		//TRACE("PX dim : %d %d\n", PX->rows, PX->cols);
// 		TRACE("==== datable ====\n");
// 		for(int i=0; i<lm.size(); i++)
// 			TRACE("%d\t", daTable[i]);
// 		TRACE("=================\n");
	}

	EllipseFitting();
	//StopWatch("EllipseFitting");
	
	return 0;
}
Ejemplo n.º 5
0
void BidirectionalIBFS::IBFS() {
    auto start = Clock::now();
    m_forward_search = false;
    m_source_tree_d = 1;
    m_sink_tree_d = 0;

    IBFSInit();

    // Set up initial current_q and search nodes to make it look like
    // we just finished scanning the sink node
    NodeQueue* current_q = &(m_sink_layers[0]);
    m_search_node_iter = current_q->end();
    m_search_node_end = current_q->end();

    while (!current_q->empty()) {
        if (m_search_node_iter == m_search_node_end) {
            // Swap queues and continue
            if (m_forward_search) {
                m_source_tree_d++;
                current_q = &(m_sink_layers[m_sink_tree_d]);
            } else {
                m_sink_tree_d++;
                current_q = &(m_source_layers[m_source_tree_d]);
            }
            m_search_node_iter = current_q->begin();
            m_search_node_end = current_q->end();
            m_forward_search = !m_forward_search;
            if (!current_q->empty()) {
                Node& n = *m_search_node_iter;
                NodeId nodeIdx = n.id;
                if (m_forward_search) {
                    ASSERT(n.state == NodeState::S || n.state == NodeState::S_orphan);
                    m_search_arc = m_graph->ArcsBegin(nodeIdx);
                    m_search_arc_end = m_graph->ArcsEnd(nodeIdx);
                } else {
                    ASSERT(n.state == NodeState::T || n.state == NodeState::T_orphan);
                    m_search_arc = m_graph->ArcsBegin(nodeIdx);
                    m_search_arc_end = m_graph->ArcsEnd(nodeIdx);
                }
            }
            continue;
        }
        Node& n = *m_search_node_iter;
        NodeId search_node = n.id;
        int distance;
        if (m_forward_search) {
            distance = m_source_tree_d;
        } else {
            distance = m_sink_tree_d;
        }
        ASSERT(n.dis == distance);
        // Advance m_search_arc until we find a residual arc
        while (m_search_arc != m_search_arc_end && !m_graph->NonzeroCap(m_search_arc, m_forward_search))
            ++m_search_arc;

        if (m_search_arc != m_search_arc_end) {
            NodeId neighbor = m_search_arc.Target();
            NodeState neighbor_state = m_graph->node(neighbor).state;
            if (neighbor_state == n.state) {
                ASSERT(m_graph->node(neighbor).dis <= n.dis + 1);
                if (m_graph->node(neighbor).dis == n.dis+1) {
                    auto reverseArc = m_search_arc.Reverse();
                    if (reverseArc < m_graph->node(neighbor).parent_arc) {
                        m_graph->node(neighbor).parent_arc = reverseArc;
                        m_graph->node(neighbor).parent = search_node;
                    }
                }
                ++m_search_arc;
            } else if (neighbor_state == NodeState::N) {
                // Then we found an unlabeled node, add it to the tree
                m_graph->node(neighbor).state = n.state;
                m_graph->node(neighbor).dis = n.dis + 1;
                AddToLayer(neighbor);
                auto reverseArc = m_search_arc.Reverse();
                m_graph->node(neighbor).parent_arc = reverseArc;
                ASSERT(m_graph->NonzeroCap(m_graph->node(neighbor).parent_arc, !m_forward_search));
                m_graph->node(neighbor).parent = search_node;
                ++m_search_arc;
            } else {
                // Then we found an arc to the other tree
                ASSERT(neighbor_state != NodeState::S_orphan && neighbor_state != NodeState::T_orphan);
                ASSERT(m_graph->NonzeroCap(m_search_arc, m_forward_search));
                Augment(m_search_arc);
                Adopt();
            }
        } else {
            // No more arcs to scan from this node, so remove from queue
            AdvanceSearchNode();
        }
    } // End while
    m_totalTime += Duration{ Clock::now() - start }.count();

    //std::cout << "Total time:      " << m_totalTime << "\n";
    //std::cout << "Init time:       " << m_initTime << "\n";
    //std::cout << "Augment time:    " << m_augmentTime << "\n";
    //std::cout << "Adopt time:      " << m_adoptTime << "\n";
}
Ejemplo n.º 6
0
	int Augmentation(std::vector<int> &free_rows) {
		if(free_rows.size() < 1)
			return 1;
		for(unsigned int ind=0 ; ind<free_rows.size() ; ind++) {
			int free_row = free_rows[ind];
			double minimum_d;
			std::deque<int> ready_deque, scan_deque;
			std::vector<int> todo_list;
			ready_deque.clear(); scan_deque.clear(); todo_list.clear();
			Eigen::ArrayXi pred = Eigen::ArrayXi::Constant(n_, free_row);
			Eigen::ArrayXd d = Eigen::ArrayXd::Zero(n_);
			for(int col=1 ; col<=n_ ; col++) {
				todo_list.push_back(col);
				d(col-1) = cost_mat_(free_row-1, col-1) - v_(col-1);
			}
			while(1) {
				int done = 0;
				if(scan_deque.empty()) {
					minimum_d = inf_;
					std::vector<int> deposit;
					std::vector<int>::iterator todo_itr = todo_list.begin();
					while(todo_itr != todo_list.end()) {
						if(d((*todo_itr)-1) <= minimum_d) {
							if(d((*todo_itr)-1) < minimum_d) {
								minimum_d = d((*todo_itr)-1);
								for(unsigned int ind2=0 ; ind2<scan_deque.size() ; ind2++)
									deposit.push_back(scan_deque[ind2]);
								scan_deque.clear();
							}
							scan_deque.push_back((*todo_itr));
							todo_itr = todo_list.erase(todo_itr);
						}else{
							++todo_itr;
						}
					}
					for(unsigned int ind2=0 ; ind2<deposit.size() ; ind2++)
						todo_list.push_back(deposit[ind2]);
					deposit.clear();
					////
					for(unsigned int ind2=0 ; ind2<scan_deque.size() ; ind2++) {
						if(y_(scan_deque[ind2]-1) == 0) {
							Augment(free_row, scan_deque[ind2], ready_deque, minimum_d, d, pred);
							done = 1;
						}
						if(done) break;
					}
					if(done) break;
				}
				int scan_col = scan_deque.front();
				scan_deque.pop_front();
				int prev_row = y_(scan_col-1);
				ready_deque.push_back(scan_col);
				////
				std::vector<int>::iterator todo_itr = todo_list.begin();
				while(todo_itr != todo_list.end()) {
					int erase = 0;
					if(minimum_d + cred(prev_row, *todo_itr) < d((*todo_itr)-1)) {
						d((*todo_itr)-1) = minimum_d + cred(prev_row, *todo_itr);
						pred((*todo_itr)-1) = prev_row;
						if(d((*todo_itr)-1) == minimum_d) {
							if(y_((*todo_itr)-1) == 0) {
								Augment(free_row, (*todo_itr), ready_deque, minimum_d, d, pred);
								done = 1;
							} else {
								scan_deque.push_back(*todo_itr);
								todo_itr = todo_list.erase(todo_itr);
								erase = 1;
							}
							if(done) break;
						}
					}
					if(!erase)
						++todo_itr;
				}
				if(done) break;
			}
		}
		return 0;
	}
Ejemplo n.º 7
0
void Augment(){
    if (mxMatch == N) return;
    int x, y, root;
    int q[MX], wr = 0, rd = 0;
    ms(S,false);
    ms(T,false);
    ms(prev,-1);
    for( x=0;x<N;x++ )
        if (xy[x] == -1){
            q[wr++] = root = x;
            prev[x] = -2;
            S[x] = true;
            break;
        }
    for( y=0;y<N;y++ ){
        slack[y] = lx[root] + ly[y] - cost[root][y];
        slackX[y] = root;
    }
    while( true ){
        while (rd < wr){
            x = q[rd++];
            for( y=0;y<N;y++ ){
                if (cost[x][y] == lx[x] + ly[y] &&  !T[y]){
                    if (yx[y] == -1) break;

                    T[y] = true;
                    q[wr++] = yx[y];

                    Add_To_Tree(yx[y], x);
                }
            }
            if (y < N) break;
        }
        if (y < N) break;
        Update_Labels();
        wr = rd = 0;
        for( y=0;y<N;y++ ){
            if (!T[y] &&  slack[y] == 0){
                if (yx[y] == -1){
                    x = slackX[y];
                    break;
                }
                else{
                    T[y] = true;
                    if (!S[yx[y]]){
                        q[wr++] = yx[y];
                        Add_To_Tree(yx[y], slackX[y]);
                    }
                }
            }
        }
        if (y < N) break;
    }
    if (y < N){
        mxMatch++;
        for (int cx = x, cy = y, ty; cx != -2; cx = prev[cx], cy = ty){
            ty = xy[cx];
            yx[cy] = cx;
            xy[cx] = cy;
        }
        Augment();
    }
}