cal_eval.cpp

有关摄像头定标的C++代码

#include "stdafx.h"
#include <stdio.h>
#include <math.h>
#include "cal_main.h"
#include "cal_eval.h"
#include "cal_tran.h"

extern struct camera_parameters cp;
extern struct calibration_constants cc;


/*******************************************************************************\
* This routine calculates the mean, standard deviation, max, and sum-of-squared *
* error of the magnitude of the error, in distorted image coordinates, between  *
* the measured location of a feature point in the image plane and the image of  *
* the 3D feature point as projected through the calibrated model.               *
* The calculation is for all of the points in the calibration data set.         *
\*******************************************************************************/
//计算存在畸变时的偏差；
void      distorted_image_plane_error_stats (double *mean, double *stddev, double *max, double *sse)
{
    int       i;

    double    Xf,
              Yf,//像素理想坐标；
              error,//误差；
              squared_error,//平方误差
              max_error = 0,//最大误差；
              sum_error = 0,//误差和；
              sum_squared_error = 0;//平方误差和；

    if (cd.point_count < 1) {
	*mean = *stddev = *max = *sse = 0;
	return;
    }

    for (i = 0; i < cd.point_count; i++) {
	/* calculate the ideal location of the image of the data point */
	world_coord_to_image_coord (cd.xw[i], cd.yw[i], cd.zw[i], &Xf, &Yf);

	/* determine the error between the ideal and actual location of the data point	 */
	/* (in distorted image coordinates)						 */
	squared_error = SQR (Xf - cd.Xf[i]) + SQR (Yf - cd.Yf[i]);
	error = sqrt (squared_error);
	sum_error += error;
	sum_squared_error += squared_error;
	max_error = MAX (max_error, error);
    }

    *mean = sum_error / cd.point_count;//定义平均误差；
    *max = max_error;//定义最大误差；
    *sse = sum_squared_error;//定义平方误差的和；

    if (cd.point_count == 1)
	*stddev = 0;//标准偏差；
    else
	*stddev = sqrt((sum_squared_error - SQR (sum_error) / cd.point_count) / (cd.point_count - 1));
}


/*******************************************************************************\
* This routine calculates the mean, standard deviation, max, and sum-of-squared *
* error of the magnitude of the error, in undistorted image coordinates, between*
* the measured location of a feature point in the image plane and the image of  *
* the 3D feature point as projected through the calibrated model.               *
* The calculation is for all of the points in the calibration data set.         *
\*******************************************************************************/
//无畸变时的偏差
void      undistorted_image_plane_error_stats (double *mean, double *stddev, double *max, double *sse)
{
    int       i;

    double    xc,
              yc,
              zc,
              Xu_1,
              Yu_1,
              Xu_2,
              Yu_2,
              Xd,
              Yd,
              distortion_factor,
              x_pixel_error,
              y_pixel_error,
              error,
              squared_error,
              max_error = 0,
              sum_error = 0,
              sum_squared_error = 0;

    if (cd.point_count < 1) {
	*mean = *stddev = *max = *sse = 0;
	return;
    }

    for (i = 0; i < cd.point_count; i++) {
	/* calculate the ideal location of the image of the data point */
	world_coord_to_camera_coord (cd.xw[i], cd.yw[i], cd.zw[i], &xc, &yc, &zc);//世界坐标系向图象坐标系的转换；

	/* convert from camera coordinates to undistorted sensor plane coordinates */
	Xu_1 = cc.f * xc / zc;//图象坐标系和点的理想图象的转化；cc.f=焦距，xu-1理想图象坐标（推导式），xc为摄象机坐标系；
	Yu_1 = cc.f * yc / zc;//同上；

	/* convert from 2D image coordinates to distorted sensor coordinates */
	Xd = cp.dpx * (cd.Xf[i] - cp.Cx) / cp.sx;//图象理想坐标和实际坐标，即畸变的表示；
	Yd = cp.dpy * (cd.Yf[i] - cp.Cy);//s=变形系数，cx=图象中心像素坐标，dpx像素代表的物理尺寸；

	/* convert from distorted sensor coordinates to undistorted sensor plane coordinates */
	distortion_factor = 1 + cc.kappa1 * (SQR (Xd) + SQR (Yd));//畸变系数的计算；
	Xu_2 = Xd * distortion_factor;//理想坐标的计算（实际计算式）；
	Yu_2 = Yd * distortion_factor;//xu2=?xu1??

	/* determine the error between the ideal and actual location of the data point	 */
	/* (in undistorted image coordinates)						 */
	x_pixel_error = cp.sx * (Xu_1 - Xu_2) / cp.dpx;
	y_pixel_error = (Yu_1 - Yu_2) / cp.dpy;
	squared_error = SQR (x_pixel_error) + SQR (y_pixel_error);
	error = sqrt (squared_error);//定义计算像素的各种误差；
	sum_error += error;
	sum_squared_error += squared_error;
	max_error = MAX (max_error, error);
    }

    *mean = sum_error / cd.point_count;
    *max = max_error;
    *sse = sum_squared_error;

    if (cd.point_count == 1)
	*stddev = 0;
    else
	*stddev = sqrt ((sum_squared_error - SQR (sum_error) / cd.point_count) / (cd.point_count - 1));
}


/*******************************************************************************\
* This routine calculates the mean, standard deviation, max, and sum-of-squared *
* error of the distance of closest approach (i.e. 3D error) between the point   *
* in object space and the line of sight formed by back projecting the measured  *
* 2D coordinates out through the camera model.                                  *
* The calculation is for all of the points in the calibration data set.         *
\*******************************************************************************/
void      object_space_error_stats (double *mean, double *stddev, double *max, double *sse)
{
    int       i;

    double    xc,
              yc,
              zc,
              Xu,
              Yu,
              Xd,
              Yd,
              t,
              distortion_factor,
              error,
              squared_error,
              max_error = 0,
              sum_error = 0,
              sum_squared_error = 0;

    if (cd.point_count < 1) {
	*mean = *stddev = *max = *sse = 0;
	return;
    }

    for (i = 0; i < cd.point_count; i++) {
	/* determine the position of the 3D object space point in camera coordinates */
	world_coord_to_camera_coord (cd.xw[i], cd.yw[i], cd.zw[i], &xc, &yc, &zc);

	/* convert the measured 2D image coordinates into distorted sensor coordinates */
	Xd = cp.dpx * (cd.Xf[i] - cp.Cx) / cp.sx;
	Yd = cp.dpy * (cd.Yf[i] - cp.Cy);

	/* convert from distorted sensor coordinates into undistorted sensor plane coordinates */
	distortion_factor = 1 + cc.kappa1 * (SQR (Xd) + SQR (Yd));
	Xu = Xd * distortion_factor;
	Yu = Yd * distortion_factor;

	/* find the magnitude of the distance (error) of closest approach */
	/* between the undistorted line of sight and the point in 3 space */
	//寻找从三维空间点到二维像点的线形误差，
	t = (xc * Xu + yc * Yu + zc * cc.f) / (SQR (Xu) + SQR (Yu) + SQR (cc.f));
	squared_error = SQR (xc - Xu * t) + SQR (yc - Yu * t) + SQR (zc - cc.f * t);
	error = sqrt (squared_error);
	sum_error += error;
	sum_squared_error += squared_error;
	max_error = MAX (max_error, error);
    }

    *mean = sum_error / cd.point_count;
    *max = max_error;
    *sse = sum_squared_error;

    if (cd.point_count == 1)
	*stddev = 0;
    else
	*stddev = sqrt ((sum_squared_error - SQR (sum_error) / cd.point_count) / (cd.point_count - 1));
}


/*******************************************************************************\
* This routine performs an error measure proposed by Weng in IEEE PAMI,         *
* October 1992.     实现规范化标定误差测量                                                            *
\*******************************************************************************/
void      normalized_calibration_error (double *mean, double *stddev)
{
    int       i;

    double    xc,
              yc,
              zc,
              xc_est,
              yc_est,
              zc_est,
              fu,
              fv,
              Xd,
              Yd,
              Xu,
              Yu,
              distortion_factor,
              squared_error,
              sum_error = 0,
              sum_squared_error = 0;

    if (cd.point_count < 1) {
	*mean = *stddev = 0;
	return;
    }

    /* This error metric is taken from						 */
    /* "Camera Calibration with Distortion Models and Accuracy Evaluation"	 */
    /* J. Weng, P. Cohen, and M. Herniou					 */
    /* IEEE Transactions on PAMI, Vol. 14, No. 10, October 1992, pp965-980	 */

    for (i = 0; i < cd.point_count; i++) {
	/* estimate the 3D coordinates of the calibration data point by back 	 */
	/* projecting its measured image location through the model to the	 */
	/* plane formed by the original z world component.			 */

	/* calculate the location of the data point in camera coordinates */
	world_coord_to_camera_coord (cd.xw[i], cd.yw[i], cd.zw[i], &xc, &yc, &zc);

	/* convert from 2D image coordinates to distorted sensor coordinates */
	Xd = cp.dpx * (cd.Xf[i] - cp.Cx) / cp.sx;
	Yd = cp.dpy * (cd.Yf[i] - cp.Cy);

	/* convert from distorted sensor coordinates to undistorted sensor plane coordinates */
	distortion_factor = 1 + cc.kappa1 * (SQR (Xd) + SQR (Yd));
	Xu = Xd * distortion_factor;
	Yu = Yd * distortion_factor;

	/* estimate the location of the data point by back projecting the image position */
	zc_est = zc;//估算物点在摄象机坐标下的坐标值；并计算坐标点误差；
	xc_est = zc_est * Xu / cc.f;
	yc_est = zc_est * Yu / cc.f;

	fu = cp.sx * cc.f / cp.dpx;
	fv = cc.f / cp.dpy;

	squared_error = (SQR (xc_est - xc) + SQR (yc_est - yc)) /
	 (SQR (zc_est) * (1 / SQR (fu) + 1 / SQR (fv)) / 12);
	sum_error += sqrt (squared_error);
	sum_squared_error += squared_error;
    }

    *mean = sum_error / cd.point_count;

    if (cd.point_count == 1)
	*stddev = 0;
    else
	*stddev = sqrt ((sum_squared_error - SQR (sum_error) / cd.point_count) / (cd.point_count - 1));
}