C++ PCL(PointCloudLibrary)にて、ばらつきの多い点群データのばらつきを抑えたい

OpenCVとPCL(PointCloudLibrary)使用して、視差画像からデプス画像を作成後、
デプスから点群データを作成しています。
以下の図のようにばらつきの多い点群データを出力します。
このばらつきをなくしたいのですが何か方法をご教授いただきたく思います。

デプスデータは以下のようになり、大きい値は約9, 小さい値は-2 ほどになります。
<Depth_data>
-2.46917822e+03 -2.46917822e+03 -2.46917822e+03 -2.46917822e+03
-2.46917822e+03 -2.46917822e+03 -2.46917822e+03 -2.46917822e+03
-2.46917822e+03 -2.46917822e+03 -2.46917822e+03 -2.46917822e+03
-2.46917822e+03 -2.46917822e+03 -2.46917822e+03 -2.46917822e+03
1.64611877e+02 1.64611877e+02 1.64611877e+02 1.64611877e+02
1.64611877e+02 1.64611877e+02 1.64611877e+02 1.64611877e+02
1.64611877e+02 1.64611877e+02 1.64611877e+02 1.64611877e+02
1.64611877e+02 1.64611877e+02 1.64611877e+02 1.64611877e+02
1.64611877e+02 1.64611877e+02 1.64611877e+02 1.64611877e+02
<以下略>

デプスデータをPCL PointCloud型に変換するプログラムは以下になります。

C++
1int ImageProc::Make3DPoints(cv::Mat cam1Dist, std::vector<float> depthData, pcl::PointCloud<pcl::PointXYZ> &cloud) {
2	int chk = 0;
3	cv::Mat testImg(cam1Dist.size(), cam1Dist.type());
4	//--- マットから得た座標情報を格納 ---//
5	pcl::PointCloud<pcl::PointXYZ> matToPoint;
6	matToPoint.width = cam1Dist.cols;
7	matToPoint.height = cam1Dist.rows;	//cam1Dist.rows or 1;
8	matToPoint.is_dense = false;
9	matToPoint.resize(matToPoint.width * matToPoint.height);
10	//cloud.width = cam1Dist.cols;
11	//cloud.height = cam1Dist.rows;	//cam1Dist.rows or 1;
12	//cloud.is_dense = false;
13	//cloud.resize(cloud.width * cloud.height);
14	
15	//--- マットから、x,yの値を取得する ---//
16	for (int y = 0; y < cam1Dist.rows; y++) {
17		for (int x = 0; x < cam1Dist.cols; x++) {
18			float z;
19			for (int c = 0; c < cam1Dist.channels(); ++c) {
20				//--- x, yと対応したz値をPointXYZに格納 ---//
21				z = depthData[y * cam1Dist.cols + x];
22				matToPoint.points[y * cam1Dist.cols + x] = pcl::PointXYZ(x, y, z);
23			}
24		}
25	}
26	//--- 表示 ---//
27	pcl::visualization::CloudViewer viewer("Simple Cloud Viewer");
28	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>(cloud));
29	viewer.showCloud(cloud_ptr);
30
31	while (!viewer.wasStopped())
32	{
33	}
34    return 0;
35}