Skip to content
Snippets Groups Projects

Resolve "ProcessorOdomIcp for 3D"

Merged Joan Vallvé Navarro requested to merge 21-processorodomicp-for-3d into devel
Files
4
@@ -29,9 +29,7 @@
#include "core/processor/processor_tracker.h"
#include "core/processor/motion_provider.h"
#include "laser/processor/params_icp.h"
#include "laser/capture/capture_laser_2d.h"
#include "core/factor/factor_relative_pose_2d_with_extrinsics.h"
#include "laser/feature/feature_icp_align.h"
#include "laser/sensor/sensor_laser_2d.h"
/**************************
* ICP includes *
@@ -99,9 +97,11 @@ class ProcessorOdomIcp : public ProcessorTracker, public MotionProvider
laserscanutils::icpOutput trf_origin_last_;
laserscanutils::icpOutput trf_origin_incoming_;
laserscanutils::icpOutput trf_last_incoming_;
Eigen::Vector3d odom_origin_;
Eigen::Vector3d odom_last_;
Eigen::Vector3d odom_incoming_;
Eigen::VectorXd odom_origin_;
Eigen::VectorXd odom_last_;
Eigen::VectorXd odom_incoming_;
Eigen::Isometry2d rl_T_sl_, ro_T_so_;
public:
ParamsProcessorOdomIcpPtr params_odom_icp_;
@@ -117,6 +117,8 @@ class ProcessorOdomIcp : public ProcessorTracker, public MotionProvider
VectorComposite getState(const TimeStamp& _ts, const StateStructure& _structure = "") const override;
void setProblem(ProblemPtr _problem_ptr) override;
Eigen::Vector3d getOriginLastTransform(double& dt) const;
protected:
void preProcess() override;
@@ -137,8 +139,41 @@ class ProcessorOdomIcp : public ProcessorTracker, public MotionProvider
bool voteForKeyFrameAngle() const;
bool voteForKeyFrameTime() const;
bool voteForKeyFrameMatchQuality() const;
template<typename D1, typename D2>
void convert2dTo3d(const Eigen::Isometry2d& T2, Eigen::MatrixBase<D1>& p3, Eigen::QuaternionBase<D2>& q3) const;
template<typename D1>
void convert2dTo3d(const Eigen::Isometry2d& T2, Eigen::MatrixBase<D1>& x3) const;
Eigen::Isometry2d computeIsometry2d(const Eigen::VectorXd& x1, const Eigen::VectorXd& x2 ) const;
void updateExtrinsicsIsometries();
};
}
#include "core/math/rotations.h"
namespace wolf {
template<typename D1, typename D2>
inline void ProcessorOdomIcp::convert2dTo3d(const Eigen::Isometry2d& T2, Eigen::MatrixBase<D1>& p3, Eigen::QuaternionBase<D2>& q3) const
{
MatrixSizeCheck<3, 1>::check(p3);
p3.head(2) = T2.translation();
p3(2) = 0;
q3 = e2q((Eigen::Vector3d() << 0, 0, Rotation2Dd(T2.rotation()).angle()).finished());
}
template<typename D1>
inline void ProcessorOdomIcp::convert2dTo3d(const Eigen::Isometry2d& T2, Eigen::MatrixBase<D1>& x3) const
{
MatrixSizeCheck<7, 1>::check(x3);
x3.head(2) = T2.translation();
x3(2) = 0;
x3.tail(4) = e2q((Eigen::Vector3d() << 0, 0, Rotation2Dd(T2.rotation()).angle()).finished()).coeffs();
}
}
#endif // SRC_PROCESSOR_ODOM_ICP_H_
Loading