diff --git a/hello_wolf/factor_bearing.h b/hello_wolf/factor_bearing.h index 56857fe0a188a5eb3ab30d8f4f6a967102f8b7d0..1b5ff40497096ac74f35024ce0b37b1c88076266 100644 --- a/hello_wolf/factor_bearing.h +++ b/hello_wolf/factor_bearing.h @@ -57,7 +57,7 @@ inline bool FactorBearing::operator ()(const T* const _p1, const T* const _o1, { // 1. produce a transformation matrix to transform from robot frame to world frame - Transform<T, 2, Affine> H_w_r = Translation<T,2>(_p1[0], _p1[1]) * Rotation2D<T>(*_o1) ; // Robot frame = robot-to-world transform + Transform<T, 2, Isometry> H_w_r = Translation<T,2>(_p1[0], _p1[1]) * Rotation2D<T>(*_o1) ; // Robot frame = robot-to-world transform // Map input pointers into meaningful Eigen elements Map<const Matrix<T, 2, 1>> point_w(_p2); Map<const Matrix<T, 1, 1>> res(_res); diff --git a/hello_wolf/factor_range_bearing.h b/hello_wolf/factor_range_bearing.h index c4a321badc8673aaa8c7ff57c61a90a987d5d831..ffe896f75932fb269068ad1210ad37350e62ce30 100644 --- a/hello_wolf/factor_range_bearing.h +++ b/hello_wolf/factor_range_bearing.h @@ -97,11 +97,11 @@ inline bool FactorRangeBearing::operator ()(const T* const _p_w_r, // robot posi Map<Matrix<T, 2, 1>> res(_res); // residual // 1. produce transformation matrices to transform from sensor frame --> to robot frame --> to world frame - Transform<T, 2, Affine> H_w_r = Translation<T,2>(_p_w_r[0], _p_w_r[1]) * Rotation2D<T>(*_o_w_r) ; // Robot frame = robot-to-world transform - Transform<T, 2, Affine> H_r_s = Translation<T,2>(_p_r_s[0], _p_r_s[1]) * Rotation2D<T>(*_o_r_s) ; // Sensor frame = sensor-to-robot transform + Transform<T, 2, Isometry> H_w_r = Translation<T,2>(_p_w_r[0], _p_w_r[1]) * Rotation2D<T>(*_o_w_r) ; // Robot frame = robot-to-world transform + Transform<T, 2, Isometry> H_r_s = Translation<T,2>(_p_r_s[0], _p_r_s[1]) * Rotation2D<T>(*_o_r_s) ; // Sensor frame = sensor-to-robot transform // 2. Transform world-referenced landmark point to sensor-referenced point - Transform<T, 2, Affine> H_w_s = H_w_r * H_r_s; // world-to-sensor transform + Transform<T, 2, Isometry> H_w_s = H_w_r * H_r_s; // world-to-sensor transform Matrix<T, 2, 1> lmk_s = H_w_s.inverse() * lmk; // point in sensor frame // 3. Get the expected range-and-bearing of the point diff --git a/hello_wolf/processor_range_bearing.h b/hello_wolf/processor_range_bearing.h index 30b2bd8654a8bc8117f761e492f889fb44918a4a..00592e5d6053dc0e75292b077529c19cbdc1a00d 100644 --- a/hello_wolf/processor_range_bearing.h +++ b/hello_wolf/processor_range_bearing.h @@ -30,7 +30,7 @@ WOLF_PTR_TYPEDEFS(ProcessorRangeBearing); class ProcessorRangeBearing : public ProcessorBase { public: - typedef Eigen::Transform<Scalar, 2, Eigen::Affine> Trf; + typedef Eigen::Transform<Scalar, 2, Eigen::Isometry> Trf; ProcessorRangeBearing(const SensorRangeBearingPtr _sensor_ptr, ProcessorParamsBasePtr _params); virtual ~ProcessorRangeBearing() {/* empty */}