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;
}
