C++ Matrix::llt方法代码示例

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

//.........这里部分代码省略.........

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

  }

//.........这里部分代码省略.........

                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");

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);
//.........这里部分代码省略.........