stereocamera.cc

来自「机器人人3D仿真工具,可以加入到Simbad仿真环境下应用。」· CC 代码 · 共 695 行 · 第 1/2 页

  this->renderOpt.farClip = this->farClip;
  
  // Render the models
  this->RenderModels();

  // Read the color buffer
  // We are being a bit naughty here, and assuming GLuint = unsigned char
  assert(sizeof(GLubyte) == sizeof(unsigned char));
  glPixelStorei(GL_PACK_ALIGNMENT, 1);
  glReadBuffer(GL_BACK);
  glReadPixels(0, 0, this->imgWidth, this->imgHeight,
               GL_RGB, GL_UNSIGNED_BYTE, this->images[camera]);
  PRINT_GL_ERR();

  return;
}


//////////////////////////////////////////////////////////////////////////////
// Render depth image in one of the cameras
void StereoCamera::RenderDepth(int camera)
{
  // Select the GL context
  if (this->depthContext.MakeCurrent() != 0)
    return;

  // Clear the window
  glClearColor(0, 0, 0, 0);
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

  // Use depth buffering
  glEnable(GL_DEPTH_TEST);

  // Enable back face culling
  glEnable(GL_CULL_FACE);
  glCullFace(GL_BACK);

  // Set the camera lens propeties (assumes square pixels)
  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();
  gluPerspective(this->hfov * 180 / M_PI, (double) this->imgWidth / this->imgHeight,
                 this->nearClip, this->farClip);

  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();

  GzPose pose;
  GzVector a, b;

  // Set the left camerapose; compute the point to look at, and the
  // orientation vector  
  pose = GzCoordPoseAdd(this->cameraPoses[camera], this->pose);
  a = GzVectorSet(1, 0, 0);
  b = GzVectorSet(1, 0, 1);
  a = GzCoordPositionAdd(a, pose.pos, pose.rot);
  b = GzCoordPositionAdd(b, pose.pos, pose.rot);
  b = GzVectorSub(b, a); 
  gluLookAt(pose.pos.x, pose.pos.y, pose.pos.z, a.x, a.y, a.z, b.x, b.y, b.z);

  // Enable lighting
  glEnable(GL_LIGHTING);

  // Set the ambient light to black
  GLfloat col[4];
  GZ_COLOR_COPY(col, GzColor(1.0, 1.0, 1.0, 1.0));
  glLightModelfv(GL_LIGHT_MODEL_AMBIENT, col);

  // Ligth model (two-sided)
  glLightModeli(GL_LIGHT_MODEL_TWO_SIDE, 1);

  // Set rendering options
  glShadeModel(GL_SMOOTH);
  glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);

  this->renderOpt.displayMaterials = false;
  this->renderOpt.displayTextures = false;
  this->renderOpt.cameraIndex = this->depthContext.GetContextIndex();
  this->renderOpt.cameraPose = pose;
  this->renderOpt.farClip = this->farClip;
  
  // Render the models
  this->RenderModels();

  // Read the depth buffer
  // We are being a bit naughty here, and assuming GLfloat = float
  assert(sizeof(GLfloat) == sizeof(float));
  glPixelStorei(GL_PACK_ALIGNMENT, 1);
  glReadBuffer(GL_BACK);
  glReadPixels(0, 0, this->imgWidth, this->imgHeight,
               GL_DEPTH_COMPONENT, GL_FLOAT, this->depthImages[camera]);

  return;
}


//////////////////////////////////////////////////////////////////////////////
// Render the models
void StereoCamera::RenderModels()
{
  int i, p;
  Model *model;

  for (p = 0; p < GZ_RENDER_MAX_PASSES; p++)
  {
    for (i = 0; i < this->world->GetNumModels(); i++)
    {
      model = this->world->GetModels()[i];
      model->Render(p, &this->renderOpt);
    }
  }

  /*int p, i, j, k;
  Model *model;
  Body *body;
  Geom *geom;

  // Make multiple passes to ensure geoms get rendered in correct
  // order
  for (p = 0; p < GZ_RENDER_MAX_PASSES; p++)
  {
    for (i = 0; i < this->world->GetNumModels(); i++)
    {
      model = this->world->GetModels()[i];
      for (j = 0; j < model->GetNumBodies(); j++)
      {
        body = model->GetBodies()[j];        
        for (k = 0; k < body->GetNumGeoms(); k++)
        {
          geom = body->GetGeoms()[k];          
          if (geom->renderOrder == p)
          {
            geom->PreRender(&this->renderOpt);
            geom->Render(&this->renderOpt);
            geom->PostRender(&this->renderOpt);
          }
        }
      }
    }
  }
  */

  return;
}


//////////////////////////////////////////////////////////////////////////////
// Post-process the disparity images to calculate occlusions, etc
void StereoCamera::ProcessDisparity()
{
  int i, j;
  float *lpix, *rpix, *ldep, *rdep;
  float n, f;
  float lz, rz;
  float qx;
  int li, ri;
  float zThresh;

  n = this->nearClip;  
  f = this->farClip;

  // Depth error threshold between left and right images
  zThresh = 0.10;
  
  // Scale the depth values so they represent z-distance
  for (j = 0; j < this->imgHeight; j++)
  {
    ldep = this->depthImages[0] + j * this->imgWidth;
    rdep = this->depthImages[1] + j * this->imgWidth;
    
    for (i = 0; i < this->imgWidth; i++, ldep++, rdep++)
    {
      *ldep = -f * n / (*ldep * (f - n) - f);
      *rdep = -f * n / (*rdep * (f - n) - f);
    }
  }
  
  // Check for occlusions
  for (j = 0; j < this->imgHeight; j++)
  {
    ldep = this->depthImages[0] + j * this->imgWidth;
    rdep = this->depthImages[1] + j * this->imgWidth;
    
    lpix = this->disparityImages[0] + j * this->imgWidth;
    rpix = this->disparityImages[1] + j * this->imgWidth;
    
    for (i = 0; i < this->imgWidth; i++)
    {
      lz = ldep[i];
      rz = rdep[i];

      if (this->disparityEnable[0])
      {
        // Transform from left image to right image.  This is optimized
        // for parallel axis, row aligned cameras.
        li = i;
        qx = this->iP.m[0][0] * (li + 0.5) + this->iP.m[0][2];
        qx -= this->baseline / lz;
        ri = (int) (this->P.m[0][0] * qx + this->P.m[0][2]);

        // Check bounds and check the z values for consistency
        if (ri < 0 || fabs(lz - rdep[ri]) > zThresh)
        {
          lpix[i] = 0;
        }
        else
        {
          lpix[i] = li - ri;
        }
      }
      else
        lpix[i] = 0;

      if (this->disparityEnable[1])
      {
        // Transform from right image to left image.  This is optimized
        // for parallel axis, row aligned cameras.
        ri = i;
        qx = this->iP.m[0][0] * (ri + 0.5) + this->iP.m[0][2];
        qx += this->baseline / rz;
        li = (int) (this->P.m[0][0] * qx + this->P.m[0][2]);

        // Check bounds and check the z values for consistency
        if (li >= this->imgWidth || fabs(rz - ldep[li]) > zThresh)
        {
          rpix[i] = 0;
        }
        else
        {
          rpix[i] = li - ri;
        }
      }
      else
        rpix[i] = 0;
    }
  }

  return;
}



//////////////////////////////////////////////////////////////////////////////
// Set the base filename for saved frames
void StereoCamera::SetSavePath(const char *pathname)
{
  this->savePathname = pathname;
  this->saveCount = 0;
  return;
}


//////////////////////////////////////////////////////////////////////////////
// Enable or disable saving
void StereoCamera::EnableSaveFrame(bool enable)
{
  char tmp[1024];
    
  this->saveEnable = enable;
  if (this->saveEnable)
  {
    sprintf(tmp, "mkdir %s 2>>/dev/null", this->savePathname);
    system(tmp);
  }

  return;
}


//////////////////////////////////////////////////////////////////////////////
// Save the current frame to disk
void StereoCamera::SaveFrame()
{
  char tmp[1024];
  FILE *fp;

  // Save left image
  if (this->imageEnable[0])
  {
    sprintf(tmp, "%s/left_%04d.pnm", this->savePathname, this->saveCount);
    fp = fopen( tmp, "wb" );
    if (!fp)
    {
      PRINT_ERR1( "unable to open file %s\n for writing", tmp );
      return;
    }
    fprintf( fp, "P6\n# Gazebo\n%d %d\n255\n", this->imgWidth, this->imgHeight);
    for (int i = this->imgHeight-1; i >= 0; i--)
      fwrite( this->images[0] + i * this->imgWidth * 3, 1, this->imgWidth * 3, fp );
    fclose( fp );
  }

  // Save right image
  if (this->imageEnable[1])
  {
    sprintf(tmp, "%s/right_%04d.pnm", this->savePathname, this->saveCount);
    fp = fopen( tmp, "wb" );
    if (!fp)
    {
      PRINT_ERR1( "unable to open file %s\n for writing", tmp );
      return;
    }  
    fprintf( fp, "P6\n# Gazebo\n%d %d\n255\n", this->imgWidth, this->imgHeight);
    for (int i = this->imgHeight-1; i >= 0; i--)
      fwrite( this->images[1] + i * this->imgWidth * 3, 1, this->imgWidth * 3, fp );
    fclose( fp );
  }

  // Compute scaling factors so we use the entire grayscale range with
  // color = 1 / depth
  float a = (this->nearClip * this->farClip) / (this->farClip - this->nearClip);
  float b = -this->nearClip / (this->farClip - this->nearClip);

  // Allocate tmp array
  unsigned char *dst = new unsigned char[this->imgWidth];
    
  // Save depth image
  sprintf(tmp, "%s/left_depth_%04d.pnm", this->savePathname, this->saveCount);
  fp = fopen( tmp, "wb" );
  if (!fp)
  {
    PRINT_ERR1( "unable to open file %s\n for writing", tmp );
    return;
  }
  fprintf( fp, "P5\n# Gazebo\n%d %d\n255\n", this->imgWidth, this->imgHeight);
  for (int i = this->imgHeight-1; i >= 0; i--)
  {
    float *src = this->depthImages[0] + i * this->imgWidth;    
    for (int j = 0; j < this->imgWidth; j++)
    {
      if (src[j] < 1e-6)
        dst[j] = 0;
      else
        dst[j] = (int) (255 * (a / src[j] + b));
    }
    fwrite( dst, 1, this->imgWidth, fp );
  }
  fclose( fp );

  // Free tmp array
  delete [] dst;
  
  this->saveCount++;

  return;
}