bool Bundle_Adjustment_Ceres::Adjust
(
SfM_Data & sfm_data, // the SfM scene to refine
const Optimize_Options options
)
{
//----------
// Add camera parameters
// - intrinsics
// - poses [R|t]
// Create residuals for each observation in the bundle adjustment problem. The
// parameters for cameras and points are added automatically.
//----------
ceres::Problem problem;
// Data wrapper for refinement:
Hash_Map<IndexT, std::vector<double> > map_intrinsics;
Hash_Map<IndexT, std::vector<double> > map_poses;
// Setup Poses data & subparametrization
for (Poses::const_iterator itPose = sfm_data.poses.begin(); itPose != sfm_data.poses.end(); ++itPose)
{
const IndexT indexPose = itPose->first;
const Pose3 & pose = itPose->second;
const Mat3 R = pose.rotation();
const Vec3 t = pose.translation();
double angleAxis[3];
ceres::RotationMatrixToAngleAxis((const double*)R.data(), angleAxis);
// angleAxis + translation
map_poses[indexPose] = {angleAxis[0], angleAxis[1], angleAxis[2], t(0), t(1), t(2)};
double * parameter_block = &map_poses[indexPose][0];
problem.AddParameterBlock(parameter_block, 6);
if (options.extrinsics_opt == Extrinsic_Parameter_Type::NONE)
{
// set the whole parameter block as constant for best performance
problem.SetParameterBlockConstant(parameter_block);
}
else // Subset parametrization
{
std::vector<int> vec_constant_extrinsic;
// If we adjust only the translation, we must set ROTATION as constant
if (options.extrinsics_opt == Extrinsic_Parameter_Type::ADJUST_TRANSLATION)
{
// Subset rotation parametrization
vec_constant_extrinsic.push_back(0);
vec_constant_extrinsic.push_back(1);
vec_constant_extrinsic.push_back(2);
}
// If we adjust only the rotation, we must set TRANSLATION as constant
if (options.extrinsics_opt == Extrinsic_Parameter_Type::ADJUST_ROTATION)
{
// Subset translation parametrization
vec_constant_extrinsic.push_back(3);
vec_constant_extrinsic.push_back(4);
vec_constant_extrinsic.push_back(5);
}
if (!vec_constant_extrinsic.empty())
{
ceres::SubsetParameterization *subset_parameterization =
new ceres::SubsetParameterization(6, vec_constant_extrinsic);
problem.SetParameterization(parameter_block, subset_parameterization);
}
}
}
// Setup Intrinsics data & subparametrization
for (Intrinsics::const_iterator itIntrinsic = sfm_data.intrinsics.begin();
itIntrinsic != sfm_data.intrinsics.end(); ++itIntrinsic)
{
const IndexT indexCam = itIntrinsic->first;
if (isValid(itIntrinsic->second->getType()))
{
map_intrinsics[indexCam] = itIntrinsic->second->getParams();
double * parameter_block = &map_intrinsics[indexCam][0];
problem.AddParameterBlock(parameter_block, map_intrinsics[indexCam].size());
if (options.intrinsics_opt == Intrinsic_Parameter_Type::NONE)
{
// set the whole parameter block as constant for best performance
problem.SetParameterBlockConstant(parameter_block);
}
else
{
const std::vector<int> vec_constant_intrinsic =
itIntrinsic->second->subsetParameterization(options.intrinsics_opt);
if (!vec_constant_intrinsic.empty())
{
ceres::SubsetParameterization *subset_parameterization =
new ceres::SubsetParameterization(
map_intrinsics[indexCam].size(), vec_constant_intrinsic);
problem.SetParameterization(parameter_block, subset_parameterization);
}
}
}
else
{
std::cerr << "Unsupported camera type." << std::endl;
}
}
// Set a LossFunction to be less penalized by false measurements
// - set it to NULL if you don't want use a lossFunction.
ceres::LossFunction * p_LossFunction = new ceres::HuberLoss(Square(4.0));
// TODO: make the LOSS function and the parameter an option
// For all visibility add reprojections errors:
for (Landmarks::iterator iterTracks = sfm_data.structure.begin();
iterTracks!= sfm_data.structure.end(); ++iterTracks)
{
const Observations & obs = iterTracks->second.obs;
for (Observations::const_iterator itObs = obs.begin();
itObs != obs.end(); ++itObs)
{
// Build the residual block corresponding to the track observation:
const View * view = sfm_data.views.at(itObs->first).get();
// Each Residual block takes a point and a camera as input and outputs a 2
// dimensional residual. Internally, the cost function stores the observed
// image location and compares the reprojection against the observation.
ceres::CostFunction* cost_function =
IntrinsicsToCostFunction(sfm_data.intrinsics[view->id_intrinsic].get(), itObs->second.x);
if (cost_function)
problem.AddResidualBlock(cost_function,
p_LossFunction,
&map_intrinsics[view->id_intrinsic][0],
&map_poses[view->id_pose][0],
iterTracks->second.X.data());
}
if (options.structure_opt == Structure_Parameter_Type::NONE)
problem.SetParameterBlockConstant(iterTracks->second.X.data());
}
if (options.control_point_opt.bUse_control_points)
{
// Use Ground Control Point:
// - fixed 3D points with weighted observations
for (Landmarks::iterator iterGCPTracks = sfm_data.control_points.begin();
iterGCPTracks!= sfm_data.control_points.end(); ++iterGCPTracks)
{
const Observations & obs = iterGCPTracks->second.obs;
for (Observations::const_iterator itObs = obs.begin();
itObs != obs.end(); ++itObs)
{
// Build the residual block corresponding to the track observation:
const View * view = sfm_data.views.at(itObs->first).get();
// Each Residual block takes a point and a camera as input and outputs a 2
// dimensional residual. Internally, the cost function stores the observed
// image location and compares the reprojection against the observation.
ceres::CostFunction* cost_function =
IntrinsicsToCostFunction(
sfm_data.intrinsics[view->id_intrinsic].get(),
itObs->second.x,
options.control_point_opt.weight);
if (cost_function)
problem.AddResidualBlock(cost_function,
nullptr,
&map_intrinsics[view->id_intrinsic][0],
&map_poses[view->id_pose][0],
iterGCPTracks->second.X.data());
}
// Set the 3D point as FIXED (it's a GCP)
problem.SetParameterBlockConstant(iterGCPTracks->second.X.data());
}
}
// Configure a BA engine and run it
// Make Ceres automatically detect the bundle structure.
ceres::Solver::Options ceres_config_options;
ceres_config_options.preconditioner_type = ceres_options_.preconditioner_type_;
ceres_config_options.linear_solver_type = ceres_options_.linear_solver_type_;
ceres_config_options.sparse_linear_algebra_library_type = ceres_options_.sparse_linear_algebra_library_type_;
ceres_config_options.minimizer_progress_to_stdout = false;
ceres_config_options.logging_type = ceres::SILENT;
ceres_config_options.num_threads = ceres_options_.nb_threads_;
ceres_config_options.num_linear_solver_threads = ceres_options_.nb_threads_;
ceres_config_options.parameter_tolerance = ceres_options_.parameter_tolerance_;
// Solve BA
ceres::Solver::Summary summary;
ceres::Solve(ceres_config_options, &problem, &summary);
if (ceres_options_.bCeres_summary_)
std::cout << summary.FullReport() << std::endl;
// If no error, get back refined parameters
if (!summary.IsSolutionUsable())
{
if (ceres_options_.bVerbose_)
std::cout << "Bundle Adjustment failed." << std::endl;
return false;
}
else // Solution is usable
{
if (ceres_options_.bVerbose_)
{
// Display statistics about the minimization
std::cout << std::endl
<< "Bundle Adjustment statistics (approximated RMSE):\n"
<< " #views: " << sfm_data.views.size() << "\n"
<< " #poses: " << sfm_data.poses.size() << "\n"
<< " #intrinsics: " << sfm_data.intrinsics.size() << "\n"
<< " #tracks: " << sfm_data.structure.size() << "\n"
<< " #residuals: " << summary.num_residuals << "\n"
<< " Initial RMSE: " << std::sqrt( summary.initial_cost / summary.num_residuals) << "\n"
<< " Final RMSE: " << std::sqrt( summary.final_cost / summary.num_residuals) << "\n"
<< " Time (s): " << summary.total_time_in_seconds << "\n"
<< std::endl;
}
// Update camera poses with refined data
if (options.extrinsics_opt != Extrinsic_Parameter_Type::NONE)
{
for (Poses::iterator itPose = sfm_data.poses.begin();
itPose != sfm_data.poses.end(); ++itPose)
{
const IndexT indexPose = itPose->first;
Mat3 R_refined;
ceres::AngleAxisToRotationMatrix(&map_poses[indexPose][0], R_refined.data());
Vec3 t_refined(map_poses[indexPose][3], map_poses[indexPose][4], map_poses[indexPose][5]);
// Update the pose
Pose3 & pose = itPose->second;
pose = Pose3(R_refined, -R_refined.transpose() * t_refined);
}
}
// Update camera intrinsics with refined data
if (options.intrinsics_opt != Intrinsic_Parameter_Type::NONE)
{
for (Intrinsics::iterator itIntrinsic = sfm_data.intrinsics.begin();
itIntrinsic != sfm_data.intrinsics.end(); ++itIntrinsic)
{
const IndexT indexCam = itIntrinsic->first;
const std::vector<double> & vec_params = map_intrinsics[indexCam];
itIntrinsic->second.get()->updateFromParams(vec_params);
}
}
// Structure is already updated directly if needed (no data wrapping)
return true;
}
}
bool Bundle_Adjustment_Ceres::Adjust(
SfM_Data & sfm_data, // the SfM scene to refine
bool bRefineRotations, // tell if pose rotations will be refined
bool bRefineTranslations,// tell if the pose translation will be refined
bool bRefineIntrinsics, // tell if the camera intrinsic will be refined
bool bRefineStructure) // tell if the structure will be refined
{
//----------
// Add camera parameters
// - intrinsics
// - poses [R|t]
// Create residuals for each observation in the bundle adjustment problem. The
// parameters for cameras and points are added automatically.
//----------
ceres::Problem problem;
// Data wrapper for refinement:
Hash_Map<IndexT, std::vector<double> > map_intrinsics;
Hash_Map<IndexT, std::vector<double> > map_poses;
// Setup Poses data & subparametrization
for (Poses::const_iterator itPose = sfm_data.poses.begin(); itPose != sfm_data.poses.end(); ++itPose)
{
const IndexT indexPose = itPose->first;
const Pose3 & pose = itPose->second;
const Mat3 R = pose.rotation();
const Vec3 t = pose.translation();
double angleAxis[3];
ceres::RotationMatrixToAngleAxis((const double*)R.data(), angleAxis);
map_poses[indexPose].reserve(6); //angleAxis + translation
map_poses[indexPose].push_back(angleAxis[0]);
map_poses[indexPose].push_back(angleAxis[1]);
map_poses[indexPose].push_back(angleAxis[2]);
map_poses[indexPose].push_back(t(0));
map_poses[indexPose].push_back(t(1));
map_poses[indexPose].push_back(t(2));
double * parameter_block = &map_poses[indexPose][0];
problem.AddParameterBlock(parameter_block, 6);
if (!bRefineTranslations && !bRefineIntrinsics)
{
//set the whole parameter block as constant for best performance.
problem.SetParameterBlockConstant(parameter_block);
}
else {
// Subset parametrization
std::vector<int> vec_constant_extrinsic;
if(!bRefineRotations)
{
vec_constant_extrinsic.push_back(0);
vec_constant_extrinsic.push_back(1);
vec_constant_extrinsic.push_back(2);
}
if(!bRefineTranslations)
{
vec_constant_extrinsic.push_back(3);
vec_constant_extrinsic.push_back(4);
vec_constant_extrinsic.push_back(5);
}
if (!vec_constant_extrinsic.empty())
{
ceres::SubsetParameterization *subset_parameterization =
new ceres::SubsetParameterization(6, vec_constant_extrinsic);
problem.SetParameterization(parameter_block, subset_parameterization);
}
}
}
// Setup Intrinsics data & subparametrization
for (Intrinsics::const_iterator itIntrinsic = sfm_data.intrinsics.begin();
itIntrinsic != sfm_data.intrinsics.end(); ++itIntrinsic)
{
const IndexT indexCam = itIntrinsic->first;
if (isValid(itIntrinsic->second->getType()))
{
map_intrinsics[indexCam] = itIntrinsic->second->getParams();
double * parameter_block = &map_intrinsics[indexCam][0];
problem.AddParameterBlock(parameter_block, map_intrinsics[indexCam].size());
if (!bRefineIntrinsics)
{
//set the whole parameter block as constant for best performance.
problem.SetParameterBlockConstant(parameter_block);
}
}
else
{
std::cerr << "Unsupported camera type." << std::endl;
}
}
// Set a LossFunction to be less penalized by false measurements
// - set it to NULL if you don't want use a lossFunction.
ceres::LossFunction * p_LossFunction = new ceres::HuberLoss(Square(4.0));
// TODO: make the LOSS function and the parameter an option
// For all visibility add reprojections errors:
for (Landmarks::iterator iterTracks = sfm_data.structure.begin();
iterTracks!= sfm_data.structure.end(); ++iterTracks)
{
const Observations & obs = iterTracks->second.obs;
for (Observations::const_iterator itObs = obs.begin();
itObs != obs.end(); ++itObs)
{
// Build the residual block corresponding to the track observation:
const View * view = sfm_data.views[itObs->first].get();
// Each Residual block takes a point and a camera as input and outputs a 2
// dimensional residual. Internally, the cost function stores the observed
// image location and compares the reprojection against the observation.
ceres::CostFunction* cost_function =
IntrinsicsToCostFunction(sfm_data.intrinsics[view->id_intrinsic].get(), itObs->second.x);
if (cost_function)
problem.AddResidualBlock(cost_function,
p_LossFunction,
&map_intrinsics[view->id_intrinsic][0],
&map_poses[view->id_pose][0],
iterTracks->second.X.data()); //Do we need to copy 3D point to avoid false motion, if failure ?
}
if (!bRefineStructure)
problem.SetParameterBlockConstant(iterTracks->second.X.data());
}
// Configure a BA engine and run it
// Make Ceres automatically detect the bundle structure.
ceres::Solver::Options options;
options.preconditioner_type = _openMVG_options._preconditioner_type;
options.linear_solver_type = _openMVG_options._linear_solver_type;
options.sparse_linear_algebra_library_type = _openMVG_options._sparse_linear_algebra_library_type;
options.minimizer_progress_to_stdout = false;
options.logging_type = ceres::SILENT;
options.num_threads = _openMVG_options._nbThreads;
options.num_linear_solver_threads = _openMVG_options._nbThreads;
// Solve BA
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
if (_openMVG_options._bCeres_Summary)
std::cout << summary.FullReport() << std::endl;
// If no error, get back refined parameters
if (!summary.IsSolutionUsable())
{
if (_openMVG_options._bVerbose)
std::cout << "Bundle Adjustment failed." << std::endl;
return false;
}
else // Solution is usable
{
if (_openMVG_options._bVerbose)
{
// Display statistics about the minimization
std::cout << std::endl
<< "Bundle Adjustment statistics (approximated RMSE):\n"
<< " #views: " << sfm_data.views.size() << "\n"
<< " #poses: " << sfm_data.poses.size() << "\n"
<< " #intrinsics: " << sfm_data.intrinsics.size() << "\n"
<< " #tracks: " << sfm_data.structure.size() << "\n"
<< " #residuals: " << summary.num_residuals << "\n"
<< " Initial RMSE: " << std::sqrt( summary.initial_cost / summary.num_residuals) << "\n"
<< " Final RMSE: " << std::sqrt( summary.final_cost / summary.num_residuals) << "\n"
<< std::endl;
}
// Update camera poses with refined data
for (Poses::iterator itPose = sfm_data.poses.begin();
itPose != sfm_data.poses.end(); ++itPose)
{
const IndexT indexPose = itPose->first;
Mat3 R_refined;
ceres::AngleAxisToRotationMatrix(&map_poses[indexPose][0], R_refined.data());
Vec3 t_refined(map_poses[indexPose][3], map_poses[indexPose][4], map_poses[indexPose][5]);
// Update the pose
Pose3 & pose = itPose->second;
pose = Pose3(R_refined, -R_refined.transpose() * t_refined);
}
// Update camera intrinsics with refined data
for (Intrinsics::iterator itIntrinsic = sfm_data.intrinsics.begin();
itIntrinsic != sfm_data.intrinsics.end(); ++itIntrinsic)
{
const IndexT indexCam = itIntrinsic->first;
const std::vector<double> & vec_params = map_intrinsics[indexCam];
itIntrinsic->second.get()->updateFromParams(vec_params);
}
return true;
}
}
/// Save SfM_Data in an ASCII BAF (Bundle Adjustment File).
// --Header
// #Intrinsics
// #Poses
// #Landmarks
// --Data
// Intrinsic parameters [foc ppx ppy, ...]
// Poses [angle axis, camera center]
// Landmarks [X Y Z #observations id_intrinsic id_pose x y ...]
//--
//- Export also a _imgList.txt file with View filename and id_intrinsic & id_pose.
// filename id_intrinsic id_pose
// The ids allow to establish a link between 3D point observations & the corresponding views
//--
// Export missing poses as Identity pose to keep tracking of the original id_pose indexes
static bool Save_BAF(
const SfM_Data & sfm_data,
const std::string & filename,
ESfM_Data flags_part)
{
std::ofstream stream(filename.c_str());
if (!stream.is_open())
return false;
bool bOk = false;
{
stream
<< sfm_data.GetIntrinsics().size() << '\n'
<< sfm_data.GetViews().size() << '\n'
<< sfm_data.GetLandmarks().size() << '\n';
const Intrinsics & intrinsics = sfm_data.GetIntrinsics();
for (Intrinsics::const_iterator iterIntrinsic = intrinsics.begin();
iterIntrinsic != intrinsics.end(); ++iterIntrinsic)
{
//get params
const std::vector<double> intrinsicsParams = iterIntrinsic->second.get()->getParams();
std::copy(intrinsicsParams.begin(), intrinsicsParams.end(),
std::ostream_iterator<double>(stream, " "));
stream << '\n';
}
const Poses & poses = sfm_data.GetPoses();
for (Views::const_iterator iterV = sfm_data.GetViews().begin();
iterV != sfm_data.GetViews().end();
++ iterV)
{
const View * view = iterV->second.get();
if (!sfm_data.IsPoseAndIntrinsicDefined(view))
{
const Mat3 R = Mat3::Identity();
const double * rotation = R.data();
std::copy(rotation, rotation+9, std::ostream_iterator<double>(stream, " "));
const Vec3 C = Vec3::Zero();
const double * center = C.data();
std::copy(center, center+3, std::ostream_iterator<double>(stream, " "));
stream << '\n';
}
else
{
// [Rotation col major 3x3; camera center 3x1]
const double * rotation = poses.at(view->id_pose).rotation().data();
std::copy(rotation, rotation+9, std::ostream_iterator<double>(stream, " "));
const double * center = poses.at(view->id_pose).center().data();
std::copy(center, center+3, std::ostream_iterator<double>(stream, " "));
stream << '\n';
}
}
const Landmarks & landmarks = sfm_data.GetLandmarks();
for (Landmarks::const_iterator iterLandmarks = landmarks.begin();
iterLandmarks != landmarks.end();
++iterLandmarks)
{
// Export visibility information
// X Y Z #observations id_cam id_pose x y ...
const double * X = iterLandmarks->second.X.data();
std::copy(X, X+3, std::ostream_iterator<double>(stream, " "));
const Observations & obs = iterLandmarks->second.obs;
stream << obs.size() << " ";
for (Observations::const_iterator iterOb = obs.begin();
iterOb != obs.end(); ++iterOb)
{
const IndexT id_view = iterOb->first;
const View * v = sfm_data.GetViews().at(id_view).get();
stream
<< v->id_intrinsic << ' '
<< v->id_pose << ' '
<< iterOb->second.x(0) << ' ' << iterOb->second.x(1) << ' ';
}
stream << '\n';
}
stream.flush();
bOk = stream.good();
stream.close();
}
// Export View filenames & ids as an imgList.txt file
{
const std::string sFile = stlplus::create_filespec(
stlplus::folder_part(filename), stlplus::basename_part(filename) + std::string("_imgList"), "txt");
stream.open(sFile.c_str());
if (!stream.is_open())
return false;
for (Views::const_iterator iterV = sfm_data.GetViews().begin();
iterV != sfm_data.GetViews().end();
++ iterV)
{
const std::string sView_filename = stlplus::create_filespec(sfm_data.s_root_path,
iterV->second->s_Img_path);
stream
<< sView_filename
<< ' ' << iterV->second->id_intrinsic
<< ' ' << iterV->second->id_pose << "\n";
}
stream.flush();
bOk = stream.good();
stream.close();
}
return bOk;
}
int main(int argc,char* argv[]) {
if (argc < 4) {
printf("./match_g2o pose_stamped.txt key.match map_point.txt\n");
return 1;
}
srand(time(NULL));
Rcl << 1,0,0,0,0,1,0,-1,0;
Tcl << 0,0.06,0;
//read pose file
FILE* pose_stamped = fopen(argv[1],"r");
if (!pose_stamped)
return 1;
char buffer[2048];
std::vector<Mat3> rotations;
std::vector<Eigen::Vector3d> translations;
while (fgets(buffer,2048,pose_stamped)) {
double t,x,y,z,qx,qy,qz,qw;
if (sscanf(buffer,"%lf %lf %lf %lf %lf %lf %lf %lf",&t,&x,&y,&z,&qw,&qx,&qy,&qz)==8) {
double r[9];
quaternionToRotation(qx,qy,qz,qw,r);
Mat3 Rwl;
memcpy(Rwl.data(),r,9*sizeof(double));
Eigen::Vector3d Twl(x,y,z);
rotations.push_back(Rcl * Rwl.transpose());
translations.push_back(- Rcl * Rwl.transpose() * Twl + Tcl);
} else {
printf("Error parsing: %s\n",buffer);
}
}
fclose(pose_stamped);
struct timespec start,end;
clock_gettime(CLOCK_MONOTONIC,&start);
int count_points = 0;
double RMSE = 0;
FILE* key_match = fopen(argv[2],"r");
FILE* map_point = fopen(argv[3],"w");
if (!(key_match && map_point))
return 1;
while (fgets(buffer,2048,key_match)) {
#if DEBUG_SINGLE
printf("key.match: %s",buffer);
#endif
int id;
char* tok = strtok(buffer," ");
std::vector<double> uc,vc;
std::vector<int> index;
while (tok) {
id = atoi(tok);
index.push_back(id);
tok = strtok(NULL," \n");
double u = atof(tok);
tok = strtok(NULL," \n");
double v = atof(tok);
tok = strtok(NULL," \n");
uc.push_back(u - cx);
vc.push_back(cy - v);
}
//optimize
Eigen::Vector3d bestEstimate, x1, x2;
double leastError = -1;
for (unsigned int i=0;i<index.size()-1;i++) {
for (unsigned int j=i+1;j<index.size();j++) {
double u1 = uc[i], v1 = vc[i];
double u2 = uc[j], v2 = vc[j];
Mat3 R1 = rotations[index[i]], R2 = rotations[index[j]];
Eigen::Vector3d T1 = translations[index[i]], T2 = translations[index[j]];
Mat3 Rcc = R2 * R1.transpose();
Eigen::Vector3d Tcc = T1 - R1 * R2.transpose() * T2;
Eigen::Vector3d r1 = Rcc.row(0), r2 = Rcc.row(1), r3 = Rcc.row(2);
Eigen::Vector3d uv(-u1/fx,-v1/fy,1);
Eigen::Vector3d mult_u = r1 + u2/fx * r3;
Eigen::Vector3d mult_v = r2 + v2/fy * r3;
double z_est[2] = {mult_u.dot(Tcc) / mult_u.dot(uv),
mult_v.dot(Tcc) / mult_v.dot(uv) } ;
for (int k=0;k<1;k++) {
x1 << -u1*z_est[k]/fx, -v1*z_est[k]/fy, z_est[k];
x2 = Rcc * (x1 - Tcc);
if (x1(2) >= 0 || x2(2) >= 0)
break;
double u_est = -fx * x2(0) / x2(2);
double v_est = -fy * x2(1) / x2(2);
double error = (u_est-u2) * (u_est-u2) + (v_est-v2) * (v_est-v2);
if (leastError < 0 || error < leastError) {
leastError = error;
bestEstimate = R1.transpose() * (x1 - T1);
}
}
}
}
//record result
RMSE += leastError;
#if DEBUG_SINGLE
printf("reprojection: ");
char* c = buffer;
c += sprintf(c,"transformation:\n");
for (unsigned int i=0;i<index.size();i++) {
id = index[i];
Eigen::Vector3d xc = rotations[id] * bestEstimate + translations[id];
double u = - fx * xc(0) / xc(2);
double v = - fy * xc(1) / xc(2);
printf("%d %f %f ",id,u+cx,cy-v);
c += sprintf(c,"%4.2f %4.2f %4.2f\n%4.2f %4.2f %4.2f\n%4.2f %4.2f %4.2f\n",
rotations[id](0,0),rotations[id](0,1),rotations[id](0,2),
rotations[id](1,0),rotations[id](1,1),rotations[id](1,2),
rotations[id](2,0),rotations[id](2,1),rotations[id](2,2));
c += sprintf(c,"[%4.2f %4.2f %4.2f]\n",translations[id](0),translations[id](1),translations[id](2));
}
printf("\n%s",buffer);
printf("estimate: %f %f %f %lu %f\n",bestEstimate(0),bestEstimate(1),bestEstimate(2),index.size(),leastError);
#endif
fprintf(map_point,"%f %f %f %lu %f\n",bestEstimate(0),bestEstimate(1),bestEstimate(2),index.size(),leastError);
count_points++;
}
clock_gettime(CLOCK_MONOTONIC,&end);
double dt = end.tv_sec - start.tv_sec + 0.000000001 * (end.tv_nsec - start.tv_nsec);
RMSE = sqrt(RMSE / count_points);
printf("Optimized %d map points (%fs, RMSE = %f)\n",count_points, dt, RMSE);
fclose(key_match);
fclose(map_point);
return 0;
}
