source: flair-src/trunk/lib/FlairFilter/src/EulerDerivative_impl.cpp@ 256

Last change on this file since 256 was 256, checked in by Sanahuja Guillaume, 4 years ago

add saturation to euler derivative

File size: 4.1 KB
Line 
1// %flair:license{
2// This file is part of the Flair framework distributed under the
3// CECILL-C License, Version 1.0.
4// %flair:license}
5// created: 2011/05/01
6// filename: EulerDerivative.cpp
7//
8// author: Guillaume Sanahuja
9// Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11// version: $Id: $
12//
13// purpose: objet permettant le calcul d'une derivee d'Euler
14//
15//
16/*********************************************************************/
17
18#include "EulerDerivative.h"
19#include "EulerDerivative_impl.h"
20#include <Matrix.h>
21#include <Layout.h>
22#include <GroupBox.h>
23#include <DoubleSpinBox.h>
24
25using std::string;
26using namespace flair::core;
27using namespace flair::gui;
28using namespace flair::filter;
29
30EulerDerivative_impl::EulerDerivative_impl(EulerDerivative *self,
31 const LayoutPosition *position,
32 string name,
33 const Matrix *init_value) {
34 this->self = self;
35
36 if (init_value != NULL) {
37 // init output matrix of same size as init
38 cvmatrix_descriptor *desc =new cvmatrix_descriptor(init_value->Rows(), init_value->Cols());
39
40 for (int i = 0; i < init_value->Rows(); i++) {
41 for (int j = 0; j < init_value->Cols(); j++) {
42 desc->SetElementName(i, j, init_value->Name(i, j));
43 }
44 }
45 output = new Matrix(self, desc,init_value->GetDataType().GetElementDataType(), name);
46 delete desc;
47 for (int i = 0; i < init_value->Rows(); i++) {
48 for (int j = 0; j < init_value->Cols(); j++) {
49 output->SetValue(i, j, init_value->Value(i,j));
50 }
51 }
52 } else {
53 // if NULL, assume dimension 1, and init=0
54 cvmatrix_descriptor *desc = new cvmatrix_descriptor(1, 1);
55 desc->SetElementName(0, 0, "output");
56 output = new Matrix(self, desc, floatType, name);
57 delete desc;
58 }
59
60
61 cvmatrix_descriptor *desc = new cvmatrix_descriptor(output->Rows(), output->Cols());
62 prev_input = new Matrix(self, desc, output->GetDataType().GetElementDataType(), name);
63 prev_output = new Matrix(self, desc, output->GetDataType().GetElementDataType(), name);
64 delete desc;
65
66
67 // init UI
68 GroupBox *reglages_groupbox = new GroupBox(position, name);
69 T = new DoubleSpinBox(reglages_groupbox->NewRow(), "period, 0 for auto:"," s", 0, 1, 0.01);
70 sat = new DoubleSpinBox(reglages_groupbox->NewRow(), "saturation, -1 to disable:",-1,100000,1,1,-1);
71}
72
73EulerDerivative_impl::~EulerDerivative_impl() {}
74
75void EulerDerivative_impl::UpdateFrom(const io_data *data) {
76 float delta_t;
77 const Matrix* input = dynamic_cast<const Matrix*>(data);
78
79 if (!input) {
80 self->Warn("casting %s to Matrix failed\n",data->ObjectName().c_str());
81 return;
82 }
83
84 // on prend une fois pour toute les mutex et on fait des accès directs
85 output->GetMutex();
86 input->GetMutex();
87
88 if (first_update == true) {
89 for (int i = 0; i < input->Rows(); i++) {
90 for (int j = 0; j < input->Cols(); j++) {
91 prev_input->SetValueNoMutex(i, j, input->ValueNoMutex(i, j));
92 }
93 }
94 first_update = false;
95 } else {
96 if (T->Value() == 0) {
97 delta_t = (float)(data->DataDeltaTime()) / 1000000000.;
98 } else {
99 delta_t = T->Value();
100 }
101
102 if(delta_t!=0) {
103 for (int i = 0; i < input->Rows(); i++) {
104 for (int j = 0; j < input->Cols(); j++) {
105 float result=(input->ValueNoMutex(i, j) - prev_input->ValueNoMutex(i, j)) / delta_t;
106 if(sat->Value()!=-1) {
107 if(result>sat->Value() && result>0) result=sat->Value();
108 if(result<-sat->Value() && result<0) result=-sat->Value();
109 }/* filter by acc
110 float acc=result-prev_output->ValueNoMutex(i, j)/delta_t;
111 if(acc>20 || acc<-20) {
112 printf("%s %f\n",self->ObjectName().c_str(),acc);
113 } else {*/
114 output->SetValueNoMutex(i, j, result);
115 //}
116 prev_output->SetValueNoMutex(i, j, result);
117 prev_input->SetValueNoMutex(i, j, input->ValueNoMutex(i, j));
118
119 }
120 }
121 }
122 }
123
124 input->ReleaseMutex();
125 output->ReleaseMutex();
126
127 output->SetDataTime(data->DataTime());
128}
Note: See TracBrowser for help on using the repository browser.