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