diff --git a/CMakeLists.txt b/CMakeLists.txt index bf88e97..744190e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -193,6 +193,28 @@ target_link_libraries(gst_get_ndi -lcrypto ) +add_executable(gst_get_ndi_v6.7 + gst_get_ndi_v6_7.cpp +) +target_link_libraries(gst_get_ndi_v6.7 + ${GST_LIBRARIES} ${GST_APP_LIBRARIES} ${GST_AUDIO_LIBRARIES} + ${OpenCV_LIBRARIES} + ${GST_VIDEO_LIBRARIES} + oscpack + ${LIBS} + ${ORB_SLAM3_DIR}/build + + #${OpenCV_LIBS} + ${EIGEN3_LIBS} + ${Pangolin_LIBRARIES} + ${Boost_LIBS} + ${ORB_SLAM3_DIR}/Thirdparty/DBoW2/lib/Release/DBoW2.lib + ${ORB_SLAM3_DIR}/Thirdparty/g2o/build/Release/g2o.lib + ${ORB_SLAM3_DIR}/build/Release/ORB_SLAM3.lib + -lboost_serialization + -lcrypto +) + #add_executable(try_1 # try_1.cpp #) diff --git a/gst_get_ndi.cpp b/gst_get_ndi.cpp index a28a199..4f6e43d 100644 --- a/gst_get_ndi.cpp +++ b/gst_get_ndi.cpp @@ -419,10 +419,12 @@ int main(int argc, char* argv[]) { strcpy(argv_gst[0], argv[0]); strcpy(argv_gst[1], argv[3]); - // QUESTION 1. - g_option_context_parse(context, &argc - 2, &argv_gst, &error); + int argc_gst = argc - 2; - g_option_context_parse(context, &argc, &argv, &error); + // QUESTION 1. + g_option_context_parse(context, &argc_gst, &argv_gst, &error); + + //g_option_context_parse(context, &argc, &argv, &error); if (!ndi_name) { std::cout << "ndi-name is not provided" << std::endl; @@ -476,324 +478,324 @@ int main(int argc, char* argv[]) { #ifdef MY_GST_USE_OPENCV auto lambda_1 = [](char** argv) { - // --------------------------------- SLAM SYSTEM VARIABLES --------------------------------- + // // --------------------------------- SLAM SYSTEM VARIABLES --------------------------------- - // Create SLAM system. It initializes all system threads and gets ready to process frames. - ORB_SLAM3::System SLAM(argv[1], argv[2], ORB_SLAM3::System::MONOCULAR, false); + //// Create SLAM system. It initializes all system threads and gets ready to process frames. + // ORB_SLAM3::System SLAM(argv[1], argv[2], ORB_SLAM3::System::MONOCULAR, false); - std::printf("SLAM system initialized\n"); + // std::printf("SLAM system initialized\n"); - // Main loop - cv::Mat frame; + // // Main loop + // cv::Mat frame; - int cnt = 0; - const double time_step = 1.0; - double ts = 0; - char matrix_name[100]; - vector vec_of_deg, values; - vector> vec_of_rot_axis; + // int cnt = 0; + // const double time_step = 1.0; + // double ts = 0; + // char matrix_name[100]; + // vector vec_of_deg, values; + // vector> vec_of_rot_axis; - // ---- INITIALIZE FOR THE PROCESSING OF AXIS LOSS AND FOR THE AXIS VECTOR INFORMATION ---- - float skew1 = 0.0; - float DIFF_TO_CENTER = 0.0; - float curr_deg; // later I'll assign the exact value - vector curr_vec; - vector mode1, mode2; + // // ---- INITIALIZE FOR THE PROCESSING OF AXIS LOSS AND FOR THE AXIS VECTOR INFORMATION ---- + // float skew1 = 0.0; + // float DIFF_TO_CENTER = 0.0; + // float curr_deg; // later I'll assign the exact value + // vector curr_vec; + // vector mode1, mode2; - vector> accum, accum2; - int counter2, j = 0; - std::cout << "J is: " << j; - vector mode_vec, mode_vec2; // 2 вектора, для аккумуляции слева и справа - // zero_flag - индикатор, что текущий элемент пошёл в обратную сторону (около нуля) - // mirror_flag - значения на данный момент должны отражаться - bool zero_flag, mirror_flag = false; - float mirror_point = 0.0; + // vector> accum, accum2; + // int counter2, j = 0; + // std::cout << "J is: " << j; + // vector mode_vec, mode_vec2; // 2 вектора, для аккумуляции слева и справа + // // zero_flag - индикатор, что текущий элемент пошёл в обратную сторону (около нуля) + // // mirror_flag - значения на данный момент должны отражаться + // bool zero_flag, mirror_flag = false; + // float mirror_point = 0.0; // --------------------------------- SLAM SYSTEM VARIABLES --------------------------------- - // Let's do two steps outside the loop. - for (int i = 1; i <= 2; i++) { + /* 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; + // 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 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; + // } + // } - cv::Mat Tcw = SLAM.TrackMonocular(frame, ts, vector(), ""); - cv::Mat Rot(3, 3, CV_32F, 0.0); - std::cout << Tcw << std::endl; + // 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; - auto deg_vec = extract_deg(Rot); + // 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; + // // 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; - } + // // 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; + // 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; - } + // //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; + // 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++; + // 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 { - // 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; + // // Если текущий градус больше 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; + // } // } - // --------- 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; + // if (zero_flag) { cout << "Zero flag is: true" << endl; } + // else { cout << "Zero flag is: false" << 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; - UdpTransmitSocket transmitSocket(IpEndpointName(ADDRESS, PORT)); + // // Если нет zero_flag, а в accum2 что-то есть, то опустошим его. + // if (!(zero_flag) && !accum2.empty()) { accum2 = {}; } - char buffer[OUTPUT_BUFFER_SIZE]; - osc::OutboundPacketStream p(buffer, OUTPUT_BUFFER_SIZE); + // // Сохраняем последние 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; - std::string str; - 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]); - char msg[40]; - strcpy(msg, str.c_str()); + // } + // // else { + // // zero_flag = false; + // // } + // } + // } - p << osc::BeginBundleImmediate - << osc::BeginMessage("/test3") << msg << osc::EndMessage - /* << osc::BeginMessage("/test2") - << true << 24 << (float)10.8 << "world" << osc::EndMessage*/ - << osc::EndBundle; + // 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; - //p << osc::BeginBundleImmediate - // << osc::BeginMessage("/test1") - // //res[0] << res[1] << res[2] << - // << true << "blah" << osc::EndMessage << osc::EndBundle; - ////<< osc::BeginMessage("/test2") - ////<< true << 24 << (float)10.8 << "world" << osc::EndMessage + // 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; - transmitSocket.Send(p.Data(), p.Size()); - std::cout << "Message sent!" << std::endl; + // if (mirror_flag) { + // mirror_flag = false; + // } + // else { + // mirror_flag = true; + // } - // ---- II PART OF THE PROCESSING ---- + // // 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; - curr_deg = new_deg; - curr_vec = new_vec; - } - cnt++; - ts += time_step; + // } + // 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; + // } + // } + // } - } - } - else { - std::cout << "Don't have any frames yet ..." << std::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; + + // // Выполнить отправку в протокол 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; + // UdpTransmitSocket transmitSocket(IpEndpointName(ADDRESS, PORT)); + + // char buffer[OUTPUT_BUFFER_SIZE]; + // osc::OutboundPacketStream p(buffer, OUTPUT_BUFFER_SIZE); + + // std::string str; + // 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]); + // char msg[40]; + // 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; + + // //p << osc::BeginBundleImmediate + // // << osc::BeginMessage("/test1") + // // //res[0] << res[1] << res[2] << + // // << true << "blah" << osc::EndMessage << osc::EndBundle; + // ////<< osc::BeginMessage("/test2") + // ////<< true << 24 << (float)10.8 << "world" << osc::EndMessage + + // transmitSocket.Send(p.Data(), p.Size()); + // std::cout << "Message sent!" << std::endl; + + // // ---- II PART OF THE PROCESSING ---- + + // curr_deg = new_deg; + // curr_vec = new_vec; + // } + // cnt++; + // ts += time_step; + + // } + //} + //else { + // std::cout << "Don't have any frames yet ..." << std::endl; //std::cout << ""; - } - } + //} + //} - delete[] buffer; - } + //delete[] buffer; + //} - std::printf("End of video\n"); - // Stop all threads - SLAM.Shutdown(); + //std::printf("End of video\n"); + //// Stop all threads + //SLAM.Shutdown(); - std::printf("Done.\n"); + //std::printf("Done.\n"); }; diff --git a/gst_get_ndi_v6_7.cpp b/gst_get_ndi_v6_7.cpp new file mode 100644 index 0000000..c5ea6d9 --- /dev/null +++ b/gst_get_ndi_v6_7.cpp @@ -0,0 +1,933 @@ +#include +#include +//#include +//#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "osc/OscOutboundPacketStream.h" +#include "ip/UdpSocket.h" + +#include "System.h" +using namespace std::chrono; + +// CHANGE HERE TO MAKE PORT +#define ADDRESS "127.0.0.1" +#define PORT 7000 + +#define OUTPUT_BUFFER_SIZE 1024 + +#define MY_GST_USE_OPENCV + +#ifdef MY_GST_USE_OPENCV +#include "opencv2/opencv.hpp" +// TODO: use synchronized deque +std::mutex g_mutex; +std::deque frameQueue; +#endif // MY_GST_USE_OPENCV + +cv::Mat& extract_rot(cv::Mat& rot, const cv::Mat& trans) { + // cv::Mat rot(3, 3, CV_32F, 0.0); + for (int row = 0; row < 3; ++row) { + for (int col = 0; col < 3; ++col) { + rot.at(row, col) = trans.at(row, col); + } + } + return rot; +} + +pair> extract_deg(const cv::Mat& rotation_matrix) { + // TODO: extract_deg is producing negative values. Fix it. + float degrees; + vector myvec = { rotation_matrix.at(1, 2) - rotation_matrix.at(2,1), rotation_matrix.at(2, 0) - rotation_matrix.at(0, 2), rotation_matrix.at(0, 1) - rotation_matrix.at(1, 0) }; + + float trace; + trace = rotation_matrix.at(0, 0) + rotation_matrix.at(1, 1) + rotation_matrix.at(2, 2); + // cout << "a11 is: " << rotation_matrix.at(0, 0) << " a22 is: " << rotation_matrix.at(1, 1) << " a33 is: " << rotation_matrix.at(2, 2) << endl; + // cout << "x is: " << (trace - 1) / 2 << endl; + degrees = acos((trace - 1) / 2); + // cout << "Calc degrees (from function) is: " << degrees << endl; + + pair> result = { degrees, myvec }; + return result; +} + +vector find_mode(const vector>& vec_of_rot_axes) { + cout << "Hello! This is find_mode() function" << endl; + int index = 0, counter = 0, max_counted = 0; + vector el; + for (int i = 0; i < vec_of_rot_axes.size(); i++) { + el = vec_of_rot_axes[i]; + + cout << "Extracted el is: "; + for (auto e : el) { + cout << " " << e << " "; + } + cout << endl; + + for (const auto& vec_of_rot_axe : vec_of_rot_axes) { + if (el == vec_of_rot_axe) { + cout << "Entered if (el == vec_of_rot_axe) statement" << endl; + counter += 1; + } + } + if (counter > max_counted) { + // cout << "Found new max element. Index is: " << index << "; i is: " << i << endl; + index = i; + max_counted = counter; + } + } + cout << "Index is: " << index << "; And arr size is: " << vec_of_rot_axes.size() << endl; + return vec_of_rot_axes[index]; +} + +GstFlowReturn new_preroll(GstAppSink *appsink, gpointer data) { + g_print ("Got preroll!\n"); + return GST_FLOW_OK; +} + +GstFlowReturn new_sample(GstAppSink *appsink, gpointer data) { + static int framecount = 0; + framecount++; + static long long ms = 0; + + auto new_ms = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); + std::cout << "nnew frame " << (new_ms - ms) << " " << framecount << std::endl; + ms = new_ms; + + GstSample *sample = gst_app_sink_pull_sample(appsink); + GstCaps *caps = gst_sample_get_caps(sample); + GstBuffer *buffer = gst_sample_get_buffer(sample); + + const auto& n_memory = gst_buffer_n_memory(buffer); + std::cout << "n_memory = " << n_memory << std::endl; + std::cout << "buffer->pts = " << buffer->pts << std::endl; + std::cout << "buffer->dts = " << buffer->dts << std::endl; + std::cout << "buffer->duration = " << buffer->duration << std::endl; + std::cout << "buffer->offset = " << buffer->offset << std::endl; + std::cout << "buffer->offset_end = " << buffer->offset_end << std::endl; + + const GstStructure *info = gst_sample_get_info(sample); + + GstMeta *gst_meta; + gpointer state = nullptr; + while ((gst_meta = gst_buffer_iterate_meta(buffer, &state))) { + if (gst_meta->info == gst_video_caption_meta_get_info()) { + auto specific_meta = (GstVideoCaptionMeta *) gst_meta; + if (specific_meta) { + auto x = (const char *) (specific_meta->data); + std::cout << "MetaInfo is recognized to be [GstVideoCaptionMeta]" + << "caption = " << std::string(x, specific_meta->size) + << std::endl; + } + } else if (gst_meta->info == gst_video_time_code_meta_get_info()) { + auto specific_meta = (GstVideoTimeCodeMeta *) gst_meta; + if (specific_meta) { + std::cout << "MetaInfo is recognized to be [GstVideoTimeCodeMeta]" + << " h = " << specific_meta->tc.hours + << " m = " << specific_meta->tc.minutes + << " s = " << specific_meta->tc.seconds + << " f = " << specific_meta->tc.frames + << std::endl; + } + } else if (gst_meta->info == gst_meta_get_info("GstNdiSrcMeta")) { + std::cout << "MetaInfo is recognized to be [GstNdiSrcMeta]" + << std::endl; + } else if (gst_meta->info == gst_meta_get_info("GstNdiSinkAudioMeta")) { + std::cout << "MetaInfo is recognized to be [GstNdiSinkAudioMeta]" + << std::endl; + } else if (gst_meta->info == gst_meta_get_info("GstVideoMeta")) { + std::cout << "MetaInfo is recognized to be [GstVideoMeta]" + << std::endl; + } else if (gst_meta->info == gst_meta_get_info("GstVideoCropMeta")) { + std::cout << "MetaInfo is recognized to be [GstVideoCropMeta]" + << std::endl; + } else if (gst_meta->info == gst_meta_get_info("GstFramePositionerMeta")) { + std::cout << "MetaInfo is recognized to be [GstFramePositionerMeta]" + << std::endl; + } else if (gst_meta->info == gst_meta_get_info("GstMetaDfbSurface")) { + std::cout << "MetaInfo is recognized to be [GstMetaDfbSurface]" + << std::endl; + } else if (gst_meta->info == gst_meta_get_info("GstSubtitleMeta")) { + std::cout << "MetaInfo is recognized to be [GstSubtitleMeta]" + << std::endl; + } else if (gst_meta->info == gst_meta_get_info("GstRtmpMeta")) { + std::cout << "MetaInfo is recognized to be [GstRtmpMeta]" + << std::endl; + } else if (gst_meta->info == gst_meta_get_info("GstMpegVideoMeta")) { + std::cout << "MetaInfo is recognized to be [GstMpegVideoMeta]" + << std::endl; + } else if (gst_meta->info == gst_meta_get_info("GstSctpReceiveMeta")) { + std::cout << "MetaInfo is recognized to be [GstSctpReceiveMeta]" + << std::endl; + } else if (gst_meta->info == gst_meta_get_info("GstSctpSendMeta")) { + std::cout << "MetaInfo is recognized to be [GstSctpSendMeta]" + << std::endl; + } else if (gst_meta->info == gst_meta_get_info("GstCoreMediaMeta")) { + std::cout << "MetaInfo is recognized to be [GstCoreMediaMeta]" + << std::endl; + } else if (gst_meta->info == gst_meta_get_info("GstCoreVideoMeta")) { + std::cout << "MetaInfo is recognized to be [GstCoreVideoMeta]" + << std::endl; + } else if (gst_meta->info == gst_meta_get_info("GstAudioDownmixMeta")) { + std::cout << "MetaInfo is recognized to be [GstAudioDownmixMeta]" + << std::endl; + } else if (gst_meta->info == gst_meta_get_info("GstAudioClippingMeta")) { + std::cout << "MetaInfo is recognized to be [GstAudioClippingMeta]" + << std::endl; + } else if (gst_meta->info == gst_meta_get_info("GstGLSyncMeta")) { + std::cout << "MetaInfo is recognized to be [GstGLSyncMeta]" + << std::endl; + } else if (gst_meta->info == gst_meta_get_info("GstRTPSourceMeta")) { + std::cout << "MetaInfo is recognized to be [GstRTPSourceMeta]" + << std::endl; + } else if (gst_meta->info == gst_meta_get_info("GstRTPSourceMeta")) { + std::cout << "MetaInfo is recognized to be [GstRTPSourceMeta]" + << std::endl; + } else if (gst_meta->info == gst_meta_get_info("GstVideoGLTextureUploadMeta")) { + std::cout << "MetaInfo is recognized to be [GstVideoGLTextureUploadMeta]" + << std::endl; + } else if (gst_meta->info == gst_meta_get_info("GstVideoRegionOfInterestMeta")) { + std::cout << "MetaInfo is recognized to be [GstVideoRegionOfInterestMeta]" + << std::endl; + } else if (gst_meta->info == gst_meta_get_info("GstVideoAFDMeta")) { + std::cout << "MetaInfo is recognized to be [GstVideoAFDMeta]" + << std::endl; + } else if (gst_meta->info == gst_meta_get_info("GstVideoBarMeta")) { + std::cout << "MetaInfo is recognized to be [GstVideoBarMeta]" + << std::endl; + } else if (gst_meta->info == gst_meta_get_info("GstVideoMultiviewMeta")) { + std::cout << "MetaInfo is recognized to be [GstVideoMultiviewMeta]" + << std::endl; + } else if (gst_meta->info == gst_meta_get_info("GstVideoOverlayCompositionMeta")) { + std::cout << "MetaInfo is recognized to be [GstVideoOverlayCompositionMeta]" + << std::endl; + } else if (gst_meta->info == gst_meta_get_info("GstMetaXImage")) { + std::cout << "MetaInfo is recognized to be [GstMetaXImage]" + << std::endl; + } else if (gst_meta->info == gst_meta_get_info("GstProtectionMeta")) { + std::cout << "MetaInfo is recognized to be [GstProtectionMeta]" + << std::endl; + } else if (gst_meta->info == gst_meta_get_info("GstNetControlMessageMeta")) { + std::cout << "MetaInfo is recognized to be [GstNetControlMessageMeta]" + << std::endl; + } else if (gst_meta->info == gst_meta_get_info("GstMetaTest")) { + std::cout << "MetaInfo is recognized to be [GstMetaTest]" + << std::endl; + } else if (gst_meta->info == gst_meta_get_info("GstNVMMParentMeta")) { + std::cout << "MetaInfo is recognized to be [GstNVMMParentMeta]" + << std::endl; + } else if (gst_meta->info == gst_meta_get_info("GstAudioMeta")) { + std::cout << "MetaInfo is recognized to be [GstAudioMeta]" + << std::endl; + } else if (gst_meta->info == gst_meta_get_info("GstAudioLevelMeta")) { + std::cout << "MetaInfo is recognized to be [GstAudioLevelMeta]" + << std::endl; + } else if (gst_meta->info == gst_meta_get_info("GstVideoAffineTransformationMeta")) { + std::cout << "MetaInfo is recognized to be [GstVideoAffineTransformationMeta]" + << std::endl; + } else if (gst_meta->info == gst_meta_get_info("GstVideoCodecAlphaMeta")) { + std::cout << "MetaInfo is recognized to be [GstVideoCodecAlphaMeta]" + << std::endl; + } else if (gst_meta->info == gst_meta_get_info("XXX")) { + std::cout << "MetaInfo is recognized to be [XXX]" + << std::endl; + } else if (gst_meta->info == gst_meta_get_info("XXX")) { + std::cout << "MetaInfo is recognized to be [XXX]" + << std::endl; + } else if (gst_meta->info == gst_meta_get_info("XXX")) { + std::cout << "MetaInfo is recognized to be [XXX]" + << std::endl; + } else if (gst_meta->info == gst_meta_get_info("XXX")) { + std::cout << "MetaInfo is recognized to be [XXX]" + << std::endl; + } else if (gst_meta->info == gst_meta_get_info("XXX")) { + std::cout << "MetaInfo is recognized to be [XXX]" + << std::endl; + } else if (gst_meta->info == gst_meta_get_info("XXX")) { + std::cout << "MetaInfo is recognized to be [XXX]" + << std::endl; + } else if (gst_meta->info == gst_meta_get_info("XXX")) { + std::cout << "MetaInfo is recognized to be [XXX]" + << std::endl; + } else if (gst_meta->info == gst_meta_get_info("XXX")) { + std::cout << "MetaInfo is recognized to be [XXX]" + << std::endl; + } else if (gst_meta->info == gst_meta_get_info("XXX")) { + std::cout << "MetaInfo is recognized to be [XXX]" + << std::endl; + } else { + std::cout << "GstMetaInfo is not recognized." + << " info = " << gst_meta->info + << " api = " << gst_meta->info->api + << std::endl; + } + } + + // ---- Read frame and convert to opencv format --------------- + +// return GST_FLOW_OK; + + GstMapInfo map; + gst_buffer_map (buffer, &map, GST_MAP_READ); + +#ifdef MY_GST_USE_OPENCV + // convert gstreamer data to OpenCV Mat, you could actually + // resolve height / width from caps... + + int width = 2560; + int height = 1440; + int depth = 4; + int bpp = -1; + + GstStructure *s = gst_caps_get_structure(caps, 0); + gboolean res = true; + res &= gst_structure_get_int (s, "width", &width); + res &= gst_structure_get_int (s, "height", &height); +// res &= gst_structure_get_int (s, "depth", &depth); +// res &= gst_structure_get_int (s, "bpp", &bpp); + + if (gst_structure_get_field_type (s, "format") == G_TYPE_STRING) { + const char *string; + string = gst_structure_get_string (s, "format"); + // std::cout << "flksjlskfjsjdlkf" << string << std::endl; +// fourcc = GST_STR_FOURCC (string); +// } else if (gst_structure_get_field_type (s, "format") == GST_TYPE_FOURCC) { +// gst_structure_get_fourcc (s, "format", &fourcc); + } else { +// fourcc = 0; + } + + assert (depth == 4); + cv::Mat frame(cv::Size(width, height), CV_8UC4, (char*)map.data, cv::Mat::AUTO_STEP); + +// int frameSize = map.size; + std::cout << "size from caps = (" << width << "," << height << "," << depth << "," << bpp << ")" << "res =" << res + << " total size = " << map.size + << std::endl; + +// if (res) { +// std::fstream file("example.bin", std::ios::out | std::ios::binary | std::ios::app); +// file.write((char*)map.data, map.size); +// file.close(); +// } +// throw 1; + { + std::lock_guard guard(g_mutex); + frameQueue.push_back(frame.clone()); + } +#endif + + gst_buffer_unmap(buffer, &map); + + // ------------------------------------------------------------ + + // print dot every 30 frames + if (framecount%30 == 0) { + g_print ("."); + } + + // show caps on first frame + if (framecount == 1) { + g_print ("%s\n", gst_caps_to_string(caps)); + } + + gst_sample_unref (sample); + return GST_FLOW_OK; +} + +static gboolean my_bus_callback (GstBus *bus, GstMessage *message, gpointer data) { + g_print ("Got %s message\n", GST_MESSAGE_TYPE_NAME (message)); + switch (GST_MESSAGE_TYPE (message)) { + case GST_MESSAGE_ERROR: { + GError *err; + gchar *debug; + + gst_message_parse_error (message, &err, &debug); + g_print ("Error: %s\n", err->message); + g_error_free (err); + g_free (debug); + break; + } + case GST_MESSAGE_EOS: { + /* end-of-stream */ + break; + } default: { + /* unhandled message */ + break; + } + } + /* we want to be notified again the next time there is a message + * on the bus, so returning TRUE (FALSE means we want to stop watching + * for messages on the bus and our callback should not be called again) + */ + return TRUE; +} + +static gboolean overrun_callback (GstElement * queue, gpointer udata) { + std::cout << "hi from overrun" << std::endl; + return TRUE; +} + +static gchar* ndi_name = nullptr; +static gint use_opencv_preview = 0; +static gboolean use_gst_preview = FALSE; + +static GOptionEntry entries[] = + { + { "ndi-name", 0, 0, G_OPTION_ARG_STRING, &ndi_name, "you can enter the string here (ndi-name)", "M" }, + { "opencv-preview", 0, 0, G_OPTION_ARG_INT, &use_opencv_preview, "use opencv preview", NULL }, + { "gst-preview", 0, 0, G_OPTION_ARG_INT, &use_gst_preview, "use gstreamer preview", NULL }, + { NULL } + }; + +int main (int argc, char *argv[]) { + std::cout << "argc = " << argc << std::endl; + + //for (int i = 0; i < argc; i++) { + // std::cout << argv[i] << std::endl; + //} + + GError *error = nullptr; + GOptionContext *context; + + context = g_option_context_new("- test tree model performance"); + g_option_context_add_main_entries(context, entries, "bla"); + + char** argv_gst; + argv_gst = new char* [4]; + argv_gst[0] = new char[200]; + argv_gst[1] = new char[200]; + argv_gst[2] = new char[200]; + argv_gst[3] = new char[200]; + + std::strcpy(argv_gst[0], argv[0]); + std::strcpy(argv_gst[1], argv[1]); + std::strcpy(argv_gst[2], argv[2]); + std::strcpy(argv_gst[3], argv[3]); + + for (int i = 0; i < argc-3; i++) { + std::cout << argv_gst[i] << std::endl; + } + + int argc_gst = argc - 3; + + // QUESTION 1. + g_option_context_parse(context, &argc_gst, &argv_gst, &error); + + //g_option_context_parse(context, &argc, &argv, &error); + + if (!ndi_name) { + std::cout << "ndi-name is not provided" << std::endl; + // ndi_name = (char*)malloc(sizeof(char) * 100); + ndi_name = "DESKTOP-O5PNOBN (Test Pattern)"; + std::cout << "ndi-name (default) = '" << ndi_name << "'" << std::endl; + } else { + std::cout << "ndi-name = '" << ndi_name << "'" << std::endl; + } + + GstStateChangeReturn ret; + + int fake_argc = 1; + gst_init (&fake_argc, &argv); + /* + s_pipeline = f'ndisrc ndi-name="{ndi_name}" ! ndisrcdemux name=demux ' \ + f'demux.video ! queue ! tee name=my_tee ' \ + f'my_tee. ! queue ! videoconvert ! autovideosink ' \ + f'my_tee. ! queue ! videoconvert ! appsink name=my_sink' + */ + auto sink_caps = "video/x-raw, format=(string)BGRA"; + std::stringstream ss; + ss << "ndisrc ndi-name=\"" << ndi_name << "\" ! ndisrcdemux name=demux " + << "demux.video ! queue ! tee name=my_tee "; + if (use_gst_preview) { + ss << "my_tee. ! queue name=q_show ! videoconvert ! autovideosink "; + } + ss << "my_tee. ! queue name=q_appsink ! videoconvert name=convert_slow ! appsink name=my_sink caps=\"" << sink_caps << "\""; + // ss << "my_tee. ! queue name=q_appsink ! glupload ! glcolorconvert ! \"video/x-raw(memory:GLMemory),format=BGR\" ! appsink name=my_sink "; + std::string my_pipeline = ss.str(); + + std::cout << "We are about to launch the pipeline = [" << my_pipeline.c_str() << "]" << std::endl; + GstElement *pipeline = gst_parse_launch(my_pipeline.c_str(), nullptr); + + std::cout << "Launching pipeline success" << std::endl; + + // g_object_set (my_src, "ndi-name", "ADMIN (lafvi 29.97fps)", NULL); + /* + [libndi_newtek @ 0x556ab959f400] Found 4 NDI sources: + [libndi_newtek @ 0x556ab959f400] 'DESKTOP-O5PNOBN (CameraVegasAR)' '185.41.112.227:5962' + [libndi_newtek @ 0x556ab959f400] 'DESKTOP-O5PNOBN (NVIDIA Quadro RTX 5000 1)' '185.41.112.227:5961' + [libndi_newtek @ 0x556ab959f400] 'DESKTOP-O5PNOBN (NVIDIA Quadro RTX 5000 2)' '185.41.112.227:5963' + [libndi_newtek @ 0x556ab959f400] 'DESKTOP-O5PNOBN (Test Pattern)' '185.41.112.227:5964' + */ +// g_object_set (my_src, "ndi-name", ndi_name, NULL); + + /* get sink */ + GstElement *sink = gst_bin_get_by_name (GST_BIN (pipeline), "my_sink"); + gst_app_sink_set_emit_signals((GstAppSink*)sink, true); + gst_app_sink_set_drop((GstAppSink*)sink, true); + gst_app_sink_set_max_buffers((GstAppSink*)sink, 1); + GstAppSinkCallbacks callbacks = { NULL, new_preroll, new_sample }; + gst_app_sink_set_callbacks (GST_APP_SINK(sink), &callbacks, NULL, NULL); + + std::cout << "Launching sink success" << std::endl; + + GstBus *bus; + guint bus_watch_id; + bus = gst_pipeline_get_bus (GST_PIPELINE (pipeline)); + bus_watch_id = gst_bus_add_watch (bus, my_bus_callback, NULL); + gst_object_unref (bus); + + std::cout << "Launching bus success" << std::endl; + + { + GstElement *e = gst_bin_get_by_name(GST_BIN (pipeline), "q_appsink"); + g_signal_connect (e, "overrun", G_CALLBACK(overrun_callback), NULL); + } + { + GstElement *e = gst_bin_get_by_name(GST_BIN (pipeline), "convert_slow"); + g_object_set(e, "n-threads", 1, nullptr); + } + + /* Start playing */ + ret = gst_element_set_state (pipeline, GST_STATE_PLAYING); + if (ret == GST_STATE_CHANGE_FAILURE) { + g_printerr ("Unable to set the pipeline to the playing state.\n"); + gst_object_unref (pipeline); + return -1; + } + + std::cout << "Start playing success" << std::endl; + +#ifdef MY_GST_USE_OPENCV + + bool pangolin_window; + std::cout << argv[6] << std::endl; + string pangolin_choice(argv[6]); + if (pangolin_choice == "--orb_slam_window=1") { + pangolin_window = true; + } + else { + pangolin_window = false; + } + + std::cout << "cout success" << std::endl; + std::cout << pangolin_window << std::endl; + + + auto lambda_1 = [] (char** argv, bool pangolin_preview) { + // --------------------------------- SLAM SYSTEM VARIABLES --------------------------------- + + // Create SLAM system. It initializes all system threads and gets ready to process frames. + ORB_SLAM3::System SLAM(argv[1], argv[2], ORB_SLAM3::System::MONOCULAR, pangolin_preview); + + std::printf("SLAM system initialized\n"); + + // Main loop + cv::Mat frame; + + // This is the queue, which will accumulate the frames. + std::vector queue; + + int cnt = 0; + const double time_step = 1.0; + double ts = 0; + char matrix_name[100]; + vector vec_of_deg, values; + vector> vec_of_rot_axis; + + // ---- INITIALIZE FOR THE PROCESSING OF AXIS LOSS AND FOR THE AXIS VECTOR INFORMATION ---- + float skew1 = 0.0; + float DIFF_TO_CENTER = 0.0; + float curr_deg; // later I'll assign the exact value + vector curr_vec; + vector mode1, mode2; + int number_of_detected_frames = 0; + + vector> accum, accum2; + int counter2, j = 0; + std::cout << "J is: " << j; + vector mode_vec, mode_vec2; // 2 вектора, для аккумуляции слева и справа + // zero_flag - индикатор, что текущий элемент пошёл в обратную сторону (около нуля) + // mirror_flag - значения на данный момент должны отражаться + bool zero_flag, mirror_flag = false; + float mirror_point = 0.0; + + + + //// FOR TESTING + //while (true) { + // cv::Mat frame; + // //char* buffer = nullptr; + + // // 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; + // } + // } + + // cv::Mat Tcw = SLAM.TrackMonocular(frame, ts, vector(), ""); + // cv::Mat Rot(3, 3, CV_32F, 0.0); + // std::cout << Tcw << std::endl; + //} + //// 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 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(), ""); + 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) { + + cv::Mat frame; + + 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; + + 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. + + 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; + + vec_of_deg.push_back(new_deg); + vec_of_rot_axis.push_back(new_vec); + j++; + 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) { 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; + + // Выполнить отправку в протокол 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; + + }; + + char** argv_orb; + argv_orb = new char* [3]; + argv_orb[0] = new char[300]; + argv_orb[1] = new char[300]; + argv_orb[2] = new char[300]; + + std::strcpy(argv_orb[0], argv[0]); + std::strcpy(argv_orb[1], argv[4]); + std::strcpy(argv_orb[2], argv[5]); + + std::thread t1(lambda_1, argv_orb, pangolin_window); + + + bool is_terminated = false; + while (!is_terminated) { + // g_main_iteration(false); + g_main_context_iteration(NULL, false); + } + t1.join(); +#else + bool is_terminated = false; + while (!is_terminated) { + g_main_context_iteration(NULL, false); + } +#endif // MY_GST_USE_OPENCV + gst_element_set_state (GST_ELEMENT (pipeline), GST_STATE_NULL); + gst_object_unref (GST_OBJECT (pipeline)); + + return 0; +} diff --git a/my_ndi_source.cpp b/my_ndi_source.cpp index 25c0d32..9aee5a2 100644 --- a/my_ndi_source.cpp +++ b/my_ndi_source.cpp @@ -45,15 +45,29 @@ int main(int argc, char* argv[]) { int64_t total_frames_counter = 0; // Let's open the video with OpenCV and read it - // std::string path_to_file = "C:\\Users\\ivan\\Source\\Repos\\cv_networking_pipeline\\gstreamer_receive_video\\videos\\camera1-2020-10-14---19-38-42---093390448_scaled.mp4"; - std::string path_to_file; - std::cout << "Please, enter the path to the video_file" << std::endl; - std::cin >> path_to_file; + std::string path_to_file = "C:\\Users\\ivan\\Source\\Repos\\cv_networking_pipeline\\gstreamer_receive_video\\videos\\camera1-2020-10-14---19-38-42---093390448_scaled.mp4"; + //std::string path_to_file; + //std::cout << "Please, enter the path to the video_file" << std::endl; + //std::cin >> path_to_file; + + std::string confirmation; + + while (true) { + std::cout << "Please, write key y or yes to continue" << std::endl; + std::cin >> confirmation; + if ( (confirmation == "y") || (confirmation == "yes") ) { + break; + } + confirmation = ""; + } + cv::VideoCapture video_file(path_to_file); std::cout << "Video File opened" << std::endl; cv::Mat frame; + int cnt = 0; + while (!is_terminated) { // Get the current time const auto start_send = high_resolution_clock::now(); @@ -62,9 +76,9 @@ int main(int argc, char* argv[]) { for (int idx = 200; idx; idx--) { // Fill in the buffer. It is likely that you would do something much smarter than this. video_file.read(frame); - std::cout << "Successfully read video_file" << std::endl; - std::cout << "Frame channels is: " << frame.channels() << " " << frame.rows << " " << frame.cols << std::endl; - std::cout << frame.total() * frame.channels() << std::endl; + //std::cout << "Successfully read video_file" << std::endl; + //std::cout << "Frame channels is: " << frame.channels() << " " << frame.rows << " " << frame.cols << std::endl; + //std::cout << frame.total() * frame.channels() << std::endl; // from https://stackoverflow.com/questions/26681713/convert-mat-to-array-vector-in-opencv // TODO: Seems that the conversion was unseccesful. Understand what really happens when you set the frame.ptr(0) and what happens when // TODO: you make (uint8_t*)malloc(xres * yres * 4) @@ -73,7 +87,7 @@ int main(int argc, char* argv[]) { cv::cvtColor(frame, continiousRGBA, CV_BGR2RGBA, 4); //uchar* p_data = (uchar*)frame.clone().data; //uchar* p_data = (uchar*)frame.ptr(0); - std::cout << "Successfully converted frame." << std::endl; + //std::cout << "Successfully converted frame." << std::endl; //std::cout << &p_data[50] << std::endl; //for (int i = 0; i < frame.total() * frame.channels(); i++) { @@ -82,11 +96,11 @@ int main(int argc, char* argv[]) { // We are going to create a 1920x1080 interlaced frame at 29.97Hz. int xres = 480; int yres = 270; - std::string metadata_string = ""; + std::string metadata_string = ""; // (uint8_t*)malloc(xres * yres * 4) is the frame itself. NDIlib_video_frame_v2_t ndi_video_frame( - xres, yres, NDIlib_FourCC_type_BGRX, + xres, yres, NDIlib_FourCC_type_RGBA, 30000, 1001, 2, NDIlib_frame_format_type_progressive, 0, @@ -94,7 +108,8 @@ int main(int argc, char* argv[]) { 0, metadata_string.c_str() ); - std::cout << "Successfully created ndi_video_frame" << std::endl; + //std::cout << "Successfully created ndi_video_frame" << std::endl; + // // std::cout << "hi there" << std::endl; // std::cout << "xres = " << ndi_video_frame.xres << std::endl; // std::cout << "yres = " << ndi_video_frame.yres << std::endl; @@ -126,6 +141,7 @@ int main(int argc, char* argv[]) { // Just display something helpful printf("200 frames sent, at %1.2ffps\n", 200.0f / duration_cast>(high_resolution_clock::now() - start_send).count()); + cnt++; } // Destroy the NDI sender