/**
* Configures the residual function.
*
* @param model Model being processed
* @param codec Decodes parameterized version of parameters
* @param obs Contains camera observations of each point.
*/
public void configure(
ModelCodec<CalibratedPoseAndPoint> codec,
CalibratedPoseAndPoint model,
List<ViewPointObservations> obs) {
this.model = model;
this.codec = codec;
this.observations = obs;
numObservations = 0;
for (int view = 0; view < model.getNumViews(); view++) {
numObservations += obs.get(view).getPoints().size() * 2;
}
}
public void process(CalibratedPoseAndPoint model, double[] output) {
int outputIndex = 0;
for (int view = 0; view < model.getNumViews(); view++) {
Se3_F64 worldToCamera = model.getWorldToCamera(view);
FastQueue<PointIndexObservation> observedPts = observations.get(view).getPoints();
for (int i = 0; i < observedPts.size; i++) {
PointIndexObservation o = observedPts.data[i];
Point3D_F64 worldPt = model.getPoint(o.pointIndex);
SePointOps_F64.transform(worldToCamera, worldPt, cameraPt);
output[outputIndex++] = cameraPt.x / cameraPt.z - o.obs.x;
output[outputIndex++] = cameraPt.y / cameraPt.z - o.obs.y;
}
}
}
