C++ Vector2d::y方法代码示例

Eigen::Vector2d CircularGroundPath::RelativePositionCalculator::localCoordinates( Eigen::Vector3d const& _center, Eigen::Vector2d global_coordinates )
{
  Eigen::Vector2d local;
  local.x() = global_coordinates.x() - _center.x();
  local.y() = global_coordinates.y() - _center.y();
  return local;
}

    /**
     * Computes the undistorted image coordinates of an input pixel via
     * bilinear interpolation of its integer-coordinate 4-neighboors.
     *
     * \param dist_uv input distorted pixel coordinates.
     * \param rect_uv output parameter.
     */
    void rectifyBilinearLookup(const Eigen::Vector2d& dist_uv,
                               Eigen::Vector2d* rect_uv) const {
      int u = (int)dist_uv.x();
      int v = (int)dist_uv.y();

      assert(u >= 0 && v >= 0 && u < _input_camera.width && v < _input_camera.height);

      // weights
      float wright  = (dist_uv.x() - u);
      float wbottom = (dist_uv.y() - v);
      float w[4] = {
        (1 - wright) * (1 - wbottom),
        wright * (1 - wbottom),
        (1 - wright) * wbottom,
        wright * wbottom
      };

      int ra_index = v * _input_camera.width + u;
      uint32_t neighbor_indices[4] = {
        ra_index,
        ra_index + 1,
        ra_index + _input_camera.width,
        ra_index + _input_camera.width + 1
      };

      rect_uv->x() = 0;
      rect_uv->y() = 0;
      for(int i = 0; i < 4; ++i) {
        rect_uv->x() += w[i] * _map_x[neighbor_indices[i]];
        rect_uv->y() += w[i] * _map_y[neighbor_indices[i]];
      }
    }

void drawEllipse(cv::Mat & im, const Eigen::Matrix2d & A, const Eigen::Vector2d & b, const double sigma, const cv::Scalar col, const int thickness, const bool bMarkCentre)
{
    if(bMarkCentre)
    {
        const cv::Point centre(cvRound(b.x()), cvRound(b.y()));
        cv::circle(im, centre, 2, col, -1);
    }
    
    const int nNumPoints = 100;
    
    const Eigen::Matrix2d rootA = matrixSqrt(A);
    
    cv::Point p_last;
    for(int i=0;i<nNumPoints;i++)
    {
        const double dTheta = i*(2*M_PI/nNumPoints);
        const Eigen::Vector2d p1 (sin(dTheta), cos(dTheta));
        
        const Eigen::Vector2d p_sigma = p1 * sigma;
        
        const Eigen::Vector2d p_trans = rootA*p_sigma + b;
        const cv::Point p_im(cvRound(p_trans.x()), cvRound(p_trans.y()));
        
        if(i>0)
            cv::line(im, p_last, p_im, col, thickness);
        
        p_last = p_im;
    }
}

/**
 * @brief The 'regression2D' method can be used to fit a line to a given point set.
 * @param points_begin      set begin iterator
 * @param points_end        set end iterator
 * @param fit_start         the start of the line fit
 * @param fit_end           the set termintating iterator position
 * @param line              the parameterized line to work with
*/
inline void regression2D(const std::vector<LaserBeam>::const_iterator &points_begin, const std::vector<LaserBeam>::const_iterator &points_end,
                         Eigen::ParametrizedLine<double, 2> &line)
{
    std::vector<LaserBeam>::const_iterator back_it = points_end;
    --back_it;
    Eigen::Vector2d front (points_begin->posX(), points_end->posY());
    Eigen::Vector2d back (back_it->posX(), back_it->posY());

    unsigned int size = std::distance(points_begin, points_end);
    Eigen::MatrixXd A(size, 2);
    Eigen::VectorXd b(size);
    A.setOnes();

    Eigen::Vector2d dxy = (front - back).cwiseAbs();
    bool solve_for_x = dxy.x() > dxy.y();
    if(solve_for_x) {
        /// SOLVE FOR X
        int i = 0;
        for(std::vector<LaserBeam>::const_iterator it = points_begin ; it != points_end ; ++it, ++i)
        {
            A(i,1) = it->posX();
            b(i)   = it->posY();
        }
    } else {
        /// SOLVE FOR Y
        int i = 0;
        for(std::vector<LaserBeam>::const_iterator it = points_begin ; it != points_end ; ++it, ++i)
        {
            A(i,1) = it->posY();
            b(i)   = it->posX();
        }
    }

    Eigen::VectorXd weights = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
    double          alpha   = weights(0);
    double          beta    = weights(1);
    Eigen::Vector2d from;
    Eigen::Vector2d to;

    if(solve_for_x) {
        from(0) = 0.0;
        from(1) = alpha;
        to(0)   = 1.0;
        to(1)   = alpha + beta;
    } else {
        from(0) = alpha;
        from(1) = 0.0;
        to(0)   = alpha + beta;
        to(1)   = 1.0;
    }

    Eigen::Vector2d fit_start;
    Eigen::Vector2d fit_end;
    line = Eigen::ParametrizedLine<double, 2>(from, (to - from).normalized());
    fit_start = line.projection(front);
    fit_end   = line.projection(back);
    line = Eigen::ParametrizedLine<double, 2>(fit_start, (fit_end - fit_start));
}

void ElevationVisualization::visualize(
    const grid_map::GridMap& map,
    const std::string& typeNameElevation,
    const std::string& typeNameColor,
    const float lowerValueBound,
    const float upperValueBound)
{
  // Set marker info.
  marker_.header.frame_id = map.getFrameId();
  marker_.header.stamp.fromNSec(map.getTimestamp());
  marker_.scale.x = map.getResolution();
  marker_.scale.y = map.getResolution();

  // Clear points.
  marker_.points.clear();
  marker_.colors.clear();

  float markerHeightOffset = static_cast<float>(markerHeight_/2.0);

  const Eigen::Array2i buffSize = map.getBufferSize();
  const Eigen::Array2i buffStartIndex = map.getBufferStartIndex();
  const bool haveColor = map.isType("color");
  for (unsigned int i = 0; i < buffSize(0); ++i)
  {
    for (unsigned int j = 0; j < buffSize(1); ++j)
    {
      // Getting map data.
      const Eigen::Array2i cellIndex(i, j);
      const Eigen::Array2i buffIndex =
          grid_map_lib::getBufferIndexFromIndex(cellIndex, buffSize, buffStartIndex);
      const float& elevation = map.at(typeNameElevation, buffIndex);
      if(std::isnan(elevation))
        continue;
      const float color = haveColor ? map.at(typeNameColor, buffIndex) : lowerValueBound;

      // Add marker point.
      Eigen::Vector2d position;
      map.getPosition(buffIndex, position);
      geometry_msgs::Point point;
      point.x = position.x();
      point.y = position.y();
      point.z = elevation - markerHeightOffset;
      marker_.points.push_back(point);

      // Add marker color.
      if(haveColor)
      {
        std_msgs::ColorRGBA markerColor =
            grid_map_visualization::color_utils::interpolateBetweenColors(
              color, lowerValueBound, upperValueBound, lowerColor_, upperColor_);
        marker_.colors.push_back(markerColor);
      }
    }
  }

  markerPublisher_.publish(marker_);
}

Edge * IEdgesExtractor::segmentalize(double m, double q, const Eigen::Vector2d & mi, const Eigen::Vector2d & ma) const{

	Eigen::Vector2d min;
	//Position min;
	Eigen::Vector2d max;
	//Position max;

	if(m < 0){
// 		min.setX(mi.x());
// 		min.setY(ma.y());
// 		max.setX(ma.x());
// 		max.setY(mi.y());
			min[0] = mi.x();
			min[1] = ma.y();
			max[0] = ma.x();
			max[1] = mi.y();
			
	}else{
// 		min.setX(mi.x());
// 		min.setY(mi.y());
// 		max.setX(ma.x());
// 		max.setY(ma.y());
			min[0] = mi.x();
			min[1] = mi.y();
			max[0] = ma.x();
			max[1] = ma.y();
	}
	
	float mm = m*m;
	float mq = m*q;

	float x1 = (min.x() + m*min.y() - mq) / (1+mm);
	float y1 = (min.x()*m + mm*min.y() + q) / (1+mm);
	float x2 = (max.x() + m*max.y() - mq) / (1+mm);
	float y2 = (max.x()*m + mm*max.y() + q) / (1+mm);

	Vertex* v1 = new Vertex(-1, x1, y1);
	Vertex* v2 = new Vertex(-1, x2, y2);
	return new Edge(0, v1, v2);

}

void displayopencv::DisplayBspline(const mathtools::application::Bspline<2> &bspline, cv::Mat &img, const mathtools::affine::Frame<2>::Ptr frame, const cv::Scalar &color, const unsigned int &nb_pts)
{
	double inf = bspline.getInfBound(),
		   sup = bspline.getSupBound();
	
	Eigen::Vector2d prev = mathtools::affine::Point<2>(bspline(inf)).getCoords(frame);

	for(unsigned int i=1;i<nb_pts;i++)
	{
		double t = inf + (double)i*(sup-inf)/(double)(nb_pts-1);

		Eigen::Vector2d curr = mathtools::affine::Point<2>(bspline(t)).getCoords(frame);

		cv::Point pt1(prev.x(), prev.y()),
				  pt2(curr.x(), curr.y());
		
		prev=curr;

		cv::line(img,pt1,pt2,color);
	}
}

void drawCurve(const SCurve* in_curve, const SVar& in_Vars)
{
	Eigen::Vector2d center = Eigen::Vector2d::Zero();

	glColor3f(0.2, 0.7, 0.2);
	::glLineWidth(4.0);
	::glBegin(GL_LINE_STRIP);

	for(int i=0; i<in_curve->nVertices; i++)
		glVertex2d(in_Vars.pos.col(i).x() - center.x(), in_Vars.pos.col(i).y() - center.y());
	if(in_curve->closed)
		glVertex2d(in_Vars.pos.col(0).x() - center.x(), in_Vars.pos.col(0).y() - center.y());
	::glEnd();

	// Inserted points
	::glPointSize(5.0); //3.0
	::glBegin(GL_POINTS);
	for(int i=0; i<in_curve->nVertices; i++)
	{
		if(in_curve->vertexIDs(i) < 0)
		{
			glColor3f(0.2, 0.7, 0.2);
			glVertex2d(in_Vars.pos.col(i).x() - center.x(), in_Vars.pos.col(i).y() - center.y());
		}	
	}
	::glEnd();

	// The input feature points
	::glPointSize(10.0); //5.0
	::glBegin(GL_POINTS);
	for(int i=0; i<in_curve->nVertices; i++)
	{
		if(in_curve->vertexIDs(i) >= 0)
		{
			glColor3f(0.7, 0.2, 0.2);	
			glVertex2d(in_Vars.pos.col(i).x() - center.x(), in_Vars.pos.col(i).y() - center.y());
		}
	}
	::glEnd();
}

void VertexCell::drawRawTopology(Time time, ViewSettings & viewSettings)
{
    bool screenRelative = viewSettings.screenRelative();
    if(screenRelative)
    {
        int n = 50;
        Eigen::Vector2d p = pos(time);
        glBegin(GL_POLYGON);
        {
            double r = 0.5 * viewSettings.vertexTopologySize() / viewSettings.zoom();
            //if(r == 0) r = 3;
            //else if (r<1) r = 1;
            for(int i=0; i<n; ++i)
            {
                double theta = 2 * (double) i * 3.14159 / (double) n ;
                glVertex2d(p.x() + r*std::cos(theta),p.y()+ r*std::sin(theta));
            }
        }
        glEnd();
    }
    else
    {
        int n = 50;
        Eigen::Vector2d p = pos(time);
        glBegin(GL_POLYGON);
        {
            double r = 0.5 * viewSettings.vertexTopologySize();
            if(r == 0) r = 3;
            else if (r<1) r = 1;
            for(int i=0; i<n; ++i)
            {
                double theta = 2 * (double) i * 3.14159 / (double) n ;
                glVertex2d(p.x() + r*std::cos(theta),p.y()+ r*std::sin(theta));
            }
        }
        glEnd();
    }
}

void mesh_core::LineSegment2D::initialize(
      const Eigen::Vector2d& a,
      const Eigen::Vector2d& b)
{
  pt_[0] = a;
  pt_[1] = b;
  Eigen::Vector2d delta = b - a;

  if (std::abs(delta.x()) <= std::numeric_limits<double>::epsilon())
  {
    vertical_ = true;
    if (std::abs(delta.y()) <= std::numeric_limits<double>::epsilon())
      inv_dx_ = 0.0;
    else
      inv_dx_ = 1.0 / delta.y();
  }
  else
  {
    vertical_ = false;
    inv_dx_ = 1.0 / delta.x();
    slope_ = delta.y() * inv_dx_;
    y_intercept_ = a.y() - slope_ * a.x();
  }
}

/**
 * @brief Retrieve the cartesian point coordinates of a range reading.
 * @param reading           the range reading
 * @param angular_res       the angular resolution of the range reading
 * @param min_angle         the minimum angle of the range reading
 * @param points            the point coordiantes in cartesian space
 */
inline void polarToCartesian(const std::vector<float> &reading, const float angular_res, const float min_angle,
                             const float min_rho, const float max_rho, std::vector<LaserBeam> &points)
{
    points.clear();
    double angle = min_angle;
    for(std::vector<float>::const_iterator it = reading.begin() ; it != reading.end() ; ++it, angle += angular_res) {
        float rho = *it;
        if(rho < max_rho && rho > min_rho && rho > 0.0) {
            Eigen::Vector2d pt;
            pt.x() = std::cos(angle) * rho;
            pt.y() = std::sin(angle) * rho;
            points.push_back(pt);
        }

    }
}

void Camera::pan(Eigen::Vector2i oldPosition, Eigen::Vector2i newPosition){

  const Eigen::Vector2d delta = 
	(newPosition - oldPosition).eval().template cast<double>();
  
  const Eigen::Vector3d forwardVector = lookAt - position;
  const Eigen::Vector3d rightVector = forwardVector.cross(up);
  const Eigen::Vector3d upVector = rightVector.cross(forwardVector);
  
  const double scale = 0.0005;
  
  position += scale*(-delta.x()*rightVector +
	  delta.y()*upVector);
  

}

void VertexCell::drawPickCustom(Time time, ViewSettings & /*viewSettings*/)
{
    if(!exists(time))
        return;

    int n = 50;
    Eigen::Vector2d p = pos(time);
    glBegin(GL_POLYGON);
    {
        double r = 0.5 * size(time);
        for(int i=0; i<n; ++i)
        {
            double theta = 2 * (double) i * 3.14159 / (double) n ;
            glVertex2d(p.x() + r*std::cos(theta),p.y()+ r*std::sin(theta));
        }
    }
    glEnd();
}

void DepthObstacleGrid::initializeStatics(NodeMap *map){
  
  Environment env = map->getEnvironment();
  
  for(std::vector<Plane>::iterator it = env.planes.begin(); it != env.planes.end(); it++){
    
    double plane_length = Eigen::Vector2d( it->span_horizontal.x(), it->span_horizontal.y() ).norm();
    Eigen::Vector2d step = Eigen::Vector2d( it->span_horizontal.x(), it->span_horizontal.y() ) / (plane_length / (0.5 * resolution) );
    double step_length = step.norm();
        
    Eigen::Vector2d lastCell(NAN, NAN);
    Eigen::Vector2d pos = Eigen::Vector2d(it->position.x(), it->position.y());
    
    //iterate through the cells
    for(double i = 0.0; i < plane_length; i += step_length){
      
      Eigen::Vector2d cell_coord = getGridCoord( pos.x(), pos.y() );
      
      //step was not wide enough, we are still in the same cell
      if(cell_coord != lastCell){       
      
        lastCell = cell_coord;
        
        Eigen::Vector2i ID = getCellID(cell_coord.x(), cell_coord.y());
        
        //Step was invalid, we are outside the grid
        if(ID.x() == -1){
          pos += step;
          continue;
        }
        
        //Set the cell static
        GridElement &elem = get(ID.x(), ID.y());      
        elem.static_obstacle = true;
                
      }
      
      pos += step;
      
    }
    
  }
  
}

const Eigen::Vector2d CMiniVisionToolbox::getPointDistortedPlumbBob( const Eigen::Vector2d& p_vecPointUndistorted, const Eigen::Vector2d& p_vecPointPrincipal, const Eigen::Vector4d& p_vecDistortionCoefficients )
{
    const int32_t iUndistortedX( std::abs( p_vecPointUndistorted.x( ) ) );
    const int32_t iUndistortedY( std::abs( p_vecPointUndistorted.y( ) ) );

    //ds current ranges
    const int32_t iXStart( iUndistortedX-40 );
    const int32_t iXStop( iUndistortedX+40 );
    const int32_t iYStart( iUndistortedY-40 );
    const int32_t iYStop( iUndistortedY+40 );

    double dErrorLowest( 10.0 );
    Eigen::Vector2d vecPointDistorted( 0, 0 );

    //ds loop around the current point
    for( int32_t i = iXStart; i < iXStop; ++i )
    {
        for( int32_t j = iYStart; j < iYStop; ++j )
        {
            /*ds get current error
            const Eigen::Vector2d vecError( p_vecPointUndistorted-CMiniVisionToolbox::getPointUndistortedPlumbBob( Eigen::Vector2i( iUndistortedX, iUndistortedY ), p_vecPointPrincipal, p_vecDistortionCoefficients ) );

            //ds error
            const double dErrorCurrent( std::fabs( vecError(0) ) + std::fabs( vecError(1) ) );

            //ds if the error is lower
            if( dErrorLowest > dErrorCurrent )
            {
                vecPointDistorted = Eigen::Vector2d( iUndistortedX, iUndistortedY );
            }*/
        }
    }

    std::printf( "error: %f\n", dErrorLowest );

    return vecPointDistorted;
}