Eigen: convert rotation matrix to quaternion then back getting completely different matrices - matrix

Can anyone help me out with Eigen? I tried to convert quaternion to matrix and then back and got completely different matrices. I am not able to trust quaternion before understanding this issue.
Here is the code:
#include <Eigen/Geometry>
#include <iostream>
void Print_Quaternion(Eigen::Quaterniond &q){
std::cout<<"["<<q.w()<<" "<<q.x()<<" "<<q.y()<<" "<<q.z()<<"]"<<std::endl;
}
void Verify_Orthogonal_Matrix(Eigen::Matrix3d &m)
{
std::cout<<"|c0|="<<m.col(0).norm()<<",|c1|="<<m.col(1).norm()<<",|c2|="<<m.col(2).norm()<<std::endl;
std::cout<<"c0c1="<<m.col(0).dot(m.col(1))<<",c1c2="<<m.col(1).dot(m.col(2))<<",c0c2="<<m.col(0).dot(m.col(2))<<std::endl;
}
int main()
{
Eigen::Matrix3d m; m<<0.991601,0.102421,-0.078975,0.125398,-0.611876,0.78095,-0.0316631,0.784294,0.619581;
std::cout<<"Input matrix:"<<std::endl<<m<<std::endl;
std::cout<<"Verify_Orthogonal_Matrix:"<<std::endl;
Verify_Orthogonal_Matrix(m);
std::cout<<"Convert to quaternion q:"<<std::endl;
Eigen::Quaterniond q(m);
Print_Quaternion(q);
std::cout<<"Convert back to rotation matrix m1="<<std::endl;
Eigen::Matrix3d m1=q.normalized().toRotationMatrix();
std::cout<<m1<<std::endl;
std::cout<<"Verify_Orthogonal_Matrix:"<<std::endl;
Verify_Orthogonal_Matrix(m1);
std::cout<<"Convert again to quaternion q1="<<std::endl;
Eigen::Quaterniond q1(m1);
Print_Quaternion(q1);
}
Here is the result I got:
Input matrix:
0.991601 0.102421 -0.078975
0.125398 -0.611876 0.78095
-0.0316631 0.784294 0.619581
Verify_Orthogonal_Matrix:
|c0|=1,|c1|=1,|c2|=1
c0c1=-4.39978e-07,c1c2=4.00139e-07,c0c2=2.39639e-08
Convert to quaternion q:
[0.706984 0.00118249 -0.0167302 0.00812501]
Convert back to rotation matrix m1=
0.998617 -0.0230481 -0.047257
0.0228899 0.99973 -0.00388638
0.0473339 0.0027993 0.998875
Verify_Orthogonal_Matrix:
|c0|=1,|c1|=1,|c2|=1
c0c1=1.73472e-18,c1c2=-4.33681e-19,c0c2=6.93889e-18
Convert again to quaternion q1=
[0.999653 0.001672 -0.0236559 0.0114885]
Did I do something wrong here? I feel that this should be a well-known problem but I got stuck here. Can someone help me out?

Input matrix is not a rotation matrix, it contains mirroring. Its determinant == -1, but rotation should have +1.
check the code for orthogonalization, and look and the signs of last col
m.col(0).normalize();
m.col(1).normalize();
m.col(2) = m.col(0).cross(m.col(1));
m.col(2).normalize();
m.col(0) = m.col(1).cross(m.col(2));
m.col(0).normalize();
std::cout << "orthogonal matrix:" << std::endl << m << std::endl;
Input matrix:
0.991601 0.102421 -0.078975
0.125398 -0.611876 0.78095
-0.0316631 0.784294 0.619581
orthogonal matrix:
0.991601 0.102421 0.078975
0.125398 -0.611876 -0.78095
-0.0316628 0.784294 -0.619581

Related

C++ Eigen AlignedBox Transformations

I am trying to make my first steps with the C++ Eigen library. The Matrix functionality was very intuitive but I have some problems using the AlignedBox type from the Geometry module.
For an exercise I have to rotate an AlignedBox around a specific point and be able to translate it within a 2D plane using Eigen::Transform.
I have tried around for quite a while.
#include <iostream>
#include <eigen3/Eigen/Dense>
int main()
{
// create 1D AlignedBox
Eigen::MatrixXf sd1(1,1);
Eigen::MatrixXf sd2(1,1);
sd1 << 0;
sd2 << 3;
Eigen::AlignedBox1f box1(sd1, sd2);
// rotation of 45 deg
typedef Eigen::Rotation2D<float> R2D;
R2D r(M_PI/4.0);
// create transformation matrix with rotation of 45 deg
typedef Eigen::Transform< float, 2, Eigen::AffineCompact > SE2;
SE2 t;
t = r;
// how to apply transformation t to box1???
return 0;
}
I thought I have to multiply the AlignedBox with t.matrix() but since the Box is no matrix type and I did not find any useful build in function I have no idea how to apply the transformation. Any help would be appreciated
Note that result will be a 2D box. You can compute it by applying the affine transformation to the two 2D extremities, and updating the 2D box with the extend method, e.g.:
AlignedBox2f box2;
box2.extend(t * Vector2f(box1.min()(0), 0));
box2.extend(t * Vector2f(box1.max()(0), 0));
To apply another transformation to box2, you can use the same principle on the 4 corners of the box that you can get using the AlignedBox::corner method.

Eigen with PointCloud (PCL)

I have been following the tutorial http://pointclouds.org/documentation/tutorials/pcl_visualizer.php#pcl-visualizer and could get a simple viewer working.
I looked up the documentation and found the function getMatrixXfMap which returns the Eigen::MatrixXf from a PointCloud.
// Get Eigen matrix
Eigen::MatrixXf M = basic_cloud_ptr->getMatrixXfMap();
cout << "(Eigen) #row :" << M.rows() << endl;
cout << "(Eigen) #col :" << M.cols() << endl;
Next I process M (basically rotations, translations and some other transforms). I how is possible to set M into the PointCloud efficiently. Or is it that I need to pushback() one point at a time?
You do not need to cast your pcl cloud to an Eigen::MatrixXF, do the tranformations and cast back. You can simply perform on your input cloud:
pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
\\ Fill the cloud
\\ .....
Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
// Define a translation of 2.5 meters on the x axis.
transform_2.translation() << 2.5, 0.0, 0.0;
// The same rotation matrix as before; theta radians arround Z axis
transform_2.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitZ()));
// Print the transformation
printf ("\nMethod #2: using an Affine3f\n");
std::cout << transform_2.matrix() << std::endl;
// Executing the transformation
pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
// You can either apply transform_1 or transform_2; they are the same
pcl::transformPointCloud (*source_cloud, *transformed_cloud, transform_2);
Taken from pcl transformation tutorial.

Rotation matrix in Eigen

Can I use the Eigen library to get the rotation matrix which rotates vector A to vector B?
I have been searching for a while, but cannot find related api.
You first have to construct a quaternion and then convert it to a matrix, for instance:
#include <Eigen/Geometry>
using namespace Eigen;
int main() {
Vector3f A, B;
Matrix3f R;
R = Quaternionf().setFromTwoVectors(A,B);
}

How to apply Gaussian filter to DFT output in OpenCV

I want to create a Gaussian high-pass filter after determining the correct padding size (e.g., if image width and height is 10X10, then should be 20X20).
I have Matlab code that I am trying to port in OpenCV, but I am having difficulty properly porting it. My Matlab code is show below:
f1_seg = imread('thumb1-small-test.jpg');
Iori = f1_seg;
% Iori = imresize(Iori, 0.2);
%Convert to grayscale
I = Iori;
if length(size(I)) == 3
I = rgb2gray(Iori);
end
%
%Determine good padding for Fourier transform
PQ = paddedsize(size(I));
I = double(I);
%Create a Gaussian Highpass filter 5% the width of the Fourier transform
D0 = 0.05*PQ(1);
H = hpfilter('gaussian', PQ(1), PQ(2), D0);
% Calculate the discrete Fourier transform of the image.
F=fft2(double(I),size(H,1),size(H,2));
% Apply the highpass filter to the Fourier spectrum of the image
HPFS_I = H.*F;
I know how to use the DFT in OpenCV, and I am able to generate its image, but I am not sure how to create the Gaussian filter. Please guide me to how I can create a high-pass Gaussian filter as is shown above?
I believe where you are stuck is that the Gaussian filter supplied by OpenCV is created in the spatial (time) domain, but you want the filter in the frequency domain. Here is a nice article on the difference between high and low-pass filtering in the frequency domain.
Once you have a good understanding of how frequency domain filtering works, then you are ready to try to create a Gaussian Filter in the frequency domain. Here is a good lecture on creating a few different (including Gaussian) filters in the frequency domain.
If you are still having difficulty, I will try to update my post with an example a bit later!
EDIT :
Here is a short example that I wrote on implementing a Gaussian high-pass filter (based on the lecture I pointed you to):
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <iostream>
using namespace cv;
using namespace std;
double pixelDistance(double u, double v)
{
return cv::sqrt(u*u + v*v);
}
double gaussianCoeff(double u, double v, double d0)
{
double d = pixelDistance(u, v);
return 1.0 - cv::exp((-d*d) / (2*d0*d0));
}
cv::Mat createGaussianHighPassFilter(cv::Size size, double cutoffInPixels)
{
Mat ghpf(size, CV_64F);
cv::Point center(size.width / 2, size.height / 2);
for(int u = 0; u < ghpf.rows; u++)
{
for(int v = 0; v < ghpf.cols; v++)
{
ghpf.at<double>(u, v) = gaussianCoeff(u - center.y, v - center.x, cutoffInPixels);
}
}
return ghpf;
}
int main(int /*argc*/, char** /*argv*/)
{
Mat ghpf = createGaussianHighPassFilter(Size(128, 128), 16.0);
imshow("ghpf", ghpf);
waitKey();
return 0;
}
This is definitely not an optimized filter generator by any means, but I tried to keep it simple and straight forward to ease understanding :) Anyway, this code displays the following filter:
NOTE : This filter is not FFT shifted (i.e., this filter works when the DC is placed in the center instead of the upper-left corner). See the OpenCV dft.cpp sample (lines 62 - 74 in particular) on how to perform FFT shifting in OpenCV.
Enjoy!

what 2 & 3 mean in this and how can i change them CvMat* rot = cvCreateMat(2,3,CV_32FC1)

What do 2 & 3 mean in this and how can I change them?
CvMat* rot = cvCreateMat(2,3,CV_32FC1)
When I change these two values I get an openCV GUI error handler.
size of input arguments do not match()
in function cvConvertScale.\cxconvert.cpp(1601)
I want to understand what that means
Update:
The code is:
#include <cv.h>
#include <highgui.h>
int main()
{
CvMat* rot = cvCreateMat(2,3,CV_32FC1);
IplImage *src, *dst;
src=cvLoadImage("doda.jpg");
// make acopy of gray image(src)
dst = cvCloneImage( src );
dst->origin = src->origin;
// make dstof zeros
cvZero( dst );
// Compute rotation matrix
double x=0.0;
// loop to get rotation from 0 to 360 by 4 press on anykey
for(int i=1;i<=5;i++)
{
CvPoint2D32f center = cvPoint2D32f(src->width/2,src->height/2);
double angle = 0+x;
double scale = 0.6;
cv2DRotationMatrix( center, angle, scale, rot );
// Do the transformation
cvWarpAffine( src, dst, rot);
cvNamedWindow( "Affine_Transform", 1 );
cvShowImage( "Affine_Transform", dst );
if (i<=4)
x=x+90.0;
else
x=0.0;
cvWaitKey();
}
cvReleaseImage( &dst );
cvReleaseMat( &rot );
return 0;
}
2 and 3 are the row and column counts of the matrix you're creating.
From Introduction to programming with OpenCV:
Allocate a matrix:
CvMat* cvCreateMat(int rows, int cols, int type);
type: Type of the matrix elements. Specified in form
CV_<bit_depth>(S|U|F)C<number_of_channels>. E.g.: CV_8UC1 means an
8-bit unsigned single-channel matrix, CV_32SC2 means a 32-bit signed
matrix with two channels.
Example:
CvMat* M = cvCreateMat(4,4,CV_32FC1);
Changing them is as simple as substituting different values. But I guess you should already know that.
2 = number of rows and 3 = number of columns in your matrix, rot.
Can you post the entire code? Or maybe tell us what you want to achieve? Are you trying to rotate an image?
Also, I'd recommend upgrading to OpenCV 2.0 which has a C++ interface. With the new version, you can extensively use the Mat class which handles everything (matrices,images,etc.) and makes things much simpler.
You get an error using any other shape than 2x3 because it is then meaningless for opencv when you use rot for rotation.
Take a look at Jacob's answer.
He describes the rotation matrix components in details.

Resources