From 6e4c635177db522b27ec37fdef5ae1691391fec2 Mon Sep 17 00:00:00 2001 From: PodmogilnyjIvan Date: Fri, 14 Jan 2022 06:08:08 -0800 Subject: [PATCH] init repo --- gst_get_ndi_v6_7.cpp | 826 ++++++++++++++++++++++++++++--------------- 1 file changed, 541 insertions(+), 285 deletions(-) diff --git a/gst_get_ndi_v6_7.cpp b/gst_get_ndi_v6_7.cpp index c5ea6d9..738a3cc 100644 --- a/gst_get_ndi_v6_7.cpp +++ b/gst_get_ndi_v6_7.cpp @@ -15,6 +15,7 @@ #include "osc/OscOutboundPacketStream.h" #include "ip/UdpSocket.h" +#include #include "System.h" using namespace std::chrono; @@ -88,6 +89,62 @@ vector find_mode(const vector>& vec_of_rot_axes) { return vec_of_rot_axes[index]; } +void send_euler_to_receiver_osc(vector 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 v) { + + cv::Mat Rot(3, 3, CV_32F, 0.0); + float c = cos(theta); + float s = sin(theta); + float t = 1 - c; + + Rot.at(0, 0) = t * v[0] * v[0] + c; + Rot.at(0, 1) = t * v[0] * v[1] - v[2] * s; + Rot.at(0, 2) = t * v[0] * v[2] + v[1] * c; + + Rot.at(1, 0) = t * v[0] * v[1] + v[2] * s; + Rot.at(1, 1) = t * v[1] * v[1] + c; + Rot.at(1, 2) = t * v[1] * v[2] - v[0] * s; + + Rot.at(2, 0) = t * v[0] * v[2] - v[1] * s; + Rot.at(2, 1) = t * v[1] * v[2] + v[0] * s; + Rot.at(2, 2) = t * v[2] * v[2] + c; + + return Rot; +}; + +vector axisVector2Euler(float theta, vector axis) { + vector 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 queue; int cnt = 0; - const double time_step = 1.0; + const double time_step = 0.033; double ts = 0; char matrix_name[100]; vector vec_of_deg, values; @@ -560,344 +617,543 @@ 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) { - // cv::Mat frame; - // //char* buffer = nullptr; + // EXTRACTING FRAME HERE. + { + std::lock_guard guard(g_mutex); + if (frameQueue.size() > 0) { - // // EXTRACTING FRAME HERE. - // { - // std::lock_guard guard(g_mutex); - // if (frameQueue.size() > 0) { - // frame = frameQueue.front(); - // frameQueue.pop_front(); - // std::cout << "we have a frame to process..." << std::endl; - // } - // } + frame = frameQueue.front(); + frameQueue.pop_front(); + //std::cout << "we have a frame to process..." << std::endl; + } + } - // cv::Mat Tcw = SLAM.TrackMonocular(frame, ts, vector(), ""); - // cv::Mat Rot(3, 3, CV_32F, 0.0); - // std::cout << Tcw << std::endl; - //} - //// FOR TESTING + if (!frame.empty()) { + cvtColor(frame, frame, cv::COLOR_RGBA2RGB); + cv::Mat Tcw = SLAM.TrackMonocular(frame, ts, vector(), ""); + 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 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 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 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; + //for (int i = 1; i <= 2; i++) { - char* buffer = nullptr; + // //if (use_gui) { + // // cv::namedWindow("preview", 1); + // //} + // //else { + // // // cv::namedWindow("no preview", 1); + // //} + // cv::Mat frame; - // EXTRACTING FRAME HERE. - { - std::lock_guard guard(g_mutex); - if (frameQueue.size() > 0) { - frame = frameQueue.front(); - frameQueue.pop_front(); - std::cout << "we have a frame to process..." << std::endl; - } - } + // char* buffer = nullptr; - cv::Mat Tcw = SLAM.TrackMonocular(frame, ts, vector(), ""); - cv::Mat Rot(3, 3, CV_32F, 0.0); - std::cout << Tcw << std::endl; + // // EXTRACTING FRAME HERE. + // { + // //std::lock_guard guard(g_mutex); + // if (frameQueue.size() > 0) { + // frame = frameQueue.front(); + // frameQueue.pop_front(); + // std::cout << "we have a frame to process..." << 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); + // cv::Mat Tcw = SLAM.TrackMonocular(frame, ts, vector(), ""); + // cv::Mat Rot(3, 3, CV_32F, 0.0); + // std::cout << Tcw << std::endl; - // 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; + // 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); - // LET'S DEFINE CONSTANT TO ZERO OUT THE START - if (i == 1) { - DIFF_TO_CENTER = 0.0; - } + // // 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; - 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; + // // LET'S DEFINE CONSTANT TO ZERO OUT THE START + // if (i == 1) { + // DIFF_TO_CENTER = 0.0; + // } - //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; - } + // 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 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 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(), ""); - 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(), ""); + // 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 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 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; - } - } + // // Если текущий градус больше 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; } + // 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 = {}; } + // // Если нет 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; + // // Сохраняем последние 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; - // } - } - } + // } + // // 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; + // 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; + // 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; - } + // 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; + // // 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; - } - } - } + // } + // 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; + // // Сохраняем значения... + // 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 --------- + // // --------- 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; + // // Запись в файл. + // //outfile << values[j - 1] << " " << new_vec[0] << " " << new_vec[1] << " " << new_vec[2] << " " << cnt << endl; + // // cout << "Successfully written to the file" << 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; + // // Выполнить отправку в протокол 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)); + // // 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()); + // 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; + // 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; + // 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; + // 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 << ""; - } - } + // transmitSocket.Send(p.Data(), p.Size()); + // std::cout << "Message sent!" << std::endl; - delete[] buffer; - } + // memset(msg, 0, 40); - std::printf("End of video\n"); - // Stop all threads - SLAM.Shutdown(); + // //std::cout << "Memory freed" << std::endl; - std::printf("Done.\n"); + // // ---- II PART OF THE PROCESSING ---- - std::cout << "Number of detected frames: " << number_of_detected_frames << std::endl; + // 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; };