@Override public void decode(double[] param, CalibratedPoseAndPoint outputModel) { int paramIndex = 0; // first decode the transformation for (int i = 0; i < numViews; i++) { // don't decode if it is already known if (knownView[i]) continue; Se3_F64 se = outputModel.getWorldToCamera(i); rotation.setParamVector(param[paramIndex++], param[paramIndex++], param[paramIndex++]); RotationMatrixGenerator.rodriguesToMatrix(rotation, se.getR()); Vector3D_F64 T = se.getT(); T.x = param[paramIndex++]; T.y = param[paramIndex++]; T.z = param[paramIndex++]; } // now decode the points for (int i = 0; i < numPoints; i++) { Point3D_F64 p = outputModel.getPoint(i); p.x = param[paramIndex++]; p.y = param[paramIndex++]; p.z = param[paramIndex++]; } }