Commit 118e9d82 authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Use Eigen::Map

parent 85aceb6e
......@@ -131,10 +131,10 @@ inline void FactorPixelHp::expectation(const T* const _frame_p,
using namespace Eigen;
// frames w: world; r: robot; c: camera
Matrix<T, 3, 1> p_wr(_frame_p);
Quaternion<T> q_wr(_frame_o);
Matrix<T, 3, 1> p_rc(_sensor_p);
Quaternion<T> q_rc(_sensor_o);
Map<const Matrix<T, 3, 1> > p_wr(_frame_p);
Map<const Quaternion<T> > q_wr(_frame_o);
Map<const Matrix<T, 3, 1> > p_rc(_sensor_p);
Map<const Quaternion<T> > q_rc(_sensor_o);
// camera pose in world frame: transforms from camera to world
Matrix<T, 3, 1> p_wc = p_wr + q_wr * p_rc;
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment