Revision 7ed40bef src/timestamped_list.cpp
| src/timestamped_list.cpp | ||
|---|---|---|
| 33 | 33 |
using namespace humotion; |
| 34 | 34 |
|
| 35 | 35 |
TimestampedList::TimestampedList(unsigned int s){
|
| 36 |
//initialize the list to its desired size:
|
|
| 36 |
//initialize the list to its desired size:
|
|
| 37 | 37 |
TimestampedFloat now; |
| 38 | 38 |
tsf_list.resize(s, now); |
| 39 | 39 |
} |
| ... | ... | |
| 73 | 73 |
} |
| 74 | 74 |
|
| 75 | 75 |
void TimestampedList::insert(Timestamp timestamp, float val){
|
| 76 |
//erase first element:
|
|
| 76 |
//erase first element:
|
|
| 77 | 77 |
tsf_list.pop_front(); |
| 78 | 78 |
tsf_list.push_back(TimestampedFloat(timestamp, val)); |
| 79 | 79 |
//printf("insert [%5.3f] = %5.1f\n",timestamp,val);
|
| ... | ... | |
| 91 | 91 |
} |
| 92 | 92 |
|
| 93 | 93 |
timestamped_float_list_t::iterator it = tsf_list.end(); |
| 94 |
it--;
|
|
| 95 |
return it->value;
|
|
| 94 |
it--;
|
|
| 95 |
return it->value;
|
|
| 96 | 96 |
} |
| 97 | 97 |
|
| 98 | 98 |
float TimestampedList::get_interpolated_value(Timestamp target_ts){
|
| ... | ... | |
| 102 | 102 |
mutex::scoped_lock scoped_lock(access_mutex); |
| 103 | 103 |
|
| 104 | 104 |
///target_ts -= 0.001 * 4; |
| 105 |
|
|
| 105 |
|
|
| 106 | 106 |
TimestampedFloat previous; |
| 107 | 107 |
//printf("> latency %3.2fms\n", (Timestamped().to_seconds() - target_ts.to_seconds())*1000.0);
|
| 108 | 108 |
|
| 109 | 109 |
for ( timestamped_float_list_t::iterator it = tsf_list.begin(); it != tsf_list.end(); ++it ){
|
| 110 |
if (it->timestamp >= target_ts){
|
|
| 111 |
//ok found close target
|
|
| 110 |
if (it->timestamp >= target_ts){
|
|
| 111 |
//ok found close target
|
|
| 112 | 112 |
if (it == tsf_list.begin()){
|
| 113 |
//no preceding element
|
|
| 113 |
//no preceding element
|
|
| 114 | 114 |
printf("> warning, timestamp %6.3f smaller than first element %6.3f in timestamped list. this should not happen (increase ts buffer?)\n",target_ts.to_seconds(),tsf_list.begin()->timestamp.to_seconds());
|
| 115 |
return it->value;
|
|
| 116 |
}else{
|
|
| 117 |
//do interpolation
|
|
| 118 |
return interpolate(*it, previous, target_ts);
|
|
| 119 |
}
|
|
| 120 |
}
|
|
| 121 |
previous = *it;
|
|
| 122 |
}
|
|
| 123 |
|
|
| 124 |
//we reached the end, return the last value
|
|
| 115 |
return it->value;
|
|
| 116 |
}else{
|
|
| 117 |
//do interpolation
|
|
| 118 |
return interpolate(*it, previous, target_ts);
|
|
| 119 |
}
|
|
| 120 |
}
|
|
| 121 |
previous = *it;
|
|
| 122 |
}
|
|
| 123 |
|
|
| 124 |
//we reached the end, return the last value
|
|
| 125 | 125 |
printf("> warning: found no timestamp bigger than %f in timestamped list...\n", target_ts.to_seconds());
|
| 126 |
printf(" this should not happen as images will always be behind\n");
|
|
| 126 |
printf(" this should not happen as images will always be behind\n");
|
|
| 127 | 127 |
printf(" the motor data. returning most recent value (ts=%f)\n", previous.timestamp.to_seconds());
|
| 128 |
|
|
| 129 |
return previous.value;
|
|
| 128 |
|
|
| 129 |
return previous.value;
|
|
| 130 | 130 |
} |
| 131 | 131 |
|
| 132 | 132 |
float TimestampedList::interpolate(TimestampedFloat a, TimestampedFloat b, Timestamp timestamp){
|
| 133 |
//a->timestamp < timestamp <= b->timestamp
|
|
| 133 |
//a->timestamp < timestamp <= b->timestamp
|
|
| 134 | 134 |
double dist_a = timestamp.to_seconds() - a.timestamp.to_seconds(); |
| 135 | 135 |
double dist_b = b.timestamp.to_seconds() - timestamp.to_seconds(); |
| 136 |
double dist = dist_a + dist_b;
|
|
| 137 |
|
|
| 136 |
double dist = dist_a + dist_b;
|
|
| 137 |
|
|
| 138 | 138 |
float interpolation = a.value + (dist_a / dist) * (b.value - a.value); |
| 139 |
return interpolation;
|
|
| 139 |
return interpolation;
|
|
| 140 | 140 |
} |
| 141 | 141 |
|
| 142 | 142 |
|
| 143 | 143 |
///tests |
| 144 | 144 |
void TimestampedList::run_tests(){
|
| 145 |
int size = 10;
|
|
| 146 |
TimestampedList list(size);
|
|
| 147 |
|
|
| 148 |
for(int i=0; i<size; i++){
|
|
| 145 |
int size = 10;
|
|
| 146 |
TimestampedList list(size);
|
|
| 147 |
|
|
| 148 |
for(int i=0; i<size; i++){
|
|
| 149 | 149 |
list.insert(Timestamp(i * 100.0,0), i*10.0); |
| 150 |
}
|
|
| 151 |
|
|
| 152 |
//test algorithm:
|
|
| 153 |
|
|
| 154 |
//test exact match:
|
|
| 155 |
for(int i=0; i<size; i++){
|
|
| 150 |
}
|
|
| 151 |
|
|
| 152 |
//test algorithm:
|
|
| 153 |
|
|
| 154 |
//test exact match:
|
|
| 155 |
for(int i=0; i<size; i++){
|
|
| 156 | 156 |
Timestamp ts(i*100.0,0); |
| 157 | 157 |
printf("> testing get_interpolated_value(%f) == %f (value read back = %f)\n",ts.to_seconds(), i*10.0, list.get_interpolated_value(ts));
|
| 158 | 158 |
assert( list.get_interpolated_value(ts) == i*10.0); |
| 159 |
}
|
|
| 160 |
printf("passed test 1\n");
|
|
| 161 |
|
|
| 159 |
}
|
|
| 160 |
printf("passed test 1\n");
|
|
| 161 |
|
|
| 162 | 162 |
assert(list.get_interpolated_value(Timestamp(0.0,0)) == 0.0); |
| 163 | 163 |
assert(list.get_interpolated_value(Timestamp(110.0,0)) == 11.0); |
| 164 | 164 |
assert(list.get_interpolated_value(Timestamp(150.0,0)) == 15.0); |
| 165 | 165 |
assert(list.get_interpolated_value(Timestamp(999990.0,0)) == 90.0); |
| 166 |
|
|
| 167 |
printf("passed test 2\n");
|
|
| 168 |
|
|
| 166 |
|
|
| 167 |
printf("passed test 2\n");
|
|
| 168 |
|
|
| 169 | 169 |
list.insert(Timestamp(1000.0 ,0), 200.0); |
| 170 | 170 |
list.insert(Timestamp(1300.0, 0), -100.0); |
| 171 | 171 |
assert(list.get_interpolated_value(Timestamp(1100, 0)) == 100.0); |
| 172 | 172 |
assert(list.get_interpolated_value(Timestamp(1200, 0)) == 0.0); |
| 173 | 173 |
assert(list.get_interpolated_value(Timestamp(1300, 0)) == -100.0); |
| 174 | 174 |
assert(list.get_interpolated_value(Timestamp(1250, 0)) == -50.0); |
| 175 |
|
|
| 176 |
printf("passed test 3\n");
|
|
| 177 |
exit(0);
|
|
| 175 |
|
|
| 176 |
printf("passed test 3\n");
|
|
| 177 |
exit(0);
|
|
| 178 | 178 |
} |
Also available in: Unified diff