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);
}
}
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, ")");
}
}
}
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);
}
}
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;
}
}
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);
}
}
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);
}
}
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);
}
}
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);
}
}
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);
}
}
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);
}
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();
}
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);
}
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();
}
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();
}
void djvColorUtilTest::run(int &, char **)
{
DJV_DEBUG("djvColorUtilTest::run");
scale();
lerp();
convert();
qt();
}
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
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);
}
}
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.;
}
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_);
}
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) );
}
}
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);
}
}
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;
}
}
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);
}
}
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;
}
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);
}
}
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);
}
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;
}
}
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();
}
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;
}
