added sending images to esp32

src/network/Server.cpp

@@ -4,20 +4,44 @@ void Server::start(){
     std::cout << "Initialising TCP sever" << std::endl;
     server.setup(port);
     osc_sender.setup(OSC_HOST, OSC_PORT);
-    http.setup(http_ip, http_port, http_page);
+    //http.setup(http_ip, http_port, http_page);
     is_active = true;
     previous_embedding = embedding;
     last_change_time = std::chrono::steady_clock::now();
+    lastUpdateTime = std::chrono::steady_clock::now();
+
+    /* allocate pixels in vector */
+    esp_pixels.resize(4);
+    for(auto& p : esp_pixels){
+        p.allocate(128, 168, GL_RGB);
+    }
 }
 
-void Server::update(){
+void Server::update(ofFbo& esp_comp){
+    /* set all past clients to false, to check connection */
+    for(auto& c : clients){
+        c.second.connected = false;
+    }
+
+    ofPixels fboPixels;
+    esp_comp.readToPixels(fboPixels);
+
+    vector<ofImage> extractedImages;
+    extractedImages.resize(4);
+
+    for (int i = 0; i < 4; i++) {
+        ofPixels cropPixels;
+        fboPixels.cropTo(cropPixels, regions[i][0], regions[i][1], regions[i][2], regions[i][3]);
+        extractedImages[i].setFromPixels(cropPixels);
+    }
+
 
     for ( int i = 0; i < server.getLastID(); i++){
         if (server.isClientConnected(i)) {
             const int buffer_size = 8;
             char buffer[buffer_size];
             int bytes_recieved = server.receiveRawBytes(i, buffer, buffer_size);
-
+            
             if (bytes_recieved == buffer_size){
                 float value;
                 int id;
@@ -26,29 +50,39 @@ void Server::update(){
 
                 std::string ip_address = server.getClientIP(i);
 
-                addOrUpdateClient(id, value, ip_address);
+                addOrUpdateClient(id, value, ip_address, true);
+
+                std::cout << ip_address << " : " << id <<  std::endl;
             }
+
+            
         }
     }
 
+    sendImagesToClients(extractedImages);
+
     updateEmbedding();
+    
+    /* check if inactive -> query vptree -> move camera */
     checkActivity();
-    sendOSCMessage();
+    //sendOSCMessage();
+}
 
-    if(debug){
-        printClients();
-    }
+void Server::disconnectAllClients(){
+    server.disconnectAllClients();
 }
 
-void Server::addOrUpdateClient(int client_id, float value, const std::string& ip_address){
+void Server::addOrUpdateClient(int client_id, float value, const std::string& ip_address, bool _connected){
     ClientInfo client;
 
     client.ip_address = ip_address;
     client.value = value;
+    client.connected = _connected;
 
     clients[client_id] = client;
 }
 
+
 void Server::updateEmbedding() {
     std::vector<std::string> emotionNames = {
         "angry", "disgust", "fear", "happy", "sad", "surprise", "neutral"
@@ -83,13 +117,34 @@ void Server::printClients(){
     std::cout << is_active << std::endl;
 }
 
+void Server::print(){
+    const std::vector<std::string> emotionNames = {
+        "angry", "disgust", "fear", "happy", "sad", "neutral"
+    };
+
+    std::ostringstream ss;
+
+    ss << "Connected Clients: " << num_of_clients << endl;
+    ss << "Embedding [";
+    
+    for (size_t i = 0; i < emotionNames.size(); ++i) {
+        if (i > 0) ss << ", ";
+        ss << std::fixed << std::setprecision(2) << embedding.emotions[emotionNames[i]];
+    }
+
+    ss << "]" << endl;
+    
+    ofSetColor(255, 255, 255);
+    ofDrawBitmapString(ss.str(), (ofGetWindowWidth() / 2) + 20 , 20);
+}
+
 /* check if the controllers are in use */
 void Server::checkActivity(){
     if (previous_embedding.emotions["neutral"] != embedding.emotions["neutral"]) {  // Check if embedding has changed
         last_change_time = std::chrono::steady_clock::now();  // Reset the timer if there is a change
         previous_embedding = embedding;  // Update the previous embedding to the current one
         is_active = true;
-        sendHttpRequest();
+        //sendHttpRequest();
     } else {
         // Calculate the time since the last change
         auto now = std::chrono::steady_clock::now();
@@ -149,4 +204,41 @@ void Server::sendHttpRequest(){
 
 std::vector<std::vector<double>> Server::generateRandomVectors(int count, int dimension){
     return tree.generateRandomVectors(count, dimension);
+}
+
+void Server::sendImagesToClients(vector<ofImage>& imgs){
+    /* convert to pixels */
+    int c = 0;
+    for(const auto& f :imgs){
+        esp_pixels[c] = f.getPixels();
+        c++;
+    }
+
+    int num_images = imgs.size();
+    if (num_images != 4) {
+        // No images to send, return early
+        return;
+    }
+
+    ofSetLogLevel(OF_LOG_FATAL_ERROR);
+
+    for (int i = 0; i < server.getLastID(); i++) {
+        if (server.isClientConnected(i)) {     
+                
+            // Send RGB pixels as raw bytes & check if the ip addresses align -> get id
+            for(const auto& c_pair : clients){
+                std::cout << c_pair.first << std::endl;  
+                if(c_pair.second.ip_address == server.getClientIP(i)){
+                    
+                    unsigned char* image_data = esp_pixels[c_pair.first].getData();
+                    server.sendRawBytes(i, (char*)image_data, esp_pixels[c_pair.first].size());
+                }
+            }            
+        }
+    }
+    ofSetLogLevel(OF_LOG_NOTICE);
+}
+
+void Server::close(){
+    server.close();
 }

src/network/Server.h

@@ -14,31 +14,35 @@
 struct ClientInfo {
     float value;
     std::string ip_address;
+    bool connected;
 };
 
 class Server{
     public:
-    Server(int _port, Embedding _embedding, VP _tree, bool debug, std::string _http_ip, int _http_port, std::string _http_page) 
+    Server(int _port, Embedding& _embedding, VP& _tree, bool debug, std::string _http_ip, int _http_port, std::string _http_page) 
         : port(_port), embedding(_embedding), tree(_tree), debug(debug), http_ip(_http_ip), http_port(_http_port), http_page(_http_page) {}
 
     void start();
-    void update();
-    void addOrUpdateClient(int client_id, float value, const std::string& ip_address);
+    void update(ofFbo& esp_comp);
+    void addOrUpdateClient(int client_id, float value, const std::string& ip_address, bool _connected);
     void printClients();
     void updateEmbedding();
     void checkActivity();
     void sendHttpRequest();
     void sendOSCMessage();
+    void print();
+    void sendImagesToClients(vector<ofImage>& imgs);
+    void disconnectAllClients();
+    void close();
     std::vector<std::vector<double>> generateRandomVectors(int count, int dimension);
     
-
-
     int port;
     ofxTCPServer server;
     std::unordered_map<int, ClientInfo> clients;
     bool debug;
     bool is_active;
     Embedding embedding;
+    VP tree;
 
     Request http;
     std::string http_ip;
@@ -46,13 +50,24 @@ class Server{
     std::string http_page;
     VPResp vp_resp;
 
+    int num_of_clients = 0;
+
+    vector<ofPixels> esp_pixels;
+
     private:
     Embedding previous_embedding;
     std::chrono::time_point<std::chrono::steady_clock> last_change_time;
 
     ofxOscSender osc_sender;
-    std::string past_audio_file;
+    std::string past_audio_file;    
 
-    /* vp tree */
-    VP tree;
+    std::chrono::steady_clock::time_point lastUpdateTime;
+    const std::chrono::milliseconds updateInterval{1000}; // 1 second interval
+
+    int regions[4][4] = {
+        {0, 0, 128, 168},    // Top-left
+        {128, 0, 128, 168},  // Top-right
+        {0, 168, 128, 168},  // Bottom-left
+        {128, 168, 128, 168} // Bottom-right
+    };
 };

src/ofApp.cpp

@@ -9,7 +9,6 @@ void ofApp::setup(){
     const int queryCount = 10;
     const int k = 5;  // Number of nearest neighbors to find
 
-    // Generate random vectors
     auto vectors = vp_tree.generateRandomVectors(vectorCount, dimension);
 
     vpt::VpTree tree = vp_tree.buildTree(vectors);
@@ -20,9 +19,23 @@ void ofApp::setup(){
     portrait_fbo.allocate((ofGetWindowWidth() / 3) * 1, ofGetWindowHeight(), GL_RGBA);
     comp_fbo.allocate(ofGetWindowWidth(), ofGetWindowHeight(), GL_RGBA);
     model_outptut_fbo.allocate((ofGetWindowWidth() / 3) * 2, ofGetWindowHeight(), GL_RGB);
+    model_esp_out_fbo.allocate(128 * 2, 168 * 2, GL_RGB);
 
+    /* k-d image comp (4-images) */
+    esp_comp_fbo.allocate(128 * 2, 168 * 2, GL_RGB);
+
+    /* input images for model */
+    model_image_esp.allocate(128 * 2, 168 * 2, OF_IMAGE_COLOR);
     model_image.allocate((ofGetWindowWidth() / 3) * 2, ofGetWindowHeight(), OF_IMAGE_COLOR);
 
+
+    /* allocated ofImages in esp_images */
+    for(int i = 0; i < 4; i++){
+        ofFbo temp;
+        temp.allocate(128, 168, GL_RGB);
+        esp_images.push_back(temp);
+    }
+
     /* of settings (not sure why putting this at the start of setup breaks the map - but it works!) */
     ofSetVerticalSync(true);
     ofDisableArbTex();
@@ -39,11 +52,10 @@ void ofApp::setup(){
     bullet.setup();
 
     depth_onnx.Setup(modelPath, true, true);
+    depth_onnx_esp.Setup(modelPath, true, true);
 
     depth_thread.setup(&model_image, &model_outptut_fbo, &depth_onnx);
-
-    server = std::make_unique<Server>(12345, embed, vp_tree, false, "192.168.0.253", 2000, "search");
-    server->start();
+    depth_esp.setup(&model_image_esp, &model_esp_out_fbo, &depth_onnx_esp);
 
     /* vp-test */
     auto queries = server->generateRandomVectors(queryCount, dimension);
@@ -90,12 +102,29 @@ void ofApp::setup(){
         bullet.addMesh(mesh_list[0], mesh_list[1], img.getTexture());
         mesh_list.clear();
     }
+
+    last_updated_time = ofGetElapsedTimef();
+
+    server = std::make_unique<Server>(6762, embed, vp_tree, false, "192.168.0.253", 2000, "search");
+    server->start();
 }
 
 //--------------------------------------------------------------
 void ofApp::update(){
+
+    server->update(model_esp_out_fbo);
+
+    float current_time = ofGetElapsedTimef();
+    if(current_time - last_updated_time >= 3){
+        getNearestImages();
+        last_updated_time = current_time;
+    }
+    
+
+
     if(ofGetFrameNum() < 1){
         depth_thread.start();
+        depth_esp.start();
     }
 
     /* write pixels to model input image */
@@ -105,7 +134,11 @@ void ofApp::update(){
 
     try{
         depth_thread.update();
+        depth_esp.update();
+
+        /* set output to fbo's */
         depth_onnx.SetPixels(model_outptut_fbo);
+        depth_onnx_esp.SetPixels(model_esp_out_fbo);
 
     } catch (exception e){
         std::cout << "Model did not run" << std::endl;
@@ -143,6 +176,9 @@ void ofApp::draw(){
 
     comp_fbo.draw(0,0);
     model_outptut_fbo.draw(ofGetWindowWidth() / 2, 0);
+    model_esp_out_fbo.draw(0, 0);
+
+    //server->print();
     //shader_fbo.draw(0, 0);
     // float planeScale = 0.75;
     // int planeWidth = ofGetWidth() * planeScale;
@@ -168,6 +204,53 @@ void ofApp::draw(){
     //t.draw(ofGetWindowWidth() / 2, 0);
 }
 
+/* creates an fbo with four cropped images, to preprare for model input */
+void ofApp::getNearestImages(){
+    esp_comp_fbo.begin();
+    ofClear(255, 255, 255, 0);
+
+    ofImage random_image;
+
+    int imageIndex = 0;
+    for(auto& img : esp_images){
+        random_image = images[ofRandom(images.size() - 1)];
+        // Calculate the scaling factor
+        float widthRatio = 128.0f / random_image.getWidth();
+        float heightRatio = 168.0f / random_image.getHeight();
+        float scale = std::max(widthRatio, heightRatio);
+        
+        // Calculate new dimensions
+        int newWidth = std::ceil(random_image.getWidth() * scale);
+        int newHeight = std::ceil(random_image.getHeight() * scale);
+        
+        // Resize the image
+        random_image.resize(newWidth, newHeight);
+        
+        // Calculate the crop position to center the image
+        int cropX = (newWidth - 128) / 2;
+        int cropY = (newHeight - 168) / 2;
+        
+        // Ensure crop dimensions don't exceed the image size
+        int cropWidth = std::min(128, newWidth);
+        int cropHeight = std::min(168, newHeight);
+        
+        // Calculate the drawing position based on the image index
+        int drawX = (imageIndex % 2) * 128;
+        int drawY = (imageIndex / 2) * 168;
+        
+        // Draw the resized and cropped image
+        random_image.drawSubsection(drawX, drawY, cropWidth, cropHeight, cropX, cropY);
+        
+        imageIndex++;
+        if (imageIndex >= 4) break; // Stop after drawing 4 images
+    }
+    esp_comp_fbo.end();
+
+    
+    esp_comp_fbo.readToPixels(esp_comp_pixels);
+    model_image_esp.setFromPixels(esp_comp_pixels);
+}
+
 //--------------------------------------------------------------
 void ofApp::keyPressed(int key){
 
@@ -223,3 +306,8 @@ void ofApp::dragEvent(ofDragInfo dragInfo){
 
 }
 
+void ofApp::exit(){
+    std::cout << "closing server" << std::endl;
+    server->close();
+}
+

src/ofApp.h

@@ -17,6 +17,7 @@ class ofApp : public ofBaseApp{
 		void setup();
 		void update();
 		void draw();
+		void getNearestImages();
 
 		void keyPressed(int key);
 		void keyReleased(int key);
@@ -29,6 +30,7 @@ class ofApp : public ofBaseApp{
 		void windowResized(int w, int h);
 		void dragEvent(ofDragInfo dragInfo);
 		void gotMessage(ofMessage msg);
+		void exit();
 
 		ofEasyCam cam;
 		ofImage img;
@@ -41,6 +43,10 @@ class ofApp : public ofBaseApp{
 		ofFbo portrait_fbo;
 		ofFbo comp_fbo;
 		ofFbo model_outptut_fbo;
+		ofFbo model_esp_out_fbo;
+
+		ofFbo esp_comp_fbo;
+		ofImage model_image_esp;
 
 		ofShader shaders;
 
@@ -50,7 +56,9 @@ class ofApp : public ofBaseApp{
 
 		/* onnx */
 		Onnx depth_onnx;
+		Onnx depth_onnx_esp;
 		ModelThread depth_thread;
+		ModelThread depth_esp;
 		ofImage model_image;
 		ofPixels map_pixels;
 
@@ -59,5 +67,10 @@ class ofApp : public ofBaseApp{
 
 		/* server */
 		std::unique_ptr<Server> server;
+
+		/* esp_image fbos */
+		vector<ofFbo> esp_images;
+		ofPixels esp_comp_pixels;
+		float last_updated_time;
 		
 };