Examples with PointDistance3 boofcv.alg.geo.pose.PointDistance3 used on opensource projects

Example 1 with PointDistance3

use of boofcv.alg.geo.pose.PointDistance3 in project BoofCV by lessthanoptimal.

the class WrapP3PLineDistance method process.

@Override
public boolean process(List<Point2D3D> inputs, FastQueue<Se3_F64> solutions) {
    if (inputs.size() != 3)
        throw new IllegalArgumentException("Three and only three inputs are required.  Not " + inputs.size());
    solutions.reset();
    Point2D3D P1 = inputs.get(0);
    Point2D3D P2 = inputs.get(1);
    Point2D3D P3 = inputs.get(2);
    // Compute the length of each side in the triangle
    double length12 = P1.location.distance(P2.getLocation());
    double length13 = P1.location.distance(P3.getLocation());
    double length23 = P2.location.distance(P3.getLocation());
    if (!alg.process(P1.observation, P2.observation, P3.observation, length23, length13, length12))
        return false;
    FastQueue<PointDistance3> distances = alg.getSolutions();
    if (distances.size == 0)
        return false;
    // convert observations into a 3D pointing vector and normalize to one
    // homogeneous coordinates
    u1.set(P1.observation.x, P1.observation.y, 1);
    u2.set(P2.observation.x, P2.observation.y, 1);
    u3.set(P3.observation.x, P3.observation.y, 1);
    u1.normalize();
    u2.normalize();
    u3.normalize();
    // set up world point cloud
    cloudWorld.clear();
    cloudWorld.add(P1.location);
    cloudWorld.add(P2.location);
    cloudWorld.add(P3.location);
    for (int i = 0; i < distances.size; i++) {
        PointDistance3 pd = distances.get(i);
        // find points in camera frame
        X1.set(u1.x * pd.dist1, u1.y * pd.dist1, u1.z * pd.dist1);
        X2.set(u2.x * pd.dist2, u2.y * pd.dist2, u2.z * pd.dist2);
        X3.set(u3.x * pd.dist3, u3.y * pd.dist3, u3.z * pd.dist3);
        if (!motionFit.process(cloudWorld, cloudCamera))
            continue;
        // NOTE: This transform is world to camera and it's perfectly valid for to have a negative Z value
        // and be behind the camera.
        Se3_F64 found = solutions.grow();
        found.set(motionFit.getTransformSrcToDst());
    }
    return solutions.size() != 0;
}