From bee16e2d0322377c436c989b0d070bdfa4d9a959 Mon Sep 17 00:00:00 2001 From: Christian Forster Date: Mon, 5 May 2014 15:35:13 +0200 Subject: [PATCH] moved jacobians to point and frame class --- svo/include/svo/frame.h | 26 ++++++++++++++++++++++++++ svo/include/svo/frame_handler_base.h | 4 ++-- svo/include/svo/point.h | 20 +++++++++++++++++--- svo/include/svo/pose_optimizer.h | 27 --------------------------- svo/src/point.cpp | 19 +------------------ svo/src/pose_optimizer.cpp | 8 ++++---- svo/src/sparse_img_align.cpp | 27 ++------------------------- 7 files changed, 52 insertions(+), 79 deletions(-) diff --git a/svo/include/svo/frame.h b/svo/include/svo/frame.h index 8fcfd2c..c926277 100644 --- a/svo/include/svo/frame.h +++ b/svo/include/svo/frame.h @@ -110,6 +110,32 @@ public: /// Return the pose of the frame in the (w)orld coordinate frame. inline Vector3d pos() const { return T_f_w_.inverse().translation(); } + + /// Frame jacobian for projection of 3D point in (f)rame coordinate to + /// unit plane coordinates uv (focal length = 1). + inline static void jacobian_xyz2uv( + const Vector3d& xyz_in_f, + Matrix& J) + { + const double x = xyz_in_f[0]; + const double y = xyz_in_f[1]; + const double z_inv = 1./xyz_in_f[2]; + const double z_inv_2 = z_inv*z_inv; + + J(0,0) = -z_inv; // -1/z + J(0,1) = 0.0; // 0 + J(0,2) = x*z_inv_2; // x/z^2 + J(0,3) = y*J(0,2); // x*y/z^2 + J(0,4) = -(1.0 + x*J(0,2)); // -(1.0 + x^2/z^2) + J(0,5) = y*z_inv; // y/z + + J(1,0) = 0.0; // 0 + J(1,1) = -z_inv; // -1/z + J(1,2) = y*z_inv_2; // y/z^2 + J(1,3) = 1.0 + y*J(1,2); // 1.0 + y^2/z^2 + J(1,4) = -J(0,3); // -x*y/z^2 + J(1,5) = -x*z_inv; // x/z + } }; diff --git a/svo/include/svo/frame_handler_base.h b/svo/include/svo/frame_handler_base.h index ad8dd56..9a78639 100644 --- a/svo/include/svo/frame_handler_base.h +++ b/svo/include/svo/frame_handler_base.h @@ -112,10 +112,10 @@ protected: virtual void resetAll() { resetCommon(); } /// Set the tracking quality based on the number of tracked features. - void setTrackingQuality(const size_t num_observations); + virtual void setTrackingQuality(const size_t num_observations); /// Optimize some of the observed 3D points. - void optimizeStructure(FramePtr frame, size_t max_n_pts, int max_iter); + virtual void optimizeStructure(FramePtr frame, size_t max_n_pts, int max_iter); }; } // namespace nslam diff --git a/svo/include/svo/point.h b/svo/include/svo/point.h index 72a7221..291f79f 100644 --- a/svo/include/svo/point.h +++ b/svo/include/svo/point.h @@ -75,11 +75,25 @@ public: /// Get number of observations. inline size_t nRefs() const { return obs_.size(); } - /// Point jacobian w.r.t frame f. - void pointJacobian(const Vector3d& p_in_f, const Matrix3d& R_f_w, Matrix23d& point_jac) const; - /// Optimize point position through minimizing the reprojection error. void optimize(const size_t n_iter); + + /// Jacobian of point projection on unit plane (focal length = 1) in frame (f). + inline static void jacobian_xyz2uv( + const Vector3d& p_in_f, + const Matrix3d& R_f_w, + Matrix23d& point_jac) + { + const double z_inv = 1.0/p_in_f[2]; + const double z_inv_sq = z_inv*z_inv; + point_jac(0, 0) = z_inv; + point_jac(0, 1) = 0.0; + point_jac(0, 2) = -p_in_f[0] * z_inv_sq; + point_jac(1, 0) = 0.0; + point_jac(1, 1) = z_inv; + point_jac(1, 2) = -p_in_f[1] * z_inv_sq; + point_jac = - point_jac * R_f_w; + } }; } // namespace svo diff --git a/svo/include/svo/pose_optimizer.h b/svo/include/svo/pose_optimizer.h index 575da32..ca048ae 100644 --- a/svo/include/svo/pose_optimizer.h +++ b/svo/include/svo/pose_optimizer.h @@ -34,33 +34,6 @@ class Point; /// Motion-only bundle adjustment. Minimize the reprojection error of a single frame. namespace pose_optimizer { -inline Matrix26d -frameJac(const SE3 & T, - const Vector3d & xyz, - const Vector2d & focal_length) -{ - const Vector3d & xyz_trans = T*xyz; - double x = xyz_trans[0]; - double y = xyz_trans[1]; - double z = xyz_trans[2]; - double z_2 = z*z; - Matrix26d frame_jac; - frame_jac(0,0) = -1./z *focal_length[0]; - frame_jac(0,1) = 0; - frame_jac(0,2) = x/z_2 *focal_length[0]; - frame_jac(0,3) = x*y/z_2 * focal_length[0]; - frame_jac(0,4) = -(1+(x*x/z_2)) *focal_length[0]; - frame_jac(0,5) = y/z *focal_length[0]; - - frame_jac(1,0) = 0; - frame_jac(1,1) = -1./z *focal_length[1]; - frame_jac(1,2) = y/z_2 *focal_length[1]; - frame_jac(1,3) = (1+y*y/z_2) *focal_length[1]; - frame_jac(1,4) = -x*y/z_2 *focal_length[1]; - frame_jac(1,5) = -x/z *focal_length[1]; - return frame_jac; -} - void optimizeGaussNewton( const double reproj_thresh, const size_t n_iter, diff --git a/svo/src/point.cpp b/svo/src/point.cpp index bdb14c5..c79053e 100644 --- a/svo/src/point.cpp +++ b/svo/src/point.cpp @@ -106,23 +106,6 @@ bool Point::getCloseViewObs(const Vector3d& framepos, Feature*& ftr) const return true; } -void Point::pointJacobian( - const Vector3d& p_in_f, - const Matrix3d& R_f_w, - Matrix23d& point_jac) const -{ - // we work on the unit plane, therefore focal lenght = 1 - const double z_inv = 1.0/p_in_f[2]; - const double z_inv_sq = z_inv*z_inv; - point_jac(0, 0) = z_inv; - point_jac(0, 1) = 0.0; - point_jac(0, 2) = -p_in_f[0] * z_inv_sq; - point_jac(1, 0) = 0.0; - point_jac(1, 1) = z_inv; - point_jac(1, 2) = -p_in_f[1] * z_inv_sq; - point_jac = - point_jac * R_f_w; -} - void Point::optimize(const size_t n_iter) { Vector3d old_point = pos_; @@ -141,7 +124,7 @@ void Point::optimize(const size_t n_iter) { Matrix23d J; const Vector3d p_in_f((*it)->frame->T_f_w_ * pos_); - pointJacobian(p_in_f, (*it)->frame->T_f_w_.rotation_matrix(), J); + Point::jacobian_xyz2uv(p_in_f, (*it)->frame->T_f_w_.rotation_matrix(), J); const Vector2d e(vk::project2d((*it)->f) - vk::project2d(p_in_f)); new_chi2 += e.squaredNorm(); A.noalias() += J.transpose() * J; diff --git a/svo/src/pose_optimizer.cpp b/svo/src/pose_optimizer.cpp index 180bb1c..5b9d25f 100644 --- a/svo/src/pose_optimizer.cpp +++ b/svo/src/pose_optimizer.cpp @@ -37,7 +37,6 @@ void optimizeGaussNewton( { // init double chi2(0.0); - Vector2d focal_length(1,1); vector chi2_vec_init, chi2_vec_final; vk::robust_cost::TukeyWeightFunction weight_function; SE3 T_old(frame->T_f_w_); @@ -79,9 +78,10 @@ void optimizeGaussNewton( { if((*it)->point == NULL) continue; - Matrix26d J = frameJac(frame->T_f_w_, (*it)->point->pos_, focal_length); - Vector2d e = vk::project2d((*it)->f) - - vk::project2d(frame->T_f_w_ * (*it)->point->pos_); + Matrix26d J; + Vector3d xyz_f(frame->T_f_w_ * (*it)->point->pos_); + Frame::jacobian_xyz2uv(xyz_f, J); + Vector2d e = vk::project2d((*it)->f) - vk::project2d(xyz_f); double sqrt_inv_cov = 1.0 / (1<<(*it)->level); e *= sqrt_inv_cov; if(iter == 0) diff --git a/svo/src/sparse_img_align.cpp b/svo/src/sparse_img_align.cpp index fc765cb..1e91fa9 100644 --- a/svo/src/sparse_img_align.cpp +++ b/svo/src/sparse_img_align.cpp @@ -82,29 +82,6 @@ Matrix SparseImgAlign::getFisherInformation() return I; } -// TODO take jacobian from Frame Class! -void frameJac(const Vector3d & xyz, Matrix & J) -{ - const double x = xyz[0]; - const double y = xyz[1]; - const double z_inv = 1./xyz[2]; - const double z_inv_2 = z_inv*z_inv; - - J(0,0) = z_inv; // 1/z - J(0,1) = 0.0; // 0 - J(0,2) = -x*z_inv_2; // -x/z^2 - J(0,3) = J(0,2)*y; // -x*y/z^2 - J(0,4) = 1.0 - J(0,2)*x; // 1.0 + x^2/z^2 - J(0,5) = -y*z_inv; // -y/z - - J(1,0) = 0.0; // 0 - J(1,1) = z_inv; // 1/z - J(1,2) = -y*z_inv_2; // -y/z^2 - J(1,3) = -1.0 + J(1,2)*y; // -1.0 - y^2/z^2 - J(1,4) = -J(0,3); // x*y/^2 - J(1,5) = x*z_inv; // x/z -} - void SparseImgAlign::precomputeReferencePatches() { const int border = patch_halfsize_+1; @@ -157,11 +134,11 @@ void SparseImgAlign::precomputeReferencePatches() // evaluate jacobian and cache Matrix frame_jac; - frameJac(xyz_ref, frame_jac); + Frame::jacobian_xyz2uv(xyz_ref, frame_jac); // cache the jacobian jacobian_cache_.col(feature_counter*patch_area_ + pixel_counter) = - - (dx*frame_jac.row(0) + dy*frame_jac.row(1))*(focal_length / (1<