CV_IMPL void
cvSVBkSb( const CvArr* warr, const CvArr* uarr,
const CvArr* varr, const CvArr* barr,
CvArr* xarr, int flags )
{
uchar* buffer = 0;
int local_alloc = 0;
CV_FUNCNAME( "cvSVBkSb" );
__BEGIN__;
CvMat wstub, *w = (CvMat*)warr;
CvMat bstub, *b = (CvMat*)barr;
CvMat xstub, *x = (CvMat*)xarr;
CvMat ustub, ustub2, *u = (CvMat*)uarr;
CvMat vstub, vstub2, *v = (CvMat*)varr;
uchar* tw = 0;
int type;
int temp_u = 0, temp_v = 0;
int u_buf_offset = 0, v_buf_offset = 0, w_buf_offset = 0, t_buf_offset = 0;
int buf_size = 0, pix_size;
int m, n, nm;
int u_rows, u_cols;
int v_rows, v_cols;
if( !CV_IS_MAT( w ))
CV_CALL( w = cvGetMat( w, &wstub ));
if( !CV_IS_MAT( u ))
CV_CALL( u = cvGetMat( u, &ustub ));
if( !CV_IS_MAT( v ))
CV_CALL( v = cvGetMat( v, &vstub ));
if( !CV_IS_MAT( x ))
CV_CALL( x = cvGetMat( x, &xstub ));
if( !CV_ARE_TYPES_EQ( w, u ) || !CV_ARE_TYPES_EQ( w, v ) || !CV_ARE_TYPES_EQ( w, x ))
CV_ERROR( CV_StsUnmatchedFormats, "All matrices must have the same type" );
type = CV_MAT_TYPE( w->type );
pix_size = CV_ELEM_SIZE(type);
if( !(flags & CV_SVD_U_T) )
{
temp_u = 1;
u_buf_offset = buf_size;
buf_size += u->cols*u->rows*pix_size;
u_rows = u->rows;
u_cols = u->cols;
}
else
{
u_rows = u->cols;
u_cols = u->rows;
}
if( !(flags & CV_SVD_V_T) )
{
temp_v = 1;
v_buf_offset = buf_size;
buf_size += v->cols*v->rows*pix_size;
v_rows = v->rows;
v_cols = v->cols;
}
else
{
v_rows = v->cols;
v_cols = v->rows;
}
m = u_rows;
n = v_rows;
nm = MIN(n,m);
if( (u_rows != u_cols && v_rows != v_cols) || x->rows != v_rows )
CV_ERROR( CV_StsBadSize, "V or U matrix must be square" );
if( (w->rows == 1 || w->cols == 1) && w->rows + w->cols - 1 == nm )
{
if( CV_IS_MAT_CONT(w->type) )
tw = w->data.ptr;
else
{
w_buf_offset = buf_size;
buf_size += nm*pix_size;
}
}
else
{
if( w->cols != v_cols || w->rows != u_cols )
CV_ERROR( CV_StsBadSize, "W must be 1d array of MIN(m,n) elements or "
"matrix which size matches to U and V" );
w_buf_offset = buf_size;
buf_size += nm*pix_size;
}
if( b )
{
if( !CV_IS_MAT( b ))
CV_CALL( b = cvGetMat( b, &bstub ));
if( !CV_ARE_TYPES_EQ( w, b ))
CV_ERROR( CV_StsUnmatchedFormats, "All matrices must have the same type" );
if( b->cols != x->cols || b->rows != m )
CV_ERROR( CV_StsUnmatchedSizes, "b matrix must have (m x x->cols) size" );
}
else
{
b = &bstub;
memset( b, 0, sizeof(*b));
}
t_buf_offset = buf_size;
buf_size += (MAX(m,n) + b->cols)*pix_size;
if( buf_size <= CV_MAX_LOCAL_SIZE )
{
buffer = (uchar*)cvStackAlloc( buf_size );
local_alloc = 1;
}
else
CV_CALL( buffer = (uchar*)cvAlloc( buf_size ));
if( temp_u )
{
cvInitMatHeader( &ustub2, u_cols, u_rows, type, buffer + u_buf_offset );
cvT( u, &ustub2 );
u = &ustub2;
}
if( temp_v )
{
cvInitMatHeader( &vstub2, v_cols, v_rows, type, buffer + v_buf_offset );
cvT( v, &vstub2 );
v = &vstub2;
}
if( !tw )
{
int i, shift = w->cols > 1 ? pix_size : 0;
tw = buffer + w_buf_offset;
for( i = 0; i < nm; i++ )
memcpy( tw + i*pix_size, w->data.ptr + i*(w->step + shift), pix_size );
}
if( type == CV_32FC1 )
{
icvSVBkSb_32f( m, n, (float*)tw, u->data.fl, u->step/sizeof(float),
v->data.fl, v->step/sizeof(float),
b->data.fl, b->step/sizeof(float), b->cols,
x->data.fl, x->step/sizeof(float),
(float*)(buffer + t_buf_offset) );
}
else if( type == CV_64FC1 )
{
icvSVBkSb_64f( m, n, (double*)tw, u->data.db, u->step/sizeof(double),
v->data.db, v->step/sizeof(double),
b->data.db, b->step/sizeof(double), b->cols,
x->data.db, x->step/sizeof(double),
(double*)(buffer + t_buf_offset) );
}
else
{
CV_ERROR( CV_StsUnsupportedFormat, "" );
}
__END__;
if( buffer && !local_alloc )
cvFree( &buffer );
}
void mexFunction(
int nargout,
mxArray *out[],
int nargin,
const mxArray *in[]
)
{
/* declare variables */
double *mx_r_in;
double *mx_r_out;
int output_dim = 3;
/* check arguments */
if (nargin != 1 || nargout > 1){
mexErrMsgTxt("Wrong Number of arguments.");
exit(1);
}
// Link input vars to pointers in C
mx_r_in = mxGetPr(in[0]);
int m = mxGetM(in[0]);
int n = mxGetN(in[0]);
// Input is a rotation matrix
if (m == 3 && n == 3){
output_dim = 1;
}
// Check input argument: avoid errors
if (!((m == 3 && n == 3) || (m == 1 && n == 3) || (m == 3 && n == 1))){
mexPrintf("HELP! ERROR! %d %d\n", m, n);
exit(1);
}
// Create OpenCV array for input variable
// If we want to use cvSetData, our matrices are actually the transposed
// versions of those that come from Matlab.
CvMat *r_in_T = cvCreateMatHeader(m, n, CV_64F);
cvSetData (r_in_T, mx_r_in, sizeof(double)*n);
// Transpose the matrix
CvMat *r_in = cvCreateMat(n, m, CV_64F);
cvT(r_in_T, r_in);
// Result
CvMat *r_out_T = cvCreateMat(output_dim, 3, CV_64F);
// Call cvRodrigues
cvRodrigues2(r_in, r_out_T);
// Allocate memory for the output var
out[0] = mxCreateNumericMatrix(3, output_dim, mxDOUBLE_CLASS, mxREAL);
mx_r_out = mxGetPr(out[0]);
CvMat* r_out = cvCreateMatHeader(3, output_dim, CV_64F);
cvSetData (r_out, mx_r_out, sizeof(double)*output_dim);
cvT(r_out_T, r_out);
// Free all array headers and return
cvReleaseMat(&r_in);
cvReleaseMatHeader(&r_in_T);
cvReleaseMatHeader(&r_out);
}
CV_IMPL void
cvSVD( CvArr* aarr, CvArr* warr, CvArr* uarr, CvArr* varr, int flags )
{
uchar* buffer = 0;
int local_alloc = 0;
CV_FUNCNAME( "cvSVD" );
__BEGIN__;
CvMat astub, *a = (CvMat*)aarr;
CvMat wstub, *w = (CvMat*)warr;
CvMat ustub, *u;
CvMat vstub, *v;
CvMat tmat;
uchar* tw = 0;
int type;
int a_buf_offset = 0, u_buf_offset = 0, buf_size, pix_size;
int temp_u = 0, /* temporary storage for U is needed */
t_svd; /* special case: a->rows < a->cols */
int m, n;
int w_rows, w_cols;
int u_rows = 0, u_cols = 0;
int w_is_mat = 0;
if( !CV_IS_MAT( a ))
CV_CALL( a = cvGetMat( a, &astub ));
if( !CV_IS_MAT( w ))
CV_CALL( w = cvGetMat( w, &wstub ));
if( !CV_ARE_TYPES_EQ( a, w ))
CV_ERROR( CV_StsUnmatchedFormats, "" );
if( a->rows >= a->cols )
{
m = a->rows;
n = a->cols;
w_rows = w->rows;
w_cols = w->cols;
t_svd = 0;
}
else
{
CvArr* t;
CV_SWAP( uarr, varr, t );
flags = (flags & CV_SVD_U_T ? CV_SVD_V_T : 0)|
(flags & CV_SVD_V_T ? CV_SVD_U_T : 0);
m = a->cols;
n = a->rows;
w_rows = w->cols;
w_cols = w->rows;
t_svd = 1;
}
u = (CvMat*)uarr;
v = (CvMat*)varr;
w_is_mat = w_cols > 1 && w_rows > 1;
if( !w_is_mat && CV_IS_MAT_CONT(w->type) && w_cols + w_rows - 1 == n )
tw = w->data.ptr;
if( u )
{
if( !CV_IS_MAT( u ))
CV_CALL( u = cvGetMat( u, &ustub ));
if( !(flags & CV_SVD_U_T) )
{
u_rows = u->rows;
u_cols = u->cols;
}
else
{
u_rows = u->cols;
u_cols = u->rows;
}
if( !CV_ARE_TYPES_EQ( a, u ))
CV_ERROR( CV_StsUnmatchedFormats, "" );
if( u_rows != m || (u_cols != m && u_cols != n))
CV_ERROR( CV_StsUnmatchedSizes, !t_svd ? "U matrix has unappropriate size" :
"V matrix has unappropriate size" );
temp_u = (u_rows != u_cols && !(flags & CV_SVD_U_T)) || u->data.ptr==a->data.ptr;
if( w_is_mat && u_cols != w_rows )
CV_ERROR( CV_StsUnmatchedSizes, !t_svd ? "U and W have incompatible sizes" :
"V and W have incompatible sizes" );
}
else
{
u = &ustub;
u->data.ptr = 0;
u->step = 0;
}
if( v )
{
int v_rows, v_cols;
if( !CV_IS_MAT( v ))
CV_CALL( v = cvGetMat( v, &vstub ));
if( !(flags & CV_SVD_V_T) )
{
v_rows = v->rows;
v_cols = v->cols;
}
else
{
v_rows = v->cols;
v_cols = v->rows;
}
if( !CV_ARE_TYPES_EQ( a, v ))
CV_ERROR( CV_StsUnmatchedFormats, "" );
if( v_rows != n || v_cols != n )
CV_ERROR( CV_StsUnmatchedSizes, t_svd ? "U matrix has unappropriate size" :
"V matrix has unappropriate size" );
if( w_is_mat && w_cols != v_cols )
CV_ERROR( CV_StsUnmatchedSizes, t_svd ? "U and W have incompatible sizes" :
"V and W have incompatible sizes" );
}
else
{
v = &vstub;
v->data.ptr = 0;
v->step = 0;
}
type = CV_MAT_TYPE( a->type );
pix_size = CV_ELEM_SIZE(type);
buf_size = n*2 + m;
if( !(flags & CV_SVD_MODIFY_A) )
{
a_buf_offset = buf_size;
buf_size += a->rows*a->cols;
}
if( temp_u )
{
u_buf_offset = buf_size;
buf_size += u->rows*u->cols;
}
buf_size *= pix_size;
if( buf_size <= CV_MAX_LOCAL_SIZE )
{
buffer = (uchar*)cvStackAlloc( buf_size );
local_alloc = 1;
}
else
{
CV_CALL( buffer = (uchar*)cvAlloc( buf_size ));
}
if( !(flags & CV_SVD_MODIFY_A) )
{
cvInitMatHeader( &tmat, m, n, type,
buffer + a_buf_offset*pix_size );
if( !t_svd )
cvCopy( a, &tmat );
else
cvT( a, &tmat );
a = &tmat;
}
if( temp_u )
{
cvInitMatHeader( &ustub, u_cols, u_rows, type, buffer + u_buf_offset*pix_size );
u = &ustub;
}
if( !tw )
tw = buffer + (n + m)*pix_size;
if( type == CV_32FC1 )
{
icvSVD_32f( a->data.fl, a->step/sizeof(float), a->rows, a->cols,
(float*)tw, u->data.fl, u->step/sizeof(float), u_cols,
v->data.fl, v->step/sizeof(float), (float*)buffer );
}
else if( type == CV_64FC1 )
{
icvSVD_64f( a->data.db, a->step/sizeof(double), a->rows, a->cols,
(double*)tw, u->data.db, u->step/sizeof(double), u_cols,
v->data.db, v->step/sizeof(double), (double*)buffer );
}
else
{
CV_ERROR( CV_StsUnsupportedFormat, "" );
}
if( tw != w->data.ptr )
{
int shift = w->cols != 1;
cvSetZero( w );
if( type == CV_32FC1 )
for( int i = 0; i < n; i++ )
((float*)(w->data.ptr + i*w->step))[i*shift] = ((float*)tw)[i];
else
for( int i = 0; i < n; i++ )
((double*)(w->data.ptr + i*w->step))[i*shift] = ((double*)tw)[i];
}
if( uarr )
{
if( !(flags & CV_SVD_U_T))
cvT( u, uarr );
else if( temp_u )
cvCopy( u, uarr );
/*CV_CHECK_NANS( uarr );*/
}
if( varr )
{
if( !(flags & CV_SVD_V_T))
cvT( v, varr );
/*CV_CHECK_NANS( varr );*/
}
CV_CHECK_NANS( w );
__END__;
if( buffer && !local_alloc )
cvFree( &buffer );
}
CV_IMPL double
cvPseudoInv( CvArr* srcarr, CvArr* dstarr, int flags )
{
uchar* buffer = 0;
int local_alloc = 0;
double condition_number = 0;
CV_FUNCNAME( "cvPseudoInv" );
__BEGIN__;
CvMat astub, *a = (CvMat*)srcarr;
CvMat bstub, *b = (CvMat*)dstarr;
CvMat ustub, *u = &ustub;
CvMat vstub, *v = &vstub;
CvMat tmat;
uchar* tw = 0;
int type, n, m, nm, mn;
int buf_size, pix_size;
if( !CV_IS_ARR( a ))
CV_CALL( a = cvGetMat( a, &astub ));
if( !CV_IS_ARR( b ))
CV_CALL( b = cvGetMat( b, &bstub ));
if( !CV_ARE_TYPES_EQ( a, b ))
CV_ERROR( CV_StsUnmatchedSizes, "" );
n = a->width;
m = a->height;
nm = MIN( n, m );
mn = MAX( n, m );
if( n != b->height || m != b->width )
CV_ERROR( CV_StsUnmatchedSizes, "" );
type = CV_ARR_TYPE( a->type );
pix_size = icvPixSize[type];
buf_size = nm*2 + mn + m*mn + n*n;
if( !(flags & CV_SVD_MODIFY_A) )
buf_size += m*n;
buf_size *= pix_size;
if( buf_size <= CV_MAX_LOCAL_SIZE )
{
buffer = (uchar*)alloca( buf_size );
local_alloc = 1;
}
else
{
CV_CALL( buffer = (uchar*)cvAlloc( buf_size ));
}
if( !(flags & CV_SVD_MODIFY_A) )
{
cvInitMatHeader( &tmat, a->height, a->width, type,
buffer + buf_size - n*m*pix_size );
cvCopy( a, &tmat );
a = &tmat;
}
tw = buffer + (nm + mn)*pix_size;
cvInitMatHeader( u, m, m, type, tw + nm*pix_size );
cvInitMatHeader( v, n, n, type, u->data.ptr + m*mn*pix_size );
if( type == CV_32FC1 )
{
IPPI_CALL( icvSVD_32f( a->data.fl, a->step/sizeof(float), (float*)tw,
u->data.fl, u->step/sizeof(float),
v->data.fl, v->step/sizeof(float),
icvGetMatSize(a), (float*)buffer ));
}
else if( type == CV_64FC1 )
{
IPPI_CALL( icvSVD_64f( a->data.db, a->step/sizeof(double), (double*)tw,
u->data.db, u->step/sizeof(double),
v->data.db, v->step/sizeof(double),
icvGetMatSize(a), (double*)buffer ));
}
else
{
CV_ERROR( CV_StsUnsupportedFormat, "" );
}
cvT( v, v );
cvGetRow( u, &tmat, 0 );
if( type == CV_32FC1 )
{
for( int i = 0; i < nm; i++ )
{
double t = ((float*)tw)[i];
tmat.data.ptr = u->data.ptr + i*u->step;
t = t > FLT_EPSILON ? 1./t : 0;
if( i == mn - 1 )
condition_number = t != 0 ? ((float*)tw)[0]*t : DBL_MAX;
cvScale( &tmat, &tmat, t );
}
}
else
{
for( int i = 0; i < nm; i++ )
{
double t = ((double*)tw)[i];
tmat.data.ptr = u->data.ptr + i*u->step;
t = t > DBL_EPSILON ? 1./t : 0;
if( i == mn - 1 )
condition_number = t != 0 ? ((double*)tw)[0]*t : DBL_MAX;
cvScale( &tmat, &tmat, t );
}
}
u->height = n;
if( n > m )
{
cvGetSubArr( u, &tmat, cvRect( 0, m, m, n - m ));
cvSetZero( &tmat );
}
cvMatMulAdd( v, u, 0, b );
CV_CHECK_NANS( b );
__END__;
if( buffer && !local_alloc )
cvFree( (void**)&buffer );
return condition_number;
}
CV_IMPL void
cvSVD( CvArr* aarr, CvArr* warr, CvArr* uarr, CvArr* varr, int flags )
{
uchar* buffer = 0;
int local_alloc = 0;
CV_FUNCNAME( "cvSVD" );
__BEGIN__;
CvMat astub, *a = (CvMat*)aarr;
CvMat wstub, *w = (CvMat*)warr;
CvMat ustub, *u = (CvMat*)uarr;
CvMat vstub, *v = (CvMat*)varr;
CvMat tmat;
uchar* tw = 0;
int type, nm, mn;
int buf_size, pix_size;
int t_svd = 0; // special case: a->rows < a->cols
if( !CV_IS_ARR( a ))
CV_CALL( a = cvGetMat( a, &astub ));
if( !CV_IS_ARR( w ))
CV_CALL( w = cvGetMat( w, &wstub ));
if( !CV_ARE_TYPES_EQ( a, w ))
CV_ERROR( CV_StsUnmatchedFormats, "" );
nm = MIN( a->width, a->height );
mn = MAX( a->width, a->height );
if( (w->width == 1 || w->height == 1) &&
CV_IS_ARR_CONT( w->type ) && w->width*w->height == nm )
{
tw = w->data.ptr;
}
else if( !CV_ARE_SIZES_EQ( w, a ))
{
CV_ERROR( CV_StsBadSize, "W must be either continuous vector of "
"size MIN(A->width,A->height) or matrix of "
"the same size as A" );
}
if( u )
{
if( !CV_IS_ARR( u ))
CV_CALL( u = cvGetMat( u, &ustub ));
if( !CV_ARE_TYPES_EQ( a, u ))
CV_ERROR( CV_StsUnmatchedFormats, "" );
if( u->width != u->height || u->height != a->height )
CV_ERROR( CV_StsUnmatchedSizes, "U matrix must be square and have the same "
"linear size as number of rows in A" );
if( u->data.ptr == a->data.ptr )
CV_ERROR( CV_StsBadArg, "U can not be equal A" );
}
else
{
u = &ustub;
u->data.ptr = 0;
u->step = 0;
}
if( v )
{
if( !CV_IS_ARR( v ))
CV_CALL( v = cvGetMat( v, &vstub ));
if( !CV_ARE_TYPES_EQ( a, v ))
CV_ERROR( CV_StsUnmatchedFormats, "" );
if( v->width != v->height || v->width != a->width )
CV_ERROR( CV_StsUnmatchedSizes, "V matrix must be square and have the same "
"linear size as number of columns in A" );
if( v->data.ptr == a->data.ptr || v->data.ptr == u->data.ptr )
CV_ERROR( CV_StsBadArg, "V can not be equal U or A" );
}
else
{
v = &vstub;
v->data.ptr = 0;
v->step = 0;
}
type = CV_ARR_TYPE( a->type );
pix_size = icvPixSize[type];
buf_size = nm*2 + mn;
if( a->rows < a->cols )
{
CvMat* t;
CV_SWAP( u, v, t );
flags = (flags & CV_SVD_U_T ? CV_SVD_V_T : 0)|
(flags & CV_SVD_V_T ? CV_SVD_U_T : 0);
t_svd = 1;
}
if( !(flags & CV_SVD_MODIFY_A) )
buf_size += a->width*a->height;
buf_size *= pix_size;
if( buf_size <= CV_MAX_LOCAL_SIZE )
{
buffer = (uchar*)alloca( buf_size );
local_alloc = 1;
}
else
{
CV_CALL( buffer = (uchar*)cvAlloc( buf_size ));
}
if( !(flags & CV_SVD_MODIFY_A) )
{
if( !t_svd )
{
cvInitMatHeader( &tmat, a->height, a->width, type,
buffer + (nm*2 + mn)*pix_size );
cvCopy( a, &tmat );
}
else
{
cvInitMatHeader( &tmat, a->width, a->height, type,
buffer + (nm*2 + mn)*pix_size );
cvT( a, &tmat );
}
a = &tmat;
}
if( !tw )
tw = buffer + (nm + mn)*pix_size;
if( type == CV_32FC1 )
{
IPPI_CALL( icvSVD_32f( a->data.fl, a->step/sizeof(float), (float*)tw,
u->data.fl, u->step/sizeof(float),
v->data.fl, v->step/sizeof(float),
icvGetMatSize(a), (float*)buffer ));
}
else if( type == CV_64FC1 )
{
IPPI_CALL( icvSVD_64f( a->data.db, a->step/sizeof(double), (double*)tw,
u->data.db, u->step/sizeof(double),
v->data.db, v->step/sizeof(double),
icvGetMatSize(a), (double*)buffer ));
}
else
{
CV_ERROR( CV_StsUnsupportedFormat, "" );
}
if( tw != w->data.ptr )
{
cvSetZero( w );
if( type == CV_32FC1 )
for( int i = 0; i < nm; i++ )
((float*)(w->data.ptr + i*w->step))[i] = ((float*)tw)[i];
else
for( int i = 0; i < nm; i++ )
((double*)(w->data.ptr + i*w->step))[i] = ((double*)tw)[i];
}
if( u->data.ptr )
{
if( !(flags & CV_SVD_U_T))
cvT( u, u );
CV_CHECK_NANS( u );
}
if( v->data.ptr)
{
if( !(flags & CV_SVD_V_T))
cvT( v, v );
CV_CHECK_NANS( v );
}
CV_CHECK_NANS( w );
__END__;
if( buffer && !local_alloc )
cvFree( (void**)&buffer );
}
int CUKF::UnscentedUpdate(int idf, int idx)
{
// Set up some variables
int dim = XX->rows;
int N = 2*dim+1;
double scale = 3;
int kappa = scale-dim;
// Create samples
// SVD
CvSize P = cvGetSize(PX);
CvMat *D = cvCreateMat(P.height, P.width, CV_64FC1);
CvMat *U = cvCreateMat(P.height, P.height, CV_64FC1);
CvMat *V = cvCreateMat(P.width, P.width, CV_64FC1);
CvMat *Ds = cvCreateMat(P.height, P.width, CV_64FC1);
CvMat *UD = cvCreateMat(P.height, P.width, CV_64FC1);
CvMat *Ps = cvCreateMat(P.height, P.width, CV_64FC1);
cvSVD(PX, D, U, V, CV_SVD_V_T);
MatSqrt(D, Ds);
cvMatMul(U, Ds, UD);
cvScale(UD, Ps, sqrt(scale), 0);
// CvMat *ss = cvCreateMat(dim, N, CV_64FC1);
// int i,j,jj;
// int row = dim;
// int col = N;
// cvRepeat(XX, ss);
// // for (i=0; i<row; i++) for (j=0; j<col; j++)
// // ss->data.db[i*col+j] = XXA->data.db[i];
//
// col -= dim;
// for(i=0; i<row; i++) for(j=1, jj=0; j<col+1; j++, jj++)
// ss->data.db[i*col+j] += Ps->data.db[i*col+jj];
// for(i=0; i<row; i++) for(j=1+dim, jj=0; j<col+1+dim; j++, jj++)
// ss->data.db[i*col+j] -= Ps->data.db[i*col+jj];
CvMat *ss = cvCreateMat(dim, N, CV_64FC1);
int i,j,jj;
int row = dim;
int col = N;
cvRepeat(XX, ss);
double *ssdb = ss->data.db;
double *Psdb = Ps->data.db;
for(i=0; i<row; i++)
{
for(j=1, jj=0; j<dim+1; j++, jj++)
{
ssdb[i*ss->cols+j] += Psdb[i*Ps->cols+jj];
}
}
for(i=0; i<row; i++)
{
for(j=1+dim, jj=0; j<ss->cols; j++, jj++)
{
ssdb[i*ss->cols+j] -= Psdb[i*Ps->cols+jj];
}
}
// Transform samples according to observation model to obtain the predicted observation samples
CvMat *zs = cvCreateMat(2, N, CV_64FC1);
// CvMat *base = cvCreateMat(3, EP, CV_64FC1);
int Nxv = 3;
int f = Nxv + idf*2 - 2;
// PrintCvMat(ss, "ss");
double *zsdb = zs->data.db;
for(j=0; j<N; j++)
{
double dx = ssdb[f*ss->cols+j] - ssdb[0*ss->cols+j];
double dy = ssdb[(f+1)*ss->cols+j] - ssdb[1*ss->cols+j];
double d2 = dx*dx + dy*dy;
double d = sqrt(d2);
zsdb[0*zs->cols+j] = d;
zsdb[1*zs->cols+j] = atan2(dy,dx) - ssdb[2*ss->cols+j];
}
// zz = repvec(z,N);
// dz = feval(dzfunc, zz, zs); % compute correct residual
// zs = zz - dz; % offset zs from z according to correct residual
// PrintCvMat(zs, "zs");
CvMat *zz = cvCreateMat(2, N, CV_64FC1);
CvMat *dz = cvCreateMat(2, N, CV_64FC1);
CvMat *zprev = cvCreateMat(2, 1, CV_64FC1);
zprev->data.db[0] = zf[idx].x;
zprev->data.db[1] = zf[idx].y;
cvRepeat(zprev, zz);
cvSub(zz, zs, dz);
double *dzdb = dz->data.db;
for(j=0; j<dz->cols; j++) dzdb[1*dz->cols+j] = PI2PI(dzdb[1*dz->cols+j]);
cvSub(zz, dz, zs);
// PrintCvMat(zs, "zs");
CvMat *zm = cvCreateMat(2, 1, CV_64FC1);
CvMat *dx = cvCreateMat(dim ,N, CV_64FC1);
// CvMat *dz = cvCreateMat(2, N, CV_64FC1);
CvMat *repx = cvCreateMat(dim ,N, CV_64FC1);
CvMat *repzm = cvCreateMat(2, N, CV_64FC1);
CvMat *Pxz = cvCreateMat(dim, 2, CV_64FC1);
CvMat *Pzz = cvCreateMat(2, 2, CV_64FC1);
CvMat *dxu = cvCreateMat(dim, 1, CV_64FC1);
CvMat *dxl = cvCreateMat(dim, N-1, CV_64FC1);
CvMat *dzu = cvCreateMat(2, 1, CV_64FC1);
CvMat *dzl = cvCreateMat(2, N-1, CV_64FC1);
CvMat *dztu = cvCreateMat(1, 2, CV_64FC1);
CvMat *dztl = cvCreateMat(N-1, 2, CV_64FC1);
CvMat *dxdzu = cvCreateMat(dim, 2, CV_64FC1);
CvMat *dxdzl = cvCreateMat(dim, 2, CV_64FC1);
CvMat *dzdzu = cvCreateMat(2, 2, CV_64FC1);
CvMat *dzdzl = cvCreateMat(2, 2, CV_64FC1);
double *zmdb = zm->data.db;
zmdb[0] = kappa*zs->data.db[0*zs->cols];
zmdb[1] = kappa*zs->data.db[1*zs->cols];
for(j=1; j<N; j++)
{
zm->data.db[0] += 0.5*zsdb[0*zs->cols+j];
zm->data.db[1] += 0.5*zsdb[1*zs->cols+j];
}
cvScale(zm, zm, 1/(double)(scale));
// Calculate predicted observation mean
cvRepeat(XX, repx);
cvRepeat(zm, repzm);
// PrintCvMat(ss, "ss");
// PrintCvMat(zs, "zs");
// PrintCvMat(repx, "repx");
// PrintCvMat(repzm, "repzm");
// Calculate observation covariance and the state-observation correlation matrix
cvSub(ss, repx, dx);
cvSub(zs, repzm, dz);
// PrintCvMat(dx, "dx");
// PrintCvMat(dz, "dz");
double *dxdb = dx->data.db;
double *dzudb = dzu->data.db;
double *dzldb = dzl->data.db;
double *dxudb = dxu->data.db;
double *dxldb = dxl->data.db;
// dx, dz를 1열과 나머지 열로 분할
for(i=0; i<2; i++) dzudb[i] = dzdb[i*dz->cols];
for(i=0; i<2; i++) for(j=1, jj=0; j<N; j++, jj++)
dzldb[i*dzl->cols+jj] = dzdb[i*dz->cols+j];
for(i=0; i<dim; i++) dxudb[i] = dxdb[i*dx->cols];
for(i=0; i<dim; i++) for(j=1, jj=0; j<N; j++, jj++)
dxldb[i*dxl->cols+jj] = dxdb[i*dx->cols+j];
cvT(dzu, dztu);
cvT(dzl, dztl);
// PrintCvMat(dxu, "dxu");
// PrintCvMat(dztu, "dztu");
// PrintCvMat(dxl, "dxl");
// PrintCvMat(dztl, "dztl");
cvMatMul(dxu, dztu, dxdzu);
cvScale(dxdzu, dxdzu, 2*kappa);
cvMatMul(dxl, dztl, dxdzl);
cvAdd(dxdzu, dxdzl, Pxz);
cvScale(Pxz, Pxz, 1/(double)(2*scale));
cvMatMul(dzu, dztu, dzdzu);
cvScale(dzdzu, dzdzu, 2*kappa);
cvMatMul(dzl, dztl, dzdzl);
cvAdd(dzdzu, dzdzl, Pzz);
cvScale(Pzz, Pzz, 1/(double)(2*scale));
// PrintCvMat(dx, "dx");
// PrintCvMat(dz, "dz");
// PrintCvMat(dzu, "dzu");
// PrintCvMat(dztu, "dztu");
// PrintCvMat(dzl, "dzl");
// PrintCvMat(dztl, "dztl");
// PrintCvMat(dxu, "dxu");
// PrintCvMat(dxl, "dxl");
// PrintCvMat(dxdzu, "dxdzu");
// PrintCvMat(dxdzl, "dxdzl");
// PrintCvMat(dzdzu, "dzdzu");
// PrintCvMat(dzdzl, "dzdzl");
// PrintCvMat(Pxz, "Pxz");
// PrintCvMat(Pzz, "Pzz");
// Compute Kalman gain
CvMat *S = cvCreateMat(2, 2, CV_64FC1);
CvMat *p = cvCreateMat(2, 2, CV_64FC1);
CvMat *Sct = cvCreateMat(2, 2, CV_64FC1);
CvMat *Sc = cvCreateMat(2, 2, CV_64FC1);
CvMat *Sci = cvCreateMat(2, 2, CV_64FC1);
CvMat *Scit = cvCreateMat(2, 2, CV_64FC1);
CvMat *Wc = cvCreateMat(dim, 2, CV_64FC1);
CvMat *W = cvCreateMat(dim, 2, CV_64FC1);
CvMat *Wz = cvCreateMat(dim, 1, CV_64FC1);
CvMat *Wct = cvCreateMat(2, dim, CV_64FC1);
CvMat *WcWc = cvCreateMat(dim, dim, CV_64FC1);
// % Compute Kalman gain
cvAdd(Pzz, R, S);
//cholesky decomposition이 실패하면 업데이트는 하지 않는다.
if(choldc(S, p, Sct) < 0)
{
TRACE("idf : %d\n", idf);
PrintCvMat(UD, "UD");
PrintCvMat(U, "U");
PrintCvMat(D, "D");
PrintCvMat(V, "V");
PrintCvMat(zprev, "zprev");
PrintCvMat(XX, "XX");
PrintCvMat(PX, "PX");
PrintCvMat(Ps, "Ps");
PrintCvMat(ss, "ss");
for(j=0; j<N; j++)
{
double dx = ss->data.db[f*ss->cols+j] - ss->data.db[0*ss->cols+j];
double dy = ss->data.db[(f+1)*ss->cols+j] - ss->data.db[1*ss->cols+j];
double d2 = dx*dx + dy*dy;
double d = sqrt(d2);
double angle = atan2(dy,dx) - ss->data.db[2*ss->cols+j];
TRACE("%.4f %.4f %.4f %.4f %.4f %.4f %.4f\n", dx, dy, d2, d, angle, atan2(dy, dx), ss->data.db[2*ss->cols+j]);
}
PrintCvMat(zs, "zs");
PrintCvMat(zz, "zz");
PrintCvMat(zm, "zm");
PrintCvMat(dx, "dx");
PrintCvMat(dz, "dz");
PrintCvMat(Pxz, "Pxz");
PrintCvMat(Pzz, "Pzz");
PrintCvMat(R, "R");
return -1;
}
cvT(Sct, Sc);
cvInv(Sc, Sci);
cvT(Sci, Scit);
// PrintCvMat(S, "S");
// PrintCvMat(Sct, "Sct");
// PrintCvMat(Sc, "Sc");
// PrintCvMat(Sci, "Sci");
cvMatMul(Pxz, Sci, Wc);
// PrintCvMat(Wc, "Wc");
cvMatMul(Wc, Scit, W);
// PrintCvMat(W, "W");
cvSub(zprev, zm, zprev);
cvMatMul(W, zprev, Wz);
cvAdd(XX, Wz, XX);
cvT(Wc, Wct);
cvMatMul(Wc, Wct, WcWc);
cvSub(PX, WcWc, PX);
// PrintCvMat(Pzz, "Pzz");
// PrintCvMat(R, "R");
// PrintCvMat(S, "S");
// PrintCvMat(Pxz, "Pxz");
// PrintCvMat(Sc, "Sc");
// PrintCvMat(Sct, "Sct");
// PrintCvMat(Sci, "Sci");
// PrintCvMat(Scit, "Scit");
// PrintCvMat(W, "W");
// PrintCvMat(zprev, "zprev");
// PrintCvMat(zm, "zm");
// PrintCvMat(Wc, "Wc");
// PrintCvMat(Wct, "Wct");
// PrintCvMat(WcWc, "WcWc");
// PrintCvMat(PX, "Px");
//
// PrintCvMat(XX, "XX");
// PrintCvMat(PX, "PX");
cvReleaseMat(&D);
cvReleaseMat(&U);
cvReleaseMat(&V);
cvReleaseMat(&Ds);
cvReleaseMat(&UD);
cvReleaseMat(&Ps);
cvReleaseMat(&ss);
cvReleaseMat(&zs);
cvReleaseMat(&zz);
cvReleaseMat(&dz);
cvReleaseMat(&zprev);
cvReleaseMat(&zm);
cvReleaseMat(&dx);
cvReleaseMat(&repx);
cvReleaseMat(&repzm);
cvReleaseMat(&Pxz);
cvReleaseMat(&Pzz);
cvReleaseMat(&dxu);
cvReleaseMat(&dxl);
cvReleaseMat(&dzu);
cvReleaseMat(&dzl);
cvReleaseMat(&dztu);
cvReleaseMat(&dztl);
cvReleaseMat(&dxdzu);
cvReleaseMat(&dxdzl);
cvReleaseMat(&dzdzu);
cvReleaseMat(&dzdzl);
cvReleaseMat(&S);
cvReleaseMat(&p);
cvReleaseMat(&Sct);
cvReleaseMat(&Sc);
cvReleaseMat(&Sci);
cvReleaseMat(&Scit);
cvReleaseMat(&Wc);
cvReleaseMat(&W);
cvReleaseMat(&Wz);
cvReleaseMat(&Wct);
cvReleaseMat(&WcWc);
return 0;
}
int CUKF::UnscentedTransform(int model)
{
// Set up some variables
int dim = cvGetSize(XX).height;
int N = 2*dim+1;
double scale = 3;
int kappa = scale-dim;
// Create samples
// SVD
CvSize P = cvGetSize(PX);
CvMat *D = cvCreateMat(P.height, P.width, CV_64FC1);
CvMat *U = cvCreateMat(P.height, P.height, CV_64FC1);
CvMat *V = cvCreateMat(P.width, P.width, CV_64FC1);
CvMat *Ds = cvCreateMat(P.height, P.width, CV_64FC1);
CvMat *UD = cvCreateMat(P.height, P.width, CV_64FC1);
CvMat *Ps = cvCreateMat(P.height, P.width, CV_64FC1);
cvSVD(PX, D, U, V, CV_SVD_V_T);
MatSqrt(D, Ds);
cvMatMul(U, Ds, UD);
cvScale(UD, Ps, sqrt(scale), 0);
// PrintCvMat(U, "U");
// PrintCvMat(D, "D");
// PrintCvMat(V, "V");
// PrintCvMat(Ds, "Ds");
// PrintCvMat(UD, "UD");
// PrintCvMat(PX, "PX");
// PrintCvMat(Ps, "Ps");
CvMat *ss = cvCreateMat(dim, N, CV_64FC1);
//for sigma points display
//cvReleaseMat(&m_sigmaPointst_1);
//cvReleaseMat(&m_sigmaPointst);
//m_sigmaPointst_1 = cvCreateMat(dim, N, CV_64FC1);
//m_sigmaPointst = cvCreateMat(dim, N, CV_64FC1);
//
int i,j,jj;
int row = dim;
int col = N;
cvRepeat(XX, ss);
double* ssdb;
double* Psdb;
ssdb = ss->data.db;
Psdb = Ps->data.db;
for(i=0; i<row; i++)
{
for(j=1, jj=0; j<dim+1; j++, jj++)
{
//ss->data.db[i*ss->cols+j] += Ps->data.db[i*Ps->cols+jj];
ssdb[i*ss->cols+j] += Psdb[i*Ps->cols+jj];
}
}
for(i=0; i<row; i++)
{
for(j=1+dim, jj=0; j<ss->cols; j++, jj++)
{
//ss->data.db[i*ss->cols+j] -= Ps->data.db[i*Ps->cols+jj];
ssdb[i*ss->cols+j] -= Psdb[i*Ps->cols+jj];
}
}
//cvRepeat(ss, m_sigmaPointst_1);
// Transform samples according to motion model
CvMat *ys;
CvMat *base;
CvMat *delta;
CvMat *reptemp;
int ysdim;
double *ysdb;
double *basedb;
double *deltadb;
double *reptempdb;
switch(model)
{
case MOTION_MODEL:
ysdim = dim-2;
ys = cvCreateMat(ysdim, N, CV_64FC1);
base = cvCreateMat(ysdim, N, CV_64FC1);
delta = cvCreateMat(ysdim, N, CV_64FC1);
reptemp = cvCreateMat(ysdim, 1, CV_64FC1);
cvRepeat(ss, ys);
ysdb = ys->data.db;
basedb = base->data.db;
deltadb = delta->data.db;
reptempdb = reptemp->data.db;
for(j=0; j<N; j++)
{
double v = ssdb[(dim-2)*N+j];
double g = ssdb[(dim-1)*N+j];
ysdb[0*N+j] = ssdb[0*N+j] + v*DT_CONTROLS*cos(g+ssdb[2*N+j]);
ysdb[1*N+j] = ssdb[1*N+j] + v*DT_CONTROLS*sin(g+ssdb[2*N+j]);
ysdb[2*N+j] = ssdb[2*N+j] + v*DT_CONTROLS*sin(g)/WHEELBASE;
}
for(i=0; i<ys->rows; i++) reptempdb[reptemp->cols*i] = ysdb[ys->cols*i];
cvRepeat(reptemp, base);
cvSub(base, ys, delta);
for(i=0; i<base->cols; i++) basedb[2*base->cols+i] = PI2PI(basedb[2*base->cols+i]);
cvSub(base, delta, ys);
//PrintCvMat(ys, "ys");
//PrintCvMat(reptemp, "reptemp");
//PrintCvMat(base, "base");
//PrintCvMat(delta, "delta");
//PrintCvMat(ys, "ys");
cvReleaseMat(&base);
cvReleaseMat(&delta);
cvReleaseMat(&reptemp);
//cvRepeat(ys, m_sigmaPointst);
break;
case AUGMENT_MODEL:
//PrintCvMat(ss, "ss");
ysdim = dim;
ys = cvCreateMat(dim, N, CV_64FC1);
double phi, r, b, s, c;
//PrintCvMat(XX, "XX");
for(j=0; j<N; j++)
{
phi = ssdb[2*ss->cols+j];
r = ssdb[(ss->rows-2)*ss->cols+j];
b = ssdb[(ss->rows-1)*ss->cols+j];
s = sin(phi+b);
c = cos(phi+b);
ssdb[(ss->rows-2)*ss->cols+j] = ssdb[0*ss->cols+j] + r*c;
ssdb[(ss->rows-1)*ss->cols+j] = ssdb[1*ss->cols+j] + r*s;
}
cvCopy(ss, ys);
//PrintCvMat(Ps, "Ps");
//PrintCvMat(ss, "ss");
break;
}
CvMat *y = cvCreateMat(ysdim, 1, CV_64FC1);
CvMat *dy = cvCreateMat(ysdim, N, CV_64FC1);
CvMat *dyu = cvCreateMat(ysdim, 1, CV_64FC1);
CvMat *dytu = cvCreateMat(1, ysdim, CV_64FC1);
CvMat *dyl = cvCreateMat(ysdim, N-1, CV_64FC1);
CvMat *dytl = cvCreateMat(N-1, ysdim, CV_64FC1);
CvMat *temp = cvCreateMat(ysdim, ysdim, CV_64FC1);
CvMat *Y = cvCreateMat(ysdim, ysdim, CV_64FC1);
CvMat *temp1 = cvCreateMat(ysdim, 1, CV_64FC1);
CvMat *repy = cvCreateMat(ysdim, N, CV_64FC1);
double *ydb = y->data.db;
double *dydb = dy->data.db;
double *dyudb = dyu->data.db;
double *dytudb = dytu->data.db;
double *dyldb = dyl->data.db;
double *dytldb = dytl->data.db;
double *tempdb = temp->data.db;
double *Ydb = Y->data.db;
double *temp1db = temp1->data.db;
double *repydb = repy->data.db;
ysdb = ys->data.db;
for(i=0; i<y->rows; i++) ydb[i] = 2*kappa*ysdb[ys->cols*i];
for(i=0; i<y->rows; i++)
{
for(j=1; j<N; j++)
{
ydb[i] += ysdb[ys->cols*i+j];
}
}
for(i=0; i<y->rows; i++) ydb[i] /= (2*scale);
for(i=0; i<y->rows; i++) temp1db[i] = ydb[y->cols*i];
cvRepeat(temp1, repy);
cvSub(ys, repy, dy);
// PrintCvMat(temp1, "temp1");
// PrintCvMat(repy, "repy");
// PrintCvMat(dy, "dy");
// for(j=0; j<N; j++)
// {
// dy->data.db[0*N+j] = ys->data.db[0*N+j] - y->data.db[0];
// dy->data.db[1*N+j] = ys->data.db[1*N+j] - y->data.db[1];
// dy->data.db[2*N+j] = ys->data.db[2*N+j] - y->data.db[2];
// }
for(i=0; i<ysdim; i++) dyudb[i] = dydb[i*dy->cols];
for(i=0; i<ysdim; i++) for(j=1, jj=0; j<N; j++, jj++)
dyldb[i*dyl->cols+jj] = dydb[i*dy->cols+j];
// PrintCvMat(ys, "ys");
// PrintCvMat(dy, "dy");
// PrintCvMat(dyu, "dyu");
// PrintCvMat(dyl, "dyl");
cvT(dyu, dytu);
cvT(dyl, dytl);
cvMatMul(dyu, dytu, Y);
// PrintCvMat(Y, "Y");
cvScale(Y, Y, 2*kappa);
// PrintCvMat(Y, "Y");
cvMatMul(dyl, dytl, temp);
// PrintCvMat(temp, "temp");
cvAdd(Y, temp, Y);
// PrintCvMat(Y, "Y");
cvScale(Y, Y, 1/(double)(2*scale));
// PrintCvMat(Y, "Y");
cvReleaseMat(&XX);
cvReleaseMat(&PX);
XX = cvCreateMat(y->rows, y->cols, CV_64FC1);
PX = cvCreateMat(Y->rows, Y->cols, CV_64FC1);
cvCopy(y, XX);
cvCopy(Y, PX);
// PrintCvMat(XX, "XX");
// PrintCvMat(PX, "PX");
cvReleaseMat(&D);
cvReleaseMat(&U);
cvReleaseMat(&V);
cvReleaseMat(&Ds);
cvReleaseMat(&UD);
cvReleaseMat(&Ps);
cvReleaseMat(&ss);
cvReleaseMat(&ys);
cvReleaseMat(&y);
cvReleaseMat(&dy);
cvReleaseMat(&dyu);
cvReleaseMat(&dytu);
cvReleaseMat(&dyl);
cvReleaseMat(&dytl);
cvReleaseMat(&temp);
cvReleaseMat(&Y);
cvReleaseMat(&temp1);
cvReleaseMat(&repy);
return 0;
}
/**
* @brief Generates a optimised point cloud from a collection of processed keyframes using sparse bundle adjustment.
* @param allFrames A vector containing keyframes which have been completely processed.
* @param outputCloud The cloud generated from the keyframes.
*/
void utils::calculateSBACloud(std::vector<KeyframeContainer>& allFrames,boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB> >& outputCloud)
{
sba::SysSBA bundleAdjuster;
//set the verbosity of the bundle adjuster
#ifndef NDEBUG
bundleAdjuster.verbose=100;
#else
bundleAdjuster.verbose=0;
#endif
//a list of keypoints that already have been added to the SBA system
std::vector<std::pair<int,uint> > addedPoints; //(frameID,keypointIndex)
//maps frame IDs to frame index in the vector
std::map<int,uint> ID2Ind;
for(uint f=0;f<allFrames.size();++f) ID2Ind[allFrames[f].ID]=f;
//maps the frame vector index to the node vector index
std::map<uint,int> fInd2NInd;
//count the number of 2D/3D-points and camera nodes if in debug mode
#ifndef NDEBUG
uint nrP3D=0;
uint nrP2D=0;
uint nrC=0;
#endif
//add every valid frame as a camera node to the sba system
for(uint f=0;f<allFrames.size();++f)
{
if(!allFrames[f].invalid)
{
//get rotation and translation from the projection matrix
Eigen::Matrix3d rot;
cv::Mat_<double> cvT;
Eigen::Vector4d t;
//get rotation
for(int x=0;x<3;++x) for(int y=0;y<3;++y) rot(x,y)=allFrames[f].projectionMatrix(x,y);
//solve for translation
cv::solve(allFrames[f].projectionMatrix(cv::Range(0,3),cv::Range(0,3)),allFrames[f].projectionMatrix(cv::Range(0,3),cv::Range(3,4)),cvT);
//save translation
for(int x=0;x<3;++x) t(x)=-cvT(x,0);
t(3)=1.0;
//convert rotation to quaternion
Eigen::Quaterniond qrot(rot);
qrot.normalize();
//convert camera calibration matrix
frame_common::CamParams cameraParameters;
cameraParameters.fx=allFrames[f].cameraCalibration(0,0);
cameraParameters.fy=allFrames[f].cameraCalibration(1,1);
cameraParameters.cx=allFrames[f].cameraCalibration(0,2);
cameraParameters.cy=allFrames[f].cameraCalibration(1,2);
cameraParameters.tx=0.0;
//add the frame as a camera node
fInd2NInd[f]=bundleAdjuster.addNode(t,qrot,cameraParameters,false);
#ifndef NDEBUG
++nrC;
#endif
}
else
fInd2NInd[f]=-1;
}
//add the points to the correct nodes
for(uint f=0;f<allFrames.size();++f)
{
if(!allFrames[f].invalid)
{
//add the points
for(uint m=0;m<allFrames[f].matches.size();++m)
{
std::pair<int,uint> myP(allFrames[f].ID,m);
bool goodToAdd=!utils::vectorContainsElement(addedPoints,myP);
//point hasn't been added yet
if(goodToAdd)
{
//check if the point has valid depth information
float depth=allFrames[f].depthImg.image.at<float>(allFrames[f].keypoints[m].pt.y,allFrames[f].keypoints[m].pt.x);
if(isnan(depth)==0)
{
//find the largest completely connected component emanating from this point (clique)
std::vector<std::pair<int,uint> > pointsComplete;
pointsComplete.push_back(std::pair<int,uint>(allFrames[f].ID,m));
//check all matches of the point if they haven't been added yet and their frame is valid
for(uint o=0;o<allFrames[f].matches[m].size();++o)
{
std::pair<int,uint> newElement(allFrames[f].matches[m][o].first.val[0],allFrames[f].matches[m][o].first.val[1]);
if(!utils::vectorContainsElement(addedPoints,newElement) && !allFrames[ID2Ind[newElement.first]].invalid)
pointsComplete.push_back(newElement);
}
//weed out all points that are not completely connected
while(pointsComplete.size()>1)
{
//find the point with the fewest connections (if the component is completely connected all points have the same number of connections)
uint minCorrespondences=pointsComplete.size();
uint worstInd=0;
for(uint i=0;i<pointsComplete.size();++i)
{
//count the number of connections that this point has
uint fInd=ID2Ind[pointsComplete[i].first];
uint myCorr=0;
for(uint q=0;q<allFrames[fInd].matches.size();++q)
for(uint r=0;r<allFrames[fInd].matches[q].size();++r)
{
std::pair<int,uint> tmpElement(allFrames[fInd].matches[q][r].first.val[0],allFrames[fInd].matches[q][r].first.val[1]);
if(utils::vectorContainsElement(pointsComplete,tmpElement))
++myCorr;
}
//save this point if it is the current worst
if(myCorr<minCorrespondences)
{
minCorrespondences=myCorr;
worstInd=i;
}
}
//if the worst point has not the maximal number of connections erase it, else break as all points have the maximal number of connections
if(minCorrespondences<pointsComplete.size()-1)
{
pointsComplete.erase(pointsComplete.begin()+worstInd);
}
else
break;
}
//now pointsComplete contains the clique if the clique has more than one isolated point, write the points to the system
if(pointsComplete.size()>1)
{
//calculate the 3D point and add it to the system
cv::Mat_<double> p2D(2,1,0.0);
p2D(0,0)=allFrames[f].keypoints[m].pt.x;
p2D(1,0)=allFrames[f].keypoints[m].pt.y;
cv::Mat_<double> p3D=reprojectImagePointTo3D(p2D,allFrames[f].cameraCalibration,allFrames[f].projectionMatrix,depth);
Eigen::Vector4d newPoint;
for(uint x=0;x<4;++x) newPoint(x)=p3D(x,0);
int pointIndex=bundleAdjuster.addPoint(newPoint);
#ifndef NDEBUG
++nrP3D;
#endif
//add all image points corresponding to the 3D point to the system
for(uint i=0;i<pointsComplete.size();++i)
{
uint fInd=ID2Ind[pointsComplete[i].first];
Eigen::Vector2d imgPoint;
imgPoint(0)=allFrames[fInd].keypoints[pointsComplete[i].second].pt.x;
imgPoint(1)=allFrames[fInd].keypoints[pointsComplete[i].second].pt.y;
bundleAdjuster.addMonoProj(fInd2NInd[fInd],pointIndex,imgPoint);
#ifndef NDEBUG
++nrP2D;
#endif
}
}
}
}
}
}
}
#ifndef NDEBUG
ROS_INFO("C: %u;3D: %u;2D: %u",nrC,nrP3D,nrP2D);
#endif
//remove bad tracks
bundleAdjuster.calcCost();
bundleAdjuster.removeBad(2);
bundleAdjuster.reduceTracks();
//run 100 iterations of sba
bundleAdjuster.doSBA(100,1e-3, SBA_SPARSE_CHOLESKY);
//save the new projective matrix calculated with sba
for(uint f=0;f<allFrames.size();++f)
if(!allFrames[f].invalid)
for(int x=0;x<3;++x) for(int y=0;y<4;++y) allFrames[f].projectionMatrix(x,y)=bundleAdjuster.nodes[fInd2NInd[f]].w2n(x,y);
//generate the point cloud from the new projection matrices
utils::generateCloud(allFrames,*outputCloud);
}
