double reimann(double xa, double xb, double dx,
double ya, double yb, double dy) {
double result = 0;
// find min Z
double z = (fxy(xa, ya) < fxy(xb, yb) ) ? fxy(xa, ya) :
fxy(xb, yb);
// std::printf("[Info] Z : %f\n", z);
result = z * dx * dy;
return result;
}
void tst1() {
ast_manager m;
reg_decl_plugins(m);
sort_ref s( m.mk_uninterpreted_sort(symbol("S")), m);
func_decl_ref g( m.mk_func_decl(symbol("g"), s, s), m);
func_decl_ref h( m.mk_func_decl(symbol("h"), s, s), m);
sort * domain[2] = {s, s};
func_decl_ref f( m.mk_func_decl(symbol("f"), 2, domain, s), m);
app_ref a( m.mk_const(symbol("a"), s), m);
app_ref b( m.mk_const(symbol("b"), s), m);
expr_ref x( m.mk_var(0, s), m);
expr_ref y( m.mk_var(1, s), m);
app_ref gx( m.mk_app(g, x), m);
app_ref fgx_x( m.mk_app(f, gx.get(), x.get()), m);
app_ref ha( m.mk_app(h, a.get()), m);
app_ref gha( m.mk_app(g, ha.get()), m);
app_ref fgha_ha( m.mk_app(f, gha.get(), ha.get()), m);
tst_match(m, fgx_x, fgha_ha);
app_ref fgha_gha( m.mk_app(f, gha.get(), gha.get()), m);
tst_match(m, fgx_x, fgha_gha);
app_ref fxy( m.mk_app(f, x.get(), y.get()), m);
app_ref fyx( m.mk_app(f, y.get(), x.get()), m);
tst_match(m, fxy, fyx);
app_ref fygx( m.mk_app(f, y.get(), gx.get()), m);
tst_match(m, fxy, fygx);
tst_match(m, fygx, fxy);
}
double reimann_split(double xa, double xb, double dxCount,
double ya, double yb, double dyCount) {
double result = 0;
// find min Z
double z = (fxy(xa, ya) < fxy(xb, yb) ) ? fxy(xa, ya) :
fxy(xb, yb);
double dx = (xb-xa)/dxCount,
dy = (yb-ya)/dyCount;
result = z * dx * dy * dyCount * dxCount;
return result;
}
//线程3执行任务
void task3(int *num)
{
int x,y;
do{
read(pipe1[0],&x,sizeof(int));
printf("fx(%d)+fy(%d)=%d\n",x,x,fxy(x,x));
write(pipe2[1],&x,sizeof(int));
}while(x<=9);
close(pipe1[0]);
close(pipe2[1]);
}
/* funzione che costruisce la griglia di approx */
void define_fun_grid()
{
float hx,hy;
int i,j;
hx = (xmax - xmin)/nx;
hy = (ymax - ymin)/ny;
/* Determino la griglia di punti cross-hatched */
for (i = 0; i<=nx;i++)
x[i] = xmin + (i*hx);
for (j = 0; j<= ny; j++)
y[j] = ymin + (j*hy);
for (i=0;i<=nx;i++)
for (j=0;j<=ny;j++)
z[i][j] = fxy(x[i],y[j]);
}
bool Intrinsic::Undistort(const Point2D &xd, Point2D *xn, LA::AlignedMatrix2x2f *JT,
UndistortionMap *UM, const bool initialized) const {
#ifdef CFG_DEBUG
if (FishEye()) {
UT::Error("TODO (haomin)\n");
}
#endif
if (!NeedUndistortion()) {
*xn = xd;
JT->MakeIdentity();
return true;
}
float dr, j, dx2;
LA::AlignedMatrix2x2f J;
LA::SymmetricMatrix2x2f A;
LA::AlignedMatrix2x2f AI;
LA::Vector2f e, dx;
#ifdef FTR_UNDIST_DOG_LEG
float dx2GN, dx2GD, delta2, beta;
LA::Vector2f dxGN, dxGD;
bool update, converge;
#endif
if (UM) {
if (UM->Empty()) {
UM->Set(*this);
}
*xn = UM->Get(xd);
//#ifdef CFG_DEBUG
#if 0
*xn = xd * xn->x();
#endif
} else if (!initialized) {
*xn = xd;
}
#if defined FTR_UNDIST_VERBOSE && FTR_UNDIST_VERBOSE == 1
const Point2D _xd = m_k.GetNormalizedToImage(xd);
UT::Print("x = %03d %03d", int(_xd.x() + 0.5f), int(_xd.y() + 0.5f));
UT::Print(" e = %f", sqrtf((GetDistorted(*xn) - xd).SquaredLength() * m_k.m_fx * m_k.m_fy));
#endif
const float *ds = m_k.m_ds, *jds = m_k.m_jds;
const float dx2Conv = FTR_UNDIST_CONVERGE * fxyI();
//const float dx2Conv = FTR_UNDIST_CONVERGE * m_k.m_fxyI;
#ifdef FTR_UNDIST_DOG_LEG
delta2 = FTR_UNDIST_DL_RADIUS_INITIAL;
#endif
for (int iIter = 0; iIter < FTR_UNDIST_MAX_ITERATIONS; ++iIter) {
#if 0
//#if 1
if (UT::Debugging()) {
UT::Print("%d %e %e\n", iIter, xn->x(), xn->y());
}
#endif
const float x = xn->x(), x2 = x * x, y = xn->y(), y2 = y * y, xy = x * y;
const float r2 = x2 + y2, r4 = r2 * r2, r6 = r2 * r4;
dr = ds[4] * r6 + ds[1] * r4 + ds[0] * r2 + 1.0f;
j = jds[4] * r4 + jds[1] * r2 + ds[0];
if (m_radial6) {
const float dr2I = 1.0f / (ds[7] * r6 + ds[6] * r4 + ds[5] * r2 + 1.0f);
dr *= dr2I;
j = (j - (jds[7] * r4 + jds[6] * r2 + ds[5]) * dr) * dr2I;
}
e.x() = dr * x;
e.y() = dr * y;
J.m00() = j * (x2 + x2) + dr;
J.m01() = j * (xy + xy);
J.m11() = j * (y2 + y2) + dr;
//#ifdef CFG_DEBUG
#if 0
J.m00() = 1 - 3 * x2 - y2;
J.m01() = -2 * xy;
J.m11() = 1 - x2 - 3 * y2;
#endif
if (m_tangential) {
const float dx = jds[2] * xy + ds[3] * (r2 + x2 + x2);
const float dy = jds[3] * xy + ds[2] * (r2 + y2 + y2);
e.x() = dx + e.x();
e.y() = dy + e.y();
const float d2x = jds[2] * x, d2y = jds[2] * y;
const float d3x = jds[3] * x, d3y = jds[3] * y;
J.m00() = d3x + d3x + d3x + d2y + J.m00();
J.m01() = d2x + d3y + J.m01();
J.m11() = d3x + d2y + d2y + d2y + J.m11();
}
J.m10() = J.m01();
e -= xd;
LA::SymmetricMatrix2x2f::AAT(J, A);
//#ifdef CFG_DEBUG
#if 0
A.Set(J.m00(), J.m01(), J.m11());
#endif
const LA::Vector2f b = J * e;
if (!A.GetInverse(AI)) {
//return false;
break;
}
LA::AlignedMatrix2x2f::Ab(AI, b, dx);
dx.MakeMinus();
dx2 = dx.SquaredLength();
#ifdef FTR_UNDIST_DOG_LEG
if (!UM) {
dxGN = dx;
dx2GN = dx2;
dx2GD = 0.0f;
const float F = e.SquaredLength();
const Point2D xnBkp = *xn;
update = true;
converge = false;
for (int iIterDL = 0; iIterDL < FTR_UNDIST_DL_MAX_ITERATIONS; ++iIterDL) {
if (dx2GN > delta2 && dx2GD == 0.0f) {
const float bl = sqrtf(b.SquaredLength());
const LA::Vector2f g = b * (1.0f / bl);
const LA::Vector2f Ag = A * g;
const float xl = bl / g.Dot(Ag);
g.GetScaled(-xl, dxGD);
dx2GD = xl * xl;
#ifdef CFG_DEBUG
UT::AssertEqual(dxGD.SquaredLength(), dx2GD);
#endif
}
if (dx2GN <= delta2) {
dx = dxGN;
dx2 = dx2GN;
beta = 1.0f;
} else if (dx2GD >= delta2) {
if (delta2 == 0.0f) {
dx = dxGD;
dx2 = dx2GD;
} else {
dxGD.GetScaled(sqrtf(delta2 / dx2GD), dx);
dx2 = delta2;
}
beta = 0.0f;
} else {
const LA::Vector2f v = dxGN - dxGD;
const float d = dxGD.Dot(v), v2 = v.SquaredLength();
//beta = float((-d + sqrt(double(d) * d + (delta2 - dx2GD) * double(v2))) / v2);
beta = (-d + sqrtf(d * d + (delta2 - dx2GD) * v2)) / v2;
dx = dxGD;
dx += v * beta;
dx2 = delta2;
}
*xn += dx;
const float dFa = F - (GetDistorted(*xn) - xd).SquaredLength();
const float dFp = F - (e + J * dx).SquaredLength();
const float rho = dFa > 0.0f && dFp > 0.0f ? dFa / dFp : -1.0f;
if (rho < FTR_UNDIST_DL_GAIN_RATIO_MIN) {
delta2 *= FTR_UNDIST_DL_RADIUS_FACTOR_DECREASE;
if (delta2 < FTR_UNDIST_DL_RADIUS_MIN) {
delta2 = FTR_UNDIST_DL_RADIUS_MIN;
}
*xn = xnBkp;
update = false;
converge = false;
continue;
} else if (rho > FTR_UNDIST_DL_GAIN_RATIO_MAX) {
delta2 = std::max(delta2, FTR_UNDIST_DL_RADIUS_FACTOR_INCREASE * dx2);
if (delta2 > FTR_UNDIST_DL_RADIUS_MAX) {
delta2 = FTR_UNDIST_DL_RADIUS_MAX;
}
}
update = true;
converge = dx2 < dx2Conv;
break;
}
if (!update || converge) {
break;
}
} else
#endif
{
*xn += dx;
if (dx2 < dx2Conv) {
break;
}
}
#if defined FTR_UNDIST_VERBOSE && FTR_UNDIST_VERBOSE == 2
const std::string str = UT::String("%02d ", iIter);
if (iIter == 0) {
UT::PrintSeparator();
m_k.GetNormalizedToImage(xd).Print(std::string(str.size(), ' ') + "x = ", false, true);
}
GetNormalizedToImage(xnBkp).Print(str + "x = ", false, false);
UT::Print(" e = %f dx = %f beta = %f\n", sqrtf(F * fxy()), sqrtf(dx2 * fxy()), beta);
#endif
}
if (JT) {
J.GetTranspose(*JT);
}
#if defined FTR_UNDIST_VERBOSE && FTR_UNDIST_VERBOSE == 1
UT::Print(" --> %f\n", sqrtf((GetDistorted(*xn) - xd).SquaredLength() * m_k.m_fx * m_k.m_fy));
#endif
return true;
}
void TypeOfFE_P2ttdc::FB(const bool *whatd,const Mesh & ,const Triangle & K,const R2 & P1,RNMK_ & val) const
{
R2 P=Shrink1(P1);
// const Triangle & K(FE.T);
R2 A(K[0]), B(K[1]),C(K[2]);
R l0=1-P.x-P.y,l1=P.x,l2=P.y;
R l4_0=(4*l0-1),l4_1=(4*l1-1),l4_2=(4*l2-1);
// throwassert(FE.N == 1);
throwassert( val.N()>=6);
throwassert(val.M()==1);
// throwassert(val.K()==3 );
val=0;
// --
if (whatd[op_id])
{
RN_ f0(val('.',0,op_id));
f0[0] = l0*(2*l0-1);
f0[1] = l1*(2*l1-1);
f0[2] = l2*(2*l2-1);
f0[3] = 4*l1*l2; // oppose au sommet 0
f0[4] = 4*l0*l2; // oppose au sommet 1
f0[5] = 4*l1*l0; // oppose au sommet 3
}
if( whatd[op_dx] || whatd[op_dy] || whatd[op_dxx] || whatd[op_dyy] || whatd[op_dxy])
{
R2 Dl0(K.H(0)*cshrink1), Dl1(K.H(1)*cshrink1), Dl2(K.H(2)*cshrink1);
if (whatd[op_dx])
{
RN_ f0x(val('.',0,op_dx));
f0x[0] = Dl0.x*l4_0;
f0x[1] = Dl1.x*l4_1;
f0x[2] = Dl2.x*l4_2;
f0x[3] = 4*(Dl1.x*l2 + Dl2.x*l1) ;
f0x[4] = 4*(Dl2.x*l0 + Dl0.x*l2) ;
f0x[5] = 4*(Dl0.x*l1 + Dl1.x*l0) ;
}
if (whatd[op_dy])
{
RN_ f0y(val('.',0,op_dy));
f0y[0] = Dl0.y*l4_0;
f0y[1] = Dl1.y*l4_1;
f0y[2] = Dl2.y*l4_2;
f0y[3] = 4*(Dl1.y*l2 + Dl2.y*l1) ;
f0y[4] = 4*(Dl2.y*l0 + Dl0.y*l2) ;
f0y[5] = 4*(Dl0.y*l1 + Dl1.y*l0) ;
}
if (whatd[op_dxx])
{
RN_ fxx(val('.',0,op_dxx));
fxx[0] = 4*Dl0.x*Dl0.x;
fxx[1] = 4*Dl1.x*Dl1.x;
fxx[2] = 4*Dl2.x*Dl2.x;
fxx[3] = 8*Dl1.x*Dl2.x;
fxx[4] = 8*Dl0.x*Dl2.x;
fxx[5] = 8*Dl0.x*Dl1.x;
}
if (whatd[op_dyy])
{
RN_ fyy(val('.',0,op_dyy));
fyy[0] = 4*Dl0.y*Dl0.y;
fyy[1] = 4*Dl1.y*Dl1.y;
fyy[2] = 4*Dl2.y*Dl2.y;
fyy[3] = 8*Dl1.y*Dl2.y;
fyy[4] = 8*Dl0.y*Dl2.y;
fyy[5] = 8*Dl0.y*Dl1.y;
}
if (whatd[op_dxy])
{
assert(val.K()>op_dxy);
RN_ fxy(val('.',0,op_dxy));
fxy[0] = 4*Dl0.x*Dl0.y;
fxy[1] = 4*Dl1.x*Dl1.y;
fxy[2] = 4*Dl2.x*Dl2.y;
fxy[3] = 4*(Dl1.x*Dl2.y + Dl1.y*Dl2.x);
fxy[4] = 4*(Dl0.x*Dl2.y + Dl0.y*Dl2.x);
fxy[5] = 4*(Dl0.x*Dl1.y + Dl0.y*Dl1.x);
}
}
}
