I am using the point cloud library and trying to match two point clouds together ICP (Iterative Closest Point) algorithm. The data sets that I have been given comes with X Y Z orientation values from an IMU sensor.
I am attaching these to the sensor_orientation_ attribute of the point cloud object to aid the matching process. Looking at the PCL documentation it's specified as: Sensor acquisition pose (rotation in the cloud data coordinate system).
Note: the data is stored in (w, x, y, z) format.
So, to convert from the IMU data I am using this function below and I wanted to ask is it correct?
Eigen::Quaterniond euler2Quaternion(const double roll, const double pitch, const double yaw)
{
Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitZ());
Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitY());
Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitX());
Eigen::Quaterniond q = rollAngle * yawAngle * pitchAngle;
return q;
}
And the method gets called:
inCloud->sensor_orientation_ = Eigen::Quaternionf(euler2Quaternion(orientX, orientY, orientZ));
Well I found the issue with the code. Something I couldn't find in the official documentation is that Eigen::AngleAxisd takes input as radians and not degrees, so once you convert your data from IMU sensor to radians everything seems to be fine :)
The function looks like this now:
Eigen::Quaterniond euler2Quaternion(const double roll, const double pitch, const double yaw)
{
Eigen::AngleAxisd rollAngle((roll*M_PI) / 180, Eigen::Vector3d::UnitZ());
Eigen::AngleAxisd yawAngle((yaw*M_PI) / 180, Eigen::Vector3d::UnitY());
Eigen::AngleAxisd pitchAngle((pitch*M_PI) / 180, Eigen::Vector3d::UnitX());
Eigen::Quaterniond q = rollAngle * yawAngle * pitchAngle;
return q;
}
Obviously the order in which pitch, yaw and roll are used depend on the usages but in my case this worked :)
Related
I'm trying to move a bullet object towards a player position. I found this angle function online, and it seems to convert it to those coordinates in the comments. But i can't make the bullet follow in the direction.
float Angle(int p1x, int p1y, int p2x,int p2y)
{
//Make point1 the origin, make point2 relative to the origin so we do point1 - point1, and point2-point1,
//since we dont need point1 for the equation to work, the equation works correctly with the origin 0,0.
int deltaY = p2y - p1y;
int deltaX = p2x - p1x; //Vector 2 is now relative to origin, the angle is the same, we have just transformed it to use the origin.
float angleInDegrees = atan2(deltaY, deltaX) * 180 / PI;
//float angleInRadians = atan2(deltaY, deltaX);
angleInDegrees *= -1; // Y axis is inverted in computer windows, Y goes down, so invert the angle.
//Angle returned as:
// 90
// 135 45
//
// 180 Origin 0
//
// -135 -45
//
// -90
return angleInDegrees;
}
if (bulletsData[3] == STDEACTIVE){ //bulletData: 0 = x, 1 = y, 2 = dir, 3 = state
bulletsData[2] = Angle(bulletsData[0],bulletsData[1], plData[0], plData[1]);
bulletsData[3] = STACTIVE;
}
if (bulletsData[3] == STACTIVE){
if (ardu.everyXFrames(1)){
bulletsData[0] += cos(bulletsData[2]) * 1; //My attempt to move it towards the direction.
bulletsData[1] += sin(bulletsData[2]) * 1;
}
}
You are explicitly calculating angles in degrees, yet sin and cos expect radians, not degrees.
Without analyzing whether or not the Angle function is correct, your code will make more sense if you change the function to return a value in radians. You even have the required line there commented out.
So, something like this perhaps:
float angleInRadians = atan2(deltaY, deltaX);
return -angleInRadians; // Return negative angle to compensate for Y-down
I know this is a very simple question, but I have been simply unable to find an answer. As a workaround, I've been using roll, but this is not ideal. I would like to be able to move the contents of my images without them rolling over to the other side using the Magick++ libary (not convert!).
Using Magick::Image.distort() is the method you need --- as pointed out by fmw42 in the comments.
Here's an example using -distort Affine, but SRT would give you more control.
#include <Magick++.h>
int main() {
Magick::Image img("wizard:");
size_t numberOfArguments = 4;
double * listOfArguments = new double[numberOfArguments];
listOfArguments[0] = 0.0;
listOfArguments[1] = 0.0;
listOfArguments[2] = img.rows() / 4.0;
listOfArguments[3] = img.columns() / 4.0;
bool bestFit = Magick::MagickFalse;
img.distort(Magick::AffineDistortion,
numberOfArguments,
listOfArguments,
bestFit);
img.write("output.png");
}
Remember that the number of arguments alter between distortion methods, and each method interprets the order of arguments differently based on argument count.
For example, with SRT the order would be mapped as ...
=============== ========================================
Total Arguments Argument Order
--------------- ----------------------------------------
1 Angle
2 Scale, Angle
3 X, Y, Angle
4 X, Y, Scale, Angle
5 X, Y, ScaleX, ScaleY, Angle
6 X, Y, Scale, Angle, NewX, NewY
7 X, Y, ScaleX, ScaleY, Angle, NewX, NewY
=============== ========================================
When I create matrix of rotation from Euler angles, Should I convert a degrees(Euler angles) to radians, and then count matrix of rotation for OpenGL?
But what should I do with a quaternions?
Should I do following:
void setQuaternionsFromEuler(float bank, float heading, float attitude)
{
float DegreeToRadian = 3.14f/180.0f;
double c1 = cos(heading/2*DegreeToRadian);
double c2 = cos(attitude/2*DegreeToRadian);
//...
double s3 = sin(bank/2*DegreeToRadian);
this.w = c1*c2*c3 - s1*s2*s3;
//...
}
And,
void setMatrixFromEuler(float x, float y, float z)
{
float DegreeToRadian = 3.14f/180.0f;
x *= DegreeToRadian;
y *= DegreeToRadian;
//...
}
Or not?
It depends on how you define your input.
The trig functions of C++ take radian exclusively, so you need to convert to radian eventually but that may be done before the data even enters your program (such that the angle values in the resources are all in radian)
Rotation matricies don't have angles anymore, so it's not really related with OpenGL, but the trigonometric functions you use from the standard library in C or C++ are defined on radians, so if your user-facing API works with degrees, then yes you should convert to take the cos/sin.
So I'm trying to make the player shoot a bullet that goes towards the mouse in a wavey pattern. I can get the bullet to move in a wavey pattern (albeit not really how I predicted), but not towards the mouse.
Vector2 BulletFun::sine(Vector2 vec) {
float w = (2 * PI) / 1000; // Where 1000 is the period
float waveNum = (2 * PI) / 5; // Where 5 is the wavelength
Vector2 k(0.0F, waveNum);
float t = k.dot(vec) - (w * _time);
float x = 5 * cos(t); // Where 5 is the amplitude
float y = 5 * sin(t);
Vector2 result(x, y);
return result;
}
Right now the speed isn't much of a concern, that shouldn't be too much of a problem once I have this figured out. I do get some angle change, but it seems to be reversed and only 1/8th a circle.
I'm probably miscalculating something somewhere. I just kind of learned about wave vectors.
I've tried a few other things, such as 1 dimensional travelling waves and another thing involving adjusting a normal sine wave by vec. Which had more or less the same result.
Thanks!
EDIT:
vec is the displacement from the player's location to the mouse click location. The return is a new vector that is adjusted to follow a wave pattern, BulletFun::sine is called each time the bullet receives and update.
The setup is something like this:
void Bullet::update() {
_velocity = BulletFun::sine(_displacement);
_location.add(_velocity); // add is a property of Tuple
// which Vector2 and Point2 inherit
}
In pseudocode, what you need to do is the following:
waveVector = Vector2(travelDistance,amplitude*cos(2*PI*frequency*travelDistance/unitDistance);
cosTheta = directionVector.norm().dot(waveVector.norm());
theta = acos(cosTheta);
waveVector.rotate(theta);
waveVector.translate(originPosition);
That should compute the wave vector in a traditional coordinate frame, and then rotate it to the local coordinate frame of the direction vector (where the direction vector is the local x-axis), and then translate the wave vector relative to your desired origin position of the wave beam or whatever...
This will result in a function very similar to
Vector2
BulletFun::sine(Bullet _bullet, float _amplitude, float _frequency, float _unitDistance)
{
float displacement = _bullet.getDisplacement();
float omega = 2.0f * PI * _frequency * _displacement / _unitDistance;
// Compute the wave coordinate on the traditional, untransformed
// Cartesian coordinate frame.
Vector2 wave(_displacement, _amplitude * cos(omega));
// The dot product of two unit vectors is the cosine of the
// angle between them.
float cosTheta = _bullet.getDirection().normalize().dot(wave.normalize());
float theta = acos(cosTheta);
// Translate and rotate the wave coordinate onto
// the direction vector.
wave.translate(_bullet.origin());
wave.rotate(theta);
}
I am currently trying to work on getting my virtual trackball to work from any angle. When I am looking at it from the z axis, it seems to work fine. I hold my mouse down, and move the mouse up... the rotation will move accordingly.
Now, if I change my viewing angle / position of my camera and try to move my mouse. The rotation will occur as if I were looking from the z axis. I cannot come up with a good way to get this to work.
Here is the code:
void Renderer::mouseMoveEvent(QMouseEvent *e)
{
// Get coordinates
int x = e->x();
int y = e->y();
if (isLeftButtonPressed)
{
// project current screen coordinates onto hemi sphere
Point sphere = projScreenCoord(x,y);
// find axis by taking cross product of current and previous hemi points
axis = Point::cross(previousPoint, sphere);
// angle can be found from magnitude of cross product
double length = sqrt( axis.x * axis.x + axis.y * axis.y + axis.z * axis.z );
// Normalize
axis = axis / length;
double lengthPrev = sqrt( previousPoint.x * previousPoint.x + previousPoint.y * previousPoint.y + previousPoint.z * previousPoint.z );
double lengthCur = sqrt( sphere.x * sphere.x + sphere.y * sphere.y + sphere.z * sphere.z );
angle = asin(length / (lengthPrev * lengthCur));
// Convert into Degrees
angle = angle * 180 / M_PI;
// 'add' this rotation matrix to our 'total' rotation matrix
glPushMatrix(); // save the old matrix so we don't mess anything up
glLoadIdentity();
glRotatef(angle, axis[0], axis[1], axis[2]); // our newly calculated rotation
glMultMatrixf(rotmatrix); // our previous rotation matrix
glGetFloatv(GL_MODELVIEW_MATRIX, (GLfloat*) rotmatrix); // we've let OpenGL do our matrix mult for us, now get this result & store it
glPopMatrix(); // return modelview to its old value;
}
// Project screen coordinates onto a unit hemisphere
Point Renderer::projScreenCoord(int x, int y)
{
// find projected x & y coordinates
double xSphere = ((double)x/width)*2.0 - 1.0;
double ySphere = ( 1 - ((double)y/height)) * 2.0 - 1.0;
double temp = 1.0 - xSphere*xSphere - ySphere*ySphere;
// Do a check so you dont do a sqrt of a negative number
double zSphere;
if (temp < 0){ zSphere = 0.0;}
else
{zSphere = sqrt(temp);}
Point sphere(xSphere, ySphere, zSphere);
// return the point on the sphere
return sphere;
}
I am still fairly new at this. Sorry for the trouble and thanks for all the help =)
The usual way involves quaternions. E.g., in sample code originally from SGI.