transf PositionStateEllipsoid::getCoreTran() const { double a = getParameter("a"); double b = getParameter("b"); double c = getParameter("c"); double beta = readVariable("beta"); double gamma = readVariable("gamma"); double tau = readVariable("tau"); double distance = readVariable("dist"); double px,py,pz; px = a * cos(beta) * cos(gamma); py = b * cos(beta) * sin(gamma); pz = c * sin(beta); //compute normal direction - for some reason this always points INSIDE the ellipsoid vec3 n1( -a*sin(beta)*cos(gamma), -b*sin(beta)*sin(gamma), c*cos(beta) ); vec3 n2( -a*cos(beta)*sin(gamma), b*cos(beta)*cos(gamma), 0 ); vec3 normal = normalise(n1) * normalise(n2); vec3 xdir(1,0,0); vec3 ydir = normal * normalise(xdir); xdir = ydir * normal; mat3 r(xdir,ydir,normal); transf handTran = transf(r, vec3(px,py,pz) - distance * normal); Quaternion zrot(tau, vec3(0,0,1)); handTran = transf(zrot, vec3(0,0,0) ) * handTran; return mHand->getApproachTran().inverse() * handTran; //So: hand tranform is: move onto the ellipsoid and rotate z axis in normal direction // --> hand approach transform inverted to get to hand origin }
QPixmap SegmentationTool::getLabel( size_t axis ) { size_t xsz = guiImage->width( axis ); size_t ysz = guiImage->heigth( axis ); if( !seedsVisible && !maskVisible ) { return( QPixmap( ) ); } if( !needUpdate[ axis ] ) { return( pixmaps[ axis ] ); } const Bial::FastTransform &transf = guiImage->getTransform( axis ); QImage res( xsz, ysz, QImage::Format_ARGB32 ); if( !seedsVisible ) { res.fill( qRgba( 0, 0, 0, 0 ) ); } else { #pragma omp parallel for for( size_t y = 0; y < ysz; ++y ) { QRgb *scanLine = ( QRgb* ) res.scanLine( y ); for( size_t x = 0; x < xsz; ++x ) { Bial::Point3D pos = transf( x, y, guiImage->currentSlice( axis ) ); char pixel = seeds( pos.x, pos.y, pos.z ); if( pixel == 1 ) { scanLine[ x ] = qRgb( 0, 255, 0 ); } else if( pixel == 2 ) { scanLine[ x ] = qRgb( 0, 0, 255 ); } else { scanLine[ x ] = qRgba( 0, 0, 0, 0 ); } } } } if( maskVisible && ( mask.size( ) == seeds.size( ) ) ) { #pragma omp parallel for for( size_t y = 0; y < ysz; ++y ) { QRgb *scanLine = ( QRgb* ) res.scanLine( y ); for( size_t x = 0; x < xsz; ++x ) { Bial::Point3D pos = transf( x, y, guiImage->currentSlice( axis ) ); if( mask( pos.x, pos.y, pos.z ) ) { scanLine[ x ] = qRgb( 255, 0, 0 ); } } } } pixmaps[ axis ] = QPixmap::fromImage( res ); return( pixmaps[ axis ] ); }
int main(){ srand(time(NULL)); int n, m, quant; printf("Digite o valor de n e m: "); scanf("%d %d", &n, &m); system("clear"); int matriz[n][m],i,j,vetor[n*m]; printf("Matriz: \n"); for (i=0; i<n; i++) { for (j=0; j<m; j++) { matriz[i][j] = rand()%10; printf("%d ",matriz[i][j]); } printf("\n"); } quant = transf(&matriz[0][0], n, m, &vetor[0]); printf("\nVetor: \n"); for (i=0; i<quant; i++) { printf("%d ", vetor[i]); } return(0); }
/* * Updates the screen. */ void graphics_update(context_t *ctx, int cycles) { // context->state starts in OAM. As soon as a certain // amount of cycles is reached, state transitions // to TRANSF, etc. if (!lcdc_display_enabled(&ctx->mem)) { // LCD is disabled. ctx->mem.io.LY = 144; ctx->gfx.state = VBLANK_WAIT; ctx->gfx.cycles = 0; return; } ctx->gfx.cycles += cycles; switch (ctx->gfx.state) { case OAM: oam(ctx); case OAM_WAIT: oam_wait(ctx); break; case TRANSF: transf(ctx); break; case HBLANK: hblank(ctx); case HBLANK_WAIT: hblank_wait(ctx); break; case VBLANK: vblank(ctx); case VBLANK_WAIT: vblank_wait(ctx); break; } }
/*! The joint Jacobian is a 6x1 matrix that relates movement of a joint (1 DOF) to movement (translation and rotation) of a point in 3D space placed at world coordinates \a toTarget. If worldCoords is true, the jacobian is expressed in world coordinate system; otherwise, it is expressed in the local coordinate system of the target point. */ Matrix Joint::jacobian(const Joint *joint, const transf &jointTran, const transf &toTarget, bool worldCoords) { transf T; if (worldCoords) { // the translation from joint coordinate system to world coordinate system T = transf(Quaternion::IDENTITY, toTarget.translation()) * jointTran.inverse(); } else { T = toTarget * jointTran.inverse(); } double M[36]; T.jacobian(M); Matrix fullJ(M,6,6,true); Matrix J(Matrix::ZEROES<Matrix>(6,1)); if (joint->getType() == REVOLUTE) { //keep rotation against z J.copySubBlock(0, 0, 6, 1, fullJ, 0, 5); } else if (joint->getType() == PRISMATIC) { //keep translation along z J.copySubBlock(0, 0, 6, 1, fullJ, 0, 2); } else { assert(0); } return J; }
void GraphOptimizer_G2O::addEdge(const int fromIdx, const int toIdx, Eigen::Matrix4f& relativePose, Eigen::Matrix<double,6,6>& informationMatrix) { #if ENABLE_DEBUG_G2O char* fileName = (char*) malloc(100); std::sprintf(fileName,"../results/matrices/edge_%i_to_%i.txt",fromIdx,toIdx); Miscellaneous::saveMatrix(relativePose,fileName); delete fileName; #endif //Transform Eigen::Matrix4f into 3D traslation and rotation for g2o g2o::Vector3d t(relativePose(0,3),relativePose(1,3),relativePose(2,3)); g2o::Quaterniond q; Eigen::Matrix3d rot = relativePose.block(0,0,3,3).cast<double>(); q = rot; g2o::SE3Quat transf(q,t); // relative transformation g2o::EdgeSE3* edge = new g2o::EdgeSE3; edge->vertices()[0] = optimizer.vertex(fromIdx); edge->vertices()[1] = optimizer.vertex(toIdx); edge->setMeasurement(transf); edge->setInformation(informationMatrix); optimizer.addEdge(edge); }
void func(double x) { double y; if (s) { last = x; y = pow(x, transf(x)); if (y < min) min = y; } }
void Leaf::computeBboxAA() { mat3 R( vec3::X, vec3::Y, vec3::Z); vec3 halfSize, center; fitBox(R, center, halfSize); mBbox.halfSize = halfSize; mBbox.setTran( transf(R, center ) ); }
void apply_default_transformation(context& ctx) { flet<bool> _enable_bv(ctx.bind_vars_enabled(), false); rule_transformer transf(ctx); ctx.ensure_closed(); transf.reset(); transf.register_plugin(alloc(datalog::mk_coi_filter, ctx)); transf.register_plugin(alloc(datalog::mk_interp_tail_simplifier, ctx)); if (ctx.get_params().xform_instantiate_arrays()) { transf.register_plugin(alloc(datalog::mk_array_instantiation, ctx, 34999)); } if(ctx.get_params().xform_transform_arrays()) transf.register_plugin(alloc(datalog::mk_array_eq_rewrite, ctx, 34998)); if (ctx.get_params().xform_quantify_arrays()) { transf.register_plugin(alloc(datalog::mk_quantifier_abstraction, ctx, 38000)); } transf.register_plugin(alloc(datalog::mk_quantifier_instantiation, ctx, 37000)); if (ctx.get_params().datalog_subsumption()) { transf.register_plugin(alloc(datalog::mk_subsumption_checker, ctx, 35005)); } transf.register_plugin(alloc(datalog::mk_rule_inliner, ctx, 35000)); transf.register_plugin(alloc(datalog::mk_coi_filter, ctx, 34990)); transf.register_plugin(alloc(datalog::mk_interp_tail_simplifier, ctx, 34980)); //and another round of inlining if (ctx.get_params().datalog_subsumption()) { transf.register_plugin(alloc(datalog::mk_subsumption_checker, ctx, 34975)); } transf.register_plugin(alloc(datalog::mk_rule_inliner, ctx, 34970)); transf.register_plugin(alloc(datalog::mk_coi_filter, ctx, 34960)); transf.register_plugin(alloc(datalog::mk_interp_tail_simplifier, ctx, 34950)); if (ctx.get_params().datalog_subsumption()) { transf.register_plugin(alloc(datalog::mk_subsumption_checker, ctx, 34940)); transf.register_plugin(alloc(datalog::mk_rule_inliner, ctx, 34930)); transf.register_plugin(alloc(datalog::mk_subsumption_checker, ctx, 34920)); transf.register_plugin(alloc(datalog::mk_rule_inliner, ctx, 34910)); transf.register_plugin(alloc(datalog::mk_subsumption_checker, ctx, 34900)); transf.register_plugin(alloc(datalog::mk_rule_inliner, ctx, 34890)); transf.register_plugin(alloc(datalog::mk_subsumption_checker, ctx, 34880)); } else { transf.register_plugin(alloc(datalog::mk_rule_inliner, ctx, 34930)); } transf.register_plugin(alloc(datalog::mk_bit_blast, ctx, 35000)); transf.register_plugin(alloc(datalog::mk_karr_invariants, ctx, 36010)); transf.register_plugin(alloc(datalog::mk_scale, ctx, 36030)); if (!ctx.get_params().xform_quantify_arrays()) { transf.register_plugin(alloc(datalog::mk_array_blast, ctx, 35999)); } if (ctx.get_params().xform_magic()) { transf.register_plugin(alloc(datalog::mk_magic_symbolic, ctx, 36020)); } ctx.transform_rules(transf); }
transf PositionStateApproach::getCoreTran() const { double dist = readVariable("dist"); double rx = readVariable("wrist 1"); double ry = readVariable("wrist 2"); transf handTran = transf(Quaternion::IDENTITY, vec3(0,0,dist)); handTran = handTran * rotate_transf(rx, vec3(1,0,0) ) * rotate_transf( ry, vec3(0,1,0) ); return mHand->getApproachTran().inverse() * handTran * mHand->getApproachTran(); }
int main( void) { char *car='c'; char *caracter; caracter=transf(&car); printf("\n %c",caracter); return 0; }
void TimeLineTrackWidget::pose_rect(TimeLineRectPtr rect) { qreal x = parent_widget_->time2pos(rect->composed()->start()); rect->setPos(transf(QPointF(x, 100 * track_->track()))); QPointF size; size.setX(parent_widget_->time2pos(rect->composed()->length())); size.setY(100); rect->sync(scale(size)); }
/*! This is a redundant way of saving the transform (since the Q doesn't need 4 variables since it's normalized. It's just the simplest way to interface with a transf so it's used as an interface between other types. Not really meant for performing searches. */ transf PositionStateComplete::getCoreTran() const { double tx = readVariable("Tx"); double ty = readVariable("Ty"); double tz = readVariable("Tz"); double qw = readVariable("Qw"); double qx = readVariable("Qx"); double qy = readVariable("Qy"); double qz = readVariable("Qz"); return transf(Quaternion(qw,qx,qy,qz), vec3(tx,ty,tz)); }
void SegmentationTool::mouseReleased( QPointF pt, Qt::MouseButtons buttons, size_t axis ) { drawing = false; if( drawing ) { const Bial::FastTransform &transf = guiImage->getTransform( axis ); Bial::Point3D actual = transf( pt.x( ), pt.y( ), ( double ) guiImage->currentSlice( axis ) ); drawSeed( lastPoint, actual ); } emit guiImage->imageUpdated( ); Q_UNUSED( buttons ); Q_UNUSED( axis ); Q_UNUSED( pt ); }
void TableCheckTask::start() { if (mStatus == FAILED) { return; } //get the details of the planning task itself if (!mDBMgr->GetPlanningTaskRecord(mPlanningTask.taskId, &mPlanningTask)) { DBGA("Failed to get planning record for task"); mStatus = FAILED; return; } loadHand(); if (mStatus == FAILED) { return; } loadObject(); if (mStatus == FAILED) { return; } //place the table in the right position //start way under the object mTable->setTran(transf(Quaternion::IDENTITY, vec3(0.0, 0.0, -200.0))); //and move up until it touches the object transf tr(Quaternion::IDENTITY, vec3(0.0, 0.0, 100.0)); World *world = graspitCore->getWorld(); world->toggleCollisions(false, mHand, mTable); mTable->moveTo(tr, 5.0, M_PI / 36.0); world->toggleCollisions(true, mHand, mTable); DBGA("Table z location: " << mTable->getTran().translation().z()); //load all the grasps std::vector<db_planner::Grasp *> graspList; if (!mDBMgr->GetGrasps(*(mPlanningTask.model), mPlanningTask.handName, &graspList)) { DBGA("Load grasps failed"); mStatus = FAILED; emptyGraspList(graspList); return; } bool success = true; std::vector<db_planner::Grasp *>::iterator it; for (it = graspList.begin(); it != graspList.end(); it++) { if (!checkSetGrasp(*it)) { success = false; break; } } emptyGraspList(graspList); if (success) { mStatus = DONE; } else { mStatus = FAILED; } }
/*! Given a point that has world transform \a toTarget, this computes the 6x6 Jacobian of this joint relative to that point. The Jacobian is either expressed in global world coordinates or in the local coordinates of the target. */ void DynJoint::jacobian(transf toTarget, Matrix *J, bool worldCoords) { transf myTran = getPrevTrans() * getPrevLink()->getTran(); transf T; if (worldCoords) { // the translation from joint coordinate system to world coordinate system T = transf(Quaternion::IDENTITY, toTarget.translation()) * myTran.inverse(); } else { T = toTarget * myTran.inverse(); } double M[36]; T.jacobian(M); J->copyMatrix(Matrix(M,6,6,true)); }
void SegmentationTool::mouseMoved( QPointF pt, size_t axis ) { if( drawing ) { const Bial::FastTransform &transf = guiImage->getTransform( axis ); Bial::Point3D actual = transf( pt.x( ), pt.y( ), ( double ) guiImage->currentSlice( axis ) ); if( lastPoint == actual ) { return; } drawSeed( lastPoint, actual ); lastPoint = actual; if( timer.elapsed( ) > 30 ) { emit guiImage->imageUpdated( ); timer.start( ); } } }
QSGNode* FeatureListModelHighlight::updatePaintNode( QSGNode* n, QQuickItem::UpdatePaintNodeData* ) { if ( mDirty && mMapSettings ) { delete n; n = new QSGNode; int count = mModel->rowCount( QModelIndex() ); QgsSGGeometry* sn = 0; QModelIndex firstIndex = mModel->index( 0, 0, QModelIndex() ); QgsVectorLayer* layer = mModel->data( firstIndex, MultiFeatureListModel::LayerRole ).value<QgsVectorLayer*>(); if ( layer ) { QgsCoordinateTransform transf( layer->crs(), mMapSettings->destinationCrs() ); for ( int i = 0; i < count; ++i ) { QgsSGGeometry* gn; QModelIndex index = mModel->index( i, 0, QModelIndex() ); QgsFeature feature = mModel->data( index, MultiFeatureListModel::FeatureRole ).value<QgsFeature>(); QgsGeometry geom( feature.geometry() ); geom.transform( transf ); if ( mSelection && mSelection->selection() == i ) { sn = new QgsSGGeometry( geom, mSelectionColor, mWidth ); sn->setFlag( QSGNode::OwnedByParent ); } else { gn = new QgsSGGeometry( geom, mColor, mWidth ); gn->setFlag( QSGNode::OwnedByParent ); n->appendChildNode( gn ); } } if ( sn ) n->appendChildNode( sn ); } mDirty = false; } return n; }
void SegmentationTool::mouseClicked( QPointF pt, Qt::MouseButtons buttons, size_t axis ) { timer.start( ); drawing = true; seedsVisible = true; if( ( drawType == 1 ) || ( drawType == 2 ) ) { switch( buttons ) { case Qt::LeftButton: drawType = 1; break; case Qt::RightButton: drawType = 2; break; } } const Bial::FastTransform &transf = guiImage->getTransform( axis ); lastPoint = transf( pt.x( ), pt.y( ), ( double ) guiImage->currentSlice( axis ) ); }
void apply_default_transformation(context& ctx) { rule_transformer transf(ctx); ctx.ensure_closed(); transf.reset(); transf.register_plugin(alloc(datalog::mk_coi_filter, ctx)); transf.register_plugin(alloc(datalog::mk_interp_tail_simplifier, ctx)); transf.register_plugin(alloc(datalog::mk_subsumption_checker, ctx, 35005)); transf.register_plugin(alloc(datalog::mk_rule_inliner, ctx, 35000)); transf.register_plugin(alloc(datalog::mk_coi_filter, ctx, 34990)); transf.register_plugin(alloc(datalog::mk_interp_tail_simplifier, ctx, 34980)); //and another round of inlining transf.register_plugin(alloc(datalog::mk_subsumption_checker, ctx, 34975)); transf.register_plugin(alloc(datalog::mk_rule_inliner, ctx, 34970)); transf.register_plugin(alloc(datalog::mk_coi_filter, ctx, 34960)); transf.register_plugin(alloc(datalog::mk_interp_tail_simplifier, ctx, 34950)); transf.register_plugin(alloc(datalog::mk_subsumption_checker, ctx, 34940)); transf.register_plugin(alloc(datalog::mk_rule_inliner, ctx, 34930)); transf.register_plugin(alloc(datalog::mk_subsumption_checker, ctx, 34920)); transf.register_plugin(alloc(datalog::mk_rule_inliner, ctx, 34910)); transf.register_plugin(alloc(datalog::mk_subsumption_checker, ctx, 34900)); transf.register_plugin(alloc(datalog::mk_rule_inliner, ctx, 34890)); transf.register_plugin(alloc(datalog::mk_subsumption_checker, ctx, 34880)); if (ctx.get_params().quantify_arrays()) { transf.register_plugin(alloc(datalog::mk_quantifier_abstraction, ctx, 33000)); transf.register_plugin(alloc(datalog::mk_array_blast, ctx, 32500)); } transf.register_plugin(alloc(datalog::mk_quantifier_instantiation, ctx, 32000)); transf.register_plugin(alloc(datalog::mk_bit_blast, ctx, 35000)); if (!ctx.get_params().quantify_arrays()) transf.register_plugin(alloc(datalog::mk_array_blast, ctx, 36000)); transf.register_plugin(alloc(datalog::mk_karr_invariants, ctx, 36010)); if (ctx.get_params().magic()) { transf.register_plugin(alloc(datalog::mk_magic_symbolic, ctx, 36020)); } transf.register_plugin(alloc(datalog::mk_scale, ctx, 36030)); ctx.transform_rules(transf); }
// read in 7 param transf given as pos(x y z) Qauternion(w x y z) int ClientSocket::readTransf(transf * tr){ bool ok = true; try{ *tr = transf(Quaternion((*(strPtr + 3)).toDouble(&ok), //qw (*(strPtr + 4)).toDouble(&ok), //qx (*(strPtr + 5)).toDouble(&ok), //qy (*(strPtr + 6)).toDouble(&ok)), //qz vec3((*strPtr).toDouble(&ok), //x (*(strPtr+1)).toDouble(&ok), //y (*(strPtr + 2)).toDouble(&ok))); //z strPtr +=7; } catch(...){ if (!ok) return FAILURE; else std::cout <<"unknown error in ClientSocket::readTransf \n"; } return SUCCESS; }
void Leaf::computeBboxOO() { //compute the covariance matrix double covMat[3][3], v[3][3]; areaWeightedCovarianceMatrix(covMat); DBGP("Cov mat:"); DBGST(print(covMat)); //perform singular value decomposition Jacobi(covMat, v); DBGP("eigenvalues:"); DBGST(print(covMat)); DBGP("eigenVectors:"); DBGST(print(v)); int first = 0, second = 1, third = 2; if (covMat[1][1] > covMat[0][0]) { std::swap(first, second); } if (covMat[2][2] > covMat[first][first]) { std::swap(first, third); } if (covMat[third][third] > covMat[second][second]) { std::swap(second, third); } DBGP("Eigenvalues: " << covMat[first][first] << " " << covMat[second][second] << " " << covMat[third][third]); //set up rotation matrix vec3 xAxis(v[0][first], v[1][first], v[2][first]); vec3 yAxis(v[0][second], v[1][second], v[2][second]); vec3 zAxis = normalise(xAxis) * normalise(yAxis); yAxis = zAxis * normalise(xAxis); xAxis = yAxis * zAxis; mat3 R(xAxis, yAxis, zAxis); DBGP("Matrix: " << R); //compute bbox extents vec3 halfSize, center; fitBox(R, center, halfSize); //rotate box so that x axis always points in the direction of largest extent first = 0; if (halfSize.y() > halfSize.x()) first = 1; if (halfSize.z() > halfSize[first]) first = 2; transf rotate = transf::IDENTITY; if (first == 1) { // y has the largest extent, rotate around z rotate = rotate_transf(M_PI/2.0, vec3(0,0,1)); } else if (first == 2) { // z has the largest extent, rotate around y rotate = rotate_transf(M_PI/2.0, vec3(0,1,0)); } halfSize = halfSize * rotate; for (int i=0; i<3; i++) { if (halfSize[i] < 0) halfSize[i] = -halfSize[i]; } mat3 RR; rotate.rotation().ToRotationMatrix(RR); R = RR * R; mBbox.halfSize = halfSize; mBbox.setTran( transf(R, center ) ); }
XpmImage *CreateXpmImage(void *data, int x, int y, const char* data_type, double (*transf)(double), const char **colormap) { XpmImage *xpmImage; double maxValue, minValue; int i, ncolors = 64; double *data_dbl; char *type; data_dbl = malloc(x * y * sizeof(double)); type = strdup(data_type); if (!data_dbl || !type) return NULL; if (strcmp(type, "signed char") && !strncmp(type, "signed ", 7)) type += 7; i = strlen(type); if (i > 3 && !strcmp(type + i - 4, " int")) type[i-4] = '\n'; if (!strcmp(type, "double")) memcpy(data_dbl, data, x * y * sizeof(double)); else if (!strcmp(type, "float")) CONVER_DATA_FROM(float); else if (!strcmp(type, "int") || !strcmp(type, "signed")) CONVER_DATA_FROM(int); else if (!strcmp(type, "unsigned")) CONVER_DATA_FROM(unsigned); else if (!strcmp(type, "long")) CONVER_DATA_FROM(long); else if (!strcmp(type, "unsigned long")) CONVER_DATA_FROM(unsigned long); else if (!strcmp(type, "short")) CONVER_DATA_FROM(short); else if (!strcmp(type, "unsigned short")) CONVER_DATA_FROM(unsigned short); else if (!strcmp(type, "char")) CONVER_DATA_FROM(char); else if (!strcmp(type, "signed char")) CONVER_DATA_FROM(signed char); else if (!strcmp(type, "unsigned char")) CONVER_DATA_FROM(unsigned char); free(type); xpmImage = (XpmImage *) malloc(sizeof(XpmImage) + ncolors * sizeof(XpmColor) + x * y * sizeof(unsigned)); if (!xpmImage) { free(data_dbl); return NULL; } xpmImage->width = x; xpmImage->height = y; xpmImage->cpp = 0; xpmImage->ncolors = ncolors; xpmImage->colorTable = (XpmColor *) (xpmImage + 1); xpmImage->mpData = (unsigned *) (xpmImage->colorTable + ncolors); memset(xpmImage->colorTable, 0, ncolors * sizeof(XpmColor)); for (i=0; i < ncolors; i++) { xpmImage->colorTable[i].c_color = (char *) colormap[i]; } if (transf) for (i=0; i < x * y; i++) data_dbl[i] = transf(data_dbl[i]); maxValue = minValue = data_dbl[0]; for (i=1; i < x * y; i++) { if (data_dbl[i] < minValue) minValue = data_dbl[i]; if (data_dbl[i] > maxValue) maxValue = data_dbl[i]; } for (i=0; i < x * y; i++) { xpmImage->mpData[i] = (unsigned)(0.5 + (ncolors-1) * (data_dbl[i] - minValue) / (maxValue - minValue)); } free(data_dbl); return xpmImage; }
int main(int argc, char *argv[]) { int step, ie, iside, i, j, k; double mflops, tmax, nelt_tot = 0.0; char Class; logical ifmortar = false, verified; double t2, trecs[t_last+1]; char *t_names[t_last+1]; //-------------------------------------------------------------------- // Initialize NUMA control //-------------------------------------------------------------------- numa_initialize_env(NUMA_MIGRATE_EXISTING); //--------------------------------------------------------------------- // Read input file (if it exists), else take // defaults from parameters //--------------------------------------------------------------------- FILE *fp; if ((fp = fopen("timer.flag", "r")) != NULL) { timeron = true; t_names[t_total] = "total"; t_names[t_init] = "init"; t_names[t_convect] = "convect"; t_names[t_transfb_c] = "transfb_c"; t_names[t_diffusion] = "diffusion"; t_names[t_transf] = "transf"; t_names[t_transfb] = "transfb"; t_names[t_adaptation] = "adaptation"; t_names[t_transf2] = "transf+b"; t_names[t_add2] = "add2"; fclose(fp); } else { timeron = false; } printf("\n\n NAS Parallel Benchmarks (NPB3.3-OMP-C) - UA Benchmark\n\n"); if ((fp = fopen("inputua.data", "r")) != NULL) { int result; printf(" Reading from input file inputua.data\n"); result = fscanf(fp, "%d", &fre); while (fgetc(fp) != '\n'); result = fscanf(fp, "%d", &niter); while (fgetc(fp) != '\n'); result = fscanf(fp, "%d", &nmxh); while (fgetc(fp) != '\n'); result = fscanf(fp, "%lf", &alpha); Class = 'U'; fclose(fp); } else { printf(" No input file inputua.data. Using compiled defaults\n"); fre = FRE_DEFAULT; niter = NITER_DEFAULT; nmxh = NMXH_DEFAULT; alpha = ALPHA_DEFAULT; Class = CLASS_DEFAULT; } dlmin = pow(0.5, REFINE_MAX); dtime = 0.04*dlmin; printf(" Levels of refinement: %8d\n", REFINE_MAX); printf(" Adaptation frequency: %8d\n", fre); printf(" Time steps: %8d dt: %15.6E\n", niter, dtime); printf(" CG iterations: %8d\n", nmxh); printf(" Heat source radius: %8.4f\n", alpha); printf(" Number of available threads: %8d\n", omp_get_max_threads()); printf("\n"); top_constants(); for (i = 1; i <= t_last; i++) { timer_clear(i); } if (timeron) timer_start(t_init); // set up initial mesh (single element) and solution (all zero) create_initial_grid(); r_init_omp((double *)ta1, ntot, 0.0); nr_init_omp((int *)sje, 4*6*nelt, -1); init_locks(); // compute tables of coefficients and weights coef(); geom1(); // compute the discrete laplacian operators setdef(); // prepare for the preconditioner setpcmo_pre(); // refine initial mesh and do some preliminary work time = 0.0; mortar(); prepwork(); adaptation(&ifmortar, 0); if (timeron) timer_stop(t_init); timer_clear(1); time = 0.0; for (step = 0; step <= niter; step++) { if (step == 1) { // reset the solution and start the timer, keep track of total no elms r_init((double *)ta1, ntot, 0.0); time = 0.0; nelt_tot = 0.0; for (i = 1; i <= t_last; i++) { if (i != t_init) timer_clear(i); } timer_start(1); } // advance the convection step convect(ifmortar); if (timeron) timer_start(t_transf2); // prepare the intital guess for cg transf(tmort, (double *)ta1); // compute residual for diffusion term based on intital guess // compute the left hand side of equation, lapacian t #pragma omp parallel default(shared) private(ie,k,j,i) { #pragma omp for for (ie = 0; ie < nelt; ie++) { laplacian(ta2[ie], ta1[ie], size_e[ie]); } // compute the residual #pragma omp for for (ie = 0; ie < nelt; ie++) { for (k = 0; k < LX1; k++) { for (j = 0; j < LX1; j++) { for (i = 0; i < LX1; i++) { trhs[ie][k][j][i] = trhs[ie][k][j][i] - ta2[ie][k][j][i]; } } } } } //end parallel // get the residual on mortar transfb(rmor, (double *)trhs); if (timeron) timer_stop(t_transf2); // apply boundary condition: zero out the residual on domain boundaries // apply boundary conidtion to trhs #pragma omp parallel for default(shared) private(ie,iside) for (ie = 0; ie < nelt; ie++) { for (iside = 0; iside < NSIDES; iside++) { if (cbc[ie][iside] == 0) { facev(trhs[ie], iside, 0.0); } } } // apply boundary condition to rmor col2(rmor, tmmor, nmor); // call the conjugate gradient iterative solver diffusion(ifmortar); // add convection and diffusion if (timeron) timer_start(t_add2); add2((double *)ta1, (double *)t, ntot); if (timeron) timer_stop(t_add2); // perform mesh adaptation time = time + dtime; if ((step != 0) && (step/fre*fre == step)) { if (step != niter) { adaptation(&ifmortar, step); } } else { ifmortar = false; } nelt_tot = nelt_tot + (double)(nelt); } timer_stop(1); tmax = timer_read(1); verify(&Class, &verified); // compute millions of collocation points advanced per second. // diffusion: nmxh advancements, convection: 1 advancement mflops = nelt_tot*(double)(LX1*LX1*LX1*(nmxh+1))/(tmax*1.e6); print_results("UA", Class, REFINE_MAX, 0, 0, niter, tmax, mflops, " coll. point advanced", verified, NPBVERSION, COMPILETIME, CS1, CS2, CS3, CS4, CS5, CS6, "(none)"); //--------------------------------------------------------------------- // More timers //--------------------------------------------------------------------- if (timeron) { for (i = 1; i <= t_last; i++) { trecs[i] = timer_read(i); } if (tmax == 0.0) tmax = 1.0; printf(" SECTION Time (secs)\n"); for (i = 1; i <= t_last; i++) { printf(" %-10s:%9.3f (%6.2f%%)\n", t_names[i], trecs[i], trecs[i]*100./tmax); if (i == t_transfb_c) { t2 = trecs[t_convect] - trecs[t_transfb_c]; printf(" --> %11s:%9.3f (%6.2f%%)\n", "sub-convect", t2, t2*100./tmax); } else if (i == t_transfb) { t2 = trecs[t_diffusion] - trecs[t_transf] - trecs[t_transfb]; printf(" --> %11s:%9.3f (%6.2f%%)\n", "sub-diffuse", t2, t2*100./tmax); } } } //-------------------------------------------------------------------- // Teardown NUMA control //-------------------------------------------------------------------- numa_shutdown(); return 0; }
/*! Given a start position which is expected to be collision-free, and a new position which which causes inter-penetration, it interpolates between the two to find the exact moment of contact. Returns false if the interpolation fails (usually because the starting point is also in collision). Only looks at possible collisions in \a colReport, which the caller must determine before calling this. */ bool WorldElement::interpolateTo(transf lastTran, transf newTran, const CollisionReport &colReport) { vec3 nextTranslation; Quaternion nextRotation; transf nextTran; int numCols = colReport.size(); //this causes the interpolation to first check the original transform //since in many cases the original position is actually the one in contact, this can save a lot of computation //as well as machine precision problems //technically, it allows t to become negative, but in practice this should never happen as long as the initial position //is collision free double t = 0.0, deltat = 1.0, minDist; bool done = false; while (!done && deltat > 1.0e-20 && t >= 0) { DBGP("move interpolation cycle") deltat /= 2.0; nextTranslation = (1.0-t)*lastTran.translation() + t*newTran.translation(); nextRotation = Quaternion::Slerp(t,lastTran.rotation(),newTran.rotation()); nextTran = transf(nextRotation,nextTranslation); DBGP("moving to time : " << t); if (setTran(nextTran) == FAILURE) { deltat = 0; break; } minDist = myWorld->getDist(colReport[0].first,colReport[0].second); for (int i=1; i<numCols; i++) { double dist = myWorld->getDist(colReport[i].first,colReport[i].second); minDist = MIN(minDist,dist); } DBGP("minDist: " << minDist); if (minDist > 0) { if (minDist < Contact::THRESHOLD * 0.5) break; t += deltat; } else t -= deltat; //debug code if ( deltat <= 1.0e-20 || t < 0) { for (int i=0; i<numCols; i++) { double dist = myWorld->getDist(colReport[i].first,colReport[i].second); DBGA(colReport[i].first->getName().latin1() << " -- " << colReport[i].second->getName().latin1() << " is " << dist); } } } if (deltat < 1.0e-20 || t < 0) { DBGP("deltat failsafe or negative t hit; interpolate failure"); fprintf(stdout,"WorldElement interpolation error!\n"); return false; } else { DBGP("deltat: " << deltat << "; minDist: " << minDist <<"; interpolate success."); return true; } }
/*! Moves the element from its current pose to the new pose specified by \a tr. This motion is performed in several steps such that the translation between each step does not exceed \a translStepSize and the angle of rotation does not exceed \a rotStepSize (expressed in radians). The intermediate poses are determined using linear interpolation for the translation and spherical linear interpolation for the rotation. If a collision is encountered during the motion, the point of first contact is determined and the element is left in that position. This function returns false if a collision was encountered (or contacts prevented the motion) or true if no collisions were encountered and the move was completed. */ bool WorldElement::moveTo(transf &newTran,double translStepSize, double rotStepSize) { bool moveFinished = false; transf origTran,nextTran,motion; Quaternion nextRotation; vec3 nextTranslation; double percentComplete,moveIncrement,translationLength; double angle; vec3 axis; bool success; CollisionReport contactReport; //DBGP("moveTo called"); origTran = getTran(); /* std::cout << "WorldElement origTran: " << origTran.translation().x() << " " << origTran.translation().y() << " " << origTran.translation().z() << " " << origTran.rotation().w << " " << origTran.rotation().x << " " << origTran.rotation().y << " " << origTran.rotation().z << " " << "\n"; */ //calculate the difference translationLength = (newTran.translation() - origTran.translation()).len(); nextRotation = newTran.rotation() * origTran.rotation().inverse(); nextRotation.ToAngleAxis(angle,axis); moveIncrement = 1.0; if (translationLength != 0.0) { if (translStepSize == ONE_STEP) moveIncrement = 1.0; else moveIncrement = MIN(moveIncrement, translStepSize / translationLength); } if (angle != 0.0) { if (rotStepSize == ONE_STEP) moveIncrement = MIN(moveIncrement, 1.0); else moveIncrement = MIN(moveIncrement, rotStepSize / angle); } // check contacts nextTranslation = (1.0-moveIncrement)*origTran.translation() + moveIncrement*newTran.translation(); nextRotation = Quaternion::Slerp(moveIncrement,origTran.rotation(), newTran.rotation()); nextTran = transf(nextRotation,nextTranslation); motion = nextTran * getTran().inverse(); if (contactsPreventMotion(motion)) { DBGP("contacts prevent motion") return false; } percentComplete = 0.0; while (!moveFinished) { percentComplete += moveIncrement; if (percentComplete >= 1.0) { percentComplete = 1.0; moveFinished = true; } nextTranslation = (1.0-percentComplete)*origTran.translation() + percentComplete*newTran.translation(); nextRotation = Quaternion::Slerp(percentComplete,origTran.rotation(), newTran.rotation()); nextTran = transf(nextRotation,nextTranslation); /* std::cout << "moveTo NextTran: " << nextTran.translation().x() << " " << nextTran.translation().y() << " " << nextTran.translation().z() << " " << nextTran.rotation().w << " " << nextTran.rotation().x << " " << nextTran.rotation().y << " " << nextTran.rotation().z << " " << "\n"; */ success = jumpTo(nextTran, &contactReport); if (!success || contactReport.size() != 0) { moveFinished = true; } } if (!success) { DBGA("JumpTo error, stopping execution. Object " << myName.latin1() << " in thread " << getWorld()->getCollisionInterface()->getThreadId()); } else { myWorld->findContacts(contactReport); } if (contactReport.size() != 0) return false; return true; }
bool HogmanSlamInterface::addEdge(const std::string& tag, int id, int dimension, int v1Id, int v2Id, const std::vector<double>& measurement, const std::vector<double>& information) { (void) tag; (void) id; if (dimension == 3) { Transformation2 transf(Vector2(measurement[0], measurement[1]), Angle(measurement[2])); Matrix3 infMat; int idx = 0; for (int r = 0; r < 3; ++r) for (int c = r; c < 3; ++c, ++idx) { assert(idx < (int)information.size()); infMat[r][c] = infMat[c][r] = information[idx]; } static Transformation2 zeroTransf; static Matrix3 I3 = Matrix3::eye(1.); PoseGraph2D::Vertex* v1=_optimizer->vertex(v1Id); if (! v1){ v1=_optimizer->addVertex(v1Id, zeroTransf, I3); assert(v1); _nodesAdded++; } PoseGraph2D::Vertex* v2=_optimizer->vertex(v2Id); if (! v2){ v2=_optimizer->addVertex(v2Id, zeroTransf, I3); assert(v2); _nodesAdded++; } _optimizer->addEdge(v1, v2, transf, infMat); } else if (dimension == 6) { Vector6 p; for (int i = 0; i < 6; ++i) p[i] = measurement[i]; // TODO verify that our conversion from Euler angles is the same as given in the example code on the webpage Transformation3 transf = Transformation3::fromVector(p); Matrix6 infMat; int idx = 0; for (int r = 0; r < 6; ++r) for (int c = r; c < 6; ++c, ++idx) { assert(idx < (int)information.size()); infMat[r][c] = infMat[c][r] = information[idx]; } static Transformation3 zeroTransf; static Matrix6 I6 = Matrix6::eye(1.); PoseGraph3D::Vertex* v1=_optimizer3D->vertex(v1Id); if (! v1){ v1=_optimizer3D->addVertex(v1Id, zeroTransf, I6); assert(v1); _nodesAdded++; } PoseGraph3D::Vertex* v2=_optimizer3D->vertex(v2Id); if (! v2){ v2=_optimizer3D->addVertex(v2Id, zeroTransf, I6); assert(v2); _nodesAdded++; } _optimizer3D->addEdge(v1, v2, transf, infMat); } else { cerr << __PRETTY_FUNCTION__ << " not implemented for this dimension" << endl; return false; } return true; }
void sample_los_extinction_clouds(std::string out_fname, TMCMCOptions &options, TLOSMCMCParams ¶ms, unsigned int N_clouds, uint64_t healpix_index) { timespec t_start, t_write, t_end; clock_gettime(CLOCK_MONOTONIC, &t_start); /*double x[] = {8., 4., -0.693, -1.61}; gsl_rng *r; seed_gsl_rng(&r); //gen_rand_los_extinction_clouds(&(x[0]), 4, r, params); double lnp_tmp = lnp_los_extinction_clouds(&(x[0]), 4, params); std::cout << lnp_tmp << std::endl; gsl_rng_free(r);*/ std::cout << "subpixel: " << std::endl; for(size_t i=0; i<params.subpixel.size(); i++) { std::cout << " " << params.subpixel[i]; } std::cout << std::endl; TNullLogger logger; unsigned int max_attempts = 2; unsigned int N_steps = options.steps; unsigned int N_samplers = options.samplers; unsigned int N_threads = options.N_threads; unsigned int ndim = 2 * N_clouds; std::vector<double> GR_transf; TLOSCloudTransform transf(ndim); double GR_threshold = 1.25; TAffineSampler<TLOSMCMCParams, TNullLogger>::pdf_t f_pdf = &lnp_los_extinction_clouds; TAffineSampler<TLOSMCMCParams, TNullLogger>::rand_state_t f_rand_state = &gen_rand_los_extinction_clouds; std::cerr << std::endl; std::cout << "Line-of-Sight Extinction Profile" << std::endl; std::cout << "====================================" << std::endl; //std::cerr << "# Setting up sampler" << std::endl; TParallelAffineSampler<TLOSMCMCParams, TNullLogger> sampler(f_pdf, f_rand_state, ndim, N_samplers*ndim, params, logger, N_threads); sampler.set_scale(2.); sampler.set_replacement_bandwidth(0.25); // Burn-in std::cerr << "# Burn-in ..." << std::endl; sampler.step(int(N_steps*25./100.), false, 0., 0., 0.); //sampler.step(int(N_steps*20./100.), false, 0., 0.5, 0.); //sampler.step(int(N_steps*5./100), false, 0., 1., 0.); sampler.step(int(N_steps*20./100.), false, 0., 0.5, 0.); sampler.step(int(N_steps*5./100.), false, 0., 1., 0.); sampler.step(int(N_steps*20./100.), false, 0., 0.5, 0.); sampler.step(int(N_steps*5./100.), false, 0., 1., 0.); sampler.step(int(N_steps*20./100.), false, 0., 0.5, 0.); sampler.step(int(N_steps*5./100), false, 0., 1., 0.); //sampler.step(N_steps, false, 0., options.p_replacement, 0.); //sampler.step(N_steps/2., false, 0., 1., 0.); sampler.print_stats(); sampler.clear(); std::cerr << "# Main run ..." << std::endl; bool converged = false; size_t attempt; for(attempt = 0; (attempt < max_attempts) && (!converged); attempt++) { sampler.step((1<<attempt)*N_steps, true, 0., options.p_replacement, 0.); std::cout << std::endl << "Transformed G-R Diagnostic:"; sampler.calc_GR_transformed(GR_transf, &transf); for(unsigned int k=0; k<ndim; k++) { std::cout << " " << std::setprecision(3) << GR_transf[k]; } std::cout << std::endl << std::endl; converged = true; for(size_t i=0; i<ndim; i++) { if(GR_transf[i] > GR_threshold) { converged = false; if(attempt != max_attempts-1) { sampler.print_stats(); std::cerr << "# Extending run ..." << std::endl; sampler.step(int(N_steps*1./5.), false, 0., 1., 0.); sampler.clear(); //logger.clear(); } break; } } } clock_gettime(CLOCK_MONOTONIC, &t_write); //std::stringstream group_name; //group_name << "/pixel " << healpix_index; //group_name << "/los clouds"; //chain.save(out_fname, group_name.str(), 0, "Delta mu, Delta E(B-V)", 3, 100, converged); std::stringstream group_name; group_name << "/pixel " << healpix_index; TChain chain = sampler.get_chain(); TChainWriteBuffer writeBuffer(ndim, 100, 1); writeBuffer.add(chain, converged); writeBuffer.write(out_fname, group_name.str(), "clouds"); clock_gettime(CLOCK_MONOTONIC, &t_end); sampler.print_stats(); std::cout << std::endl; if(!converged) { std::cerr << "# Failed to converge." << std::endl; } std::cerr << "# Number of steps: " << (1<<(attempt-1))*N_steps << std::endl; std::cerr << "# Time elapsed: " << std::setprecision(2) << (t_end.tv_sec - t_start.tv_sec) + 1.e-9*(t_end.tv_nsec - t_start.tv_nsec) << " s" << std::endl; std::cerr << "# Sample time: " << std::setprecision(2) << (t_write.tv_sec - t_start.tv_sec) + 1.e-9*(t_write.tv_nsec - t_start.tv_nsec) << " s" << std::endl; std::cerr << "# Write time: " << std::setprecision(2) << (t_end.tv_sec - t_write.tv_sec) + 1.e-9*(t_end.tv_nsec - t_write.tv_nsec) << " s" << std::endl << std::endl; }
bool G2oSlamInterface::addEdge(const std::string& tag, int id, int dimension, int v1Id, int v2Id, const std::vector<double>& measurement, const std::vector<double>& information) { (void) tag; (void) id; size_t oldEdgesSize = _optimizer->edges().size(); if (dimension == 3) { SE2 transf(measurement[0], measurement[1], measurement[2]); Eigen::Matrix3d infMat; int idx = 0; for (int r = 0; r < 3; ++r) for (int c = r; c < 3; ++c, ++idx) { assert(idx < (int)information.size()); infMat(r,c) = infMat(c,r) = information[idx]; } //cerr << PVAR(infMat) << endl; int doInit = 0; SparseOptimizer::Vertex* v1 = _optimizer->vertex(v1Id); SparseOptimizer::Vertex* v2 = _optimizer->vertex(v2Id); if (! v1) { OptimizableGraph::Vertex* v = v1 = addVertex(dimension, v1Id); _verticesAdded.insert(v); doInit = 1; ++_nodesAdded; } if (! v2) { OptimizableGraph::Vertex* v = v2 = addVertex(dimension, v2Id); _verticesAdded.insert(v); doInit = 2; ++_nodesAdded; } if (_optimizer->edges().size() == 0) { cerr << "FIRST EDGE "; if (v1->id() < v2->id()) { cerr << "fixing " << v1->id() << endl; v1->setFixed(true); } else { cerr << "fixing " << v2->id() << endl; v2->setFixed(true); } } OnlineEdgeSE2* e = new OnlineEdgeSE2; e->vertices()[0] = v1; e->vertices()[1] = v2; e->setMeasurement(transf); e->setInformation(infMat); _optimizer->addEdge(e); _edgesAdded.insert(e); if (doInit) { OptimizableGraph::Vertex* from = static_cast<OptimizableGraph::Vertex*>(e->vertices()[0]); OptimizableGraph::Vertex* to = static_cast<OptimizableGraph::Vertex*>(e->vertices()[1]); switch (doInit){ case 1: // initialize v1 from v2 { HyperGraph::VertexSet toSet; toSet.insert(to); if (e->initialEstimatePossible(toSet, from) > 0.) { e->initialEstimate(toSet, from); } break; } case 2: { HyperGraph::VertexSet fromSet; fromSet.insert(from); if (e->initialEstimatePossible(fromSet, to) > 0.) { e->initialEstimate(fromSet, to); } break; } default: cerr << "doInit wrong value\n"; } } } else if (dimension == 6) { Eigen::Isometry3d transf; Matrix<double, 6, 6> infMat; if (measurement.size() == 7) { // measurement is a Quaternion Vector7d meas; for (int i=0; i<7; ++i) meas(i) = measurement[i]; // normalize the quaternion to recover numerical precision lost by storing as human readable text Vector4d::MapType(meas.data()+3).normalize(); transf = internal::fromVectorQT(meas); for (int i = 0, idx = 0; i < infMat.rows(); ++i) for (int j = i; j < infMat.cols(); ++j){ infMat(i,j) = information[idx++]; if (i != j) infMat(j,i)=infMat(i,j); } } else { // measurement consists of Euler angles Vector6d aux; aux << measurement[0], measurement[1], measurement[2],measurement[3], measurement[4], measurement[5]; transf = internal::fromVectorET(aux); Matrix<double, 6, 6> infMatEuler; int idx = 0; for (int r = 0; r < 6; ++r) for (int c = r; c < 6; ++c, ++idx) { assert(idx < (int)information.size()); infMatEuler(r,c) = infMatEuler(c,r) = information[idx]; } // convert information matrix to our internal representation Matrix<double, 6, 6> J; SE3Quat transfAsSe3(transf.matrix().topLeftCorner<3,3>(), transf.translation()); jac_quat3_euler3(J, transfAsSe3); infMat.noalias() = J.transpose() * infMatEuler * J; //cerr << PVAR(transf.matrix()) << endl; //cerr << PVAR(infMat) << endl; } int doInit = 0; SparseOptimizer::Vertex* v1 = _optimizer->vertex(v1Id); SparseOptimizer::Vertex* v2 = _optimizer->vertex(v2Id); if (! v1) { OptimizableGraph::Vertex* v = v1 = addVertex(dimension, v1Id); _verticesAdded.insert(v); doInit = 1; ++_nodesAdded; } if (! v2) { OptimizableGraph::Vertex* v = v2 = addVertex(dimension, v2Id); _verticesAdded.insert(v); doInit = 2; ++_nodesAdded; } if (_optimizer->edges().size() == 0) { cerr << "FIRST EDGE "; if (v1->id() < v2->id()) { cerr << "fixing " << v1->id() << endl; v1->setFixed(true); } else { cerr << "fixing " << v2->id() << endl; v2->setFixed(true); } } OnlineEdgeSE3* e = new OnlineEdgeSE3; e->vertices()[0] = v1; e->vertices()[1] = v2; e->setMeasurement(transf); e->setInformation(infMat); _optimizer->addEdge(e); _edgesAdded.insert(e); if (doInit) { OptimizableGraph::Vertex* from = static_cast<OptimizableGraph::Vertex*>(e->vertices()[0]); OptimizableGraph::Vertex* to = static_cast<OptimizableGraph::Vertex*>(e->vertices()[1]); switch (doInit){ case 1: // initialize v1 from v2 { HyperGraph::VertexSet toSet; toSet.insert(to); if (e->initialEstimatePossible(toSet, from) > 0.) { e->initialEstimate(toSet, from); } break; } case 2: { HyperGraph::VertexSet fromSet; fromSet.insert(from); if (e->initialEstimatePossible(fromSet, to) > 0.) { e->initialEstimate(fromSet, to); } break; } default: cerr << "doInit wrong value\n"; } } } else { cerr << __PRETTY_FUNCTION__ << " not implemented for this dimension" << endl; return false; } if (oldEdgesSize == 0) { _optimizer->jacobianWorkspace().allocate(); } return true; }
void sample_los_extinction(std::string out_fname, TMCMCOptions &options, TLOSMCMCParams ¶ms, unsigned int N_regions, uint64_t healpix_index) { timespec t_start, t_write, t_end; clock_gettime(CLOCK_MONOTONIC, &t_start); std::cout << "guess of EBV max = " << params.EBV_guess_max << std::endl; guess_EBV_profile(options, params, N_regions); //monotonic_guess(img_stack, N_regions, params.EBV_prof_guess, options); TNullLogger logger; unsigned int max_attempts = 2; unsigned int N_steps = options.steps; unsigned int N_samplers = options.samplers; unsigned int N_threads = options.N_threads; unsigned int ndim = N_regions + 1; std::vector<double> GR_transf; TLOSTransform transf(ndim); double GR_threshold = 1.25; TAffineSampler<TLOSMCMCParams, TNullLogger>::pdf_t f_pdf = &lnp_los_extinction; TAffineSampler<TLOSMCMCParams, TNullLogger>::rand_state_t f_rand_state = &gen_rand_los_extinction_from_guess; std::cerr << std::endl; std::cout << "Line-of-Sight Extinction Profile" << std::endl; std::cout << "====================================" << std::endl; TParallelAffineSampler<TLOSMCMCParams, TNullLogger> sampler(f_pdf, f_rand_state, ndim, N_samplers*ndim, params, logger, N_threads); sampler.set_scale(1.2); sampler.set_replacement_bandwidth(0.50); // Burn-in std::cerr << "# Burn-in ..." << std::endl; sampler.step(int(N_steps*40./100.), false, 0., 0.4, 0.); sampler.step(int(N_steps*10./100), false, 0., 1.0, 0., false); sampler.step(int(N_steps*40./100.), false, 0., 0.4, 0.); sampler.step(int(N_steps*10./100), false, 0., 0.8, 0.); //sampler.step(N_steps, false, 0., options.p_replacement, 0.); //sampler.step(N_steps/2., false, 0., 1., 0.); sampler.print_stats(); sampler.clear(); std::cerr << "# Main run ..." << std::endl; bool converged = false; size_t attempt; for(attempt = 0; (attempt < max_attempts) && (!converged); attempt++) { sampler.step((1<<attempt)*N_steps, true, 0., options.p_replacement, 0.); std::cout << std::endl << "Transformed G-R Diagnostic:"; sampler.calc_GR_transformed(GR_transf, &transf); for(unsigned int k=0; k<ndim; k++) { std::cout << " " << std::setprecision(3) << GR_transf[k]; } std::cout << std::endl << std::endl; converged = true; for(size_t i=0; i<ndim; i++) { if(GR_transf[i] > GR_threshold) { converged = false; if(attempt != max_attempts-1) { sampler.print_stats(); std::cerr << "# Extending run ..." << std::endl; sampler.step(int(N_steps*1./5.), false, 0., 1., 0.); sampler.clear(); //logger.clear(); } break; } } } clock_gettime(CLOCK_MONOTONIC, &t_write); std::stringstream group_name; group_name << "/pixel " << healpix_index; TChain chain = sampler.get_chain(); TChainWriteBuffer writeBuffer(ndim, 500, 1); writeBuffer.add(chain, converged); writeBuffer.write(out_fname, group_name.str(), "los"); std::stringstream los_group_name; los_group_name << group_name.str() << "/los"; H5Utils::add_watermark<double>(out_fname, los_group_name.str(), "DM_min", params.img_stack->rect->min[0]); H5Utils::add_watermark<double>(out_fname, los_group_name.str(), "DM_max", params.img_stack->rect->max[0]); clock_gettime(CLOCK_MONOTONIC, &t_end); sampler.print_stats(); std::cout << std::endl; if(!converged) { std::cerr << "# Failed to converge." << std::endl; } std::cerr << "# Number of steps: " << (1<<(attempt-1))*N_steps << std::endl; std::cerr << "# Time elapsed: " << std::setprecision(2) << (t_end.tv_sec - t_start.tv_sec) + 1.e-9*(t_end.tv_nsec - t_start.tv_nsec) << " s" << std::endl; std::cerr << "# Sample time: " << std::setprecision(2) << (t_write.tv_sec - t_start.tv_sec) + 1.e-9*(t_write.tv_nsec - t_start.tv_nsec) << " s" << std::endl; std::cerr << "# Write time: " << std::setprecision(2) << (t_end.tv_sec - t_write.tv_sec) + 1.e-9*(t_end.tv_nsec - t_write.tv_nsec) << " s" << std::endl << std::endl; }