void computeEdgeSE3PriorGradient(Isometry3d& E,
Matrix6d& J,
const Isometry3d& Z,
const Isometry3d& X,
const Isometry3d& P){
// compute the error at the linearization point
const Isometry3d A = Z.inverse()*X;
const Isometry3d B = P;
const Matrix3d Ra = A.rotation();
const Matrix3d Rb = B.rotation();
const Vector3d tb = B.translation();
E = A*B;
const Matrix3d Re=E.rotation();
Matrix<double, 3 , 9 > dq_dR;
compute_dq_dR (dq_dR,
Re(0,0),Re(1,0),Re(2,0),
Re(0,1),Re(1,1),Re(2,1),
Re(0,2),Re(1,2),Re(2,2));
J.setZero();
// dte/dt
J.block<3,3>(0,0)=Ra;
// dte/dq =0
// dte/dqj
{
Matrix3d S;
skew(S,tb);
J.block<3,3>(0,3)=Ra*S;
}
// dre/dt =0
// dre/dq
{
double buf[27];
Map<Matrix<double, 9,3> > M(buf);
Matrix3d Sx,Sy,Sz;
skew(Sx,Sy,Sz,Rb);
Map<Matrix3d> Mx(buf); Mx = Ra*Sx;
Map<Matrix3d> My(buf+9); My = Ra*Sy;
Map<Matrix3d> Mz(buf+18); Mz = Ra*Sz;
J.block<3,3>(3,3) = dq_dR * M;
}
}
template <typename PointSource, typename PointTarget, typename Scalar> inline void
pcl::registration::TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar>::
estimateRigidTransformation (ConstCloudIterator<PointSource>& source_it,
ConstCloudIterator<PointTarget>& target_it,
typename std::vector<Scalar>::const_iterator& weights_it,
Matrix4 &transformation_matrix) const
{
typedef Eigen::Matrix<double, 6, 1> Vector6d;
typedef Eigen::Matrix<double, 6, 6> Matrix6d;
Matrix6d ATA;
Vector6d ATb;
ATA.setZero ();
ATb.setZero ();
while (source_it.isValid () && target_it.isValid ())
{
if (!pcl_isfinite (source_it->x) ||
!pcl_isfinite (source_it->y) ||
!pcl_isfinite (source_it->z) ||
!pcl_isfinite (target_it->x) ||
!pcl_isfinite (target_it->y) ||
!pcl_isfinite (target_it->z) ||
!pcl_isfinite (target_it->normal_x) ||
!pcl_isfinite (target_it->normal_y) ||
!pcl_isfinite (target_it->normal_z))
{
++ source_it;
++ target_it;
++ weights_it;
continue;
}
const float & sx = source_it->x;
const float & sy = source_it->y;
const float & sz = source_it->z;
const float & dx = target_it->x;
const float & dy = target_it->y;
const float & dz = target_it->z;
const float & nx = target_it->normal[0] * (*weights_it);
const float & ny = target_it->normal[1] * (*weights_it);
const float & nz = target_it->normal[2] * (*weights_it);
double a = nz*sy - ny*sz;
double b = nx*sz - nz*sx;
double c = ny*sx - nx*sy;
// 0 1 2 3 4 5
// 6 7 8 9 10 11
// 12 13 14 15 16 17
// 18 19 20 21 22 23
// 24 25 26 27 28 29
// 30 31 32 33 34 35
ATA.coeffRef (0) += a * a;
ATA.coeffRef (1) += a * b;
ATA.coeffRef (2) += a * c;
ATA.coeffRef (3) += a * nx;
ATA.coeffRef (4) += a * ny;
ATA.coeffRef (5) += a * nz;
ATA.coeffRef (7) += b * b;
ATA.coeffRef (8) += b * c;
ATA.coeffRef (9) += b * nx;
ATA.coeffRef (10) += b * ny;
ATA.coeffRef (11) += b * nz;
ATA.coeffRef (14) += c * c;
ATA.coeffRef (15) += c * nx;
ATA.coeffRef (16) += c * ny;
ATA.coeffRef (17) += c * nz;
ATA.coeffRef (21) += nx * nx;
ATA.coeffRef (22) += nx * ny;
ATA.coeffRef (23) += nx * nz;
ATA.coeffRef (28) += ny * ny;
ATA.coeffRef (29) += ny * nz;
ATA.coeffRef (35) += nz * nz;
double d = nx*dx + ny*dy + nz*dz - nx*sx - ny*sy - nz*sz;
ATb.coeffRef (0) += a * d;
ATb.coeffRef (1) += b * d;
ATb.coeffRef (2) += c * d;
ATb.coeffRef (3) += nx * d;
ATb.coeffRef (4) += ny * d;
ATb.coeffRef (5) += nz * d;
++ source_it;
++ target_it;
++ weights_it;
}
ATA.coeffRef (6) = ATA.coeff (1);
ATA.coeffRef (12) = ATA.coeff (2);
ATA.coeffRef (13) = ATA.coeff (8);
ATA.coeffRef (18) = ATA.coeff (3);
ATA.coeffRef (19) = ATA.coeff (9);
ATA.coeffRef (20) = ATA.coeff (15);
ATA.coeffRef (24) = ATA.coeff (4);
ATA.coeffRef (25) = ATA.coeff (10);
ATA.coeffRef (26) = ATA.coeff (16);
ATA.coeffRef (27) = ATA.coeff (22);
ATA.coeffRef (30) = ATA.coeff (5);
ATA.coeffRef (31) = ATA.coeff (11);
ATA.coeffRef (32) = ATA.coeff (17);
ATA.coeffRef (33) = ATA.coeff (23);
ATA.coeffRef (34) = ATA.coeff (29);
// Solve A*x = b
Vector6d x = static_cast<Vector6d> (ATA.inverse () * ATb);
// Construct the transformation matrix from x
constructTransformationMatrix (x (0), x (1), x (2), x (3), x (4), x (5), transformation_matrix);
}
void computeEdgeSE3Gradient(Isometry3d& E,
Matrix6d& Ji,
Matrix6d& Jj,
const Isometry3d& Z,
const Isometry3d& Xi,
const Isometry3d& Xj,
const Isometry3d& Pi,
const Isometry3d& Pj
){
// compute the error at the linearization point
const Isometry3d A=Z.inverse()*Pi.inverse();
const Isometry3d B=Xi.inverse()*Xj;
const Isometry3d C=Pj;
const Isometry3d AB=A*B;
const Isometry3d BC=B*C;
E=AB*C;
const Matrix3d Re=E.rotation();
const Matrix3d Ra=A.rotation();
const Matrix3d Rb=B.rotation();
const Matrix3d Rc=C.rotation();
const Vector3d tc=C.translation();
//const Vector3d tab=AB.translation();
const Matrix3d Rab=AB.rotation();
const Vector3d tbc=BC.translation();
const Matrix3d Rbc=BC.rotation();
Matrix<double, 3 , 9 > dq_dR;
compute_dq_dR (dq_dR,
Re(0,0),Re(1,0),Re(2,0),
Re(0,1),Re(1,1),Re(2,1),
Re(0,2),Re(1,2),Re(2,2));
Ji.setZero();
Jj.setZero();
// dte/dti
Ji.block<3,3>(0,0)=-Ra;
// dte/dtj
Jj.block<3,3>(0,0)=Ra*Rb;
// dte/dqi
{
Matrix3d S;
skewT(S,tbc);
Ji.block<3,3>(0,3)=Ra*S;
}
// dte/dqj
{
Matrix3d S;
skew(S,tc);
Jj.block<3,3>(0,3)=Rab*S;
}
// dre/dqi
{
double buf[27];
Map<Matrix<double, 9,3> > M(buf);
Matrix3d Sxt,Syt,Szt;
skewT(Sxt,Syt,Szt,Rbc);
Map<Matrix3d> Mx(buf); Mx = Ra*Sxt;
Map<Matrix3d> My(buf+9); My = Ra*Syt;
Map<Matrix3d> Mz(buf+18); Mz = Ra*Szt;
Ji.block<3,3>(3,3) = dq_dR * M;
}
// dre/dqj
{
double buf[27];
Map<Matrix<double, 9,3> > M(buf);
Matrix3d Sx,Sy,Sz;
skew(Sx,Sy,Sz,Rc);
Map<Matrix3d> Mx(buf); Mx = Rab*Sx;
Map<Matrix3d> My(buf+9); My = Rab*Sy;
Map<Matrix3d> Mz(buf+18); Mz = Rab*Sz;
Jj.block<3,3>(3,3) = dq_dR * M;
}
}
void optimizeGaussNewton(
const double reproj_thresh,
const size_t n_iter,
const bool verbose,
FramePtr& frame,
double& estimated_scale,
double& error_init,
double& error_final,
size_t& num_obs)
{
// init
double chi2(0.0);
vector<double> chi2_vec_init, chi2_vec_final;
vk::robust_cost::TukeyWeightFunction weight_function;
SE3d T_old(frame->T_f_w_);
Matrix6d A;
Vector6d b;
// compute the scale of the error for robust estimation
std::vector<float> errors; errors.reserve(frame->fts_.size());
for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); ++it)
{
if((*it)->point == NULL)
continue;
Vector2d e = vk::project2d((*it)->f)
- vk::project2d(frame->T_f_w_ * (*it)->point->pos_);
e *= 1.0 / (1<<(*it)->level);
errors.push_back(e.norm());
}
if(errors.empty())
return;
vk::robust_cost::MADScaleEstimator scale_estimator;
estimated_scale = scale_estimator.compute(errors);
num_obs = errors.size();
chi2_vec_init.reserve(num_obs);
chi2_vec_final.reserve(num_obs);
double scale = estimated_scale;
for(size_t iter=0; iter<n_iter; iter++)
{
// overwrite scale
if(iter == 5)
scale = 0.85/frame->cam_->errorMultiplier2();
b.setZero();
A.setZero();
double new_chi2(0.0);
// compute residual
for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); ++it)
{
if((*it)->point == NULL)
continue;
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)
chi2_vec_init.push_back(e.squaredNorm()); // just for debug
J *= sqrt_inv_cov;
double weight = weight_function.value(e.norm()/scale);
A.noalias() += J.transpose()*J*weight;
b.noalias() -= J.transpose()*e*weight;
new_chi2 += e.squaredNorm()*weight;
}
// solve linear system
const Vector6d dT(A.ldlt().solve(b));
// check if error increased
if((iter > 0 && new_chi2 > chi2) || (bool) std::isnan((double)dT[0]))
{
if(verbose)
std::cout << "it " << iter
<< "\t FAILURE \t new_chi2 = " << new_chi2 << std::endl;
frame->T_f_w_ = T_old; // roll-back
break;
}
// update the model
SE3d T_new = SE3d::exp(dT)*frame->T_f_w_;
T_old = frame->T_f_w_;
frame->T_f_w_ = T_new;
chi2 = new_chi2;
if(verbose)
std::cout << "it " << iter
<< "\t Success \t new_chi2 = " << new_chi2
<< "\t norm(dT) = " << vk::norm_max(dT) << std::endl;
// stop when converged
if(vk::norm_max(dT) <= EPS)
break;
}
// Set covariance as inverse information matrix. Optimistic estimator!
const double pixel_variance=1.0;
frame->Cov_ = pixel_variance*(A*std::pow(frame->cam_->errorMultiplier2(),2)).inverse();
// Remove Measurements with too large reprojection error
double reproj_thresh_scaled = reproj_thresh / frame->cam_->errorMultiplier2();
size_t n_deleted_refs = 0;
for(Features::iterator it=frame->fts_.begin(); it!=frame->fts_.end(); ++it)
{
if((*it)->point == NULL)
continue;
Vector2d e = vk::project2d((*it)->f) - vk::project2d(frame->T_f_w_ * (*it)->point->pos_);
double sqrt_inv_cov = 1.0 / (1<<(*it)->level);
e *= sqrt_inv_cov;
chi2_vec_final.push_back(e.squaredNorm());
if(e.norm() > reproj_thresh_scaled)
{
// we don't need to delete a reference in the point since it was not created yet
(*it)->point = NULL;
++n_deleted_refs;
}
}
error_init=0.0;
error_final=0.0;
if(!chi2_vec_init.empty())
error_init = sqrt(vk::getMedian(chi2_vec_init))*frame->cam_->errorMultiplier2();
if(!chi2_vec_final.empty())
error_final = sqrt(vk::getMedian(chi2_vec_final))*frame->cam_->errorMultiplier2();
estimated_scale *= frame->cam_->errorMultiplier2();
if(verbose)
std::cout << "n deleted obs = " << n_deleted_refs
<< "\t scale = " << estimated_scale
<< "\t error init = " << error_init
<< "\t error end = " << error_final << std::endl;
num_obs -= n_deleted_refs;
}
