VINS reprojection error calculation initialization ceres

Detailed analysis VINS initialized when the calculated camera pose and the feature point coordinates of the 3D reprojection error minimization operations with ceres.

References:

【1】https://www.jianshu.com/p/e5b03cf22c80

【2】http://www.ceres-solver.org/index.html

【3】 http://www.cnblogs.com/decade-dnbc66/p/5347088.html

Recommend a look at a small example [1] in the beginning.

In this part of the code VINS "initial_sfm.cpp" "initial_sfm.h" in.

(1) Construction of a cost function. Of the order code to first analyze the most important parts: the definition of heavy costfunction projection errors. VINS defines a functor ReprojectionError3D named, i.e., in the structure of "()" is overloaded, i.e. ReprojectionError3D () can be used as a function to use, and can easily pass parameters , details refer to [3 ]. In vivo this structure,

        //"operator()" is ONE name
bool operator()(const T* const camera_R, const T* const camera_T, const T* point, T* residuals) const
	{
	T p[3];
        //Pc=Rw2c*Pw+Tw2c	
        ceres::QuaternionRotatePoint(camera_R, point, p); //Rotates a point pt by a quaternion q, pose is the result
	p[0] += camera_T[0]; p[1] += camera_T[1]; p[2] += camera_T[2]; //
	T xp = p[0] / p[2];
    	T yp = p[1] / p[2];
    	residuals[0] = xp - T(observed_u);
    	residuals[1] = yp - T(observed_v);
    	return true;
	}

Is defined reprojection errors, the parameters passed in front of the three variables are optimized, there is an initial parameter passed when AddResidualBlock. The last requirement is a residual in this place is the first characteristic point in the world coordinate system to the camera coordinate system conversion, and then projected onto the normalized plane, then the measured normalized plane Determining the coordinates of the error.

ReprojectionError3D(double observed_u, double observed_v)
		:observed_u(observed_u), observed_v(observed_v)
		{}

This is said above, to facilitate transmission parameters, the observation points in the normalized coordinates of a plane (undistorted) into a residual calculated. Taking into account the optimization of time, the observations are not optimized, so that observations should be as accurate as possible, so

Optical flow tracking
Distortion correction and the projection (the camera parameter)

The quality is very important.

(2) Construction of optimization problems. First defined type ceres :: CostFunction * cost function, and constructs using the above structure:

ceres::CostFunction* cost_function = ReprojectionError3D::Create(sfm_f[i].observation[j].second.x(),
		sfm_f[i].observation[j].second.y());
problem.AddResidualBlock(cost_function, NULL, c_rotation[l], c_translation[l], sfm_f[i].position);

In problem.AddResidualBlock (), a subsequent optimization parameters are variables, passed in front of cost_function. This is an automatic derivation of the cost function, a fixed configuration method. Reference [1] can be.

In the new ceres :: AutoDiffCostFunction <> specified source (structure) of the cost function, the residual dimension, and the dimension of each input optimization variables.

(3) Configuration solving

This part is relatively simple and not difficult, reference [1] can then be assigned to the variables in the final will be optimized.

(4) Further, in the VINS:

problem.AddParameterBlock(c_rotation[i], 4, local_parameterization);
problem.AddParameterBlock(c_translation[i], 3);

【2】中有说明：The user has the option of explicitly adding the parameter blocks using AddParameterBlock. This causes additional correctness checking; however, AddResidualBlock implicitly adds the parameter blocks if they are not present, so calling AddParameterBlock explicitly is not required.

That is no special needs, then, this part is not necessary, in AddResidualBlock time will automatically be added as a variable parameter optimization block, but for VINS, for the operation of the quaternion (plus, Jacobian) needs four yuan operands method, and the authors of the first fixed frame and the last frame of the camera posture unchanged, alone add a parameter block, and the parameter block are ignored repeated added, so add in advance does not affect the back AddResidualBlock.

VINS reprojection error calculation initialization ceres

Guess you like