boofcv.abst.sfm.d3.PyramidDirectColorDepth_to_DepthVisualOdometry Maven / Gradle / Ivy

Show more of this group Show more artifacts with this name
Show all versions of boofcv-sfm Show documentation
BoofCV is an open source Java library for real-time computer vision and robotics applications.
There is a newer version: 1.1.5
/*
 * Copyright (c) 2011-2019, 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.abst.sfm.d3;

import boofcv.abst.sfm.DepthSparse3D_to_PixelTo3D;
import boofcv.alg.distort.*;
import boofcv.alg.sfm.DepthSparse3D;
import boofcv.alg.sfm.d3.direct.PyramidDirectColorDepth;
import boofcv.core.image.ConvertImageFilter;
import boofcv.factory.distort.LensDistortionFactory;
import boofcv.struct.border.BorderType;
import boofcv.struct.calib.CameraPinhole;
import boofcv.struct.calib.CameraPinholeBrown;
import boofcv.struct.distort.PixelTransform;
import boofcv.struct.distort.Point2Transform2_F32;
import boofcv.struct.distort.SequencePoint2Transform2_F32;
import boofcv.struct.image.ImageBase;
import boofcv.struct.image.ImageGray;
import boofcv.struct.image.ImageType;
import boofcv.struct.image.Planar;
import georegression.struct.ConvertFloatType;
import georegression.struct.point.Point2D_F32;
import georegression.struct.se.Se3_F32;
import georegression.struct.se.Se3_F64;

/**
 * TODO write
 *
 * @author Peter Abeles
 */
public class PyramidDirectColorDepth_to_DepthVisualOdometry, Depth extends ImageGray>
	implements DepthVisualOdometry
{
	ImageType inputType;
	Class depthType;
	ImageType algType;
	DepthSparse3D sparse3D;
	DepthSparse3D_to_PixelTo3D wrapSparse3D;

	ConvertImageFilter convertInput;
	Planar inputConverted;

	PyramidDirectColorDepth alg;

	ImageDistort adjustImage;
	Planar undistorted;

	CameraPinhole paramAdjusted = new CameraPinhole();

	Se3_F32 worldToCurrent = new Se3_F32();
	Se3_F64 w2c_64 = new Se3_F64();

	public PyramidDirectColorDepth_to_DepthVisualOdometry(DepthSparse3D sparse3D,
														  PyramidDirectColorDepth alg,
														  Class depthType ) {
		this.sparse3D = sparse3D;
		this.alg = alg;
		this.inputType = alg.getInputType();
		this.depthType = depthType;
		this.algType = alg.getInputType();

		undistorted = algType.createImage(1,1);
		wrapSparse3D = new DepthSparse3D_to_PixelTo3D<>(sparse3D);
	}

	public PyramidDirectColorDepth_to_DepthVisualOdometry(DepthSparse3D sparse3D,
														  ConvertImageFilter convertImage,
														  PyramidDirectColorDepth alg,
														  Class depthType ) {
		this.sparse3D = sparse3D;
		this.alg = alg;
		this.inputType = convertImage.getInputType();
		this.algType = (ImageType) convertImage.getOutputType();
		this.depthType = depthType;
		this.convertInput = convertImage;

		inputConverted = algType.createImage(1,1);
		undistorted = algType.createImage(1,1);
		wrapSparse3D = new DepthSparse3D_to_PixelTo3D<>(sparse3D);
	}

	@Override
	public void reset() {
		alg.reset();
	}

	@Override
	public boolean isFault() {
		return alg.isFatalError();
	}

	@Override
	public Se3_F64 getCameraToWorld() {
		alg.worldToCurrent().invert(worldToCurrent);
		ConvertFloatType.convert(worldToCurrent, w2c_64);
		return w2c_64;
	}

	@Override
	public void setCalibration(CameraPinholeBrown paramVisual, Point2Transform2_F32 visToDepth) {

		// the algorithms camera model assumes no lens distortion and that skew = 0
		CameraPinhole desired = new CameraPinhole(paramVisual);
		desired.skew = 0;

		adjustImage = LensDistortionOps.changeCameraModel(
				AdjustmentType.EXPAND, BorderType.ZERO, paramVisual,desired,paramAdjusted, algType);

		Point2Transform2_F32 desiredToOriginal = LensDistortionOps_F32.transformChangeModel(
				AdjustmentType.EXPAND, paramVisual, desired, false, null);

		// the adjusted undistorted image pixel to the depth image transform
		Point2Transform2_F32 adjustedToDepth = new SequencePoint2Transform2_F32(desiredToOriginal,visToDepth);

		// Create a lookup table to make the math much faster
		PixelTransform pixelAdjToDepth = new PixelTransformCached_F32(
				paramAdjusted.width, paramAdjusted.height,adjustedToDepth);

		// adjusted pixels to normalized image coordinates in RGB frame
		sparse3D.configure(LensDistortionFactory.narrow(paramAdjusted), pixelAdjToDepth);

		undistorted.reshape(paramAdjusted.width, paramAdjusted.height);
		if( convertInput != null ) {
			inputConverted.reshape(paramAdjusted.width, paramAdjusted.height);
		}

		alg.setCameraParameters(
				(float)paramAdjusted.fx, (float)paramAdjusted.fy,
				(float)paramAdjusted.cx, (float)paramAdjusted.cy,
				paramAdjusted.width, paramAdjusted.height);
	}

	@Override
	public boolean process(T visual, Depth depth) {
		if( convertInput != null ) {
			convertInput.process(visual,inputConverted);
			adjustImage.apply(inputConverted, undistorted);

		} else {
			adjustImage.apply((Planar)visual, undistorted);
		}

		sparse3D.setDepthImage(depth);
		return alg.process(undistorted, wrapSparse3D);
	}

	public double getFractionInBounds() {
		return alg.getFractionInBounds();
	}

	@Override
	public ImageType getVisualType() {
		return inputType;
	}

	@Override
	public Class getDepthType() {
		return depthType;
	}

	public Planar getUndistorted() {
		return undistorted;
	}
}