Laser Projector Calibration in 3D Space

亡梦爱人 提交于 2019-12-06 15:54:46

Here a sample code that show the proper calculation required. Imagine you real table X,Y is X horizontal with the picture and Y going up. The origin being 0,0,0 for convenience and easier sample to follow. Then the second image imagine the proper X,Y to be the top left corner where X axis is going down and Y going right (-90 degree rotation ish). Don't worry about that it's easily adjustable later. The comments in the quick sample below explain what every line does and why it's there.

// compute from the points the table height and width
var realTableDimX = 902d;
var realTableDimY = 597d;

// the real table matrix is 0,0,0 on bottom corner
// so matrix is identity (our source)
var realTableMatrix = new Matrix3D();

// the laser is rotated 90 degree and position at the top left based compared to the realTablematrix
var laserMatrix = new Matrix3D();

// rotate and translate the laser matrix into position
// 90 degree doesnt account for the little skew (see comment outside code for explaination on how to fix that)
laserMatrix.Rotate(new Quaternion(new Vector3D(0, 0, 1), -90d)); 
laserMatrix.Translate(new Vector3D(0, realTableDimY, 0d));

// compute the laser dimensions (i used your points you found out)
var laserDimX = 20392d - (-16300d);
var laserDimY = 12746 - (-11409d);

// calculate the ratio to factor in to displace the point to the real value onto the laser matrix
var laserXRatio = laserDimX / realTableDimX;
var laserYRatio = laserDimY / realTableDimY;

// since matrix are 1 to 1 when you compute if you have an equal ratio of 40 in X and 40 in Y then the point at 10,10,0 on
// the real table is correcponding to the point 400,400,0 on the laser matrix. But since it's rotated 90 degree you
// will need to apply the Y ration to the X point after the transform and the X ratio to the Y point to get the actual point.

// sample point on table. Logic can be flipped with laser matrix to do it the other way
var sampleRealTablePoint = new Point3D(450, 300, 0); // roughly centered

// transform the point from one UCS to another is simply multiplying by it's current UCS (matrix it's in)
// to transform it to world UCS then multiply by the inverse of other UCS we want it in (the other matrix)
var sampleRealTablePointWorld = realTableMatrix.Transform(sampleRealTablePoint);

// convert that point into the laser matrix but first create and inverted matrix of the laser matrix
// we copy a matrix to not modify the current one when inverting it
var laserInvertedMatrix = new Matrix3D() * laserMatrix;
laserInvertedMatrix.Invert();

// get the sample point in the world of the laser matrix
var sampleRealTablePointToLaserMatrix = laserInvertedMatrix.Transform(sampleRealTablePointWorld);

// not adjust the X and Y like said earlier
var finalAdjustedPoint = new Point3D(sampleRealTablePointToLaserMatrix.X * laserXRatio, sampleRealTablePointToLaserMatrix.Y * laserYRatio, 0d);

 // this is if you want the point in the world of the laser matrix and not it's offset from the 0,0,0
 // the vector here is the top left corner of your laser matrix (x, y, z)
 var laserWorldFinalPoint = finalAdjustedPoint + new Vector3D(-11409d, -16155d, 0d);

So here you have it. The sample center point on the real table translated to {12203,5947,0} using this code which is pretty much dead center in the laser matrix.

Now for the skew part. This is also pretty simple. this line :

laserMatrix.Rotate(new Quaternion(new Vector3D(0, 0, 1), -90d));

It's all you need to change. What you want is always consider that top left corner is the origin so all you want to do is imagine a straight line going toward the right (on you laser image) and you want to figure out what is the angle between this perfect X axis (vector(1,0,0)) and the vector of the bottom left point which in this case is slightly more than 90 degree (probably between -91 to -95 degree). You want to figure out the angle in Z to achieve it. It doesn't matter if the formula you use return +270 degree instead of -90 degree since it's a matrix it will give the same results.

Here a method that comput angle in degree between 2 vector based of a rotation axis given

public double AngleToInDegree(Vector3D v, Vector3D vector, Vector3D normal)
{
    var dotNormal = Vector3D.DotProduct(normal, Vector3D.CrossProduct(v, vector));

    var dotVector = Vector3D.DotProduct(v, vector);

    var angle = Math.Atan2(dotNormal, dotVector);

    return angle * 180d / Math.PI;
}

a sample usable is :

var angle = AngleToInDegree(new Vector3D(1,0,0), new Vector3D(0,1,0), new Vector3D(0,0,1));

This checks the angle between a perfect X vector and perfect Y vector relative to a perfect Z vector which will give you 90 degree.

易学教程内所有资源均来自网络或用户发布的内容,如有违反法律规定的内容欢迎反馈
该文章没有解决你所遇到的问题?点击提问,说说你的问题,让更多的人一起探讨吧!