Add pointcloud display

7 files changed, 372 insertions, 40 deletions

diff --git a/AlignSample.cpp b/AlignSample.cpp
index b6afc2a..50b818e 100644
--- a/AlignSample.cpp
+++ b/AlignSample.cpp
@@ -210,6 +210,31 @@ CorrelationResult AlignSample::crosscorrelate(size_t len)
     return result;
 }
 
+std::pair<std::vector<complexf>, std::vector<complexf> > AlignSample::get_samples(
+                size_t len, size_t rx_delay)
+{
+    std::pair<std::vector<complexf>, std::vector<complexf> > rval;
+
+    std::lock_guard<std::mutex> lock(m_mutex);
+    if (align() and
+            m_rxsamples.size() > len + rx_delay
+            and m_txsamples.size() > len) {
+
+        rval.first.reserve(len);
+        rval.second.reserve(len);
+
+        std::copy(m_rxsamples.begin() + rx_delay,
+                m_rxsamples.begin() + rx_delay + len,
+                std::back_inserter(rval.first));
+
+        std::copy(m_txsamples.begin(),
+                m_txsamples.begin() + len,
+                std::back_inserter(rval.second));
+    }
+
+    return rval;
+}
+
 void AlignSample::consume(size_t samples)
 {
     std::lock_guard<std::mutex> lock(m_mutex);
diff --git a/AlignSample.hpp b/AlignSample.hpp
index 5757ff9..f61d0bb 100644
--- a/AlignSample.hpp
+++ b/AlignSample.hpp
@@ -83,6 +83,9 @@ class AlignSample {
 
         CorrelationResult crosscorrelate(size_t len);
 
+        std::pair<std::vector<complexf>, std::vector<complexf> > get_samples(
+                size_t len, size_t rx_delay);
+
         void consume(size_t samples);
 
     private:
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 59067e1..2304b01 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -52,6 +52,8 @@ find_package(PkgConfig)
 pkg_check_modules(UHD uhd)
 pkg_check_modules(ZMQ libzmq)
 pkg_check_modules(FFTW3F fftw3f)
+pkg_check_modules(SDL sdl)
+pkg_check_modules(GLU glu)
 # Threads
 find_package(Threads REQUIRED)
 
@@ -64,10 +66,13 @@ list(APPEND odrdpd_sources
     main.cpp
     OutputUHD.cpp
     AlignSample.cpp
+    pointcloud.cpp
     )
 
 list(APPEND common_link_list
     ${UHD_LIBRARIES}
+    ${SDL_LIBRARIES}
+    ${GLU_LIBRARIES}
     ${ZMQ_LIBRARIES}
     ${FFTW3F_LIBRARIES}
     ${CMAKE_THREAD_LIBS_INIT})
diff --git a/main.cpp b/main.cpp
index 4379a3e..46137f7 100644
--- a/main.cpp
+++ b/main.cpp
@@ -22,6 +22,7 @@
  */
 
 #include "OutputUHD.hpp"
+#include "pointcloud.hpp"
 #include "AlignSample.hpp"
 #include "utils.hpp"
 #include <zmq.hpp>
@@ -58,6 +59,8 @@ size_t read_samples_from_file(FILE* fd, std::vector<complexf>& samples, size_t c
 
 AlignSample aligner;
 
+PointCloud cloud(10000);
+
 size_t do_receive(OutputUHD* output_uhd)
 {
     std::vector<complexf> samps(samps_per_buffer);
@@ -89,52 +92,86 @@ size_t do_receive(OutputUHD* output_uhd)
     return total_received;
 }
 
-void find_peak_correlation()
+const size_t correlation_length = 16 * 1024; // 8ms at 2048000
+
+void push_to_point_cloud(size_t rx_delay)
 {
-    FILE* fd = nullptr; //fopen("correlation.debug", "w");
+    auto points = aligner.get_samples(correlation_length, rx_delay);
 
-    while (running) {
-        const size_t correlation_length = 16 * 1024; // 8ms at 2048000
-        if (aligner.ready(correlation_length)) {
-            double max_norm = 0.0;
-            size_t pos_max = 0;
+    if (points.first.size() > 0) {
+        cloud.push_samples(points);
+    }
+}
 
-            auto result = aligner.crosscorrelate(correlation_length);
-            auto& xcs = result.correlation;
+size_t find_peak_correlation(size_t correlation_length)
+{
+    double max_norm = 0.0;
+    size_t pos_max = 0;
+
+    auto result = aligner.crosscorrelate(correlation_length);
+    auto& xcs = result.correlation;
+
+    // Find correlation peak
+    for (size_t offset = 0; offset < xcs.size(); offset++) {
+        complexf xc = xcs[offset];
+        if (std::norm(xc) >= max_norm) {
+            max_norm = std::norm(xc);
+            pos_max = offset;
+        }
+    }
+    char msg[512];
+    snprintf(msg, 512, "Max correlation is %f at %fms (%zu), with RX %fdB and TX %fdB, RXtime %f, TXtime %f\n",
+            std::sqrt(max_norm),
+            (double)pos_max / (double)samplerate * 1000.0,
+            pos_max,
+            10*std::log(result.rx_power),
+            10*std::log(result.tx_power),
+            result.rx_timestamp,
+            result.tx_timestamp);
+    std::cerr << msg;
+    std::this_thread::sleep_for(std::chrono::microseconds(1));
+
+    // Eat much more than we correlate, because correlation is slow
+    aligner.consume(204800);
+
+    return pos_max;
+}
 
-            // Find correlation peak
-            for (size_t offset = 0; offset < xcs.size(); offset++) {
-                complexf xc = xcs[offset];
-                if (fd) {
-                    fprintf(fd, "%f ", std::norm(xc));
-                }
-                if (std::norm(xc) >= max_norm) {
-                    max_norm = std::norm(xc);
-                    pos_max = offset;
-                }
+void analyse_correlation()
+{
+    const size_t num_analyse = 10;
+    std::vector<double> max_positions(num_analyse);
+
+    while (running) {
+        for (size_t i = 0; running and i < num_analyse; i++) {
+            if (aligner.ready(correlation_length)) {
+                max_positions[i] = find_peak_correlation(correlation_length);
             }
-            char msg[512];
-            snprintf(msg, 512, "Max correlation is %f at %fms (%zu), with RX %fdB and TX %fdB, RXtime %f, TXtime %f\n",
-                        std::sqrt(max_norm),
-                        (double)pos_max / (double)samplerate * 1000.0,
-                        pos_max,
-                        10*std::log(result.rx_power),
-                        10*std::log(result.tx_power),
-                        result.rx_timestamp,
-                        result.tx_timestamp);
-            if (fd) {
-                fprintf(fd, "\n%s", msg);
+            else {
+                MDEBUG("Waiting for correlation\n");
+                aligner.debug();
+                std::this_thread::sleep_for(std::chrono::seconds(1));
+            }
+        }
+
+        bool all_identical = true;
+
+        double mean = std::accumulate(max_positions.begin(), max_positions.end(), 0.0) / (double)max_positions.size();
+
+        for (size_t i = 0; i < num_analyse; i++) {
+            if (std::fabs(max_positions[i] - mean) > 1) {
+                all_identical = false;
+                break;
             }
-            std::cerr << msg;
-            std::this_thread::sleep_for(std::chrono::microseconds(1));
+        }
 
-            // Eat much more than we correlate, because correlation is slow
-            aligner.consume(204800);
+        if (all_identical) {
+            size_t delay_samples = max_positions[0];
+
+            push_to_point_cloud(delay_samples);
         }
         else {
-            MDEBUG("Waiting for correlation\n");
-            aligner.debug();
-            std::this_thread::sleep_for(std::chrono::seconds(1));
+            MDEBUG("Not all delays identical\n");
         }
     }
 }
@@ -172,7 +209,7 @@ int main(int argc, char **argv)
     zmq::socket_t zmq_sock(ctx, ZMQ_SUB);
 
     FILE* fd = nullptr;
-    if (uri == "test") {
+    if (uri == "test") { //{{{
         FILE* fd_rx = fopen("rx.test", "r");
         if (!fd_rx) {
             std::cerr << "fx_rx open error" << std::endl;
@@ -228,7 +265,7 @@ int main(int argc, char **argv)
             aligner.consume(correlation_length / 2);
         }
         return 0;
-    }
+    } // }}}
     else if (uri.find("tcp://") != 0) {
         fd = fopen(uri.c_str(), "rb");
         if (!fd) {
@@ -256,7 +293,7 @@ int main(int argc, char **argv)
 
     std::thread receive_thread(do_receive, &output_uhd);
 
-    std::thread correlator_thread(find_peak_correlation);
+    std::thread correlator_thread(analyse_correlation);
 
     do {
         const double first_sample_time = 4.0;
@@ -293,6 +330,15 @@ int main(int argc, char **argv)
         }
 
         total_samps_read += samps_read;
+
+        try {
+            cloud.handle_event();
+        }
+        catch (sdl_quit &e) {
+            running = false;
+        }
+
+        cloud.draw();
     }
     while (samps_read and sent and running);
     MDEBUG("Leaving main loop with running=%d\n", running ? 1 : 0);
diff --git a/pointcloud.cpp b/pointcloud.cpp
new file mode 100644
index 0000000..417c31c
--- /dev/null
+++ b/pointcloud.cpp
@@ -0,0 +1,183 @@
+/*
+   Copyright (C) 2015
+   Matthias P. Braendli, matthias.braendli@mpb.li
+
+    http://opendigitalradio.org
+ */
+/*
+   This file is part of ODR-DPD.
+
+   ODR-DPD is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as
+   published by the Free Software Foundation, either version 3 of the
+   License, or (at your option) any later version.
+
+   ODR-DPD is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with ODR-DPD.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "pointcloud.hpp"
+
+#include <iostream>
+#include <vector>
+
+#include <SDL/SDL.h>
+#include <GL/gl.h>
+#include <GL/glu.h>
+#include <algorithm>
+
+using namespace std;
+
+PointCloud::PointCloud(size_t num_points) :
+    m_num_points(num_points),
+    m_rxgain(1.0),
+    m_txgain(1.0)
+{
+    SDL_Init(SDL_INIT_VIDEO);
+    atexit(SDL_Quit);
+    SDL_WM_SetCaption("Point Cloud", NULL);
+    SDL_SetVideoMode(720,640, 32, SDL_OPENGL);
+
+    glMatrixMode(GL_PROJECTION);
+    glLoadIdentity();
+    gluPerspective(6,(double)640/480,1,1000);
+}
+
+// Not really a border, but you get the idea
+vector<Point> PointCloud::generate_border(void)
+{
+    vector<Point> points(4);
+    points[0].x = 0;
+    points[0].y = 0;
+
+    points[1].x = 1;
+    points[1].y = 0;
+
+    points[2].x = 0;
+    points[2].y = 1;
+
+    points[3].x = 1;
+    points[3].y = 1;
+
+    for (auto &p : points) {
+        p.r = 255;
+        p.g = 255;
+        p.b = 0;
+    }
+
+    return points;
+}
+
+void PointCloud::push_samples(std::pair<std::vector<complexf>, std::vector<complexf> > &data)
+{
+    auto &rx = data.first;
+    auto &tx = data.second;
+
+    if (rx.size() != tx.size()) {
+        throw std::runtime_error("RX and TX have different size");
+    }
+
+    std::lock_guard<std::mutex> lock(m_mutex);
+
+    for (size_t i = 0; i < rx.size(); i++) {
+        Point p;
+
+        // Magnitude is position
+        p.x = std::abs(rx[i]) * m_rxgain;
+        p.y = std::abs(tx[i]) * m_txgain;
+        p.z = 0.0;
+
+        double arg_rx = std::arg(rx[i]);
+        double arg_tx = std::arg(tx[i]);
+
+        // Phase is colour
+        p.r = p.g = p.b = 255;
+        if (arg_rx > arg_tx) {
+            int corr = 255 - 10 * (arg_rx - arg_tx);
+
+            if (corr < 0) {
+                corr = 0;
+            }
+
+            p.r = corr;
+        }
+        else {
+            int corr = 255 - 10 * (arg_tx - arg_rx);
+
+            if (corr < 0) {
+                corr = 0;
+            }
+
+            p.b = corr;
+        }
+
+        m_points.push_back(p);
+    }
+
+    if (m_points.size() > m_num_points + rx.size()) {
+        m_points.erase(m_points.begin(), m_points.begin() + rx.size());
+    }
+}
+
+void PointCloud::handle_event()
+{
+    SDL_Event event;
+    while (SDL_PollEvent(&event))
+    {
+        switch(event.type){
+            case SDL_QUIT:
+                throw sdl_quit();
+                break;
+        }
+    }
+}
+
+void PointCloud::draw()
+{
+    std::lock_guard<std::mutex> lock(m_mutex);
+
+    size_t num_points_drawn = std::min(m_num_points, m_points.size());
+    vector<Point> v(m_num_points);
+    for (size_t i = 0; i < num_points_drawn; i++) {
+        v[i] = m_points[i];
+    }
+
+    m_mutex.unlock();
+
+    vector<Point> border = generate_border();
+    v.insert(v.end(), border.begin(), border.end());
+
+    glPushAttrib(GL_ALL_ATTRIB_BITS);
+    glPushMatrix();
+
+    glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
+
+    glMatrixMode(GL_MODELVIEW);
+    glLoadIdentity();
+
+    gluLookAt(
+            0.5,0.5,10,
+            0.5,0.5,0,
+            0,1,0);
+
+    glPointSize(4.0);
+
+    glBegin(GL_POINTS);
+    for (size_t n = 0; n < v.size(); n++){
+        glColor3ub(v[n].r, v[n].g, v[n].b);
+        glVertex3d(v[n].x, v[n].y, v[n].z);
+    }
+    glEnd();
+
+    glFlush();
+    SDL_GL_SwapBuffers();
+
+    glPopMatrix();
+    glPopAttrib();
+}
+
diff --git a/pointcloud.hpp b/pointcloud.hpp
new file mode 100644
index 0000000..a2dc7c3
--- /dev/null
+++ b/pointcloud.hpp
@@ -0,0 +1,66 @@
+/*
+   Copyright (C) 2015
+   Matthias P. Braendli, matthias.braendli@mpb.li
+
+    http://opendigitalradio.org
+ */
+/*
+   This file is part of ODR-DPD.
+
+   ODR-DPD is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as
+   published by the Free Software Foundation, either version 3 of the
+   License, or (at your option) any later version.
+
+   ODR-DPD is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with ODR-DPD.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "utils.hpp"
+#include <exception>
+#include <deque>
+#include <vector>
+#include <complex>
+#include <mutex>
+
+// Exception to quit the program
+class sdl_quit : public std::exception {};
+
+struct Point {
+    double x, y, z;
+    uint8_t r, g, b;
+};
+
+
+class PointCloud
+{
+    public:
+        PointCloud(size_t num_points);
+
+        // Must call this regularly
+        void handle_event(void);
+
+        // The caller must guarantee that the two vectors have the same size
+        void push_samples(std::pair<std::vector<complexf>, std::vector<complexf> > &data);
+
+        // Update the display. Must be called from the same thread as the
+        // one that constructed this instance.
+        void draw(void);
+
+    private:
+        std::vector<Point> generate_border(void);
+
+        size_t m_num_points;
+
+        double m_rxgain;
+        double m_txgain;
+
+        std::mutex m_mutex;
+        std::deque<Point> m_points;
+};
+
diff --git a/utils.hpp b/utils.hpp
index e4d0fa9..9cff9e0 100644
--- a/utils.hpp
+++ b/utils.hpp
@@ -5,6 +5,10 @@
 const size_t samps_per_buffer = 20480;
 const size_t samplerate = 2048000;
 
+#include <complex>
+
+typedef std::complex<float> complexf;
+
 
 #include <cstdio>