bool pcl::EnsensoGrabber::setExtrinsicCalibration (const double euler_angle, Eigen::Vector3d &rotation_axis, const Eigen::Vector3d &translation, const std::string target) { if (!device_open_) return (false); if (rotation_axis != Eigen::Vector3d (0, 0, 0)) // Otherwise the vector becomes NaN rotation_axis.normalize (); try { NxLibItem calibParams = camera_[itmLink]; calibParams[itmTarget].set (target); calibParams[itmRotation][itmAngle].set (euler_angle); calibParams[itmRotation][itmAxis][0].set (rotation_axis[0]); calibParams[itmRotation][itmAxis][1].set (rotation_axis[1]); calibParams[itmRotation][itmAxis][2].set (rotation_axis[2]); calibParams[itmTranslation][0].set (translation[0] * 1000.0); // Convert in millimeters calibParams[itmTranslation][1].set (translation[1] * 1000.0); calibParams[itmTranslation][2].set (translation[2] * 1000.0); } catch (NxLibException &ex) { ensensoExceptionHandling (ex, "setExtrinsicCalibration"); return (false); } return (true); }
bool pcl::EnsensoGrabber::initExtrinsicCalibration (const int grid_spacing) const { if (!device_open_) return (false); if (running_) return (false); try { if (!clearCalibrationPatternBuffer ()) return (false); (*root_)[itmParameters][itmPattern][itmGridSpacing].set (grid_spacing); // GridSize can't be changed, it's protected in the tree // With the speckle projector on it is nearly impossible to recognize the pattern // (the 3D calibration is based on stereo images, not on 3D depth map) // Most important parameters are: projector=off, front_light=on configureCapture (true, true, 1, 0.32, true, 1, false, false, false, 10, false, 80, "Software", false); } catch (NxLibException &ex) { ensensoExceptionHandling (ex, "initExtrinsicCalibration"); return (false); } return (true); }
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); } }
bool pcl::EnsensoGrabber::jsonTransformationToAngleAxis (const std::string json, double &alpha, Eigen::Vector3d &axis, Eigen::Vector3d &translation) const { try { NxLibItem tf ("/tmpTF"); tf.setJson(json); translation[0] = tf[itmTranslation][0].asDouble (); translation[1] = tf[itmTranslation][1].asDouble (); translation[2] = tf[itmTranslation][2].asDouble (); alpha = tf[itmRotation][itmAngle].asDouble (); // Angle of rotation axis[0] = tf[itmRotation][itmAxis][0].asDouble (); // X component of Euler vector axis[1] = tf[itmRotation][itmAxis][1].asDouble (); // Y component of Euler vector axis[2] = tf[itmRotation][itmAxis][2].asDouble (); // Z component of Euler vector return (true); } catch (NxLibException &ex) { ensensoExceptionHandling (ex, "jsonTransformationToAngleAxis"); return (false); } }
bool pcl::EnsensoGrabber::openDevice (const int device) { if (device_open_) PCL_THROW_EXCEPTION (pcl::IOException, "Cannot open multiple devices!"); PCL_INFO ("Opening Ensenso stereo camera id = %d\n", device); try { // Create a pointer referencing the camera's tree item, for easier access: camera_ = (*root_)[itmCameras][itmBySerialNo][device]; if (!camera_.exists () || camera_[itmType] != valStereo) { PCL_THROW_EXCEPTION (pcl::IOException, "Please connect a single stereo camera to your computer!"); } NxLibCommand open (cmdOpen); open.parameters ()[itmCameras] = camera_[itmSerialNumber].asString (); open.execute (); } catch (NxLibException &ex) { ensensoExceptionHandling (ex, "openDevice"); return (false); } device_open_ = true; return (true); }
int pcl::EnsensoGrabber::enumDevices () const { int camera_count = 0; try { NxLibItem cams = NxLibItem ("/Cameras/BySerialNo"); camera_count = cams.count (); // Print information for all cameras in the tree PCL_INFO ("Number of connected cameras: %d\n", camera_count); PCL_INFO ("Serial No Model Status\n"); for (int n = 0; n < cams.count (); ++n) { PCL_INFO ("%s %s %s\n", cams[n][itmSerialNumber].asString ().c_str (), cams[n][itmModelName].asString ().c_str (), cams[n][itmStatus].asString ().c_str ()); } PCL_INFO ("\n"); } catch (NxLibException &ex) { ensensoExceptionHandling (ex, "enumDevices"); } return (camera_count); }
bool pcl::EnsensoGrabber::estimateCalibrationPatternPose (Eigen::Affine3d &pattern_pose) const { if (!device_open_) return (false); if (running_) return (false); try { NxLibCommand estimate_pattern_pose (cmdEstimatePatternPose); estimate_pattern_pose.execute (); NxLibItem tf = estimate_pattern_pose.result ()[itmPatternPose]; // Convert tf into a matrix if (!jsonTransformationToMatrix (tf.asJson (), pattern_pose)) return (false); pattern_pose.translation () /= 1000.0; // Convert translation in meters (Ensenso API returns milimeters) return (true); } catch (NxLibException &ex) { ensensoExceptionHandling (ex, "estimateCalibrationPatternPoses"); return (false); } }
bool pcl::EnsensoGrabber::angleAxisTransformationToJson (const double x, const double y, const double z, const double rx, const double ry, const double rz, const double alpha, std::string &json, const bool pretty_format) const { try { NxLibItem tf ("/tmpTF"); tf[itmTranslation][0].set (x); tf[itmTranslation][1].set (y); tf[itmTranslation][2].set (z); tf[itmRotation][itmAngle].set (alpha); // Angle of rotation tf[itmRotation][itmAxis][0].set (rx); // X component of Euler vector tf[itmRotation][itmAxis][1].set (ry); // Y component of Euler vector tf[itmRotation][itmAxis][2].set (rz); // Z component of Euler vector json = tf.asJson (pretty_format); tf.erase (); return (true); } catch (NxLibException &ex) { ensensoExceptionHandling (ex, "angleAxisTransformationToJson"); return (false); } }
std::string pcl::EnsensoGrabber::getTreeAsJson (const bool pretty_format) const { try { return (root_->asJson (pretty_format)); } catch (NxLibException &ex) { ensensoExceptionHandling (ex, "getTreeAsJson"); return (""); } }
bool pcl::EnsensoGrabber::grabSingleCloud (pcl::PointCloud<pcl::PointXYZ> &cloud) { if (!device_open_) return (false); if (running_) return (false); try { NxLibCommand (cmdCapture).execute (); // Stereo matching task NxLibCommand (cmdComputeDisparityMap).execute (); // Convert disparity map into XYZ data for each pixel NxLibCommand (cmdComputePointMap).execute (); // Get info about the computed point map and copy it into a std::vector double timestamp; std::vector<float> pointMap; int width, height; camera_[itmImages][itmRaw][itmLeft].getBinaryDataInfo (0, 0, 0, 0, 0, ×tamp); // Get raw image timestamp camera_[itmImages][itmPointMap].getBinaryDataInfo (&width, &height, 0, 0, 0, 0); camera_[itmImages][itmPointMap].getBinaryData (pointMap, 0); // Copy point cloud and convert in meters cloud.header.stamp = getPCLStamp (timestamp); cloud.resize (height * width); cloud.width = width; cloud.height = height; cloud.is_dense = false; // Copy data in point cloud (and convert milimeters in meters) for (size_t i = 0; i < pointMap.size (); i += 3) { cloud.points[i / 3].x = pointMap[i] / 1000.0; cloud.points[i / 3].y = pointMap[i + 1] / 1000.0; cloud.points[i / 3].z = pointMap[i + 2] / 1000.0; } return (true); } catch (NxLibException &ex) { ensensoExceptionHandling (ex, "grabSingleCloud"); return (false); } }
bool pcl::EnsensoGrabber::storeEEPROMExtrinsicCalibration () const { try { NxLibCommand store (cmdStoreCalibration); store.execute (); return (true); } catch (NxLibException &ex) { ensensoExceptionHandling (ex, "storeEEPROMExtrinsicCalibration"); return (false); } }
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::closeTcpPort () { try { nxLibCloseTcpPort (); tcp_open_ = false; } catch (NxLibException &ex) { ensensoExceptionHandling (ex, "closeTcpPort"); return (false); } return (true); }
bool pcl::EnsensoGrabber::openTcpPort (const int port) { try { nxLibOpenTcpPort (port); tcp_open_ = true; } catch (NxLibException &ex) { ensensoExceptionHandling (ex, "openTcpPort"); return (false); } return (true); }
bool pcl::EnsensoGrabber::setExtrinsicCalibration (const double euler_angle, Eigen::Vector3d &rotation_axis, const Eigen::Vector3d &translation, const std::string target) { if (!device_open_) return (false); if (rotation_axis != Eigen::Vector3d (0, 0, 0)) // Otherwise the vector becomes NaN rotation_axis.normalize (); try { NxLibItem calibParams = camera_[itmLink]; calibParams[itmTarget].set (target); calibParams[itmRotation][itmAngle].set (euler_angle); /* FIXME: Bug in Ensenso API: http://ensenso.de/manual/index.html?about.htm see version 1.2.125 The re-normalisation is still not working proprely, when it does, use this code rather than the workaround calibParams[itmRotation][itmAxis][0].set (rotation_axis[0]); calibParams[itmRotation][itmAxis][1].set (rotation_axis[1]); calibParams[itmRotation][itmAxis][2].set (rotation_axis[2]); */ // Workaround std::string axis_x, axis_y, axis_z; axis_x = boost::lexical_cast<std::string> (rotation_axis[0]); axis_y = boost::lexical_cast<std::string> (rotation_axis[1]); axis_z = boost::lexical_cast<std::string> (rotation_axis[2]); calibParams[itmRotation][itmAxis][0].setJson (axis_x); calibParams[itmRotation][itmAxis][1].setJson (axis_y); calibParams[itmRotation][itmAxis][2].setJson (axis_z); // End of workaround calibParams[itmTranslation][0].set (translation[0] * 1000.0); // Convert in millimeters calibParams[itmTranslation][1].set (translation[1] * 1000.0); calibParams[itmTranslation][2].set (translation[2] * 1000.0); } catch (NxLibException &ex) { ensensoExceptionHandling (ex, "setExtrinsicCalibration"); return (false); } return (true); }
bool pcl::EnsensoGrabber::configureCapture (const bool auto_exposure, const bool auto_gain, const int bining, const float exposure, const bool front_light, const int gain, const bool gain_boost, const bool hardware_gamma, const bool hdr, const int pixel_clock, const bool projector, const int target_brightness, const std::string trigger_mode, const bool use_disparity_map_area_of_interest) const { if (!device_open_) return (false); try { NxLibItem captureParams = camera_[itmParameters][itmCapture]; captureParams[itmAutoExposure].set (auto_exposure); captureParams[itmAutoGain].set (auto_gain); captureParams[itmBinning].set (bining); captureParams[itmExposure].set (exposure); captureParams[itmFrontLight].set (front_light); captureParams[itmGain].set (gain); captureParams[itmGainBoost].set (gain_boost); captureParams[itmHardwareGamma].set (hardware_gamma); captureParams[itmHdr].set (hdr); captureParams[itmPixelClock].set (pixel_clock); captureParams[itmProjector].set (projector); captureParams[itmTargetBrightness].set (target_brightness); captureParams[itmTriggerMode].set (trigger_mode); captureParams[itmUseDisparityMapAreaOfInterest].set (use_disparity_map_area_of_interest); } catch (NxLibException &ex) { ensensoExceptionHandling (ex, "configureCapture"); return (false); } return (true); }
bool pcl::EnsensoGrabber::clearCalibrationPatternBuffer () const { if (!device_open_) return (false); if (running_) return (false); try { NxLibCommand (cmdDiscardPatterns).execute (); } catch (NxLibException &ex) { ensensoExceptionHandling (ex, "clearCalibrationPatternBuffer"); return (false); } return (true); }
bool pcl::EnsensoGrabber::closeDevice () { if (!device_open_) return (false); stop (); PCL_INFO ("Closing Ensenso stereo camera\n"); try { NxLibCommand (cmdClose).execute (); device_open_ = false; } catch (NxLibException &ex) { ensensoExceptionHandling (ex, "closeDevice"); return (false); } return (true); }
pcl::EnsensoGrabber::EnsensoGrabber () : device_open_ (false), tcp_open_ (false), running_ (false) { point_cloud_signal_ = createSignal<sig_cb_ensenso_point_cloud> (); images_signal_ = createSignal<sig_cb_ensenso_images> (); point_cloud_images_signal_ = createSignal<sig_cb_ensenso_point_cloud_images> (); PCL_INFO ("Initialising nxLib\n"); try { nxLibInitialize (); root_.reset (new NxLibItem); } catch (NxLibException &ex) { ensensoExceptionHandling (ex, "EnsensoGrabber"); PCL_THROW_EXCEPTION (pcl::IOException, "Could not initialise NxLib."); // If constructor fails; throw exception } }
bool pcl::EnsensoGrabber::matrixTransformationToJson (const Eigen::Affine3d &matrix, std::string &json, const bool pretty_format) const { try { NxLibCommand convert_transformation (cmdConvertTransformation); // Rotation convert_transformation.parameters ()[itmTransformation][0][0].set (matrix.linear ().col (0)[0]); convert_transformation.parameters ()[itmTransformation][0][1].set (matrix.linear ().col (0)[1]); convert_transformation.parameters ()[itmTransformation][0][2].set (matrix.linear ().col (0)[2]); convert_transformation.parameters ()[itmTransformation][0][3].set (0.0); convert_transformation.parameters ()[itmTransformation][1][0].set (matrix.linear ().col (1)[0]); convert_transformation.parameters ()[itmTransformation][1][1].set (matrix.linear ().col (1)[1]); convert_transformation.parameters ()[itmTransformation][1][2].set (matrix.linear ().col (1)[2]); convert_transformation.parameters ()[itmTransformation][1][3].set (0.0); convert_transformation.parameters ()[itmTransformation][2][0].set (matrix.linear ().col (2)[0]); convert_transformation.parameters ()[itmTransformation][2][1].set (matrix.linear ().col (2)[1]); convert_transformation.parameters ()[itmTransformation][2][2].set (matrix.linear ().col (2)[2]); convert_transformation.parameters ()[itmTransformation][2][3].set (0.0); // Translation convert_transformation.parameters ()[itmTransformation][3][0].set (matrix.translation ()[0]); convert_transformation.parameters ()[itmTransformation][3][1].set (matrix.translation ()[1]); convert_transformation.parameters ()[itmTransformation][3][2].set (matrix.translation ()[1]); convert_transformation.parameters ()[itmTransformation][3][3].set (1.0); convert_transformation.execute (); json = convert_transformation.result ()[itmTransformation].asJson (pretty_format); return (true); } catch (NxLibException &ex) { ensensoExceptionHandling (ex, "matrixTransformationToJson"); return (false); } }
int pcl::EnsensoGrabber::captureCalibrationPattern () const { if (!device_open_) return (-1); if (running_) return (-1); try { NxLibCommand (cmdCapture).execute (); NxLibCommand (cmdCollectPattern).execute (); } catch (NxLibException &ex) { ensensoExceptionHandling (ex, "captureCalibrationPattern"); return (-1); } return ( (*root_)[itmParameters][itmPatternCount].asInt ()); }
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::jsonTransformationToMatrix (const std::string transformation, Eigen::Affine3d &matrix) const { try { NxLibCommand convert_transformation (cmdConvertTransformation); convert_transformation.parameters ()[itmTransformation].setJson (transformation); convert_transformation.execute (); Eigen::Affine3d tmp (Eigen::Affine3d::Identity ()); // Rotation tmp.linear ().col (0) = Eigen::Vector3d (convert_transformation.result ()[itmTransformation][0][0].asDouble (), convert_transformation.result ()[itmTransformation][0][1].asDouble (), convert_transformation.result ()[itmTransformation][0][2].asDouble ()); tmp.linear ().col (1) = Eigen::Vector3d (convert_transformation.result ()[itmTransformation][1][0].asDouble (), convert_transformation.result ()[itmTransformation][1][1].asDouble (), convert_transformation.result ()[itmTransformation][1][2].asDouble ()); tmp.linear ().col (2) = Eigen::Vector3d (convert_transformation.result ()[itmTransformation][2][0].asDouble (), convert_transformation.result ()[itmTransformation][2][1].asDouble (), convert_transformation.result ()[itmTransformation][2][2].asDouble ()); // Translation tmp.translation () = Eigen::Vector3d (convert_transformation.result ()[itmTransformation][3][0].asDouble (), convert_transformation.result ()[itmTransformation][3][1].asDouble (), convert_transformation.result ()[itmTransformation][3][2].asDouble ()); matrix = tmp; return (true); } catch (NxLibException &ex) { ensensoExceptionHandling (ex, "jsonTransformationToMatrix"); 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); } }
void pcl::EnsensoGrabber::processGrabbing () { bool continue_grabbing = running_; while (continue_grabbing) { try { // Publish cloud / images if (num_slots<sig_cb_ensenso_point_cloud> () > 0 || num_slots<sig_cb_ensenso_images> () > 0 || num_slots<sig_cb_ensenso_point_cloud_images> () > 0) { pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>); boost::shared_ptr<PairOfImages> images (new PairOfImages); fps_mutex_.lock (); frequency_.event (); fps_mutex_.unlock (); NxLibCommand (cmdCapture).execute (); double timestamp; camera_[itmImages][itmRaw][itmLeft].getBinaryDataInfo (0, 0, 0, 0, 0, ×tamp); // Gather images if (num_slots<sig_cb_ensenso_images> () > 0 || num_slots<sig_cb_ensenso_point_cloud_images> () > 0) { // Rectify images NxLibCommand (cmdRectifyImages).execute (); int width, height, channels, bpe; bool isFlt, collected_pattern = false; try // Try to collect calibration pattern, if not possible, publish RAW images instead { NxLibCommand collect_pattern (cmdCollectPattern); collect_pattern.parameters ()[itmBuffer].set (false); // Do NOT store the pattern into the buffer! collect_pattern.execute (); collected_pattern = true; } catch (const NxLibException &ex) { // We failed to collect the pattern but the RAW images are available! } if (collected_pattern) { camera_[itmImages][itmWithOverlay][itmLeft].getBinaryDataInfo (&width, &height, &channels, &bpe, &isFlt, 0); images->first.header.stamp = images->second.header.stamp = getPCLStamp (timestamp); images->first.width = images->second.width = width; images->first.height = images->second.height = height; images->first.data.resize (width * height * sizeof(float)); images->second.data.resize (width * height * sizeof(float)); images->first.encoding = images->second.encoding = getOpenCVType (channels, bpe, isFlt); camera_[itmImages][itmWithOverlay][itmLeft].getBinaryData (images->first.data.data (), images->first.data.size (), 0, 0); camera_[itmImages][itmWithOverlay][itmRight].getBinaryData (images->second.data.data (), images->second.data.size (), 0, 0); } else { camera_[itmImages][itmRaw][itmLeft].getBinaryDataInfo (&width, &height, &channels, &bpe, &isFlt, 0); images->first.header.stamp = images->second.header.stamp = getPCLStamp (timestamp); images->first.width = images->second.width = width; images->first.height = images->second.height = height; images->first.data.resize (width * height * sizeof(float)); images->second.data.resize (width * height * sizeof(float)); images->first.encoding = images->second.encoding = getOpenCVType (channels, bpe, isFlt); camera_[itmImages][itmRaw][itmLeft].getBinaryData (images->first.data.data (), images->first.data.size (), 0, 0); camera_[itmImages][itmRaw][itmRight].getBinaryData (images->second.data.data (), images->second.data.size (), 0, 0); } } // Gather point cloud if (num_slots<sig_cb_ensenso_point_cloud> () > 0 || num_slots<sig_cb_ensenso_point_cloud_images> () > 0) { // Stereo matching task NxLibCommand (cmdComputeDisparityMap).execute (); // Convert disparity map into XYZ data for each pixel NxLibCommand (cmdComputePointMap).execute (); // Get info about the computed point map and copy it into a std::vector std::vector<float> pointMap; int width, height; camera_[itmImages][itmPointMap].getBinaryDataInfo (&width, &height, 0, 0, 0, 0); camera_[itmImages][itmPointMap].getBinaryData (pointMap, 0); // Copy point cloud and convert in meters cloud->header.stamp = getPCLStamp (timestamp); cloud->points.resize (height * width); cloud->width = width; cloud->height = height; cloud->is_dense = false; // Copy data in point cloud (and convert milimeters in meters) for (size_t i = 0; i < pointMap.size (); i += 3) { cloud->points[i / 3].x = pointMap[i] / 1000.0; cloud->points[i / 3].y = pointMap[i + 1] / 1000.0; cloud->points[i / 3].z = pointMap[i + 2] / 1000.0; } } // Publish signals if (num_slots<sig_cb_ensenso_point_cloud_images> () > 0) point_cloud_images_signal_->operator () (cloud, images); else if (num_slots<sig_cb_ensenso_point_cloud> () > 0) point_cloud_signal_->operator () (cloud); else if (num_slots<sig_cb_ensenso_images> () > 0) images_signal_->operator () (images); } continue_grabbing = running_; } catch (NxLibException &ex) { ensensoExceptionHandling (ex, "processGrabbing"); } } }
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); } }