// calculate the mean and the mean norm.
void inline calculate_mean(const std::vector<cv::Point_<float> >& offsets,
cv::Point_<float>* mean, float* mean_norm ) {
CHECK_GT(offsets.size(), 0);
// calculate the mean and the mean norm.
cv::Point_<float> mean_offset(0,0);
float m_norm = 0;
for(int i=0; i < offsets.size(); i++) {
const cv::Point_<float>& offset = offsets[i];
m_norm += cv::norm(offset);
mean_offset += offset;
}
mean_offset.x /= offsets.size();
mean_offset.y /= offsets.size();
m_norm /= offsets.size();
*mean = mean_offset;
*mean_norm = m_norm;
}
int RBS(const exlcm_sync_t * msg)
/*****************************************************************************
* Input :
* Output :
* Function :
******************************************************************************/
{
struct timeval tv; // create timestruct
exlcm_sync_t my_data; // create a struct to LCM
switch(msg->operation) // statemachine of handling LCM packets
{
case BEACONSENDER: // case when beacon message is recieved
tv = get_localrealtime(); // get timestamp for received packets
rbs.realtime = (double)tv.tv_sec + (double)tv.tv_usec / (double)1000000; // calculate time in sec
my_data = construct_msg(NODE,ALL_NODES,RECIEVER_TO_RECIEVER,rbs.realtime); // define send struct
exlcm_sync_t_publish(lcm, SYNC_CHANNEL, &my_data); // publish the struct
print_info(msg); // print all approprivately informations
break;
case RECIEVER_TO_RECIEVER: // case when nodes inform each other
if(msg->sender != NODE) // ONly run RBS for others nodes
{
//printf("---------------- RECEIVER TO REVEVER SENDER ------------------------\n");
handle_linkedlist(msg); // handle list with packets
if(inserting(msg, REMOTETIME, sample_remoterealtime(msg))) // inserting time for remote nodes
printf("FAILURE IN INSERTING REMOTETIME \n");
if(getting(msg, DATASIZE, ZERO) == ZERO & rbs.nodes[msg->sender].reach_max == ZERO) // this is also run the first time
{
rbs.realtime_0 = rbs.realtime; // first time for current stored
if(inserting(msg, LOCALTIME, 0)) // store realtime in nodes
printf("FAILURE IN INSERTING REMOTETIME first run \n");
}
else
{
if(inserting(msg, SCALETIME, rbs.realtime - rbs.realtime_0)) // store scalerealtime in nodes
printf("FAILURE IN INSERTING SCALETIME \n");
}
if(inserting(msg, LOCALTIME, rbs.realtime)) // store realtime in nodes
printf("FAILURE IN INSERTING REALTIME \n");
if(inserting(msg, OFFSET, sample_remoterealtime(msg) - rbs.realtime)) // store offset (remote - current)
printf("FAILURE IN INSERTING OFFSET \n");
mean_offset(rbs.nodes[msg->sender].end); // calculate meanoffset
if(rbs.nodes[msg->sender].end->next != NULL) // skip first sample
leastsquarelinarregression(rbs.nodes[msg->sender].end); // calculate leastsquareregression
//printf("----------------END RECEIVER TO REVEVER SENDER ------------------------\n");
print_info(msg); // print all approprivately informations
rbs.nodes[msg->sender].n++; // count x increment
}
break;
default:
break;
}
return 0;
}
