boofcv.alg.geo.WorldToCameraToPixel Maven / Gradle / Ivy

Go to download
Show more of this group Show more artifacts with this name
Show all versions of boofcv-geo Show documentation
BoofCV is an open source Java library for real-time computer vision and robotics applications.
There is a newer version: 1.1.7
/*
 * Copyright (c) 2023, Peter Abeles. All Rights Reserved.
 *
 * This file is part of BoofCV (http://boofcv.org).
 *
 * 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 boofcv.alg.geo;

import boofcv.alg.distort.LensDistortionNarrowFOV;
import boofcv.alg.distort.brown.LensDistortionBrown;
import boofcv.alg.distort.pinhole.LensDistortionPinhole;
import boofcv.struct.calib.CameraPinhole;
import boofcv.struct.calib.CameraPinholeBrown;
import boofcv.struct.distort.Point2Transform2_F64;
import georegression.struct.point.Point2D_F64;
import georegression.struct.point.Point3D_F64;
import georegression.struct.point.Point4D_F64;
import georegression.struct.se.Se3_F64;
import georegression.transform.se.SePointOps_F64;
import lombok.Getter;
import org.jetbrains.annotations.Nullable;

/**
 * Convenience class which will take a point in world coordinates, translate it to camera reference frame,
 * then project onto the image plane and compute its pixels. Supports lens distortion
 *
 * @author Peter Abeles
 */
@SuppressWarnings({"NullAway.Init"})
public class WorldToCameraToPixel {

	/** transform from world to camera reference frames */
	@Getter private Se3_F64 worldToCamera;

	/** storage for point in camera frame */
	@Getter private Point3D_F64 cameraPt = new Point3D_F64();

	@Getter private Point4D_F64 cameraPt4 = new Point4D_F64();

	/** transform from normalized image coordinates into pixels */
	@Getter private Point2Transform2_F64 normToPixel;

	/**
	 * Specifies intrinsic camera parameters and  the transform from world to camera.
	 *
	 * @param intrinsic camera parameters
	 * @param worldToCamera transform from world to camera
	 */
	public void configure( CameraPinholeBrown intrinsic, Se3_F64 worldToCamera ) {
		configure(new LensDistortionBrown(intrinsic), worldToCamera);
	}

	public void configure( CameraPinhole intrinsic, Se3_F64 worldToCamera ) {
		configure(new LensDistortionPinhole(intrinsic), worldToCamera);
	}

	/**
	 * Specifies intrinsic camera parameters and  the transform from world to camera.
	 *
	 * @param distortion camera parameters
	 * @param worldToCamera transform from world to camera
	 */
	public void configure( LensDistortionNarrowFOV distortion, Se3_F64 worldToCamera ) {
		this.worldToCamera = worldToCamera;

		normToPixel = distortion.distort_F64(false, true);
	}

	/**
	 * Computes the observed location of the specified point in world coordinates in the camera pixel. If
	 * the object can't be viewed because it is behind the camera then false is returned.
	 *
	 * @param worldPt Location of point in world frame
	 * @param pixelPt Pixel observation of point.
	 * @return True if visible (+z) or false if not visible (-z)
	 */
	public boolean transform( Point3D_F64 worldPt, Point2D_F64 pixelPt ) {
		SePointOps_F64.transform(worldToCamera, worldPt, cameraPt);

		// can't see the point
		if (cameraPt.z <= 0)
			return false;

		normToPixel.compute(cameraPt.x/cameraPt.z, cameraPt.y/cameraPt.z, pixelPt);
		return true;
	}

	public boolean transform( Point3D_F64 worldPt, Point2D_F64 pixelPt, Point2D_F64 normPt ) {
		SePointOps_F64.transform(worldToCamera, worldPt, cameraPt);

		// can't see the point
		if (cameraPt.z <= 0)
			return false;

		normPt.x = cameraPt.x/cameraPt.z;
		normPt.y = cameraPt.y/cameraPt.z;

		normToPixel.compute(normPt.x, normPt.y, pixelPt);
		return true;
	}

	/**
	 * Computes the observed location of the specified point in world coordinates in the camera pixel. If
	 * the object can't be viewed because it is behind the camera then false is returned.
	 *
	 * @param worldPt Location of point in world frame. Homogenous coordinates.
	 * @param pixelPt Pixel observation of point.
	 * @return True if visible (+z) or false if not visible (-z)
	 */
	public boolean transform( Point4D_F64 worldPt, Point2D_F64 pixelPt ) {
		SePointOps_F64.transform(worldToCamera, worldPt, cameraPt4);

		// can't see the point
		if (PerspectiveOps.isBehindCamera(cameraPt4))
			return false;

		normToPixel.compute(cameraPt4.x/cameraPt4.z, cameraPt4.y/cameraPt4.z, pixelPt);
		return true;
	}

	/**
	 * Computes location of 3D point in world as observed in the camera. Point is returned if visible or null
	 * if not visible.
	 *
	 * @param worldPt Location of point on world reference frame
	 * @return Pixel coordinate of point or null if not visible
	 */
	public @Nullable Point2D_F64 transform( Point3D_F64 worldPt ) {
		Point2D_F64 out = new Point2D_F64();
		if (transform(worldPt, out))
			return out;
		else
			return null;
	}
}