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

Compare commits

base: pi:master
Branches Tags
pi:master pi:direct_process pi:f01_accum_vector
..
compare: pi:f01_accum_vector
Branches Tags
pi:master pi:direct_process pi:f01_accum_vector

1 Commits
master ... f01_accum_

Author SHA1 Message Date
PodmogilnyjIvan
f43b9c4732 start of feature 01 repo 2022-01-11 08:20:09 -08:00
5 changed files with 1258 additions and 292 deletions
Expand all files Collapse all files

22
CMakeLists.txt
View File

@@ -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
#)

7
README.md
View File

@@ -1,12 +1,5 @@
# ORB_SLAM3 data processor and the networking pipeline.
When downloading to your local machine in CMakeLists.txt file change the paths:
1. ORB_SLAM3_DIR
2. OSC_DIR
3. OpenCV_DIR
4. Boost_INCLUDE_DIR
5. Pangolin_DIR
6. Eigen3_DIR
## Prerequisites

550
gst_get_ndi.cpp
View File

@@ -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<float> vec_of_deg, values;
vector<vector<float>> vec_of_rot_axis;
// int cnt = 0;
// const double time_step = 1.0;
// double ts = 0;
// char matrix_name[100];
// vector<float> vec_of_deg, values;
// vector<vector<float>> 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<float> curr_vec;
vector<float> 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<float> curr_vec;
// vector<float> mode1, mode2;
vector<vector<float>> accum, accum2;
int counter2, j = 0;
std::cout << "J is: " << j;
vector<float> mode_vec, mode_vec2; // 2 вектора, для аккумуляции слева и справа
// zero_flag - индикатор, что текущий элемент пошёл в обратную сторону (около нуля)
// mirror_flag - значения на данный момент должны отражаться
bool zero_flag, mirror_flag = false;
float mirror_point = 0.0;
// vector<vector<float>> accum, accum2;
// int counter2, j = 0;
// std::cout << "J is: " << j;
// vector<float> 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<std::mutex> guard(g_mutex);
if (frameQueue.size() > 0) {
frame = frameQueue.front();
frameQueue.pop_front();
std::cout << "we have a frame to process..." << std::endl;
}
}
// {
// std::lock_guard<std::mutex> guard(g_mutex);
// if (frameQueue.size() > 0) {
// frame = frameQueue.front();
// frameQueue.pop_front();
// std::cout << "we have a frame to process..." << std::endl;
// }
// }
cv::Mat Tcw = SLAM.TrackMonocular(frame, ts, vector<ORB_SLAM3::IMU::Point>(), "");
cv::Mat Rot(3, 3, CV_32F, 0.0);
std::cout << Tcw << std::endl;
// cv::Mat Tcw = SLAM.TrackMonocular(frame, ts, vector<ORB_SLAM3::IMU::Point>(), "");
// cv::Mat Rot(3, 3, CV_32F, 0.0);
// std::cout << Tcw << std::endl;
if (!Tcw.empty()) {
sprintf(matrix_name, "matrix%d", cnt);
extract_rot(Rot, Tcw);
// cout << "Extracted rotation matrix is: " << Rot;
auto deg_vec = extract_deg(Rot);
// 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<std::mutex> guard(g_mutex);
if (frameQueue.size() > 0) {
frame = frameQueue.front();
frameQueue.pop_front();
std::cout << "we have a frame to process..." << std::endl;
//{
//std::lock_guard<std::mutex> guard(g_mutex);
//if (frameQueue.size() > 0) {
// frame = frameQueue.front();
// frameQueue.pop_front();
// std::cout << "we have a frame to process..." << std::endl;
if (!frame.empty()) {
// if (!frame.empty()) {
cv::Mat Tcw = SLAM.TrackMonocular(frame, ts, vector<ORB_SLAM3::IMU::Point>(), "");
cv::Mat Rot(3, 3, CV_32F, 0.0);
std::cout << Tcw << std::endl;
if (!Tcw.empty()) {
sprintf(matrix_name, "matrix%d", cnt);
extract_rot(Rot, Tcw);
// cout << "Extracted rotation matrix is: " << Rot;
// Extract the degree and the vector from the rotation matrix.
auto deg_vec = extract_deg(Rot); // returns a degree and a vector of rotation.
// cv::Mat Tcw = SLAM.TrackMonocular(frame, ts, vector<ORB_SLAM3::IMU::Point>(), "");
// cv::Mat Rot(3, 3, CV_32F, 0.0);
// std::cout << Tcw << std::endl;
// if (!Tcw.empty()) {
// sprintf(matrix_name, "matrix%d", cnt);
// extract_rot(Rot, Tcw);
// // cout << "Extracted rotation matrix is: " << Rot;
// // Extract the degree and the vector from the rotation matrix.
// auto deg_vec = extract_deg(Rot); // returns a degree and a vector of rotation.
float new_deg = -deg_vec.first * 57.29 - DIFF_TO_CENTER;
vector<float> new_vec = deg_vec.second;
cout << "Successfully created curr_deg and curr_vec" << endl;
// float new_deg = -deg_vec.first * 57.29 - DIFF_TO_CENTER;
// vector<float> new_vec = deg_vec.second;
// 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");
};

933
gst_get_ndi_v6_7.cpp Normal file
View File

@@ -0,0 +1,933 @@
#include <gst/gst.h>
#include <gst/app/gstappsink.h>
//#include <gst/
#include <sstream>
//#include <format>
#include <gst/video/gstvideometa.h>
#include <cstdlib>
#include <deque>
#include <iostream>
#include <thread>
#include <mutex>
#include <fstream>
#include <chrono>
#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<cv::Mat> 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<float>(row, col) = trans.at<float>(row, col);
}
}
return rot;
}
pair<float, vector<float>> extract_deg(const cv::Mat& rotation_matrix) {
// TODO: extract_deg is producing negative values. Fix it.
float degrees;
vector<float> myvec = { rotation_matrix.at<float>(1, 2) - rotation_matrix.at<float>(2,1), rotation_matrix.at<float>(2, 0) - rotation_matrix.at<float>(0, 2), rotation_matrix.at<float>(0, 1) - rotation_matrix.at<float>(1, 0) };
float trace;
trace = rotation_matrix.at<float>(0, 0) + rotation_matrix.at<float>(1, 1) + rotation_matrix.at<float>(2, 2);
// cout << "a11 is: " << rotation_matrix.at<float>(0, 0) << " a22 is: " << rotation_matrix.at<float>(1, 1) << " a33 is: " << rotation_matrix.at<float>(2, 2) << endl;
// cout << "x is: " << (trace - 1) / 2 << endl;
degrees = acos((trace - 1) / 2);
// cout << "Calc degrees (from function) is: " << degrees << endl;
pair<float, vector<float>> result = { degrees, myvec };
return result;
}
vector<float> find_mode(const vector<vector<float>>& vec_of_rot_axes) {
cout << "Hello! This is find_mode() function" << endl;
int index = 0, counter = 0, max_counted = 0;
vector<float> 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::milliseconds>(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<std::mutex> 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<cv::Mat> queue;
int cnt = 0;
const double time_step = 1.0;
double ts = 0;
char matrix_name[100];
vector<float> vec_of_deg, values;
vector<vector<float>> 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<float> curr_vec;
vector<float> mode1, mode2;
int number_of_detected_frames = 0;
vector<vector<float>> accum, accum2;
int counter2, j = 0;
std::cout << "J is: " << j;
vector<float> 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<std::mutex> guard(g_mutex);
// if (frameQueue.size() > 0) {
// frame = frameQueue.front();
// frameQueue.pop_front();
// std::cout << "we have a frame to process..." << std::endl;
// }
// }
// cv::Mat Tcw = SLAM.TrackMonocular(frame, ts, vector<ORB_SLAM3::IMU::Point>(), "");
// cv::Mat Rot(3, 3, CV_32F, 0.0);
// std::cout << Tcw << std::endl;
//}
//// FOR TESTING
// --------------------------------- SLAM SYSTEM VARIABLES ---------------------------------
/* Let's do two steps outside the loop.*/
for (int i = 1; i <= 2; i++) {
//if (use_gui) {
// cv::namedWindow("preview", 1);
//}
//else {
// // cv::namedWindow("no preview", 1);
//}
cv::Mat frame;
char* buffer = nullptr;
// EXTRACTING FRAME HERE.
{
std::lock_guard<std::mutex> guard(g_mutex);
if (frameQueue.size() > 0) {
frame = frameQueue.front();
frameQueue.pop_front();
std::cout << "we have a frame to process..." << std::endl;
}
}
cv::Mat Tcw = SLAM.TrackMonocular(frame, ts, vector<ORB_SLAM3::IMU::Point>(), "");
cv::Mat Rot(3, 3, CV_32F, 0.0);
std::cout << Tcw << std::endl;
if (!Tcw.empty()) {
sprintf(matrix_name, "matrix%d", cnt);
extract_rot(Rot, Tcw);
// cout << "Extracted rotation matrix is: " << Rot;
auto deg_vec = extract_deg(Rot);
// QUESTION 2.
curr_deg = -deg_vec.first * 57.29;
// TODO: Invert curr_vec too. (put the minus sign to each element). (You can define the - operator fot the vector).
curr_vec = deg_vec.second;
cout << "Successfully created curr_deg and curr_vec" << endl;
// LET'S DEFINE CONSTANT TO ZERO OUT THE START
if (i == 1) {
DIFF_TO_CENTER = 0.0;
}
vec_of_deg.push_back(curr_deg - DIFF_TO_CENTER);
vec_of_rot_axis.push_back(curr_vec);
values.push_back(curr_deg - DIFF_TO_CENTER);
cout << "Successfully pushed to the vectors " << endl;
//cout << curr_deg - DIFF_TO_CENTER << " " << curr_vec[0] << " " << curr_vec[1] << " " << curr_vec[2] << endl;
// SEND THE RESULT THROUGH OSC
//outfile << curr_deg - DIFF_TO_CENTER << " " << curr_vec[0] << " " << curr_vec[1] << " " << curr_vec[2] << endl;
cout << "Successfully written to the file" << endl;
j++;
}
cnt++;
ts += time_step;
}
while (true) {
cv::Mat frame;
char* buffer = nullptr;
{
std::lock_guard<std::mutex> guard(g_mutex);
if (frameQueue.size() > 0) {
frame = frameQueue.front();
frameQueue.pop_front();
std::cout << "we have a frame to process..." << std::endl;
if (!frame.empty()) {
cv::Mat Tcw = SLAM.TrackMonocular(frame, ts, vector<ORB_SLAM3::IMU::Point>(), "");
cv::Mat Rot(3, 3, CV_32F, 0.0);
std::cout << Tcw << std::endl;
if (!Tcw.empty()) {
sprintf(matrix_name, "matrix%d", cnt);
extract_rot(Rot, Tcw);
// cout << "Extracted rotation matrix is: " << Rot;
// Extract the degree and the vector from the rotation matrix.
auto deg_vec = extract_deg(Rot); // returns a degree and a vector of rotation.
float new_deg = -deg_vec.first * 57.29 - DIFF_TO_CENTER;
vector<float> new_vec = deg_vec.second;
cout << "Successfully created curr_deg and curr_vec" << endl;
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;
}

38
my_ndi_source.cpp
View File

@@ -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<uint8_t>(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 = "<metadata_string " + std::to_string(total_frames_counter) + ">";
std::string metadata_string = "<metadata_string " + std::to_string(total_frames_counter) + std::to_string(cnt) + ">";
// (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<duration<float>>(high_resolution_clock::now() - start_send).count());
cnt++;
}
// Destroy the NDI sender