From d418ca703413cd67d1339af536c3c38f28c6dcf0 Mon Sep 17 00:00:00 2001 From: iory Date: Thu, 7 Jun 2018 10:44:24 +0900 Subject: [PATCH 01/15] Add handeye_calib_auto --- CMakeLists.txt | 7 + src/handeye_calibration_auto.cpp | 545 +++++++++++++++++++++++++++++++ 2 files changed, 552 insertions(+) create mode 100644 src/handeye_calibration_auto.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 0af1407..c756472 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -208,6 +208,10 @@ add_executable(handeye_calib_camodocal src/handeye_calibration.cpp src/camodocal/calib/HandEyeCalibration.cc) +add_executable(handeye_calib_camodocal_auto + src/handeye_calibration_auto.cpp + src/camodocal/calib/HandEyeCalibration.cc) + ## Add cmake target dependencies of the executable ## same as for the library above # add_dependencies(handeye_calib_camodocal_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) @@ -217,6 +221,9 @@ message("GLOG = ${GLOG_LIBRARIES} ${GLOG_LIB} ${GLOG_LIBS}") target_link_libraries(handeye_calib_camodocal ${catkin_LIBRARIES} ${GLOG_LIBRARIES} ${GLOG_LIB} ${GLOG_LIBS} ${OpenCV_LIBRARIES} ${CERES_LIBRARIES} ) +target_link_libraries(handeye_calib_camodocal_auto + ${catkin_LIBRARIES} ${GLOG_LIBRARIES} ${GLOG_LIB} ${GLOG_LIBS} ${OpenCV_LIBRARIES} ${CERES_LIBRARIES} +) ############# ## Install ## ############# diff --git a/src/handeye_calibration_auto.cpp b/src/handeye_calibration_auto.cpp new file mode 100644 index 0000000..4abc492 --- /dev/null +++ b/src/handeye_calibration_auto.cpp @@ -0,0 +1,545 @@ +#include "ceres/ceres.h" +#include "ceres/types.h" +#include +#include +#include +#include +#include +#include +#include +#include + +typedef std::vector> + eigenVector; + +typedef std::vector> + EigenAffineVector; + +/////////////////////////////////////////////////////// +// DEFINING GLOBAL VARIABLES + +std::string ARTagTFname, cameraTFname; +std::string EETFname, baseTFname; +tf::TransformListener* listener; +Eigen::Affine3d firstEEInverse, firstCamInverse; +bool firstTransform = true; +eigenVector tvecsArm, rvecsArm, tvecsFiducial, rvecsFiducial; +std::string key; + +EigenAffineVector baseToTip, cameraToTag; + +template +Eigen::Vector3d eigenRotToEigenVector3dAngleAxis(Input eigenQuat) { + Eigen::AngleAxisd ax3d(eigenQuat); + return ax3d.angle() * ax3d.axis(); +} + +/// @return 0 on success, otherwise error code +int writeTransformPairsToFile(const EigenAffineVector& t1, + const EigenAffineVector& t2, + std::string filename) { + std::cerr << "Writing pairs to \"" << filename << "\"...\n"; + cv::FileStorage fs(filename, cv::FileStorage::WRITE); + int frameCount = t1.size(); + fs << "frameCount" << frameCount; + auto t1_it = t1.begin(); + auto t2_it = t2.begin(); + + if (fs.isOpened()) { + + for (int i = 0; i < t1.size(); ++i, ++t1_it, ++t2_it) { + cv::Mat_ t1cv = cv::Mat_::ones(4, 4); + cv::eigen2cv(t1_it->matrix(), t1cv); + cv::Mat_ t2cv = cv::Mat_::ones(4, 4); + cv::eigen2cv(t2_it->matrix(), t2cv); + + std::stringstream ss1; + ss1 << "T1_" << i; + fs << ss1.str() << t1cv; + + std::stringstream ss2; + ss2 << "T2_" << i; + fs << ss2.str() << t2cv; + } + fs.release(); + } else { + std::cerr << "failed to open output file " << filename << "\n"; + return 1; + } + return 0; +} + +/// @return 0 on success, otherwise error code +int readTransformPairsFromFile(std::string filename, EigenAffineVector& t1, + EigenAffineVector& t2) { + cv::FileStorage fs(filename, cv::FileStorage::READ); + int frameCount; + + fs["frameCount"] >> frameCount; + auto t1_it = std::back_inserter(t1); + auto t2_it = std::back_inserter(t2); + + if (fs.isOpened()) { + + for (int i = 0; i < frameCount; ++i, ++t1_it, ++t2_it) { + // read in frame one + { + cv::Mat_ t1cv = cv::Mat_::ones(4, 4); + std::stringstream ss1; + ss1 << "T1_" << i; + fs[ss1.str()] >> t1cv; + Eigen::Affine3d t1e; + cv::cv2eigen(t1cv, t1e.matrix()); + *t1_it = t1e; + } + + // read in frame two + { + cv::Mat_ t2cv = cv::Mat_::ones(4, 4); + std::stringstream ss2; + ss2 << "T2_" << i; + fs[ss2.str()] >> t2cv; + Eigen::Affine3d t2e; + cv::cv2eigen(t2cv, t2e.matrix()); + *t2_it = t2e; + } + } + fs.release(); + } else { + std::cerr << "failed to open input file " << filename << "\n"; + return 1; + } + return 0; +} + +Eigen::Affine3d estimateHandEye(const EigenAffineVector& baseToTip, + const EigenAffineVector& camToTag) { + + auto t1_it = baseToTip.begin(); + auto t2_it = camToTag.begin(); + + Eigen::Affine3d firstEEInverse, firstCamInverse; + eigenVector tvecsArm, rvecsArm, tvecsFiducial, rvecsFiducial; + + bool firstTransform = true; + + for (int i = 0; i < baseToTip.size(); ++i, ++t1_it, ++t2_it) { + auto& eigenEE = *t1_it; + auto& eigenCam = *t2_it; + if (firstTransform) { + firstEEInverse = eigenEE.inverse(); + firstCamInverse = eigenCam.inverse(); + ROS_INFO("Adding first transformation."); + firstTransform = false; + } else { + Eigen::Affine3d robotTipinFirstTipBase = firstEEInverse * eigenEE; + Eigen::Affine3d fiducialInFirstFiducialBase = + firstCamInverse * eigenCam; + + rvecsArm.push_back(eigenRotToEigenVector3dAngleAxis( + robotTipinFirstTipBase.rotation())); + tvecsArm.push_back(robotTipinFirstTipBase.translation()); + + rvecsFiducial.push_back(eigenRotToEigenVector3dAngleAxis( + fiducialInFirstFiducialBase.rotation())); + tvecsFiducial.push_back(fiducialInFirstFiducialBase.translation()); + ROS_INFO("Hand Eye Calibration Transform Pair Added"); + + Eigen::Vector4d r_tmp = robotTipinFirstTipBase.matrix().col(3); + r_tmp[3] = 0; + Eigen::Vector4d c_tmp = fiducialInFirstFiducialBase.matrix().col(3); + c_tmp[3] = 0; + + std::cerr + << "L2Norm EE: " + << robotTipinFirstTipBase.matrix().block(0, 3, 3, 1).norm() + << " vs Cam:" + << fiducialInFirstFiducialBase.matrix().block(0, 3, 3, 1).norm() + << std::endl; + } + std::cerr << "EE transform: \n" << eigenEE.matrix() << std::endl; + std::cerr << "Cam transform: \n" << eigenCam.matrix() << std::endl; + } + + camodocal::HandEyeCalibration calib; + Eigen::Matrix4d result; + calib.estimateHandEyeScrew(rvecsArm, tvecsArm, rvecsFiducial, tvecsFiducial, + result, false); + std::cerr << "Result from " << EETFname << " to " << ARTagTFname << ":\n" + << result << std::endl; + Eigen::Transform resultAffine(result); + std::cerr << "Translation (x,y,z) : " + << resultAffine.translation().transpose() << std::endl; + Eigen::Quaternion quaternionResult(resultAffine.rotation()); + std::stringstream ss; + ss << quaternionResult.w() << ", " << quaternionResult.x() << ", " + << quaternionResult.y() << ", " << quaternionResult.z() << std::endl; + std::cerr << "Rotation (w,x,y,z): " << ss.str() << std::endl; + + std::cerr << "Result from " << ARTagTFname << " to " << EETFname << ":\n" + << result << std::endl; + Eigen::Transform resultAffineInv = + resultAffine.inverse(); + std::cerr << "Inverted translation (x,y,z) : " + << resultAffineInv.translation().transpose() << std::endl; + quaternionResult = Eigen::Quaternion(resultAffineInv.rotation()); + ss.clear(); + ss << quaternionResult.w() << " " << quaternionResult.x() << " " + << quaternionResult.y() << " " << quaternionResult.z() << std::endl; + std::cerr << "Inverted rotation (w,x,y,z): " << ss.str() << std::endl; + return resultAffine; +} + +Eigen::Affine3d estimateHandEye(const EigenAffineVector& baseToTip, + const EigenAffineVector& camToTag, + ceres::Solver::Summary& summary) { + + auto t1_it = baseToTip.begin(); + auto t2_it = camToTag.begin(); + + Eigen::Affine3d firstEEInverse, firstCamInverse; + eigenVector tvecsArm, rvecsArm, tvecsFiducial, rvecsFiducial; + + bool firstTransform = true; + + for (int i = 0; i < baseToTip.size(); ++i, ++t1_it, ++t2_it) { + auto& eigenEE = *t1_it; + auto& eigenCam = *t2_it; + if (firstTransform) { + firstEEInverse = eigenEE.inverse(); + firstCamInverse = eigenCam.inverse(); + ROS_INFO("Adding first transformation."); + firstTransform = false; + } else { + Eigen::Affine3d robotTipinFirstTipBase = firstEEInverse * eigenEE; + Eigen::Affine3d fiducialInFirstFiducialBase = + firstCamInverse * eigenCam; + + rvecsArm.push_back(eigenRotToEigenVector3dAngleAxis( + robotTipinFirstTipBase.rotation())); + tvecsArm.push_back(robotTipinFirstTipBase.translation()); + + rvecsFiducial.push_back(eigenRotToEigenVector3dAngleAxis( + fiducialInFirstFiducialBase.rotation())); + tvecsFiducial.push_back(fiducialInFirstFiducialBase.translation()); + ROS_INFO("Hand Eye Calibration Transform Pair Added"); + + Eigen::Vector4d r_tmp = robotTipinFirstTipBase.matrix().col(3); + r_tmp[3] = 0; + Eigen::Vector4d c_tmp = fiducialInFirstFiducialBase.matrix().col(3); + c_tmp[3] = 0; + + std::cerr + << "L2Norm EE: " + << robotTipinFirstTipBase.matrix().block(0, 3, 3, 1).norm() + << " vs Cam:" + << fiducialInFirstFiducialBase.matrix().block(0, 3, 3, 1).norm() + << std::endl; + } + std::cerr << "EE transform: \n" << eigenEE.matrix() << std::endl; + std::cerr << "Cam transform: \n" << eigenCam.matrix() << std::endl; + } + + camodocal::HandEyeCalibration calib; + Eigen::Matrix4d result; + calib.estimateHandEyeScrew(rvecsArm, tvecsArm, rvecsFiducial, tvecsFiducial, + result, summary, false); + std::cerr << "Result from " << EETFname << " to " << ARTagTFname << ":\n" + << result << std::endl; + Eigen::Transform resultAffine(result); + std::cerr << "Translation (x,y,z) : " + << resultAffine.translation().transpose() << std::endl; + Eigen::Quaternion quaternionResult(resultAffine.rotation()); + std::stringstream ss; + ss << quaternionResult.w() << ", " << quaternionResult.x() << ", " + << quaternionResult.y() << ", " << quaternionResult.z() << std::endl; + std::cerr << "Rotation (w,x,y,z): " << ss.str() << std::endl; + + std::cerr << "Result from " << ARTagTFname << " to " << EETFname << ":\n" + << result << std::endl; + Eigen::Transform resultAffineInv = + resultAffine.inverse(); + std::cerr << "Inverted translation (x,y,z) : " + << resultAffineInv.translation().transpose() << std::endl; + quaternionResult = Eigen::Quaternion(resultAffineInv.rotation()); + ss.clear(); + ss << quaternionResult.w() << " " << quaternionResult.x() << " " + << quaternionResult.y() << " " << quaternionResult.z() << std::endl; + std::cerr << "Inverted rotation (w,x,y,z): " << ss.str() << std::endl; + return resultAffine; +} + +void writeCalibration(const Eigen::Affine3d& result, + const std::string& filename) { + cv::FileStorage fs(filename, cv::FileStorage::WRITE); + + std::cerr << "Writing calibration to \"" << filename << "\"...\n"; + if (fs.isOpened()) { + cv::Mat_ t1cv = cv::Mat_::ones(4, 4); + cv::eigen2cv(result.matrix(), t1cv); + + fs << "ArmTipToMarkerTagTransform" << t1cv; + + fs.release(); + } +} + +void writeCalibration(const Eigen::Affine3d& result, + const std::string& filename, + ceres::Solver::Summary& summary) { + cv::FileStorage fs(filename, cv::FileStorage::WRITE); + + std::cerr << "FULL CONVERGENCE REPORT \"" + << "\"...\n"; + std::cout << summary.BriefReport() << "\n"; + std::cout << summary.termination_type << "\n"; + std::cerr << "Writing calibration to \"" << filename << "\"...\n"; + if (fs.isOpened()) { + cv::Mat_ t1cv = cv::Mat_::ones(4, 4); + cv::eigen2cv(result.matrix(), t1cv); + + fs << "ArmTipToMarkerTagTransform" << t1cv; + fs << "initial_cost" << summary.initial_cost; + fs << "final_cost" << summary.final_cost; + fs << "change_cost" << summary.initial_cost - summary.final_cost; + fs << "termination_type" << summary.termination_type; + fs << "num_successful_iteration" << summary.num_successful_steps; + fs << "num_unsuccessful_iteration" << summary.num_unsuccessful_steps; + fs << "num_iteration" + << summary.num_unsuccessful_steps + summary.num_successful_steps; + fs.release(); + } +} + +Eigen::Affine3d estimateHandEye(const EigenAffineVector& baseToTip, + const EigenAffineVector& camToTag, + const std::string& filename, + const bool addSolverSummary) { + if (addSolverSummary) { + + ceres::Solver::Summary summary; + auto result = estimateHandEye(baseToTip, camToTag, summary); + writeCalibration(result, filename, summary); + return result; + } else { + auto result = estimateHandEye(baseToTip, camToTag); + writeCalibration(result, filename); + return result; + } +} + +// function getch is from +// http://answers.ros.org/question/63491/keyboard-key-pressed/ +int getch() { + static struct termios oldt, newt; + tcgetattr(STDIN_FILENO, &oldt); // save old settings + newt = oldt; + newt.c_lflag &= ~(ICANON); // disable buffering + tcsetattr(STDIN_FILENO, TCSANOW, &newt); // apply new settings + + int c = getchar(); // read character (non-blocking) + + tcsetattr(STDIN_FILENO, TCSANOW, &oldt); // restore old settings + return c; +} + +void addFrame() { + ros::Time now = ros::Time::now(); + tf::StampedTransform EETransform, CamTransform; + bool hasEE = true, hasCam = true; + + if (listener->waitForTransform(cameraTFname, ARTagTFname, now, + ros::Duration(1))) + listener->lookupTransform(cameraTFname, ARTagTFname, now, CamTransform); + else { + hasCam = false; + ROS_WARN("Fail to Cam TF transform between %s to %s", + cameraTFname.c_str(), ARTagTFname.c_str()); + } + + if (listener->waitForTransform(baseTFname, EETFname, now, ros::Duration(1))) + listener->lookupTransform(baseTFname, EETFname, now, EETransform); + else { + hasEE = false; + ROS_WARN("Fail to EE TF transform between %s to %s", baseTFname.c_str(), + EETFname.c_str()); + } + + if (hasEE and hasCam) { + Eigen::Affine3d eigenEE, eigenCam; + tf::transformTFToEigen(EETransform, eigenEE); + tf::transformTFToEigen(CamTransform, eigenCam); + + baseToTip.push_back(eigenEE); + cameraToTag.push_back(eigenCam); + + if (firstTransform) { + firstEEInverse = eigenEE.inverse(); + firstCamInverse = eigenCam.inverse(); + ROS_INFO("Adding first transformation."); + firstTransform = false; + } else { + Eigen::Affine3d robotTipinFirstTipBase = firstEEInverse * eigenEE; + Eigen::Affine3d fiducialInFirstFiducialBase = + firstCamInverse * eigenCam; + + rvecsArm.push_back(eigenRotToEigenVector3dAngleAxis( + robotTipinFirstTipBase.rotation())); + tvecsArm.push_back(robotTipinFirstTipBase.translation()); + + rvecsFiducial.push_back(eigenRotToEigenVector3dAngleAxis( + fiducialInFirstFiducialBase.rotation())); + tvecsFiducial.push_back(fiducialInFirstFiducialBase.translation()); + ROS_INFO("Hand Eye Calibration Transform Pair Added"); + + std::cerr << "EE Relative transform: \n" + << robotTipinFirstTipBase.matrix() << std::endl; + std::cerr << "Cam Relative transform: \n" + << fiducialInFirstFiducialBase.matrix() << std::endl; + std::cerr << "EE pos: (" << EETransform.getOrigin().getX() << ", " + << EETransform.getOrigin().getY() << ", " + << EETransform.getOrigin().getZ() << ")\n"; + std::cerr << "EE rot: (" + << EETransform.getRotation().getAxis().getX() << ", " + << EETransform.getRotation().getAxis().getY() << ", " + << EETransform.getRotation().getAxis().getZ() << ", " + << EETransform.getRotation().getW() << ")\n"; + Eigen::Vector4d r_tmp = robotTipinFirstTipBase.matrix().col(3); + r_tmp[3] = 0; + Eigen::Vector4d c_tmp = fiducialInFirstFiducialBase.matrix().col(3); + c_tmp[3] = 0; + + std::cerr + << "L2Norm EE: " + << robotTipinFirstTipBase.matrix().block(0, 3, 3, 1).norm() + << " vs Cam:" + << fiducialInFirstFiducialBase.matrix().block(0, 3, 3, 1).norm() + << std::endl; + } + std::cerr << "EE transform: \n" << eigenEE.matrix() << std::endl; + std::cerr << "Cam transform: \n" << eigenCam.matrix() << std::endl; + } else { + ROS_WARN("Fail to get one/both of needed TF transform"); + } +} + +void keyCallback(const std_msgs::String::ConstPtr& msg) { + key = msg->data.c_str(); +} + +int main(int argc, char** argv) { + ros::init(argc, argv, "handeye_calib_camodocal"); + ros::NodeHandle nh("~"); + std::string transformPairsRecordFile; + std::string transformPairsLoadFile; + std::string calibratedTransformFile; + bool loadTransformsFromFile = false; + bool addSolverSummary = false; + + // getting TF names + nh.param("ARTagTF", ARTagTFname, std::string("/camera_2/ar_marker_0")); + nh.param("cameraTF", cameraTFname, std::string("/camera_2_link")); + nh.param("EETF", EETFname, std::string("/ee_fixed_link")); + nh.param("baseTF", baseTFname, std::string("/base_link")); + nh.param("add_solver_summary", addSolverSummary, false); + nh.param("transform_pairs_record_filename", transformPairsRecordFile, + std::string("TransformPairsInput.yml")); + nh.param("transform_pairs_load_filename", transformPairsLoadFile, + std::string("TransformPairsOutput.yml")); + nh.param("output_calibrated_transform_filename", calibratedTransformFile, + std::string("CalibratedTransform.yml")); + + std::cerr << "Calibrated output file: " << calibratedTransformFile << "\n"; + std::cerr << "Transform pairs recording to file: " + << transformPairsRecordFile << "\n"; + + key = ""; + ros::Subscriber sub = nh.subscribe("/handeye_calib/key", 1, keyCallback); + ros::Rate r(10); // 10 hz + listener = new (tf::TransformListener); + + ros::Duration(1.0).sleep(); // cache the TF transforms + + while (ros::ok()) { + if ((key == "s") || (key == "S")) { + std::cerr << "Adding Transform #:" << rvecsArm.size() << "\n"; + addFrame(); + writeTransformPairsToFile(baseToTip, cameraToTag, + transformPairsRecordFile); + } else if ((key == "d") || (key == "D")) { + ROS_INFO("Deleted last frame transformation. Number of Current " + "Transformations: %u", + (unsigned int)rvecsArm.size()); + rvecsArm.pop_back(); + tvecsArm.pop_back(); + rvecsFiducial.pop_back(); + tvecsFiducial.pop_back(); + baseToTip.pop_back(); + cameraToTag.pop_back(); + } else if ((key == "q") || (key == "Q")) { + if (rvecsArm.size() < 5) { + ROS_WARN("Number of calibration transform pairs < 5."); + ROS_INFO("Node Quit"); + } + ROS_INFO("Calculating Calibration..."); + camodocal::HandEyeCalibration calib; + Eigen::Matrix4d result; + ceres::Solver::Summary summary; + + if (addSolverSummary) { + calib.estimateHandEyeScrew(rvecsArm, tvecsArm, rvecsFiducial, + tvecsFiducial, result, summary, + false); + } else { + calib.estimateHandEyeScrew(rvecsArm, tvecsArm, rvecsFiducial, + tvecsFiducial, result, false); + } + + std::cerr << "Quaternion values are output in wxyz order\n"; + + std::cerr << "Calibration result (" << ARTagTFname << " pose in " + << EETFname << " frame): \n" + << result << std::endl; + Eigen::Transform resultAffine(result); + std::cerr << "Translation (x,y,z) : " + << resultAffine.translation().transpose() << std::endl; + Eigen::Quaternion quaternionResult(resultAffine.rotation()); + std::stringstream ss; + ss << quaternionResult.w() << " " << quaternionResult.x() << " " + << quaternionResult.y() << " " << quaternionResult.z() + << std::endl; + std::cerr << "Rotation (w,x,y,z): " << ss.str() << std::endl; + + if (addSolverSummary) { + writeCalibration(resultAffine, calibratedTransformFile, + summary); + } else { + writeCalibration(resultAffine, calibratedTransformFile); + } + + Eigen::Transform resultAffineInv = + resultAffine.inverse(); + std::cerr << "Inverted Calibration result (" << EETFname + << " pose in " << ARTagTFname << " frame): \n"; + std::cerr << "Translation (x,y,z): " + << resultAffineInv.translation().transpose() << std::endl; + quaternionResult = + Eigen::Quaternion(resultAffineInv.rotation()); + ss.clear(); + ss << quaternionResult.w() << " " << quaternionResult.x() << " " + << quaternionResult.y() << " " << quaternionResult.z() + << std::endl; + std::cerr << "Rotation (w,x,y,z): " << ss.str() << std::endl; + + break; + } else { + std::cerr << key << " received.\n"; + } + key = ""; + ros::spinOnce(); + r.sleep(); + } + + ros::shutdown(); + return (0); +} From 5c5def77517db05317ac7b4e89b724bbee41a65c Mon Sep 17 00:00:00 2001 From: iory Date: Thu, 7 Jun 2018 10:50:26 +0900 Subject: [PATCH 02/15] Modified package.xml for dependencies --- package.xml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/package.xml b/package.xml index ad10e18..07bb960 100644 --- a/package.xml +++ b/package.xml @@ -40,11 +40,14 @@ catkin + std_msgs eigen roscpp tf trajectory_msgs + libceres-dev + std_msgs eigen roscpp tf From 1b7a68ccf9a198b41cfc3f9f8de1517307d75048 Mon Sep 17 00:00:00 2001 From: iory Date: Thu, 7 Jun 2018 11:27:22 +0900 Subject: [PATCH 03/15] [handeye_calibration_auto] Modified --- src/handeye_calibration_auto.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/handeye_calibration_auto.cpp b/src/handeye_calibration_auto.cpp index 4abc492..ce036b6 100644 --- a/src/handeye_calibration_auto.cpp +++ b/src/handeye_calibration_auto.cpp @@ -532,7 +532,7 @@ int main(int argc, char** argv) { std::cerr << "Rotation (w,x,y,z): " << ss.str() << std::endl; break; - } else { + } else if (key.length() > 0) { std::cerr << key << " received.\n"; } key = ""; From dfb9b8244f54e748361312c3395664c86d3f8bf6 Mon Sep 17 00:00:00 2001 From: iory Date: Thu, 7 Jun 2018 17:21:10 +0900 Subject: [PATCH 04/15] Add --- src/handeye_calibration_auto.cpp | 90 ++++++++++++++++++++++++++++++++ 1 file changed, 90 insertions(+) diff --git a/src/handeye_calibration_auto.cpp b/src/handeye_calibration_auto.cpp index ce036b6..bf737c2 100644 --- a/src/handeye_calibration_auto.cpp +++ b/src/handeye_calibration_auto.cpp @@ -2,6 +2,7 @@ #include "ceres/types.h" #include #include +#include #include #include #include @@ -435,12 +436,19 @@ int main(int argc, char** argv) { std::string calibratedTransformFile; bool loadTransformsFromFile = false; bool addSolverSummary = false; + std::string output_launch_filename; + std::string base_to_camera_output_launch_filename; // getting TF names nh.param("ARTagTF", ARTagTFname, std::string("/camera_2/ar_marker_0")); nh.param("cameraTF", cameraTFname, std::string("/camera_2_link")); nh.param("EETF", EETFname, std::string("/ee_fixed_link")); nh.param("baseTF", baseTFname, std::string("/base_link")); + nh.param("output_launch_filename", output_launch_filename, + std::string("/tmp/maker_to_ee_static_transform_publisher.launch")); + nh.param( + "output_launch_filename", base_to_camera_output_launch_filename, + std::string("/tmp/base_to_camera_static_transform_publisher.launch")); nh.param("add_solver_summary", addSolverSummary, false); nh.param("transform_pairs_record_filename", transformPairsRecordFile, std::string("TransformPairsInput.yml")); @@ -510,6 +518,88 @@ int main(int argc, char** argv) { << std::endl; std::cerr << "Rotation (w,x,y,z): " << ss.str() << std::endl; + double x = resultAffine.translation().transpose()[0]; + double y = resultAffine.translation().transpose()[1]; + double z = resultAffine.translation().transpose()[2]; + std::ofstream outputfile(output_launch_filename); + outputfile << "" << std::endl; + outputfile << " "; + outputfile << " "; + outputfile << " " + << std::endl; + outputfile << "" << std::endl; + outputfile.close(); + + tf::StampedTransform EETransform; + tf::StampedTransform Camera2MarkerTransform; + Eigen::Affine3d eigenEE; + Eigen::Affine3d eigenCamera2Marker; + ros::Time now = ros::Time::now(); + if (listener->waitForTransform(baseTFname, EETFname, now, + ros::Duration(1)) && + listener->waitForTransform(ARTagTFname, cameraTFname, now, + ros::Duration(1))) { + listener->lookupTransform(baseTFname, EETFname, now, + EETransform); + listener->lookupTransform(ARTagTFname, cameraTFname, now, + Camera2MarkerTransform); + tf::transformTFToEigen(EETransform, eigenEE); + tf::transformTFToEigen(Camera2MarkerTransform, + eigenCamera2Marker); + Eigen::Affine3d BaseToCamera = + (eigenEE * resultAffine * eigenCamera2Marker.inverse()) + .inverse(); + Eigen::Transform result_affine( + BaseToCamera); + Eigen::Quaternion quaternion_result( + result_affine.rotation()); + x = result_affine.translation().transpose()[0]; + y = result_affine.translation().transpose()[1]; + z = result_affine.translation().transpose()[2]; + std::ofstream base_to_camera_outputfile( + base_to_camera_output_launch_filename); + base_to_camera_outputfile << "" << std::endl; + base_to_camera_outputfile + << " "; + base_to_camera_outputfile + << " "; + base_to_camera_outputfile + << " " + << std::endl; + base_to_camera_outputfile << "" << std::endl; + base_to_camera_outputfile.close(); + } else { + std::cerr << "Could not get TF" << std::endl; + } + if (addSolverSummary) { writeCalibration(resultAffine, calibratedTransformFile, summary); From d8c70fbac469e23aa2948d7db4e75e8d62cbbe8a Mon Sep 17 00:00:00 2001 From: iory Date: Thu, 7 Jun 2018 21:25:56 +0900 Subject: [PATCH 05/15] Fixed --- src/handeye_calibration_auto.cpp | 45 ++++++++++++++++---------------- 1 file changed, 23 insertions(+), 22 deletions(-) diff --git a/src/handeye_calibration_auto.cpp b/src/handeye_calibration_auto.cpp index bf737c2..d052026 100644 --- a/src/handeye_calibration_auto.cpp +++ b/src/handeye_calibration_auto.cpp @@ -524,28 +524,28 @@ int main(int argc, char** argv) { std::ofstream outputfile(output_launch_filename); outputfile << "" << std::endl; outputfile << " "; + << "\" />" << std::endl; outputfile << " "; + << ARTagTFname << "\" />" << std::endl; outputfile << " " + << " $(arg ee_frame) /endpoint_marker 1000\" />" << std::endl; outputfile << "" << std::endl; outputfile.close(); tf::StampedTransform EETransform; - tf::StampedTransform Camera2MarkerTransform; + tf::StampedTransform Marker2CameraTransform; Eigen::Affine3d eigenEE; - Eigen::Affine3d eigenCamera2Marker; + Eigen::Affine3d eigenMarker2Camera; ros::Time now = ros::Time::now(); if (listener->waitForTransform(baseTFname, EETFname, now, ros::Duration(1)) && @@ -554,15 +554,16 @@ int main(int argc, char** argv) { listener->lookupTransform(baseTFname, EETFname, now, EETransform); listener->lookupTransform(ARTagTFname, cameraTFname, now, - Camera2MarkerTransform); + Marker2CameraTransform); tf::transformTFToEigen(EETransform, eigenEE); - tf::transformTFToEigen(Camera2MarkerTransform, - eigenCamera2Marker); - Eigen::Affine3d BaseToCamera = - (eigenEE * resultAffine * eigenCamera2Marker.inverse()) - .inverse(); - Eigen::Transform result_affine( - BaseToCamera); + tf::transformTFToEigen(Marker2CameraTransform, + eigenMarker2Camera); + Eigen::Matrix4d m = eigenEE.matrix(); + m *= resultAffine.matrix(); + m *= eigenMarker2Camera.matrix(); + Eigen::Affine3d result_affine; + result_affine.matrix() = m; + Eigen::Quaternion quaternion_result( result_affine.rotation()); x = result_affine.translation().transpose()[0]; @@ -573,10 +574,10 @@ int main(int argc, char** argv) { base_to_camera_outputfile << "" << std::endl; base_to_camera_outputfile << " "; + << "\" />" << std::endl; base_to_camera_outputfile << " "; + << "\" />" << std::endl; base_to_camera_outputfile << " " + "$(arg camera_frame) 1000\" />" << std::endl; base_to_camera_outputfile << "" << std::endl; base_to_camera_outputfile.close(); From b1807856b6912b2007bc0914c48bda152da2ed4a Mon Sep 17 00:00:00 2001 From: iory Date: Thu, 7 Jun 2018 21:30:25 +0900 Subject: [PATCH 06/15] Add --- src/handeye_calibration_auto.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/handeye_calibration_auto.cpp b/src/handeye_calibration_auto.cpp index d052026..dded6df 100644 --- a/src/handeye_calibration_auto.cpp +++ b/src/handeye_calibration_auto.cpp @@ -447,7 +447,8 @@ int main(int argc, char** argv) { nh.param("output_launch_filename", output_launch_filename, std::string("/tmp/maker_to_ee_static_transform_publisher.launch")); nh.param( - "output_launch_filename", base_to_camera_output_launch_filename, + "base_to_camera_output_launch_filename", + base_to_camera_output_launch_filename, std::string("/tmp/base_to_camera_static_transform_publisher.launch")); nh.param("add_solver_summary", addSolverSummary, false); nh.param("transform_pairs_record_filename", transformPairsRecordFile, From 50765805a3c00b9ae752c8335d2bc7c7ba500d8d Mon Sep 17 00:00:00 2001 From: iory Date: Mon, 25 Jun 2018 11:03:56 +0900 Subject: [PATCH 07/15] Modified --- src/handeye_calibration_auto.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/handeye_calibration_auto.cpp b/src/handeye_calibration_auto.cpp index dded6df..078e4b8 100644 --- a/src/handeye_calibration_auto.cpp +++ b/src/handeye_calibration_auto.cpp @@ -350,7 +350,7 @@ void addFrame() { bool hasEE = true, hasCam = true; if (listener->waitForTransform(cameraTFname, ARTagTFname, now, - ros::Duration(1))) + ros::Duration(3.0))) listener->lookupTransform(cameraTFname, ARTagTFname, now, CamTransform); else { hasCam = false; @@ -549,9 +549,9 @@ int main(int argc, char** argv) { Eigen::Affine3d eigenMarker2Camera; ros::Time now = ros::Time::now(); if (listener->waitForTransform(baseTFname, EETFname, now, - ros::Duration(1)) && + ros::Duration(3.0)) && listener->waitForTransform(ARTagTFname, cameraTFname, now, - ros::Duration(1))) { + ros::Duration(3.0))) { listener->lookupTransform(baseTFname, EETFname, now, EETransform); listener->lookupTransform(ARTagTFname, cameraTFname, now, @@ -600,6 +600,9 @@ int main(int argc, char** argv) { base_to_camera_outputfile.close(); } else { std::cerr << "Could not get TF" << std::endl; + ROS_WARN( + "Fail to get TF, so base_to_camera transformation " + "data not generated. Please check the camera position."); } if (addSolverSummary) { From f64320bc5bf43fe6a29fa7f106a343c912c23232 Mon Sep 17 00:00:00 2001 From: iory Date: Thu, 5 Jul 2018 16:48:12 +0900 Subject: [PATCH 08/15] Modified log --- src/handeye_calibration.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/handeye_calibration.cpp b/src/handeye_calibration.cpp index 0afbadc..ce52baf 100644 --- a/src/handeye_calibration.cpp +++ b/src/handeye_calibration.cpp @@ -104,7 +104,7 @@ int readTransformPairsFromFile(std::string filename, EigenAffineVector& t1, } fs.release(); } else { - std::cerr << "failed to open input file " << filename << "\n"; + ROS_ERROR("failed to open input file %s", filename.c_str()); return 1; } return 0; From 69812fcb5f9ca46dd2537e3dbba50960c7f57183 Mon Sep 17 00:00:00 2001 From: iory Date: Thu, 5 Jul 2018 16:48:35 +0900 Subject: [PATCH 09/15] Modified for error processing --- src/handeye_calibration.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/handeye_calibration.cpp b/src/handeye_calibration.cpp index ce52baf..c9f18e6 100644 --- a/src/handeye_calibration.cpp +++ b/src/handeye_calibration.cpp @@ -450,10 +450,11 @@ int main(int argc, char** argv) { std::cerr << "Transform pairs loading file: " << transformPairsLoadFile << "\n"; EigenAffineVector t1, t2; - readTransformPairsFromFile(transformPairsLoadFile, t1, t2); - auto result = - estimateHandEye(t1, t2, calibratedTransformFile, addSolverSummary); - + int ret = readTransformPairsFromFile(transformPairsLoadFile, t1, t2); + if (ret == 0) { + auto result = estimateHandEye(t1, t2, calibratedTransformFile, + addSolverSummary); + } return 0; } From 8eaac1a8c310fdc9636c030b209f0ef313c876ab Mon Sep 17 00:00:00 2001 From: iory Date: Thu, 5 Jul 2018 17:17:17 +0900 Subject: [PATCH 10/15] output launch file --- src/handeye_calibration.cpp | 31 +++++++++++++++++++++++++++++++ 1 file changed, 31 insertions(+) diff --git a/src/handeye_calibration.cpp b/src/handeye_calibration.cpp index c9f18e6..f7f547e 100644 --- a/src/handeye_calibration.cpp +++ b/src/handeye_calibration.cpp @@ -2,6 +2,7 @@ #include "ceres/types.h" #include #include +#include #include #include #include @@ -429,6 +430,7 @@ int main(int argc, char** argv) { std::string calibratedTransformFile; bool loadTransformsFromFile = false; bool addSolverSummary = false; + std::string output_launch_filename; // getting TF names nh.param("ARTagTF", ARTagTFname, std::string("/camera_2/ar_marker_0")); @@ -437,6 +439,8 @@ int main(int argc, char** argv) { nh.param("baseTF", baseTFname, std::string("/base_link")); nh.param("add_solver_summary", addSolverSummary, false); nh.param("load_transforms_from_file", loadTransformsFromFile, false); + nh.param("output_launch_filename", output_launch_filename, + std::string("/tmp/maker_to_ee_static_transform_publisher.launch")); nh.param("transform_pairs_record_filename", transformPairsRecordFile, std::string("TransformPairsInput.yml")); nh.param("transform_pairs_load_filename", transformPairsLoadFile, @@ -454,6 +458,33 @@ int main(int argc, char** argv) { if (ret == 0) { auto result = estimateHandEye(t1, t2, calibratedTransformFile, addSolverSummary); + + Eigen::Transform resultAffine(result); + Eigen::Quaternion quaternionResult(resultAffine.rotation()); + double x = resultAffine.translation().transpose()[0]; + double y = resultAffine.translation().transpose()[1]; + double z = resultAffine.translation().transpose()[2]; + + std::ofstream outputfile(output_launch_filename); + outputfile << "" << std::endl; + outputfile << " " << std::endl; + outputfile << " " << std::endl; + outputfile << " " + << std::endl; + outputfile << "" << std::endl; + outputfile.close(); } return 0; } From 9ee611bcb569f8bd93fd4cfb6d7ed04b6d043184 Mon Sep 17 00:00:00 2001 From: iory Date: Thu, 5 Jul 2018 17:45:40 +0900 Subject: [PATCH 11/15] Add service --- CMakeLists.txt | 10 ++++++++++ package.xml | 2 ++ srv/HandEyeCalibCommand.srv | 3 +++ 3 files changed, 15 insertions(+) create mode 100644 srv/HandEyeCalibCommand.srv diff --git a/CMakeLists.txt b/CMakeLists.txt index c756472..2b6f456 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -20,12 +20,22 @@ endif() ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS + cmake_modules + message_generation + std_msgs tf_conversions roscpp tf trajectory_msgs #vrep_common ) + +add_service_files(FILES + HandEyeCalibCommand.srv) + +generate_messages( + DEPENDENCIES std_msgs) + find_package(Gflags REQUIRED) find_package(Glog REQUIRED) diff --git a/package.xml b/package.xml index 07bb960..1c271a4 100644 --- a/package.xml +++ b/package.xml @@ -46,12 +46,14 @@ tf trajectory_msgs libceres-dev + message_generation std_msgs eigen roscpp tf trajectory_msgs + message_runtime diff --git a/srv/HandEyeCalibCommand.srv b/srv/HandEyeCalibCommand.srv new file mode 100644 index 0000000..a15dca6 --- /dev/null +++ b/srv/HandEyeCalibCommand.srv @@ -0,0 +1,3 @@ +std_msgs/String command +--- +std_msgs/Bool succeeded From d43f0e519d4a90f047037d58124eef1c7bdcfeab Mon Sep 17 00:00:00 2001 From: iory Date: Thu, 5 Jul 2018 17:46:08 +0900 Subject: [PATCH 12/15] Add service --- src/handeye_calibration_auto.cpp | 24 +++++++++++++++++++++++- 1 file changed, 23 insertions(+), 1 deletion(-) diff --git a/src/handeye_calibration_auto.cpp b/src/handeye_calibration_auto.cpp index 078e4b8..cdc96ba 100644 --- a/src/handeye_calibration_auto.cpp +++ b/src/handeye_calibration_auto.cpp @@ -1,10 +1,12 @@ #include "ceres/ceres.h" #include "ceres/types.h" +#include "handeye_calib_camodocal/HandEyeCalibCommand.h" #include #include #include #include #include +#include #include #include #include @@ -26,6 +28,7 @@ Eigen::Affine3d firstEEInverse, firstCamInverse; bool firstTransform = true; eigenVector tvecsArm, rvecsArm, tvecsFiducial, rvecsFiducial; std::string key; +std::string transformPairsRecordFile; EigenAffineVector baseToTip, cameraToTag; @@ -428,10 +431,26 @@ void keyCallback(const std_msgs::String::ConstPtr& msg) { key = msg->data.c_str(); } +bool handeye_calib_command( + handeye_calib_camodocal::HandEyeCalibCommand::Request& req, + handeye_calib_camodocal::HandEyeCalibCommand::Response& res) { + int ret; + if (req.command.data == "s") { + addFrame(); + ret = writeTransformPairsToFile(baseToTip, cameraToTag, + transformPairsRecordFile); + if (ret == 0) { + res.succeeded.data = true; + } else { + res.succeeded.data = false; + } + } + return true; +} + int main(int argc, char** argv) { ros::init(argc, argv, "handeye_calib_camodocal"); ros::NodeHandle nh("~"); - std::string transformPairsRecordFile; std::string transformPairsLoadFile; std::string calibratedTransformFile; bool loadTransformsFromFile = false; @@ -439,6 +458,9 @@ int main(int argc, char** argv) { std::string output_launch_filename; std::string base_to_camera_output_launch_filename; + ros::ServiceServer service = + nh.advertiseService("handeye_calib", handeye_calib_command); + // getting TF names nh.param("ARTagTF", ARTagTFname, std::string("/camera_2/ar_marker_0")); nh.param("cameraTF", cameraTFname, std::string("/camera_2_link")); From a87e36a9c62d9e30f577dd6b92df68ac1e1c2fa4 Mon Sep 17 00:00:00 2001 From: iory Date: Tue, 18 Sep 2018 22:57:53 +0900 Subject: [PATCH 13/15] Add loadTransformFromfile --- src/handeye_calibration.cpp | 2 +- src/handeye_calibration_auto.cpp | 40 ++++++++++++++++++++++++++++++++ 2 files changed, 41 insertions(+), 1 deletion(-) diff --git a/src/handeye_calibration.cpp b/src/handeye_calibration.cpp index f7f547e..382fe80 100644 --- a/src/handeye_calibration.cpp +++ b/src/handeye_calibration.cpp @@ -481,7 +481,7 @@ int main(int argc, char** argv) { << quaternionResult.y() << " " << quaternionResult.z() << " " << quaternionResult.w() << " " << std::endl; outputfile - << " $(arg ee_frame) /endpoint_marker 1000\" />" + << " $(arg ee_frame) $(arg marker_frame) 1000\" />" << std::endl; outputfile << "" << std::endl; outputfile.close(); diff --git a/src/handeye_calibration_auto.cpp b/src/handeye_calibration_auto.cpp index cdc96ba..8a358c8 100644 --- a/src/handeye_calibration_auto.cpp +++ b/src/handeye_calibration_auto.cpp @@ -466,6 +466,7 @@ int main(int argc, char** argv) { nh.param("cameraTF", cameraTFname, std::string("/camera_2_link")); nh.param("EETF", EETFname, std::string("/ee_fixed_link")); nh.param("baseTF", baseTFname, std::string("/base_link")); + nh.param("load_transforms_from_file", loadTransformsFromFile, false); nh.param("output_launch_filename", output_launch_filename, std::string("/tmp/maker_to_ee_static_transform_publisher.launch")); nh.param( @@ -484,6 +485,45 @@ int main(int argc, char** argv) { std::cerr << "Transform pairs recording to file: " << transformPairsRecordFile << "\n"; + if (loadTransformsFromFile) { + std::cerr << "Transform pairs loading file: " << transformPairsLoadFile + << "\n"; + EigenAffineVector t1, t2; + int ret = readTransformPairsFromFile(transformPairsLoadFile, t1, t2); + if (ret == 0) { + auto result = estimateHandEye(t1, t2, calibratedTransformFile, + addSolverSummary); + + Eigen::Transform resultAffine(result); + Eigen::Quaternion quaternionResult(resultAffine.rotation()); + double x = resultAffine.translation().transpose()[0]; + double y = resultAffine.translation().transpose()[1]; + double z = resultAffine.translation().transpose()[2]; + + std::ofstream outputfile(output_launch_filename); + outputfile << "" << std::endl; + outputfile << " " << std::endl; + outputfile << " " << std::endl; + outputfile << " " + << std::endl; + outputfile << "" << std::endl; + outputfile.close(); + } + return 0; + } + key = ""; ros::Subscriber sub = nh.subscribe("/handeye_calib/key", 1, keyCallback); ros::Rate r(10); // 10 hz From 672b9313907338dea311178353d9f8548aa36d8f Mon Sep 17 00:00:00 2001 From: iory Date: Wed, 19 Sep 2018 01:18:49 +0900 Subject: [PATCH 14/15] Add --- src/handeye_calibration_auto.cpp | 112 +++++++++++++++++++++++-------- 1 file changed, 83 insertions(+), 29 deletions(-) diff --git a/src/handeye_calibration_auto.cpp b/src/handeye_calibration_auto.cpp index 8a358c8..9b9c754 100644 --- a/src/handeye_calibration_auto.cpp +++ b/src/handeye_calibration_auto.cpp @@ -491,35 +491,89 @@ int main(int argc, char** argv) { EigenAffineVector t1, t2; int ret = readTransformPairsFromFile(transformPairsLoadFile, t1, t2); if (ret == 0) { - auto result = estimateHandEye(t1, t2, calibratedTransformFile, - addSolverSummary); - - Eigen::Transform resultAffine(result); - Eigen::Quaternion quaternionResult(resultAffine.rotation()); - double x = resultAffine.translation().transpose()[0]; - double y = resultAffine.translation().transpose()[1]; - double z = resultAffine.translation().transpose()[2]; - - std::ofstream outputfile(output_launch_filename); - outputfile << "" << std::endl; - outputfile << " " << std::endl; - outputfile << " " << std::endl; - outputfile << " " - << std::endl; - outputfile << "" << std::endl; - outputfile.close(); + { + auto result = estimateHandEye(t1, t2, calibratedTransformFile, + addSolverSummary); + + Eigen::Transform resultAffine(result); + Eigen::Quaternion quaternionResult( + resultAffine.rotation()); + double x = resultAffine.translation().transpose()[0]; + double y = resultAffine.translation().transpose()[1]; + double z = resultAffine.translation().transpose()[2]; + + std::ofstream outputfile(output_launch_filename); + outputfile << "" << std::endl; + outputfile << " " << std::endl; + outputfile << " " << std::endl; + outputfile << " " + << std::endl; + outputfile << "" << std::endl; + outputfile.close(); + } + { + EigenAffineVector tag_to_cam; + tag_to_cam.resize(t2.size()); + for (int tag_to_cam_index = 0; + tag_to_cam_index < tag_to_cam.size(); ++tag_to_cam_index) { + tag_to_cam[tag_to_cam_index] = + t2[tag_to_cam_index].inverse(); + } + EigenAffineVector ee_to_base; + ee_to_base.resize(t2.size()); + for (int ee_to_base_index = 0; + ee_to_base_index < ee_to_base.size(); ++ee_to_base_index) { + ee_to_base[ee_to_base_index] = + t1[ee_to_base_index].inverse(); + } + auto result = + estimateHandEye(ee_to_base, tag_to_cam, + calibratedTransformFile, addSolverSummary); + + Eigen::Transform resultAffine(result); + Eigen::Quaternion quaternionResult( + resultAffine.rotation()); + double x = resultAffine.translation().transpose()[0]; + double y = resultAffine.translation().transpose()[1]; + double z = resultAffine.translation().transpose()[2]; + + std::ofstream outputfile(base_to_camera_output_launch_filename); + outputfile << "" << std::endl; + outputfile << " " << std::endl; + outputfile << " " << std::endl; + outputfile << " " + << std::endl; + outputfile << "" << std::endl; + outputfile.close(); + } } return 0; } From 561e56fe1d27ee66f1ed3d7cd559f24b9f2e0975 Mon Sep 17 00:00:00 2001 From: iory Date: Wed, 19 Sep 2018 01:35:50 +0900 Subject: [PATCH 15/15] Refactor --- src/handeye_calibration_auto.cpp | 328 +++++++++---------------------- 1 file changed, 94 insertions(+), 234 deletions(-) diff --git a/src/handeye_calibration_auto.cpp b/src/handeye_calibration_auto.cpp index 9b9c754..fc19898 100644 --- a/src/handeye_calibration_auto.cpp +++ b/src/handeye_calibration_auto.cpp @@ -476,8 +476,6 @@ int main(int argc, char** argv) { nh.param("add_solver_summary", addSolverSummary, false); nh.param("transform_pairs_record_filename", transformPairsRecordFile, std::string("TransformPairsInput.yml")); - nh.param("transform_pairs_load_filename", transformPairsLoadFile, - std::string("TransformPairsOutput.yml")); nh.param("output_calibrated_transform_filename", calibratedTransformFile, std::string("CalibratedTransform.yml")); @@ -485,159 +483,62 @@ int main(int argc, char** argv) { std::cerr << "Transform pairs recording to file: " << transformPairsRecordFile << "\n"; - if (loadTransformsFromFile) { - std::cerr << "Transform pairs loading file: " << transformPairsLoadFile - << "\n"; - EigenAffineVector t1, t2; - int ret = readTransformPairsFromFile(transformPairsLoadFile, t1, t2); - if (ret == 0) { - { - auto result = estimateHandEye(t1, t2, calibratedTransformFile, - addSolverSummary); - - Eigen::Transform resultAffine(result); - Eigen::Quaternion quaternionResult( - resultAffine.rotation()); - double x = resultAffine.translation().transpose()[0]; - double y = resultAffine.translation().transpose()[1]; - double z = resultAffine.translation().transpose()[2]; - - std::ofstream outputfile(output_launch_filename); - outputfile << "" << std::endl; - outputfile << " " << std::endl; - outputfile << " " << std::endl; - outputfile << " " - << std::endl; - outputfile << "" << std::endl; - outputfile.close(); - } - { - EigenAffineVector tag_to_cam; - tag_to_cam.resize(t2.size()); - for (int tag_to_cam_index = 0; - tag_to_cam_index < tag_to_cam.size(); ++tag_to_cam_index) { - tag_to_cam[tag_to_cam_index] = - t2[tag_to_cam_index].inverse(); - } - EigenAffineVector ee_to_base; - ee_to_base.resize(t2.size()); - for (int ee_to_base_index = 0; - ee_to_base_index < ee_to_base.size(); ++ee_to_base_index) { - ee_to_base[ee_to_base_index] = - t1[ee_to_base_index].inverse(); - } - auto result = - estimateHandEye(ee_to_base, tag_to_cam, - calibratedTransformFile, addSolverSummary); - - Eigen::Transform resultAffine(result); - Eigen::Quaternion quaternionResult( - resultAffine.rotation()); - double x = resultAffine.translation().transpose()[0]; - double y = resultAffine.translation().transpose()[1]; - double z = resultAffine.translation().transpose()[2]; - - std::ofstream outputfile(base_to_camera_output_launch_filename); - outputfile << "" << std::endl; - outputfile << " " << std::endl; - outputfile << " " << std::endl; - outputfile << " " - << std::endl; - outputfile << "" << std::endl; - outputfile.close(); + if (loadTransformsFromFile == false) { + key = ""; + ros::Subscriber sub = + nh.subscribe("/handeye_calib/key", 1, keyCallback); + ros::Rate r(10); // 10 hz + listener = new (tf::TransformListener); + + ros::Duration(1.0).sleep(); // cache the TF transforms + + while (ros::ok()) { + if ((key == "s") || (key == "S")) { + std::cerr << "Adding Transform #:" << rvecsArm.size() << "\n"; + addFrame(); + writeTransformPairsToFile(baseToTip, cameraToTag, + transformPairsRecordFile); + } else if ((key == "d") || (key == "D")) { + ROS_INFO("Deleted last frame transformation. Number of Current " + "Transformations: %u", + (unsigned int)rvecsArm.size()); + rvecsArm.pop_back(); + tvecsArm.pop_back(); + rvecsFiducial.pop_back(); + tvecsFiducial.pop_back(); + baseToTip.pop_back(); + cameraToTag.pop_back(); + } else if ((key == "q") || (key == "Q")) { + break; + } else if (key.length() > 0) { + std::cerr << key << " received.\n"; } + key = ""; + ros::spinOnce(); + r.sleep(); } - return 0; } - key = ""; - ros::Subscriber sub = nh.subscribe("/handeye_calib/key", 1, keyCallback); - ros::Rate r(10); // 10 hz - listener = new (tf::TransformListener); - - ros::Duration(1.0).sleep(); // cache the TF transforms - - while (ros::ok()) { - if ((key == "s") || (key == "S")) { - std::cerr << "Adding Transform #:" << rvecsArm.size() << "\n"; - addFrame(); - writeTransformPairsToFile(baseToTip, cameraToTag, - transformPairsRecordFile); - } else if ((key == "d") || (key == "D")) { - ROS_INFO("Deleted last frame transformation. Number of Current " - "Transformations: %u", - (unsigned int)rvecsArm.size()); - rvecsArm.pop_back(); - tvecsArm.pop_back(); - rvecsFiducial.pop_back(); - tvecsFiducial.pop_back(); - baseToTip.pop_back(); - cameraToTag.pop_back(); - } else if ((key == "q") || (key == "Q")) { - if (rvecsArm.size() < 5) { - ROS_WARN("Number of calibration transform pairs < 5."); - ROS_INFO("Node Quit"); - } - ROS_INFO("Calculating Calibration..."); - camodocal::HandEyeCalibration calib; - Eigen::Matrix4d result; - ceres::Solver::Summary summary; - - if (addSolverSummary) { - calib.estimateHandEyeScrew(rvecsArm, tvecsArm, rvecsFiducial, - tvecsFiducial, result, summary, - false); - } else { - calib.estimateHandEyeScrew(rvecsArm, tvecsArm, rvecsFiducial, - tvecsFiducial, result, false); - } + if (rvecsArm.size() < 5) { + ROS_WARN("Number of calibration transform pairs < 5."); + } + ROS_INFO("Calculating Calibration..."); - std::cerr << "Quaternion values are output in wxyz order\n"; + std::cerr << "Transform pairs loading file: " << transformPairsRecordFile + << "\n"; + EigenAffineVector t1, t2; + int ret = readTransformPairsFromFile(transformPairsRecordFile, t1, t2); + if (ret == 0) { + { + auto result = estimateHandEye(t1, t2, calibratedTransformFile, + addSolverSummary); - std::cerr << "Calibration result (" << ARTagTFname << " pose in " - << EETFname << " frame): \n" - << result << std::endl; Eigen::Transform resultAffine(result); - std::cerr << "Translation (x,y,z) : " - << resultAffine.translation().transpose() << std::endl; Eigen::Quaternion quaternionResult(resultAffine.rotation()); - std::stringstream ss; - ss << quaternionResult.w() << " " << quaternionResult.x() << " " - << quaternionResult.y() << " " << quaternionResult.z() - << std::endl; - std::cerr << "Rotation (w,x,y,z): " << ss.str() << std::endl; - double x = resultAffine.translation().transpose()[0]; double y = resultAffine.translation().transpose()[1]; double z = resultAffine.translation().transpose()[2]; + std::ofstream outputfile(output_launch_filename); outputfile << "" << std::endl; outputfile << " " - << std::endl; + outputfile << " $(arg ee_frame) $(arg " + "marker_frame) 100\" />" + << std::endl; outputfile << "" << std::endl; outputfile.close(); - - tf::StampedTransform EETransform; - tf::StampedTransform Marker2CameraTransform; - Eigen::Affine3d eigenEE; - Eigen::Affine3d eigenMarker2Camera; - ros::Time now = ros::Time::now(); - if (listener->waitForTransform(baseTFname, EETFname, now, - ros::Duration(3.0)) && - listener->waitForTransform(ARTagTFname, cameraTFname, now, - ros::Duration(3.0))) { - listener->lookupTransform(baseTFname, EETFname, now, - EETransform); - listener->lookupTransform(ARTagTFname, cameraTFname, now, - Marker2CameraTransform); - tf::transformTFToEigen(EETransform, eigenEE); - tf::transformTFToEigen(Marker2CameraTransform, - eigenMarker2Camera); - Eigen::Matrix4d m = eigenEE.matrix(); - m *= resultAffine.matrix(); - m *= eigenMarker2Camera.matrix(); - Eigen::Affine3d result_affine; - result_affine.matrix() = m; - - Eigen::Quaternion quaternion_result( - result_affine.rotation()); - x = result_affine.translation().transpose()[0]; - y = result_affine.translation().transpose()[1]; - z = result_affine.translation().transpose()[2]; - std::ofstream base_to_camera_outputfile( - base_to_camera_output_launch_filename); - base_to_camera_outputfile << "" << std::endl; - base_to_camera_outputfile - << " " << std::endl; - base_to_camera_outputfile - << " " << std::endl; - base_to_camera_outputfile - << " " - << std::endl; - base_to_camera_outputfile << "" << std::endl; - base_to_camera_outputfile.close(); - } else { - std::cerr << "Could not get TF" << std::endl; - ROS_WARN( - "Fail to get TF, so base_to_camera transformation " - "data not generated. Please check the camera position."); + ROS_INFO("ee_frame to marker_frame transform saved %s", + output_launch_filename); + } + { + EigenAffineVector tag_to_cam; + tag_to_cam.resize(t2.size()); + for (int tag_to_cam_index = 0; tag_to_cam_index < tag_to_cam.size(); + ++tag_to_cam_index) { + tag_to_cam[tag_to_cam_index] = t2[tag_to_cam_index].inverse(); } - - if (addSolverSummary) { - writeCalibration(resultAffine, calibratedTransformFile, - summary); - } else { - writeCalibration(resultAffine, calibratedTransformFile); + EigenAffineVector ee_to_base; + ee_to_base.resize(t2.size()); + for (int ee_to_base_index = 0; ee_to_base_index < ee_to_base.size(); + ++ee_to_base_index) { + ee_to_base[ee_to_base_index] = t1[ee_to_base_index].inverse(); } + auto result = + estimateHandEye(ee_to_base, tag_to_cam, calibratedTransformFile, + addSolverSummary); + + Eigen::Transform resultAffine(result); + Eigen::Quaternion quaternionResult(resultAffine.rotation()); + double x = resultAffine.translation().transpose()[0]; + double y = resultAffine.translation().transpose()[1]; + double z = resultAffine.translation().transpose()[2]; - Eigen::Transform resultAffineInv = - resultAffine.inverse(); - std::cerr << "Inverted Calibration result (" << EETFname - << " pose in " << ARTagTFname << " frame): \n"; - std::cerr << "Translation (x,y,z): " - << resultAffineInv.translation().transpose() << std::endl; - quaternionResult = - Eigen::Quaternion(resultAffineInv.rotation()); - ss.clear(); - ss << quaternionResult.w() << " " << quaternionResult.x() << " " - << quaternionResult.y() << " " << quaternionResult.z() - << std::endl; - std::cerr << "Rotation (w,x,y,z): " << ss.str() << std::endl; - - break; - } else if (key.length() > 0) { - std::cerr << key << " received.\n"; + std::ofstream outputfile(base_to_camera_output_launch_filename); + outputfile << "" << std::endl; + outputfile << " " << std::endl; + outputfile << " " << std::endl; + outputfile << " " + << std::endl; + outputfile << "" << std::endl; + outputfile.close(); + ROS_INFO("base_frame to camera_frame transform saved %s", + base_to_camera_output_launch_filename); } - key = ""; - ros::spinOnce(); - r.sleep(); } ros::shutdown();