What I want to do is to raycast a pointcloud to a 2D image. What I have is a 3D PointCloud and a Viewpoint which is different to the general world coordinate system. I would like to raycast from this Viewpoint to generate a 2D image of the point cloud. So, I just need a method like getintersectedvoxel which is doing the casting for the whole area and not only for a single ray.
That is a projection from 3D to a camera. You can get it with the pinhole camera model equations (as shown here).
You need first 3 parameters that define your camera: the focal length f, and the center of the projection plane: cx, cy. With this you create a 3x3 matrix (I will use matlab syntax):
A = [ f 0 cx;
0 f cy;
0 0 1 ];
You can use something like cx = 0.5 * image_width, cy = 0.5 * image_height, and some value as f = 800 (try some of them to check how the image looks better).
Then, a 3x4 matrix with the transformation from the camera frame to the point cloud frame (you say you have it):
T = [ r11 r12 r13 tx;
r21 r22 r23 ty;
r31 r32 r33 tz ];
And finally, your point cloud in homogeneous coordinates, i.e. in a 4xN matrix for a point cloud with N points:
P = [ x1 x2 ... xN;
y1 y2 ... yN;
z1 z2 ... zN;
1 1 ... 1 ];
Now you can project the points:
S = A * T * P;
S is a 3xN matrix where the pixel coordinates of each i-th 3D point are:
x = S(1, i) / S(3, i);
y = S(2, i) / S(3, i);
Related
I have the following (simplified) code which represents a 3D pose of a robot estimated by visual odometry:
// (r11 r12 r13 tx)
// (r21 r22 r23 ty)
// (r31 r32 r33 tz)
// (0 0 0 1)
Eigen::Matrix4f pose_prev_to_cur;
Eigen::Matrix4f pose_cur_to_world;
while (1) {
pose_prev_to_cur = ... /* from visual odometry */
pose_cur_to_world = pose_cur_to_world * pose_prev_to_cur.inverse();
}
As you can see, the pose matrix contains a 3x3 3D rotation matrix and a 3x1 translation vector.
I am plotting the robot trajectory by creating a point on a map for every frame:
// x translation maps to 2D x axis
int x = WINDOW_SCALE * pose_cur_to_world(0, 3) + WINDOW_SIZE / 2;
// z translation maps to 2D y axis
int y = WINDOW_SCALE * pose_cur_to_world(2, 3) + WINDOW_SIZE / 2;
// plotting the point
cv::circle(traj, cv::Point(x, y), 1, cv::Scalar(0, 255, 0), 2);
This works well for me and the trajectory is plotted, as you can see in the image:
The trajectory is green, the current robot position is red.
Now I have depth readings for each frame too. For each image pixel obtained from the camera, I have a depth measurement in meters. I'd like to plot the distance in the map as well.
My current idea is to just 'add' the depth value to the z translation value of the pose matrix like so:
Eigen::Matrix4f pose_depth_reading = pose_cur_to_world;
// offset z location by depth reading in meters
pose_depth_reading(2, 3) += 2.0;
Now my question is: how to plot this new point in the map?
If I just plot the point using the x/z coordinates of the 3D points, it will obviously not be rotated properly, as it will just have an offset on the Y axis.
I thought about taking the calculated 2D map point and offsetting its y coordinate by the depth reading and then do 2D rotation about the origin instead. But my problem is I'd need to acquire the 2D rotation angle from the 3x3 3D rotation matrix in pose_cur_to_world and I'm not quite sure how to do that.
The end result I expect is shown in the following image:
The depth reading(s) are marked blue and I added a gray coordinate frame with X and Y axis for brevity.
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.
The following link suggests that we can convert dual fisheye coordinates to equirectangular coordinates using the following equations:
// 2D fisheye to 3D vector
phi = r * aperture / 2
theta = atan2(y, x)
// 3D vector to longitude/latitude
longitude = atan2(Py, Px)
latitude = atan2(Pz, (Px^2 + Py^2)^(0.5))
// 3D vector to 2D equirectangular
x = longitude / PI
y = 2 * latitude / PI
I applied to above equations to write my source code like this:
const float FOV = 220.0f * PI / 180.0f;
float r = sqrt(u*u + v*v);
float theta = atan2(v, u);
float phi = r * FOV * 0.5f;
float px = u;
float py = r * sin(phi);
float pz = v;
float longitude = atan2(py, px); // theta
float latitude = atan2(pz, sqrt(px*px + py*py)); // phi
x = longitude / PI;
y = 2.0f * latitude / PI;
Unfortunately my math is not good enough to understand this and not sure if I write the above code correctly, where I tried to guess the values for px, py and pz.
Assume my camera FOV is 220 degrees, and the camera resolution is 2880x1440, I would expect the point (358, 224) for rear camera in the overlapped area and the point (2563, 197) for front camera in the overlapped area would both map to a coordinate close to (2205, 1009). However the actual mapping points are (515.966370,1834.647949) and (1644.442017,1853.060669) respectively, which are both very far away from (2205,1009). Please kindly suggest how to fix the above code. Many thanks!
You are building the equirectangular image, so I would suggest you to use the inverse mapping.
Start with pixel locations in the target image you are painting. Convert the 2D location to longitude/latitude. Then convert that to a 3D point on the surface of the unit sphere. Then convert from the 3D point to a location in the 2D fisheye source image. In Paul Bourke page, you would start with the bottom equation, then the rightmost one, then the topmost one.
Use landmark points like 90° long 0° lat, to verify the results make sense at each step.
The final result should be a location in the source fisheye image in the [-1..+1] range. Remap to pixel or to UV as needed. Since the source is split in two eye images you will also need a mapping from target (equirect) longitudes to the correct source sub-image.
I have a 3D scene with a movable camera. I have the 3D coordinates for that camera (x, y, z -> Y being the height) and the X and Y rotation (Up/Down, Left/Right).
I want get the coordinates (x1, z1) in the floor where I'm looking at.
Basically if the camera is at (0, 4096, 0) (4096 is the height) and my xRotation is 45º and my yRotation is 0, I will be looking at the point on the floor (4096, 0, 0)
I was trying to program it but i got stuck with the trigonometry. Help me with it.
The following code is what I have right now and not fully working:
float x1, z1, anguloX, anguloY;
anguloX = (90 - Xrotation) / 180 * Pi;
anguloY = (90 - Yrotation) / 180 * Pi;
x1 = Yposition * tan(anguloX) * cos(anguloY);
z1 = Yposition * tan(anguloY) * cos(anguloY);
x1 += Xposition;
z1 += Zposition;
Do not bother yourself with this kind of trigonometry, It is often more practical and understandable to use matrices and transformation to do this kind of stuff. I suggest to try this approach instead which is called Ray Casting:
Calculate the direction of camera sight, you must get the looking
direction of camera
Use ray casting to determine the intersection point of sight ray and
scene meshes(or just a plane that represents the floor).
Now about the first, here is the pseudo code :
XRotMat = CreateRotationMatrixAroundXAxis(verticalCameraAngel);
YRotMat = CreateRotationMatrixAroundYAxis(horizentalCameraAngel);
CameraSightDir = YRotMat*XRotMa*initialCameraDir;
and about the second step:
SightRay.Source = Camera.Position;
SightRay.Direction = CameraSightDir;
Intersection = IntersectRayWithPlane(SightRay , FloorPlane);
IntersectRayWithPlane is quite a simple procedure, you can read about it here.
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.