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