// A feedback functor, which is called on each iteration by the optimizer to let // us know on the progress: void my_BundleAdjustmentFeedbackFunctor( const size_t cur_iter, const double cur_total_sq_error, const size_t max_iters, const TSequenceFeatureObservations& input_observations, const TFramePosesVec& current_frame_estimate, const TLandmarkLocationsVec& current_landmark_estimate) { const double avr_err = std::sqrt(cur_total_sq_error / input_observations.size()); history_avr_err.push_back(std::log(1e-100 + avr_err)); if ((cur_iter % 10) == 0) { cout << "[PROGRESS] Iter: " << cur_iter << " avrg err in px: " << avr_err << endl; cout.flush(); } }
// ------------------------------------------------------ // bundle_adj_full_demo // ------------------------------------------------------ void bundle_adj_full_demo( const TCamera& camera_params, const TSequenceFeatureObservations& allObs, TFramePosesVec& frame_poses, TLandmarkLocationsVec& landmark_points) { cout << "Optimizing " << allObs.size() << " feature observations.\n"; TParametersDouble extra_params; // extra_params["verbose"] = 1; extra_params["max_iterations"] = 2000; // 250; // extra_params["num_fix_frames"] = 1; // extra_params["num_fix_points"] = 0; extra_params["robust_kernel"] = 0; extra_params["kernel_param"] = 5.0; extra_params["profiler"] = 1; mrpt::vision::bundle_adj_full( allObs, camera_params, frame_poses, landmark_points, extra_params, &my_BundleAdjustmentFeedbackFunctor); }
// ------------------------------------------------------ // MAIN // ------------------------------------------------------ int main(int argc, char** argv) { try { // Simulation or real-data? (read at the top of this file): if ((argc != 1 && argc != 3 && argc != 4) || (argc == 2 && !strcpy(argv[1], "--help"))) { cout << "Usage:\n" << argv[0] << " --help -> Shows this help\n" << argv[0] << " -> Simulation\n" << argv[0] << " <feats.txt> <cam_model.cfg> -> Data in MRPT format\n" << argv[0] << " <cams.txt> <points.cfg> <calib.txt> -> SBA format\n"; return 1; } // BA data: TCamera camera_params; TSequenceFeatureObservations allObs; TFramePosesVec frame_poses; TLandmarkLocationsVec landmark_points; // Only for simulation mode: TFramePosesVec frame_poses_real, frame_poses_noisy; // Ground truth & starting point TLandmarkLocationsVec landmark_points_real, landmark_points_noisy; // Ground truth & starting point if (argc == 1) { random::CRandomGenerator rg(1234); // Simulation // -------------------------- // The projective camera model: camera_params.ncols = 800; camera_params.nrows = 600; camera_params.fx(400); camera_params.fy(400); camera_params.cx(400); camera_params.cy(300); // Generate synthetic dataset: // ------------------------------------- const size_t nPts = 100; // # of 3D landmarks const double L1 = 60; // Draw random poses in the rectangle L1xL2xL3 const double L2 = 10; const double L3 = 10; const double max_camera_dist = L1 * 4; const double cameraPathLen = L1 * 1.2; // const double cameraPathEllipRadius1 = L1*2; // const double cameraPathEllipRadius2 = L2*2; // Noise params: const double STD_PX_ERROR = 0.10; // pixels const double STD_PX_ERROR_OUTLIER = 5; // pixels const double PROBABILITY_OUTLIERS = 0; // 0.01; const double STD_PT3D = 0.10; // meters const double STD_CAM_XYZ = 0.05; // meters const double STD_CAM_ANG = DEG2RAD(5); // degs landmark_points_real.resize(nPts); for (size_t i = 0; i < nPts; i++) { landmark_points_real[i].x = rg.drawUniform(-L1, L1); landmark_points_real[i].y = rg.drawUniform(-L2, L2); landmark_points_real[i].z = rg.drawUniform(-L3, L3); } // const double angStep = M_PI*2.0/40; const double camPosesSteps = 2 * cameraPathLen / 20; frame_poses_real.clear(); for (double x = -cameraPathLen; x < cameraPathLen; x += camPosesSteps) { TPose3D p; p.x = x; // cameraPathEllipRadius1 * cos(ang); p.y = 4 * L2; // cameraPathEllipRadius2 * sin(ang); p.z = 0; p.yaw = DEG2RAD(-90) - DEG2RAD(30) * x / cameraPathLen; // wrapToPi(ang+M_PI); p.pitch = 0; p.roll = 0; // Angles above is for +X pointing to the (0,0,0), but we want // instead +Z pointing there, as typical in camera models: frame_poses_real.push_back( CPose3D(p) + CPose3D(0, 0, 0, DEG2RAD(-90), 0, DEG2RAD(-90))); } // Simulate the feature observations: size_t numOutliers = 0; allObs.clear(); map<TCameraPoseID, size_t> numViewedFrom; for (size_t i = 0; i < frame_poses_real.size(); i++) // for each pose { // predict all landmarks: for (size_t j = 0; j < landmark_points_real.size(); j++) { TPixelCoordf px = mrpt::vision::pinhole::projectPoint_no_distortion< false>( camera_params, frame_poses_real[i], landmark_points_real[j]); const bool is_outlier = (rg.drawUniform(0.0, 1.0) < PROBABILITY_OUTLIERS); px.x += rg.drawGaussian1D( 0, is_outlier ? STD_PX_ERROR_OUTLIER : STD_PX_ERROR); px.y += rg.drawGaussian1D( 0, is_outlier ? STD_PX_ERROR_OUTLIER : STD_PX_ERROR); // Out of image? if (px.x < 0 || px.y < 0 || px.x > camera_params.ncols || px.y > camera_params.nrows) continue; // Too far? const double dist = math::distance( TPoint3D(frame_poses_real[i].asTPose()), landmark_points_real[j]); if (dist > max_camera_dist) continue; // Ok, accept it: if (is_outlier) numOutliers++; allObs.push_back(TFeatureObservation(j, i, px)); numViewedFrom[i]++; } } cout << "Simulated: " << allObs.size() << " observations (of which: " << numOutliers << " are outliers).\n"; ASSERT_EQUAL_(numViewedFrom.size(), frame_poses_real.size()); // Make sure all poses and all LMs appear at least once! { TSequenceFeatureObservations allObs2 = allObs; std::map<TCameraPoseID, TCameraPoseID> old2new_camIDs; std::map<TLandmarkID, TLandmarkID> old2new_lmIDs; allObs2.compressIDs(&old2new_camIDs, &old2new_lmIDs); ASSERT_EQUAL_(old2new_camIDs.size(), frame_poses_real.size()); ASSERT_EQUAL_( old2new_lmIDs.size(), landmark_points_real.size()); } // Add noise to the data: frame_poses_noisy = frame_poses_real; landmark_points_noisy = landmark_points_real; for (size_t i = 0; i < landmark_points_noisy.size(); i++) landmark_points_noisy[i] += TPoint3D( rg.drawGaussian1D(0, STD_PT3D), rg.drawGaussian1D(0, STD_PT3D), rg.drawGaussian1D(0, STD_PT3D)); for (size_t i = 1; i < frame_poses_noisy.size(); i++) // DON'T add error to frame[0], the global reference! { CPose3D bef = frame_poses_noisy[i]; frame_poses_noisy[i].setFromValues( frame_poses_noisy[i].x() + rg.drawGaussian1D(0, STD_CAM_XYZ), frame_poses_noisy[i].y() + rg.drawGaussian1D(0, STD_CAM_XYZ), frame_poses_noisy[i].z() + rg.drawGaussian1D(0, STD_CAM_XYZ), frame_poses_noisy[i].yaw() + rg.drawGaussian1D(0, STD_CAM_ANG), frame_poses_noisy[i].pitch() + rg.drawGaussian1D(0, STD_CAM_ANG), frame_poses_noisy[i].roll() + rg.drawGaussian1D(0, STD_CAM_ANG)); } // Optimize it: frame_poses = frame_poses_noisy; landmark_points = landmark_points_noisy; #if 0 vector<std::array<double,2> > resids; const double initial_total_sq_err = mrpt::vision::reprojectionResiduals(allObs,camera_params,frame_poses, landmark_points,resids, false); cout << "Initial avr error in px: " << std::sqrt(initial_total_sq_err/allObs.size()) << endl; #endif // Run Bundle Adjustmen bundle_adj_full_demo( camera_params, allObs, frame_poses, landmark_points); // Evaluate vs. ground truth: double landmarks_total_sq_err = 0; for (size_t i = 0; i < landmark_points.size(); i++) landmarks_total_sq_err += square( landmark_points_real[i].distanceTo(landmark_points[i])); double cam_point_total_sq_err = 0; for (size_t i = 0; i < frame_poses.size(); i++) cam_point_total_sq_err += square(frame_poses[i].distanceTo(frame_poses_real[i])); cout << "RMSE of recovered landmark positions: " << std::sqrt(landmarks_total_sq_err / landmark_points.size()) << endl; cout << "RMSE of recovered camera positions: " << std::sqrt(cam_point_total_sq_err / frame_poses.size()) << endl; } else { // Real data // -------------------------- if (argc == 3) { const string feats_fil = string(argv[1]); const string cam_fil = string(argv[2]); cout << "Loading observations from: " << feats_fil << "..."; cout.flush(); allObs.loadFromTextFile(feats_fil); cout << "Done.\n"; allObs.decimateCameraFrames(20); allObs.compressIDs(); ASSERT_(mrpt::system::fileExists(cam_fil)); cout << "Loading camera params from: " << cam_fil; mrpt::config::CConfigFile cfgCam(cam_fil); camera_params.loadFromConfigFile("CAMERA", cfgCam); cout << "Done.\n"; cout << "Initial gross estimate..."; mrpt::vision::ba_initial_estimate( allObs, camera_params, frame_poses, landmark_points); cout << "OK\n"; } else { // Load data from 3 files in the same format as used by // "eucsbademo" in the SBA library: const string cam_frames_fil = string(argv[1]); const string obs_fil = string(argv[2]); const string calib_fil = string(argv[3]); { cout << "Loading initial camera frames from: " << cam_frames_fil << "..."; cout.flush(); mrpt::io::CTextFileLinesParser fil(cam_frames_fil); frame_poses.clear(); std::istringstream ss; while (fil.getNextLine(ss)) { double q[4], t[3]; ss >> q[0] >> q[1] >> q[2] >> q[3] >> t[0] >> t[1] >> t[2]; mrpt::poses::CPose3DQuat p( t[0], t[1], t[2], mrpt::math::CQuaternionDouble( q[0], q[1], q[2], q[3])); // cout << "cam: " << p << endl; frame_poses.push_back(CPose3D(p)); } cout << "Done. " << frame_poses.size() << " cam frames loaded\n"; frame_poses_noisy = frame_poses; // To draw in 3D the // initial values as well. } { cout << "Loading observations & feature 3D points from: " << obs_fil << "..."; cout.flush(); mrpt::io::CTextFileLinesParser fil(obs_fil); landmark_points.clear(); allObs.clear(); std::istringstream ss; while (fil.getNextLine(ss)) { // # X Y Z nframes frame0 x0 y0 frame1 x1 y1 ... double t[3]; size_t N = 0; ss >> t[0] >> t[1] >> t[2] >> N; const TLandmarkID feat_id = landmark_points.size(); const TPoint3D pt(t[0], t[1], t[2]); landmark_points.push_back(pt); // Read obs: for (size_t i = 0; i < N; i++) { TCameraPoseID frame_id; TPixelCoordf px; ss >> frame_id >> px.x >> px.y; allObs.push_back( TFeatureObservation(feat_id, frame_id, px)); // cout << "feat: " << feat_id << " cam: " << // frame_id << " px: " << px.x << "," << px.y << // endl; } } cout << "Done. " << landmark_points.size() << " points, " << allObs.size() << " observations read.\n"; landmark_points_real = landmark_points; // To draw in 3D // the initial // values as well. } CMatrixDouble33 cam_pars; cam_pars.loadFromTextFile(calib_fil); // cout << "Calib:\n" << cam_pars << endl; camera_params.fx(cam_pars(0, 0)); camera_params.fy(cam_pars(1, 1)); camera_params.cx(cam_pars(0, 2)); camera_params.cy(cam_pars(1, 2)); cout << "camera calib:\n" << camera_params.dumpAsText() << endl; // Change world scale: WORLD_SCALE = 2000; } // Do it: bundle_adj_full_demo( camera_params, allObs, frame_poses, landmark_points); } // Display results in 3D: // ------------------------------- gui::CDisplayWindow3D win("Bundle adjustment demo", 800, 600); COpenGLScene::Ptr& scene = win.get3DSceneAndLock(); { // Ground plane: CGridPlaneXY::Ptr obj = mrpt::make_aligned_shared<CGridPlaneXY>( -200, 200, -200, 200, 0, 5); obj->setColor(0.7, 0.7, 0.7); scene->insert(obj); } if (!landmark_points_real.empty()) { // Feature points: ground truth CPointCloud::Ptr obj = mrpt::make_aligned_shared<CPointCloud>(); obj->setPointSize(2); obj->setColor(0, 0, 0); obj->loadFromPointsList(landmark_points_real); obj->setScale(WORLD_SCALE); scene->insert(obj); } if (!landmark_points_noisy.empty()) { // Feature points: noisy CPointCloud::Ptr obj = mrpt::make_aligned_shared<CPointCloud>(); obj->setPointSize(4); obj->setColor(0.7, 0.2, 0.2, 0); obj->loadFromPointsList(landmark_points_noisy); obj->setScale(WORLD_SCALE); scene->insert(obj); } { // Feature points: estimated CPointCloud::Ptr obj = mrpt::make_aligned_shared<CPointCloud>(); obj->setPointSize(3); obj->setColor(0, 0, 1, 1.0); obj->loadFromPointsList(landmark_points); obj->setScale(WORLD_SCALE); scene->insert(obj); } // Camera Frames: estimated scene->insert(framePosesVecVisualize(frame_poses_noisy, 1.0, 1)); scene->insert(framePosesVecVisualize(frame_poses_real, 2.0, 1)); scene->insert(framePosesVecVisualize(frame_poses, 2.0, 3)); win.setCameraZoom(100); win.unlockAccess3DScene(); win.repaint(); // Also, show history of error: gui::CDisplayWindowPlots winPlot( "Avr log-error with iterations", 500, 200); // winPlot.setPos(0,620); winPlot.plot(history_avr_err, "b.3"); winPlot.axis_fit(); cout << "Close the 3D window or press a key to exit.\n"; win.waitForKey(); return 0; } catch (std::exception& e) { std::cout << "MRPT exception caught: " << e.what() << std::endl; return -1; } catch (...) { printf("Untyped exception!!"); return -1; } }
/* ---------------------------------------------------------- bundle_adj_full See bundle_adjustment.h for docs. This implementation is almost entirely an adapted version from RobotVision (at OpenSLAM.org) (C) by Hauke Strasdat (Imperial College London), licensed under GNU LGPL. See the related paper: H. Strasdat, J.M.M. Montiel, A.J. Davison: "Scale Drift-Aware Large Scale Monocular SLAM", RSS2010, http://www.roboticsproceedings.org/rss06/p10.html ---------------------------------------------------------- */ double mrpt::vision::bundle_adj_full( const TSequenceFeatureObservations & observations, const TCamera & camera_params, TFramePosesVec & frame_poses, TLandmarkLocationsVec & landmark_points, const mrpt::utils::TParametersDouble & extra_params, const TBundleAdjustmentFeedbackFunctor user_feedback ) { MRPT_START // Generic BA problem dimension numbers: static const unsigned int FrameDof = 6; // Poses: x y z yaw pitch roll static const unsigned int PointDof = 3; // Landmarks: x y z static const unsigned int ObsDim = 2; // Obs: x y (pixels) // Typedefs for this specific BA problem: typedef JacData<FrameDof,PointDof,ObsDim> MyJacData; typedef aligned_containers<MyJacData>::vector_t MyJacDataVec; typedef CArray<double,ObsDim> Array_O; typedef CArrayDouble<FrameDof> Array_F; typedef CArrayDouble<PointDof> Array_P; typedef CMatrixFixedNumeric<double,FrameDof,FrameDof> Matrix_FxF; typedef CMatrixFixedNumeric<double,PointDof,PointDof> Matrix_PxP; typedef CMatrixFixedNumeric<double,FrameDof,PointDof> Matrix_FxP; // Extra params: const bool use_robust_kernel = 0!=extra_params.getWithDefaultVal("robust_kernel",1); const bool verbose = 0!=extra_params.getWithDefaultVal("verbose",0); const double initial_mu = extra_params.getWithDefaultVal("mu",-1); const size_t max_iters = extra_params.getWithDefaultVal("max_iterations",50); const size_t num_fix_frames = extra_params.getWithDefaultVal("num_fix_frames",1); const size_t num_fix_points = extra_params.getWithDefaultVal("num_fix_points",0); const double kernel_param = extra_params.getWithDefaultVal("kernel_param",3.0); const bool enable_profiler = 0!=extra_params.getWithDefaultVal("profiler",0); mrpt::utils::CTimeLogger profiler(enable_profiler); profiler.enter("bundle_adj_full (complete run)"); // Input data sizes: const size_t num_points = landmark_points.size(); const size_t num_frames = frame_poses.size(); const size_t num_obs = observations.size(); ASSERT_ABOVE_(num_frames,0) ASSERT_ABOVE_(num_points,0) ASSERT_(num_fix_frames>=1) ASSERT_ABOVEEQ_(num_frames,num_fix_frames); ASSERT_ABOVEEQ_(num_points,num_fix_points); #ifdef USE_INVERSE_POSES // *Warning*: This implementation assumes inverse camera poses: inverse them at the entrance and at exit: profiler.enter("invert_poses"); for (size_t i=0;i<num_frames;i++) frame_poses[i].inverse(); profiler.leave("invert_poses"); #endif MyJacDataVec jac_data_vec(num_obs); vector<Array_O> residual_vec(num_obs); vector<double> kernel_1st_deriv(num_obs); // prepare structure of sparse Jacobians: for (size_t i=0; i<num_obs; i++) { jac_data_vec[i].frame_id = observations[i].id_frame; jac_data_vec[i].point_id = observations[i].id_feature; } // Compute sparse Jacobians: profiler.enter("compute_Jacobians"); ba_compute_Jacobians<INV_POSES_BOOL>(frame_poses, landmark_points, camera_params, jac_data_vec, num_fix_frames, num_fix_points); profiler.leave("compute_Jacobians"); profiler.enter("reprojectionResiduals"); double res = mrpt::vision::reprojectionResiduals( observations, camera_params, frame_poses, landmark_points, residual_vec, INV_POSES_BOOL, // are poses inverse? use_robust_kernel, kernel_param, use_robust_kernel ? &kernel_1st_deriv : NULL ); profiler.leave("reprojectionResiduals"); MRPT_CHECK_NORMAL_NUMBER(res) VERBOSE_COUT << "res: " << res << endl; // Auxiliary vars: Array_F arrF_zeros; arrF_zeros.assign(0); Array_P arrP_zeros; arrP_zeros.assign(0); const size_t num_free_frames = num_frames-num_fix_frames; const size_t num_free_points = num_points-num_fix_points; const size_t len_free_frames = FrameDof * num_free_frames; const size_t len_free_points = PointDof * num_free_points; aligned_containers<Matrix_FxF>::vector_t H_f (num_free_frames); aligned_containers<Array_F>::vector_t eps_frame (num_free_frames, arrF_zeros); aligned_containers<Matrix_PxP>::vector_t H_p (num_free_points); aligned_containers<Array_P>::vector_t eps_point (num_free_points, arrP_zeros); profiler.enter("build_gradient_Hessians"); ba_build_gradient_Hessians(observations,residual_vec,jac_data_vec, H_f,eps_frame,H_p,eps_point, num_fix_frames, num_fix_points, use_robust_kernel ? &kernel_1st_deriv : NULL ); profiler.leave("build_gradient_Hessians"); double nu = 2; double eps = 1e-16; // 0.000000000000001; bool stop = false; double mu = initial_mu; // Automatic guess of "mu": if (mu<0) { double norm_max_A = 0; for (size_t j=0; j<num_free_frames; ++j) for (size_t dim=0; dim<FrameDof; dim++) keep_max(norm_max_A, H_f[j](dim,dim) ); for (size_t i=0; i<num_free_points; ++i) for (size_t dim=0; dim<PointDof; dim++) keep_max(norm_max_A, H_p[i](dim,dim) ); double tau = 1e-3; mu = tau*norm_max_A; } Matrix_FxF I_muFrame(UNINITIALIZED_MATRIX); Matrix_PxP I_muPoint(UNINITIALIZED_MATRIX); // Cholesky object, as a pointer to reuse it between iterations: #if MRPT_HAS_CXX11 typedef std::unique_ptr<CSparseMatrix::CholeskyDecomp> SparseCholDecompPtr; #else typedef std::auto_ptr<CSparseMatrix::CholeskyDecomp> SparseCholDecompPtr; #endif SparseCholDecompPtr ptrCh; for (size_t iter=0; iter<max_iters; iter++) { VERBOSE_COUT << "iteration: "<< iter << endl; // provide feedback to the user: if (user_feedback) (*user_feedback)(iter, res, max_iters, observations, frame_poses, landmark_points ); bool has_improved = false; do { profiler.enter("COMPLETE_ITER"); VERBOSE_COUT << "mu: " <<mu<< endl; I_muFrame.unit(FrameDof,mu); I_muPoint.unit(PointDof,mu); aligned_containers<Matrix_FxF>::vector_t U_star(num_free_frames, I_muFrame); aligned_containers<Matrix_PxP>::vector_t V_inv (num_free_points); for (size_t i=0; i<U_star.size(); ++i) U_star[i] += H_f[i]; for (size_t i=0; i<H_p.size(); ++i) (H_p[i]+I_muPoint).inv_fast( V_inv[i] ); typedef aligned_containers<pair<TCameraPoseID,TLandmarkID>,Matrix_FxP>::map_t WMap; WMap W,Y; // For quick look-up of entries in W affecting a given point ID: vector<vector<WMap::iterator> > W_entries(num_points); // Index is "TLandmarkID" MyJacDataVec::const_iterator jac_iter = jac_data_vec.begin(); for (TSequenceFeatureObservations::const_iterator it_obs=observations.begin(); it_obs!=observations.end(); ++it_obs) { const TFeatureID feat_id = it_obs->id_feature; const TCameraPoseID frame_id = it_obs->id_frame; if (jac_iter->J_frame_valid && jac_iter->J_point_valid) { const CMatrixFixedNumeric<double,ObsDim,FrameDof> & J_frame = jac_iter->J_frame; const CMatrixFixedNumeric<double,ObsDim,PointDof> & J_point = jac_iter->J_point; const pair<TCameraPoseID,TLandmarkID> id_pair = make_pair(frame_id, feat_id); Matrix_FxP tmp(UNINITIALIZED_MATRIX); tmp.multiply_AtB(J_frame, J_point); // W[ids] = J_f^T * J_p // Was: W[id_pair] = tmp; const pair<WMap::iterator,bool> &retInsert = W.insert( make_pair(id_pair,tmp) ); ASSERT_(retInsert.second==true) W_entries[feat_id].push_back(retInsert.first); // Keep the iterator // Y[ids] = W[ids] * H_p^{-1} Y[id_pair].multiply_AB(tmp, V_inv[feat_id-num_fix_points]); } ++jac_iter; } aligned_containers<pair<TCameraPoseID,TLandmarkID>,Matrix_FxF>::map_t YW_map; for (size_t i=0; i<H_f.size(); ++i) YW_map[std::pair<TCameraPoseID,TLandmarkID>(i,i)] = U_star[i]; CVectorDouble delta( len_free_frames + len_free_points ); // The optimal step CVectorDouble e ( len_free_frames ); profiler.enter("Schur.build.reduced.frames"); for (size_t j=0; j<num_free_frames; ++j) ::memcpy( &e[j*FrameDof], &eps_frame[j][0], sizeof(e[0])*FrameDof ); // e.slice(j*FrameDof,FrameDof) = AT(eps_frame,j); for (WMap::iterator Y_ij=Y.begin(); Y_ij!=Y.end(); ++Y_ij) { const TLandmarkID point_id = Y_ij->first.second; const size_t i = Y_ij->first.second - num_fix_points; // point index const size_t j = Y_ij->first.first - num_fix_frames; // frame index const vector<WMap::iterator> &iters = W_entries[point_id]; //->second; for (size_t itIdx=0; itIdx<iters.size(); itIdx++) //for (size_t k=0; k<num_free_frames; ++k) { const WMap::iterator &W_ik = iters[itIdx]; const TLandmarkID k = W_ik->first.first-num_fix_frames; Matrix_FxF YWt(UNINITIALIZED_MATRIX); //-(Y_ij->second) * (W_ik->second).T(); YWt.multiply_ABt( Y_ij->second, W_ik->second ); //YWt*=-1.0; // The "-" sign is taken into account below: const pair<TCameraPoseID,TLandmarkID> ids_jk = pair<TCameraPoseID,TLandmarkID>(j,k); aligned_containers<pair<TCameraPoseID,TLandmarkID>,Matrix_FxF>::map_t::iterator it = YW_map.find(ids_jk); if(it!=YW_map.end()) it->second -= YWt; // += (-YWt); else YW_map[ids_jk] = YWt*(-1.0); } CArrayDouble<FrameDof> r; Y_ij->second.multiply_Ab( eps_point[i] ,r); for (size_t k=0; k<FrameDof; k++) e[j*FrameDof+k]-=r[k]; } profiler.leave("Schur.build.reduced.frames"); profiler.enter("sS:ALL"); profiler.enter("sS:fill"); VERBOSE_COUT << "Entries in YW_map:" << YW_map.size() << endl; CSparseMatrix sS(len_free_frames, len_free_frames); for (aligned_containers<pair<TCameraPoseID,TLandmarkID>,Matrix_FxF>::map_t::const_iterator it= YW_map.begin(); it!=YW_map.end(); ++it) { const pair<TCameraPoseID,TLandmarkID> & ids = it->first; const Matrix_FxF & YW = it->second; sS.insert_submatrix(ids.first*FrameDof,ids.second*FrameDof, YW); } profiler.leave("sS:fill"); // Compress the sparse matrix: profiler.enter("sS:compress"); sS.compressFromTriplet(); profiler.leave("sS:compress"); try { profiler.enter("sS:chol"); if (!ptrCh.get()) ptrCh = SparseCholDecompPtr(new CSparseMatrix::CholeskyDecomp(sS) ); else ptrCh.get()->update(sS); profiler.leave("sS:chol"); profiler.enter("sS:backsub"); CVectorDouble bck_res; ptrCh->backsub(e, bck_res); // Ax = b --> delta= x* ::memcpy(&delta[0],&bck_res[0],bck_res.size()*sizeof(bck_res[0])); // delta.slice(0,...) = Ch.backsub(e); profiler.leave("sS:backsub"); profiler.leave("sS:ALL"); } catch (CExceptionNotDefPos &) { profiler.leave("sS:ALL"); // not positive definite so increase mu and try again mu *= nu; nu *= 2.; stop = (mu>999999999.f); continue; } profiler.enter("PostSchur.landmarks"); CVectorDouble g(len_free_frames+len_free_points); ::memcpy(&g[0],&e[0],len_free_frames*sizeof(g[0])); //g.slice(0,FrameDof*(num_frames-num_fix_frames)) = e; for (size_t i=0; i<num_free_points; ++i) { Array_P tmp = eps_point[i]; // eps_point.slice(PointDof*i,PointDof); for (size_t j=0; j<num_free_frames; ++j) { WMap::iterator W_ij; W_ij = W.find(make_pair<TCameraPoseID,TLandmarkID>(j+num_fix_frames,i+num_fix_points)); if (W_ij!=W.end()) { //tmp -= W_ij->second.T() * delta.slice(j*FrameDof,FrameDof); const Array_F v( &delta[j*FrameDof] ); Array_P r; W_ij->second.multiply_Atb(v, r); // r= A^t * v tmp-=r; } } Array_P Vi_tmp; V_inv[i].multiply_Ab(tmp, Vi_tmp); // Vi_tmp = V_inv[i] * tmp ::memcpy(&delta[len_free_frames + i*PointDof], &Vi_tmp[0], sizeof(Vi_tmp[0])*PointDof ); ::memcpy(&g[len_free_frames + i*PointDof], &eps_point[i][0], sizeof(eps_point[0][0])*PointDof ); } profiler.leave("PostSchur.landmarks"); // Vars for temptative new estimates: TFramePosesVec new_frame_poses; TLandmarkLocationsVec new_landmark_points; profiler.enter("add_se3_deltas_to_frames"); add_se3_deltas_to_frames( frame_poses, delta, 0,len_free_frames, new_frame_poses, num_fix_frames ); profiler.leave("add_se3_deltas_to_frames"); profiler.enter("add_3d_deltas_to_points"); add_3d_deltas_to_points( landmark_points, delta, len_free_frames, len_free_points, new_landmark_points, num_fix_points ); profiler.leave("add_3d_deltas_to_points"); vector<Array_O> new_residual_vec(num_obs); vector<double> new_kernel_1st_deriv(num_obs); profiler.enter("reprojectionResiduals"); double res_new = mrpt::vision::reprojectionResiduals( observations, camera_params, new_frame_poses, new_landmark_points, new_residual_vec, INV_POSES_BOOL, // are poses inverse? use_robust_kernel, kernel_param, use_robust_kernel ? &new_kernel_1st_deriv : NULL ); profiler.leave("reprojectionResiduals"); MRPT_CHECK_NORMAL_NUMBER(res_new) has_improved = (res_new<res); if(has_improved) { //rho = (res-)/ (delta.array()*(mu*delta + g).array() ).sum(); // Good: Accept new values VERBOSE_COUT << "new total sqr.err=" << res_new << " avr.err(px):"<< std::sqrt(res/num_obs) <<"->" << std::sqrt(res_new/num_obs) << endl; // swap is faster than "=" frame_poses.swap(new_frame_poses); landmark_points.swap(new_landmark_points); residual_vec.swap( new_residual_vec ); kernel_1st_deriv.swap( new_kernel_1st_deriv ); res = res_new; profiler.enter("compute_Jacobians"); ba_compute_Jacobians<INV_POSES_BOOL>(frame_poses, landmark_points, camera_params, jac_data_vec, num_fix_frames, num_fix_points); profiler.leave("compute_Jacobians"); // Reset to zeros: H_f.assign(num_free_frames,Matrix_FxF() ); H_p.assign(num_free_points,Matrix_PxP() ); eps_frame.assign(num_free_frames, arrF_zeros); eps_point.assign(num_free_points, arrP_zeros); profiler.enter("build_gradient_Hessians"); ba_build_gradient_Hessians(observations,residual_vec,jac_data_vec,H_f,eps_frame,H_p,eps_point, num_fix_frames, num_fix_points, use_robust_kernel ? &kernel_1st_deriv : NULL ); profiler.leave("build_gradient_Hessians"); stop = norm_inf(g)<=eps; //mu *= max(1.0/3.0, 1-std::pow(2*rho-1,3.0) ); mu *= 0.1; mu = std::max(mu, 1e-100); nu = 2.0; } else { VERBOSE_COUT << "no update: res vs.res_new " << res << " vs. " << res_new << endl; mu *= nu; nu *= 2.0; stop = (mu>1e9); } profiler.leave("COMPLETE_ITER"); } while(!has_improved && !stop); if (stop) break; } // end for each "iter" // *Warning*: This implementation assumes inverse camera poses: inverse them at the entrance and at exit: #ifdef USE_INVERSE_POSES profiler.enter("invert_poses"); for (size_t i=0;i<num_frames;i++) frame_poses[i].inverse(); profiler.leave("invert_poses"); #endif profiler.leave("bundle_adj_full (complete run)"); return res; MRPT_END }