bool pcl::EnsensoGrabber::jsonTransformationToEulerAngles (const std::string &json, double &x, double &y, double &z, double &w, double &p, double &r) const { try { NxLibCommand convert (cmdConvertTransformation); convert.parameters ()[itmTransformation].setJson (json, false); convert.parameters ()[itmSplitRotation].set (valXYZ); convert.execute (); NxLibItem tf = convert.result ()[itmTransformations]; x = tf[0][itmTranslation][0].asDouble (); y = tf[0][itmTranslation][1].asDouble (); z = tf[0][itmTranslation][2].asDouble (); r = tf[0][itmRotation][itmAngle].asDouble (); // Roll p = tf[1][itmRotation][itmAngle].asDouble (); // Pitch w = tf[2][itmRotation][itmAngle].asDouble (); // yaW return (true); } catch (NxLibException &ex) { ensensoExceptionHandling (ex, "jsonTransformationToEulerAngles"); return (false); } }
void ensensoExceptionHandling (const NxLibException &ex, std::string func_nam) { PCL_ERROR ("%s: NxLib error %s (%d) occurred while accessing item %s.\n", func_nam.c_str (), ex.getErrorText ().c_str (), ex.getErrorCode (), ex.getItemPath ().c_str ()); if (ex.getErrorCode () == NxLibExecutionFailed) { NxLibCommand cmd (""); PCL_WARN ("\n%s\n", cmd.result ().asJson (true, 4, false).c_str ()); } }
std::string pcl::EnsensoGrabber::getResultAsJson (const bool pretty_format) const { try { NxLibCommand cmd (""); return (cmd.result ().asJson (pretty_format)); } catch (NxLibException &ex) { ensensoExceptionHandling (ex, "getResultAsJson"); return (""); } }
bool pcl::EnsensoGrabber::eulerAnglesTransformationToJson (const double x, const double y, const double z, const double w, const double p, const double r, std::string &json, const bool pretty_format) const { try { NxLibCommand chain (cmdChainTransformations); NxLibItem tf = chain.parameters ()[itmTransformations]; if (!angleAxisTransformationToJson (x, y, z, 0, 0, 1, r, json)) return (false); tf[0].setJson (json, false); // Roll if (!angleAxisTransformationToJson (0, 0, 0, 0, 1, 0, p, json)) return (false); tf[1].setJson (json, false); // Pitch if (!angleAxisTransformationToJson (0, 0, 0, 1, 0, 0, w, json)) return (false); tf[2].setJson (json, false); // yaW chain.execute (); json = chain.result ()[itmTransformation].asJson (pretty_format); return (true); } catch (NxLibException &ex) { ensensoExceptionHandling (ex, "eulerAnglesTransformationToJson"); return (false); } }
bool pcl::EnsensoGrabber::computeCalibrationMatrix (const std::vector<Eigen::Affine3d, Eigen::aligned_allocator<Eigen::Affine3d> > &robot_poses, std::string &json, const std::string setup, const std::string target, const Eigen::Affine3d &guess_tf, const bool pretty_format) const { try { std::vector<std::string> robot_poses_json; robot_poses_json.resize (robot_poses.size ()); for (uint i = 0; i < robot_poses_json.size (); ++i) { if (!matrixTransformationToJson (robot_poses[i], robot_poses_json[i])) return (false); } // Feed all robot poses into the calibration command NxLibCommand calibrate (cmdCalibrateHandEye); for (uint i = 0; i < robot_poses_json.size (); ++i) { // Very weird behaviour here: // If you modify this loop, check that all the transformations are still here in the [itmExecute][itmParameters] node // because for an unknown reason sometimes the old transformations are erased in the tree ("null" in the tree) calibrate.parameters ()[itmTransformations][i].setJson (robot_poses_json[i], false); } // Set Hand-Eye calibration parameters if (boost::iequals (setup, "Fixed")) calibrate.parameters ()[itmSetup].set (valFixed); else calibrate.parameters ()[itmSetup].set (valMoving); calibrate.parameters ()[itmTarget].set (target); // Set guess matrix if (guess_tf.matrix () != Eigen::Matrix4d::Identity ()) { // Matrix > JSON > Angle axis NxLibItem tf ("/tmpTF"); if (!matrixTransformationToJson (guess_tf, json)) return (false); tf.setJson (json); // Rotation double theta = tf[itmRotation][itmAngle].asDouble (); // Angle of rotation double x = tf[itmRotation][itmAxis][0].asDouble (); // X component of Euler vector double y = tf[itmRotation][itmAxis][1].asDouble (); // Y component of Euler vector double z = tf[itmRotation][itmAxis][2].asDouble (); // Z component of Euler vector (*root_)[itmLink][itmRotation][itmAngle].set (theta); (*root_)[itmLink][itmRotation][itmAxis][0].set (x); (*root_)[itmLink][itmRotation][itmAxis][1].set (y); (*root_)[itmLink][itmRotation][itmAxis][2].set (z); // Translation (*root_)[itmLink][itmTranslation][0].set (guess_tf.translation ()[0]); (*root_)[itmLink][itmTranslation][1].set (guess_tf.translation ()[1]); (*root_)[itmLink][itmTranslation][2].set (guess_tf.translation ()[2]); } calibrate.execute (); // Might take up to 120 sec (maximum allowed by Ensenso API) if (calibrate.successful ()) { json = calibrate.result ().asJson (pretty_format); return (true); } else return (false); } catch (NxLibException &ex) { ensensoExceptionHandling (ex, "computeCalibrationMatrix"); return (false); } }
bool pcl::EnsensoGrabber::computeCalibrationMatrix (const std::vector<Eigen::Affine3d, Eigen::aligned_allocator<Eigen::Affine3d> > &robot_poses, std::string &json, const std::string setup, const std::string target, const Eigen::Affine3d &guess_tf, const bool pretty_format) const { if ( (*root_)[itmVersion][itmMajor] < 2 && (*root_)[itmVersion][itmMinor] < 3) PCL_WARN ("EnsensoSDK 1.3.x fixes bugs into extrinsic calibration matrix optimization, please update your SDK!\n" "http://www.ensenso.de/support/sdk-download/\n"); try { std::vector<Eigen::Affine3d, Eigen::aligned_allocator<Eigen::Affine3d> > robot_poses_mm (robot_poses); std::vector<std::string> robot_poses_json; robot_poses_json.resize (robot_poses.size ()); for (size_t i = 0; i < robot_poses_json.size (); ++i) { robot_poses_mm[i].translation () *= 1000.0; // Convert meters in millimeters if (!matrixTransformationToJson (robot_poses_mm[i], robot_poses_json[i])) return (false); } NxLibCommand calibrate (cmdCalibrateHandEye); // Set Hand-Eye calibration parameters if (boost::iequals (setup, "Fixed")) calibrate.parameters ()[itmSetup].set (valFixed); else calibrate.parameters ()[itmSetup].set (valMoving); calibrate.parameters ()[itmTarget].set (target); // Set guess matrix if (guess_tf.matrix () != Eigen::Matrix4d::Identity ()) { // Matrix > JSON > Angle axis NxLibItem tf ("/tmpTF"); if (!matrixTransformationToJson (guess_tf, json)) return (false); tf.setJson (json); // Rotation double theta = tf[itmRotation][itmAngle].asDouble (); // Angle of rotation double x = tf[itmRotation][itmAxis][0].asDouble (); // X component of Euler vector double y = tf[itmRotation][itmAxis][1].asDouble (); // Y component of Euler vector double z = tf[itmRotation][itmAxis][2].asDouble (); // Z component of Euler vector tf.erase(); // Delete tmpTF node calibrate.parameters ()[itmLink][itmRotation][itmAngle].set (theta); calibrate.parameters ()[itmLink][itmRotation][itmAxis][0].set (x); calibrate.parameters ()[itmLink][itmRotation][itmAxis][1].set (y); calibrate.parameters ()[itmLink][itmRotation][itmAxis][2].set (z); // Translation calibrate.parameters ()[itmLink][itmTranslation][0].set (guess_tf.translation ()[0] * 1000.0); calibrate.parameters ()[itmLink][itmTranslation][1].set (guess_tf.translation ()[1] * 1000.0); calibrate.parameters ()[itmLink][itmTranslation][2].set (guess_tf.translation ()[2] * 1000.0); } // Feed all robot poses into the calibration command for (size_t i = 0; i < robot_poses_json.size (); ++i) { // Very weird behavior here: // If you modify this loop, check that all the transformations are still here in the [itmExecute][itmParameters] node // because for an unknown reason sometimes the old transformations are erased in the tree ("null" in the tree) // Ensenso SDK 2.3.348: If not moved after guess calibration matrix, the vector is empty. calibrate.parameters ()[itmTransformations][i].setJson (robot_poses_json[i], false); } calibrate.execute (); // Might take up to 120 sec (maximum allowed by Ensenso API) if (calibrate.successful ()) { json = calibrate.result ().asJson (pretty_format); return (true); } else { json.clear (); return (false); } } catch (NxLibException &ex) { ensensoExceptionHandling (ex, "computeCalibrationMatrix"); return (false); } }