diff --git a/CMakeLists.txt b/CMakeLists.txt
index 0af1407..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)
@@ -208,6 +218,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 +231,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/package.xml b/package.xml
index ad10e18..1c271a4 100644
--- a/package.xml
+++ b/package.xml
@@ -40,15 +40,20 @@
catkin
+ std_msgs
eigen
roscpp
tf
trajectory_msgs
+ libceres-dev
+ message_generation
+ std_msgs
eigen
roscpp
tf
trajectory_msgs
+ message_runtime
diff --git a/src/handeye_calibration.cpp b/src/handeye_calibration.cpp
index 0afbadc..382fe80 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
@@ -104,7 +105,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;
@@ -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,
@@ -450,10 +454,38 @@ 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);
+ 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;
}
diff --git a/src/handeye_calibration_auto.cpp b/src/handeye_calibration_auto.cpp
new file mode 100644
index 0000000..fc19898
--- /dev/null
+++ b/src/handeye_calibration_auto.cpp
@@ -0,0 +1,616 @@
+#include "ceres/ceres.h"
+#include "ceres/types.h"
+#include "handeye_calib_camodocal/HandEyeCalibCommand.h"
+#include
+#include
+#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;
+std::string transformPairsRecordFile;
+
+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(3.0)))
+ 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();
+}
+
+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 transformPairsLoadFile;
+ std::string calibratedTransformFile;
+ bool loadTransformsFromFile = false;
+ bool addSolverSummary = false;
+ 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"));
+ 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(
+ "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,
+ std::string("TransformPairsInput.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";
+
+ 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();
+ }
+ }
+
+ if (rvecsArm.size() < 5) {
+ ROS_WARN("Number of calibration transform pairs < 5.");
+ }
+ ROS_INFO("Calculating Calibration...");
+
+ 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);
+
+ 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();
+ 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();
+ }
+ 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();
+ ROS_INFO("base_frame to camera_frame transform saved %s",
+ base_to_camera_output_launch_filename);
+ }
+ }
+
+ ros::shutdown();
+ return (0);
+}
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