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);
}
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::storeEEPROMExtrinsicCalibration () const
{
try
{
NxLibCommand store (cmdStoreCalibration);
store.execute ();
return (true);
}
catch (NxLibException &ex)
{
ensensoExceptionHandling (ex, "storeEEPROMExtrinsicCalibration");
return (false);
}
}
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);
}
}
