georegression.fitting.points.MatchCloudToCloudIcp Maven / Gradle / Ivy

Go to download
Show more of this group Show more artifacts with this name
Show all versions of georegression Show documentation
GeoRegression is a free Java based geometry library for scientific computing in fields such as robotics and computer vision with a focus on 2D/3D space.
The newest version!
/*
 * Copyright (C) 2021, Peter Abeles. All Rights Reserved.
 *
 * This file is part of Geometric Regression Library (GeoRegression).
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

package georegression.fitting.points;

import georegression.fitting.MotionTransformPoint;
import georegression.fitting.points.IterativeClosestPoint.Distance;
import georegression.misc.StoppingCondition;
import georegression.struct.GeoTuple;
import georegression.struct.InvertibleTransform;
import org.ddogleg.nn.NearestNeighbor;
import org.ddogleg.nn.NnData;
import org.jetbrains.annotations.Nullable;

import java.util.List;

/**
 * Abstract implementation of ICP for point cloud fitting using an
 * {@link InvertibleTransform arbitrary transform}.
 *
 * @author Peter Abeles
 * @see IterativeClosestPoint
 */
@SuppressWarnings("NullAway.Init")
public abstract class MatchCloudToCloudIcp
		implements MatchCloudToCloud {
	// Nearest-neighbor algorithm
	NearestNeighbor nn;
	NearestNeighbor.Search
 searchNN;

	// computes the distance betwene two points
	Distance
 distance;

	// reference to destination list
	List
 source;
	// maximum distance apart two points can be. Euclidean squared
	double maxDistanceSq;
	// storage for NN results
	NnData
 storageNN = new NnData<>();

	// the actual ICP algorithm
	IterativeClosestPoint icp;

	/**
	 * Configures ICP
	 *
	 * @param motion        estimates motion between two sets of associated points
	 * @param nn            Nearest-Neighbor search
	 * @param maxDistanceSq Maximum distance between two paired points. Euclidean squared
	 * @param stop          Stopping criteria for ICP iterations
	 */
	protected MatchCloudToCloudIcp(MotionTransformPoint motion,
								   NearestNeighbor
 nn,
								   Distance
 distance,
								   double maxDistanceSq, StoppingCondition stop) {
		this.maxDistanceSq = maxDistanceSq;
		this.nn = nn;
		this.distance = distance;
		icp = new IterativeClosestPoint<>(stop, motion, distance);
		icp.setModel(new Model());

		searchNN = nn.createSearch();
	}

	@Override
	public void setSource(List
 source) {
		this.source = source;
	}

	@Override
	public void setDestination(List
 destination) {
		nn.setPoints(destination, false);
	}

	@Override
	public boolean compute() {
		return icp.process(source);
	}

	@Override
	public SE getSourceToDestination() {
		return icp.getPointsToModel();
	}

	@Override
	public boolean isModifiedSource() {
		return true;
	}

	@Override
	public int getMatchedSourcePoints() {
		return icp.getTotalMatched();
	}

	public IterativeClosestPoint getIcp() {
		return icp;
	}

	private class Model implements ClosestPointToModel {
		@Override
		public @Nullable P findClosestPoint(P target) {
			if (searchNN.findNearest(target, maxDistanceSq, storageNN)) {
				return storageNN.point;
			} else {
				return null;
			}
		}
	}
}