Skip to content
Snippets Groups Projects
Commit 0ab0f460 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

Resolve "ProcessorOdomIcp for 3D"

parent 421cbaba
No related branches found
No related tags found
2 merge requests!32new tag,!31Resolve "ProcessorOdomIcp for 3D"
Hide whitespace changes
Inline Side-by-side
include/laser/processor/processor_odom_icp.h
+ 41
− 6
View file @ 0ab0f460
......@@ -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_
include/laser/sensor/sensor_laser_2d.h
+ 0
− 3
View file @ 0ab0f460
......@@ -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
......
src/processor/processor_odom_icp.cpp
+ 219
− 57
View file @ 0ab0f460
......@@ -22,17 +22,23 @@
#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()),
rl_T_sl_(Eigen::Isometry2d::Identity()),
ro_T_so_(Eigen::Isometry2d::Identity()),
params_odom_icp_(_params)
{
// Icp algorithm
......@@ -62,29 +68,60 @@ 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
{
assert((x1.size() == 3 and x2.size() == 3) or (x1.size() == 7 and x2.size() == 7));
// 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 +130,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 +145,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 +164,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 +252,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 +293,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 +348,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 +401,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
// 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();
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();
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 +450,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 +460,21 @@ VectorComposite ProcessorOdomIcp::getState(const TimeStamp& _ts, const StateStru
}
else
{
WOLF_WARN(getName(), ": 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 +483,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
......
test/gtest_processor_odom_icp.cpp
+ 51
− 6
View file @ 0ab0f460
......@@ -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)
{
......
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