pi/cv_networking_pipeline
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

init repo

Browse Source
This commit is contained in:
PodmogilnyjIvan
2022-01-14 06:08:08 -08:00
parent f43b9c4732
commit 6e4c635177
1 changed files with 541 additions and 285 deletions
Download Patch File Download Diff File Expand all files Collapse all files

840
gst_get_ndi_v6_7.cpp
View File

@@ -15,6 +15,7 @@
#include "osc/OscOutboundPacketStream.h"
#include "ip/UdpSocket.h"
#include <Converter.h>
#include "System.h"
using namespace std::chrono;
@@ -88,6 +89,62 @@ vector<float> find_mode(const vector<vector<float>>& vec_of_rot_axes) {
return vec_of_rot_axes[index];
}
void send_euler_to_receiver_osc(vector<float> euler_angles, int counter = -1) {
// Euler angles are recieved in Radians.
euler_angles[0] *= 57.29;
euler_angles[1] *= 57.29;
euler_angles[2] *= 57.29;
std::string str;
char msg[40];
UdpTransmitSocket transmitSocket(IpEndpointName(ADDRESS, PORT));
char buffer[OUTPUT_BUFFER_SIZE];
osc::OutboundPacketStream p(buffer, OUTPUT_BUFFER_SIZE);
if (counter == -1) {
str = std::to_string(euler_angles[0]) + " " + std::to_string(euler_angles[1]) + " " + std::to_string(euler_angles[2]);
}
else {
str = std::to_string(euler_angles[0]) + " " + std::to_string(euler_angles[1]) + " " + std::to_string(euler_angles[2]) + " " + std::to_string(counter);
}
strcpy(msg, str.c_str());
p << osc::BeginBundleImmediate << osc::BeginMessage("/test3") << msg << osc::EndMessage << osc::EndBundle;
transmitSocket.Send(p.Data(), p.Size());
memset(msg, 0, 40);
};
cv::Mat axisVector2Rot(float theta, vector<float> v) {
cv::Mat Rot(3, 3, CV_32F, 0.0);
float c = cos(theta);
float s = sin(theta);
float t = 1 - c;
Rot.at<float>(0, 0) = t * v[0] * v[0] + c;
Rot.at<float>(0, 1) = t * v[0] * v[1] - v[2] * s;
Rot.at<float>(0, 2) = t * v[0] * v[2] + v[1] * c;
Rot.at<float>(1, 0) = t * v[0] * v[1] + v[2] * s;
Rot.at<float>(1, 1) = t * v[1] * v[1] + c;
Rot.at<float>(1, 2) = t * v[1] * v[2] - v[0] * s;
Rot.at<float>(2, 0) = t * v[0] * v[2] - v[1] * s;
Rot.at<float>(2, 1) = t * v[1] * v[2] + v[0] * s;
Rot.at<float>(2, 2) = t * v[2] * v[2] + c;
return Rot;
};
vector<float> axisVector2Euler(float theta, vector<float> axis) {
vector<float> euler_angles;
cv::Mat Rot = axisVector2Rot(theta, axis);
euler_angles = ORB_SLAM3::Converter::toEuler(Rot);
return euler_angles;
};
GstFlowReturn new_preroll(GstAppSink *appsink, gpointer data) {
g_print ("Got preroll!\n");
return GST_FLOW_OK;
@@ -537,7 +594,7 @@ int main (int argc, char *argv[]) {
std::vector<cv::Mat> queue;
int cnt = 0;
const double time_step = 1.0;
const double time_step = 0.033;
double ts = 0;
char matrix_name[100];
vector<float> vec_of_deg, values;
@@ -560,16 +617,245 @@ int main (int argc, char *argv[]) {
bool zero_flag, mirror_flag = false;
float mirror_point = 0.0;
cnt = 0;
// FOR TESTING DO NOT DELETE!!!
while (true) {
cv::Mat frame;
char* buffer = nullptr;
//// FOR TESTING
//while (true) {
// EXTRACTING FRAME HERE.
{
std::lock_guard<std::mutex> guard(g_mutex);
if (frameQueue.size() > 0) {
frame = frameQueue.front();
frameQueue.pop_front();
//std::cout << "we have a frame to process..." << std::endl;
}
}
if (!frame.empty()) {
cvtColor(frame, frame, cv::COLOR_RGBA2RGB);
cv::Mat Tcw = SLAM.TrackMonocular(frame, ts, vector<ORB_SLAM3::IMU::Point>(), "");
cv::Mat Rot(3, 3, CV_32F, 0.0);
if (!Tcw.empty()) {
/*std::cout << std::endl << "Frame process succcess" << std::endl;
std::cout << Tcw << std::endl;*/
sprintf(matrix_name, "matrix%d", cnt);
extract_rot(Rot, Tcw);
/* TESTING CODE */
//// cout << "Extracted rotation matrix is: " << Rot;
//// Extract the degree and the vector from the rotation matrix.
//auto deg_vec = extract_deg(Rot); // returns a degree and a vector of rotation.
//float new_deg = -deg_vec.first * 57.29 - DIFF_TO_CENTER;
//vector<float> new_vec = deg_vec.second;
//std::cout << "Successfully created curr_deg and curr_vec" << endl;
//vec_of_deg.push_back(new_deg);
//vec_of_rot_axis.push_back(new_vec);
//j++;
//std::cout << "Pushed to the vectors. Line 207" << endl;
//// ---- II PART OF THE PROCESSING ----
//// TODO: II PART OF PROCESSING MIRRORED FIRST CHANGE, BUT NOT THE REST.
//// Если текущий градус больше epsilon = 5, то zero_flag = false
//// Can cause a problem, when accumulating values after turning on the zero_flag.
//// TODO: accum2 is full when the zero_flag enables, which is bad. work on that.
//if (zero_flag) {
// if ((vec_of_deg[j - 1] < -5 || vec_of_deg[j - 1] > 5) && accum2.size() == 5) {
// zero_flag = false;
// }
//}
//if (zero_flag) { std::cout << "Zero flag is: true" << endl; }
//else { cout << "Zero flag is: false" << endl; }
//// Если нет zero_flag, а в accum2 что-то есть, то опустошим его.
//if (!(zero_flag) && !accum2.empty()) { accum2 = {}; }
//// Сохраняем последние 5 значений векторов
//if (!zero_flag) {
// cout << "Line 211 ok..." << endl;
// if (accum.size() == 5) {
// cout << "Accum size = 5." << endl;
// accum.erase(accum.begin());
// cout << "Line 215 ok..." << endl;
// accum.push_back(vec_of_rot_axis[j - 1]);
// cout << "Line 217 ok..." << endl;
// }
// else {
// cout << "Accum size != 5." << endl;
// cout << "j is: " << j << " len of vec_of_rot_axis is: " << vec_of_rot_axis.size() << endl;
// accum.push_back(vec_of_rot_axis[j - 1]);
// cout << "Line 223 ok..." << endl;
// }
//}
//// Найдем элемент, который начал расти, а не убывать около нуля
//if (!zero_flag) {
// if (vec_of_deg[j - 1] > -5 && vec_of_deg[j - 1] < 5) {
// // Если нынешний элемент уже не меньше предыдущего, а предыдущая разность тоже около нуля, при этом абсолютная разность между градусами больше, чем 0.01
// if (abs(vec_of_deg[j - 1]) >= abs(vec_of_deg[j - 2]) && (abs(vec_of_deg[j - 2] - vec_of_deg[j - 3]) < 10) && (abs(vec_of_deg[j - 1] - vec_of_deg[j - 2]) > .3)) {
// zero_flag = true;
// cout << "Line 233 and 232 ok..." << endl;
// }
// // else {
// // zero_flag = false;
// // }
// }
//}
//cout << "Accum size is: " << accum.size() << endl;
//cout << "Accum2 size is: " << accum2.size() << endl;
//if (zero_flag) {
// // Если набрали 5 элементов
// cout << "Entered in zero_flag if..." << endl;
// cout << "Accum2.size() is: " << accum2.size() << endl;
// if (accum2.size() == 5 && accum.size() == 5) {
// // Имеем массивы векторов. Найдём их моды и сравним.
// cout << "Accum size: " << accum.size() << endl;
// cout << "Accum2 size: " << accum2.size() << endl;
// mode1 = find_mode(accum);
// mode2 = find_mode(accum2);
// cout << "Line 246 and 245 ok..." << endl;
// bool compar_res = mode1 == mode2;
// cout << "Line 250 ok..." << endl;
// // Если градусы около нуля, а значения векторов поменялись, то отражаем
// // Input data leave it as it as, but the output data has to be processed.
// if (!(compar_res)) {
// // Если мы нашли ту самую точку, то отразим точки, которые мы накопили, и прибавим к ним точку
// // отражения, а также изменим точку отражения, и изменим флаг mirror_flag = True
// cout << "Нашли ту самую точку!" << endl;
// // mirror_point += values[j-6];
// // cout << "Mirror point after: " << mirror_point << endl;
// cout << "Line 255 ok..." << endl;
// if (mirror_flag) {
// mirror_flag = false;
// }
// else {
// mirror_flag = true;
// }
// // for (int i = j-6; i < j-1; i++){
// // values[i] = -values[i] + mirror_point;
// // }
// // cout << "Lines 263 and 264 are ok" << "j is: " << j << endl;
// }
// accum2 = {};
// cout << "Making zero flag false..." << endl;
// zero_flag = false;
// }
// else {
// if (accum2.size() < 5) {
// accum2.push_back(vec_of_rot_axis[j - 1]);
// cout << "Line 274 ok..." << endl;
// }
// }
//}
//// Сохраняем значения...
//if (mirror_flag) {
// ; cout << "Mirror flag is on;" << " vec_of_deg size: " << vec_of_deg.size() << "; j is: " << j << endl;
// values.push_back(-vec_of_deg[j - 1] + mirror_point);
// // cout << "Line 281 ok..." << endl;
//}
//else {
// cout << "Mirror flag is off" << " vec_of_deg size: " << vec_of_deg.size() << "; j is: " << j << endl;
// values.push_back(vec_of_deg[j - 1]);
// // cout << "Line 284 ok..." << endl;
//}
//cout << "Processed value is: " << values[j - 1] << endl; cout << " " << endl;
//// --------- I PART OF THE PROCESSING ---------
//values[j-1] += skew1;
//float diff = (values[j-2] - values[j-1]);
//cout << "New deg is: " << new_deg << "Diff is: " << diff << endl;
//// Если разница больше 10, то скорее всего произошла потеря.
//if (abs(diff) > 10) {
// cout << "Diff is more than 10; Correcting... " << endl;
// values[j-1] += diff;
// skew1 += diff;
//}
//// --------- I PART OF THE PROCESSING ---------
//// Запись в файл.
////outfile << values[j - 1] << " " << new_vec[0] << " " << new_vec[1] << " " << new_vec[2] << " " << cnt << endl;
//// cout << "Successfully written to the file" << endl;
//// Sending in Radians
//vector<float> euler_angles = axisVector2Euler(values[j-1] / 57.29, new_vec);
//
//// UDP Sending in degrees.
//send_euler_to_receiver_osc(euler_angles, cnt);
//// ---- II PART OF THE PROCESSING ----
//curr_deg = new_deg;
//curr_vec = new_vec;
//number_of_detected_frames += 1;
}
else {
vector<float> euler_angles = { 0.0, 0.0, 0.0 };
send_euler_to_receiver_osc(euler_angles, cnt);
}
cnt++;
ts += time_step;
}
if (use_opencv_preview) {
if (!frame.empty()) {
cv::namedWindow("preview", 1);
cv::Mat edges;
cvtColor(frame, edges, cv::COLOR_BGR2BGRA);
cv::imshow("preview", frame);
}
cv::waitKey(30);
//cv::destroyAllWindows();
}
delete[] buffer;
}
// FOR TESTING
// --------------------------------- SLAM SYSTEM VARIABLES ---------------------------------
/* Let's do two steps outside the loop.*/
//for (int i = 1; i <= 2; i++) {
// //if (use_gui) {
// // cv::namedWindow("preview", 1);
// //}
// //else {
// // // cv::namedWindow("no preview", 1);
// //}
// cv::Mat frame;
// //char* buffer = nullptr;
// char* buffer = nullptr;
// // EXTRACTING FRAME HERE.
// {
// std::lock_guard<std::mutex> guard(g_mutex);
// //std::lock_guard<std::mutex> guard(g_mutex);
// if (frameQueue.size() > 0) {
// frame = frameQueue.front();
// frameQueue.pop_front();
@@ -580,324 +866,294 @@ int main (int argc, char *argv[]) {
// cv::Mat Tcw = SLAM.TrackMonocular(frame, ts, vector<ORB_SLAM3::IMU::Point>(), "");
// cv::Mat Rot(3, 3, CV_32F, 0.0);
// std::cout << Tcw << std::endl;
// if (!Tcw.empty()) {
// sprintf(matrix_name, "matrix%d", cnt);
// extract_rot(Rot, Tcw);
// // cout << "Extracted rotation matrix is: " << Rot;
// auto deg_vec = extract_deg(Rot);
// // QUESTION 2.
// curr_deg = -deg_vec.first * 57.29;
// // TODO: Invert curr_vec too. (put the minus sign to each element). (You can define the - operator fot the vector).
// curr_vec = deg_vec.second;
// cout << "Successfully created curr_deg and curr_vec" << endl;
// // LET'S DEFINE CONSTANT TO ZERO OUT THE START
// if (i == 1) {
// DIFF_TO_CENTER = 0.0;
// }
// vec_of_deg.push_back(curr_deg - DIFF_TO_CENTER);
// vec_of_rot_axis.push_back(curr_vec);
// values.push_back(curr_deg - DIFF_TO_CENTER);
// cout << "Successfully pushed to the vectors " << endl;
// //cout << curr_deg - DIFF_TO_CENTER << " " << curr_vec[0] << " " << curr_vec[1] << " " << curr_vec[2] << endl;
// // SEND THE RESULT THROUGH OSC
// //outfile << curr_deg - DIFF_TO_CENTER << " " << curr_vec[0] << " " << curr_vec[1] << " " << curr_vec[2] << endl;
// cout << "Successfully written to the file" << endl;
// j++;
// }
// cnt++;
// ts += time_step;
//}
//// FOR TESTING
// --------------------------------- SLAM SYSTEM VARIABLES ---------------------------------
/* Let's do two steps outside the loop.*/
for (int i = 1; i <= 2; i++) {
//if (use_gui) {
// cv::namedWindow("preview", 1);
//}
//else {
// // cv::namedWindow("no preview", 1);
//}
cv::Mat frame;
char* buffer = nullptr;
// EXTRACTING FRAME HERE.
{
std::lock_guard<std::mutex> guard(g_mutex);
if (frameQueue.size() > 0) {
frame = frameQueue.front();
frameQueue.pop_front();
std::cout << "we have a frame to process..." << std::endl;
}
}
cv::Mat Tcw = SLAM.TrackMonocular(frame, ts, vector<ORB_SLAM3::IMU::Point>(), "");
cv::Mat Rot(3, 3, CV_32F, 0.0);
std::cout << Tcw << std::endl;
if (!Tcw.empty()) {
sprintf(matrix_name, "matrix%d", cnt);
extract_rot(Rot, Tcw);
// cout << "Extracted rotation matrix is: " << Rot;
auto deg_vec = extract_deg(Rot);
// QUESTION 2.
curr_deg = -deg_vec.first * 57.29;
// TODO: Invert curr_vec too. (put the minus sign to each element). (You can define the - operator fot the vector).
curr_vec = deg_vec.second;
cout << "Successfully created curr_deg and curr_vec" << endl;
// LET'S DEFINE CONSTANT TO ZERO OUT THE START
if (i == 1) {
DIFF_TO_CENTER = 0.0;
}
vec_of_deg.push_back(curr_deg - DIFF_TO_CENTER);
vec_of_rot_axis.push_back(curr_vec);
values.push_back(curr_deg - DIFF_TO_CENTER);
cout << "Successfully pushed to the vectors " << endl;
//cout << curr_deg - DIFF_TO_CENTER << " " << curr_vec[0] << " " << curr_vec[1] << " " << curr_vec[2] << endl;
// SEND THE RESULT THROUGH OSC
//outfile << curr_deg - DIFF_TO_CENTER << " " << curr_vec[0] << " " << curr_vec[1] << " " << curr_vec[2] << endl;
cout << "Successfully written to the file" << endl;
j++;
}
cnt++;
ts += time_step;
}
while (true) {
//while (true) {
cv::Mat frame;
// cv::Mat frame;
char* buffer = nullptr;
// char* buffer = nullptr;
{
std::lock_guard<std::mutex> guard(g_mutex);
if (frameQueue.size() > 0) {
frame = frameQueue.front();
frameQueue.pop_front();
std::cout << "we have a frame to process..." << std::endl;
// {
// //std::lock_guard<std::mutex> guard(g_mutex);
// if (frameQueue.size() > 0) {
// frame = frameQueue.front();
// frameQueue.pop_front();
// std::cout << "we have a frame to process..." << std::endl;
if (!frame.empty()) {
// if (!frame.empty()) {
cv::Mat Tcw = SLAM.TrackMonocular(frame, ts, vector<ORB_SLAM3::IMU::Point>(), "");
cv::Mat Rot(3, 3, CV_32F, 0.0);
std::cout << Tcw << std::endl;
if (!Tcw.empty()) {
sprintf(matrix_name, "matrix%d", cnt);
extract_rot(Rot, Tcw);
// cout << "Extracted rotation matrix is: " << Rot;
// Extract the degree and the vector from the rotation matrix.
auto deg_vec = extract_deg(Rot); // returns a degree and a vector of rotation.
// cv::Mat Tcw = SLAM.TrackMonocular(frame, ts, vector<ORB_SLAM3::IMU::Point>(), "");
// cv::Mat Rot(3, 3, CV_32F, 0.0);
// std::cout << Tcw << std::endl;
// if (!Tcw.empty()) {
// sprintf(matrix_name, "matrix%d", cnt);
// extract_rot(Rot, Tcw);
// // cout << "Extracted rotation matrix is: " << Rot;
// // Extract the degree and the vector from the rotation matrix.
// auto deg_vec = extract_deg(Rot); // returns a degree and a vector of rotation.
float new_deg = -deg_vec.first * 57.29 - DIFF_TO_CENTER;
vector<float> new_vec = deg_vec.second;
cout << "Successfully created curr_deg and curr_vec" << endl;
// float new_deg = -deg_vec.first * 57.29 - DIFF_TO_CENTER;
// vector<float> new_vec = deg_vec.second;
// std::cout << "Successfully created curr_deg and curr_vec" << endl;
vec_of_deg.push_back(new_deg);
vec_of_rot_axis.push_back(new_vec);
j++;
cout << "Pushed to the vectors. Line 207" << endl;
// vec_of_deg.push_back(new_deg);
// vec_of_rot_axis.push_back(new_vec);
// j++;
// std::cout << "Pushed to the vectors. Line 207" << endl;
// ---- II PART OF THE PROCESSING ----
// // ---- II PART OF THE PROCESSING ----
// TODO: II PART OF PROCESSING MIRRORED FIRST CHANGE, BUT NOT THE REST.
// // TODO: II PART OF PROCESSING MIRRORED FIRST CHANGE, BUT NOT THE REST.
// Если текущий градус больше epsilon = 5, то zero_flag = false
// Can cause a problem, when accumulating values after turning on the zero_flag.
// TODO: accum2 is full when the zero_flag enables, which is bad. work on that.
if (zero_flag) {
if ((vec_of_deg[j - 1] < -5 || vec_of_deg[j - 1] > 5) && accum2.size() == 5) {
zero_flag = false;
}
}
if (zero_flag) { cout << "Zero flag is: true" << endl; }
else { cout << "Zero flag is: false" << endl; }
// Если нет zero_flag, а в accum2 что-то есть, то опустошим его.
if (!(zero_flag) && !accum2.empty()) { accum2 = {}; }
// Сохраняем последние 5 значений векторов
if (!zero_flag) {
cout << "Line 211 ok..." << endl;
if (accum.size() == 5) {
cout << "Accum size = 5." << endl;
accum.erase(accum.begin());
cout << "Line 215 ok..." << endl;
accum.push_back(vec_of_rot_axis[j - 1]);
cout << "Line 217 ok..." << endl;
}
else {
cout << "Accum size != 5." << endl;
cout << "j is: " << j << " len of vec_of_rot_axis is: " << vec_of_rot_axis.size() << endl;
accum.push_back(vec_of_rot_axis[j - 1]);
cout << "Line 223 ok..." << endl;
}
}
// Найдем элемент, который начал расти, а не убывать около нуля
if (!zero_flag) {
if (vec_of_deg[j - 1] > -5 && vec_of_deg[j - 1] < 5) {
// Если нынешний элемент уже не меньше предыдущего, а предыдущая разность тоже около нуля, при этом абсолютная разность между градусами больше, чем 0.01
if (abs(vec_of_deg[j - 1]) >= abs(vec_of_deg[j - 2]) && (abs(vec_of_deg[j - 2] - vec_of_deg[j - 3]) < 10) && (abs(vec_of_deg[j - 1] - vec_of_deg[j - 2]) > .3)) {
zero_flag = true;
cout << "Line 233 and 232 ok..." << endl;
}
// else {
// // Если текущий градус больше epsilon = 5, то zero_flag = false
// // Can cause a problem, when accumulating values after turning on the zero_flag.
// // TODO: accum2 is full when the zero_flag enables, which is bad. work on that.
// if (zero_flag) {
// if ((vec_of_deg[j - 1] < -5 || vec_of_deg[j - 1] > 5) && accum2.size() == 5) {
// zero_flag = false;
// }
}
}
cout << "Accum size is: " << accum.size() << endl;
cout << "Accum2 size is: " << accum2.size() << endl;
if (zero_flag) {
// Если набрали 5 элементов
cout << "Entered in zero_flag if..." << endl;
cout << "Accum2.size() is: " << accum2.size() << endl;
if (accum2.size() == 5 && accum.size() == 5) {
// Имеем массивы векторов. Найдём их моды и сравним.
cout << "Accum size: " << accum.size() << endl;
cout << "Accum2 size: " << accum2.size() << endl;
mode1 = find_mode(accum);
mode2 = find_mode(accum2);
cout << "Line 246 and 245 ok..." << endl;
bool compar_res = mode1 == mode2;
cout << "Line 250 ok..." << endl;
// Если градусы около нуля, а значения векторов поменялись, то отражаем
// Input data leave it as it as, but the output data has to be processed.
if (!(compar_res)) {
// Если мы нашли ту самую точку, то отразим точки, которые мы накопили, и прибавим к ним точку
// отражения, а также изменим точку отражения, и изменим флаг mirror_flag = True
cout << "Нашли ту самую точку!" << endl;
// mirror_point += values[j-6];
// cout << "Mirror point after: " << mirror_point << endl;
cout << "Line 255 ok..." << endl;
if (mirror_flag) {
mirror_flag = false;
}
else {
mirror_flag = true;
}
// for (int i = j-6; i < j-1; i++){
// values[i] = -values[i] + mirror_point;
// }
// cout << "Lines 263 and 264 are ok" << "j is: " << j << endl;
}
accum2 = {};
cout << "Making zero flag false..." << endl;
zero_flag = false;
}
else {
if (accum2.size() < 5) {
accum2.push_back(vec_of_rot_axis[j - 1]);
cout << "Line 274 ok..." << endl;
}
}
}
// if (zero_flag) { std::cout << "Zero flag is: true" << endl; }
// else { cout << "Zero flag is: false" << endl; }
// // Если нет zero_flag, а в accum2 что-то есть, то опустошим его.
// if (!(zero_flag) && !accum2.empty()) { accum2 = {}; }
// Сохраняем значения...
if (mirror_flag) {
; cout << "Mirror flag is on;" << " vec_of_deg size: " << vec_of_deg.size() << "; j is: " << j << endl;
values.push_back(-vec_of_deg[j - 1] + mirror_point);
// cout << "Line 281 ok..." << endl;
}
else {
cout << "Mirror flag is off" << " vec_of_deg size: " << vec_of_deg.size() << "; j is: " << j << endl;
values.push_back(vec_of_deg[j - 1]);
// cout << "Line 284 ok..." << endl;
}
cout << "Processed value is: " << values[j - 1] << endl; cout << " " << endl;
// --------- I PART OF THE PROCESSING ---------
// values[j-1] += skew1;
// float diff = (values[j-2] - values[j-1]);
// cout << "New deg is: " << new_deg << "Diff is: " << diff << endl;
//
//
// // Если разница больше 10, то скорее всего произошла потеря.
// if (abs(diff) > 10) {
// cout << "Diff is more than 10; Correcting... " << endl;
// values[j-1] += diff;
// skew1 += diff;
// // Сохраняем последние 5 значений векторов
// if (!zero_flag) {
// cout << "Line 211 ok..." << endl;
// if (accum.size() == 5) {
// cout << "Accum size = 5." << endl;
// accum.erase(accum.begin());
// cout << "Line 215 ok..." << endl;
// accum.push_back(vec_of_rot_axis[j - 1]);
// cout << "Line 217 ok..." << endl;
// }
// --------- I PART OF THE PROCESSING ---------
// else {
// cout << "Accum size != 5." << endl;
// cout << "j is: " << j << " len of vec_of_rot_axis is: " << vec_of_rot_axis.size() << endl;
// accum.push_back(vec_of_rot_axis[j - 1]);
// cout << "Line 223 ok..." << endl;
// }
// }
// // Найдем элемент, который начал расти, а не убывать около нуля
// if (!zero_flag) {
// if (vec_of_deg[j - 1] > -5 && vec_of_deg[j - 1] < 5) {
// // Если нынешний элемент уже не меньше предыдущего, а предыдущая разность тоже около нуля, при этом абсолютная разность между градусами больше, чем 0.01
// if (abs(vec_of_deg[j - 1]) >= abs(vec_of_deg[j - 2]) && (abs(vec_of_deg[j - 2] - vec_of_deg[j - 3]) < 10) && (abs(vec_of_deg[j - 1] - vec_of_deg[j - 2]) > .3)) {
// zero_flag = true;
// cout << "Line 233 and 232 ok..." << endl;
// Запись в файл.
//outfile << values[j - 1] << " " << new_vec[0] << " " << new_vec[1] << " " << new_vec[2] << " " << cnt << endl;
// cout << "Successfully written to the file" << endl;
// }
// // else {
// // zero_flag = false;
// // }
// }
// }
// Выполнить отправку в протокол OSC.
//cv::Vec3d res(1., 1., 1.);
//std::cout << "defined Vector is: " << res[0] << res[1] << res[2] << std::endl;
std::cout << "message received!" << std::endl;
// cout << "Accum size is: " << accum.size() << endl;
// cout << "Accum2 size is: " << accum2.size() << endl;
// if (zero_flag) {
// // Если набрали 5 элементов
// cout << "Entered in zero_flag if..." << endl;
// cout << "Accum2.size() is: " << accum2.size() << endl;
// if (accum2.size() == 5 && accum.size() == 5) {
// // Имеем массивы векторов. Найдём их моды и сравним.
// cout << "Accum size: " << accum.size() << endl;
// cout << "Accum2 size: " << accum2.size() << endl;
// mode1 = find_mode(accum);
// mode2 = find_mode(accum2);
// cout << "Line 246 and 245 ok..." << endl;
// UDP Variables
std::string str;
char msg[40];
UdpTransmitSocket transmitSocket(IpEndpointName(ADDRESS, PORT));
// bool compar_res = mode1 == mode2;
// cout << "Line 250 ok..." << endl;
// // Если градусы около нуля, а значения векторов поменялись, то отражаем
// // Input data leave it as it as, but the output data has to be processed.
// if (!(compar_res)) {
// // Если мы нашли ту самую точку, то отразим точки, которые мы накопили, и прибавим к ним точку
// // отражения, а также изменим точку отражения, и изменим флаг mirror_flag = True
// cout << "Нашли ту самую точку!" << endl;
// // mirror_point += values[j-6];
// // cout << "Mirror point after: " << mirror_point << endl;
// cout << "Line 255 ok..." << endl;
char buffer[OUTPUT_BUFFER_SIZE];
osc::OutboundPacketStream p(buffer, OUTPUT_BUFFER_SIZE);
// if (mirror_flag) {
// mirror_flag = false;
// }
// else {
// mirror_flag = true;
// }
str = std::to_string(values[j-1]) + " " + std::to_string(new_vec[0]) + " " + std::to_string(new_vec[1]) + " " + std::to_string(new_vec[2]) + " " + std::to_string(cnt);
strcpy(msg, str.c_str());
// // for (int i = j-6; i < j-1; i++){
// // values[i] = -values[i] + mirror_point;
// // }
// // cout << "Lines 263 and 264 are ok" << "j is: " << j << endl;
std::cout << "Message filled" << std::endl;
p << osc::BeginBundleImmediate
<< osc::BeginMessage("/test3") << msg << osc::EndMessage
/* << osc::BeginMessage("/test2")
<< true << 24 << (float)10.8 << "world" << osc::EndMessage*/
<< osc::EndBundle;
std::cout << "Bundle filled" << std::endl;
transmitSocket.Send(p.Data(), p.Size());
std::cout << "Message sent!" << std::endl;
memset(msg, 0, 40);
//std::cout << "Memory freed" << std::endl;
// ---- II PART OF THE PROCESSING ----
curr_deg = new_deg;
curr_vec = new_vec;
number_of_detected_frames += 1;
}
else {
// UDP Variables
std::string str;
char msg[40];
UdpTransmitSocket transmitSocket(IpEndpointName(ADDRESS, PORT));
char buffer[OUTPUT_BUFFER_SIZE];
osc::OutboundPacketStream p(buffer, OUTPUT_BUFFER_SIZE);
str = "0 0 0 0" + std::to_string(cnt);
strcpy(msg, str.c_str());
p << osc::BeginBundleImmediate
<< osc::BeginMessage("/test3") << msg << osc::EndMessage
/* << osc::BeginMessage("/test2")
<< true << 24 << (float)10.8 << "world" << osc::EndMessage*/
<< osc::EndBundle;
transmitSocket.Send(p.Data(), p.Size());
std::cout << "Message sent!" << std::endl;
memset(msg, 0, 40);
}
cnt++;
ts += time_step;
}
}
else {
//std::cout << "Don't have any frames yet ..." << std::endl;
//std::cout << "";
}
}
// }
// accum2 = {};
// cout << "Making zero flag false..." << endl;
// zero_flag = false;
// }
// else {
// if (accum2.size() < 5) {
// accum2.push_back(vec_of_rot_axis[j - 1]);
// cout << "Line 274 ok..." << endl;
// }
// }
// }
delete[] buffer;
}
// // Сохраняем значения...
// if (mirror_flag) {
// ; cout << "Mirror flag is on;" << " vec_of_deg size: " << vec_of_deg.size() << "; j is: " << j << endl;
// values.push_back(-vec_of_deg[j - 1] + mirror_point);
// // cout << "Line 281 ok..." << endl;
// }
// else {
// cout << "Mirror flag is off" << " vec_of_deg size: " << vec_of_deg.size() << "; j is: " << j << endl;
// values.push_back(vec_of_deg[j - 1]);
// // cout << "Line 284 ok..." << endl;
// }
// cout << "Processed value is: " << values[j - 1] << endl; cout << " " << endl;
std::printf("End of video\n");
// Stop all threads
SLAM.Shutdown();
// // --------- I PART OF THE PROCESSING ---------
// // values[j-1] += skew1;
// // float diff = (values[j-2] - values[j-1]);
// // cout << "New deg is: " << new_deg << "Diff is: " << diff << endl;
// //
// //
// // // Если разница больше 10, то скорее всего произошла потеря.
// // if (abs(diff) > 10) {
// // cout << "Diff is more than 10; Correcting... " << endl;
// // values[j-1] += diff;
// // skew1 += diff;
// // }
// // --------- I PART OF THE PROCESSING ---------
std::printf("Done.\n");
// // Запись в файл.
// //outfile << values[j - 1] << " " << new_vec[0] << " " << new_vec[1] << " " << new_vec[2] << " " << cnt << endl;
// // cout << "Successfully written to the file" << endl;
std::cout << "Number of detected frames: " << number_of_detected_frames << std::endl;
// // Выполнить отправку в протокол OSC.
// //cv::Vec3d res(1., 1., 1.);
// //std::cout << "defined Vector is: " << res[0] << res[1] << res[2] << std::endl;
// std::cout << "message received!" << std::endl;
// // UDP Variables
// std::string str;
// char msg[40];
// UdpTransmitSocket transmitSocket(IpEndpointName(ADDRESS, PORT));
// char buffer[OUTPUT_BUFFER_SIZE];
// osc::OutboundPacketStream p(buffer, OUTPUT_BUFFER_SIZE);
//
// str = std::to_string(values[j-1]) + " " + std::to_string(new_vec[0]) + " " + std::to_string(new_vec[1]) + " " + std::to_string(new_vec[2]) + " " + std::to_string(cnt);
// strcpy(msg, str.c_str());
// std::cout << "Message filled" << std::endl;
// p << osc::BeginBundleImmediate
// << osc::BeginMessage("/test3") << msg << osc::EndMessage
// /* << osc::BeginMessage("/test2")
// << true << 24 << (float)10.8 << "world" << osc::EndMessage*/
// << osc::EndBundle;
// std::cout << "Bundle filled" << std::endl;
// transmitSocket.Send(p.Data(), p.Size());
// std::cout << "Message sent!" << std::endl;
// memset(msg, 0, 40);
// //std::cout << "Memory freed" << std::endl;
// // ---- II PART OF THE PROCESSING ----
// curr_deg = new_deg;
// curr_vec = new_vec;
// number_of_detected_frames += 1;
// }
// else {
// // UDP Variables
// std::string str;
// char msg[40];
// UdpTransmitSocket transmitSocket(IpEndpointName(ADDRESS, PORT));
// char buffer[OUTPUT_BUFFER_SIZE];
// osc::OutboundPacketStream p(buffer, OUTPUT_BUFFER_SIZE);
// str = "0 0 0 0" + std::to_string(cnt);
// strcpy(msg, str.c_str());
// p << osc::BeginBundleImmediate
// << osc::BeginMessage("/test3") << msg << osc::EndMessage
// /* << osc::BeginMessage("/test2")
// << true << 24 << (float)10.8 << "world" << osc::EndMessage*/
// << osc::EndBundle;
// transmitSocket.Send(p.Data(), p.Size());
// std::cout << "Message sent!" << std::endl;
// memset(msg, 0, 40);
// }
// cnt++;
// ts += time_step;
// }
// }
// else {
// //std::cout << "Don't have any frames yet ..." << std::endl;
// //std::cout << "";
// }
// }
// delete[] buffer;
//}
//std::printf("End of video\n");
//// Stop all threads
//SLAM.Shutdown();
//std::printf("Done.\n");
//std::cout << "Number of detected frames: " << number_of_detected_frames << std::endl;
};