inline void Project(const Rigid3D &T12, const Point2D &x1, LA::Vector2f &x2,
LA::Vector2f &Jx2) const {
const xp128f t = xp128f::get(x1.x(), x1.y(), 1.0f, u());
const float d12 = 1.0f / (T12.r20_r21_r22_tz() * t).vsum_all();
x2.x() = (T12.r00_r01_r02_tx() * t).vsum_all() * d12;
x2.y() = (T12.r10_r11_r12_ty() * t).vsum_all() * d12;
Jx2.x() = (T12.tx() - x2.x() * T12.tz()) * d12;
Jx2.y() = (T12.ty() - x2.y() * T12.tz()) * d12;
}
i think the jacobian should be
Jx2.x() = (T12.tx() - d12 * x2.x() * T12.tz()) * d12;
Jx2.y() = (T12.ty() - d12 * x2.y() * T12.tz()) * d12;
inline void Project(const Rigid3D &T12, const Point2D &x1, LA::Vector2f &x2,
LA::Vector2f &Jx2) const {
const xp128f t = xp128f::get(x1.x(), x1.y(), 1.0f, u());
const float d12 = 1.0f / (T12.r20_r21_r22_tz() * t).vsum_all();
x2.x() = (T12.r00_r01_r02_tx() * t).vsum_all() * d12;
x2.y() = (T12.r10_r11_r12_ty() * t).vsum_all() * d12;
Jx2.x() = (T12.tx() - x2.x() * T12.tz()) * d12;
Jx2.y() = (T12.ty() - x2.y() * T12.tz()) * d12;
}
i think the jacobian should be
Jx2.x() = (T12.tx() - d12 * x2.x() * T12.tz()) * d12;
Jx2.y() = (T12.ty() - d12 * x2.y() * T12.tz()) * d12;