| 790 | } |
| 791 | |
| 792 | void |
| 793 | pcl::simulation::RangeLikelihood::getPointCloud( |
| 794 | pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc, |
| 795 | bool make_global, |
| 796 | const Eigen::Isometry3d& pose, |
| 797 | bool organized) const |
| 798 | { |
| 799 | // TODO: check if this works for for rows/cols >1 and for width&height != 640x480 |
| 800 | // i.e. multiple tiled images |
| 801 | pc->width = col_width_; |
| 802 | pc->height = row_height_; |
| 803 | // Was: |
| 804 | // pc->width = camera_width_; |
| 805 | // pc->height = camera_height_; |
| 806 | |
| 807 | pc->is_dense = true; |
| 808 | pc->points.resize(pc->width * pc->height); |
| 809 | |
| 810 | int points_added = 0; |
| 811 | |
| 812 | float camera_fx_reciprocal_ = 1.0f / camera_fx_; |
| 813 | float camera_fy_reciprocal_ = 1.0f / camera_fy_; |
| 814 | float zn = z_near_; |
| 815 | float zf = z_far_; |
| 816 | |
| 817 | const std::uint8_t* color_buffer = getColorBuffer(); |
| 818 | |
| 819 | // TODO: support decimation |
| 820 | // Copied the format of RangeImagePlanar::setDepthImage() |
| 821 | // Use this as a template for decimation |
| 822 | for (int y = 0; y < row_height_; ++y) // camera_height_ |
| 823 | { |
| 824 | for (int x = 0; x < col_width_; ++x) // camera_width_ |
| 825 | { |
| 826 | // Find XYZ from normalized 0->1 mapped disparity |
| 827 | int idx; |
| 828 | if (organized) |
| 829 | idx = y * col_width_ + x; |
| 830 | else |
| 831 | idx = points_added; // y*camera_width_ + x; |
| 832 | |
| 833 | float d = depth_buffer_[y * camera_width_ + x]; |
| 834 | |
| 835 | if (d < 1.0) // only add points with depth buffer less than max (20m) range |
| 836 | { |
| 837 | float z = zf * zn / ((zf - zn) * (d - zf / (zf - zn))); |
| 838 | |
| 839 | // TODO: add mode to ignore points with no return i.e. depth_buffer_ ==1 |
| 840 | // NB: OpenGL uses a Right Hand system with +X right, +Y up, +Z back out of the |
| 841 | // screen, The Z-buffer is natively -1 (far) to 1 (near). But in this class we |
| 842 | // invert this to be 0 (near, 0.7m) and 1 (far, 20m), so by negating y we get to |
| 843 | // a right-hand computer vision system which is also used by PCL and OpenNi. |
| 844 | (*pc)[idx].z = z; |
| 845 | (*pc)[idx].x = |
| 846 | (static_cast<float>(x) - camera_cx_) * z * (-camera_fx_reciprocal_); |
| 847 | (*pc)[idx].y = |
| 848 | (static_cast<float>(y) - camera_cy_) * z * (-camera_fy_reciprocal_); |
| 849 | |