2016-10-13 4 views
0

Depth Point cloud Привет Я преобразую изображение глубины в облако точек и, кажется, останавливается на 1/3 ширины изображения. Может быть, что-то в моей итерации через координаты ошибочно?Глубина, чтобы облако точки, итератор неправильно?

float* p = (float*)depth.data; // one option to iterate through 
    int index1 = 0; // just for counting the points 
    for (int y = 0; y < depthImageSize.height; ++y) 
    { 
     for (int x = 0; x < depthImageSize.width; ++x) 
     { 
      pcl::PointXYZRGBA pt; 
      //float depthValue = static_cast<float>(*p); 
      float depthValue = depth.at<float>(y,x); 
      //cout << p.pos() << endl; 
      //cout << x << ", " << y << ": " << depthValue << endl; 
      //++p; 
      index1++; 

      if (depthValue == -1000.f || depthValue == 0.0f || boost::math::isnan(depthValue)) 
      { 
      } 
      else 
      { 

       pt.x = (static_cast<float>(x) - centerX) * scaleFactorX * depthValue; 
       pt.y = (centerY - static_cast<float>(y)) * scaleFactorY * depthValue; 
       pt.z = depthValue; 

       //bgr has to be resized 
       cv::resize(bgr,bgr,depth.size()); 
       cv::Vec3b colorValue = bgr.at<cv::Vec3b>(y, x); 
       RGBValue color; 
       color.Red = colorValue[2]; 
       color.Green = colorValue[1]; 
       color.Blue = colorValue[0]; 
       pt.rgba = color.long_value; 


      } 
      result->points.push_back(pt); 
     } 

    } 

ответ

0

Я бы сказал, что это должно быть

depth.at<float>(x,y)