Some of the laservision functions, including: an implementation of the Hough space, calculation of the misalignment, creation of the minimap and calculation of the y-position error.

laservision.cpp

#include <math.h>      
#include <ctime>  // get the time to time this calculation
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <iostream>
#include <stdio.h>
#include <algorithm>
#include "laservision.h"

// Contains all the functions that have to do with laser data
// For a global overvieuw of the functionallities see: 
// http://cstwiki.wtb.tue.nl/index.php?title=Embedded_Motion_Control_2015_Group_4  

// ----- GLOBAL Variable Context --------------
// All these variables can be used by all the functions in the program
// All variables declared here start with lv_ stands for laser vision

double lv_par_worldangle = M_PI/2; //  world angles in rad, when all corners are 90 deg then pi/2 when there are corners 45 deg then pi/4
double lv_par_hough_trustsize = 3.0; // only the data in the trust area is used for calculating the Hough space [in meter]
int lv_Hspace_Asize = 400; // number of angles to check between 0 and 180 deg (matrix size = accuracy)
double lv_par_Haccuracy_p = 0.05; // accuracy of p

double lv_Haccuracy_angle = M_PI/lv_Hspace_Asize;
int lv_Hspace_Psize = (2*round(sqrt((2*lv_par_hough_trustsize)*(2*lv_par_hough_trustsize))/lv_par_Haccuracy_p)+1); // size of matrix in p direction (depent on maximum expected values, which is half the diagonal of the trust size)
std::vector< std::vector<int> > lv_Hspace(lv_Hspace_Asize, std::vector<int>(lv_Hspace_Asize)); // Hough space (matrix)
int lv_Hspace_Phalfsize = floor(lv_Hspace_Psize/2)+1;
int lv_maxval = 0;
int lv_pmaxpos = 0;
int lv_amaxpos = 0;
double lv_misalign;
double lv_angle;
bool lv_anglewrong = false;

// for the minimaps
double lv_par_trustsize = 1.6; // trust size of minimap in meter
double lv_par_accuracy_xy = 0.05; // in meter
double lv_par_sidedoorsize = 0.35;
int lv_minimapsize = round(2*lv_par_trustsize/lv_par_accuracy_xy)+1;
int lv_minimapsizehalf = floor(lv_minimapsize/2);
std::vector< std::vector<int> > lv_minimap1(round(2*lv_par_trustsize/lv_par_accuracy_xy)+1, std::vector<int>(round(2*lv_par_trustsize/lv_par_accuracy_xy)+1)); // matrix minimap1
std::vector< std::vector<int> > lv_minimap2(round(2*lv_par_trustsize/lv_par_accuracy_xy)+1, std::vector<int>(round(2*lv_par_trustsize/lv_par_accuracy_xy)+1)); // matrix minimap1

// for the position error in corridor
double lv_yposerror;

bool lv_show_xydata = false;
bool lv_show_houghspace = false;
bool lv_show_minimap1 = false;
bool lv_show_minimap2 = true;

// ---- FUNCTIONS- ---------------------

// In this function:
// -Calculate the Hough Space
// -Calculate the lv_angle
// -Calculate the lv_misalignment
double lv_updatecompass (emc::LaserData scan)
{    
    // clock_t begintime = clock(); // to time this function

    // copy of structure of laser data from sensor
    // Types from .h file
    // double range_min;
    // double range_max;
    // double angle_min;
    // double angle_max;
    // double angle_increment;
    // double timestamp;
    // std::vector<float> ranges;

    double  range_min = scan.range_min;
    double  range_max = scan.range_max;
    double  angle_min = scan.angle_min;
    double  angle_max = scan.angle_max;
    double  angle_increment = scan.angle_increment;

    int     n_samp    = scan.ranges.size();    // number of sample points in given vector

    std::vector<double> radius; // radius corresponding to samples
    radius.assign (n_samp,0.0);

    std::vector<double> angle; // angles corresponding to samples
    angle.assign (n_samp,0.0);

    std::vector<double> xdata; // x data corresponding to samples
    xdata.assign (n_samp,0.0);

    std::vector<double> ydata; // y data corresponding to samples
    ydata.assign (n_samp,0.0);

    // double delta_angle = (range_max - range_min)/(n_samp-1);



    // Read data, create angles, radius, xdata and ydata
    double r = 0.0; double theta = 0.0; double x =0.0; double y =0.0;
    int pita = 0; // for counting the points in the trust area
    // Transform laser data to x-y data, and count the points in the trust area:
    for (int k = 0; k < n_samp; k++)
    {
        r = scan.ranges[k];
        theta = angle_increment * k + angle_min;
        if (r <= range_min)
        {
            r = range_max *2;
        }
        radius[k] = r;
        angle[k] = theta;
        x = r*cos(theta);
        y = r*sin(theta);
        xdata[k] = x;
        ydata[k] = y;
        if ((fabs(x) < lv_par_hough_trustsize) && (fabs(x) < lv_par_hough_trustsize))
        {
            pita++;
        }
    }

    // If there are enough points in the trust area then proceed
    if (pita > 50)
    {
        if (lv_show_xydata) {
            // --- Show image on screen --- //
            using namespace cv;
            using namespace std;
            cv::Mat LaserXYImage = cv::Mat::zeros( 400, 400, CV_8UC3 );
            for (int k = 0; k < n_samp; k++)
            {
                int x = floor((xdata[k]/lv_par_hough_trustsize*200)+200);
                int y = floor((ydata[k]/lv_par_hough_trustsize*200)+200);
                if ((x >= 0) && (x < 400) && (y >= 0) && (y < 400))
                {
                    LaserXYImage.at<cv::Vec3b>(x,y)[0] = 0;
                    LaserXYImage.at<cv::Vec3b>(x,y)[1] = 0;
                    LaserXYImage.at<cv::Vec3b>(x,y)[2] = 200;
                }

            }
            cv::flip(LaserXYImage, LaserXYImage,0);
            cv::flip(LaserXYImage, LaserXYImage,1);
            cv::namedWindow( "Laser XY data", WINDOW_AUTOSIZE );// Create a window for display.
            cv::imshow( "Laser XY data", LaserXYImage );                   // Show our image inside it.
            cv::waitKey(30);                                          // Wait for a keystroke in the window
            // --- End Show image on screen --- //
        }

        // Calculating the Hough space

        // Clear Hough matrix
        for (int p = 0; p < lv_Hspace_Psize; p++)
        {
            for (int a = 0; a < lv_Hspace_Asize; a++)
            {
                lv_Hspace[p][a] = 0;
            }
        }

        // pre calculate the cos and sin (because it is multiple times used in the next operation, to save time)
        std::vector<double> Hsin (lv_Hspace_Asize ,0.0);
        std::vector<double> Hcos (lv_Hspace_Asize ,0.0);
        for (int a = 0; a < lv_Hspace_Asize; a++)
        {
            Hsin[a] = sin(lv_Haccuracy_angle*a);
            Hcos[a] = cos(lv_Haccuracy_angle*a);
        }

        // Apply the Hough transformation
        for (int k = 0; k < n_samp; k++) // for each sample
        {
            if ((fabs(xdata[k]) <= lv_par_hough_trustsize) && (fabs(ydata[k]) <= lv_par_hough_trustsize)) // check if the sample is in the trust area
                for (int a = 0; a < lv_Hspace_Asize; a++) // for all possible angles 0...180 deg
                {
                    int p = 0;
                    p = round(( xdata[k] * Hcos[a] + ydata[k] * Hsin[a] ) / lv_par_Haccuracy_p) + lv_Hspace_Phalfsize; // calculate the corresponding p parameter and calculate the position of the pixel
                    if (( p >= 0) && p < lv_Hspace_Psize) // check if the pixel lays in the matrix
                    {
                        lv_Hspace[p][a]++; // add 1 to that pixel
                    }
                }
        }

        // find the peak in the Hough space (this point gives the parameters of our reference line)
        int lv_maxval = 0; int lv_pmaxpos = 0; int lv_amaxpos = 0;
        // just walk through all the elements and remember the highest one.
        for (int p = 0; p < lv_Hspace_Psize; p++)
        {
            for (int a = 0; a < lv_Hspace_Asize; a++)
            {
                if ( lv_Hspace[p][a] > lv_maxval )
                {
                    lv_maxval = lv_Hspace[p][a];
                    lv_pmaxpos = p; // perpendicular distance
                    lv_amaxpos = a; // angle
                }
            }
        }

        if (lv_show_houghspace) {
            // --- Show image on screen --- //
            using namespace cv;
            using namespace std;
            cv::Mat HoughSpaceImage = cv::Mat::zeros( lv_Hspace_Psize, lv_Hspace_Asize, CV_8UC3 );

            int graytint = 0;
            for (int p = 0; p < lv_Hspace_Psize; p++)
            {
                for (int a = 0; a < lv_Hspace_Asize; a++)
                {
                    graytint = round(lv_Hspace[p][a]*255/lv_maxval);
                    HoughSpaceImage.at<cv::Vec3b>(p,a)[0] = graytint;
                    HoughSpaceImage.at<cv::Vec3b>(p,a)[1] = graytint;
                    HoughSpaceImage.at<cv::Vec3b>(p,a)[2] = graytint;
                    if (a==lv_amaxpos)
                    {
                        HoughSpaceImage.at<cv::Vec3b>(p,a)[0] = 0;
                        HoughSpaceImage.at<cv::Vec3b>(p,a)[1] = 0;
                        HoughSpaceImage.at<cv::Vec3b>(p,a)[2] = 200;
                    }
                }
            }
            cv::namedWindow( "Hough space", WINDOW_AUTOSIZE );// Create a window for display.
            cv::imshow( "Hough space", HoughSpaceImage );                   // Show our image inside it.
            cv::waitKey(30);                                          // Wait for a keystroke in the window
            // --- End Show image on screen --- //
        }


        // CALCULATE THE "ABSOLUTE" ANGLE
        // Here the angle:   -\infty < lv_angle < \infty    is calculated
        // Approach: assumed is that the angle with respect to the world can not change faster than (par_worldangle/2) if it changed faster then
        //           this means that a line perpendicular is selected to the line of the last measurement (this is compensated by the while loop).
        //           The lv_angle has a range of -\infty < lv_angle < \infty
        //           Can be reset by by the operation: lv_angle = 0.0;
        double angle_temp = -lv_Haccuracy_angle * lv_amaxpos; // note the - sign because it is the negative of the error
        double old_angle = lv_angle;
        int correction_turns = round(old_angle/lv_par_worldangle); // approximate turns, to avoid unnecessary loop iterations
        while (fabs(old_angle - (correction_turns*lv_par_worldangle + angle_temp)) > lv_par_worldangle*2/3){

            if ((correction_turns*lv_par_worldangle + angle_temp)<(old_angle - lv_par_worldangle*2/3)){
                correction_turns++;
            }else{
                correction_turns--;
            }
        }
        lv_angle = correction_turns*lv_par_worldangle + angle_temp;
        //       std:: //cout << "lv_angle: " << lv_angle <<  std::endl;

        // CALCULATE THE MISALIGNMENT
        // Here the misalignment:   -par_worldangle/2 < lv_angle < par_worldangle/2     with the world is calculated
        // Approach: Assumed is that the misalignment is not larger than par_worldangle/2, and if it is larger then
        //           we correct with steps of par_worldangle such that we find a misaligment smaller than par_worldangle/2
        //           with the world. (the value of lv_misalign can be dirrectly used in a positive feedback loop)

        double misalignment_temp = lv_Haccuracy_angle * lv_amaxpos;
        double misalignment = 0.0;
        correction_turns = 0;
        while (fabs(correction_turns*lv_par_worldangle + misalignment_temp) > lv_par_worldangle/2){

            if ((correction_turns*lv_par_worldangle + misalignment_temp)<(-lv_par_worldangle/2)){
                correction_turns++;
            }else{
                correction_turns--;
            }
        }
        lv_misalign = correction_turns*lv_par_worldangle + misalignment_temp;

        //  clock_t endtime = clock(); // time this function if you like
        //  double elapsed_secs = double(endtime - begintime) / CLOCKS_PER_SEC;
        //  std:: //cout << "sec:" << elapsed_secs << std::endl;

        lv_anglewrong = false;

        return lv_misalign;
    }
    else // if there are not enough points in the trust area (does only happen when in complete open area)
    {
        lv_anglewrong = true;
        return 0.0;
    }
}




// -------------------------------------------------------------------------------------------------------------
// In this function:
// Create the Minimaps
// The minimap is aligned with the world instead of with the robot
void lv_updateminimap(emc::LaserData scan, double misalignment)
{
    //    clock_t begintime = clock();

    double  range_min = scan.range_min;
    double  range_max = scan.range_max;
    double  angle_min = scan.angle_min;
    double  angle_max = scan.angle_max;
    double  angle_increment = scan.angle_increment;

    int     n_samp    = scan.ranges.size();    // number of sample points in given vector

    std::vector<double> radius; // radius corresponding to samples
    radius.assign (n_samp,0.0);

    std::vector<double> angle; // angles corresponding to samples
    angle.assign (n_samp,0.0);

    std::vector<double> xdata; // x data corresponding to samples
    xdata.assign (n_samp,0.0);

    std::vector<double> ydata; // y data corresponding to samples
    ydata.assign (n_samp,0.0);

    // double delta_angle = (range_max - range_min)/(n_samp-1);

    // Read data, create angles, radius, xdata and ydata
    double r = 0.0;
    double theta = 0.0;
    double x =0.0;
    double y =0.0;
    int pita = 0; // points in trust area
    for (int k = 0; k < n_samp; k++)
    {
        r = scan.ranges[k];
        theta = angle_increment * k + angle_min - misalignment;
        if (r <= range_min)
        {
            r = range_max *2;
        }
        radius[k] = r;
        angle[k] = theta;
        x = r*cos(theta);
        y = r*sin(theta);
        xdata[k] = x;
        ydata[k] = y;
        if ((fabs(x) < lv_par_trustsize) && (fabs(x) < lv_par_trustsize))
        {
            pita++;
        }
    }

    // make sure that there are enough data points in the trust area
    if (pita > 50)
    {

        int lv_minimapsize = round(2*lv_par_trustsize/lv_par_accuracy_xy)+1;
        int lv_minimapsizehalf = floor(lv_minimapsize/2);

        for (int x = 0; x < lv_minimapsize; x++)
        {
            for (int y = 0; y < lv_minimapsize; y++)
            {
                lv_minimap1[x][y] = 0;
                lv_minimap2[x][y] = 0;

            }
        }

        // Create map of walls
        for (int k = 0; k < n_samp; k++)
        {
            int x = floor((xdata[k]/lv_par_accuracy_xy)+lv_minimapsizehalf);
            int y = floor((ydata[k]/lv_par_accuracy_xy)+lv_minimapsizehalf);
            if ((x >= 0) && (x < lv_minimapsize) && (y >= 0) && (y < lv_minimapsize))
            {
                lv_minimap1[x][y] = 1;
            }
        }

        // Create map with free area

        double map_radius_max = sqrt(2*lv_minimapsize*lv_minimapsize)+1;
        double map_angles_res = 1 / map_radius_max/2;
        int map_num_of_angles = round( (angle_max - angle_min)/map_angles_res);



        for (int a = 0; a < map_num_of_angles; a++)
        {
            //            std:: //cout << "a:" << map_num_of_angles << std::endl;
            double Msin = sin(map_angles_res*a + angle_min - misalignment); //- misalignment
            double Mcos = cos(map_angles_res*a + angle_min - misalignment);
            //            std:: //cout << "t:" << Msin << std::endl;

            for (int r = 0; r < map_radius_max*2; r++)
            {
                //                std:: //cout << "r:" << map_radius_max*2 << std::endl;
                int x = round((r*Mcos)+lv_minimapsizehalf);
                int y = round((r*Msin)+lv_minimapsizehalf);
                //                int w = 5;
                //                int z = 5;
                //                 std:: //cout << "xy:" << w << " y:" << z << std::endl;
                if ((x >= 0) && (x < lv_minimapsize) && (y >= 0) && (y < lv_minimapsize))
                {
                    //lv_minimap2[w][z] = 1;
                    //                    std:: //cout << "executed" << std::endl;
                    //                    std:: //cout << "size:" << lv_minimapsize << std::endl;
                    if (lv_minimap1[x][y] < 1)
                    {
                        lv_minimap2[x][y] = 1;
                        // std:: //cout << "executed" << std::endl;

                    }else
                    {
                        break;
                    }

                }else
                {
                    break;
                }

            }
        }

        if (lv_show_minimap1 || lv_show_minimap2){
            // --- Show image on screen --- //
            using namespace cv;
            using namespace std;
            cv::Mat Minimap1Image = cv::Mat::zeros( lv_minimapsize, lv_minimapsize, CV_8UC3 );
            cv::Mat Minimap2Image = cv::Mat::zeros( lv_minimapsize, lv_minimapsize, CV_8UC3 );

            for (int x = 0; x < lv_minimapsize; x++)
            {
                for (int y = 0; y < lv_minimapsize; y++)
                {
                    int color = lv_minimap1[x][y] * 250;
                    Minimap1Image.at<cv::Vec3b>(x,y)[0] = color;
                    Minimap1Image.at<cv::Vec3b>(x,y)[1] = color;
                    Minimap1Image.at<cv::Vec3b>(x,y)[2] = color;

                    color = lv_minimap2[x][y] * 250;
                    Minimap2Image.at<cv::Vec3b>(x,y)[0] = color;
                    Minimap2Image.at<cv::Vec3b>(x,y)[1] = color;
                    Minimap2Image.at<cv::Vec3b>(x,y)[2] = color;
                }
            }
            if (lv_show_minimap1) {
                cv::flip(Minimap1Image, Minimap1Image,0);
                cv::flip(Minimap1Image, Minimap1Image,1);
                cv::namedWindow( "Minimap 1 Image", WINDOW_NORMAL );// Create a window for display.
                cv::imshow( "Minimap 1 Image", Minimap1Image );                   // Show our image inside it.
                cv::waitKey(30);                                          // Wait for a keystroke in the window
            }
            if (lv_show_minimap2) {
                cv::flip(Minimap2Image, Minimap2Image,0);
                cv::flip(Minimap2Image, Minimap2Image,1);
                cv::namedWindow( "Minimap 2 Image", WINDOW_NORMAL );// Create a window for display.
                cv::imshow( "Minimap 2 Image", Minimap2Image );                   // Show our image inside it.
                cv::waitKey(1);                                          // Wait for a keystroke in the window
            }
            //--- End Show image on screen --- //
        }


        //        clock_t endtime = clock();
        //        double elapsed_secs = double(endtime - begintime) / CLOCKS_PER_SEC;
        //        std:: //cout << "sec:" << elapsed_secs << std::endl;
    }
}


// -------------------------------------------------------------------------------------------------------------
// In this function:
// Calculate the y-position error (the distance from the center of the corridor to the robot)
// The minimap is aligned with the world instead of with the robot
double lv_updateypositionerror ()
{
    int rightbound = 0;
    int leftbound = 0;
    int stop = 0;
    double bound_depth_in_meter = 1.0; // How far the robot looks forward
    int bound_depth = round(bound_depth_in_meter/lv_par_accuracy_xy);
    double max_corridor_size_in_meter = 2;
    int max_corridor_size = round(max_corridor_size_in_meter/lv_par_accuracy_xy);
    double min_corridor_size_in_meter = 0.3;
    int min_corridor_size = round(min_corridor_size_in_meter/lv_par_accuracy_xy);

    // Right bound
    // Check from the center to the right, column by column if there is a object
    // If there is a object in the current column then break the loop, the measured bound is then current-1 
    for (int y = 0; y < lv_minimapsizehalf; y++)
    {
        for (int x = 0; x < bound_depth; x++)
        {
            if (lv_minimap2[lv_minimapsizehalf + x][lv_minimapsizehalf - y] == 0)
            {
                stop = 1;
                break;
            }
            rightbound = -y;
        }
        if (stop==1)
        {
            break;
        }

    }

    // Left bound
    // Check from the center to the left, column by column if there is a object
    // If there is a object in the current column then break the loop, the measured bound is then current-1 
    stop =0;
    for (int y = 0; y < lv_minimapsizehalf; y++)
    {
        for (int x = 0; x < bound_depth; x++)
        {
            if (lv_minimap2[lv_minimapsizehalf + x][lv_minimapsizehalf + y] == 0)
            {
                stop = 1;
                break;
            }
            leftbound = y;
        }
        if (stop==1)
        {
            break;
        }
    }

    double ypositionerror = (leftbound - (fabs(leftbound) + fabs(rightbound))/2 ) * lv_par_accuracy_xy;


    // check if bounds are reliable
    // if bounds in front are not oke
    if ( fabs(leftbound) >= lv_minimapsizehalf-1 || fabs(rightbound) >= lv_minimapsizehalf-1
         || (fabs(rightbound) + fabs(leftbound)) > round(max_corridor_size) || (fabs(leftbound) + fabs(rightbound)) < min_corridor_size)
    {
            ypositionerror = 0.0;
    }

    // turn off y error when turning to much
    // (fast solution only)
    if (fabs(lv_misalign)<0.26){
        lv_yposerror = ypositionerror;
        return ypositionerror;
    }
    else{
        lv_yposerror = 0.0;
        return 0.0;
    }

}

laservision.h

#ifndef LASERVISION
#define LASERVISION

#include <iostream>
#include <emc/io.h>
#include <emc/rate.h>
#include <math.h>
#include <stdio.h>



// Public functions -------------------------------------------------------------

// returns double with wall misalignment
double lv_updatecompass (emc::LaserData scan);

// update minimap (stored inside laservision scope, hidden from supervisor)
void lv_updateminimap(emc::LaserData scan, double misalignment);

// return y-position error (error from "ideal" center of corridor)
// can be used for position control
double lv_updateypositionerror();


// Public variables ---------------------------------------------------------------

extern double lv_misalign;
extern double lv_yposerror;

extern double lv_par_worldangle;
extern double lv_par_hough_trustsize;
extern int lv_Hspace_Asize;
extern double lv_par_Haccuracy_p;

extern double lv_Haccuracy_angle;
extern int lv_Hspace_Psize;
extern std::vector< std::vector<int> > lv_Hspace;
extern int lv_Hspace_Phalfsize;
extern int lv_maxval;
extern int lv_pmaxpos;
extern int lv_amaxpos;
extern double lv_misalign;
extern double lv_angle;
extern bool lv_anglewrong;

// for the minimaps
extern double lv_par_trustsize;
extern double lv_par_accuracy_xy;
extern double lv_par_sidedoorsize;
extern int lv_minimapsize;
extern int lv_minimapsizehalf;
extern std::vector< std::vector<int> > lv_minimap1;
extern std::vector< std::vector<int> > lv_minimap2;


// for the position error in corridor
extern double lv_yposerror;

extern bool lv_show_xydata;
extern bool lv_show_houghspace;
extern bool lv_show_minimap1;
extern bool lv_show_minimap2;

#endif // LASERVISION