Matrix6d uncTinv_se3(Matrix4d T, Matrix6d covT ){
Matrix6d covTinv = Matrix6d::Zero();
Matrix6d adjTinv;
adjTinv = adjoint_se3( inverse_se3(T) );
covTinv = adjTinv * covT * adjTinv.transpose();
return covTinv;
}
Matrix6d adjoint_se3(Matrix4d T){
Matrix6d AdjT = Matrix6d::Zero();
Matrix3d R = T.block(0,0,3,3);
AdjT.block(0,0,3,3) = R;
AdjT.block(0,3,3,3) = skew( T.block(0,3,3,1) ) * R ;
AdjT.block(3,3,3,3) = R;
return AdjT;
}
// 3. System update
void Ekf::systemUpdate(double dt){
// update state
state_ = fSystem(state_,dt);
// Then calculate F
Matrix6d F;
F = FSystem(state_,dt);
// Then update covariance
cov_ = F*cov_*F.transpose() + Q_;
cov_ = zeroOutBiasXYThetaCov(cov_);
}
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;
}
}
void PwnTracker::processFrame(const DepthImage& depthImage_,
const Eigen::Isometry3f& sensorOffset_,
const Eigen::Matrix3f& cameraMatrix_,
const Eigen::Isometry3f& initialGuess){
int r,c;
Eigen::Matrix3f scaledCameraMatrix = cameraMatrix_;
_currentCloudOffset = sensorOffset_;
_currentCameraMatrix = cameraMatrix_;
_currentDepthImage = depthImage_;
_currentCloud = makeCloud(r,c,scaledCameraMatrix,_currentCloudOffset,_currentDepthImage);
bool newFrame = false;
bool newRelation = false;
PwnTrackerFrame* currentTrackerFrame=0;
Eigen::Isometry3d relationMean;
if (_previousCloud){
_aligner->setCurrentSensorOffset(_currentCloudOffset);
_aligner->setCurrentFrame(_currentCloud);
_aligner->setReferenceSensorOffset(_previousCloudOffset);
_aligner->setReferenceFrame(_previousCloud);
_aligner->correspondenceFinder()->setImageSize(r,c);
PinholePointProjector* alprojector=dynamic_cast<PinholePointProjector*>(_aligner->projector());
alprojector->setCameraMatrix(scaledCameraMatrix);
alprojector->setImageSize(r,c);
Eigen::Isometry3f guess=_previousCloudTransform.inverse()*_globalT*initialGuess;
_aligner->setInitialGuess(guess);
double t0, t1;
t0 = g2o::get_time();
_aligner->align();
t1 = g2o::get_time();
std::cout << "Time: " << t1 - t0 << " seconds " << std::endl;
cerr << "inliers: " << _aligner->inliers() << endl;
// cerr << "chi2: " << _aligner->error() << endl;
// cerr << "chi2/inliers: " << _aligner->error()/_aligner->inliers() << endl;
// cerr << "initialGuess: " << t2v(guess).transpose() << endl;
// cerr << "transform : " << t2v(_aligner->T()).transpose() << endl;
if (_aligner->inliers()>0){
_globalT = _previousCloudTransform*_aligner->T();
//cerr << "TRANSFORM FOUND" << endl;
} else {
//cerr << "FAILURE" << endl;
_globalT = _globalT*guess;
}
convertScalar(relationMean, _aligner->T());
if (! (_counter%50) ) {
Eigen::Matrix3f R = _globalT.linear();
Eigen::Matrix3f E = R.transpose() * R;
E.diagonal().array() -= 1;
_globalT.linear() -= 0.5 * R * E;
}
_globalT.matrix().row(3) << 0,0,0,1;
newAlignmentCallbacks(_globalT, _aligner->T(), _aligner->inliers(), _aligner->error());
int maxInliers = r*c;
float inliersFraction = (float) _aligner->inliers()/(float) maxInliers;
cerr << "inliers/maxinliers/fraction: " << _aligner->inliers() << "/" << maxInliers << "/" << inliersFraction << endl;
if (inliersFraction<_newFrameInliersFraction){
newFrame = true;
newRelation = true;
// char filename[1024];
// sprintf (filename, "frame-%05d.pwn", _numKeyframes);
// frame->save(filename,1,true,_globalT);
_numKeyframes ++;
if (!_cache)
delete _previousCloud;
else{
_previousCloudHandle.release();
}
//_cache->unlock(_previousTrackerFrame);
// _aligner->setReferenceSensorOffset(_currentCloudOffset);
// _aligner->setReferenceFrame(_currentCloud);
_previousCloud = _currentCloud;
_previousCloudTransform = _globalT;
// cerr << "new frame added (" << _numKeyframes << ")" << endl;
// cerr << "inliers: " << _aligner->inliers() << endl;
// cerr << "maxInliers: " << maxInliers << endl;
// cerr << "chi2: " << _aligner->error() << endl;
// cerr << "chi2/inliers: " << _aligner->error()/_aligner->inliers() << endl;
// cerr << "initialGuess: " << t2v(guess).transpose() << endl;
// cerr << "transform : " << t2v(_aligner->T()).transpose() << endl;
// cerr << "globalTransform : " << t2v(_globalT).transpose() << endl;
} else { // previous frame but offset is small
delete _currentCloud;
_currentCloud = 0;
}
} else { // first frame
//ser.writeObject(*manager);
newFrame = true;
// _aligner->setReferenceSensorOffset(_currentCloudOffset);
// _aligner->setReferenceFrame(_currentCloud);
_previousCloud = _currentCloud;
_previousCloudTransform = _globalT;
_previousCloudOffset = _currentCloudOffset;
_numKeyframes ++;
/*Eigen::Isometry3f t = _globalT;
geometry_msgs::Point p;
p.x = t.translation().x();
p.y = t.translation().y();
p.z = t.translation().z();
m_odometry.points.push_back(p);
*/
}
_counter++;
if (newFrame) {
//cerr << "maing new frame, previous: " << _previousTrackerFrame << endl;
currentTrackerFrame = new PwnTrackerFrame(_manager);
//currentTrackerFrame->cloud.set(cloud);
currentTrackerFrame->cloud=_currentCloud;
currentTrackerFrame->sensorOffset = _currentCloudOffset;
boss_map::ImageBLOB* depthBLOB=new boss_map::ImageBLOB();
DepthImage_convert_32FC1_to_16UC1(depthBLOB->cvImage(),_currentDepthImage);
//depthImage.toCvMat(depthBLOB->cvImage());
depthBLOB->adjustFormat();
currentTrackerFrame->depthImage.set(depthBLOB);
boss_map::ImageBLOB* normalThumbnailBLOB=new boss_map::ImageBLOB();
boss_map::ImageBLOB* depthThumbnailBLOB=new boss_map::ImageBLOB();
makeThumbnails(depthThumbnailBLOB->cvImage(), normalThumbnailBLOB->cvImage(), _currentCloud,
_currentDepthImage.rows,
_currentDepthImage.cols,
_currentCloudOffset,
_currentCameraMatrix,
0.1);
normalThumbnailBLOB->adjustFormat();
depthThumbnailBLOB->adjustFormat();
currentTrackerFrame->depthThumbnail.set(depthThumbnailBLOB);
currentTrackerFrame->normalThumbnail.set(normalThumbnailBLOB);
currentTrackerFrame->cameraMatrix = _currentCameraMatrix;
currentTrackerFrame->imageRows = _currentDepthImage.rows;
currentTrackerFrame->imageCols = _currentDepthImage.cols;
Eigen::Isometry3d _iso;
convertScalar(_iso, _globalT);
currentTrackerFrame->setTransform(_iso);
convertScalar(currentTrackerFrame->sensorOffset, _currentCloudOffset);
currentTrackerFrame->setSeq(_seq++);
_manager->addNode(currentTrackerFrame);
//cerr << "AAAA" << endl;
if (_cache)
_currentCloudHandle=_cache->get(currentTrackerFrame);
//cerr << "BBBB" << endl;
//_cache->lock(currentTrackerFrame);
newFrameCallbacks(currentTrackerFrame);
currentTrackerFrame->depthThumbnail.set(0);
currentTrackerFrame->normalThumbnail.set(0);
currentTrackerFrame->depthImage.set(0);
}
if (newRelation) {
PwnTrackerRelation* rel = new PwnTrackerRelation(_manager);
rel->setTransform(relationMean);
Matrix6d omega;
convertScalar(omega, _aligner->omega());
omega.setIdentity();
omega *= 100;
rel->setInformationMatrix(omega);
rel->setTo(currentTrackerFrame);
rel->setFrom(_previousTrackerFrame);
//cerr << "saved relation" << _previousTrackerFrame << " " << currentTrackerFrame << endl;
_manager->addRelation(rel);
newRelationCallbacks(rel);
//ser.writeObject(*rel);
}
if (currentTrackerFrame) {
_previousTrackerFrame = currentTrackerFrame;
}
}
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 GaussianSamplingPose3D::update( const Vector6d& mean, const Matrix6d& llt )
{
poseMean = mean;
poseCov = llt * llt.transpose();
poseLT = llt;
}
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;
}
void DirectPoseEstimationSingleLayer(
const cv::Mat &img1,
const cv::Mat &img2,
const VecVector2d &px_ref,
const vector<double> depth_ref,
Sophus::SE3 &T21
) {
// parameters
int half_patch_size = 4;
int iterations = 100;
double cost = 0, lastCost = 0;
int nGood = 0; // good projections
VecVector2d goodProjection;
for (int iter = 0; iter < iterations; iter++) {
nGood = 0;
goodProjection.clear();
// Define Hessian and bias
Matrix6d H = Matrix6d::Zero(); // 6x6 Hessian
Vector6d b = Vector6d::Zero(); // 6x1 bias
for (size_t i = 0; i < px_ref.size(); i++) {
// compute the projection in the second image
// TODO START YOUR CODE HERE
float u =0, v = 0;
Eigen::Vector2d p1 = px_ref[i];
Eigen::Vector3d pw((p1(0) - cx) / fx * depth_ref[i], (p1(1) - cy) / fy * depth_ref[i], depth_ref[i]);
Eigen::Vector3d pc = T21 * pw;
Eigen::Vector2d p2(fx * pc(0) / pc(2) + cx, fy * pc(1) / pc(2) + cy);
u = p2(0);
v = p2(1);
if(u <= half_patch_size + 1 || u >=img2.cols - half_patch_size - 1 || v <= half_patch_size - 1 || v >= img2.rows - half_patch_size - 1)
{
continue;
}
nGood++;
goodProjection.push_back(Eigen::Vector2d(u, v));
// and compute error and jacobian
for (int x = -half_patch_size; x < half_patch_size; x++)
for (int y = -half_patch_size; y < half_patch_size; y++) {
double error = GetPixelValue(img1, p1(0)+x, p1(1)+y) - GetPixelValue(img2, p2(0)+x, p2(1)+y);
double Z = depth_ref[i];
double X = (p1(0) + x - cx) / fx * Z;
double Y = (p1(1) + y - cy) / fy * Z;
Matrix26d J_pixel_xi; // pixel to \xi in Lie algebra
J_pixel_xi(0, 0) = fx / Z;
J_pixel_xi(0, 1) = 0;
J_pixel_xi(0, 2) = -fx * X / Z / Z;
J_pixel_xi(0, 3) = -fx * X * Y / Z / Z;
J_pixel_xi(0, 4) = fx + fx * X * X / Z / Z;
J_pixel_xi(0, 5) = -fx * Y / Z;
J_pixel_xi(1, 0) = 0;
J_pixel_xi(1, 1) = fy / Z;
J_pixel_xi(1, 2) = -fy * Y / Z / Z;
J_pixel_xi(1, 3) = -fy - fy * Y * Y / Z / Z;
J_pixel_xi(1, 4) = fy * X * Y / Z / Z;
J_pixel_xi(1, 5) = fy * X / Z;
Eigen::Vector2d J_img_pixel; // image gradients
J_img_pixel[0] = (GetPixelValue(img2, p2(0)+x+1, p2(1)+y) - GetPixelValue(img2,p2(0)+x-1,p2(1)+y))/2;
J_img_pixel[1] = (GetPixelValue(img2, p2(0)+x, p2(1)+y+1) - GetPixelValue(img2,p2(0)+x,p2(1)+y-1))/2;
// total jacobian
Vector6d J= -J_img_pixel.transpose() * J_pixel_xi;
H += J * J.transpose();
b += -error * J;
cost += error * error;
}
// END YOUR CODE HERE
}
// solve update and put it into estimation
// TODO START YOUR CODE HERE
Vector6d update = H.inverse() * b;
T21 = Sophus::SE3::exp(update) * T21;
// END YOUR CODE HERE
cost /= nGood;
if (isnan(update[0])) {
// sometimes occurred when we have a black or white patch and H is irreversible
cout << "update is nan" << endl;
break;
}
if (iter > 0 && cost > lastCost) {
cout << "cost increased: " << cost << ", " << lastCost << endl;
break;
}
lastCost = cost;
cout << "cost = " << cost << ", good = " << nGood << endl;
}
cout << "good projection: " << nGood << endl;
cout << "T21 = \n" << T21.matrix() << endl;
// in order to help you debug, we plot the projected pixels here
cv::Mat img1_show, img2_show;
cv::cvtColor(img1, img1_show, CV_GRAY2BGR);
cv::cvtColor(img2, img2_show, CV_GRAY2BGR);
for (auto &px: px_ref) {
cv::rectangle(img1_show, cv::Point2f(px[0] - 2, px[1] - 2), cv::Point2f(px[0] + 2, px[1] + 2),
cv::Scalar(0, 250, 0));
}
for (auto &px: goodProjection) {
cv::rectangle(img2_show, cv::Point2f(px[0] - 2, px[1] - 2), cv::Point2f(px[0] + 2, px[1] + 2),
cv::Scalar(0, 250, 0));
}
cv::imshow("reference", img1_show);
cv::imshow("current", img2_show);
cv::waitKey();
}
bool DenseTracker::match(dvo::core::PointSelection& reference, dvo::core::RgbdImagePyramid& current, dvo::DenseTracker::Result& result)
{
current.compute(cfg.getNumLevels());
bool success = true;
if(cfg.UseInitialEstimate)
{
assert(!result.isNaN() && "Provided initialization is NaN!");
}
else
{
result.setIdentity();
}
// our first increment is the given guess
Sophus::SE3d inc(result.Transformation.rotation(), result.Transformation.translation());
Revertable<Sophus::SE3d> initial(inc);
Revertable<Sophus::SE3d> estimate;
bool accept = true;
//static stopwatch_collection sw_level(5, "l", 100);
//static stopwatch_collection sw_it(5, "it@l", 500);
//static stopwatch_collection sw_error(5, "err@l", 500);
//static stopwatch_collection sw_linsys(5, "linsys@l", 500);
//static stopwatch_collection sw_prep(5, "prep@l", 100);
if(points_error.size() < reference.getMaximumNumberOfPoints(cfg.LastLevel))
points_error.resize(reference.getMaximumNumberOfPoints(cfg.LastLevel));
if(residuals.size() < reference.getMaximumNumberOfPoints(cfg.LastLevel))
residuals.resize(reference.getMaximumNumberOfPoints(cfg.LastLevel));
if(weights.size() < reference.getMaximumNumberOfPoints(cfg.LastLevel))
weights.resize(reference.getMaximumNumberOfPoints(cfg.LastLevel));
std::vector<uint8_t> valid_residuals;
bool debug = false;
if(debug)
{
reference.debug(true);
valid_residuals.resize(reference.getMaximumNumberOfPoints(cfg.LastLevel));
}
/*
std::stringstream name;
name << std::setiosflags(std::ios::fixed) << std::setprecision(2) << current.timestamp() << "_error.avi";
cv::Size s = reference.getRgbdImagePyramid().level(size_t(cfg.LastLevel)).intensity.size();
cv::Mat video_frame(s.height, s.width * 2, CV_32FC1), video_frame_u8;
cv::VideoWriter vw(name.str(), CV_FOURCC('P','I','M','1'), 30, video_frame.size(), false);
float rgb_max = 0.0;
float depth_max = 0.0;
std::stringstream name1;
name1 << std::setiosflags(std::ios::fixed) << std::setprecision(2) << current.timestamp() << "_ref.png";
cv::imwrite(name1.str(), current.level(0).rgb);
std::stringstream name2;
name2 << std::setiosflags(std::ios::fixed) << std::setprecision(2) << current.timestamp() << "_cur.png";
cv::imwrite(name2.str(), reference.getRgbdImagePyramid().level(0).rgb);
*/
Eigen::Vector2f mean;
mean.setZero();
Eigen::Matrix2f /*first_precision,*/ precision;
precision.setZero();
for(itctx_.Level = cfg.FirstLevel; itctx_.Level >= cfg.LastLevel; --itctx_.Level)
{
result.Statistics.Levels.push_back(LevelStats());
LevelStats& level_stats = result.Statistics.Levels.back();
mean.setZero();
precision.setZero();
// reset error after every pyramid level? yes because errors from different levels are not comparable
itctx_.Iteration = 0;
itctx_.Error = std::numeric_limits<double>::max();
RgbdImage& cur = current.level(itctx_.Level);
const IntrinsicMatrix& K = cur.camera().intrinsics();
Vector8f wcur, wref;
// i z idx idy zdx zdy
float wcur_id = 0.5f, wref_id = 0.5f, wcur_zd = 1.0f, wref_zd = 0.0f;
wcur << 1.0f / 255.0f, 1.0f, wcur_id * K.fx() / 255.0f, wcur_id * K.fy() / 255.0f, wcur_zd * K.fx(), wcur_zd * K.fy(), 0.0f, 0.0f;
wref << -1.0f / 255.0f, -1.0f, wref_id * K.fx() / 255.0f, wref_id * K.fy() / 255.0f, wref_zd * K.fx(), wref_zd * K.fy(), 0.0f, 0.0f;
// sw_prep[itctx_.Level].start();
PointSelection::PointIterator first_point, last_point;
reference.select(itctx_.Level, first_point, last_point);
cur.buildAccelerationStructure();
level_stats.Id = itctx_.Level;
level_stats.MaxValidPixels = reference.getMaximumNumberOfPoints(itctx_.Level);
level_stats.ValidPixels = last_point - first_point;
// sw_prep[itctx_.Level].stopAndPrint();
NormalEquationsLeastSquares ls;
Matrix6d A;
Vector6d x, b;
x = inc.log();
ComputeResidualsResult compute_residuals_result;
compute_residuals_result.first_point_error = points_error.begin();
compute_residuals_result.first_residual = residuals.begin();
compute_residuals_result.first_valid_flag = valid_residuals.begin();
// sw_level[itctx_.Level].start();
do
{
level_stats.Iterations.push_back(IterationStats());
IterationStats& iteration_stats = level_stats.Iterations.back();
iteration_stats.Id = itctx_.Iteration;
// sw_it[itctx_.Level].start();
double total_error = 0.0f;
// sw_error[itctx_.Level].start();
Eigen::Affine3f transformf;
inc = Sophus::SE3d::exp(x);
initial.update() = inc.inverse() * initial();
estimate.update() = inc * estimate();
transformf = estimate().matrix().cast<float>();
if(debug)
{
dvo::core::computeResidualsAndValidFlagsSse(first_point, last_point, cur, K, transformf, wref, wcur, compute_residuals_result);
}
else
{
dvo::core::computeResidualsSse(first_point, last_point, cur, K, transformf, wref, wcur, compute_residuals_result);
}
size_t n = (compute_residuals_result.last_residual - compute_residuals_result.first_residual);
iteration_stats.ValidConstraints = n;
if(n < 6)
{
initial.revert();
estimate.revert();
level_stats.TerminationCriterion = TerminationCriteria::TooFewConstraints;
break;
}
if(itctx_.IsFirstIterationOnLevel())
{
std::fill(weights.begin(), weights.begin() + n, 1.0f);
}
else
{
dvo::core::computeWeightsSse(compute_residuals_result.first_residual, compute_residuals_result.last_residual, weights.begin(), mean, precision);
}
precision = dvo::core::computeScaleSse(compute_residuals_result.first_residual, compute_residuals_result.last_residual, weights.begin(), mean).inverse();
float ll = computeCompleteDataLogLikelihood(compute_residuals_result.first_residual, compute_residuals_result.last_residual, weights.begin(), mean, precision);
iteration_stats.TDistributionLogLikelihood = -ll;
iteration_stats.TDistributionMean = mean.cast<double>();
iteration_stats.TDistributionPrecision = precision.cast<double>();
iteration_stats.PriorLogLikelihood = cfg.Mu * initial().log().squaredNorm();
total_error = -ll;//iteration_stats.TDistributionLogLikelihood + iteration_stats.PriorLogLikelihood;
itctx_.LastError = itctx_.Error;
itctx_.Error = total_error;
// sw_error[itctx_.Level].stopAndPrint();
// accept the last increment?
accept = itctx_.Error < itctx_.LastError;
if(!accept)
{
initial.revert();
estimate.revert();
level_stats.TerminationCriterion = TerminationCriteria::LogLikelihoodDecreased;
break;
}
// now build equation system
// sw_linsys[itctx_.Level].start();
WeightVectorType::iterator w_it = weights.begin();
Matrix2x6 J, Jw;
Eigen::Vector2f Ji;
Vector6 Jz;
ls.initialize(1);
for(PointIterator e_it = compute_residuals_result.first_point_error; e_it != compute_residuals_result.last_point_error; ++e_it, ++w_it)
{
computeJacobianOfProjectionAndTransformation(e_it->getPointVec4f(), Jw);
compute3rdRowOfJacobianOfTransformation(e_it->getPointVec4f(), Jz);
J.row(0) = e_it->getIntensityDerivativeVec2f().transpose() * Jw;
J.row(1) = e_it->getDepthDerivativeVec2f().transpose() * Jw - Jz.transpose();
ls.update(J, e_it->getIntensityAndDepthVec2f(), (*w_it) * precision);
}
ls.finish();
A = ls.A.cast<double>() + cfg.Mu * Matrix6d::Identity();
b = ls.b.cast<double>() + cfg.Mu * initial().log();
x = A.ldlt().solve(b);
// sw_linsys[itctx_.Level].stopAndPrint();
iteration_stats.EstimateIncrement = x;
iteration_stats.EstimateInformation = A;
itctx_.Iteration++;
// sw_it[itctx_.Level].stopAndPrint();
}
while(accept && x.lpNorm<Eigen::Infinity>() > cfg.Precision && !itctx_.IterationsExceeded());
if(x.lpNorm<Eigen::Infinity>() <= cfg.Precision)
level_stats.TerminationCriterion = TerminationCriteria::IncrementTooSmall;
if(itctx_.IterationsExceeded())
level_stats.TerminationCriterion = TerminationCriteria::IterationsExceeded;
// sw_level[itctx_.Level].stopAndPrint();
}
LevelStats& last_level = result.Statistics.Levels.back();
IterationStats& last_iteration = last_level.TerminationCriterion != TerminationCriteria::LogLikelihoodDecreased ? last_level.Iterations[last_level.Iterations.size() - 1] : last_level.Iterations[last_level.Iterations.size() - 2];
result.Transformation = estimate().inverse().matrix();
result.Information = last_iteration.EstimateInformation * 0.008 * 0.008;
result.LogLikelihood = last_iteration.TDistributionLogLikelihood + last_iteration.PriorLogLikelihood;
return success;
}
