diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h
index befd277b2d1e11ed573249e070e69cc769afbbea..cd343e7fc71f95e7b22baef5e9e709ea690d64a8 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h
@@ -119,9 +119,8 @@ public:
- static double prop_gain;
- static double integ_gain;
- static double deriv_gain;
+ static double Pos_P,Pos_I,Pos_D;
+ static double Ang_P,Ang_I,Ang_D;
// ---- CONTROLL PARAMETERS ------ //
static double prev_errorX;
@@ -153,7 +152,8 @@ public:
// ---- CONTROLL PARAMETERS ------ //
- static double zeroMin,zeroMax,eps;
+ static double zeroMin,zeroMax;
+ static double P_eps,A_eps;
double marpose[6];
double campose[6];
diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~
index ee062152358fbb77bdcff65de6bef288e88d0496..cd343e7fc71f95e7b22baef5e9e709ea690d64a8 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~
@@ -96,7 +96,7 @@ private:
void Controller(double RefX, double MarPoseX, double refY, double MarPoseY, double refYAW, double MarPoseYAW, double dt);
void dock(double VelX, double VelY, double omegaZ);
- //void dock(double speed[6]);
+
public:
ImageConverter();
@@ -119,9 +119,8 @@ public:
- static double prop_gain;
- static double integ_gain;
- static double deriv_gain;
+ static double Pos_P,Pos_I,Pos_D;
+ static double Ang_P,Ang_I,Ang_D;
// ---- CONTROLL PARAMETERS ------ //
static double prev_errorX;
@@ -153,7 +152,8 @@ public:
// ---- CONTROLL PARAMETERS ------ //
- static double zeroMin,zeroMax,eps;
+ static double zeroMin,zeroMax;
+ static double P_eps,A_eps;
double marpose[6];
double campose[6];
diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
index d3fb9faacecd172f053a28695759ef342cc0023a..63ad81f3aa80f07cb1affb80e7a83a6ea9258ae1 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
@@ -43,17 +43,22 @@ using namespace cv;
using namespace aruco;
using namespace std;
-double ImageConverter::prop_gain = .01;
-double ImageConverter::integ_gain = .5;
-double ImageConverter::deriv_gain = 0;
+double ImageConverter::Pos_P = .01;
+double ImageConverter::Pos_I = .5;
+double ImageConverter::Pos_D = 0;
+
+
+double ImageConverter::Ang_P = .001;
+double ImageConverter::Ang_I = .05;
+double ImageConverter::Ang_D = 0;
float ImageConverter::TheMarkerSize = -1;
int ImageConverter::Thresh1_min = 5;
int ImageConverter::Thresh2_min = 5;
-int ImageConverter::Thresh1_max = 100;
-int ImageConverter::Thresh2_max =100;
+int ImageConverter::Thresh1_max = 200;
+int ImageConverter::Thresh2_max = 200;
const string ImageConverter::trackbarWindowName = "Trackbars";
@@ -98,7 +103,8 @@ double ImageConverter::control_signalYAW;
double ImageConverter::zeroMax = .0000000000000000001;
double ImageConverter::zeroMin = -.0000000000000000001;
-double ImageConverter::eps = .0000001;
+double ImageConverter::P_eps = .01;
+double ImageConverter::A_eps = .1;
ImageConverter::ImageConverter() :
@@ -173,10 +179,12 @@ void ImageConverter::myhandler(int value)
}
void ImageConverter::createTrackbars()
{
+
namedWindow(trackbarWindowName, 0);
+
createTrackbar("ThresParam 1", trackbarWindowName, &Thresh1_min, Thresh1_max, cvTackBarEvents, this);
createTrackbar("ThresParam 2", trackbarWindowName, &Thresh2_min, Thresh2_max, cvTackBarEvents, this);
- MDetector.setCornerRefinementMethod(MarkerDetector::LINES);
+
}
void ImageConverter::imageCb(const sensor_msgs::ImageConstPtr& msg)
{
@@ -254,6 +262,11 @@ void ImageConverter::ProgStart(int argc,char** argv)
MDetector.pyrDown(ThePyrDownLevel);
}
+
+
+
+ MDetector.setCornerRefinementMethod(MarkerDetector::LINE);
+
char key=0;
int index=0;
// Capture until press ESC or until the end of the video
@@ -458,9 +471,9 @@ RefPose[3] = -.68952;
if (
- (abs(RefPose[1] - CamFB->pose.position.y) <= eps) && // Y
- (abs(RefPose[2] - CamFB->pose.position.z) <= eps) && // Z
- (abs(RefPose[3] - CamFB->pose.orientation.x) <= eps) // Yaw
+ (abs(RefPose[1] - CamFB->pose.position.y) <= P_eps) && // Y
+ (abs(RefPose[2] - CamFB->pose.position.z) <= P_eps) && // Z
+ (abs(RefPose[3] - CamFB->pose.orientation.x) <= A_eps) // Yaw
)
{
@@ -493,9 +506,9 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl
diffX = ((curr_errorX - prev_errorX) / dt);
// scalling
- p_termX = prop_gain * curr_errorX;
- i_termX = integ_gain * int_errorX;
- d_termX = deriv_gain * diffX;
+ p_termX = Pos_P * curr_errorX;
+ i_termX = Pos_I * int_errorX;
+ d_termX = Pos_D * diffX;
// control signal
control_signalX = p_termX + i_termX + d_termX;
@@ -522,9 +535,9 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl
diffY = ((curr_errorY - prev_errorY) / dt);
// scalling
- p_termY = prop_gain * curr_errorY;
- i_termY = integ_gain * int_errorY;
- d_termY = deriv_gain * diffY;
+ p_termY = Pos_P * curr_errorY;
+ i_termY = Pos_I * int_errorY;
+ d_termY = Pos_D * diffY;
// control signal
control_signalY = p_termY + i_termY + d_termY;
@@ -551,9 +564,9 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl
diffYAW = ((curr_errorYAW - prev_errorYAW) / dt);
// scalling
- p_termYAW = prop_gain * curr_errorYAW;
- i_termYAW = integ_gain * int_errorYAW;
- d_termYAW = deriv_gain * diffYAW;
+ p_termYAW = Ang_P * curr_errorYAW;
+ i_termYAW = Ang_I * int_errorYAW;
+ d_termYAW = Ang_D * diffYAW;
// control signal
control_signalYAW = p_termYAW + i_termYAW + d_termYAW;
@@ -563,7 +576,7 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl
// for the next iteration.
prev_errorYAW = curr_errorYAW;
- dock(-control_signalX, -control_signalY, -control_signalYAW);
+ dock(-control_signalX, -control_signalY, control_signalYAW);
}
void ImageConverter::dock(double VelX, double VelY, double omegaZ)
{
diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
index 1868461d136e8ae7c4b09b3fe0a03a80b758c3bb..c94bc35d7231c2ea0617631665da8473ba13001c 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
@@ -43,17 +43,22 @@ using namespace cv;
using namespace aruco;
using namespace std;
-double ImageConverter::prop_gain = .01;
-double ImageConverter::integ_gain = .5;
-double ImageConverter::deriv_gain = 0;
+double ImageConverter::Pos_P = .01;
+double ImageConverter::Pos_I = .5;
+double ImageConverter::Pos_D = 0;
+
+
+double ImageConverter::Ang_P = .001;
+double ImageConverter::Ang_I = .05;
+double ImageConverter::Ang_D = 0;
float ImageConverter::TheMarkerSize = -1;
int ImageConverter::Thresh1_min = 5;
int ImageConverter::Thresh2_min = 5;
-int ImageConverter::Thresh1_max = 100;
-int ImageConverter::Thresh2_max =100;
+int ImageConverter::Thresh1_max = 200;
+int ImageConverter::Thresh2_max = 200;
const string ImageConverter::trackbarWindowName = "Trackbars";
@@ -98,7 +103,8 @@ double ImageConverter::control_signalYAW;
double ImageConverter::zeroMax = .0000000000000000001;
double ImageConverter::zeroMin = -.0000000000000000001;
-double ImageConverter::eps = .0000001;
+double ImageConverter::P_eps = .01;
+double ImageConverter::A_eps = .1;
ImageConverter::ImageConverter() :
@@ -173,10 +179,12 @@ void ImageConverter::myhandler(int value)
}
void ImageConverter::createTrackbars()
{
+
namedWindow(trackbarWindowName, 0);
+
createTrackbar("ThresParam 1", trackbarWindowName, &Thresh1_min, Thresh1_max, cvTackBarEvents, this);
createTrackbar("ThresParam 2", trackbarWindowName, &Thresh2_min, Thresh2_max, cvTackBarEvents, this);
- MDetector.setCornerRefinementMethod(MarkerDetector::LINES);
+
}
void ImageConverter::imageCb(const sensor_msgs::ImageConstPtr& msg)
{
@@ -228,9 +236,9 @@ void ImageConverter::ProgStart(int argc,char** argv)
const std::string vsa = "http://192.168.0.101:8080/video?x.mjpeg";
// -- publishing video stream with Android Camera--
- TheVideoCapturer.open(vsa);
+ //TheVideoCapturer.open(vsa);
- //TheVideoCapturer.open(0);
+ TheVideoCapturer.open(1);
// Check video is open
if (!TheVideoCapturer.isOpened())
@@ -254,6 +262,11 @@ void ImageConverter::ProgStart(int argc,char** argv)
MDetector.pyrDown(ThePyrDownLevel);
}
+
+
+
+ MDetector.setCornerRefinementMethod(MarkerDetector::LINE);
+
char key=0;
int index=0;
// Capture until press ESC or until the end of the video
@@ -458,9 +471,9 @@ RefPose[3] = -.68952;
if (
- (abs(RefPose[1] - CamFB->pose.position.y) <= eps) && // Y
- (abs(RefPose[2] - CamFB->pose.position.z) <= eps) && // Z
- (abs(RefPose[3] - CamFB->pose.orientation.x) <= eps) // Yaw
+ (abs(RefPose[1] - CamFB->pose.position.y) <= P_eps) && // Y
+ (abs(RefPose[2] - CamFB->pose.position.z) <= P_eps) && // Z
+ (abs(RefPose[3] - CamFB->pose.orientation.x) <= A_eps) // Yaw
)
{
@@ -493,9 +506,9 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl
diffX = ((curr_errorX - prev_errorX) / dt);
// scalling
- p_termX = prop_gain * curr_errorX;
- i_termX = integ_gain * int_errorX;
- d_termX = deriv_gain * diffX;
+ p_termX = Pos_P * curr_errorX;
+ i_termX = Pos_I * int_errorX;
+ d_termX = Pos_D * diffX;
// control signal
control_signalX = p_termX + i_termX + d_termX;
@@ -522,9 +535,9 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl
diffY = ((curr_errorY - prev_errorY) / dt);
// scalling
- p_termY = prop_gain * curr_errorY;
- i_termY = integ_gain * int_errorY;
- d_termY = deriv_gain * diffY;
+ p_termY = Pos_P * curr_errorY;
+ i_termY = Pos_I * int_errorY;
+ d_termY = Pos_D * diffY;
// control signal
control_signalY = p_termY + i_termY + d_termY;
@@ -563,7 +576,7 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl
// for the next iteration.
prev_errorYAW = curr_errorYAW;
- dock(-control_signalX, -control_signalY, -control_signalYAW);
+ dock(-control_signalX, -control_signalY, control_signalYAW);
}
void ImageConverter::dock(double VelX, double VelY, double omegaZ)
{
diff --git a/MobileRobot/AugReaMarker/ar_raw_detection/ar_det/src/ar_detection.cpp b/MobileRobot/AugReaMarker/ar_raw_detection/ar_det/src/ar_detection.cpp
index 7b5dbffc159d2f18839fad624b74375d749232ac..69dcc4766c764f4fcc3c72e4fbe6ec14c27fb58d 100644
--- a/MobileRobot/AugReaMarker/ar_raw_detection/ar_det/src/ar_detection.cpp
+++ b/MobileRobot/AugReaMarker/ar_raw_detection/ar_det/src/ar_detection.cpp
@@ -158,12 +158,12 @@ int main(int argc,char **argv){
cv::namedWindow("INPUT IMAGE",1);
MDetector.getThresholdParams( ThresParam1,ThresParam2);
- MDetector.setCornerRefinementMethod(MarkerDetector::LINES);
+ MDetector.setCornerRefinementMethod(MarkerDetector::NONE);
iThresParam1=ThresParam1;
iThresParam2=ThresParam2;
- cv::createTrackbar("ThresParam1", "INPUT IMAGE",&iThresParam1, 13, cvTackBarEvents);
- cv::createTrackbar("ThresParam2", "INPUT IMAGE",&iThresParam2, 13, cvTackBarEvents);
+ cv::createTrackbar("ThresParam1", "INPUT IMAGE",&iThresParam1, 100, cvTackBarEvents);
+ cv::createTrackbar("ThresParam2", "INPUT IMAGE",&iThresParam2, 100, cvTackBarEvents);
char key=0;
int index=0;
diff --git a/MobileRobot/AugReaMarker/ar_raw_detection/ar_det/src/ar_detection.cpp~ b/MobileRobot/AugReaMarker/ar_raw_detection/ar_det/src/ar_detection.cpp~
index 4c90b19eb7a2005a6eafb391f4bd966b86989a68..a319aea752024c95a0204dadbdb043a2bf5a6cc2 100644
--- a/MobileRobot/AugReaMarker/ar_raw_detection/ar_det/src/ar_detection.cpp~
+++ b/MobileRobot/AugReaMarker/ar_raw_detection/ar_det/src/ar_detection.cpp~
@@ -158,12 +158,12 @@ int main(int argc,char **argv){
cv::namedWindow("INPUT IMAGE",1);
MDetector.getThresholdParams( ThresParam1,ThresParam2);
- MDetector.setCornerRefinementMethod(MarkerDetector::LINES);
+ MDetector.setCornerRefinementMethod(MarkerDetector::HARRIS);
iThresParam1=ThresParam1;
iThresParam2=ThresParam2;
- cv::createTrackbar("ThresParam1", "INPUT IMAGE",&iThresParam1, 13, cvTackBarEvents);
- cv::createTrackbar("ThresParam2", "INPUT IMAGE",&iThresParam2, 13, cvTackBarEvents);
+ cv::createTrackbar("ThresParam1", "INPUT IMAGE",&iThresParam1, 100, cvTackBarEvents);
+ cv::createTrackbar("ThresParam2", "INPUT IMAGE",&iThresParam2, 100, cvTackBarEvents);
char key=0;
int index=0;
@@ -254,10 +254,10 @@ int main(int argc,char **argv){
}
// Print other rectangles that contains no valid markers
- /** for (unsigned int i=0;i<MDetector.getCandidates().size();i++) {
+ for (unsigned int i=0;i<MDetector.getCandidates().size();i++) {
aruco::Marker m( MDetector.getCandidates()[i],999);
m.draw(TheInputImageCopy,cv::Scalar(255,0,0));
- }*/
+ }
// Draw a 3d cube in each marker if there is 3d info
if (TheCameraParameters.isValid()){
diff --git a/MobileRobot/AugReaMarker/ar_raw_detection/build/ar_det/CMakeFiles/ar_det.dir/src/ar_detection.cpp.o b/MobileRobot/AugReaMarker/ar_raw_detection/build/ar_det/CMakeFiles/ar_det.dir/src/ar_detection.cpp.o
index 5432255b2c7a9608c246b223cdd85279282cc6ec..36ace1b871ea7f4882d63946646474305fb8ea3e 100644
Binary files a/MobileRobot/AugReaMarker/ar_raw_detection/build/ar_det/CMakeFiles/ar_det.dir/src/ar_detection.cpp.o and b/MobileRobot/AugReaMarker/ar_raw_detection/build/ar_det/CMakeFiles/ar_det.dir/src/ar_detection.cpp.o differ
diff --git a/MobileRobot/AugReaMarker/ar_raw_detection/devel/lib/ar_det/ar_det b/MobileRobot/AugReaMarker/ar_raw_detection/devel/lib/ar_det/ar_det
index f16c23cca75131daac0504890eb0355073b866f6..622a5409785a426b1b6048552bb936c3b5080aff 100755
Binary files a/MobileRobot/AugReaMarker/ar_raw_detection/devel/lib/ar_det/ar_det and b/MobileRobot/AugReaMarker/ar_raw_detection/devel/lib/ar_det/ar_det differ