void EstimateScales(double max_reproj_error, const Pose& Pa1,
const Pose& Pa2,
const Pose& Pb1,
const std::vector<Vector2d>& yb1s,
const std::vector<Vector2d>& yb2s,
std::vector<double>& scale_nums,
std::vector<double>& scale_dens,
std::vector<double>& iratios)
{
Pose Pba = Pb1 * Pa1.inverse(); // Known relative transform from camera a to b
Pose Pa21 = Pa2 * Pa1.inverse(); // Relative transform from Pa1 to Pa2
auto Rba = Pba.rotation(); // Must not be identity
auto tba = Pba.translation();
auto Pab = Pba.inverse();
auto Rab = Pab.rotation();
auto tab = Pab.translation();
auto Ra21 = Pa21.rotation();
auto ta21 = Pa21.translation();
auto Rb21 = Rba * Ra21 * Rab; // Relative rotation from Pb1 to Pb2
auto A = crossMatrix(tba + Rba * Ra21 * tab) * Rb21;
auto B = crossMatrix(Rba * ta21) * Rb21;
#if 0
int id = 2;
printf("[%d] Tba = %s\n", id, toString(Tba).c_str());
printf("[%d] Ta21 = %s\n", id, toString(Ta21).c_str());
printf("[%d] Tab = %s\n", id, toString(Tab).c_str());
printf("[%d] Rab = %s\n", id, toString(Ra21).c_str());
printf("[%d] Ra21 = %s\n", id, toString(Ra21).c_str());
printf("[%d] Rb21 = %s\n", id, toString(Rb21).c_str());
printf("[%d] A = %s\n", id, toString(A).c_str());
printf("[%d] B = %s\n", id, toString(B).c_str());
#endif
uint n = (uint)yb1s.size();
scale_nums.clear();
scale_dens.clear();
iratios.clear();
for (uint i = 0; i < n; i++) {
// Get a scale estimate from one correspondence pair in camera b
auto y1 = yb1s[i].homogeneous();
auto y2 = yb2s[i].homogeneous();
double s_num = y2.dot(A * y1);
double s_den = (-y2).dot(B * y1);
double s = s_num / s_den;
// Count the inliers
Pose Pb2 = Pba * Pose(Pa2.rotation(), s * Pa2.translation());
Pose Pb21 = Pb2 * Pb1.inverse(); // Relative pose
vector<bool> inliers(n, true);
uint icnt = getReprojectionInliers(max_reproj_error, yb1s, yb2s, Pb21, inliers);
iratios.push_back(icnt / (float)n);
scale_nums.push_back(s_num);
scale_dens.push_back(s_den);
}
}
bool OPTCalibrationNode::save()
{
// Save tfs between sensors and world coordinate system (last checherboard) to file
std::string file_name = ros::package::getPath("opt_calibration") + "/conf/camera_poses.yaml";
std::ofstream file;
file.open(file_name.c_str());
if (file.is_open())
{
ros::Time time = ros::Time::now();
file << "# Auto generated file." << std::endl;
file << "calibration_id: " << time.sec << std::endl << std::endl;
Pose new_world_pose = Pose::Identity();
if (world_computation_ == LAST_CHECKERBOARD)
{
new_world_pose = calibration_->getLastCheckerboardPose().inverse();
for (size_t i = 0; i < sensor_vec_.size(); ++i)
{
const Sensor::Ptr & sensor = sensor_vec_[i];
Pose pose = new_world_pose * sensor->pose();
if(pose.translation().z() < 0)
{
ROS_INFO_STREAM("[" << sensor->frameId() << "] z < 0. Flipping /world orientation.");
AngleAxis rotation(M_PI, Vector3(1.0, 1.0, 0.0).normalized());
new_world_pose = rotation * calibration_->getLastCheckerboardPose().inverse();
break;
}
}
}
else if (world_computation_ == UPDATE)
{
new_world_pose = fixed_sensor_pose_ * fixed_sensor_->pose().inverse();
}
// Write TF transforms between cameras and world frame
file << "# Poses w.r.t. the \"world\" reference frame" << std::endl;
file << "poses:" << std::endl;
for (size_t i = 0; i < sensor_vec_.size(); ++i)
{
const Sensor::Ptr & sensor = sensor_vec_[i];
Pose pose = new_world_pose * sensor->pose();
file << " " << sensor->frameId().substr(1) << ":" << std::endl;
file << " translation:" << std::endl
<< " x: " << pose.translation().x() << std::endl
<< " y: " << pose.translation().y() << std::endl
<< " z: " << pose.translation().z() << std::endl;
Quaternion rotation(pose.rotation());
file << " rotation:" << std::endl
<< " x: " << rotation.x() << std::endl
<< " y: " << rotation.y() << std::endl
<< " z: " << rotation.z() << std::endl
<< " w: " << rotation.w() << std::endl;
}
file << std::endl << "# Inverse poses" << std::endl;
file << "inverse_poses:" << std::endl;
for (size_t i = 0; i < sensor_vec_.size(); ++i)
{
const Sensor::Ptr & sensor = sensor_vec_[i];
Pose pose = new_world_pose * sensor->pose();
pose = pose.inverse();
file << " " << sensor->frameId().substr(1) << ":" << std::endl;
file << " translation:" << std::endl
<< " x: " << pose.translation().x() << std::endl
<< " y: " << pose.translation().y() << std::endl
<< " z: " << pose.translation().z() << std::endl;
Quaternion rotation(pose.rotation());
file << " rotation:" << std::endl
<< " x: " << rotation.x() << std::endl
<< " y: " << rotation.y() << std::endl
<< " z: " << rotation.z() << std::endl
<< " w: " << rotation.w() << std::endl;
}
}
file.close();
ROS_INFO_STREAM(file_name << " created!");
return true;
}
