| 3421 | |
| 3422 | template <class Float> |
| 3423 | int SparseBundleCPU<Float>::SolveNormalEquationPCGX(float lambda) { |
| 3424 | //---------------------------------------------------------- |
| 3425 | //(Jt * J + lambda * diag(Jt * J)) X = Jt * e |
| 3426 | //------------------------------------------------------------- |
| 3427 | TimerBA timer(this, TIMER_CG_ITERATION); |
| 3428 | __recent_cg_status = ' '; |
| 3429 | |
| 3430 | // diagonal for jacobian preconditioning... |
| 3431 | int plen = GetParameterLength(); |
| 3432 | VectorF null; |
| 3433 | VectorF& VectorDP = __lm_use_diagonal_damp ? _cuVectorJJ : null; // diagonal |
| 3434 | ComputeBlockPC(lambda, __lm_use_diagonal_damp); |
| 3435 | |
| 3436 | //////////////////////////////////////////////// |
| 3437 | |
| 3438 | /////////////////////////////////////////////////////// |
| 3439 | // B = [BC 0 ; 0 BP] |
| 3440 | // m = [mc 0; 0 mp]; |
| 3441 | // A x= BC * x - JcT * Jp * mp * JpT * Jc * x |
| 3442 | // = JcT * Jc x + lambda * D * x + ........ |
| 3443 | //////////////////////////////////////////////////////////// |
| 3444 | |
| 3445 | VectorF r; |
| 3446 | r.set(_cuVectorRK.data(), 8 * _num_camera); |
| 3447 | VectorF p; |
| 3448 | p.set(_cuVectorPK.data(), 8 * _num_camera); |
| 3449 | VectorF z; |
| 3450 | z.set(_cuVectorZK.data(), 8 * _num_camera); |
| 3451 | VectorF x; |
| 3452 | x.set(_cuVectorXK.data(), 8 * _num_camera); |
| 3453 | VectorF d; |
| 3454 | d.set(VectorDP.data(), 8 * _num_camera); |
| 3455 | |
| 3456 | VectorF& u = _cuVectorRK; |
| 3457 | VectorF& v = _cuVectorPK; |
| 3458 | VectorF up; |
| 3459 | up.set(u.data() + 8 * _num_camera, 3 * _num_point); |
| 3460 | VectorF vp; |
| 3461 | vp.set(v.data() + 8 * _num_camera, 3 * _num_point); |
| 3462 | VectorF uc; |
| 3463 | uc.set(z.data(), 8 * _num_camera); |
| 3464 | |
| 3465 | VectorF& e = _cuVectorJX; |
| 3466 | VectorF& e2 = _cuImageProj; |
| 3467 | |
| 3468 | ApplyBlockPC(_cuVectorJtE, u, 2); |
| 3469 | ComputeJX(u, e, 2); |
| 3470 | ComputeJtE(e, uc, 1); |
| 3471 | ComputeSAXPY(Float(-1.0f), uc, _cuVectorJtE, r); // r |
| 3472 | ApplyBlockPC(r, p, 1); // z = p = M r |
| 3473 | |
| 3474 | float_t rtz0 = (float_t)ComputeVectorDot(r, p); // r(0)' * z(0) |
| 3475 | ComputeJX(p, e, 1); // Jc * x |
| 3476 | ComputeJtE(e, u, 2); // JpT * jc * x |
| 3477 | ApplyBlockPC(u, v, 2); |
| 3478 | float_t qtq0 = |
| 3479 | (float_t)ComputeVectorNorm(e, __num_cpu_thread[FUNC_VS]); // q(0)' * q(0) |
| 3480 | float_t pdp0 = (float_t)ComputeVectorNormW(p, d); // p(0)' * DDD * p(0) |
nothing calls this directly
no test coverage detected