inline Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>
wishart_rng(const double nu,
const Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic>& S,
RNG& rng) {
static const char* function = "stan::prob::wishart_rng(%1%)";
using stan::math::check_size_match;
using stan::math::check_positive;
check_positive(function,nu,"degrees of freedom",(double*)0);
check_size_match(function,
S.rows(), "Rows of scale parameter",
S.cols(), "columns of scale parameter",
(double*)0);
Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> B(S.rows(), S.cols());
B.setZero();
for(int i = 0; i < S.cols(); i++) {
B(i,i) = std::sqrt(chi_square_rng(nu - i, rng));
for(int j = 0; j < i; j++)
B(j,i) = normal_rng(0,1,rng);
}
return stan::math::multiply_lower_tri_self_transpose(S.llt().matrixL() * B);
}
bool CameraPoseFinderICP::minimizePointToPlaneErrFunc(unsigned level,Eigen::Matrix<float, 6, 1> &six_dof,const Mat44& cur_transform,const Mat44& last_transform_inv)
{
cudaCalPointToPlaneErrSolverParams( level,cur_transform,last_transform_inv, _camera_params_pyramid[level],AppParams::instance()->_icp_params.fDistThres,
AppParams::instance()->_icp_params.fNormSinThres);
CudaMap1D<float> buf=(CudaDeviceDataMan::instance()->rigid_align_buf_reduced).clone(CPU);
int shift = 0;
Eigen::Matrix<float, 6, 6, Eigen::RowMajor> ATA;
Eigen::Matrix<float, 6, 1> ATb;
for (int i = 0; i < 6; ++i) //rows
{
for (int j = i; j < 7; ++j) // cols + b
{
float value = buf.at(shift++);
if (j == 6)
{
ATb(i) = value;
}
else
{
ATA(i, j) = value;
ATA(j, i) = value;
}
}
}
//cout<<ATb<<endl;
if (ATA.determinant()<1E-10)
{
return false;
}
six_dof = ATA.llt().solve(ATb).cast<float>();
return true;
}
inline Eigen::VectorXd
multi_normal_rng(const Eigen::Matrix<double,Eigen::Dynamic,1>& mu,
const Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic>& S,
RNG& rng) {
using boost::variate_generator;
using boost::normal_distribution;
static const char* function = "stan::prob::multi_normal_rng(%1%)";
using stan::math::check_positive;
using stan::math::check_finite;
using stan::math::check_symmetric;
check_positive(function, S.rows(), "Covariance matrix rows");
check_symmetric(function, S, "Covariance matrix");
check_finite(function, mu, "Location parameter");
variate_generator<RNG&, normal_distribution<> >
std_normal_rng(rng, normal_distribution<>(0,1));
Eigen::VectorXd z(S.cols());
for(int i = 0; i < S.cols(); i++)
z(i) = std_normal_rng();
return mu + S.llt().matrixL() * z;
}
void CCubicGrids::extractRTFromBuffer(const cuda::GpuMat& cvgmSumBuf_, Eigen::Matrix3f* peimRw_, Eigen::Vector3f* peivTw_) const{
Mat cvmSumBuf;
cvgmSumBuf_.download(cvmSumBuf);
double* aHostTmp = (double*)cvmSumBuf.data;
//declare A and b
Eigen::Matrix<double, 6, 6, Eigen::RowMajor> A;
Eigen::Matrix<double, 6, 1> b;
//retrieve A and b from cvmSumBuf
short sShift = 0;
for (int i = 0; i < 6; ++i){ // rows
for (int j = i; j < 7; ++j) { // cols + b
double value = aHostTmp[sShift++];
if (j == 6) // vector b
b.data()[i] = value;
else
A.data()[j * 6 + i] = A.data()[i * 6 + j] = value;
}//for each col
}//for each row
//checking nullspace
double dDet = A.determinant();
if (fabs(dDet) < 1e-15 || dDet != dDet){
if (dDet != dDet)
PRINTSTR("Failure -- dDet cannot be qnan. ");
//reset ();
return;
}//if dDet is rational
//float maxc = A.maxCoeff();
Eigen::Matrix<float, 6, 1> result = A.llt().solve(b).cast<float>();
//Eigen::Matrix<float, 6, 1> result = A.jacobiSvd(ComputeThinU | ComputeThinV).solve(b);
float alpha = result(0);
float beta = result(1);
float gamma = result(2);
Eigen::Matrix3f Rinc = (Eigen::Matrix3f)Eigen::AngleAxisf(gamma, Eigen::Vector3f::UnitZ()) * Eigen::AngleAxisf(beta, Eigen::Vector3f::UnitY()) * Eigen::AngleAxisf(alpha, Eigen::Vector3f::UnitX());
Eigen::Vector3f tinc = result.tail<3>();
//compose
//eivTwCur = Rinc * eivTwCur + tinc;
//eimrmRwCur = Rinc * eimrmRwCur;
Eigen::Vector3f eivTinv = -peimRw_->transpose()* (*peivTw_);
Eigen::Matrix3f eimRinv = peimRw_->transpose();
eivTinv = Rinc * eivTinv + tinc;
eimRinv = Rinc * eimRinv;
*peivTw_ = -eimRinv.transpose() * eivTinv;
*peimRw_ = eimRinv.transpose();
}
void dare(const Eigen::Matrix<double,xDim,xDim> &A, const Eigen::Matrix<double,xDim,uDim> &B, Eigen::Matrix<double,xDim,xDim> &P)
{
Eigen::Matrix<double,xDim,xDim> Ainv = A.inverse();
Eigen::Matrix<double,xDim,xDim> ABRB = Ainv * B * _R.llt().solve(B.transpose());
Eigen::Matrix<double,2*xDim,2*xDim> Z;
Z.block(0,0,xDim,xDim) = Ainv;
Z.block(0,xDim,xDim,xDim) = ABRB;
Z.block(xDim,0,xDim,xDim) = _Q * Ainv;
Z.block(xDim,xDim,xDim,xDim) = A.transpose() + _Q * ABRB;
Eigen::ComplexEigenSolver <Eigen::Matrix<double,2*xDim,2*xDim> > ces;
ces.compute(Z);
Eigen::Matrix<std::complex<double>,2*xDim,1> eigVal = ces.eigenvalues();
Eigen::Matrix<std::complex<double>,2*xDim,2*xDim> eigVec = ces.eigenvectors();
Eigen::Matrix<std::complex<double>,2*xDim,xDim> unstableEigVec;
int ctr = 0;
for (int i = 0; i < 2*xDim; i++) {
if (eigVal(i).real()*eigVal(i).real() + eigVal(i).imag()*eigVal(i).imag() > 1) {
unstableEigVec.col(ctr) = eigVec.col(i);
ctr++;
if (ctr > xDim)
break;
}
}
Eigen::Matrix<std::complex<double>,xDim,xDim> U21inv = unstableEigVec.block(0,0,xDim,xDim).inverse();
Eigen::Matrix<std::complex<double>,xDim,xDim> PP = unstableEigVec.block(xDim,0,xDim,xDim) * U21inv;
for (int i = 0; i < xDim; i++) {
for (int j = 0; j < xDim; j++) {
P(i,j) = PP(i,j).real();
}
}
}
template<int N> void calculateGaussPdf(Eigen::Matrix<double,Eigen::Dynamic,N> X, Eigen::Matrix<double,1,N> mu, Eigen::Matrix<double,N,N> C, double* result){
Eigen::Matrix<double,N,N> L = C.llt().matrixL().transpose(); // cholesky decomposition
Eigen::Matrix<double,N,N> Linv = L.inverse();
double det = L.diagonal().prod(); //determinant of L is equal to square rooot of determinant of C
double lognormconst = -log(2 * M_PI)*X.cols()/2 - log(fabs(det));
Eigen::Matrix<double,1,N> x = mu;
Eigen::Matrix<double,1,N> tmp = x;
for (int i=0; i<X.rows(); i++){
x.noalias() = X.row(i) - mu;
tmp.noalias() = x*Linv;
double exponent = -0.5 * (tmp.cwiseProduct(tmp)).sum();
result[i] = lognormconst+exponent;
}
/*
Eigen::Matrix<double,Eigen::Dynamic,N> X0 = (X.rowwise() - mu)*Linv;
Eigen::Map<Eigen::Matrix<double,Eigen::Dynamic,1> > resultMap(result, X.rows());
resultMap = (X0.rowwise().squaredNorm()).array() * (-0.5) + lognormconst;
*/
fmath::expd_v(result, X.rows());
}
void LipmVarHeightPlanner::backwardPass(const Traj<3,3> &com)
{
Eigen::Matrix<double,6,6> Qxx;
Eigen::Matrix<double,3,3> Quu;
Eigen::Matrix<double,3,3> invQuu;
Eigen::Matrix<double,3,6> Qux;
Eigen::Matrix<double,6,3> Qxu;
Eigen::Matrix<double,6,1> Qx;
Eigen::Matrix<double,3,1> Qu;
Eigen::Matrix<double,6,6> A;
Eigen::Matrix<double,6,3> B;
Eigen::Matrix<double,6,6> Vxx = _Vxx;
Eigen::Matrix<double,6,1> Vx = Eigen::Matrix<double,6,1>::Zero();
Eigen::Matrix<double,6,1> cur_x;
Eigen::Matrix<double,3,1> cur_u;
Eigen::Matrix<double,6,1> x_h;
Eigen::Matrix<double,3,1> u_h;
//NEW_GSL_MATRIX(tmpXX0, tmpXX0_d, tmpXX0_v, 6, 6);
//NEW_GSL_MATRIX(tmpUX0, tmpUX0_d, tmpUX0_v, 3, 6);
const double *x0, *u0;
for (int t = (int)_K.size()-1; t >= 0; t--) {
//printf("======================================\n");
// get x u
x0 = _traj0[t].x;
u0 = _traj0[t].u;
//printf("%g %g %g %g %g %g\n", x0[0], x0[1], x0[2], x0[3], x0[4], x0[5]);
//printf("%g %g %g\n", u0[0], u0[1], u0[2]);
for (int i = 0; i < 6; i++) {
cur_x(i) = x0[i];
x_h(i) = x0[i] - _zmp_d[t].x[i];
}
for (int i = 0; i < 3; i++) {
cur_u(i) = u0[i];
u_h(i) = u0[i] - _zmp_d[t].u[i];
}
// get A B
getAB(x0, u0, _z0[t], A, B);
//printGSLMatrix("A", A);
//printGSLMatrix("B", B);
// Qx = Q*x_hat + A'*Vx
Qx = _Q*x_h + A.transpose()*Vx;
// Qu = R*u_hat + B'*Vx
Qu = _R*u_h + B.transpose()*Vx;
// Qxx = Q + A'*Vxx*A
Qxx = _Q + A.transpose()*Vxx*A;
// Qxu = A'*Vxx*B
Qxu = A.transpose()*Vxx*B;
// Quu = R + B'*Vxx*B
Quu = _R + B.transpose()*Vxx*B;
// Qux = Qxu'
Qux = B.transpose()*Vxx*A;
// K = -inv(Quu)*Qux
_K[t] = -(Quu.llt().solve(Qux));
// du = -inv(Quu)*Qu
_du[t] = -(Quu.llt().solve(Qu));
// Vx = Qx + K'*Qu
Vx = Qx + _K[t].transpose()*Qu;
// Vxx = Qxx + Qxu*K
Vxx = Qxx + Qxu*_K[t];
//getchar();
//assert(!hasInfNan(_K[t]));
//assert(!hasInfNan(_du[t]));
}
}
bool
pcl::gpu::KinfuTracker::operator() (const DepthMap& depth_raw,
Eigen::Affine3f *hint)
{
device::Intr intr (fx_, fy_, cx_, cy_);
if (!disable_icp_)
{
{
//ScopeTime time(">>> Bilateral, pyr-down-all, create-maps-all");
//depth_raw.copyTo(depths_curr[0]);
device::bilateralFilter (depth_raw, depths_curr_[0]);
if (max_icp_distance_ > 0)
device::truncateDepth(depths_curr_[0], max_icp_distance_);
for (int i = 1; i < LEVELS; ++i)
device::pyrDown (depths_curr_[i-1], depths_curr_[i]);
for (int i = 0; i < LEVELS; ++i)
{
device::createVMap (intr(i), depths_curr_[i], vmaps_curr_[i]);
//device::createNMap(vmaps_curr_[i], nmaps_curr_[i]);
computeNormalsEigen (vmaps_curr_[i], nmaps_curr_[i]);
}
pcl::device::sync ();
}
//can't perform more on first frame
if (global_time_ == 0)
{
Matrix3frm init_Rcam = rmats_[0]; // [Ri|ti] - pos of camera, i.e.
Vector3f init_tcam = tvecs_[0]; // transform from camera to global coo space for (i-1)th camera pose
Mat33& device_Rcam = device_cast<Mat33> (init_Rcam);
float3& device_tcam = device_cast<float3>(init_tcam);
Matrix3frm init_Rcam_inv = init_Rcam.inverse ();
Mat33& device_Rcam_inv = device_cast<Mat33> (init_Rcam_inv);
float3 device_volume_size = device_cast<const float3>(tsdf_volume_->getSize());
//integrateTsdfVolume(depth_raw, intr, device_volume_size, device_Rcam_inv, device_tcam, tranc_dist, volume_);
device::integrateTsdfVolume(depth_raw, intr, device_volume_size, device_Rcam_inv, device_tcam, tsdf_volume_->getTsdfTruncDist(), tsdf_volume_->data(), depthRawScaled_);
for (int i = 0; i < LEVELS; ++i)
device::tranformMaps (vmaps_curr_[i], nmaps_curr_[i], device_Rcam, device_tcam, vmaps_g_prev_[i], nmaps_g_prev_[i]);
++global_time_;
return (false);
}
///////////////////////////////////////////////////////////////////////////////////////////
// Iterative Closest Point
Matrix3frm Rprev = rmats_[global_time_ - 1]; // [Ri|ti] - pos of camera, i.e.
Vector3f tprev = tvecs_[global_time_ - 1]; // tranfrom from camera to global coo space for (i-1)th camera pose
Matrix3frm Rprev_inv = Rprev.inverse (); //Rprev.t();
//Mat33& device_Rprev = device_cast<Mat33> (Rprev);
Mat33& device_Rprev_inv = device_cast<Mat33> (Rprev_inv);
float3& device_tprev = device_cast<float3> (tprev);
Matrix3frm Rcurr;
Vector3f tcurr;
if(hint)
{
Rcurr = hint->rotation().matrix();
tcurr = hint->translation().matrix();
}
else
{
Rcurr = Rprev; // transform to global coo for ith camera pose
tcurr = tprev;
}
{
//ScopeTime time("icp-all");
for (int level_index = LEVELS-1; level_index>=0; --level_index)
{
int iter_num = icp_iterations_[level_index];
MapArr& vmap_curr = vmaps_curr_[level_index];
MapArr& nmap_curr = nmaps_curr_[level_index];
//MapArr& vmap_g_curr = vmaps_g_curr_[level_index];
//MapArr& nmap_g_curr = nmaps_g_curr_[level_index];
MapArr& vmap_g_prev = vmaps_g_prev_[level_index];
MapArr& nmap_g_prev = nmaps_g_prev_[level_index];
//CorespMap& coresp = coresps_[level_index];
for (int iter = 0; iter < iter_num; ++iter)
{
Mat33& device_Rcurr = device_cast<Mat33> (Rcurr);
float3& device_tcurr = device_cast<float3>(tcurr);
Eigen::Matrix<double, 6, 6, Eigen::RowMajor> A;
Eigen::Matrix<double, 6, 1> b;
#if 0
device::tranformMaps(vmap_curr, nmap_curr, device_Rcurr, device_tcurr, vmap_g_curr, nmap_g_curr);
findCoresp(vmap_g_curr, nmap_g_curr, device_Rprev_inv, device_tprev, intr(level_index), vmap_g_prev, nmap_g_prev, distThres_, angleThres_, coresp);
device::estimateTransform(vmap_g_prev, nmap_g_prev, vmap_g_curr, coresp, gbuf_, sumbuf_, A.data(), b.data());
//cv::gpu::GpuMat ma(coresp.rows(), coresp.cols(), CV_32S, coresp.ptr(), coresp.step());
//cv::Mat cpu;
//ma.download(cpu);
//cv::imshow(names[level_index] + string(" --- coresp white == -1"), cpu == -1);
#else
estimateCombined (device_Rcurr, device_tcurr, vmap_curr, nmap_curr, device_Rprev_inv, device_tprev, intr (level_index),
vmap_g_prev, nmap_g_prev, distThres_, angleThres_, gbuf_, sumbuf_, A.data (), b.data ());
#endif
//checking nullspace
double det = A.determinant ();
if (fabs (det) < 1e-15 || pcl_isnan (det))
{
if (pcl_isnan (det)) cout << "qnan" << endl;
reset ();
return (false);
}
//float maxc = A.maxCoeff();
Eigen::Matrix<float, 6, 1> result = A.llt ().solve (b).cast<float>();
//Eigen::Matrix<float, 6, 1> result = A.jacobiSvd(ComputeThinU | ComputeThinV).solve(b);
float alpha = result (0);
float beta = result (1);
float gamma = result (2);
Eigen::Matrix3f Rinc = (Eigen::Matrix3f)AngleAxisf (gamma, Vector3f::UnitZ ()) * AngleAxisf (beta, Vector3f::UnitY ()) * AngleAxisf (alpha, Vector3f::UnitX ());
Vector3f tinc = result.tail<3> ();
//compose
tcurr = Rinc * tcurr + tinc;
Rcurr = Rinc * Rcurr;
}
}
}
//save transform
rmats_.push_back (Rcurr);
tvecs_.push_back (tcurr);
}
else /* if (disable_icp_) */
{
if (global_time_ == 0)
++global_time_;
Matrix3frm Rcurr = rmats_[global_time_ - 1];
Vector3f tcurr = tvecs_[global_time_ - 1];
rmats_.push_back (Rcurr);
tvecs_.push_back (tcurr);
}
Matrix3frm Rprev = rmats_[global_time_ - 1];
Vector3f tprev = tvecs_[global_time_ - 1];
Matrix3frm Rcurr = rmats_.back();
Vector3f tcurr = tvecs_.back();
///////////////////////////////////////////////////////////////////////////////////////////
// Integration check - We do not integrate volume if camera does not move.
float rnorm = rodrigues2(Rcurr.inverse() * Rprev).norm();
float tnorm = (tcurr - tprev).norm();
const float alpha = 1.f;
bool integrate = (rnorm + alpha * tnorm)/2 >= integration_metric_threshold_;
if (disable_icp_)
integrate = true;
///////////////////////////////////////////////////////////////////////////////////////////
// Volume integration
float3 device_volume_size = device_cast<const float3> (tsdf_volume_->getSize());
Matrix3frm Rcurr_inv = Rcurr.inverse ();
Mat33& device_Rcurr_inv = device_cast<Mat33> (Rcurr_inv);
float3& device_tcurr = device_cast<float3> (tcurr);
if (integrate)
{
//ScopeTime time("tsdf");
//integrateTsdfVolume(depth_raw, intr, device_volume_size, device_Rcurr_inv, device_tcurr, tranc_dist, volume_);
integrateTsdfVolume (depth_raw, intr, device_volume_size, device_Rcurr_inv, device_tcurr, tsdf_volume_->getTsdfTruncDist(), tsdf_volume_->data(), depthRawScaled_);
}
///////////////////////////////////////////////////////////////////////////////////////////
// Ray casting
Mat33& device_Rcurr = device_cast<Mat33> (Rcurr);
{
//ScopeTime time("ray-cast-all");
raycast (intr, device_Rcurr, device_tcurr, tsdf_volume_->getTsdfTruncDist(), device_volume_size, tsdf_volume_->data(), vmaps_g_prev_[0], nmaps_g_prev_[0]);
for (int i = 1; i < LEVELS; ++i)
{
resizeVMap (vmaps_g_prev_[i-1], vmaps_g_prev_[i]);
resizeNMap (nmaps_g_prev_[i-1], nmaps_g_prev_[i]);
}
pcl::device::sync ();
}
++global_time_;
return (true);
}
bool
pcl::gpu::KinfuTracker::operator() (const DepthMap& depth_raw)
{
device::Intr intr (fx_, fy_, cx_, cy_);
{
//ScopeTime time(">>> Bilateral, pyr-down-all, create-maps-all");
//depth_raw.copyTo(depths_curr[0]);
device::bilateralFilter (depth_raw, depths_curr_[0]);
if (max_icp_distance_ > 0)
device::truncateDepth(depths_curr_[0], max_icp_distance_);
for (int i = 1; i < LEVELS; ++i)
device::pyrDown (depths_curr_[i-1], depths_curr_[i]);
for (int i = 0; i < LEVELS; ++i)
{
device::createVMap (intr(i), depths_curr_[i], vmaps_curr_[i]);
//device::createNMap(vmaps_curr_[i], nmaps_curr_[i]);
computeNormalsEigen (vmaps_curr_[i], nmaps_curr_[i]);
}
pcl::device::sync ();
}
//can't perform more on first frame
if (global_time_ == 0)
{
Matrix3frm initial_cam_rot = rmats_[0]; // [Ri|ti] - pos of camera, i.e.
Matrix3frm initial_cam_rot_inv = initial_cam_rot.inverse ();
Vector3f initial_cam_trans = tvecs_[0]; // transform from camera to global coo space for (i-1)th camera pose
Mat33& device_initial_cam_rot = device_cast<Mat33> (initial_cam_rot);
Mat33& device_initial_cam_rot_inv = device_cast<Mat33> (initial_cam_rot_inv);
float3& device_initial_cam_trans = device_cast<float3>(initial_cam_trans);
float3 device_volume_size = device_cast<const float3>(tsdf_volume_->getSize());
device::integrateTsdfVolume(depth_raw, intr, device_volume_size, device_initial_cam_rot_inv, device_initial_cam_trans, tsdf_volume_->getTsdfTruncDist(), tsdf_volume_->data(), getCyclicalBufferStructure (), depthRawScaled_);
/*
Matrix3frm init_Rcam = rmats_[0]; // [Ri|ti] - pos of camera, i.e.
Vector3f init_tcam = tvecs_[0]; // transform from camera to global coo space for (i-1)th camera pose
Mat33& device_Rcam = device_cast<Mat33> (init_Rcam);
float3& device_tcam = device_cast<float3>(init_tcam);
Matrix3frm init_Rcam_inv = init_Rcam.inverse ();
Mat33& device_Rcam_inv = device_cast<Mat33> (init_Rcam_inv);
float3 device_volume_size = device_cast<const float3>(tsdf_volume_->getSize ());
//integrateTsdfVolume(depth_raw, intr, device_volume_size, device_Rcam_inv, device_tcam, tranc_dist, volume_);
device::integrateTsdfVolume(depth_raw, intr, device_volume_size, device_Rcam_inv, device_tcam, tsdf_volume_->getTsdfTruncDist (), tsdf_volume_->data (), getCyclicalBufferStructure (), depthRawScaled_);
*/
for (int i = 0; i < LEVELS; ++i)
device::tranformMaps (vmaps_curr_[i], nmaps_curr_[i], device_initial_cam_rot, device_initial_cam_trans, vmaps_g_prev_[i], nmaps_g_prev_[i]);
if(perform_last_scan_)
finished_ = true;
++global_time_;
return (false);
}
///////////////////////////////////////////////////////////////////////////////////////////
// Iterative Closest Point
// GET PREVIOUS GLOBAL TRANSFORM
// Previous global rotation
Matrix3frm cam_rot_global_prev = rmats_[global_time_ - 1]; // [Ri|ti] - pos of camera, i.e.
// Previous global translation
Vector3f cam_trans_global_prev = tvecs_[global_time_ - 1]; // transform from camera to global coo space for (i-1)th camera pose
// Previous global inverse rotation
Matrix3frm cam_rot_global_prev_inv = cam_rot_global_prev.inverse (); // Rprev.t();
// GET CURRENT GLOBAL TRANSFORM
Matrix3frm cam_rot_global_curr = cam_rot_global_prev; // transform to global coo for ith camera pose
Vector3f cam_trans_global_curr = cam_trans_global_prev;
// CONVERT TO DEVICE TYPES
//LOCAL PREVIOUS TRANSFORM
Mat33& device_cam_rot_local_prev_inv = device_cast<Mat33> (cam_rot_global_prev_inv);
float3& device_cam_trans_local_prev_tmp = device_cast<float3> (cam_trans_global_prev);
float3 device_cam_trans_local_prev;
device_cam_trans_local_prev.x = device_cam_trans_local_prev_tmp.x - (getCyclicalBufferStructure ())->origin_metric.x;
device_cam_trans_local_prev.y = device_cam_trans_local_prev_tmp.y - (getCyclicalBufferStructure ())->origin_metric.y;
device_cam_trans_local_prev.z = device_cam_trans_local_prev_tmp.z - (getCyclicalBufferStructure ())->origin_metric.z;
/*
Matrix3frm Rprev = rmats_[global_time_ - 1]; // [Ri|ti] - pos of camera, i.e.
Vector3f tprev = tvecs_[global_time_ - 1]; // tranfrom from camera to global coo space for (i-1)th camera pose
Matrix3frm Rprev_inv = Rprev.inverse (); //Rprev.t();
//Mat33& device_Rprev = device_cast<Mat33> (Rprev);
Mat33& device_Rprev_inv = device_cast<Mat33> (Rprev_inv);
float3& device_tprev = device_cast<float3> (tprev);
Matrix3frm Rcurr = Rprev; // tranform to global coo for ith camera pose
Vector3f tcurr = tprev;
*/
{
//ScopeTime time("icp-all");
for (int level_index = LEVELS-1; level_index>=0; --level_index)
{
int iter_num = icp_iterations_[level_index];
// current maps
MapArr& vmap_curr = vmaps_curr_[level_index];
MapArr& nmap_curr = nmaps_curr_[level_index];
// previous maps
MapArr& vmap_g_prev = vmaps_g_prev_[level_index];
MapArr& nmap_g_prev = nmaps_g_prev_[level_index];
// We need to transform the maps from global to the local coordinates
Mat33& rotation_id = device_cast<Mat33> (rmats_[0]); // Identity Rotation Matrix. Because we only need translation
float3 cube_origin = (getCyclicalBufferStructure ())->origin_metric;
cube_origin.x = -cube_origin.x;
cube_origin.y = -cube_origin.y;
cube_origin.z = -cube_origin.z;
MapArr& vmap_temp = vmap_g_prev;
MapArr& nmap_temp = nmap_g_prev;
device::tranformMaps (vmap_temp, nmap_temp, rotation_id, cube_origin, vmap_g_prev, nmap_g_prev);
/*
MapArr& vmap_curr = vmaps_curr_[level_index];
MapArr& nmap_curr = nmaps_curr_[level_index];
//MapArr& vmap_g_curr = vmaps_g_curr_[level_index];
//MapArr& nmap_g_curr = nmaps_g_curr_[level_index];
MapArr& vmap_g_prev = vmaps_g_prev_[level_index];
MapArr& nmap_g_prev = nmaps_g_prev_[level_index];
*/
//CorespMap& coresp = coresps_[level_index];
for (int iter = 0; iter < iter_num; ++iter)
{
/*
Mat33& device_Rcurr = device_cast<Mat33> (Rcurr);
float3& device_tcurr = device_cast<float3>(tcurr);
*/
//CONVERT TO DEVICE TYPES
// CURRENT LOCAL TRANSFORM
Mat33& device_cam_rot_local_curr = device_cast<Mat33> (cam_rot_global_curr);/// We have not dealt with changes in rotations
float3& device_cam_trans_local_curr_tmp = device_cast<float3> (cam_trans_global_curr);
float3 device_cam_trans_local_curr;
device_cam_trans_local_curr.x = device_cam_trans_local_curr_tmp.x - (getCyclicalBufferStructure ())->origin_metric.x;
device_cam_trans_local_curr.y = device_cam_trans_local_curr_tmp.y - (getCyclicalBufferStructure ())->origin_metric.y;
device_cam_trans_local_curr.z = device_cam_trans_local_curr_tmp.z - (getCyclicalBufferStructure ())->origin_metric.z;
Eigen::Matrix<double, 6, 6, Eigen::RowMajor> A;
Eigen::Matrix<double, 6, 1> b;
estimateCombined (device_cam_rot_local_curr, device_cam_trans_local_curr, vmap_curr, nmap_curr, device_cam_rot_local_prev_inv, device_cam_trans_local_prev, intr (level_index),
vmap_g_prev, nmap_g_prev, distThres_, angleThres_, gbuf_, sumbuf_, A.data (), b.data ());
/*
estimateCombined (device_Rcurr, device_tcurr, vmap_curr, nmap_curr, device_Rprev_inv, device_tprev, intr (level_index),
vmap_g_prev, nmap_g_prev, distThres_, angleThres_, gbuf_, sumbuf_, A.data (), b.data ());
*/
//checking nullspace
double det = A.determinant ();
if ( fabs (det) < 1e-15 || pcl_isnan (det) )
{
if (pcl_isnan (det)) cout << "qnan" << endl;
PCL_ERROR ("LOST...\n");
reset ();
return (false);
}
//float maxc = A.maxCoeff();
Eigen::Matrix<float, 6, 1> result = A.llt ().solve (b).cast<float>();
//Eigen::Matrix<float, 6, 1> result = A.jacobiSvd(ComputeThinU | ComputeThinV).solve(b);
float alpha = result (0);
float beta = result (1);
float gamma = result (2);
Eigen::Matrix3f cam_rot_incremental = (Eigen::Matrix3f)AngleAxisf (gamma, Vector3f::UnitZ ()) * AngleAxisf (beta, Vector3f::UnitY ()) * AngleAxisf (alpha, Vector3f::UnitX ());
Vector3f cam_trans_incremental = result.tail<3> ();
//compose
cam_trans_global_curr = cam_rot_incremental * cam_trans_global_curr + cam_trans_incremental;
cam_rot_global_curr = cam_rot_incremental * cam_rot_global_curr;
/*
tcurr = Rinc * tcurr + tinc;
Rcurr = Rinc * Rcurr;
*/
}
}
}
//save tranform
rmats_.push_back (cam_rot_global_curr);
tvecs_.push_back (cam_trans_global_curr);
/*
rmats_.push_back (Rcurr);
tvecs_.push_back (tcurr);
*/
//check for shift
bool has_shifted = cyclical_.checkForShift(tsdf_volume_, getCameraPose (), 0.6 * volume_size_, true, perform_last_scan_);
if(has_shifted)
PCL_WARN ("SHIFTING\n");
// get NEW local rotation
Matrix3frm cam_rot_local_curr_inv = cam_rot_global_curr.inverse ();
Mat33& device_cam_rot_local_curr_inv = device_cast<Mat33> (cam_rot_local_curr_inv);
Mat33& device_cam_rot_local_curr = device_cast<Mat33> (cam_rot_global_curr);
// get NEW local translation
float3& device_cam_trans_local_curr_tmp = device_cast<float3> (cam_trans_global_curr);
float3 device_cam_trans_local_curr;
device_cam_trans_local_curr.x = device_cam_trans_local_curr_tmp.x - (getCyclicalBufferStructure ())->origin_metric.x;
device_cam_trans_local_curr.y = device_cam_trans_local_curr_tmp.y - (getCyclicalBufferStructure ())->origin_metric.y;
device_cam_trans_local_curr.z = device_cam_trans_local_curr_tmp.z - (getCyclicalBufferStructure ())->origin_metric.z;
///////////////////////////////////////////////////////////////////////////////////////////
// Integration check - We do not integrate volume if camera does not move.
float rnorm = rodrigues2(cam_rot_global_curr.inverse() * cam_rot_global_prev).norm();
float tnorm = (cam_trans_global_curr - cam_trans_global_prev).norm();
const float alpha = 1.f;
bool integrate = (rnorm + alpha * tnorm)/2 >= integration_metric_threshold_;
//~ if(integrate)
//~ std::cout << "\tCamera movement since previous frame was " << (rnorm + alpha * tnorm)/2 << " integrate is set to " << integrate << std::endl;
//~ else
//~ std::cout << "Camera movement since previous frame was " << (rnorm + alpha * tnorm)/2 << " integrate is set to " << integrate << std::endl;
///////////////////////////////////////////////////////////////////////////////////////////
// Volume integration
float3 device_volume_size = device_cast<const float3> (tsdf_volume_->getSize());
/*
Matrix3frm Rcurr_inv = Rcurr.inverse ();
Mat33& device_Rcurr_inv = device_cast<Mat33> (Rcurr_inv);
float3& device_tcurr = device_cast<float3> (tcurr);*/
if (integrate)
{
//integrateTsdfVolume(depth_raw, intr, device_volume_size, device_Rcurr_inv, device_tcurr, tranc_dist, volume_);
integrateTsdfVolume (depth_raw, intr, device_volume_size, device_cam_rot_local_curr_inv, device_cam_trans_local_curr, tsdf_volume_->getTsdfTruncDist (), tsdf_volume_->data (), getCyclicalBufferStructure (), depthRawScaled_);
}
///////////////////////////////////////////////////////////////////////////////////////////
// Ray casting
/*Mat33& device_Rcurr = device_cast<Mat33> (Rcurr);*/
{
raycast (intr, device_cam_rot_local_curr, device_cam_trans_local_curr, tsdf_volume_->getTsdfTruncDist (), device_volume_size, tsdf_volume_->data (), getCyclicalBufferStructure (), vmaps_g_prev_[0], nmaps_g_prev_[0]);
// POST-PROCESSING: We need to transform the newly raycasted maps into the global space.
Mat33& rotation_id = device_cast<Mat33> (rmats_[0]); /// Identity Rotation Matrix. Because we only need translation
float3 cube_origin = (getCyclicalBufferStructure ())->origin_metric;
//~ PCL_INFO ("Raycasting with cube origin at %f, %f, %f\n", cube_origin.x, cube_origin.y, cube_origin.z);
MapArr& vmap_temp = vmaps_g_prev_[0];
MapArr& nmap_temp = nmaps_g_prev_[0];
device::tranformMaps (vmap_temp, nmap_temp, rotation_id, cube_origin, vmaps_g_prev_[0], nmaps_g_prev_[0]);
for (int i = 1; i < LEVELS; ++i)
{
resizeVMap (vmaps_g_prev_[i-1], vmaps_g_prev_[i]);
resizeNMap (nmaps_g_prev_[i-1], nmaps_g_prev_[i]);
}
pcl::device::sync ();
}
if(has_shifted && perform_last_scan_)
extractAndMeshWorld ();
++global_time_;
return (true);
}
bool MyPointCloud::alignPointClouds(std::vector<Matrix3frm>& Rcam, std::vector<Vector3f>& tcam, MyPointCloud *globalPreviousPointCloud, device::Intr& intrinsics, int globalTime) {
Matrix3frm Rprev = Rcam[globalTime - 1]; // [Ri|ti] - pos of camera, i.e.
Vector3f tprev = tcam[globalTime - 1]; // tranfrom from camera to global coo space for (i-1)th camera pose
Matrix3frm Rprev_inv = Rprev.inverse(); //Rprev.t();
device::Mat33& device_Rprev_inv = device_cast<device::Mat33> (Rprev_inv);
float3& device_tprev = device_cast<float3> (tprev);
Matrix3frm Rcurr = Rprev; // tranform to global coo for ith camera pose
Vector3f tcurr = tprev;
#pragma omp parallel for
for(int level = LEVELS - 1; level >= 0; --level) {
int iterations = icpIterations_[level];
#pragma omp parallel for
for(int iteration = 0; iteration < iterations; ++iteration) {
{
printf(" ICP level %d iteration %d", level, iteration);
pcl::ScopeTime time("");
device::Mat33& device_Rcurr = device_cast<device::Mat33> (Rcurr);
float3& device_tcurr = device_cast<float3>(tcurr);
Eigen::Matrix<float, 6, 6, Eigen::RowMajor> A;
Eigen::Matrix<float, 6, 1> b;
if(level == 2 && iteration == 0)
error_.create(rows_ * 4, cols_);
device::estimateCombined (device_Rcurr, device_tcurr, vmaps_[level], nmaps_[level], device_Rprev_inv, device_tprev, intrinsics (level),
globalPreviousPointCloud->getVertexMaps()[level], globalPreviousPointCloud->getNormalMaps()[level], distThres_, angleThres_,
gbuf_, sumbuf_, A.data (), b.data (), error_);
//checking nullspace
float det = A.determinant ();
if (fabs (det) < 1e-15 || !pcl::device::valid_host (det)) {
// printf("ICP failed at level %d, iteration %d (global time %d)\n", level, iteration, globalTime);
return (false);
} //else printf("ICP succeed at level %d, iteration %d (global time %d)\n", level, iteration, globalTime);
Eigen::Matrix<float, 6, 1> result = A.llt ().solve (b);
//Eigen::Matrix<float, 6, 1> result = A.jacobiSvd(ComputeThinU | ComputeThinV).solve(b);
float alpha = result (0);
float beta = result (1);
float gamma = result (2);
Eigen::Matrix3f Rinc = (Eigen::Matrix3f)AngleAxisf (gamma, Vector3f::UnitZ ()) * AngleAxisf (beta, Vector3f::UnitY ()) * AngleAxisf (alpha, Vector3f::UnitX ());
Vector3f tinc = result.tail<3> ();
//compose
tcurr = Rinc * tcurr + tinc;
Rcurr = Rinc * Rcurr;
}
}
}
//save tranform
Rcam[globalTime] = Rcurr;
tcam[globalTime] = tcurr;
return (true);
}
inline bool
pcl::gpu::kinfuLS::KinfuTracker::performPairWiseICP(const Intr cam_intrinsics, Matrix3frm& resulting_rotation , Vector3f& resulting_translation)
{
// we assume that both v and n maps are in the same coordinate space
// initialize rotation and translation to respectively identity and zero
Matrix3frm previous_rotation = Eigen::Matrix3f::Identity ();
Matrix3frm previous_rotation_inv = previous_rotation.inverse ();
Vector3f previous_translation = Vector3f(0,0,0);
///////////////////////////////////////////////
// Convert pose to device type
Mat33 device_cam_rot_prev_inv;
float3 device_cam_trans_prev;
convertTransforms(previous_rotation_inv, previous_translation, device_cam_rot_prev_inv, device_cam_trans_prev);
// Initialize output pose to current pose (i.e. identity and zero translation)
Matrix3frm current_rotation = previous_rotation;
Vector3f current_translation = previous_translation;
///////////////////////////////////////////////
// Run ICP
{
//ScopeTime time("icp-all");
for (int level_index = LEVELS-1; level_index>=0; --level_index)
{
int iter_num = icp_iterations_[level_index];
// current vertex and normal maps
MapArr& vmap_curr = vmaps_curr_[level_index];
MapArr& nmap_curr = nmaps_curr_[level_index];
// previous vertex and normal maps
MapArr& vmap_prev = vmaps_prev_[level_index];
MapArr& nmap_prev = nmaps_prev_[level_index];
// no need to transform maps from global to local since they are both in camera coordinates
// run ICP for iter_num iterations (return false when lost)
for (int iter = 0; iter < iter_num; ++iter)
{
//CONVERT POSES TO DEVICE TYPES
// CURRENT LOCAL POSE
Mat33 device_current_rotation = device_cast<Mat33> (current_rotation);
float3 device_current_translation_local = device_cast<float3> (current_translation);
Eigen::Matrix<double, 6, 6, Eigen::RowMajor> A;
Eigen::Matrix<double, 6, 1> b;
// call the ICP function (see paper by Kok-Lim Low "Linear Least-squares Optimization for Point-to-Plane ICP Surface Registration")
estimateCombined (device_current_rotation, device_current_translation_local, vmap_curr, nmap_curr, device_cam_rot_prev_inv, device_cam_trans_prev, cam_intrinsics (level_index),
vmap_prev, nmap_prev, distThres_, angleThres_, gbuf_, sumbuf_, A.data (), b.data ());
// checking nullspace
double det = A.determinant ();
if ( fabs (det) < 1e-15 || pcl_isnan (det) )
{
if (pcl_isnan (det)) cout << "qnan" << endl;
PCL_WARN ("ICP PairWise LOST...\n");
//reset ();
return (false);
}
Eigen::Matrix<float, 6, 1> result = A.llt ().solve (b).cast<float>();
float alpha = result (0);
float beta = result (1);
float gamma = result (2);
// deduce incremental rotation and translation from ICP's results
Eigen::Matrix3f cam_rot_incremental = (Eigen::Matrix3f)AngleAxisf (gamma, Vector3f::UnitZ ()) * AngleAxisf (beta, Vector3f::UnitY ()) * AngleAxisf (alpha, Vector3f::UnitX ());
Vector3f cam_trans_incremental = result.tail<3> ();
//compose global pose
current_translation = cam_rot_incremental * current_translation + cam_trans_incremental;
current_rotation = cam_rot_incremental * current_rotation;
}
}
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// since raw depthmaps are quite noisy, we make sure the estimated transform is big enought to be taken into account
float rnorm = rodrigues2(current_rotation).norm();
float tnorm = (current_translation).norm();
const float alpha = 1.f;
bool integrate = (rnorm + alpha * tnorm)/2 >= integration_metric_threshold_ * 2.0f;
if(integrate)
{
resulting_rotation = current_rotation;
resulting_translation = current_translation;
}
else
{
resulting_rotation = Eigen::Matrix3f::Identity ();
resulting_translation = Vector3f(0,0,0);
}
// ICP has converged
return (true);
}
inline bool
pcl::gpu::kinfuLS::KinfuTracker::performICP(const Intr& cam_intrinsics, Matrix3frm& previous_global_rotation, Vector3f& previous_global_translation, Matrix3frm& current_global_rotation , Vector3f& current_global_translation)
{
if(disable_icp_)
{
lost_=false;
return (true);
}
// Compute inverse rotation
Matrix3frm previous_global_rotation_inv = previous_global_rotation.inverse (); // Rprev.t();
///////////////////////////////////////////////
// Convert pose to device type
Mat33 device_cam_rot_local_prev_inv;
float3 device_cam_trans_local_prev;
convertTransforms(previous_global_rotation_inv, previous_global_translation, device_cam_rot_local_prev_inv, device_cam_trans_local_prev);
device_cam_trans_local_prev -= getCyclicalBufferStructure ()->origin_metric; ;
// Use temporary pose, so that we modify the current global pose only if ICP converged
Matrix3frm resulting_rotation;
Vector3f resulting_translation;
// Initialize output pose to current pose
current_global_rotation = previous_global_rotation;
current_global_translation = previous_global_translation;
///////////////////////////////////////////////
// Run ICP
{
//ScopeTime time("icp-all");
for (int level_index = LEVELS-1; level_index>=0; --level_index)
{
int iter_num = icp_iterations_[level_index];
// current vertex and normal maps
MapArr& vmap_curr = vmaps_curr_[level_index];
MapArr& nmap_curr = nmaps_curr_[level_index];
// previous vertex and normal maps
MapArr& vmap_g_prev = vmaps_g_prev_[level_index];
MapArr& nmap_g_prev = nmaps_g_prev_[level_index];
// We need to transform the maps from global to local coordinates
Mat33& rotation_id = device_cast<Mat33> (rmats_[0]); // Identity Rotation Matrix. Because we only need translation
float3 cube_origin = (getCyclicalBufferStructure ())->origin_metric;
cube_origin = -cube_origin;
MapArr& vmap_temp = vmap_g_prev;
MapArr& nmap_temp = nmap_g_prev;
transformMaps (vmap_temp, nmap_temp, rotation_id, cube_origin, vmap_g_prev, nmap_g_prev);
// run ICP for iter_num iterations (return false when lost)
for (int iter = 0; iter < iter_num; ++iter)
{
//CONVERT POSES TO DEVICE TYPES
// CURRENT LOCAL POSE
Mat33 device_current_rotation = device_cast<Mat33> (current_global_rotation); // We do not deal with changes in rotations
float3 device_current_translation_local = device_cast<float3> (current_global_translation);
device_current_translation_local -= getCyclicalBufferStructure ()->origin_metric;
Eigen::Matrix<double, 6, 6, Eigen::RowMajor> A;
Eigen::Matrix<double, 6, 1> b;
// call the ICP function (see paper by Kok-Lim Low "Linear Least-squares Optimization for Point-to-Plane ICP Surface Registration")
estimateCombined (device_current_rotation, device_current_translation_local, vmap_curr, nmap_curr, device_cam_rot_local_prev_inv, device_cam_trans_local_prev, cam_intrinsics (level_index),
vmap_g_prev, nmap_g_prev, distThres_, angleThres_, gbuf_, sumbuf_, A.data (), b.data ());
// checking nullspace
double det = A.determinant ();
if ( fabs (det) < 1e-15 /*100000 */ || pcl_isnan (det) ) //TODO find a threshold that makes ICP track well, but prevents it from generating wrong transforms
{
if (pcl_isnan (det)) cout << "qnan" << endl;
if(lost_ == false)
PCL_ERROR ("ICP LOST... PLEASE COME BACK TO THE LAST VALID POSE (green)\n");
//reset (); //GUI will now show the user that ICP is lost. User needs to press "R" to reset the volume
lost_ = true;
return (false);
}
Eigen::Matrix<float, 6, 1> result = A.llt ().solve (b).cast<float>();
float alpha = result (0);
float beta = result (1);
float gamma = result (2);
// deduce incremental rotation and translation from ICP's results
Eigen::Matrix3f cam_rot_incremental = (Eigen::Matrix3f)AngleAxisf (gamma, Vector3f::UnitZ ()) * AngleAxisf (beta, Vector3f::UnitY ()) * AngleAxisf (alpha, Vector3f::UnitX ());
Vector3f cam_trans_incremental = result.tail<3> ();
//compose global pose
current_global_translation = cam_rot_incremental * current_global_translation + cam_trans_incremental;
current_global_rotation = cam_rot_incremental * current_global_rotation;
}
}
}
// ICP has converged
lost_ = false;
return (true);
}