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);
}
}
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::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);
}
}
