void InteractiveMarkerDisplay::updateCb( visualization_msgs::InteractiveMarkerUpdateConstPtr msg )
{
updateMarkers( msg->server_id, msg->markers );
updatePoses( msg->server_id, msg->poses );
eraseMarkers( msg->server_id, msg->erases );
}
void RGBDCalibration :: loadFromFile(const char* filename)
{
QFileInfo f (filename);
ntk_throw_exception_if(!f.exists(), "Could not find calibration file.");
cv::FileStorage calibration_file (filename, CV_STORAGE_READ);
readMatrix(calibration_file, "rgb_intrinsics", rgb_intrinsics);
readMatrix(calibration_file, "rgb_distortion", rgb_distortion);
zero_rgb_distortion = rgb_distortion(0,0) < 1e-5 ? true : false;
readMatrix(calibration_file, "depth_intrinsics", depth_intrinsics);
readMatrix(calibration_file, "depth_distortion", depth_distortion);
zero_depth_distortion = depth_distortion(0,0) < 1e-5 ? true : false;
readMatrix(calibration_file, "R", R);
readMatrix(calibration_file, "T", T);
cv::Mat1i size_mat;
readMatrix(calibration_file, "rgb_size", size_mat);
rgb_size = cv::Size(size_mat(0,0), size_mat(0,1));
readMatrix(calibration_file, "raw_rgb_size", size_mat);
raw_rgb_size = cv::Size(size_mat(0,0), size_mat(0,1));
readMatrix(calibration_file, "depth_size", size_mat);
depth_size = cv::Size(size_mat(0,0), size_mat(0,1));
readMatrix(calibration_file, "raw_depth_size", size_mat);
raw_depth_size = cv::Size(size_mat(0,0), size_mat(0,1));
try {
readMatrix(calibration_file, "R_extrinsics", R_extrinsics);
readMatrix(calibration_file, "T_extrinsics", T_extrinsics);
}
catch (...)
{
ntk_dbg(0) << "Warning: could not load extrinsics (R_extrinsics, T_extrinsics).";
}
try
{
cv::Mat1i size_mat;
readMatrix(calibration_file, "infrared_size", size_mat);
infrared_size = cv::Size(size_mat(0,0), size_mat(0,1));
}
catch (...)
{
ntk_dbg(1) << "Could not read infrared image size, setting default 1280x1024";
infrared_size = cv::Size(1280, 1024);
}
try
{
readMatrix(calibration_file, "infrared_intrinsics", infrared_intrinsics);
readMatrix(calibration_file, "infrared_distortion", infrared_distortion);
} catch (...)
{
ntk_dbg(1) << "Warning: cannot read infrared camera intrinsics, computing from depth.";
computeInfraredIntrinsicsFromDepth();
}
cv::Mat1f depth_calib (1,2);
try {
readMatrix(calibration_file, "depth_base_and_offset", depth_calib);
depth_baseline = depth_calib(0,0);
depth_offset = depth_calib(0,1);
}
catch(...)
{
ntk_dbg(1) << "Warning: could not load depth offset";
}
try {
cv::Mat1f depth_multiplicative_correction_factor (1,1);
readMatrix(calibration_file, "depth_multiplicative_correction_factor", depth_multiplicative_correction_factor);
this->depth_multiplicative_correction_factor = depth_multiplicative_correction_factor(0,0);
}
catch(...)
{
ntk_dbg(1) << "Warning: could not load depth multiplicative factor";
}
try {
cv::Mat1f depth_additive_correction_factor (1,1);
readMatrix(calibration_file, "depth_additive_correction_factor", depth_additive_correction_factor);
this->depth_additive_correction_factor = depth_additive_correction_factor(0,0);
}
catch(...)
{
ntk_dbg(1) << "Warning: could not load depth additive correction factor";
}
calibration_file.release();
depth_pose = new Pose3D();
rgb_pose = new Pose3D();
updatePoses();
}
