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

 UncertainTransformation::covariance_t UncertainTransformation::UOplus() const
 {
   Eigen::Matrix<double,6,6> S;
   S.setIdentity();
   S.topRightCorner<3,3>() = -sm::kinematics::crossMx(_t_a_b_a);
   return S.inverse().eval()*_U*S.transpose().inverse().eval();
 }

    bool operator() (const T* const q_coeffs, const T* const t_coeffs, const T* const s, T* residuals) const
    {
        T q_ceres[4] = {q_coeffs[3], q_coeffs[0], q_coeffs[1], q_coeffs[2]};
        Eigen::Matrix<T,3,3> R;
        ceres::QuaternionToRotation(q_ceres, ceres::ColumnMajorAdapter3x3(R.data()));

        Eigen::Matrix<T,4,4> H = Eigen::Matrix<T,4,4>::Identity();
        H.block(0,0,3,3) = *s * R;
        H(0,3) = t_coeffs[0];
        H(1,3) = t_coeffs[1];
        H(2,3) = t_coeffs[2];

        Eigen::Matrix<T,4,4> pred_H2 = H * m_H1.cast<T>() * H.inverse();

        Eigen::Matrix<T,4,4> err_H = m_H2.cast<T>().inverse() * pred_H2;
        Eigen::Matrix<T,3,3> err_R = err_H.block(0,0,3,3);

        T roll, pitch, yaw;
        mat2RPY(err_R, roll, pitch, yaw);

        residuals[0] = err_H(0,3);
        residuals[1] = err_H(1,3);
        residuals[2] = err_H(2,3);

        residuals[3] = roll;
        residuals[4] = pitch;
        residuals[5] = yaw;

        return true;
    }

void GIPCam::SetParams(float fx, float fy, float cx, float cy, uint width, uint height) {
	// TODO: NOTE: WE IGNORE THE SKEW TERM IN THE GIVEN K MATRIX

	const float scale_x = 240.f; //      // width*( 1.f/(MAX_X-MIN_X) ); // width*0.5.
	const float scale_y = 240.f; // height*( 1.f/(MAX_Y-MIN_Y) ); // height*0.75*0.5.
	
	const float new_fx = scale_x * fx;
	const float new_fy = scale_y * fy;

	const int new_cx = (int)(scale_x*cx + 180.f); // (float)width/2;
	const int new_cy = (int)(scale_y*cy + 240.f); //(float)height/2;

	//m_params.m_height = height;
	//m_params.m_width = width;
	m_params.m_intrinsic.Set(-new_fx, -new_fy, new_cx, new_cy);

	Eigen::Matrix<float, 3, 3, Eigen::RowMajor> intrinsic;
	intrinsic.setZero();
	intrinsic(0,0) = m_params.m_intrinsic.m_fx;
	intrinsic(1,1) = m_params.m_intrinsic.m_fy;
	intrinsic(2,2) = 1.f;
	intrinsic(0,2) = m_params.m_intrinsic.m_cx;
	intrinsic(1,2) = m_params.m_intrinsic.m_cy;

	Eigen::Matrix<float, 3, 3, Eigen::RowMajor> invIntrinsic = intrinsic.inverse();

	m_params.m_invIntrinsic.Set(invIntrinsic(0,0), invIntrinsic(1,1), invIntrinsic(0,2), invIntrinsic(1,2));
}

  ErrorTermTransformation::ErrorTermTransformation(aslam::backend::TransformationExpression T, sm::kinematics::Transformation prior, Eigen::Matrix<double,6,6> N, int debug) : 
    _T(T), _prior(prior), _debug(debug)
  {
    // Fill in the inverse covariance.
    setInvR(N.inverse());

    // Tell the super class about the design variables:
    aslam::backend::DesignVariable::set_t dv;
    _T.getDesignVariables(dv);
    setDesignVariablesIterator(dv.begin(), dv.end());
  }

	/**
	 * invert the matrix
	 */
	Matrix<M, N> inversed(void) const {
#ifdef CONFIG_ARCH_ARM
		Matrix<M, N> res;
		arm_mat_inverse_f32(&this->arm_mat, &res.arm_mat);
		return res;
#else
		Eigen::Matrix<float, M, N, Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float, M, N, Eigen::RowMajor> >
				(this->arm_mat.pData);
		Eigen::Matrix<float, M, N, Eigen::RowMajor> MyInverse = Me.inverse(); //not sure if A = A.inverse() is a good idea
		Matrix<M, N> res(MyInverse.data());
		return res;
#endif
	}

/**
 * @brief findIntersection for 2D lines. Can handle pll lines (return nothing)
 * @param other
 * @return
 */
optional<const C2dImagePointPx> C2dLine::findIntersection(const C2dLine & other) const
{
    Eigen::Matrix<double, 2, 2> directions;
    directions.block<2,1>(0,0) = direction;
    directions.block<2,1>(0,1) = -other.direction;

    if(fabs(directions.determinant()) < 1e-8)//Should usually be about 1
        return optional<const C2dImagePointPx>();

    const C2dImagePointPx diff = other.pointOnLine - pointOnLine;

    const TEigen2dPoint lambda_mu = directions.inverse() * diff;
    const C2dImagePointPx pointOnLine = point(lambda_mu(0));
    if(IS_DEBUG) CHECK(!zero((other.point(lambda_mu(1)) - pointOnLine).squaredNorm()), "findIntersection failed (parallel lines?)");

    return optional<const C2dImagePointPx>(pointOnLine);
}

void StateEstimatorKinematic::dare(const Eigen::Matrix<double,6,6> &A, const Eigen::Matrix<double,6,6> &B, Eigen::Matrix<double,6,6> &P,int zDim)
{
    Eigen::Matrix<double,6,6> Ainv = A.inverse();
    Eigen::Matrix<double,6,6> ABRB;
    if (zDim == 6)
    {
        ABRB = Ainv * B * _R.llt().solve(B.transpose());
    }
    else {
        ABRB = Ainv * B.topLeftCorner(6,zDim) * _R.topLeftCorner(zDim,zDim).llt().solve(B.topLeftCorner(6,zDim).transpose());
    }
    Eigen::Matrix<double,2*6,2*6> Z;
    Z.block(0,0,6,6) = Ainv;
    Z.block(0,6,6,6) = ABRB;
    Z.block(6,0,6,6) = _Q * Ainv;
    Z.block(6,6,6,6) = A.transpose() + _Q * ABRB;

    Eigen::ComplexEigenSolver <Eigen::Matrix<double,2*6,2*6> > ces;
    ces.compute(Z);

    Eigen::Matrix<std::complex<double>,2*6,1> eigVal = ces.eigenvalues();
    Eigen::Matrix<std::complex<double>,2*6,2*6> eigVec = ces.eigenvectors();

    Eigen::Matrix<std::complex<double>,2*6,6> unstableEigVec;

    int ctr = 0;
    for (int i = 0; i < 2*6; i++) {
        if (eigVal(i).real()*eigVal(i).real() + eigVal(i).imag()*eigVal(i).imag() > 1) {
            unstableEigVec.col(ctr) = eigVec.col(i);
            ctr++;
            if (ctr > 6)
                break;
        }
    }

    Eigen::Matrix<std::complex<double>,6,6> U21inv = unstableEigVec.block(0,0,6,6).inverse();
    Eigen::Matrix<std::complex<double>,6,6> PP = unstableEigVec.block(6,0,6,6) * U21inv;

    for (int i = 0; i < 6; i++) {
        for (int j = 0; j < 6; j++) {
            P(i,j) = PP(i,j).real();
        }
    }
}

/// Trajectory Planner
Vector6d Isella3Controller::Trajectory(double desired_pos, unsigned long time_in, unsigned long time_fin, int iArmModuleId, int iAxisId)
{    
  Vector6d b;
  Vector6d x;
  Eigen::Matrix<double, 6, 6> A;
  b << theApp.fGetAngleAxisIs(iArmModuleId,iAxisId), 0, 0, desired_pos, 0, 0;
  A << 1,time_in,pow(time_in,2),pow(time_in,3),pow(time_in,4),pow(time_in,5),
    0,1,2*time_in,3*pow(time_in,2),4*pow(time_in,3),5*pow(time_in,4),
    0,0,2,6*time_in,12*pow(time_in,2),20*pow(time_in,3),
    1,time_fin,pow(time_fin,2),pow(time_fin,3),pow(time_fin,4),pow(time_fin,5),
    0,1,2*time_fin,3*pow(time_fin,2),4*pow(time_fin,3),5*pow(time_fin,4),
    0,0,2,6*time_fin,12*pow(time_fin,2),20*pow(time_fin,3);
  x = A.inverse()*b;

  ROS_INFO("Trajectory ---> desired_pos = %d, AngleAxisIs = %d, time_in = %d, time_fin = %d", (int)desired_pos, (int)theApp.fGetAngleAxisIs(iArmModuleId,iAxisId), (int)time_in, (int)time_fin);
  std::cout << "Matrix A:\n" << A << "\nVector b:\n" << b << "\nVector x:\n" << x << "\nVector b = A*x:\n" << A*x << endl;

  return x;
}

bool GimbalCalibResidual::operator()(const T *const tau_s,
                                     const T *const tau_d,
                                     const T *const w1,
                                     const T *const w2,
                                     const T *const Lambda1,
                                     const T *const Lambda2,
                                     T *residual) const {
  // clang-format off
  // Form the transform from static camera to dynamic camera
  const Eigen::Matrix<T, 4, 4> T_ds = this->T_ds(tau_s, tau_d, w1, w2, Lambda1, Lambda2);

  // Calculate reprojection error by projecting 3D world point observed in
  // dynamic camera to static camera
  // -- Transform 3D world point from dynamic to static camera
  const Eigen::Matrix<T, 3, 1> P_d{T(this->P_d[0]), T(this->P_d[1]), T(this->P_d[2])};
  const Eigen::Matrix<T, 3, 1> P_s_cal = (T_ds.inverse() * P_d.homogeneous()).head(3);
  // -- Project 3D world point to image plane
  const Eigen::Matrix<T, 3, 3> K_s = this->K(T(this->fx_s), T(this->fy_s), T(this->cx_s), T(this->cy_s));
  // const Eigen::Matrix<T, 4, 1> D_s = this->D(T(this->k1_s), T(this->k2_s), T(this->k3_s), T(this->k4_s));
  // const Eigen::Matrix<T, 2, 1> Q_s_cal = this->project_pinhole_equi(K_s, D_s, P_s_cal);
  const Eigen::Matrix<T, 2, 1> Q_s_cal = this->project_pinhole(K_s, P_s_cal);
  // -- Calculate reprojection error
  residual[0] = T(this->Q_s[0]) - Q_s_cal(0);
  residual[1] = T(this->Q_s[1]) - Q_s_cal(1);

  // Calculate reprojection error by projecting 3D world point observed in
  // static camera to dynamic camera
  // -- Transform 3D world point from static to dynamic camera
  const Eigen::Matrix<T, 3, 1> P_s{T(this->P_s[0]), T(this->P_s[1]), T(this->P_s[2])};
  const Eigen::Matrix<T, 3, 1> P_d_cal = (T_ds * P_s.homogeneous()).head(3);
  // -- Project 3D world point to image plane
  const Eigen::Matrix<T, 3, 3> K_d = this->K(T(this->fx_d), T(this->fy_d), T(this->cx_d), T(this->cy_d));
  // const Eigen::Matrix<T, 4, 1> D_d = this->D(T(this->k1_d), T(this->k2_d), T(this->k3_d), T(this->k4_d));
  // const Eigen::Matrix<T, 2, 1> Q_d_cal = this->project_pinhole_equi(K_d, D_d, P_d_cal);
  const Eigen::Matrix<T, 2, 1> Q_d_cal = this->project_pinhole(K_d, P_d_cal);
  // -- Calculate reprojection error
  residual[2] = T(this->Q_d[0]) - Q_d_cal(0);
  residual[3] = T(this->Q_d[1]) - Q_d_cal(1);

  return true;
  // clang-format on
}

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

void NuTo::ContinuumElementIGA<TDim>::CalculateNMatrixBMatrixDetJacobian(EvaluateDataContinuum<TDim>& rData,
                                                                         int rTheIP) const
{
    // calculate Jacobian
    const auto ipCoords = this->GetIntegrationType().GetLocalIntegrationPointCoordinates(rTheIP);
    const Eigen::MatrixXd& derivativeShapeFunctionsGeometryNatural =
            this->mInterpolationType->Get(Node::eDof::COORDINATES)
                    .DerivativeShapeFunctionsNaturalIGA(ipCoords, mKnotIDs);

    Eigen::Matrix<double, TDim, TDim> jacobian = this->CalculateJacobian(derivativeShapeFunctionsGeometryNatural,
                                                                         rData.mNodalValues[Node::eDof::COORDINATES]);

    // there are two mappings in IGA: 1) reference element <=> parametric space (knots) 2) parametric space <=> physical
    // space
    // the B-matrix only the invJac of mapping 2) is needed
    rData.mDetJacobian = jacobian.determinant();
    for (int i = 0; i < TDim; i++)
        rData.mDetJacobian *= 0.5 * (mKnots(i, 1) - mKnots(i, 0));

    if (rData.mDetJacobian == 0)
        throw Exception(std::string("[") + __PRETTY_FUNCTION__ +
                        "] Determinant of the Jacobian is zero, no inversion possible.");

    Eigen::Matrix<double, TDim, TDim> invJacobian = jacobian.inverse();

    // calculate shape functions and their derivatives
    for (auto dof : this->mInterpolationType->GetDofs())
    {
        if (dof == Node::eDof::COORDINATES)
            continue;
        const InterpolationBase& interpolationType = this->mInterpolationType->Get(dof);
        rData.mNIGA[dof] = interpolationType.MatrixNIGA(ipCoords, mKnotIDs);

        rData.mB[dof] = this->CalculateMatrixB(
                dof, interpolationType.DerivativeShapeFunctionsNaturalIGA(ipCoords, mKnotIDs), invJacobian);
    }
}

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 PositionCommand::control()
{
    geometry_msgs::Twist        vel_cmd_msg;
    geometry_msgs::Pose         current_error_msg ;

    Eigen::Matrix<double,6,1>   current_error;

    if (control_)
    {
        current_error= goal_pose_ - current_pose_;

        // if the error is less than epsilon goal is reached
        if(current_error.norm() < .1)
        {
            ROS_INFO("Reached goal!");
        }

        accum_error_ = accum_error_+ period_*(current_error + previous_error_)/2.0;

        Eigen::Matrix<double,6,6> Cp = Kp * current_error.transpose();
        Eigen::Matrix<double,6,6> Ci = Ki * accum_error_.transpose();
        Eigen::Matrix<double,6,6> Cd = Kd * (current_error - previous_error_).transpose()/period_;

        //std::cout << "Cp ="  <<  Cp << std::endl;
        //std::cout << "Ci ="  <<  Ci << std::endl;
        //std::cout << "Cd ="  <<  Cd << std::endl;

        Eigen::Matrix<double,6,6> vel_cmd_current = Cp + Ci + Cd ;
        Eigen::Matrix<double,3,1> linear_vel_cmd_l_frame;

        linear_vel_cmd_l_frame << vel_cmd_current(0,0),
                vel_cmd_current(1,1),
                vel_cmd_current(2,2);

        Eigen::Matrix<double,3,1> linear_vel_cmd_g_frame = rot_matrix_.inverse() * linear_vel_cmd_l_frame;

        if (chirp_enable_)
        {
            linear_vel_cmd_g_frame(0,0) = linear_vel_cmd_g_frame(0,0) + chirp_command();
        }

        vel_cmd_msg.linear.x  = Sat(linear_vel_cmd_g_frame(0,0),1,-1);
        vel_cmd_msg.linear.y  = Sat(linear_vel_cmd_g_frame(1,0),1,-1);
        vel_cmd_msg.linear.z  = Sat(linear_vel_cmd_g_frame(2,0),1,-1);
        vel_cmd_msg.angular.z = Sat(vel_cmd_current(5,5),1,-1);

        previous_error_     = current_error;
        control_            = false;

        current_error_pub.publish(current_error_msg);

        std::cout << "CE: x ="  <<  current_error(0,0)
                  << " y = "    <<  current_error(1,0)
                  << " z = "    <<  current_error(2,0)
                  << " w = "    <<  current_error(5,0)
                  << std::endl;
    }

    current_error_msg.position.x    = current_error(0,0);
    current_error_msg.position.y    = current_error(1,0);
    current_error_msg.position.z    = current_error(2,0);
    current_error_msg.orientation.x = current_error(0,0);
    current_error_msg.orientation.y = current_error(1,0);
    current_error_msg.orientation.z = current_error(2,0);

    vel_cmd.publish(vel_cmd_msg);

    //    std::cout << "AE: x ="  <<  accum_error_(0,0)
    //              << " y = "    <<  accum_error_(1,0)
    //              << " z = "    <<  accum_error_(2,0)
    //             << " w = "    <<  accum_error_(5,0)
    //              << std::endl;

    std::cout << "Kp = "  << Kp.transpose()<< std::endl;
    std::cout << "Ki = "  << Ki.transpose()<< std::endl;
    std::cout << "Kd = "  << Kd.transpose()<< std::endl;

    std::cout << "vel: x =" <<  vel_cmd_msg.linear.x
              << " y = "    <<  vel_cmd_msg.linear.y
              << " z = "    <<  vel_cmd_msg.linear.z
              << " w = "    <<  vel_cmd_msg.angular.z
              << std::endl;

}

unsigned int fast_inclusion(const tetra *t, const point *p) {

    auto &a = *t->p[0],
         &b = *t->p[1],
         &c = *t->p[2],
         &d = *t->p[3];

    Eigen::Matrix<double, 3, 3> A;

    A.col(0) << b.x - a.x, b.y - a.y, b.z - a.z;
    A.col(1) << c.x - a.x, c.y - a.y, c.z - a.z;
    A.col(2) << d.x - a.x, d.y - a.y, d.z - a.z;

    if (verbose == 3)
        std::cout << A << std::endl;

    Eigen::Matrix<double, 3, 1> x, B(p->x - a.x, p->y - a.y, p->z - a.z);

    x = A.inverse() * B;

    double sum = 0;

    for (unsigned int i = 0; i < 3; ++i) {

        if (std::abs(x[i]) < 1e-10)
            x[i] = 0;

        if (x[i] < 0)
            return 0; // outside
        else
            sum += x[i];
    }


    if (std::abs(sum - 1) < 1e-10)
        sum = 1;//return exact_inclusion(t, p);

    if (std::abs(sum) < 1e-10)
        sum = 0;

    if (sum > 1)
        return 0; // outside

    if (sum == 0)
        return 1; // vertex 0

    double u(x[0]), v(x[1]), w(x[2]);

    if (u == 1) {

        return 2; // vertex 1
    }

    else if (u > 0) {

        if (v > 0) {
            
            if (w > 0) {

                if (sum == 1)
                    return 14; // surface 321
                else
                    return 15; // inside
            }

            else {

                if (sum == 1)
                    return 6; // edge 21
                else
                    return 7; // surface 012
            }
        }

        else {
            
            if (w > 0) {

                if (sum == 1)
                    return 10; // edge 31
                else
                    return 11; // surface 031
            }

            else {

                return 3; // edge 10
            }
        } 
    } else {

        if (v == 1)
            return 4; // vertex 2

        else if (v > 0) {

            if (w > 0) {

                if (sum == 1)
                    return 12; // edge 32
//.........这里部分代码省略.........

void ICPOdometry::getIncrementalTransformation(Eigen::Vector3f & trans, Eigen::Matrix<float, 3, 3, Eigen::RowMajor> & rot, int threads, int blocks)
{
    iterations[0] = 10;
    iterations[1] = 5;
    iterations[2] = 4;

    Eigen::Matrix<float, 3, 3, Eigen::RowMajor> Rprev = rot;
    Eigen::Vector3f tprev = trans;

    Eigen::Matrix<float, 3, 3, Eigen::RowMajor> Rcurr = Rprev;
    Eigen::Vector3f tcurr = tprev;

    Eigen::Matrix<float, 3, 3, Eigen::RowMajor> Rprev_inv = Rprev.inverse();
    Mat33 & device_Rprev_inv = device_cast<Mat33>(Rprev_inv);
    float3& device_tprev = device_cast<float3>(tprev);

    cv::Mat resultRt = cv::Mat::eye(4, 4, CV_64FC1);

    for(int i = NUM_PYRS - 1; i >= 0; i--)
    {
        for(int j = 0; j < iterations[i]; j++)
        {
            Eigen::Matrix<float, 6, 6, Eigen::RowMajor> A_icp;
            Eigen::Matrix<float, 6, 1> b_icp;

            Mat33&  device_Rcurr = device_cast<Mat33> (Rcurr);
            float3& device_tcurr = device_cast<float3>(tcurr);

            DeviceArray2D<float>& vmap_curr = vmaps_curr_[i];
            DeviceArray2D<float>& nmap_curr = nmaps_curr_[i];

            DeviceArray2D<float>& vmap_g_prev = vmaps_g_prev_[i];
            DeviceArray2D<float>& nmap_g_prev = nmaps_g_prev_[i];

            float residual[2];

            icpStep(device_Rcurr,
                    device_tcurr,
                    vmap_curr,
                    nmap_curr,
                    device_Rprev_inv,
                    device_tprev,
                    intr(i),
                    vmap_g_prev,
                    nmap_g_prev,
                    distThres_,
                    angleThres_,
                    sumData,
                    outData,
                    A_icp.data(),
                    b_icp.data(),
                    &residual[0],
                    threads,
                    blocks);

            lastICPError = sqrt(residual[0]) / residual[1];
            lastICPCount = residual[1];

            Eigen::Matrix<double, 6, 1> result;
            Eigen::Matrix<double, 6, 6, Eigen::RowMajor> dA_icp = A_icp.cast<double>();
            Eigen::Matrix<double, 6, 1> db_icp = b_icp.cast<double>();

            lastA = dA_icp;
            lastb = db_icp;
            result = lastA.ldlt().solve(lastb);

            Eigen::Isometry3f incOdom;

            OdometryProvider::computeProjectiveMatrix(resultRt, result, incOdom);

            Eigen::Isometry3f currentT;
            currentT.setIdentity();
            currentT.rotate(Rprev);
            currentT.translation() = tprev;

            currentT = currentT * incOdom.inverse();

            tcurr = currentT.translation();
            Rcurr = currentT.rotation();
        }
    }

    trans = tcurr;
    rot = Rcurr;
}