int main(int argc, char *argv[]) { typedef DDCompactView::graph_type::const_adj_iterator adjl_iterator; std::cout << "main:: initialize" << std::endl; // AlgoInit(); // Copied from example stand-alone program in Message Logger July 18, 2007 std::string const kProgramName = argv[0]; int rc = 0; try { // A. Instantiate a plug-in manager first. edm::AssertHandler ah; // B. Load the message service plug-in. Forget this and bad things happen! // In particular, the job hangs as soon as the output buffer fills up. // That's because, without the message service, there is no mechanism for // emptying the buffers. boost::shared_ptr<edm::Presence> theMessageServicePresence; theMessageServicePresence = boost::shared_ptr<edm::Presence>(edm::PresenceFactory::get()-> makePresence("MessageServicePresence").release()); // C. Manufacture a configuration and establish it. std::string config = "import FWCore.ParameterSet.Config as cms\n" "process = cms.Process('TEST')\n" "process.maxEvents = cms.untracked.PSet(\n" " input = cms.untracked.int32(5)\n" ")\n" "process.source = cms.Source('EmptySource')\n" "process.JobReportService = cms.Service('JobReportService')\n" "process.InitRootHandlers = cms.Service('InitRootHandlers')\n" // "process.MessageLogger = cms.Service('MessageLogger')\n" "process.m1 = cms.EDProducer('IntProducer',\n" " ivalue = cms.int32(11)\n" ")\n" "process.out = cms.OutputModule('PoolOutputModule',\n" " fileName = cms.untracked.string('testStandalone.root')\n" ")\n" "process.p = cms.Path(process.m1)\n" "process.e = cms.EndPath(process.out)\n"; boost::shared_ptr<std::vector<edm::ParameterSet> > pServiceSets; boost::shared_ptr<edm::ParameterSet> params_; edm::makeParameterSets(config, params_, pServiceSets); // D. Create the services. edm::ServiceToken tempToken(edm::ServiceRegistry::createSet(*pServiceSets.get())); // E. Make the services available. edm::ServiceRegistry::Operate operate(tempToken); // END Copy from example stand-alone program in Message Logger July 18, 2007 std::cout << "main::initialize DDL parser" << std::endl; DDCompactView cpv; DDLParser myP(cpv);// = DDLParser::instance(); // std::cout << "main:: about to start parsing field configuration..." << std::endl; // FIPConfiguration dp2; // dp2.readConfig("Geometry/CMSCommonData/data/FieldConfiguration.xml"); // myP->parse(dp2); std::cout << "main::about to start parsing main configuration... " << std::endl; FIPConfiguration dp(cpv); dp.readConfig("DetectorDescription/Parser/test/cmsIdealGeometryXML.xml"); myP.parse(dp); std::cout << "main::completed Parser" << std::endl; std::cout << std::endl << std::endl << "main::Start checking!" << std::endl << std::endl; DDCheckMaterials(std::cout); // cpv.setRoot(DDLogicalPart(DDName("cms:World"))); std::cout << "edge size of produce graph:" << cpv.writeableGraph().edge_size() << std::endl; DDExpandedView ev(cpv); std::cout << "== got the epv ==" << std::endl; // for now just count! std::ofstream plist("plist.out"); int numPhysParts(0); while ( ev.next() ) { ++numPhysParts; plist << ev.geoHistory() << std::endl; } plist.close(); std::cout << "Traversing the tree went to " << numPhysParts << " nodes, or \"PhysicalParts\" in online db terms." << std::endl; cpv.writeableGraph().clear(); // cpv.clear(); std::cout << "cleared DDCompactView. " << std::endl; } catch (DDException& e) { std::cerr << "DDD-PROBLEM:" << std::endl << e << std::endl; } // Deal with any exceptions that may have been thrown. catch (cms::Exception& e) { std::cout << "cms::Exception caught in " << kProgramName << "\n" << e.explainSelf(); rc = 1; } catch (std::exception& e) { std::cout << "Standard library exception caught in " << kProgramName << "\n" << e.what(); rc = 1; } catch (...) { std::cout << "Unknown exception caught in " << kProgramName; rc = 2; } return rc; }
int main(int argc, char *argv[]) { try { edmplugin::PluginManager::configure(edmplugin::standard::config()); } catch (cms::Exception& e) { edm::LogInfo("DDCompareCPV") << "Attempting to configure the plugin manager. Exception message: " << e.what(); return 1; } // Process the command line arguments std::string descString("DDCompareCPV"); descString += " [options] configurationFileName1 configurationFileName2 Compares two DDCompactViews\n"; descString += "Allowed options"; boost::program_options::options_description desc(descString); desc.add_options() ("help,h", "Print this help message") ("file1,f", boost::program_options::value<std::string>(), "XML configuration file name. " "Default is DetectorDescription/RegressionTest/test/configuration.xml") ("file2,g", boost::program_options::value<std::string>(), "XML configuration file name. " "Default is DetectorDescription/RegressionTest/test/configuration.xml") ("dist-tolerance,t", boost::program_options::value<std::string>(), "Value tolerance for distances (in mm). " "Default value 0.0004 (anything larger is an error)") ("rot-tolerance,r", boost::program_options::value<std::string>(), "Value tolerance for rotation matrix elements. " "Default value is 0.0004 (anything larger is an error)") ("spec-tolerance,s", boost::program_options::value<std::string>(), "Value tolerance for rotation matrix elements. " "Default value is 0.0004 (anything larger is an error) NOT USED YET") ("user-ns,u", "Use the namespaces in each file and do NOT use the filename as namespace. " "Default is to use the filename of each file in the configuration.xml file as a filename") ("comp-rot,c", "Use the rotation name when comparing rotations. " "Default is to use the matrix only and not the name when comparing DDRotations") ("continue-on-error,e", "Continue after an error in values. " "Default is to stop at the first error. NOT IMPLEMENTED") ("attempt-resync,a", "Continue after an error in graph position, attempt to resync. " "Default is to stop at the first mis-match of the graph. NOT IMPLEMENTED"); boost::program_options::positional_options_description p; p.add("file1", 1); p.add("file2", 2); boost::program_options::variables_map vm; try { store(boost::program_options::command_line_parser(argc,argv).options(desc).positional(p).run(),vm); notify(vm); } catch(boost::program_options::error const& iException) { std::cerr << "Exception from command line processing: " << iException.what() << "\n"; std::cerr << desc << std::endl; return 1; } if(vm.count("help")) { std::cout << desc << std::endl; return 0; } bool fullPath = false; std::string configfile("DetectorDescription/RegressionTest/test/configuration.xml"); std::string configfile2("DetectorDescription/RegressionTest/test/configuration.xml"); DDCompOptions ddco; // double dtol(0.0004), rottol(0.0004); // bool usrns(false), comprot(false); bool usrns(false); try { if (vm.count("file1")) { configfile = vm["file1"].as<std::string>(); if (vm.count("file2")) { configfile2 = vm["file2"].as<std::string>(); } } if (vm.count("dist-tolerance")) ddco.distTol_ = vm["dist-tolerance"].as<double>(); if (vm.count("rot-tolerance")) ddco.rotTol_ = vm["rot-tolerance"].as<double>(); if (vm.count("spec-tolerance")) ddco.rotTol_ = vm["spec-tolerance"].as<double>(); if (vm.count("user-ns")) usrns = true; if (vm.count("comp-rot")) ddco.compRotName_ = true; if (vm.count("continue-on-error")) ddco.contOnError_ = true; if (vm.count("attempt-resync")) ddco.attResync_ = true; } catch(boost::exception& e) { edm::LogInfo("DDCompareCPV") << "Attempting to parse the options. Exception message: " << boost::diagnostic_information(e); return 1; } std::ios_base::fmtflags originalFlags = std::cout.flags(); std::cout << "Settings are: " << std::endl; std::cout << "Configuration file 1: " << configfile << std::endl; std::cout << "Configuration file 2: " << configfile2 << std::endl; std::cout << "Length/distance tolerance: " << ddco.distTol_ << std::endl; std::cout << "Rotation matrix element tolerance: " << ddco.rotTol_ << std::endl; std::cout << "SpecPar tolerance: " << ddco.specTol_ << std::endl; std::cout << "User controlled namespace (both file sets)? " << std::boolalpha << usrns << std::endl; std::cout << "Compare Rotation names? " << ddco.compRotName_ << std::endl; std::cout << "Continue on error (data mismatch)? " << ddco.contOnError_ << std::endl; std::cout << "Attempt resyncronization of disparate graphs? " << ddco.attResync_ << std::endl; DDCompactView cpv1; DDLParser myP(cpv1); myP.getDDLSAX2FileHandler()->setUserNS(usrns); /* The configuration file tells the parser what to parse. The sequence of files to be parsed does not matter but for one exception: XML containing SpecPar-tags must be parsed AFTER all corresponding PosPart-tags were parsed. (Simply put all SpecPars-tags into seperate files and mention them at end of configuration.xml. Functional SW will not suffer from this restriction). */ // Use the File-In-Path configuration document provider. FIPConfiguration fp(cpv1); try { fp.readConfig(configfile, fullPath); } catch (cms::Exception& e) { edm::LogInfo("DDCompareCPV") << "Attempting to read config. Exception message: " << e.what(); return 1; } std::cout << "FILE 1: " << configfile << std::endl; if ( fp.getFileList().size() == 0 ) { std::cout << "FILE 1: configuration file has no DDD xml files in it!" << std::endl; exit(1); } int parserResult = myP.parse(fp); if (parserResult != 0) { std::cout << "FILE 1: problem encountered during parsing. exiting ... " << std::endl; exit(1); } cpv1.lockdown(); DDCompactView cpv2; DDLParser myP2(cpv2); myP2.getDDLSAX2FileHandler()->setUserNS(usrns); /* The configuration file tells the parser what to parse. The sequence of files to be parsed does not matter but for one exception: XML containing SpecPar-tags must be parsed AFTER all corresponding PosPart-tags were parsed. (Simply put all SpecPars-tags into seperate files and mention them at end of configuration.xml. Functional SW will not suffer from this restriction). */ // Use the File-In-Path configuration document provider. FIPConfiguration fp2(cpv2); fp2.readConfig(configfile2, fullPath); std::cout << "FILE 2: " << configfile2 << std::endl; if ( fp2.getFileList().size() == 0 ) { std::cout << "FILE 2: configuration file has no DDD xml files in it!" << std::endl; exit(1); } int parserResult2 = myP2.parse(fp2); if (parserResult2 != 0) { std::cout << "FILE 2: problem encountered during parsing. exiting ... " << std::endl; exit(1); } cpv2.lockdown(); std::cout << "Parsing completed. Start comparing." << std::endl; // DDErrorDetection ed(cpv1); // bool noErrors = ed.noErrorsInTheReport(cpv1); // if (noErrors && fullPath) { // std::cout << "DDCompareCPV did not find any errors and is finished." << std::endl; // } // else { // ed.report(cpv1, std::cout); // if (!noErrors) { // return 1; // } // } DDCompareCPV ddccpv(ddco); bool graphmatch = ddccpv(cpv1, cpv2); if (graphmatch) { std::cout << "DDCompactView graphs match" << std::endl; } else { std::cout << "DDCompactView graphs do NOT match" << std::endl; } // Now set everything back to defaults std::cout.flags( originalFlags ); return 0; }
int main(int argc, char *argv[]) { using Graph = DDCompactView::Graph; using adjl_iterator = Graph::const_adj_iterator; // Copied from example stand-alone program in Message Logger July 18, 2007 std::string const kProgramName = argv[0]; int rc = 0; try { // A. Instantiate a plug-in manager first. edm::AssertHandler ah; // B. Load the message service plug-in. Forget this and bad things happen! // In particular, the job hangs as soon as the output buffer fills up. // That's because, without the message service, there is no mechanism for // emptying the buffers. std::shared_ptr<edm::Presence> theMessageServicePresence; theMessageServicePresence = std::shared_ptr<edm::Presence>(edm::PresenceFactory::get()-> makePresence("MessageServicePresence").release()); // C. Manufacture a configuration and establish it. std::string config = "import FWCore.ParameterSet.Config as cms\n" "process = cms.Process('TEST')\n" "process.maxEvents = cms.untracked.PSet(\n" " input = cms.untracked.int32(5)\n" ")\n" "process.source = cms.Source('EmptySource')\n" "process.JobReportService = cms.Service('JobReportService')\n" "process.InitRootHandlers = cms.Service('InitRootHandlers')\n" // "process.MessageLogger = cms.Service('MessageLogger')\n" "process.m1 = cms.EDProducer('IntProducer',\n" " ivalue = cms.int32(11)\n" ")\n" "process.out = cms.OutputModule('PoolOutputModule',\n" " fileName = cms.untracked.string('testStandalone.root')\n" ")\n" "process.p = cms.Path(process.m1)\n" "process.e = cms.EndPath(process.out)\n"; // D. Create the services. std::unique_ptr<edm::ParameterSet> params; edm::makeParameterSets(config, params); edm::ServiceToken tempToken(edm::ServiceRegistry::createServicesFromConfig(std::move(params))); // E. Make the services available. edm::ServiceRegistry::Operate operate(tempToken); // END Copy from example stand-alone program in Message Logger July 18, 2007 std::cout << "main::initialize DDL parser" << std::endl; DDCompactView cpv; DDLParser myP(cpv);// = DDLParser::instance(); // std::cout << "main:: about to start parsing field configuration..." << std::endl; // FIPConfiguration dp2; // dp2.readConfig("Geometry/CMSCommonData/data/FieldConfiguration.xml"); // myP->parse(dp2); std::cout << "main::about to start parsing main configuration... " << std::endl; FIPConfiguration dp(cpv); dp.readConfig("DetectorDescription/Parser/test/cmsIdealGeometryXML.xml"); myP.parse(dp); std::cout << "main::completed Parser" << std::endl; std::cout << std::endl << std::endl << "main::Start checking!" << std::endl << std::endl; DDCheckMaterials(std::cout); // cpv.setRoot(DDLogicalPart(DDName("cms:World"))); std::cout << "edge size of produce graph:" << cpv.graph().edge_size() << std::endl; const auto& gt = cpv.graph(); adjl_iterator git = gt.begin(); adjl_iterator gend = gt.end(); Graph::index_type i=0; for (; git != gend; ++git) { const DDLogicalPart & ddLP = gt.nodeData(git); std::cout << ++i << " P " << ddLP.name() << std::endl; if (!git->empty()) { auto cit = git->begin(); auto cend = git->end(); for (; cit != cend; ++cit) { const DDLogicalPart & ddcurLP = gt.nodeData(cit->first); std::cout << ++i << " c--> " << gt.edgeData(cit->second)->copyno() << " " << ddcurLP.name() << std::endl; } } } } // Deal with any exceptions that may have been thrown. catch (cms::Exception& e) { std::cout << "cms::Exception caught in " << kProgramName << "\n" << e.explainSelf(); rc = 1; } catch (std::exception& e) { std::cout << "Standard library exception caught in " << kProgramName << "\n" << e.what(); rc = 1; } catch (...) { std::cout << "Unknown exception caught in " << kProgramName; rc = 2; } return rc; }
/** * @brief Generates a optimised point cloud from a collection of processed keyframes using sparse bundle adjustment. * @param allFrames A vector containing keyframes which have been completely processed. * @param outputCloud The cloud generated from the keyframes. */ void utils::calculateSBACloud(std::vector<KeyframeContainer>& allFrames,boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB> >& outputCloud) { sba::SysSBA bundleAdjuster; //set the verbosity of the bundle adjuster #ifndef NDEBUG bundleAdjuster.verbose=100; #else bundleAdjuster.verbose=0; #endif //a list of keypoints that already have been added to the SBA system std::vector<std::pair<int,uint> > addedPoints; //(frameID,keypointIndex) //maps frame IDs to frame index in the vector std::map<int,uint> ID2Ind; for(uint f=0;f<allFrames.size();++f) ID2Ind[allFrames[f].ID]=f; //maps the frame vector index to the node vector index std::map<uint,int> fInd2NInd; //count the number of 2D/3D-points and camera nodes if in debug mode #ifndef NDEBUG uint nrP3D=0; uint nrP2D=0; uint nrC=0; #endif //add every valid frame as a camera node to the sba system for(uint f=0;f<allFrames.size();++f) { if(!allFrames[f].invalid) { //get rotation and translation from the projection matrix Eigen::Matrix3d rot; cv::Mat_<double> cvT; Eigen::Vector4d t; //get rotation for(int x=0;x<3;++x) for(int y=0;y<3;++y) rot(x,y)=allFrames[f].projectionMatrix(x,y); //solve for translation cv::solve(allFrames[f].projectionMatrix(cv::Range(0,3),cv::Range(0,3)),allFrames[f].projectionMatrix(cv::Range(0,3),cv::Range(3,4)),cvT); //save translation for(int x=0;x<3;++x) t(x)=-cvT(x,0); t(3)=1.0; //convert rotation to quaternion Eigen::Quaterniond qrot(rot); qrot.normalize(); //convert camera calibration matrix frame_common::CamParams cameraParameters; cameraParameters.fx=allFrames[f].cameraCalibration(0,0); cameraParameters.fy=allFrames[f].cameraCalibration(1,1); cameraParameters.cx=allFrames[f].cameraCalibration(0,2); cameraParameters.cy=allFrames[f].cameraCalibration(1,2); cameraParameters.tx=0.0; //add the frame as a camera node fInd2NInd[f]=bundleAdjuster.addNode(t,qrot,cameraParameters,false); #ifndef NDEBUG ++nrC; #endif } else fInd2NInd[f]=-1; } //add the points to the correct nodes for(uint f=0;f<allFrames.size();++f) { if(!allFrames[f].invalid) { //add the points for(uint m=0;m<allFrames[f].matches.size();++m) { std::pair<int,uint> myP(allFrames[f].ID,m); bool goodToAdd=!utils::vectorContainsElement(addedPoints,myP); //point hasn't been added yet if(goodToAdd) { //check if the point has valid depth information float depth=allFrames[f].depthImg.image.at<float>(allFrames[f].keypoints[m].pt.y,allFrames[f].keypoints[m].pt.x); if(isnan(depth)==0) { //find the largest completely connected component emanating from this point (clique) std::vector<std::pair<int,uint> > pointsComplete; pointsComplete.push_back(std::pair<int,uint>(allFrames[f].ID,m)); //check all matches of the point if they haven't been added yet and their frame is valid for(uint o=0;o<allFrames[f].matches[m].size();++o) { std::pair<int,uint> newElement(allFrames[f].matches[m][o].first.val[0],allFrames[f].matches[m][o].first.val[1]); if(!utils::vectorContainsElement(addedPoints,newElement) && !allFrames[ID2Ind[newElement.first]].invalid) pointsComplete.push_back(newElement); } //weed out all points that are not completely connected while(pointsComplete.size()>1) { //find the point with the fewest connections (if the component is completely connected all points have the same number of connections) uint minCorrespondences=pointsComplete.size(); uint worstInd=0; for(uint i=0;i<pointsComplete.size();++i) { //count the number of connections that this point has uint fInd=ID2Ind[pointsComplete[i].first]; uint myCorr=0; for(uint q=0;q<allFrames[fInd].matches.size();++q) for(uint r=0;r<allFrames[fInd].matches[q].size();++r) { std::pair<int,uint> tmpElement(allFrames[fInd].matches[q][r].first.val[0],allFrames[fInd].matches[q][r].first.val[1]); if(utils::vectorContainsElement(pointsComplete,tmpElement)) ++myCorr; } //save this point if it is the current worst if(myCorr<minCorrespondences) { minCorrespondences=myCorr; worstInd=i; } } //if the worst point has not the maximal number of connections erase it, else break as all points have the maximal number of connections if(minCorrespondences<pointsComplete.size()-1) { pointsComplete.erase(pointsComplete.begin()+worstInd); } else break; } //now pointsComplete contains the clique if the clique has more than one isolated point, write the points to the system if(pointsComplete.size()>1) { //calculate the 3D point and add it to the system cv::Mat_<double> p2D(2,1,0.0); p2D(0,0)=allFrames[f].keypoints[m].pt.x; p2D(1,0)=allFrames[f].keypoints[m].pt.y; cv::Mat_<double> p3D=reprojectImagePointTo3D(p2D,allFrames[f].cameraCalibration,allFrames[f].projectionMatrix,depth); Eigen::Vector4d newPoint; for(uint x=0;x<4;++x) newPoint(x)=p3D(x,0); int pointIndex=bundleAdjuster.addPoint(newPoint); #ifndef NDEBUG ++nrP3D; #endif //add all image points corresponding to the 3D point to the system for(uint i=0;i<pointsComplete.size();++i) { uint fInd=ID2Ind[pointsComplete[i].first]; Eigen::Vector2d imgPoint; imgPoint(0)=allFrames[fInd].keypoints[pointsComplete[i].second].pt.x; imgPoint(1)=allFrames[fInd].keypoints[pointsComplete[i].second].pt.y; bundleAdjuster.addMonoProj(fInd2NInd[fInd],pointIndex,imgPoint); #ifndef NDEBUG ++nrP2D; #endif } } } } } } } #ifndef NDEBUG ROS_INFO("C: %u;3D: %u;2D: %u",nrC,nrP3D,nrP2D); #endif //remove bad tracks bundleAdjuster.calcCost(); bundleAdjuster.removeBad(2); bundleAdjuster.reduceTracks(); //run 100 iterations of sba bundleAdjuster.doSBA(100,1e-3, SBA_SPARSE_CHOLESKY); //save the new projective matrix calculated with sba for(uint f=0;f<allFrames.size();++f) if(!allFrames[f].invalid) for(int x=0;x<3;++x) for(int y=0;y<4;++y) allFrames[f].projectionMatrix(x,y)=bundleAdjuster.nodes[fInd2NInd[f]].w2n(x,y); //generate the point cloud from the new projection matrices utils::generateCloud(allFrames,*outputCloud); }