diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h index e1e6b02f88db1880a8c1945ffddcc88e5d8f3d16..545a6ce6939f9e060dbbe001376fb58171f0358d 100644 --- a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h +++ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h @@ -122,6 +122,9 @@ public: static double Pos_Px,Pos_Ix,Pos_Dx; static double Pos_Py,Pos_Iy,Pos_Dy; + //static double S_yPos_P,S_yPos_I,S_yPos_D; + //static double L_yPos_P,L_yPos_I,L_yPos_D; + static double S_Ang_P,S_Ang_I,S_Ang_D; static double L_Ang_P,L_Ang_I,L_Ang_D; diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~ index fdfa7f228957bf3b2828a8296665c990157bb060..e1e6b02f88db1880a8c1945ffddcc88e5d8f3d16 100644 --- a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~ +++ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~ @@ -119,10 +119,11 @@ public: void myhandler(int value); - - static double Pos_P,Pos_I,Pos_D; - static double Ang_P,Ang_I,Ang_D; - + static double Pos_Px,Pos_Ix,Pos_Dx; + static double Pos_Py,Pos_Iy,Pos_Dy; + + static double S_Ang_P,S_Ang_I,S_Ang_D; + static double L_Ang_P,L_Ang_I,L_Ang_D; // ---- CONTROLL PARAMETERS ------ // static double prev_errorX; diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp index 786e01e4e1875dc28691c55ccc4b5d019a2c6daa..4f0cbb897dfeb536c9881d7f349b6effa0b11651 100644 --- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp +++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp @@ -42,23 +42,35 @@ using namespace cv; using namespace aruco; using namespace std; -double ImageConverter::Pos_Px = .0001; + +double ImageConverter::Pos_Px = .00005; double ImageConverter::Pos_Ix = 0.002; double ImageConverter::Pos_Dx = 0; -double ImageConverter::Pos_Py = .25 * ImageConverter::Pos_Px; -double ImageConverter::Pos_Iy = .25 * ImageConverter::Pos_Ix; -double ImageConverter::Pos_Dy = .25 * ImageConverter::Pos_Dx; -double ImageConverter::S_Ang_P = 0.05 * ImageConverter::Pos_Px; -double ImageConverter::S_Ang_I = 0.05 * ImageConverter::Pos_Ix; -double ImageConverter::S_Ang_D = 0.05 * ImageConverter::Pos_Dx; +double ImageConverter::Pos_Py = .37 * ImageConverter::Pos_Px; +double ImageConverter::Pos_Iy = .37 * ImageConverter::Pos_Ix; +double ImageConverter::Pos_Dy = .37 * ImageConverter::Pos_Dx; + +/* +double ImageConverter::S_yPos_P = .16 * ImageConverter::Pos_Px; +double ImageConverter::S_yPos_I = .16 * ImageConverter::Pos_Ix; +double ImageConverter::S_yPos_D = .16 * ImageConverter::Pos_Dx; + +double ImageConverter::L_yPos_P = .3 * ImageConverter::Pos_Px; +double ImageConverter::L_yPos_I = .3 * ImageConverter::Pos_Ix; +double ImageConverter::L_yPos_D = .3 * ImageConverter::Pos_Dx; +*/ + +double ImageConverter::S_Ang_P = .07 * ImageConverter::Pos_Px; +double ImageConverter::S_Ang_I = .07 * ImageConverter::Pos_Ix; +double ImageConverter::S_Ang_D = .07 * ImageConverter::Pos_Dx; -double ImageConverter::L_Ang_P = 0.9 * ImageConverter::Pos_Px; -double ImageConverter::L_Ang_I = 0.9 * ImageConverter::Pos_Ix; -double ImageConverter::L_Ang_D = 0.9 * ImageConverter::Pos_Dx; +double ImageConverter::L_Ang_P = .1 * ImageConverter::Pos_Px; +double ImageConverter::L_Ang_I = .1 * ImageConverter::Pos_Ix; +double ImageConverter::L_Ang_D = .1 * ImageConverter::Pos_Dx; -float ImageConverter::TheMarkerSize = -1; +float ImageConverter::TheMarkerSize = .1; // Default marker size int ImageConverter::Thresh1_min = 20; int ImageConverter::Thresh2_min = 20; @@ -108,9 +120,9 @@ double ImageConverter::control_signalYAW; double ImageConverter::zeroMax = .0000000000000000001; double ImageConverter::zeroMin = -.0000000000000000001; -double ImageConverter::Pz_eps = .01; -double ImageConverter::Py_eps = .001; -double ImageConverter::A_eps = .02; // ? deg. +double ImageConverter::Pz_eps = .005; +double ImageConverter::Py_eps = .007; +double ImageConverter::A_eps = .01; // ? deg. ImageConverter::ImageConverter() : @@ -255,7 +267,6 @@ void ImageConverter::ProgStart(int argc,char** argv) MDetector.pyrDown(ThePyrDownLevel); } - MDetector.setCornerRefinementMethod(MarkerDetector::LINES); char key=0; @@ -385,17 +396,24 @@ void ImageConverter::ProgStart(int argc,char** argv) // -------------------------Removed----------------------------- // } - // Print other rectangles that contains no valid markers + /*// Print other rectangles that contains no valid markers for (unsigned int i=0;i<MDetector.getCandidates().size();i++) { Marker m( MDetector.getCandidates()[i],10); - m.draw(TheInputImageCopy,cv::Scalar(0,255,0),2); - } + m.draw(TheInputImageCopy,Scalar(0,255,0),2); + }*/ for (unsigned int i=0;i<TheMarkers.size();i++) { + int currentMarID = TheMarkers[i].id; + TheMarkers[i].draw(TheInputImageCopy,Scalar(0,255,0),2) ; + CvDrawingUtils::draw3dCube(TheInputImageCopy,TheMarkers[i],TheCameraParameters); CvDrawingUtils::draw3dAxis(TheInputImageCopy,TheMarkers[i],TheCameraParameters); + + //Marker ID to string + stringstream marker_id_string; + marker_id_string << "marker_ " << currentMarID; } if (update_images) @@ -435,9 +453,9 @@ void ImageConverter::camCB(const geometry_msgs::PoseStamped::ConstPtr& CamFB) { RefPose[0] = -.0957; -RefPose[1] = -0.0965533; -RefPose[2] = 0.371; -RefPose[3] = -0.69; +RefPose[1] = -0.0203153; +RefPose[2] = 0.307922; +RefPose[3] = -0.705028; camPose[0] = CamFB->pose.position.x; camPose[1] = CamFB->pose.position.y; @@ -543,7 +561,24 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl p_termY = Pos_Py * curr_errorY; i_termY = Pos_Iy * int_errorY; d_termY = Pos_Dy * diffY; - + + /*int y_offset = 5; + if (curr_errorY < y_offset && curr_errorY > -y_offset) // -5 < err < +5 + { + ROS_INFO_STREAM(" INSIDE boundry of " << y_offset << " m. \n"); + // scalling + p_termYAW = S_yPos_P * curr_errorY; + i_termYAW = S_yPos_I * int_errorY; + d_termYAW = S_yPos_D * diffY; + } else if (curr_errorY > y_offset || curr_errorYAW < y_offset) // err > +5 or err < -5 + { + ROS_INFO_STREAM(" OUTSIDE boundry of " << y_offset << " m. \n"); + // scalling + p_termYAW = L_yPos_P * curr_errorY; + i_termYAW = L_yPos_I * int_errorY; + d_termYAW = L_yPos_D * diffY; + }*/ + // control signal control_signalY = p_termY + i_termY + d_termY; @@ -575,16 +610,17 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl // differentiation diffYAW = ((curr_errorYAW - prev_errorYAW) / dt); - if (curr_errorYAW < .08726 && curr_errorYAW > -.08726) // -5 < err < +5 + int yaw_offset = 5; + if (curr_errorYAW < ((CV_PI/180) * yaw_offset) && curr_errorYAW > ((CV_PI/180) * -yaw_offset)) // -5 < err < +5 { - ROS_INFO(" -5 < err < +5 \n"); + ROS_INFO_STREAM(" INSIDE boundry of " << yaw_offset << " deg. \n"); // scalling p_termYAW = S_Ang_P * curr_errorYAW; i_termYAW = S_Ang_I * int_errorYAW; d_termYAW = S_Ang_D * diffYAW; - } else if (curr_errorYAW > .08726 || curr_errorYAW < -.08726) // err > +5 or err < -5 + } else if (curr_errorYAW > ((CV_PI/180) * yaw_offset) || curr_errorYAW < ((CV_PI/180) * -yaw_offset)) // err > +5 or err < -5 { - ROS_INFO(" err > +5 or err < -5 \n"); + ROS_INFO_STREAM(" OUTSIDE boundry of " << yaw_offset << " deg. \n"); // scalling p_termYAW = L_Ang_P * curr_errorYAW; i_termYAW = L_Ang_I * int_errorYAW; diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~ index e7668aefd6ad6c76fb473140433c781a1837a4d5..786e01e4e1875dc28691c55ccc4b5d019a2c6daa 100644 --- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~ +++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~ @@ -46,17 +46,17 @@ double ImageConverter::Pos_Px = .0001; double ImageConverter::Pos_Ix = 0.002; double ImageConverter::Pos_Dx = 0; -double ImageConverter::Pos_Py = 4 * ImageConverter::Pos_Px; -double ImageConverter::Pos_Iy = 4 * ImageConverter::Pos_Ix; -double ImageConverter::Pos_Dy = 4 * ImageConverter::Pos_Dx; +double ImageConverter::Pos_Py = .25 * ImageConverter::Pos_Px; +double ImageConverter::Pos_Iy = .25 * ImageConverter::Pos_Ix; +double ImageConverter::Pos_Dy = .25 * ImageConverter::Pos_Dx; -double ImageConverter::S_Ang_P = 0.01 * ImageConverter::Pos_Px; -double ImageConverter::S_Ang_I = 0.01 * ImageConverter::Pos_Ix; -double ImageConverter::S_Ang_D = 0.01 * ImageConverter::Pos_Dx; +double ImageConverter::S_Ang_P = 0.05 * ImageConverter::Pos_Px; +double ImageConverter::S_Ang_I = 0.05 * ImageConverter::Pos_Ix; +double ImageConverter::S_Ang_D = 0.05 * ImageConverter::Pos_Dx; -double ImageConverter::L_Ang_P = 0.7 * ImageConverter::Pos_Px; -double ImageConverter::L_Ang_I = 0.7 * ImageConverter::Pos_Ix; -double ImageConverter::L_Ang_D = 0.7 * ImageConverter::Pos_Dx; +double ImageConverter::L_Ang_P = 0.9 * ImageConverter::Pos_Px; +double ImageConverter::L_Ang_I = 0.9 * ImageConverter::Pos_Ix; +double ImageConverter::L_Ang_D = 0.9 * ImageConverter::Pos_Dx; float ImageConverter::TheMarkerSize = -1; @@ -108,8 +108,8 @@ double ImageConverter::control_signalYAW; double ImageConverter::zeroMax = .0000000000000000001; double ImageConverter::zeroMin = -.0000000000000000001; -double ImageConverter::Pz_eps = .012; -double ImageConverter::Py_eps = .0001; +double ImageConverter::Pz_eps = .01; +double ImageConverter::Py_eps = .001; double ImageConverter::A_eps = .02; // ? deg. @@ -269,7 +269,6 @@ void ImageConverter::ProgStart(int argc,char** argv) { // Detection of markers in the image passed MDetector.detect(TheInputImage,TheMarkers,TheCameraParameters,TheMarkerSize); - //MDetector.detectRectangles(MDetector.getThresholdedImage(),MDetector._candidates); TheInputImage.copyTo(TheInputImageCopy); @@ -290,17 +289,17 @@ void ImageConverter::ProgStart(int argc,char** argv) if(curr_errorYAW > RefPose[3]) { - move2docking(-control_signalX, control_signalY, control_signalYAW); // CW (---|| \\) + move2docking(-control_signalX, -control_signalY, control_signalYAW); // CW (---|| \\) } else { - move2docking(-control_signalX, control_signalY, -control_signalYAW); // CCW (// || ---) - } + move2docking(-control_signalX, -control_signalY, -control_signalYAW); // CCW (// || ---) + } } if (ros::ok() && found) { - x_t = -TheMarkers[0].Tvec.at<Vec3f>(0,0)[0]; - y_t = TheMarkers[0].Tvec.at<Vec3f>(0,0)[1]; + y_t = -TheMarkers[0].Tvec.at<Vec3f>(0,0)[0]; + x_t = TheMarkers[0].Tvec.at<Vec3f>(0,0)[1]; z_t = TheMarkers[0].Tvec.at<Vec3f>(0,0)[2]; //printf("\n X = %4.2f Y = %4.2f Z = %4.2f\n",x_t,y_t,z_t); @@ -436,9 +435,9 @@ void ImageConverter::camCB(const geometry_msgs::PoseStamped::ConstPtr& CamFB) { RefPose[0] = -.0957; -RefPose[1] = 0.0054; -RefPose[2] = 0.365; -RefPose[3] = -0.65; +RefPose[1] = -0.0965533; +RefPose[2] = 0.371; +RefPose[3] = -0.69; camPose[0] = CamFB->pose.position.x; camPose[1] = CamFB->pose.position.y; @@ -610,10 +609,10 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl if(curr_errorYAW > RefYAW) { - dock(-control_signalX, control_signalY, control_signalYAW); // CW (---|| \\) + dock(-control_signalX, -control_signalY, control_signalYAW); // CW (---|| \\) } else { - dock(-control_signalX, control_signalY, -control_signalYAW); // CCW (// || ---) + dock(-control_signalX, -control_signalY, -control_signalYAW); // CCW (// || ---) } } 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 56678bd618ff18b4dc350f1625d076564a0c67b2..3ba8c943ec7e0cfa85e01a3b16e3af3b7f4fa11d 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 @@ -201,7 +201,10 @@ int main(int argc,char **argv){ geometry_msgs::Pose msg; float x_t, y_t, z_t; + float roll,yaw,pitch; + float rollE,yawE,pitchE; + bool found = (TheMarkers.size()>0)?true:false; if (found) { @@ -213,6 +216,34 @@ int main(int argc,char **argv){ cv::Mat rot_mat(3,3,cv::DataType<float>::type); // You need to apply cv::Rodrigues() in order to obatain angles wrt to camera coords cv::Rodrigues(TheMarkers[0].Rvec,rot_mat); + + // ----------- Euler angle -----------------// + + float roll1 = -asin(rot_mat.at<float>(2,0)); + float roll2 = CV_PI - roll1; + + float pitch1 = atan2(rot_mat.at<float>(2,1) / cos(roll1), rot_mat.at<float>(2,2) / cos(roll1)); + float pitch2 = atan2(rot_mat.at<float>(2,1) / cos(roll2), rot_mat.at<float>(2,2) / cos(roll2)); + + float yaw2 = atan2(rot_mat.at<float>(1,0) / cos(roll2), rot_mat.at<float>(0,0) / cos(roll2)); + float yaw1 = atan2(rot_mat.at<float>(1,0) / cos(roll1), rot_mat.at<float>(0,0) / cos(roll1)); + + //choose one solution to return + //for example the "shortest" rotation + if ((abs(roll1) + abs(pitch1) + abs(yaw1)) <= (abs(roll2) + abs(pitch2) + abs(yaw2))) + { + rollE = roll1; + pitchE= pitch1; + yawE = yaw1; + } else + + { + rollE = roll2; + pitchE = pitch2; + yawE = yaw2; + } + + // ----------- Euler angle -----------------// pitch = -atan2(rot_mat.at<float>(2,0), rot_mat.at<float>(2,1)); yaw = acos(rot_mat.at<float>(2,2)); @@ -232,14 +263,23 @@ int main(int argc,char **argv){ float r_off = CV_PI/2; float y_off = CV_PI/2; - // See: http://en.wikipedia.org/wiki/Flight_dynamics if (found){ //printf( "Angles (deg) wrt Flight Dynamics: roll:%5.2f pitch:%5.2f yaw:%5.2f \n", (roll-r_off)*(180.0/CV_PI), (pitch-p_off)*(180.0/CV_PI), (yaw-y_off)*(180.0/CV_PI)); //printf( " Marker distance in metres: x_d:%5.2f y_d:%5.2f z_d:%5.2f \n", x_t, y_t, z_t); ROS_INFO_STREAM("Y = " << y_t << "m. \n"); - ROS_INFO_STREAM(" X = " << x_t << "m. \n"); + //ROS_INFO_STREAM(" X = " << x_t << "m. \n"); //ROS_INFO_STREAM("Z = " << z_t << "m. \n"); - + ROS_INFO_STREAM("roll = " << (roll/*-r_off*/)*(180.0/CV_PI) << " deg. \n"); + ROS_INFO_STREAM("EULER R = " << (rollE/*-r_off*/)*(180.0/CV_PI) << " deg. \n"); + + + ROS_INFO_STREAM("pitch = " << (pitch/*-r_off*/)*(180.0/CV_PI) << " deg. \n"); + ROS_INFO_STREAM("EULER P = " << (pitchE/*-r_off*/)*(180.0/CV_PI) << " deg. \n"); + + ROS_INFO_STREAM("yaw = " << (yaw/*-r_off*/)*(180.0/CV_PI) << " deg. \n"); + ROS_INFO_STREAM("EULER Y = " << (yawE/*-r_off*/)*(180.0/CV_PI) << " deg. \n"); + + ROS_INFO_STREAM(" -------------------------------------- \n"); } if (ros::ok()){ @@ -259,8 +299,8 @@ int main(int argc,char **argv){ // Print other rectangles that contains no valid markers 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)); + aruco::Marker m( MDetector.getCandidates()[i],11); + m.draw(TheInputImageCopy,cv::Scalar(150,10,80),2); } // Draw a 3d cube in each marker if there is 3d info 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 ce0df1e971d51c529545c5d86630cd096612b569..55fe539254c55e38fef83e422d63c2652dd6498b 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~ @@ -201,7 +201,10 @@ int main(int argc,char **argv){ geometry_msgs::Pose msg; float x_t, y_t, z_t; + float roll,yaw,pitch; + float rollE,yawE,pitchE; + bool found = (TheMarkers.size()>0)?true:false; if (found) { @@ -213,6 +216,34 @@ int main(int argc,char **argv){ cv::Mat rot_mat(3,3,cv::DataType<float>::type); // You need to apply cv::Rodrigues() in order to obatain angles wrt to camera coords cv::Rodrigues(TheMarkers[0].Rvec,rot_mat); + + // ----------- Euler angle -----------------// + + float roll1 = -asin(rot_mat.at<float>(2,0)); + float roll2 = CV_PI - roll1; + + float pitch1 = atan2(rot_mat.at<float>(2,1) / cos(roll1), rot_mat.at<float>(2,2) / cos(roll1)); + float pitch2 = atan2(rot_mat.at<float>(2,1) / cos(roll2), rot_mat.at<float>(2,2) / cos(roll2)); + + float yaw2 = atan2(rot_mat.at<float>(1,0) / cos(roll2), rot_mat.at<float>(0,0) / cos(roll2)); + float yaw1 = atan2(rot_mat.at<float>(1,0) / cos(roll1), rot_mat.at<float>(0,0) / cos(roll1)); + + //choose one solution to return + //for example the "shortest" rotation + if ((abs(roll1) + abs(pitch1) + abs(yaw1)) <= (abs(roll2) + abs(pitch2) + abs(yaw2))) + { + rollE = roll1; + pitchE= pitch1; + yawE = yaw1; + } else + + { + rollE = roll2; + pitchE = pitch2; + yawE = yaw2; + } + + // ----------- Euler angle -----------------// pitch = -atan2(rot_mat.at<float>(2,0), rot_mat.at<float>(2,1)); yaw = acos(rot_mat.at<float>(2,2)); @@ -232,14 +263,23 @@ int main(int argc,char **argv){ float r_off = CV_PI/2; float y_off = CV_PI/2; - // See: http://en.wikipedia.org/wiki/Flight_dynamics if (found){ //printf( "Angles (deg) wrt Flight Dynamics: roll:%5.2f pitch:%5.2f yaw:%5.2f \n", (roll-r_off)*(180.0/CV_PI), (pitch-p_off)*(180.0/CV_PI), (yaw-y_off)*(180.0/CV_PI)); //printf( " Marker distance in metres: x_d:%5.2f y_d:%5.2f z_d:%5.2f \n", x_t, y_t, z_t); - ROS_INFO_STREAM("X = " << x_t << "m. \n"); - ROS_INFO_STREAM(" Y = " << y_t << "m. \n"); + //ROS_INFO_STREAM("Y = " << y_t << "m. \n"); + //ROS_INFO_STREAM(" X = " << x_t << "m. \n"); //ROS_INFO_STREAM("Z = " << z_t << "m. \n"); - + ROS_INFO_STREAM("roll = " << (roll/*-r_off*/)*(180.0/CV_PI) << " deg. \n"); + ROS_INFO_STREAM("EULER R = " << (rollE/*-r_off*/)*(180.0/CV_PI) << " deg. \n"); + + + ROS_INFO_STREAM("pitch = " << (pitch/*-r_off*/)*(180.0/CV_PI) << " deg. \n"); + ROS_INFO_STREAM("EULER P = " << (pitchE/*-r_off*/)*(180.0/CV_PI) << " deg. \n"); + + ROS_INFO_STREAM("yaw = " << (yaw/*-r_off*/)*(180.0/CV_PI) << " deg. \n"); + ROS_INFO_STREAM("EULER Y = " << (yawE/*-r_off*/)*(180.0/CV_PI) << " deg. \n"); + + ROS_INFO_STREAM(" -------------------------------------- \n"); } if (ros::ok()){ @@ -259,8 +299,8 @@ int main(int argc,char **argv){ // Print other rectangles that contains no valid markers 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)); + aruco::Marker m( MDetector.getCandidates()[i],11); + m.draw(TheInputImageCopy,cv::Scalar(150,10,80),2); } // Draw a 3d cube in each marker if there is 3d info 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 f89348a836ed8d431e2fc5548c15f69710269e3f..46a2ec0587c06a375afcc0e94d353b0d32e868ea 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 c2a2d5e866972e2f8e849b21e8bf08c0edef38c9..92a96555055252093fd1df9562ad9000553eb9a0 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