Good day

Note: I have found out that my code works. I have placed a minor explanation to be further expounded.

I have been having trouble obtaining the right directional output from my implementation. I noticed that every time I put an obstacle at the right, it gives left, it gives the right steering direction, the problem is with the presence of a left obstacle where it still tends to steer itself towards that obstacle. I have checked the occupancy map generated using matlab and was found to be correct. I couldn't pinpoint what is exactly wrong with my code for I have been debugging this for almost a week now and was hoping if someone can see the error I cannot.

Here is my code implementation:

//1st:: Create Occupancy Grid from Data-------------------------------------------------
// > Cell Size/Grid Resolution  = 5meters/33 cells = 0.15meters each = 15cm each
// > Grid Dimension = 5meters by 5meters / 33x33 cells //Field of view of robot is 54 degrees
//or 31 meters horizontal if subject is 5 meters away
// > Robot Width = 1meter 100cm
// > Because the focal length of the lens is roughly the same as the width of the sensor,
// it is easy to remember the field of view: at x meters away, you can see about x meters horizontally,
// assuming 4x3 stills mode. Horizontal field of view in 1080p video mode is 75%
// of that (75% H x 55% V sensor crop for 1:1 pixels at 1920x1080).

//Converting the Position into an Angle--------------------------------------------
//  A. https://decibel.ni.com/content/docs/DOC-17771
//1. Get angle
// > Each pixel from the image represents an angle
// > angle = ith pixel in row * (field of view in degrees/number of pixels in row)
// > field of view of Pi camera is 54 degrees horizontal
//2. Convert Polar to Cartesian
// > x = z*cos(angle)
// > y = z*sin(angle)

int arrOccupancyGrid[33][33];
float matDepthZ[33][33];
int robotPosX = 0;
int robotPosY = 0;
int xCoor=0; //Coordinates of Occupancy Map
int yCoor=0;
int xPosRobot=0; //Present cordinates of robot
int yPosRobot=0;
float fov = 54; // 54 degrees field of view in degrees must be converted to radians
float nop = 320; //number of pixels in row
int mapDimension = 33; // 33by33 array or 33*15cm = 5mby5m grid
int mapResolution = 15; //cm

//Limit max distance measured
for(i=0; i< nop ;i++){
        arrDepthZ.at(i) = 500;

for (i=0 ; i < nop; i++){
    //Process data/Get coordinates for mapping
        //Get Angle
        int angle = ((float)(i-160.0f) * ((float)fov/(float)nop)); //if robot is centered at zero add -160 to i
        //cout << "arrDepthZ " << arrDepthZ.at(i) << endl;
            //cout << "i " << i << endl;
            //cout << "fov " << fov << endl;
            //cout << "nop " << nop << endl;
            //cout << "angle " << i * (fov/nop) << endl;
        //Get position X and Y use floor() to output nearest integer
        //Get X --------
        xCoor = (arrDepthZ.at(i) / mapResolution) * cos(angle*PI/180.0f); //angle must be in radians because cpp
        //cout << "xCoor " << xCoor << endl;
        //Get Y --------
        yCoor = (arrDepthZ.at(i) / mapResolution) * sin(angle*PI/180.0f); //angle must be in radians because cpp
                //cout << "yCoor " << yCoor << endl;
    //Populate Occupancy Map / Cartesian Histogram Grid

        if((xCoor <= 33) && (yCoor <= 33)){ //Condition Check if obtained X and Y coordinates are within the dimesion of the grid
            arrOccupancyGrid[xCoor][yCoor] = 1; //[increment] equate obstacle certainty value of cell by 1
            matDepthZ[xCoor][yCoor] = arrDepthZ.at(i);

        //cout << "arrCoorX.size()" << arrCoorX.size() << endl;
        //cout << "arrCoorY.size()" << arrCoorY.size() << endl;


for (i=0 ; i < arrCoorX.size(); i++){
  file43 << arrCoorX.at(i) << endl;

for (i=0 ; i < arrCoorY.size(); i++){
  file44 << arrCoorY.at(i) << endl;

for (i=0 ; i < arrDepthZ.size(); i++){
  file45 << arrDepthZ.at(i) << endl;

//------------------------- End Create Occupancy Grid -------------------------

//2nd:: Create 1st/Primary Polar Histogram ------------------------------------------------------
//1. Define angular resolution alpha
// > n = 360degrees/alpha;
// > set alpha to 5 degrees resulting in 72 sectors from 360/5 = 72 ///// change 180/5 = 35
//2. Define number of sectors (k is the sector index for sector array eg kth sector)
// > k=INT(beta/alpha), where beta is the direction from the active cell
//to the Vehicle Center Point (VCP(xPosRobot, yPosRobot)). Note INT asserts k to be an integer

cout << "2nd:: Create 1st/Primary Polar Histogram" << endl;

//Put this at the start of the code away from the while loop ----------------
int j=0;
int sectorResolution = 5; //degrees 72 sectors, alpha
int sectorTotal = 36; // 360/5 = 72 //// change 180/5 = 36
int k=0; //sector index (kth)
int maxDistance = 500; //max distance limit in cm
//vector<int>arrAlpha; //already initiated

float matMagnitude[33][33]; //m(i,j)
float matDirection[33][33]; //beta(i,j)
float matAngleEnlarge[33][33]; //gamma(i,j)
int matHconst[33][33]; //h(i,j) either = 1 or 0

float robotRadius = 100; //cm
float robotSafeDist = 50; //cm
float robotSize4Sector = robotRadius + robotSafeDist;

for (i=0; i<sectorTotal; i++){
//---------end initiating sectors----------

//Determine magnitude (m or matMagnitude) and direction (beta or matDirection) of each obstacle vector
//Modify m(i,j) = c(i,j)*(a-bd(i,j)) to m(i,j) = c(i,j)*(dmax-d(i,j)) from sir Lounell Gueta's work (RAL MS)
//Compute beta as is, beta(i,j) = arctan((yi-yo)/(xi-xo))
//Enlarge robot and compute the enlargment angle (gamma or matAngleEnlarge)
int wew =0;
int firstfillPrimaryH = 0; //flag for arrayPrimaryH storage
for (k=0; k<sectorTotal; k++){
    for (i=0; i<mapDimension; i++){
        for (j=0; j<mapDimension; j++){
            //cout << "i" << i << "j" << j << "k" << k << endl;
            //cout << "mapDimension" << mapDimension << endl;
            //cout << "sectorTotal" << sectorTotal << endl;
            //Compute magnitude m, direction beta, and enlargment angle gamma
            matMagnitude[i][j] = (arrOccupancyGrid[i][j])*( maxDistance-matDepthZ[i][j]); //m(i,j)
            //cout << "matMagnitude[i][j]" <<  (arrOccupancyGrid[i][j])*( maxDistance-matDepthZ[i][j]) << endl;
            matDirection[i][j] = ((float)atan2f( (float)(i-yPosRobot), (float)(j-xPosRobot))*180.0f/PI); //beta(i,j)
            //cout << "matDirection[i][j]" << ((float)atan2f( (float)(i-yPosRobot), (float)(j-xPosRobot))*180.000/PI) << endl;
            //cout << "matDepthZ[i][j]" << matDepthZ[i][j] << endl;
            if(matDepthZ[i][j] == 0){ //if matDepthZ[i][j] == 0; obstable is very far thus path is free, no enlargement angle
                matAngleEnlarge[i][j] = 0; //gamma(i,j)
                //cout << "matAngleEnlarge[i][j]" << 0 << endl;
            else{ //if matDepthZ[i][j] > 0 there is an obstacle so compute enlargement angle
                matAngleEnlarge[i][j] = asin( robotSize4Sector / matDepthZ[i][j])*180/PI; //gamma(i,j)
                //cout << "matAngleEnlarge[i][j]" << asin( robotSize4Sector / matDepthZ[i][j])*180.0f/PI << endl;

            wew = k*sectorResolution; //k*alpha
            //cout << "wew" << k*sectorResolution << endl;
            //Check if magnitude is a part of the sector
            if ( ((matDirection[i][j]-matAngleEnlarge[i][j]) <= wew) && (wew <= (matDirection[i][j]+matAngleEnlarge[i][j])) ){
                matHconst[i][j]=1; //Part of the sector
                //cout << "Part of the sector ---------------------------------------------------------------" << endl;
                //cout << "matHconst[i][j]=1" << matHconst[i][j] << endl;
                matHconst[i][j]=0; //Not Part of the sector
                //cout << "Not Part of the sector" << endl;
                //cout << "matHconst[i][j]=0" << matHconst[i][j] << endl;
            //Compute primary polar histogram Hp(k)
            //cout << "firstfillPrimaryH" << firstfillPrimaryH << endl;
            if (firstfillPrimaryH==0){ //If first fill at sector
                //cout << "matMagnitude[i][j]" << matMagnitude[i][j] << endl;
                //cout << "matHconst[i][j]" << matHconst[i][j] << endl;
                float temp = matMagnitude[i][j]*matHconst[i][j];
                //cout << "matMagnitude[i][j]*matHconst[i][j]" << temp << endl;
                firstfillPrimaryH=1; //Trigger flag
                //cout << "arrPrimaryH kth" << arrPrimaryH.at(k) << endl;
            else{ //If sector filled previously
                arrPrimaryH.at(k) = arrPrimaryH.at(k)+(matMagnitude[i][j]*matHconst[i][j]);
                //cout << "arrPrimaryH kth" << arrPrimaryH.at(k) << endl;

    firstfillPrimaryH=0; //Reset flag
  • 1
    $\begingroup$ Welcome to Robotics, user123456098. As it stands, it's not clear what you're asking. The only reference you provide is a link to NI.com in the source code you provide, which in turn provides some more code and videos. Please break up your code with explanations of what you are expecting to happen with each portion of the code, along with the sample data you provided to the code, the output you expected, and the output you received. While code review isn't explicitly disallowed here, clarity is a requirement and it's missing in your question. $\endgroup$
    – Chuck
    May 23 '16 at 18:02
  • $\begingroup$ As it stands, it looks like your question is, "Every time I put an obstacle at the right, it gives left, it gives the right steering direction, the problem is with the presence of a left obstacle where it still tends to steer itself towards that obstacle." Please use "right" when you mean a direction and "correct" when you mean correct; your question is currently not especially clear. Please revise your question and then we can re-open it. $\endgroup$
    – Chuck
    May 23 '16 at 18:03
  • $\begingroup$ @Chuck I just found out that my code is working, it's just my lack of undersanding on how the vector field histogram works :)) It turns out that the part of the data that gets updated on the histogram is dependent on the field of view of the camera used on the stereo vision setup. I will post an explanation of the code when I have time after I have finished my project. $\endgroup$ May 24 '16 at 15:08
  • 1
    $\begingroup$ Please move your "Note" to an answer and you can then accept it. I would also just caution you that LIDAR would also not update all fields simultaneously - this is only true of LIDAR units that have a 360 degree field of view (Velodyne VLP-XX, SICK LD-LRS, etc.). LIDAR units that don't have a 360 degree field of view (SICK LMSXXX, etc.) will have the same issue as the camera. $\endgroup$
    – Chuck
    May 25 '16 at 17:33

I have found out that my code works. It is just that most of the literature I have read uses Lidar or Sonar sensors for histogram updates. I had assumed that in the case of a stereo vision set-up, all sectors are updated simultaneously. However only the sectors in the field of view of the camera is update unlike in the lidar implementation that samples a complete 360 degree sweep of its surroundings. Another thing I have found out that when deriving the X and Y coordinates from the depthmap, the resulting map is a mirror image of the actual surrounding thus the sectors must be labeled counter clockwise. I used the formula from NI' s paper linked at the code. The same case is also true when using the camera matrix with open cv to obtain the real world X,Y coordinates from the depth map. I shall edit this question to a clearer one soon :)


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.