int main(){
double dt = 0.1;
double *time;
double t_start, t_end;
int time_steps, Nsamples;
Matrix *xSaved, *zSaved;
double z;
t_start = 0;
t_end = 10;
time_steps =(int)((double)(t_end - t_start)/dt + 0.5);
//arredonda para cima o resultado da divisão
time = (double *)aloca(time_steps*sizeof(double));
int i;
for(i = 0; i < time_steps; i++){
time[i] = i*dt;
}
Nsamples = time_steps;
xSaved = new_M(Nsamples, 3);
zSaved = new_M(Nsamples, 2);
for(i = 0; i < Nsamples; i++)
{
z = getVel();
Matrix *X = NULL;
X = intKalman(z);
xSaved->a[i][0] = time[i];
xSaved->a[i][1] = X->a[0][0];
xSaved->a[i][2] = X->a[1][0];
zSaved->a[i][0] = time[i];
zSaved->a[i][1] = z;
kill_M(&X);
}
const char fileX[] = "../data/X.dat";
const char fileZ[] = "../data/Z.dat";
MatrixPrint2File(xSaved, fileX);
MatrixPrint2File(zSaved, fileZ);
kill_M(&xSaved);
kill_M(&zSaved);
libera(time);
KalmanFilter_End();
EndProgram();
reportGood("O programa chegou ao fim!");
return(0);
}
void get_simply_LU_fatoration(Matrix *A, Matrix **L, Matrix **U){
/*
*acha a fatoração LU sem tentar permutações
* */
int i, j;
double l;
*U = copy_M(A);
*L = new_M((*U)->rows, (*U)->columns);
//L matriz triangular inferior especial
for(i = 0; i < (*L)->rows; i++)
(*L)->a[i][i] = 1;//diagonal 1
for(i = 0; i < (*L)->rows; i++)
for(j = i + 1; j < (*L)->columns; j++)
(*L)->a[i][j] =0;//superior 0
for(j = 0; j < (*U)->columns; j++)
{
if((*U)->a[j][j] == 0)
printf("A matriz não é regular!\n");
else
{
for(i = j+1; i < (*U)->rows; i++)
{
l = -1.0*(*U)->a[i][j]/(*U)->a[j][j];
add_line((*U), j, l, i);
(*L)->a[i][j] = -1.0 * l;
}//end for
}//end else
}//end for
}
Matrix *M_cross_N(Matrix *M, Matrix *N){
/*retorna ponteiro alocad para o produto matricial*/
int i, j, k, n;
Matrix *P;
if(M && N)
{
if(M->columns == N->rows)
{
n = M->columns; // = N->rows
P = new_M(M->rows, N->columns);
for(i = 0; i < P->rows; i++)
{
for(j = 0; j < P->columns; j++)
{
P->a[i][j] = 0;
for(k = 0; k < n; k++)
P->a[i][j] += M->a[i][k] * N->a[k][j];
}//end for colunas
}//end for linhas
}//end if igualdade
else
printf("Tentou-se multiplicar matrizes n compatíveis!\n");
}//end if existe
else
printf("Um dos argumentos de M_cross_N não existe!\n");
return(P);
}
Matrix *subtract(Matrix *A,Matrix *B)
{
/*retorna ponteiro alocado para a matriz soma das entradas*/
int i, j, M, N;
Matrix *S;
if(A->rows == B->rows && A->columns == B->columns)
{
M = A->rows;
N = A->columns;
S = new_M(M, N);
for(i = 0; i < M; i++)
{
for(j = 0; j < N; j++)
S->a[i][j] = A->a[i][j] - B->a[i][j];
}
}
else
{
char msg[50];
sprintf(msg, "Matrizes não tem mesmas dimensões!\n");
reportBad(msg);
}
return(S);
}
void simply_LU_fatoration(Matrix *A, Matrix *x, Matrix *b){
/*Resolvemos
* Ax = B
*
* fazendo A = LU
* Ax = LUx = b
* definimos Ux = c
* e portanto
* basta resolver
* - Lc = b
* - Ux = c
*
* aplica-se substituição direta no primeiro
* e inversa no segundo
* **PARA CASO ACHE 0 NA DIAGONAL PRINCIPAL**
* */
Matrix *L, *U, *c;
if(A && x && b)
{
if(A->columns != A->rows)
{
printf("solve_by_LU_fatoration recebeu A não quadrado!\n");
}
else
{
get_simply_LU_fatoration(A, &L, &U);
c = new_M(x->rows, x->columns);
if(check_diagonal(U))
{
printf("A matriz A é:\n");
print_M(A);
printf("A matriz b é:\n");
print_M(b);
printf("A fatoração L é:\n");
print_M(L);
printf("A fatoração U é:\n");
print_M(U);
ForwardSubstitution(L, c, b);
BackSubstitution(U, x, c);
printf("A solução x é:\n");
print_M(x);
}//end if check
else
{
printf("Falha na fatoração LU! U apresenta 0's na diagonal!\n");
}
kill_M(&L);
kill_M(&U);
kill_M(&c);
}//end else A quadrado
}//end if existe
else
printf("solve_by_LU_fatoration recebeu alguma matriz que n existe!\n");
}
void get_GaussSeidel_method_matrixs (
Matrix *A, Matrix *u, Matrix *b, Matrix **T, Matrix **c
)
//calcula as matrizes necessárias para o algoritmo de
//Jacobi
{
Matrix *L, *D, *U, *L_plus_D_inv, *aux;
int i,j;
L = new_M(A->rows, A->columns);
D = new_M(A->rows, A->columns);
U = new_M(A->rows, A->columns);
for (i = 1 ; i < A->rows; i++)
for (j = 0 ; j < i; j++)
L->a[i][j] = A->a[i][j];
for (i = 0 ; i < A->rows; i++)
D->a[i][i] = A->a[i][i];
for (i = 0 ; i < A->rows - 1; i++)
for (j = i+1 ; j < A->columns; j++)
U->a[i][j] = A->a[i][j];
aux = sum(L, D);
L_plus_D_inv = Inverse(aux);
kill_M(&aux);
*T = M_cross_N(L_plus_D_inv, U);
scalar_cross_M(*T, -1);
//T = -(L + D)^{-1}.U
*c = M_cross_N(L_plus_D_inv, b);
//c = D^{-1}b
kill_M(&L);
kill_M(&D);
kill_M(&U);
kill_M(&L_plus_D_inv);
}
Matrix *Matrix_const(int M, int N, double value){
Matrix *A = new_M(M, N);
int i,j;
for(i = 0; i < A->rows; i++)
for(j = 0; j < A->columns; j++)
A->a[i][j] = value;
return A;
}
Matrix *new_M_I(int size){
/*retorna matriz Identidade de tamanho size x size*/
Matrix *I = NULL;
int i;
I = new_M(size, size);
if(I)
{
for(i = 0; i < I->rows; i++)
I->a[i][i] = 1;
}
return(I);
}
void MarkerDetectorImpl::TrackMarkerAdd(int id, PointDouble corners[4]) {
Marker *mn = new_M(edge_length, res, margin);
if (map_edge_length.find(id) != map_edge_length.end()) {
mn->SetMarkerSize(map_edge_length[id], res, margin);
}
mn->SetId(id);
mn->marker_corners_img.clear();
mn->marker_corners_img.push_back(corners[0]);
mn->marker_corners_img.push_back(corners[1]);
mn->marker_corners_img.push_back(corners[2]);
mn->marker_corners_img.push_back(corners[3]);
_track_markers_push_back(mn);
delete mn;
}
Matrix *copy_M(Matrix *A){
/*
* retorna ponteiro para matriz alocada igual a recebida
* */
Matrix *S;
int i, j;
if(A)
{
S = new_M(A->rows, A->columns);
for(i = 0; i < S->rows; i++)
for(j = 0; j < S->columns; j++)
S->a[i][j] = A->a[i][j];
}
else
printf("copy_M recebeu uma matriz que não existe!\n");
return(S);
}
Matrix *new_M_from_string(int Ma, int Na, const char *elements){
/*cria matriz apartir de string recebdi
*sem enter, virgulas, barra ou o diabo...somente os números
* linhas e colunas devem ser definidos por Ma e Na
* */
Matrix *M;
int i,j;
FILE *fp;
fp =fopen("temp.temp", "w");
fprintf(fp,"%s\n", elements);
fp =freopen("temp.temp", "r", fp);
M = new_M(Ma, Na);
for(i = 0; i < M->rows; i++)
for(j = 0; j < M->columns; j++)
fscanf(fp, "%lf", &(M->a[i][j]));
fclose(fp);
system("rm temp.temp");
return(M);
}
Matrix *Transpose_M(Matrix *M){
/*retorna ponteiro alocado para matriz transporta da recebida*/
int i, j;
Matrix *M_t;
M_t = NULL;
if(M)
{
M_t = new_M(M->columns, M->rows);
for(i = 0; i < M_t->rows; i++)
{
for(j = 0; j < M_t->columns; j++)
{
M_t->a[i][j] += M->a[j][i];
}//end for colunas
}//end for linhas
}//end if existe
else
printf("Transpose_M recebeu matriz não existente!\n");
return(M_t);
}
void Permuted_LU_fatoration(Matrix *A, Matrix *x, Matrix *b){
/*Resolvemos
* Ax = B
*
* fazendo PA = LU
* PAx = LUx = Pb = B
* definimos Ux = c
* e portanto
* basta resolver
* - Lc = B
* - Ux = c
*
* aplica-se substituição direta no primeiro
* e inversa no segundo
* **TENTA CORRIGIR CASO ACHE 0 NA DIAGONAL PRINCIPAL**
* */
Matrix *L, *U,*P, *B, *c;
//L:matriz trang inferior especial
//U:matriz triang superior com diagonal não nula
//P:matriz das permutações
//B: permutação aplicada em b
//c: para o sistema auxlia Lc = B
if(A && x && b)
{
if(A->columns != A->rows)
{
printf("solve_by_LU_fatoration recebeu A não quadrado!\n");
}
else
{
get_Permuted_LU_fatoration(A, &L, &U, &P);
B = M_cross_N(P, b);
c = new_M(x->rows, x->columns);
if(check_diagonal(U))
{
printf("A matriz A é:\n");
print_M(A);
printf("A matriz b é:\n");
print_M(b);
printf("A matriz P é:\n");
print_M(P);
printf("A matriz B é:\n");
print_M(B);
printf("A fatoração L é:\n");
print_M(L);
printf("A fatoração U é:\n");
print_M(U);
ForwardSubstitution(L, c, B);
printf(" c é:\n");
print_M(c);
BackSubstitution(U, x, c);
printf("A solução x é:\n");
print_M(x);
}//end if check
else
{
printf("Falha na fatoração LU! U apresenta 0's na diagonal!\n");
}
kill_M(&L);
kill_M(&U);
kill_M(&c);
kill_M(&P);
kill_M(&B);
}//end else A quadrado
}//end if existe
else
printf("solve_by_LU_fatoration recebeu alguma matriz que n existe!\n");
}
int MarkerDetectorImpl::Detect(IplImage *image,
Camera *cam,
bool track,
bool visualize,
double max_new_marker_error,
double max_track_error,
LabelingMethod labeling_method,
bool update_pose)
{
assert(image->origin == 0); // Currently only top-left origin supported
double error=-1;
// Swap marker tables
_swap_marker_tables();
_markers_clear();
switch(labeling_method)
{
case CVSEQ :
if(!labeling)
labeling = new LabelingCvSeq();
((LabelingCvSeq*)labeling)->SetOptions(detect_pose_grayscale);
break;
}
labeling->SetCamera(cam);
labeling->LabelSquares(image, visualize);
vector<vector<PointDouble> >& blob_corners = labeling->blob_corners;
IplImage* gray = labeling->gray;
int orientation;
// When tracking we find the best matching blob and test if it is near enough?
if (track) {
for (size_t ii=0; ii<_track_markers_size(); ii++) {
Marker *mn = _track_markers_at(ii);
if (mn->GetError(Marker::DECODE_ERROR|Marker::MARGIN_ERROR) > 0) continue; // We track only perfectly decoded markers
int track_i=-1;
int track_orientation=0;
double track_error=1e200;
for(unsigned i = 0; i < blob_corners.size()/*blobs_ret.size()*/; ++i) {
if (blob_corners[i].empty()) continue;
mn->CompareCorners(blob_corners[i], &orientation, &error);
if (error < track_error) {
track_i = i;
track_orientation = orientation;
track_error = error;
}
}
if (track_error <= max_track_error) {
mn->SetError(Marker::DECODE_ERROR, 0);
mn->SetError(Marker::MARGIN_ERROR, 0);
mn->SetError(Marker::TRACK_ERROR, track_error);
mn->UpdatePose(blob_corners[track_i], cam, track_orientation, update_pose);
_markers_push_back(mn);
blob_corners[track_i].clear(); // We don't want to handle this again...
if (visualize) mn->Visualize(image, cam, CV_RGB(255,255,0));
}
}
}
// Now we go through the rest of the blobs -- in case there are new markers...
for(size_t i = 0; i < blob_corners.size(); ++i)
{
if (blob_corners[i].empty()) continue;
Marker *mn = new_M(edge_length, res, margin);
if (mn->UpdateContent(blob_corners[i], gray, cam) &&
mn->DecodeContent(&orientation) &&
(mn->GetError(Marker::MARGIN_ERROR | Marker::DECODE_ERROR) <= max_new_marker_error))
{
if (map_edge_length.find(mn->GetId()) != map_edge_length.end()) {
mn->SetMarkerSize(map_edge_length[mn->GetId()], res, margin);
}
mn->UpdatePose(blob_corners[i], cam, orientation, update_pose);
_markers_push_back(mn);
if (visualize) mn->Visualize(image, cam, CV_RGB(255,0,0));
}
delete mn;
}
return (int) _markers_size();
}
Matrix *Matrix_zeros(int M, int N){
return new_M(M, N);
}
