// ----------------------------------------------------------------------------
Track::Track(f32 tx, f32 tz)
{
ISceneManager* sm = Editor::getEditor()->getSceneManager();
m_terrain = new Terrain(sm->getRootSceneNode(), sm, 1, tx, tz,
min_(2 * ((s32) tx),200), min_(2 * ((s32) tz),200));
ISpline* spline = new TCR(sm->getRootSceneNode(), sm, 0);
m_driveline = new DriveLine(sm->getRootSceneNode(), sm, 0, spline, L"DriveLine");
m_roads.push_back(m_driveline);
m_music = "Origin.music";
m_valid = true;
} // Track
/* This routine simply does stereo rematixing for the 2 channel
* stereo mode */
void rematrix(audblk_t *audblk, stream_samples_t samples)
{
uint_32 num_bands;
uint_32 start;
uint_32 end;
uint_32 i,j;
float left,right;
if(!audblk->cplinu || audblk->cplbegf > 2)
num_bands = 4;
else if (audblk->cplbegf > 0)
num_bands = 3;
else
num_bands = 2;
for(i=0;i < num_bands; i++)
{
if(!audblk->rematflg[i])
continue;
start = rematrix_band[i].start;
end = min_(rematrix_band[i].end ,12 * audblk->cplbegf + 36);
for(j=start;j < end; j++)
{
left = samples[0][j] + samples[1][j];
right = samples[0][j] - samples[1][j];
samples[0][j] = left;
samples[1][j] = right;
}
}
}
bool SavitzkyGolayFilter::calCoeff(){
int np = (int)numPoints;
int nl = (int)numLeftHandPoints;
int nr = (int)numRightHandPoints;
int ld = (int)derivativeOrder;
int m = (int)smoothingPolynomialOrder;
int i,j,k,imj,ipj,kk,mm,pos;
Float fac,sum;
VectorFloat indx(m+1);
MatrixDouble a(m+1,m+1);
VectorFloat b(m+1);
VectorFloat c(np);
for (ipj=0; ipj<=(m << 1); ipj++) {
sum=(ipj ? 0.0 : 1.0);
for (k=1; k<=nr; k++) sum += pow(Float(k),Float(ipj));
for (k=1; k<=nl; k++) sum += pow(Float(-k),Float(ipj));
mm = min_(ipj,2*m-ipj);
for (imj = -mm; imj<=mm; imj+=2) a[(ipj+imj)/2][(ipj-imj)/2] = sum;
}
LUDecomposition alud(a);
for (j=0;j<m+1;j++) b[j]=0.0;
b[ld]=1.0;
if( !alud.solve_vector(b,b) ){
return false;
}
for (kk=0; kk<np; kk++) c[kk]=0.0;
for (k = -nl; k<=nr; k++) {
sum=b[0];
fac=1.0;
for(mm=1; mm<=m; mm++)
sum += b[mm]*(fac *= k);
kk=(np-k) % np;
c[kk]=sum;
}
//Reorder coefficients and place them in coeff
//Reorder last=0 future = np-1
pos = nl;
for(i=0; i<np; i++){
coeff[i] = c[pos--];
if(pos==0)pos=np-1;
}
return true;
}
void AdaptiveManifoldFilterN::compute_w_k(vector<Mat>& etak, Mat& dst, float sigma, int curTreeLevel)
{
CV_DbgAssert((int)etak.size() == jointCnNum);
dst.create(srcSize, CV_32FC1);
float argConst = -0.5f / (sigma*sigma);
for (int i = 0; i < srcSize.height; i++)
{
float *dstRow = dst.ptr<float>(i);
for (int cn = 0; cn < jointCnNum; cn++)
{
float *eta_kCnRow = etak[cn].ptr<float>(i);
float *jointCnRow = jointCn[cn].ptr<float>(i);
if (cn == 0)
{
sqr_dif(dstRow, eta_kCnRow, jointCnRow, srcSize.width);
}
else
{
add_sqr_dif(dstRow, eta_kCnRow, jointCnRow, srcSize.width);
}
}
if (adjust_outliers_)
{
float *minDistRow = minDistToManifoldSquared.ptr<float>(i);
if (curTreeLevel != 1)
{
min_(minDistRow, minDistRow, dstRow, srcSize.width);
}
else
{
std::memcpy(minDistRow, dstRow, srcSize.width*sizeof(float));
}
}
mul(dstRow, dstRow, argConst, srcSize.width);
//Exp_32f(dstRow, dstRow, srcSize.width);
}
cv::exp(dst, dst);
}
void x2pmp(FILE *in, FILE *out,
int scale,
int p_width, int p_length, int x_pos, int y_pos, /* in pels (units of PPI) */
char *head, char *foot,
enum orientation orient,
int invert)
{
unsigned char *buffer, *win_name;
unsigned int win_name_size, width, height, ncolors;
unsigned int buffer_size, one_plane_size, byte_width, fixed_width;
int no_of_bits;
unsigned long swaptest = 1;
XWDFileHeader header;
/* Read header from file */
if (fread((char *)&header, sizeof(header), 1, in) != 1) {
if (feof(in))
return;
else
leave("fread");
}
if (*(char *) &swaptest)
_swaplong((char *) &header, sizeof(header));
if (header.file_version != XWD_FILE_VERSION) {
fprintf(stderr,"%s: file format version %d, not %d\n", progname,
(int)header.file_version, XWD_FILE_VERSION);
}
win_name_size = abs_(header.header_size - sizeof(header));
if ((win_name = (unsigned char *)
calloc(win_name_size, (unsigned) sizeof(char))) == NULL)
leave("Can't calloc window name storage.");
/* Read window name from file */
if (fread((char *) win_name, sizeof(char), (int) win_name_size, in) !=
win_name_size)
leave("Unable to read window name from dump file.");
DEBUG(>= 1)
fprintf(stderr,"win_name =%s\n", win_name);
width = header.pixmap_width;
height = header.pixmap_height;
fixed_width = 8 * (byte_width = header.bytes_per_line);
one_plane_size = byte_width * height;
buffer_size = one_plane_size *
((header.pixmap_format == ZPixmap)? header.pixmap_depth: 1);
/* Determine orientation and scale if not specified */
if (orient == UNSPECIFIED)
orient = (fixed_width <= height)? PORTRAIT: LANDSCAPE;
if (scale <= 0) {
int real_height = height;
if (head) real_height += FONT_HEIGHT_PIXELS << 1;
if (foot) real_height += FONT_HEIGHT_PIXELS << 1;
switch(orient) {
default:
case PORTRAIT:
case UPSIDE_DOWN:
scale = min_((p_width - 2*x_pos) / fixed_width,
(p_length - 2*y_pos) / real_height);
break;
case LANDSCAPE:
case LANDSCAPE_LEFT:
scale = min_((p_length - 2*y_pos) / fixed_width,
(p_width - 2*x_pos) / real_height);
break;
}
if (scale <= 0)
leave("PixMap doesn't fit on page.");
else DEBUG(>1)
fprintf(stderr, "scaling by %d to yield %d x %d image\n",
scale, fixed_width*scale, height*scale);
}
C_FLOAT64 CPraxis::praxis_(C_FLOAT64 *t0, C_FLOAT64 *machep, C_FLOAT64 *h0,
C_INT *n, C_INT *prin, C_FLOAT64 *x, FPraxis *f, C_FLOAT64 *fmin)
{
/* System generated locals */
C_INT i__1, i__2, i__3;
C_FLOAT64 ret_val, d__1;
/* Local variables */
static C_FLOAT64 scbd;
static C_INT idim;
static bool illc;
static C_INT klmk;
static C_FLOAT64 d__[100], h__, ldfac;
static C_INT i__, j, k;
static C_FLOAT64 s, t, y[100], large, z__[100], small, value, f1;
static C_INT k2;
static C_FLOAT64 m2, m4, t2, df, dn;
static C_INT kl, ii;
static C_FLOAT64 sf;
static C_INT kt;
static C_FLOAT64 sl, vlarge;
static C_FLOAT64 vsmall;
static C_INT km1, im1;
static C_FLOAT64 dni, lds;
static C_INT ktm;
C_FLOAT64 lastValue = std::numeric_limits<C_FLOAT64>::infinity();
/* LAST MODIFIED 3/1/73 */
/* PRAXIS RETURNS THE MINIMUM OF THE FUNCTION F(X,N) OF N VARIABLES */
/* USING THE PRINCIPAL AXIS METHOD. THE GRADIENT OF THE FUNCTION IS */
/* NOT REQUIRED. */
/* FOR A DESCRIPTION OF THE ALGORITHM, SEE CHAPTER SEVEN OF */
/* "ALGORITHMS FOR FINDING ZEROS AND EXTREMA OF FUNCTIONS WITHOUT */
/* CALCULATING DERIVATIVES" BY RICHARD P BRENT. */
/* THE PARAMETERS ARE: */
/* T0 IS A TOLERANCE. PRAXIS ATTEMPTS TO RETURN PRAXIS=F(X) */
/* SUCH THAT IF X0 IS THE TRUE LOCAL MINIMUM NEAR X, THEN */
/* NORM(X-X0) < T0 + SQUAREROOT(MACHEP)*NORM(X). */
/* MACHEP IS THE MACHINE PRECISION, THE SMALLEST NUMBER SUCH THAT */
/* 1 + MACHEP > 1. MACHEP SHOULD BE 16.**-13 (ABOUT */
/* 2.22D-16) FOR REAL*8 ARITHMETIC ON THE IBM 360. */
/* H0 IS THE MAXIMUM STEP SIZE. H0 SHOULD BE SET TO ABOUT THE */
/* MAXIMUM DISTANCE FROM THE INITIAL GUESS TO THE MINIMUM. */
/* (IF H0 IS SET TOO LARGE OR TOO SMALL, THE INITIAL RATE OF */
/* CONVERGENCE MAY BE SLOW.) */
/* N (AT LEAST TWO) IS THE NUMBER OF VARIABLES UPON WHICH */
/* THE FUNCTION DEPENDS. */
/* PRIN CONTROLS THE PRINTING OF INTERMEDIATE RESULTS. */
/* IF PRIN=0, NOTHING IS PRINTED. */
/* IF PRIN=1, F IS PRINTED AFTER EVERY N+1 OR N+2 LINEAR */
/* MINIMIZATIONS. FINAL X IS PRINTED, BUT INTERMEDIATE X IS */
/* PRINTED ONLY IF N IS AT MOST 4. */
/* IF PRIN=2, THE SCALE FACTORS AND THE PRINCIPAL VALUES OF */
/* THE APPROXIMATING QUADRATIC FORM ARE ALSO PRINTED. */
/* IF PRIN=3, X IS ALSO PRINTED AFTER EVERY FEW LINEAR */
/* MINIMIZATIONS. */
/* IF PRIN=4, THE PRINCIPAL VECTORS OF THE APPROXIMATING */
/* QUADRATIC FORM ARE ALSO PRINTED. */
/* X IS AN ARRAY CONTAINING ON ENTRY A GUESS OF THE POINT OF */
/* MINIMUM, ON RETURN THE ESTIMATED POINT OF MINIMUM. */
/* F(X,N) IS THE FUNCTION TO BE MINIMIZED. F SHOULD BE A REAL*8 */
/* FUNCTION DECLARED EXTERNAL IN THE CALLING PROGRAM. */
/* FMIN IS AN ESTIMATE OF THE MINIMUM, USED ONLY IN PRINTING */
/* INTERMEDIATE RESULTS. */
/* THE APPROXIMATING QUADRATIC FORM IS */
/* Q(X') = F(X,N) + (1/2) * (X'-X)-TRANSPOSE * A * (X'-X) */
/* WHERE X IS THE BEST ESTIMATE OF THE MINIMUM AND A IS */
/* INVERSE(V-TRANSPOSE) * D * INVERSE(V) */
/* (V(*,*) IS THE MATRIX OF SEARCH DIRECTIONS; D(*) IS THE ARRAY */
/* OF SECOND DIFFERENCES). IF F HAS CONTINUOUS SECOND DERIVATIVES */
/* NEAR X0, A WILL TEND TO THE HESSIAN OF F AT X0 AS X APPROACHES X0. */
/* IT IS ASSUMED THAT ON FLOATING-POINT UNDERFLOW THE RESULT IS SET */
/* TO ZERO. */
/* THE USER SHOULD OBSERVE THE COMMENT ON HEURISTIC NUMBERS AFTER */
/* THE INITIALIZATION OF MACHINE DEPENDENT NUMBERS. */
/* .....IF N>20 OR IF N<20 AND YOU NEED MORE SPACE, CHANGE '20' TO THE */
/* LARGEST VALUE OF N IN THE NEXT CARD, IN THE CARD 'IDIM=20', AND */
/* IN THE DIMENSION STATEMENTS IN SUBROUTINES MINFIT,MIN,FLIN,QUAD. */
/* .....INITIALIZATION..... */
/* MACHINE DEPENDENT NUMBERS: */
/* Parameter adjustments */
--x;
/* Function Body */
small = *machep * *machep;
vsmall = small * small;
large = 1. / small;
vlarge = 1. / vsmall;
m2 = sqrt(*machep);
m4 = sqrt(m2);
/* HEURISTIC NUMBERS: */
/* IF THE AXES MAY BE BADLY SCALED (WHICH IS TO BE AVOIDED IF */
/* POSSIBLE), THEN SET SCBD=10. OTHERWISE SET SCBD=1. */
/* IF THE PROBLEM IS KNOWN TO BE ILL-CONDITIONED, SET ILLC=TRUE. */
/* OTHERWISE SET ILLC=FALSE. */
/* KTM IS THE NUMBER OF ITERATIONS WITHOUT IMPROVEMENT BEFORE THE */
/* ALGORITHM TERMINATES. KTM=4 IS VERY CAUTIOUS; USUALLY KTM=1 */
/* IS SATISFACTORY. */
scbd = 1.;
illc = FALSE_;
ktm = 1;
ldfac = .01;
if (illc)
{
ldfac = .1;
}
kt = 0;
global_1.nl = 0;
global_1.nf = 1;
global_1.fx = (*f)(&x[1], n);
q_1.qf1 = global_1.fx;
t = small + fabs(*t0);
t2 = t;
global_1.dmin__ = small;
h__ = *h0;
if (h__ < t * 100)
{
h__ = t * 100;
}
global_1.ldt = h__;
/* .....THE FIRST SET OF SEARCH DIRECTIONS V IS THE IDENTITY MATRIX.....
*/
i__1 = *n;
for (i__ = 1; i__ <= i__1; ++i__)
{
i__2 = *n;
for (j = 1; j <= i__2; ++j)
{
/* L10: */
q_1.v[i__ + j * 100 - 101] = 0.;
}
/* L20: */
q_1.v[i__ + i__ * 100 - 101] = 1.;
}
d__[0] = 0.;
q_1.qd0 = 0.;
i__1 = *n;
for (i__ = 1; i__ <= i__1; ++i__)
{
q_1.q0[i__ - 1] = x[i__];
/* L30: */
q_1.q1[i__ - 1] = x[i__];
}
if (*prin > 0)
{
print_(n, &x[1], prin, fmin);
}
/* .....THE MAIN LOOP STARTS HERE..... */
L40:
sf = d__[0];
d__[0] = 0.;
s = 0.;
/* .....MINIMIZE ALONG THE FIRST DIRECTION V(*,1). */
/* FX MUST BE PASSED TO MIN BY VALUE. */
value = global_1.fx;
min_(n, &c__1, &c__2, d__, &s, &value, &c_false, f, &x[1], &t,
machep, &h__);
if (s > 0.)
{
goto L50;
}
i__1 = *n;
for (i__ = 1; i__ <= i__1; ++i__)
{
/* L45: */
q_1.v[i__ - 1] = -q_1.v[i__ - 1];
}
L50:
if (sf > d__[0] * .9 && sf * .9 < d__[0])
{
goto L70;
}
i__1 = *n;
for (i__ = 2; i__ <= i__1; ++i__)
{
/* L60: */
d__[i__ - 1] = 0.;
}
/* .....THE INNER LOOP STARTS HERE..... */
L70:
i__1 = *n;
for (k = 2; k <= i__1; ++k)
{
i__2 = *n;
for (i__ = 1; i__ <= i__2; ++i__)
{
/* L75: */
y[i__ - 1] = x[i__];
}
sf = global_1.fx;
if (kt > 0)
{
illc = TRUE_;
}
L80:
kl = k;
df = 0.;
/* .....A RANDOM STEP FOLLOWS (TO AVOID RESOLUTION VALLEYS). */
/* PRAXIS ASSUMES THAT RANDOM RETURNS A RANDOM NUMBER UNIFORMLY */
/* DISTRIBUTED IN (0,1). */
if (! illc)
{
goto L95;
}
i__2 = *n;
for (i__ = 1; i__ <= i__2; ++i__)
{
s = (global_1.ldt * .1 + t2 * pow(10.0, (C_FLOAT64)kt)) * (mpRandom->getRandomCC() - 0.5);
z__[i__ - 1] = s;
i__3 = *n;
for (j = 1; j <= i__3; ++j)
{
/* L85: */
x[j] += s * q_1.v[j + i__ * 100 - 101];
}
/* L90: */
}
global_1.fx = (*f)(&x[1], n);
++global_1.nf;
/* .....MINIMIZE ALONG THE "NON-CONJUGATE" DIRECTIONS V(*,K),...,V(*,N
) */
L95:
i__2 = *n;
for (k2 = k; k2 <= i__2; ++k2)
{
sl = global_1.fx;
s = 0.;
value = global_1.fx;
min_(n, &k2, &c__2, &d__[k2 - 1], &s, &value, &c_false, f, &
x[1], &t, machep, &h__);
if (illc)
{
goto L97;
}
s = sl - global_1.fx;
goto L99;
L97:
/* Computing 2nd power */
d__1 = s + z__[k2 - 1];
s = d__[k2 - 1] * (d__1 * d__1);
L99:
if (df > s)
{
goto L105;
}
df = s;
kl = k2;
L105:
;
}
if (illc || df >= (d__1 = *machep * 100 * global_1.fx, fabs(d__1)))
{
goto L110;
}
/* .....IF THERE WAS NOT MUCH IMPROVEMENT ON THE FIRST TRY, SET */
/* ILLC=TRUE AND START THE INNER LOOP AGAIN..... */
illc = TRUE_;
goto L80;
L110:
if (k == 2 && *prin > 1)
{
vcprnt_(&c__1, d__, n);
}
/* .....MINIMIZE ALONG THE "CONJUGATE" DIRECTIONS V(*,1),...,V(*,K-1)
*/
km1 = k - 1;
i__2 = km1;
for (k2 = 1; k2 <= i__2; ++k2)
{
s = 0.;
value = global_1.fx;
min_(n, &k2, &c__2, &d__[k2 - 1], &s, &value, &c_false, f, &
x[1], &t, machep, &h__);
/* L120: */
}
f1 = global_1.fx;
global_1.fx = sf;
lds = 0.;
i__2 = *n;
for (i__ = 1; i__ <= i__2; ++i__)
{
sl = x[i__];
x[i__] = y[i__ - 1];
sl -= y[i__ - 1];
y[i__ - 1] = sl;
/* L130: */
lds += sl * sl;
}
lds = sqrt(lds);
if (lds <= small)
{
goto L160;
}
/* .....DISCARD DIRECTION V(*,KL). */
/* IF NO RANDOM STEP WAS TAKEN, V(*,KL) IS THE "NON-CONJUGATE" */
/* DIRECTION ALONG WHICH THE GREATEST IMPROVEMENT WAS MADE..... */
klmk = kl - k;
if (klmk < 1)
{
goto L141;
}
i__2 = klmk;
for (ii = 1; ii <= i__2; ++ii)
{
i__ = kl - ii;
i__3 = *n;
for (j = 1; j <= i__3; ++j)
{
/* L135: */
q_1.v[j + (i__ + 1) * 100 - 101] = q_1.v[j + i__ * 100 - 101];
}
/* L140: */
d__[i__] = d__[i__ - 1];
}
L141:
d__[k - 1] = 0.;
i__2 = *n;
for (i__ = 1; i__ <= i__2; ++i__)
{
/* L145: */
q_1.v[i__ + k * 100 - 101] = y[i__ - 1] / lds;
}
/* .....MINIMIZE ALONG THE NEW "CONJUGATE" DIRECTION V(*,K), WHICH IS */
/* THE NORMALIZED VECTOR: (NEW X) - (0LD X)..... */
value = f1;
min_(n, &k, &c__4, &d__[k - 1], &lds, &value, &c_true, f, &x[1],
&t, machep, &h__);
if (lds > 0.)
{
goto L160;
}
lds = -lds;
i__2 = *n;
for (i__ = 1; i__ <= i__2; ++i__)
{
/* L150: */
q_1.v[i__ + k * 100 - 101] = -q_1.v[i__ + k * 100 - 101];
}
L160:
global_1.ldt = ldfac * global_1.ldt;
if (global_1.ldt < lds)
{
global_1.ldt = lds;
}
if (*prin > 0)
{
print_(n, &x[1], prin, fmin);
}
t2 = 0.;
i__2 = *n;
for (i__ = 1; i__ <= i__2; ++i__)
{
/* L165: */
/* Computing 2nd power */
d__1 = x[i__];
t2 += d__1 * d__1;
}
t2 = m2 * sqrt(t2) + t;
/* .....SEE WHETHER THE LENGTH OF THE STEP TAKEN SINCE STARTING THE */
/* INNER LOOP EXCEEDS HALF THE TOLERANCE..... */
if (global_1.ldt > t2 * .5f)
{
kt = -1;
}
++kt;
if (kt > ktm)
{
goto L400;
}
/* L170: */
}
/* added manually by Pedro Mendes 11/11/1998 */
// if(callback != 0/*NULL*/) callback(global_1.fx);
/* .....THE INNER LOOP ENDS HERE. */
/* TRY QUADRATIC EXTRAPOLATION IN CASE WE ARE IN A CURVED VALLEY. */
/* L171: */
quad_(n, f, &x[1], &t, machep, &h__);
dn = 0.;
i__1 = *n;
for (i__ = 1; i__ <= i__1; ++i__)
{
d__[i__ - 1] = 1. / sqrt(d__[i__ - 1]);
if (dn < d__[i__ - 1])
{
dn = d__[i__ - 1];
}
/* L175: */
}
if (*prin > 3)
{
maprnt_(&c__1, q_1.v, &idim, n);
}
i__1 = *n;
for (j = 1; j <= i__1; ++j)
{
s = d__[j - 1] / dn;
i__2 = *n;
for (i__ = 1; i__ <= i__2; ++i__)
{
/* L180: */
q_1.v[i__ + j * 100 - 101] = s * q_1.v[i__ + j * 100 - 101];
}
}
/* .....SCALE THE AXES TO TRY TO REDUCE THE CONDITION NUMBER..... */
if (scbd <= 1.)
{
goto L200;
}
s = vlarge;
i__2 = *n;
for (i__ = 1; i__ <= i__2; ++i__)
{
sl = 0.;
i__1 = *n;
for (j = 1; j <= i__1; ++j)
{
/* L182: */
sl += q_1.v[i__ + j * 100 - 101] * q_1.v[i__ + j * 100 - 101];
}
z__[i__ - 1] = sqrt(sl);
if (z__[i__ - 1] < m4)
{
z__[i__ - 1] = m4;
}
if (s > z__[i__ - 1])
{
s = z__[i__ - 1];
}
/* L185: */
}
i__2 = *n;
for (i__ = 1; i__ <= i__2; ++i__)
{
sl = s / z__[i__ - 1];
z__[i__ - 1] = 1. / sl;
if (z__[i__ - 1] <= scbd)
{
goto L189;
}
sl = 1. / scbd;
z__[i__ - 1] = scbd;
L189:
i__1 = *n;
for (j = 1; j <= i__1; ++j)
{
/* L190: */
q_1.v[i__ + j * 100 - 101] = sl * q_1.v[i__ + j * 100 - 101];
}
/* L195: */
}
/* .....CALCULATE A NEW SET OF ORTHOGONAL DIRECTIONS BEFORE REPEATING */
/* THE MAIN LOOP. */
/* FIRST TRANSPOSE V FOR MINFIT: */
L200:
i__2 = *n;
for (i__ = 2; i__ <= i__2; ++i__)
{
im1 = i__ - 1;
i__1 = im1;
for (j = 1; j <= i__1; ++j)
{
s = q_1.v[i__ + j * 100 - 101];
q_1.v[i__ + j * 100 - 101] = q_1.v[j + i__ * 100 - 101];
/* L210: */
q_1.v[j + i__ * 100 - 101] = s;
}
/* L220: */
}
/* .....CALL MINFIT TO FIND THE SINGULAR VALUE DECOMPOSITION OF V. */
/* THIS GIVES THE PRINCIPAL VALUES AND PRINCIPAL DIRECTIONS OF THE */
/* APPROXIMATING QUADRATIC FORM WITHOUT SQUARING THE CONDITION */
/* NUMBER..... */
minfit_(&idim, n, machep, &vsmall, q_1.v, d__);
/* .....UNSCALE THE AXES..... */
if (scbd <= 1.)
{
goto L250;
}
i__2 = *n;
for (i__ = 1; i__ <= i__2; ++i__)
{
s = z__[i__ - 1];
i__1 = *n;
for (j = 1; j <= i__1; ++j)
{
/* L225: */
q_1.v[i__ + j * 100 - 101] = s * q_1.v[i__ + j * 100 - 101];
}
/* L230: */
}
i__2 = *n;
for (i__ = 1; i__ <= i__2; ++i__)
{
s = 0.;
i__1 = *n;
for (j = 1; j <= i__1; ++j)
{
/* L235: */
/* Computing 2nd power */
d__1 = q_1.v[j + i__ * 100 - 101];
s += d__1 * d__1;
}
s = sqrt(s);
d__[i__ - 1] = s * d__[i__ - 1];
s = 1 / s;
i__1 = *n;
for (j = 1; j <= i__1; ++j)
{
/* L240: */
q_1.v[j + i__ * 100 - 101] = s * q_1.v[j + i__ * 100 - 101];
}
/* L245: */
}
L250:
i__2 = *n;
for (i__ = 1; i__ <= i__2; ++i__)
{
dni = dn * d__[i__ - 1];
if (dni > large)
{
goto L265;
}
if (dni < small)
{
goto L260;
}
d__[i__ - 1] = 1 / (dni * dni);
goto L270;
L260:
d__[i__ - 1] = vlarge;
goto L270;
L265:
d__[i__ - 1] = vsmall;
L270:
;
}
/* .....SORT THE EIGENVALUES AND EIGENVECTORS..... */
sort_(&idim, n, d__, q_1.v);
global_1.dmin__ = d__[*n - 1];
if (global_1.dmin__ < small)
{
global_1.dmin__ = small;
}
illc = FALSE_;
if (m2 * d__[0] > global_1.dmin__)
{
illc = TRUE_;
}
if (*prin > 1 && scbd > 1.)
{
vcprnt_(&c__2, z__, n);
}
if (*prin > 1)
{
vcprnt_(&c__3, d__, n);
}
if (*prin > 3)
{
maprnt_(&c__2, q_1.v, &idim, n);
}
/* .....THE MAIN LOOP ENDS HERE..... */
/* added manually by Pedro Mendes 29/1/1998 */
/* if(callback != 0) callback(global_1.fx);*/
// We need a stopping condition for 1 dimensional problems.
// We compare the values between the last and current steps.
if (*n > 1 ||
global_1.fx < lastValue)
{
lastValue = global_1.fx;
goto L40;
}
/* .....RETURN..... */
L400:
if (*prin > 0)
{
vcprnt_(&c__4, &x[1], n);
}
ret_val = global_1.fx;
return ret_val;
} /* praxis_ */
/* Subroutine */ int CPraxis::quad_(C_INT *n, FPraxis *f, C_FLOAT64 *x, C_FLOAT64 *t,
C_FLOAT64 *machep, C_FLOAT64 *h__)
{
/* System generated locals */
C_INT i__1;
C_FLOAT64 d__1;
/* Local variables */
static C_INT i__;
static C_FLOAT64 l, s, value;
/* ...QUAD LOOKS FOR THE MINIMUM OF F ALONG A CURVE DEFINED BY Q0,Q1,X...
*/
/* Parameter adjustments */
--x;
/* Function Body */
s = global_1.fx;
global_1.fx = q_1.qf1;
q_1.qf1 = s;
q_1.qd1 = 0.;
i__1 = *n;
for (i__ = 1; i__ <= i__1; ++i__)
{
s = x[i__];
l = q_1.q1[i__ - 1];
x[i__] = l;
q_1.q1[i__ - 1] = s;
/* L1: */
/* Computing 2nd power */
d__1 = s - l;
q_1.qd1 += d__1 * d__1;
}
q_1.qd1 = sqrt(q_1.qd1);
l = q_1.qd1;
s = 0.;
if (q_1.qd0 <= 0. || q_1.qd1 <= 0. || global_1.nl < *n * 3 * *n)
{
goto L2;
}
value = q_1.qf1;
min_(n, &c__0, &c__2, &s, &l, &value, &c_true, f, &x[1], t, machep,
h__);
q_1.qa = l * (l - q_1.qd1) / (q_1.qd0 * (q_1.qd0 + q_1.qd1));
q_1.qb = (l + q_1.qd0) * (q_1.qd1 - l) / (q_1.qd0 * q_1.qd1);
q_1.qc = l * (l + q_1.qd0) / (q_1.qd1 * (q_1.qd0 + q_1.qd1));
goto L3;
L2:
global_1.fx = q_1.qf1;
q_1.qa = 0.;
q_1.qb = q_1.qa;
q_1.qc = 1.;
L3:
q_1.qd0 = q_1.qd1;
i__1 = *n;
for (i__ = 1; i__ <= i__1; ++i__)
{
s = q_1.q0[i__ - 1];
q_1.q0[i__ - 1] = x[i__];
/* L4: */
x[i__] = q_1.qa * s + q_1.qb * x[i__] + q_1.qc * q_1.q1[i__ - 1];
}
return 0;
} /* quad_ */
void WorldInternal::Culling(const Matrix44& cameraProjMat, bool isOpenGL)
{
objs.clear();
#ifdef _WIN32
if (_finite(cameraProjMat.Values[2][2]) != 0 &&
cameraProjMat.Values[0][0] != 0.0f &&
cameraProjMat.Values[1][1] != 0.0f)
{
#else
if (isfinite(cameraProjMat.Values[2][2]) != 0 &&
cameraProjMat.Values[0][0] != 0.0f &&
cameraProjMat.Values[1][1] != 0.0f)
{
#endif
Matrix44 cameraProjMatInv = cameraProjMat;
cameraProjMatInv.SetInverted();
float maxx = 1.0f;
float minx = -1.0f;
float maxy = 1.0f;
float miny = -1.0f;
float maxz = 1.0f;
float minz = 0.0f;
if (isOpenGL) minz = -1.0f;
Vector3DF eyebox[8];
eyebox[0 + 0] = Vector3DF(minx, miny, maxz);
eyebox[1 + 0] = Vector3DF(maxx, miny, maxz);
eyebox[2 + 0] = Vector3DF(minx, maxy, maxz);
eyebox[3 + 0] = Vector3DF(maxx, maxy, maxz);
eyebox[0 + 4] = Vector3DF(minx, miny, minz);
eyebox[1 + 4] = Vector3DF(maxx, miny, minz);
eyebox[2 + 4] = Vector3DF(minx, maxy, minz);
eyebox[3 + 4] = Vector3DF(maxx, maxy, minz);
for (int32_t i = 0; i < 8; i++)
{
eyebox[i] = cameraProjMatInv.Transform3D(eyebox[i]);
}
// 0-right 1-left 2-top 3-bottom 4-front 5-back
Vector3DF facePositions[6];
facePositions[0] = eyebox[5];
facePositions[1] = eyebox[4];
facePositions[2] = eyebox[6];
facePositions[3] = eyebox[4];
facePositions[4] = eyebox[4];
facePositions[5] = eyebox[0];
Vector3DF faceDir[6];
faceDir[0] = Vector3DF::Cross(eyebox[1] - eyebox[5], eyebox[7] - eyebox[5]);
faceDir[1] = Vector3DF::Cross(eyebox[6] - eyebox[4], eyebox[0] - eyebox[4]);
faceDir[2] = Vector3DF::Cross(eyebox[7] - eyebox[6], eyebox[2] - eyebox[6]);
faceDir[3] = Vector3DF::Cross(eyebox[0] - eyebox[4], eyebox[5] - eyebox[4]);
faceDir[4] = Vector3DF::Cross(eyebox[5] - eyebox[4], eyebox[6] - eyebox[4]);
faceDir[5] = Vector3DF::Cross(eyebox[2] - eyebox[0], eyebox[1] - eyebox[5]);
for (int32_t i = 0; i < 6; i++)
{
faceDir[i].Normalize();
}
for (int32_t z = 0; z < viewCullingZDiv; z++)
{
for (int32_t y = 0; y < viewCullingYDiv; y++)
{
for (int32_t x = 0; x < viewCullingXDiv; x++)
{
Vector3DF eyebox_[8];
float xsize = 1.0f / (float) viewCullingXDiv;
float ysize = 1.0f / (float) viewCullingYDiv;
float zsize = 1.0f / (float) viewCullingZDiv;
for (int32_t e = 0; e < 8; e++)
{
float x_, y_, z_;
if (e == 0){ x_ = xsize * x; y_ = ysize * y; z_ = zsize * z; }
if (e == 1){ x_ = xsize * (x + 1); y_ = ysize * y; z_ = zsize * z; }
if (e == 2){ x_ = xsize * x; y_ = ysize * (y + 1); z_ = zsize * z; }
if (e == 3){ x_ = xsize * (x + 1); y_ = ysize * (y + 1); z_ = zsize * z; }
if (e == 4){ x_ = xsize * x; y_ = ysize * y; z_ = zsize * (z + 1); }
if (e == 5){ x_ = xsize * (x + 1); y_ = ysize * y; z_ = zsize * (z + 1); }
if (e == 6){ x_ = xsize * x; y_ = ysize * (y + 1); z_ = zsize * (z + 1); }
if (e == 7){ x_ = xsize * (x + 1); y_ = ysize * (y + 1); z_ = zsize * (z + 1); }
Vector3DF yzMid[4];
yzMid[0] = eyebox[0] * x_ + eyebox[1] * (1.0f - x_);
yzMid[1] = eyebox[2] * x_ + eyebox[3] * (1.0f - x_);
yzMid[2] = eyebox[4] * x_ + eyebox[5] * (1.0f - x_);
yzMid[3] = eyebox[6] * x_ + eyebox[7] * (1.0f - x_);
Vector3DF zMid[2];
zMid[0] = yzMid[0] * y_ + yzMid[1] * (1.0f - y_);
zMid[1] = yzMid[2] * y_ + yzMid[3] * (1.0f - y_);
eyebox_[e] = zMid[0] * z_ + zMid[1] * (1.0f - z_);
}
Vector3DF max_(-FLT_MAX, -FLT_MAX, -FLT_MAX);
Vector3DF min_(FLT_MAX, FLT_MAX, FLT_MAX);
for (int32_t i = 0; i < 8; i++)
{
if (eyebox_[i].X > max_.X) max_.X = eyebox_[i].X;
if (eyebox_[i].Y > max_.Y) max_.Y = eyebox_[i].Y;
if (eyebox_[i].Z > max_.Z) max_.Z = eyebox_[i].Z;
if (eyebox_[i].X < min_.X) min_.X = eyebox_[i].X;
if (eyebox_[i].Y < min_.Y) min_.Y = eyebox_[i].Y;
if (eyebox_[i].Z < min_.Z) min_.Z = eyebox_[i].Z;
}
/* 範囲内に含まれるグリッドを取得 */
for (size_t i = 0; i < layers.size(); i++)
{
layers[i]->AddGrids(max_, min_, grids);
}
}
}
}
/* 外領域追加 */
grids.push_back(&outofLayers);
grids.push_back(&allLayers);
/* グリッドからオブジェクト取得 */
for (size_t i = 0; i < grids.size(); i++)
{
for (size_t j = 0; j < grids[i]->GetObjects().size(); j++)
{
Object* o = grids[i]->GetObjects()[j];
ObjectInternal* o_ = (ObjectInternal*) o;
if (
o_->GetNextStatus().Type == OBJECT_SHAPE_TYPE_ALL ||
IsInView(o_->GetPosition(), o_->GetNextStatus().CalcRadius(), facePositions, faceDir))
{
objs.push_back(o);
}
}
}
/* 取得したグリッドを破棄 */
for (size_t i = 0; i < grids.size(); i++)
{
grids[i]->IsScanned = false;
}
grids.clear();
}
else
{
grids.push_back(&allLayers);
/* グリッドからオブジェクト取得 */
for (size_t i = 0; i < grids.size(); i++)
{
for (size_t j = 0; j < grids[i]->GetObjects().size(); j++)
{
Object* o = grids[i]->GetObjects()[j];
ObjectInternal* o_ = (ObjectInternal*) o;
if (o_->GetNextStatus().Type == OBJECT_SHAPE_TYPE_ALL)
{
objs.push_back(o);
}
}
}
/* 取得したグリッドを破棄 */
for (size_t i = 0; i < grids.size(); i++)
{
grids[i]->IsScanned = false;
}
grids.clear();
}
}
bool WorldInternal::Reassign()
{
/* 数が少ない */
if (outofLayers.GetObjects().size() < 10) return false;
objs.clear();
for (size_t i = 0; i < layers.size(); i++)
{
delete layers[i];
}
layers.clear();
outofLayers.GetObjects().clear();
allLayers.GetObjects().clear();
outofLayers.IsScanned = false;
allLayers.IsScanned = false;
for (auto& it : containedObjects)
{
auto o = (ObjectInternal*) (it);
o->ObjectIndex = -1;
}
float xmin = FLT_MAX;
float xmax = -FLT_MAX;
float ymin = FLT_MAX;
float ymax = -FLT_MAX;
float zmin = FLT_MAX;
float zmax = -FLT_MAX;
for (auto& it : containedObjects)
{
ObjectInternal* o_ = (ObjectInternal*) it;
if (o_->GetNextStatus().Type == OBJECT_SHAPE_TYPE_ALL) continue;
if (xmin > o_->GetNextStatus().Position.X) xmin = o_->GetNextStatus().Position.X;
if (xmax < o_->GetNextStatus().Position.X) xmax = o_->GetNextStatus().Position.X;
if (ymin > o_->GetNextStatus().Position.Y) ymin = o_->GetNextStatus().Position.Y;
if (ymax < o_->GetNextStatus().Position.Y) ymax = o_->GetNextStatus().Position.Y;
if (zmin > o_->GetNextStatus().Position.Z) zmin = o_->GetNextStatus().Position.Z;
if (zmax < o_->GetNextStatus().Position.Z) zmax = o_->GetNextStatus().Position.Z;
}
auto xlen = Max(abs(xmax), abs(xmin)) * 2.0f;
auto ylen = Max(abs(ymax), abs(ymin)) * 2.0f;
auto zlen = Max(abs(zmax), abs(zmin)) * 2.0f;
WorldInternal(xlen, ylen, zlen, this->layerCount);
for (auto& it: containedObjects)
{
ObjectInternal* o_ = (ObjectInternal*) (it);
AddObjectInternal(o_);
}
return true;
}
void WorldInternal::Dump(const char* path, const Matrix44& cameraProjMat, bool isOpenGL)
{
std::ofstream ofs(path);
/* カメラ情報出力 */
Matrix44 cameraProjMatInv = cameraProjMat;
cameraProjMatInv.SetInverted();
float maxx = 1.0f;
float minx = -1.0f;
float maxy = 1.0f;
float miny = -1.0f;
float maxz = 1.0f;
float minz = 0.0f;
if (isOpenGL) minz = -1.0f;
Vector3DF eyebox[8];
eyebox[0 + 0] = Vector3DF(minx, miny, maxz);
eyebox[1 + 0] = Vector3DF(maxx, miny, maxz);
eyebox[2 + 0] = Vector3DF(minx, maxy, maxz);
eyebox[3 + 0] = Vector3DF(maxx, maxy, maxz);
eyebox[0 + 4] = Vector3DF(minx, miny, minz);
eyebox[1 + 4] = Vector3DF(maxx, miny, minz);
eyebox[2 + 4] = Vector3DF(minx, maxy, minz);
eyebox[3 + 4] = Vector3DF(maxx, maxy, minz);
for (int32_t i = 0; i < 8; i++)
{
eyebox[i] = cameraProjMatInv.Transform3D(eyebox[i]);
}
ofs << viewCullingXDiv << "," << viewCullingYDiv << "," << viewCullingZDiv << std::endl;
for (int32_t i = 0; i < 8; i++)
{
ofs << eyebox[i].X << "," << eyebox[i].Y << "," << eyebox[i].Z << std::endl;
}
ofs << std::endl;
for (int32_t z = 0; z < viewCullingZDiv; z++)
{
for (int32_t y = 0; y < viewCullingYDiv; y++)
{
for (int32_t x = 0; x < viewCullingXDiv; x++)
{
Vector3DF eyebox_[8];
float xsize = 1.0f / (float) viewCullingXDiv;
float ysize = 1.0f / (float) viewCullingYDiv;
float zsize = 1.0f / (float) viewCullingZDiv;
for (int32_t e = 0; e < 8; e++)
{
float x_, y_, z_;
if (e == 0){ x_ = xsize * x; y_ = ysize * y; z_ = zsize * z; }
if (e == 1){ x_ = xsize * (x + 1); y_ = ysize * y; z_ = zsize * z; }
if (e == 2){ x_ = xsize * x; y_ = ysize * (y + 1); z_ = zsize * z; }
if (e == 3){ x_ = xsize * (x + 1); y_ = ysize * (y + 1); z_ = zsize * z; }
if (e == 4){ x_ = xsize * x; y_ = ysize * y; z_ = zsize * (z + 1); }
if (e == 5){ x_ = xsize * (x + 1); y_ = ysize * y; z_ = zsize * (z + 1); }
if (e == 6){ x_ = xsize * x; y_ = ysize * (y + 1); z_ = zsize * (z + 1); }
if (e == 7){ x_ = xsize * (x + 1); y_ = ysize * (y + 1); z_ = zsize * (z + 1); }
Vector3DF yzMid[4];
yzMid[0] = eyebox[0] * x_ + eyebox[1] * (1.0f - x_);
yzMid[1] = eyebox[2] * x_ + eyebox[3] * (1.0f - x_);
yzMid[2] = eyebox[4] * x_ + eyebox[5] * (1.0f - x_);
yzMid[3] = eyebox[6] * x_ + eyebox[7] * (1.0f - x_);
Vector3DF zMid[2];
zMid[0] = yzMid[0] * y_ + yzMid[1] * (1.0f - y_);
zMid[1] = yzMid[2] * y_ + yzMid[3] * (1.0f - y_);
eyebox_[e] = zMid[0] * z_ + zMid[1] * (1.0f - z_);
}
Vector3DF max_(-FLT_MAX, -FLT_MAX, -FLT_MAX);
Vector3DF min_(FLT_MAX, FLT_MAX, FLT_MAX);
for (int32_t i = 0; i < 8; i++)
{
if (eyebox_[i].X > max_.X) max_.X = eyebox_[i].X;
if (eyebox_[i].Y > max_.Y) max_.Y = eyebox_[i].Y;
if (eyebox_[i].Z > max_.Z) max_.Z = eyebox_[i].Z;
if (eyebox_[i].X < min_.X) min_.X = eyebox_[i].X;
if (eyebox_[i].Y < min_.Y) min_.Y = eyebox_[i].Y;
if (eyebox_[i].Z < min_.Z) min_.Z = eyebox_[i].Z;
}
ofs << x << "," << y << "," << z << std::endl;
for (int32_t i = 0; i < 8; i++)
{
ofs << eyebox_[i].X << "," << eyebox_[i].Y << "," << eyebox_[i].Z << std::endl;
}
ofs << max_.X << "," << max_.Y << "," << max_.Z << std::endl;
ofs << min_.X << "," << min_.Y << "," << min_.Z << std::endl;
ofs << std::endl;
}
}
}
ofs << std::endl;
/* レイヤー情報 */
ofs << layers.size() << std::endl;
for (size_t i = 0; i < layers.size(); i++)
{
auto& layer = layers[i];
ofs << layer->GetGridXCount() << "," << layer->GetGridYCount() << "," << layer->GetGridZCount()
<< "," << layer->GetOffsetX() << "," << layer->GetOffsetY() << "," << layer->GetOffsetZ() << "," << layer->GetGridSize() << std::endl;
for (size_t j = 0; j < layer->GetGrids().size(); j++)
{
auto& grid = layer->GetGrids()[j];
if (grid.GetObjects().size() > 0)
{
ofs << j << "," << grid.GetObjects().size() << std::endl;
}
}
}
Culling(cameraProjMat, isOpenGL);
}
float_type operator()(float_type const & x, float_type const & y) const
{
return min_(x, y);
}
float_type operator()(float_type const & f, float_type const & limit) const
{
float_type zero(0);
float_type neg = zero - float_type(limit);
return max_(neg, min_(f, limit));
}
void visualizeOdometry_GT(node_map& nodes, FrameGenerator& frame_gen, bool estimate)
{
int size_px = 800;
float border_m = 0.25; // size of border around bounding box
cv::Mat odo_img(size_px, size_px,CV_32FC3);
odo_img.setTo(cv::Scalar::all(0));
// compute the track's bounding box
cv::Point2f min_(1e5,1e5);
cv::Point2f max_(-1e5,-1e5);
for (node_it it = nodes.begin(); it != nodes.end(); ++it)
{
cv::Point2f p;
if (estimate){
p.x = (*it).second.opt_pose(0,3);
p.y = (*it).second.opt_pose(1,3);
}
else
p = (*it).second.gt_pos_xy;
min_.x = min(min_.x,p.x); max_.x = max(max_.x,p.x);
min_.y = min(min_.y,p.y); max_.y = max(max_.y,p.y);
}
// 0.5 Abstand um die Bounding Box
double m2px_x = (2*border_m+ (max_.x-min_.x))/size_px;
double m2px_y = (2*border_m+ (max_.y-min_.y))/size_px;
double m2px = max(m2px_x,m2px_y);
// ROS_INFO("m2px: %f", m2px);
// ROS_INFO("min: %f %f", min_.x, min_.y);
map<uint, cv::Point> px_pos;
for (node_it it = nodes.begin(); it != nodes.end(); ++it)
{
cv::Point2f p;
if (estimate){
p.x = (*it).second.opt_pose(0,3);
p.y = (*it).second.opt_pose(1,3);
}
else
p = (*it).second.gt_pos_xy;
// printf("gt_pos: %f %f \n", p.x,p.y);
px_pos[it->first] = cv::Point2i( (p.x-(min_.x-border_m))/m2px , (p.y-(min_.y-border_m))/m2px ) ;
// ROS_INFO("px: %i %i",px_pos[it->first].x, px_pos[it->first].y );
// px_pos[it->first] = cv::Point2i( (border_m+(p.x-min_.x))/m2px, (border_m+(p.y-min_.y))/m2px );
}
for (float x = floor(min_.x-border_m); x <= ceil(max_.x+border_m); x+=0.25) {
int px_x = (x-(min_.x-border_m))/m2px;
cv::line(odo_img,cv::Point2i(px_x,0),cv::Point2i(px_x,size_px), cv::Scalar(255,255,255),1);
}
for (float y = floor(min_.y-border_m); y <= ceil(max_.y+border_m); y+=0.25) {
int px_y = (y-(min_.y-border_m))/m2px;
cv::line(odo_img,cv::Point2i(0,px_y),cv::Point2i(size_px,px_y), cv::Scalar(255,255,255),1);
}
// show node positions
for (node_it it = nodes.begin(); it != nodes.end(); ++it)
{
Node* current = &(*it).second;
if (current->is_keyframe)
cv::circle(odo_img,px_pos[current->node_id],3,cv::Scalar(255,0,0),1 );
else
cv::circle(odo_img,px_pos[current->node_id],1,cv::Scalar(255,255,255),1 );
}
// if (nodes.size() > 0){
// Node* last = &nodes.rbegin()->second;
// for (uint i=0; i<last->tree_proposals.size(); ++i)
// cv::line(odo_img,px_pos[last->node_id],px_pos[nodes[last->tree_proposals[i]].node_id],cv::Scalar::all(255),1);
// }
// show edges:
for (node_it it = nodes.begin(); it != nodes.end(); ++it)
{
Node* current = &(*it).second;
for (uint j=0; j<current->matches.size(); j++)
{
Node_match* nm = ¤t->matches.at(j);
if (current->node_id < nm->other_id)
continue;
// ROS_INFO("edges: %i %i", current->node_id, nm->other_id);
Node* neighbour = &nodes[nm->other_id];
int r,g,b;
r=g=b=0;
// ROS_INFO("type: %i", nm->type);
switch (nm->type) {
case ET_Direct_neighbour:
r = b = 255; break; // yellow
case ET_Tree_Proposal:
r = 255; break; // red
case ET_Ring:
g = 255; break;
case ET_Last_match:
g = r = 255; break;
default:
r=g=b=255; break;
}
cv::Scalar col = cv::Scalar(b,g,r);
cv::line(odo_img,px_pos[current->node_id],px_pos[neighbour->node_id],col,1);
}
}
// if (frame_gen.node_id_running % 10 == 0){
// char buffer[300];
// sprintf(buffer, "/home/engelhar/Desktop/img/%i.png", (int)frame_gen.node_id_running);;
// cv::imwrite(buffer, odo_img);
// }
if (estimate){
cv::namedWindow("pos_Est",1);
cv::imshow("pos_Est",odo_img);
}
else
{
cv::namedWindow("odometry_GT",1);
cv::imshow("odometry_GT",odo_img);
}
cv::waitKey(10);
}
