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

Eigen::Vector3d CalibrateData::calibrate(Eigen::Vector3d data,
                                         Eigen::Matrix3d alignment_matrix,
                                         Eigen::Matrix3d sensitivity_matrix,
                                         Eigen::Vector3d offset_vector)
{
  Eigen::Vector3d calibrated_data;
  calibrated_data = alignment_matrix.inverse() *
      sensitivity_matrix.inverse() *
      (data - offset_vector);
  return calibrated_data;
}

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

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

Eigen::Matrix3d LinearAlgebra::invMatrixM(const Eigen::Matrix3d& M) {
	Eigen::Matrix3d result;

	// TODO: return the inverse of matrix M
	result = M.inverse();

	return result;
}

void KeyFrame::cam2Imu(Eigen::Vector3d T_c_w,
                       Eigen::Matrix3d R_c_w,
                       Eigen::Vector3d &t_w_i,
                       Eigen::Matrix3d &r_w_i)
{
    r_w_i = (qic * R_c_w).inverse();
    t_w_i = -R_c_w.inverse() * T_c_w - r_w_i * tic;
}

ON_NurbsSurface
FittingCylinder::initNurbsCylinderWithAxes (int order, NurbsDataSurface *data, Eigen::Matrix3d &axes)
{
  Eigen::Vector3d mean;

  unsigned s = unsigned (data->interior.size ());
  mean = NurbsTools::computeMean (data->interior);

  data->mean = mean;

  Eigen::Vector3d v_max (0.0, 0.0, 0.0);
  Eigen::Vector3d v_min (DBL_MAX, DBL_MAX, DBL_MAX);
  for (unsigned i = 0; i < s; i++)
  {
    Eigen::Vector3d p (axes.inverse () * (data->interior[i] - mean));

    if (p (0) > v_max (0))
      v_max (0) = p (0);
    if (p (1) > v_max (1))
      v_max (1) = p (1);
    if (p (2) > v_max (2))
      v_max (2) = p (2);

    if (p (0) < v_min (0))
      v_min (0) = p (0);
    if (p (1) < v_min (1))
      v_min (1) = p (1);
    if (p (2) < v_min (2))
      v_min (2) = p (2);
  }

  int ncpsU (order);
  int ncpsV (2 * order + 4);
  ON_NurbsSurface nurbs (3, false, order, order, ncpsU, ncpsV);
  nurbs.MakeClampedUniformKnotVector (0, 1.0);
  nurbs.MakePeriodicUniformKnotVector (1, 1.0 / (ncpsV - order + 1));

  double dcu = (v_max (0) - v_min (0)) / (ncpsU - 1);
  double dcv = (2.0 * M_PI) / (ncpsV - order + 1);

  double ry = std::max<double> (std::fabs (v_min (1)), std::fabs (v_max (1)));
  double rz = std::max<double> (std::fabs (v_min (2)), std::fabs (v_max (2)));

  Eigen::Vector3d cv_t, cv;
  for (int i = 0; i < ncpsU; i++)
  {
    for (int j = 0; j < ncpsV; j++)
    {
      cv (0) = v_min (0) + dcu * i;
      cv (1) = ry * sin (dcv * j);
      cv (2) = rz * cos (dcv * j);
      cv_t = axes * cv + mean;
      nurbs.SetCV (i, j, ON_3dPoint (cv_t (0), cv_t (1), cv_t (2)));
    }
  }

  return nurbs;
}

const Eigen::Matrix3d CMiniVisionToolbox::getFundamental( const Eigen::Isometry3d& p_matTransformationFrom, const Eigen::Isometry3d& p_matTransformationTo, const Eigen::Matrix3d& p_matIntrinsic )
{
    //ds get essential matrix
    const Eigen::Matrix3d matEssential( CMiniVisionToolbox::getEssential( p_matTransformationFrom, p_matTransformationTo ) );

    //ds compute fundamental matrix: http://en.wikipedia.org/wiki/Fundamental_matrix_%28computer_vision%29
    const Eigen::Matrix3d matIntrinsicInverse( p_matIntrinsic.inverse( ) );
    return matIntrinsicInverse.transpose( )*matEssential*matIntrinsicInverse;
}

    std::unique_ptr<Image> leastSquarePatch(const std::unique_ptr<Image>& image)
    {
        // process grayscale patches only
        if (image->getChannelNum() > 1)
            return std::unique_ptr<Image>();

        auto data_ptr = image->getValues(0);
        const auto w = image->getWidth();
        const auto h = image->getHeight();

        // least square fit to linear plane for light compensation
        float xsum_orig = 0;
        float ysum_orig = 0;
        float csum_orig = 0;
        for (size_t j = 0; j < h; j++)
        {
            for (size_t i = 0; i < w; i++)
            {
                xsum_orig += (i * data_ptr[j*w + i]);
                ysum_orig += (j * data_ptr[j*w + i]);
                csum_orig += (data_ptr[j*w + i]);
            }
        }
        Eigen::Vector3d vsum(xsum_orig, ysum_orig, csum_orig);

        float x2sum = 0, y2sum = 0, xysum = 0, xsum = 0, ysum = 0;
        float csum = w*h;
        for (size_t j = 0; j < h; j++)
        {
            for (size_t i = 0; i < w; i++)
            {
                x2sum += (i*i);
                y2sum += (j*j);
                xysum += (i*j);
                xsum += i;
                ysum += j;
            }
        }
        Eigen::Matrix3d msum;
        msum << x2sum, xysum, xsum,
             xysum, y2sum, ysum,
             xsum, ysum, csum;
        
        auto vcoeff = msum.inverse() * vsum;
        
        auto newImage = std::make_unique<Image>(w, h, 1);
        for (size_t j = 0; j < h; j++)
        {
            for (size_t i = 0; i < w; i++)
            {
                (newImage->getValues(0))[j*w + i] = data_ptr[j*w + i]
                    - i*vcoeff[0] - j*vcoeff[1] - vcoeff[2];
            }
        }
        return newImage;
    }

int hormodular::Orientation::getRelativeOrientation(int connector, hormodular::Orientation localOrient, hormodular::Orientation remoteOrient)
{
    //-- Create rotation matrices for local orientation:
    Eigen::AngleAxisd rollAngle( deg2rad(localOrient.getRoll()), Eigen::Vector3d::UnitZ());
    Eigen::AngleAxisd pitchAngle( deg2rad(localOrient.getPitch()), Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd yawAngle( deg2rad(localOrient.getYaw()), Eigen::Vector3d::UnitX());

    Eigen::Quaterniond q0 = yawAngle * pitchAngle * rollAngle ;
    Eigen::Matrix3d rotationMatrix = q0.matrix();

    //-- Create rotation matrices for the other orientation:
    Eigen::AngleAxisd otherRollAngle(  deg2rad(remoteOrient.getRoll()), Eigen::Vector3d::UnitZ());
    Eigen::AngleAxisd otherPitchAngle( deg2rad(remoteOrient.getPitch()), Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd otherYawAngle(   deg2rad(remoteOrient.getYaw()), Eigen::Vector3d::UnitX());

    Eigen::Quaterniond q1 = otherYawAngle * otherPitchAngle * otherRollAngle;
    Eigen::Matrix3d otherRotationMatrix = q1.matrix();

    Eigen::Matrix3d relativeRotation = rotationMatrix.inverse() * otherRotationMatrix;

    Eigen::Vector3d new_z = relativeRotation * Eigen::Vector3d::UnitZ();

//    std::cout << "New_z: " << std::endl << new_z << std::endl << std::endl;

    //-- Get connector base vector for the rotations:
    Eigen::Vector3d base_vector;
    if ( connector == 0 || connector == 2)
    {
        //-- Y axis
        base_vector = Eigen::Vector3d::UnitY();
    }
    else if ( connector == 1 || connector == 3)
    {
        //-- X axis
        base_vector = Eigen::Vector3d::UnitX();
    }

    //-- Try for rotations to fit the vector
    for ( int i = 0; i < 4; i++)
    {
        Eigen::AngleAxisd rotAngle( deg2rad(i*90), base_vector);
        Eigen::Matrix3d rotMatrix = rotAngle.matrix();
        Eigen::Vector3d result_vector = rotMatrix * Eigen::Vector3d::UnitZ();

//        std::cout << "i = " << i << std::endl << result_vector << std::endl << std::endl;

        if ( vector_equal(result_vector, new_z) )
            return i;
    }

    return -1;
}

        // overloading constructor
        ProcessLaserScan(ros::NodeHandle& nh,Eigen::Matrix3d initialPose, Eigen::Matrix3d neighborIP,std::string laserTopic)
        {
            nh_ = nh;
            
            // initialize subscriber and publisher
            scanSub_ = nh_.subscribe<sensor_msgs::LaserScan>(laserTopic, 1000, &ProcessLaserScan::laserScanCallback,this); 
            Eigen::Matrix3d relPose;

            relPose = initialPose.inverse()*neighborIP;

            minAngle = atan2(relPose(2-1,1-1),relPose(2-1,2-1));

            minRange = sqrt(pow(relPose(1-1,3-1),2)+pow(relPose(2-1,3-1),2));
        }

Vector3d get_robot_position(Vector3d position)
{
    Rb2w = att_body.normalized().toRotationMatrix();
    Rc2w = Rb2w * Rc2b;
    Rc2i = Ri2w.inverse() * Rc2w;
    //pos_i is normlized coordinate; the POS_I is the real coordinate; both are in intermidiate frame
    Vector3d pos_i, POS_I, POS_W, CAM_W;
    pos_i = Rc2i * position.normalized();
    pos_i = pos_i / pos_i.z();
    POS_I = pos_body.z() * pos_i;
    CAM_W = pos_body + Rb2w * cam_off_b;
    POS_W = Ri2w * POS_I + CAM_W;
    return POS_W;
}

kinematic_constraints::ConstraintEvaluationResult kinematic_constraints::OrientationConstraint::decide(const robot_state::RobotState &state, bool verbose) const
{
  if (!link_model_)
    return ConstraintEvaluationResult(true, 0.0);

  const robot_state::LinkState *link_state = state.getLinkState(link_model_->getName());

  if (!link_state)
  {
    logWarn("No link in state with name '%s'", link_model_->getName().c_str());
    return ConstraintEvaluationResult(false, 0.0);
  }

  Eigen::Vector3d xyz;
  if (mobile_frame_)
  {
    Eigen::Matrix3d tmp;
    tf_->transformRotationMatrix(state, desired_rotation_frame_id_, desired_rotation_matrix_, tmp);
    Eigen::Affine3d diff(tmp.inverse() * link_state->getGlobalLinkTransform().rotation());
    xyz = diff.rotation().eulerAngles(0, 1, 2);
    // 0,1,2 corresponds to ZXZ, the convention used in sampling constraints
  }
  else
  {
    Eigen::Affine3d diff(desired_rotation_matrix_inv_ * link_state->getGlobalLinkTransform().rotation());
    xyz = diff.rotation().eulerAngles(0, 1, 2); // 0,1,2 corresponds to ZXZ, the convention used in sampling constraints
  }

  xyz(0) = std::min(fabs(xyz(0)), boost::math::constants::pi<double>() - fabs(xyz(0)));
  xyz(1) = std::min(fabs(xyz(1)), boost::math::constants::pi<double>() - fabs(xyz(1)));
  xyz(2) = std::min(fabs(xyz(2)), boost::math::constants::pi<double>() - fabs(xyz(2)));
  bool result = xyz(2) < absolute_z_axis_tolerance_+std::numeric_limits<double>::epsilon()
    && xyz(1) < absolute_y_axis_tolerance_+std::numeric_limits<double>::epsilon()
    && xyz(0) < absolute_x_axis_tolerance_+std::numeric_limits<double>::epsilon();

  if (verbose)
  {
    Eigen::Quaterniond q_act(link_state->getGlobalLinkTransform().rotation());
    Eigen::Quaterniond q_des(desired_rotation_matrix_);
    logInform("Orientation constraint %s for link '%s'. Quaternion desired: %f %f %f %f, quaternion actual: %f %f %f %f, error: x=%f, y=%f, z=%f, tolerance: x=%f, y=%f, z=%f",
             result ? "satisfied" : "violated", link_model_->getName().c_str(),
             q_des.x(), q_des.y(), q_des.z(), q_des.w(),
             q_act.x(), q_act.y(), q_act.z(), q_act.w(), xyz(0), xyz(1), xyz(2),
             absolute_x_axis_tolerance_, absolute_y_axis_tolerance_, absolute_z_axis_tolerance_);
  }

  return ConstraintEvaluationResult(result, constraint_weight_ * (xyz(0) + xyz(1) + xyz(2)));
}

bool isPositivelyDecomposable(const Point_3 &a, const Point_3 &b,
		const Point_3 &c, const Point_3 &decomposed)
{
	DEBUG_START;
	Eigen::Matrix3d matrix;
	matrix << a.x(), b.x(), c.x(),
	       a.y(), b.y(), c.y(),
	       a.z(), b.z(), c.z();
	Eigen::Vector3d vector;
	vector << decomposed.x(), decomposed.y(), decomposed.z();
	Eigen::Vector3d coefficients = matrix.inverse() * vector;
	std::cout << "Tried to decompose vector, result: " << std::endl
		<< coefficients << std::endl;
	DEBUG_END;
	return coefficients(0) >= 0.
		&& coefficients(1) >= 0.
		&& coefficients(2) >= 0.;
}

bool extractPCFromColorModel(const PCRGB::Ptr& pc_in, PCRGB::Ptr& pc_out,
                             Eigen::Vector3d& mean, Eigen::Matrix3d& cov, double std_devs)
{
    double hue_weight;
    ros::param::param<double>("~hue_weight", hue_weight, 1.0);
    printf("%f\n", hue_weight);

    Eigen::Vector3d x, x_m;
    Eigen::Matrix3d cov_inv = cov.inverse();
    for(uint32_t i=0;i<pc_in->size();i++) {
        extractHSL(pc_in->points[i].rgb, x(0), x(1), x(2));
        x_m = x - mean;
        x_m(0) *= hue_weight;
        double dist = std::sqrt(x_m.transpose() * cov_inv * x_m);
        if(dist <= std_devs) 
            pc_out->points.push_back(pc_in->points[i]);
    }
}

void inverseTransform(Eigen::Vector3d &src, cspace config, Eigen::Vector3d &dest)
{
    Eigen::Matrix3d rotationC;
    rotationC << cos(config[5]), -sin(config[5]), 0,
               sin(config[5]), cos(config[5]), 0,
               0, 0, 1;
    Eigen::Matrix3d rotationB;
    rotationB << cos(config[4]), 0 , sin(config[4]),
               0, 1, 0,
               -sin(config[4]), 0, cos(config[4]);
    Eigen::Matrix3d rotationA;
    rotationA << 1, 0, 0 ,
               0, cos(config[3]), -sin(config[3]),
               0, sin(config[3]), cos(config[3]);
    Eigen::Vector3d transitionV(config[0], config[1], config[2]);
    Eigen::Matrix3d rotationM = rotationC * rotationB * rotationA;
    dest = rotationM.inverse() * (src - transitionV);
}