void save_as_png256_hex(T1 & file, T2 const& image, int colors = 256, int trans_mode = -1, double gamma = 2.0) { unsigned width = image.width(); unsigned height = image.height(); // structure for color quantization hextree<mapnik::rgba> tree(colors); if (trans_mode >= 0) tree.setTransMode(trans_mode); if (gamma > 0) tree.setGamma(gamma); for (unsigned y = 0; y < height; ++y) { typename T2::pixel_type const * row = image.getRow(y); for (unsigned x = 0; x < width; ++x) { unsigned val = row[x]; tree.insert(mapnik::rgba(U2RED(val), U2GREEN(val), U2BLUE(val), U2ALPHA(val))); } } //transparency values per palette index std::vector<mapnik::rgba> pal; tree.create_palette(pal); assert(int(pal.size()) <= colors); std::vector<mapnik::rgb> palette; std::vector<unsigned> alphaTable; for(unsigned i=0; i<pal.size(); i++) { palette.push_back(rgb(pal[i].r, pal[i].g, pal[i].b)); alphaTable.push_back(pal[i].a); } if (palette.size() > 16 ) { // >16 && <=256 colors -> write 8-bit color depth image_data_8 reduced_image(width, height); for (unsigned y = 0; y < height; ++y) { mapnik::image_data_32::pixel_type const * row = image.getRow(y); mapnik::image_data_8::pixel_type * row_out = reduced_image.getRow(y); for (unsigned x = 0; x < width; ++x) { unsigned val = row[x]; mapnik::rgba c(U2RED(val), U2GREEN(val), U2BLUE(val), U2ALPHA(val)); row_out[x] = tree.quantize(c); } } save_as_png(file, palette, reduced_image, width, height, 8, alphaTable); } else if (palette.size() == 1) { // 1 color image -> write 1-bit color depth PNG unsigned image_width = (int(0.125*width) + 7)&~7; unsigned image_height = height; image_data_8 reduced_image(image_width, image_height); reduced_image.set(0); save_as_png(file, palette, reduced_image, width, height, 1, alphaTable); } else { // <=16 colors -> write 4-bit color depth PNG unsigned image_width = (int(0.5*width) + 3)&~3; unsigned image_height = height; image_data_8 reduced_image(image_width, image_height); for (unsigned y = 0; y < height; ++y) { mapnik::image_data_32::pixel_type const * row = image.getRow(y); mapnik::image_data_8::pixel_type * row_out = reduced_image.getRow(y); byte index = 0; for (unsigned x = 0; x < width; ++x) { unsigned val = row[x]; mapnik::rgba c(U2RED(val), U2GREEN(val), U2BLUE(val), U2ALPHA(val)); index = tree.quantize(c); if (x%2 == 0) index = index<<4; row_out[x>>1] |= index; } } save_as_png(file, palette, reduced_image, width, height, 4, alphaTable); } }
void KonqSidebarBookmarkModule::slotMoved(Q3ListViewItem *i, Q3ListViewItem*, Q3ListViewItem *after) { KonqSidebarBookmarkItem *item = dynamic_cast<KonqSidebarBookmarkItem*>( i ); if (!item) return; KBookmark bookmark = item->bookmark(); KBookmark afterBookmark; KonqSidebarBookmarkItem *afterItem = dynamic_cast<KonqSidebarBookmarkItem*>(after); if (afterItem) afterBookmark = afterItem->bookmark(); KBookmarkGroup oldParentGroup = bookmark.parentGroup(); KBookmarkGroup parentGroup; // try to get the parent group (assume that the QListViewItem has been reparented by K3ListView)... // if anything goes wrong, use the root. if (item->parent()) { bool error = false; KonqSidebarBookmarkItem *parent = dynamic_cast<KonqSidebarBookmarkItem*>( (item->parent()) ); if (!parent) { error = true; } else { if (parent->bookmark().isGroup()) parentGroup = parent->bookmark().toGroup(); else error = true; } if (error) parentGroup = s_bookmarkManager->root(); } else { // No parent! This means the user dropped it before the top level item // And K3ListView has moved the item there, we need to correct it tree()->moveItem(item, m_topLevelItem, 0L); parentGroup = s_bookmarkManager->root(); } // remove the old reference. oldParentGroup.deleteBookmark( bookmark ); // insert the new item. parentGroup.moveBookmark(bookmark, afterBookmark); // inform others about the changed groups. quite expensive, so do // our best to update them in only one emitChanged call. QString oldAddress = oldParentGroup.address(); QString newAddress = parentGroup.address(); if (oldAddress == newAddress) { s_bookmarkManager->emitChanged( parentGroup ); } else { int i = 0; while (true) { QChar c1 = oldAddress[i]; QChar c2 = newAddress[i]; if (c1 == QChar::null) { // oldParentGroup is probably parent of parentGroup. s_bookmarkManager->emitChanged( oldParentGroup ); break; } else if (c2 == QChar::null) { // parentGroup is probably parent of oldParentGroup. s_bookmarkManager->emitChanged( parentGroup ); break; } else { if (c1 == c2) { // step to the next character. ++i; } else { // ugh... need to update both groups separately. s_bookmarkManager->emitChanged( oldParentGroup ); s_bookmarkManager->emitChanged( parentGroup ); break; } } } } }
tree sql_exec (url db_name, string cmd) { if (!sqlite3_initialized) tm_sqlite3_initialize (); if (sqlite3_error) { cout << "TeXmacs] ERROR: SQLite support not properly configured.\n"; return tree (TUPLE); } string name= concretize (db_name); if (!sqlite3_connections->contains (name)) { c_string _name (name); sqlite3* db= NULL; //cout << "Opening " << _name << "\n"; int status= SQLITE3_open (_name, &db); if (status == SQLITE_OK) sqlite3_connections (name) = (void*) db; } if (!sqlite3_connections->contains (name)) { cout << "TeXmacs] SQL error: database " << name << " could not be opened\n"; return tree (TUPLE); } tree ret (TUPLE); sqlite3* db= (sqlite3*) sqlite3_connections [name]; c_string _cmd (sql_escape (cmd)); char** tab; int rows, cols; char* err; //cout << "Executing " << _cmd << "\n"; int status= SQLITE3_get_table (db, _cmd, &tab, &rows, &cols, &err); int attempt= 0; while (status != SQLITE_OK && string (err) == string ("database is locked") && attempt < 100) { usleep (100000); attempt++; status= SQLITE3_get_table (db, _cmd, &tab, &rows, &cols, &err); } if (status != SQLITE_OK) { // TODO: improve error handling cout << "TeXmacs] SQL error\n"; if (err != NULL) cout << "TeXmacs] " << err << "\n"; } for (int r=0; r<=rows; r++) { tree row (TUPLE); //cout << " Row " << r << LF; for (int c=0; c<cols; c++) { int i= r*cols + c; if (tab[i] == NULL) row << tree (TUPLE); else { row << tree (scm_quote (sql_unescape (tab[i]))); //cout << " Column " << c << ": " << tab[i] << LF; } } ret << row; } SQLITE3_free_table (tab); //cout << "Return " << ret << "\n"; return ret; }
//*********************************************************** // segmentation // セグメンテーションを行う. //*********************************************************** void segmentation(pcl::PointCloud<pcl::PointXYZRGB>& cloud, double th, int c) { std::cout << "segmentation" << std::endl; pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp_rgb (new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp_cloud (new pcl::PointCloud<pcl::PointXYZRGB>); for(int i=0;i<cloud.points.size();i++){ if((m_size.max_z <= cloud.points[i].z) && (cloud.points[i].z <= m_size.max_z+0.3)){ tmp_cloud->points.push_back(cloud.points[i]); } } pcl::copyPointCloud(*tmp_cloud, cloud); // Creating the KdTree object for the search method of the extraction pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>); tree->setInputCloud (cloud.makeShared ()); std::vector<pcl::PointIndices> cluster_indices; pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec; ec.setClusterTolerance (th); ec.setMinClusterSize (200); ec.setMaxClusterSize (300000); ec.setSearchMethod (tree); ec.setInputCloud (cloud.makeShared ()); ec.extract (cluster_indices); std::cout << "クラスタリング開始" << std::endl; int m = 0; for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it){ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster_rgb (new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZHSV>::Ptr cloud_cluster_hsv (new pcl::PointCloud<pcl::PointXYZHSV>); pcl::PointXYZ g; for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++){ cloud_cluster_rgb->points.push_back (cloud.points[*pit]); } cloud_cluster_rgb->width = cloud_cluster_rgb->points.size (); cloud_cluster_rgb->height = 1; cloud_cluster_rgb->is_dense = true; //クラスタ重心を求める g = g_pos(*cloud_cluster_rgb); if(((g.x < m_size.min_x) || (m_size.max_x < g.x)) || ((g.y < m_size.min_y) || (m_size.max_y < g.y))){ continue; } m++; if((m_size.max_z + 0.02 < g.z) && (g.z < m_size.max_z + 0.12)){ object_map map; map.cloud_rgb = *cloud_cluster_rgb; map.g = g; map.tf = c; make_elevation(map); // RGB -> HSV pcl::PointCloudXYZRGBtoXYZHSV(*cloud_cluster_rgb, *cloud_cluster_hsv); for(int i=0;i<MAP_H;i++){ for(int j=0;j<MAP_S;j++){ map.histogram[i][j] = 0.000; } } map.cloud_hsv = *cloud_cluster_hsv; // H-Sヒストグラムの作成 for(int i=0;i<cloud_cluster_hsv->points.size();i++){ if(((int)(255*cloud_cluster_hsv->points[i].h)/((256/MAP_H)*360) >= 32) || ((int)(255*cloud_cluster_hsv->points[i].h)/((256/MAP_H)*360) < 0)) continue; if(((int)(255*cloud_cluster_hsv->points[i].s)/(256/MAP_H) >= 32) || ((int)(255*cloud_cluster_hsv->points[i].s)/(256/MAP_H) < 0)) continue; // 正規化のため,セグメントの点数で割る. map.histogram[(int)(255*cloud_cluster_hsv->points[i].h)/((256/MAP_H)*360)][(int)(255*cloud_cluster_hsv->points[i].s)/(256/MAP_H)] += 1.000/(float)cloud_cluster_hsv->points.size(); } Object_Map.push_back(map); *tmp_rgb += *cloud_cluster_rgb; } } pcl::copyPointCloud(*tmp_rgb, cloud); return; }
tree drd_info_rep::get_env (tree_label l, int nr) { if (nr >= N(info[l]->ci)) return tree (ATTR); return drd_decode (info[l]->ci[nr].env); }
void ScalarCompiler::setSharingCount(Tree sig, int count) { //cerr << "setSharingCount of : " << *sig << " <- " << count << endl; setProperty(sig, fSharingKey, tree(count)); }
unsigned int scale_of_anisotropy (const Point_set& points, double& size) { Tree tree(points.begin(), points.end()); double ratio_kept = (points.size() < 1000) ? 1. : 1000. / (points.size()); std::vector<Point> subset; for (std::size_t i = 0; i < points.size (); ++ i) if (rand() / (double)RAND_MAX < ratio_kept) subset.push_back (points[i]); std::vector<unsigned int> scales; generate_scales (std::back_inserter (scales)); std::vector<unsigned int> chosen; for (std::size_t i = 0; i < subset.size (); ++ i) { Neighbor_search search(tree, subset[i],scales.back()); double current = 0.; unsigned int nb = 0; std::size_t index = 0; double maximum = 0.; unsigned int c = 0; for (Search_iterator search_iterator = search.begin(); search_iterator != search.end (); ++ search_iterator, ++ nb) { current += search_iterator->second; if (nb + 1 == scales[index]) { double score = std::sqrt (current / scales[index]) / std::pow (scales[index], 0.75); // NB ^ (3/4) if (score > maximum) { maximum = score; c = scales[index]; } ++ index; if (index == scales.size ()) break; } } chosen.push_back (c); } double mean = 0.; for (std::size_t i = 0; i < chosen.size(); ++ i) mean += chosen[i]; mean /= chosen.size(); unsigned int aniso_scale = static_cast<unsigned int>(mean); size = 0.; for (std::size_t i = 0; i < subset.size (); ++ i) { Neighbor_search search(tree, subset[i], aniso_scale); size += std::sqrt ((-- search.end())->second); } size /= subset.size(); return aniso_scale; }
void swcode(Swtch swp, int b[], int lb, int ub) { int hilab, lolab, l, u, k = (lb + ub)/2; long *v = swp->values; if (k > lb && k < ub) { lolab = genlabel(1); hilab = genlabel(1); } else if (k > lb) { lolab = genlabel(1); hilab = swp->deflab->u.l.label; } else if (k < ub) { lolab = swp->deflab->u.l.label; hilab = genlabel(1); } else lolab = hilab = swp->deflab->u.l.label; l = b[k]; u = b[k+1] - 1; if (u - l + 1 <= 3) { int i; for (i = l; i <= u; i++) cmp(EQ, swp->sym, v[i], swp->labels[i]->u.l.label); if (k > lb && k < ub) cmp(GT, swp->sym, v[u], hilab); else if (k > lb) cmp(GT, swp->sym, v[u], hilab); else if (k < ub) cmp(LT, swp->sym, v[l], lolab); else assert(lolab == hilab), branch(lolab); walk(NULL, 0, 0); } else { Tree e; Type ty = signedint(swp->sym->type); Symbol table = genident(STATIC, array(voidptype, u - l + 1, 0), GLOBAL); (*IR->defsymbol)(table); if (!isunsigned(swp->sym->type) || v[l] != 0) cmp(LT, swp->sym, v[l], lolab); cmp(GT, swp->sym, v[u], hilab); e = (*optree['-'])(SUB, cast(idtree(swp->sym), ty), cnsttree(ty, v[l])); if (e->type->size < unsignedptr->size) e = cast(e, unsignedlong); walk(tree(JUMP, voidtype, rvalue((*optree['+'])(ADD, pointer(idtree(table)), e)), NULL), 0, 0); code(Switch); codelist->u.swtch.table = table; codelist->u.swtch.sym = swp->sym; codelist->u.swtch.deflab = swp->deflab; codelist->u.swtch.size = u - l + 1; codelist->u.swtch.values = &v[l]; codelist->u.swtch.labels = &swp->labels[l]; if (v[u] - v[l] + 1 >= 10000) warning("switch generates a huge table\n"); } if (k > lb) { assert(lolab != swp->deflab->u.l.label); definelab(lolab); swcode(swp, b, lb, k - 1); } if (k < ub) { assert(hilab != swp->deflab->u.l.label); definelab(hilab); swcode(swp, b, k + 1, ub); } }
int main(int argc, char * argv[]) { MPI_Comm comm = MPI_COMM_WORLD; MPI_Init(&argc, &argv); int nRank, myRank; MPI_Comm_size(comm, &nRank); MPI_Comm_rank(comm, &myRank); assert(nRank == 1 && myRank == 0); if (argc < 5) { if (myRank == 0) { fprintf(stderr, " ------------------------------------------------------------------------\n"); fprintf(stderr, " Usage: \n"); fprintf(stderr, " %s fileIn fileOut reduceDM reduceStars \n", argv[0]); fprintf(stderr, " ------------------------------------------------------------------------\n"); } exit(-1); } const std::string fileIn (argv[1]); const std::string fileOut(argv[2]); const int reduceDM = atoi(argv[3]); const int reduceS = atoi(argv[4]); if (myRank == 0) { fprintf(stderr, " Input file: %s\n", fileIn.c_str()); fprintf(stderr, " reduceStars= %d \n", reduceS); fprintf(stderr, " reduceDM = %d \n", reduceDM); fprintf(stderr, " Output file: %s\n", fileOut.c_str()); } std::vector<BonsaiIO::DataTypeBase*> dataInput; /************* read ***********/ { const double tOpen = MPI_Wtime(); BonsaiIO::Core in(myRank, nRank, comm, BonsaiIO::READ, fileIn); double dtOpen = MPI_Wtime() - tOpen; if (myRank == 0) in.getHeader().printFields(); double dtRead; if (reduceDM > 0) { assert(0); std::vector<BonsaiIO::DataTypeBase*> dataDM; dataDM.push_back(new BonsaiIO::DataType<IDType>("DM:IDType")); dataDM.push_back(new BonsaiIO::DataType<float4>("DM:POS:real4")); dataDM.push_back(new BonsaiIO::DataType<float3>("DM:VEL:float[3]")); dataDM.push_back(new BonsaiIO::DataType<float2>("DM:RHOH:float[2]")); dtRead += read(myRank, comm, dataDM, in, reduceDM); dataInput.insert(dataInput.end(), dataDM.begin(), dataDM.end()); } if (reduceS > 0) { std::vector<BonsaiIO::DataTypeBase*> dataStars; dataStars.push_back(new BonsaiIO::DataType<IDType>("Stars:IDType")); dataStars.push_back(new BonsaiIO::DataType<float4>("Stars:POS:real4")); dataStars.push_back(new BonsaiIO::DataType<float3>("Stars:VEL:float[3]")); dataStars.push_back(new BonsaiIO::DataType<float2>("Stars:RHOH:float[2]")); dtRead += read(myRank, comm, dataStars, in, reduceS); dataInput.insert(dataInput.end(), dataStars.begin(), dataStars.end()); } double readBW = in.computeBandwidth(); const double tClose = MPI_Wtime(); in.close(); double dtClose = MPI_Wtime() - tClose; double dtOpenGlb = 0; double dtReadGlb = 0; double dtCloseGlb = 0; MPI_Allreduce(&dtOpen, &dtOpenGlb, 1, MPI_DOUBLE, MPI_SUM, comm); MPI_Allreduce(&dtRead, &dtReadGlb, 1, MPI_DOUBLE, MPI_SUM, comm); MPI_Allreduce(&dtClose, &dtCloseGlb, 1, MPI_DOUBLE, MPI_SUM, comm); if (myRank == 0) { fprintf(stderr, "dtOpen = %g sec \n", dtOpenGlb); fprintf(stderr, "dtRead = %g sec \n", dtReadGlb); fprintf(stderr, "dtClose= %g sec \n", dtCloseGlb); fprintf(stderr, "Read BW= %g MB/s \n", readBW/1e6); } } /************* estimate density **********/ std::vector<BonsaiIO::DataTypeBase*> data; data.push_back(new BonsaiIO::DataType<float3>("Stars:XYZ:float[3]")); data.push_back(new BonsaiIO::DataType<float5>("Stars:VxVyVz,DENS,H:float[5]")); { const auto &posArray = *dynamic_cast<BonsaiIO::DataType<float4>*>(dataInput[1]); const auto &velArray = *dynamic_cast<BonsaiIO::DataType<float3>*>(dataInput[2]); const size_t np = posArray.getNumElements(); Particle::Vector ptcl(np); fprintf(stderr, " -- create tree particles -- \n"); for (size_t i = 0; i < np; i++) { const auto &pos = posArray[i]; const auto &vel = velArray[i]; ptcl[i].pos = vec3(pos[0], pos[1], pos[2]); ptcl[i].mass = pos[3]; ptcl[i].vel = vec3(vel[0], vel[1], vel[2]); } Node::allocate(np,np); fprintf(stderr, " -- build tree -- \n"); Tree tree(ptcl); const auto &leafArray = tree.leafArray; const int nLeaf = leafArray.size(); fprintf(stderr, " np= %d nleaf= %d NLEAF= %d\n", (int)np, nLeaf, Node::NLEAF); auto &posOut = *dynamic_cast<BonsaiIO::DataType<float3>*>(data[0]); auto &attrOut = *dynamic_cast<BonsaiIO::DataType<float5>*>(data[1]); posOut.resize(nLeaf); attrOut.resize(nLeaf); fprintf(stderr, " -- generate output data -- \n"); size_t npMean = 0; for (int i = 0; i < nLeaf; i++) { const auto &leaf = *leafArray[i]; vec3 pos(0.0); vec3 vel(0.0); float mass = 0.0; npMean += leaf.np; for (int i = 0; i < leaf.np; i++) { const auto &p = Node::ptcl[leaf.pfirst+i]; mass += p.mass; pos += p.pos*p.mass; vel += p.vel*p.mass; } pos *= 1.0/mass; vel *= 1.0/mass; posOut[i][0] = pos.x; posOut[i][1] = pos.y; posOut[i][2] = pos.z; attrOut[i][0] = vel.x; attrOut[i][1] = vel.y; attrOut[i][2] = vel.z; const float volume = leaf.size*leaf.size*leaf.size; attrOut[i][3] = mass/volume; attrOut[i][4] = leaf.size; #if 0 if (i%1000 == 0) { fprintf(stderr, "i= %d: pos= %g %g %g vel= %g %g %g d= %g h= %g\n", i, posOut[i][0], posOut[i][1], posOut[i][2], attrOut[i][0], attrOut[i][1], attrOut[i][2], attrOut[i][3], attrOut[i][4]); } #endif } fprintf(stderr, "npMean= %g\n", 1.0*npMean/nLeaf); } /************* write ***********/ { fprintf(stderr, " -- write output -- \n"); const double tOpen = MPI_Wtime(); BonsaiIO::Core out(myRank, nRank, comm, BonsaiIO::WRITE, fileOut); double dtOpen = MPI_Wtime() - tOpen; double dtWrite = 0; dtWrite += write(myRank, comm, data, out); double writeBW = out.computeBandwidth(); const double tClose = MPI_Wtime(); out.close(); double dtClose = MPI_Wtime() - tClose; double dtOpenGlb = 0; double dtWriteGlb = 0; double dtCloseGlb = 0; MPI_Allreduce(&dtOpen, &dtOpenGlb, 1, MPI_DOUBLE, MPI_SUM, comm); MPI_Allreduce(&dtWrite, &dtWriteGlb, 1, MPI_DOUBLE, MPI_SUM, comm); MPI_Allreduce(&dtClose, &dtCloseGlb, 1, MPI_DOUBLE, MPI_SUM, comm); if (myRank == 0) { fprintf(stderr, "dtOpen = %g sec \n", dtOpenGlb); fprintf(stderr, "dtWrite = %g sec \n", dtWriteGlb); fprintf(stderr, "dtClose= %g sec \n", dtCloseGlb); fprintf(stderr, "Write BW= %g MB/s \n", writeBW/1e6); } } /*********** write stats **********/ MPI_Finalize(); return 0; }
operator tree () { return tree ("table"); }
template<typename F> void tree(const F& f) { if (m_structure.hasCold()) tree(f, ChunkState(m_metadata)); }
int main(int argc, char *argv[]) { WINDOW *mainwin; int ch; int maxY = 0, maxX = 0; double startFrom = 0.0; double max = 19.0; char isLeft = 1; ///* mainwin = initscr(); if (mainwin == NULL) { fprintf(stderr, "Error initialising ncurses.\n"); exit(-1); } // Get the maximum size of the screen getmaxyx(mainwin, maxY, maxX); // Check if colors are supported if (!has_colors()) { delwin(mainwin); endwin(); fprintf(stderr,"Your terminal does not support color\n"); exit(-1); } else { // Initialize colors start_color(); // Assign terminal default foreground/background colors to color number -1 use_default_colors(); init_pair(1, COLOR_RED, COLOR_BLACK); init_pair(2, COLOR_GREEN, COLOR_BLACK); init_pair(3, COLOR_YELLOW, COLOR_BLACK); init_pair(4, COLOR_BLUE, COLOR_BLACK); init_pair(5, COLOR_CYAN, COLOR_BLACK); init_pair(6, COLOR_MAGENTA, COLOR_BLACK); init_pair(7, COLOR_WHITE, COLOR_BLACK); init_pair(8, COLOR_RED, COLOR_WHITE); init_pair(9, COLOR_GREEN, COLOR_WHITE); } clear(); // Turn off key echoing noecho(); // Line buffering disabled cbreak(); // This determines the visibility of the cursor. curs_set(FALSE); // Tell curses not to do NL->CR/NL on output nonl(); // Enable the keypad for non-char keys keypad(mainwin, TRUE); //*/ tree(isLeft, startFrom, max); ///* // Loop until press ESC while ((ch = getch()) != 27) { switch(ch) { case '0': // Set foreground/background colors to default case '1': // Set foreground/background colors according to init_pairs case '2': case '3': case '4': case '5': case '6': case '7': case '8': case '9': attrset(COLOR_PAIR(ch - '0')); break; case 'l': if (isLeft) isLeft = 0; else isLeft = 1; break; case 'q': dx += 0.1; if (dx > maxX) dx = maxX; break; case 'w': dy += 0.1; if (dy > maxY) dy = maxY; break; case 'e': dz += 0.1; break; case KEY_UP: // Press Up to increase height break; case KEY_DOWN: // Press Down to decrease height break; case KEY_RIGHT: // Press right key reverse foreground/background colour. attron(A_REVERSE | A_BLINK); break; case KEY_LEFT: // Press left key return to default foreground/background colour. attroff(A_REVERSE | A_BLINK); break; } erase(); tree(isLeft, startFrom, max); } delwin(mainwin); endwin(); refresh(); //*/ return 0; }
int main(int argc, char** argv) { static int mc = false; static int elist = false; const char *input_file = 0; int c; while (1) { static struct option long_options[] ={ /* These options set a flag. */ {"mc" , no_argument, &mc , 1}, {"elist" , no_argument, &elist , 1}, {0, 0, 0, 0} }; /* getopt_long stores the option index here. */ int option_index = 0; c = getopt_long(argc, argv, "i:", long_options, &option_index); /* Detect the end of the options. */ if (c == -1) break; switch (c) { case 0: /* If this option set a flag, do nothing else now. */ if (long_options[option_index].flag != 0) break; case 'i': input_file = optarg; break; case '?': usage(); default: usage(); } } if (!input_file){ std::cout << argv[0] << " II: Missing input_file " << std::endl; usage(); } AA::newJob(); AA::newAnaJob(); AA::newSelectJob(); params(); AA::displayParameters(); bool ok = false; if (elist){ std::cout << argv[0] << " II: Doing event lists " << std::endl; ok = AA::dst.eventLst(input_file); } else { std::cout << argv[0] << " II: Doing sequential mode" << std::endl; ok = AA::dst.fileLst(input_file); } if (!ok) { std::cout << argv[0] << " EE: Input stream is not valid or is empty." << std::endl; exit(EXIT_FAILURE); } /* ================================ MAIN ===================================*/ /* -- We will save all in this tree --*/ TFile root_file("upsilon.root", "RECREATE"); TTree tree("tree", "all info."); TTree *treeMC=0; if (mc) treeMC = new TTree("treeMC", "all mc info."); EvtSaver evt_saver(tree); UpsilonFinder upsilon_finder(tree); /* -- Initilization of geometry, field and beam spot. * Should be done after the fileLst or eventLst initilization -- */ AA::det.input(); AA::field.input(); AA::spot.input(); AA::newEvent(); AA::newAnaEvent(); AA::newSelectEvent(); unsigned long p_events = 0; while (nextEvent()) { p_events++; AA::setField(); //define beam-spot and field value AA::spot.set(); AA::analyse(); AA::select(AA::TAG); //std::cout << "Run:" << AA::runNumber << " Evt: " << AA::evtNumber << std::endl; if (!upsilon_finder.find()) continue; while (upsilon_finder.next()){ // std::cout << xyg_finder.getMass() << ' ' << std::endl; upsilon_finder.fill(); evt_saver.fill(); tree.Fill(); } AA::dst.outEventLst("upsilon_elist"); }//End while next event. tree.Write(); if (mc) treeMC->Write(); root_file.Write(); root_file.Close(); std::cout << argv[0] << " II: upsilon_finder ended. " << p_events << " events seen."<< std::endl; std::exit(EXIT_SUCCESS); }
int main (int argc, char** argv) { //using namespace mapnik; namespace po = boost::program_options; bool verbose = false; bool validate_features = false; unsigned int depth = DEFAULT_DEPTH; double ratio = DEFAULT_RATIO; std::vector<std::string> files; char separator = 0; char quote = 0; std::string manual_headers; try { po::options_description desc("Mapnik CSV/GeoJSON index utility"); desc.add_options() ("help,h", "produce usage message") ("version,V","print version string") ("verbose,v","verbose output") ("depth,d", po::value<unsigned int>(), "max tree depth\n(default 8)") ("ratio,r",po::value<double>(),"split ratio (default 0.55)") ("separator,s", po::value<char>(), "CSV columns separator") ("quote,q", po::value<char>(), "CSV columns quote") ("manual-headers,H", po::value<std::string>(), "CSV manual headers string") ("files",po::value<std::vector<std::string> >(),"Files to index: file1 file2 ...fileN") ("validate-features", "Validate GeoJSON features") ; po::positional_options_description p; p.add("files",-1); po::variables_map vm; po::store(po::command_line_parser(argc, argv).options(desc).positional(p).run(), vm); po::notify(vm); if (vm.count("version")) { std::clog << "version 1.0.0" << std::endl; return 1; } if (vm.count("help")) { std::clog << desc << std::endl; return 1; } if (vm.count("verbose")) { verbose = true; } if (vm.count("validate-features")) { validate_features = true; } if (vm.count("depth")) { depth = vm["depth"].as<unsigned int>(); } if (vm.count("ratio")) { ratio = vm["ratio"].as<double>(); } if (vm.count("separator")) { separator = vm["separator"].as<char>(); } if (vm.count("quote")) { quote = vm["quote"].as<char>(); } if (vm.count("manual-headers")) { manual_headers = vm["manual-headers"].as<std::string>(); } if (vm.count("files")) { files=vm["files"].as<std::vector<std::string> >(); } } catch (std::exception const& ex) { std::clog << "Error: " << ex.what() << std::endl; return EXIT_FAILURE; } std::vector<std::string> files_to_process; for (auto const& filename : files) { if (!mapnik::util::exists(filename)) { continue; } if (mapnik::detail::is_csv(filename) || mapnik::detail::is_geojson(filename)) { files_to_process.push_back(filename); } } if (files_to_process.size() == 0) { std::clog << "no files to index" << std::endl; return EXIT_FAILURE; } std::clog << "max tree depth:" << depth << std::endl; std::clog << "split ratio:" << ratio << std::endl; using box_type = mapnik::box2d<float>; using item_type = std::pair<box_type, std::pair<std::size_t, std::size_t>>; for (auto const& filename : files_to_process) { if (!mapnik::util::exists(filename)) { std::clog << "Error : file " << filename << " does not exist" << std::endl; continue; } std::vector<item_type> boxes; box_type extent; if (mapnik::detail::is_csv(filename)) { std::clog << "processing '" << filename << "' as CSV\n"; auto result = mapnik::detail::process_csv_file(boxes, filename, manual_headers, separator, quote); if (!result.first) { std::clog << "Error: failed to process " << filename << std::endl; return EXIT_FAILURE; } extent = result.second; } else if (mapnik::detail::is_geojson(filename)) { std::clog << "processing '" << filename << "' as GeoJSON\n"; auto result = mapnik::detail::process_geojson_file(boxes, filename, validate_features, verbose); if (!result.first) { std::clog << "Error: failed to process " << filename << std::endl; return EXIT_FAILURE; } extent = result.second; } if (extent.valid()) { std::clog << extent << std::endl; mapnik::box2d<double> extent_d(extent.minx(), extent.miny(), extent.maxx(), extent.maxy()); mapnik::quad_tree<std::pair<std::size_t, std::size_t>> tree(extent_d, depth, ratio); for (auto const& item : boxes) { auto ext_f = std::get<0>(item); tree.insert(std::get<1>(item), mapnik::box2d<double>(ext_f.minx(), ext_f.miny(), ext_f.maxx(), ext_f.maxy())); } std::fstream file((filename + ".index").c_str(), std::ios::in | std::ios::out | std::ios::trunc | std::ios::binary); if (!file) { std::clog << "cannot open index file for writing file \"" << (filename + ".index") << "\"" << std::endl; } else { tree.trim(); std::clog << "number nodes=" << tree.count() << std::endl; std::clog << "number element=" << tree.count_items() << std::endl; file.exceptions(std::ios::failbit | std::ios::badbit); tree.write(file); file.flush(); file.close(); } } else { std::clog << "Invalid extent " << extent << std::endl; return EXIT_FAILURE; } } std::clog << "done!" << std::endl; return EXIT_SUCCESS; }
tree drd_info_rep::arg_access (tree t, tree arg, tree env, int& type) { // returns "" if unaccessible and the env if accessible //cout << " arg_access " << t << ", " << arg << ", " << env << "\n"; if (is_atomic (t)) return ""; else if (t == arg) return env; else if (is_func (t, QUOTE_ARG, 1) && N(arg) == 1 && t[0] == arg[0]) return env; else if (is_func (t, MAP_ARGS) && (t[2] == arg[0])) { if ((N(t) >= 4) && (N(arg) >= 2) && (as_int (t[3]) > as_int (arg[1]))) return ""; if ((N(t) == 5) && (N(arg) >= 2) && (as_int (t[3]) <= as_int (arg[1]))) return ""; tree_label inner= make_tree_label (as_string (t[0])); tree_label outer= make_tree_label (as_string (t[1])); if (get_nr_indices (inner) > 0) type= get_type_child (tree (inner, arg), 0); if ((get_nr_indices (inner) > 0) && (get_accessible (inner, 0) == ACCESSIBLE_ALWAYS) && all_accessible (outer)) return env; return ""; } else if (is_func (t, MACRO)) return ""; else if (is_func (t, WITH)) { int n= N(t)-1; //cout << "env= " << drd_env_merge (env, t (0, n)) << "\n"; return arg_access (t[n], arg, drd_env_merge (env, t (0, n)), type); } else if (is_func (t, TFORMAT)) { int n= N(t)-1; tree oldf= drd_env_read (env, CELL_FORMAT, tree (TFORMAT)); tree newf= oldf * tree (TFORMAT, A (t (0, n))); tree w = tree (ATTR, CELL_FORMAT, newf); tree cenv= get_env_child (t, n, drd_env_merge (env, w)); return arg_access (t[n], arg, cenv, type); } else if (is_func (t, COMPOUND) && N(t) >= 1 && is_atomic (t[0])) return arg_access (compound (t[0]->label, A (t (1, N(t)))), arg, env, type); else if ((is_func (t, IF) || is_func (t, VAR_IF)) && N(t) >= 2) return arg_access (t[1], arg, env, type); else { int i, n= N(t); for (i=0; i<n; i++) { int ctype= get_type_child (t, i); tree cenv = get_env_child (t, i, env); tree aenv = arg_access (t[i], arg, cenv, ctype); if (aenv != "") { if (ctype != TYPE_INVALID) type= ctype; if (is_accessible_child (t, i)) return aenv; } else if (type == TYPE_UNKNOWN && ctype != TYPE_INVALID && ctype != TYPE_UNKNOWN) { type= ctype; //cout << " found type " << t << ", " << arg << ", " << type << "\n"; } } return ""; } }
tree drd_info_rep::get_env_child (tree t, int i, string var, tree val) { tree env= get_env_child (t, i, tree (ATTR)); return drd_env_read (env, var, val); }
tree sql_exec (url db_name, string cmd) { (void) db_name; (void) cmd; return tree (TUPLE); }
bool SnapIt::processModelCylinder(jsk_pcl_ros::CallSnapIt::Request& req, jsk_pcl_ros::CallSnapIt::Response& res) { pcl::PointCloud<pcl::PointXYZ>::Ptr candidate_points (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointIndices::Ptr inliers(new pcl::PointIndices); Eigen::Vector3f n, C_orig; if (!extractPointsInsideCylinder(req.request.center, req.request.direction, req.request.radius, req.request.height, inliers, n, C_orig, 1.3)) { return false; } if (inliers->indices.size() < 3) { NODELET_ERROR("not enough points inside of the target_plane"); return false; } geometry_msgs::PointStamped centroid; centroid.point.x = C_orig[0]; centroid.point.y = C_orig[1]; centroid.point.z = C_orig[2]; centroid.header = req.request.header; pcl::ExtractIndices<pcl::PointXYZ> extract; extract.setInputCloud(input_); extract.setIndices(inliers); extract.filter(*candidate_points); publishPointCloud(debug_candidate_points_pub_, candidate_points); // first, to remove plane we estimate the plane pcl::ModelCoefficients::Ptr plane_coefficients (new pcl::ModelCoefficients); pcl::PointIndices::Ptr plane_inliers (new pcl::PointIndices); extractPlanePoints(candidate_points, plane_inliers, plane_coefficients, n, req.request.eps_angle); if (plane_inliers->indices.size() == 0) { NODELET_ERROR ("plane estimation failed"); return false; } // remove the points blonging to the plane pcl::PointCloud<pcl::PointXYZ>::Ptr points_inside_pole_wo_plane (new pcl::PointCloud<pcl::PointXYZ>); extract.setInputCloud (candidate_points); extract.setIndices (plane_inliers); extract.setNegative (true); extract.filter (*points_inside_pole_wo_plane); publishPointCloud(debug_candidate_points_pub2_, points_inside_pole_wo_plane); // normal estimation pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>); pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne; pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ()); ne.setSearchMethod (tree); ne.setInputCloud (points_inside_pole_wo_plane); ne.setKSearch (50); ne.compute (*cloud_normals); // segmentation pcl::ModelCoefficients::Ptr cylinder_coefficients (new pcl::ModelCoefficients); pcl::PointIndices::Ptr cylinder_inliers (new pcl::PointIndices); // Create the segmentation object pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> seg; // Optional seg.setOptimizeCoefficients (true); // Mandatory seg.setModelType (pcl::SACMODEL_CYLINDER); seg.setMethodType (pcl::SAC_RANSAC); seg.setRadiusLimits (0.01, req.request.radius * 1.2); seg.setDistanceThreshold (0.05); seg.setInputCloud(points_inside_pole_wo_plane); seg.setInputNormals (cloud_normals); seg.setMaxIterations (10000); seg.setNormalDistanceWeight (0.1); seg.setAxis(n); if (req.request.eps_angle != 0.0) { seg.setEpsAngle(req.request.eps_angle); } else { seg.setEpsAngle(0.35); } seg.segment (*cylinder_inliers, *cylinder_coefficients); if (cylinder_inliers->indices.size () == 0) { NODELET_ERROR ("Could not estimate a cylinder model for the given dataset."); return false; } debug_centroid_pub_.publish(centroid); pcl::PointCloud<pcl::PointXYZ>::Ptr cylinder_points (new pcl::PointCloud<pcl::PointXYZ>); extract.setInputCloud (points_inside_pole_wo_plane); extract.setIndices (cylinder_inliers); extract.setNegative (false); extract.filter (*cylinder_points); publishPointCloud(debug_candidate_points_pub3_, cylinder_points); Eigen::Vector3f n_prime; Eigen::Vector3f C_new; for (size_t i = 0; i < 3; i++) { C_new[i] = cylinder_coefficients->values[i]; n_prime[i] = cylinder_coefficients->values[i + 3]; } double radius = fabs(cylinder_coefficients->values[6]); // inorder to compute centroid, we project all the points to the center line. // and after that, get the minimum and maximum points in the coordinate system of the center line double min_alpha = DBL_MAX; double max_alpha = -DBL_MAX; for (size_t i = 0; i < cylinder_points->points.size(); i++ ) { pcl::PointXYZ q = cylinder_points->points[i]; double alpha = (q.getVector3fMap() - C_new).dot(n_prime); if (alpha < min_alpha) { min_alpha = alpha; } if (alpha > max_alpha) { max_alpha = alpha; } } // the center of cylinder Eigen::Vector3f C_new_prime = C_new + (max_alpha + min_alpha) / 2.0 * n_prime; Eigen::Vector3f n_cross = n.cross(n_prime); if (n.dot(n_prime)) { n_cross = - n_cross; } double theta = asin(n_cross.norm()); Eigen::Quaternionf trans (Eigen::AngleAxisf(theta, n_cross.normalized())); Eigen::Vector3f C = trans * C_orig; Eigen::Affine3f A = Eigen::Translation3f(C) * Eigen::Translation3f(C_new_prime - C) * trans * Eigen::Translation3f(C_orig).inverse(); tf::poseEigenToMsg((Eigen::Affine3d)A, res.transformation); geometry_msgs::PointStamped centroid_transformed; centroid_transformed.point.x = C_new_prime[0]; centroid_transformed.point.y = C_new_prime[1]; centroid_transformed.point.z = C_new_prime[2]; centroid_transformed.header = req.request.header; debug_centroid_after_trans_pub_.publish(centroid_transformed); // publish marker visualization_msgs::Marker marker; marker.type = visualization_msgs::Marker::CYLINDER; marker.scale.x = radius; marker.scale.y = radius; marker.scale.z = (max_alpha - min_alpha); marker.pose.position.x = C_new_prime[0]; marker.pose.position.y = C_new_prime[1]; marker.pose.position.z = C_new_prime[2]; // n_prime -> z // n_cross.normalized() -> x Eigen::Vector3f z_axis = n_prime.normalized(); Eigen::Vector3f y_axis = n_cross.normalized(); Eigen::Vector3f x_axis = (y_axis.cross(z_axis)).normalized(); Eigen::Matrix3f M; for (size_t i = 0; i < 3; i++) { M(i, 0) = x_axis[i]; M(i, 1) = y_axis[i]; M(i, 2) = z_axis[i]; } Eigen::Quaternionf q (M); marker.pose.orientation.x = q.x(); marker.pose.orientation.y = q.y(); marker.pose.orientation.z = q.z(); marker.pose.orientation.w = q.w(); marker.color.g = 1.0; marker.color.a = 1.0; marker.header = input_header_; marker_pub_.publish(marker); return true; }
void testQuadTree() { Vector3 origin( 0.f, 0.f, 0.f ); Vector2 origin2d( origin.x, origin.z ); TestTree tree( origin.x, origin.z ); const int WIDTH = (1 << TestTreeDesc::DEPTH); const float RANGE = TestTreeDesc::RANGE; const float CELL_SIZE = RANGE/WIDTH; TestElement elements[ WIDTH ][ WIDTH ]; const Vector2 cellSize( CELL_SIZE - 0.001f, CELL_SIZE - 0.001f); for (int j = 0; j < WIDTH; j++) { for (int i = 0; i < WIDTH; i++) { TestElement & curr = elements[i][j]; curr.x_ = i; curr.y_ = j; curr.minBounds_.set( i * CELL_SIZE, j * CELL_SIZE ); curr.maxBounds_ = curr.minBounds_ + cellSize; curr.minBounds_ += origin2d; curr.maxBounds_ += origin2d; tree.add( curr ); } } tree.print(); printTraverse( tree, Vector3( 30.f, 0.f, 0.f ), Vector3( 30.f, 0.f, 100.f ) ); printTraverse( tree, Vector3( 0.f, 0.f, 30.f ), Vector3( 100.f, 0.f, 30.f ) ); printTraverse( tree, Vector3( 30.0001f, 0.f, 30.f ), Vector3( 30.f, 0.f, 30.f ) ); return; Vector3 src( 0.f, 0.f, -1.f ); Vector3 dst( 100.f, 0.f, 99.f ); src.set( 30.f, 0.f, 30.f ); dst.set( 30.f, 0.f, 30.f ); src += origin; dst += origin; printTraverse( tree, src, dst ); printTraverse( tree, dst, src ); // src.set( 0.f, 0.f, 71.f ); // dst.set( 70.f, 0.f, 1.f ); src.set( 0.f, 0.f, 101.f ); dst.set( 100.f, 0.f, 1.f ); src += origin; dst += origin; // printTraverse( tree, src, dst ); // printTraverse( tree, dst, src ); src += Vector3( 0.f, 0.f, 50.f ); dst += Vector3( 0.f, 0.f, 50.f ); printTraverse( tree, src, dst ); printTraverse( tree, dst, src ); }
void MUONTriggerEfficiency(const char* filenameSim="galice_sim.root", const char* filenameRec="galice.root", Bool_t readFromRP = 0) { // Set default CDB storage AliCDBManager* man = AliCDBManager::Instance(); man->SetDefaultStorage("local://$ALICE_ROOT/OCDB"); man->SetRun(0); // output file AliMUONMCDataInterface diSim(filenameSim); AliMUONDataInterface diRec(filenameRec); if (!diSim.IsValid()) { cerr << "Cannot access " << filenameSim << endl; return; } if (!diRec.IsValid()) { cerr << "Cannot access " << filenameRec << endl; return; } FILE* fdat = fopen("MUONTriggerEfficiency.out","w"); if (!fdat) { cerr << "Cannot create output file" << endl; return; } Int_t coincmuon,muonlpt,muonhpt; Int_t CoincMuPlus,CoincMuMinus; coincmuon=0; muonlpt=0; muonhpt=0; Int_t nevents = diSim.NumberOfEvents(); for (Int_t ievent=0; ievent<nevents; ++ievent) { CoincMuPlus=0; CoincMuMinus=0; if (ievent%1000==0) printf("\t Event = %d\n",ievent); // Hits Int_t NbHits[2][4]; for (Int_t j=0; j<2; j++) { for (Int_t jj=0; jj<4; jj++) { NbHits[j][jj]=0; } } Int_t ntracks = (Int_t) diSim.NumberOfTracks(ievent); for ( Int_t itrack=0; itrack<ntracks; ++itrack ) { AliMUONVHitStore* hitStore = diSim.HitStore(ievent,itrack); AliMUONHit* mHit; TIter next(hitStore->CreateIterator()); while ( ( mHit = static_cast<AliMUONHit*>(next()) ) ) { Int_t Nch = mHit->Chamber(); Int_t hittrack = mHit->Track(); Float_t IdPart = mHit->Particle(); for (Int_t j=0;j<4;j++) { Int_t kch=11+j; if (hittrack==1 || hittrack==2) { if (Nch==kch && IdPart==-13 && NbHits[0][j]==0) NbHits[0][j]++; if (Nch==kch && IdPart==13 && NbHits[1][j]==0) NbHits[1][j]++; } } } } // end track loop // 3/4 coincidence Int_t SumNbHits=NbHits[0][0]+NbHits[0][1]+NbHits[0][2]+NbHits[0][3]; if (SumNbHits==3 || SumNbHits==4) CoincMuPlus=1; SumNbHits=NbHits[1][0]+NbHits[1][1]+NbHits[1][2]+NbHits[1][3]; if (SumNbHits==3 || SumNbHits==4) CoincMuMinus=1; if (CoincMuPlus==1 && CoincMuMinus==1) coincmuon++; TString tree("D"); if ( readFromRP ) tree = "R"; AliMUONVTriggerStore* triggerStore = diRec.TriggerStore(ievent,tree.Data()); AliMUONGlobalTrigger* gloTrg = triggerStore->Global(); if (gloTrg->PairUnlikeLpt()>=1) muonlpt++; if (gloTrg->PairUnlikeHpt()>=1) muonhpt++; } // end loop on event // calculate efficiency with as a ref. at least 3/4 planes fired Float_t efficiencylpt,efficiencyhpt; Double_t coincmu,lptmu,hptmu; Float_t error; coincmu=Double_t(coincmuon); lptmu=Double_t(muonlpt); hptmu=Double_t(muonhpt); // output fprintf(fdat,"\n"); fprintf(fdat," Number of events = %d \n",nevents); fprintf(fdat," Number of events with 3/4 coinc = %d \n",coincmuon); fprintf(fdat," Nomber of dimuons with 3/4 coinc Lpt cut = %d \n",muonlpt); fprintf(fdat," Number of dimuons with 3/4 coinc Hpt cut = %d \n",muonhpt); fprintf(fdat,"\n"); efficiencylpt=lptmu/coincmu; error=efficiencylpt*TMath::Sqrt((lptmu+coincmu)/(lptmu*coincmu)); fprintf(fdat," Efficiency Lpt cut = %4.4f +/- %4.4f\n",efficiencylpt,error); efficiencyhpt=hptmu/coincmu; error=efficiencyhpt*TMath::Sqrt((hptmu+coincmu)/(hptmu*coincmu)); fprintf(fdat," Efficiency Hpt cut = %4.4f +/- %4.4f\n",efficiencyhpt,error); fclose(fdat); }
void DenseReconstruction::activeSegmentation (const pcl::PointCloud<pcl::PointXYZLRegionF> &cloud_input, float search_radius, double eps_angle, int fp_index, pcl::PointIndices &indices_out) { pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>); pcl::PointCloud<pcl::Boundary> boundary; normalsEstimation(cloud_input.makeShared(),normals); boundaryEstimation(cloud_input.makeShared(),normals,boundary); std::cerr<<"Index of seed point is: "<<fp_index<<std::endl; std::cerr<<"Curvature of seed point: "<<normals->points[fp_index].curvature<<std::endl; pcl::PointCloud<pcl::PointXYZRGBA> cloud_in; pcl::copyPointCloud(cloud_input,cloud_in); pcl::search::KdTree<pcl::PointXYZRGBA>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGBA> ()); tree->setInputCloud(cloud_in.makeShared()); if (fp_index > cloud_in.points.size () || fp_index <0) { PCL_ERROR ("[pcl::activeSegmentation] Fixation point given is invalid\n"); return; } if (boundary.points.size () != cloud_in.points.size ()) { PCL_ERROR ("[pcl::activeSegmentation] Boundary map given was built for a different dataset (%zu) than the input cloud (%zu)!\n", boundary.points.size () , cloud_in.points.size ()); return; } if (tree->getInputCloud ()->points.size () != cloud_in.points.size ()) { PCL_ERROR ("[pcl::activeSegmentation] Tree built for a different point cloud dataset (%zu) than the input cloud (%zu)!\n", tree->getInputCloud ()->points.size (), cloud_in.points.size ()); return; } if (normals->points.size () != cloud_in.points.size ()) { PCL_ERROR ("[pcl::activeSegmentation] Input Normals are for a different point cloud dataset (%zu) than the input cloud (%zu)!\n", normals->points.size (), cloud_in.points.size ()); return; } std::vector<int> seed_queue; std::vector<bool> processed (cloud_in.size(), false); seed_queue.push_back (fp_index); indices_out.indices.push_back (fp_index); processed[fp_index] = true; std::vector<int> nn_indices; std::vector<float> nn_distances; //process while there are valid seed points for(size_t seed_idx = 0; seed_idx < seed_queue.size();++seed_idx) { int curr_seed; curr_seed = seed_queue[seed_idx]; // Search for seeds if (!tree->radiusSearch (curr_seed, search_radius, nn_indices, nn_distances)) continue; //process all points found in the neighborhood bool stop_growing = false; size_t indices_old_size = indices_out.indices.size(); for (size_t i=1; i < nn_indices.size (); ++i) { if (processed[nn_indices.at (i)]) continue; if (boundary.points[nn_indices.at (i)].boundary_point != 0) { stop_growing=true; indices_out.indices.push_back (nn_indices.at (i)); processed[nn_indices.at (i)] = true; break; } bool is_convex = false; pcl::PointXYZRGBA temp; temp.x = cloud_in.points[fp_index].x - cloud_in.points[nn_indices.at (i)].x; temp.y = cloud_in.points[fp_index].y - cloud_in.points[nn_indices.at (i)].y; temp.z = cloud_in.points[fp_index].z - cloud_in.points[nn_indices.at (i)].z; double dot_p = normals->points[nn_indices.at (i)].normal[0] * temp.x + normals->points[nn_indices.at (i)].normal[1] * temp.y + normals->points[nn_indices.at (i)].normal[2] * temp.z; dot_p = dot_p>1? 1:dot_p; dot_p = dot_p<-1 ? -1:dot_p; if ((acos (dot_p) > eps_angle*M_PI / 180)) is_convex = true; if (is_convex) { indices_out.indices.push_back (nn_indices.at (i)); processed[nn_indices.at (i)] = true; } else break; }//new neighbor if(!stop_growing && (indices_old_size != indices_out.indices.size())) { for (size_t j = indices_old_size-1; j < indices_out.indices.size(); ++j) seed_queue.push_back(indices_out.indices.at (j)); } }//new seed point }
int main (int argc, char** argv) { // Read in the cloud data pcl::PCDReader reader; pcl::PCDWriter writer; pcl::PointCloud<PointType>::Ptr cloud (new pcl::PointCloud<PointType>); std::vector<int> filenames; filenames = pcl::console::parse_file_extension_argument (argc, argv, ".pcd"); reader.read (argv[filenames[0]], *cloud); std::string pcd_filename; std::string png_filename = argv[filenames[0]]; // Take the origional png image out png::image<png::rgb_pixel> origin_image(cloud->width, cloud->height); int origin_index = 0; for (size_t y = 0; y < origin_image.get_height (); ++y) { for (size_t x = 0; x < origin_image.get_width (); ++x) { const PointType & p = cloud->points[origin_index++]; origin_image[y][x] = png::rgb_pixel(p.r, p.g, p.b); } } png_filename.replace(png_filename.length () - 4, 4, ".png"); origin_image.write(png_filename); std::cout << "PointCloud before filtering has: " << cloud->points.size () << " data points." << std::endl; //* float min_depth = 0.1; pcl::console::parse_argument (argc, argv, "-min_depth", min_depth); float max_depth = 3.0; pcl::console::parse_argument (argc, argv, "-max_depth", max_depth); float max_x = 1.0; pcl::console::parse_argument (argc, argv, "-max_x", max_x); float min_x = -1.0; pcl::console::parse_argument (argc, argv, "-min_x", min_x); float max_y = 1.0; pcl::console::parse_argument (argc, argv, "-max_y", max_y); float min_y = -1.0; pcl::console::parse_argument (argc, argv, "-min_y", min_y); int plane_seg_times = 1; pcl::console::parse_argument (argc, argv, "-plane_seg_times", plane_seg_times); for (int i = 0; i < plane_seg_times; i++) { remove_plane(cloud, min_depth, max_depth, min_x, max_x, min_y, max_y); } pcd_filename = argv[filenames[0]]; pcd_filename.replace(pcd_filename.length () - 4, 8, "plane.pcd"); pcl::io::savePCDFile(pcd_filename, *cloud); // std::cout << "PointCloud after removing the plane has: " << cloud->points.size () << " data points." << std::endl; //* uint32_t xmin = 1000, xmax = 0, ymin = 1000, ymax = 0; pcl::PointCloud<PointXYZRGBUV>::Ptr cloud_uv (new pcl::PointCloud<PointXYZRGBUV>); for (size_t index = 0; index < cloud->points.size(); index++) { const pcl::PointXYZRGB & p = cloud->points[index]; if (p.x != p.x || p.y != p.y || p.z != p.z) { // if current point is invalid continue; } PointXYZRGBUV cp = PointXYZRGBUV(p, index % cloud-> width, index / cloud->width); cloud_uv->points.push_back (cp); } cloud_uv->width = cloud_uv->points.size (); cloud_uv->height = 1; // Creating the KdTree object for the search method of the extraction pcl::search::KdTree<PointXYZRGBUV>::Ptr tree (new pcl::search::KdTree<PointXYZRGBUV>); tree->setInputCloud (cloud_uv); std::vector<pcl::PointIndices> cluster_indices; pcl::EuclideanClusterExtraction<PointXYZRGBUV> ec; ec.setClusterTolerance (0.02); // 2cm ec.setMinClusterSize (500); ec.setMaxClusterSize (25000); ec.setSearchMethod (tree); ec.setInputCloud (cloud_uv); ec.extract (cluster_indices); int j = 0; for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it) { pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZRGB>); xmin = 1000; xmax = 0; ymin = 1000; ymax = 0; for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit) { PointXYZRGBUV& p = cloud_uv->points[*pit]; pcl::PointXYZRGB cp_rgb; cp_rgb.x = p.x; cp_rgb.y = p.y; cp_rgb.z = p.z; cp_rgb.rgb = p.rgb; cloud_cluster->points.push_back(cp_rgb); xmin = std::min(xmin, p.u); xmax = std::max(xmax, p.u); ymin = std::min(ymin, p.v); ymax = std::max(ymax, p.v); } cloud_cluster->is_dense = true; cloud_cluster->width = cloud_cluster->points.size(); cloud_cluster->height = 1; std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl; pcd_filename = argv[filenames[0]]; std::stringstream ss; ss << "cluster_" << j++ << ".pcd"; pcd_filename.replace(pcd_filename.length () - 4, ss.str().length(), ss.str()); pcl::io::savePCDFile(pcd_filename, *cloud_cluster); png::image<png::rgb_pixel> image(cloud_cluster->width, cloud_cluster->height); int i = 0; for (size_t y = 0; y < image.get_height (); ++y) { for (size_t x = 0; x < image.get_width (); ++x) { const PointType & p = cloud_cluster->points[i++]; image[y][x] = png::rgb_pixel(p.r, p.g, p.b); } } pcd_filename.replace(pcd_filename.length () - 4, 4, ".png"); image.write(pcd_filename); //crop out image patch png::image<png::rgb_pixel> image_patch(xmax - xmin + 1, ymax - ymin + 1); for (size_t y = 0; y < image_patch.get_height (); ++y) { for (size_t x = 0; x < image_patch.get_width (); ++x) { image_patch[y][x] = origin_image[y+ymin][x+xmin]; } } pcd_filename.replace(pcd_filename.length () - 4, 7, "box.png"); image_patch.write(pcd_filename); // writer.write<pcl::PointXYZRGB> (ss.str (), *cloud_cluster, false); //* } return (0); }
obj* php_load_impl(const char* base, const char*& beg, const char* end) { #define THROW_INPUT_ERROR2(msg, ptr) \ do{ char buf[256]; \ int blen = snprintf(buf, sizeof(buf), "%s at pos: %ld", msg, long(ptr-base)); \ std::string strMsg(buf, blen); \ throw std::logic_error(strMsg); \ } while (0) #define THROW_INPUT_ERROR(msg) THROW_INPUT_ERROR2(msg, beg) switch (*beg) { default: { std::string msg = "php_load: invalid type: "; msg.push_back(*beg); msg += ", input:"; msg.append(beg, std::min<size_t>(100, end-beg)); throw std::logic_error(msg); } case 'i': { // i:val; if (++beg >= end) THROW_INPUT_ERROR("php_load: Incompleted php integer"); if (':' != *beg) THROW_INPUT_ERROR("php_load: integer, expect ':'"); char* pp = NULL; long long val = strtoll(beg+1, &pp, 10); if (';' != *pp) THROW_INPUT_ERROR2("php_load: integer, expect ';'", pp); beg = pp + 1; if (sizeof(long) == sizeof(long long) || (LONG_MIN <= val&&val <= LONG_MAX)) return new obj_long((long)val); else return new obj_llong(val); } case 'b': { // b:val; if (++beg >= end) THROW_INPUT_ERROR("php_load: Incompleted php boolean"); if (':' != *beg) THROW_INPUT_ERROR("php_load: boolean, expect ':'"); char* pp = NULL; long val = strtol(beg+1, &pp, 10); if (';' != *pp) THROW_INPUT_ERROR2("php_load: boolean, expect ';'", pp); beg = pp + 1; return new obj_bool(val ? 1 : 0); } #if 0 case 'd': { // d:val; if (++beg >= end) THROW_INPUT_ERROR("php_load: Incompleted php double"); if (':' != *beg) THROW_INPUT_ERROR("php_load: double, expect ':'"); char* pp = NULL; double val = strtod(beg+1, &pp); if (';' != *pp) THROW_INPUT_ERROR("php_load: double, expect ';'"); beg = pp + 1; return new obj_double(val); } #else case 'd': { // d:val; if (++beg >= end) THROW_INPUT_ERROR("php_load: Incompleted php double"); if (':' != *beg) THROW_INPUT_ERROR("php_load: long double, expect ':'"); char* pp = NULL; #ifdef __CYGWIN__ //_LDBL_EQ_DBL double val = strtod(beg+1, &pp); #else long double val = strtold(beg+1, &pp); #endif if (';' != *pp) THROW_INPUT_ERROR2("php_load: long double, expect ';'", pp); beg = pp + 1; return new obj_ldouble(val); } #endif case 's': { // s:size:"content"; if (++beg >= end) THROW_INPUT_ERROR("php_load: Incompleted php string"); if (':' != *beg) THROW_INPUT_ERROR("php_load: string.size expect ':'"); char* pp = NULL; long len = strtol(beg+1, &pp, 10); if (':' != *pp) THROW_INPUT_ERROR2("php_load: string.content expect ':'", pp); if (len < 0) THROW_INPUT_ERROR2("php_load: string.size is negtive", beg+1); if ('"' != pp[1]) THROW_INPUT_ERROR2("php_load: string.content expect '\"'", pp+1); if ('"' != pp[len+2]) THROW_INPUT_ERROR2("php_load: string.content not found right quote", pp+len+2); if (';' != pp[len+3]) THROW_INPUT_ERROR2("php_load: string didn't end with ';'", pp+len+3); std::auto_ptr<obj_string> x(new obj_string); x->resize(len); x->assign(pp+2, len); beg = pp + len + 4; // s:size:"content"; return x.release(); } case 'a': { // A:size:{key;value; ....} // fprintf(stderr, "loading array: "); if (++beg >= end) THROW_INPUT_ERROR("php_load: Incompleted php array"); if (':' != *beg) THROW_INPUT_ERROR("php_load: array.size expect ':'"); char* pp = NULL; long len = strtol(beg+1, &pp, 10); // fprintf(stderr, "size=%ld\n", len); if (':' != pp[0] || '{' != pp[1]) THROW_INPUT_ERROR2("php_load: array.size should followed by ':{'", pp); std::auto_ptr<obj_array> arr(new obj_array); std::auto_ptr<php_array> map; arr->resize(2*len); beg = pp + 2; long max_idx = -1; for (long i = 0; i < len; ++i) { // fprintf(stderr, "loading array[%ld]:\n", i); const char* key_pos = beg; std::auto_ptr<obj> key(php_load_impl(base, beg, end)); if (key.get() == NULL) { THROW_INPUT_ERROR2("php_load: array.key must not be NULL", key_pos); } if (arr.get()) { obj_long* l = dynamic_cast<obj_long*>(key.get()); if (l) { long idx = l->t; if (idx >= 0 && idx < (long)arr->size()) { (*arr)[idx].reset(php_load_impl(base, beg, end)); max_idx = std::max(idx, max_idx); continue; } } } if (map.get() == NULL) map.reset(new php_array); if (arr.get()) { for (long j = 0; j <= max_idx; ++j) { if ((*arr)[j]) (*map)[obj_ptr(new obj_long(j))] = (*arr)[j]; } arr.reset(NULL); } (*map)[obj_ptr(key.get())].reset(php_load_impl(base, beg, end)); key.release(); } if ('}' != *beg) THROW_INPUT_ERROR("php_load: array not correctly closed"); beg += 1; if (arr.get()) { arr->resize(max_idx+1); arr->shrink_to_fit(); return arr.release(); } return map.release(); } case 'N': { if (++beg >= end) THROW_INPUT_ERROR("php_load: Incompleted php NULL"); if (';' != *beg) THROW_INPUT_ERROR("php_load: NULL expect ';'"); beg += 1; return NULL; } case 'O': { // O:strlen(class name):"class name":fields_num:{s:strlen(field name):"field name":field definition;(repeated per field)} if (++beg >= end) THROW_INPUT_ERROR("php_load: Incompleted php Object"); if (':' != *beg) THROW_INPUT_ERROR("php_load: Object.namelen expect ':' 1"); char* pp = NULL; long len = strtol(beg+1, &pp, 10); if (':' != pp[0] && '"' != pp[1]) THROW_INPUT_ERROR2("php_load: Object.namelen expect ':\"' 2", pp); if (pp + 4 + len > end) THROW_INPUT_ERROR2("php_load: Object 3", pp); std::auto_ptr<php_object> tree(new php_object); tree->cls_name.assign(pp + 2, len); long fields = strtol(pp + 4 + len, &pp, 10); if (':' != pp[0] || '{' != pp[1]) THROW_INPUT_ERROR2("php_load: Object 4", pp); beg = pp + 2; for (long i = 0; i < fields; ++i) { std::auto_ptr<obj> pname(php_load_impl(base, beg, end)); obj_string* name = dynamic_cast<obj_string*>(pname.get()); if (NULL == name) THROW_INPUT_ERROR("object field name is not a string"); std::auto_ptr<obj> value(php_load_impl(base, beg, end)); tree->fields[*name].reset(value.release()); } if ('}' != beg[0]) THROW_INPUT_ERROR("php_load: Object not correctly closed"); beg += 1; return tree.release(); } } assert(0); return NULL; }
tree edit_env_rep::rewrite_inactive_default ( tree t, tree var, bool block, bool flush) { int i, d= 0, n= N(t); tree op= as_string (L(t)); if ((L(t) == COMPOUND) && is_atomic (t[0]) && (src_special >= SPECIAL_NORMAL)) { d = 1; op= highlight (subvar (var, 0), t[0], TYPE_VARIABLE); } if (inactive_mode == INACTIVE_INLINE_ERROR || inactive_mode == INACTIVE_BLOCK_ERROR) op= highlight (op, "", TYPE_INVALID); if ((N(t) == d) || (src_compact == COMPACT_ALL) || ((!block) && (src_compact != COMPACT_NONE)) || ((!is_long (t)) && (src_compact != COMPACT_NONE))) { tree r (INLINE_TAG, n+1-d); r[0]= op; for (i=d; i<n; i++) r[i+1-d]= rewrite_inactive_arg (t, var, i, false, false); return tree (MARK, var, r); } else { tree doc (DOCUMENT); bool compact= (src_compact < COMPACT_INLINE); for (i=d; i<n; i++) { tree next; if ((!compact) || is_long_arg (t, i)) { if (i==d) doc << tree (OPEN_TAG, op); next= rewrite_inactive_arg (t, var, i, true, src_close >= CLOSE_LONG); next= compound ("indent*", next); i++; } int start= i; for (; i<n; i++) if ((!compact) || is_long_arg (t, i)) break; int end= i; tree_label l= MIDDLE_TAG; if (end == n) l= CLOSE_TAG; if (start == d) l= OPEN_TAG; tree u (l, end - start + 1); u[0]= op; for (i=0; i<end-start; i++) u[i+1]= rewrite_inactive_arg (t, var, start+i, false, false); i= end-1; compact= (src_compact < COMPACT_INLINE_START); if (start==d) doc << u; else { if (src_close < CLOSE_LONG) doc << tree (SURROUND, "", u, next); else doc << next << u; } } if (flush) doc= tree (SURROUND, "", compound ("right-flush"), doc); return tree (MARK, var, doc); } }
void ClusterExtraction::extract() { /* pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = in_pcl.read(); // Creating the KdTree object for the search method of the extraction pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>); tree->setInputCloud (cloud); std::vector<pcl::PointIndices> cluster_indices; pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec; ec.setClusterTolerance (clusterTolerance); // 2cm ec.setMinClusterSize (minClusterSize); ec.setMaxClusterSize (maxClusterSize); ec.setSearchMethod (tree); ec.setInputCloud (cloud); ec.extract (cluster_indices); std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clusters; int j = 0; for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it) { pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>); for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++) cloud_cluster->points.push_back (cloud->points[*pit]); //* cloud_cluster->width = cloud_cluster->points.size (); cloud_cluster->height = 1; cloud_cluster->is_dense = true; clusters.push_back(cloud_cluster); std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl; std::stringstream ss; // if(j==0) // { // ss << "cloud_cluster_0" << ".pcd"; // writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false); // j++; // } // if(j==0) //{ std::cin.ignore(); out_the_biggest_cluster.write(cloud_cluster); j++; std::cout<<"Zapis!!!\n"; // } } out_indices.write(cluster_indices); out_clusters.write(clusters); //std::cout<<"j=="<<j<<endl; //std::cout<<clusters.size()<<endl; */ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZ>); // Read in the cloud data pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = in_pcl.read(); std::cout << "PointCloud before filtering has: " << cloud->points.size () << " data points." << std::endl; //* // Create the filtering object: downsample the dataset using a leaf size of 1cm pcl::VoxelGrid<pcl::PointXYZ> vg; pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>); vg.setInputCloud (cloud); vg.setLeafSize (0.01f, 0.01f, 0.01f); vg.filter (*cloud_filtered); std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl; //* // Create the segmentation object for the planar model and set all the parameters pcl::SACSegmentation<pcl::PointXYZ> seg; pcl::PointIndices::Ptr inliers (new pcl::PointIndices); pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ()); //pcl::PCDWriter writer; seg.setOptimizeCoefficients (true); seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); seg.setMaxIterations (100); seg.setDistanceThreshold (0.02); int i=0, nr_points = (int) cloud_filtered->points.size (); /* while (cloud_filtered->points.size () > 0.3 * nr_points) { // Segment the largest planar component from the remaining cloud seg.setInputCloud (cloud_filtered); seg.segment (*inliers, *coefficients); if (inliers->indices.size () == 0) { std::cout << "Could not estimate a planar model for the given dataset." << std::endl; break; } // Extract the planar inliers from the input cloud pcl::ExtractIndices<pcl::PointXYZ> extract; extract.setInputCloud (cloud_filtered); extract.setIndices (inliers); extract.setNegative (false); // Get the points associated with the planar surface extract.filter (*cloud_plane); std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl; // Remove the planar inliers, extract the rest extract.setNegative (true); extract.filter (*cloud_f); *cloud_filtered = *cloud_f; } */ // Creating the KdTree object for the search method of the extraction pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>); tree->setInputCloud (cloud_filtered); std::vector<pcl::PointIndices> cluster_indices; pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec; ec.setClusterTolerance (0.02); // 2cm ec.setMinClusterSize (100); ec.setMaxClusterSize (25000); ec.setSearchMethod (tree); ec.setInputCloud (cloud_filtered); ec.extract (cluster_indices); int j = 0; for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it) { pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>); for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++) cloud_cluster->points.push_back (cloud_filtered->points[*pit]); //* cloud_cluster->width = cloud_cluster->points.size (); cloud_cluster->height = 1; cloud_cluster->is_dense = true; std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl; //std::stringstream ss; //ss << "cloud_cluster_" << j << ".pcd"; //writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false); //* //std::cin.ignore(); // check if cluster contain interesting us plane, if so, return cluster, next remove plane and we have object :) //if (include_plane){ out_the_biggest_cluster.write(cloud_cluster); break; //} j++; } }
int PointCloudStitching::stitch(pcl::PointCloud<PointT>& points, double epsilon, double maxCorrespondanceDistance) { if (this->stitching.size() == 0) { pcl::copyPointCloud(points, this->stitching); return 0; } // Compute surface normals and curvature pcl::PointCloud<PointT> tempTarget; pcl::copyPointCloud(this->stitching, tempTarget); pcl::PointCloud<pcl::PointNormal>::Ptr pointsWithNormalSource (new pcl::PointCloud<pcl::PointNormal>); pcl::PointCloud<pcl::PointNormal>::Ptr pointsWithNormalTarget (new pcl::PointCloud<pcl::PointNormal>); pcl::NormalEstimation<PointT, pcl::PointNormal> normalEstimate; pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT> ()); normalEstimate.setSearchMethod(tree); normalEstimate.setKSearch(30); normalEstimate.setInputCloud(points.makeShared()); normalEstimate.compute(*pointsWithNormalSource); pcl::copyPointCloud(points, *pointsWithNormalSource); normalEstimate.setInputCloud (tempTarget.makeShared()); normalEstimate.compute(*pointsWithNormalTarget); pcl::copyPointCloud (tempTarget, *pointsWithNormalTarget); // Instantiate custom point representation MyPointNormal pointNormal; // ... and weight the 'curvature' dimension so that it is balanced against x, y, and z float alpha[4] = {1.0, 1.0, 1.0, 1.0}; pointNormal.setRescaleValues(alpha); // Align pcl::IterativeClosestPointNonLinear<pcl::PointNormal, pcl::PointNormal> registration; registration.setTransformationEpsilon(epsilon); // Set the maximum distance between two correspondences registration.setMaxCorrespondenceDistance(maxCorrespondanceDistance); // Set the point representation registration.setPointRepresentation (boost::make_shared<const MyPointNormal> (pointNormal)); registration.setInputSource(pointsWithNormalSource); registration.setInputTarget(pointsWithNormalTarget); registration.setMaximumIterations(30); PCL_ERROR("Source size: %d -- Target size: %d\n", (int)pointsWithNormalSource.get()->size(), (int)pointsWithNormalTarget.get()->size()); Eigen::Matrix4f tf = Eigen::Matrix4f::Identity(); pcl::PointCloud<pcl::PointNormal>::Ptr regResult = pointsWithNormalSource; PCL_ERROR("Stitching ... "); registration.align(*regResult); PCL_ERROR("Done!\n"); tf = registration.getFinalTransformation().inverse(); // PCL_ERROR("\nTransform:\n"); // PCL_ERROR("| %f %f %f %f |\n", tf(0,0), tf(0,1), tf (0,2), tf(0,3)); // PCL_ERROR("| %f %f %f %f |\n", tf(1,0), tf(1,1), tf (1,2), tf(1,3)); // PCL_ERROR("| %f %f %f %f |\n", tf(2,0), tf(2,1), tf (2,2), tf(2,3)); // PCL_ERROR("| %f %f %f %f |\n\n", tf(3,0), tf(3,1), tf (3,2), tf(3,3)); pcl::transformPointCloud(*pointsWithNormalSource, *regResult, tf); *regResult += *pointsWithNormalTarget; pcl::copyPointCloud(*regResult, this->stitching); return 0; }
unsigned int scale_of_noise (const Point_set& points, double& size) { Tree tree(points.begin(), points.end()); double ratio_kept = (points.size() < 1000) ? 1. : 1000. / (points.size()); std::vector<Point> subset; for (std::size_t i = 0; i < points.size (); ++ i) if (rand() / (double)RAND_MAX < ratio_kept) subset.push_back (points[i]); std::vector<unsigned int> scales; generate_scales (std::back_inserter (scales)); std::vector<unsigned int> chosen; for (std::size_t i = 0; i < subset.size (); ++ i) { Neighbor_search search(tree, subset[i],scales.back()); double current = 0.; unsigned int nb = 0; std::size_t index = 0; double minimum = (std::numeric_limits<double>::max)(); unsigned int c = 0; for (Search_iterator search_iterator = search.begin(); search_iterator != search.end (); ++ search_iterator, ++ nb) { current += search_iterator->second; if (nb + 1 == scales[index]) { double score = std::sqrt (current / scales[index]) / std::pow (scales[index], 0.375); // NB ^ (5/12) if (score < minimum) { minimum = score; c = scales[index]; } ++ index; if (index == scales.size ()) break; } } chosen.push_back (c); } std::sort (chosen.begin (), chosen.end()); unsigned int noise_scale = chosen[chosen.size() / 2]; size = 0.; for (std::size_t i = 0; i < subset.size (); ++ i) { Neighbor_search search(tree, subset[i], noise_scale); size += std::sqrt ((-- search.end())->second); } size /= subset.size(); return noise_scale; }
void BinomialVanillaEngine<T>::calculate() const { DayCounter rfdc = process_->riskFreeRate()->dayCounter(); DayCounter divdc = process_->dividendYield()->dayCounter(); DayCounter voldc = process_->blackVolatility()->dayCounter(); Calendar volcal = process_->blackVolatility()->calendar(); Real s0 = process_->stateVariable()->value(); QL_REQUIRE(s0 > 0.0, "negative or null underlying given"); Volatility v = process_->blackVolatility()->blackVol( arguments_.exercise->lastDate(), s0); Date maturityDate = arguments_.exercise->lastDate(); Rate r = process_->riskFreeRate()->zeroRate(maturityDate, rfdc, Continuous, NoFrequency); Rate q = process_->dividendYield()->zeroRate(maturityDate, divdc, Continuous, NoFrequency); Date referenceDate = process_->riskFreeRate()->referenceDate(); // binomial trees with constant coefficient Handle<YieldTermStructure> flatRiskFree( boost::shared_ptr<YieldTermStructure>( new FlatForward(referenceDate, r, rfdc))); Handle<YieldTermStructure> flatDividends( boost::shared_ptr<YieldTermStructure>( new FlatForward(referenceDate, q, divdc))); Handle<BlackVolTermStructure> flatVol( boost::shared_ptr<BlackVolTermStructure>( new BlackConstantVol(referenceDate, volcal, v, voldc))); boost::shared_ptr<PlainVanillaPayoff> payoff = boost::dynamic_pointer_cast<PlainVanillaPayoff>(arguments_.payoff); QL_REQUIRE(payoff, "non-plain payoff given"); Time maturity = rfdc.yearFraction(referenceDate, maturityDate); boost::shared_ptr<StochasticProcess1D> bs( new GeneralizedBlackScholesProcess( process_->stateVariable(), flatDividends, flatRiskFree, flatVol)); TimeGrid grid(maturity, timeSteps_); boost::shared_ptr<T> tree(new T(bs, maturity, timeSteps_, payoff->strike())); boost::shared_ptr<BlackScholesLattice<T> > lattice( new BlackScholesLattice<T>(tree, r, maturity, timeSteps_)); DiscretizedVanillaOption option(arguments_, *process_, grid); option.initialize(lattice, maturity); // Partial derivatives calculated from various points in the // binomial tree (Odegaard) // Rollback to third-last step, and get underlying price (s2) & // option values (p2) at this point option.rollback(grid[2]); Array va2(option.values()); QL_ENSURE(va2.size() == 3, "Expect 3 nodes in grid at second step"); Real p2h = va2[2]; // high-price Real s2 = lattice->underlying(2, 2); // high price // Rollback to second-last step, and get option value (p1) at // this point option.rollback(grid[1]); Array va(option.values()); QL_ENSURE(va.size() == 2, "Expect 2 nodes in grid at first step"); Real p1 = va[1]; // Finally, rollback to t=0 option.rollback(0.0); Real p0 = option.presentValue(); Real s1 = lattice->underlying(1, 1); // Calculate partial derivatives Real delta0 = (p1-p0)/(s1-s0); // dp/ds Real delta1 = (p2h-p1)/(s2-s1); // dp/ds // Store results results_.value = p0; results_.delta = delta0; results_.gamma = 2.0*(delta1-delta0)/(s2-s0); //d(delta)/ds results_.theta = blackScholesTheta(process_, results_.value, results_.delta, results_.gamma); }
bool cloud_cb(data_collection::process_cloud::Request &req, data_collection::process_cloud::Response &res) { //Segment from cloud: //May need to tweak segmentation parameters to get just the cluster I'm looking for. std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clouds; nrg_object_recognition::segmentation seg_srv; seg_srv.request.scene = req.in_cloud; seg_srv.request.min_x = -.75, seg_srv.request.max_x = .4; seg_srv.request.min_y = -5, seg_srv.request.max_y = .5; seg_srv.request.min_z = 0.0, seg_srv.request.max_z = 1.15; seg_client.call(seg_srv); //SegmentCloud(req.in_cloud, clouds); //For parameter tweaking, may be good to write all cluseters to file here for examination in pcd viewer. pcl::PointCloud<pcl::PointXYZ>::Ptr cluster (new pcl::PointCloud<pcl::PointXYZ>); //cluster = clouds.at(0); pcl::fromROSMsg(seg_srv.response.clusters.at(0), *cluster); //std::cout << "found " << clouds.size() << " clusters.\n"; std::cout << "cluster has " << cluster->height*cluster->width << " points.\n"; //Write raw pcd file (objecName_angle.pcd) std::stringstream fileName_ss; fileName_ss << "data/" << req.objectName << "_" << req.angle << ".pcd"; std::cout << "writing raw cloud to file...\n"; std::cout << fileName_ss.str() << std::endl; pcl::io::savePCDFile(fileName_ss.str(), *cluster); std::cout << "done.\n"; //Write vfh feature to file: pcl::VFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::VFHSignature308> vfh; vfh.setInputCloud (cluster); //Estimate normals: pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne; ne.setInputCloud (cluster); pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ()); ne.setSearchMethod (tree); pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>); ne.setRadiusSearch (0.03); ne.compute (*cloud_normals); vfh.setInputNormals (cloud_normals); //Estimate vfh: vfh.setSearchMethod (tree); pcl::PointCloud<pcl::VFHSignature308>::Ptr vfhs (new pcl::PointCloud<pcl::VFHSignature308> ()); // Compute the feature vfh.compute (*vfhs); //Write to file: (objectName_angle_vfh.pcd) fileName_ss.str(""); std::cout << "writing vfh descriptor to file...\n"; fileName_ss << "data/" << req.objectName << "_" << req.angle << "_vfh.pcd"; pcl::io::savePCDFile(fileName_ss.str(), *vfhs); std::cout << "done.\n"; //Extract cph std::vector<float> feature; CPHEstimation cph(5,72); cph.setInputCloud(cluster); cph.compute(feature); //Write cph to file. (objectName_angle.csv) std::ofstream outFile; fileName_ss.str(""); fileName_ss << "data/" << req.objectName << "_" << req.angle << ".csv"; outFile.open(fileName_ss.str().c_str()); std::cout << "writing cph descriptor to file...\n"; for(unsigned int j=0; j<feature.size(); j++){ outFile << feature.at(j) << " "; } outFile.close(); fileName_ss.str(""); std::cout << "done.\n"; res.result = 1; }
void BinomialBarrierEngine<T,D>::calculate() const { DayCounter rfdc = process_->riskFreeRate()->dayCounter(); DayCounter divdc = process_->dividendYield()->dayCounter(); DayCounter voldc = process_->blackVolatility()->dayCounter(); Calendar volcal = process_->blackVolatility()->calendar(); Real s0 = process_->stateVariable()->value(); QL_REQUIRE(s0 > 0.0, "negative or null underlying given"); Volatility v = process_->blackVolatility()->blackVol( arguments_.exercise->lastDate(), s0); Date maturityDate = arguments_.exercise->lastDate(); Rate r = process_->riskFreeRate()->zeroRate(maturityDate, rfdc, Continuous, NoFrequency); Rate q = process_->dividendYield()->zeroRate(maturityDate, divdc, Continuous, NoFrequency); Date referenceDate = process_->riskFreeRate()->referenceDate(); // binomial trees with constant coefficient Handle<YieldTermStructure> flatRiskFree( boost::shared_ptr<YieldTermStructure>( new FlatForward(referenceDate, r, rfdc))); Handle<YieldTermStructure> flatDividends( boost::shared_ptr<YieldTermStructure>( new FlatForward(referenceDate, q, divdc))); Handle<BlackVolTermStructure> flatVol( boost::shared_ptr<BlackVolTermStructure>( new BlackConstantVol(referenceDate, volcal, v, voldc))); boost::shared_ptr<StrikedTypePayoff> payoff = boost::dynamic_pointer_cast<StrikedTypePayoff>(arguments_.payoff); QL_REQUIRE(payoff, "non-striked payoff given"); Time maturity = rfdc.yearFraction(referenceDate, maturityDate); boost::shared_ptr<StochasticProcess1D> bs( new GeneralizedBlackScholesProcess( process_->stateVariable(), flatDividends, flatRiskFree, flatVol)); // correct timesteps to ensure a (local) minimum, using Boyle and Lau // approach. See Journal of Derivatives, 1/1994, // "Bumping up against the barrier with the binomial method" // Note: this approach works only for CoxRossRubinstein lattices, so // is disabled if T is not a CoxRossRubinstein or derived from it. Size optimum_steps = timeSteps_; if (boost::is_base_of<CoxRossRubinstein, T>::value && maxTimeSteps_ > timeSteps_ && s0 > 0 && arguments_.barrier > 0) { Real divisor; if (s0 > arguments_.barrier) divisor = std::pow(std::log(s0 / arguments_.barrier), 2); else divisor = std::pow(std::log(arguments_.barrier / s0), 2); if (!close(divisor,0)) { for (Size i=1; i < timeSteps_ ; ++i) { Size optimum = Size(( i*i * v*v * maturity) / divisor); if (timeSteps_ < optimum) { optimum_steps = optimum; break; // found first minimum with iterations>=timesteps } } } if (optimum_steps > maxTimeSteps_) optimum_steps = maxTimeSteps_; // too high, limit } TimeGrid grid(maturity, optimum_steps); boost::shared_ptr<T> tree(new T(bs, maturity, optimum_steps, payoff->strike())); boost::shared_ptr<BlackScholesLattice<T> > lattice( new BlackScholesLattice<T>(tree, r, maturity, optimum_steps)); D option(arguments_, *process_, grid); option.initialize(lattice, maturity); // Partial derivatives calculated from various points in the // binomial tree // (see J.C.Hull, "Options, Futures and other derivatives", 6th edition, pp 397/398) // Rollback to third-last step, and get underlying prices (s2) & // option values (p2) at this point option.rollback(grid[2]); Array va2(option.values()); QL_ENSURE(va2.size() == 3, "Expect 3 nodes in grid at second step"); Real p2u = va2[2]; // up Real p2m = va2[1]; // mid Real p2d = va2[0]; // down (low) Real s2u = lattice->underlying(2, 2); // up price Real s2m = lattice->underlying(2, 1); // middle price Real s2d = lattice->underlying(2, 0); // down (low) price // calculate gamma by taking the first derivate of the two deltas Real delta2u = (p2u - p2m)/(s2u-s2m); Real delta2d = (p2m-p2d)/(s2m-s2d); Real gamma = (delta2u - delta2d) / ((s2u-s2d)/2); // Rollback to second-last step, and get option values (p1) at // this point option.rollback(grid[1]); Array va(option.values()); QL_ENSURE(va.size() == 2, "Expect 2 nodes in grid at first step"); Real p1u = va[1]; Real p1d = va[0]; Real s1u = lattice->underlying(1, 1); // up (high) price Real s1d = lattice->underlying(1, 0); // down (low) price Real delta = (p1u - p1d) / (s1u - s1d); // Finally, rollback to t=0 option.rollback(0.0); Real p0 = option.presentValue(); // Store results results_.value = p0; results_.delta = delta; results_.gamma = gamma; // theta can be approximated by calculating the numerical derivative // between mid value at third-last step and at t0. The underlying price // is the same, only time varies. results_.theta = (p2m - p0) / grid[2]; }