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

void ObjectModelLine::projectPoints (const std::vector<int> &inliers, const Eigen::VectorXd &model_coefficients,
		PointCloud3D* projectedPointCloud){


	assert (model_coefficients.size () == 6);

	// Obtain the line point and direction
	Eigen::Vector4d line_pt  (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
	Eigen::Vector4d line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);

	// Iterate through the 3d points and calculate the distances from them to the line
	for (size_t i = 0; i < inliers.size (); ++i)
	{
		Eigen::Vector4d pt ((*inputPointCloud->getPointCloud())[inliers[i]].getX(), (*inputPointCloud->getPointCloud())[inliers[i]].getY(),
				(*inputPointCloud->getPointCloud())[inliers[i]].getZ(), 0);
		// double k = (DOT_PROD_3D (points[i], p21) - dotA_B) / dotB_B;
		double k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir);

		Eigen::Vector4d pp = line_pt + k * line_dir;
		// Calculate the projection of the point on the line (pointProj = A + k * B)
//		std::vector<Point3D> *projectedPoints = projectedPointCloud->getPointCloud();
		(*projectedPointCloud->getPointCloud())[i].setX(pp[0]);
		(*projectedPointCloud->getPointCloud())[i].setY(pp[1]);
		(*projectedPointCloud->getPointCloud())[i].setZ(pp[2]);
	}

}

Eigen::Vector3d
StereoDisparity::getXyzValues(int u, int v, float disparity)
{
  Eigen::Vector4d uvd1((double) u, (double) v, disparity, 1);
  Eigen::Vector4d xyzw = (*_uvd1_to_xyz) * uvd1;
  return xyzw.head<3>() / xyzw.w();
}

 inline sm::kinematics::Transformation transformationFromPropertyTree(const sm::ConstPropertyTree & config)
 {
     Eigen::Vector4d q = sm::eigen::quaternionFromPropertyTree( sm::ConstPropertyTree(config, "q_a_b") );
     q = q / q.norm();
     Eigen::Vector3d t = sm::eigen::vector3FromPropertyTree( sm::ConstPropertyTree(config, "t_a_b_a" ) );
     return sm::kinematics::Transformation(q,t);
 }

void BezierBoundarySolver::_IterateCurvatureReduction(BezierBoundaryProblem* pProblem,Eigen::Vector4d& params)
{
    double epsilon = 0.0001;
    //create a jacobian for the parameters by perturbing them
    Eigen::Vector4d Jt; //transpose of the jacobian
    BezierBoundaryProblem origProblem = *pProblem;
    double maxK = _GetMaximumCurvature(pProblem);
    for(int ii = 0; ii < 4 ; ii++){
        Eigen::Vector4d epsilonParams = params;
        epsilonParams[ii] += epsilon;
        _Get5thOrderBezier(pProblem,epsilonParams);
        _Sample5thOrderBezier(pProblem);
        double kPlus = _GetMaximumCurvature(pProblem);

        epsilonParams[ii] -= 2*epsilon;
        _Get5thOrderBezier(pProblem,epsilonParams);
        _Sample5thOrderBezier(pProblem);
        double kMinus = _GetMaximumCurvature(pProblem);
        Jt[ii] = (kPlus-kMinus)/(2*epsilon);
    }

    //now that we have Jt, we can calculate JtJ
    Eigen::Matrix4d JtJ = Jt*Jt.transpose();
    //thikonov regularization
    JtJ += Eigen::Matrix4d::Identity();

    Eigen::Vector4d deltaParams = JtJ.inverse() * Jt*maxK;
    params -= deltaParams;
    _Get5thOrderBezier(pProblem,params);
    _Sample5thOrderBezier(pProblem);
    //double finalMaxK = _GetMaximumCurvature(pProblem);

    //dout("2D Iteration took k from " << maxK << " to " << finalMaxK);
}

    static void qprod(const Eigen::Vector4d & q, const Eigen::Vector4d & p, Eigen::Vector4d & prod_result) {
        double a=q(0);
        Eigen::Vector3d v = q.segment(1, 3); //coefficients q
        double x=p(0);
        Eigen::Vector3d u = p.segment(1, 3); //coefficients p

        prod_result << a*x-v.transpose()*u, (a*u+x*v) + v.cross(u);
    }

 void ExpMapQuaternion::pseudoLog_(RefVec out, const ConstRefVec& x, const ConstRefVec& y)
 {
   Eigen::Vector4d tmp;
   toQuat q(tmp.data());
   const toConstQuat xQ(x.data());
   const toConstQuat yQ(y.data());
   q = xQ.inverse()*yQ; //TODO double-check that formula
   logarithm(out,tmp);
 }

 Eigen::Vector3d PointcloudProc::projectPoint(Eigen::Vector4d& point, CamParams cam) const
 {
   Eigen::Vector3d keypoint;
   
   keypoint(0) = (cam.fx*point.x()) / point.z() + cam.cx;
   keypoint(1) = (cam.fy*point.y()) / point.z() + cam.cy;
   keypoint(2) = (cam.fx*(point.x()-cam.tx)/point.z() + cam.cx);
   
   return keypoint;
 }

Eigen::Vector3d intersectRayPlane(const Eigen::Vector3d ray,
  const Eigen::Vector3d ray_origin, const Eigen::Vector4d plane)
{
    // Plane defined as ax + by + cz + d = 0
    // Ray: P = P0 + tV
    // Plane: P.N + d = 0, where P is intersection point
    // t = -(P0.N + d)/(V.N) , P = P0 + tV
    float t = -(ray_origin.dot(plane.head(3)) + plane[3]) / (ray.dot(plane.head(3)));
    Eigen::Vector3d P = ray_origin + t*ray;
    return P;
}

void randomUnitQuaternion(Eigen::Vector4d &quat) {
    static boost::random::mt19937 rng(time(NULL));
    static boost::random::normal_distribution<> normal;

    do {
        quat(0) = normal(rng);
        quat(1) = normal(rng);
        quat(2) = normal(rng);
        quat(3) = normal(rng);
    } while (quat.norm() < 0.00001);
    quat.normalize();
}

const double CMiniVisionToolbox::getTransformationDelta( const Eigen::Isometry3d& p_matTransformationFrom, const Eigen::Isometry3d& p_matTransformationTo )
{
    //ds compute pose change
    const Eigen::Isometry3d matTransformChange( p_matTransformationTo*p_matTransformationFrom.inverse( ) );

    //ds check point
    const Eigen::Vector4d vecSamplePoint( 40.2, -1.25, 2.5, 1.0 );
    double dNorm( vecSamplePoint.norm( ) );
    const Eigen::Vector4d vecDifference( vecSamplePoint-matTransformChange*vecSamplePoint );
    dNorm = ( dNorm + vecDifference.norm( ) )/2;

    //ds return norm
    return ( std::fabs( vecDifference(0) ) + std::fabs( vecDifference(1) ) + std::fabs( vecDifference(2) ) )/dNorm;
}

bool WHeadPositionCorrection::checkMovementThreshold( const WLEMDHPI::TransformationT& trans )
{
    const Eigen::Vector4d diffTrans = m_transExc.data().col( 3 ) - trans.data().col( 3 );
    if( diffTrans.norm() > m_movementThreshold )
    {
        m_transExc = trans;
        return true;
    }
    // TODO(pieloth): check rotation, e.g. with 1 or more key points
    // initial: pick 6 key points from dipoles min/max (x,0,0); (0,y,0); (0,0,z)
    // compare max distance
    // keyPoint 3x6
    // (trans * keyPoints).colwise().norm() - (m_transExc * keyPoints).colwise().norm()).maxCoeff(), mind homog. coord.
    return false;
}

mathtools::geometry::euclidian::Line<4> skeleton::model::Perspective::toObj(
		const Eigen::Matrix<double,skeleton::model::meta<skeleton::model::Projective>::stordim,1> &vec,
		const mathtools::geometry::euclidian::Line<4> &) const
{
	Eigen::Vector4d origin;
	origin.block<3,1>(0,0) = m_frame3->getOrigin();
	origin(3) = 0.0;

	Eigen::Vector4d vecdir;
	vecdir.block<3,1>(0,0) = m_frame3->getBasis()->getMatrix()*Eigen::Vector3d(vec(0),vec(1),1.0);
	vecdir(3) = vec(2);
	vecdir.normalize();

	return mathtools::geometry::euclidian::Line<4>(origin,vecdir);
}

Visualization::Visualization(bot_lcmgl_t* lcmgl, const StereoCalibration* calib)
  : _lcmgl(lcmgl),
    _calibration(calib)
{
  // take the  Z corresponding to disparity 5 px as 'max Z'
  Eigen::Matrix4d uvdtoxyz = calib->getUvdToXyz();
  Eigen::Vector4d xyzw = uvdtoxyz * Eigen::Vector4d(1, 1, 5, 1);
  xyzw /= xyzw.w();
  _max_z = xyzw.z();

  // take the  Z corresponding to 3/4 disparity img width px as 'min Z'
  xyzw = uvdtoxyz * Eigen::Vector4d(1, 1, (3*calib->getWidth())/4, 1);
  xyzw /= xyzw.w();
  _min_z = xyzw.z();
}

  void Node::motors_cb(const controller::Motors::ConstPtr& msg)
  {
    //get quad charectaristics
    float d = msg->d;
    float kf = msg->kf;
    float kt = msg->kt;

    //get motor speeds (rad/s)
    const Eigen::Vector4d motor_speeds(
        msg->motor1,
        msg->motor2,
        msg->motor3,
        msg->motor4);

    //calculate forces and moments
    const Eigen::Vector4d motor_thrust = kf * motor_speeds.cwiseProduct(motor_speeds);
    Eigen::Matrix4d convert;
    convert << 1, 1, 1, 1,
               0, d, 0, -d,
               -d, 0, d, 0,
               kt, -kt, kt, -kt;
    
    const Eigen::Vector4d f_moments = convert * motor_thrust;

    //publish message
    geometry_msgs::Twist out;
    
    out.linear.z = f_moments[0];
    out.angular.x = f_moments[1];
    out.angular.y = f_moments[2];
    out.angular.z = f_moments[3];

    force_pub_.publish(out);
  }

Eigen::Vector2i
pcl::visualization::worldToView (const Eigen::Vector4d &world_pt, const Eigen::Matrix4d &view_projection_matrix, int width, int height)
{
  // Transform world to clipping coordinates
  Eigen::Vector4d world (view_projection_matrix * world_pt);
  // Normalize w-component
  world /= world.w ();

  // X/Y screen space coordinate
  int screen_x = int (floor (double (((world.x () + 1) / 2.0) * width) + 0.5));
  int screen_y = int (floor (double (((world.y () + 1) / 2.0) * height) + 0.5));

  // Calculate -world_pt.y () because the screen Y axis is oriented top->down, ie 0 is top-left
  //int winY = (int) floor ( (double) (((1 - world_pt.y ()) / 2.0) * height) + 0.5); // top left

  return (Eigen::Vector2i (screen_x, screen_y));
}