Exemplos de func_zeroedyi_and_dzeroedyi_by_dxp_and_dzeroedyi_by_dyi em C++ (Cpp)

Exemplo n.º 1

Arquivo: part_feature_model.cpp Projeto: xiongxoy/SceneLib2

void PartFeatureModel::func_hpi_and_dhpi_by_dxp_and_dhpi_by_dyi(
    const Eigen::VectorXd &yi, const Eigen::VectorXd &xp, const Eigen::VectorXd &lambda)
{
  // This function gives relative position of feature: also call this hR
  // (vector from camera to feature in robot frame)
  func_zeroedyi_and_dzeroedyi_by_dxp_and_dzeroedyi_by_dyi(yi, xp);

  // Parameters of vector hLR from camera to feature in robot frame
  // hLR = zeroedri + lambda * zeroedhhati
  // Calculate the vector from the camera to the feature given the current
  // lambda
  Eigen::Vector3d hLR = Eigen::Vector3d(zeroedyiRES_(0),zeroedyiRES_(1),zeroedyiRES_(2)) +
                        lambda(0) *
                        Eigen::Vector3d(zeroedyiRES_(3),zeroedyiRES_(4),zeroedyiRES_(5));

  // Project this into the image
  hpiRES_ = camera_->Project(hLR);

  // What is the Jacobian of this projection?
  Eigen::MatrixXd dhpi_by_dhLRi(2,3);
  dhpi_by_dhLRi = camera_->ProjectionJacobian();

  // Calculate the required result Jacobians

  // Now how the vector to the feature depends on the parameterised line
  // (this is a function of lambda)
  Eigen::Matrix<double, 3, 6> dhLRi_by_dzeroedyi;

  dhLRi_by_dzeroedyi << 1.0, 0.0, 0.0, lambda(0), 0.0, 0.0,
                        0.0, 1.0, 0.0, 0.0, lambda(0), 0.0,
                        0.0, 0.0, 1.0, 0.0, 0.0, lambda(0);

  dhpi_by_dxpRES_ = dhpi_by_dhLRi * dhLRi_by_dzeroedyi * dzeroedyi_by_dxpRES_;
  dhpi_by_dyiRES_ = dhpi_by_dhLRi * dhLRi_by_dzeroedyi * dzeroedyi_by_dyiRES_;
}

Exemplo n.º 2

Arquivo: feature_model.cpp Projeto: 499704069/SceneLib2

void FeatureModel::func_zeroedyigraphics_and_Pzeroedyigraphics(
    const Eigen::VectorXd &yi, const Eigen::VectorXd &xv, const Eigen::MatrixXd &Pxx,
    const Eigen::MatrixXd &Pxyi, const Eigen::MatrixXd &Pyiyi)
{
  motion_model_->func_xp(xv);

  // In this case (where the feature state is the same as the graphics
  // state) zeroedyigraphics is the same as zeroedyi
  func_zeroedyi_and_dzeroedyi_by_dxp_and_dzeroedyi_by_dyi(yi, motion_model_->xpRES_);
  zeroedyigraphicsRES_ = zeroedyiRES_;

  Eigen::MatrixXd dzeroedyigraphics_by_dxv = dzeroedyi_by_dxpRES_ * motion_model_->dxp_by_dxvRES_;

  PzeroedyigraphicsRES_ = dzeroedyigraphics_by_dxv * Pxx * dzeroedyigraphics_by_dxv.transpose() +
                          dzeroedyi_by_dyiRES_ * Pxyi.transpose() * dzeroedyigraphics_by_dxv.transpose() +
                          dzeroedyigraphics_by_dxv * Pxyi * dzeroedyi_by_dyiRES_.transpose() +
                          dzeroedyi_by_dyiRES_ * Pyiyi * dzeroedyi_by_dyiRES_.transpose();
}