C++ PointCloud::resize方法代码示例

void SuperFrameParser::convertImageToPointCloud (const sensor_msgs::ImagePtr& depth_msg, const pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud)
{
    cloud->height = depth_msg->height;
    cloud->width = depth_msg->width;
    cloud->resize (cloud->height * cloud->width);
    // Use correct principal point from calibration
    float center_x = depth_info_->K[2]; // c_x
    float center_y = depth_info_->K[5]; // c_y

    // Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y)
    double unit_scaling = 0.001f;
    float constant_x = unit_scaling / depth_info_->K[0]; // f_x
    float constant_y = unit_scaling / depth_info_->K[4]; // f_y
    float bad_point = std::numeric_limits<float>::quiet_NaN ();

    pcl::PointCloud<pcl::PointXYZ>::iterator pt_iter = cloud->begin ();
    const uint16_t* depth_row = reinterpret_cast<const uint16_t*> (&depth_msg->data[0]);
    int row_step = depth_msg->step / sizeof (uint16_t);
    for (int v = 0; v < (int)depth_msg->height; ++v, depth_row += row_step)
    {
        for (int u = 0; u < (int)depth_msg->width; ++u)
        {
            pcl::PointXYZ& pt = *pt_iter++;
            uint16_t depth = depth_row[u];

            // Missing points denoted by NaNs
            if (depth == 0.0f)
            {
                pt.x = pt.y = pt.z = bad_point;
                continue;
            }

            // Fill in XYZ
            pt.x = (u - center_x) * depth * constant_x;
            pt.y = (v - center_y) * depth * constant_y;
            pt.z = depth * 0.001f;
        }
    }
}

void 
pcl::StereoMatching::getVisualMap (pcl::PointCloud<pcl::RGB>::Ptr vMap)
{
  if ( static_cast<int> (vMap->width) != width_ || static_cast<int> (vMap->height) != height_)
  {
    vMap->resize(width_*height_);
    vMap->width = width_;
    vMap->height = height_;
  }

  if ( vMap->is_dense)
    vMap->is_dense = false;

  pcl::RGB invalid_val;
  invalid_val.r = 0;
  invalid_val.g = 255;
  invalid_val.b = 0;

  float scale = 255.0f / (16.0f * static_cast<float> (max_disp_));

  for (int y = 0; y<height_; y++)
  {
    for (int x = 0; x<width_; x++)
    {
      if (disp_map_[y * width_+ x] <= 0)
      {
        vMap->at (x,y) = invalid_val;
      }
      else
      {
        unsigned char val = static_cast<unsigned char> (floor (scale*disp_map_[y * width_+ x]));
        vMap->at (x, y).r = val;
        vMap->at (x, y).g = val;
        vMap->at (x, y).b = val;
      }
    }
  }
}

template <typename PointInT, typename PointOutT> void
pcl_1_8::Edge<PointInT, PointOutT>::detectEdgeRoberts (pcl::PointCloud<PointOutT> &output)
{
  convolution_.setInputCloud (input_);

  pcl::PointCloud<pcl::PointXYZI>::Ptr kernel_x (new pcl::PointCloud<pcl::PointXYZI>);
  pcl::PointCloud<pcl::PointXYZI>::Ptr magnitude_x (new pcl::PointCloud<pcl::PointXYZI>);
  kernel_.setKernelType (kernel<pcl::PointXYZI>::ROBERTS_X);
  kernel_.fetchKernel (*kernel_x);
  convolution_.setKernel (*kernel_x);
  convolution_.filter (*magnitude_x);

  pcl::PointCloud<pcl::PointXYZI>::Ptr kernel_y (new pcl::PointCloud<pcl::PointXYZI>);
  pcl::PointCloud<pcl::PointXYZI>::Ptr magnitude_y (new pcl::PointCloud<pcl::PointXYZI>);
  kernel_.setKernelType (kernel<pcl::PointXYZI>::ROBERTS_Y);
  kernel_.fetchKernel (*kernel_y);
  convolution_.setKernel (*kernel_y);
  convolution_.filter (*magnitude_y);

  const int height = input_->height;
  const int width = input_->width;

  output.resize (height * width);
  output.height = height;
  output.width = width;

  for (size_t i = 0; i < output.size (); ++i)
  {
    output[i].magnitude_x = (*magnitude_x)[i].intensity;
    output[i].magnitude_y = (*magnitude_y)[i].intensity;
    output[i].magnitude = 
      sqrtf ((*magnitude_x)[i].intensity * (*magnitude_x)[i].intensity + 
             (*magnitude_y)[i].intensity * (*magnitude_y)[i].intensity);
    output[i].direction = 
      atan2f ((*magnitude_y)[i].intensity, (*magnitude_x)[i].intensity);
  }
}

int computeNormalsAndEigenvalues(const typename pcl::PointCloud<PointInT>::Ptr
                                 &in_cloud,
                                 const float &radius, const int &n_threads,
                                 pcl::PointCloud<PointOutT> &out_cloud)
{
    size_t n_points = in_cloud->size();
    // resize the out cloud
    out_cloud.resize(n_points);

    // build the kdtree
    pcl::KdTreeFLANN<PointInT> flann;
    flann.setInputCloud(in_cloud);

    // place for neighs ids and distances
    std::vector<int> indices;
    std::vector<float> distances;
#ifdef USE_OPENMP
#pragma omp parallel for shared(out_cloud) private(indices, distances)         \
    num_threads(n_threads)
#endif
    for (int i = 0; i < n_points; ++i) {
        // get neighbors for this point
        flann.radiusSearch((*in_cloud)[i], radius, indices, distances);

        // estimate normal and eigenvalues
        computePointNormal(*in_cloud, indices, out_cloud[i].normal_x,
                           out_cloud[i].normal_y, out_cloud[i].normal_z,
                           out_cloud[i].lam0, out_cloud[i].lam1,
                           out_cloud[i].lam2);

        flipNormal
            <PointInT>((*(in_cloud))[i], 0.0, 0.0, 0.0, out_cloud[i].normal_x,
                       out_cloud[i].normal_y, out_cloud[i].normal_z);
    }

    return 1;
}

  void
  FeatureFactory::extractRangeFeature (cv_bridge::CvImagePtr cv_ptr, pcl::PointCloud<feature_factory::FeaturePoint<
      RANGE_FEATURES_HISTOGRAM> > &range_features, FeatureType feature_type)
  {
    double max_feature_val;
    if (feature_type == DEPTH)
      max_feature_val = MAX_DEPTH_FEATURE_VAL;
    else
      max_feature_val = MAX_CONF_AMP_FEATURE_VAL;

    const uint16_t n_grids_per_row = RANGE_IMAGE_WIDTH / grid_edge_size;
    const uint16_t n_grids_per_col = RANGE_IMAGE_HEIGHT / grid_edge_size;
    const uint16_t n_grids = n_grids_per_col * n_grids_per_row;

    range_features.resize (n_grids);
    for (uint16_t gri = 0; gri < n_grids_per_col; gri++)
      for (uint16_t gci = 0; gci < n_grids_per_row; gci++)
      {
        // Hint: Rect(top_left_x, top_left_y, width, height)
        range_features[gri * n_grids_per_row + gci]
            = feature_factory::FeaturePoint<RANGE_FEATURES_HISTOGRAM>::calculate (cv_ptr->image, cv::Rect (gci
                * grid_edge_size, gri * grid_edge_size, grid_edge_size, grid_edge_size), 0, max_feature_val);
      }
  }

//////////////////////////////////////////////////////////////////////////////
//const pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pcl::StereoMatching::getPointCloud(float uC, float vC, float focal, float baseline)
bool 
pcl::StereoMatching::getPointCloud (
    float u_c, float v_c, float focal, float baseline, 
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
  
  //disp map has not been computed yet..
  if ( disp_map_ == NULL)
  {

    PCL_ERROR(
      "[pcl::StereoMatching::getPointCloud] Error: a disparity map has not been computed yet. The resulting cloud can not be computed..\n"
    );

    return false;
  }

  //cloud needs to be re-allocated
  if (static_cast<int> (cloud->width) != width_ || static_cast<int> (cloud->height) != height_)
  {
    cloud->resize(width_*height_);
    cloud->width = width_;
    cloud->height = height_;
    cloud->is_dense = false;
  }

  if ( cloud->is_dense)
    cloud->is_dense = false;

  //Loop
  pcl::PointXYZ temp_point;
  pcl::PointXYZ nan_point;
  nan_point.x = std::numeric_limits<float>::quiet_NaN();
  nan_point.y = std::numeric_limits<float>::quiet_NaN();
  nan_point.z = std::numeric_limits<float>::quiet_NaN();
  //nan_point.intensity = std::numeric_limits<float>::quiet_NaN();

  //all disparities are multiplied by a constant equal to 16; 
  //this must be taken into account when computing z values
  float depth_scale = baseline * focal * 16.0f;

  for ( int j=0; j<height_; j++)
  {
    for ( int i=0; i<width_; i++)
    {
      if ( disp_map_[ j*width_ + i] > 0 )
      {

        temp_point.z = depth_scale / disp_map_[j * width_ + i];
        temp_point.x = ((static_cast<float> (i) - u_c) * temp_point.z) / focal;
        temp_point.y = ((static_cast<float> (j) - v_c) * temp_point.z) / focal;
        //temp_point.intensity = 255;

        (*cloud)[j * width_ + i] = temp_point;
      }
      //adding NaN value
      else
      {
        (*cloud)[j * width_ + i] = nan_point;
      }
    }
  }

  return (true);
}

template <typename PointNT> void
pcl::Poisson<PointNT>::performReconstruction (pcl::PointCloud<PointNT> &points,
                                              std::vector<pcl::Vertices> &polygons)
{
  poisson::CoredVectorMeshData mesh;
  poisson::Point3D<float> center;
  float scale = 1.0f;

  switch (degree_)
  {
  case 1:
  {
    execute<1> (mesh, center, scale);
    break;
  }
  case 2:
  {
    execute<2> (mesh, center, scale);
    break;
  }
  case 3:
  {
    execute<3> (mesh, center, scale);
    break;
  }
  case 4:
  {
    execute<4> (mesh, center, scale);
    break;
  }
  case 5:
  {
    execute<5> (mesh, center, scale);
    break;
  }
  default:
  {
    PCL_ERROR (stderr, "Degree %d not supported\n", degree_);
  }
  }

  // Write output PolygonMesh
  // Write vertices
  points.resize (int (mesh.outOfCorePointCount () + mesh.inCorePoints.size ()));
  poisson::Point3D<float> p;
  for (int i = 0; i < int(mesh.inCorePoints.size ()); i++)
  {
    p = mesh.inCorePoints[i];
    points.points[i].x = p.coords[0]*scale+center.coords[0];
    points.points[i].y = p.coords[1]*scale+center.coords[1];
    points.points[i].z = p.coords[2]*scale+center.coords[2];
  }
  for (int i = int(mesh.inCorePoints.size()); i < int (mesh.outOfCorePointCount() + mesh.inCorePoints.size ()); i++)
  {
    mesh.nextOutOfCorePoint (p);
    points.points[i].x = p.coords[0]*scale+center.coords[0];
    points.points[i].y = p.coords[1]*scale+center.coords[1];
    points.points[i].z = p.coords[2]*scale+center.coords[2];
  }

  polygons.resize (mesh.polygonCount ());

  // Write faces
  std::vector<poisson::CoredVertexIndex> polygon;
  for (int p_i = 0; p_i < mesh.polygonCount (); p_i++)
  {
    pcl::Vertices v;
    mesh.nextPolygon (polygon);
    v.vertices.resize (polygon.size ());

    for (int i = 0; i < static_cast<int> (polygon.size ()); ++i)
      if (polygon[i].inCore )
        v.vertices[i] = polygon[i].idx;
      else
        v.vertices[i] = polygon[i].idx + int (mesh.inCorePoints.size ());

    polygons[p_i] = v;
  }
}

template <typename PointT> void
pcl::copyPointCloud (const pcl::PointCloud<PointT> &cloud_in, pcl::PointCloud<PointT> &cloud_out,
                     int top, int bottom, int left, int right, pcl::InterpolationType border_type, const PointT& value)
{
  if (top < 0 || left < 0 || bottom < 0 || right < 0)
  {
    std::string faulty = (top < 0) ? "top" : (left < 0) ? "left" : (bottom < 0) ? "bottom" : "right";
    PCL_THROW_EXCEPTION (pcl::BadArgumentException, "[pcl::copyPointCloud] error: " << faulty << " must be positive!");
    return;
  }

  if (top == 0 && left == 0 && bottom == 0 && right == 0)
   cloud_out = cloud_in;
  else
  {
    // Allocate enough space and copy the basics
    cloud_out.header   = cloud_in.header;
    cloud_out.width    = cloud_in.width + left + right;
    cloud_out.height   = cloud_in.height + top + bottom;
    if (cloud_out.size () != cloud_out.width * cloud_out.height)
      cloud_out.resize (cloud_out.width * cloud_out.height);
    cloud_out.is_dense = cloud_in.is_dense;
    cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
    cloud_out.sensor_origin_ = cloud_in.sensor_origin_;

    if (border_type == pcl::BORDER_TRANSPARENT)
    {
      const PointT* in = &(cloud_in.points[0]);
      PointT* out = &(cloud_out.points[0]);
      PointT* out_inner = out + cloud_out.width*top + left;
      for (uint32_t i = 0; i < cloud_in.height; i++, out_inner += cloud_out.width, in += cloud_in.width)
      {
        if (out_inner != in)
          memcpy (out_inner, in, cloud_in.width * sizeof (PointT));
      }
    }
    else
    {
      // Copy the data
      if (border_type != pcl::BORDER_CONSTANT)
      {
        try
        {
          std::vector<int> padding (cloud_out.width - cloud_in.width);
          int right = cloud_out.width - cloud_in.width - left;
          int bottom = cloud_out.height - cloud_in.height - top;

          for (int i = 0; i < left; i++)
            padding[i] = pcl::interpolatePointIndex (i-left, cloud_in.width, border_type);

          for (int i = 0; i < right; i++)
            padding[i+left] = pcl::interpolatePointIndex (cloud_in.width+i, cloud_in.width, border_type);

          const PointT* in = &(cloud_in.points[0]);
          PointT* out = &(cloud_out.points[0]);
          PointT* out_inner = out + cloud_out.width*top + left;

          for (uint32_t i = 0; i < cloud_in.height; i++, out_inner += cloud_out.width, in += cloud_in.width)
          {
            if (out_inner != in)
              memcpy (out_inner, in, cloud_in.width * sizeof (PointT));

            for (int j = 0; j < left; j++)
              out_inner[j - left] = in[padding[j]];

            for (int j = 0; j < right; j++)
              out_inner[j + cloud_in.width] = in[padding[j + left]];
          }

          for (int i = 0; i < top; i++)
          {
            int j = pcl::interpolatePointIndex (i - top, cloud_in.height, border_type);
            memcpy (out + i*cloud_out.width,
                    out + (j+top) * cloud_out.width,
                    sizeof (PointT) * cloud_out.width);
          }

          for (int i = 0; i < bottom; i++)
          {
            int j = pcl::interpolatePointIndex (i + cloud_in.height, cloud_in.height, border_type);
            memcpy (out + (i + cloud_in.height + top)*cloud_out.width,
                    out + (j+top)*cloud_out.width,
                    cloud_out.width * sizeof (PointT));
          }
        }
        catch (pcl::BadArgumentException &e)
        {
          PCL_ERROR ("[pcl::copyPointCloud] Unhandled interpolation type %d!\n", border_type);
        }
      }
      else
      {
        int right = cloud_out.width - cloud_in.width - left;
        int bottom = cloud_out.height - cloud_in.height - top;
        std::vector<PointT> buff (cloud_out.width, value);
        PointT* buff_ptr = &(buff[0]);
        const PointT* in = &(cloud_in.points[0]);
        PointT* out = &(cloud_out.points[0]);
        PointT* out_inner = out + cloud_out.width*top + left;

//.........这里部分代码省略.........

template <typename PointInT, typename PointOutT> void
pcl_1_8::Edge<PointInT, PointOutT>::canny (
    const pcl::PointCloud<PointInT> &input_x, 
    const pcl::PointCloud<PointInT> &input_y,
    pcl::PointCloud<PointOutT> &output)
{
  float tHigh = hysteresis_threshold_high_;
  float tLow = hysteresis_threshold_low_;
  const int height = input_x.height;
  const int width = input_x.width;

  output.resize (height * width);
  output.height = height;
  output.width = width;

  // Noise reduction using gaussian blurring
  pcl::PointCloud<pcl::PointXYZI>::Ptr gaussian_kernel (new pcl::PointCloud<pcl::PointXYZI>);
  kernel_.setKernelSize (3);
  kernel_.setKernelSigma (1.0);
  kernel_.setKernelType (kernel<pcl::PointXYZI>::GAUSSIAN);
  kernel_.fetchKernel (*gaussian_kernel);
  convolution_.setKernel (*gaussian_kernel);

  PointCloudIn smoothed_cloud_x;
  convolution_.setInputCloud (input_x.makeShared());
  convolution_.filter (smoothed_cloud_x);

  PointCloudIn smoothed_cloud_y;
  convolution_.setInputCloud (input_y.makeShared());
  convolution_.filter (smoothed_cloud_y);


  // Edge detection usign Sobel
  pcl::PointCloud<PointXYZIEdge>::Ptr edges (new pcl::PointCloud<PointXYZIEdge>);
  sobelMagnitudeDirection (smoothed_cloud_x, smoothed_cloud_y, *edges.get ());

  // Edge discretization
  discretizeAngles (*edges);

  pcl::PointCloud<pcl::PointXYZI>::Ptr maxima (new pcl::PointCloud<pcl::PointXYZI>);
  suppressNonMaxima (*edges, *maxima, tLow);

  // Edge tracing
  for (int i = 0; i < height; i++)
  {
    for (int j = 0; j < width; j++)
    {
      if ((*maxima)(j, i).intensity < tHigh || (*maxima)(j, i).intensity == std::numeric_limits<float>::max ())
        continue;

      (*maxima)(j, i).intensity = std::numeric_limits<float>::max ();
      cannyTraceEdge ( 1, 0, i, j, *maxima);
      cannyTraceEdge (-1, 0, i, j, *maxima);
      cannyTraceEdge ( 1, 1, i, j, *maxima);
      cannyTraceEdge (-1, -1, i, j, *maxima);
      cannyTraceEdge ( 0, -1, i, j, *maxima);
      cannyTraceEdge ( 0, 1, i, j, *maxima);
      cannyTraceEdge (-1, 1, i, j, *maxima);
      cannyTraceEdge ( 1, -1, i, j, *maxima);
    }
  }

  // Final thresholding
  for (int i = 0; i < height; i++)
  {
    for (int j = 0; j < width; j++)
    {
      if ((*maxima)(j, i).intensity == std::numeric_limits<float>::max ())
        output (j, i).magnitude = 255;
      else
        output (j, i).magnitude = 0;
    }
  }
}

template<typename PointInT, typename PointOutT> void
pcl_1_8::Edge<PointInT, PointOutT>::detectEdgeCanny (pcl::PointCloud<PointOutT> &output)
{
  float tHigh = hysteresis_threshold_high_;
  float tLow = hysteresis_threshold_low_;
  const int height = input_->height;
  const int width = input_->width;

  output.resize (height * width);
  output.height = height;
  output.width = width;

  //pcl::console::TicToc tt;
  //tt.tic ();
  
  // Noise reduction using gaussian blurring
  pcl::PointCloud<pcl::PointXYZI>::Ptr gaussian_kernel (new pcl::PointCloud<pcl::PointXYZI>);
  PointCloudInPtr smoothed_cloud (new PointCloudIn);
  kernel_.setKernelSize (3);
  kernel_.setKernelSigma (1.0);
  kernel_.setKernelType (kernel<pcl::PointXYZI>::GAUSSIAN);
  kernel_.fetchKernel (*gaussian_kernel);
  convolution_.setKernel (*gaussian_kernel);
  convolution_.setInputCloud (input_);
  convolution_.filter (*smoothed_cloud);
  //PCL_ERROR ("Gaussian blur: %g\n", tt.toc ()); tt.tic ();
  
  // Edge detection usign Sobel
  pcl::PointCloud<PointXYZIEdge>::Ptr edges (new pcl::PointCloud<PointXYZIEdge>);
  setInputCloud (smoothed_cloud);
  detectEdgeSobel (*edges);
  //PCL_ERROR ("Sobel: %g\n", tt.toc ()); tt.tic ();
  
  // Edge discretization
  discretizeAngles (*edges);
  //PCL_ERROR ("Discretize: %g\n", tt.toc ()); tt.tic ();

  // tHigh and non-maximal supression
  pcl::PointCloud<pcl::PointXYZI>::Ptr maxima (new pcl::PointCloud<pcl::PointXYZI>);
  suppressNonMaxima (*edges, *maxima, tLow);
  //PCL_ERROR ("NM suppress: %g\n", tt.toc ()); tt.tic ();

  // Edge tracing
  for (int i = 0; i < height; i++)
  {
    for (int j = 0; j < width; j++)
    {
      if ((*maxima)(j, i).intensity < tHigh || (*maxima)(j, i).intensity == std::numeric_limits<float>::max ())
        continue;

      (*maxima)(j, i).intensity = std::numeric_limits<float>::max ();
      cannyTraceEdge ( 1, 0, i, j, *maxima);
      cannyTraceEdge (-1, 0, i, j, *maxima);
      cannyTraceEdge ( 1, 1, i, j, *maxima);
      cannyTraceEdge (-1, -1, i, j, *maxima);
      cannyTraceEdge ( 0, -1, i, j, *maxima);
      cannyTraceEdge ( 0, 1, i, j, *maxima);
      cannyTraceEdge (-1, 1, i, j, *maxima);
      cannyTraceEdge ( 1, -1, i, j, *maxima);
    }
  }
  //PCL_ERROR ("Edge tracing: %g\n", tt.toc ());

  // Final thresholding
  for (size_t i = 0; i < input_->size (); ++i)
  {
    if ((*maxima)[i].intensity == std::numeric_limits<float>::max ())
      output[i].magnitude = 255;
    else
      output[i].magnitude = 0;
  }
}

void get_en_image(pcl::PointCloud<pcl::PointXYZ> &cloud)
{
    char flag = 'g';
    int i = 0;
    while(flag != 'q')
    {
        ostringstream conv;
        conv << i;
        cout<<"Capturing new calibration image from the ensenso stereo vision camera."<<endl;
        ///Read the Ensenso stereo cameras:
        try {
            // Initialize NxLib and enumerate cameras
            nxLibInitialize(true);

            // Reference to the first camera in the node BySerialNo
            NxLibItem root;
            NxLibItem camera = root[itmCameras][itmBySerialNo][0];

            // Open the Ensenso
            NxLibCommand open(cmdOpen);
            open.parameters()[itmCameras] = camera[itmSerialNumber].asString();
            open.execute();

            // Capture an image
            NxLibCommand (cmdCapture).execute();

            // Stereo matching task
            NxLibCommand (cmdComputeDisparityMap).execute ();

            // Convert disparity map into XYZ data for each pixel
            NxLibCommand (cmdComputePointMap).execute ();

            // Get info about the computed point map and copy it into a std::vector
            double timestamp;
            std::vector<float> pointMap;
            int width, height;
            camera[itmImages][itmRaw][itmLeft].getBinaryDataInfo (0, 0, 0, 0, 0, &timestamp);  // Get raw image timestamp
            camera[itmImages][itmPointMap].getBinaryDataInfo (&width, &height, 0, 0, 0, 0);
            camera[itmImages][itmPointMap].getBinaryData (pointMap, 0);

            // Copy point cloud and convert in meters
            //cloud.header.stamp = getPCLStamp (timestamp);
            cloud.resize (height * width);
            cloud.width = width;
            cloud.height = height;
            cloud.is_dense = false;

            // Copy data in point cloud (and convert milimeters in meters)
            for (size_t i = 0; i < pointMap.size (); i += 3)
            {
              cloud.points[i / 3].x = pointMap[i] / 1000.0;
              cloud.points[i / 3].y = pointMap[i + 1] / 1000.0;
              cloud.points[i / 3].z = pointMap[i + 2] / 1000.0;
            }

            NxLibCommand (cmdRectifyImages).execute();

            // Save images
            NxLibCommand saveImage(cmdSaveImage);
            //   raw left
            saveImage.parameters()[itmNode] = camera[itmImages][itmRaw][itmLeft].path;
            saveImage.parameters()[itmFilename] = "calib_en/raw_left" + conv.str()+".png";
            saveImage.execute();
            //   raw right
            /*saveImage.parameters()[itmNode] = camera[itmImages][itmRaw][itmRight].path;
            saveImage.parameters()[itmFilename] = "calib_en/raw_right.png";
            saveImage.execute();
            //   rectified left
            saveImage.parameters()[itmNode] = camera[itmImages][itmRectified][itmLeft].path;
            saveImage.parameters()[itmFilename] = "calib_en/rectified_left.png";
            saveImage.execute();
            //   rectified right
            saveImage.parameters()[itmNode] = camera[itmImages][itmRectified][itmRight].path;
            saveImage.parameters()[itmFilename] = "calib_en/rectified_right.png";
            saveImage.execute();*/
        } catch (NxLibException& e) { // Display NxLib API exceptions, if any
            printf("An NxLib API error with code %d (%s) occurred while accessing item %s.\n", e.getErrorCode(), e.getErrorText().c_str(), e.getItemPath().c_str());
            if (e.getErrorCode() == NxLibExecutionFailed) printf("/Execute:\n%s\n", NxLibItem(itmExecute).asJson(true).c_str());
        }
        /*catch (NxLibException &ex)
        {
            ensensoExceptionHandling (ex, "grabSingleCloud");
        }*/
        catch (...) { // Display other exceptions
            printf("Something, somewhere went terribly wrong!\n");
        }

        /*cout<<"Plug in the RGB camera and press any key to continue."<<endl;
        cin.ignore();
        cin.get();*/
        cout<<"Capturing new calibration image from the ensenso RGB camera."<<endl;

        ///Read the IDS RGB Camera attached to the Ensenso stereo camera
        HIDS hCam = 0;
        printf("Success-Code: %d\n",IS_SUCCESS);
        //Kamera öffnen
        INT nRet = is_InitCamera (&hCam, NULL);
        printf("Status Init %d\n",nRet);

        //Pixel-Clock setzen
//.........这里部分代码省略.........