C++ eigen::Matrix3d类代码示例

    /*
     * Creates a rotation matrix which describes how a point in an auxiliary
     * coordinate system, whose x axis is desbibed by vec_along_x_axis and has
     * a point on its xz-plane vec_on_xz_plane, rotates into the real coordinate
     * system.
     */
    void Cornea::createRotationMatrix(const Eigen::Vector3d &vec_along_x_axis,
                                      const Eigen::Vector3d &vec_on_xz_plane,
                                      Eigen::Matrix3d &R) {

        // normalise pw
        Eigen::Vector3d vec_on_xz_plane_n = vec_on_xz_plane.normalized();

        // define helper variables x, y and z
        // x
        Eigen::Vector3d xn = vec_along_x_axis.normalized();

        // y
        Eigen::Vector3d tmp = vec_on_xz_plane_n.cross(xn);
        Eigen::Vector3d yn = tmp.normalized();

        // z
        tmp = xn.cross(yn);
        Eigen::Vector3d zn = tmp.normalized();

        // create the rotation matrix
        R.col(0) << xn(0), xn(1), xn(2);
        R.col(1) << yn(0), yn(1), yn(2);
        R.col(2) << zn(0), zn(1), zn(2);

    }

void mrpt::vision::pnp::rpnp::calcampose(Eigen::MatrixXd& XXc, Eigen::MatrixXd& XXw, Eigen::Matrix3d& R2, Eigen::Vector3d& t2)
{
	Eigen::MatrixXd X = XXc;
	Eigen::MatrixXd Y = XXw;
	Eigen::MatrixXd K = Eigen::MatrixXd::Identity(n, n) - Eigen::MatrixXd::Ones(n, n) * 1 / n;
	Eigen::VectorXd ux, uy;
	uy = X.rowwise().mean();
	ux = Y.rowwise().mean();

	// Need to verify sigmax2
	double sigmax2 = (((X*K).array() * (X*K).array()).colwise().sum()).mean();

	Eigen::MatrixXd SXY = Y*K*(X.transpose()) / n;

	Eigen::JacobiSVD<Eigen::MatrixXd> svd(SXY, Eigen::ComputeThinU | Eigen::ComputeThinV);

	Eigen::Matrix3d S = Eigen::MatrixXd::Identity(3, 3);
	if (SXY.determinant() <0)
		S(2, 2) = -1;

	R2 = svd.matrixV()*S*svd.matrixU().transpose();

	double c2 = (svd.singularValues().asDiagonal()*S).trace() / sigmax2;
	t2 = uy - c2*R2*ux;
	 
	Eigen::Vector3d x, y, z;
	x = R2.col(0);
	y = R2.col(1);
	z = R2.col(2);

	if ((x.cross(y) - z).norm()>0.02)
		R2.col(2) = -R2.col(2);
}

DensityGrid get_rasterized_fast(const Gaussian3Ds &gmm, const Floats &weights,
                                double cell_width, const BoundingBox3D &bb, double factor) {
  DensityGrid ret(cell_width, bb, 0);
  for (unsigned int ng = 0; ng < gmm.size(); ng++) {
    Eigen::Matrix3d covar = get_covariance(gmm[ng]);
    Eigen::Matrix3d inverse = Eigen::Matrix3d::Zero(3, 3);

    double determinant;
    bool invertible;
    covar.computeInverseAndDetWithCheck(inverse, determinant, invertible);
    IMP_INTERNAL_CHECK((invertible && determinant > 0),
                       "Tried to invert Gaussian, but it's not proper matrix");
    double pre(get_gaussian_eval_prefactor(determinant));
    Eigen::Vector3d evals = covar.eigenvalues().real();
    double maxeval = sqrt(evals.maxCoeff());
    double cutoff = factor * maxeval;
    double cutoff2 = cutoff * cutoff;
    Vector3D c = gmm[ng].get_center();
    Vector3D lower = c - Vector3D(cutoff, cutoff, cutoff);
    Vector3D upper = c + Vector3D(cutoff, cutoff, cutoff);
    GridIndex3D lowerindex = ret.get_nearest_index(lower);
    GridIndex3D upperindex = ret.get_nearest_index(upper);
    Eigen::Vector3d center(c.get_data());
    IMP_INTERNAL_CHECK(invertible, "matrix wasn't invertible! uh oh!");
    IMP_GRID3D_FOREACH_SMALLER_EXTENDED_INDEX_RANGE(ret, upperindex, lowerindex,
                                                    upperindex, {
      GridIndex3D i(voxel_index[0], voxel_index[1], voxel_index[2]);
      Eigen::Vector3d r(get_vec_from_center(i, ret, center));
      if (r.squaredNorm() < cutoff2) {
        update_value(&ret, i, r, inverse, pre, weights[ng]);
      }
    })
  }
  return ret;
}

void GreenStrain_LIMSolver3D::prepareProblemData(std::vector<int>& hessRowIdx, std::vector<int>& hessColIdx)
{
  const int numNodes = mesh->InitalVertices->rows();

  // Compute deformation gradients
  int numTets = mesh->Tetrahedra->rows();
  Ms.resize(4,3*numTets);
  MMTs.resize(4,4*numTets);

  Eigen::Matrix<double,4,3> SelectorM;
  SelectorM.block<3,3>(0,0) = Eigen::Matrix3d::Identity();
  SelectorM.row(3) = Eigen::Vector3d::Ones()*-1;
  
  for(int t=0;t<numTets;t++)
  {
    Eigen::VectorXi indices = TetrahedronVertexIdx.col(t);

    Eigen::Vector3d A = mesh->InitalVertices->row(mesh->Tetrahedra->coeff(t,0)).cast<double>();
    Eigen::Vector3d B = mesh->InitalVertices->row(mesh->Tetrahedra->coeff(t,1)).cast<double>();
    Eigen::Vector3d C = mesh->InitalVertices->row(mesh->Tetrahedra->coeff(t,2)).cast<double>();
    Eigen::Vector3d D = mesh->InitalVertices->row(mesh->Tetrahedra->coeff(t,3)).cast<double>();

    Eigen::Matrix3d V;
    V << A-D,B-D,C-D;
    
    Eigen::Matrix<double,4,3> Mtemp = SelectorM*V.inverse().cast<double>();
    Ms.block<4,3>(0,3*t) = Mtemp;
    MMTs.block<4,4>(0,4*t) = Mtemp*Mtemp.transpose();
  }
}

Gaussian3D get_gaussian_from_covariance(const Eigen::Matrix3d &covar,
                                        const Vector3D &center) {
  Rotation3D rot;
  Vector3D radii;

  // get eigen decomposition and sort by eigen vector
  Eigen::EigenSolver<Eigen::Matrix3d> es(covar);
  Eigen::Matrix3d evecs = es.eigenvectors().real();
  Eigen::Vector3d evals = es.eigenvalues().real();

  // fill in sorted stuff
  for (int i = 0; i < 3; i++) {
    radii[i] = evals[i];
  }

  // reflect if necessary
  double det = evecs.determinant();
  // std::cout<<"Determinant is "<<det<<std::endl;
  if (det < 0) {
    Eigen::Matrix3d reflect = Eigen::Vector3d(1, 1, -1).asDiagonal();
    evecs = evecs * reflect;
  }

  // create rotation matrix and return
  Eigen::Quaterniond eq(evecs);
  rot = Rotation3D(eq.w(), eq.x(), eq.y(), eq.z());
  return Gaussian3D(ReferenceFrame3D(Transformation3D(rot, center)), radii);
}

void sdh_moveto_cb(boost::shared_ptr<std::string> data){
    Eigen::Vector3d p,o;
    p.setZero();
    o.setZero();
    p(0) = right_newP_x;
    p(1) = right_newP_y;
    p(2) = right_newP_z;

    Eigen::Vector3d desired_euler;
    Eigen::Matrix3d Ident;
    desired_euler.setZero();
    Ident.setIdentity();
    desired_euler(0) = 0;
    desired_euler(1) = -1*M_PI;
    desired_euler(2) = 0.5*M_PI;

    o = euler2axisangle(desired_euler,Ident);

    mutex_act.lock();
    right_ac_vec.clear();
    right_task_vec.clear();
    right_ac_vec.push_back(new ProActController(*right_pm));
    right_task_vec.push_back(new KukaSelfCtrlTask(RP_NOCONTROL));
    right_task_vec.back()->mt = JOINTS;
    right_task_vec.back()->mft = GLOBAL;
    right_task_vec.back()->set_desired_p_eigen(p);
    right_task_vec.back()->set_desired_o_ax(o);
    mutex_act.unlock();
    std::cout<<"kuka sdh self movement and move to new pose"<<std::endl;

}

CSUtils::ENUPoint CSUtils::ecef_to_enu(const CSUtils::ECEFPoint& _point, const CSUtils::GCSPoint& _origin)
{
  double phi = _origin.latitude;
  double lamda = _origin.longitude;
  double h = _origin.altitude;

  Eigen::Vector3d lccs_center_point;

  lccs_center_point(0) = (a * cos(atan(b * tan(phi) / a)) + h * cos(phi)) * cos(lamda);
  lccs_center_point(1) = (a * cos(atan(b * tan(phi) / a)) + h * cos(phi)) * sin(lamda);
  lccs_center_point(2) = b * sin(atan(b * tan(phi) / a)) + h * sin(phi);

  Eigen::Matrix3d C_1 = Eigen::Matrix3d::Zero();
  C_1(0, 2) = -1.0; C_1(1, 1) = 1.0; C_1(2, 0) = 1.0;

  Eigen::Matrix3d C_2 = Eigen::Matrix3d::Zero();
  C_2(0, 0) = 1.0;
  C_2(1, 1) = cos(phi);   C_2(1, 2) = -sin(phi);
  C_2(2, 1) = sin(phi);   C_2(2, 2) = cos(phi);

  Eigen::Matrix3d C_3 = Eigen::Matrix3d::Zero();
  C_3(0, 0) = sin(lamda);    C_3(0, 1) = cos(lamda);   
  C_3(1, 0) = -cos(lamda);   C_3(1, 1) = sin(lamda);
  C_3(2, 2) = 1.0;

  Eigen::Matrix3d C = C_3 * C_2 * C_1;
  C.transposeInPlace();
  Eigen::Vector3d result = C * Eigen::Vector3d(_point.x, _point.y, _point.z) - C * lccs_center_point;

  return CSUtils::ENUPoint(result(0), result(1), result(2));
}

bool MotionEstimator::solveRelativeRT(const vector<pair<Vector3d, Vector3d>> &corres, Matrix3d &Rotation, Vector3d &Translation)
{
    if (corres.size() >= 15)
    {
        vector<cv::Point2f> ll, rr;
        for (int i = 0; i < int(corres.size()); i++)
        {
            ll.push_back(cv::Point2f(corres[i].first(0), corres[i].first(1)));
            rr.push_back(cv::Point2f(corres[i].second(0), corres[i].second(1)));
        }
        cv::Mat mask;
        cv::Mat E = cv::findFundamentalMat(ll, rr, cv::FM_RANSAC, 0.3 / 460, 0.99, mask);
        cv::Mat cameraMatrix = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1);
        cv::Mat rot, trans;
        int inlier_cnt = cv::recoverPose(E, ll, rr, cameraMatrix, rot, trans, mask);
        //cout << "inlier_cnt " << inlier_cnt << endl;

        Eigen::Matrix3d R;
        Eigen::Vector3d T;
        for (int i = 0; i < 3; i++)
        {   
            T(i) = trans.at<double>(i, 0);
            for (int j = 0; j < 3; j++)
                R(i, j) = rot.at<double>(i, j);
        }

        Rotation = R.transpose();
        Translation = -R.transpose() * T;
        if(inlier_cnt > 12)
            return true;
        else
            return false;
    }
    return false;
}

double GreenStrain_LIMSolver3D::computeFunction(const Eigen::Matrix<double,Eigen::Dynamic,1>& x)
{
  // green strain energy
  double shape = 0;
  Eigen::Matrix3d I = Eigen::Matrix3d::Identity();
  for(int t=0;t<mesh->Tetrahedra->rows();t++)
  {
    Eigen::Vector3d A(x[TetrahedronVertexIdx.coeff(0,t)],x[TetrahedronVertexIdx.coeff(1,t)],x[TetrahedronVertexIdx.coeff(2,t)]);
    Eigen::Vector3d B(x[TetrahedronVertexIdx.coeff(3,t)],x[TetrahedronVertexIdx.coeff(4,t)],x[TetrahedronVertexIdx.coeff(5,t)]);
    Eigen::Vector3d C(x[TetrahedronVertexIdx.coeff(6,t)],x[TetrahedronVertexIdx.coeff(7,t)],x[TetrahedronVertexIdx.coeff(8,t)]);
    Eigen::Vector3d D(x[TetrahedronVertexIdx.coeff(9,t)],x[TetrahedronVertexIdx.coeff(10,t)],x[TetrahedronVertexIdx.coeff(11,t)]);

    Eigen::Matrix<double,3,4> V;
    V.col(0) = A;
    V.col(1) = B;
    V.col(2) = C;
    V.col(3) = D;

    Eigen::Matrix3d F = V*Ms.block<4,3>(0,3*t);
    Eigen::Matrix3d E = (F.transpose()*F - I);
    shape += E.squaredNorm()*Divider;
  }

  return shape;
}

  // Assume t = double[3], q = double[4]
  void EstimateTfSVD(double* t, double* q)
  {
    // Assemble the correlation matrix H = target * reference'
    Eigen::Matrix3d H = (cloud_tgt_demean * cloud_ref_demean.transpose ()).topLeftCorner<3, 3>();

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

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

    //    std::cout<< "centroid_src: "<<centroid_src(0) <<" "<< centroid_src(1) <<" "<< centroid_src(2) << " "<< centroid_src(3)<<std::endl;
    //    std::cout<< "centroid_tgt: "<<centroid_tgt(0) <<" "<< centroid_tgt(1) <<" "<< centroid_tgt(2) << " "<< centroid_tgt(3)<<std::endl;
    
    Eigen::Matrix3d R = v * u.transpose ();

    const Eigen::Vector3d Rc (R * centroid_tgt.head<3> ());
    Eigen::Vector3d T = centroid_ref.head<3> () - Rc;

    // Make sure these memory locations are valid
    assert(t != NULL && q!=NULL);
    Eigen::Quaterniond Q(R);
    t[0] = T(0);  t[1] = T(1);  t[2] = T(2);
    q[0] = Q.w(); q[1] = Q.x(); q[2] = Q.y(); q[3] = Q.z();
  }

void PartFilter::computeDistance(int partition)
{
	std::multimap<int, std::string>::iterator it;
	for (int i=0 ; i<mModels.size() ; i++)
	{
		double distance=0;
		it = mOffsetPartToName[i].find(partition);
		for (it = mOffsetPartToName[i].equal_range(partition).first ; it != mOffsetPartToName[i].equal_range(partition).second ; ++it)
		{
			if (mJointNameToPos[(*it).second] != -1)
			{
				int pos = mJointNameToPos[(*it).second];
				double distTemp=0;
				// Mahalanobis distance
				//cout << (*it).second << "=>" << mPosNames[pos] << endl;
				Eigen::Vector3d jtPos = mModels[i]->getJoint((*it).second)->getXYZVect();
				Eigen::Vector3d jtObs(mCurrentFrame[pos][1], mCurrentFrame[pos][2], mCurrentFrame[pos][3]);
				Eigen::Vector3d diff = jtPos - jtObs;
				Eigen::Matrix3d cov;
				cov.setIdentity();
				distTemp = diff.transpose()*(cov*diff);
				distance += distTemp;
			}
		}
		mCurrentDistances[i] = distance;
	}
}

/*
 * Given the line parameters  [n_x,n_y,a_x,a_y] check if
 * [n_x, n_y] dot [data.x-a_x, data.y-a_y] < m_delta
 */
bool RSTEstimator::agree(std::vector<double> &parameters, std::pair<Eigen::Vector3d,Eigen::Vector3d> &data)
{
	
	Eigen::Matrix3d R;
	Eigen::Vector3d T;
	Eigen::Vector3d dif;
	double  E1,E2;
	
	
	R << parameters[0] , parameters[1] , parameters[2],
	     parameters[3] , parameters[4] , parameters[5],
	     parameters[6] , parameters[7] , parameters[8];
	
	T << parameters[9] , parameters[10] , parameters[11];
	

    dif = data.first - R*data.second + T; //X21
	E1 = dif.transpose()*dif;
	
	dif = data.second - R.inverse() * (data.first-T); //X12
	E2 = dif.transpose()*dif;
	
	//std::cout << "E1= " << E1 << "\nE2= " << E2 << "\nE1+E2= "<< E1+E2 << " " << this->deltaSquared <<"\n";
	
	return ( (E1 + E2) < this->deltaSquared);
}

void tangent_and_bitangent(const Eigen::Vector3d & n_,
    Eigen::Vector3d & t_, Eigen::Vector3d & b_)
{
  double rmin = 0.99;
  double n0 = n_(0), n1 = n_(1), n2 = n_(2);
  if (std::abs(n0) <= rmin) {
    rmin = std::abs(n0);
    t_(0) = 0.0;
    t_(1) = - n2 / std::sqrt(1.0 - std::pow(n0, 2));
    t_(2) =   n1 / std::sqrt(1.0 - std::pow(n0, 2));
  }
  if (std::abs(n1) <= rmin) {
    rmin = std::abs(n1);
    t_(0) =   n2 / std::sqrt(1.0 - std::pow(n1, 2));
    t_(1) =   0.0;
    t_(2) = - n0 / std::sqrt(1.0 - std::pow(n1, 2));
  }
  if (std::abs(n2) <= rmin) {
    rmin = std::abs(n2);
    t_(0) =  n1 / std::sqrt(1.0 - std::pow(n2, 2));
    t_(1) = -n0 / std::sqrt(1.0 - std::pow(n2, 2));
    t_(2) =  0.0;
  }
  b_ = n_.cross(t_);
  // Check that the calculated Frenet-Serret frame is left-handed (levogiro)
  // by checking that the determinant of the matrix whose columns are the normal,
  // tangent and bitangent vectors has determinant 1 (the system is orthonormal!)
  Eigen::Matrix3d M;
  M.col(0) = n_;
  M.col(1) = t_;
  M.col(2) = b_;
  if (boost::math::sign(M.determinant()) != 1) {
    PCMSOLVER_ERROR("Frenet-Serret local frame is not left-handed!", BOOST_CURRENT_FUNCTION);
  }
}

SRBASolver::SRBASolver()
{
  rba_.setVerbosityLevel( 2 );   // 0: None; 1:Important only; 2:Verbose
  
// =========== Topology parameters ===========
  rba_.parameters.srba.max_tree_depth       = 3;
  rba_.parameters.srba.max_optimize_depth   = 3; 
  rba_.parameters.ecp.min_obs_to_loop_closure = 1; // This is a VERY IMPORTANT PARAM, if it is set to 1 everything goes to shit

  rba_.parameters.srba.use_robust_kernel = false;
  rba_.parameters.srba.optimize_new_edges_alone = true;
  rba_.parameters.srba.dumpToConsole();

  first_keyframe_ = true;
  curr_kf_id_ = 0;

  marker_count_ = 0;

  relative_map_frame_ = "relative_map";
  global_map_frame_ = "global_map";
  loop_closed_ = false;

  // Information matrix for relative pose observations:
  {
    Eigen::Matrix3d ObsL;
  ObsL.setZero();
  ObsL(0,0) = 1/(STD_NOISE_XY*STD_NOISE_XY); // x
  ObsL(1,1) = 1/(STD_NOISE_XY*STD_NOISE_XY); // y
  ObsL(2,2) = 1/(STD_NOISE_YAW*STD_NOISE_YAW); // phi
  // Set:
  rba_.parameters.obs_noise.lambda = ObsL;
  }
}

void mesh_core::Plane::leastSquaresGeneral(
      const EigenSTL::vector_Vector3d& points,
      Eigen::Vector3d* average)
{
  if (points.empty())
  {
    normal_ = Eigen::Vector3d(0,0,1);
    d_ = 0;
    if (average)
      *average = Eigen::Vector3d::Zero();
    return;
  }

  // find c, the average of the points
  Eigen::Vector3d c;
  c.setZero();

  EigenSTL::vector_Vector3d::const_iterator p = points.begin();
  EigenSTL::vector_Vector3d::const_iterator end = points.end();
  for ( ; p != end ; ++p)
    c += *p;

  c *= 1.0/double(points.size());

  // Find the matrix
  Eigen::Matrix3d m;
  m.setZero();

  p = points.begin();
  for ( ; p != end ; ++p)
  {
    Eigen::Vector3d cp = *p - c;
    m(0,0) += cp.x() * cp.x();
    m(1,0) += cp.x() * cp.y();
    m(2,0) += cp.x() * cp.z();
    m(1,1) += cp.y() * cp.y();
    m(2,1) += cp.y() * cp.z();
    m(2,2) += cp.z() * cp.z();
  }
  m(0,1) = m(1,0);
  m(0,2) = m(2,0);
  m(1,2) = m(2,1);

  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver(m);
  if (eigensolver.info() == Eigen::Success)
  {
    normal_ = eigensolver.eigenvectors().col(0);
    normal_.normalize();
  }
  else
  {
    normal_ = Eigen::Vector3d(0,0,1);
  }

  d_ = -c.dot(normal_);

  if (average)
    *average = c;
}