int calib_gimbal_data_load(calib_gimbal_data_t &data,
const std::string &data_dir) {
// Load joint data
const std::string joint_filepath = data_dir + "/joint.csv";
if (csv2mat(joint_filepath, false, data.joint_data) != 0) {
LOG_ERROR("Failed to load joint data [%s]!", joint_filepath.c_str());
return -1;
}
// Load data
data.nb_measurements = data.joint_data.rows();
for (int i = 0; i < data.nb_measurements; i++) {
const std::string set_path = data_dir + "/set" + std::to_string(i);
std::cout << "Loading measurement set " << i << " ";
std::cout << "[" << set_path << "]" << std::endl;
// P_s_i
matx_t P_s_i;
const std::string P_s_fp = set_path + "/P_s";
if (csv2mat(P_s_fp, false, P_s_i) != 0) {
LOG_ERROR("Failed to P_s data [%s]!", P_s_fp.c_str());
return -1;
}
// P_d_i
matx_t P_d_i;
const std::string P_d_fp = set_path + "/P_d";
if (csv2mat(P_d_fp, false, P_d_i) != 0) {
LOG_ERROR("Failed to P_d data [%s]!", P_d_fp.c_str());
return -1;
}
// Q_s_i
matx_t Q_s_i;
const std::string Q_s_fp = set_path + "/Q_s";
if (csv2mat(Q_s_fp, false, Q_s_i) != 0) {
LOG_ERROR("Failed to Q_s data [%s]!", Q_s_fp.c_str());
return -1;
}
// Q_d_i
matx_t Q_d_i;
const std::string Q_d_fp = set_path + "/Q_d";
if (csv2mat(Q_d_fp, false, Q_d_i) != 0) {
LOG_ERROR("Failed to Q_d data [%s]!", Q_d_fp.c_str());
return -1;
}
// Store it
data.P_s.emplace_back(P_s_i);
data.P_d.emplace_back(P_d_i);
data.Q_s.emplace_back(Q_s_i);
data.Q_d.emplace_back(Q_d_i);
}
data.ok = true;
return 0;
}
int calib_gimbal_params_load(calib_gimbal_params_t &data,
const std::string &camchain_file,
const std::string &joint_file) {
// Parse camchain file
// if (this->camchain.load(3, camchain_file) != 0) {
// LOG_ERROR("Failed to load camchain file [%s]!", camchain_file.c_str());
// return -1;
// }
// Load joint angles file
matx_t joint_data;
if (csv2mat(joint_file, false, joint_data) != 0) {
LOG_ERROR("Failed to load joint angles file [%s]!", joint_file.c_str());
return -1;
}
data.nb_measurements = joint_data.rows();
// Initialize optimization params
// data.tau_s = vec2array(data.camchain.tau_s);
// data.tau_d = vec2array(data.camchain.tau_d);
// data.w1 = vec2array(data.camchain.w1);
// data.w2 = vec2array(data.camchain.w2);
data.theta1_offset = (double *) malloc(sizeof(double) * 1);
data.theta2_offset = (double *) malloc(sizeof(double) * 1);
// *data.theta1_offset = data.camchain.theta1_offset;
// *data.theta2_offset = data.camchain.theta2_offset;
data.Lambda1 = vec2array(joint_data.col(0));
data.Lambda2 = vec2array(joint_data.col(1));
return 0;
}
ColorCloud::Ptr TokyoCreator::createPointCloud(
const std::string &jpgPath, const std::string &csvPath) {
cv::Mat colorImg = cv::imread(jpgPath, -1);
cv::Mat spaceImg = csv2mat(csvPath);
if( !colorImg.data )
throw std::runtime_error("Cloud not read image " + jpgPath);
if( !spaceImg.data )
throw std::runtime_error("Cloud not read image " + csvPath);
int height = spaceImg.rows;
int width = spaceImg.cols;
if( spaceImg.rows != colorImg.rows || spaceImg.cols != colorImg.cols )
throw std::runtime_error("Images have different dimensions");
ColorCloud::Ptr cloud( new ColorCloud() );
cloud->height = height;
cloud->width = width;
cloud->reserve(height * width);
cloud->is_dense = false;
pcl::PointXYZRGB newPoint;
for (int i = 0; i < height; i++) {
cv::Vec3b *spacePtr = spaceImg.ptr<cv::Vec3b>(i);
cv::Vec3b *colorPtr = colorImg.ptr<cv::Vec3b>(i);
for (int j = 0; j < width; j++) {
cv::Vec3b colorVec = colorPtr[j];
cv::Vec3b spaceVec = spacePtr[j];
newPoint.x = spaceVec[0];
newPoint.y = spaceVec[1];
newPoint.z = spaceVec[2];
newPoint.r = colorVec[2];
newPoint.g = colorVec[1];
newPoint.b = colorVec[0];
cloud->push_back(newPoint);
}
}
std::vector<int> vec;
pcl::removeNaNFromPointCloud( *cloud, *cloud, vec );
return cloud;
}
int calib_gimbal_calc_reprojection_errors(calib_gimbal_t &calib) {
// Form gimbal model
gimbal_model_t gimbal_model;
gimbal_model.tau_s = vec6_t{calib.params.tau_s};
gimbal_model.tau_d = vec6_t{calib.params.tau_d};
gimbal_model.w1 = vec3_t{calib.params.w1};
gimbal_model.w2 = vec3_t{calib.params.w2};
gimbal_model.theta1_offset = *calib.params.theta1_offset;
gimbal_model.theta2_offset = *calib.params.theta2_offset;
// Load joint data
matx_t joint_data;
const std::string joint_filepath = calib.data_dir + "/joint.csv";
if (csv2mat(joint_filepath, false, joint_data) != 0) {
LOG_ERROR("Failed to load joint data [%s]!", joint_filepath.c_str());
return -1;
}
// // Camera matrix for both static and dynamic camera
// const mat3_t K_s = calib.params.camchain.cam[0].K();
// const mat3_t K_d = calib.params.camchain.cam[2].K();
// // Calculate reprojection error
// double sse_s = 0.0;
// double sse_d = 0.0;
// int points = 0;
//
// for (int i = 0; i < calib.data.nb_measurements; i++) {
// for (int j = 0; j < calib.data.P_s[i].rows(); j++) {
// // Get 3D point from static and dyanmic camera
// const vec3_t P_s = calib.data.P_s[i].row(j);
// const vec3_t P_d = calib.data.P_d[i].row(j);
//
// // Undistort pixel measurements from static camera
// const vec2_t p_s = calib.data.Q_s[i].row(j);
// cv::Point2f pt_s(p_s(0), p_s(1));
// pt_s = calib.params.camchain.cam[0].undistortPoint(pt_s);
// vec3_t x_s{pt_s.x, pt_s.y, 1.0};
// x_s = calib.params.camchain.cam[0].K() * x_s;
// const vec2_t Q_s{x_s(0), x_s(1)};
//
// // Undistort pixel measurements from dynamic camera
// const vec2_t p_d = calib.data.Q_d[i].row(j);
// cv::Point2f pt_d(p_d(0), p_d(1));
// pt_d = calib.params.camchain.cam[2].undistortPoint(pt_d);
// vec3_t x_d{pt_d.x, pt_d.y, 1.0};
// x_d = calib.params.camchain.cam[2].K() * x_d;
// const vec2_t Q_d{x_d(0), x_d(1)};
//
// // Get gimbal roll and pitch angles then form T_ds transform
// const double joint_roll = calib.params.Lambda1[i];
// const double joint_pitch = calib.params.Lambda2[i];
//
// // std::cout << joint_roll << " " << joint_pitch << std::endl;
// // std::cout << joint_data(i, 0) << " " << joint_data(i, 1) <<
// std::endl;
// // std::cout << "Roll Error: " << joint_roll - joint_data(i, 0) <<
// std::endl;
// // std::cout << "Pitch Error: " << joint_pitch - joint_data(i, 1) <<
// std::endl;
// // // exit(0);
// // std::cout << std::endl;
// // std::cout << std::endl;
//
// gimbal_model.setAttitude(joint_roll, joint_pitch);
// const mat4_t T_ds = gimbal_model.T_ds();
//
// // Project 3D point observed from static to dynamic camera
// const vec3_t P_d_cal = (T_ds * P_s.homogeneous()).head(3);
// const vec3_t X{P_d_cal(0) / P_d_cal(2), P_d_cal(1) /
// P_d_cal(2), 1.0}; const vec2_t Q_d_cal = (K_d * X).head(2);
//
// // Project 3D point observed from dynamic to static camera
// const vec3_t P_s_cal = (T_ds.inverse() * P_d.homogeneous()).head(3);
// const vec3_t X_s{P_s_cal(0) / P_s_cal(2), P_s_cal(1) /
// P_s_cal(2), 1.0}; const vec2_t Q_s_cal = (K_s * X_s).head(2);
//
// // Calculate reprojection error in static camera
// const vec2_t diff_s = Q_s_cal - Q_s;
// const double squared_error_s = diff_s.norm();
// sse_s += squared_error_s;
//
// // Calculate reprojection error in dynamic camera
// const vec2_t diff_d = Q_d_cal - Q_d;
// const double squared_error_d = diff_d.norm();
// sse_d += squared_error_d;
//
// points++;
// }
// }
//
// // Print reprojection errors
// std::cout << "Static camera RMSE: " << sqrt(sse_s / points) << std::endl;
// std::cout << "Dynamic camera RMSE: " << sqrt(sse_d / points) <<
// std::endl;
return 0;
}
