Ejemplo n.º 1
0
void diff2hfunc_rho_tCopula_new(double* u, double* v, int* n, double* param, int* copula, double* out)
{
	int j;
	double t1, t2, t3, t4, t5, t6, t7, t8, t9, t10, t11, t12, t13, t14;
	double diff_t=0;

	double rho = param[0];
	double nu = param[1];

	for(j=0;j<*n;j++)
	{
		t1 = qt(u[j],nu,1,0);
		t2 = qt(v[j],nu,1,0);
		t3 = t1-rho*t2;
		t4 = nu+t2*t2;
		t5 = 1.0-rho*rho;
		t6 = nu+1.0;
		t7 = t4*t5/t6;
		t8 = sqrt(t7);
		t14=t4/t6;
		
		t10=t3/t8;
		t9 = dt(t10,nu+1.0,0);
		t11=nu+1.0;
		
		diff_dt_x(&t10,&t11,&diff_t);

		t12 = -t2/t8;
		t13 = t10/t7*rho*t14;
		
		out[j]=diff_t*(t12+t13)*(t12+t13) + t9*((t2/t7/t8 - 1.5*t13/t7) * (-2.0*rho)*t14 + t10/t7*t14);
		
	}
}
Ejemplo n.º 2
0
void qt(int startx, int starty, int size, char *res)
{
    char lt[2500] = { 0, }, rt[2500] = { 0, }, lb[2500] = { 0, }, rb[2500] = { 0, };
    if(size == 1) res[0] = board[starty][startx];
    else
    {
        qt(startx, starty, size / 2, lt);
        qt(startx + size / 2, starty, size / 2, rt);
        qt(startx, starty + size / 2, size / 2, lb);
        qt(startx + size / 2, starty + size / 2, size / 2, rb);

        if( (strlen(lt) + strlen(rt) + strlen(lb) + strlen(rb) == 4) &&
                !(strcmp(lt, rt) || strcmp(rt, lb) || strcmp(lb, rb)) )
            strcat(res, lt);
        else
        {
            strcat(res, "(");
            strcat(res, lt);
            strcat(res, rt);
            strcat(res, lb);
            strcat(res, rb);
            strcat(res, ")");
        }
    }
}
Ejemplo n.º 3
0
void diffhfunc_v_tCopula_new(double* u, double* v, int* n, double* param, int* copula, double* out)
{
	int j;
	double t1, t2, t3, t4, t5, t6, t7, t8, t9, t10, t11, t12, t13, t14, t15;

	double rho = param[0];
	double nu = param[1];

	for(j=0;j<*n;j++)
	{
		t1 = qt(u[j],nu,1,0);
		t2 = qt(v[j],nu,1,0);
		t3 = t1-rho*t2;
		t4 = nu+t2*t2;
		t5 = 1.0-rho*rho;
		t6 = nu+1.0;
		t7 = t4*t5/t6;
		t8 = sqrt(t7);
		
		t10=t3/t8;
		t9 = dt(t10,nu+1.0,0);
		t11=nu+1.0;

		t12=1.0/dt(t2,nu,0);
		t13=-rho/t8;
		t14=t10/t7;
		t15=t5/t11*t2;
		
		out[j]=t9*t12*(t13-t14*t15);
		
	}
}
Ejemplo n.º 4
0
void diff2PDF_rho_u_tCopula_new(double* u, double* v, int* n, double* param, int* copula, double* out)
{
	double x1, x2;
	int j=0, k=1;

	double t1, t2, t3, t4,  M, c=0, out1=0, out2=0;

	double rho = param[0];
	double nu = param[1];
	
	t1=nu+2.0;
	t4=1.0-rho*rho;

	for(j=0;j<*n;j++)
	{
		LL(copula, &k, &u[j], &v[j], &rho, &nu, &c);
		c=exp(c);
		x1=qt(u[j],nu,1,0);
		x2=qt(v[j],nu,1,0);
		M = ( nu*t4 + x1*x1 + x2*x2 - 2.0*rho*x1*x2 );
		t2=nu*rho+x1*x2;
		t3=x1-rho*x2;
		diffPDF_rho_tCopula(&u[j], &v[j], &k, param, copula, &out1);
		diffPDF_u_tCopula_new(&u[j], &v[j], &k, param, copula, &out2);
		
		out[j]=c*(t1/M/dt(x1,nu,0) * (x2 - 2.0*t2/M*t3)) + out1/c*out2;
	}
}
Ejemplo n.º 5
0
void diffPDF_u_tCopula_new(double* u, double* v, int* n, double* param, int* copula, double* out)
{
	double c, out1, t1, t2, t3, t4, t6, t7, t8, x1, x2;

	int j=0, k=1;

	double rho = param[0];
	double nu = param[1];


	for(j=0;j<*n;j++)
	{
		LL(copula, &k, &u[j], &v[j], &rho, &nu, &c);
		c=exp(c);
		x1=qt(u[j],nu,1,0);
		x2=qt(v[j],nu,1,0);
		diff_dt_u(&x1, &nu, &out1);
		t1=c/dt(x1,nu,0);
		t2=(nu+2.0)*(x1-rho*x2);
		t6=rho*rho;
		t7=x1*x1;
		t8=x2*x2;
		t3=nu*(1.0-t6)+t7+t8-2*rho*x1*x2;
		t4=t2/t3;
		out[j]=-t1*(t4+out1);
	}
}
Ejemplo n.º 6
0
void diff2hfunc_nu_v_tCopula_new(double* u, double* v, int* n, double* param, int* copula, double* out)
{
	int j;
	double t1, t2, t3, t4, t5, t6, t7, t8, t9, t10, t11, t12, t13, t14, t15, t16, t17, t18, t19, t20, t21, t22;
	double diff_t=0, diff_t2=0, diff_t3=0, diff_t4=0;
	double out1=0, out2=0;

	double rho = param[0];
	double nu = param[1];

	for(j=0;j<*n;j++)
	{
		t1 = qt(u[j],nu,1,0);
		t2 = qt(v[j],nu,1,0);
		t3 = t1-rho*t2;
		t4 = nu+t2*t2;
		t5 = 1.0-rho*rho;
		t6 = nu+1.0;
		t7 = t4*t5/t6;
		t8 = sqrt(t7);
		
		t10=t3/t8;
		t9 = dt(t10,nu+1.0,0);
		t11=nu+1.0;
		
		diff_dt_nu(&t2,&nu,&diff_t);
		diff_dt_x(&t10,&t11,&diff_t2);
		diff_dt_nu(&t10,&t11,&diff_t3);
		diff_dt_x(&t2,&nu,&diff_t4);
		
		diffX_nu_tCopula(&t1, param, &out1);
		diffX_nu_tCopula(&t2, param, &out2);
		
		t12=dt(t2,nu,0);
		
		t13=out1-rho*out2;
		t14=t13/t8;
		
		t16=(1.0+2.0*t2*out2)/t6 - t4/t6/t6;
		t15=t14 - 0.5*t10/t7*t5*t16;
		
		t17=-rho/t8 - t10/t7*t5/t6*t2;
		t18=0.5*rho/t8/t7*t5*t16;
		t19=t14/t7*t5/t6*t2;
		t20=t10/t7*t5/t6*out2;
		t21=t10/t7*t5/t6/t6*t2;
		t22=1.5*t10/t7/t7*t5*t5/t6*t16*t2;
		
		out[j]=(diff_t3/t12 + diff_t2/t12*t15 - t9*(diff_t/t12/t12 + diff_t4/t12/t12*out2)) * t17 + t9/t12*(t18-t19-t20+t21+t22);
		
	}
}
Ejemplo n.º 7
0
void diff2hfunc_nu_tCopula_new(double* u, double* v, int* n, double* param, int* copula, double* out)
{
	int j;
	double t1, t2, t3, t4, t5, t6, t7, t8, t9, t10, t11, t12, t13, t14, t15, t16, t17, t18, t19, t20, t21;
	double diff_t=0, diff_t2=0, diff_t3=0;
	double out1=0, out2=0, out3=0, out4=0;

	double rho = param[0];
	double nu = param[1];

	for(j=0;j<*n;j++)
	{
		t1 = qt(u[j],nu,1,0);
		t2 = qt(v[j],nu,1,0);
		t3 = t1-rho*t2;
		t4 = nu+t2*t2;
		t5 = 1.0-rho*rho;
		t6 = nu+1.0;
		t7 = t4*t5/t6;
		t8 = sqrt(t7);
		
		t10=t3/t8;
		t9 = dt(t10,t6,0);
		t11=nu+1.0;
		
		diff_dt_x(&t10,&t11,&diff_t);
		diff_t_nu_nu(&t10,&t11,&diff_t2);
		diff_dt_nu(&t10,&t11,&diff_t3);
		
		diffX_nu_tCopula(&t1, param, &out1);
		diffX_nu_tCopula(&t2, param, &out2);
		diff2_x_nu(&t1, &nu, &out3);
		diff2_x_nu(&t2, &nu, &out4);
		
		t12=out1-rho*out2;
		t13=t12/t8;
		t14=1.0+2.0*t2*out2;
		t15=t14/t6;
		t16=t4/t6/t6;
		t17=t15-t16;
		
		t18=(t13-0.5*t10/t7*t5*t17);
		t19=-t13/t7 + 0.75*t10/t7/t7*t5*t17;
		t20=(2.0*out2*out2 + 2.0*t2*out4)/t6 - 2.0*t15/t6 + 2.0*t16/t6;
		
		t21=(out3-rho*out4)/t8;
		
		out[j]=diff_t2+2.0*diff_t3*t18 + diff_t*t18*t18 + t9*(t19*t5*t17 + t21 - 0.5*t10/t7*t5*t20);
		
	}
}
Ejemplo n.º 8
0
void diff2hfunc_rho_nu_tCopula_new(double* u, double* v, int* n, double* param, int* copula, double* out)
{
	int j;
	double t1, t2, t3, t4, t5, t6, t7, t8, t9, t10, t11, t12, t13, t14, t15, t16, t17, t18, t19, t20, t21, t22, t23;
	double diff_t=0, diff_t3=0;
	double out1=0, out2=0;

	double rho = param[0];
	double nu = param[1];

	for(j=0;j<*n;j++)
	{
		t1 = qt(u[j],nu,1,0);
		t2 = qt(v[j],nu,1,0);
		t3 = t1-rho*t2;
		t4 = nu+t2*t2;
		t5 = 1.0-rho*rho;
		t6 = nu+1.0;
		t7 = t4*t5/t6;
		t8 = sqrt(t7);
		
		t10=t3/t8;
		t9 = dt(t10,nu+1.0,0);
		t11=nu+1.0;
		
		diff_dt_x(&t10,&t11,&diff_t);
		diff_dt_nu(&t10,&t11,&diff_t3);
		
		diffX_nu_tCopula(&t1, param, &out1);
		diffX_nu_tCopula(&t2, param, &out2);
		
		t12=out1-rho*out2;
		t13=t12/t8;
		t14=1.0+2.0*t2*out2;
		t15=t14/t6;
		t16=t4/t6/t6;
		t17=t15-t16;
		
		t18=t13 - 0.5*t10/t7*t5*t17;
		t19=-t2/t8 + t10/t7*rho*t4/t6;
		t20=0.5*t2/t7/t8 - 1.5*t10/t7/t7*rho*t4/t6;
		t21=out2/t8;
		t22=t13/t7*rho*t4/t6;
		t23=t10/t7*rho*(t6*t14-t4)/t6/t6;
		
		out[j]=(diff_t3 + diff_t*t18) * t19 + t9*(t20*t5*t17 - t21 + t22 + t23);
	}
}
Ejemplo n.º 9
0
void diff2PDF_nu_u_tCopula_new(double* u, double* v, int* n, double* param, int* copula, double* out)
{
	double x1, x2;
	int j=0, k=1;

	double t1, t2, t3, t4, t6, t7, t8, t9, t10, t11, t12, t13, M, c=0, out1=0, out2=0, diffPDF=0, diff_dt=0, diff_dt2=0, diff_dt3=0, M_nu=0;
	double t14, t15, t16, t17;

	double rho = param[0];
	double nu = param[1];
	
	t1=nu+2.0;
	t3=(nu+1.0)/nu;
	t14=rho*rho;
	t4=1.0-t14;
	t17=nu*nu;

	for(j=0;j<*n;j++)
	{
		LL(copula, &k, &u[j], &v[j], &rho, &nu, &c);
		c=exp(c);
		x1=qt(u[j],nu,1,0);
		x2=qt(v[j],nu,1,0);
		t15=x1*x1;
		t16=x2*x2;
		M = ( nu*t4 + t15 + t16 - 2.0*rho*x1*x2 );
		t2=dt(x1,nu,0);
		diffPDF_nu_tCopula_new(&u[j], &v[j], &k, param, copula, &diffPDF);
		diff_dt_nu(&x1, &nu, &diff_dt);
		diff_dt_u(&x1, &nu, &diff_dt2);
		diff_dt_x(&x1, &nu, &diff_dt3);
		diffX_nu_tCopula(&x1, param, &out1);
		diffX_nu_tCopula(&x2, param, &out2);
		t8=(x1*out2+out1*x2);
		M_nu=t4+2.0*x1*out1+2.0*x2*out2-2.0*rho*t8;
		
		t6=1.0+t15/nu;
		t7=x1-rho*x2;
		t9=-diffPDF/t2 + c/t2/t2*(diff_dt+diff_dt3*out1);
		t10=t1*t7/M + diff_dt2;
		t11=c/t2;
		t12=t7/M - t1*t7/M/M*M_nu + t1*(out1-rho*out2)/M;
		t13=-out1*t3/t6 + x1/(t17+nu*t15) + x1*t3/t6/t6 * (2.0*x1*out1/nu - t15/t17);
		
		out[j]=t9*t10 - t11*(t12+t13);
	}
	
}
Ejemplo n.º 10
0
gnm_float
qst (gnm_float p, gnm_float n, gnm_float shape,
     gboolean lower_tail, gboolean log_p)
{
	gnm_float x0;
	gnm_float params[2];

	if (n <= 0 || gnm_isnan (p) || gnm_isnan (n) || gnm_isnan (shape))
		return gnm_nan;

	if (shape == 0.)
		return qt (p, n, lower_tail, log_p);

	if (!log_p && p > 0.9) {
		/* We're far into the tail.  Flip.  */
		p = 1 - p;
		lower_tail = !lower_tail;
	}

	x0 = 0.0;
	params[0] = n;
	params[1] = shape;
	return pfuncinverter (p, params, lower_tail, log_p,
			      gnm_ninf, gnm_pinf, x0,
			      pst1, dst1);
}
Ejemplo n.º 11
0
void XmlModelResource::query(Orchid::Request* request, const QModelIndex& index) {
	if(!model()) return;

	request->setMimeType("text/xml");
	if(!request->open(QIODevice::ReadWrite)) return;
	QXmlStreamWriter xml(request);

	QString qt("qt-orchid-mapping");
	xml.setAutoFormatting(true);
	xml.writeStartDocument();
	xml.writeNamespace(qt, "qt");
	xml.writeStartElement(qt, "item-model");
	int rows = model()->rowCount(index);
	int cols = model()->columnCount(index);
	for(int i = 0; i < rows; ++i) {
		xml.writeStartElement(qt, "row");
		xml.writeAttribute("pos", QString::number(i));
		for(int j = 0; j < cols; ++j) {
			QModelIndex child = model()->index(i,j, index);
			QString url = request->url(request->location().relative(name(child)));
			xml.writeStartElement(qt, "cell");
			xml.writeAttribute("pos", QString::number(j));
			xml.writeAttribute("href", url);
			xml.writeStartElement(qt, "data");
			xml.writeAttribute("role", "display");
			xml.writeCDATA(child.data(Qt::DisplayRole).toString());
			xml.writeEndElement();
			xml.writeEndElement();
		}
		xml.writeEndElement();
	}
	xml.writeEndDocument();
}
Ejemplo n.º 12
0
void
fromPose(const geometry_msgs::Pose &source, Eigen::Matrix4f &dest, tf::Transform &tf_dest)
{
    Eigen::Quaternionf q(source.orientation.w, source.orientation.x, source.orientation.y, source.orientation.z);
    q.normalize();
    Eigen::Vector3f t(source.position.x, source.position.y, source.position.z);
    Eigen::Matrix3f R(q.toRotationMatrix());
    dest(0,0) = R(0,0);
    dest(0,1) = R(0,1);
    dest(0,2) = R(0,2);
    dest(1,0) = R(1,0);
    dest(1,1) = R(1,1);
    dest(1,2) = R(1,2);
    dest(2,0) = R(2,0);
    dest(2,1) = R(2,1);
    dest(2,2) = R(2,2);
    dest(3,0) = dest(3,1)= dest(3,2) = 0;
    dest(3,3) = 1;
    dest(0,3) = t(0);
    dest(1,3) = t(1);
    dest(2,3) = t(2);
    tf::Quaternion qt(q.x(), q.y(), q.z(), q.w());
    tf_dest.setOrigin(tf::Vector3(t(0), t(1), t(2)));
    tf_dest.setRotation(qt);
}
Ejemplo n.º 13
0
int main(int argc,char *argv[])
{
  QApplication a(argc,argv);
  
  //
  // Load Translations
  //
  QTranslator qt(0);
  qt.load(QString(QTDIR)+QString("/translations/qt_")+QTextCodec::locale(),".");
  a.installTranslator(&qt);
  QTranslator rd(0);
  rd.load(QString(PREFIX)+QString("/share/rivendell/librd_")+
	     QTextCodec::locale(),".");
  a.installTranslator(&rd);

  QTranslator rdhpi(0);
  rdhpi.load(QString(PREFIX)+QString("/share/rivendell/librdhpi_")+
	     QTextCodec::locale(),".");
  a.installTranslator(&rdhpi);

  QTranslator tr(0);
  tr.load(QString(PREFIX)+QString("/share/rivendell/rdcartslots_")+
	     QTextCodec::locale(),".");
  a.installTranslator(&tr);

  MainWidget *w=new MainWidget();
  a.setMainWidget(w);
  w->setGeometry(QRect(QPoint(0,0),w->sizeHint()));
  w->show();
  return a.exec();
}
Ejemplo n.º 14
0
int main(int argc,char *argv[])
{
  QApplication a(argc,argv);
  
  //
  // Load Translations
  //
  QTranslator qt(0);
  qt.load(QString(QTDIR)+QString("/translations/qt_")+QTextCodec::locale(),
	  ".");
  a.installTranslator(&qt);

  QTranslator rd(0);
  rd.load(QString(PREFIX)+QString("/share/rivendell/librd_")+
	     QTextCodec::locale(),".");
  a.installTranslator(&rd);

  QTranslator rdhpi(0);
  rdhpi.load(QString(PREFIX)+QString("/share/rivendell/librdhpi_")+
	     QTextCodec::locale(),".");
  a.installTranslator(&rdhpi);

  QTranslator tr(0);
  tr.load(QString(PREFIX)+QString("/share/rivendell/rdselect_")+
	     QTextCodec::locale(),".");
  a.installTranslator(&tr);

  //
  // Start Event Loop
  //
  MainWidget *w=new MainWidget(NULL,"main");
  a.setMainWidget(w);
  w->show();
  return a.exec();
}
Ejemplo n.º 15
0
void djvColorUtilTest::run(int &, char **)
{
    DJV_DEBUG("djvColorUtilTest::run");
    
    scale();
    lerp();
    convert();
    qt();
}
Ejemplo n.º 16
0
  void Cfunc(double *xvec, int *xlen, int *M, double *beta0, double *alpha, double *res) {

    //    double qt(double p, double ndf, int lower_tail,int log_p);
    //    double runif(double a, double b);
    int d = 0, m, i, n = xlen[0];
    double *yvec;
    yvec = new double[n];
    double meanxy = 0.0, meanx = 0.0, meany = 0.0, meanx2 = 0.0, meany2 = 0.0;
    double thresh, num = 0.0, denom = 0.0, tobs, beta1hat, beta0hat, sighat, sighatbeta1hat;
    thresh = qt(1.0 - alpha[0] / 2.0, (double)(n - 2), 1, 0);
    //   Rprintf("Value of thresh: %g", thresh);
    //   Rprintf("\n");

    for (i = 0; i < n; i++) {
      meanx = meanx + xvec[i];
      meanx2 = meanx2 + R_pow(xvec[i], 2.0);
    }
    meanx = meanx / (double)n;
    meanx2 = meanx2 / (double)n;

    GetRNGstate();
    for (m = 0; m < M[0]; m++) {
      meany = 0;
      meany2 = 0;
      meanxy = 0;
      for (i = 0; i < n; i++) { 
	yvec[i] = beta0[0] + runif(0.0, 1.0);
	meany = meany + yvec[i];
	meany2 = meany2 + R_pow(yvec[i], 2.0);
	meanxy = meanxy + xvec[i] * yvec[i]; 
      }
      meany = meany / (double)n;
      meany2 = meany2 / (double)n;
      meanxy = meanxy / (double)n;
      
      num = meanxy - meanx * meany;
      denom = meanx2 - meanx * meanx;
      
      beta1hat = num / denom;
      beta0hat = meany - beta1hat * meanx;
      
      sighat = sqrt((double)n * (meany2 + beta0hat * beta0hat + beta1hat * beta1hat * meanx2 - 2.0 * beta0hat * meany
				 - 2.0 * beta1hat * meanxy + 2.0 * beta0hat * beta1hat * meanx) / (double)(n - 2));
      
      sighatbeta1hat = sighat / sqrt((double)n * denom);
      
      tobs = beta1hat / sighatbeta1hat;
      
      if (fabs(tobs) > thresh) d = d + 1;
    }

    PutRNGstate();
    res[0] = (double)d / (double)M[0];
    
    delete[] yvec;
  }	// End of Cfunc
Ejemplo n.º 17
0
void diffPDF_nu_tCopula_new(double* u, double* v, int* n, double* param, int* copula, double* out)
{
	double out1=0, out2=0, x1, x2;
	int j=0, k=1;

	double t1, t2, t3, t4, t5, t6, t7, t8, t9, t10, t11, t12, t13, t14, t15, t16, M, c;

	double rho = param[0];
	double nu = param[1];

	

	t1=digamma((nu+1.0)/2.0);
	t2=digamma(nu/2.0);
	t14=rho*rho;
	t3=0.5*log(1.0-t14);
	t4=(nu-2.0)/(2.0*nu);
	t5=0.5*log(nu);
	t6=-t1+t2+t3-t4-t5;
	t10=(nu+2.0)/2.0;

	for(j=0;j<*n;j++)
	{
		LL(copula, &k, &u[j], &v[j], &rho, &nu, &c);
		c=exp(c);
		x1=qt(u[j],nu,1,0);
		x2=qt(v[j],nu,1,0);
		diffX_nu_tCopula(&x1, param, &out1);
		diffX_nu_tCopula(&x2, param, &out2);
		t7=1.0+2.0*x1*out1;
		t8=1.0+2.0*x2*out2;
		t15=x2*x2;
		t16=x1*x1;
		t9=(nu+1.0)/2.0*( t7/(nu+x1*x1) + t8/(nu+t15) );
		M=nu*(1.0-t14) + t16 + t15 - 2.0*rho*x1*x2;
		t11=1.0 - t14 + 2.0*x1*out1 + 2.0*x2*out2 - 2.0*rho*(x1*out2+x2*out1);
		t12=0.5*log((nu+t16)*(nu+t15));
		t13=0.5*log(M);

		out[j]=c*(t6 + t9 + t12 - t10*t11/M - t13);	
	}

}
Ejemplo n.º 18
0
gnm_float
qst (gnm_float p, gnm_float n, gnm_float shape, gboolean lower_tail, gboolean log_p)
{
    if (shape == 0.)
        return qt (p, n, lower_tail, log_p);
    else if (log_p)
        return 0.;
    else
        return 0.;
}
Ejemplo n.º 19
0
void diff2PDF_u_tCopula_new(double* u, double* v, int* n, double* param, int* copula, double* out)
{
	double x1, x2;
	int j=0, k=1;

	double t1, t2, t4, t5, t6, t7, t8, t9, t11, t12, t13, M, c=0, diffPDF=0, diff_dt2=0;
	double t14, t15, t16;

	double rho = param[0];
	double nu = param[1];
	
	t1=nu+2.0;
	t14=rho*rho;
	t4=1.0-t14;

	for(j=0;j<*n;j++)
	{
		LL(copula, &k, &u[j], &v[j], &rho, &nu, &c);
		c=exp(c);
		x1=qt(u[j],nu,1,0);
		x2=qt(v[j],nu,1,0);
		t15=x1*x1;
		t16=x2*x2;
		M = ( nu*t4 + t15 + t16 - 2.0*rho*x1*x2 );
		t2=dt(x1,nu,0);
		diffPDF_u_tCopula_new(&u[j], &v[j], &k, param, copula, &diffPDF);
		diff_dt_u(&x1, &nu, &diff_dt2);
		
		t7=x1-rho*x2;
		
		t5=-diffPDF/t2 + diff_dt2/t2/t2*c;
		t6=t1*t7/M + diff_dt2;
		
		t11=c/t2;
		t8=1.0/t2;
		t9=t1/M - 2.0*t1*t7*t7/M/M;
		t13=1.0+t15/nu;
		
		t12=t8*( -(nu+1.0)/(nu+t15) + 2.0*t15* (nu+1.0)/nu/nu / t13/t13);
		
		out[j]=t5*t6 - t11*(t8*t9 + t12);
	}
}
  void GazeboRosSkidSteerDrive::publishOdometry(double step_time) {
    ros::Time current_time = ros::Time::now();
    std::string odom_frame =
      tf::resolve(tf_prefix_, odometry_frame_);
    std::string base_footprint_frame =
      tf::resolve(tf_prefix_, robot_base_frame_);

    // TODO create some non-perfect odometry!
    // getting data for base_footprint to odom transform
    math::Pose pose = this->parent->GetWorldPose();

    tf::Quaternion qt(pose.rot.x, pose.rot.y, pose.rot.z, pose.rot.w);
    tf::Vector3 vt(pose.pos.x, pose.pos.y, pose.pos.z);

    tf::Transform base_footprint_to_odom(qt, vt);
    if (this->broadcast_tf_) {

    	transform_broadcaster_->sendTransform(
        tf::StampedTransform(base_footprint_to_odom, current_time,
            odom_frame, base_footprint_frame));

    }

    // publish odom topic
    odom_.pose.pose.position.x = pose.pos.x;
    odom_.pose.pose.position.y = pose.pos.y;

    odom_.pose.pose.orientation.x = pose.rot.x;
    odom_.pose.pose.orientation.y = pose.rot.y;
    odom_.pose.pose.orientation.z = pose.rot.z;
    odom_.pose.pose.orientation.w = pose.rot.w;
    odom_.pose.covariance[0] = 0.00001;
    odom_.pose.covariance[7] = 0.00001;
    odom_.pose.covariance[14] = 1000000000000.0;
    odom_.pose.covariance[21] = 1000000000000.0;
    odom_.pose.covariance[28] = 1000000000000.0;
    odom_.pose.covariance[35] = 0.01;

    // get velocity in /odom frame
    math::Vector3 linear;
    linear = this->parent->GetWorldLinearVel();
    odom_.twist.twist.angular.z = this->parent->GetWorldAngularVel().z;

    // convert velocity to child_frame_id (aka base_footprint)
    float yaw = pose.rot.GetYaw();
    odom_.twist.twist.linear.x = cosf(yaw) * linear.x + sinf(yaw) * linear.y;
    odom_.twist.twist.linear.y = cosf(yaw) * linear.y - sinf(yaw) * linear.x;

    odom_.header.stamp = current_time;
    odom_.header.frame_id = odom_frame;
    odom_.child_frame_id = base_footprint_frame;

    odometry_publisher_.publish(odom_);
  }
Ejemplo n.º 21
0
void diffhfunc_nu_tCopula_new(double* u, double* v, int* n, double* param, int* copula, double* out)
{
	int j;
	double t1, t2, t3, t4, t5, t6, t7, t8, t9, t10, t11, t12, t13, t14, t15, t16;
	double diff_t=0, out1=0, out2=0;

	double rho = param[0];
	double nu = param[1];

	for(j=0;j<*n;j++)
	{
		t1 = qt(u[j],nu,1,0);
		t2 = qt(v[j],nu,1,0);
		t3 = t1-rho*t2;
		t4 = nu+t2*t2;
		t5 = 1.0-rho*rho;
		t6 = nu+1.0;
		t7 = t4*t5/t6;
		t8 = sqrt(t7);
		
		t10=t3/t8;
		t9 = dt(t10,t6,0);
		t11=nu+1.0;
		
		diff_t_nu(&t10,&t11,&diff_t);
		
		diffX_nu_tCopula(&t1, param, &out1);
		diffX_nu_tCopula(&t2, param, &out2);
		
		t12=out1-rho*out2;
		t13=t12/t8;
		t14=1.0+2.0*t2*out2;
		t15=t14/t6;
		t16=t4/t6/t6;
		
		out[j]=diff_t + t9*(t13 - 0.5*t10/t7*t5*(t15-t16) );
		
	}
}
Ejemplo n.º 22
0
void diff2hfunc_rho_v_tCopula_new(double* u, double* v, int* n, double* param, int* copula, double* out)
{
	int j;
	double t1, t2, t3, t4, t5, t6, t7, t8, t9, t10, t11, t12, t13, t14, t15, t16, t17, t18, t19;
	double diff_t=0;

	double rho = param[0];
	double nu = param[1];

	for(j=0;j<*n;j++)
	{
		t1 = qt(u[j],nu,1,0);
		t2 = qt(v[j],nu,1,0);
		t3 = t1-rho*t2;
		t4 = nu+t2*t2;
		t5 = 1.0-rho*rho;
		t6 = nu+1.0;
		t7 = t4*t5/t6;
		t8 = sqrt(t7);
		
		t10=t3/t8;
		t9 = dt(t10,t6,0);
		t11=nu+1.0;
		
		diff_dt_x(&t10,&t11,&diff_t);
		
		t12=dt(t2,nu,0);
		t13=-t2/t8 + t10/t7*rho*t4/t6;
		t14=-rho/t8 - t10/t7*t2*t5/t6;
		t15=-1.0/t8;
		t16=t2*t2/t8/t7*t5/t6;
		t17=t10/t7*2.0*rho*t2/t6;
		t18=1.5*t10/t7/t7*t5/t6*t2 + 0.5*rho/t8/t7;
		t19=-2.0*rho*t4/t6;
		
		out[j]=diff_t/t12*t13*t14 + t9/t12*(t15+t16+t17+t18*t19);
	}
}
Ejemplo n.º 23
0
void diff2PDF_rho_nu_tCopula_new(double* u, double* v, int* n, double* param, int* copula, double* out)
{
	double out1=0, out2=0, x1, x2;
	int j=0, k=1;

	double t3, t4, t5, t6, t7, t8, t9, t10, t11, M_rho, M_nu, M, c;

	double rho = param[0];
	double nu = param[1];

	
	t4=1.0-rho*rho;
	t3=rho/t4;
	t5=nu+2.0;

	

	for(j=0;j<*n;j++)
	{
		LL(copula, &k, &u[j], &v[j], &rho, &nu, &c);
		c=exp(c);
		x1=qt(u[j],nu,1,0);
		x2=qt(v[j],nu,1,0);
		diffX_nu_tCopula(&x1, param, &out1);
		diffX_nu_tCopula(&x2, param, &out2);
		t10=x1*x1;
		t11=x2*x2;
		M = ( nu*t4 + t10 + t11 - 2.0*rho*x1*x2 );
		diffPDF_rho_tCopula(&u[j], &v[j], &k, param, copula, &t6);
		diffPDF_nu_tCopula_new(&u[j], &v[j], &k, param, copula, &t7);
		M_rho=-2.0*(nu*rho+x1*x2);
		t8=(x1*out2+out1*x2);
		M_nu=t4+2.0*x1*out1+2.0*x2*out2-2.0*rho*t8;
		t9=-t3+t5/M*(rho+t8+0.5*M_nu*M_rho/M)-0.5*M_rho/M;

		out[j]=c*t9+t6*t7/c;	
	}
}
Ejemplo n.º 24
0
void diff2PDF_u_v_tCopula_new(double* u, double* v, int* n, double* param, int* copula, double* out)
{
	double x1, x2;
	int j=0, k=1;

	double t1, t2, t4, t5, t6, t7, t8, t9, t10, t11, t12, M, c=0, diff_dt1=0, diff_dt2=0;

	double rho = param[0];
	double nu = param[1];
	
	t1=nu+2.0;
	t4=1.0-rho*rho;

	for(j=0;j<*n;j++)
	{
		LL(copula, &k, &u[j], &v[j], &rho, &nu, &c);
		c=exp(c);
		x1=qt(u[j],nu,1,0);
		x2=qt(v[j],nu,1,0);
		M = ( nu*t4 + x1*x1 + x2*x2 - 2.0*rho*x1*x2 );
		t2=dt(x1,nu,0);
		t5=dt(x2,nu,0);
		
		t6=x1-rho*x2;
		t7=x2-rho*x1;
		
		diff_dt_u(&x1, &nu, &diff_dt1);
		diff_dt_u(&x2, &nu, &diff_dt2);
		
		t8=c/t2/t5;
		t9=t1*t6/M + diff_dt1;
		t10=t1*t7/M + diff_dt2;
		t11=t1*rho/M;
		t12=2.0*t1*t6*t7/M/M;
		
		out[j]=t8*(t9*t10 + t11 + t12);
	}
}
Ejemplo n.º 25
0
int main()
{
    int n, i;
    char res[10000] = { 0, };
    scanf("%d", &n);

    for(i = 0 ; i < n ; ++i)
        scanf("%s", *(board + i));

    qt(0, 0, n, res);
    printf("%s\n", res);

    return 0;
}
Ejemplo n.º 26
0
void diff2hfunc_v_tCopula_new(double* u, double* v, int* n, double* param, int* copula, double* out)
{
	int j;
	double t1, t2, t3, t4, t5, t6, t7, t8, t9, t10, t11, t12, t13, t14, t15, t16;
	double diff_t=0, diff_t2=0;

	double rho = param[0];
	double nu = param[1];

	for(j=0;j<*n;j++)
	{
		t1 = qt(u[j],nu,1,0);
		t2 = qt(v[j],nu,1,0);
		t3 = t1-rho*t2;
		t4 = nu+t2*t2;
		t5 = 1.0-rho*rho;
		t6 = nu+1.0;
		t7 = t4*t5/t6;
		t8 = sqrt(t7);
		
		t10=t3/t8;
		t9 = dt(t10,t6,0);
		t11=nu+1.0;
		
		diff_dt_x(&t10,&t11,&diff_t);
		diff_dt_x(&t2,&nu,&diff_t2);
		t12=dt(t2,nu,0);
		
		t13=-rho/t8 - t10/t7*t5/t6*t2;
		
		t14=0.5*rho/t8/t7 + 1.5*t10/t7/t7*t5/t6*t2;
		t15=t5/t6*2.0*t2/t12;
		t16=t5/t6/t12*(t3-rho*t2)/t7/t8;
		
		out[j]=(diff_t/t12/t12*t13 - t9*diff_t2/t12/t12/t12) * t13 + t9/t12*(t14*t15 - t16);
	}
}
Ejemplo n.º 27
0
Archivo: qnt.c Proyecto: csilles/cxxr
double qnt(double p, double df, double ncp, int lower_tail, int log_p)
{
    const static double accu = 1e-13;
    const static double Eps = 1e-11; /* must be > accu */

    double ux, lx, nx, pp;

#ifdef IEEE_754
    if (ISNAN(p) || ISNAN(df) || ISNAN(ncp))
	return p + df + ncp;
#endif
    if (!R_FINITE(df)) ML_ERR_return_NAN;

    /* Was
     * df = floor(df + 0.5);
     * if (df < 1 || ncp < 0) ML_ERR_return_NAN;
     */
    if (df <= 0.0) ML_ERR_return_NAN;

    if(ncp == 0.0 && df >= 1.0) return qt(p, df, lower_tail, log_p);

    R_Q_P01_boundaries(p, ML_NEGINF, ML_POSINF);

    p = R_DT_qIv(p);

    /* Invert pnt(.) :
     * 1. finding an upper and lower bound */
    if(p > 1 - DBL_EPSILON) return ML_POSINF;
    pp = fmin2(1 - DBL_EPSILON, p * (1 + Eps));
    for(ux = fmax2(1., ncp);
	ux < DBL_MAX && pnt(ux, df, ncp, TRUE, FALSE) < pp;
	ux *= 2);
    pp = p * (1 - Eps);
    for(lx = fmin2(-1., -ncp);
	lx > -DBL_MAX && pnt(lx, df, ncp, TRUE, FALSE) > pp;
	lx *= 2);

    /* 2. interval (lx,ux)  halving : */
    do {
	nx = 0.5 * (lx + ux);
	if (pnt(nx, df, ncp, TRUE, FALSE) > p) ux = nx; else lx = nx;
    }
    while ((ux - lx) / fabs(nx) > accu);

    return 0.5 * (lx + ux);
}
Ejemplo n.º 28
0
void pointwise(prediction *pre, int m, double coverage, conf_inv *ci)
{
    double    t_dist, limit, fit, qt();
    int    i;

    ci->fit = (double *) malloc(m * sizeof(double));
    ci->upper = (double *) malloc(m * sizeof(double));
    ci->lower = (double *) malloc(m * sizeof(double));

    t_dist = qt(1 - (1 - coverage)/2, pre->df);
    for(i = 0; i < m; i++) {
        limit = pre->se_fit[i] * t_dist;
        ci->fit[i] = fit = pre->fit[i];
        ci->upper[i] = fit + limit;
        ci->lower[i] = fit - limit;
    }
}
Ejemplo n.º 29
0
int main(int argc,char *argv[])
{
  QApplication a(argc,argv);
  
  //
  // Load Translations
  //
  QString tr_path;
  QString qt_path;
#ifdef WIN32
  QSettings settings;
  settings.insertSearchPath(QSettings::Windows,"/SalemRadioLabs");
  tr_path=QString().sprintf("%s\\",
			    (const char *)settings.
			    readEntry("/Rivendell/InstallDir"));
  qt_path=tr_path;
#else
  tr_path=QString(PREFIX)+QString("/share/rivendell/");
  qt_path=QString(QTDIR)+QString("/translation/");
#endif  // WIN32
  QTranslator qt(0);
  qt.load(qt_path+QString("qt_")+QTextCodec::locale(),".");
  a.installTranslator(&qt);

  QTranslator rd(0);
  rd.load(tr_path+QString("librd_")+QTextCodec::locale(),".");
  a.installTranslator(&rd);

  QTranslator rdhpi(0);
  rdhpi.load(tr_path+QString("librdhpi_")+QTextCodec::locale(),".");
  a.installTranslator(&rdhpi);

  QTranslator tr(0);
  tr.load(tr_path+QString("rdlogmanager_")+QTextCodec::locale(),".");
  a.installTranslator(&tr);

  //
  // Start Event Loop
  //
  MainWidget *w=new MainWidget(NULL,"main");
  a.setMainWidget(w);
  w->setGeometry(QRect(QPoint(w->geometry().x(),w->geometry().y()),
		       w->sizeHint()));
  w->show();
  return a.exec();
}
Ejemplo n.º 30
0
geometry_msgs::Pose Sensors::robot2sensorTransformation(geometry_msgs::Pose pose)
{


    Eigen::Matrix4d robotPoseMat, robot2sensorMat, sensorPoseMat;
    //Robot matrix pose
    Eigen::Matrix3d R; Eigen::Vector3d T1(pose.position.x,pose.position.y,pose.position.z);
    tf::Quaternion qt(pose.orientation.x,pose.orientation.y,pose.orientation.z,pose.orientation.w);
    tf::Matrix3x3 R1(qt);
    tf::matrixTFToEigen(R1,R);
    robotPoseMat.setZero ();
    robotPoseMat.block (0, 0, 3, 3) = R;
    robotPoseMat.block (0, 3, 3, 1) = T1;
    robotPoseMat (3, 3) = 1;

    //transformation matrix
    qt = tf::createQuaternionFromRPY(sensorRPY[0],sensorRPY[1],sensorRPY[2]);
    tf::Matrix3x3 R2(qt);Eigen::Vector3d T2(sensorPose[0], sensorPose[1], sensorPose[2]);
    tf::matrixTFToEigen(R2,R);
    robot2sensorMat.setZero ();
    robot2sensorMat.block (0, 0, 3, 3) = R;
    robot2sensorMat.block (0, 3, 3, 1) = T2;
    robot2sensorMat (3, 3) = 1;

    //preform the transformation
    sensorPoseMat = robotPoseMat * robot2sensorMat;

    Eigen::Matrix4d sensor2sensorMat; //the frustum culling sensor needs this
    //the transofrmation is rotation by +90 around x axis of the sensor
    sensor2sensorMat << 1, 0, 0, 0,
                        0, 0,-1, 0,
                        0, 1, 0, 0,
                        0, 0, 0, 1;
    Eigen::Matrix4d newSensorPoseMat = sensorPoseMat * sensor2sensorMat;
    geometry_msgs::Pose p;
    Eigen::Vector3d T3;Eigen::Matrix3d Rd; tf::Matrix3x3 R3;
    Rd = newSensorPoseMat.block (0, 0, 3, 3);
    tf::matrixEigenToTF(Rd,R3);
    T3 = newSensorPoseMat.block (0, 3, 3, 1);
    p.position.x=T3[0];p.position.y=T3[1];p.position.z=T3[2];
    R3.getRotation(qt);
    p.orientation.x = qt.getX(); p.orientation.y = qt.getY();p.orientation.z = qt.getZ();p.orientation.w = qt.getW();

    return p;

}