diff --git a/include/core/factor/factor_diff_drive.h b/include/core/factor/factor_diff_drive.h index 94de8c5de9b48cc25396815412258067aaad97d8..effc3271915cb4070d97d06229ba953566c592e0 100644 --- a/include/core/factor/factor_diff_drive.h +++ b/include/core/factor/factor_diff_drive.h @@ -147,16 +147,4 @@ inline bool FactorDiffDrive::operator()(const T* const _p1, return true; } -// inline Eigen::VectorXd FactorDiffDrive::residual() -// { -// VectorXd residual(3); -// operator()(getFrameOther()->getP()->getState().data(), -// getFrameOther()->getO()->getState().data(), -// getFrame()->getP()->getState().data(), -// getFrame()->getO()->getState().data(), -// getCaptureOther()->getSensorIntrinsic()->getState().data(), -// residual.data()); -// return residual; -// } - } // namespace wolf diff --git a/include/core/factor/factor_relative_pose_2d_with_extrinsics.h b/include/core/factor/factor_relative_pose_2d_with_extrinsics.h index 4fa2f7276f2f4768e036a77b45f456fa3974e483..66c7820e8988ec4e9e365764e3ceb4cd4ab86556 100644 --- a/include/core/factor/factor_relative_pose_2d_with_extrinsics.h +++ b/include/core/factor/factor_relative_pose_2d_with_extrinsics.h @@ -193,33 +193,4 @@ inline bool FactorRelativePose2dWithExtrinsics::operator()(const T* const _p_ref return true; } -// inline Eigen::Vector3d FactorRelativePose2dWithExtrinsics::residual() const -// { -// Eigen::Vector3d res; -// Eigen::VectorXd p_sensor, o_sensor, p_ref, o_ref, p_target, o_target; -// p_sensor = getCapture()->getSensorP()->getState(); -// o_sensor = getCapture()->getSensorO()->getState(); -// // FRAME CASE -// if (not getFramesFactored().empty()) -// { -// p_ref = getFrameOther()->getP()->getState(); -// o_ref = getFrameOther()->getO()->getState(); -// p_target = getCapture()->getFrame()->getP()->getState(); -// o_target = getCapture()->getFrame()->getO()->getState(); -// } -// // LANDMARK CASE -// else if (not getLandmarksFactored().empty()) -// { -// p_ref = getCapture()->getFrame()->getP()->getState(); -// o_ref = getCapture()->getFrame()->getO()->getState(); -// p_target = getLandmarkOther()->getP()->getState(); -// o_target = getLandmarkOther()->getO()->getState(); -// } - -// operator()( -// p_ref.data(), o_ref.data(), p_target.data(), o_target.data(), p_sensor.data(), o_sensor.data(), res.data()); - -// return res; -// } - } // namespace wolf diff --git a/include/core/factor/factor_relative_pose_3d_with_extrinsics.h b/include/core/factor/factor_relative_pose_3d_with_extrinsics.h index 618d48c477938e5b72590e6cbf76cd02e298615c..c7b6cc8731902ca82dc53b86361e34eab52dbc94 100644 --- a/include/core/factor/factor_relative_pose_3d_with_extrinsics.h +++ b/include/core/factor/factor_relative_pose_3d_with_extrinsics.h @@ -186,38 +186,4 @@ inline bool FactorRelativePose3dWithExtrinsics::operator()(const T* const _p_ref return true; } -// inline Eigen::Vector6d FactorRelativePose3dWithExtrinsics::residual() const -// { -// Eigen::Vector6d res; -// Eigen::VectorXd p_sensor, o_sensor, p_ref, o_ref, p_target, o_target; -// p_sensor = getCapture()->getSensorP()->getState(); -// o_sensor = getCapture()->getSensorO()->getState(); -// // FRAME CASE -// if (not getFramesFactored().empty()) -// { -// p_ref = getFrameOther()->getP()->getState(); -// o_ref = getFrameOther()->getO()->getState(); -// p_target = getCapture()->getFrame()->getP()->getState(); -// o_target = getCapture()->getFrame()->getO()->getState(); -// } -// // LANDMARK CASE -// else if (not getLandmarksFactored().empty()) -// { -// p_ref = getCapture()->getFrame()->getP()->getState(); -// o_ref = getCapture()->getFrame()->getO()->getState(); -// p_target = getLandmarkOther()->getP()->getState(); -// o_target = getLandmarkOther()->getO()->getState(); -// } - -// operator()( -// p_ref.data(), o_ref.data(), p_target.data(), o_target.data(), p_sensor.data(), o_sensor.data(), res.data()); - -// return res; -// } - -// inline double FactorRelativePose3dWithExtrinsics::cost() const -// { -// return residual().squaredNorm(); -// } - } // namespace wolf diff --git a/include/core/factor/factor_relative_position_3d_with_extrinsics.h b/include/core/factor/factor_relative_position_3d_with_extrinsics.h index c48db36be7c09e08068765fc6b7fe626440483fa..025f4e5efeb12a7fd316ee0b1972ba5708aab4ed 100644 --- a/include/core/factor/factor_relative_position_3d_with_extrinsics.h +++ b/include/core/factor/factor_relative_position_3d_with_extrinsics.h @@ -123,38 +123,4 @@ inline bool FactorRelativePosition3dWithExtrinsics::operator()(const T* const _p return true; } -// inline Eigen::Vector3d FactorRelativePosition3dWithExtrinsics::residual() const -// { -// Eigen::Vector3d res; -// Eigen::VectorXd p_sensor, o_sensor, p_ref, o_ref, p_target; -// p_sensor = getCapture()->getSensorP()->getState(); -// o_sensor = getCapture()->getSensorO()->getState(); -// // FRAME-FRAME CASE -// if (getFramesFactored().size() == 2) -// { -// p_ref = getFrameOther()->getP()->getState(); -// o_ref = getFrameOther()->getO()->getState(); -// p_target = getCapture()->getFrame()->getP()->getState(); -// } -// // FRAME-LANDMARK CASE -// else if (getLandmarksFactored().size() == 1) -// { -// p_ref = getCapture()->getFrame()->getP()->getState(); -// o_ref = getCapture()->getFrame()->getO()->getState(); -// p_target = getLandmarkOther()->getP()->getState(); -// } -// else -// throw std::runtime_error( -// "FactorRelativePosition3dWithExtrinsics::operator(): unknown case! not FRAME-FRAME or FRAME-LANDMARK"); - -// operator()(p_ref.data(), o_ref.data(), p_target.data(), p_sensor.data(), o_sensor.data(), res.data()); - -// return res; -// } - -// inline double FactorRelativePosition3dWithExtrinsics::cost() const -// { -// return residual().squaredNorm(); -// } - } // namespace wolf diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h index d9e489c7df2ba882d878c4646f02604801a2f849..9825c82fc1802fc4003fc45f88cbfb12f0c510ce 100644 --- a/include/core/processor/processor_base.h +++ b/include/core/processor/processor_base.h @@ -26,9 +26,9 @@ #include "core/processor/buffer.h" #include "core/processor/motion_provider.h" #include "core/processor/factory_processor.h" -#include "core/sensor/sensor_base.h" #include "core/frame/frame_base.h" #include "core/common/time_stamp.h" +#include "core/processor/buffer.h" // std #include <memory>