Exemplo n.º 1
0
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
}
Exemplo n.º 2
0
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 ] );
}
Exemplo n.º 3
0
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);
}
Exemplo n.º 4
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;
    }
}
Exemplo n.º 5
0
/*! 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);
}
Exemplo n.º 7
0
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 ) );
}
Exemplo n.º 9
0
    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);
    }
Exemplo n.º 10
0
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();
}
Exemplo n.º 11
0
   int main( void) {

  char *car='c';
  char *caracter;
  caracter=transf(&car);

      printf("\n %c",caracter);
      return 0;
   }
Exemplo n.º 12
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));
}
Exemplo n.º 13
0
/*! 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));	
}
Exemplo n.º 14
0
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 );
}
Exemplo n.º 15
0
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));
}
Exemplo n.º 17
0
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;
}
Exemplo n.º 19
0
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 ) );
}
Exemplo n.º 20
0
    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);
    }
Exemplo n.º 21
0
// 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 ) );
}
Exemplo n.º 23
0
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;
}
Exemplo n.º 25
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;
    }

}
Exemplo n.º 26
0
/*!
  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;
}
Exemplo n.º 27
0
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;
}
Exemplo n.º 28
0
void sample_los_extinction_clouds(std::string out_fname, TMCMCOptions &options, TLOSMCMCParams &params,
                                  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;
}
Exemplo n.º 30
0
void sample_los_extinction(std::string out_fname, TMCMCOptions &options, TLOSMCMCParams &params,
                           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;
}