Method AddPointcloudPoints

ScanThread.cpp:348–416

Compute where the 3d points are based on where the laser was detected in the image

// Compute where the 3d points are based on where the laser was detected in the image
void ScanThread::AddPointcloudPoints(vector<float> *laserPos)
{
    // compute distance for this pixel
    // calculate phi from camera parameters and Y pixel location
    // (assume dimensions of noLaser are the same as all other images)

    float widthReference = GetReferenceLaserLocation(laserPos);

    if (widthReference < 0)
    {
        // we don't know where the refernce laser is -- abort
        return;
    }

    float laserCenter;

    int r, g, b;
    double pxDist, theta, phi;

    // set up single-pixel access to the subtracted and original image
    RgbImage noLaserPx(noLaser);
    int h = 0;
    for (h=0;h<int(laserPos->size());h++)
    {
        laserCenter = (*laserPos)[h];

        // check to make sure that we have data for this row
        if (laserCenter >= 0)
        {

            phi = (noLaser->height/2 - h) * double(CAMERA_Y_MAX - CAMERA_Y_MIN)/double(noLaser->height);

            // convert to radians
            phi = phi * 3.14159 / 180.0;

            pxDist = PixelToDistance2(laserCenter, widthReference);

            if (pxDist < 0)
            {
                // not a valid point
                return;
            }


            // compute theta based on laserCenter
            theta = (laserCenter - noLaser->width/2) * double(CAMERA_X_MAX - CAMERA_X_MIN)/double(noLaser->width);

            // convert to radians
            theta = theta * 3.14159 / 180.0;

            r = noLaserPx[h][int(laserCenter + 0.5)].r;
            g = noLaserPx[h][int(laserCenter + 0.5)].g;
            b = noLaserPx[h][int(laserCenter + 0.5)].b;

//          if (h == 100)
//          {
//              wxString tmp = wxT("\ndiff: ");
//              tmp << widthReference - laserCenter;