public/vio
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

moved jacobians to point and frame class

Browse Source
This commit is contained in:
Christian Forster
2014-05-05 15:35:13 +02:00
parent 5550258493
commit bee16e2d03
7 changed files with 52 additions and 79 deletions
Download Patch File Download Diff File Expand all files Collapse all files
svo/include/svo/frame.h
+26
View File
@@ -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<double,2,6>& 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
}
};
svo/include/svo/frame_handler_base.h
+2 -2
View File
@@ -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
svo/include/svo/point.h
+17 -3
View File
@@ -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
svo/include/svo/pose_optimizer.h
-27
View File
@@ -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,
svo/src/point.cpp
+1 -18
View File
@@ -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;
svo/src/pose_optimizer.cpp
+4 -4
View File
@@ -37,7 +37,6 @@ void optimizeGaussNewton(
{
// init
double chi2(0.0);
Vector2d focal_length(1,1);
vector<double> 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)
svo/src/sparse_img_align.cpp
+2 -25
View File
@@ -82,29 +82,6 @@ Matrix<double, 6, 6> SparseImgAlign::getFisherInformation()
return I;
}
// TODO take jacobian from Frame Class!
void frameJac(const Vector3d & xyz, Matrix<double,2,6> & 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<double,2,6> 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<<level_));
(dx*frame_jac.row(0) + dy*frame_jac.row(1))*(focal_length / (1<<level_));
}
}
}