Made some optimization about pointscloud

samples/tutorials/util/pc_viewer.cc

@@ -84,6 +84,7 @@ void PCViewer::ConvertMatToPointCloud(
   for (int i = 0; i < xyz.rows; i++) {
     for (int j = 0; j < xyz.cols; j++) {
       auto &&p = xyz.at<cv::Point3f>(i, j);
+			if (std::abs(p.z) > 9999) continue;
       if (std::isfinite(p.x) && std::isfinite(p.y) && std::isfinite(p.z)) {
         // LOG(INFO) << "[" << i << "," << j << "] x: " << p.x << ", y: " << p.y
         // << ", z: " << p.z;

src/api/processor/disparity_processor.cc

@@ -26,6 +26,7 @@ const char DisparityProcessor::NAME[] = "DisparityProcessor";
 DisparityProcessor::DisparityProcessor(std::int32_t proc_period)
     : Processor(std::move(proc_period)) {
   VLOG(2) << __func__ << ": proc_period=" << proc_period;
+
 	int blockSize_ = 15;           // 15
 	int minDisparity_ = 0;         // 0
 	int numDisparities_ = 64;      // 64
@@ -68,7 +69,7 @@ bool DisparityProcessor::OnProcess(
 
   cv::Mat disparity;
   bm_->compute(input->first, input->second, disparity);
-  output->value = disparity;
+	disparity.convertTo(output->value, CV_32F, 1./16);
   return true;
 }
 

src/api/processor/points_processor.cc

@@ -45,7 +45,42 @@ bool PointsProcessor::OnProcess(
   UNUSED(parent)
   const ObjMat *input = Object::Cast<ObjMat>(in);
   ObjMat *output = Object::Cast<ObjMat>(out);
-  cv::reprojectImageTo3D(input->value, output->value, Q_, true);
+  // cv::reprojectImageTo3D(input->value, output->value, Q_, true, -1);
+	bool handleMissingValues = true;
+	cv::Mat disparity = input->value;
+	output->value.create(disparity.size(), CV_MAKETYPE(CV_32FC3, 3));
+	cv::Mat _3dImage = output->value;
+
+	const float bigZ = 10000.f;
+	cv::Matx44d Q;
+	Q_.convertTo(Q, CV_64F);
+
+	int x, cols = disparity.cols;
+	CV_Assert(cols >= 0);
+
+	double minDisparity = FLT_MAX;
+
+	if (handleMissingValues) {
+		cv::minMaxIdx(disparity, &minDisparity, 0, 0, 0);
+	}
+
+	for (int y = 0; y <	disparity.rows; y++) {
+
+		float *sptr = disparity.ptr<float>(y);
+		cv::Vec3f *dptr = _3dImage.ptr<cv::Vec3f>(y);
+
+		for( x = 0; x < cols; x++)
+		{
+			double d = sptr[x];
+			cv::Vec4d homg_pt = Q*cv::Vec4d(x, y, d, 1.0);
+			dptr[x] = cv::Vec3d(homg_pt.val);
+			dptr[x] /= homg_pt[3];
+
+			if( fabs(d-minDisparity) <= FLT_EPSILON ) {
+				dptr[x][2] = bigZ;
+			}
+		}
+	}
   return true;
 }
 

src/api/processor/rectify_processor.cc

@@ -69,12 +69,16 @@ void RectifyProcessor::InitParams(
        in_right.cy, 0, 0, 1);
   cv::Mat D1(1, 5, CV_64F, in_left.coeffs);
   cv::Mat D2(1, 5, CV_64F, in_right.coeffs);
+	/*
   cv::Mat R =
       (cv::Mat_<double>(3, 3) << ex_right_to_left.rotation[0][0],
        ex_right_to_left.rotation[0][1], ex_right_to_left.rotation[0][2],
        ex_right_to_left.rotation[1][0], ex_right_to_left.rotation[1][1],
        ex_right_to_left.rotation[1][2], ex_right_to_left.rotation[2][0],
        ex_right_to_left.rotation[2][1], ex_right_to_left.rotation[2][2]);
+			 */
+	cv::Mat R =
+      (cv::Mat_<double>(3, 3) << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0);
   cv::Mat T(3, 1, CV_64F, ex_right_to_left.translation);
 
   VLOG(2) << "InitParams size: " << size;