diff --git a/README.md b/README.md index 95aa9d06..28989dc0 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,7 @@ industrial_calibration ====================== -:warning: EXPERIMENTAL KINETIC-DEVEL branch :warning: +:warning: EXPERIMENTAL MELODIC-DEVEL branch :warning: === Contains libraries/algorithms for calibration industrial systems. @@ -10,6 +10,7 @@ extrinsic_cal calibration nodes: service_node -- peforms quiet a number of calbirations using very complex yaml files range_excal -- performs extrinsic calibration of a 3D camera by finding the target points in the image and the xyz values from the depth image. wrist_cal_srv -- performs extrinsic calibration when either the target or the camera is mounted on the end of arm tooling +gantry_cal_srv -- performs extrinsic calibration to refine the transform between the gantry_tool and the robot_base stereo_cal_srv -- performs stereo extrinsic cal using a robot (target or pair mounted on end of arm tooling) extrinsic_cal helper nodes: diff --git a/calibration_guis/src/calibration_gui.cpp b/calibration_guis/src/calibration_gui.cpp index e17540bd..be4b03df 100644 --- a/calibration_guis/src/calibration_gui.cpp +++ b/calibration_guis/src/calibration_gui.cpp @@ -30,7 +30,7 @@ namespace calibration_guis // this client does not change with the type of calibration ros::NodeHandle pnh("~"); - ecp_client_ = pnh.serviceClient("store_mutable_joint_states"); + ecp_client_ = pnh.serviceClient("/store_mutable_joint_states"); // these are the currently implemented types of service defined calibration routines // Note, WristCal, ICal and StereoCal expect a robot moves to change the scene @@ -40,6 +40,7 @@ namespace calibration_guis calibration_selection_->addItem("WristCalSrv"); // Either Camera or Target on EOAT, uses TF to get IC for Camera to Target Transform calibration_selection_->addItem("ICalSrv"); // Either Camera or Target on EOAT, uses TF to get IC for Camera to Target Transform calibration_selection_->addItem("StereoCalSrv");// Either Cameras or Target on EOAT, uses TF to get IC for Camera to Target Transform + calibration_selection_->addItem("GantryCalSrv"); // Calibrate the mount of a manipulator to a gantry's tool plate calibration_selection_->addItem("RailICalSrv"); // uses the 8D method where target to rail is changed after each sweep of rail, must happen at least once calibration_selection_->setCurrentIndex(0); reset_services(); // setup the services to use the default calibration type diff --git a/industrial_extrinsic_cal/CMakeLists.txt b/industrial_extrinsic_cal/CMakeLists.txt index 577110dd..e7fdb32e 100644 --- a/industrial_extrinsic_cal/CMakeLists.txt +++ b/industrial_extrinsic_cal/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 2.8.3) project(industrial_extrinsic_cal) -add_compile_options(-std=c++11) +add_compile_options(-std=c++14) find_package(catkin REQUIRED COMPONENTS actionlib @@ -168,6 +168,7 @@ add_executable(range_excal src/nodes/range_camera_excal.cpp add_executable(rangeNmono src/nodes/rangeNmono_excal.cpp) add_executable(target_display src/nodes/target_display_node.cpp) add_executable(wrist_cal_srv src/nodes/wrist_cal_srv.cpp) +add_executable(gantry_cal_srv src/nodes/gantry_cal_srv.cpp) add_executable(stereo_cal_srv src/nodes/stereo_cal_srv.cpp) #add_executable(check_visible src/check_if_points_in_pic.cpp) @@ -180,6 +181,7 @@ add_dependencies(range_excal ${catkin_EXPORTED_TARGETS} ${i add_dependencies(rangeNmono ${catkin_EXPORTED_TARGETS} ${industrial_extrinsic_cal_EXPORTED_TARGETS}) add_dependencies(target_display ${catkin_EXPORTED_TARGETS} ${industrial_extrinsic_cal_EXPORTED_TARGETS}) add_dependencies(wrist_cal_srv ${catkin_EXPORTED_TARGETS} ${industrial_extrinsic_cal_EXPORTED_TARGETS}) +add_dependencies(gantry_cal_srv ${catkin_EXPORTED_TARGETS} ${industrial_extrinsic_cal_EXPORTED_TARGETS}) add_dependencies(stereo_cal_srv ${catkin_EXPORTED_TARGETS} ${industrial_extrinsic_cal_EXPORTED_TARGETS}) #add_dependencies(check_visible ${catkin_EXPORTED_TARGETS} ${industrial_extrinsic_cal_EXPORTED_TARGETS}) @@ -193,6 +195,7 @@ target_link_libraries(trigger_service ${catkin_LIBRARIES}) target_link_libraries(range_excal industrial_extrinsic_cal ${CERES_LIBRARIES}) target_link_libraries(rangeNmono industrial_extrinsic_cal ${CERES_LIBRARIES}) target_link_libraries(wrist_cal_srv industrial_extrinsic_cal ${CERES_LIBRARIES}) +target_link_libraries(gantry_cal_srv industrial_extrinsic_cal ${CERES_LIBRARIES}) target_link_libraries(stereo_cal_srv industrial_extrinsic_cal ${CERES_LIBRARIES}) #target_link_libraries(check_visible industrial_extrinsic_cal ${CERES_LIBRARIES}) @@ -209,6 +212,7 @@ install( rangeNmono target_display wrist_cal_srv + gantry_cal_srv stereo_cal_srv ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} diff --git a/industrial_extrinsic_cal/cfg/circle_grid_finder.cfg b/industrial_extrinsic_cal/cfg/circle_grid_finder.cfg index 058f6545..dac04161 100755 --- a/industrial_extrinsic_cal/cfg/circle_grid_finder.cfg +++ b/industrial_extrinsic_cal/cfg/circle_grid_finder.cfg @@ -7,17 +7,17 @@ gen = ParameterGenerator() gen.add("filter_by_area", bool_t, 0, "filter blobs by their area", True) -gen.add("filter_by_color", bool_t, 0, "filter blobs by their color", True) -gen.add("filter_by_circularity", bool_t, 0, "filter blobs by their circularity", True) -gen.add("filter_by_convexity", bool_t, 0, "filter blobs by their convexity", True) -gen.add("filter_by_inertia", bool_t, 0, "filter blobs by their aspect ration", True) +gen.add("filter_by_color", bool_t, 0, "filter blobs by their color", False) +gen.add("filter_by_circularity", bool_t, 0, "filter blobs by their circularity", False) +gen.add("filter_by_convexity", bool_t, 0, "filter blobs by their convexity", False) +gen.add("filter_by_inertia", bool_t, 0, "filter blobs by their aspect ration", False) gen.add("min_repeatability", int_t, 0, "Minimum repeatability", 2, 1, 8) gen.add("min_area", int_t, 0, "Minimum area of a blob", 25, 1, 5000) -gen.add("max_area", int_t, 0, "Maximum area of a blob", 40000, 1, 80000) +gen.add("max_area", int_t, 0, "Maximum area of a blob", 600, 1, 80000) gen.add("min_circularity", double_t, 0, "The min aspect ratio necessary to be a circlular blob", 0.0, .1, 1.0) gen.add("max_circularity", double_t, 0, "The max aspect ratio necessary to be a circlular blob", 0.5, .75, 1.0) -gen.add("min_threshold", int_t, 0, "The contrast minimum threshold to be a blob", 50, 1, 1000) -gen.add("max_threshold", int_t, 0, "The contrast maximum threshold to be a blob", 120, 1, 1000) +gen.add("min_threshold", int_t, 0, "The contrast minimum threshold to be a blob", 10, 1, 1000) +gen.add("max_threshold", int_t, 0, "The contrast maximum threshold to be a blob", 65, 1, 1000) gen.add("min_distance", double_t, 0, "The min distance between blobs", 5.0, 1.0, 500) gen.add("min_radius_diff", double_t, 0, "The min radius difference between blobs", 10.0, 1.0, 500) gen.add("min_convexity", double_t, 0, "The mininum convexity", 0.0, 0.1, 1.0) diff --git a/industrial_extrinsic_cal/include/industrial_extrinsic_cal/camera_observer.hpp b/industrial_extrinsic_cal/include/industrial_extrinsic_cal/camera_observer.hpp index c10b0b39..29c2101c 100644 --- a/industrial_extrinsic_cal/include/industrial_extrinsic_cal/camera_observer.hpp +++ b/industrial_extrinsic_cal/include/industrial_extrinsic_cal/camera_observer.hpp @@ -136,7 +136,7 @@ class CameraObserver bool save_current_image(int scene, std::string& filename) { std::string full_file_path_name; - char scene_chars[8]; + char scene_chars[30]; sprintf(scene_chars,"_%03d.jpg",scene); if(filename == ""){ // build file name from image_directory_, full_file_path_name = image_directory_ + std::string("/") + camera_name_ + std::string(scene_chars); @@ -163,7 +163,7 @@ class CameraObserver bool load_current_image(int scene, std::string& filename) { std::string full_image_file_path_name; - char scene_chars[8]; + char scene_chars[30]; sprintf(scene_chars,"_%03d.jpg",scene); if(filename == ""){ // build file name from image_directory_, full_image_file_path_name = image_directory_ + std::string("/") + camera_name_ + std::string(scene_chars); diff --git a/industrial_extrinsic_cal/include/industrial_extrinsic_cal/ceres_costs_utils.h b/industrial_extrinsic_cal/include/industrial_extrinsic_cal/ceres_costs_utils.h index a54cee05..8c216f55 100644 --- a/industrial_extrinsic_cal/include/industrial_extrinsic_cal/ceres_costs_utils.h +++ b/industrial_extrinsic_cal/include/industrial_extrinsic_cal/ceres_costs_utils.h @@ -35,6 +35,7 @@ namespace industrial_extrinsic_cal LinkTargetCameraReprjError, LinkTargetCameraReprjErrorPK, wristCal, + GantryCal, PosedTargetCameraReprjErrorPK, LinkCameraTargetReprjError, LinkCameraTargetReprjErrorPK, diff --git a/industrial_extrinsic_cal/include/industrial_extrinsic_cal/ceres_costs_utils.hpp b/industrial_extrinsic_cal/include/industrial_extrinsic_cal/ceres_costs_utils.hpp index 6b214210..c48e4c22 100644 --- a/industrial_extrinsic_cal/include/industrial_extrinsic_cal/ceres_costs_utils.hpp +++ b/industrial_extrinsic_cal/include/industrial_extrinsic_cal/ceres_costs_utils.hpp @@ -1323,7 +1323,6 @@ class LinkTargetCameraReprjErrorPK }; // used whenever the target or camera is on the wrist of a robot while the camera or target is in the workcell -// The class WristCal { public: @@ -1367,8 +1366,7 @@ class WristCal const double cx, const double cy, Pose6d pose, Point3d point) { - return (new ceres::AutoDiffCostFunction( - new LinkTargetCameraReprjErrorPK(o_x, o_y, fx, fy, cx, cy, pose, point))); + return (new ceres::AutoDiffCostFunction(new WristCal(o_x, o_y, fx, fy, cx, cy, pose, point))); } double ox_; /** observed x location of object in image */ double oy_; /** observed y location of object in image */ @@ -1385,6 +1383,73 @@ class WristCal */ }; +// used to calibrate a robot slung from a gantry, must first have performed wrist calibration (eye-hand calibration) +class GantryCal +{ +public: + GantryCal(double ob_x, double ob_y, double fx, double fy, double cx, double cy, + Pose6d target_to_robotm, + Pose6d robot_base_to_optical, + Point3d point) + : ox_(ob_x), oy_(ob_y), fx_(fx), fy_(fy), cx_(cx), cy_(cy), + target_to_robotm_(target_to_robotm), + robot_base_to_optical_(robot_base_to_optical), + point_(point) + { + } + + template + bool operator()(const T* const p1, /** mount transform parameters */ + T* residual) const + { + const T* mount_T(&p1[0]); + T robot_mount_point[3]; /** point in robot mount frame */ + T robot_base_point[3]; /** point in robot base frame */ + T camera_point[3]; /** point in camera frame */ + T point[3]; + point[0] = T(point_.x); + point[1] = T(point_.y); + point[2] = T(point_.z); + + /** transform point into camera coordinates */ + poseTransformPoint(target_to_robotm_, point, robot_mount_point); + eTransformPoint(mount_T, robot_mount_point, robot_base_point); + poseTransformPoint(robot_base_to_optical_, robot_base_point, camera_point); + + /** compute project point into image plane and compute residual */ + T fx = T(fx_); + T fy = T(fy_); + T cx = T(cx_); + T cy = T(cy_); + T ox = T(ox_); + T oy = T(oy_); + cameraPntResidual(camera_point, fx, fy, cx, cy, ox, oy, residual); + + return true; + } /** end of operator() */ + + /** Factory to hide the construction of the CostFunction object from */ + /** the client code. */ + static ceres::CostFunction* Create(const double o_x, const double o_y, + const double fx, const double fy, + const double cx, const double cy, + const Pose6d pose1, const Pose6d pose2, + const Point3d point) + + { + return (new ceres::AutoDiffCostFunction(new GantryCal(o_x, o_y, fx, fy, cx, cy, pose1, pose2, point))); + } + double ox_; /** observed x location of object in image */ + double oy_; /** observed y location of object in image */ + double fx_; /*!< known focal length of camera in x */ + double fy_; /*!< known focal length of camera in y */ + double cx_; /*!< known optical center of camera in x */ + double cy_; /*!< known optical center of camera in y */ + Point3d point_; /*! location of point in target coordinates */ + Pose6d target_to_robotm_; /*!< transform from target to robot's mounting frame */ + Pose6d robot_base_to_optical_; /*!< transform from robot_base to optical frame */ +}; + // reprojection error of a single point attatched to a target observed by a camera with NO lens distortion // should subscribe to a rectified image when using the error function // diff --git a/industrial_extrinsic_cal/launch/calibrationTarget.py b/industrial_extrinsic_cal/launch/calibrationTarget.py old mode 100644 new mode 100755 diff --git a/industrial_extrinsic_cal/src/basic_types.cpp b/industrial_extrinsic_cal/src/basic_types.cpp index 0315ecfb..8fecf45a 100644 --- a/industrial_extrinsic_cal/src/basic_types.cpp +++ b/industrial_extrinsic_cal/src/basic_types.cpp @@ -217,7 +217,7 @@ void Pose6d::show(std::string message) this->getEulerZYX(ez_yaw, ey_pitch, ex_roll); this->getQuaternion(qx, qy, qz, qw); printf("%s =[\n %6.4lf %6.4lf %6.4lf %6.4lf\n %6.4lf %6.4lf %6.4lf %6.4lf\n %6.4lf %6.4lf %6.4lf %6.4lf\n " - "%6.4lf %6.4lf %6.4lf %6.4lf];\n rpy= %6.4lf %6.4lf %6.4lf\n quat= %6.4lf %6.4lf %6.4lf %6.4lf\n ", + "%6.4lf %6.4lf %6.4lf %6.4lf];\n rpy= %6.4lf %6.4lf %6.4lf\n xyzw= %6.4lf %6.4lf %6.4lf %6.4lf\n ", message.c_str(), basis[0][0], basis[0][1], basis[0][2], this->x, basis[1][0], basis[1][1], basis[1][2], this->y, basis[2][0], basis[2][1], basis[2][2], this->z, 0.0, 0.0, 0.0, 1.0, ez_yaw, ey_pitch, ex_roll, qx, qy, qz, qw); diff --git a/industrial_extrinsic_cal/src/nodes/gantry_cal_srv.cpp b/industrial_extrinsic_cal/src/nodes/gantry_cal_srv.cpp new file mode 100644 index 00000000..ca17af70 --- /dev/null +++ b/industrial_extrinsic_cal/src/nodes/gantry_cal_srv.cpp @@ -0,0 +1,535 @@ +#include + +#include +#include +#include + +// ros includes +#include +#include +#include +#include +#include + +// ros actionlib includes +#include +#include + +// industrial_extrinsic_cal includes +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +//defined services +#include +#include + +// boost includes +#include +#include +#include +#include + +#include + +using boost::shared_ptr; +using boost::filesystem::path; +using boost::dynamic_pointer_cast; + +using ceres::Solver; +using ceres::CostFunction; + +using industrial_extrinsic_cal::Camera; +using industrial_extrinsic_cal::Target; +using industrial_extrinsic_cal::CeresBlocks; +using industrial_extrinsic_cal::Roi; +using industrial_extrinsic_cal::CameraObservations; +using industrial_extrinsic_cal::Point3d; +using industrial_extrinsic_cal::Pose6d; +using industrial_extrinsic_cal::P_BLOCK; + +class gantryCalServiceNode +{ +public: + gantryCalServiceNode(const ros::NodeHandle& nh) + :nh_(nh), P_(NULL), problem_initialized_(false), total_observations_(0), scene_(0) + { + std::string nn = ros::this_node::getName(); + ros::NodeHandle priv_nh("~"); + + // load global prameters + if(!priv_nh.getParam("camera_file_name", camera_file_)){ + ROS_ERROR("Must set param: camera_file, this defines the camera to calibrate"); + } + if(!priv_nh.getParam("target_file_name", target_file_)){ + ROS_ERROR("Must set param: target_file, this defines the target used for calibration"); + } + // load frame parameters + if(!priv_nh.getParam("robot_mount_frame", robot_mount_frame_)){ + ROS_ERROR("Must set param: robot_mount_frame, this defines the tf-frame on which the robot base is mounted"); + } + if(!priv_nh.getParam("robot_base_frame", robot_base_frame_)){ + ROS_ERROR("Must set param: robot_base_frame, this defines the tf-frame the robot base"); + } + + + priv_nh.getParam("save_data", save_data_); + priv_nh.getParam("data_directory", data_directory_); + + ROS_INFO("camera_file: %s", camera_file_.c_str()); + ROS_INFO("target_file: %s", target_file_.c_str()); + if(save_data_){ + ROS_INFO("saving data to %s", data_directory_.c_str()); + } + else{ + ROS_WARN("Not saving data for gantry calibration"); + } + + // Load cameras from yaml file. There should only be one static camera + if(!load_camera()){ + ROS_ERROR("can't load the camera from %s", camera_file_.c_str()); + exit(1); + } + else{ // get camera's optical frame for a transform listener + camera_optical_frame_ = all_cameras_[0]->transform_interface_->getTransformFrame(); + fx_ = all_cameras_[0]->camera_parameters_.focal_length_x; + fy_ = all_cameras_[0]->camera_parameters_.focal_length_y; + cx_ = all_cameras_[0]->camera_parameters_.center_x; + cy_ = all_cameras_[0]->camera_parameters_.center_y; + roi_.x_min = 0; + roi_.y_min = 0; + roi_.x_max = all_cameras_[0]->camera_parameters_.width; + roi_.y_max = all_cameras_[0]->camera_parameters_.height; + + ROS_INFO("loaded camera from %s with optical_frame = %s intrinsics:[ %f %f %f %f ]", camera_file_.c_str(), + camera_optical_frame_.c_str(), + fx_, fy_, cx_, cy_); + } + + // Load targets from yaml file. There should only be one static target + if(!load_target()){ + ROS_ERROR("can't load the target from %s", target_file_.c_str()); + exit(1); + } + + + // initialize the target to robot_mount transform listener + target_to_robotm_TI_ = new industrial_extrinsic_cal::ROSListenerTransInterface(all_targets_[0]->target_frame_); + target_to_robotm_TI_->setReferenceFrame(robot_mount_frame_); + target_to_robotm_TI_->setDataDirectory(data_directory_); + + // initialize the target to robot_mount transform listener + robot_base_to_optical_TI_ = new industrial_extrinsic_cal::ROSListenerTransInterface(robot_base_frame_); + robot_base_to_optical_TI_->setReferenceFrame(camera_optical_frame_); + robot_base_to_optical_TI_->setDataDirectory(data_directory_); + + // advertise services + start_server_ = nh_.advertiseService( "GantryCalSrvStart", &gantryCalServiceNode::startCallBack, this); + load_server_ = nh_.advertiseService( "GantryCalSrvLoad", &gantryCalServiceNode::loadCallBack, this); + observation_server_ = nh_.advertiseService( "GantryCalSrvObs", &gantryCalServiceNode::observationCallBack, this); + run_server_ = nh_.advertiseService( "GantryCalSrvRun", &gantryCalServiceNode::runCallBack, this); + save_server_ = nh_.advertiseService( "GantryCalSrvSave", &gantryCalServiceNode::saveCallBack, this); + covariance_server_ = nh_.advertiseService( "GantryCalSrvCov", &gantryCalServiceNode::covCallBack, this); + + };// end of constructor + + + // read the camera yaml file + bool load_camera() + { + bool rtn = true; + if (!parseCameras(camera_file_, all_cameras_)) + { + ROS_ERROR("failed to parse cameras from %s", camera_file_.c_str()); + rtn = false; + } + for(int i=0;i<(int)all_cameras_.size();i++){ + all_cameras_[i]->transform_interface_->setDataDirectory(data_directory_); + all_cameras_[i]->transform_interface_->setReferenceFrame(robot_mount_frame_); + } + return rtn; + };// end of load_camera() + + // read the target yaml file + bool load_target() + { + bool rtn = true; + + if (!parseTargets(target_file_, all_targets_)) + { + ROS_ERROR("failed to parse targets from %s", target_file_.c_str()); + rtn = false; + } + for(int i=0;i<(int)all_targets_.size();i++){ + all_targets_[i]->transform_interface_->setDataDirectory(data_directory_); + } + + return rtn; + }; // end load_target() + + // callback functions + bool startCallBack( std_srvs::TriggerRequest &req, std_srvs::TriggerResponse &res) + { + if(problem_initialized_) delete(P_); + P_ = new ceres::Problem; + problem_initialized_ = true; + total_observations_ = 0; + scene_=0; + res.message = std::string("Gantry calibration service started"); + res.success = true; + return(true); + } + + // called to collect observations for the current pose of the scene + bool observationCallBack( std_srvs::TriggerRequest &req, std_srvs::TriggerResponse &res) + { + char msg[100]; + Pose6d TtoRm = target_to_robotm_TI_->pullTransform(); + Pose6d RbtoO = robot_base_to_optical_TI_->pullTransform(); + TtoRm.show("Target_2 Robot_Mount"); + RbtoO.show("Robot_base_2_Optical"); + + if(problem_initialized_ != true ){ + std_srvs::TriggerRequest sreq; + std_srvs::TriggerResponse sres; + ROS_INFO("Problem not yet initialized, calling start service"); + startCallBack(sreq, sres); + } + + // get observations + all_cameras_[0]->camera_observer_->clearTargets(); + all_cameras_[0]->camera_observer_->clearObservations(); + industrial_extrinsic_cal::Cost_function cost_type = industrial_extrinsic_cal::cost_functions::GantryCal; + int total_pts=0; + // add target to camera observer + all_cameras_[0]->camera_observer_->addTarget(all_targets_[0], roi_, cost_type); + total_pts += all_targets_[0]->num_points_; + all_cameras_[0]->camera_observer_->triggerCamera(); + while (!all_cameras_[0]->camera_observer_->observationsDone()); + CameraObservations camera_observations; + all_cameras_[0]->camera_observer_->getObservations(camera_observations); + int num_observations = (int)camera_observations.size(); + ROS_INFO("Found %d observations", (int)camera_observations.size()); + + // add observations to problem + num_observations = (int)camera_observations.size(); + if (num_observations != total_pts) + { + ROS_ERROR("Target Locator could not find all targets found %d out of %d", num_observations, total_pts); + } + else if(!all_cameras_[0]->camera_observer_->checkObservationProclivity(camera_observations)) + { + ROS_ERROR("Proclivities Check not successful"); + } + else + { // add a new cost to the problem for each observation + total_observations_ += num_observations; + for (int k = 0; k < num_observations; k++) + { + shared_ptr target = camera_observations[k].target; + double image_x = camera_observations[k].image_loc_x; + double image_y = camera_observations[k].image_loc_y; + Point3d point = target->pts_[camera_observations[k].point_id]; + CostFunction* cost_function = industrial_extrinsic_cal::GantryCal::Create(image_x, image_y, fx_, fy_, cx_, cy_, TtoRm, RbtoO, point); + P_->AddResidualBlock(cost_function, NULL, gantrycal_pose_.pb_pose); + } // for each observation at this camera_location + } // end of else (there are some observations to add) + + + if (save_data_){ + char pose_scene_chars[20]; + char image_scene_chars[20]; + sprintf(pose_scene_chars,"_%03d.yaml",scene_); + sprintf(image_scene_chars,"_%03d.jpg",scene_); + std::string image_file = all_cameras_[0]->camera_name_ + std::string(image_scene_chars); + std::string target_to_robot_mount = std::string("t_to_rm") + std::string(pose_scene_chars); // save2 to data_directory_/t_to_rm_sceneID.yaml + std::string robot_base_to_optical = std::string("rb_to_o") + std::string(pose_scene_chars); // save2 to data_directory_/t_to_rm_sceneID.yaml + all_cameras_[0]->camera_observer_->save_current_image(scene_,image_file); + target_to_robotm_TI_->saveCurrentPose(scene_,target_to_robot_mount); + robot_base_to_optical_TI_->saveCurrentPose(scene_,robot_base_to_optical); + }// end save_data_ + else{ + ROS_ERROR("save_data_ not set"); + } + ROS_INFO("now have %d observations after scene %d",total_observations_, scene_); + scene_++; + + sprintf(msg, "Ical_srv now has %d observations after scene %d",total_observations_, scene_); + res.message = std::string(msg); + res.success = true; + return(true); + + }; // end observation service}; + + bool covCallBack( std_srvs::TriggerRequest &req, std_srvs::TriggerResponse &res) + { + std::string covariance_file_name = data_directory_ + "/gantry_cov.txt"; + if(!computeCovariance(covariance_file_name)){ + ROS_ERROR("could not compute covariance"); + res.success = false; + res.message = "failure"; + } + else{ + res.message = "good job"; + res.success = true; + } + return(true); + }; + + bool computeCovariance(std::string& covariance_file_name) + { + FILE* fp; + if ((fp = fopen(covariance_file_name.c_str(), "w")) != NULL) + { + ceres::Covariance::Options covariance_options; + covariance_options.algorithm_type = ceres::DENSE_SVD; + ceres::Covariance covariance(covariance_options); + + std::vector > covariance_pairs; + covariance_pairs.push_back(std::make_pair(gantrycal_pose_.pb_pose, gantrycal_pose_.pb_pose)); + + if(covariance.Compute(covariance_pairs, P_)) + { + fprintf(fp, "covariance blocks:\n"); + double cov[6*6]; + + if(covariance.GetCovarianceBlock(gantrycal_pose_.pb_pose, gantrycal_pose_.pb_pose, cov)) + { + fprintf(fp, "cov is 6x6\n"); + for(int i=0;i<6;i++) + { + double sigma_i = sqrt(fabs(cov[i*6+i])); + for(int j=0;j<6;j++) + { + double sigma_j = sqrt(fabs(cov[j*6+j])); + double value; + if(i==j) + { + value = sigma_i; + } // end if i==j + else + { + if(sigma_i==0) sigma_i = 1; + if(sigma_j==0) sigma_j = 1; + value = cov[i * 6 + j]/(sigma_i*sigma_j); + } // end i!=j + fprintf(fp, "%16.5f ", value); + } // end of j loop + fprintf(fp, "\n"); + } // end of i loop + }// end if covariance could be gotten + else{ + ROS_ERROR("could not get covariance"); + fclose(fp); + return false; + } + }// end if covariance could be computed + else + { + ROS_ERROR("covariance file could not be opened"); + return false; + } + }// end if cov file could open + fclose(fp); + return true; + } + bool runCallBack( industrial_extrinsic_cal::cal_srv_solveRequest &req, industrial_extrinsic_cal::cal_srv_solveResponse &res) + { + char msg[100]; + // check for obvious errors + if(problem_initialized_==false){ + ROS_ERROR("must call start service"); + sprintf(msg, "must call start service to initialized ical service"); + res.message = std::string(msg); + res.success = false; + return(true); + } + if(total_observations_ == 0){ + ROS_ERROR("must call observations service at least once"); + sprintf(msg, "must call observations service at least once"); + res.message = std::string(msg); + res.success = false; + return(true); + } + + // output initial conditions + gantrycal_pose_.show("initial gantrycal extrinsics"); + + Solver::Options options; + Solver::Summary summary; + options.linear_solver_type = ceres::DENSE_SCHUR; + options.minimizer_progress_to_stdout = true; + options.max_num_iterations = 1000; + ceres::Solve(options, P_, &summary); + if (summary.termination_type != ceres::NO_CONVERGENCE) + { + double initial_cost = sqrt(2*summary.initial_cost / total_observations_); + double final_cost = sqrt(2*summary.final_cost / total_observations_); + res.final_cost_per_observation = final_cost; + + ROS_INFO("Problem solved, initial cost = %lf, final cost = %lf", initial_cost, final_cost); + if (final_cost <= req.allowable_cost_per_observation) + { + ROS_INFO("Gantry calibration was successful"); + } + else + { + ROS_ERROR("allowable cost exceeded %f > %f", final_cost, req.allowable_cost_per_observation); + sprintf(msg, "allowable cost exceeded %f > %f", final_cost, req.allowable_cost_per_observation); + gantrycal_pose_.show("final gantrycal pose"); + res.message = std::string(msg); + res.success = false; + return(true); + } + sprintf(msg, "final cost %f", final_cost); + res.message = std::string(msg); + res.success = true; + return(true); + } + ROS_ERROR("Gantry Cal NO CONVERGENCE"); + sprintf(msg, "Gantry Cal NO CONVERGENCE"); + res.message = std::string(msg); + res.success = false; + return(true); + }; // end runCallBack() + + bool saveCallBack( std_srvs::TriggerRequest &req, std_srvs::TriggerResponse &res) + { + ROS_ERROR("it is expected that the pose computed by gantrycal is part of a urdf, please copy this info there"); + gantrycal_pose_.show("final gantrycal pose"); + res.success = true; + return(true); + }// end saveCallback() + + /** + * \brief Load and set the calibration data from a previous job. + */ + bool loadCallBack(industrial_extrinsic_cal::FileOp::Request &req, industrial_extrinsic_cal::FileOp::Response &resp) + { + industrial_extrinsic_cal::Cost_function cost_type = industrial_extrinsic_cal::cost_functions::CameraReprjErrorWithDistortionPK; + int total_pts= all_targets_[0]->num_points_; + + if(problem_initialized_ != true ){ // scene_id=0, problem re-initialized + ROS_INFO("calling start"); + std_srvs::TriggerRequest sreq; + std_srvs::TriggerResponse sres; + startCallBack(sreq,sres); + } + + bool data_read_ok=true; + while(data_read_ok){ + char pose_scene_chars[20]; + char image_scene_chars[20]; + sprintf(pose_scene_chars,"_%03d.yaml",scene_); + sprintf(image_scene_chars,"_%03d.jpg",scene_); + std::string image_file = all_cameras_[0]->camera_name_ + std::string(image_scene_chars); + std::string target_to_robot_mount = std::string("t_to_rm") + std::string(pose_scene_chars); + std::string robot_base_to_optical = std::string("rb_to_o") + std::string(pose_scene_chars); + data_read_ok &= all_cameras_[0]->camera_observer_->load_current_image(scene_,image_file); + data_read_ok &= target_to_robotm_TI_->loadPose(scene_,target_to_robot_mount); + data_read_ok &= robot_base_to_optical_TI_->loadPose(scene_,robot_base_to_optical); + + if(data_read_ok){ + Pose6d TtoRm = target_to_robotm_TI_->getCurrentPose(); + Pose6d RbtoO = robot_base_to_optical_TI_->getCurrentPose(); + TtoRm.show("TtoRm"); + RbtoO.show("RbtoO"); + + // set target and get observations from the image + CameraObservations camera_observations; + all_cameras_[0]->camera_observer_->clearTargets(); + all_cameras_[0]->camera_observer_->clearObservations(); + all_cameras_[0]->camera_observer_->addTarget(all_targets_[0], roi_, cost_type); + while (!all_cameras_[0]->camera_observer_->observationsDone()); + all_cameras_[0]->camera_observer_->getObservations(camera_observations); + + int num_observations = (int)camera_observations.size(); + ROS_INFO("Found %d observations", (int)camera_observations.size()); + + // add observations to problem + num_observations = (int)camera_observations.size(); + if (num_observations != total_pts) + { + ROS_ERROR("Target Locator could not find all targets found %d out of %d", num_observations, total_pts); + } + else // add a new cost to the problem for each observation + { + total_observations_ += num_observations; + for (int k = 0; k < num_observations; k++) + { + shared_ptr target = camera_observations[k].target; + double image_x = camera_observations[k].image_loc_x; + double image_y = camera_observations[k].image_loc_y; + Point3d point = target->pts_[camera_observations[k].point_id]; + CostFunction* cost_function = industrial_extrinsic_cal::GantryCal::Create(image_x, image_y, fx_, fy_, cx_, cy_, TtoRm, RbtoO, point); + P_->AddResidualBlock(cost_function, NULL, gantrycal_pose_.pb_pose); + } // for each observation at this camera_location + } // end of else (there are some observations to add) + scene_++; + }// end if data_read_ok + } + return true; + } + +private: + ros::NodeHandle nh_; + std::vector > all_cameras_; + std::vector > all_targets_; + std::string yaml_file_path_; + std::string camera_file_; + std::string target_file_; + std::string robot_mount_frame_; // frame where the robot is mounted + std::string robot_base_frame_; // robot base frame + std::string camera_optical_frame_; + + + ros::ServiceServer start_server_; + ros::ServiceServer observation_server_; + ros::ServiceServer run_server_; + ros::ServiceServer save_server_; + ros::ServiceServer load_server_; + ros::ServiceServer covariance_server_; + Pose6d robotm_to_robotb_pose; + Pose6d gantrycal_pose_; + ceres::Problem *P_; + bool problem_initialized_; + + bool save_data_; + std::string data_directory_; + + int total_observations_; + int scene_; + industrial_extrinsic_cal::ROSListenerTransInterface *target_to_robotm_TI_; + industrial_extrinsic_cal::ROSListenerTransInterface *robot_base_to_optical_TI_; + double fx_; + double fy_; + double cx_; + double cy_; + Roi roi_; + +};// end of class gantryCalServiceNode + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "gantry_cal_service"); + ros::NodeHandle node_handle; + gantryCalServiceNode WCSN(node_handle); + ros::spin(); + ros::waitForShutdown(); + return 0; +} diff --git a/industrial_extrinsic_cal/src/nodes/wrist_cal_srv.cpp b/industrial_extrinsic_cal/src/nodes/wrist_cal_srv.cpp index 86420d72..d523f235 100644 --- a/industrial_extrinsic_cal/src/nodes/wrist_cal_srv.cpp +++ b/industrial_extrinsic_cal/src/nodes/wrist_cal_srv.cpp @@ -548,7 +548,6 @@ class wristCalServiceNode bool saveCallBack( std_srvs::TriggerRequest &req, std_srvs::TriggerResponse &res) { - ROS_ERROR("inside saveCallBack()"); for(int i=0; ipushTransform(); } diff --git a/industrial_extrinsic_cal/src/transform_interface.cpp b/industrial_extrinsic_cal/src/transform_interface.cpp index 81c93c3e..a7335102 100644 --- a/industrial_extrinsic_cal/src/transform_interface.cpp +++ b/industrial_extrinsic_cal/src/transform_interface.cpp @@ -26,7 +26,7 @@ namespace industrial_extrinsic_cal bool TransformInterface::saveCurrentPose(int scene, std::string& filename) { std::string full_file_path_name; - char scene_chars[9]; + char scene_chars[30]; sprintf(scene_chars,"_%03d.yaml",scene); if(filename == ""){ // build file name from data_directory_, full_file_path_name = data_directory_ + "/" + transform_frame_ + std::string(scene_chars); @@ -41,7 +41,7 @@ namespace industrial_extrinsic_cal bool TransformInterface::loadPose(int scene, std::string& filename) { std::string full_file_path_name; - char scene_chars[8]; + char scene_chars[30]; sprintf(scene_chars,"_%03d.yaml",scene); if(filename == ""){ // build file name from data_directory_, full_file_path_name = data_directory_ + "/" + transform_frame_ + std::string(scene_chars); diff --git a/intrinsic_cal/src/ical_srv.cpp b/intrinsic_cal/src/ical_srv.cpp index 15a11b0a..90ecdd7b 100644 --- a/intrinsic_cal/src/ical_srv.cpp +++ b/intrinsic_cal/src/ical_srv.cpp @@ -533,7 +533,6 @@ class icalServiceNode bool saveCallBack( std_srvs::TriggerRequest &req, std_srvs::TriggerResponse &res) { char msg[100]; - ROS_ERROR("inside saveCallBack()"); for(int i=0; icamera_observer_->pushCameraInfo( all_cameras_[i]->camera_parameters_.focal_length_x, all_cameras_[i]->camera_parameters_.focal_length_y, diff --git a/intrinsic_cal/src/robocyl_ical_8D.cpp b/intrinsic_cal/src/robocyl_ical_8D.cpp index ab720c97..5764fac3 100644 --- a/intrinsic_cal/src/robocyl_ical_8D.cpp +++ b/intrinsic_cal/src/robocyl_ical_8D.cpp @@ -679,7 +679,7 @@ class robocyl_ical_8D //construct image file from scene name std::string current_image_file (int scene, std::string& filename){ std::string present_image_file_path_name; - char scene_chars[8]; + char scene_chars[30]; sprintf(scene_chars,"_%03d.jpg",scene); if(filename == ""){ // build file name from image_directory_, present_image_file_path_name = image_directory_ + std::string("/") + camera_name_ + std::string(scene_chars); diff --git a/target_finder/config/target_finder_astra.rviz b/target_finder/config/target_finder_astra.rviz new file mode 100644 index 00000000..48235a02 --- /dev/null +++ b/target_finder/config/target_finder_astra.rviz @@ -0,0 +1,150 @@ +Panels: + - Class: rviz/Displays + Help Height: 138 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /TF1 + - /TF1/Frames1 + Splitter Ratio: 0.5 + Tree Height: 1017 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + camera_depth_frame: + Value: false + camera_depth_optical_frame: + Value: false + camera_link: + Value: false + camera_rgb_frame: + Value: false + camera_rgb_optical_frame: + Value: true + world: + Value: false + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + world: + camera_rgb_optical_frame: + {} + Update Interval: 0 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 2.7850098609924316 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.785398006439209 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.785398006439209 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1523 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000406000004f1fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006e000004f10000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015f000004f1fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000006e000004f10000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009ef0000005afc0100000002fb0000000800540069006d00650100000000000009ef0000057100fffffffb0000000800540069006d0065010000000000000450000000000000000000000472000004f100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2543 + X: 134 + Y: 55 diff --git a/target_finder/launch/target_locator_astra.launch b/target_finder/launch/target_locator_astra.launch new file mode 100644 index 00000000..0b95e510 --- /dev/null +++ b/target_finder/launch/target_locator_astra.launch @@ -0,0 +1,38 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +