915 lines
37 KiB
C++
915 lines
37 KiB
C++
// Created by ivan on 16.05.2022.
|
||
|
||
// Бинарник, который принимает координаты камеры и кадр и отрисовывает
|
||
// изображение с кубами. Это необходимо для проверки работы первого бинарника.
|
||
|
||
#include <gst/gst.h>
|
||
#include <gst/app/gstappsink.h>
|
||
#include <gst/video/gstvideometa.h>
|
||
#include <deque>
|
||
#include <iostream>
|
||
#include <mutex>
|
||
#include <opencv2/opencv.hpp>
|
||
#include <pangolin/pangolin.h>
|
||
#include <cstring>
|
||
#include <unistd.h>
|
||
#include <thread>
|
||
|
||
#include "osc/OscReceivedElements.h"
|
||
#include "osc/OscPacketListener.h"
|
||
#include "ip/UdpSocket.h"
|
||
#include "osc/OscOutboundPacketStream.h"
|
||
#include "sophus/se3.hpp"
|
||
|
||
#define MY_GST_USE_OPENCV
|
||
// CHANGE HERE TO MAKE PORT
|
||
#define ADDRESS "127.0.0.1"
|
||
#define PORT 7000
|
||
#define OUTPUT_BUFFER_SIZE 1024
|
||
|
||
#if defined(__BORLANDC__) // workaround for BCB4 release build intrinsics bug
|
||
namespace std {
|
||
using ::__strcmp__; // avoid error: E2316 '__strcmp__' is not a member of 'std'.
|
||
}
|
||
#endif
|
||
|
||
std::mutex g_mutex;
|
||
std::deque<cv::Mat> frameQueue;
|
||
std::deque<double> timestampsQueue;
|
||
//std::deque<std::string> TwcQueue;
|
||
std::unordered_map<double, Sophus::SE3f> TwcMap;
|
||
|
||
pangolin::OpenGlRenderState ar_3d_camera;
|
||
pangolin::OpenGlMatrixSpec P;
|
||
pangolin::OpenGlMatrix Twc;
|
||
|
||
pangolin::Var<bool> showCube1("menu.Cube1", true, false);
|
||
pangolin::Var<bool> showCube2("menu.Cube2", true, false);
|
||
pangolin::Var<bool> showCube3("menu.Cube3", true, false);
|
||
pangolin::Var<bool> showCube4("menu.Cube4", true, false);
|
||
pangolin::Var<bool> showCube5("menu.Cube5", true, false);
|
||
pangolin::Var<bool> showCube6("menu.Cube6", true, false);
|
||
pangolin::Var<bool> showCube7("menu.Cube7", true, false);
|
||
pangolin::Var<bool> drawTexture("menu.drawTexture", true, false);
|
||
pangolin::Var<bool> pauseSeq("menu.pauseSeq", false, true);
|
||
pangolin::Var<bool> saveRender("menu.saveRender", false, false);
|
||
pangolin::Var<float> saveRenderScale("menu.saveRenderScale", 1.0, 0.1, 2.0);
|
||
|
||
pangolin::Var<int> cubeSize("menu.cubeSize", 2, 1, 5);
|
||
pangolin::Var<bool> drawLineCubeBool("menu.drawLineCubeBool", false, true);
|
||
pangolin::Var<double> xSkew("menu.x", 0, -40, 40);
|
||
pangolin::Var<double> ySkew("menu.y", 0, -5, 5);
|
||
pangolin::Var<double> zSkew("menu.z", 0, -360, 360);
|
||
|
||
void drawLinesCube(float x, float y, float z, int scale){
|
||
// float size = 1.0;
|
||
// float size = 1.0;
|
||
pangolin::OpenGlMatrix M = pangolin::OpenGlMatrix::Translate(-x,-y,-z);
|
||
x = 0; y = 0; z = 0;
|
||
glPushMatrix();
|
||
M.Multiply();
|
||
glColor3f(0, 1, 0);
|
||
glLineWidth(5);
|
||
// glColor3ui(133, 247, 208);
|
||
glBegin(GL_LINES);
|
||
|
||
// Bottom
|
||
// glColor3ui(133, 247, 208);
|
||
glVertex3f(x, y, z);
|
||
glVertex3f(x, y+scale, z);
|
||
|
||
// glColor3ui(253, 59, 86);
|
||
glVertex3f(x, y, z);
|
||
glVertex3f(x+scale, y, z);
|
||
|
||
// glColor3ui(147, 175, 215);
|
||
glVertex3f(x, y+scale, z);
|
||
glVertex3f(x+scale, y+scale, z);
|
||
|
||
// glColor3ui(80, 209, 168);
|
||
glVertex3f(x+scale, y, z);
|
||
glVertex3f(x+scale, y+scale, z);
|
||
|
||
// Top
|
||
// glColor3ui(154, 13, 88);
|
||
glVertex3f(x, y, z+scale);
|
||
glVertex3f(x, y+scale, z+scale);
|
||
|
||
// glColor3ui(253, 59, 86);
|
||
glVertex3f(x, y, z+scale);
|
||
glVertex3f(x+scale, y, z+scale);
|
||
|
||
// glColor3ui(5, 26, 72);
|
||
glVertex3f(x, y+scale, z+scale);
|
||
glVertex3f(x+scale, y+scale, z+scale);
|
||
|
||
// glColor3ui(72, 182, 8);
|
||
glVertex3f(x+scale, y, z+scale);
|
||
glVertex3f(x+scale, y+scale, z+scale);
|
||
|
||
// Sides
|
||
// glColor3ui(28, 122, 71);
|
||
glVertex3f(x, y, z);
|
||
glVertex3f(x, y, z+scale);
|
||
|
||
// glColor3ui(244, 207, 185);
|
||
glVertex3f(x, y+scale, z);
|
||
glVertex3f(x, y+scale, z+scale);
|
||
|
||
// glColor3ui(88, 153, 225);
|
||
glVertex3f(x+scale, y, z);
|
||
glVertex3f(x+scale, y, z+scale);
|
||
|
||
// glColor3ui(184, 151, 253);
|
||
glVertex3f(x+scale, y+scale, z);
|
||
glVertex3f(x+scale, y+scale, z+scale);
|
||
|
||
glEnd();
|
||
glPopMatrix();
|
||
}
|
||
|
||
void draw_scene_no_camera(pangolin::View& view){
|
||
// UNUSED(view);
|
||
view.Activate(ar_3d_camera);
|
||
|
||
// myMapDrawer->DrawCurrentCamera(Twc);
|
||
// if(menuShowKeyFrames || menuShowGraph || menuShowInertialGraph || menuShowOptLba)
|
||
// myMapDrawer->DrawKeyFrames(menuShowKeyFrames,menuShowGraph, menuShowInertialGraph, menuShowOptLba);
|
||
// if(menuShowPoints)
|
||
// myMapDrawer->DrawMapPoints();
|
||
|
||
glClear(GL_DEPTH_BUFFER_BIT);
|
||
|
||
glMatrixMode(GL_PROJECTION);
|
||
P.Load();
|
||
|
||
glMatrixMode(GL_MODELVIEW);
|
||
ar_3d_camera.Follow(Twc);
|
||
|
||
if (drawLineCubeBool){
|
||
drawLinesCube(xSkew, ySkew, zSkew, cubeSize);
|
||
}
|
||
|
||
// For the 1st sequence mono-kitti
|
||
// if (showCube1) {drawLinesCube(0.0, 1.0, -95.0, cubeSize);}
|
||
// if (showCube2) {drawLinesCube(0.0, 1.5, -35.0, cubeSize);}
|
||
// if (showCube3) {drawLinesCube(5.0, 1.5, -40.0, cubeSize);}
|
||
// if (showCube4) {drawLinesCube(-32.0, 1.5, -85.0, cubeSize);}
|
||
// if (showCube5) {drawLinesCube(-32.0, 1.5, -95.0, cubeSize);}
|
||
// if (showCube6) {drawLinesCube(-64.0, 3.0, -95.0, cubeSize);}
|
||
// if (showCube7) {drawLinesCube(-64.0, 3.0, -85.0, cubeSize);}
|
||
|
||
// For the 6th sequence mono-kitti
|
||
if (showCube1) {drawLinesCube(5.15, 0.82, -33.81, cubeSize);}
|
||
if (showCube2) {drawLinesCube(0.7, 0.56, -11.5, cubeSize);}
|
||
if (showCube3) {drawLinesCube(5.15, 0.8, -95.0, cubeSize);}
|
||
if (showCube4) {drawLinesCube(4.6, 2.1, -130.3, cubeSize);}
|
||
// This ones' height can be wrong
|
||
if (showCube5) {drawLinesCube(4.1, 5.9, -346.5, cubeSize);}
|
||
if (showCube6) {drawLinesCube(25.7, 4.8, -346.5, cubeSize);}
|
||
// if (showCube7) {drawLinesCube(32.5, 3.8, -345.7, cubeSize);}
|
||
|
||
pangolin::glDrawAxis(Sophus::SE3d().matrix(), 1.0);
|
||
}
|
||
|
||
void TwcMsgProcessor(const std::string& msg){
|
||
double ts;
|
||
Eigen::Matrix<float, 3, 3> Rwc;
|
||
Eigen::Matrix<float, 1, 3> twc;
|
||
std::stringstream ss;
|
||
if (!msg.empty()){
|
||
ss << msg;
|
||
}
|
||
ss >> ts;
|
||
ss >> Rwc(0,0) >> Rwc(0,1) >> Rwc(0,2) >> twc(0) >>
|
||
Rwc(1,0) >> Rwc(1,1) >> Rwc(1,2) >> twc(1) >>
|
||
Rwc(2,0) >> Rwc(2,1) >> Rwc(2,2) >> twc(2);
|
||
Sophus::SE3f Twc;
|
||
Twc.setRotationMatrix(Rwc);
|
||
Twc.translation() = twc;
|
||
TwcMap[ts] = Twc;
|
||
}
|
||
|
||
namespace osc{
|
||
|
||
class OscReceiveTestPacketListener : public OscPacketListener{
|
||
protected:
|
||
|
||
void ProcessMessage( const osc::ReceivedMessage& m,
|
||
const IpEndpointName& remoteEndpoint )
|
||
{
|
||
(void) remoteEndpoint; // suppress unused parameter warning
|
||
|
||
// a more complex scheme involving std::map or some other method of
|
||
// processing address patterns could be used here
|
||
// (see MessageMappingOscPacketListener.h for example). however, the main
|
||
// purpose of this example is to illustrate and test different argument
|
||
// parsing methods
|
||
|
||
try {
|
||
// argument stream, and argument iterator, used in different
|
||
// examples below.
|
||
ReceivedMessageArgumentStream args = m.ArgumentStream();
|
||
ReceivedMessage::const_iterator arg = m.ArgumentsBegin();
|
||
|
||
if( std::strcmp( m.AddressPattern(), "/test1" ) == 0 ){
|
||
|
||
// example #1:
|
||
// parse an expected format using the argument stream interface:
|
||
bool a1;
|
||
osc::int32 a2;
|
||
float a3;
|
||
const char *a4;
|
||
args >> a1 >> a2 >> a3 >> a4 >> osc::EndMessage;
|
||
|
||
std::cout << "received '/test1' message with arguments: "
|
||
<< a1 << " " << a2 << " " << a3 << " " << a4 << "\n";
|
||
|
||
}else if( std::strcmp( m.AddressPattern(), "/test2" ) == 0 ){
|
||
|
||
// example #2:
|
||
// parse an expected format using the argument iterator interface
|
||
// this is a more complicated example of doing the same thing
|
||
// as above.
|
||
bool a1 = (arg++)->AsBool();
|
||
int a2 = (arg++)->AsInt32();
|
||
float a3 = (arg++)->AsFloat();
|
||
const char *a4 = (arg++)->AsString();
|
||
if( arg != m.ArgumentsEnd() )
|
||
throw ExcessArgumentException();
|
||
|
||
std::cout << "received '/test2' message with arguments: "
|
||
<< a1 << " " << a2 << " " << a3 << " " << a4 << "\n";
|
||
|
||
}else if( std::strcmp( m.AddressPattern(), "/test3" ) == 0 ){
|
||
|
||
// example #3:
|
||
// parse a variable argument format using the argument iterator
|
||
// interface. this is where it is necessary to use
|
||
// argument iterators instead of streams.
|
||
// When messages may contain arguments of varying type, you can
|
||
// use the argument iterator interface to query the types at
|
||
// runtime. this is more flexible that the argument stream
|
||
// interface, which requires each argument to have a fixed type
|
||
|
||
if( arg->IsBool() ){
|
||
bool a = (arg++)->AsBoolUnchecked();
|
||
std::cout << "received '/test3' message with bool argument: "
|
||
<< a << "\n";
|
||
}else if( arg->IsInt32() ){
|
||
int a = (arg++)->AsInt32Unchecked();
|
||
std::cout << "received '/test3' message with int32 argument: "
|
||
<< a << "\n";
|
||
}else if( arg->IsFloat() ){
|
||
float a = (arg++)->AsFloatUnchecked();
|
||
std::cout << "received '/test3' message with float argument: "
|
||
<< a << "\n";
|
||
}else if( arg->IsString() ){
|
||
const char *a = (arg++)->AsStringUnchecked();
|
||
std::cout << "received '/test3' message with string argument: '"
|
||
<< a << "'\n";
|
||
std::string msg(a);
|
||
TwcMsgProcessor(msg);
|
||
}else{
|
||
std::cout << "received '/test3' message with unexpected argument type\n";
|
||
}
|
||
|
||
if( arg != m.ArgumentsEnd() )
|
||
throw ExcessArgumentException();
|
||
|
||
|
||
}else if( std::strcmp( m.AddressPattern(), "/no_arguments" ) == 0 ){
|
||
|
||
args >> osc::EndMessage;
|
||
std::cout << "received '/no_arguments' message\n";
|
||
|
||
}else if( std::strcmp( m.AddressPattern(), "/a_bool" ) == 0 ){
|
||
|
||
bool a;
|
||
args >> a >> osc::EndMessage;
|
||
std::cout << "received '/a_bool' message: " << a << "\n";
|
||
|
||
}else if( std::strcmp( m.AddressPattern(), "/nil" ) == 0 ){
|
||
|
||
std::cout << "received '/nil' message\n";
|
||
|
||
}else if( std::strcmp( m.AddressPattern(), "/inf" ) == 0 ){
|
||
|
||
std::cout << "received '/inf' message\n";
|
||
|
||
}else if( std::strcmp( m.AddressPattern(), "/an_int" ) == 0 ){
|
||
|
||
osc::int32 a;
|
||
args >> a >> osc::EndMessage;
|
||
std::cout << "received '/an_int' message: " << a << "\n";
|
||
|
||
}else if( std::strcmp( m.AddressPattern(), "/a_float" ) == 0 ){
|
||
|
||
float a;
|
||
args >> a >> osc::EndMessage;
|
||
std::cout << "received '/a_float' message: " << a << "\n";
|
||
|
||
}else if( std::strcmp( m.AddressPattern(), "/a_char" ) == 0 ){
|
||
|
||
char a;
|
||
args >> a >> osc::EndMessage;
|
||
char s[2] = {0};
|
||
s[0] = a;
|
||
std::cout << "received '/a_char' message: '" << s << "'\n";
|
||
|
||
}else if( std::strcmp( m.AddressPattern(), "/an_rgba_color" ) == 0 ){
|
||
|
||
osc::RgbaColor a;
|
||
args >> a >> osc::EndMessage;
|
||
std::cout << "received '/an_rgba_color' message: " << a.value << "\n";
|
||
|
||
}else if( std::strcmp( m.AddressPattern(), "/a_midi_message" ) == 0 ){
|
||
|
||
osc::MidiMessage a;
|
||
args >> a >> osc::EndMessage;
|
||
std::cout << "received '/a_midi_message' message: " << a.value << "\n";
|
||
|
||
}else if( std::strcmp( m.AddressPattern(), "/an_int64" ) == 0 ){
|
||
|
||
osc::int64 a;
|
||
args >> a >> osc::EndMessage;
|
||
std::cout << "received '/an_int64' message: " << a << "\n";
|
||
|
||
}else if( std::strcmp( m.AddressPattern(), "/a_time_tag" ) == 0 ){
|
||
|
||
osc::TimeTag a;
|
||
args >> a >> osc::EndMessage;
|
||
std::cout << "received '/a_time_tag' message: " << a.value << "\n";
|
||
|
||
}else if( std::strcmp( m.AddressPattern(), "/a_double" ) == 0 ){
|
||
|
||
double a;
|
||
args >> a >> osc::EndMessage;
|
||
std::cout << "received '/a_double' message: " << a << "\n";
|
||
|
||
}else if( std::strcmp( m.AddressPattern(), "/a_string" ) == 0 ){
|
||
|
||
const char *a;
|
||
args >> a >> osc::EndMessage;
|
||
std::cout << "received '/a_string' message: '" << a << "'\n";
|
||
|
||
}else if( std::strcmp( m.AddressPattern(), "/a_symbol" ) == 0 ){
|
||
|
||
osc::Symbol a;
|
||
args >> a >> osc::EndMessage;
|
||
std::cout << "received '/a_symbol' message: '" << a.value << "'\n";
|
||
|
||
}else if( std::strcmp( m.AddressPattern(), "/a_blob" ) == 0 ){
|
||
|
||
osc::Blob a;
|
||
args >> a >> osc::EndMessage;
|
||
std::cout << "received '/a_blob' message\n";
|
||
|
||
}else{
|
||
std::cout << "unrecognised address pattern: "
|
||
<< m.AddressPattern() << "\n";
|
||
}
|
||
|
||
}catch( Exception& e ){
|
||
std::cout << "error while parsing message: "
|
||
<< m.AddressPattern() << ": " << e.what() << "\n";
|
||
}
|
||
}
|
||
};
|
||
|
||
|
||
void RunReceiveTest( int port )
|
||
{
|
||
osc::OscReceiveTestPacketListener listener;
|
||
UdpListeningReceiveSocket s(
|
||
IpEndpointName( IpEndpointName::ANY_ADDRESS, port ),
|
||
&listener );
|
||
|
||
std::cout << "listening for input on port " << port << "...\n";
|
||
std::cout << "press ctrl-c to end\n";
|
||
|
||
s.RunUntilSigInt();
|
||
|
||
std::cout << "finishing.\n";
|
||
}
|
||
|
||
// namespace osc
|
||
}
|
||
|
||
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++;
|
||
|
||
std::cout << "nnew frame " << framecount << std::endl;
|
||
|
||
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;
|
||
std::string meta(x);
|
||
int idx = meta.find("timestamp: ");
|
||
int end_idx = meta.find(">");
|
||
std::string ts_str = meta.substr(idx+11, end_idx-1-(idx+11));
|
||
// std::cout << ts_str << std::endl;
|
||
std::lock_guard<std::mutex> guard(g_mutex);
|
||
timestampsQueue.push_back(std::stod(ts_str));
|
||
|
||
}
|
||
} 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]";
|
||
} 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 ---------------
|
||
|
||
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;
|
||
|
||
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);
|
||
|
||
cv::Mat frame(cv::Size(width, height), CV_8UC4, (char*)map.data, cv::Mat::AUTO_STEP);
|
||
|
||
std::printf("The image width and height: %d %d", frame.cols, frame.rows);
|
||
|
||
// int frameSize = map.size;
|
||
// std::cout << "size from caps = (" << width << "," << height << ")" << "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);\
|
||
std::cout << "Got the frame. Saving it..." << std::endl;
|
||
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 gint repeats = 2;
|
||
static gchar* ndi_name = nullptr;
|
||
static gint use_gui = 1;
|
||
static gboolean beep = FALSE;
|
||
|
||
static GOptionEntry entries[] =
|
||
{
|
||
{ "repeats", 'r', 0, G_OPTION_ARG_INT, &repeats, "Среднее число повторений N", "N" },
|
||
{ "ndi-name", 'n', 0, G_OPTION_ARG_STRING, &ndi_name, "you can enter the string here (ndi-name)", "M" },
|
||
{ "gui", 'g', 0, G_OPTION_ARG_INT, &use_gui, "use gui", nullptr },
|
||
{ "beep", 'b', 0, G_OPTION_ARG_NONE, &beep, "Сигнал при выполнениии", NULL },
|
||
{ NULL }
|
||
};
|
||
|
||
int main (int argc, char *argv[]) {
|
||
std::cout << "argc = " << argc << std::endl;
|
||
GError *error = nullptr;
|
||
GOptionContext *context;
|
||
|
||
char** argv_gst;
|
||
int argc_gst = 2;
|
||
argv_gst = new char* [2];
|
||
argv_gst[0] = new char[200];
|
||
argv_gst[1] = new char[200];
|
||
|
||
strcpy(argv_gst[0], argv[0]);
|
||
strcpy(argv_gst[1], argv[1]);
|
||
// std::cout << "The second parameter of the arguments: " << argv[2] << " " << argv[1] << " " << argv[0] << std::endl;
|
||
|
||
|
||
context = g_option_context_new("- test tree model performance");
|
||
g_option_context_add_main_entries(context, entries, "bla");
|
||
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_gst);
|
||
|
||
std::stringstream ss;
|
||
ss << "ndisrc ndi-name=\"" << ndi_name << "\" ! ndisrcdemux name=demux "
|
||
<< "demux.video ! queue ! tee name=my_tee "
|
||
<< "my_tee. ! queue ! videoconvert ! autovideosink "
|
||
<< "my_tee. ! queue ! videoconvert ! appsink name=my_sink";
|
||
std::string my_pipeline = ss.str();
|
||
|
||
GstElement *pipeline = gst_parse_launch(my_pipeline.c_str(), nullptr);
|
||
|
||
/* 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);
|
||
|
||
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);
|
||
|
||
/* 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;
|
||
}
|
||
|
||
// ------------ Pangolin ------------
|
||
// Create OpenGL window in single line
|
||
pangolin::CreateWindowAndBind("Main",640,480);
|
||
|
||
pangolin::CreatePanel("menu").SetBounds(0.0,1.0,0.0,pangolin::Attach::Pix(175));
|
||
// 3D Mouse handler requires depth testing to be enabled
|
||
glEnable(GL_DEPTH_TEST);
|
||
|
||
pangolin::OpenGlRenderState s_cam(
|
||
pangolin::ProjectionMatrix(640,480,420,420,320,240,0.1,1000),
|
||
pangolin::ModelViewLookAt(0,0,0, 0,0,1, pangolin::AxisNegY)
|
||
);
|
||
|
||
// Aspect ratio allows us to constrain width and height whilst fitting within specified
|
||
// bounds. A positive aspect ratio makes a view 'shrink to fit' (introducing empty bars),
|
||
// whilst a negative ratio makes the view 'grow to fit' (cropping the view).
|
||
pangolin::View& d_cam = pangolin::Display("cam")
|
||
.SetBounds(0,1.0f,pangolin::Attach::Pix(175),1.0f,-640/480.0)
|
||
.SetHandler(new pangolin::Handler3D(s_cam));
|
||
|
||
float fxForAR = 707.0912, fyForAR = 707.0912, cxForAR = 601.8873, cyForAR = 183.1104;
|
||
|
||
ar_3d_camera
|
||
.SetProjectionMatrix(pangolin::ProjectionMatrix((int)1226.0, (int)370, fxForAR, fyForAR,
|
||
cxForAR, cyForAR, 0.1, 1000))
|
||
.SetModelViewMatrix(pangolin::ModelViewLookAt(0, 0, 0, 0, 0, 1, pangolin::AxisNegY));
|
||
|
||
// Define the projection matrix
|
||
P = pangolin::ProjectionMatrix((int)1226.0, (int)370.0, fxForAR, fyForAR, cxForAR, cyForAR, 0.001, 10000);
|
||
|
||
pangolin::GlTexture imageTexture(1226,370,GL_RGBA,false,0,GL_RGBA,GL_UNSIGNED_BYTE);
|
||
|
||
// This view will take up no more than a third of the windows width or height, and it
|
||
// will have a fixed aspect ratio to match the image that it will display. When fitting
|
||
// within the specified bounds, push to the top-left (as specified by SetLock).
|
||
pangolin::View& d_image = pangolin::Display("image")
|
||
.SetBounds(0,pangolin::Attach::Pix(370), pangolin::Attach::Pix(175),pangolin::Attach::Pix(175 + 1226), 1226.0/370.0)
|
||
.SetHandler(new pangolin::Handler3D(ar_3d_camera))
|
||
.SetLock(pangolin::LockLeft, pangolin::LockTop);
|
||
|
||
d_image.extern_draw_function = draw_scene_no_camera;
|
||
// std::cout << "The second parameter of the arguments: " << argv[2] << " " << argv[1] << " " << argv[0] << std::endl;
|
||
|
||
|
||
#ifdef MY_GST_USE_OPENCV
|
||
auto lambda_1 = [&d_cam, &s_cam, &imageTexture, &d_image] () {
|
||
int counter = 0;
|
||
cv::Mat frame;
|
||
double ts;
|
||
Sophus::SE3f Twc;
|
||
while (true) {
|
||
if (use_gui) {
|
||
cv::namedWindow("preview", 1);
|
||
} else {
|
||
// cv::namedWindow("no preview", 1);
|
||
}
|
||
|
||
{
|
||
std::lock_guard<std::mutex> guard(g_mutex);
|
||
if (frameQueue.size() > 0) {
|
||
frame = frameQueue.front();
|
||
ts = timestampsQueue.front();
|
||
frameQueue.pop_front();
|
||
timestampsQueue.pop_front();
|
||
std::cout << "we have a frame to process..." << std::endl;
|
||
}
|
||
}
|
||
|
||
if (!frame.empty()){
|
||
cv::Rect leftROI(0, 0, frame.cols/2, frame.rows);
|
||
cv::Rect rightROI(frame.cols/2, 0, frame.cols, frame.rows);
|
||
cv::Mat imLeft = frame(leftROI);
|
||
std::printf("The Left Image width and height: %d %d", imLeft.cols, imLeft.rows);
|
||
cv::Mat imRight = frame(rightROI);
|
||
std::printf("The Right Image width and height: %d %d", imRight.cols, imRight.rows);
|
||
std::printf("The timestamp is: ", ts);
|
||
|
||
if ( TwcMap.find(ts) != TwcMap.end() ){
|
||
Twc = TwcMap[ts];
|
||
}
|
||
|
||
// Visualizing using Pangolin.
|
||
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
|
||
|
||
d_cam.Activate(s_cam);
|
||
glClearColor(1.0, 1.0, 1.0, 1.0);
|
||
glColor3f(1.0,1.0,1.0);
|
||
if (drawLineCubeBool){
|
||
drawLinesCube(xSkew, ySkew, zSkew, cubeSize);
|
||
std::cout << "DrawLinesCube after d_cam Activate success" << std::endl;
|
||
}
|
||
|
||
// For the 1st sequence mono-kitti
|
||
// if (showCube1) {drawLinesCube(0.0, 1.0, -95.0, cubeSize);}
|
||
// if (showCube2) {drawLinesCube(0.0, 1.5, -35.0, cubeSize);}
|
||
// if (showCube3) {drawLinesCube(5.0, 1.5, -40.0, cubeSize);}
|
||
// if (showCube4) {drawLinesCube(-32.0, 1.5, -85.0, cubeSize);}
|
||
// if (showCube5) {drawLinesCube(-32.0, 1.5, -95.0, cubeSize);}
|
||
// if (showCube6) {drawLinesCube(-64.0, 3.0, -95.0, cubeSize);}
|
||
// if (showCube7) {drawLinesCube(-64.0, 3.0, -85.0, cubeSize);}
|
||
|
||
// For the 6th sequence mono-kitti
|
||
if (showCube1) {drawLinesCube(5.15, 0.82, -33.81, cubeSize);}
|
||
if (showCube2) {drawLinesCube(0.7, 0.56, -11.5, cubeSize);}
|
||
if (showCube3) {drawLinesCube(5.15, 0.8, -95.0, cubeSize);}
|
||
if (showCube4) {drawLinesCube(4.6, 2.1, -130.3, cubeSize);}
|
||
// This ones' height can be wrong
|
||
if (showCube5) {drawLinesCube(4.1, 5.9, -346.5, cubeSize);}
|
||
if (showCube6) {drawLinesCube(25.7, 4.8, -346.5, cubeSize);}
|
||
// if (showCube7) {drawLinesCube(32.5, 3.8, -345.7, cubeSize);}
|
||
|
||
pangolin::glDrawAxis(Sophus::SE3d().matrix(), 1.0);
|
||
|
||
//display the image
|
||
d_image.Activate(ar_3d_camera);
|
||
glColor3f(1.0,1.0,1.0);
|
||
// Shows the image uploaded to the device (GPU or CPU) in the ViewPort.
|
||
cv::cvtColor(imLeft, imLeft, cv::COLOR_BGR2RGBA);
|
||
imageTexture.Upload(imLeft.data,GL_RGBA,GL_UNSIGNED_BYTE);
|
||
// std::cout << "Number of channels: " << imLeft.channels() << std::endl;
|
||
// s_cam.Follow(Twc);
|
||
if (drawTexture){
|
||
imageTexture.RenderToViewportFlipY();
|
||
}
|
||
if (saveRender){
|
||
d_image.SaveRenderNow(std::to_string(counter) + "_demo", saveRenderScale);
|
||
// ar_view.SaveRenderNow(mpFrameDrawer->getCurrImgFileName() + "_demo", saveRenderScale);
|
||
}
|
||
pangolin::FinishFrame();
|
||
}
|
||
|
||
counter += 1;
|
||
usleep(33);
|
||
|
||
if (use_gui) {
|
||
if (!frame.empty()) {
|
||
cv::Mat edges;
|
||
cvtColor(frame, edges, cv::COLOR_BGR2BGRA);
|
||
cv::imshow("preview", frame);
|
||
}
|
||
cv::waitKey(30);
|
||
}
|
||
// delete [] buffer;
|
||
}
|
||
};
|
||
|
||
auto lambda_2 = [&argc, &argv] () {
|
||
if( argc >= 2 && std::strcmp( argv[1], "-h" ) == 0 ){
|
||
std::cout << "usage: OscReceiveTest [port]\n";
|
||
return 0;
|
||
}
|
||
int port = 7000;
|
||
if( argc >= 2 ) {
|
||
// std::cout << "The second parameter of the arguments: " << argv[2] << " " << argv[1] << " " << argv[0] << std::endl;
|
||
port = std::atoi( argv[2] );
|
||
std::cout << "Detected the port input: " << port << std::endl;
|
||
}
|
||
// Listener to fill the TwcQueue
|
||
osc::RunReceiveTest( port );
|
||
};
|
||
|
||
std::thread t2(lambda_2);
|
||
std::cout << "Lambda 2 function works in the thread t2 ..." << std::endl;
|
||
std::thread t1(lambda_1);
|
||
std::cout << "Lambda 1 function works in the thread t1 ..." << std::endl;
|
||
|
||
bool is_terminated = false;
|
||
while (!is_terminated) {
|
||
// g_main_iteration(false);
|
||
g_main_context_iteration(NULL, false);
|
||
}
|
||
t2.join();
|
||
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;
|
||
}
|
||
|
||
|
||
|