In this paper, starting from ceres :: QuaternionParameterization, creating awareness of ceres :: LocalParameterization in understanding ceres :: QuaternionParameterization basis.
ceres-solver in the following explanation of LocalParameterization.
Sometimes the parameters \(\mathbf{x}\) can overparameterize a problem. In that case it is desirable to choose a parameterization to remove the null directions of the cost. More generally, if \(\mathbf{x}\) lies on a manifold of a smaller dimension than the ambient space that it is embedded in, then it is numerically and computationally more effective to optimize it using a parameterization that lives in the tangent space of that manifold at each point.
general speaking. LocalParameterization is optimized when the Manifold variables need to be considered, the Manifold variable is over parameterized, i.e. Manifold variable dimension larger than the degree of freedom. This can lead to constraints exist between the various variable amount Manifold, if the direct derivation of these quantities, optimization, then this is a constrained optimization, difficult to achieve. To solve this problem, in mathematics it is a tangent space (Tangent Space) Manifold formed in the current value of the variable derivation, optimization in the tangent space, and finally back projection Manifold.
For SLAM problem, Manifold widely encountered, the rotation only use 3 amount (so (3)) can be expressed, but because it involves the practical application of universal lock problems, express rotation in higher dimensions. Expression is the quaternion rotation in three-dimensional space in three degrees of freedom of the 4 dimensions.
QuaternionParameterization method int GlobalSize () Returns 4 shows Manifold space (quaternion) is four-dimensional, the method int LocalSize () Returns 3, represents the tangent space is three-dimensional.
The method of QuaternionParameterization bool ComputeJacobian (const double * x, double * jacobian) calculated to give a 4x3 matrix. These calculations are obtained by the matrix is referred ComputeJacobian "global_to_local" in ceres code, the meaning is the derivative of the Tangent Space Manifold variable on the variables. In ceres :: CostFunction derivative residuals provided at the upper Manifold variable ( \ ({\ partial \ mathbf {E} \ over \ partial \} ^ {X-mathbf {(G)}} \) ), multiplied by the matrix ( \ ({\ partial \ mathbf { X} ^ {(G)} \ over \ partial \ mathbf {X} ^ {(L)}} \) becomes later) derivative of the Tangent Space variable ( \ ({\ partial \ mathbf {E} \ over \ partial \} ^ {X-mathbf {(L)}} \) ).
QuaternionParameterization method bool Plus (const double * x, const double * delta, double * x_plus_delta) optimized incremental obtained ( deltain Tangent Space) was added to the variable to be optimized ( xin Manifold) formed optimization results ( x_plus_deltain Manifold ), completed an optimization iteration.
note. 1. ceres source code does not explicitly place thought matrix raw memory storage is Row Major, which is the default Col Major and Eigen is the opposite. 2. ceres default Quaternion raw memory storage is w, x, y, z, and the storage Eigen Quaternion is x, y, z, w, which leads in addition also in ceres ceres :: QuaternionParameterization code :: EigenQuaternionParameterization Ceres . 3. ceres in Quaternion is Hamilton Quaternion, follow Hamilton multiplication rule.
1. ceres :: QuaternionParameterization fit with AutoDiff
First, the method of QuaternionParameterization::ComputeJacobiansource code excerpt follows.
bool QuaternionParameterization::ComputeJacobian(const double* x,
double* jacobian) const {
jacobian[0] = -x[1]; jacobian[1] = -x[2]; jacobian[2] = -x[3]; // NOLINT
jacobian[3] = x[0]; jacobian[4] = x[3]; jacobian[5] = -x[2]; // NOLINT
jacobian[6] = -x[3]; jacobian[7] = x[0]; jacobian[8] = x[1]; // NOLINT
jacobian[9] = x[2]; jacobian[10] = -x[1]; jacobian[11] = x[0]; // NOLINT
return true;
}Row Major according storage, in accordance with the variable Manifold ( x) storage of w, x, y, z, the above code can be converted to a mathematical formula.
\[\begin{align} {\partial \mathbf{X}^{(G)} \over \partial \mathbf{X}^{(L)}} = \begin{bmatrix} -x & -y & -z \\ w & z & -y \\ -z & w & x \\ y & -x & w \end{bmatrix} \end{align}\]
We need to understand the origin of this matrix. Equation (18) Reference [1], more matrix corresponding to \ ([\ mathbf {q} ] _ R \) to the right of three. If the upper Manifold variable \ (\ mathbf {X} ^ {(G)} \) Writing \ (\ mathbf {Q} \) , the Tangent Space to the variable write \ (\ mathbf {\ Theta} \) , then the above matrix \ ({\ partial \ mathbf { X} ^ {(G)} \ over \ partial \ mathbf {X} ^ {(L)}} = {\ partial \ mathbf {\ theta} \ otimes \ mathbf {q} \ over \ partial \ mathbf {\ Theta}} \) . This is a form of disturbance left, inspection methods QuaternionParameterization :: Plus , source code excerpt below.
bool QuaternionParameterization::Plus(const double* x,
const double* delta,
double* x_plus_delta) const {
const double norm_delta =
sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]);
if (norm_delta > 0.0) {
const double sin_delta_by_delta = (sin(norm_delta) / norm_delta);
double q_delta[4];
q_delta[0] = cos(norm_delta);
q_delta[1] = sin_delta_by_delta * delta[0];
q_delta[2] = sin_delta_by_delta * delta[1];
q_delta[3] = sin_delta_by_delta * delta[2];
QuaternionProduct(q_delta, x, x_plus_delta);
} else {
for (int i = 0; i < 4; ++i) {
x_plus_delta[i] = x[i];
}
}
return true;
}It can be seen QuaternionProduct(q_delta, x, x_plus_delta);, optimize gain results q_deltais left by the original variables xto obtain a new variable x_plus_delta, which is QuaternionParameterization::ComputeJacobianthe left-Jacques disturbances corresponding ratio.
Experiment, the QuaternionParameterization::ComputeJacobianJacobian is calculated to Reference [1] Equation (18 is) \ ([\ mathbf {Q}] _ L \) right three, the QuaternionParameterization::Plusincrement is added to the process variable to the right by the original, after that is changing QuaternionProduct(x, q_delta, x_plus_delta);. This disturbance will then form the right and AutoDiff QuaternionParameterization binding to solve practical problems, the results obtained can be seen within the error allowable range of the disturbance and the left consistent results.
in conclusion. QuaternionParameterization the Plus and ComputeJacobian jointly decided to use the left and right disturbances or perturbations in the form.
When get more than the benefits of QuaternionParameterization it is not in use AutoDiff or NumericDiff, user code provided Jacobi, can provide guidance. According to existing data, usually on so (3) derivative, it is difficult to find the derivative of the quaternion. If ceres :: QuaternionParameterization, ceres desirable to provide a user code derivative of the quaternion, which provide users with much trouble derivative.
If the user-provided a QuaternionParameterization, gain control of the parameterization. The user may provide the derivative of so (3) in CostFunction, define QuaternionParameterization::ComputeJacobianreturn matrix \ (\ begin {bmatrix} \ mathbf {I} _ {3 \ times3} \\ (\ mathbf {0} _ {3 \ times1 }) ^ T \ bmatrix End {} \) . Matrix \ (\ begin {bmatrix} \ mathbf {I} _ {3 \ times3} \\ (\ mathbf {0} _ {3 \ times1}) ^ T \ end {bmatrix} \) returns the user \ (N (e) \ times4 \) matrix taken as \ (N (e) \ times3 \) matrix. Note, \ (N (E) \) represents the number of error residuals. And QuaternionParameterization::Plusincremental addition of the right and left should CostFunction so (3) consistent perturbations derivative. In this process, the user is returned to the CostFunction so (3) the derivative should be the last one to remain 0, the first three columns of padding values. (Note that ceres::QuaternionParameterization::Plusthe operation angle of the axle, instead of axis angle, if directly applied, will be calculated so that a difference in coefficient Jacobian 2.)
Of course, operating above this text description can be achieved is a prerequisite, "all users of derivatives Manifold variables after entering ceres will go through a number of Tangent Space variable guide LocalParameterization :: ComputeJacobian convert, and then used elsewhere." . Within the current range of ceres, the above assumptions are not broken. .
Now that the use of the calculated error AutoDiff derivative of the quaternion multiplying the ceres::QuaternionParameterization::ComputeJacobianreturned result is the error matrix to the left so (3) the derivative of the disturbance.
Quaternion in error calculation is the most extensive use of a three-dimensional point coordinate transformation from one coordinate system to another coordinate system, such as converting the coordinates in the world coordinate system to the camera coordinate system coordinates, \ (^ {} { C} \ mathbf {X-} = \ mathbf {R & lt} \ {{} ^ C \ mathbf {Q} _W \} {} ^ W is \ mathbf {X-} + {} ^ C \ mathbf {T} _W \) . According to the chain rule, only proof \ ({\ partial {} ^ {C} \ mathbf {X} \ over \ partial {} ^ C \ mathbf {q} _W} {\ partial \ mathbf {\ theta} \ otimes {} ^ C \ mathbf {Q} _W \ over \ partial \ mathbf {\ Theta}} = {\ partial {} ^ {C} \ mathbf {X-} \ over \ partial \ mathbf {\ Theta}} \) .
References [1] Formula (115), to give \ ({C} ^ {} \ X-mathbf {} \) .
\[\begin{align} {}^{C}\mathbf{X} &= \begin{bmatrix} [1-2(q_y^2+q_z^2)]X + 2(q_xq_y-q_wq_z)Y + 2(q_xq_z+q_wq_y)Z + t_x \\ 2(q_xq_y+q_wq_z)X + [1-2(q_x^2+q_z^2)]Y + 2(q_yq_z-q_wq_x)Z + t_y \\ 2(q_xq_z-q_wq_y)X + 2(q_yq_z+q_wq_x)Y + [1-2(q_x^2+q_y^2)]Z + t_z \end{bmatrix} \\ {\partial {}^{C}\mathbf{X} \over \partial {}^C\mathbf{q}_W} &= \begin{bmatrix} {\partial {}^{C}X \over \partial q_w} & {\partial {}^{C}X \over \partial q_x} & {\partial {}^{C}X \over \partial q_y} & {\partial {}^{C}X \over \partial q_z} \\ {\partial {}^{C}Y \over \partial q_w} & {\partial {}^{C}Y \over \partial q_x} & {\partial {}^{C}Y \over \partial q_y} & {\partial {}^{C}Y \over \partial q_z} \\ {\partial {}^{C}Z \over \partial q_w} & {\partial {}^{C}Z \over \partial q_x} & {\partial {}^{C}Z \over \partial q_y} & {\partial {}^{C}Z \over \partial q_z} \end{bmatrix} \notag \\ &= 2 \begin{bmatrix} q_yZ-q_zY & q_yY+q_zZ & q_wZ+q_xY-2q_yX & -q_wY+q_xZ-2q_zX \\ -q_xZ+q_zX & -q_wZ-2q_xY+q_yX & q_xX+q_zZ & q_wX+q_yZ-2q_zY \\ q_xY-q_yX & q_wY-2q_xZ+q_zX & -q_wX-2q_yZ+q_zY & q_xX+q_yY \end{bmatrix} \\ {\partial {}^{C}\mathbf{X} \over \partial {}^C\mathbf{q}_W}{\partial \mathbf{\theta} \otimes {}^C\mathbf{q}_W \over \partial \mathbf{\theta}} &= 2\begin{bmatrix} q_yZ-q_zY & q_yY+q_zZ & q_wZ+q_xY-2q_yX & -q_wY+q_xZ-2q_zX \\ -q_xZ+q_zX & -q_wZ-2q_xY+q_yX & q_xX+q_zZ & q_wX+q_yZ-2q_zY \\ q_xY-q_yX & q_wY-2q_xZ+q_zX & -q_wX-2q_yZ+q_zY & q_xX+q_yY \end{bmatrix} \notag \\ &\phantom{=} {1 \over 2}\begin{bmatrix} -q_x & -q_y & -q_z \\ q_w & q_z & -q_y \\ -q_z & q_w & q_x \\ q_y & -q_x & q_w \end{bmatrix} \notag \\ &= \begin{bmatrix} 0 & 2(-q_wq_y+q_xq_z)+2(q_wq_x+q_yq_z)Y+[1-2(q_x^2+q_y^2)]Z & -2(q_wq_z+q_xq_y)X+[1-2(q_w^2+q_y^2)]Y+2(q_wq_x-q_yq_z)Z \\ 2(q_wq_y-q_xq_z)-2(q_wq_x+q_yq_z)Y-[1-2(q_x^2+q_y^2)]Z & 0 & [1-2(q_y^2+q_z^2)]X+2(-q_wq_z+q_xq_y)Y+2(q_wq_y+q_xq_z)Z \\ -2(q_wq_z+q_xq_y)X-[1-2(q_w^2+q_y^2)]Y+2(-q_wq_x+q_yq_z)Z & -[1-2(q_y^2+q_z^2)]X+2(q_wq_z-q_xq_y)Y-2(q_wq_y+q_xq_z)Z & 0 \end{bmatrix} \end{align}\]
Reference "Visual SLAM fourteen stresses" 4.3.5, left to know so (3) is a derivative of perturbation \ (- (C ^ {} \ X-mathbf {}) ^ {\ Wedge} \) , which results in the above derivation the result is that substantially corresponds to the difference between a displacement \ (C ^ {} \ mathbf {T} _W \) . This difference can be explained using se (3) of the disturbance model is \ (\ Delta \ mathbf {R } (\ mathbf {R} \ mathbf {p} + \ mathbf {t}) + \ Delta \ mathbf {t} \ ) , the number of so (3) contained in the obtained guide displacement. The disturbance model in the above formula is \ ((\ of Delta \ mathbf {R & lt}) \ mathbf {R & lt} \ mathbf {P} + \ mathbf {T} + \ of Delta \ mathbf {T} \) , should apply "visual SLAM fourteen stresses "4.3.4, a disturbance so on (3). Summary, if it is (3) to do a whole se Parameterization (refer to Zheng sail blog "a using the On-Manifold Optimization Demo Ceres Solver" ), then you need to use "visual SLAM fourteen stresses" derivative model 4.3.5, if just QuaternionParameterization, you only need to use "visual SLAM fourteen stresses" derivative model 4.3.4. In determining Parameterization of derivative model, attention should also CostFunction the residual is calculated.
2. ceres :: Other uses of LocalParameterization
:: QuaternionParameterization above has the two most important functions ceres :: QuaternionParameterization :: ComputeJacobian with ceres :: QuaternionParameterization :: Plus for ceres explained. ceres :: QuaternionParameterization inherited ceres :: LocalParameterization, in the case of understanding the operating principle ceres :: QuaternionParameterization may inherit realize their ceres :: LocalParameterization according to their needs.
A simple example is a very useful "fixed die length optimization." As previously explained ceres :: LocalParameterization optimization variables for Manifold, there is a correlation between the Manifold constraint variables, it can be seen there is a limit to optimize conditions. "Stented length optimization" is to treat the optimization variables die length restrictions, the most common "stented length optimization" is to optimize the direction of gravitational acceleration, the reference VINS [2] of the Algorithm 1optimization, the gravitational acceleration direction during initialization.
In the implementation process, can be calculated CostFunction the derivative of gravity three variables, determine the main direction in ceres :: LocalParameterization :: ComputeJacobian calculated herein the \ (\ mathbf {b} _1 , \ mathbf {b} _2 \) derivative of gravity will be projected onto the three-dimensional variables \ (\ mathbf {b} _1 , \ mathbf _2 \ {b}) in both directions. Optimization out \ (\ mathbf {b} _1 , \ mathbf {b} _2 \) increments in both directions, were combined in ceres :: LocalParameterization :: Plus gravity into the three-dimensional coordinates, and during the consolidation process, Note that normalization, to achieve a fixed die length. This approach, after my experiment was a success. VINS code does not look, glance where the function void RefineGravity (map <double, ImageFrame > & all_image_frame, Vector3d & g, VectorXd & x) does not use ceres :: LocalParameterization process.