How to convert camera extrinsic matrix to SCNCamera position and rotation - ios

I'm trying to achieve Augmented Reality with SceneKit.
I got a intrinsic camera matrix and a extrinsic matrix by estimating pose of a marker, using ARuco (OpenCV augmented reality library).
And I set up the SCNCamera's projectionTransform with parameters of the intrinsic matrix (fovy, aspect, zNear, zFar).
Normally in OpenGL, world coordinate relative to camera coordinate is calculated with ModelView but in SceneKit, there is no things such as modelView.
So I calculated inverse matrix of the extrinsic matrix to get the camera coordinate relative to the world coordinate(the marker coordinate).
And I think I've got correct camera's position by the inverse matrix which contains rotation and translate matrix.
However I cannot get camera's rotation from that.
Do you have any ideas?

SceneKit has the same view matrixes that you've come across in OpenGL, they're just a little hidden until you start toying with shaders. A little too hidden IMO.
You seem to have most of this figured out. The projection matrix comes from your camera projectionTransform, and the view matrix comes from the inverse of your camera matrix SCNMatrix4Invert(cameraNode.transform). In my case everything was in world coordinates making my model matrix a simple identity matrix.
The code I ended up using to get the classic model-view-projection matrix was something like...
let projection = camera.projectionTransform()
let view = SCNMatrix4Invert(cameraNode.transform)
let model = SCNMatrix4Identity
let viewProjection = SCNMatrix4Mult(view, projection)
let modelViewProjection = SCNMatrix4Mult(model, viewProjection)
For some reason I found SCNMatrix4Mult(...) took arguments in a different order than I was expecting (eg; opposite to GLKMatrix4Multiply(...)).
I'm still not 100% on this, so would welcome edits/tips. Using this method I was unable to get the SceneKit MVP matrix (as passed to shader) to match up with that calculated by the code above... but it was close enough for what I needed.

#lock's answer looks good with a couple additions:
(1) access SCNNode worldTransform instead of transform in case the cameraNode is animated or parented:
let view = SCNMatrix4Invert(cameraNode.presentationNode.worldTransform)
(2) the code doesn't account for the view's aspect ratio. e.g., assuming a perspective projection, you'll want to do:
perspMatrix.m11 /= viewportAR; //if using Yfov -> adjust Y`
/* or, */
perspMatrix.m22 *= viewportAR; //if using Xfov -> adjust X`
Where, viewportAR = viewport.width / viewport.height
Another way to do it is to have one node with a rendered delegate in the scene, and retrieve SceneKit’s matrices from that delegate (they are passed as options):
FOUNDATION_EXTERN NSString * const SCNModelTransform;
FOUNDATION_EXTERN NSString * const SCNViewTransform;
FOUNDATION_EXTERN NSString * const SCNProjectionTransform;
FOUNDATION_EXTERN NSString * const SCNNormalTransform;
FOUNDATION_EXTERN NSString * const SCNModelViewTransform;
FOUNDATION_EXTERN NSString * const SCNModelViewProjectionTransform;

Related

How to use OpenCV triangulatePoints and GPS data properly?

I am trying to estimate the 3D position of a world coordinate from 2 frames. The frames are captured with the same camera from different positions. The problem is, the estimation is wrong.
I have
Camera Intrinsic parameters
K = [4708.29296875, 0, 1218.51806640625;
0, 4708.8935546875, 1050.080322265625;
0, 0, 1]
Translation and Rotation data:
Frame X-Coord Y-Coord Z-Coord(altitude) Pitch Roll Yaw
1 353141.23 482097.85 38.678 0.042652439 1.172694124 16.72142499
2 353141.82 482099.69 38.684 0.097542931 1.143224387 16.79931141
Note: GPS data uses cartesian coordinate system (X,Y,Z Coordinates) is in meter units based on British National Grid GPS system.
To get the rotation matrix I used
https://stackoverflow.com/a/56666686/16432598 which is based on http://www.tobias-weis.de/triangulate-3d-points-from-3d-imagepoints-from-a-moving-camera/.
Using above data I calculate Extrinsic Parameters and the Projection Matrices as follows.
Rt0 = [-0.5284449976982357, 0.308213375891041, -0.7910438668806931, 353141.21875;
-0.8478960766271159, -0.2384055118949635, 0.4735346398506075, 482097.84375;
-0.04263950806535898, 0.9209600028339713, 0.3873171123665929, 38.67800140380859]
Rt1 = [-0.4590975294881605, 0.3270290779984009, -0.8260032933114635, 353141.8125;
-0.8830316937622665, -0.2699087096524321, 0.3839326975722462, 482099.6875;
-0.097388326965866, 0.905649640091175, 0.4126914624432091, 38.68399810791016]
P = K * Rt;
P1 = [-2540.030877954028, 2573.365272473235, -3252.513377560185, 1662739447.059914;
-4037.427278644764, -155.5442017945203, 2636.538291686695, 2270188044.171295;
-0.04263950806535898, 0.9209600028339713, 0.3873171123665929, 38.67800140380859]
P2 = [-2280.235105924588, 2643.299156802081, -3386.193495224041, 1662742249.915956;
-4260.36781710715, -319.9665173096691, 2241.257388910372, 2270196732.490808;
-0.097388326965866, 0.905649640091175, 0.4126914624432091, 38.68399810791016]
triangulatePoints(Points2d, projection_matrices, out);
Now, I pick the same point in both images for triangulation
p2d_1(205,806) and p2d_2(116,813)
For the 3D position of this particular point I expect something like;
[353143.7, 482130.3, 40.80]
whereas I calculate
[549845.5109014747, -417294.6070425579, -201805.410744677]
I know that my Intrinsic parameters and GPS data is very accurate.
Can anybody tell me what is missing or what do I do wrong here?
Thanks

How to use ARCamera’s Projection Matrix to de-skew image?

I wanted to de skew the image(snapshot of ARSCNView) using the values we get in projectionMatrix, or eulerAngles or transform properties of ARCamera. Or is there any other way to de skew image using any values we get from ARKit? My assumption is that projectionMatrix gives us perspective distortion values.
Can we de skew it without selection four points? Any pointers in right direction will be of great help.
Here is one of my try using values of projectionMatrix and eulerAngles:
func deskewImage(camera: ARCamera) {
let rotationZ = CGFloat(camera.eulerAngles.z)
var transform = CATransform3DIdentity
transform.m34 = CGFloat(camera.projectionMatrix.columns.2.z)
transform = CATransform3DRotate(transform, rotationZ, 0, 0, 1)
self.imageView.layer.transform = transform
}
I was able to de-skew image using CIPerspeciveCorrection using rectangle detection (just like CamScanner apps) but it de-skews only detected rectangle and crops it. I want complete image to be de skewed just like shown in this example without selecting four points.
I am looking for the approach using ARKit as values are already threre.

How to convert TangoXyxIjData into a matrix of z-values

I am currently using a Project Tango tablet for robotic obstacle avoidance. I want to create a matrix of z-values as they would appear on the Tango screen, so that I can use OpenCV to process the matrix. When I say z-values, I mean the distance each point is from the Tango. However, I don't know how to extract the z-values from the TangoXyzIjData and organize the values into a matrix. This is the code I have so far:
public void action(TangoPoseData poseData, TangoXyzIjData depthData) {
byte[] buffer = new byte[depthData.xyzCount * 3 * 4];
FileInputStream fileStream = new FileInputStream(
depthData.xyzParcelFileDescriptor.getFileDescriptor());
try {
fileStream.read(buffer, depthData.xyzParcelFileDescriptorOffset, buffer.length);
fileStream.close();
} catch (IOException e) {
e.printStackTrace();
}
Mat m = new Mat(depthData.ijRows, depthData.ijCols, CvType.CV_8UC1);
m.put(0, 0, buffer);
}
Does anyone know how to do this? I would really appreciate help.
The short answer is it can't be done, at least not simply. The XYZij struct in the Tango API does not work completely yet. There is no "ij" data. Your retrieval of buffer will work as you have it coded. The contents are a set of X, Y, Z values for measured depth points, roughly 10000+ each callback. Each X, Y, and Z value is of type float, so not CV_8UC1. The problem is that the points are not ordered in any way, so they do not correspond to an "image" or xy raster. They are a random list of depth points. There are ways to get them into some xy order, but it is not straightforward. I have done both of these:
render them to an image, with the depth encoded as color, and pull out the image as pixels
use the model/view/perspective from OpenGL and multiply out the locations of each point and then figure out their screen space location (like OpenGL would during rendering). Sort the points by their xy screen space. Instead of the calculated screen-space depth just keep the Z value from the original buffer.
or
wait until (if) the XYZij struct is fixed so that it returns ij values.
I too wish to use Tango for object avoidance for robotics. I've had some success by simplifying the use case to be only interested in the distance of any object located at the center view of the Tango device.
In Java:
private Double centerCoordinateMax = 0.020;
private TangoXyzIjData xyzIjData;
final FloatBuffer xyz = xyzIjData.xyz;
double cumulativeZ = 0.0;
int numberOfPoints = 0;
for (int i = 0; i < xyzIjData.xyzCount; i += 3) {
float x = xyz.get(i);
float y = xyz.get(i + 1);
if (Math.abs(x) < centerCoordinateMax &&
Math.abs(y) < centerCoordinateMax) {
float z = xyz.get(i + 2);
cumulativeZ += z;
numberOfPoints++;
}
}
Double distanceInMeters;
if (numberOfPoints > 0) {
distanceInMeters = cumulativeZ / numberOfPoints;
} else {
distanceInMeters = null;
}
Said simply this code is taking the average distance of a small square located at the origin of x and y axes.
centerCoordinateMax = 0.020 was determined to work based on observation and testing. The square typically contains 50 points in ideal conditions and fewer when held close to the floor.
I've tested this using version 2 of my tango-caminada application and the depth measuring seems quite accurate. Standing 1/2 meter from a doorway I slid towards the open door and the distance changed form 0.5 meters to 2.5 meters which is the wall at the end of the hallway.
Simulating a robot being navigated I moved the device towards a trash can in the path until 0.5 meters separation and then rotated left until the distance was more than 0.5 meters and proceeded forward. An oversimplified simulation, but the basis for object avoidance using Tango depth perception.
You can do this by using camera intrinsics to convert XY coordinates to normalized values -- see this post - Google Tango: Aligning Depth and Color Frames - it's talking about texture coordinates but it's exactly the same problem
Once normalized, move to screen space x[1280,720] and then the Z coordinate can be used to generate a pixel value for openCV to chew on. You'll need to decide how to color pixels that don't correspond to depth points on your own, and advisedly, before you use the depth information to further colorize pixels.
The main thing is to remember that the raw coordinates returned are already using the basis vectors you want, i.e. you do not want the pose attitude or location

problems in 3D reconstruction by triangulation opencv

I'm currently working in an stereo vision project, in which I'm supposed to reconstruct 3D points from correspondences found in each camera view, and for that I'm using OpenCV 2.4.7 for C++.
I was able to correctly calibrate both cameras, compute fundamental matrix, compute re-projection matrix and also rectify images.
My problem lies on the final part of the project, which is compute 3D world coordinates from 2D point correspondences. I already tried using cv::triangulatePoints, but the results where points with coordinates (0, 0, 0), no matter what the input points were. I also tried the linear triangulation algorithm by Hartley & Strum, but that didn't give me good results either.
Could somebody give me a hint on what function I should use? Or maybe some tips on how to correctly implement the ones I've talked about. My biggest problem is to find good documentation on the internet, so that's why I decided to ask here.
Thank you!
I tried cv::triangulatePoints also and it calculates garbage. I was forced to implement a linear triangulation method manually, which returns the triangulated 3D point given a stereo pixel correspondence:
Mat triangulate_Linear_LS(Mat mat_P_l, Mat mat_P_r, Mat warped_back_l, Mat warped_back_r)
{
Mat A(4,3,CV_64FC1), b(4,1,CV_64FC1), X(3,1,CV_64FC1), X_homogeneous(4,1,CV_64FC1), W(1,1,CV_64FC1);
W.at<double>(0,0) = 1.0;
A.at<double>(0,0) = (warped_back_l.at<double>(0,0)/warped_back_l.at<double>(2,0))*mat_P_l.at<double>(2,0) - mat_P_l.at<double>(0,0);
A.at<double>(0,1) = (warped_back_l.at<double>(0,0)/warped_back_l.at<double>(2,0))*mat_P_l.at<double>(2,1) - mat_P_l.at<double>(0,1);
A.at<double>(0,2) = (warped_back_l.at<double>(0,0)/warped_back_l.at<double>(2,0))*mat_P_l.at<double>(2,2) - mat_P_l.at<double>(0,2);
A.at<double>(1,0) = (warped_back_l.at<double>(1,0)/warped_back_l.at<double>(2,0))*mat_P_l.at<double>(2,0) - mat_P_l.at<double>(1,0);
A.at<double>(1,1) = (warped_back_l.at<double>(1,0)/warped_back_l.at<double>(2,0))*mat_P_l.at<double>(2,1) - mat_P_l.at<double>(1,1);
A.at<double>(1,2) = (warped_back_l.at<double>(1,0)/warped_back_l.at<double>(2,0))*mat_P_l.at<double>(2,2) - mat_P_l.at<double>(1,2);
A.at<double>(2,0) = (warped_back_r.at<double>(0,0)/warped_back_r.at<double>(2,0))*mat_P_r.at<double>(2,0) - mat_P_r.at<double>(0,0);
A.at<double>(2,1) = (warped_back_r.at<double>(0,0)/warped_back_r.at<double>(2,0))*mat_P_r.at<double>(2,1) - mat_P_r.at<double>(0,1);
A.at<double>(2,2) = (warped_back_r.at<double>(0,0)/warped_back_r.at<double>(2,0))*mat_P_r.at<double>(2,2) - mat_P_r.at<double>(0,2);
A.at<double>(3,0) = (warped_back_r.at<double>(1,0)/warped_back_r.at<double>(2,0))*mat_P_r.at<double>(2,0) - mat_P_r.at<double>(1,0);
A.at<double>(3,1) = (warped_back_r.at<double>(1,0)/warped_back_r.at<double>(2,0))*mat_P_r.at<double>(2,1) - mat_P_r.at<double>(1,1);
A.at<double>(3,2) = (warped_back_r.at<double>(1,0)/warped_back_r.at<double>(2,0))*mat_P_r.at<double>(2,2) - mat_P_r.at<double>(1,2);
b.at<double>(0,0) = -((warped_back_l.at<double>(0,0)/warped_back_l.at<double>(2,0))*mat_P_l.at<double>(2,3) - mat_P_l.at<double>(0,3));
b.at<double>(1,0) = -((warped_back_l.at<double>(1,0)/warped_back_l.at<double>(2,0))*mat_P_l.at<double>(2,3) - mat_P_l.at<double>(1,3));
b.at<double>(2,0) = -((warped_back_r.at<double>(0,0)/warped_back_r.at<double>(2,0))*mat_P_r.at<double>(2,3) - mat_P_r.at<double>(0,3));
b.at<double>(3,0) = -((warped_back_r.at<double>(1,0)/warped_back_r.at<double>(2,0))*mat_P_r.at<double>(2,3) - mat_P_r.at<double>(1,3));
solve(A,b,X,DECOMP_SVD);
vconcat(X,W,X_homogeneous);
return X_homogeneous;
}
the input parameters are two 3x4 camera projection matrices and the left/right corresponding homogeneous pixel coordinates.

Xna SpriteBatch Matrix.Decompose()

What I am looking to do is to be able to push and pop transform matrices into SpriteBatch. I have 2 sprites, parent and child, and the child should be scaled, rotated, translated relative to the parent.
I currently have an implementation using textured quads but I think this should be possible using the built in SpriteBatch class and using Matrix.Decompose(). I'm not sure how to pass the decomposed values to SpriteBatch once I've decomposed them though.
I understand how to keep the matrix stack, I'm just looking for an example of using values coming from Matrix.Decompose() in conjunction with SpriteBatch.
Figured this one out myself finally. Most of the credit belongs to this blog post though.
You can use this method to decompose your matrix:
private void DecomposeMatrix(ref Matrix matrix, out Vector2 position, out float rotation, out Vector2 scale)
{
Vector3 position3, scale3;
Quaternion rotationQ;
matrix.Decompose(out scale3, out rotationQ, out position3);
Vector2 direction = Vector2.Transform(Vector2.UnitX, rotationQ);
rotation = (float)Math.Atan2((double)(direction.Y), (double)(direction.X));
position = new Vector2(position3.X, position3.Y);
scale = new Vector2(scale3.X, scale3.Y);
}
You can then build a transform matrix your leaf sprites like so:
Matrix transform =
Matrix.CreateScale(new Vector3(this.Scale, 1.0f)) *
Matrix.CreateRotationZ(this.Rotation) *
Matrix.CreateTranslation(new Vector3(this.Position, 0));
For your parent sprites, include the origin:
Matrix transform =
Matrix.CreateTranslation(new Vector3(-this.Origin, 0)) *
Matrix.CreateScale(new Vector3(this.Scale, 1.0f)) *
Matrix.CreateRotationZ(this.Rotation) *
Matrix.CreateTranslation(new Vector3(this.position, 0));
Multiply by all the matrices in the stack in reverse order.

Resources