void PointcloudDrawer::setScanGraph(const ScanGraph& graph){ clear(); // count points first: for (octomap::ScanGraph::const_iterator it = graph.begin(); it != graph.end(); it++) { m_numberPoints += (*it)->scan->size(); } m_pointsArray = new GLfloat[3*m_numberPoints]; unsigned i = 0; for (octomap::ScanGraph::const_iterator graph_it = graph.begin(); graph_it != graph.end(); graph_it++) { octomap::Pointcloud* scan = new Pointcloud((*graph_it)->scan); scan->transformAbsolute((*graph_it)->pose); for (Pointcloud::iterator pc_it = scan->begin(); pc_it != scan->end(); ++pc_it){ m_pointsArray[3*i] = pc_it->x(); m_pointsArray[3*i +1] = pc_it->y(); m_pointsArray[3*i +2] = pc_it->z(); i++; } delete scan; } }
int main(int argc, char** argv) { if (argc != 4) { printf("usage: in.graph offset out.graph\n"); exit(0); } ScanGraph* graph = new ScanGraph(); graph->readBinary(argv[1]); double offset = atof(argv[2]); Pose6D trans(0,0,-offset,0,0,0); for (ScanGraph::iterator scan_it = graph->begin(); scan_it != graph->end(); scan_it++) { (*scan_it)->scan->transform(trans); (*scan_it)->pose *= trans.inv(); } graph->writeBinary(argv[3]); return 0; }
int main(int argc, char** argv) { // default values: double res = 0.1; if (argc < 2) printUsage(argv[0]); string graphFilename = std::string(argv[1]); double maxrange = -1; int max_scan_no = -1; int skip_scan_eval = 5; int arg = 1; while (++arg < argc) { if (! strcmp(argv[arg], "-i")) graphFilename = std::string(argv[++arg]); else if (! strcmp(argv[arg], "-res")) res = atof(argv[++arg]); else if (! strcmp(argv[arg], "-m")) maxrange = atof(argv[++arg]); else if (! strcmp(argv[arg], "-n")) max_scan_no = atoi(argv[++arg]); else { printUsage(argv[0]); } } cout << "\nReading Graph file\n===========================\n"; ScanGraph* graph = new ScanGraph(); if (!graph->readBinary(graphFilename)) exit(2); size_t num_points_in_graph = 0; if (max_scan_no > 0) { num_points_in_graph = graph->getNumPoints(max_scan_no-1); cout << "\n Data points in graph up to scan " << max_scan_no << ": " << num_points_in_graph << endl; } else { num_points_in_graph = graph->getNumPoints(); cout << "\n Data points in graph: " << num_points_in_graph << endl; } cout << "\nCreating tree\n===========================\n"; OcTree* tree = new OcTree(res); size_t numScans = graph->size(); unsigned int currentScan = 1; for (ScanGraph::iterator scan_it = graph->begin(); scan_it != graph->end(); scan_it++) { if (currentScan % skip_scan_eval != 0){ if (max_scan_no > 0) cout << "("<<currentScan << "/" << max_scan_no << ") " << flush; else cout << "("<<currentScan << "/" << numScans << ") " << flush; tree->insertPointCloud(**scan_it, maxrange); } else cout << "(SKIP) " << flush; if ((max_scan_no > 0) && (currentScan == (unsigned int) max_scan_no)) break; currentScan++; } tree->expand(); cout << "\nEvaluating scans\n===========================\n"; currentScan = 1; size_t num_points = 0; size_t num_voxels_correct = 0; size_t num_voxels_wrong = 0; size_t num_voxels_unknown = 0; for (ScanGraph::iterator scan_it = graph->begin(); scan_it != graph->end(); scan_it++) { if (currentScan % skip_scan_eval == 0){ if (max_scan_no > 0) cout << "("<<currentScan << "/" << max_scan_no << ") " << flush; else cout << "("<<currentScan << "/" << numScans << ") " << flush; pose6d frame_origin = (*scan_it)->pose; point3d sensor_origin = frame_origin.inv().transform((*scan_it)->pose.trans()); // transform pointcloud: Pointcloud scan (*(*scan_it)->scan); scan.transform(frame_origin); point3d origin = frame_origin.transform(sensor_origin); KeySet free_cells, occupied_cells; tree->computeUpdate(scan, origin, free_cells, occupied_cells, maxrange); num_points += scan.size(); // count free cells for (KeySet::iterator it = free_cells.begin(); it != free_cells.end(); ++it) { OcTreeNode* n = tree->search(*it); if (n){ if (tree->isNodeOccupied(n)) num_voxels_wrong++; else num_voxels_correct++; } else num_voxels_unknown++; } // count occupied cells for (KeySet::iterator it = occupied_cells.begin(); it != occupied_cells.end(); ++it) { OcTreeNode* n = tree->search(*it); if (n){ if (tree->isNodeOccupied(n)) num_voxels_correct++; else num_voxels_wrong++; } else num_voxels_unknown++; } } if ((max_scan_no > 0) && (currentScan == (unsigned int) max_scan_no)) break; currentScan++; } cout << "\nFinished evaluating " << num_points <<"/"<< num_points_in_graph << " points.\n" <<"Voxels correct: "<<num_voxels_correct<<" #wrong: " <<num_voxels_wrong << " #unknown: " <<num_voxels_unknown <<". % correct: "<< num_voxels_correct/double(num_voxels_correct+num_voxels_wrong)<<"\n\n"; delete graph; delete tree; return 0; }
int main(int argc, char** argv) { // default values: double res = 0.1; string graphFilename = ""; string treeFilename = ""; double maxrange = -1; int max_scan_no = -1; bool detailedLog = false; bool simpleUpdate = false; bool discretize = false; unsigned char compression = 1; // get default sensor model values: OcTree emptyTree(0.1); double clampingMin = emptyTree.getClampingThresMin(); double clampingMax = emptyTree.getClampingThresMax(); double probMiss = emptyTree.getProbMiss(); double probHit = emptyTree.getProbHit(); timeval start; timeval stop; int arg = 0; while (++arg < argc) { if (! strcmp(argv[arg], "-i")) graphFilename = std::string(argv[++arg]); else if (!strcmp(argv[arg], "-o")) treeFilename = std::string(argv[++arg]); else if (! strcmp(argv[arg], "-res") && argc-arg < 2) printUsage(argv[0]); else if (! strcmp(argv[arg], "-res")) res = atof(argv[++arg]); else if (! strcmp(argv[arg], "-log")) detailedLog = true; else if (! strcmp(argv[arg], "-simple")) simpleUpdate = true; else if (! strcmp(argv[arg], "-discretize")) discretize = true; else if (! strcmp(argv[arg], "-compress")) OCTOMAP_WARNING("Argument -compress no longer has an effect, incremental pruning is done during each insertion.\n"); else if (! strcmp(argv[arg], "-compressML")) compression = 2; else if (! strcmp(argv[arg], "-m")) maxrange = atof(argv[++arg]); else if (! strcmp(argv[arg], "-n")) max_scan_no = atoi(argv[++arg]); else if (! strcmp(argv[arg], "-clamping") && (argc-arg < 3)) printUsage(argv[0]); else if (! strcmp(argv[arg], "-clamping")){ clampingMin = atof(argv[++arg]); clampingMax = atof(argv[++arg]); } else if (! strcmp(argv[arg], "-sensor") && (argc-arg < 3)) printUsage(argv[0]); else if (! strcmp(argv[arg], "-sensor")){ probMiss = atof(argv[++arg]); probHit = atof(argv[++arg]); } else { printUsage(argv[0]); } } if (graphFilename == "" || treeFilename == "") printUsage(argv[0]); // verify input: if (res <= 0.0){ OCTOMAP_ERROR("Resolution must be positive"); exit(1); } if (clampingMin >= clampingMax || clampingMin < 0.0 || clampingMax > 1.0){ OCTOMAP_ERROR("Error in clamping values: 0.0 <= [%f] < [%f] <= 1.0\n", clampingMin, clampingMax); exit(1); } if (probMiss >= probHit || probMiss < 0.0 || probHit > 1.0){ OCTOMAP_ERROR("Error in sensor model (hit/miss prob.): 0.0 <= [%f] < [%f] <= 1.0\n", probMiss, probHit); exit(1); } std::string treeFilenameOT = treeFilename + ".ot"; std::string treeFilenameMLOT = treeFilename + "_ml.ot"; cout << "\nReading Graph file\n===========================\n"; ScanGraph* graph = new ScanGraph(); if (!graph->readBinary(graphFilename)) exit(2); unsigned int num_points_in_graph = 0; if (max_scan_no > 0) { num_points_in_graph = graph->getNumPoints(max_scan_no-1); cout << "\n Data points in graph up to scan " << max_scan_no << ": " << num_points_in_graph << endl; } else { num_points_in_graph = graph->getNumPoints(); cout << "\n Data points in graph: " << num_points_in_graph << endl; } // transform pointclouds first, so we can directly operate on them later for (ScanGraph::iterator scan_it = graph->begin(); scan_it != graph->end(); scan_it++) { pose6d frame_origin = (*scan_it)->pose; point3d sensor_origin = frame_origin.inv().transform((*scan_it)->pose.trans()); (*scan_it)->scan->transform(frame_origin); point3d transformed_sensor_origin = frame_origin.transform(sensor_origin); (*scan_it)->pose = pose6d(transformed_sensor_origin, octomath::Quaternion()); } std::ofstream logfile; if (detailedLog){ logfile.open((treeFilename+".log").c_str()); logfile << "# Memory of processing " << graphFilename << " over time\n"; logfile << "# Resolution: "<< res <<"; compression: " << int(compression) << "; scan endpoints: "<< num_points_in_graph << std::endl; logfile << "# [scan number] [bytes octree] [bytes full 3D grid]\n"; } cout << "\nCreating tree\n===========================\n"; OcTree* tree = new OcTree(res); tree->setClampingThresMin(clampingMin); tree->setClampingThresMax(clampingMax); tree->setProbHit(probHit); tree->setProbMiss(probMiss); gettimeofday(&start, NULL); // start timer unsigned int numScans = graph->size(); unsigned int currentScan = 1; for (ScanGraph::iterator scan_it = graph->begin(); scan_it != graph->end(); scan_it++) { if (max_scan_no > 0) cout << "("<<currentScan << "/" << max_scan_no << ") " << flush; else cout << "("<<currentScan << "/" << numScans << ") " << flush; if (simpleUpdate) tree->insertPointCloudRays((*scan_it)->scan, (*scan_it)->pose.trans(), maxrange); else tree->insertPointCloud((*scan_it)->scan, (*scan_it)->pose.trans(), maxrange, false, discretize); if (compression == 2){ tree->toMaxLikelihood(); tree->prune(); } if (detailedLog) logfile << currentScan << " " << tree->memoryUsage() << " " << tree->memoryFullGrid() << "\n"; if ((max_scan_no > 0) && (currentScan == (unsigned int) max_scan_no)) break; currentScan++; } gettimeofday(&stop, NULL); // stop timer double time_to_insert = (stop.tv_sec - start.tv_sec) + 1.0e-6 *(stop.tv_usec - start.tv_usec); // get rid of graph in mem before doing anything fancy with tree (=> memory) delete graph; if (logfile.is_open()) logfile.close(); cout << "\nDone building tree.\n\n"; cout << "time to insert scans: " << time_to_insert << " sec" << endl; cout << "time to insert 100.000 points took: " << time_to_insert/ ((double) num_points_in_graph / 100000) << " sec (avg)" << endl << endl; std::cout << "Pruned tree (lossless compression)\n" << "===========================\n"; outputStatistics(tree); tree->write(treeFilenameOT); std::cout << "Pruned max-likelihood tree (lossy compression)\n" << "===========================\n"; tree->toMaxLikelihood(); tree->prune(); outputStatistics(tree); cout << "\nWriting tree files\n===========================\n"; tree->write(treeFilenameMLOT); std::cout << "Full Octree (pruned) written to "<< treeFilenameOT << std::endl; std::cout << "Full Octree (max.likelihood, pruned) written to "<< treeFilenameMLOT << std::endl; tree->writeBinary(treeFilename); std::cout << "Bonsai tree written to "<< treeFilename << std::endl; cout << endl; delete tree; exit(0); }