From c4ae94fc518fa503f477157054d6a3d669432823 Mon Sep 17 00:00:00 2001 From: Aryan Jaiswal Date: Wed, 6 Dec 2017 23:21:23 +0530 Subject: [PATCH 1/6] "Added RigidTransform" --- include/rigidtransform.h | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) create mode 100644 include/rigidtransform.h diff --git a/include/rigidtransform.h b/include/rigidtransform.h new file mode 100644 index 0000000..547f34d --- /dev/null +++ b/include/rigidtransform.h @@ -0,0 +1,20 @@ +//using Eigen's SVD to fastly compute the rigid transformation between two point clouds. +#ifndef RIGIDTRANSFORM +#define RIGIDTRANSFORM + +#include +#include + +#include +#include +#include +#include + +using namespace Eigen; +using namespace std; + +typedef std::pair TransformType; +typedef std::vector PointsType; + +TransformType computeRigidTransform(const PointsType& src, const PointsType& dst); +#endif From 19fc9e710b2dd84510271a07ded0259a8b7f262f Mon Sep 17 00:00:00 2001 From: Aryan Jaiswal Date: Wed, 6 Dec 2017 23:22:59 +0530 Subject: [PATCH 2/6] "Added popt_pp" --- include/popt_pp.h | 39 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 39 insertions(+) create mode 100644 include/popt_pp.h diff --git a/include/popt_pp.h b/include/popt_pp.h new file mode 100644 index 0000000..61c5bdb --- /dev/null +++ b/include/popt_pp.h @@ -0,0 +1,39 @@ +#ifndef _INCLUDED_POPT_PP_H_ +#define _INCLUDED_POPT_PP_H_ + +#include + +class POpt{ +protected: + poptContext con; +public: + // creation and deletion + POpt(const char *name, int argc, const char **argv, + const poptOption *options, int flags) + {con = poptGetContext(name,argc,argv,options,flags);} + POpt(const char *name, int argc, char **argv, + const poptOption *options, int flags) + {con = poptGetContext(name,argc,(const char **)argv,options,flags);} + ~POpt() + {poptFreeContext(con);} + + // functions for processing options + int getNextOpt() + {return(poptGetNextOpt(con));} + void ignoreOptions() + {while(getNextOpt() >= 0);} + const char *getOptArg() + {return(poptGetOptArg(con));} + const char *strError(int error) + {return(poptStrerror(error));} + const char *badOption(int flags = POPT_BADOPTION_NOALIAS) + {return(poptBadOption(con,flags));} + + // processing other arguments + const char *getArg() + {return(poptGetArg(con));} + void ignoreArgs() + {while(getArg());} +}; + +#endif From 52783b0fa0e4ba09cfaa1e5568611b62714ecdf4 Mon Sep 17 00:00:00 2001 From: Aryan Jaiswal Date: Wed, 6 Dec 2017 23:23:51 +0530 Subject: [PATCH 3/6] "Modified CMakeLists" --- CMakeLists.txt | 55 ++------------------------------------------------ 1 file changed, 2 insertions(+), 53 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index df2520f..e70911b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -102,61 +102,10 @@ file(GLOB SOURCE_FILES "src/apriltags/*.cc") ## Declare a cpp executable -add_executable(april_tag_node src/april_tag_node.cpp ${SOURCE_FILES}) +add_executable(april_tag_node src/april_tag_node.cpp src/rigidtransform.cc ${SOURCE_FILES}) -## Add cmake target dependencies of the executable/library -## as an example, message headers may need to be generated before nodes -# add_dependencies(april_tag_node april_tag_generate_messages_cpp) ## Specify libraries to link a library or executable target against target_link_libraries(april_tag_node - ${catkin_LIBRARIES} + ${catkin_LIBRARIES} popt config++ ) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables and/or libraries for installation -# install(TARGETS april_tag april_tag_node -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_april_tag.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) From 49fd0ef3f3296cdbdbc1d772bdac4bc8c78a8eb4 Mon Sep 17 00:00:00 2001 From: Aryan Jaiswal Date: Wed, 6 Dec 2017 23:25:42 +0530 Subject: [PATCH 4/6] "Modified Launch File" --- launch/april_tag_usb.launch | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/launch/april_tag_usb.launch b/launch/april_tag_usb.launch index 33d2a3a..990fd33 100644 --- a/launch/april_tag_usb.launch +++ b/launch/april_tag_usb.launch @@ -10,8 +10,8 @@ - - + + From 446ab6cf31b6707ebfcded8235eace320f8d2662 Mon Sep 17 00:00:00 2001 From: Aryan Jaiswal Date: Wed, 6 Dec 2017 23:26:23 +0530 Subject: [PATCH 5/6] "Added Calibration and Normal Mode" --- src/april_tag_node.cpp | 445 ++++++++++++++++++++++++++++------------- 1 file changed, 309 insertions(+), 136 deletions(-) diff --git a/src/april_tag_node.cpp b/src/april_tag_node.cpp index 5acf91a..81b0b9f 100644 --- a/src/april_tag_node.cpp +++ b/src/april_tag_node.cpp @@ -1,33 +1,44 @@ // // adapted from ros example and april tag examples - palash // +#include #include +#include +#include +#include #include #include #include #include #include #include +#include #include "AprilTags/TagDetector.h" #include "AprilTags/Tag36h11.h" +#include +#include "popt_pp.h" +#include "seer/AprilTag.h" // rosmsg +#include "seer/AprilTagList.h" // rosmsg +#include "rigidtransform.h" -#include "seer/AprilTag.h" // rosmsg -#include "seer/AprilTagList.h" // rosmsg - +using std::ofstream; +using namespace cv; +using namespace std; +using namespace libconfig; static const std::string OPENCV_WINDOW = "Image window"; const double PI = 3.14159265358979323846; -const double TWOPI = 2.0*PI; +const double TWOPI = 2.0 * PI; /** * Normalize angle to be within the interval [-pi,pi]. */ inline double standardRad(double t) { if (t >= 0.) { - t = fmod(t+PI, TWOPI) - PI; + t = fmod(t + PI, TWOPI) - PI; } else { - t = fmod(t-PI, -TWOPI) + PI; + t = fmod(t - PI, -TWOPI) + PI; } return t; } @@ -35,182 +46,344 @@ inline double standardRad(double t) { /** * Convert rotation matrix to Euler angles */ -void wRo_to_euler(const Eigen::Matrix3d& wRo, double& yaw, double& pitch, double& roll) { - yaw = standardRad(atan2(wRo(1,0), wRo(0,0))); - double c = cos(yaw); - double s = sin(yaw); - pitch = standardRad(atan2(-wRo(2,0), wRo(0,0)*c + wRo(1,0)*s)); - roll = standardRad(atan2(wRo(0,2)*s - wRo(1,2)*c, -wRo(0,1)*s + wRo(1,1)*c)); +void wRo_to_euler(const Eigen::Matrix3d& wRo, double& yaw, double& pitch, + double& roll) { + yaw = standardRad(atan2(wRo(1, 0), wRo(0, 0))); + double c = cos(yaw); + double s = sin(yaw); + pitch = standardRad(atan2(-wRo(2, 0), wRo(0, 0) * c + wRo(1, 0) * s)); + roll = standardRad( + atan2(wRo(0, 2) * s - wRo(1, 2) * c, -wRo(0, 1) * s + wRo(1, 1) * c)); } - -class AprilTagNode -{ +class AprilTagNode { ros::NodeHandle nh_; image_transport::ImageTransport it_; image_transport::Subscriber image_sub_; image_transport::Publisher image_pub_; ros::Publisher odom_pub_; - //ros::Publisher tag_list_pub; + // ros::Publisher tag_list_pub; AprilTags::TagDetector* tag_detector; // allow configurations for these: AprilTags::TagCodes tag_codes; - double camera_focal_length_x; // in pixels. late 2013 macbookpro retina = 700 - double camera_focal_length_y; // in pixels - double tag_size; // tag side length of frame in meters - bool show_debug_image; - -public: - AprilTagNode() : - it_(nh_), - tag_codes(AprilTags::tagCodes36h11), - tag_detector(NULL), - camera_focal_length_y(414), - camera_focal_length_x(414), - tag_size(0.015), // 1 1/8in marker = 0.029m - show_debug_image(false) - { + double camera_focal_length_x; // in pixels. late 2013 macbookpro retina = 700 + double camera_focal_length_y; // in pixels + double tag_size; // tag side length of frame in meters + bool show_debug_image; + vector P1s, P2s,P2s_calc; + + /*int i,j; + config_t cfg,*cf; + for(i=0;i<3;i++) + for(j=0;j<3;j++) + config_setting_t *R = Eigen::Matrix3d::Constant(i,j,1.0); + config_setting_t *T = {0,0,0}; + cf = &cfg; + config_init(cf);*/ + + // cv::Mat imgL; + // cv::Mat imgR; + + public: + AprilTagNode() + : it_(nh_), + tag_codes(AprilTags::tagCodes36h11), + tag_detector(NULL), + camera_focal_length_y(414), + camera_focal_length_x(414), + tag_size(0.015), // 1 1/8in marker = 0.029m + show_debug_image(false) { // Subscrive to input video feed and publish output video feed - image_sub_ = it_.subscribe("/usb_cam/image_raw", 1, &AprilTagNode::imageCb, this); + //image_sub_ = + // it_.subscribe("/usb_cam/image_raw", 1, &AprilTagNode::imageCb, this); image_pub_ = it_.advertise("/seer_debug/output_video", 1); odom_pub_ = nh_.advertise("/ground_truth", 1); - //tag_list_pub = nh_.advertise("/seers", 100); + // tag_list_pub = nh_.advertise("/seers", 100); // Use a private node handle so that multiple instances of the node can // be run simultaneously while using different parameters. ros::NodeHandle private_node_handle("~"); - private_node_handle.param("focal_length_px", camera_focal_length_x, 700.0); + private_node_handle.param("focal_length_px", camera_focal_length_x, + 700.0); private_node_handle.param("tag_size_cm", tag_size, 2.9); - private_node_handle.param("show_debug_image", show_debug_image, false); - - camera_focal_length_y = camera_focal_length_x; // meh - tag_size = tag_size / 100.0; // library takes input in meters + private_node_handle.param("show_debug_image", show_debug_image, + false); + camera_focal_length_y = camera_focal_length_x; // meh + tag_size = tag_size / 100.0; // library takes input in meters cout << "got focal length " << camera_focal_length_x << endl; cout << "got tag size " << tag_size << endl; tag_detector = new AprilTags::TagDetector(tag_codes); + if (show_debug_image) { cv::namedWindow(OPENCV_WINDOW); } - + ROS_INFO("Done"); } + int m_mode; - ~AprilTagNode() - { + ~AprilTagNode() { if (show_debug_image) { - cv::destroyWindow(OPENCV_WINDOW); + cv::destroyWindow(OPENCV_WINDOW); + delete tag_detector; } } - /*seer::AprilTag convert_to_msg(AprilTags::TagDetection& detection, int width, int height) { - // recovering the relative pose of a tag: - - // NOTE: for this to be accurate, it is necessary to use the - // actual camera parameters here as well as the actual tag size - // (m_fx, m_fy, m_px, m_py, m_tagSize) - - Eigen::Vector3d translation; - Eigen::Matrix3d rotation; - detection.getRelativeTranslationRotation(tag_size, - camera_focal_length_x, - camera_focal_length_y, - width / 2, - height / 2, - translation, - rotation); - - Eigen::Matrix3d F; - F << - 1, 0, 0, - 0, 1, 0, - 0, 0, 1; - Eigen::Matrix3d fixed_rot = F*rotation; - double yaw, pitch, roll; - wRo_to_euler(fixed_rot, yaw, pitch, roll); - - - seer::AprilTag tag_msg; - - tag_msg.id = detection.id; - tag_msg.hamming_distance = detection.hammingDistance; - tag_msg.distance = translation.norm(); - tag_msg.z = translation(0); // depth from camera - tag_msg.x = translation(1); // horizontal displacement (camera pov right = +ve) - tag_msg.y = translation(2); // vertical displacement - tag_msg.yaw = yaw; - tag_msg.pitch = pitch; - tag_msg.roll = roll; - return tag_msg; - }*/ - - - void processCvImage(cv_bridge::CvImagePtr cv_ptr) - { - cv::Mat image_gray; - cv::cvtColor(cv_ptr->image, image_gray, CV_BGR2GRAY); - vector detections = tag_detector->extractTags(image_gray); + float Eucl_distance(Eigen::Vector3d v1,Eigen::Vector3d v2){ + return sqrt(pow((v1[0]-v2[0]),2)+pow((v1[1]-v2[1]),2)+pow((v1[2]-v1[2]),2)); + } + void calibrate(char key, Mat& image_grayL, Mat& image_grayR) { + vector detectionsL = + AprilTagNode::tag_detector->extractTags(image_grayL); vector tag_msgs; - - if(detections.size()>0){ - Eigen::Vector3d translation; - Eigen::Matrix3d rotation; - detections[0].getRelativeTranslationRotation(tag_size, - camera_focal_length_x, - camera_focal_length_y, - cv_ptr->image.cols / 2, - cv_ptr->image.rows / 2, - translation, - rotation); - - if (show_debug_image) { - // Update GUI Window - detections[0].draw(cv_ptr->image); - cv::imshow(OPENCV_WINDOW, cv_ptr->image); - cv::waitKey(3); + vector detectionsR = + tag_detector->extractTags(image_grayR); + // vector tag_msgs; + if (detectionsR.size() > 0 && detectionsL.size() > 0) { + Eigen::Vector3d translationL, translationR; + Eigen::Matrix3d rotationL, rotationR; + + static const char *file ="T_matrix.cfg"; + Config cfg; + Setting &root=cfg.getRoot(); + + if(!root.exists("Matrix")) + root.add("Matrix",Setting::TypeGroup); + + Setting &Matrix=root["Matrix"]; + + if (key != 27) { + //for (size_t i = 0; i < detectionsL.size(); i++) { + detectionsL[0].getRelativeTranslationRotation( + tag_size, camera_focal_length_x, camera_focal_length_y, + image_grayL.cols / 2, image_grayL.rows / 2, translationL, + rotationL); + + P1s.push_back(Eigen::Vector3d(translationL(1), -translationL(2), + 2.89698162598 - translationL(0))); + //} + //for (size_t i = 0; i < detectionsR.size(); i++) { + detectionsR[0].getRelativeTranslationRotation( + tag_size, camera_focal_length_x, camera_focal_length_y, + image_grayR.cols / 2, image_grayR.rows / 2, translationR, + rotationR); + P2s.push_back(Eigen::Vector3d(translationR(1), -translationR(2), + 2.89698162598 - translationR(0))); + //} + ROS_INFO("Grabbed a set of points"); + } else { + TransformType RT = computeRigidTransform(P1s, P2s); + // if(config_read_file(&cfg,"T_matrix")==CONFIG_TRUE){ + std::cout << RT.first << endl; + std::cout << (RT.second)[0] << " " << (RT.second)[1] << " " + << (RT.second)[2] << endl; + cout << endl; + /* R=config_lookup(&cfg,"matrix.R"); + T=config_lookup(&cfg,"matrix.T"); + config_setting_add(R,RT.first); + config_setting_add(T,RT.second); + config_write_file(&cfg,"T_matrix");*/ + Setting &R1=Matrix.add("R1",Setting::TypeList); + Setting &R2=Matrix.add("R2",Setting::TypeList); + Setting &R3=Matrix.add("R3",Setting::TypeList); + Setting &T=Matrix.add("T",Setting::TypeList); + for(int i=0;i<3;i++){ + R1.add(Setting::TypeFloat) = (RT.first)(0,i); + R2.add(Setting::TypeFloat) = (RT.first)(1,i); + R3.add(Setting::TypeFloat) = (RT.first)(2,i); + T.add(Setting::TypeFloat) = (RT.second)[i]; + } + + cfg.writeFile(file); + cerr<<"New Configuration successfully written to: "< detectionsL = + AprilTagNode::tag_detector->extractTags(image_grayL); + vector tag_msgs; + vector detectionsR = + tag_detector->extractTags(image_grayR); + // vector tag_msgs; + if (detectionsR.size() > 0 && detectionsL.size() > 0) { + Eigen::Vector3d translationL, translationR; + Eigen::Matrix3d rotationL, rotationR; + + static const char *file ="/home/pikachu/catkin_ws/T_matrix.cfg"; + Config cfg; + try{ + cfg.readFile(file); + } + catch(const FileIOException &fioex) + { + std::cerr<<"I/O error while reading file"< r1,r2,r3,t; + float error; + //const char* var3; + Eigen::Matrix3d R; + Eigen::Vector3d T; + /*if(cfg.exists("Matrix.R1")&&cfg.exists("Matrix.R2")&&cfg.exists("Matrix.R3")&&cfg.exists("Matrix.T")) + cout<<"Exists"<toImageMsg()); + void imgCallback(const sensor_msgs::ImageConstPtr& msg_left, + const sensor_msgs::ImageConstPtr& msg_right) { + cv::Mat tmpL, tmpR, image_grayL, image_grayR; + tmpL = cv_bridge::toCvShare(msg_left, "bgr8")->image; + tmpR = cv_bridge::toCvShare(msg_right, "bgr8")->image; + if (tmpL.empty() || tmpR.empty()) return; + + if (m_mode==1){ + cv::cvtColor(tmpL, image_grayL, CV_BGR2GRAY); + cv::cvtColor(tmpR, image_grayR, CV_BGR2GRAY); + + imshow("Left_Input_Calibration", image_grayL); + imshow("Right_Input_Calibration", image_grayR); + char key = (char)cv::waitKey(30); + if (key > 0) { + //calibrate(key, image_grayL, image_grayR); + normal(key, image_grayL, image_grayR); + } + exit(EXIT_FAILURE); + } + else if(m_mode == 0){ + cv::cvtColor(tmpL, image_grayL, CV_BGR2GRAY); + cv::cvtColor(tmpR, image_grayR, CV_BGR2GRAY); + + imshow("Left_Input", image_grayL); + imshow("Right_Input", image_grayR); + char key = (char)cv::waitKey(30); + if (key > 0) { + calibrate(key, image_grayL, image_grayR); + } + exit(EXIT_FAILURE); + } } + //config_destroy(cf); }; -int main(int argc, char** argv) -{ +/************************************************* MAIN + * ************************************************/ + +/* +* TODO: +* Write R and T to file. +* Read R and T from file. +* Read all hard coded values from lib config file +* Calculate RMSE of calibration and display with the R and T cout +* Write Mode 2: Detect april tags and simply cout euclidean distance +*/ + + +int main(int argc, char** argv) { ros::init(argc, argv, "seer_node"); + ros::NodeHandle nh; AprilTagNode atn; + int mode,m_mode; + static struct poptOption options[] = { + {"mode", 'm', POPT_ARG_INT, &mode, 0, + "Set m=1 for calibration, m=0 for testing", "NUM"}, + POPT_AUTOHELP{NULL, 0, 0, NULL, 0, NULL, NULL}}; + POpt popt(NULL, argc, argv, options, 0); + int c; + while ((c = popt.getNextOpt()) >= 0) { + } + message_filters::Subscriber sub_img_left( + nh, "/usb_cam/image_raw", 1); + message_filters::Subscriber sub_img_right( + nh, "/usb_cam2/image_raw", 1); + typedef message_filters::sync_policies::ApproximateTime< + sensor_msgs::Image, sensor_msgs::Image> SyncPolicy; + message_filters::Synchronizer sync(SyncPolicy(10), sub_img_left, + sub_img_right); + atn.m_mode = mode; + sync.registerCallback(boost::bind(&AprilTagNode::imgCallback, atn, _1, _2)); + + + ros::spin(); return 0; } From 01770477b36f8303be60a031d2918166e19afdd8 Mon Sep 17 00:00:00 2001 From: Aryan Jaiswal Date: Wed, 6 Dec 2017 23:30:32 +0530 Subject: [PATCH 6/6] "Added RigidTransform" --- src/rigidtransform.cc | 46 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 46 insertions(+) create mode 100644 src/rigidtransform.cc diff --git a/src/rigidtransform.cc b/src/rigidtransform.cc new file mode 100644 index 0000000..2a693cb --- /dev/null +++ b/src/rigidtransform.cc @@ -0,0 +1,46 @@ +#include "rigidtransform.h" + +TransformType computeRigidTransform(const PointsType& src, const PointsType& dst) +{ + assert(src.size() == dst.size()); + int pairSize = src.size(); + Eigen::Vector3d center_src(0, 0, 0), center_dst(0, 0, 0); + for (int i=0; i svd; + Eigen::MatrixXd H_(3, 3); + for (int i=0; i<3; ++i) for (int j=0; j<3; ++j) H_(i, j) = H(i, j); + svd.compute(H_, Eigen::ComputeThinU | Eigen::ComputeThinV ); + if (!svd.computeU() || !svd.computeV()) + { + std::cerr << "decomposition error" << endl; + return std::make_pair(Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero()); + } + Eigen::Matrix3d Vt = svd.matrixV().transpose(); + Eigen::Matrix3d R = svd.matrixU()*Vt; + Eigen::Vector3d t = center_dst - R*center_src; + //std::cout << RT.first << endl; + //std::cout << (RT.second)[0] << " " << (RT.second)[1] << " " << (RT.second)[2] << endl; + //cout << endl; + //getchar(); + //return 0; + return std::make_pair(R, t); +}