boofcv.alg.geo.pose.UtilLepetitEPnP Maven / Gradle / Ivy
Go to download
Show more of this group Show more artifacts with this name
Show all versions of geo Show documentation
Show all versions of geo Show documentation
BoofCV is an open source Java library for real-time computer vision and robotics applications.
/*
* Copyright (c) 2011-2013, 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.pose;
import georegression.struct.point.Point3D_F64;
import org.ddogleg.struct.FastQueue;
import org.ejml.data.DenseMatrix64F;
import java.util.List;
/**
* Various utility functions for {@link PnPLepetitEPnP}.
*
* @author Peter Abeles
*/
public class UtilLepetitEPnP {
/**
* Computes the camera control points as weighted sum of null points.
*
* @param beta Beta values which describe the weights of null points
* @param nullPts Null points that the camera point is a weighted sum of
* @param cameraPts The output
*/
public static void computeCameraControl(double beta[],
List nullPts[],
FastQueue cameraPts ,
int numControl )
{
cameraPts.reset();
for( int i = 0; i < numControl; i++ ) {
cameraPts.grow().set(0,0,0);
}
for( int i = 0; i < numControl; i++ ) {
double b = beta[i];
// System.out.printf("%7.3f ", b);
for( int j = 0; j < numControl; j++ ) {
Point3D_F64 s = cameraPts.get(j);
Point3D_F64 p = nullPts[i].get(j);
s.x += b*p.x;
s.y += b*p.y;
s.z += b*p.z;
}
}
}
/**
* Extracts the linear constraint matrix for case 1 from the full 6x10 constraint matrix.
*/
public static void constraintMatrix6x4( DenseMatrix64F L_6x10 , DenseMatrix64F L_6x4 ) {
int index = 0;
for( int i = 0; i < 6; i++ ) {
L_6x4.data[index++] = L_6x10.get(i,0);
L_6x4.data[index++] = L_6x10.get(i,1);
L_6x4.data[index++] = L_6x10.get(i,3);
L_6x4.data[index++] = L_6x10.get(i,6);
}
}
/**
* Extracts the linear constraint matrix for case 2 from the full 6x10 constraint matrix.
*/
public static void constraintMatrix6x3( DenseMatrix64F L_6x10 , DenseMatrix64F L_6x3 ) {
int index = 0;
for( int i = 0; i < 6; i++ ) {
L_6x3.data[index++] = L_6x10.get(i,0);
L_6x3.data[index++] = L_6x10.get(i,1);
L_6x3.data[index++] = L_6x10.get(i,4);
}
}
/**
* Extracts the linear constraint matrix for case 3 from the full 6x10 constraint matrix.
*/
public static void constraintMatrix6x6( DenseMatrix64F L_6x10 , DenseMatrix64F L_6x6 ) {
int index = 0;
for( int i = 0; i < 6; i++ ) {
L_6x6.data[index++] = L_6x10.get(i,0);
L_6x6.data[index++] = L_6x10.get(i,1);
L_6x6.data[index++] = L_6x10.get(i,2);
L_6x6.data[index++] = L_6x10.get(i,4);
L_6x6.data[index++] = L_6x10.get(i,5);
L_6x6.data[index++] = L_6x10.get(i,7);
}
}
/**
* Linear constraint matrix used in case 4 in the general case
*
* @param L Constraint matrix
* @param y Vector containing distance between world control points
* @param controlWorldPts List of world control points
* @param nullPts Null points
*/
public static void constraintMatrix6x10( DenseMatrix64F L , DenseMatrix64F y ,
FastQueue controlWorldPts ,
List nullPts[] ) {
int row = 0;
for( int i = 0; i < 4; i++ ) {
Point3D_F64 ci = controlWorldPts.get(i);
Point3D_F64 vai = nullPts[0].get(i);
Point3D_F64 vbi = nullPts[1].get(i);
Point3D_F64 vci = nullPts[2].get(i);
Point3D_F64 vdi = nullPts[3].get(i);
for( int j = i+1; j < 4; j++ , row++) {
Point3D_F64 cj = controlWorldPts.get(j);
Point3D_F64 vaj = nullPts[0].get(j);
Point3D_F64 vbj = nullPts[1].get(j);
Point3D_F64 vcj = nullPts[2].get(j);
Point3D_F64 vdj = nullPts[3].get(j);
y.set(row,0,ci.distance2(cj));
double xa = vai.x-vaj.x;
double ya = vai.y-vaj.y;
double za = vai.z-vaj.z;
double xb = vbi.x-vbj.x;
double yb = vbi.y-vbj.y;
double zb = vbi.z-vbj.z;
double xc = vci.x-vcj.x;
double yc = vci.y-vcj.y;
double zc = vci.z-vcj.z;
double xd = vdi.x-vdj.x;
double yd = vdi.y-vdj.y;
double zd = vdi.z-vdj.z;
double da = xa*xa + ya*ya + za*za;
double db = xb*xb + yb*yb + zb*zb;
double dc = xc*xc + yc*yc + zc*zc;
double dd = xd*xd + yd*yd + zd*zd;
double dab = xa*xb + ya*yb + za*zb;
double dac = xa*xc + ya*yc + za*zc;
double dad = xa*xd + ya*yd + za*zd;
double dbc = xb*xc + yb*yc + zb*zc;
double dbd = xb*xd + yb*yd + zb*zd;
double dcd = xc*xd + yc*yd + zc*zd;
L.set(row,0,da);
L.set(row,1,2*dab);
L.set(row,2,2*dac);
L.set(row,3,2*dad);
L.set(row,4,db);
L.set(row,5,2*dbc);
L.set(row,6,2*dbd);
L.set(row,7,dc);
L.set(row,8,2*dcd);
L.set(row,9,dd);
}
}
}
/**
* Extracts the linear constraint matrix for case 1 from the full 6x10 constraint matrix.
*/
public static void constraintMatrix3x3a( DenseMatrix64F L_3x6 , DenseMatrix64F L_3x3 ) {
int index = 0;
for( int i = 0; i < 3; i++ ) {
L_3x3.data[index++] = L_3x6.get(i,0);
L_3x3.data[index++] = L_3x6.get(i,1);
L_3x3.data[index++] = L_3x6.get(i,2);
}
}
/**
* Extracts the linear constraint matrix for planar case 2 from the full 4x6 constraint matrix.
*/
public static void constraintMatrix3x3( DenseMatrix64F L_3x6 ,
DenseMatrix64F L_6x3 ) {
int index = 0;
for( int i = 0; i < 3; i++ ) {
L_6x3.data[index++] = L_3x6.get(i,0);
L_6x3.data[index++] = L_3x6.get(i,1);
L_6x3.data[index++] = L_3x6.get(i,3);
}
}
/**
* Linear constraint matrix for case 4 in the planar case
*
* @param L Constraint matrix
* @param y Vector containing distance between world control points
* @param controlWorldPts List of world control points
* @param nullPts Null points
*/
public static void constraintMatrix3x6( DenseMatrix64F L , DenseMatrix64F y ,
FastQueue controlWorldPts ,
List nullPts[] ) {
int row = 0;
for( int i = 0; i < 3; i++ ) {
Point3D_F64 ci = controlWorldPts.get(i);
Point3D_F64 vai = nullPts[0].get(i);
Point3D_F64 vbi = nullPts[1].get(i);
Point3D_F64 vci = nullPts[2].get(i);
for( int j = i+1; j < 3; j++ , row++) {
Point3D_F64 cj = controlWorldPts.get(j);
Point3D_F64 vaj = nullPts[0].get(j);
Point3D_F64 vbj = nullPts[1].get(j);
Point3D_F64 vcj = nullPts[2].get(j);
y.set(row,0,ci.distance2(cj));
double xa = vai.x-vaj.x;
double ya = vai.y-vaj.y;
double za = vai.z-vaj.z;
double xb = vbi.x-vbj.x;
double yb = vbi.y-vbj.y;
double zb = vbi.z-vbj.z;
double xc = vci.x-vcj.x;
double yc = vci.y-vcj.y;
double zc = vci.z-vcj.z;
double da = xa*xa + ya*ya + za*za;
double db = xb*xb + yb*yb + zb*zb;
double dc = xc*xc + yc*yc + zc*zc;
double dab = xa*xb + ya*yb + za*zb;
double dac = xa*xc + ya*yc + za*zc;
double dbc = xb*xc + yb*yc + zb*zc;
L.set(row,0,da);
L.set(row,1,2*dab);
L.set(row,2,2*dac);
L.set(row,3,db);
L.set(row,4,2*dbc);
L.set(row,5,dc);
}
}
}
/**
* Computes the residuals (difference between observed and predicted) given 4 control points.
*/
public static void residuals_Control4( DenseMatrix64F L_full , DenseMatrix64F y ,
double beta[] , double r[] )
{
double b0 = beta[0]; double b1 = beta[1]; double b2 = beta[2]; double b3 = beta[3];
final double ld[] = L_full.data;
for( int i = 0; i < 6; i++ ) {
int li = L_full.numCols*i;
double residual = -y.data[i];
residual += ld[li++]*b0*b0;
residual += ld[li++]*b0*b1;
residual += ld[li++]*b0*b2;
residual += ld[li++]*b0*b3;
residual += ld[li++]*b1*b1;
residual += ld[li++]*b1*b2;
residual += ld[li++]*b1*b3;
residual += ld[li++]*b2*b2;
residual += ld[li++]*b2*b3;
residual += ld[li ]*b3*b3;
r[i] = residual;
}
}
/**
* Computes the residuals (difference between observed and predicted given 3 control points.
*/
public static void residuals_Control3( DenseMatrix64F L_full , DenseMatrix64F y ,
double beta[] , double r[] )
{
double b0 = beta[0]; double b1 = beta[1]; double b2 = beta[2];
final double ld[] = L_full.data;
for( int i = 0; i < 3; i++ ) {
double residual = -y.data[i];
int li = L_full.numCols*i;
residual += ld[li++]*b0*b0;
residual += ld[li++]*b0*b1;
residual += ld[li++]*b0*b2;
residual += ld[li++]*b1*b1;
residual += ld[li++]*b1*b2;
residual += ld[li ]*b2*b2;
r[i] = residual;
}
}
/**
* Computes the Jacobian given 4 control points.
*/
public static void jacobian_Control4( DenseMatrix64F L_full ,
double beta[] , DenseMatrix64F A )
{
int indexA = 0;
double b0 = beta[0]; double b1 = beta[1]; double b2 = beta[2]; double b3 = beta[3];
final double ld[] = L_full.data;
for( int i = 0; i < 6; i++ ) {
int li = L_full.numCols*i;
A.data[indexA++] = 2*ld[li+0]*b0 + ld[li+1]*b1 + ld[li+2]*b2 + ld[li+3]*b3;
A.data[indexA++] = ld[li+1]*b0 + 2*ld[li+4]*b1 + ld[li+5]*b2 + ld[li+6]*b3;
A.data[indexA++] = ld[li+2]*b0 + ld[li+5]*b1 + 2*ld[li+7]*b2 + ld[li+8]*b3;
A.data[indexA++] = ld[li+3]*b0 + ld[li+6]*b1 + ld[li+8]*b2 + 2*ld[li+9]*b3;
}
}
/**
* Computes the Jacobian given 3 control points.
*/
public static void jacobian_Control3( DenseMatrix64F L_full ,
double beta[] , DenseMatrix64F A)
{
int indexA = 0;
double b0 = beta[0]; double b1 = beta[1]; double b2 = beta[2];
final double ld[] = L_full.data;
for( int i = 0; i < 3; i++ ) {
int li = L_full.numCols*i;
A.data[indexA++] = 2*ld[li+0]*b0 + ld[li+1]*b1 + ld[li+2]*b2;
A.data[indexA++] = ld[li+1]*b0 + 2*ld[li+3]*b1 + ld[li+4]*b2;
A.data[indexA++] = ld[li+2]*b0 + ld[li+4]*b1 + 2*ld[li+5]*b2;
}
}
}
© 2015 - 2025 Weber Informatics LLC | Privacy Policy