boofcv.alg.geo.RodriguesRotationJacobian_F32 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) 2021, 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 javax.annotation.Generated;
import org.ejml.data.FMatrixRMaj;

/**
 * Computes the rotation matrix derivative for Rodrigues coordinates
 * which have been parameterized by a 3 vector. The rotation is equal to
 * the vector's magnitude and the rotation vector is the unit vector.
 *
 * @author Peter Abeles
 */
@Generated("boofcv.alg.geo.RodriguesRotationJacobian_F64")
public class RodriguesRotationJacobian_F32 {
	// partial for vector x component
	public FMatrixRMaj Rx = new FMatrixRMaj(3, 3);
	// partial for vector x component
	public FMatrixRMaj Ry = new FMatrixRMaj(3, 3);
	// partial for vector y component
	public FMatrixRMaj Rz = new FMatrixRMaj(3, 3);

	/**
	 * Computes the Rodrigues coordinate Jacobian
	 *
	 * @param x x-component of Rodrigues parametrization.
	 * @param y y-component of Rodrigues parametrization.
	 * @param z z-component of Rodrigues parametrization.
	 */
	public void process( float x, float y, float z ) {

		float theta2 = x*x + y*y + z*z;
		float theta = (float)Math.sqrt(theta2);
		float theta4 = theta2*theta2;
		float theta3 = theta2*theta;

		if (theta4 == 0) {
			Rx.zero();
			Ry.zero();
			Rz.zero();

			Rx.set(1, 2, -1);
			Rx.set(2, 1, 1);

			Ry.set(0, 2, 1);
			Ry.set(2, 0, -1);

			Rz.set(0, 1, -1);
			Rz.set(1, 0, 1);
		} else {
			// computed using sage by differentiating:
			// R = I + (hat(w)/theta)*sin(theta) + ((hat(w)/theta)^2)*(1-cos(theta))
			// theta = sqrt(x*x + y*y + z*z)
			// Then the equations were further simplified by hand

			float s = (float)Math.sin(theta);
			float c = (float)Math.cos(theta);
			float cm = c - 1;

			float xxx = x*x*x*s/theta3 + 2*cm*x*x*x/theta4;
			float xxy = x*x*y*s/theta3 + 2*cm*x*x*y/theta4;
			float xxz = x*x*z*s/theta3 + 2*cm*x*x*z/theta4;
			float xyy = x*y*y*s/theta3 + 2*cm*x*y*y/theta4;
			float xyz = x*y*z*s/theta3 + 2*cm*x*y*z/theta4;
			float xzz = x*z*z*s/theta3 + 2*cm*x*z*z/theta4;
			float yyy = y*y*y*s/theta3 + 2*cm*y*y*y/theta4;
			float yyz = y*y*z*s/theta3 + 2*cm*y*y*z/theta4;
			float yzz = y*z*z*s/theta3 + 2*cm*y*z*z/theta4;
			float zzz = z*z*z*s/theta3 + 2*cm*z*z*z/theta4;

			Rx.data[0] = xxx - x*s/theta - 2*cm*x/theta2;
			Rx.data[1] = xxy - x*z*c/theta2 + x*z*s/theta3 - cm*y/theta2;
			Rx.data[2] = xxz + x*y*c/theta2 - x*y*s/theta3 - cm*z/theta2;
			Rx.data[3] = xxy + x*z*c/theta2 - x*z*s/theta3 - cm*y/theta2;
			Rx.data[4] = xyy - x*s/theta;
			Rx.data[5] = xyz - x*x*c/theta2 + x*x*s/theta3 - s/theta;
			Rx.data[6] = xxz - x*y*c/theta2 + x*y*s/theta3 - cm*z/theta2;
			Rx.data[7] = xyz + x*x*c/theta2 - x*x*s/theta3 + s/theta;
			Rx.data[8] = xzz - x*s/theta;

			Ry.data[0] = xxy - y*s/theta;
			Ry.data[1] = xyy - y*z*c/theta2 + y*z*s/theta3 - cm*x/theta2;
			Ry.data[2] = xyz + y*y*c/theta2 - y*y*s/theta3 + s/theta;
			Ry.data[3] = xyy + y*z*c/theta2 - y*z*s/theta3 - cm*x/theta2;
			Ry.data[4] = yyy - y*s/theta - 2*cm*y/theta2;
			Ry.data[5] = yyz - x*y*c/theta2 + x*y*s/theta3 - cm*z/theta2;
			Ry.data[6] = xyz - y*y*c/theta2 + y*y*s/theta3 - s/theta;
			Ry.data[7] = yyz + x*y*c/theta2 - x*y*s/theta3 - cm*z/theta2;
			Ry.data[8] = yzz - y*s/theta;

			Rz.data[0] = xxz - z*s/theta;
			Rz.data[1] = xyz - z*z*c/theta2 + z*z*s/theta3 - s/theta;
			Rz.data[2] = xzz + y*z*c/theta2 - y*z*s/theta3 - cm*x/theta2;
			Rz.data[3] = xyz + z*z*c/theta2 - z*z*s/theta3 + s/theta;
			Rz.data[4] = yyz - z*s/theta;
			Rz.data[5] = yzz - x*z*c/theta2 + x*z*s/theta3 - cm*y/theta2;
			Rz.data[6] = xzz - y*z*c/theta2 + y*z*s/theta3 - cm*x/theta2;
			Rz.data[7] = yzz + x*z*c/theta2 - x*z*s/theta3 - cm*y/theta2;
			Rz.data[8] = zzz - z*s/theta - 2*cm*z/theta2;
		}
	}
}