/* ************************************************************************* */
Point2 Cal3Bundler::uncalibrate(const Point2& p, //
boost::optional<Matrix&> Dcal, boost::optional<Matrix&> Dp) const {
// r = x^2 + y^2;
// g = (1 + k(1)*r + k(2)*r^2);
// pi(:,i) = g * pn(:,i)
const double x = p.x(), y = p.y();
const double r = x * x + y * y;
const double g = 1. + (k1_ + k2_ * r) * r;
const double u = g * x, v = g * y;
// Derivatives make use of intermediate variables above
if (Dcal) {
double rx = r * x, ry = r * y;
Dcal->resize(2, 3);
*Dcal << u, f_ * rx, f_ * r * rx, v, f_ * ry, f_ * r * ry;
}
if (Dp) {
const double a = 2. * (k1_ + 2. * k2_ * r);
const double axx = a * x * x, axy = a * x * y, ayy = a * y * y;
Dp->resize(2,2);
*Dp << g + axx, axy, axy, g + ayy;
*Dp *= f_;
}
return Point2(u0_ + f_ * u, v0_ + f_ * v);
}
Vector OrientedPlane3DirectionPrior::evaluateError(const OrientedPlane3& plane,
boost::optional<Matrix&> H) const {
if (H) {
Matrix H_p;
Unit3 n_hat_p = measured_p_.normal();
Unit3 n_hat_q = plane.normal();
Vector e = n_hat_p.error(n_hat_q, H_p);
H->resize(2, 3);
H->block<2, 2>(0, 0) << H_p;
H->block<2, 1>(0, 2) << Matrix::Zero(2, 1);
return e;
} else {
Unit3 n_hat_p = measured_p_.normal();
Unit3 n_hat_q = plane.normal();
Vector e = n_hat_p.error(n_hat_q);
return e;
}
}
/* ************************************************************************** */
void Pose2MobileVetLinArm::forwardKinematics(const Pose2Vector& p,
boost::optional<const gtsam::Vector&> v,
std::vector<gtsam::Pose3>& px, boost::optional<std::vector<gtsam::Vector3>&> vx,
boost::optional<std::vector<gtsam::Matrix>&> J_px_p,
boost::optional<std::vector<gtsam::Matrix>&> J_vx_p,
boost::optional<std::vector<gtsam::Matrix>&> J_vx_v) const {
if (v)
throw runtime_error("[Pose2MobileArm] TODO: velocity not implemented");
if (!v && (vx || J_vx_p || J_vx_v))
throw runtime_error("[Pose2MobileArm] ERROR: only ask for velocity in workspace given velocity in "
"configuration space");
// space for output
px.resize(nr_links());
if (vx) vx->resize(nr_links());
if (J_px_p) J_px_p->assign(nr_links(), Matrix::Zero(6, dof()));
if (J_vx_p) J_vx_p->assign(nr_links(), Matrix::Zero(3, dof()));
if (J_vx_v) J_vx_v->assign(nr_links(), Matrix::Zero(3, dof()));
// vehicle & arm base pose
Pose3 veh_base, tso_base, arm_base;
Matrix63 Hveh_base;
Matrix64 Htso_base, Harm_base;
if (J_px_p || J_vx_p || J_vx_v) {
veh_base = computeBasePose3(p.pose(), Hveh_base);
tso_base = liftBasePose3(p.pose(), p.configuration()(0), base_T_torso_, reverse_linact_,
Htso_base);
Matrix6 H_tso_comp;
arm_base = tso_base.compose(torso_T_arm_, H_tso_comp);
Harm_base = H_tso_comp * Htso_base;
} else {
veh_base = computeBasePose3(p.pose());
tso_base = liftBasePose3(p.pose(), p.configuration()(0), base_T_torso_, reverse_linact_);
arm_base = tso_base.compose(torso_T_arm_);
}
// veh base link
px[0] = veh_base;
if (J_px_p) (*J_px_p)[0].block<6,3>(0,0) = Hveh_base;
// torso link
px[1] = tso_base;
if (J_px_p) (*J_px_p)[1].block<6,4>(0,0) = Htso_base;
// arm links
vector<Pose3> armjpx;
vector<Matrix> Jarm_jpx_jp;
arm_.updateBasePose(arm_base);
arm_.forwardKinematics(p.configuration().tail(arm_.dof()), boost::none, armjpx, boost::none,
J_px_p ? boost::optional<vector<Matrix>&>(Jarm_jpx_jp) : boost::none);
for (size_t i = 0; i < arm_.dof(); i++) {
px[i+2] = armjpx[i];
if (J_px_p) {
// see compose's jacobian
(*J_px_p)[i+2].block<6,4>(0,0) = (armjpx[i].inverse() * arm_base).AdjointMap() * Harm_base;
(*J_px_p)[i+2].block(0,4,6,arm_.dof()) = Jarm_jpx_jp[i];
}
}
}
//------------------------------------------------------------------------------
Vector CombinedImuFactor::evaluateError(const Pose3& pose_i,
const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2,
boost::optional<Matrix&> H3, boost::optional<Matrix&> H4,
boost::optional<Matrix&> H5, boost::optional<Matrix&> H6) const {
// error wrt bias evolution model (random walk)
Matrix6 Hbias_i, Hbias_j;
Vector6 fbias = traits<imuBias::ConstantBias>::Between(bias_j, bias_i,
H6 ? &Hbias_j : 0, H5 ? &Hbias_i : 0).vector();
Matrix96 D_r_pose_i, D_r_pose_j, D_r_bias_i;
Matrix93 D_r_vel_i, D_r_vel_j;
// error wrt preintegrated measurements
Vector9 r_Rpv = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i,
H1 ? &D_r_pose_i : 0, H2 ? &D_r_vel_i : 0, H3 ? &D_r_pose_j : 0,
H4 ? &D_r_vel_j : 0, H5 ? &D_r_bias_i : 0);
// if we need the jacobians
if (H1) {
H1->resize(15, 6);
H1->block<9, 6>(0, 0) = D_r_pose_i;
// adding: [dBiasAcc/dPi ; dBiasOmega/dPi]
H1->block<6, 6>(9, 0).setZero();
}
if (H2) {
H2->resize(15, 3);
H2->block<9, 3>(0, 0) = D_r_vel_i;
// adding: [dBiasAcc/dVi ; dBiasOmega/dVi]
H2->block<6, 3>(9, 0).setZero();
}
if (H3) {
H3->resize(15, 6);
H3->block<9, 6>(0, 0) = D_r_pose_j;
// adding: [dBiasAcc/dPj ; dBiasOmega/dPj]
H3->block<6, 6>(9, 0).setZero();
}
if (H4) {
H4->resize(15, 3);
H4->block<9, 3>(0, 0) = D_r_vel_j;
// adding: [dBiasAcc/dVi ; dBiasOmega/dVi]
H4->block<6, 3>(9, 0).setZero();
}
if (H5) {
H5->resize(15, 6);
H5->block<9, 6>(0, 0) = D_r_bias_i;
// adding: [dBiasAcc/dBias_i ; dBiasOmega/dBias_i]
H5->block<6, 6>(9, 0) = Hbias_i;
}
if (H6) {
H6->resize(15, 6);
H6->block<9, 6>(0, 0).setZero();
// adding: [dBiasAcc/dBias_j ; dBiasOmega/dBias_j]
H6->block<6, 6>(9, 0) = Hbias_j;
}
// overall error
Vector r(15);
r << r_Rpv, fbias; // vector of size 15
return r;
}
/* ************************************************************************** */
void Pose2MobileArm::forwardKinematics(
const Pose2Vector& p, boost::optional<const gtsam::Vector&> v,
std::vector<gtsam::Pose3>& px, boost::optional<std::vector<gtsam::Vector3>&> vx,
boost::optional<std::vector<gtsam::Matrix>&> J_px_p,
boost::optional<std::vector<gtsam::Matrix>&> J_vx_p,
boost::optional<std::vector<gtsam::Matrix>&> J_vx_v) const {
if (v)
throw runtime_error("[Pose2MobileArm] TODO: velocity not implemented");
if (!v && (vx || J_vx_p || J_vx_v))
throw runtime_error("[Pose2MobileArm] ERROR: only ask for velocity in workspace given velocity in "
"configuration space");
// space for output
px.resize(nr_links());
if (vx) vx->resize(nr_links());
if (J_px_p) J_px_p->assign(nr_links(), Matrix::Zero(6, dof()));
if (J_vx_p) J_vx_p->assign(nr_links(), Matrix::Zero(3, dof()));
if (J_vx_v) J_vx_v->assign(nr_links(), Matrix::Zero(3, dof()));
// vehicle & arm base pose
Pose3 veh_base, arm_base;
Matrix63 Hveh_base, Harm_base;
if (J_px_p || J_vx_p || J_vx_v) {
veh_base = computeBasePose3(p.pose(), Hveh_base);
arm_base = computeBaseTransPose3(p.pose(), base_T_arm_, Harm_base);
} else {
veh_base = computeBasePose3(p.pose());
arm_base = computeBaseTransPose3(p.pose(), base_T_arm_);
}
// call arm pose and velocity, for arm links
// px[0] = base_pose3; px[i] = arm_base * px_arm[i-1]
// vx[0] = v(0:1,0); vx[i] = vx[0] + angular x arm_base_pos + arm_base_rot * vx_arm[i-1]
// veh base link
px[0] = veh_base;
if (J_px_p) (*J_px_p)[0].block<6,3>(0,0) = Hveh_base;
if (vx) {
(*vx)[0] = Vector3((*v)[0], (*v)[1], 0.0);
// (*J_vx_p)[0] is zero
if (J_vx_v)
(*J_vx_v)[0].block<2,2>(0,0) = Matrix2::Identity();
}
// arm links
vector<Pose3> armjpx;
vector<Vector3> armjvx;
vector<Matrix> Jarm_jpx_jp, Jarm_jvx_jp, Jarm_jvx_jv;
arm_.updateBasePose(arm_base);
if (v) {
const Vector varm = v->tail(arm_.dof());
arm_.forwardKinematics(p.configuration(), boost::optional<const Vector&>(varm),
armjpx, vx ? boost::optional<vector<Vector3>&>(armjvx) : boost::none,
J_px_p ? boost::optional<vector<Matrix>&>(Jarm_jpx_jp) : boost::none,
J_vx_p ? boost::optional<vector<Matrix>&>(Jarm_jvx_jp) : boost::none,
J_vx_v ? boost::optional<vector<Matrix>&>(Jarm_jvx_jv) : boost::none);
} else {
arm_.forwardKinematics(p.configuration(), boost::none,
armjpx, vx ? boost::optional<vector<Vector3>&>(armjvx) : boost::none,
J_px_p ? boost::optional<vector<Matrix>&>(Jarm_jpx_jp) : boost::none);
}
for (size_t i = 0; i < arm_.dof(); i++) {
px[i+1] = armjpx[i];
if (J_px_p) {
// see compose's jacobian
(*J_px_p)[i+1].block<6,3>(0,0) = (armjpx[i].inverse() * arm_base).AdjointMap() * Harm_base;
(*J_px_p)[i+1].block(0,3,6,arm_.dof()) = Jarm_jpx_jp[i];
}
if (vx) {
//(*vx)[i+1] = Vector3((*v)[0], (*v)[1], 0.0) + armjvx[i];
}
}
}
