Revision 32327f15 src/timestamped_list.cpp

View differences:

src/timestamped_list.cpp
57 57 
    *target = tsf_list;
58 58 
}
59 59 

  		




  60
  
  
    
double TimestampedList::get_last_timestamp(){


  		




  
  60
  
    
Timestamp TimestampedList::get_last_timestamp(){


  		




  61
  61
  
    
    //lock the tsf_list for this access. by doing this we assure


  		




  62
  62
  
    
    //that no other thread accessing this element can diturb the


  		




  63
  63
  
    
    //following atomic instructions:


  		




  64
  64
  
    
    mutex::scoped_lock scoped_lock(access_mutex);


  		




  65
  65
  
    

  		




  66
  66
  
    
    if (tsf_list.empty()){


  		




  67
  
  
    
        return 0.0;


  		




  
  67
  
    
        return Timestamp(0,0);


  		




  68
  68
  
    
    }


  		




  69
  69
  
    

  		




  70
  70
  
    
    timestamped_float_list_t::iterator it = tsf_list.end();


  		




  ......




  72
  72
  
    
    return it->timestamp;


  		




  73
  73
  
    
}


  		




  74
  74
  
    

  		




  75
  
  
    
void TimestampedList::insert(double timestamp, float val){


  		




  
  75
  
    
void TimestampedList::insert(Timestamp timestamp, float val){


  		




  76
  76
  
    
	//erase first element:


  		




  77
  77
  
    
    tsf_list.pop_front();


  		




  78
  78
  
    
    tsf_list.push_back(TimestampedFloat(timestamp, val));


  		




  ......




  86
  86
  
    
    mutex::scoped_lock scoped_lock(access_mutex);


  		




  87
  87
  
    

  		




  88
  88
  
    
    if (tsf_list.empty()){


  		




  
  89
  
    
        printf("> WARNING: requested newest value from empty list, returning 0.0\n");


  		




  89
  90
  
    
        return 0.0;


  		




  90
  91
  
    
    }


  		




  91
  92
  
    

  		




  ......




  94
  95
  
    
	return it->value;


  		




  95
  96
  
    
}


  		




  96
  97
  
    

  		




  97
  
  
    
float TimestampedList::get_interpolated_value(double target_ts){


  		




  
  98
  
    
float TimestampedList::get_interpolated_value(Timestamp target_ts){


  		




  98
  99
  
    
    //lock the tsf_list for this access. by doing this we assure


  		




  99
  100
  
    
    //that no other thread accessing this element can diturb the


  		




  100
  101
  
    
    //following atomic instructions:


  		




  ......




  103
  104
  
    
    ///target_ts -= 0.001 * 4;


  		




  104
  105
  
    
	


  		




  105
  106
  
    
    TimestampedFloat previous;


  		




  106
  
  
    
    //printf("> latency %3.2fms\n", (TimestampedFloat::get_timestamp() - target_ts)*1000.0);


  		




  
  107
  
    
    //printf("> latency %3.2fms\n", (Timestamped().to_seconds() - target_ts.to_seconds())*1000.0);


  		




  107
  108
  
    

  		




  108
  109
  
    
    for ( timestamped_float_list_t::iterator it = tsf_list.begin(); it != tsf_list.end(); ++it ){


  		




  109
  110
  
    
		if (it->timestamp >= target_ts){


  		




  110
  111
  
    
			//ok found close target


  		




  111
  112
  
    
            if (it == tsf_list.begin()){


  		




  112
  113
  
    
				//no preceding element


  		




  113
  
  
    
                printf("> warning, timestamp %6.3f smaller than first element %6.3f in timestamped list. this should not happen (increase ts buffer?)\n",target_ts,tsf_list.begin()->timestamp);


  		




  
  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());


  		




  114
  115
  
    
				return it->value;


  		




  115
  116
  
    
			}else{


  		




  116
  117
  
    
				//do interpolation


  		




  ......




  119
  120
  
    
		}


  		




  120
  121
  
    
		previous = *it;


  		




  121
  122
  
    
	}


  		




  
  123
  
    

  		




  122
  124
  
    
	//we reached the end, return the last value


  		




  123
  
  
    
	printf("> warning: found no timestamp bigger than %f in timestamped list...\n", target_ts);


  		




  
  125
  
    
    printf("> warning: found no timestamp bigger than %f in timestamped list...\n", target_ts.to_seconds());


  		




  124
  126
  
    
	printf("           this should not happen as images will always be behind\n");


  		




  125
  
  
    
	printf("           the motor data. returning most recent value (ts=%f)\n", previous.timestamp);


  		




  
  127
  
    
    printf("           the motor data. returning most recent value (ts=%f)\n", previous.timestamp.to_seconds());


  		




  126
  128
  
    
	


  		




  127
  129
  
    
	return previous.value;


  		




  128
  130
  
    
}


  		




  129
  131
  
    

  		




  130
  
  
    
float TimestampedList::interpolate(TimestampedFloat a, TimestampedFloat b, double timestamp){


  		




  
  132
  
    
float TimestampedList::interpolate(TimestampedFloat a, TimestampedFloat b, Timestamp timestamp){


  		




  131
  133
  
    
	//a->timestamp < timestamp <= b->timestamp


  		




  132
  
  
    
    double dist_a = timestamp - a.timestamp;


  		




  133
  
  
    
	double dist_b = b.timestamp - timestamp;


  		




  
  134
  
    
    double dist_a = timestamp.to_seconds() - a.timestamp.to_seconds();


  		




  
  135
  
    
    double dist_b = b.timestamp.to_seconds() - timestamp.to_seconds();


  		




  134
  136
  
    
	double dist   = dist_a + dist_b;


  		




  135
  137
  
    
	


  		




  136
  138
  
    
    float interpolation = a.value + (dist_a / dist) * (b.value - a.value);


  		




  ......




  144
  146
  
    
	TimestampedList list(size);


  		




  145
  147
  
    
	


  		




  146
  148
  
    
	for(int i=0; i<size; i++){


  		




  147
  
  
    
		list.insert(i * 100.0, i*10.0);


  		




  
  149
  
    
        list.insert(Timestamp(i * 100.0,0), i*10.0);


  		




  148
  150
  
    
	}


  		




  149
  151
  
    
	


  		




  150
  152
  
    
	//test algorithm:


  		




  151
  153
  
    
	


  		




  152
  154
  
    
	//test exact match:


  		




  153
  155
  
    
	for(int i=0; i<size; i++){


  		




  154
  
  
    
		printf("> testing get_interpolated_value(%f) == %f (value read back = %f)\n",i*100.0, i*10.0, list.get_interpolated_value(i*100.0));


  		




  155
  
  
    
		assert( list.get_interpolated_value(i*100.0) == i*10.0);


  		




  
  156
  
    
        Timestamp ts(i*100.0,0);


  		




  
  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
  
    
        assert( list.get_interpolated_value(ts) == i*10.0);


  		




  156
  159
  
    
	}


  		




  157
  160
  
    
	printf("passed test 1\n");


  		




  158
  161
  
    
	


  		




  159
  
  
    
	assert(list.get_interpolated_value(0.0) == 0.0);


  		




  160
  
  
    
	assert(list.get_interpolated_value(110.0) == 11.0);


  		




  161
  
  
    
	assert(list.get_interpolated_value(150.0) == 15.0);


  		




  162
  
  
    
	assert(list.get_interpolated_value(999990.0) == 90.0);


  		




  
  162
  
    
    assert(list.get_interpolated_value(Timestamp(0.0,0)) == 0.0);


  		




  
  163
  
    
    assert(list.get_interpolated_value(Timestamp(110.0,0)) == 11.0);


  		




  
  164
  
    
    assert(list.get_interpolated_value(Timestamp(150.0,0)) == 15.0);


  		




  
  165
  
    
    assert(list.get_interpolated_value(Timestamp(999990.0,0)) == 90.0);


  		




  163
  166
  
    
	


  		




  164
  167
  
    
	printf("passed test 2\n");


  		




  165
  168
  
    
	


  		




  166
  
  
    
	list.insert(1000.0, 200.0);


  		




  167
  
  
    
	list.insert(1300.0, -100.0);


  		




  168
  
  
    
	assert(list.get_interpolated_value(1100) == 100.0);


  		




  169
  
  
    
	assert(list.get_interpolated_value(1200) == 0.0);


  		




  170
  
  
    
	assert(list.get_interpolated_value(1300) == -100.0);


  		




  171
  
  
    
	assert(list.get_interpolated_value(1250) == -50.0);


  		




  
  169
  
    
    list.insert(Timestamp(1000.0 ,0), 200.0);


  		




  
  170
  
    
    list.insert(Timestamp(1300.0, 0), -100.0);


  		




  
  171
  
    
    assert(list.get_interpolated_value(Timestamp(1100, 0)) == 100.0);


  		




  
  172
  
    
    assert(list.get_interpolated_value(Timestamp(1200, 0)) == 0.0);


  		




  
  173
  
    
    assert(list.get_interpolated_value(Timestamp(1300, 0)) == -100.0);


  		




  
  174
  
    
    assert(list.get_interpolated_value(Timestamp(1250, 0)) == -50.0);


  		




  172
  175
  
    
	


  		




  173
  176
  
    
	printf("passed test 3\n");


  		




  174
  177
  
    
	exit(0);


  		












Also available in:  Unified diff