N point perspective problem

Recommended Posts

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

Share this post


Link to post
Share on other sites

This is an interesting problem I have never actually solved myself. Could you provide us with a realistic sample input and the desired output? I think I'll have some time to give it a try this weekend.

 

Edited by alvaro

Share this post


Link to post
Share on other sites

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);
 

Share this post


Link to post
Share on other sites

Create an account or sign in to comment

You need to be a member in order to leave a comment

Create an account

Sign up for a new account in our community. It's easy!

Register a new account

Sign in

Already have an account? Sign in here.

Sign In Now