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

IGL_INLINE void igl::sort_vectors_ccw(
  const Eigen::PlainObjectBase<DerivedS>& P,
  const Eigen::PlainObjectBase<DerivedS>& N,
  Eigen::PlainObjectBase<DerivedI> &order)
{
  int half_degree = P.cols()/3;
  //local frame
  Eigen::Matrix<typename DerivedS::Scalar,1,3> e1 = P.head(3).normalized();
  Eigen::Matrix<typename DerivedS::Scalar,1,3> e3 = N.normalized();
  Eigen::Matrix<typename DerivedS::Scalar,1,3> e2 = e3.cross(e1);

  Eigen::Matrix<typename DerivedS::Scalar,3,3> F; F<<e1.transpose(),e2.transpose(),e3.transpose();

  Eigen::Matrix<typename DerivedS::Scalar,Eigen::Dynamic,1> angles(half_degree,1);
  for (int i=0; i<half_degree; ++i)
  {
    Eigen::Matrix<typename DerivedS::Scalar,1,3> Pl = F.colPivHouseholderQr().solve(P.segment(i*3,3).transpose()).transpose();
//    assert(fabs(Pl(2))/Pl.cwiseAbs().maxCoeff() <1e-5);
    angles[i] = atan2(Pl(1),Pl(0));
  }

  igl::sort( angles, 1, true, angles, order);
  //make sure that the first element is always  at the top
  while (order[0] != 0)
  {
    //do a circshift
    int temp = order[0];
    for (int i =0; i< half_degree-1; ++i)
      order[i] = order[i+1];
    order(half_degree-1) = temp;
  }
}

void lstsq(const Eigen::Matrix<ScalarT, Eigen::Dynamic, Eigen::Dynamic> &A,
           const Eigen::Matrix<ScalarT, Eigen::Dynamic, 1> &b,
           Eigen::Matrix<ScalarT, Eigen::Dynamic, 1> &x)
{

    if (A.rows() == A.cols()) {
        // solve via pivoting
        x = A.colPivHouseholderQr().solve(b);
    } else if (A.rows() > A.cols()) {
        // solving via SVD
        x = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
    } else {
        x.fill(std::numeric_limits<ScalarT>::quiet_NaN());
        std::cout << "Error solving linear system" << std::endl;
        return;
    }
}

void ForwardDynamicsABM::calcABMPhase3()
{
    const LinkTraverse& traverse = body->linkTraverse();

    Link* root = traverse[0];

    if(root->jointType == Link::FREE_JOINT){

        // - | Ivv  trans(Iwv) | * | dvo | = | pf   |
        //   | Iwv     Iww     |   | dw  |   | ptau |

        Eigen::Matrix<double, 6, 6> M;
        M << root->Ivv, root->Iwv.transpose(),
             root->Iwv, root->Iww;
        
        Eigen::Matrix<double, 6, 1> f;
        f << root->pf,
             root->ptau;
        f *= -1.0;

        Eigen::Matrix<double, 6, 1> a(M.colPivHouseholderQr().solve(f));

        root->dvo = a.head<3>();
        root->dw = a.tail<3>();

    } else {
        root->dvo.setZero();
        root->dw.setZero();
    }

    int n = traverse.numLinks();
    for(int i=1; i < n; ++i){
        Link* link = traverse[i];
        Link* parent = link->parent;
        if(link->jointType != Link::FIXED_JOINT){
            link->ddq = (link->uu - (link->hhv.dot(parent->dvo) + link->hhw.dot(parent->dw))) / link->dd;
            link->dvo = parent->dvo + link->cv + link->sv * link->ddq;
            link->dw  = parent->dw  + link->cw + link->sw * link->ddq;
        }else{
            link->ddq = 0.0;
            link->dvo = parent->dvo;
            link->dw  = parent->dw; 
        }
    }
}

void ForwardDynamicsABM::calcABMPhase3()
{
    const LinkTraverse& traverse = body->linkTraverse();

    DyLink* root = static_cast<DyLink*>(traverse[0]);

    if(root->isFreeJoint()){

        // - | Ivv  trans(Iwv) | * | dvo | = | pf   |
        //   | Iwv     Iww     |   | dw  |   | ptau |

        Eigen::Matrix<double, 6, 6> M;
        M << root->Ivv(), root->Iwv().transpose(),
            root->Iwv(), root->Iww();
        
        Eigen::Matrix<double, 6, 1> f;
        f << root->pf(),
            root->ptau();
        f *= -1.0;

        Eigen::Matrix<double, 6, 1> a(M.colPivHouseholderQr().solve(f));

        root->dvo() = a.head<3>();
        root->dw() = a.tail<3>();

    } else {
        root->dvo().setZero();
        root->dw().setZero();
    }

    const int n = traverse.numLinks();
    for(int i=1; i < n; ++i){
        DyLink* link = static_cast<DyLink*>(traverse[i]);
        const DyLink* parent = link->parent();
        if(!link->isFixedJoint()){
            link->ddq() = (link->uu() - (link->hhv().dot(parent->dvo()) + link->hhw().dot(parent->dw()))) / link->dd();
            link->dvo().noalias() = parent->dvo() + link->cv() + link->sv() * link->ddq();
            link->dw().noalias()  = parent->dw()  + link->cw() + link->sw() * link->ddq();
        }else{
            link->ddq() = 0.0;
            link->dvo() = parent->dvo();
            link->dw()  = parent->dw(); 
        }
    }
}

void ColorWheel::setColor(const Color &rgb) {
    float r = rgb[0], g = rgb[1], b = rgb[2];

    float max = std::max({ r, g, b });
    float min = std::min({ r, g, b });
    float l = (max + min) / 2;

    if (max == min) {
        mHue = 0.;
        mBlack = 1. - l;
        mWhite = l;
    } else {
        float d = max - min, h;
        /* float s = l > 0.5 ? d / (2 - max - min) : d / (max + min); */
        if (max == r)
            h = (g - b) / d + (g < b ? 6 : 0);
        else if (max == g)
            h = (b - r) / d + 2;
        else
            h = (r - g) / d + 4;
        h /= 6;

        mHue = h;

        Eigen::Matrix<float, 4, 3> M;
		auto c = hue2rgb(h).rgb().rgb();
        M.topLeftCorner<3, 1>() = Eigen::Map<Eigen::Vector3f>(glm::value_ptr(c));
        M(3, 0) = 1.;
        M.col(1) = Eigen::Vector4f{ 0., 0., 0., 1. };
        M.col(2) = Eigen::Vector4f{ 1., 1., 1., 1. };

		Eigen::Vector4f rgb4{ rgb[0], rgb[1], rgb[2], 1. };
		Eigen::Vector3f bary = M.colPivHouseholderQr().solve(rgb4);

        mBlack = bary[1];
        mWhite = bary[2];
    }
}

 inline T log_determinant(const Eigen::Matrix<T, R, C>& m) {
   stan::math::check_square("log_determinant", "m", m);
   return m.colPivHouseholderQr().logAbsDeterminant();
 }

void ProbabilisticStereoTriangulator<CAMERA_GEOMETRY_T>::getUncertainty(
    size_t keypointIdxA, size_t keypointIdxB,
    const Eigen::Vector4d& homogeneousPoint_A,
    Eigen::Matrix3d& outPointUOplus_A, bool& outCanBeInitialized) const {
  OKVIS_ASSERT_TRUE_DBG(Exception,frameA_&&frameB_,"initialize with frames before use!");

  // also get the point in the other coordinate representation
  //Eigen::Vector4d& homogeneousPoint_B=_T_BA*homogeneousPoint_A;
  Eigen::Vector4d hPA = homogeneousPoint_A;

  // calculate point uncertainty by constructing the lhs of the Gauss-Newton equation system.
  // note: the transformation T_WA is assumed constant and identity w.l.o.g.
  Eigen::Matrix<double, 9, 9> H = H_;

  //	keypointA_t& kptA = _frameA_ptr->keypoint(keypointIdxA);
  //	keypointB_t& kptB = _frameB_ptr->keypoint(keypointIdxB);
  Eigen::Vector2d kptA, kptB;
  frameA_->getKeypoint(camIdA_, keypointIdxA, kptA);
  frameB_->getKeypoint(camIdB_, keypointIdxB, kptB);

  // assemble the stuff from the reprojection errors
  double keypointStdDev;
  frameA_->getKeypointSize(camIdA_, keypointIdxA, keypointStdDev);
  keypointStdDev = 0.8 * keypointStdDev / 12.0;
  Eigen::Matrix2d inverseMeasurementCovariance = Eigen::Matrix2d::Identity()
      * (1.0 / (keypointStdDev * keypointStdDev));
  ::okvis::ceres::ReprojectionError<CAMERA_GEOMETRY_T> reprojectionErrorA(
      frameA_->geometryAs<CAMERA_GEOMETRY_T>(camIdA_), 0, kptA,
      inverseMeasurementCovariance);
  //typename keypointA_t::measurement_t residualA;
  Eigen::Matrix<double, 2, 1> residualA;
  Eigen::Matrix<double, 2, 4, Eigen::RowMajor> J_hpA;
  Eigen::Matrix<double, 2, 3, Eigen::RowMajor> J_hpA_min;
  double* jacobiansA[3];
  jacobiansA[0] = 0;  // do not calculate, T_WA is fixed identity transform
  jacobiansA[1] = J_hpA.data();
  jacobiansA[2] = 0;  // fixed extrinsics
  double* jacobiansA_min[3];
  jacobiansA_min[0] = 0;  // do not calculate, T_WA is fixed identity transform
  jacobiansA_min[1] = J_hpA_min.data();
  jacobiansA_min[2] = 0;  // fixed extrinsics
  const double* parametersA[3];
  //const double* test = _poseA.parameters();
  parametersA[0] = poseA_.parameters();
  parametersA[1] = hPA.data();
  parametersA[2] = extrinsics_.parameters();
  reprojectionErrorA.EvaluateWithMinimalJacobians(parametersA, residualA.data(),
                                                  jacobiansA, jacobiansA_min);

  inverseMeasurementCovariance.setIdentity();
  frameB_->getKeypointSize(camIdB_, keypointIdxB, keypointStdDev);
  keypointStdDev = 0.8 * keypointStdDev / 12.0;
  inverseMeasurementCovariance *= 1.0 / (keypointStdDev * keypointStdDev);

  ::okvis::ceres::ReprojectionError<CAMERA_GEOMETRY_T> reprojectionErrorB(
      frameB_->geometryAs<CAMERA_GEOMETRY_T>(camIdB_), 0, kptB,
      inverseMeasurementCovariance);
  Eigen::Matrix<double, 2, 1> residualB;
  Eigen::Matrix<double, 2, 7, Eigen::RowMajor> J_TB;
  Eigen::Matrix<double, 2, 6, Eigen::RowMajor> J_TB_min;
  Eigen::Matrix<double, 2, 4, Eigen::RowMajor> J_hpB;
  Eigen::Matrix<double, 2, 3, Eigen::RowMajor> J_hpB_min;
  double* jacobiansB[3];
  jacobiansB[0] = J_TB.data();
  jacobiansB[1] = J_hpB.data();
  jacobiansB[2] = 0;  // fixed extrinsics
  double* jacobiansB_min[3];
  jacobiansB_min[0] = J_TB_min.data();
  jacobiansB_min[1] = J_hpB_min.data();
  jacobiansB_min[2] = 0;  // fixed extrinsics
  const double* parametersB[3];
  parametersB[0] = poseB_.parameters();
  parametersB[1] = hPA.data();
  parametersB[2] = extrinsics_.parameters();
  reprojectionErrorB.EvaluateWithMinimalJacobians(parametersB, residualB.data(),
                                                  jacobiansB, jacobiansB_min);

  // evaluate again closer:
  hPA.head<3>() = 0.8 * (hPA.head<3>() - T_AB_.r() / 2.0 * hPA[3])
      + T_AB_.r() / 2.0 * hPA[3];
  reprojectionErrorB.EvaluateWithMinimalJacobians(parametersB, residualB.data(),
                                                  jacobiansB, jacobiansB_min);
  if (residualB.transpose() * residualB < 4.0)
    outCanBeInitialized = false;
  else
    outCanBeInitialized = true;

  // now add to H:
  H.bottomRightCorner<3, 3>() += J_hpA_min.transpose() * J_hpA_min;
  H.topLeftCorner<6, 6>() += J_TB_min.transpose() * J_TB_min;
  H.topRightCorner<6, 3>() += J_TB_min.transpose() * J_hpB_min;
  H.bottomLeftCorner<3, 6>() += J_hpB_min.transpose() * J_TB_min;
  H.bottomRightCorner<3, 3>() += J_hpB_min.transpose() * J_hpB_min;

  // invert (if invertible) to get covariance:
  Eigen::Matrix<double, 9, 9> cov;
  if (H.colPivHouseholderQr().rank() < 9) {
    outCanBeInitialized = false;
    return;
  }
//.........这里部分代码省略.........