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

template <typename PointSource, typename PointTarget, typename Scalar> void
pcl::registration::TransformationEstimationSVD<PointSource, PointTarget, Scalar>::getTransformationFromCorrelation (
    const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_src_demean,
    const Eigen::Matrix<Scalar, 4, 1> &centroid_src,
    const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_tgt_demean,
    const Eigen::Matrix<Scalar, 4, 1> &centroid_tgt,
    Matrix4 &transformation_matrix) const
{
  transformation_matrix.setIdentity ();

  // Assemble the correlation matrix H = source * target'
  Eigen::Matrix<Scalar, 3, 3> H = (cloud_src_demean * cloud_tgt_demean.transpose ()).topLeftCorner (3, 3);

  // Compute the Singular Value Decomposition
  Eigen::JacobiSVD<Eigen::Matrix<Scalar, 3, 3> > svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV);
  Eigen::Matrix<Scalar, 3, 3> u = svd.matrixU ();
  Eigen::Matrix<Scalar, 3, 3> v = svd.matrixV ();

  // Compute R = V * U'
  if (u.determinant () * v.determinant () < 0)
  {
    for (int x = 0; x < 3; ++x)
      v (x, 2) *= -1;
  }

  Eigen::Matrix<Scalar, 3, 3> R = v * u.transpose ();

  // Return the correct transformation
  transformation_matrix.topLeftCorner (3, 3) = R;
  const Eigen::Matrix<Scalar, 3, 1> Rc (R * centroid_src.head (3));
  transformation_matrix.block (0, 3, 3, 1) = centroid_tgt.head (3) - Rc;
}

template <typename PointSource, typename PointTarget, typename Scalar> void
pcl::registration::TransformationEstimationSVDScale<PointSource, PointTarget, Scalar>::getTransformationFromCorrelation (
    const Eigen::MatrixXf &cloud_src_demean,
    const Eigen::Vector4f &centroid_src,
    const Eigen::MatrixXf &cloud_tgt_demean,
    const Eigen::Vector4f &centroid_tgt,
    Matrix4 &transformation_matrix) const
{
  transformation_matrix.setIdentity ();

  // Assemble the correlation matrix H = source * target'
  Eigen::Matrix<Scalar, 3, 3> H = (cloud_src_demean.cast<Scalar> () * cloud_tgt_demean.cast<Scalar> ().transpose ()).topLeftCorner (3, 3);

  // Compute the Singular Value Decomposition
  Eigen::JacobiSVD<Eigen::Matrix<Scalar, 3, 3> > svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV);
  Eigen::Matrix<Scalar, 3, 3> u = svd.matrixU ();
  Eigen::Matrix<Scalar, 3, 3> v = svd.matrixV ();

  // Compute R = V * U'
  if (u.determinant () * v.determinant () < 0)
  {
    for (int x = 0; x < 3; ++x)
      v (x, 2) *= -1;
  }

  Eigen::Matrix<Scalar, 3, 3> R = v * u.transpose ();

  // rotated cloud
  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> src_ = R * cloud_src_demean.cast<Scalar> ();
  
  float scale1, scale2;
  double sum_ss = 0.0f, sum_tt = 0.0f, sum_tt_ = 0.0f;
  for (unsigned corrIdx = 0; corrIdx < cloud_src_demean.cols (); ++corrIdx)
  {
    sum_ss += cloud_src_demean (0, corrIdx) * cloud_src_demean (0, corrIdx);
    sum_ss += cloud_src_demean (1, corrIdx) * cloud_src_demean (1, corrIdx);
    sum_ss += cloud_src_demean (2, corrIdx) * cloud_src_demean (2, corrIdx);
    
    sum_tt += cloud_tgt_demean (0, corrIdx) * cloud_tgt_demean (0, corrIdx);
    sum_tt += cloud_tgt_demean (1, corrIdx) * cloud_tgt_demean (1, corrIdx);
    sum_tt += cloud_tgt_demean (2, corrIdx) * cloud_tgt_demean (2, corrIdx);
    
    sum_tt_ += cloud_tgt_demean (0, corrIdx) * src_ (0, corrIdx);
    sum_tt_ += cloud_tgt_demean (1, corrIdx) * src_ (1, corrIdx);
    sum_tt_ += cloud_tgt_demean (2, corrIdx) * src_ (2, corrIdx);
  }
  
  scale1 = sqrt (sum_tt / sum_ss);
  scale2 = sum_tt_ / sum_ss;
  float scale = scale2;
  transformation_matrix.topLeftCorner (3, 3) = scale * R;
  const Eigen::Matrix<Scalar, 3, 1> Rc (R * centroid_src.cast<Scalar> ().head (3));
  transformation_matrix.block (0, 3, 3, 1) = centroid_tgt.cast<Scalar> (). head (3) - Rc;
}

tetra::tetra(point *a, point *b, point *c, point *d) {

    p[0] = a;
    p[1] = b;
    p[2] = c;
    p[3] = d;

    ngb.fill(nullptr);
    nid.fill(-1);

    f = { { { p[3], p[2], p[1] }, // 0 - 321
            { p[0], p[2], p[3] }, // 1 - 023
            { p[0], p[3], p[1] }, // 2 - 031
            { p[0], p[1], p[2] }  // 3 - 012
          } };

    #ifndef NDEBUG

        if (a) {

            Eigen::Matrix<double, 4, 4> ort;

            ort << 1, a->x, a->y, a->z,
                   1, b->x, b->y, b->z,
                   1, c->x, c->y, c->z,
                   1, d->x, d->y, d->z;

            assert(ort.determinant() > 0); // assert positive orientation
        }

    #endif

    return;
}

void ConstraintManager::computeConstraintForces()
{
	for (std::vector< FixedDistanceConstraint* >::iterator iter = m_constraintForces.begin(); iter < m_constraintForces.end(); ++iter)
	{
		(*iter)->populateConstraintMatrix( m_constraint );
		(*iter)->populateJacobian( m_jacobian );
		(*iter)->populateJacobianDerivative( m_jacobianDeriv );
	}
	Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > invMassMatrix;
	Eigen::Matrix< double, Eigen::Dynamic, 1 > velocityCurrent;
	Eigen::Matrix< double, Eigen::Dynamic, 1 > forceCurrent;
	m_particleManager.getInvMassMatrix( invMassMatrix );
	m_particleManager.getCurrentState( velocityCurrent, false, true );
	m_particleManager.getCurrentDerivative( forceCurrent, false, true );
	m_constraintDeriv = m_jacobian * velocityCurrent;
	Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > jacobianInvJacobianT = m_jacobian * invMassMatrix * m_jacobian.transpose();
	Eigen::Matrix< double, Eigen::Dynamic, 1 > jacobianDerivVelocity = m_jacobianDeriv * velocityCurrent;
	Eigen::Matrix< double, Eigen::Dynamic, 1 > jacobianInvMassForce = m_jacobian * invMassMatrix * forceCurrent;
	Eigen::Matrix< double, Eigen::Dynamic, 1 > b = -jacobianDerivVelocity - jacobianInvMassForce - m_ks*m_constraint - m_kd*m_constraintDeriv;
	if (jacobianInvJacobianT.determinant() != 0)
	{
		m_lagrangeMultipliers = jacobianInvJacobianT.ldlt().solve( b );
	}
	else
	{
		m_lagrangeMultipliers.resize( m_constraintForces.size() );
		m_lagrangeMultipliers.setZero();
	}
}

//-----------------------------------------------------------------------------------------------
// Manipulability
//-----------------------------------------------------------------------------------------------
void LWR4Kinematics::getManipulabilityIdx(const KDL::Jacobian jac, unsigned int dof_param, double & manp_idx){


	Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> jj;
	Eigen::Matrix<double,Eigen::Dynamic,7> jac_dof;

	switch(dof_param){

	case 1:
		// Translation
		jac_dof = jac.data.block<3,7>(0,0);
		jj = (jac_dof * jac_dof.transpose());
		break;

	case 2:
		// Rotation
		jac_dof = jac.data.block<3,7>(3,0);
		jj = (jac_dof * jac_dof.transpose());
		break;

	default:
		// All
		jj = (jac.data * jac.data.transpose());
		break;
	}

	manp_idx = std::sqrt(jj.determinant());
}

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

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

    std::array<int, 4> ort;

    for (int i = 0; i < 4; ++i) {

        point &a = *t->f[i][0];
        point &b = *t->f[i][1];
        point &c = *t->f[i][2];

        A << 1, a.x, a.y, a.z,
             1, b.x, b.y, b.z,
             1, c.x, c.y, c.z,
             1, p->x, p->y, p->z;

        auto tmp = A.determinant();

        if (tmp < 0)
            return 0;
        else if (tmp == 0)
            ort[i] = 0;
        else
            ort[i] = 1;
    }    

    return ort[0] + 2 * ort[1] + 4 * ort[2] + 8 * ort[3];
}

// Task: calculates third deviatoric invariant. Note that this routine requires a
// Kelvin mapped DEVIATORIC vector
// A deviatoric mapping was not done here in order to avoid unnecessary calculations
double CalJ3(const KVec& dev_vec)
{
	Eigen::Matrix<double, 3, 3> tens;
	tens = KelvinVectorToTensor(dev_vec); // TN: Not strictly necessary. Can be written explicitly for vector
	                                      // coordinates
	return tens.determinant();
}

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

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

/**
 * @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 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);
    }
}

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

          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_) */
  {

 inline T determinant(const Eigen::Matrix<T,R,C>& m) {
   stan::math::validate_square(m,"determinant");
   return m.determinant();
 }    

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

 inline T determinant(const Eigen::Matrix<T,R,C>& m) {
   stan::math::check_square("determinant(%1%)",m,"m",(double*)0);
   return m.determinant();
 }