void GraphNodeCtr::add_node(IndexType frame, IndexType label, IndexType index)
{
GraphCutNode *new_space = allocator_.allocate<GraphCutNode>();
GraphCutNode *new_node = new(new_space) GraphCutNode(frame,label,index,cur_graph_index_++);
node_vec.push_back(new_node);
node_map[frame_index_to_key(frame,index)] = new_node;
}
void MultiObjectSeg::addGraphEdge(PCloudGraph& pcGraph, Sample& smp)
{
IndexType nSize = smp.num_vertices();
unordered_map<IndexType,bool> recordEdges;
buildKdTree(smp);
IndexType gIndex = 0;
const IndexType k = 6;
IndexType neighbours[k];
ScalarType dist[k];
IndexType edNum = 0;
for (; gIndex < nSize; ++ gIndex)
{
downSample->neighbours(gIndex,k,neighbours,dist);
for (IndexType i = 1; i < k; ++i)
{
bool temp = recordEdges[frame_index_to_key(gIndex,neighbours[i]) ];
if (!temp)
{
PCEdgeProperty ep;
ep.index = edNum;
ep.start_ = gIndex;
ep.end_ = neighbours[i];
ep.dist = dist[i];
boost::add_edge(gIndex,neighbours[i],ep,pcGraph);
recordEdges[frame_index_to_key(neighbours[i],gIndex)] = true;
++ edNum;
}
}
}
}
ScalarType GraphNodeCtr::dist_inside_frame_all(GraphCutNode* s_node,GraphCutNode* e_node)
{
if (s_node->frame != e_node->frame || s_node->label != e_node->label)
{
return 1e8;
}else//identity frame and label
{
ScalarType diag = 1.0;// diagonal of the box(Index by frame and label)
PointType start,end;//index
start = m_smpSet[s_node->frame].vertices_matrix().col(s_node->index);
end = m_smpSet[e_node->frame].vertices_matrix().col(e_node->index);
//correspodence of start and end point are in the same label?2014-11-28
map<IndexType,IndexType>::iterator start_isInValidIter,end_IsValidIter;
PCABox* box = box_bucket[frame_label_to_key(s_node->frame,s_node->label) ];//calculate diag of box
diag = box->diagLen;
ScalarType dis = 0.0;
bool flagDiff = false;
IndexType labelsize = box->vtxSize;
assert(labelsize>0);
IndexType nDiff = 0;
IndexType nIden = 0;
for(auto iter = s_node->cor_frame_index.begin(); iter!= s_node->cor_frame_index.end(); iter++)
{
start_isInValidIter = s_node->cor_frame_index.find(iter->first);
end_IsValidIter = e_node->cor_frame_index.find(iter->first);
GraphCutNode *s_cor = node_map[frame_index_to_key(start_isInValidIter->first,start_isInValidIter->second)];
GraphCutNode *e_cor = node_map[frame_index_to_key(end_IsValidIter->first,end_IsValidIter->second)];
dis = (start- end).norm();
if(s_cor->label != e_cor->label)//in different label
{
//return 3 *(dis/diag);
nDiff ++;
//flagDiff = true;
}else
{
//return dis/(diag);
nIden ++;
}
}
if(flagDiff || (nDiff > nIden))
{
return 3 *(dis/diag);
}
return dis/(diag);
///return 4 *(dis/(diag*labelsize));
//return dis/(diag*labelsize);
}
}
ScalarType GraphNodeCtr::dist_inside_frame(GraphCutNode* s_node,GraphCutNode* e_node)
{
if (s_node->frame != e_node->frame || s_node->label != e_node->label)
{
return 1e8;
}
Sample &smp = SampleSet::get_instance()[s_node->frame];
const IndexType k = 100;
IndexType neighbours[k];
ScalarType dist[k];
bool is_neighbour=false;
smp.neighbours(s_node->index, k, neighbours, dist);
for ( int i=0; i<k; i++ )
{
if(neighbours[i]==e_node->index)
{
is_neighbour = true;
break;
}
}
if (is_neighbour==false)
{
return 1e8;
}
{
ScalarType diag = 1.0;// diagonal of the box(Index by frame and label)
PointType start,end;//index
start = m_smpSet[s_node->frame].vertices_matrix().col(s_node->index);
end = m_smpSet[e_node->frame].vertices_matrix().col(e_node->index);
map<IndexType,IndexType>::iterator start_isInValidIter,end_IsValidIter;
PCABox* box = box_bucket[frame_label_to_key(s_node->frame,s_node->label) ];//calculate diag of box
ScalarType dis = (start- end).norm();
diag = box->diagLen;
IndexType labelsize = box->vtxSize;
assert(labelsize>0);
for(auto iter = s_node->cor_frame_index.begin(); iter!= s_node->cor_frame_index.end(); iter++)
{
start_isInValidIter = s_node->cor_frame_index.find(iter->first);
end_IsValidIter = e_node->cor_frame_index.find(iter->first);
GraphCutNode *s_cor = node_map[frame_index_to_key(start_isInValidIter->first,start_isInValidIter->second)];
GraphCutNode *e_cor = node_map[frame_index_to_key(end_IsValidIter->first,end_IsValidIter->second)];
ScalarType dis = (start- end).norm();
if(s_cor->label != e_cor->label)//in different label
{
return 8*(dis/(diag*labelsize));
}else
{
return dis/(diag * labelsize);
}
}
}
}
void GraphNodeCtr::add_corresponding_relation( IndexType frame, IndexType index, IndexType cor_frame, IndexType cor_idx )
{
node_map[frame_index_to_key(frame,index)]->cor_frame_index.insert(make_pair(cor_frame,cor_idx));
}
