diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index 6a09f31ceaa2365cb06d9058c2962f364b890a91..3665b17773c0e18ad6591df9081bc4b63d8e2dbe 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -38,10 +38,10 @@ stages:
- git checkout devel
- git pull
- git checkout $WOLF_CORE_BRANCH
+ - git pull
- else
- - git clone ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_lib/wolf.git
+ - git clone -b $WOLF_CORE_BRANCH ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_lib/wolf.git
- cd wolf
- - git checkout $WOLF_CORE_BRANCH
- fi
- mkdir -pv build
- cd build
@@ -147,9 +147,9 @@ stages:
############ LICENSE HEADERS ############
license_headers:
stage: license
- image: labrobotica/wolf_deps:16.04
+ image: labrobotica/wolf_deps:20.04
cache:
- - key: wolf-xenial
+ - key: wolf-focal
paths:
- ci_deps/wolf/
except:
@@ -160,99 +160,6 @@ license_headers:
script:
- *license_header_definition
-############ UBUNTU 16.04 TESTS ############
-build_and_test_none:xenial:
- stage: none
- image: labrobotica/wolf_deps:16.04
- cache:
- - key: laserscanutils-xenial
- paths:
- - ci_deps/laser_scan_utils/
- - key: wolf-xenial
- paths:
- - ci_deps/wolf/
- except:
- - master
- before_script:
- - *preliminaries_definition
- - *install_wolf_definition
- - *install_laserscanutils_definition
- script:
- - *build_and_test_definition
-
-build_and_test_csm:xenial:
- stage: csm
- image: labrobotica/wolf_deps:16.04
- cache:
- - key: wolf-xenial
- paths:
- - ci_deps/wolf/
- - key: laserscanutils-xenial
- paths:
- - ci_deps/laser_scan_utils/
- - key: csm-xenial
- paths:
- - ci_deps/csm/
- except:
- - master
- before_script:
- - *preliminaries_definition
- - *install_wolf_definition
- - *install_csm_definition
- - *install_laserscanutils_definition
- script:
- - *build_and_test_definition
-
-build_and_test_falko:xenial:
- stage: falko
- image: labrobotica/wolf_deps:16.04
- cache:
- - key: wolf-xenial
- paths:
- - ci_deps/wolf/
- - key: laserscanutils-xenial
- paths:
- - ci_deps/laser_scan_utils/
- - key: falko-xenial
- paths:
- - ci_deps/falkolib/
- except:
- - master
- before_script:
- - *preliminaries_definition
- - *install_wolf_definition
- - *install_falko_definition
- - *install_laserscanutils_definition
- script:
- - *build_and_test_definition
-
-build_and_test_csm_falko:xenial:
- stage: csm_falko
- image: labrobotica/wolf_deps:16.04
- cache:
- - key: wolf-xenial
- paths:
- - ci_deps/wolf/
- - key: laserscanutils-xenial
- paths:
- - ci_deps/laser_scan_utils/
- - key: csm-xenial
- paths:
- - ci_deps/csm/
- - key: falko-xenial
- paths:
- - ci_deps/falkolib/
- except:
- - master
- before_script:
- - *preliminaries_definition
- - *install_wolf_definition
- - *install_falko_definition
- - *install_csm_definition
- - *install_laserscanutils_definition
- script:
- - *build_and_test_definition
-
############ UBUNTU 18.04 TESTS ############
build_and_test_none:bionic:
stage: none
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 88c9fea397911a5317573b7376ac4e3c9cdeec03..6a8ad1eab8d460fd4c256b2f48b0bed9106b361e 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -175,26 +175,24 @@ if (falkolib_FOUND)
SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
include/laser/processor/processor_loop_closure_falko.h
)
- # falko & CSM
- if (csm_FOUND)
- SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
- include/laser/processor/processor_loop_closure_falko_icp.h
- )
- SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
- src/processor/processor_loop_closure_falko_icp.cpp
- )
-
- endif()
+ SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
+ src/processor/processor_loop_closure_falko.cpp
+ )
SET(HDRS_FEATURE ${HDRS_FEATURE}
include/laser/feature/feature_scene_falko.h
)
SET(SRCS_FEATURE ${SRCS_FEATURE}
src/feature/feature_scene_falko.cpp
)
-
- SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
- src/processor/processor_loop_closure_falko.cpp
+ # falko & CSM
+ if (csm_FOUND)
+ SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
+ include/laser/processor/processor_loop_closure_falko_icp.h
+ )
+ SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
+ src/processor/processor_loop_closure_falko_icp.cpp
)
+ endif()
endif()
# CSM
diff --git a/include/laser/processor/processor_odom_icp.h b/include/laser/processor/processor_odom_icp.h
index a802d9232fb0362cef9892721f5d83c81ec9da00..9ef7904912e713aebad83e338fcca6bd9cce2e7f 100644
--- a/include/laser/processor/processor_odom_icp.h
+++ b/include/laser/processor/processor_odom_icp.h
@@ -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_
diff --git a/include/laser/sensor/sensor_laser_2d.h b/include/laser/sensor/sensor_laser_2d.h
index 5568961c21fe2049e5472759cb32ba28323b49cd..047de263741d3b3de6321ed0f337b7f13b4f7914 100644
--- a/include/laser/sensor/sensor_laser_2d.h
+++ b/include/laser/sensor/sensor_laser_2d.h
@@ -125,9 +125,6 @@ class SensorLaser2d : public SensorBase
**/
const laserscanutils::LaserScanParams & getScanParams() const;
- public:
-// static ParamsSensorBasePtr createParams(const std::string& _filename_dot_yaml);
-
};
} // namespace wolf
diff --git a/src/processor/processor_odom_icp.cpp b/src/processor/processor_odom_icp.cpp
index 100dd66185496d1c923ba4e1ba48fe24f549a7b4..aa4bbd55eb9e30eee6606fa94ace0d0c8ba68e30 100644
--- a/src/processor/processor_odom_icp.cpp
+++ b/src/processor/processor_odom_icp.cpp
@@ -22,17 +22,26 @@
#include "laser/processor/processor_odom_icp.h"
#include "laser/math/laser_tools.h"
#include "core/math/covariance.h"
+#include "core/math/rotations.h"
+#include "core/math/SE3.h"
+#include "core/math/SE2.h"
+#include "laser/capture/capture_laser_2d.h"
+#include "core/factor/factor_relative_pose_2d_with_extrinsics.h"
+#include "core/factor/factor_relative_pose_3d.h"
+#include "laser/feature/feature_icp_align.h"
using namespace laserscanutils;
namespace wolf
{
ProcessorOdomIcp::ProcessorOdomIcp(ParamsProcessorOdomIcpPtr _params):
- ProcessorTracker("ProcessorOdomIcp", "PO", 2, _params),
+ ProcessorTracker("ProcessorOdomIcp", "PO", 0, _params),
MotionProvider("PO", _params),
- odom_origin_(Eigen::Vector3d::Zero()),
- odom_last_(Eigen::Vector3d::Zero()),
- odom_incoming_(Eigen::Vector3d::Zero()),
+ odom_origin_(Eigen::VectorXd::Zero(3)),
+ odom_last_(Eigen::VectorXd::Zero(3)),
+ odom_incoming_(Eigen::VectorXd::Zero(3)),
+ rl_T_sl_(Eigen::Isometry2d::Identity()),
+ ro_T_so_(Eigen::Isometry2d::Identity()),
params_odom_icp_(_params)
{
// Icp algorithm
@@ -62,29 +71,62 @@ void ProcessorOdomIcp::preProcess()
else if (params_odom_icp_->initial_guess == "state" )
{
odom_incoming_ = getProblem()->getState("PO").vector("PO");
+
if(last_ptr_)
- {
- odom_last_ = getProblem()->getState(last_ptr_->getTimeStamp(), "PO").vector("PO");
- }
+ odom_last_ = getProblem()->getState(last_ptr_->getTimeStamp(), "PO").vector("PO");
if(origin_ptr_)
- {
- odom_origin_ = getProblem()->getState(origin_ptr_->getTimeStamp(), "PO").vector("PO");
- }
+ odom_origin_ = getProblem()->getState(origin_ptr_->getTimeStamp(), "PO").vector("PO");
}
-
- assert(odom_incoming_.size() == 3);
}
void ProcessorOdomIcp::configure(SensorBasePtr _sensor)
{
sensor_laser_ = std::static_pointer_cast<SensorLaser2d>(_sensor);
laser_scan_params_ = sensor_laser_->getScanParams();
+
+ // initialize extrinsics if static
+ if (not sensor_laser_->isStateBlockDynamic('P') and not sensor_laser_->isStateBlockDynamic('O') )
+ {
+ ro_T_so_ = laser::trf2isometry(sensor_laser_->getP()->getState(), sensor_laser_->getO()->getState());
+ rl_T_sl_ = ro_T_so_;
+ }
+}
+
+void ProcessorOdomIcp::updateExtrinsicsIsometries()
+{
+ // dynamics
+ if (sensor_laser_->isStateBlockDynamic('P') or sensor_laser_->isStateBlockDynamic('O'))
+ {
+ ro_T_so_ = laser::trf2isometry(origin_ptr_->getSensorP()->getState(), origin_ptr_->getSensorP()->getState());
+ rl_T_sl_ = laser::trf2isometry(last_ptr_->getSensorP()->getState(), last_ptr_->getSensorP()->getState());
+ }
+ // statics not fixed (otherwise nothing to do)
+ else if (not sensor_laser_->getP()->isFixed() or not sensor_laser_->getO()->isFixed())
+ {
+ ro_T_so_ = laser::trf2isometry(sensor_laser_->getP()->getState(), sensor_laser_->getO()->getState());
+ rl_T_sl_ = ro_T_so_;
+ }
+}
+
+Eigen::Isometry2d ProcessorOdomIcp::computeIsometry2d(const Eigen::VectorXd& x1, const Eigen::VectorXd& x2 ) const
+{
+ // any vector not properly filled
+ if (not (x1.size() == 3 and x2.size() == 3) and not (x1.size() == 7 and x2.size() == 7))
+ return Eigen::Isometry2d::Identity();
+
+ // 2D
+ if (x1.size() == 3)
+ return laser::trf2isometry(Eigen::Rotation2Dd(-x1(2)) * (x2.head<2>() - x1.head<2>()),
+ Eigen::Vector1d(x2(2) - x1(2)));
+
+ // 3D
+ auto dx = SE3::between(x1, x2);
+ return laser::trf2isometry(dx.head<2>(), q2e(Eigen::Quaterniond(dx.tail<4>())).tail<1>());
}
unsigned int ProcessorOdomIcp::processKnown()
{
// Match the incoming with the origin, passing the transform from origin to last as initialization
-
if (origin_ptr_) // Make sure it's not the FIRST_TIME
{
CaptureLaser2dPtr origin_ptr = std::static_pointer_cast<CaptureLaser2d>(origin_ptr_);
@@ -93,9 +135,13 @@ unsigned int ProcessorOdomIcp::processKnown()
Eigen::Vector3d initial_guess = this->trf_origin_last_.res_transf;
if (params_odom_icp_->initial_guess == "odom" or params_odom_icp_->initial_guess == "state" )
{
- //TODO: Add sensor extrinsics
- initial_guess.head(2) += Eigen::Rotation2Dd(-odom_origin_(2)) * (odom_last_.head(2) - odom_incoming_.head(2));
- initial_guess(2) += -(odom_incoming_(2) - odom_last_(2));
+ // update extrinsics (if necessary)
+ updateExtrinsicsIsometries();
+
+ auto ri_T_ro = computeIsometry2d(odom_incoming_, odom_origin_);
+ auto si_T_so = rl_T_sl_.inverse() * ri_T_ro * ro_T_so_;
+ initial_guess.head<2>() = si_T_so.translation();
+ initial_guess(2) = Rotation2Dd(si_T_so.rotation()).angle();
}
else if (params_odom_icp_->initial_guess != "zero")
throw std::runtime_error("unknown value for param 'initial_guess'. Should be 'odom', 'state' or 'zero'");
@@ -104,14 +150,12 @@ unsigned int ProcessorOdomIcp::processKnown()
origin_ptr->getScan(),
this->laser_scan_params_,
params_odom_icp_->icp_params,
- initial_guess); // Check order
+ initial_guess);
WOLF_DEBUG("ProcessorOdomIcp::processKnown odom_origin_: ", odom_origin_.transpose());
WOLF_DEBUG("ProcessorOdomIcp::processKnown odom_incoming_: ", odom_incoming_.transpose());
WOLF_DEBUG("ProcessorOdomIcp::processKnown initial guess: ", initial_guess.transpose());
WOLF_DEBUG("ProcessorOdomIcp::processKnown ICP transform: ", trf_origin_incoming_.res_transf.transpose());
WOLF_DEBUG("ProcessorOdomIcp::processKnown ICP cov: \n", trf_origin_incoming_.res_covar);
-
- //trf_origin_incoming_.valid = trf_origin_incoming_.valid && trf_origin_incoming_.error / trf_origin_incoming_.nvalid < 5e-2;
}
return 0;
}
@@ -125,8 +169,13 @@ unsigned int ProcessorOdomIcp::processNew(const int& _max_features)
Eigen::Vector3d initial_guess(Eigen::Vector3d::Zero());
if (params_odom_icp_->initial_guess == "odom" or params_odom_icp_->initial_guess == "state" )
{
- initial_guess.head(2) = Eigen::Rotation2Dd(-odom_incoming_(2)) * (odom_last_.head(2) - odom_incoming_.head(2));
- initial_guess(2) = -(odom_incoming_(2) - odom_last_(2));
+ // update extrinsics (if necessary)
+ updateExtrinsicsIsometries();
+
+ auto ri_T_rl = computeIsometry2d(odom_incoming_, odom_last_);
+ auto si_T_sl = rl_T_sl_.inverse() * ri_T_rl * rl_T_sl_;
+ initial_guess.head<2>() = si_T_sl.translation();
+ initial_guess(2) = Rotation2Dd(si_T_sl.rotation()).angle();
}
else if (params_odom_icp_->initial_guess != "zero")
throw std::runtime_error("unknown value for param 'initial_guess'. Should be 'odom', 'state' or 'zero'");
@@ -208,12 +257,37 @@ void ProcessorOdomIcp::advanceDerived()
odom_last_ = odom_incoming_;
+ // init odometry_
+ if (odometry_.empty())
+ odometry_ = getProblem()->stateZero("PO");
+
+ // update extrinsics (if necessary)
+ updateExtrinsicsIsometries();
+
// computing odometry
auto so_T_si = laser::trf2isometry(trf_origin_incoming_.res_transf);
auto so_T_sl = laser::trf2isometry(trf_origin_last_.res_transf);
- Isometry2d sl_T_si = so_T_sl.inverse() * so_T_si;
- odometry_['P'] = sl_T_si.translation();
- odometry_['O'] = Vector1d(Rotation2Dd(sl_T_si.rotation()).angle());
+ Isometry2d rl_T_ri = rl_T_sl_ * so_T_sl.inverse() * so_T_si * rl_T_sl_.inverse();
+
+ // 2D
+ if (getProblem()->getDim() == 2)
+ {
+ auto m_T_rl = laser::trf2isometry(odometry_['P'], odometry_['O']);
+ auto m_T_ri = m_T_rl * rl_T_ri;
+ odometry_['P'] = m_T_ri.translation();
+ odometry_['O'] = Vector1d(Rotation2Dd(m_T_ri.rotation()).angle());
+ }
+ // 3D
+ else
+ {
+ Eigen::Vector7d rl_T3d_ri;
+
+ convert2dTo3d(rl_T_ri, rl_T3d_ri);
+
+ SE3::compose(odometry_['P'], odometry_['O'],
+ rl_T3d_ri.head<3>(), rl_T3d_ri.tail<4>(),
+ odometry_['P'], odometry_['O']);
+ }
// advance transforms
trf_origin_last_ = trf_origin_incoming_;
@@ -224,16 +298,36 @@ void ProcessorOdomIcp::resetDerived()
// Using processing_step_ to ensure that origin, last and incoming are different
if (processing_step_ != FIRST_TIME_WITH_KEYFRAME && processing_step_ != FIRST_TIME_WITHOUT_KEYFRAME)
{
- // Notation explanation:
- // x1_R_x2: Rotation from frame x1 to frame x2
- // x1_p_y1_y2: translation vector from y1 to y2 expressed in frame x1
+ // init odometry_
+ if (odometry_.empty())
+ odometry_ = getProblem()->stateZero("PO");
+
+ // update extrinsics (if necessary)
+ updateExtrinsicsIsometries();
// computing odometry
auto so_T_si = laser::trf2isometry(trf_origin_incoming_.res_transf);
auto so_T_sl = laser::trf2isometry(trf_origin_last_.res_transf);
- Isometry2d sl_T_si = so_T_sl.inverse() * so_T_si;
- odometry_['P'] = sl_T_si.translation();
- odometry_['O'] = Vector1d(Rotation2Dd(sl_T_si.rotation()).angle());
+ Isometry2d rl_T_ri = rl_T_sl_ * so_T_sl.inverse() * so_T_si * rl_T_sl_.inverse();
+ // 2D
+ if (getProblem()->getDim() == 2)
+ {
+ auto m_T_rl = laser::trf2isometry(odometry_['P'], odometry_['O']);
+ auto m_T_ri = m_T_rl * rl_T_ri;
+ odometry_['P'] = m_T_ri.translation();
+ odometry_['O'] = Vector1d(Rotation2Dd(m_T_ri.rotation()).angle());
+ }
+ // 3D
+ else
+ {
+ Eigen::Vector7d rl_T3d_ri;
+
+ convert2dTo3d(rl_T_ri, rl_T3d_ri);
+
+ SE3::compose(odometry_['P'], odometry_['O'],
+ rl_T3d_ri.head<3>(), rl_T3d_ri.tail<4>(),
+ odometry_['P'], odometry_['O']);
+ }
// advance transforms
trf_origin_last_ = trf_last_incoming_;
@@ -259,20 +353,51 @@ FeatureBasePtr ProcessorOdomIcp::emplaceFeature(CaptureBasePtr _capture_laser)
if (not isCovariance(trf_origin_last_.res_covar))
trf_origin_last_.res_covar = 1e-4 * Eigen::Matrix3d::Identity();
- return FeatureBase::emplace<FeatureICPAlign>(_capture_laser,
- trf_origin_last_);
+ // 2D
+ if (getProblem()->getDim() == 2)
+ return FeatureBase::emplace<FeatureICPAlign>(_capture_laser,
+ trf_origin_last_);
+ // 3D
+ else
+ {
+ // update extrinsics (if necessary)
+ updateExtrinsicsIsometries();
+
+ Eigen::Vector7d meas_3d = Eigen::Vector7d::Zero();
+ auto so_T_sl = ro_T_so_.inverse() * laser::trf2isometry(trf_origin_last_.res_transf) * rl_T_sl_;
+ convert2dTo3d(so_T_sl, meas_3d);
+
+ Eigen::Matrix6d cov_3d = Eigen::Matrix6d::Identity() * Constants::EPS;
+ cov_3d.topLeftCorner<2,2>() = trf_origin_last_.res_covar.topLeftCorner<2,2>();
+ cov_3d.topRightCorner<2,1>() = trf_origin_last_.res_covar.topRightCorner<2,1>();
+ cov_3d.bottomLeftCorner<1,2>() = trf_origin_last_.res_covar.bottomLeftCorner<1,2>();
+ cov_3d.bottomRightCorner<1,1>() = trf_origin_last_.res_covar.bottomRightCorner<1,1>();
+
+ return FeatureBase::emplace<FeatureBase>(_capture_laser, "FeatureICPAlign", meas_3d, cov_3d);
+ }
}
FactorBasePtr ProcessorOdomIcp::emplaceFactor(FeatureBasePtr _feature)
{
WOLF_DEBUG("ProcessorOdomIcp::emplaceFactor: feature = ", _feature->getMeasurement().transpose());
WOLF_DEBUG("ProcessorOdomIcp::emplaceFactor: cov = \n", _feature->getMeasurementCovariance());
- return FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(_feature,
- _feature,
- origin_ptr_->getFrame(),
- shared_from_this(),
- params_->apply_loss_function,
- TOP_MOTION);
+
+ // 2D
+ if (getProblem()->getDim() == 2)
+ return FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(_feature,
+ _feature,
+ origin_ptr_->getFrame(),
+ shared_from_this(),
+ params_->apply_loss_function,
+ TOP_MOTION);
+ // 3D
+ else
+ return FactorBase::emplace<FactorRelativePose3d>(_feature,
+ _feature,
+ origin_ptr_->getFrame(),
+ shared_from_this(),
+ params_->apply_loss_function,
+ TOP_MOTION);
}
VectorComposite ProcessorOdomIcp::getState( const StateStructure& _structure ) const
@@ -281,27 +406,48 @@ VectorComposite ProcessorOdomIcp::getState( const StateStructure& _structure ) c
origin_ptr_->isRemoving() or
last_ptr_ == nullptr or
last_ptr_->getFrame() == nullptr) // We do not have any info of where to find a valid state
- // Further checking here for origin_ptr is redundant: if last=null, then origin=null too.
+ // Further checking here for origin_ptr is redundant: if last=null, then origin=null too.
{
- WOLF_DEBUG("Processor has no state. Returning an empty VectorComposite with no blocks");
+ WOLF_WARN("Processor has no state. Returning an empty VectorComposite");
return VectorComposite(); // return empty state
}
- auto w_T_ro = laser::trf2isometry(origin_ptr_->getFrame()->getP()->getState(), origin_ptr_->getFrame()->getO()->getState());
- auto ro_T_so = laser::trf2isometry(origin_ptr_->getSensorP()->getState(), origin_ptr_->getSensorO()->getState());
- const auto& rl_T_sl = ro_T_so; // A reference just to have nice names without computing overhead
+ // we cannot update extrinsics since the function is const
- auto so_T_sl = laser::trf2isometry(trf_origin_last_.res_transf);
- Isometry2d w_T_rl = w_T_ro * ro_T_so * so_T_sl * rl_T_sl.inverse();
+ // origin-last transf.
+ auto so_T_sl = laser::trf2isometry(trf_origin_last_.res_transf);
+
+ // 2D
+ if (getProblem()->getDim() == 2)
+ {
+ auto m_T_ro = laser::trf2isometry(origin_ptr_->getFrame()->getP()->getState(), origin_ptr_->getFrame()->getO()->getState());
+ auto m_T_rl = m_T_ro * ro_T_so_ * so_T_sl * rl_T_sl_.inverse();
+
+ return VectorComposite("PO", {m_T_rl.translation(), Eigen::Vector1d(Eigen::Rotation2Dd(m_T_rl.rotation()).angle())});
+ }
+ // 3D
+ else
+ {
+ auto ro_T_rl = ro_T_so_ * so_T_sl * rl_T_sl_.inverse();
+
+ Eigen::Vector7d ro_T3d_rl;
+ Eigen::Vector3d last_p;
+ Eigen::Vector4d last_q;
- VectorComposite state("PO", {w_T_rl.translation(), Vector1d(Rotation2Dd(w_T_rl.rotation()).angle())});
+ convert2dTo3d(ro_T_rl, ro_T3d_rl);
+
+ SE3::compose(origin_ptr_->getFrame()->getState("P").vector("P"),
+ origin_ptr_->getFrame()->getState("O").vector("O"),
+ ro_T3d_rl.head<3>(), ro_T3d_rl.tail<4>(),
+ last_p, last_q);
- return state;
+ return VectorComposite("PO", {last_p, last_q});
+ }
}
TimeStamp ProcessorOdomIcp::getTimeStamp() const
{
- if( last_ptr_ == nullptr )
+ if ( last_ptr_ == nullptr )
return TimeStamp::Invalid();
else
return last_ptr_->getTimeStamp();
@@ -309,9 +455,7 @@ TimeStamp ProcessorOdomIcp::getTimeStamp() const
VectorComposite ProcessorOdomIcp::getState(const TimeStamp& _ts, const StateStructure& _structure) const
{
- // todo fix this code to get any state in the whole trajectory!
-
- //
+ // valid last...
if (last_ptr_ != nullptr)
{
double d = fabs(_ts - last_ptr_->getTimeStamp());
@@ -321,20 +465,21 @@ VectorComposite ProcessorOdomIcp::getState(const TimeStamp& _ts, const StateStru
}
else
{
- WOLF_WARN("Requested state with time stamp out of tolerance. Returning zero state");
- return getProblem()->stateZero("PO"); // return zero
+ WOLF_WARN(getName(), ": Requested state with time stamp out of tolerance. Returning empty VectorComposite");
+ return VectorComposite();
}
}
// return state at origin if possible
else
{
- if (origin_ptr_ == nullptr || fabs(_ts - last_ptr_->getTimeStamp()) > params_->time_tolerance)
- return VectorComposite("PO", {Vector2d(0,0), Vector1d(0)});
+ if (origin_ptr_ == nullptr || fabs(_ts - origin_ptr_->getTimeStamp()) > params_->time_tolerance)
+ {
+ WOLF_WARN(getName(), ": Requested state with time stamp out of tolerance. Returning empty VectorComposite");
+ return VectorComposite();
+ }
else
return origin_ptr_->getFrame()->getState("PO");
}
-
-
}
void ProcessorOdomIcp::setProblem(ProblemPtr _problem_ptr)
@@ -343,6 +488,28 @@ void ProcessorOdomIcp::setProblem(ProblemPtr _problem_ptr)
addToProblem(_problem_ptr, std::dynamic_pointer_cast<MotionProvider>(shared_from_this()));
}
+Eigen::Vector3d ProcessorOdomIcp::getOriginLastTransform(double& dt) const
+{
+ // not ready
+ if (not last_ptr_ or not origin_ptr_)
+ {
+ dt = -1;
+ return Eigen::Vector3d::Zero();
+ }
+
+ // dt
+ dt = last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp();
+
+ // origin-last transf.
+ auto so_T_sl = laser::trf2isometry(trf_origin_last_.res_transf);
+ auto ro_T_rl = ro_T_so_ * so_T_sl * rl_T_sl_.inverse();
+ Eigen::Vector3d ro_v_rl;
+ ro_v_rl.head<2>() = ro_T_rl.translation();
+ ro_v_rl(2) = Eigen::Rotation2Dd(ro_T_rl.rotation()).angle();
+
+ return ro_v_rl;
+}
+
} // namespace wolf
diff --git a/test/gtest_processor_odom_icp.cpp b/test/gtest_processor_odom_icp.cpp
index 4b09cad83ef2e1200bd9c14503b1f6d093fa8f58..b0f24d0d3bbedd22b82cda213ec0499925ea2669 100644
--- a/test/gtest_processor_odom_icp.cpp
+++ b/test/gtest_processor_odom_icp.cpp
@@ -32,7 +32,9 @@
#include <core/ceres_wrapper/solver_ceres.h>
#include <core/utils/utils_gtest.h>
-#include "laser/processor/processor_odom_icp.h" // THIS AT THE END OTHERWISE IT FAILS COMPILING
+#include "laser/processor/processor_odom_icp.h"
+#include "laser/capture/capture_laser_2d.h"
+
using namespace wolf;
using namespace Eigen;
@@ -66,12 +68,13 @@ class ProcessorOdomIcp_Test : public testing::Test
TimeStamp t0;
VectorComposite x0; // prior state
- VectorComposite s0; // prior sigmas
FrameBasePtr F0, F; // keyframes
+ SizeEigen dim = 2;
+
void SetUp() override
{
- problem = Problem::create("PO", 2);
+ problem = Problem::create("PO", dim);
solver = std::make_shared<SolverCeres>(problem);
auto sen = problem->installSensor("SensorLaser2d", "lidar", Eigen::Vector3d(0,0,0), laser_root_dir + "/test/yaml/sensor_laser_2d.yaml");
@@ -83,9 +86,8 @@ class ProcessorOdomIcp_Test : public testing::Test
ranges = std::vector<float>({49.97591781616211, 49.996429443359375, 49.999759674072266, 50.0, 50.0, 50.0, 49.998634338378906, 50.0, 50.0, 49.99236297607422, 49.99384307861328, 50.0, 49.9869270324707, 50.0, 49.99005889892578, 49.99773406982422, 50.0, 49.98741912841797, 50.0, 49.99842071533203, 50.0, 49.99243927001953, 49.997291564941406, 50.0, 50.0, 49.98580551147461, 49.991844177246094, 49.98896026611328, 50.0, 50.0, 49.9897346496582, 49.998111724853516, 49.99882125854492, 50.0, 50.0, 50.0, 50.0, 49.999698638916016, 50.0, 50.0, 50.0, 50.0, 49.991397857666016, 49.99360275268555, 49.999027252197266, 49.99750900268555, 49.99100112915039, 49.998714447021484, 49.98794174194336, 50.0, 50.0, 50.0, 50.0, 50.0, 49.98186492919922, 50.0, 50.0, 50.0, 49.99155807495117, 49.997196197509766, 49.98872375488281, 49.99138259887695, 50.0, 49.99021530151367, 49.99164581298828, 50.0, 49.991390228271484, 50.0, 50.0, 50.0, 49.997249603271484, 50.0, 49.991851806640625, 50.0, 50.0, 50.0, 50.0, 50.0, 50.0, 49.97983169555664, 49.98630142211914, 50.0, 20.6888370513916, 49.9797477722168, 49.98846435546875, 49.99418640136719, 50.0, 50.0, 50.0, 49.99342346191406, 50.0, 49.9906005859375, 50.0, 49.99853515625, 49.989444732666016, 38.552852630615234, 38.28703689575195, 38.04865264892578, 37.78112030029297, 37.54747772216797, 37.28171157836914, 37.206565856933594, 36.835411071777344, 36.61864471435547, 36.39655685424805, 36.1579475402832, 35.95964431762695, 35.75605773925781, 35.552188873291016, 35.75926208496094, 36.27781677246094, 35.16993713378906, 34.99699401855469, 34.82910919189453, 34.6483039855957, 34.48637390136719, 34.32539749145508, 34.16202163696289, 34.232051849365234, 33.86043167114258, 33.71691131591797, 33.566650390625, 33.42384338378906, 33.2882080078125, 33.16693115234375, 33.041419982910156, 32.906009674072266, 33.008323669433594, 33.706356048583984, 34.43825149536133, 35.25088119506836, 36.05652618408203, 36.930118560791016, 37.83384704589844, 33.12321472167969, 33.02351760864258, 32.9192008972168, 32.82925796508789, 32.74382781982422, 32.64959716796875, 32.580204010009766, 32.49120330810547, 33.05714797973633, 32.343536376953125, 32.26381301879883, 32.21063232421875, 32.12488555908203, 32.06965255737305, 32.0222282409668, 31.954957962036133, 31.903532028198242, 31.83578872680664, 32.51456832885742, 34.189456939697266, 31.12668228149414, 31.076339721679688, 31.047151565551758, 30.967018127441406, 30.956220626831055, 30.924589157104492, 30.893285751342773, 30.869199752807617, 30.843050003051758, 32.791847229003906, 30.809431076049805, 30.79128074645996, 30.779237747192383, 30.776460647583008, 30.74305534362793, 30.74994468688965, 30.7137393951416, 30.734609603881836, 30.719928741455078, 30.71673011779785, 49.99970626831055, 50.0, 49.987911224365234, 33.68583679199219, 31.76846694946289, 31.8026123046875, 31.802202224731445, 31.818490982055664, 31.85223960876465, 31.86141014099121, 31.906801223754883, 31.93423843383789, 31.964210510253906, 33.567230224609375, 32.055015563964844, 32.07001876831055, 32.13076400756836, 32.16000747680664, 32.22781753540039, 32.26890563964844, 32.323944091796875, 32.36326217651367, 32.430908203125, 49.980655670166016, 34.32135772705078, 33.09465789794922, 32.27247619628906, 32.33710479736328, 32.41763687133789, 32.498661041259766, 32.57213592529297, 32.67158126831055, 32.74591827392578, 32.814476013183594, 32.93477249145508, 33.04751968383789, 33.136863708496094, 33.23999786376953, 33.34675979614258, 33.42970657348633, 33.53573226928711, 33.66716003417969, 33.78378677368164, 33.905670166015625, 34.02836608886719, 34.151817321777344, 34.2794189453125, 34.41516876220703, 34.551273345947266, 34.702728271484375, 34.84151840209961, 34.986881256103516, 35.162757873535156, 35.332794189453125, 35.47941970825195, 35.65633010864258, 35.82624435424805, 36.00060272216797, 36.17729187011719, 36.36515808105469, 36.55763626098633, 36.744773864746094, 38.46407699584961, 37.869808197021484, 37.767921447753906, 37.958900451660156, 38.20857620239258, 38.38622283935547, 38.68323516845703, 38.871334075927734, 39.151519775390625, 39.377750396728516, 39.68268966674805, 39.89873123168945, 40.197330474853516, 40.47549819946289, 40.73743438720703, 41.04566955566406, 42.33650207519531, 41.92591094970703, 41.43912124633789, 41.045528411865234, 41.32114028930664, 41.581878662109375, 41.944580078125, 42.318912506103516, 42.6725959777832, 43.07264709472656, 43.443477630615234, 43.83216094970703, 44.19996643066406, 44.63225555419922, 45.06049346923828, 45.468536376953125, 45.89896774291992, 46.330604553222656, 46.778343200683594, 47.31666946411133, 47.789310455322266, 48.26376724243164, 48.826602935791016, 49.33188247680664, 49.990909576416016, 50.0, 50.0, 50.0, 50.0, 49.995697021484375, 49.99568176269531, 49.98163986206055, 50.0, 50.0, 49.9764289855957, 50.0, 50.0, 49.98639678955078, 49.99431228637695, 50.0, 50.0, 50.0, 50.0, 49.9874267578125, 50.0, 49.98714828491211, 50.0, 49.99470901489258, 49.99113464355469, 50.0, 50.0, 50.0, 49.985504150390625, 49.99067306518555, 50.0, 49.997161865234375, 50.0, 50.0, 50.0, 49.995513916015625, 49.993038177490234, 50.0, 49.99763107299805, 50.0, 49.98752975463867, 50.0, 49.988037109375, 50.0, 50.0, 50.0, 49.9975700378418, 50.0, 49.998756408691406, 49.97819519042969, 49.99104690551758, 49.99087905883789, 49.94268798828125, 49.85968017578125, 49.786617279052734, 49.70594787597656, 49.62562561035156, 49.56686782836914, 49.50475311279297, 49.416934967041016, 49.35699462890625, 49.308589935302734, 49.990482330322266, 50.0, 49.998836517333984, 50.0, 50.0, 50.0, 50.0, 50.0, 50.0, 49.980472564697266, 49.99903869628906, 50.0, 50.0, 49.989845275878906, 49.98395919799805, 50.0, 49.99302673339844, 49.99530792236328, 49.99745559692383, 50.0, 49.99560546875, 21.569303512573242});
t0 = 0.0;
- x0 = VectorComposite("PO", {Vector2d(0,0), Vector1d(0)});
- s0 = VectorComposite("PO", {Vector2d(1,1), Vector1d(1)});
- F0 = problem->setPriorFactor(x0, s0, t0);
+ x0 = problem->stateZero();
+ F0 = problem->setPriorFix(x0, t0);
}
void TearDown() override{}
};
@@ -209,6 +211,49 @@ TEST_F(ProcessorOdomIcp_Test, solve)
}
}
+// 3D
+TEST_F(ProcessorOdomIcp_Test, setup_3D)
+{
+ dim = 3;
+ SetUp();
+}
+
+TEST_F(ProcessorOdomIcp_Test, solve_3D)
+{
+ dim = 3;
+ SetUp();
+
+ TimeStamp t(t0);
+
+ for (int i = 0; i < 6; i++)
+ {
+ std::cout << "\n========== Process " << t.getSeconds() << " sec. ==========" << std::endl;
+
+ CaptureLaser2dPtr scan = std::make_shared<CaptureLaser2d>(t, lidar, ranges);
+ scan->process();
+
+ // check that feature and factor has been emplaced
+ ASSERT_TRUE(i <= 1 or processor->getOrigin()->getFeatureList().size() > 0);
+
+ FactorBasePtrList factor_list;
+ processor->getOrigin()->getFactorList(factor_list);
+ ASSERT_TRUE(i <= 1 or factor_list.size() > 0);
+
+ t += 1.0;
+ }
+
+ for (auto F : *problem->getTrajectory())
+ F->perturb(0.5);
+
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+ WOLF_TRACE(report);
+
+ for (auto F : *problem->getTrajectory())
+ {
+ ASSERT_MATRIX_APPROX(F->getState().vector("PO") , x0.vector("PO") , 1e-6);
+ }
+}
+
int main(int argc, char **argv)
{