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/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_));
}
}
}