@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++];
}
}
