Update astra_driver.cpp

zch-soft · Feb 12, 2020 · bae2caa · bae2caa
1 parent 4a9ae0b
commit bae2caa
Showing 1 changed file with 12 additions and 12 deletions.
diff --git a/src/astra_driver.cpp b/src/astra_driver.cpp
@@ -823,16 +823,12 @@ sensor_msgs::CameraInfoPtr AstraDriver::getColorCameraInfo(int width, int height
   double scaling = (double)width / 640;
   info->K[0] *= scaling; // fx
   info->K[2] *= scaling; // cx
- // info->K[2] -= depth_ir_offset_x_ * scaling;
   info->K[4] *= scaling; // fy
   info->K[5] *= scaling; // cy
- // info->K[5] -= depth_ir_offset_y_ * scaling;
   info->P[0] *= scaling; // fx
   info->P[2] *= scaling; // cx
- // info->P[2] -= depth_ir_offset_x_ * scaling;
   info->P[5] *= scaling; // fy
   info->P[6] *= scaling; // cy
-//  info->P[6] -= depth_ir_offset_y_ * scaling;
 /* 02112020 end*/
 
     }
@@ -897,6 +893,18 @@ sensor_msgs::CameraInfoPtr AstraDriver::getIRCameraInfo(int width, int height, r
       info->P[5] = info->K[4];
       info->P[6] = info->K[5];
       info->P[10] = 1.0;
+/* 02122020 Scale IR Params */
+  double scaling = (double)width / 640;
+  info->K[0] *= scaling; // fx
+  info->K[2] *= scaling; // cx
+  info->K[4] *= scaling; // fy
+  info->K[5] *= scaling; // cy
+  info->P[0] *= scaling; // fx
+  info->P[2] *= scaling; // cx
+  info->P[5] *= scaling; // fy
+  info->P[6] *= scaling; // cy
+/* 02122020 end */	    
+
     }
   }
 
@@ -914,17 +922,9 @@ sensor_msgs::CameraInfoPtr AstraDriver::getDepthCameraInfo(int width, int height
   // (probably 9x9 or 9x7 in 640x480 mode). See http://www.ros.org/wiki/kinect_calibration/technical
   double scaling = (double)width / 640;
   sensor_msgs::CameraInfoPtr info = getIRCameraInfo(width, height, time);
-  info->K[0] *= scaling; // fx
-  info->K[2] *= scaling; // cx
   info->K[2] -= depth_ir_offset_x_ * scaling;
-  info->K[4] *= scaling; // fy
-  info->K[5] *= scaling; // cy
   info->K[5] -= depth_ir_offset_y_ * scaling;
-  info->P[0] *= scaling; // fx
-  info->P[2] *= scaling; // cx
   info->P[2] -= depth_ir_offset_x_ * scaling;
-  info->P[5] *= scaling; // fy
-  info->P[6] *= scaling; // cy
   info->P[6] -= depth_ir_offset_y_ * scaling;
 
   /// @todo Could put this in projector frame so as to encode the baseline in P[3]