diff --git a/CMakeLists.txt b/CMakeLists.txt index 744190e..9985a78 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -104,6 +104,14 @@ ${Boost_INCLUDE_DIR} ${Boost_INCLUDE_DIR}/stage/lib ) +add_library(processing SHARED +processing_functions.h +processing_functions.cpp +) +target_link_libraries(processing +${OpenCV_LIBRARIES} +) + ##################### ORB_SLAM3 END ##################### GStreamer BEGIN @@ -197,6 +205,7 @@ add_executable(gst_get_ndi_v6.7 gst_get_ndi_v6_7.cpp ) target_link_libraries(gst_get_ndi_v6.7 + processing ${GST_LIBRARIES} ${GST_APP_LIBRARIES} ${GST_AUDIO_LIBRARIES} ${OpenCV_LIBRARIES} ${GST_VIDEO_LIBRARIES} diff --git a/gst_get_ndi_v6_7.cpp b/gst_get_ndi_v6_7.cpp index 738a3cc..fcd6387 100644 --- a/gst_get_ndi_v6_7.cpp +++ b/gst_get_ndi_v6_7.cpp @@ -15,6 +15,8 @@ #include "osc/OscOutboundPacketStream.h" #include "ip/UdpSocket.h" +#include "processing_functions.h" + #include #include "System.h" using namespace std::chrono; @@ -91,9 +93,9 @@ vector find_mode(const vector>& vec_of_rot_axes) { 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; + //euler_angles[0] *= 57.29; + //euler_angles[1] *= 57.29; + //euler_angles[2] *= 57.29; std::string str; char msg[40]; @@ -587,39 +589,31 @@ int main (int argc, char *argv[]) { 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 = 0.033; - double ts = 0; - char matrix_name[100]; - vector vec_of_deg, values; - vector> vec_of_rot_axis; + int ts = 0; + float time_step = 1.0; + char matrix_name[100] = "ORB_SLAM3 matrix"; - // ---- 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; + ofstream frames_log; + frames_log.open("lost_log.txt"); + if (!frames_log) { + cerr << "Error; File could not be opened"; + exit(1); + } - 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; + bool lost_flag = 0; + float initial_skew = atof(argv[3]); + std::cout << initial_skew << std::endl; + vector all_maps_id = { 0 }; + vector euler_prev = { 0.0, 0.0, 0.0 }, euler_now = { 0.0, 0.0, 0.0 }, skew_angle = { 0.0, 0.0, 0.0 }; + int prevID, currID; - cnt = 0; + // Processing lost of the frames. Just substituting with the average velocity + vector> recent_values; + int recent_values_desired_length = 15; + vector avg_velocity = { 0.0, 0.0, 0.0 }; - // FOR TESTING DO NOT DELETE!!! while (true) { cv::Mat frame; char* buffer = nullptr; @@ -642,180 +636,48 @@ int main (int argc, char *argv[]) { 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;*/ - + lost_flag = 0; sprintf(matrix_name, "matrix%d", cnt); + //file << matrix_name << Tcw; + + cv::Mat Rot(3, 3, CV_32F, 1); extract_rot(Rot, Tcw); - /* TESTING CODE */ + auto euler = ORB_SLAM3::Converter::toEuler(Rot); + euler = euler * 57.29f; + euler_now = -euler; + cout << euler_now[0] << " " << euler_now[1] << " " << euler_now[2] << " " << endl; + currID = SLAM.GetCurID(); - //// 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. + process_euler(euler_prev, euler_now, skew_angle, all_maps_id, prevID, currID, avg_velocity); + fill_recent_values(euler_now, recent_values, recent_values_desired_length); - //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; + frames_log << euler_now[0] << " " << euler_now[1] + initial_skew << " " << euler_now[2] << " " << cnt \ + << " " << SLAM.GetCurID() << " " << lost_flag << endl; + cout << euler_now[0] << " " << euler_now[1] + initial_skew << " " << euler_now[2] << " " << cnt << endl; + cout << "Map ID right now: " << SLAM.GetCurID() << endl; + cout << 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; + send_euler_to_receiver_osc(euler_now + initial_skew, cnt); + } else { - vector euler_angles = { 0.0, 0.0, 0.0 }; - send_euler_to_receiver_osc(euler_angles, cnt); + cout << "Tcw is empty. Processing lost values." << endl; + lost_flag = 1; + + // process_lost_euler(euler_prev, euler_now, recent_values_desired_length, recent_values, avg_velocity); + euler_now = { 0.0, 0.0, 0.0 }; + // frames_log << euler_now[0] << " " << euler_now[1] - initial_skew << " " << euler_now[2] << " " << cnt << " " \ + // << SLAM.GetCurrID() << " " << lost_flag << endl; + + frames_log << euler_now[0] << " " << euler_now[1] << " " << euler_now[2] << " " << cnt << " " \ + << SLAM.GetCurID() << " " << lost_flag << endl; + // cout << euler_now[0] << " " << euler_now[1] - initial_skew << " " << euler_now[2] << " " << cnt << endl; + cout << euler_now[0] << " " << euler_now[1] << " " << euler_now[2] << " " << cnt << endl; + + send_euler_to_receiver_osc(euler_now + initial_skew, cnt); } cnt++; ts += time_step; @@ -835,337 +697,25 @@ int main (int argc, char *argv[]) { delete[] buffer; } - // FOR TESTING + printf("End of video\n"); + // Stop all threads + SLAM.Shutdown(); - // --------------------------------- 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; - // 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; - - // // Выполнить отправку в протокол 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; - - }; + printf("Done.\n"); +}; char** argv_orb; - argv_orb = new char* [3]; + argv_orb = new char* [4]; argv_orb[0] = new char[300]; argv_orb[1] = new char[300]; argv_orb[2] = new char[300]; + argv_orb[3] = 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::strcpy(argv_orb[3], argv[7]); + std::thread t1(lambda_1, argv_orb, pangolin_window);