Opengl: Keeping Arcball camera up-vector alligned with y-axis - c++

I'm essentially trying to mimic the way the camera rotates in Maya. The arcball in Maya is always aligned with the with the y-axis. So no matter where the up-vector is pointing, it's still rotated or registered with it's up-vector along the y-axis.
I've been able to implement is arcball in OpenGL using C++ and Qt. But I can't figure out how to keep it's up-vector aligned. I've been able to keep it aligned at times by my code below:
void ArcCamera::setPos (Vector3 np)
{
Vector3 up(0, 1, 0);
Position = np;
ViewDir = (ViewPoint - Position); ViewDir.normalize();
RightVector = ViewDir ^ up; RightVector.normalize();
UpVector = RightVector ^ ViewDir; UpVector.normalize();
}
This works up until the position is at 90-degrees, then the right vector changes and everything is inverted.
So instead I've been maintaining the total rotation (in quaternions) and rotating the original positions (up, right, pos) by it. This works best to keep everything coherent, but now I simply can't align the up-vector to the y-axis. Below is the function for the rotation.
void CCamera::setRot (QQuaternion q)
{
tot = tot * q;
Position = tot.rotatedVector(PositionOriginal);
UpVector = tot.rotatedVector(UpVectorOriginal);
UpVector.normalize();
RightVector = tot.rotatedVector(RightVectorOriginal);
RightVector.normalize();
}
The QQuaternion q is generated from the axis-angle pair derived from the mouse drag. I'm confident this is done correctly. The rotation itself is fine, it just doesn't keep the orientation aligned.
I've noticed in my chosen implementation, dragging in the corners provides a rotation around my view direction, and I can always realign the up-vector to straighten out to the world's y-axis direction. So If I could figure out how much to roll I could probably do two rotations each time to make sure it's all straight. However, I'm not sure how to go about this.

The reason this isn't working is because Maya's camera manipulation in the viewport does not use an arcball interface. What you want to do is Maya's tumble command. The best resource I've found for explaining this is this document from Professor Orr's Computer Graphics class.
Moving the mouse left and right corresponds to the azimuth angle, and specifies a rotation around the world space Y axis. Moving the mouse up and down corresponds to the elevation angle, and specifies a rotation around the view space X axis. The goal is to generate the new world-to-view matrix, then extract the new camera orientation and eye position from that matrix, based on however you've parameterized your camera.
Start with the current world-to-view matrix. Next, we need to define the pivot point in world space. Any pivot point will work to begin with, and it can be simplest to use the world origin.
Recall that pure rotation matrices generate rotations centered around the origin. This means that to rotate around an arbitrary pivot point, you first translate to the origin, perform the rotation, and translate back. Remember also that transformation composition happens from right to left, so the negative translation to get to the origin goes on the far right:
translate(pivotPosition) * rotate(angleX, angleY, angleZ) * translate(-pivotPosition)
We can use this to calculate the azimuth rotation component, which is a rotation around the world Y axis:
azimuthRotation = translate(pivotPosition) * rotateY(angleY) * translate(-pivotPosition)
We have to do a little additional work for the elevation rotation component, because it happens in view space, around the view space X axis:
elevationRotation = translate(worldToViewMatrix * pivotPosition) * rotateX(angleX) * translate(worldToViewMatrix * -pivotPosition)
We can then get the new view matrix with:
newWorldToViewMatrix = elevationRotation * worldToViewMatrix * azimuthRotation
Now that we have the new worldToView matrix, we're left with having to extract the new world space position and orientation from the view matrix. To do this, we want the viewToWorld matrix, which is the inverse of the worldToView matrix.
newOrientation = transpose(mat3(newWorldToViewMatrix))
newPosition = -((newOrientation * newWorldToViewMatrix).column(3))
At this point, we have the elements separated. If your camera is parameterized so that you're only storing a quaternion for your orientation, you just need to do the rotation matrix -> quaternion conversion. Of course, Maya is going to convert to Euler angles for display in the channel box, which will be dependent on the camera's rotation order (note that the math for tumbling doesn't change when the rotation order changes, just the way that the rotation matrix -> Euler angles conversion is done).
Here's a sample implementation in Python:
#!/usr/bin/env python
import numpy as np
from math import *
def translate(amount):
'Make a translation matrix, to move by `amount`'
t = np.matrix(np.eye(4))
t[3] = amount.T
t[3, 3] = 1
return t.T
def rotateX(amount):
'Make a rotation matrix, that rotates around the X axis by `amount` rads'
c = cos(amount)
s = sin(amount)
return np.matrix([
[1, 0, 0, 0],
[0, c,-s, 0],
[0, s, c, 0],
[0, 0, 0, 1],
])
def rotateY(amount):
'Make a rotation matrix, that rotates around the Y axis by `amount` rads'
c = cos(amount)
s = sin(amount)
return np.matrix([
[c, 0, s, 0],
[0, 1, 0, 0],
[-s, 0, c, 0],
[0, 0, 0, 1],
])
def rotateZ(amount):
'Make a rotation matrix, that rotates around the Z axis by `amount` rads'
c = cos(amount)
s = sin(amount)
return np.matrix([
[c,-s, 0, 0],
[s, c, 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1],
])
def rotate(x, y, z, pivot):
'Make a XYZ rotation matrix, with `pivot` as the center of the rotation'
m = rotateX(x) * rotateY(y) * rotateZ(z)
I = np.matrix(np.eye(4))
t = (I-m) * pivot
m[0, 3] = t[0, 0]
m[1, 3] = t[1, 0]
m[2, 3] = t[2, 0]
return m
def eulerAnglesZYX(matrix):
'Extract the Euler angles from an ZYX rotation matrix'
x = atan2(-matrix[1, 2], matrix[2, 2])
cy = sqrt(1 - matrix[0, 2]**2)
y = atan2(matrix[0, 2], cy)
sx = sin(x)
cx = cos(x)
sz = cx * matrix[1, 0] + sx * matrix[2, 0]
cz = cx * matrix[1, 1] + sx * matrix[2, 1]
z = atan2(sz, cz)
return np.array((x, y, z),)
def eulerAnglesXYZ(matrix):
'Extract the Euler angles from an XYZ rotation matrix'
z = atan2(matrix[1, 0], matrix[0, 0])
cy = sqrt(1 - matrix[2, 0]**2)
y = atan2(-matrix[2, 0], cy)
sz = sin(z)
cz = cos(z)
sx = sz * matrix[0, 2] - cz * matrix[1, 2]
cx = cz * matrix[1, 1] - sz * matrix[0, 1]
x = atan2(sx, cx)
return np.array((x, y, z),)
class Camera(object):
def __init__(self, worldPos, rx, ry, rz, coi):
# Initialize the camera orientation. In this case the original
# orientation is built from XYZ Euler angles. orientation is the top
# 3x3 XYZ rotation matrix for the view-to-world matrix, and can more
# easily be thought of as the world space orientation.
self.orientation = \
(rotateZ(rz) * rotateY(ry) * rotateX(rx))
# position is a point in world space for the camera.
self.position = worldPos
# Construct the world-to-view matrix, which is the inverse of the
# view-to-world matrix.
self.view = self.orientation.T * translate(-self.position)
# coi is the "center of interest". It defines a point that is coi
# units in front of the camera, which is the pivot for the tumble
# operation.
self.coi = coi
def tumble(self, azimuth, elevation):
'''Tumble the camera around the center of interest.
Azimuth is the number of radians to rotate around the world-space Y axis.
Elevation is the number of radians to rotate around the view-space X axis.
'''
# Find the world space pivot point. This is the view position in world
# space minus the view direction vector scaled by the center of
# interest distance.
pivotPos = self.position - (self.coi * self.orientation.T[2]).T
# Construct the azimuth and elevation transformation matrices
azimuthMatrix = rotate(0, -azimuth, 0, pivotPos)
elevationMatrix = rotate(elevation, 0, 0, self.view * pivotPos)
# Get the new view matrix
self.view = elevationMatrix * self.view * azimuthMatrix
# Extract the orientation from the new view matrix
self.orientation = np.matrix(self.view).T
self.orientation.T[3] = [0, 0, 0, 1]
# Now extract the new view position
negEye = self.orientation * self.view
self.position = -(negEye.T[3]).T
self.position[3, 0] = 1
np.set_printoptions(precision=3)
pos = np.matrix([[5.321, 5.866, 4.383, 1]]).T
orientation = radians(-60), radians(40), 0
coi = 1
camera = Camera(pos, *orientation, coi=coi)
print 'Initial attributes:'
print np.round(np.degrees(eulerAnglesXYZ(camera.orientation)), 3)
print np.round(camera.position, 3)
print 'Attributes after tumbling:'
camera.tumble(azimuth=radians(-40), elevation=radians(-60))
print np.round(np.degrees(eulerAnglesXYZ(camera.orientation)), 3)
print np.round(camera.position, 3)

Keep track of you view and right vectors, from the beginning and update them with the rotation matrix. Then calculate your up vector.

Related

PTB-OpengGL stereo rendering and eye seperation value

I tried to draw a 3D dot cloud using OpenGL asymmetric frustum parallel axis projection. The general principle can be found on this website(http://paulbourke.net/stereographics/stereorender/#). But the problem now is that when I use real eye separation(0.06m), my eyes do not fuse well. When using eye separation = 1/30 * focal length, there is no pressure. I don’t know if there is a problem with the calculation, or there is a problem with the parameters? Part of the code is posted below. Thank you all.
for view = 0:stereoViews
% Select 'view' to render (left- or right-eye):
Screen('SelectStereoDrawbuffer', win, view);
% Manually reenable 3D mode in preparation of eye draw cycle:
Screen('BeginOpenGL', win);
% Set the eye seperation:
eye = 0.06; % in meter
% Caculate the frustum shift at the near plane:
fshift = 0.5 * eye * depthrangen/(vdist/100); % vdist is the focal length, 56cm, 0.56m
right_near = depthrangen * tand(FOV/2); % depthrangen is the depth of the near plane, 0.4. %FOV is the field of view, 18°
left_near = -right_near;
top_near = right_near* aspectr;
bottom_near = -top_near;
% Setup frustum projection for this eyes 'view':
glMatrixMode(GL.PROJECTION)
glLoadIdentity;
eyeside = 1+(-2*view); % 1 for left eye, -1 for right eye
glFrustum(left_near + eyeside * fshift, right_near + eyeside * fshift, bottom_near, top_near, %depthrangen, depthrangefObj);
% Setup camera for this eyes 'view':
glMatrixMode(GL.MODELVIEW);
glLoadIdentity;
gluLookAt(0 - eyeside * 0.5 * eye, 0, 0, 0 - eyeside * 0.5 * eye, 0, -1, 0, 1, 0);
% Clear color and depths buffers:
glClear;
moglDrawDots3D(win, xyz(:,:,iframe), 10, [], [], 1);
moglDrawDots3D(win, xyzObj(:,:,iframe), 10, [], [], 1);
% Manually disable 3D mode before calling Screen('Flip')!
Screen('EndOpenGL', win);
% Repeat for other eyes view if in stereo presentation mode...
end

How to determine the intersection between the camera direction and a plane?

I have a 3D scene with an infinite horizontal plane (parallel to the xz coordinates) at a height H along the Y vertical axis.
I would like to know how to determine the intersection between the axis of my camera and this plane.
The camera is defined by a view-matrix and a projection-matrix.
There are two sub-problems here: 1) Extracting the position and view-direction from the camera matrix. 2) Calculating the intersection between the view-ray and the plane.
Extracting position and view-direction
The view matrix describes how points are transformed from world-space to view space. The view-space in OpenGL is usually defined such that the camera is in the origin and looks into the -z direction.
To get the position of the camera, we have to transform the origin [0,0,0] of the view-space back into world-space. Mathematically speaking, we have to calculate:
camera_pos_ws = inverse(view_matrix) * [0,0,0,1]
but when looking at the equation we'll see that we are only interrested in the 4th column of the inverse matrix which will contain 1
camera_pos_ws = [-view_matrix[12], -view_matrix[13], -view_matrix[14]]
The orientation of the camera can be found by a similar calculation. We know that the camera looks in -z direction in view-space thus the world space direction is given by
camera_dir_ws = inverse(view_matrix) * [0,0,-1,0];
Again, when looking at the equation, we'll see that this only takes the third row of the inverse matrix into account which is given by2
camera_dir_ws = [-view_matrix[2], -view_matrix[6], -view_matrix[10]]
Calculating the intersection
We now know the camera position P and the view direction D, thus we have to find the x,z value along the ray R(x,y,z) = P + l * D where y equals H. Since there is only one unknown, l, we can calculate that from
y = Py + l * Dy
H = Py + l * Dy
l = (H - Py) / Dy
The intersection point is then given by pasting l back into the ray equation.
Notes
1 The indices assume that the matrix is stored in a column-major linear array.
2 Note, that the inverse of a matrix of the form
M = [ R T ]
0 1
, where R is a orthogonal 3x3 matrix, is given by
inv(M) = [ transpose(R) -T ]
0 1
For a general line-plane intersection there are lot of answers and tutorials.
Your case is simple due to the plane is horizontal.
I suppose the camera is at C(cx, cy, cz) and it looks at T(tx, ty,tz).
Then the line camera-target can be defined by:
cx - x cy - y cz - z
------ = ------ = ------ /// These are two independant equations
tx - cx ty - cy tz - cz
For a horizontal plane, only a equation is needed: y = H.
Substitute this value in the line equations and you get
(cx-x)/(tx-cx) = (cy-H)/(ty-cy)
(cz-z)/(tz-cz) = (cy-H)/(ty-cy)
So
x = cx - (tx-cx)*(cy-H)/(ty-cy)
y = H
z = cz - (tz-cz)*(cy-H)/(ty-cy)
Of course if your camera looks in an also horizontal line then ty=cy and there is not solution.

Aruco markers with openCv, get the 3d corner coordinates?

I am detecting a printed Aruco marker using opencv 3.2:
aruco::estimatePoseSingleMarkers(corners, markerLength, camMatrix, distCoeffs, rvecs,tvecs);
this returns a translation and rotation vector for the marker. What I need, is the 3d coordinates for each corner of the marker.
As i know the marker length, i could do something like
corner1 = tvecs[0] - markerlength /2;
corner2 = tvecs[0] + markerlength /2;
....
But is there an better way? Or an existing function?
To sum up, I have:
a 3d point in the center of a 2d square.
the length of the sides of that square.
the rotation value of the square.
How can I find the 3d coordinates of the corners?
First, let's assume that we only have one marker given with side = 2 * half_side.
Second, aruco::detectMarker returns the relative position of the camera in the marker's world. Thus, I assume that you are looking for the coordinates of the corners in camera's world.
Then, in marker's space:
[ half_side ] [ 0 ]
E = [ 0 ], F = [ half_side ]
[ 0 ] [ 0 ]
where the center O of the square has coordinate tvec (in camera's world) and rotation mat of the marker rot_mat is computed by cv::Rodrigues(rvec,rot_mat).
Now, using the pinhole camera model, the relation between coordinates of a point P in cam's world and marker's world is:
[P_x_cam] [P_x_marker]
[P_y_cam] = rot_mat * [P_y_marker] + tvec
[P_z_cam] [P_z_marker]
for example, the center O, which is [0,0,0] in marker's world, is tvec in cam's world.
So, the coordinates of E in cam's world are:
[E_x_cam] [half_side]
|E_y_cam| = rot_mat * | 0 | + tvec
[E_z_cam] [ 0 ]
Magically, it is the sum of rot_mat's first column multiplied by half_size and tvec. Similarly,
the coodinates of F is rot_mat's second column multiplied by half_size and tvec.
Now, the corners can be computed, for example
C - O = (E - O) + (F - O), B - O = (E - O) - (F - O)
where E-O is exactly rot_mat's first column multiplied by half_size.
With all that in mind, we can compose the function:
vector<Point3f> getCornersInCameraWorld(double side, Vec3d rvec, Vec3d tvec){
double half_side = side/2;
// compute rot_mat
Mat rot_mat;
Rodrigues(rvec, rot_mat);
// transpose of rot_mat for easy columns extraction
Mat rot_mat_t = rot_mat.t();
// the two E-O and F-O vectors
double * tmp = rot_mat_t.ptr<double>(0);
Point3f camWorldE(tmp[0]*half_side,
tmp[1]*half_side,
tmp[2]*half_side);
tmp = rot_mat_t.ptr<double>(1);
Point3f camWorldF(tmp[0]*half_side,
tmp[1]*half_side,
tmp[2]*half_side);
// convert tvec to point
Point3f tvec_3f(tvec[0], tvec[1], tvec[2]);
// return vector:
vector<Point3f> ret(4,tvec_3f);
ret[0] += camWorldE + camWorldF;
ret[1] += -camWorldE + camWorldF;
ret[2] += -camWorldE - camWorldF;
ret[3] += camWorldE - camWorldF;
return ret;
}
Note 1: I hate that SO doesn't have MathJax
Note 2: there must be some faster implementation which I don't know of.
A python implementation I wrote for the above described rotation of marker corners using rvec and tvec as returned from cv2.aruco.estimatePoseSingleMarkers(). Thanks #Quang Hoang for the detailed explanation.
import numpy as np
# rotate a markers corners by rvec and translate by tvec if given
# input is the size of a marker.
# In the markerworld the 4 markercorners are at (x,y) = (+- markersize/2, +- markersize/2)
# returns the rotated and translated corners and the rotation matrix
def rotate_marker_corners(rvec, markersize, tvec = None):
mhalf = markersize / 2.0
# convert rot vector to rot matrix both do: markerworld -> cam-world
mrv, jacobian = cv2.Rodrigues(rvec)
#in markerworld the corners are all in the xy-plane so z is zero at first
X = mhalf * mrv[:,0] #rotate the x = mhalf
Y = mhalf * mrv[:,1] #rotate the y = mhalf
minusX = X * (-1)
minusY = Y * (-1)
# calculate 4 corners of the marker in camworld. corners are enumerated clockwise
markercorners = []
markercorners.append(np.add(minusX, Y)) #was upper left in markerworld
markercorners.append(np.add(X, Y)) #was upper right in markerworld
markercorners.append(np.add( X, minusY)) #was lower right in markerworld
markercorners.append(np.add(minusX, minusY)) #was lower left in markerworld
# if tvec given, move all by tvec
if tvec is not None:
C = tvec #center of marker in camworld
for i, mc in enumerate(markercorners):
makercorners[i] = np.add(C,mc) #add tvec to each corner
#print('Vec X, Y, C, dot(X,Y)', X,Y,C, np.dot(X,Y)) # just for debug
markercorners = np.array(markercorners,dtype=np.float32) # type needed when used as input to cv2
return markercorners, mrv
'''
Copyright 2019 Marco Noll, Garmin International Inc. 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.
'''
Building upon #Quang's answer, C# code for transforming any point to camera coordinates. Of course it needs R and t vectors, so you're going to need a marker in order to get them.
private Point3d GetWorldPoint(Point3d input, Vec3d rvec, Vec3d tvec)
{
var rot_mat = new Mat();
Cv2.Rodrigues(MatOfDouble.FromArray(rvec.Item0, rvec.Item1, rvec.Item2), rot_mat);
var pointProject = (rot_mat * MatOfDouble.FromArray(input.X, input.Y, input.Z)).ToMat();
return tvec + new Point3d(pointProject.Get<double>(0, 0), pointProject.Get<double>(0, 1), pointProject.Get<double>(0, 2));
}

A function for spinning objects around a center point

I am trying to code a game in C++ using SFML, in which several space ships attack a planet from different sides. I want the space ships that attack the planet to face the planet while they attack (obviously). The problem I am having is figuring out a formula that would work for every x and y position. If the sprite for the ship starts off by facing upwards, what would be the best way to approach this?
For example, if the ship spawns on the right side of the planet, the ship sprite should rotate to the left, towards the center of the planet.
Below is the math for rotation, which is what you seem to be asking for, however you're probably better off simply using SFML's api for rotating entities: http://www.sfml-dev.org/tutorials/2.0/graphics-transform.php.
Rotating a point around the origin can be represented as a matrix multiplication with the following matrix:
[[ cos(a), sin(a), 0],
[-sin(a), cos(a), 0],
[ 0, 0, 1]]
where a is the angle to rotate by.
(This can be derived from the trig identities cos(a)sin(b) + cos(b)sin(a) = sin(a + b) and cos(a)cos(b) - sin(a)sin(b) = cos(a + b))
Translating a point so that it is positioned where you want to rotate relative to the origin can also be represented as a matrix multiplication, and of course translating it back can be as well.
To translate a point by a in the horizontal direction and b in the vertical direction you multiply your point [x, y, 1] by the matrix
[[1, 0, 0],
[0, 1, 0],
[a, b, 1]]
This produces the matrix [x+a, y+b, 1], which is the translated point (x+a, y+b).
To translate a point in preparation for rotation around a point (Cx, Cy), you first translate it by -Cx in the horizontal direction and -Cy in the vertical direction. The matrix for that is [[1, 0, 0], [0, 1, 0], [-Cx, -Cy, 1]]. After the rotation you translate by the same amounts in the opposite directions, which is the same as multiplying by the inverse matrix: [[1, 0, 0], [0, 1, 0], [Cx, Cy, 1]].
Say your initial point is p, a matrix A translates it to be relative to the origin for rotation, a matrix R rotates about the origin. Then your rotated point is
((p × A) × R) × A-1
Since matrix multiplication is associative you can rearrange this to:
p × ((A × R) × A-1)
Which means you can compute the matrix ARA-1 and then multiply any point by that matrix to rotate that point around a common center. So for each object you want to rotate, compute the matrix that rotates points around the object's center point, and then rotate every point of that object using the matrix.
When you combine the translation matrices with the rotation matrix you get a single matrix:
[[ cos(a), sin(a), 0],
[ -sin(a), cos(a), 0],
[-cos(a) * Cx + Cx + Cy * sin(a), -cos(a) * Cy + Cy - Cx * sin(a), 1]]
Multiplying a point (x, y) by this matrix produces a point with the x coordinate:
x × cos(a) - y × sin(a) + (-cos(a) × Cx + Cx + Cy × sin(a))
and the y coordinate:
x × sin(a) + y × cos(a) + (-cos(a) × Cy + Cy - Cx × sin(a))
To try this out, say we want to rotate points around (1, 1). If we rotate the point (2, 1) by pi/2 (90° counter clockwise) we should get the point (1, 2), and rotating the point (2, 1) should give us (0, 1).
Plugging in the coordinates (2, 1) and the angle pi/2 into the equation for the rotated x coordinates produces:
2 * cos(pi/2) - 1 * sin(pi/2) + -cos(pi/2) * 1 + 1 + 1 * sin(pi/2) = 1
and plugging them into the equation for the y coordinate produces:
2 * sin(pi/2) + 1 * cos(pi/2) + (-cos(pi/2) * 1 + 1 - 1 * sin(pi/2)) = 2
Which means the point (2, 1) rotated 90° around (1, 1) is the point (1, 2), just as expected.
Then pluggin in the point (1, 2):
1 * cos(pi/2) - 2 * sin(pi/2) + -cos(pi/2) * 1 + 1 + 1 * sin(pi/2) = 0
2 * sin(pi/2) + 1 * cos(pi/2) + (-cos(pi/2) * 1 + 1 - 1 * sin(pi/2)) = 1
produces the point (0, 1) which is, again, just as expected.
Denote the position of the planet as (xP,yP) and the position of the ship as (xS,yS) with its point facing upwards, direction (0,1)' (the prime denoting transposition to a column vector). To have the ship pointing at the planet, we need it pointing in the opposite direction of the difference vector
(x,y) = (xS-xP, yS-yP).
More precisely, the normalized direction of the point should be (-x/r,-y/r)', using r=sqrt(x^2+y^2). The rotation of the direction vector (0,1)' to (-x/r,-y/r)' is performed using the rotation matrix
( -y/r -x/r ) ( 0 ) ( -x/r )
( x/r -y/r ) * ( 1 ) = ( -y/r )
for the convention matrix-vector-product. If it is vector-matrix product, use the transpose of it.
Use that if specifying the matrix directly. When using rotation angles, the angle of that rotation is atan2(x,-y) in radians.

OpenCV Computing Camera Position && Rotation

for a project I need to compute the real world position and orientation of a camera
with respect to a known object.
I have a set of photos, each displays a chessboard from different points of view.
Using CalibrateCamera and solvePnP I am able to reproject Points in 2d, to get a AR-thing.
So my situation is as such:
Intrinsic parameters are known
Distortioncoefficients are known
translation Vector and rotation Vector are known per photo.
I simply cannot figure out how to compute the position of the camera. My guess was:
invert translation vector. (=t')
transform rotation vector to degree (seems to be radian) and invert
use rodriguez on rotation vector
compute RotationMatrix * t'
But the results are somehow totally off...
Basically I want to to compute a ray for each pixel in world coordinates.
If more informations on my problem are needed, I'd be glad to answer quickly.
I dont' get it... somehow the rays are still off. This is my Code btw:
Mat image1CamPos = tvecs[0].clone(); //From calibrateCamera
Mat rot = rvecs[0].clone(); //From calibrateCamera
Rodrigues(rot, rot);
rot = rot.t();
//Position of Camera
Mat pos = rot * image1CamPos;
//Ray-Normal (( (double)mk[i][k].x) are known image-points)
float x = (( (double)mk[i][0].x) / fx) - (cx / fx);
float y = (( (double)mk[i][0].y) / fy) - (cy / fy);
float z = 1;
float mag = sqrt(x*x + y*y + z*z);
x /= mag;
y /= mag;
z /= mag;
Mat unit(3, 1, CV_64F);
unit.at<double>(0, 0) = x;
unit.at<double>(1, 0) = y;
unit.at<double>(2, 0) = z;
//Rotation of Ray
Mat rot = stof1 * unit;
But when plotting this, the rays are off :/
The translation t (3x1 vector) and rotation R (3x3 matrix) of an object with respect to the camera equals the coordinate transformation from object into camera space, which is given by:
v' = R * v + t
The inversion of the rotation matrix is simply the transposed:
R^-1 = R^T
Knowing this, you can easily resolve the transformation (first eq.) to v:
v = R^T * v' - R^T * t
This is the transformation from camera into object space, i.e., the position of the camera with respect to the object (rotation = R^T and translation = -R^T * t).
You can simply get a 4x4 homogeneous transformation matrix from this:
T = ( R^T -R^T * t )
( 0 1 )
If you now have any point in camera coordinates, you can transform it into object coordiantes:
p' = T * (x, y, z, 1)^T
So, if you'd like to project a ray from a pixel with coordinates (a,b) (probably you will need to define the center of the image, i.e. the principal point as reported by CalibrateCamera, as (0,0)) -- let that pixel be P = (a,b)^T. Its 3D coordinates in camera space are then P_3D = (a,b,0)^T. Let's project a ray 100 pixel in positive z-direction, i.e. to the point Q_3D = (a,b,100)^T. All you need to do is transform both 3D coordinates into the object coordinate system using the transformation matrix T and you should be able to draw a line between both points in object space. However, make sure that you don't confuse units: CalibrateCamera will report pixel values while your object coordinate system might be defined in, e.g., cm or mm.