sensor_msgs::CameraInfo toSensorMsgsCameraInfo(vpCameraParameters& cam_info, unsigned int cam_image_width, unsigned int cam_image_height ){
sensor_msgs::CameraInfo ret;
std::vector<double> D(5);
D[0]=cam_info.get_kdu();
D[1] = D[2] = D[3] = D[4] = 0.;
ret.D = D;
ret.P.assign(0.);
ret.K.assign(0.);
ret.R.assign(0.);
ret.R[0] = 1.;
ret.R[1 * 3 + 1] = 1.;
ret.R[2 * 3 + 2] = 1.;
ret.P[0 * 4 + 0] = cam_info.get_px();
ret.P[1 * 4 + 1] = cam_info.get_py();
ret.P[0 * 4 + 2] = cam_info.get_u0();
ret.P[1 * 4 + 2] = cam_info.get_v0();
ret.P[2 * 4 + 2] = 1;
ret.K[0 * 3 + 0] = cam_info.get_px();
ret.K[1 * 3 + 1] = cam_info.get_py();
ret.K[0 * 3 + 2] = cam_info.get_u0();
ret.K[1 * 3 + 2] = cam_info.get_v0();
ret.K[2 * 3 + 2] = 1;
ret.distortion_model = "plumb_bob";
ret.binning_x = 0;
ret.binning_y = 0;
ret.width = cam_image_width;
ret.height = cam_image_height;
return ret;
}
/*!
Compute and return the standard deviation expressed in pixel
for pose matrix and camera intrinsic parameters with pixel to meter model.
\param cMo_est : the matrix that defines the pose to be tested.
\param camera : camera intrinsic parameters to be tested.
\return the standard deviation by point of the error in pixel .
*/
double vpCalibration::computeStdDeviation_dist(const vpHomogeneousMatrix& cMo_est,
const vpCameraParameters& camera)
{
double residual_ = 0 ;
std::list<double>::const_iterator it_LoX = LoX.begin();
std::list<double>::const_iterator it_LoY = LoY.begin();
std::list<double>::const_iterator it_LoZ = LoZ.begin();
std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
double u0 = camera.get_u0() ;
double v0 = camera.get_v0() ;
double px = camera.get_px() ;
double py = camera.get_py() ;
double kud = camera.get_kud() ;
double kdu = camera.get_kdu() ;
double inv_px = 1/px;
double inv_py = 1/px;
vpImagePoint ip;
for (unsigned int i =0 ; i < npt ; i++)
{
double oX = *it_LoX;
double oY = *it_LoY;
double oZ = *it_LoZ;
double cX = oX*cMo_est[0][0]+oY*cMo_est[0][1]+oZ*cMo_est[0][2] + cMo_est[0][3];
double cY = oX*cMo_est[1][0]+oY*cMo_est[1][1]+oZ*cMo_est[1][2] + cMo_est[1][3];
double cZ = oX*cMo_est[2][0]+oY*cMo_est[2][1]+oZ*cMo_est[2][2] + cMo_est[2][3];
double x = cX/cZ ;
double y = cY/cZ ;
ip = *it_Lip;
double u = ip.get_u() ;
double v = ip.get_v() ;
double r2ud = 1+kud*(vpMath::sqr(x)+vpMath::sqr(y)) ;
double xp = u0 + x*px*r2ud;
double yp = v0 + y*py*r2ud;
residual_ += (vpMath::sqr(xp-u) + vpMath::sqr(yp-v)) ;
double r2du = (vpMath::sqr((u-u0)*inv_px)+vpMath::sqr((v-v0)*inv_py)) ;
xp = u0 + x*px - kdu*(u-u0)*r2du;
yp = v0 + y*py - kdu*(v-v0)*r2du;
residual_ += (vpMath::sqr(xp-u) + vpMath::sqr(yp-v)) ;
++it_LoX;
++it_LoY;
++it_LoZ;
++it_Lip;
}
residual_ /=2;
this->residual_dist = residual_;
return sqrt(residual_/npt) ;
}
