Program: 150567
Project: 150567
Buggy source:
/** 
 * Uses the previously found motion between the two cameras to estimate the scale and 3D point of common features. If a feature already has a known 3D point that is not modified. Scale is found by computing the 3D coordinate of all points with a 3D point again then dividing the two distances. New features are also triangulated and have their location's update using this scale. A known feature has the current view added to its list of views.
 */
private boolean determinePose(CameraView target){
  List<Point2D3D> list=new ArrayList<>();
  List<Feature3D> features=new ArrayList<>();
  GrowQueue_I32 featureIndexes=new GrowQueue_I32();
  for (  CameraMotion c : target.connections) {
    boolean isSrc=c.viewSrc == target;
    CameraView other=c.destination(target);
    if (other.distanceFromRoot < 0)     continue;
    for (int i=0; i < c.features.size(); i++) {
      AssociatedIndex a=c.features.get(i);
      Feature3D f=other.features3D[isSrc ? a.dst : a.src];
      if (f == null || f.mark == target.index)       continue;
      f.mark=target.index;
      features.add(f);
      featureIndexes.add(isSrc ? a.src : a.dst);
      Point2D_F64 norm=target.featureNorm.get(isSrc ? a.src : a.dst);
      Point2D3D p=new Point2D3D();
      p.location.set(f.worldPt);
      p.observation.set(norm);
      list.add(p);
    }
  }
  if (list.size() < 100 || !estimatePnP.process(list)) {
    System.out.println("String_Node_Str" + target.index + "String_Node_Str"+ list.size());
    return false;
  }
  int N=estimatePnP.getMatchSet().size();
  System.out.println("String_Node_Str" + target.index + "String_Node_Str"+ N+ "String_Node_Str"+ list.size());
  for (int i=0; i < N; i++) {
    int which=estimatePnP.getInputIndex(i);
    Feature3D f=features.get(which);
    if (f.views.contains(target.index))     continue;
    f.views.add(target.index);
    f.feature.add(featureIndexes.get(which));
    target.features3D[featureIndexes.get(which)]=f;
  }
  Se3_F64 worldToView=estimatePnP.getModelParameters();
  target.viewToWorld.set(worldToView.invert(null));
  return true;
}

Fixed version (ground truth):
/** 
 * Uses the previously found motion between the two cameras to estimate the scale and 3D point of common features. If a feature already has a known 3D point that is not modified. Scale is found by computing the 3D coordinate of all points with a 3D point again then dividing the two distances. New features are also triangulated and have their location's update using this scale. A known feature has the current view added to its list of views.
 */
private boolean determinePose(CameraView target){
  List<Point2D3D> list=new ArrayList<>();
  List<Feature3D> features=new ArrayList<>();
  GrowQueue_I32 featureIndexes=new GrowQueue_I32();
  int closestDistance=Integer.MAX_VALUE;
  for (  CameraMotion c : target.connections) {
    boolean isSrc=c.viewSrc == target;
    CameraView other=c.destination(target);
    if (other.distanceFromRoot < 0)     continue;
    closestDistance=Math.min(closestDistance,other.distanceFromRoot);
    for (int i=0; i < c.features.size(); i++) {
      AssociatedIndex a=c.features.get(i);
      Feature3D f=other.features3D[isSrc ? a.dst : a.src];
      if (f == null || f.mark == target.index)       continue;
      f.mark=target.index;
      features.add(f);
      featureIndexes.add(isSrc ? a.src : a.dst);
      Point2D_F64 norm=target.featureNorm.get(isSrc ? a.src : a.dst);
      Point2D3D p=new Point2D3D();
      p.location.set(f.worldPt);
      p.observation.set(norm);
      list.add(p);
    }
  }
  if (list.size() < 100 || !estimatePnP.process(list)) {
    System.out.println("String_Node_Str" + target.index + "String_Node_Str"+ list.size());
    return false;
  }
  target.distanceFromRoot=closestDistance + 1;
  int N=estimatePnP.getMatchSet().size();
  System.out.println("String_Node_Str" + target.index + "String_Node_Str"+ N+ "String_Node_Str"+ list.size());
  for (int i=0; i < N; i++) {
    int which=estimatePnP.getInputIndex(i);
    Feature3D f=features.get(which);
    if (f.views.contains(target.index))     continue;
    f.views.add(target);
    f.feature.add(featureIndexes.get(which));
    target.features3D[featureIndexes.get(which)]=f;
  }
  Se3_F64 worldToView=estimatePnP.getModelParameters();
  target.viewToWorld.set(worldToView.invert(null));
  return true;
}