如何从传感器的3D点位置集中计算传感器覆盖的总距离？

Question

I am using Qt GUI to track the motion of a sensor. The mainwindow.cpp file is:

#include "mainwindow.h"
#include "ui_mainwindow.h"
#include "ATC3DG.h"
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/core/core.hpp>
#include "QTimer"
#include "qtimer.h"
#include "math.h"

double square(double x)
{
    return x*x;
}


MainWindow::MainWindow(QWidget *parent) :
    QMainWindow(parent),
    ui(new Ui::MainWindow)
{
    ui->setupUi(this);
}

MainWindow::~MainWindow()
{
    delete ui;
}

void MainWindow::on_start_clicked()
{
    points.clear();   //points is a global std::vector<cv::Point3> declared in mainwindow.h
    errorCode = InitializeBIRDSystem();
    errorCode = GetBIRDSystemConfiguration(&ATC3DG.m_config);
    id = 0;
    errorCode = SetSystemParameter(SELECT_TRANSMITTER, &id, sizeof(id));
    EM_time = new QTimer(this);
    connect(EM_time, SIGNAL(timeout()), this, SLOT(showValues()));
    EM_time->start();
}

void MainWindow::showValues()
{
    EM_time->stop();
    pRecord = &record;
    {
        sensorID = 0;
        {
            errorCode = GetAsynchronousRecord(sensorID, pRecord, sizeof(record));
            unsigned int status = GetSensorStatus(sensorID);
            if ( status == VALID_STATUS )
            {
                points.push_back(cv::Point3f(record.x, record.y, record.z));
                QString str;
                str.sprintf("%f, %f, %f",record.x, record.y, record.z );
                this->ui->label->setText(str);
            }
        }
    }
    EM_time->start();
}

void MainWindow::on_stop_clicked()
{
    EM_time->stop();
    double sum = 0;
    double dist;
    QString str;

    for (int i=0; i<points.size()-1; i++)
    {
       dist = sqrt(square(points[i].x - points[i+1].x) + square(points[i].y - points[i+1].y) + square(points[i].z - points[i+1].z));
       sum = sum+dist;
    }
    str.sprintf("%d cm", sum*2.54);
    this->ui->distance->setText(str);
}

ATC3DG.h is the header file of the sensor. record.x, record.y, record.z gives the 3D location of x, y and z location of the sensor in inches . Basically what I am doing is, when I click the start button, the sensor is switched on and the QTimer starts with its signal emitted during timeouts and the showvalues() function will start to execute. This function displays the position of the sensor in label of the Qt GUI. During this loop, points will be filled with all the position values of the sensor.

The stop button stops the timer and calculates the distance using all the points containing in the points vector. This is done using:

double sum=0;
double dist;
for (int i=0; i<points.size()-1; i++)
    {
       dist = sqrt(square(points[i].x - points[i+1].x) + square((int)points[i].y - (int)points[i+1].y) + square(points[i].z - points[i+1].z));
       sum = sum+dist;
    }

The sum is giving me totally weird values. For example, when the sensor has moved only about 5 or 6 inches, it is showing values in the range of 100s and like that.

My mainwindow.h file is:

#ifndef MAINWINDOW_H
#define MAINWINDOW_H

#include <QMainWindow>
#include "ATC3DG.h"
#include "QTimer"
#include <opencv2/core/core.hpp>

namespace Ui {
class MainWindow;
}

class CSystem
{
public:
    SYSTEM_CONFIGURATION m_config;
};
class CSensor
{
public: SENSOR_CONFIGURATION m_config;
};
class CXmtr
{
public: TRANSMITTER_CONFIGURATION m_config;
};

class MainWindow : public QMainWindow
{
    Q_OBJECT

public:
    explicit MainWindow(QWidget *parent = 0);
    ~MainWindow();


public slots:
    void on_start_clicked();
    void showValues();
    void on_stop_clicked();

private:
    Ui::MainWindow *ui;
private:
    DOUBLE_POSITION_ANGLES_RECORD record, *pRecord;
    CSystem ATC3DG;
    CSensor *pSensor;
    CXmtr *pXmtr;
    int errorCode;
    int sensorID;
    int i;
    short id;
    QTimer *EM_time;
    std::vector<cv::Point3f> points;
};

#endif // MAINWINDOW_H

Answer 1

issues I can see in your code:

1. overuse of braces (they do nothing) - this looks strange and may led to errors
2. GetAsynchronousRecord suggest asynchronous action and you are using value immediately! I don't known this library but this looks suspicious.
3. start and stop timer in same method.
4. you are calculating sum of distances from probably very noisy data. This means that when you do not move sensor over a time you are calculating sum of noise and as a result you have large distance when sensor is not moved at all. You have to filter data before calculating such distance (the easiest is low pass filter).