// 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; }