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