I am building and application where I need to switch an IMU on for around 5 seconds, and get a good orientation within the first 1-2 seconds. I am using the Madwick quaternion algorithm and it works relatively well. The problems is, if the sensor is rotated about 180 degrees while it being off, it takes a long time to find this new orientation.
Is there a way to initialise the quaternion values from the IMU sensor (instead of the standard 1, 0, 0, 0) to something close to the correct orientation? I am trying to do this by first calculating roll, pitch and yaw from the imu with the following:
pitch = 180 * atan2(accelX, sqrt(accelY*accelY + accelZ*accelZ))/PI;
roll = 180 * atan2(accelY, sqrt(accelX*accelX + accelZ*accelZ))/PI;
mag_x = magReadX*cos(pitch) + magReadY*sin(roll)*sin(pitch) + magReadZ*cos(roll)*sin(pitch)
mag_y = magReadY * cos(roll) - magReadZ * sin(roll)
yaw = 180 * atan2(-mag_y,mag_x)/M_PI;
and the converting these to quaternions with the following:
double cy = cos(yaw * 0.5);
double sy = sin(yaw * 0.5);
double cp = cos(pitch * 0.5);
double sp = sin(pitch * 0.5);
double cr = cos(roll * 0.5);
double sr = sin(roll * 0.5);
Quaternion q;
q.w = cr * cp * cy + sr * sp * sy;
q.x = sr * cp * cy - cr * sp * sy;
q.y = cr * sp * cy + sr * cp * sy;
q.z = cr * cp * sy - sr * sp * cy;
However the heading seems very random. Please let me know if you know of a method to initialise the quaternions with semi-correct values.
I have developed a custom CIWarpKernel which is the transposition of my Numbers model design as follows:
The cell for positive y is: SIN($A2×PI()÷2)×(−COS(B$1×PI())×($A2+1)÷$W$3+SIN($A2 ×PI()÷2)) where $A2 is y, B$1 is x and $W$3 is the center stretching factor.
The code is as follows:
kernel vec2 panoramaDistortion (vec2 center) {
float pi = 3.141592653589793;
vec2 t = destCoord();
float x = t.x / center.x - 1.0; // x ∈ -1...1
float y = t.y / center.y - 1.0; // y ∈ -1...1
float rx = x;
float delta = 50.0;
float siny = sin(y * pi / 2.0);
// See my model in Numbers: Aladdin PanoramPinch CIKernel file where $a2 = x and b$1 = y
// sin($a2×pi()÷2)×(−cos(b$1×pi())×($a2+1)÷$w$3+sin($a2 ×pi()÷$w$5))
// sin(y×pi()÷2)×(−cos(x×pi())×(y+1)÷$w$3+sin(y×pi()÷2))
// sin($a22×pi()÷2)×(−cos(b$1×pi())×abs(1−$a22)÷$w$3−sin($a22 ×pi()÷$w$5))
float ry =
y >= 0.0 ?
siny * (-cos(x * pi) * (y + 1.0) / delta + siny)
: siny * (-cos(x * pi) * (1.0 - y) / delta - siny)
;
return vec2(center.x * (rx + 1.0), center.y * (ry + 1.0));
}
The issue I have is that, though the transposition is 100% exact, I don't get the same result I have in my model. See the resulting warping of tiled-image:
How it comes I have the dilatation at the center y=0 and at the upper- and bottom- sides?
For information, my tiled-image is as follows:
Using Metal I am drawing line using Bezier Curves using four points. I am using nearly 1500 triangles for the lines. The line is Pixellated. How can i reduce pixellated.
vertex VertexOutBezier bezier_vertex(constant BezierParameters *allParams[[buffer(0)]],
constant GlobalParameters& globalParams[[buffer(1)]],
uint vertexId [[vertex_id]],
uint instanceId [[instance_id]])
{
float t = (float) vertexId / globalParams.elementsPerInstance;
rint(t);
BezierParameters params = allParams[instanceId];
float lineWidth = (1 - (((float) (vertexId % 2)) * 2.0)) * params.lineThickness;
float2 a = params.a;
float2 b = params.b;
float cx = distance(a , b);
float2 p1 = params.p1 * 3.0; // float2 p1 = params.p1 * 3.0;
float2 p2 = params.p2 * 3.0; // float2 p2 = params.p2 * 3.0;
float nt = 1.0f - t;
float nt_2 = nt * nt;
float nt_3 = nt_2 * nt;
float t_2 = t * t;
float t_3 = t_2 * t;
// Calculate a single point in this Bezier curve:
float2 point = a * nt_3 + p1 * nt_2 * t + p2 * nt * t_2 + b * t_3;
float2 tangent = -3.0 * a * nt_2 + p1 * (1.0 - 4.0 * t + 3.0 * t_2) + p2 * (2.0 * t - 3.0 * t_2) + 3 * b * t_2;
tangent = (float2(-tangent.y , tangent.x ));
VertexOutBezier vo;
vo.pos.xy = point + (tangent * (lineWidth / 2.0f));
vo.pos.zw = float2(0, 1);
vo.color = params.color;
return vo;
}
You need to enable MSAA (multisample anti-aliasing). How you do this depends on your exact Metal view configuration, but the easiest way is if you're using MTKView. To enable MSAA in an MTKView, all you have to do is:
metalView.sampleCount = 4
Then, when you configure your MTLRenderPipelineDescriptor before calling makeRenderPipelineState(), add the following:
pipelineDescriptor.sampleCount = 4
This should greatly improve the quality of your curves and reduce pixelation. It does come with a performance cost however, as the GPU has to do substantially more work to render your frame.
I have a problem in my iOS application where i attempt to obtain a view matrix using solvePnP and render a 3d cube using modern OpenGL. While my code attempts to render a 3d cube directly on top of the detected marker, it seems to render with a certain offset from the marker (see video for example)
https://www.youtube.com/watch?v=HhP5Qr3YyGI&feature=youtu.be
(on the bottom right of the image you can see an opencv render of the homography around the tracker marker. the rest of the screen is an opengl render of the camera input frame and a 3d cube at location (0,0,0).
the cube rotates and translates correctly whenever i move the marker, though it is very telling that there is some difference in the scale of translations (IE, if i move my marker 5cm in the real world, it hardly moves by 1cm on screen)
these are what i believe to be the relevant parts of the code where the error could come from :
Extracting view matrix from homography :
AVCaptureDevice *deviceInput = [AVCaptureDevice defaultDeviceWithMediaType:AVMediaTypeVideo];
AVCaptureDeviceFormat *format = deviceInput.activeFormat;
CMFormatDescriptionRef fDesc = format.formatDescription;
CGSize dim = CMVideoFormatDescriptionGetPresentationDimensions(fDesc, true, true);
const float cx = float(dim.width) / 2.0;
const float cy = float(dim.height) / 2.0;
const float HFOV = format.videoFieldOfView;
const float VFOV = ((HFOV)/cx)*cy;
const float fx = abs(float(dim.width) / (2 * tan(HFOV / 180 * float(M_PI) / 2)));
const float fy = abs(float(dim.height) / (2 * tan(VFOV / 180 * float(M_PI) / 2)));
Mat camIntrinsic = Mat::zeros(3, 3, CV_64F);
camIntrinsic.at<double>(0, 0) = fx;
camIntrinsic.at<double>(0, 2) = cx;
camIntrinsic.at<double>(1, 1) = fy;
camIntrinsic.at<double>(1, 2) = cy;
camIntrinsic.at<double>(2, 2) = 1.0;
std::vector<cv::Point3f> object3dPoints;
object3dPoints.push_back(cv::Point3f(-0.5f,-0.5f,0));
object3dPoints.push_back(cv::Point3f(+0.5f,-0.5f,0));
object3dPoints.push_back(cv::Point3f(+0.5f,+0.5f,0));
object3dPoints.push_back(cv::Point3f(-0.5f,+0.5f,0));
cv::Mat raux,taux;
cv::Mat Rvec, Tvec;
cv::solvePnP(object3dPoints, mNewImageBounds, camIntrinsic, Mat(),raux,taux); //mNewImageBounds are the 4 corner of the homography detected by perspectiveTransform (the green outline seen in the image)
raux.convertTo(Rvec,CV_32F);
taux.convertTo(Tvec ,CV_64F);
Mat Rot(3,3,CV_32FC1);
Rodrigues(Rvec, Rot);
// [R | t] matrix
Mat_<double> para = Mat_<double>::eye(4,4);
Rot.convertTo(para(cv::Rect(0,0,3,3)),CV_64F);
Tvec.copyTo(para(cv::Rect(3,0,1,3)));
Mat cvToGl = Mat::zeros(4, 4, CV_64F);
cvToGl.at<double>(0, 0) = 1.0f;
cvToGl.at<double>(1, 1) = -1.0f; // Invert the y axis
cvToGl.at<double>(2, 2) = -1.0f; // invert the z axis
cvToGl.at<double>(3, 3) = 1.0f;
para = cvToGl * para;
Mat_<double> modelview_matrix;
Mat(para.t()).copyTo(modelview_matrix); // transpose to col-major for OpenGL
glm::mat4 openGLViewMatrix;
for(int col = 0; col < modelview_matrix.cols; col++)
{
for(int row = 0; row < modelview_matrix.rows; row++)
{
openGLViewMatrix[col][row] = modelview_matrix.at<double>(col,row);
}
}
i made sure the camera intrinsic matrix contains correct values, the portion which converts the opencv Mat to an opengl view matrix i believe to be correct as the cube translates and rotates in the right directions.
once the view matrix is calculated, i use it to draw the cube as follows :
_projectionMatrix = glm::perspective<float>(radians(60.0f), fabs(view.bounds.size.width / view.bounds.size.height), 0.1f, 100.0f);
_cube_ModelMatrix = glm::translate(glm::vec3(0,0,0));
const mat4 MVP = _projectionMatrix * openGLViewMatrix * _cube_ModelMatrix;
glUniformMatrix4fv(glGetUniformLocation(_cube_program, "ModelMatrix"), 1, GL_FALSE, value_ptr(MVP));
glDrawElements(GL_TRIANGLES, 36, GL_UNSIGNED_INT, BUFFER_OFFSET(0));
Is anyone able to spot my error?
You should create perspective matrix as explained here: http://ksimek.github.io/2013/06/03/calibrated_cameras_in_opengl
Here is quick code:
const float fx = intrinsicParams(0, 0); // Focal length in x axis
const float fy = intrinsicParams(1, 1); // Focal length in y axis
const float cx = intrinsicParams(0, 2); // Primary point x
const float cy = intrinsicParams(1, 2); // Primary point y
projectionMatrix(0, 0) = 2.0f * fx;
projectionMatrix(0, 1) = 0.0f;
projectionMatrix(0, 2) = 0.0f;
projectionMatrix(0, 3) = 0.0f;
projectionMatrix(1, 0) = 0.0f;
projectionMatrix(1, 1) = 2.0f * fy;
projectionMatrix(1, 2) = 0.0f;
projectionMatrix(1, 3) = 0.0f;
projectionMatrix(2, 0) = 2.0f * cx - 1.0f;
projectionMatrix(2, 1) = 2.0f * cy - 1.0f;
projectionMatrix(2, 2) = -(far + near) / (far - near);
projectionMatrix(2, 3) = -1.0f;
projectionMatrix(3, 0) = 0.0f;
projectionMatrix(3, 1) = 0.0f;
projectionMatrix(3, 2) = -2.0f * far * near / (far - near);
projectionMatrix(3, 3) = 0.0f;
For more information about intrinsic matrix: http://ksimek.github.io/2013/08/13/intrinsic
I'm trying to draw a triangle like this one in a view (one UIView, one NSView):
My first thought was CoreGraphics, but I couldn't find any information that would help me draw a gradient between three points of arbitrary color.
Any help?
Thanks!
Actually it's pretty simple with CoreGraphics. Below you can find code that renders given triangle, but first let's think how we can solve this problem.
Theory
Imagine equilateral triangle with side length w. All three angles are equal to 60 degrees:
Each angle will represent component of a pixel: red, green or blue.
Lets analyze intensity of a green component in a pixel near top angle:
The more closer pixel to the angle, the more component intense it'll have and vice versa. Here we can decompose our main goal to smaller ones:
Draw triangle pixel by pixel.
For each pixel calculate value for each component based on distance from corresponding angle.
To solve first task we will use CoreGraphics bitmap context. It will have four components per pixel each 8 bits long. This means that component value may vary from 0 to 255. Fourth component is alpha channel and will be always equal to max value - 255. Here is example of how values will be interpolated for the top angle:
Now we need to think how we can calculate value for component.
First, let's define main color for each angle:
Now let's choose an arbitrary point A with coordinates (x,y) on the triangle:
Next, we draw a line from an angle associated with red component and it passes through the A till it intersects with opposite side of a triangle:
If we could find d and c their quotient will equal to normalized value of component, so value can be calculated easily:
(source: sciweavers.org)
Formula for finding distance between two points is simple:
(source: sciweavers.org)
We can easily find distance for d, but not for c, because we don't have coordinates of intersection. Actually it's not that hard. We just need to build line equations for line that passes through A and line that describes opposite side of a triangle and find their intersection:
Having intersection point we can apply distance formula to find c and finally calculate component value for current point.
Same flow applies for another components.
Code
Here is the code that implements concepts above:
+ (UIImage *)triangleWithSideLength:(CGFloat)sideLength {
return [self triangleWithSideLength:sideLength scale:[UIScreen mainScreen].scale];
}
+ (UIImage *)triangleWithSideLength:(CGFloat)sideLength
scale:(CGFloat)scale {
UIImage *image = nil;
CGSize size = CGSizeApplyAffineTransform((CGSize){sideLength, sideLength * sin(M_PI / 3)}, CGAffineTransformMakeScale(scale, scale));
size_t const numberOfComponents = 4;
size_t width = ceilf(size.width);
size_t height = ceilf(size.height);
size_t realBytesPerRow = width * numberOfComponents;
size_t alignedBytesPerRow = (realBytesPerRow + 0xFF) & ~0xFF;
size_t alignedPixelsPerRow = alignedBytesPerRow / numberOfComponents;
CGContextRef ctx = CGBitmapContextCreate(NULL,
width,
height,
8,
alignedBytesPerRow,
CGColorSpaceCreateDeviceRGB(),
(CGBitmapInfo)kCGImageAlphaPremultipliedLast);
char *data = CGBitmapContextGetData(ctx);
for (int i = 0; i < height; i++) {
for (int j = 0; j < width; j++) {
int edge = ceilf((height - i) / sqrt(3));
if (j < edge || j > width - edge) {
continue;
}
CGFloat redNormalized = 0;
CGFloat greenNormalized = 0;
CGFloat blueNormalized = 0;
CGPoint currentTrianglePoint = (CGPoint){j / scale, (height - i) / scale};
[self calculateCurrentValuesAtGiventPoint:currentTrianglePoint
sideLength:sideLength
sideOne:&redNormalized
sideTwo:&greenNormalized
sideThree:&blueNormalized];
int32_t red = redNormalized * 0xFF;
int32_t green = greenNormalized * 0xFF;
int32_t blue = blueNormalized * 0xFF;
char *pixel = data + (j + i * alignedPixelsPerRow) * numberOfComponents;
*pixel = red;
*(pixel + 1) = green;
*(pixel + 2) = blue;
*(pixel + 3) = 0xFF;
}
}
CGImageRef cgImage = CGBitmapContextCreateImage(ctx);
image = [[UIImage alloc] initWithCGImage:cgImage];
CGContextRelease(ctx);
CGImageRelease(cgImage);
return image;
}
+ (void)calculateCurrentValuesAtGiventPoint:(CGPoint)point
sideLength:(CGFloat)length
sideOne:(out CGFloat *)sideOne
sideTwo:(out CGFloat *)sideTwo
sideThree:(out CGFloat *)sideThree {
CGFloat height = sin(M_PI / 3) * length;
if (sideOne != NULL) {
// Side one is at 0, 0
CGFloat currentDistance = sqrt(point.x * point.x + point.y * point.y);
if (currentDistance != 0) {
CGFloat a = point.y / point.x;
CGFloat b = 0;
CGFloat c = -height / (length / 2);
CGFloat d = 2 * height;
CGPoint intersection = (CGPoint){(d - b) / (a - c), (a * d - c * b) / (a - c)};
CGFloat currentH = sqrt(intersection.x * intersection.x + intersection.y * intersection.y);
*sideOne = 1 - currentDistance / currentH;
} else {
*sideOne = 1;
}
}
if (sideTwo != NULL) {
// Side two is at w, 0
CGFloat currentDistance = sqrt(pow((point.x - length), 2) + point.y * point.y);
if (currentDistance != 0) {
CGFloat a = point.y / (point.x - length);
CGFloat b = height / (length / 2);
CGFloat c = a * -point.x + point.y;
CGFloat d = b * -length / 2 + height;
CGPoint intersection = (CGPoint){(d - c) / (a - b), (a * d - b * c) / (a - b)};
CGFloat currentH = sqrt(pow(length - intersection.x, 2) + intersection.y * intersection.y);
*sideTwo = 1 - currentDistance / currentH;
} else {
*sideTwo = 1;
}
}
if (sideThree != NULL) {
// Side three is at w / 2, w * sin60 degrees
CGFloat currentDistance = sqrt(pow((point.x - length / 2), 2) + pow(point.y - height, 2));
if (currentDistance != 0) {
float dy = point.y - height;
float dx = (point.x - length / 2);
if (fabs(dx) > FLT_EPSILON) {
CGFloat a = dy / dx;
CGFloat b = 0;
CGFloat c = a * -point.x + point.y;
CGFloat d = 0;
CGPoint intersection = (CGPoint){(d - c) / (a - b), (a * d - b * c) / (a - b)};
CGFloat currentH = sqrt(pow(length / 2 - intersection.x, 2) + pow(height - intersection.y, 2));
*sideThree = 1 - currentDistance / currentH;
} else {
*sideThree = 1 - currentDistance / height;
}
} else {
*sideThree = 1;
}
}
}
Here is a triangle image produced by this code: