update FullBilloirVertexFitter to correctly initialize all variables

Core/include/Acts/Vertexing/FullBilloirVertexFitter.ipp

@@ -196,14 +196,9 @@ Acts::FullBilloirVertexFitter<input_track_t, linearizer_t>::fit(
         billoirVertex.Amat -
         billoirVertex.BCBmat;  // VwgtMat = Amat-sum{BiMat*Ci^-1*BiMat^T}
     if (isConstraintFit) {
-      SpacePointVector posInBilloirFrame;
       // this will be 0 for first iteration but != 0 from second on
-      posInBilloirFrame[0] =
-          vertexingOptions.vertexConstraint.position()[0] - linPoint[0];
-      posInBilloirFrame[1] =
-          vertexingOptions.vertexConstraint.position()[1] - linPoint[1];
-      posInBilloirFrame[2] =
-          vertexingOptions.vertexConstraint.position()[2] - linPoint[2];
+      SpacePointVector posInBilloirFrame =
+          vertexingOptions.vertexConstraint.fullPosition() - linPoint;
 
       Vdel += vertexingOptions.vertexConstraint.fullCovariance().inverse() *
               posInBilloirFrame;
@@ -225,9 +220,7 @@ Acts::FullBilloirVertexFitter<input_track_t, linearizer_t>::fit(
           (bTrack.CiInv) * (bTrack.UiVec - bTrack.BiMat.transpose() * deltaV);
 
       // update track momenta
-      trackMomenta[iTrack][0] += deltaP[0];
-      trackMomenta[iTrack][1] += deltaP[1];
-      trackMomenta[iTrack][2] += deltaP[2];
+      trackMomenta[iTrack] += deltaP;
 
       // correct for 2PI / PI periodicity
       auto correctedPhiTheta = detail::ensureThetaBounds(
@@ -286,23 +279,19 @@ Acts::FullBilloirVertexFitter<input_track_t, linearizer_t>::fit(
     }
 
     if (isConstraintFit) {
-      Vector3D deltaTrk;
       // last term will also be 0 again but only in the first iteration
       // = calc. vtx in billoir frame - (    isConstraintFit pos. in billoir
       // frame )
-      deltaTrk[0] =
-          deltaV[0] -
-          (vertexingOptions.vertexConstraint.position()[0] - linPoint[0]);
-      deltaTrk[1] =
-          deltaV[1] -
-          (vertexingOptions.vertexConstraint.position()[1] - linPoint[1]);
-      deltaTrk[2] =
-          deltaV[2] -
-          (vertexingOptions.vertexConstraint.position()[2] - linPoint[2]);
+
+      SpacePointVector deltaTrk =
+          deltaV -
+          (vertexingOptions.vertexConstraint.fullPosition() - linPoint);
+
       newChi2 +=
           (deltaTrk.transpose())
-              .dot(vertexingOptions.vertexConstraint.covariance().inverse() *
-                   deltaTrk);
+              .dot(
+                  vertexingOptions.vertexConstraint.fullCovariance().inverse() *
+                  deltaTrk);
     }
 
     if (!std::isnormal(newChi2)) {