| 1161 | } |
| 1162 | |
| 1163 | int SparseBundleCU::SolveNormalEquationPCGX(float lambda) { |
| 1164 | //---------------------------------------------------------- |
| 1165 | //(Jt * J + lambda * diag(Jt * J)) X = Jt * e |
| 1166 | //------------------------------------------------------------- |
| 1167 | TimerBA timer(this, TIMER_CG_ITERATION); |
| 1168 | __recent_cg_status = ' '; |
| 1169 | |
| 1170 | // diagonal for jacobian preconditioning... |
| 1171 | int plen = GetParameterLength(); |
| 1172 | CuTexImage null; |
| 1173 | CuTexImage& VectorDP = |
| 1174 | __lm_use_diagonal_damp ? _cuVectorJJ : null; // diagonal |
| 1175 | ComputeBlockPC(lambda, __lm_use_diagonal_damp); |
| 1176 | |
| 1177 | /////////////////////////////////////////////////////// |
| 1178 | // B = [BC 0 ; 0 BP] |
| 1179 | // m = [mc 0; 0 mp]; |
| 1180 | // A x= BC * x - JcT * Jp * mp * JpT * Jc * x |
| 1181 | // = JcT * Jc x + lambda * D * x + ........ |
| 1182 | //////////////////////////////////////////////////////////// |
| 1183 | |
| 1184 | CuTexImage r; |
| 1185 | r.SetTexture(_cuVectorRK.data(), 8 * _num_camera); |
| 1186 | CuTexImage p; |
| 1187 | p.SetTexture(_cuVectorPK.data(), 8 * _num_camera); |
| 1188 | CuTexImage z; |
| 1189 | z.SetTexture(_cuVectorZK.data(), 8 * _num_camera); |
| 1190 | CuTexImage x; |
| 1191 | x.SetTexture(_cuVectorXK.data(), 8 * _num_camera); |
| 1192 | CuTexImage d; |
| 1193 | d.SetTexture(VectorDP.data(), 8 * _num_camera); |
| 1194 | |
| 1195 | CuTexImage& u = _cuVectorRK; |
| 1196 | CuTexImage& v = _cuVectorPK; |
| 1197 | CuTexImage up; |
| 1198 | up.SetTexture(u.data() + 8 * _num_camera, 4 * _num_point); |
| 1199 | CuTexImage vp; |
| 1200 | vp.SetTexture(v.data() + 8 * _num_camera, 4 * _num_point); |
| 1201 | CuTexImage uc; |
| 1202 | uc.SetTexture(z.data(), 8 * _num_camera); |
| 1203 | |
| 1204 | CuTexImage& e = _cuVectorJX; |
| 1205 | CuTexImage& e2 = _cuImageProj; |
| 1206 | |
| 1207 | ApplyBlockPC(_cuVectorJtE, u, 2); |
| 1208 | ComputeJX(u, e, 2); |
| 1209 | ComputeJtE(e, uc, 1); |
| 1210 | ComputeSAXPY(-1.0f, uc, _cuVectorJtE, r); // r |
| 1211 | ApplyBlockPC(r, p, 1); // z = p = M r |
| 1212 | |
| 1213 | float_t rtz0 = (float_t)ComputeVectorDot(r, p, _cuBufferData); // r(0)' * |
| 1214 | // z(0) |
| 1215 | ComputeJX(p, e, 1); // Jc * x |
| 1216 | ComputeJtE(e, u, 2); // JpT * jc * x |
| 1217 | ApplyBlockPC(u, v, 2); |
| 1218 | float_t qtq0 = (float_t)ComputeVectorNorm(e, _cuBufferData); // q(0)' * q(0) |
| 1219 | float_t pdp0 = |
| 1220 | (float_t)ComputeVectorNormW(p, d, _cuBufferData); // p(0)' * DDD * p(0) |
nothing calls this directly
no test coverage detected