SimpleCamera simpleCamera(const Matrix& P) { // P = [A|a] = s K cRw [I|-T], with s the unknown scale Matrix A = P.topLeftCorner(3, 3); Vector a = P.col(3); // do RQ decomposition to get s*K and cRw angles Matrix sK; Vector xyz; boost::tie(sK, xyz) = RQ(A); // Recover scale factor s and K double s = sK(2, 2); Matrix K = sK / s; // Recover cRw itself, and its inverse Rot3 cRw = Rot3::RzRyRx(xyz); Rot3 wRc = cRw.inverse(); // Now, recover T from a = - s K cRw T = - A T Vector T = -(A.inverse() * a); return SimpleCamera(Pose3(wRc, T), Cal3_S2(K(0, 0), K(1, 1), K(0, 1), K(0, 2), K(1, 2))); }
Pose3 operator () (const Pose3 & pose) const { return Pose3( pose.rotation() * _pose.rotation().transpose(), this->operator()(pose.center())); }
Similarity3(): _pose(Pose3()), _scale(1.0) {}
// Inverse Pose3 inverse() const { return Pose3(_rotation.transpose(), -(_rotation * _center)); }
// Composition Pose3 operator * (const Pose3& P) const { return Pose3(_rotation * P._rotation, P._center + P._rotation.transpose() * _center ); }
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; } }