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