From 36dda2abab5d00d670098a858b6811ea519677c9 Mon Sep 17 00:00:00 2001
From: mrgrandsky <farid.alijani@student.lut.fi>
Date: Fri, 29 Jul 2016 18:25:41 +0200
Subject: [PATCH] a new AR added
---
Files_4_thesis/docking_1.asv | 405 -----------------------------------
MobileRobot/ej6.cpp | 172 +++++++++++++++
2 files changed, 172 insertions(+), 405 deletions(-)
delete mode 100644 Files_4_thesis/docking_1.asv
create mode 100644 MobileRobot/ej6.cpp
diff --git a/Files_4_thesis/docking_1.asv b/Files_4_thesis/docking_1.asv
deleted file mode 100644
index b198c32b..00000000
--- a/Files_4_thesis/docking_1.asv
+++ /dev/null
@@ -1,405 +0,0 @@
-clc;
-close all;
-clear all;
-%% Import data and time conversion
-
-% data 1: 556 ros messages for geometry_msgs/Twist
-% 557 ros messages for geometry_msgs/PoseStamped
-
-% y-axis gains:
-% (280716_2024) better!!!
-% P = .44; I = 0; D = 0;
-
-% theta-axis gains:
-% P = .08; I = 0; D = 0;
-
-% y-axis gains:
-% (280716_2002)
-% P = .46; I = 0; D = 0;
-
-% theta-axis gains:
-% P = .08; I = 0; D = 0;
-
-Pose_Matrix_data_1 = csvread('Pose28_7_16_2024.txt',1,0);
-Vel_Matrix_data_1 = csvread('Velocity28_7_16_2024.txt',1,0);
-
-% time in position
-TimeP_ros_data_1 = Pose_Matrix_data_1(:,1); % ros time, needs to be converted to sec...
-
-% duriation = end_time - start_time
- durationP_data_1 = (TimeP_ros_data_1(size(TimeP_ros_data_1,1),:) - TimeP_ros_data_1(1,:))*10^(-9);
-
-t_P_sec_data_1 = 0:durationP_data_1/size(TimeP_ros_data_1,1):durationP_data_1;
-t_P_sec_data_1(:,size(t_P_sec_data_1,2)) = [];
-
-% time in vel
-TimeV_ros_data_1 = Vel_Matrix_data_1(:,1); % ros time, needs to be converted to sec...
-
-% duriation = end_time - start_time
- durationV_data_1 = (TimeV_ros_data_1(size(TimeV_ros_data_1,1),:) - TimeV_ros_data_1(1,:))*10^(-9);
-
-t_V_sec_data_1 = 0:durationV_data_1/size(TimeV_ros_data_1,1):durationV_data_1;
-t_V_sec_data_1(:,size(t_V_sec_data_1,2)) = [];
-
-% -----------------------------------------------------------------------------------------------------
-% data 2: 588 ros messages for geometry_msgs/Twist
-% 589 ros messages for geometry_msgs/PoseStamped
-% y-axis gains:
-% P = .5; I = 0.001; D = 0.03;
-
-% theta-axis gains:
-% P = .28; I = 0.1; D = 0.15;
-
-Pose_Matrix_data_2 = csvread('Pose18_7_16_1815.txt',1,0);
-Vel_Matrix_data_2 = csvread('Velocity18_7_16_1815.txt',1,0);
-
-% time in position
-TimeP_ros_data_2 = Pose_Matrix_data_2(:,1); % ros time, needs to be converted to sec...
-
-% duriation = end_time - start_time
- durationP_data_2 = (TimeP_ros_data_2(size(TimeP_ros_data_2,1),:) - TimeP_ros_data_2(1,:))*10^(-9);
-
-t_P_sec_data_2 = 0:durationP_data_2/size(TimeP_ros_data_2,1):durationP_data_2;
-t_P_sec_data_2(:,size(t_P_sec_data_2,2)) = [];
-
-% time in vel
-TimeV_ros_data_2 = Vel_Matrix_data_2(:,1); % ros time, needs to be converted to sec...
-
-% duriation = end_time - start_time
- durationV_data_2 = (TimeV_ros_data_2(size(TimeV_ros_data_2,1),:) - TimeV_ros_data_2(1,:))*10^(-9);
-
-t_V_sec_data_2 = 0:durationV_data_2/size(TimeV_ros_data_2,1):durationV_data_2;
-t_V_sec_data_2(:,size(t_V_sec_data_2,2)) = [];
-
-% -----------------------------------------------------------------------------------------------------
-% data 3: 628 ros messages for geometry_msgs/Twist
-% 629 ros messages for geometry_msgs/PoseStamped
-% y-axis gains:
-% P = .44; I = 0; D = 0.07;
-
-% theta-axis gains:
-% P = .08; I = 0; D = 0;
-
-Pose_Matrix_data_3 = csvread('Pose28_7_16_2052.txt',1,0);
-Vel_Matrix_data_3 = csvread('Velocity18_7_16_2052.txt',1,0);
-
-% time in position
-TimeP_ros_data_3 = Pose_Matrix_data_3(:,1); % ros time, needs to be converted to sec...
-
-% duriation = end_time - start_time
- durationP_data_3 = (TimeP_ros_data_3(size(TimeP_ros_data_3,1),:) - TimeP_ros_data_3(1,:))*10^(-9);
-
-t_P_sec_data_3 = 0:durationP_data_3/size(TimeP_ros_data_3,1):durationP_data_3;
-t_P_sec_data_3(:,size(t_P_sec_data_3,2)) = [];
-
-% time in vel
-TimeV_ros_data_3 = Vel_Matrix_data_3(:,1); % ros time, needs to be converted to sec...
-
-% duriation = end_time - start_time
- durationV_data_3 = (TimeV_ros_data_3(size(TimeV_ros_data_3,1),:) - TimeV_ros_data_3(1,:))*10^(-9);
-
-t_V_sec_data_3 = 0:durationV_data_3/size(TimeV_ros_data_3,1):durationV_data_3;
-t_V_sec_data_3(:,size(t_V_sec_data_3,2)) = [];
-
-
-%% Pose estimation
-
-%data 1:
-% when using marker pose ,,,,
-Pose_X_data_1 = Pose_Matrix_data_1(:,4);
-Pose_Y_data_1 = Pose_Matrix_data_1(:,3);
-Theta_data_1 = Pose_Matrix_data_1(:,5);
-
-
-% when using robot odometry ,,,,
-% Pose_X = Pose_Matrix(:,2);
-% Pose_Y = Pose_Matrix(:,3);
-% Theta = Pose_Matrix(:,7);
-
-% Extracting reference values when the robot is manually docked!
-ref_X_data_1 = Pose_X_data_1(size(Pose_X_data_1,1));
-ref_Y_data_1 = Pose_Y_data_1(size(Pose_Y_data_1,1));
-ref_Theta_data_1 = Theta_data_1(size(Theta_data_1,1));
-ref_Pose_data_1 =[ref_X_data_1;ref_Y_data_1];
-
-%data 2:
-% when using marker pose ,,,,
-Pose_X_data_2 = Pose_Matrix_data_2(:,4);
-Pose_Y_data_2 = Pose_Matrix_data_2(:,3);
-Theta_data_2 = Pose_Matrix_data_2(:,5);
-
-% when using robot odometry ,,,,
-% Pose_X = Pose_Matrix(:,2);
-% Pose_Y = Pose_Matrix(:,3);
-% Theta = Pose_Matrix(:,7);
-
-% Extracting reference values when the robot is manually docked!
-ref_X_data_2 = Pose_X_data_2(size(Pose_X_data_2,1));
-ref_Y_data_2 = Pose_Y_data_2(size(Pose_Y_data_2,1));
-ref_Theta_data_2 = Theta_data_2(size(Theta_data_2,1));
-ref_Pose_data_2 =[ref_X_data_2;ref_Y_data_2];
-
-%data 3:
-% when using marker pose ,,,,
-Pose_X_data_3 = Pose_Matrix_data_3(:,4);
-Pose_Y_data_3 = Pose_Matrix_data_3(:,3);
-Theta_data_3 = Pose_Matrix_data_3(:,5);
-
-% when using robot odometry ,,,,
-% Pose_X = Pose_Matrix(:,2);
-% Pose_Y = Pose_Matrix(:,3);
-% Theta = Pose_Matrix(:,7);
-
-% Extracting reference values when the robot is manually docked!
-ref_X_data_3 = Pose_X_data_3(size(Pose_X_data_3,1));
-ref_Y_data_3 = Pose_Y_data_3(size(Pose_Y_data_3,1));
-ref_Theta_data_3 = Theta_data_3(size(Theta_data_3,1));
-ref_Pose_data_3 =[ref_X_data_3;ref_Y_data_3];
-
-thresh_X = .001;
-theta = 0:.001:2*pi;
-
-% needs to be adjusted manually if docking platform is replaced!
-ref_x = .19979;
-ref_y = .00903;
-
-x_circle = thresh_X*cos(theta) + ref_x; % ref_X needs to be recorded
-y_circle = thresh_X*sin(theta) + ref_y; % ref_Y needs to be recorded
-
-
-%% Velocity estimation
-
-% data 1:
-Vel_X_data_1 = Vel_Matrix_data_1(:,2);
-Vel_Y_data_1 = Vel_Matrix_data_1(:,3);
-Omega_Z_data_1 = Vel_Matrix_data_1(:,7);
-
-% data 2:
-Vel_X_data_2 = Vel_Matrix_data_2(:,2);
-Vel_Y_data_2 = Vel_Matrix_data_2(:,3);
-Omega_Z_data_2 = Vel_Matrix_data_2(:,7);
-
-% data 3:
-Vel_X_data_3 = Vel_Matrix_data_3(:,2);
-Vel_Y_data_3 = Vel_Matrix_data_3(:,3);
-Omega_Z_data_3 = Vel_Matrix_data_3(:,7);
-
-
-%% Plots
-
-% for the presenation all pose estimation(x,y,theta) in one plot
-
-% figure;
-% plot(t_P_sec,Pose_X,'b',t_P_sec,Pose_Y,'r',t_P_sec,Theta,'g');
-%
-% title('Marker Pose');
-% xlabel('Time [sec]');
-% ylabel('Pose');
-% legend('X [m]','Y [m]','\theta [rad]');
-% grid on
-
-% marker position
-figure;
-subplot(3,1,1);
-plot(t_P_sec_data_1,Pose_X_data_1);
-hold on
-plot(t_P_sec_data_2,Pose_X_data_2,'r');
-hold on
-plot(t_P_sec_data_3,Pose_X_data_3,'g');
-
-title('Marker X-axis');
-% xlabel('Time [sec]');
-ylabel('X [m]');
-grid on
-
-subplot(3,1,2);
-plot(t_P_sec_data_1,Pose_Y_data_1);
-hold on
-plot(t_P_sec_data_2,Pose_Y_data_2,'r');
-hold on
-plot(t_P_sec_data_3,Pose_Y_data_3,'g');
-
-title('Marker Y-axis');
-% xlabel('Time [sec]');
-ylabel('Y [m]');
-grid on
-
-subplot(3,1,3);
-plot(t_P_sec_data_1,Theta_data_1);
-hold on
-plot(t_P_sec_data_2,Theta_data_2,'r');
-hold on
-plot(t_P_sec_data_3,Theta_data_3,'g');
-
-title('Marker \theta-axis');
-xlabel('Time [sec]');
-ylabel('\theta [rad]');
-grid on
-
-% control signals
-figure;
-subplot(3,1,1);
-
-plot(t_V_sec_data_1,Vel_X_data_1,'LineWidth',1.2);
-hold on
-plot(t_V_sec_data_2,Vel_X_data_2,'r','LineWidth',1.2);
-hold on
-plot(t_V_sec_data_3,Vel_X_data_3,'g','LineWidth',1.2);
-
-title('Control Signal X-axis');
-% xlabel('Time [sec]');
-ylabel('V_x [m/s]');
-grid on
-% axis([0 46 -.01 .11]);
-
-subplot(3,1,2);
-plot(t_V_sec_data_1,Vel_Y_data_1,'LineWidth',1.2);
-hold on
-plot(t_V_sec_data_2,Vel_Y_data_2,'r','LineWidth',1.2);
-hold on
-plot(t_V_sec_data_3,Vel_Y_data_3,'g','LineWidth',1.2);
-
-title('Control Signal Y-axis');
-% xlabel('Time [sec]');
-ylabel('V_y [m/s]');
-grid on
-% axis([0 46 -.2 .1]);
-
-subplot(3,1,3);
-plot(t_V_sec_data_1,Omega_Z_data_1,'LineWidth',1.2);
-hold on
-plot(t_V_sec_data_2,Omega_Z_data_2,'r','LineWidth',1.2);
-hold on
-plot(t_V_sec_data_3,Omega_Z_data_3,'g','LineWidth',1.2);
-
-title('Control Signal \theta-axis');
-xlabel('Time [sec]');
-ylabel('\omega [rad/s]');
-grid on
-% axis([0 46 -.1 .15]);
-
-% Trajectory
-% figure;
-% plot(Pose_Y_data_1,Pose_X_data_1,'b',ref_Y_data_1,ref_X_data_1,'k*');
-% hold on
-% plot(Pose_Y_data_2,Pose_X_data_2,'r',ref_Y_data_2,ref_X_data_2,'k*');
-%
-% hold on
-% plot(Pose_Y_data_3,Pose_X_data_3,'g',ref_Y_data_3,ref_X_data_3,'k*');
-%
-
-%
-% for k = Pose_X_data_1(1):Pose_X_data_1(size(Pose_X_data_1,1))
-% x_arrow(k) =
-%
-% end
-%
-% x_arrow = linspace(Pose_X_data_1(1),Pose_X_data_1(size(Pose_X_data_1,1)),150);
-% y_arrow = linspace(Pose_Y_data_1(1),Pose_Y_data_1(size(Pose_Y_data_1,1)),150);
-%
-% Vx = gradient(x_arrow,.01);
-% Vy = gradient(y_arrow,.01);
-%
-% figure;
-% plot(y_arrow,x_arrow,'b--','LineWidth',.5);
-% % hold on
-% % quiver(y_arrow,x_arrow,Vy,Vx,'r--','LineWidth',1.5);
-%
-
-
-figure;
-subplot(1,3,1);
-plot(Pose_Y_data_1,Pose_X_data_1,'b','LineWidth',2);
-hold on
-plot(Pose_Y_data_2,Pose_X_data_2,'r','LineWidth',2);
-hold on
-plot(Pose_Y_data_3,Pose_X_data_3,'g','LineWidth',2);
-hold on
-plot(y_circle,x_circle,'k--','LineWidth',3.4)
-
-title('Docking Trajectroy (Whole area)');
-xlabel('Y [m]');
-ylabel('X [m]');
-axis([-.3 .2 .18 1.3]);
-% legend('Trajectory 1','Reference Position 1', 'Trajectory 2','Reference Position 2','Trajectory 3','Reference Position 3');
-grid on
-
-subplot(1,3,2);
-plot(Pose_Y_data_1,Pose_X_data_1,'b','LineWidth',2);
-hold on
-plot(Pose_Y_data_2,Pose_X_data_2,'r','LineWidth',2);
-hold on
-plot(Pose_Y_data_3,Pose_X_data_3,'g','LineWidth',2);
-hold on
-plot(y_circle,x_circle,'k--','LineWidth',3.4);
-title('Docking Trajectroy (SM zone)');
-xlabel('Y [m]');
-ylabel('X [m]');
-% axis([-.08 0 .265 .355]);
-grid on
-
-subplot(1,3,3);
-plot(Pose_Y_data_1,Pose_X_data_1,'b','LineWidth',2);
-hold on
-plot(Pose_Y_data_2,Pose_X_data_2,'r','LineWidth',2);
-hold on
-plot(Pose_Y_data_3,Pose_X_data_3,'g','LineWidth',2);
-hold on
-plot(y_circle,x_circle,'k--','LineWidth',3.4);
-title('Docking Trajectroy (reference area)');
-xlabel('Y [m]');
-ylabel('X [m]');
-% axis([-.04 -.03 .266 .269]);
-grid on
-
-figure;
-subplot(2,1,1);
-plot(diff(TimeP_ros_data_1)/1e9,'g')
-hold on
-plot (diff(t_P_sec_data_1),'k--')
-title('Position sampling time');
-ylabel('Sampling time');
-legend('ROS calc.','Real time calc. [sec]','Orientation','horizontal');
-% grid on
-% axis([0 428 .05 .15]);
-
-subplot(2,1,2);
-plot(diff(TimeV_ros_data_1)/1e9,'g');
-hold on
-plot (diff(t_V_sec_data_1),'k--')
-title('Velocity sampling time');
-xlabel('samples');
-ylabel('Sampling time');
-% legend('ROS calc.','Ordinary calc.','Orientation','horizontal');
-% grid on
-% axis([0 428 -.005 .025]);
-%
-% subplot(3,1,3);
-% plot(diff(TimeV_ros_data_1)/1e9,'g')
-% hold on
-% plot (diff(t_V_sec_data_1),'k--')
-% xlabel('samples');
-% ylabel('time step');
-% axis([150 250 0 .02]);
-
-figure;
-subplot(3,1,1);
-plot(diff(Pose_X_data_1));
-title('X-Pose diff');
-xlabel('samples');
-ylabel('X difference [m]');
-grid on
-
-subplot(3,1,2);
-plot(diff(Pose_X_data_1));
-title('Y-Pose diff');
-xlabel('samples');
-ylabel('Y difference [m]');
-grid on
-
-subplot(3,1,3);
-plot(diff(Theta_data_1));
-title('\theta-Pose diff');
-xlabel('samples');
-ylabel('\theta difference [rad]');
-grid on
\ No newline at end of file
diff --git a/MobileRobot/ej6.cpp b/MobileRobot/ej6.cpp
new file mode 100644
index 00000000..faada45b
--- /dev/null
+++ b/MobileRobot/ej6.cpp
@@ -0,0 +1,172 @@
+//*************************************************************************
+// Example program using OpenCV library
+//
+// Luis M. Jim�nez
+//
+// Course: Computer Vision (1782)
+// Dpo. of Systems Engineering and Automation
+// Automation, Robotics and Computer Vision Lab (ARVC)
+// http://arvc.umh.es
+// University Miguel Hern�ndez
+//
+// Description:
+// - Loads camera calibration data from file (YAML type)
+// - Capture images from a camera (decrease resolution)
+// - Detects markers using ARUCO library
+// - Calculates POSE from camera calibration file
+// - Draws 3D cube on each maker over captured image
+//
+//*************************************************************************
+
+#pragma warning( disable : 4290 ) // avoids warning 4290
+
+#include <cassert> // debugging
+#include <iostream> // c++ standar input/output
+#include <sstream> // string to number conversion
+#include <string> // handling strings
+#include <vector> // handling vectors
+#include <math.h> // math functions
+#include <stdio.h> // standar C input/ouput
+
+#include <opencv2/opencv.hpp> // OpenCV library headers
+#include <opencv2/nonfree/nonfree.hpp> // Nonfree opencv library headers
+
+// ARUCO Library (Markers)
+#include <aruco/aruco.h>
+
+
+//*************************************************************************
+// C++ namespaces C++ (avoid using prefix for starndar and openCV classes)
+//*************************************************************************
+using namespace std;
+using namespace cv;
+
+
+
+//*************************************************************************
+// Function prototypes
+//*************************************************************************
+
+
+//*************************************************************************
+// Constants
+//*************************************************************************
+const char * WINDOW_CAMERA1 = "(W1) Camera 1 (Marker Detector)"; // windows id
+
+//*************************************************************************
+// Variables Globales
+//*************************************************************************
+int CAMERA_ID = 0; // default camera
+
+
+//*************************************************************************
+// Funciones
+//*************************************************************************
+
+int main (int argc, char * const argv[])
+{
+ int key;
+ VideoCapture camera; // Cameras
+ Mat capture; // Images
+ Mat gray_image;
+
+ locale::global(locale("spanish")); // Character set (so you can use accents)
+
+ Size camSize(640, 480); // capture resolution
+
+ bool markerWasFound = false;
+ aruco::MarkerDetector MDetector; // handler for marker detector
+ vector<aruco::Marker> Markers; // storage for detected markers
+
+ float markerSize = (float)0.076; // size of ARUCO marker (meters)
+ aruco::BoardConfiguration boardInfo; // ARUCO board info
+ aruco::CameraParameters cameraParameters; //ARUCO class for camera parameters
+
+ Mat cameraMatrix, distCoeffs; // Intrinsic Camera Calibration parameters
+
+ // Load ARUCO board info
+ boardInfo.readFromFile("ARUCO_board.yaml");
+
+ // Load camera calibration data
+ FileStorage fs("camera.yaml", FileStorage::READ);
+ if (fs.isOpened())
+ {
+ fs["camera_matrix"] >> cameraMatrix;
+ fs["distortion_coefficients"] >> distCoeffs;
+ cout << "Camera Calibration Matrix A: " << endl << cameraMatrix << endl;
+ cout << "Distortion Coefs: " << distCoeffs << endl;
+
+ // configure internal ARUCO cameraParameters object
+ cameraParameters.setParams(cameraMatrix, distCoeffs, camSize);
+ }
+ else
+ {
+ cout << "you need a camera calibration file, sorry.\n";
+ getchar(); // wait for a keystroke and exits
+ return -1;
+ }
+
+ // check command line parameters (camera id)
+ if(argc>2 && string(argv[1])=="-c")
+ CAMERA_ID = atoi(argv[2]);
+
+ // Configure cameras
+ camera.open(CAMERA_ID); // open camera
+ if (!camera.isOpened())
+ {
+ cout << "you need to connect a camera, sorry.\n";
+ getchar(); // wait for a keystroke and exits
+ return -1;
+ }
+ //set capture properties (low resolution to speed up detection)
+ camera.set(CV_CAP_PROP_FRAME_WIDTH, camSize.width);
+ camera.set(CV_CAP_PROP_FRAME_HEIGHT, camSize.height);
+
+
+ // Create the visualization windows
+ namedWindow (WINDOW_CAMERA1, WINDOW_AUTOSIZE);
+
+ cout << "Capturing images.\n Hit q/Q to exit.\n";
+
+ // while there are images ...
+ while (camera.read(capture))
+ {
+ if(capture.empty())
+ continue; // capture has failed, continue
+
+ cvtColor( capture, gray_image, CV_BGR2GRAY ); // transforms to gray level
+
+ MDetector.detect(gray_image, Markers); // detects markers on image
+ markerWasFound = (Markers.size()>0); // checks if some marker was found
+
+ if(markerWasFound)
+ {
+ //for each marker calculates POSE and draw 3D cube
+ for (unsigned int i=0; i<Markers.size(); i++)
+ {
+ Markers[i].calculateExtrinsics(markerSize, cameraParameters, false);
+ cout << "Marker id:" << Markers[i].id << " Rvec: " << Markers[i].Rvec << " Tvec: " << Markers[i].Tvec << endl;
+
+ Markers[i].draw(capture, Scalar(0,0,255), 1);
+ aruco::CvDrawingUtils::draw3dCube(capture, Markers[i], cameraParameters);
+
+ }
+ // Draws coordinate axis for first marker
+ aruco::CvDrawingUtils::draw3dAxis(capture, Markers[0], cameraParameters);
+ }
+
+ imshow(WINDOW_CAMERA1, capture); // show image in a window
+
+ // wait 10ms for a keystroke to exit (image window must be on focus)
+ key = waitKey (10);
+ if (key == 'q' || key == 'Q')
+ break;
+ }
+
+ // free windows and camera resources
+ destroyAllWindows();
+ if (camera.isOpened()) camera.release();
+
+ // programm exits with no errors
+ return 0;
+}
--
GitLab