2D平面约束 #35
Description
你好，我想添加一个2d平面约束，这样对吗？
void forceTwoD()
{
Eigen::Matrix<double, 9, 1> residual;
residual.setZero();
residual(2) = -kf_ptr_->state_ptr_->p_GI(2) - 47;
residual(5) = -kf_ptr_->state_ptr_->v_GI(2);
// Eigen::Matrix3d r_GI = kf_ptr_->state_ptr_->r_GI;
Eigen::Vector3d angle = computeAngles(kf_ptr_->state_ptr_->r_GI);
residual(6) = -angle(0);
residual(7) = -angle(1);
Eigen::Matrix<double, 9, 16> Hx_;
Hx_.setZero();
// H.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity();
// H(2,2) = 1;
Hx_(5,5) = 1;
Hx_(6,6) = 1;
Hx_(7,7) = 1;
Eigen::Matrix<double, 16, 15> X_dx_;
X_dx_.setZero();
X_dx_.block<6,6>(0,0) = Eigen::Matrix<double,6,6>::Identity();
X_dx_.block<6,6>(10,9) = Eigen::Matrix<double,6,6>::Identity();
// Eigen::Isometry3d Twb;
// Twb.matrix() = kf_ptr_->state_ptr_->r_GI;
// X_dx_.block<3,3>(6,6) = -Twb.linear();
Eigen::Quaterniond q(kf_ptr_->state_ptr_->r_GI);
X_dx_.block<4,3>(6,6) = 0.5*(Eigen::Matrix<double,4,3>()<<-q.x(),-q.y(),-q.z(),
q.w(),-q.z(),q.y(),
q.z(),q.w(),-q.x(),
-q.y(),q.x(),q.w()).finished();
Eigen::Matrix<double, 9, 15> H;
H.setZero();
H = Hx_ * X_dx_;
Eigen::Matrix<double,9,9> V;
// V.setZero();
// V = Eigen::Matrix<double,9,9>::Identity() * std::numeric_limits<double>::max();
V = Eigen::Matrix<double,9,9>::Identity();
// V(2,2) = 1;
V(5,5) = 5e1;
V(6,6) = 0.1;
V(7,7) = 0.1;
kf_ptr_->update_measurement(H, V, residual);
// DebugLog(kf_ptr_->ss,"Yellow");
}