virtual void search_knn(const float_type* qus, unsigned N, unsigned K, unsigned* argmins, accum_float_type* mins) const { std::vector< std::pair<unsigned, accum_float_type> > nns(K); for (unsigned n=0; n < N; ++n) { kdt_.search(qus + n*ndims_, dist_, K, &nns[0], nchecks_); for (unsigned k=0; k < K; ++k) { argmins[n*K + k] = nns[k].first; mins[n*K + k] = nns[k].second; } } }
HypothesesGraph* SingleTimestepTraxel_HypothesesBuilder::add_edges_at(HypothesesGraph* graph, int timestep) const { const HypothesesGraph::node_timestep_map& timemap = graph->get( node_timestep()); typedef property_map<node_traxel, HypothesesGraph::base_graph>::type traxelmap_t; const traxelmap_t& traxelmap = graph->get(node_traxel()); const TraxelStoreByTimeid& traxels_by_timeid = ts_->get<by_timeid>(); const TraxelStoreByTimestep& traxels_by_timestep = ts_->get<by_timestep>(); //// find k nearest neighbors in next timestep // init nearest neighbor search pair<TraxelStoreByTimestep::const_iterator, TraxelStoreByTimestep::const_iterator> traxels_at = traxels_by_timestep.equal_range(timestep + 1); for (TraxelStoreByTimestep::const_iterator it = traxels_at.first; it != traxels_at.second; ++it) { assert(it->Timestep == (timestep+1)); } NearestNeighborSearch nns(traxels_at.first, traxels_at.second); // establish transition edges between a current node and appropriate nodes in next timestep for (HypothesesGraph::node_timestep_map::ItemIt curr_node(timemap, timestep); curr_node != lemon::INVALID; ++curr_node) { assert(timemap[curr_node] == timestep); assert(traxelmap[curr_node].Timestep == timestep); // search map<unsigned int, double> nearest_neighbors = nns.knn_in_range( traxelmap[curr_node], options_.distance_threshold, options_.max_nearest_neighbors); //// connect current node with k nearest neighbor nodes for (map<unsigned int, double>::const_iterator neighbor = nearest_neighbors.begin(); neighbor != nearest_neighbors.end(); ++neighbor) { // connect with one of the neighbor nodes TraxelStoreByTimeid::iterator neighbor_traxel = traxels_by_timeid.find( boost::make_tuple(timestep + 1, neighbor->first)); assert(neighbor_traxel->Timestep == (timestep + 1)); assert(neighbor_traxel->Timestep != traxelmap[curr_node].Timestep); traxelmap_t::ItemIt neighbor_node(traxelmap, *neighbor_traxel); assert(curr_node != neighbor_node); graph->addArc(curr_node, neighbor_node); } } return graph; }
int ri_inter (void) { /* This is the shortest version of the interpreter, containing only those RASL operators which * are produced by the REFAL compiler. */ /* July, 27, 1985. D.T. */ /* Some macros have been expanded because the PC compiler can't handle too many macros. (its stack overflows.) * DT July 1 1986. */ /* Some other macros have been replaced by functions to reduce the size of object module. March 7 1987. DT. */ register short n; int error; char c, ins = 0; long mdig, bifnum; char *arg; /* short m;*/ error = 0; restart: while (error == 0) { ins = *p; # if MDEBUG if (dump_toggle) printf ("%lx: %d\n", p, ins); # endif switch (ins) { case ACT1: ASGN_CHARP (++p, arg); p += sizeof (char *); act1 (arg); curk ++; break; case BL: p++; bl; break; case BLR: p++; ri_blr (); break; case BR: p++; br; break; case CL: p++; cl; break; case SYM: c = (unsigned char) * ++p; p++; sym (c); break; case SYMR: c = (unsigned char) * ++p; p++; symr (c); break; case EMP: p++; emp; break; case EST: curk --; est; p = break0; break; case MULE: /* n = * ++p; ++p; mule ((int) n); */ ASGN_LONG (++p, mdig); p += sizeof (long); mule (mdig); break; case MULS: /*n = * ++p; muls (n); p++; break;*/ ASGN_LONG (++p, mdig); p += sizeof (long); muls (mdig); break; case PLEN: p++; plen; p++; break; case PLENS: p++; plens; break; case PLENP: p++; plenp; break; case PS: ++p; ps; break; case PSR: ++p; psr; break; case OEXP: n = (unsigned char) * ++p; ++p; oexp (n); break; case OEXPR: n = (unsigned char) * ++p; ++p; oexpr (n); break; case OVSYM: n = (unsigned char) * ++p; ovsym (n); ++p; break; case OVSYMR: n = (unsigned char) * ++p; ovsymr (n); p++; break; case TERM: p++; term; break; case TERMR: p++; termr; break; case RDY: n = (unsigned char) * ++p; ++p; rdy (n); break; case SETB: /* n = (unsigned char) * ++p; m = (unsigned char) * ++p; ++p; setb (n,m); break; */ { long l_n, l_m; ASGN_LONG (++p, l_n); p += sizeof (long); ASGN_LONG (p, l_m); p += sizeof (long); setb (l_n, l_m); } break; case LEN: p++; len; break; case LENS: c = (unsigned char) *++p; p++; lens (c); break; case LENP: ++p; lenp; break; case SYMS: n = (unsigned char) * ++p; p++; syms (n); break; case SYMSR: n = (unsigned char) * ++p; p++; symsr (n) break; case TEXT: n = (unsigned char) * ++p; p++; text (n); break; case NS: c = (unsigned char) * ++p; ++p; ns (c); break; case TPLE: /* n = * ++p; p++; tple (n); break; */ ASGN_LONG (++p, mdig); p += sizeof (long); tple (mdig); break; case TPLS: /* n = * ++p; p++; tpls (n); break; */ ASGN_LONG (++p, mdig); p += sizeof (long); tpls (mdig); break; case TRAN: ASGN_CHARP (++p, arg); p += sizeof (char *); tran (arg); break; case VSYM: p++; vsym; break; case VSYMR: ++p; vsymr; break; case OUTEST: curk --; out (2); est; p = break0; break; case ECOND: if (tel - te + nel + 100 >= size_table_element) { if (fp_debugInfo != NULL) ri_print_error_code(fp_debugInfo,13); ri_error (13); } ASGN_CHARP (++p, arg); b = st[sp].b1; b = st[sp].b1; act1 (arg); tel += (teoff = nel); est; p = break0; break; case POPVF: ++p; tel -= teoff; nel = teoff + 3; sp = stoff-1; teoff = st[sp].nel; stoff = (long) st[sp].b2; break; case PUSHVF: if (sp + 20 >= size_local_stack) { if (fp_debugInfo != NULL) ri_print_error_code(fp_debugInfo,14); ri_error (14); } ++p; b = tbel (2) -> prec; blr; pushst (b->prec,b,NULL,NULL); sp++; pushst (b,stoff,teoff, IMP_); b = b -> prec; stoff = sp + 1; break; case STLEN: ++p; sp = stoff; break; case CSYM: ASGN_CHARP (++p, arg); p += sizeof (char *); csym (arg); break; case CSYMR: ASGN_CHARP (++p, arg); p += sizeof (char *); csymr (arg); break; case NSYM: ASGN_LONG (++p, mdig); p += sizeof (long); nsym (mdig); break; case NSYMR: ASGN_LONG (++p, mdig); p += sizeof (long); nsymr (mdig); break; case NCS: ASGN_CHARP (++p, arg); p += sizeof (char *); ncs (arg); break; case NNS: ASGN_LONG (++p, mdig); p += sizeof (long); nns (mdig); break; /* builtin functions: R.N. - 20 Jul 85 */ case BUILT_IN: /* a call to a built in function no arguments. */ curk --; ASGN_LONG (p+1, bifnum); error = ri_bif (bifnum,NULL); p = break0; break; /* builtin functions with one argument: D.T. - July 27, 1985. */ case BUILT_IN1: /* a call to a function with one argument. */ /* Arguments are stored before function address. */ curk --; ASGN_CHARP(++p, arg); ASGN_LONG (p + (sizeof (char *)), bifnum); error = ri_bif (bifnum, arg); p = break0; break; default: ri_default (ins, &error); break; } } if (error != 0) { fprintf (stderr,"RASL instruction: %4d at address: %lx\n", *p, (unsigned long) p); if (fp_debugInfo != NULL) fprintf (fp_debugInfo,"RASL instruction: %4d at address: %lx\n", *p, (unsigned long) p); ri_error(4); } return 0; }
int main () { double t0 = elapsed(); // dimension of the vectors to index int d = 128; // size of the database we plan to index size_t nb = 200 * 1000; // make a set of nt training vectors in the unit cube // (could be the database) size_t nt = 100 * 1000; // make the index object and train it faiss::IndexFlatL2 coarse_quantizer (d); // a reasonable number of centroids to index nb vectors int ncentroids = int (4 * sqrt (nb)); // the coarse quantizer should not be dealloced before the index // 4 = nb of bytes per code (d must be a multiple of this) // 8 = nb of bits per sub-code (almost always 8) faiss::IndexIVFPQ index (&coarse_quantizer, d, ncentroids, 4, 8); { // training printf ("[%.3f s] Generating %ld vectors in %dD for training\n", elapsed() - t0, nt, d); std::vector <float> trainvecs (nt * d); for (size_t i = 0; i < nt * d; i++) { trainvecs[i] = drand48(); } printf ("[%.3f s] Training the index\n", elapsed() - t0); index.verbose = true; index.train (nt, trainvecs.data()); } { // I/O demo const char *outfilename = "/tmp/index_trained.faissindex"; printf ("[%.3f s] storing the pre-trained index to %s\n", elapsed() - t0, outfilename); write_index (&index, outfilename); } size_t nq; std::vector<float> queries; { // populating the database printf ("[%.3f s] Building a dataset of %ld vectors to index\n", elapsed() - t0, nb); std::vector <float> database (nb * d); for (size_t i = 0; i < nb * d; i++) { database[i] = drand48(); } printf ("[%.3f s] Adding the vectors to the index\n", elapsed() - t0); index.add (nb, database.data()); printf ("[%.3f s] imbalance factor: %g\n", elapsed() - t0, index.imbalance_factor ()); // remember a few elements from the database as queries int i0 = 1234; int i1 = 1243; nq = i1 - i0; queries.resize (nq * d); for (int i = i0; i < i1; i++) { for (int j = 0; j < d; j++) { queries [(i - i0) * d + j] = database [i * d + j]; } } } { // searching the database int k = 5; printf ("[%.3f s] Searching the %d nearest neighbors " "of %ld vectors in the index\n", elapsed() - t0, k, nq); std::vector<faiss::Index::idx_t> nns (k * nq); std::vector<float> dis (k * nq); index.search (nq, queries.data(), k, dis.data(), nns.data()); printf ("[%.3f s] Query results (vector ids, then distances):\n", elapsed() - t0); for (int i = 0; i < nq; i++) { printf ("query %2d: ", i); for (int j = 0; j < k; j++) { printf ("%7ld ", nns[j + i * k]); } printf ("\n dis: "); for (int j = 0; j < k; j++) { printf ("%7g ", dis[j + i * k]); } printf ("\n"); } printf ("note that the nearest neighbor is not at " "distance 0 due to quantization errors\n"); } return 0; }
int main(int argc, char* argv[]){ unsigned num_threads = 16; bool rgb_is_compressed = false; std::string stream_filename; CMDParser p("basefilename_cv .... basefilename_for_output"); p.addOpt("s",1,"stream_filename", "specify the stream filename which should be converted"); p.addOpt("n",1,"num_threads", "specify how many threads should be used, default 16"); p.addOpt("c",-1,"rgb_is_compressed", "enable compressed support for rgb stream, default: false (not compressed)"); p.addOpt("p1d",1,"filter_pass_1_max_avg_dist_in_meter", "filter pass 1 skips points which have an average distance of more than this to it k neighbors, default 0.025"); p.addOpt("p1k",1,"filter_pass_1_k", "filter pass 1 number of neighbors, default 50"); p.addOpt("p2s",1,"filter_pass_2_sd_fac", "filter pass 2, specify how many times a point's distance should be above (values higher than 1.0) / below (values smaller than 1.0) is allowed to be compared to standard deviation of its k neighbors, default 1.0"); p.addOpt("p2k",1,"filter_pass_2_k", "filter pass 2 number of neighbors (the higher the more to process) , default 50"); p.addOpt("bbx",6,"bounding_box", "specify the bounding box x_min y_min z_min x_max y_max z_max in meters, default -1.2 -0.05 -1.2 1.2 2.4 1.2"); p.init(argc,argv); if(p.isOptSet("bbx")){ bbx_min = glm::vec3(p.getOptsFloat("bbx")[0], p.getOptsFloat("bbx")[1], p.getOptsFloat("bbx")[2]); bbx_max = glm::vec3(p.getOptsFloat("bbx")[3], p.getOptsFloat("bbx")[4], p.getOptsFloat("bbx")[5]); std::cout << "setting bounding box to min: " << bbx_min << " -> max: " << bbx_max << std::endl; } if(p.isOptSet("p1d")){ filter_pass_1_max_avg_dist_in_meter = p.getOptsFloat("p1d")[0]; std::cout << "setting filter_pass_1_max_avg_dist_in_meter to " << filter_pass_1_max_avg_dist_in_meter << std::endl; } if(p.isOptSet("p1k")){ filter_pass_1_k = p.getOptsInt("p1k")[0]; std::cout << "setting filter_pass_1_k to " << filter_pass_1_k << std::endl; } if(p.isOptSet("p2s")){ filter_pass_2_sd_fac = p.getOptsFloat("p2s")[0]; std::cout << "setting filter_pass_2_sd_fac to " << filter_pass_2_sd_fac << std::endl; } if(p.isOptSet("p2k")){ filter_pass_2_k = p.getOptsInt("p2k")[0]; std::cout << "setting filter_pass_2_k to " << filter_pass_2_k << std::endl; } if(p.isOptSet("s")){ stream_filename = p.getOptsString("s")[0]; } else{ std::cerr << "ERROR, please specify stream filename with flag -s, see " << argv[0] << " -h for help" << std::endl; return 0; } if(p.isOptSet("n")){ num_threads = p.getOptsInt("n")[0]; } if(p.isOptSet("c")){ rgb_is_compressed = true; } const unsigned num_streams(p.getArgs().size() - 1); const std::string basefilename_for_output = p.getArgs()[num_streams]; std::vector<CalibVolume*> cvs; for(unsigned i = 0; i < num_streams; ++i){ std::string basefilename = p.getArgs()[i]; std::string filename_xyz(basefilename + "_xyz"); std::string filename_uv(basefilename + "_uv"); cvs.push_back(new CalibVolume(filename_xyz.c_str(), filename_uv.c_str())); } RGBDConfig cfg; cfg.size_rgb = glm::uvec2(1280, 1080); cfg.size_d = glm::uvec2(512, 424); RGBDSensor sensor(cfg, num_streams - 1); unsigned char* tmp_rgb = 0; unsigned char* tmp_rgba = 0; const unsigned colorsize_tmp = cfg.size_rgb.x * cfg.size_rgb.y * 3; const unsigned colorsize_tmpa = cfg.size_rgb.x * cfg.size_rgb.y * 4; if(rgb_is_compressed){ tmp_rgb = new unsigned char [colorsize_tmp]; tmp_rgba = new unsigned char [colorsize_tmpa]; } const unsigned colorsize = rgb_is_compressed ? 691200 : cfg.size_rgb.x * cfg.size_rgb.y * 3; const unsigned depthsize = cfg.size_d.x * cfg.size_d.y * sizeof(float); FileBuffer fb(stream_filename.c_str()); if(!fb.open("r")){ std::cerr << "ERROR, while opening " << stream_filename << " exiting..." << std::endl; return 1; } const unsigned num_frames = fb.calcNumFrames(num_streams * (colorsize + depthsize)); double curr_frame_time = 0.0; unsigned frame_num = 0; while(frame_num < num_frames){ ++frame_num; for(unsigned s_num = 0; s_num < num_streams; ++s_num){ fb.read((unsigned char*) (s_num == 0 ? sensor.frame_rgb : sensor.slave_frames_rgb[s_num - 1]), colorsize); if(s_num == 0){ memcpy((char*) &curr_frame_time, (const char*) sensor.frame_rgb, sizeof(double)); std::cout << "curr_frame_time: " << curr_frame_time << std::endl; } if(rgb_is_compressed){ // uncompress to rgb_tmp from (unsigned char*) (s_num == 0 ? sensor.frame_rgb : sensor.slave_frames_rgb[s_num - 1]) to tmp_rgba squish::DecompressImage (tmp_rgba, cfg.size_rgb.x, cfg.size_rgb.y, (unsigned char*) (s_num == 0 ? sensor.frame_rgb : sensor.slave_frames_rgb[s_num - 1]), squish::kDxt1); // copy back rgbsensor unsigned buffida = 0; unsigned buffid = 0; for(unsigned y = 0; y < cfg.size_rgb.y; ++y){ for(unsigned x = 0; x < cfg.size_rgb.x; ++x){ tmp_rgb[buffid++] = tmp_rgba[buffida++]; tmp_rgb[buffid++] = tmp_rgba[buffida++]; tmp_rgb[buffid++] = tmp_rgba[buffida++]; buffida++; } } memcpy((unsigned char*) (s_num == 0 ? sensor.frame_rgb : sensor.slave_frames_rgb[s_num - 1]), tmp_rgb, colorsize_tmp); } fb.read((unsigned char*) (s_num == 0 ? sensor.frame_d : sensor.slave_frames_d[s_num - 1]), depthsize); } std::vector<nniSample> nnisamples; for(unsigned s_num = 0; s_num < num_streams; ++s_num){ // do 3D reconstruction for each depth pixel for(unsigned y = 0; y < sensor.config.size_d.y; ++y){ for(unsigned x = 0; x < (sensor.config.size_d.x - 3); ++x){ const unsigned d_idx = y* sensor.config.size_d.x + x; float d = s_num == 0 ? sensor.frame_d[d_idx] : sensor.slave_frames_d[s_num - 1][d_idx]; if(d < cvs[s_num]->min_d || d > cvs[s_num]->max_d){ continue; } glm::vec3 pos3D; glm::vec2 pos2D_rgb; pos3D = cvs[s_num]->lookupPos3D( x * 1.0/sensor.config.size_d.x, y * 1.0/sensor.config.size_d.y, d); if(clip(pos3D)){ continue; } nniSample nnis; nnis.s_pos.x = pos3D.x; nnis.s_pos.y = pos3D.y; nnis.s_pos.z = pos3D.z; glm::vec2 pos2D_rgb_norm = cvs[s_num]->lookupPos2D_normalized( x * 1.0/sensor.config.size_d.x, y * 1.0/sensor.config.size_d.y, d); pos2D_rgb = glm::vec2(pos2D_rgb_norm.x * sensor.config.size_rgb.x, pos2D_rgb_norm.y * sensor.config.size_rgb.y); glm::vec3 rgb = sensor.get_rgb_bilinear_normalized(pos2D_rgb, s_num); nnis.s_pos_off.x = rgb.x; nnis.s_pos_off.y = rgb.y; nnis.s_pos_off.z = rgb.z; nnisamples.push_back(nnis); } } } std::cout << "start building acceleration structure for filtering " << nnisamples.size() << " points..." << std::endl; NearestNeighbourSearch nns(nnisamples); std::vector<std::vector<nniSample> > results; for(unsigned tid = 0; tid < num_threads; ++tid){ results.push_back(std::vector<nniSample>() ); } std::cout << "start filtering frame " << frame_num << " using " << num_threads << " threads." << std::endl; boost::thread_group threadGroup; for (unsigned tid = 0; tid < num_threads; ++tid){ threadGroup.create_thread(boost::bind(&filterPerThread, &nns, &nnisamples, &results, tid, num_threads)); } threadGroup.join_all(); #if 0 unsigned tid = 0; for(unsigned sid = 0; sid < nnisamples.size(); ++sid){ nniSample s = nnisamples[sid]; std::vector<nniSample> neighbours = nns.search(s,filter_pass_1_k); if(neighbours.empty()){ continue; } const float avd = calcAvgDist(neighbours, s); if(avd > filter_pass_1_max_avg_dist_in_meter){ continue; } std::vector<float> dists; for(const auto& n : neighbours){ std::vector<nniSample> local_neighbours = nns.search(n,filter_pass_2_k); if(!local_neighbours.empty()){ dists.push_back(calcAvgDist(local_neighbours, n)); } } double mean; double sd; calcMeanSD(dists, mean, sd); if((avd - mean) > filter_pass_2_sd_fac * sd){ continue; } results[tid].push_back(s); } #endif const std::string pcfile_name(basefilename_for_output + "_" + toStringP(frame_num, 5 /*fill*/) + ".xyz"); std::ofstream pcfile(pcfile_name.c_str()); std::cout << "start writing to file " << pcfile_name << " ..." << std::endl; for(unsigned tid = 0; tid < num_threads; ++tid){ for(const auto& s : results[tid]){ int red = (int) std::max(0.0f , std::min(255.0f, s.s_pos_off.x * 255.0f)); int green = (int) std::max(0.0f , std::min(255.0f, s.s_pos_off.y * 255.0f)); int blue = (int) std::max(0.0f , std::min(255.0f, s.s_pos_off.z * 255.0f)); pcfile << s.s_pos.x << " " << s.s_pos.y << " " << s.s_pos.z << " " << red << " " << green << " " << blue << std::endl; } } pcfile.close(); const std::string tsfile_name(basefilename_for_output + "_" + toStringP(frame_num, 5 /*fill*/) + ".timestamp"); std::ofstream tsfile(tsfile_name.c_str()); tsfile << curr_frame_time << std::endl; tsfile.close(); std::cout << frame_num << " from " << num_frames << " processed and saved to: " << pcfile_name << std::endl << std::endl; } return 0; }