rotate axis by angle

gistfile1.txt

#include <iostream>
#include <fstream>
#include <opencv2\opencv.hpp>
#include <math.h>


using namespace std;
using namespace cv;
using namespace cv::dnn;


Mat getXRotMat(float _grados)
{
	Mat rotMat = cv::Mat::zeros(3, 3, CV_64F);

	_grados = _grados * (3.1416 / 180);
	rotMat.at<double>(0, 0) = 1;
	rotMat.at<double>(0, 1) = 0;
	rotMat.at<double>(0, 2) = 0;
	rotMat.at<double>(1, 0) = 0;
	rotMat.at<double>(1, 1) = cos(_grados);
	rotMat.at<double>(1, 2) = -sin(_grados);
	rotMat.at<double>(2, 0) = 0;
	rotMat.at<double>(2, 1) = sin(_grados);
	rotMat.at<double>(2, 2) = cos(_grados);

	return rotMat;
}

Mat getYRotMat(float _grados)
{
	_grados = _grados * (3.1416 / 180);
	Mat rotMat = cv::Mat::zeros(3, 3, CV_64F);
	// - .at<xxx> format -> (row, col)
	rotMat.at<double>(0, 0) = cos(_grados);
	rotMat.at<double>(0, 1) = 0;
	rotMat.at<double>(0, 2) = sin(_grados);
	rotMat.at<double>(1, 0) = 0;
	rotMat.at<double>(1, 1) = 1;
	rotMat.at<double>(1, 2) = 0;
	rotMat.at<double>(2, 0) = -sin(_grados);
	rotMat.at<double>(2, 1) = 0;
	rotMat.at<double>(2, 2) = cos(_grados);

	return rotMat;
}

Mat getZRotMat(float _grados)
{
	Mat rotMat = cv::Mat::zeros(3, 3, CV_64F);
	_grados = _grados * (3.1416 / 180);
	rotMat.at<double>(0, 0) = cos(_grados);
	rotMat.at<double>(0, 1) = -sin(_grados);
	rotMat.at<double>(0, 2) = 0;
	rotMat.at<double>(1, 0) = sin(_grados);
	rotMat.at<double>(1, 1) = cos(_grados);
	rotMat.at<double>(1, 2) = 0;
	rotMat.at<double>(2, 0) = 0;
	rotMat.at<double>(2, 1) = 0;
	rotMat.at<double>(2, 2) = 1;

	return rotMat;
}



int main(int argc, char *argv[])
{
	vector<Point3f> axis;
	axis.push_back(Point3f(0, 0, 0));
	axis.push_back(Point3f(50, 0, 0));
	axis.push_back(Point3f(0, 50, 0));
	axis.push_back(Point3f(0, 0, 50));

	cv::Mat test_rot = getYRotMat(45);
	cout << "Rotation Matrix: " << endl << test_rot << endl << endl;
	Mat test_rot_vec;
	Rodrigues(test_rot, test_rot_vec);
	cout << "Rotation Vector: " << endl << test_rot_vec << endl << endl;
	vector<Point2f> axis_2d;

	Mat cameraMat = cv::Mat::eye(3, 3, CV_64F);
	cameraMat.at<double>(0, 0) = 1;
	cameraMat.at<double>(0, 1) = 0;
	cameraMat.at<double>(0, 2) = 450;
	cameraMat.at<double>(1, 0) = 0;
	cameraMat.at<double>(1, 1) = 1;
	cameraMat.at<double>(1, 2) = 450;
	cameraMat.at<double>(2, 0) = 0;
	cameraMat.at<double>(2, 1) = 0;
	cameraMat.at<double>(2, 2) = 1;

	cv::projectPoints(axis, test_rot_vec, cv::Mat::zeros(3, 1, CV_64F), cameraMat, cv::noArray(), axis_2d);

	Mat draw = Mat::zeros(Size(900, 900), CV_8UC3);
	for (auto &pt : axis_2d)
	{
		cout << pt << endl;
		circle(draw, pt, 1, Scalar(255, 255, 1), -1);
	}

	line(draw, axis_2d[0], axis_2d[1], Scalar(0, 255, 0), 2);
	line(draw, axis_2d[0], axis_2d[2], Scalar(255, 0, 0), 2);
	line(draw, axis_2d[0], axis_2d[3], Scalar(0, 0, 255), 2);

	imshow("www", draw);
	waitKey();

}