void LKH::LKHAlg::AllocateSegments() { Segment *S = 0, *SPrev; SSegment *SS = 0, *SSPrev; int i; FreeSegments(); #ifdef THREE_LEVEL_TREE GroupSize = pow((double) Dimension, 1.0 / 3.0); #elif defined TWO_LEVEL_TREE GroupSize = sqrt((double) Dimension); #else GroupSize = Dimension; #endif Groups = 0; for (i = Dimension, SPrev = 0; i > 0; i -= GroupSize, SPrev = S) { assert(S = (Segment *) malloc(sizeof(Segment))); S->Rank = ++Groups; if (!SPrev) FirstSegment = S; else SLink(SPrev, S); } SLink(S, FirstSegment); #ifdef THREE_LEVEL_TREE SGroupSize = sqrt((double) Groups); #else SGroupSize = Dimension; #endif SGroups = 0; for (i = Groups, SSPrev = 0; i > 0; i -= SGroupSize, SSPrev = SS) { SS = (SSegment *) malloc(sizeof(SSegment)); SS->Rank = ++SGroups; if (!SSPrev) FirstSSegment = SS; else SLink(SSPrev, SS); } SLink(SS, FirstSSegment); }
void SimpleSkeletonGrower::backward_grow() { if(_surface.is_loaded()) { std::vector <osg::Vec3> backward_grow = _surface._relative_pts; //1. Initialization: setup a grow_map for book-keeping the backward growing //Key: rank of the <x,z> coords; Value: <low depth(y), high depth(y), isDone(1 or -1)> //this is used in addition to the relative bound for growing a dense foilage std::map <int, osg::Vec3> grow_map; for(unsigned int i=0; i<_surface._surface_pts.size(); i++) { osg::Vec3 pt = _surface._surface_pts[i]; //only consider points that lay inside the boundary if(!_pruner.is_inside_ortho(pt)) continue; int rank = (pt.x()+1000)*2500 + (pt.z()+1000); //check if exists, update its value if(grow_map.find(rank) != grow_map.end()) { osg::Vec3 paras = grow_map[rank]; osg::Vec3 new_paras = paras; if(pt.y() < paras.x()) new_paras.x() = pt.y(); if(pt.y() > paras.y()) new_paras.y() = pt.y(); grow_map[rank] = new_paras; } //new cloud point else { osg::Vec3 paras(pt.y(), pt.y(), -1);//new point has the same-depth and is un-done grow_map[rank] = paras; } } float inter_cons = 2.0f; float neighbor_dist = pow(inter_cons*1.05, 0.5) * _close_threshold; float neighbor_diag = neighbor_dist * 0.707f; //2. Loop each node pt in grow_map, un-rank it, and pretend to be a point in the relative bound for(std::map <int, osg::Vec3>::iterator it=grow_map.begin(); it!=grow_map.end(); it++) { //a. recover point, the y-coord is randomly picked between low and high int rank = it->first; //osg::Vec3 paras = it->second; //int y = rand()%(int(paras.y()-paras.x())) + paras.x(); //osg::Vec3 pt(rank/2500-1000, y, rank%2500-1000); osg::Vec3 pt(rank/2500-1000, rank%2500-1000, 0.0f); backward_grow.push_back(pt); //add more neighbor points osg::Vec3 probe; if(false) { probe = osg::Vec3(pt.x()+neighbor_dist, pt.y(), 0.0f);//right if(!_pruner.is_inside_ortho(probe)) backward_grow.push_back(probe); probe = osg::Vec3(pt.x()-neighbor_dist, pt.y(), 0.0f);//left if(!_pruner.is_inside_ortho(probe)) backward_grow.push_back(probe); probe = osg::Vec3(pt.x(), pt.y()+neighbor_dist, 0.0f);//top if(!_pruner.is_inside_ortho(probe)) backward_grow.push_back(probe); probe = osg::Vec3(pt.x(), pt.y()-neighbor_dist, 0.0f);//bottom if(!_pruner.is_inside_ortho(probe)) backward_grow.push_back(probe); } probe = osg::Vec3(pt.x()+neighbor_diag, pt.y()+neighbor_diag, 0.0f);//top-right if(!_pruner.is_inside_ortho(probe)) backward_grow.push_back(probe); probe = osg::Vec3(pt.x()-neighbor_diag, pt.y()+neighbor_diag, 0.0f);//top-left if(!_pruner.is_inside_ortho(probe)) backward_grow.push_back(probe); probe = osg::Vec3(pt.x()+neighbor_diag, pt.y()-neighbor_diag, 0.0f);//bottom-right if(!_pruner.is_inside_ortho(probe)) backward_grow.push_back(probe); probe = osg::Vec3(pt.x()-neighbor_diag, pt.y()-neighbor_diag, 0.0f);//bottom-left if(!_pruner.is_inside_ortho(probe)) backward_grow.push_back(probe); } //given the non-even retaive contour points, backward grow it to the nearest tree nodes //each each node cannot grow more than twice, also the newly added retaive points are //required to be mutually separated from previously added points, in addition to the old tree nodes std::vector <LibraryElement> elements = _library._library_element; std::vector <osg::Vec3> newly_added; std::map <BDLSkeletonNode *, int> grown_cnt; //put inside the loop becasue trees is updated, put outside for non-continuous effect std::vector <BDLSkeletonNode *> trees = bfs(); BDLSkeletonNode *copied_root = BDLSkeletonNode::copy_tree(_root); for(unsigned int j=0; j<backward_grow.size(); j++) { osg::Vec3 temp = backward_grow[j]; osg::Vec3 pt(temp.x(), 0.0f, temp.y());//add depth later //b. find min projected distance from tree float min_proj_dist = -1.0f; for(unsigned int i=0; i<trees.size(); i++) { osg::Vec3 T = Transformer::toVec3(trees[i]); float proj_dist = osg::Vec2(pt.x()-T.x(), pt.z()-T.z()).length2(); if(min_proj_dist == -1.0f || proj_dist < min_proj_dist) min_proj_dist = proj_dist; } //c. find min projected distance from previously added points float min_proj_dist2 = -1.0f; for(unsigned int i=0; i<newly_added.size(); i++) { osg::Vec3 na = newly_added[i]; float proj_dist = osg::Vec2(pt.x()-na.x(), pt.z()-na.z()).length2(); if(min_proj_dist2 == -1.0f || proj_dist < min_proj_dist2) min_proj_dist2 = proj_dist; } //c. if minimum projected distance from trees T is larger than _close_threshold, then backward grow it float close_threshold2 = _close_threshold * _close_threshold; if(min_proj_dist > close_threshold2 && (min_proj_dist2 > inter_cons*close_threshold2 || newly_added.empty())) { //find the closest node N in trees T to grow BDLSkeletonNode *N = NULL; float min_tree_dist = -1.0f; for(unsigned int i=0; i<trees.size(); i++) { BDLSkeletonNode *tnode = trees[i]; //ensure N has _prev if(!tnode->_prev) continue; //ensure N has no child //if(tnode->_children.size() > 1) // continue; //check if N has been used more than twice first if(grown_cnt.find(tnode) != grown_cnt.end() && grown_cnt[tnode] > 0) continue; float cur_dist = (pt - Transformer::toVec3(tnode)).length2(); if(min_tree_dist == -1.0f || cur_dist < min_tree_dist) { min_tree_dist = cur_dist; N = tnode; } } //fuel for backward grow is used up if(!N) return; //add depth of pt = 'depth of N' +/- 'half length of N and N->_prev' float par_len = BDLSkeletonNode::dist(N, N->_prev); if(par_len > 1.0f) pt.y() = N->_sy + rand()%int(par_len) - par_len/2.0f; else pt.y() = N->_sy + (rand()%3-1)*par_len/2.0f; //update book-keeping newly_added.push_back(pt); if(grown_cnt.find(N) != grown_cnt.end()) grown_cnt[N]++; else grown_cnt[N] = 1; //shorten the N-pt segment by 10% to prevent unwanted pruning osg::Vec3 wrap = Transformer::toVec3(N); osg::Vec3 dir = pt - wrap; float desired_len = dir.length() * 0.95f; dir.normalize(); pt = wrap + dir * desired_len; //new tip node BDLSkeletonNode *new_node = new BDLSkeletonNode(pt.x(), pt.y(), pt.z()); //use advanced approach to grow it as in laser data if(true) { //before attaching, check if the divergent angle is too large float div_ang = Transformer::divergent_angle(N->_prev, N, new_node); if(div_ang > 30.0f) { //osg::Vec3 translateN = Transformer::toVec3(N->_prev) - wrap; //float len = translateN.length(); //translateN.normalize(); //translateN = wrap + translateN * len * 0.5f; //N->_sx = wrap.x(); //N->_sy = wrap.y(); //N->_sz = wrap.z(); //if(N->_prev->_prev) //{ // N = N->_prev; // div_ang = Transformer::divergent_angle(N->_prev, N, new_node); //} //search for the most similar divergent angle in the original tree BDLSkeletonNode *a, *b; if(Transformer::similar_angle_in_tree(copied_root, div_ang, a, b)) { //printf("angle(%f) div(%f)\n", angle, Transformer::divergent_angle(a->_prev, a, b)); //construct an elemnt out of a and b BDLSkeletonNode *support_tip = NULL; BDLSkeletonNode *sub_tree = Transformer::extract_subtree(a->_prev, b, support_tip); if(sub_tree && support_tip) { //should not add //new_node->_prev = N; //new_node->_prev_support = N; //N->_children.push_back(new_node); N->_prev_support = N->_prev; LibraryElement element(sub_tree, SLink(sub_tree, sub_tree->_children[0])); //rotation can't be random! int min_tran_ang = -1; float min_tran_ang_dist = -1.0f; for(int ta=0; ta<360; ta+=12) { LibraryElement element_test = element; Transformer::transform(N, ta, element_test); std::vector <BDLSkeletonNode *> compare = Transformer::terminal_nodes(element_test._root); for(unsigned int ter=0; ter<compare.size(); ter++) { float cur_dist = BDLSkeletonNode::dist(compare[ter], new_node); if(min_tran_ang_dist == -1.0f || cur_dist < min_tran_ang_dist) { min_tran_ang_dist = cur_dist; min_tran_ang = ta; } } } //printf("min_tran_ang_dist(%f)\n", min_tran_ang_dist); std::vector <BDLSkeletonNode *> subtree = replace_branch(N, element, min_tran_ang); prune(subtree); //printf("%f %f %f\n", pt.x(), pt.y(), pt.z()); //pt = Transformer::toVec3(N); //printf("%f %f %f\n", pt.x(), pt.y(), pt.z()); if(false || j==617) { Transformer::straight_up(element); char buffer[100]; sprintf(buffer, "/tmp/log_sk/log_skeleton_%d", j); BDLSkeletonNode::save_skeleton(element._root, buffer); } } else printf("SimplSkeletonGrower::backward_grow():extract_subtree: a(%p) b(%p) error\n", a, b); delete new_node; } else { //if no suitable sub-tree can be found, just don't grow the node //printf("%f %f %f\n", pt.x(), pt.y(), pt.z()); delete new_node; } } else { //directly attach it to produce a straight branch new_node->_prev = N; N->_children.push_back(new_node); //too far away, replace it //if(desired_len > _close_threshold * 2.0f) //{ // new_node->_prev_support = N; // std::vector <BDLSkeletonNode *> subtree = replace_branch(new_node, elements[rand()%elements.size()], rand()%360); // //prune(subtree); //prune leniently //} } } //visualize pt //printf("%f %f %f\n", pt.x(), pt.y(), pt.z()); } //d. continue for the next point } //end for loop BDLSkeletonNode::delete_this(copied_root); } }