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

// @from http://listengine.tuxfamily.org/lists.tuxfamily.org/eigen/2010/01/msg00173.html
static bool pinv(const Eigen::Matrix2d& a, Eigen::Matrix2d& a_pinv)
{
    // see : http://en.wikipedia.org/wiki/Moore-Penrose_pseudoinverse#The_general_case_and_the_SVD_method

    if (a.rows() < a.cols())
        return false;

    // SVD
    Eigen::JacobiSVD<Eigen::Matrix2d> svdA(a, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::Vector2d vSingular = svdA.singularValues();

    // Build a diagonal matrix with the Inverted Singular values
    // The pseudo inverted singular matrix is easy to compute :
    // is formed by replacing every nonzero entry by its reciprocal (inversing).
    Eigen::Vector2d vPseudoInvertedSingular(svdA.matrixV().cols(),1);

    for (int iRow = 0; iRow < vSingular.rows(); iRow++)
    {
        if (fabs(vSingular(iRow)) <= 1e-10) // Todo : Put epsilon in parameter
        {
            vPseudoInvertedSingular(iRow, 0) = 0.;
        }
        else
        {
            vPseudoInvertedSingular(iRow, 0) = 1. / vSingular(iRow);
        }
    }

    // A little optimization here
    Eigen::Matrix2d mAdjointU = svdA.matrixU().adjoint().block(0, 0, vSingular.rows(), svdA.matrixU().adjoint().cols());

    // Pseudo-Inversion : V * S * U'
    a_pinv = (svdA.matrixV() * vPseudoInvertedSingular.asDiagonal()) * mAdjointU;

    return true;
}

void SelfRobot::selfLandmarkDataCallback(const read_omni_dataset::LRMLandmarksData::ConstPtr& landmarkData, int RobotNumber)
{
  ROS_WARN(" got landmark data from self robot (ID=%d)",RobotNumber);  
  
  uint seq = landmarkData->header.seq;

  // There are a total of 10 distinct, known and static landmarks in this dataset.
  for(int i=0;i<10; i++)
  {
    if(landmarkData->found[i])
    {
    
      ///Below is the procedure to calculate the observation covariance associate with the ball measurement made by the robots. Caution: Make sure the validity of the calculations below by crosschecking the obvious things, e.g., covariance cannot be negative or very close to 0     
      
      Eigen::Vector2d tempLandmarkObsVec = Eigen::Vector2d(landmarkData->x[i],landmarkData->y[i]);

      double d = tempLandmarkObsVec.norm(),
	     phi = atan2(landmarkData->y[i],landmarkData->x[i]);
      
      double covDD = (K1*fabs(1.0-(landmarkData->AreaLandMarkActualinPixels[i]/landmarkData->AreaLandMarkExpectedinPixels[i])))*(d*d);
      double covPhiPhi = K2*(1/(d+1));
      
      double covXX = pow(cos(phi),2) * covDD 
				  + pow(sin(phi),2) * ( pow(d,2) * covPhiPhi + covDD * covPhiPhi );
      double covYY = pow(sin(phi),2) * covDD 
				  + pow(cos(phi),2) * ( pow(d,2) * covPhiPhi + covDD * covPhiPhi );
      
      ROS_INFO("Landmark %d found in the image, refer to the method to see how covariances are calculated",i);  
    }
  }
  
 }

void SelfRobot::selfTargetDataCallback(const read_omni_dataset::BallData::ConstPtr& ballData, int RobotNumber)
{
  ROS_WARN("Got ball data from self robot %d",RobotNumber);  
  Time curObservationTime = ballData->header.stamp;
  
  if(ballData->found)
  {    
    ///Below is the procedure to calculate the observation covariance associate with the ball measurement made by the robots. Caution: Make sure the validity of the calculations below by crosschecking the obvious things, e.g., covariance cannot be negative or very close to 0
    
    Eigen::Vector2d tempBallObsVec = Eigen::Vector2d(ballData->x,ballData->y);
    
    double d = tempBallObsVec.norm(),
	   phi = atan2(ballData->y,ballData->x);
    
    double covDD = (double)(1/ballData->mismatchFactor)*(K3*d + K4*(d*d));
    double covPhiPhi = K5*(1/(d+1));
    
    double covXX = pow(cos(phi),2) * covDD 
				+ pow(sin(phi),2) * ( pow(d,2) * covPhiPhi + covDD * covPhiPhi );
    double covYY = pow(sin(phi),2) * covDD 
				+ pow(cos(phi),2) * ( pow(d,2) * covPhiPhi + covDD * covPhiPhi );
    ROS_INFO("Ball found in the image, refer to the method to see how covariances are calculated");	
    
  }
  else
  {
    ROS_INFO("Ball not found in the image");
  }
  
}

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

bool ProbabilisticStereoTriangulator<CAMERA_GEOMETRY_T>::computeReprojectionError4(
    const std::shared_ptr<okvis::MultiFrame>& frame, size_t camId,
    size_t keypointId, const Eigen::Vector4d& homogeneousPoint,
    double& outError) const {

  OKVIS_ASSERT_LT_DBG(Exception, keypointId, frame->numKeypoints(camId),
      "Index out of bounds");
  Eigen::Vector2d y;
  okvis::cameras::CameraBase::ProjectionStatus status = frame
      ->geometryAs<CAMERA_GEOMETRY_T>(camId)->projectHomogeneous(
      homogeneousPoint, &y);
  if (status == okvis::cameras::CameraBase::ProjectionStatus::Successful) {
    Eigen::Vector2d k;
    Eigen::Matrix2d inverseCov = Eigen::Matrix2d::Identity();
    double keypointStdDev;
    frame->getKeypoint(camId, keypointId, k);
    frame->getKeypointSize(camId, keypointId, keypointStdDev);
    keypointStdDev = 0.8 * keypointStdDev / 12.0;
    inverseCov *= 1.0 / (keypointStdDev * keypointStdDev);

    y -= k;
    outError = y.dot(inverseCov * y);
    return true;
  } else
    return false;
}

  void
  LaserSensorModel2D::setInformationForVertexPoint(EdgeSE2PointXYCov*& edge, VertexPointXYCov*& point, LaserSensorParams& params)
  {
    double beamAngle = atan2(edge->measurement()(1), edge->measurement()(0));
    ///calculate incidence angle in point frame
    Eigen::Vector2d beam = edge->measurement();

    Eigen::Vector2d normal = point->normal();
    beam = beam.normalized();
    normal = normal.normalized();
    double incidenceAngle = 0.0;
    if(point->covariance() != Eigen::Matrix2d::Identity())
      incidenceAngle = acos(normal.dot(beam));

    double d = (tan(params.angularResolution * 0.5) * edge->measurement().norm());
    ///uncertainty of the surface measurement in direction of the beam
    double dk = fabs(params.scale * (d / cos(incidenceAngle)));
    edge->information().setIdentity();
    edge->information()(0,0) = 1.0 / ((dk + params.sensorPrecision) * (dk + params.sensorPrecision));
    double cError = 0.001 * edge->measurement().norm();
    edge->information()(1,1) = 1.0 / (cError * cError);

    Eigen::Rotation2Dd rot(beamAngle);
    Eigen::Matrix2d mrot = rot.toRotationMatrix();
    edge->information() = mrot * edge->information() * mrot.transpose();
  }

std::vector<cv::DMatch> Registrator3P::getInliers(const ptam::KeyFrame& kfa, const ptam::KeyFrame& kfb,
                                                  const std::vector<cv::DMatch>& matches,
                                                  const Sophus::SE3d& relPoseAB,
                                                  double threshold,
                                                  std::vector<Observation>& obs)
{
    Sophus::SE3d relPoseBA = relPoseAB.inverse();
    std::vector<cv::DMatch> inliers;
    double thresh2 = threshold*threshold;
    for (uint i = 0; i < matches.size(); i++) {
        const cv::DMatch& m = matches[i];

        const Eigen::Vector3d& mpa   = cs_geom::toEigenVec(kfa.mapPoints[m.queryIdx]->v3RelativePos);
        cv::Point2f     imbcv;
        int scale = 1;// << kfb.keypoints[m.trainIdx].octave;
        imbcv.x = kfb.keypoints[m.trainIdx].pt.x * scale;
        imbcv.y = kfb.keypoints[m.trainIdx].pt.y * scale;
        Eigen::Vector2d ipb(imbcv.x, imbcv.y);

        Eigen::Vector2d err = cam_[kfb.nSourceCamera].project3DtoPixel(relPoseBA*mpa) - ipb;;

        if (err.dot(err) < thresh2) {
            inliers.push_back(m);
            obs.push_back(Observation(mpa, cam_[kfb.nSourceCamera].unprojectPixel(ipb)));
        }
    }

    return inliers;
}

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

void ConvexPolygon::calculateMatrixForm() {

    //every two adjacent endpoints define a line -> inequality constraint
    //first, resize A and b. x and b are column vectors
    this->A = Eigen::MatrixXd (this->endpoints.size(),2);
    this->b = Eigen::VectorXd (this->endpoints.size());
    Eigen::Vector2d normal;

    for(size_t i=1; i<endpoints.size(); i++) {
	//to define the correct line direction, we also need a point on the inside of the constraint - the seed
	normal(0) = endpoints[i-1](1) - endpoints[i](1);
	normal(1) = endpoints[i](0) - endpoints[i-1](0);
	if(normal.dot(seed) > 0) { //we want the outward pointing normal, so n.dot(s) < 0
	    normal = -normal;
	}
	normal.normalize();
	b(i-1) = -endpoints[i].dot(normal); //endpoints[i];
	A(i-1,0) = normal(0);
	A(i-1,1) = normal(1);
    }
    normal(0) = endpoints.back()(1) - endpoints.front()(1);
    normal(1) = endpoints.front()(0) - endpoints.back()(0);
    if(normal.dot(seed) > 0) { //we want the outward pointing normal, so n.dot(s) < 0
	normal = -normal;
    }
    normal.normalize();
    b(endpoints.size()-1) = -endpoints.front().dot(normal); //endpoints[i];
    A(endpoints.size()-1,0) = normal(0);
    A(endpoints.size()-1,1) = normal(1);
}

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

void PolygonIterator::findSubmapParameters(const grid_map::Polygon& polygon, Eigen::Array2i& startIndex, Eigen::Array2i& bufferSize) const
{
  Eigen::Vector2d topLeft = polygon_.getVertices()[0];
  Eigen::Vector2d bottomRight = topLeft;
  for (const auto& vertex : polygon_.getVertices()) {
    topLeft = topLeft.array().max(vertex.array());
    bottomRight = bottomRight.array().min(vertex.array());
  }
  limitPositionToRange(topLeft, mapLength_, mapPosition_);
  limitPositionToRange(bottomRight, mapLength_, mapPosition_);
  getIndexFromPosition(startIndex, topLeft, mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_);
  Eigen::Array2i endIndex;
  getIndexFromPosition(endIndex, bottomRight, mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_);
  bufferSize = endIndex - startIndex + Eigen::Array2i::Ones();
}

double
FittingCurve2d::findClosestElementMidPoint (const ON_NurbsCurve &nurbs, const Eigen::Vector2d &pt, double hint)
{
  // evaluate hint
  double param = hint;
  double points[2];
  nurbs.Evaluate (param, 0, 2, points);
  Eigen::Vector2d p (points[0], points[1]);
  Eigen::Vector2d r = p - pt;

  double d_shortest_hint = r.squaredNorm ();
  double d_shortest_elem (DBL_MAX);

  // evaluate elements
  std::vector<double> elements = pcl::on_nurbs::FittingCurve2d::getElementVector (nurbs);
  double seg = 1.0 / (nurbs.Order () - 1);

  for (unsigned i = 0; i < elements.size () - 1; i++)
  {
    double &xi0 = elements[i];
    double &xi1 = elements[i + 1];
    double dxi = xi1 - xi0;

    for (unsigned j = 0; j < nurbs.Order (); j++)
    {
      double xi = xi0 + (seg * j) * dxi;

      nurbs.Evaluate (xi, 0, 2, points);
      p (0) = points[0];
      p (1) = points[1];

      r = p - pt;

      double d = r.squaredNorm ();

      if (d < d_shortest_elem)
      {
        d_shortest_elem = d;
        param = xi;
      }
    }
  }

  if(d_shortest_hint < d_shortest_elem)
    return hint;
  else
    return param;
}

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

}