Changeset 68 in pacpussensors for trunk/StereoVisionDisparity

Timestamp:

10/08/14 15:02:31 (10 years ago)

Author:

phudelai

Message:

New function

Location:

trunk/StereoVisionDisparity

Files:

• ObstacleDetectionComponent.cpp (modified) (2 diffs)
• ObstacleDetectionComponent.h (modified) (2 diffs)

trunk/StereoVisionDisparity/ObstacleDetectionComponent.cpp

@@ -273 +273 @@
         if (config.getProperty("cam_channels") != QString::null)
                 this->cam_channels = config.getProperty("cam_channels").toInt();
+
+        if (config.getProperty("cam_ref_cx") != QString::null)
+                this->cam_ref_cx = config.getProperty("cam_ref_cx").toDouble(); // cx camera divide by x image size
+
+        if (config.getProperty("cam_ref_cy") != QString::null)
+                this->cam_ref_cy = config.getProperty("cam_ref_cy").toDouble(); // cy camera divide by y image size
+
+        if (config.getProperty("cam_ref_fx") != QString::null)
+                this->cam_ref_fx = config.getProperty("cam_ref_fx").toDouble(); // fx camera divide by x image size
+
+        if (config.getProperty("cam_ref_fy") != QString::null)
+                this->cam_ref_fy = config.getProperty("cam_ref_fy").toDouble(); // fy camera divide by y image size
+
+        if (config.getProperty("cam_baseline") != QString::null)
+                this->cam_baseline = config.getProperty("cam_baseline").toDouble(); // distance between two cameras in meter
 
         if (config.getProperty("min_disp") != QString::null)
@@ -631 +646 @@
 x,y,z = world coordinates
 disparity = disparity value
-
+*/
 bool ObstacleDetectionComponent::PointTriangulate(int row, int col, double &x, double &y, double &z, double disparity)
 {
-                bool valid_point = false;
-
         if(disparity > 0.0 && disparity < 255.0)
         {
-                z = this->cam_width * this->cam_fx * this->cam_baseline / disparity;
-                double u = col / (this->cam_width - 1.0) - this->cam_cx;
-                double v = row / (this->cam_height - 1.0) - this->cam_cy;
-
-                x = u * z / this->cam_fx;
-
-                y = v * z / this->cam_fy;
-
-                valid_point = true;
-        }
-
-        return valid_point;
-}*/
+                z = this->cam_width * this->cam_ref_fx * this->cam_baseline / disparity;
+                double u = col / (this->cam_width - 1) - this->cam_ref_cx;
+                double v = row / (this->cam_height - 1) - this->cam_ref_cy;
+
+                x = u * z / this->cam_ref_fx;
+                y = v * z / this->cam_ref_fy;
+
+                return true;
+        }
+
+        return false;
+}//*/
 
 

trunk/StereoVisionDisparity/ObstacleDetectionComponent.h

@@ -74 +74 @@
         double cam_ref_fx;              // fx camera divide by x image size
         double cam_ref_fy;              // fy camera divide by y image size
+        double cam_baseline;    // distance between two cameras in meter
         bool showdebug;                 // Show frame acquired
         int min_disp;                   // Minimum disparity value to detect obstacles in the u/v-disp map
@@ -194 +195 @@
         A CalcMedian(std::vector<A> vetor) const;
 
-        //bool PointTriangulate(int row, int col, double &x, double &y, double &z, double disparity);
+        bool PointTriangulate(int row, int col, double &x, double &y, double &z, double disparity);
 };