// 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
}
