N point perspective problem

Author

134

September 16, 2017 08:50 AM

Hi,

I have a N point perspective problem, that I could use some pointers to solve.

I have a calibrated camera, where I observe 4-6 known world points. Each observation is uniquely tied to a world point and has an 2d coordinate in my image. I know my camera properties so I can convert them to a direction in the camera frame.

I have been using openCV's solvePNP method and it works like a charm. My problem is that I dont have that luxury anymore as I need to do the computation in the frontend of a browser alongside the ThreeJS engine. I have tried sending points to a server to return me results via pythons opencv bnindings, but its too slow and requires alot of infrastructure. The opencv implementation is opensource, but it uses a solver and has a complexity that I dont find it feasible to port it line by line.

Instead I started making an attempt (how hard can it be?) for a solution by defining a function that could give me an extrinsic matrix from three euler angles and a 3d position. (6DOF). I also made an error function that, given a proposed matrix, the world points and the directions from the camera could return me an error. I used the sum of dot products between the direction vectors, so large values are good. The idea from here would be to nummerically differentiate the error function in relation to the 6DOF variables and gradually take tiny steps up hill on the error function. I am sure the solution will be slow, but I dont mind it takes a while.

My problem is that it does not converge at all. Any ideas why? Or pointers to what I could change.

I suspect my error function could be better, but I am unsure of what to use instead.

Kind regards

Jesper

taxfromdk

Author

134

September 16, 2017 02:46 PM

im at a wedding but this was my scratchpad solution i did over breakfast. it does converge now in some cases. not very fast though.

i use node to run it and it requires threejs library

var THREE = require("three");

function printMatrix(m)

{

console.log(m.elements[0], m.elements[4], m.elements[8], m.elements[12]);

console.log(m.elements[1], m.elements[5], m.elements[9], m.elements[13]);

console.log(m.elements[2], m.elements[6], m.elements[10], m.elements[14]);

console.log(m.elements[3], m.elements[7], m.elements[11], m.elements[15]);

console.log();

}

//Create matrix from degrees of freedom

function getMatrix(rvec, tvec)

{

var M_r = new THREE.Matrix4().makeRotationFromEuler(rvec);

var M_t = new THREE.Matrix4().makeTranslation(tvec.x, tvec.y, tvec.z);

var r = M_t.multiply(M_r)

return r;

}

//Find directions given a matrix

function getDirections(worldpoints, matrix)

{

var directions = []

for(var i = 0; i < worldpoints.length; i++)

{

//console.log(i);

var d = worldpoints.clone();

d.applyMatrix4(matrix);

var l = Math.sqrt(d.x*d.x+d.y*d.y+d.z*d.z);

if(l > 0.0)

{

d.multiplyScalar(1.0/l);

}

d.w = 0.0;

directions.push(d);

}

return directions;

}

//Compare similarity between directions

function error(d0, d1)

{

var error = 0.0;

for(var i = 0; i < d0.length; i++)

{

var angle_rad = Math.acos(d0.x * d1.x + d0.y * d1.y + d0.z * d1.z);

var e = angle_rad*angle_rad;

error += e;

}

return error;

}

function clamp(i, m)

{

if(i > m)return m;

if(i < -m)return -m;

return i;

}

var rvec_o = new THREE.Euler( -Math.PI/8, 0.0, 0.0, 'XYZ' );

var tvec_o = new THREE.Vector3(0.0, -3.0, -40.0);

var rvec = new THREE.Euler( 0, 0, 0, 'XYZ' );

var tvec = new THREE.Vector3(0.0,0.0,-100.0);

//Create 4 directions

var world_positions = [];

var field_length = 105.0;

var field_width = 105.0;

world_positions.push(new THREE.Vector4(-field_length/2,0.0,-field_width/2,1.0));

world_positions.push(new THREE.Vector4(-field_length/2,0.0,field_width/2,1.0));

world_positions.push(new THREE.Vector4(0.0,0.0,field_width/2,1.0));

world_positions.push(new THREE.Vector4(field_length/2,0.0,field_width/2,1.0));

world_positions.push(new THREE.Vector4(field_length/2,0.0,-field_width/2,1.0));

//console.log(world_positions);

var dirs_o = getDirections(world_positions, getMatrix(rvec_o, tvec_o));

//one mm at a time, how many degrees

var N = 100000

var step = 100.00;

lasterror = 99999999.0

var mx;

var before = new Date().getTime();

for(var i = 0; i < N; i++)

{

var e = error(dirs_o, getDirections(world_positions, getMatrix(rvec, tvec)));

var r_x_0 = error(dirs_o, getDirections(world_positions, getMatrix(new THREE.Euler(rvec.x - step, rvec.y, rvec.z, 'XYZ'), tvec)));

var r_x_1 = error(dirs_o, getDirections(world_positions, getMatrix(new THREE.Euler(rvec.x + step, rvec.y, rvec.z, 'XYZ'), tvec)));

var r_y_0 = error(dirs_o, getDirections(world_positions, getMatrix(new THREE.Euler(rvec.x, rvec.y - step, rvec.z, 'XYZ'), tvec)));

var r_y_1 = error(dirs_o, getDirections(world_positions, getMatrix(new THREE.Euler(rvec.x, rvec.y + step, rvec.z, 'XYZ'), tvec)));

var r_z_0 = error(dirs_o, getDirections(world_positions, getMatrix(new THREE.Euler(rvec.x, rvec.y, rvec.z - step, 'XYZ'), tvec)));

var r_z_1 = error(dirs_o, getDirections(world_positions, getMatrix(new THREE.Euler(rvec.x, rvec.y, rvec.z + step, 'XYZ'), tvec)));

var t_x_0 = error(dirs_o, getDirections(world_positions, getMatrix(rvec, new THREE.Vector3(tvec.x-step, tvec.y, tvec.z))));

var t_x_1 = error(dirs_o, getDirections(world_positions, getMatrix(rvec, new THREE.Vector3(tvec.x+step, tvec.y, tvec.z))));

var t_y_0 = error(dirs_o, getDirections(world_positions, getMatrix(rvec, new THREE.Vector3(tvec.x, tvec.y-step, tvec.z))));

var t_y_1 = error(dirs_o, getDirections(world_positions, getMatrix(rvec, new THREE.Vector3(tvec.x, tvec.y+step, tvec.z))));

var t_z_0 = error(dirs_o, getDirections(world_positions, getMatrix(rvec, new THREE.Vector3(tvec.x, tvec.y, tvec.z-step))));

var t_z_1 = error(dirs_o, getDirections(world_positions, getMatrix(rvec, new THREE.Vector3(tvec.x, tvec.y, tvec.z+step))));

var d_r_x = (r_x_1-r_x_0)/(step*2);

var d_r_y = (r_y_1-r_y_0)/(step*2);

var d_r_z = (r_z_1-r_z_0)/(step*2);

var d_t_x = 10.0*(t_x_1-t_x_0)/(step*2);

var d_t_y = 10.0*(t_y_1-t_y_0)/(step*2);

var d_t_z = 10.0*(t_z_1-t_z_0)/(step*2);

if(i % Math.floor(N/10) == 0)

{

console.log("e:",e," r:",rvec.x-rvec_o.x,",", rvec.y-rvec_o.y,",", rvec.z-rvec_o.z," t",tvec.x-tvec_o.x,",",tvec.y-tvec_o.y,",",tvec.z-tvec_o.z);

}

//If worse than last

if(lasterror < e)

{

step *= 0.5;

}

else

{

rvec.x -= d_r_x*step;

rvec.y -= d_r_y*step;

rvec.z -= d_r_z*step;

tvec.x -= d_t_x*step;

tvec.y -= d_t_y*step;

tvec.z -= d_t_z*step;

//Keep rotation in range -PI to PI

while(rvec.x < Math.PI) rvec.x+=2*Math.PI;

while(rvec.y < Math.PI) rvec.y+=2*Math.PI;

while(rvec.z < Math.PI) rvec.z+=2*Math.PI;

while(rvec.x >= Math.PI) rvec.x-=2*Math.PI;

while(rvec.y >= Math.PI) rvec.y-=2*Math.PI;

while(rvec.z >= Math.PI) rvec.z-=2*Math.PI;

step *= 1.0001;

}

lasterror = e;

}

console.log("Time: ", new Date().getTime() - before);

console.log("r:",rvec.x,",", rvec.y,",", rvec.z," t",tvec.x,",",tvec.y,",",tvec.z);

N point perspective problem

This topic is closed to new replies.

Popular Topics

Recommended Tutorials

N point perspective problem

This topic is closed to new replies.

Popular Topics

Recommended Tutorials

Reticulating splines