Merge pull request #1091 from k-okada/add_camera_info

add VisionSensorPortHandler intrinsic paramter to TimedCamera
fkanehiro · Jan 24, 2017 · c64be63 · c64be63
2 parents a38cec0 + b7c751c
commit c64be63
Showing 1 changed file with 20 additions and 0 deletions.
diff --git a/lib/util/PortHandler.cpp b/lib/util/PortHandler.cpp
@@ -272,13 +272,33 @@ VisionSensorPortHandler::VisionSensorPortHandler(
  m_data.data.image.format = Img::CF_RGB;
  int len = m_sensor->width*m_sensor->height*3;
  m_data.data.image.raw_data.length(len);
+ m_data.data.intrinsic.distortion_coefficient.length(5);
+ for(int i = 0; i < 5; i++){
+ m_data.data.intrinsic.distortion_coefficient[i] = 0;
+ }
+ double fovx = m_sensor->width/m_sensor->height*m_sensor->fovy;
+ m_data.data.intrinsic.matrix_element[0]=0.5 * m_sensor->width/tan(fovx/2.0);
+ m_data.data.intrinsic.matrix_element[1]=0.0;
+ m_data.data.intrinsic.matrix_element[2]=m_sensor->width/2;
+ m_data.data.intrinsic.matrix_element[3]=0.5 * m_sensor->height/tan(m_sensor->fovy/2.0);
+ m_data.data.intrinsic.matrix_element[4]=m_sensor->height/2;
  }else if(m_sensor->imageType == VisionSensor::MONO
  || m_sensor->imageType == VisionSensor::MONO_DEPTH){
  m_data.data.image.width = m_sensor->width;
  m_data.data.image.height = m_sensor->height;
  m_data.data.image.format = Img::CF_GRAY;
  int len = m_sensor->width*m_sensor->height;
  m_data.data.image.raw_data.length(len);
+ m_data.data.intrinsic.distortion_coefficient.length(5);
+ for(int i = 0; i < 5; i++){
+ m_data.data.intrinsic.distortion_coefficient[i] = 0;
+ }
+ double fovx = m_sensor->width/m_sensor->height*m_sensor->fovy;
+ m_data.data.intrinsic.matrix_element[0]=0.5 * m_sensor->width/tan(fovx/2.0);
+ m_data.data.intrinsic.matrix_element[1]=0.0;
+ m_data.data.intrinsic.matrix_element[2]=m_sensor->width/2;
+ m_data.data.intrinsic.matrix_element[3]=0.5 * m_sensor->height/tan(m_sensor->fovy/2.0);
+ m_data.data.intrinsic.matrix_element[4]=m_sensor->height/2;
  }
 }