void Horus::getPCA(const vector<cv::Point> &contour, float ¢er, float &angle)
{
//Construct a buffer used by the pca analysis
int sz = static_cast<int>(contour.size());
Mat data_pts = Mat(sz, 2, CV_32FC1);
for (int i = 0; i < data_pts.rows; ++i) {
data_pts.at<float>(i, 0) = contour[i].x;
data_pts.at<float>(i, 1) = contour[i].y;
}
//Perform PCA analysis
PCA pca_analysis(data_pts, Mat(), CV_PCA_DATA_AS_ROW);
//Store the center of the object
cv::Point cntr = cv::Point(static_cast<int>(pca_analysis.mean.at<float>(0, 0)),
static_cast<int>(pca_analysis.mean.at<float>(0, 1)));
//Store the eigenvalues and eigenvectors
vector<cv::Point2d> eigen_vecs(2);
vector<float> eigen_val(2);
for (int i = 0; i < 2; ++i) {
eigen_vecs[i] = Point2d(pca_analysis.eigenvectors.at<float>(i, 0),
pca_analysis.eigenvectors.at<float>(i, 1));
eigen_val[i] = pca_analysis.eigenvalues.at<float>(0, i);
}
angle = atan2(eigen_vecs[0].y, eigen_vecs[0].x); // orientation in radians
}
double HandDetector::GetOrientation(const vector<Point> &pts, Mat &img)
{
if (pts.size() == 0) return false;
//Construct a buffer used by the pca analysis
Mat data_pts = Mat(pts.size(), 2, CV_64FC1);
for (int i = 0; i < data_pts.rows; ++i)
{
data_pts.at<double>(i, 0) = pts[i].x;
data_pts.at<double>(i, 1) = pts[i].y;
}
//Perform PCA analysis
PCA pca_analysis(data_pts, Mat(), CV_PCA_DATA_AS_ROW);
//Store the position of the object
Point pos = Point(pca_analysis.mean.at<double>(0, 0),
pca_analysis.mean.at<double>(0, 1));
//Store the eigenvalues and eigenvectors
vector<Point2d> eigen_vecs(2);
vector<double> eigen_val(2);
for (int i = 0; i < 2; ++i)
{
eigen_vecs[i] = Point2d(pca_analysis.eigenvectors.at<double>(i, 0),
pca_analysis.eigenvectors.at<double>(i, 1));
eigen_val[i] = pca_analysis.eigenvalues.at<double>(i);
}
// Draw the principal components
circle(img, pos, 3, CV_RGB(255, 0, 255), 2);
//line(img, pos, pos + 0.02 * Point(eigen_vecs[0].x * eigen_val[0], eigen_vecs[0].y * eigen_val[0]) , CV_RGB(255, 255, 0));
//line(img, pos, pos + 0.02 * Point(eigen_vecs[1].x * eigen_val[1], eigen_vecs[1].y * eigen_val[1]) , CV_RGB(0, 255, 255));
Point p1 = pos + 0.02 * Point(eigen_vecs[0].x * eigen_val[0], eigen_vecs[0].y * eigen_val[0]);
Point p2 = pos + 0.02 * Point(eigen_vecs[1].x * eigen_val[1], eigen_vecs[1].y * eigen_val[1]);
DrawAxis(img, pos, p1, Scalar(0, 255, 0), 1);
DrawAxis(img, pos, p2, Scalar(255, 255, 0), 5);
return atan2(eigen_vecs[0].y, eigen_vecs[0].x)*(180/CV_PI);
}
// Calculate the angle in degrees of a certain line.
double NaoVision::getAngleDegrees(const vector<Point> &pts, Mat &img) {
// Construct a buffer used by the pca analysis.
int sz = static_cast<int>(pts.size());
Mat data_pts = Mat(sz, 2, CV_64FC1);
for(int i = 0; i < data_pts.rows; ++i) {
data_pts.at<double>(i, 0) = pts[i].x;
data_pts.at<double>(i, 1) = pts[i].y;
}
// Perform PCA analysis.
PCA pca_analysis(data_pts, Mat(), CV_PCA_DATA_AS_ROW);
// Store the center of the object.
Point cntr = Point(static_cast<int>(pca_analysis.mean.at<double>(0, 0)),
static_cast<int>(pca_analysis.mean.at<double>(0, 1)));
// Store the eigenvalues and eigenvectors.
vector<Point2d> eigen_vecs(2);
vector<double> eigen_val(2);
for(int i = 0; i < 2; ++i) {
eigen_vecs[i] = Point2d(pca_analysis.eigenvectors.at<double>(i, 0),
pca_analysis.eigenvectors.at<double>(i, 1));
eigen_val[i] = pca_analysis.eigenvalues.at<double>(0, i);
}
// Draw the principal components.
circle(img, cntr, 3, Scalar(255, 0, 255), 2);
Point p1 = cntr + 0.02 * Point(static_cast<int>(eigen_vecs[0].x * eigen_val[0]), static_cast<int>(eigen_vecs[0].y * eigen_val[0]));
Point p2 = cntr - 0.02 * Point(static_cast<int>(eigen_vecs[1].x * eigen_val[1]), static_cast<int>(eigen_vecs[1].y * eigen_val[1]));
drawAxis(img, cntr, p1, Scalar(0, 255, 0), 1);
drawAxis(img, cntr, p2, Scalar(255, 255, 0), 5);
double angle = atan2(eigen_vecs[0].y, eigen_vecs[0].x); // Angle in radians.
double degrees = angle * 180 / CV_PI; // Convert radians to degrees (0-180 range).
degrees = degrees < 0 ? degrees + 180 : degrees;
return degrees;
}
double Segmentation::getOrientation(std::vector<cv::Point> &pts, cv::Mat &img)
{
if (pts.size() == 0) return false;
//Construct a buffer used by the pca analysis
cv::Mat data_pts = cv::Mat(pts.size(), 2, CV_64FC1);
for (int i = 0; i < data_pts.rows; ++i)
{
data_pts.at<double>(i, 0) = pts[i].x;
data_pts.at<double>(i, 1) = pts[i].y;
}
//Perform PCA analysis
cv::PCA pca_analysis(data_pts, cv::Mat(), CV_PCA_DATA_AS_ROW);
//Store the position of the object
cv::Point2i pos = cv::Point2i(static_cast<int>(pca_analysis.mean.at<double>(0, 0)),
static_cast<int>(pca_analysis.mean.at<double>(0, 1)));
//Store the eigenvalues and eigenvectors
std::vector<cv::Point2d> eigen_vecs(2);
std::vector<double> eigen_val(2);
for (int i = 0; i < 2; ++i)
{
eigen_vecs[i] = cv::Point2d(pca_analysis.eigenvectors.at<double>(i, 0),
pca_analysis.eigenvectors.at<double>(i, 1));
eigen_val[i] = pca_analysis.eigenvalues.at<double>(0, i);
}
// Draw the principal components
cv::circle(img, pos, 3, CV_RGB(255, 0, 255), 2);
cv::line(img, pos, pos + 0.02 * cv::Point(static_cast<int>(eigen_vecs[0].x * eigen_val[0]),static_cast<int> (eigen_vecs[0].y * eigen_val[0])) , CV_RGB(255, 255, 0));
cv::line(img, pos, pos + 0.02 * cv::Point(static_cast<int>(eigen_vecs[1].x * eigen_val[1]), static_cast<int>(eigen_vecs[1].y * eigen_val[1])) , CV_RGB(0, 255, 255));
return atan2(eigen_vecs[0].y, eigen_vecs[0].x);
}
void Projector::objProjectionOffline(std::string objPath, std::string objName, bool gpuView)
{
std::cout << "Camera init: ";
objObject obj(objPath, objName);
obj.loadData();
cout << "DONE\n";
cv::namedWindow("objTest", CV_WINDOW_NORMAL);
cv::moveWindow("objTest", 0, 0);
indices.resize(480);
for (int i = 0; i < 480; i++) indices[i].resize(640);
libfreenect2::Registration* registration = new libfreenect2::Registration(_dev->getIrCameraParams(), _dev->getColorCameraParams());
libfreenect2::Frame undistorted(512, 424, 4), registered(512, 424, 4);
libfreenect2::FrameMap frames;
SimpleViewer viewer;
bool shutdown = false;
cv::Mat board(480, 640, CV_8UC4, cv::Scalar::all(255));
cv::Vec3f prevNormal(-1, -1, -1);
if (!gpuView) {
cv::namedWindow("reprojection", CV_WINDOW_NORMAL);
cv::moveWindow("reprojection", 200, 200);
//setWindowProperty("reprojection", CV_WND_PROP_FULLSCREEN, CV_WINDOW_FULLSCREEN);
}
else {
viewer.setSize(480, 640); // TO-DO change resolution
viewer.initialize();
libfreenect2::Frame b(640, 480, 4);
b.data = board.data;
viewer.addFrame("RGB", &b);
shutdown = shutdown || viewer.render();
}
while (!shutdown)
{
board = cv::Mat(480, 640, CV_8UC4, cv::Scalar::all(255));
std::vector<cv::Point3f> plnSrc;
if (!gpuView) cv::imshow("reprojection", board);
(_listener)->waitForNewFrame(frames);
libfreenect2::Frame *rgb = frames[libfreenect2::Frame::Color];
libfreenect2::Frame *depth = frames[libfreenect2::Frame::Depth];
registration->apply(rgb, depth, &undistorted, ®istered, true, NULL, NULL);
PlaneData pln = findRectangle(registration, &undistorted, ®istered);
if (pln.points.size() > 0) {
std::vector<cv::Point2f> projected = projectPoints(pln.points);
cv::Mat cont = cv::Mat(480, 640, CV_8UC1, cv::Scalar::all(0));
for (int i = 0; i < projected.size(); i++)
{
if (480 - projected[i].x >0 && projected[i].y > 0 && 480 - projected[i].x < 475 && projected[i].y < 630) {
cv::Mat ROI = board(cv::Rect(static_cast<int>(projected[i].y), static_cast<int>(480 - projected[i].x), 2, 2));
ROI.setTo(cv::Scalar(250, 100, 100, 100));
cont.at<uchar>(static_cast<int>(480 - projected[i].x), static_cast<int>(projected[i].y), 0) = 255;
plnSrc.push_back(pln.points[i]);
}
}
vector<vector<cv::Point> > contours;
vector<cv::Vec4i> hierarchy;
cv::GaussianBlur(cont, cont, cv::Size(7, 7), 5, 11);
findContours(cont, contours, hierarchy, cv::RETR_CCOMP, cv::CHAIN_APPROX_NONE, cv::Point(0, 0));
vector<vector<cv::Point> > contours_poly(contours.size());
vector<cv::Rect> boundRect(contours.size());
vector<cv::Point2f>center(contours.size());
vector<float>radius(contours.size());
for (int i = 0; i < contours.size(); i++)
{
cv::approxPolyDP(cv::Mat(contours[i]), contours_poly[i], 10, true);
}
for (int i = 0; i < contours.size(); i++)
{
drawContours(board, contours_poly, 0, cv::Scalar(0, 255, 0), 5);
}
cv::Mat data_pts = cv::Mat(300, 3, CV_64FC1);
cv::Vec3f normal(0, 0, 0);
int jump = plnSrc.size() / 300;
for (int i = 0; i < 100; i++) {
data_pts.at<double>(i, 0) = plnSrc[i*jump].x;
data_pts.at<double>(i, 1) = plnSrc[i*jump].y;
data_pts.at<double>(i, 2) = plnSrc[i*jump].z;
data_pts.at<double>(i + 100, 0) = plnSrc[(i + 100)*jump].x;
data_pts.at<double>(i + 100, 1) = plnSrc[(i + 100)*jump].y;
data_pts.at<double>(i + 100, 2) = plnSrc[(i + 100)*jump].z;
data_pts.at<double>(i + 200, 0) = plnSrc[(i + 200) *jump].x;
data_pts.at<double>(i + 200, 1) = plnSrc[(i + 200)*jump].y;
data_pts.at<double>(i + 200, 2) = plnSrc[(i + 200)*jump].z;
}
cv::PCA pca_analysis(data_pts, cv::Mat(), CV_PCA_DATA_AS_ROW);
cv::Vec3f cntr = cv::Vec3f((pca_analysis.mean.at<double>(0, 0)),
(pca_analysis.mean.at<double>(0, 1)),
(pca_analysis.mean.at<double>(0, 2)));
vector<cv::Point3f> eigen_vecs(2);
vector<double> eigen_val(2);
for (int i = 0; i < 2; ++i)
{
eigen_vecs[i] = cv::Point3f(pca_analysis.eigenvectors.at<double>(i, 0),
pca_analysis.eigenvectors.at<double>(i, 1),
pca_analysis.eigenvectors.at<double>(i, 2));
eigen_val[i] = pca_analysis.eigenvalues.at<double>(0, i);
}
cv::Vec3f p1 = cv::Vec3f((eigen_vecs[0].x * eigen_val[0]), (eigen_vecs[0].y * eigen_val[0]), (eigen_vecs[0].z * eigen_val[0]));
cv::Vec3f p2 = cv::Vec3f((eigen_vecs[1].x * eigen_val[1]), (eigen_vecs[1].y * eigen_val[1]), (eigen_vecs[1].z * eigen_val[1]));
normal = p1.cross(p2);
normal = cv::normalize(normal);
//pln.center = cntr;
pln.normal = normal;
obj.setCamera(cv::Point3f(pln.center.x, -pln.center.y, -pln.center.z + 150),
cv::Vec3f(pln.normal[0], pln.normal[1], pln.normal[2]));
if (!gpuView) imshow("reprojection", board);
else {
libfreenect2::Frame b(640, 480, 4);
b.data = board.data;
viewer.addFrame("RGB", &b);
shutdown = shutdown || viewer.render();
}
}
cv::Mat im = obj.render();
cv::imshow("objTest", im);
//}
(_listener)->release(frames);
if (!gpuView) {
int op = cv::waitKey(50);
if (op == 100 || (char)(op) == 'd') right -= 1;
if (op == 115 || (char)(op) == 's') up += 1;
if (op == 97 || (char)(op) == 'a') right += 1;
if (op == 119 || (char)(op) == 'w') up -= 1;
if (op == 114 || (char)(op) == 'r') rotX -= 0.5;
if (op == 102 || (char)(op) == 'f') rotX += 0.5;
if (op == 1113997 || op == 1048586 || op == 1048608 || op == 10 || op == 32)
{
std::cout << "right = " << right << ";\nup = " << up << ";\nrotX = " << rotX << ";\n";
break;
}
}
else {
//right = 0;
//up = 0;
//rotX = 0;
right = viewer.offsetX;
up = viewer.offsetY;
rotX = viewer.rot;
}
}
if (!gpuView) cv::destroyWindow("reprojection");
else {
viewer.stopWindow();
}
cv::destroyWindow("objTest");
}
