int main() { int m=1; int l=1; double dt=0.01; pendulum1d_cost::N(0,0)=0.3; pendulum1d_cost::N(1,1)=0.3; pendulum1d_cost::Q=0.1; pendulum1d::Cost cost=pendulum1d_cost::cost; pendulum1d::DCost dcost=pendulum1d_cost::dcost; pendulum1d::Cost costN=pendulum1d_cost::costN; pendulum1d::DCost dcostN=pendulum1d_cost::dcostN; SAC<pendulum1d>::Functions func(cost, dcost, costN, dcostN); SAC<pendulum1d>::Params params; params.alpha=-20; params.beta=0.6; params.Tc=0.64; params.Tp=2.5; params.Ts=0.02; params.Tcal=0.01; params.kmax=6; pendulum1d::System sys(m,l); pendulum1d::State state0(-2,7); std::list<pendulum1d::Params> list_pars(1000, pendulum1d::State(0, 0)); std::list<Eigen::Matrix<double,1,1> > list_u; Sim<pendulum1d>::State_Sim sim(sys,dt); sim.init(state0); SAC<pendulum1d> sac(sys,dt); for(double t=0; t<30; t+=params.Ts) { sac.init(sim.get_state(), params, func, list_pars.begin(), list_pars.end(), &list_u); sac.sac_ctrl(list_u); sim.update(list_u.front()); list_u.pop_front(); sim.update(list_u.front()); list_u.pop_front(); } sim.save("/media/fantaosha/Documents/JHU/Summer 2015/results/result3.mat"); return 0; }
void MCF::saveXml(const char* file) { UTIL::FS::FileHandle handle(file, UTIL::FS::FILE_WRITE); try { std::sort(m_pFileList.begin(), m_pFileList.end(), OffsetSortKey()); XMLSaveAndCompress sac(&handle, false); genXml(&sac); sac.finish(); } catch (gcException &e) { Warning(gcString("Failed to save mcf xml to {0}: {1}", file, e)); } }
void MCF::saveMCF_Header() { //donot use getDLSize here as the file might have a gap in it. :( uint64 offset = 0; for (size_t x=0; x<m_pFileList.size(); x++) { MCFFile* file = m_pFileList[x]; if (!file->isSaved()) continue; uint64 pos = file->getOffSet() + file->getCurSize(); if (offset < pos) offset = pos; uint64 diffpos = file->getDiffOffSet() + file->getDiffSize(); if (file->hasDiff() && offset < diffpos) offset = diffpos; } if (offset == 0) offset = m_sHeader->getSize(); UTIL::FS::recMakeFolder(UTIL::FS::PathWithFile(m_szFile)); UTIL::FS::FileHandle hFile(m_szFile.c_str(), UTIL::FS::FILE_APPEND); hFile.seek(offset); XMLSaveAndCompress sac(&hFile, isCompressed()); genXml(&sac); sac.finish(); m_sHeader->setXmlStart(offset); m_sHeader->setXmlSize((uint32)sac.getTotalSize()); m_sHeader->saveToFile(hFile); }
template <typename PointT> void pcl::registration::CorrespondenceRejectorSampleConsensus<PointT>::getRemainingCorrespondences ( const pcl::Correspondences& original_correspondences, pcl::Correspondences& remaining_correspondences) { if (!input_) { PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] No input cloud dataset was given!\n", getClassName ().c_str ()); return; } if (!target_) { PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] No input target dataset was given!\n", getClassName ().c_str ()); return; } if (save_inliers_) inlier_indices_.clear (); int nr_correspondences = static_cast<int> (original_correspondences.size ()); std::vector<int> source_indices (nr_correspondences); std::vector<int> target_indices (nr_correspondences); // Copy the query-match indices for (size_t i = 0; i < original_correspondences.size (); ++i) { source_indices[i] = original_correspondences[i].index_query; target_indices[i] = original_correspondences[i].index_match; } // from pcl/registration/icp.hpp: std::vector<int> source_indices_good; std::vector<int> target_indices_good; { // From the set of correspondences found, attempt to remove outliers // Create the registration model typedef typename pcl::SampleConsensusModelRegistration<PointT>::Ptr SampleConsensusModelRegistrationPtr; SampleConsensusModelRegistrationPtr model; model.reset (new pcl::SampleConsensusModelRegistration<PointT> (input_, source_indices)); // Pass the target_indices model->setInputTarget (target_, target_indices); // Create a RANSAC model pcl::RandomSampleConsensus<PointT> sac (model, inlier_threshold_); sac.setMaxIterations (max_iterations_); // Compute the set of inliers if (!sac.computeModel ()) { remaining_correspondences = original_correspondences; best_transformation_.setIdentity (); return; } else { if (refine_ && !sac.refineModel ()) { PCL_ERROR ("[pcl::registration::CorrespondenceRejectorSampleConsensus::getRemainingCorrespondences] Could not refine the model! Returning an empty solution.\n"); return; } std::vector<int> inliers; sac.getInliers (inliers); if (inliers.size () < 3) { remaining_correspondences = original_correspondences; best_transformation_.setIdentity (); return; } boost::unordered_map<int, int> index_to_correspondence; for (int i = 0; i < nr_correspondences; ++i) index_to_correspondence[original_correspondences[i].index_query] = i; remaining_correspondences.resize (inliers.size ()); for (size_t i = 0; i < inliers.size (); ++i) remaining_correspondences[i] = original_correspondences[index_to_correspondence[inliers[i]]]; if (save_inliers_) { inlier_indices_.reserve (inliers.size ()); for (const int &inlier : inliers) inlier_indices_.push_back (index_to_correspondence[inlier]); } // get best transformation Eigen::VectorXf model_coefficients; sac.getModelCoefficients (model_coefficients); best_transformation_.row (0) = model_coefficients.segment<4>(0); best_transformation_.row (1) = model_coefficients.segment<4>(4); best_transformation_.row (2) = model_coefficients.segment<4>(8); best_transformation_.row (3) = model_coefficients.segment<4>(12); } } }
bool test(bool is_kernel_exact = true) { // types typedef typename K::FT FT; typedef typename K::Line_3 Line; typedef typename K::Point_3 Point; typedef typename K::Segment_3 Segment; typedef typename K::Ray_3 Ray; typedef typename K::Line_3 Line; typedef typename K::Triangle_3 Triangle; /* ------------------------------------- // Test data is something like that (in t supporting plane) // Triangle is (p1,p2,p3) // // +E +1 // / \ // +C 6+ +8 +4 +B // / 9++7 \ // 3+-------+5--+2 // // +F +A ------------------------------------- */ Point p1(FT(1.), FT(0.), FT(0.)); Point p2(FT(0.), FT(1.), FT(0.)); Point p3(FT(0.), FT(0.), FT(1.)); Triangle t(p1,p2,p3); // Edges of t Segment s12(p1,p2); Segment s21(p2,p1); Segment s13(p1,p3); Segment s23(p2,p3); Segment s32(p3,p2); Segment s31(p3,p1); bool b = test_aux(is_kernel_exact,t,s12,"t-s12",s12); b &= test_aux(is_kernel_exact,t,s21,"t-s21",s21); b &= test_aux(is_kernel_exact,t,s13,"t-s13",s13); b &= test_aux(is_kernel_exact,t,s23,"t-s23",s23); // Inside points Point p4(FT(0.5), FT(0.5), FT(0.)); Point p5(FT(0.), FT(0.75), FT(0.25)); Point p6(FT(0.5), FT(0.), FT(0.5)); Point p7(FT(0.25), FT(0.625), FT(0.125)); Point p8(FT(0.5), FT(0.25), FT(0.25)); Segment s14(p1,p4); Segment s41(p4,p1); Segment s24(p2,p4); Segment s42(p4,p2); Segment s15(p1,p5); Segment s25(p2,p5); Segment s34(p3,p4); Segment s35(p3,p5); Segment s36(p3,p6); Segment s45(p4,p5); Segment s16(p1,p6); Segment s26(p2,p6); Segment s62(p6,p2); Segment s46(p4,p6); Segment s48(p4,p8); Segment s56(p5,p6); Segment s65(p6,p5); Segment s64(p6,p4); Segment s17(p1,p7); Segment s67(p6,p7); Segment s68(p6,p8); Segment s86(p8,p6); Segment s78(p7,p8); Segment s87(p8,p7); b &= test_aux(is_kernel_exact,t,s14,"t-s14",s14); b &= test_aux(is_kernel_exact,t,s41,"t-s41",s41); b &= test_aux(is_kernel_exact,t,s24,"t-s24",s24); b &= test_aux(is_kernel_exact,t,s42,"t-s42",s42); b &= test_aux(is_kernel_exact,t,s15,"t-s15",s15); b &= test_aux(is_kernel_exact,t,s25,"t-s25",s25); b &= test_aux(is_kernel_exact,t,s34,"t-s34",s34); b &= test_aux(is_kernel_exact,t,s35,"t-s35",s35); b &= test_aux(is_kernel_exact,t,s36,"t-s36",s36); b &= test_aux(is_kernel_exact,t,s45,"t-s45",s45); b &= test_aux(is_kernel_exact,t,s16,"t-s16",s16); b &= test_aux(is_kernel_exact,t,s26,"t-s26",s26); b &= test_aux(is_kernel_exact,t,s62,"t-s62",s62); b &= test_aux(is_kernel_exact,t,s46,"t-s46",s46); b &= test_aux(is_kernel_exact,t,s65,"t-s65",s65); b &= test_aux(is_kernel_exact,t,s64,"t-s64",s64); b &= test_aux(is_kernel_exact,t,s48,"t-s48",s48); b &= test_aux(is_kernel_exact,t,s56,"t-s56",s56); b &= test_aux(is_kernel_exact,t,s17,"t-t17",s17); b &= test_aux(is_kernel_exact,t,s67,"t-t67",s67); b &= test_aux(is_kernel_exact,t,s68,"t-s68",s68); b &= test_aux(is_kernel_exact,t,s86,"t-s86",s86); b &= test_aux(is_kernel_exact,t,s78,"t-t78",s78); b &= test_aux(is_kernel_exact,t,s87,"t-t87",s87); // Outside points (in triangle plane) Point pA(FT(-0.5), FT(1.), FT(0.5)); Point pB(FT(0.5), FT(1.), FT(-0.5)); Point pC(FT(0.5), FT(-0.5), FT(1.)); Point pE(FT(1.), FT(-1.), FT(1.)); Point pF(FT(-1.), FT(0.), FT(2.)); Segment sAB(pA,pB); Segment sBC(pB,pC); Segment s2E(p2,pE); Segment sE2(pE,p2); Segment s2A(p2,pA); Segment s6E(p6,pE); Segment sB8(pB,p8); Segment sC8(pC,p8); Segment s8C(p8,pC); Segment s1F(p1,pF); Segment sF6(pF,p6); b &= test_aux(is_kernel_exact,t,sAB,"t-sAB",p2); b &= test_aux(is_kernel_exact,t,sBC,"t-sBC",s46); b &= test_aux(is_kernel_exact,t,s2E,"t-s2E",s26); b &= test_aux(is_kernel_exact,t,sE2,"t-sE2",s62); b &= test_aux(is_kernel_exact,t,s2A,"t-s2A",p2); b &= test_aux(is_kernel_exact,t,s6E,"t-s6E",p6); b &= test_aux(is_kernel_exact,t,sB8,"t-sB8",s48); b &= test_aux(is_kernel_exact,t,sC8,"t-sC8",s68); b &= test_aux(is_kernel_exact,t,s8C,"t-s8C",s86); b &= test_aux(is_kernel_exact,t,s1F,"t-s1F",s13); b &= test_aux(is_kernel_exact,t,sF6,"t-sF6",s36); // Outside triangle plane Point pa(FT(0.), FT(0.), FT(0.)); Point pb(FT(2.), FT(0.), FT(0.)); Point pc(FT(1.), FT(0.), FT(1.)); Point pe(FT(1.), FT(0.5), FT(0.5)); Segment sab(pa,pb); Segment sac(pa,pc); Segment sae(pa,pe); Segment sa8(pa,p8); Segment sb2(pb,p2); b &= test_aux(is_kernel_exact,t,sab,"t-sab",p1); b &= test_aux(is_kernel_exact,t,sac,"t-sac",p6); b &= test_aux(is_kernel_exact,t,sae,"t-sae",p8); b &= test_aux(is_kernel_exact,t,sa8,"t-sa8",p8); b &= test_aux(is_kernel_exact,t,sb2,"t-sb2",p2); // ----------------------------------- // ray queries // ----------------------------------- // Edges of t Ray r12(p1,p2); Ray r21(p2,p1); Ray r13(p1,p3); Ray r23(p2,p3); b &= test_aux(is_kernel_exact,t,r12,"t-r12",s12); b &= test_aux(is_kernel_exact,t,r21,"t-r21",s21); b &= test_aux(is_kernel_exact,t,r13,"t-r13",s13); b &= test_aux(is_kernel_exact,t,r23,"t-r23",s23); // In triangle Point p9_(FT(0.), FT(0.5), FT(0.5)); Point p9(FT(0.25), FT(0.375), FT(0.375)); Ray r14(p1,p4); Ray r41(p4,p1); Ray r24(p2,p4); Ray r42(p4,p2); Ray r15(p1,p5); Ray r25(p2,p5); Ray r34(p3,p4); Ray r35(p3,p5); Ray r36(p3,p6); Ray r45(p4,p5); Ray r16(p1,p6); Ray r26(p2,p6); Ray r62(p6,p2); Ray r46(p4,p6); Ray r48(p4,p8); Ray r56(p5,p6); Ray r47(p4,p7); Ray r89(p8,p9); Ray r86(p8,p6); Ray r68(p6,p8); Segment r89_res(p8,p9_); b &= test_aux(is_kernel_exact,t,r14,"t-r14",s12); b &= test_aux(is_kernel_exact,t,r41,"t-r41",s41); b &= test_aux(is_kernel_exact,t,r24,"t-r24",s21); b &= test_aux(is_kernel_exact,t,r42,"t-r42",s42); b &= test_aux(is_kernel_exact,t,r15,"t-r15",s15); b &= test_aux(is_kernel_exact,t,r25,"t-r25",s23); b &= test_aux(is_kernel_exact,t,r34,"t-r34",s34); b &= test_aux(is_kernel_exact,t,r35,"t-r35",s32); b &= test_aux(is_kernel_exact,t,r36,"t-r36",s31); b &= test_aux(is_kernel_exact,t,r45,"t-r45",s45); b &= test_aux(is_kernel_exact,t,r16,"t-r16",s13); b &= test_aux(is_kernel_exact,t,r26,"t-r26",s26); b &= test_aux(is_kernel_exact,t,r62,"t-r62",s62); b &= test_aux(is_kernel_exact,t,r46,"t-r46",s46); b &= test_aux(is_kernel_exact,t,r48,"t-r48",s46); b &= test_aux(is_kernel_exact,t,r56,"t-r56",s56); b &= test_aux(is_kernel_exact,t,r47,"t-r47",s45); b &= test_aux(is_kernel_exact,t,r89,"t-t89",r89_res); b &= test_aux(is_kernel_exact,t,r68,"t-r68",s64); b &= test_aux(is_kernel_exact,t,r86,"t-r86",s86); // Outside points (in triangre prane) Ray rAB(pA,pB); Ray rBC(pB,pC); Ray r2E(p2,pE); Ray rE2(pE,p2); Ray r2A(p2,pA); Ray r6E(p6,pE); Ray rB8(pB,p8); Ray rC8(pC,p8); Ray r8C(p8,pC); Ray r1F(p1,pF); Ray rF6(pF,p6); b &= test_aux(is_kernel_exact,t,rAB,"t-rAB",p2); b &= test_aux(is_kernel_exact,t,rBC,"t-rBC",s46); b &= test_aux(is_kernel_exact,t,r2E,"t-r2E",s26); b &= test_aux(is_kernel_exact,t,rE2,"t-rE2",s62); b &= test_aux(is_kernel_exact,t,r2A,"t-r2A",p2); b &= test_aux(is_kernel_exact,t,r6E,"t-r6E",p6); b &= test_aux(is_kernel_exact,t,rB8,"t-rB8",s46); b &= test_aux(is_kernel_exact,t,rC8,"t-rC8",s64); b &= test_aux(is_kernel_exact,t,r8C,"t-r8C",s86); b &= test_aux(is_kernel_exact,t,r1F,"t-r1F",s13); b &= test_aux(is_kernel_exact,t,rF6,"t-rF6",s31); // Outside triangle plane Ray rab(pa,pb); Ray rac(pa,pc); Ray rae(pa,pe); Ray ra8(pa,p8); Ray rb2(pb,p2); b &= test_aux(is_kernel_exact,t,rab,"t-rab",p1); b &= test_aux(is_kernel_exact,t,rac,"t-rac",p6); b &= test_aux(is_kernel_exact,t,rae,"t-rae",p8); b &= test_aux(is_kernel_exact,t,ra8,"t-ra8",p8); b &= test_aux(is_kernel_exact,t,rb2,"t-rb2",p2); // ----------------------------------- // Line queries // ----------------------------------- // Edges of t Line l12(p1,p2); Line l21(p2,p1); Line l13(p1,p3); Line l23(p2,p3); b &= test_aux(is_kernel_exact,t,l12,"t-l12",s12); b &= test_aux(is_kernel_exact,t,l21,"t-l21",s21); b &= test_aux(is_kernel_exact,t,l13,"t-l13",s13); b &= test_aux(is_kernel_exact,t,l23,"t-l23",s23); // In triangle Line l14(p1,p4); Line l41(p4,p1); Line l24(p2,p4); Line l42(p4,p2); Line l15(p1,p5); Line l25(p2,p5); Line l34(p3,p4); Line l35(p3,p5); Line l36(p3,p6); Line l45(p4,p5); Line l16(p1,p6); Line l26(p2,p6); Line l62(p6,p2); Line l46(p4,p6); Line l48(p4,p8); Line l56(p5,p6); Line l47(p4,p7); Line l89(p8,p9); Line l86(p8,p6); Line l68(p6,p8); Segment l89_res(p1,p9_); b &= test_aux(is_kernel_exact,t,l14,"t-l14",s12); b &= test_aux(is_kernel_exact,t,l41,"t-l41",s21); b &= test_aux(is_kernel_exact,t,l24,"t-l24",s21); b &= test_aux(is_kernel_exact,t,l42,"t-l42",s12); b &= test_aux(is_kernel_exact,t,l15,"t-l15",s15); b &= test_aux(is_kernel_exact,t,l25,"t-l25",s23); b &= test_aux(is_kernel_exact,t,l34,"t-l34",s34); b &= test_aux(is_kernel_exact,t,l35,"t-l35",s32); b &= test_aux(is_kernel_exact,t,l36,"t-l36",s31); b &= test_aux(is_kernel_exact,t,l45,"t-l45",s45); b &= test_aux(is_kernel_exact,t,l16,"t-l16",s13); b &= test_aux(is_kernel_exact,t,l26,"t-l26",s26); b &= test_aux(is_kernel_exact,t,l62,"t-l62",s62); b &= test_aux(is_kernel_exact,t,l46,"t-l46",s46); b &= test_aux(is_kernel_exact,t,l48,"t-l48",s46); b &= test_aux(is_kernel_exact,t,l56,"t-l56",s56); b &= test_aux(is_kernel_exact,t,l47,"t-l47",s45); b &= test_aux(is_kernel_exact,t,l89,"t-t89",l89_res); b &= test_aux(is_kernel_exact,t,l68,"t-l68",s64); b &= test_aux(is_kernel_exact,t,l86,"t-l86",s46); // Outside points (in triangle plane) Line lAB(pA,pB); Line lBC(pB,pC); Line l2E(p2,pE); Line lE2(pE,p2); Line l2A(p2,pA); Line l6E(p6,pE); Line lB8(pB,p8); Line lC8(pC,p8); Line l8C(p8,pC); Line l1F(p1,pF); Line lF6(pF,p6); b &= test_aux(is_kernel_exact,t,lAB,"t-lAB",p2); b &= test_aux(is_kernel_exact,t,lBC,"t-lBC",s46); b &= test_aux(is_kernel_exact,t,l2E,"t-l2E",s26); b &= test_aux(is_kernel_exact,t,lE2,"t-lE2",s62); b &= test_aux(is_kernel_exact,t,l2A,"t-l2A",p2); b &= test_aux(is_kernel_exact,t,l6E,"t-l6E",s26); b &= test_aux(is_kernel_exact,t,lB8,"t-lB8",s46); b &= test_aux(is_kernel_exact,t,lC8,"t-lC8",s64); b &= test_aux(is_kernel_exact,t,l8C,"t-l8C",s46); b &= test_aux(is_kernel_exact,t,l1F,"t-l1F",s13); b &= test_aux(is_kernel_exact,t,lF6,"t-lF6",s31); // Outside triangle plane Line lab(pa,pb); Line lac(pa,pc); Line lae(pa,pe); Line la8(pa,p8); Line lb2(pb,p2); b &= test_aux(is_kernel_exact,t,lab,"t-lab",p1); b &= test_aux(is_kernel_exact,t,lac,"t-lac",p6); b &= test_aux(is_kernel_exact,t,lae,"t-lae",p8); b &= test_aux(is_kernel_exact,t,la8,"t-la8",p8); b &= test_aux(is_kernel_exact,t,lb2,"t-lb2",p2); return b; }
int main(int argc, char *argv[]) { if(argc!=2) { std::cerr<<"Usage: "<<argv[0]<<" [Parameters_file]"<<std::endl; return -1; } bool SyncNorm = true; logger.Rename("logs_Seed2Cor/Test"); try { /* Initialize the CC Database with the input parameter file */ CCDatabase cdb( argv[1] ); //const CCPARAM& cdbParams = cdb.GetParams(); /* check total memory available */ float MemTotal = memo.MemTotal(); logger.Hold( INFO, "Estimated total memory = "+std::to_string(MemTotal)+" Mb", FuncName ); logger.flush(); /* iterate through the database and handle all possible events */ #pragma omp parallel { // parallel region S while( 1 ) { // main loop //int ithread = omp_get_thread_num(); /* dynamically assign events to threads, one at a time */ bool got; std::vector<DailyInfo> dinfoV; #pragma omp critical(cdb) { // critical S got = cdb.GetRec_AllCH(dinfoV); cdb.NextEvent(); } // critical E if( !got ) break; try { // handle current event //std::vector<SacRec> sacV; std::deque<SacRec> sacV; //sacV.reserve(dinfoV.size()); std::vector<std::stringstream> reportV( dinfoV.size() ); /* seed to fsac */ for( int ich=0; ich<dinfoV.size(); ich++ ) { auto& dinfo = dinfoV[ich]; /* daily info from the database */ logger.Hold( INFO, dinfo.seedname + " " + dinfo.staname + " " + dinfo.chname, FuncName ); /* stringstream for reporting */ auto& report = reportV[ich]; //std::cerr<<"memory consumed @1 = "<<memo.MemConsumed()<<" Mb (ithread = "<<ithread<<")"<<std::endl; /* extract the original sac from seed */ float gapfrac; SacRec sac( report ); sac.SetMaxMemForParallel( MemTotal * dinfo.memomax * 0.8 / omp_get_num_threads() ); SeedRec seedcur( dinfo.seedname, dinfo.rdsexe, report ); if( ! seedcur.ExtractSac( dinfo.staname, dinfo.netname, dinfo.chname, dinfo.sps, dinfo.rec_outname, dinfo.resp_outname, gapfrac, sac ) ) { sacV.push_back( std::move(sac) ); //sacV.push_back( SacRec() ); continue; } sac.Write( dinfo.osac_outname ); /* remove response and cut */ sac.RmRESP( dinfo.resp_outname, dinfo.perl*0.8, dinfo.perh*1.3, dinfo.evrexe ); char evtime[15]; sprintf( evtime, "%04d%02d%02d000000\0", dinfo.year, dinfo.month, dinfo.day ); sac.ZoomToEvent( evtime, -12345., -12345., dinfo.t1, dinfo.tlen ); sac.Write( dinfo.fsac_outname ); //sac.WriteHD("/usr/temp.SAC"); sacV.push_back( std::move(sac) ); } /* time-domain normalization */ TNormAll( sacV, dinfoV, SyncNorm ); /* fre-domain normalization */ // convert sacs to am&ph and store ams in sacV for( int isac=0; isac<sacV.size(); isac++ ) { auto& dinfo = dinfoV[isac]; auto& sac = sacV[isac]; if( ! sac.sig ) continue; SacRec sac_am, sac_ph; sac.ToAmPh( sac_am, sac_ph ); sac = std::move(sac_am); //sac = sac_am; sac_ph.Write( dinfo.fsac_outname + ".ph" ); } // normalize FNormAll( sacV, dinfoV, SyncNorm ); // write am for( int isac=0; isac<sacV.size(); isac++ ) { auto& dinfo = dinfoV[isac]; auto& sac = sacV[isac]; if( ! sac.sig ) continue; sac.Write( dinfo.fsac_outname + ".am" ); } /* log if any warning */ for( const auto& report : reportV ) { std::string warning = report.str(); if( ! warning.empty() ) logger.Hold( WARNING, "\n" + warning, FuncName ); logger.flush(); } // for dinfo } catch ( std::exception& e ) { logger.Hold( ERROR, e.what(), FuncName ); } // current event done } // main while loop } // parallel region E logger.Hold( INFO, "All threads finished.", FuncName ); } catch ( std::exception& e ) { logger.Hold(FATAL, e.what(), FuncName); return -2; } catch (...) { logger.Hold(FATAL, "unknown exception", FuncName); return -2; } return 0; }
template <typename PointT> void pcl::registration::CorrespondenceRejectorProcrustes<PointT>::getRemainingCorrespondences ( const pcl::Correspondences& original_correspondences, pcl::Correspondences& remaining_correspondences) { int nr_correspondences = (int)original_correspondences.size (); std::vector<int> source_indices (nr_correspondences); std::vector<int> target_indices (nr_correspondences); // Copy the query-match indices for (size_t i = 0; i < original_correspondences.size (); ++i) { source_indices[i] = original_correspondences[i].index_query; target_indices[i] = original_correspondences[i].index_match; } // from pcl/registration/icp.hpp: std::vector<int> source_indices_good; std::vector<int> target_indices_good; { // From the set of correspondences found, attempt to remove outliers // Create the registration model typedef typename pcl::SampleConsensusModelNonRigid<PointT>::Ptr SampleConsensusModelNonRigidPtr; SampleConsensusModelNonRigidPtr model; model.reset (new pcl::SampleConsensusModelNonRigid<PointT> (input_, source_indices)); // Pass the target_indices model->setInputTarget (target_, target_indices); // Create a RANSAC model pcl::RandomSampleConsensus<PointT> sac (model, inlier_threshold_); sac.setMaxIterations (max_iterations_); // Compute the set of inliers if (!sac.computeModel ()) { remaining_correspondences = original_correspondences; best_transformation_.setIdentity (); return; } else { std::vector<int> inliers; sac.getInliers (inliers); if (inliers.size () < 3) { remaining_correspondences = original_correspondences; best_transformation_.setIdentity (); return; } boost::unordered_map<int, int> index_to_correspondence; for (int i = 0; i < nr_correspondences; ++i) index_to_correspondence[original_correspondences[i].index_query] = i; remaining_correspondences.resize (inliers.size ()); for (size_t i = 0; i < inliers.size (); ++i) remaining_correspondences[i] = original_correspondences[index_to_correspondence[inliers[i]]]; // get best transformation Eigen::VectorXf model_coefficients; sac.getModelCoefficients (model_coefficients); best_transformation_.row (0) = model_coefficients.segment<4>(0); best_transformation_.row (1) = model_coefficients.segment<4>(4); best_transformation_.row (2) = model_coefficients.segment<4>(8); best_transformation_.row (3) = model_coefficients.segment<4>(12); } } }
template <typename PointT> void pcl::registration::CorrespondenceRejectorSampleConsensus2D<PointT>::getRemainingCorrespondences ( const pcl::Correspondences& original_correspondences, pcl::Correspondences& remaining_correspondences) { if (!input_) { PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] No input cloud dataset was given!\n", getClassName ().c_str ()); return; } if (!target_) { PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] No input target dataset was given!\n", getClassName ().c_str ()); return; } if (projection_matrix_ == Eigen::Matrix3f::Identity ()) { PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] Intrinsic camera parameters not given!\n", getClassName ().c_str ()); return; } int nr_correspondences = static_cast<int> (original_correspondences.size ()); std::vector<int> source_indices (nr_correspondences); std::vector<int> target_indices (nr_correspondences); // Copy the query-match indices for (size_t i = 0; i < original_correspondences.size (); ++i) { source_indices[i] = original_correspondences[i].index_query; target_indices[i] = original_correspondences[i].index_match; } // from pcl/registration/icp.hpp: std::vector<int> source_indices_good; std::vector<int> target_indices_good; // From the set of correspondences found, attempt to remove outliers typename pcl::SampleConsensusModelRegistration2D<PointT>::Ptr model (new pcl::SampleConsensusModelRegistration2D<PointT> (input_, source_indices)); // Pass the target_indices model->setInputTarget (target_, target_indices); model->setProjectionMatrix (projection_matrix_); // Create a RANSAC model pcl::RandomSampleConsensus<PointT> sac (model, inlier_threshold_); sac.setMaxIterations (max_iterations_); // Compute the set of inliers if (!sac.computeModel ()) { PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] Error computing model! Returning the original correspondences...\n", getClassName ().c_str ()); remaining_correspondences = original_correspondences; best_transformation_.setIdentity (); return; } else { if (refine_ && !sac.refineModel (2.0)) PCL_WARN ("[pcl::registration::%s::getRemainingCorrespondences] Error refining model!\n", getClassName ().c_str ()); std::vector<int> inliers; sac.getInliers (inliers); if (inliers.size () < 3) { PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] Less than 3 correspondences found!\n", getClassName ().c_str ()); remaining_correspondences = original_correspondences; best_transformation_.setIdentity (); return; } boost::unordered_map<int, int> index_to_correspondence; for (int i = 0; i < nr_correspondences; ++i) index_to_correspondence[original_correspondences[i].index_query] = i; remaining_correspondences.resize (inliers.size ()); for (size_t i = 0; i < inliers.size (); ++i) remaining_correspondences[i] = original_correspondences[index_to_correspondence[inliers[i]]]; // get best transformation Eigen::VectorXf model_coefficients; sac.getModelCoefficients (model_coefficients); best_transformation_.row (0) = model_coefficients.segment<4>(0); best_transformation_.row (1) = model_coefficients.segment<4>(4); best_transformation_.row (2) = model_coefficients.segment<4>(8); best_transformation_.row (3) = model_coefficients.segment<4>(12); } }