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

proc_pose template, factorpose2dextrinsics & tests

parent 32ff61e7
No related branches found
No related tags found
1 merge request!448Draft: Resolve "Implementation of new nodes creation"
Pipeline #12374 failed
Showing with 555 additions and 169 deletions
......@@ -132,12 +132,13 @@ SET(HDRS_FACTOR
include/${PROJECT_NAME}/factor/factor_diff_drive.h
include/${PROJECT_NAME}/factor/factor_distance_3d.h
include/${PROJECT_NAME}/factor/factor_pose_2d.h
include/${PROJECT_NAME}/factor/factor_pose_2d_with_extrinsics.h
include/${PROJECT_NAME}/factor/factor_pose_3d.h
include/${PROJECT_NAME}/factor/factor_pose_3d_with_extrinsics.h
include/${PROJECT_NAME}/factor/factor_quaternion_absolute.h
include/${PROJECT_NAME}/factor/factor_relative_pose_2d.h
include/${PROJECT_NAME}/factor/factor_relative_pose_2d_with_extrinsics.h
include/${PROJECT_NAME}/factor/factor_relative_pose_3d.h
include/${PROJECT_NAME}/factor/factor_pose_3d_with_extrinsics.h
include/${PROJECT_NAME}/factor/factor_relative_pose_3d_with_extrinsics.h
include/${PROJECT_NAME}/factor/factor_velocity_local_direction_3d.h
)
......@@ -290,7 +291,6 @@ SET(SRCS_PROCESSOR
src/processor/processor_motion.cpp
src/processor/processor_odom_2d.cpp
src/processor/processor_odom_3d.cpp
src/processor/processor_pose.cpp
src/processor/processor_tracker.cpp
src/processor/processor_tracker_feature.cpp
src/processor/processor_tracker_landmark.cpp
......
......@@ -43,14 +43,18 @@ class CapturePose : public CaptureBase
public:
CapturePose(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXd& _data, const Eigen::MatrixXd& _data_covariance);
CapturePose(const TimeStamp& _ts,
SensorBasePtr _sensor_ptr,
const Eigen::VectorXd& _data,
const Eigen::MatrixXd& _data_covariance);
~CapturePose() override;
virtual void emplaceFeatureAndFactor();
Eigen::VectorXd getData(){ return data_;}
Eigen::MatrixXd getDataCovariance(){ return data_covariance_;}
Eigen::VectorXd getData() const { return data_;}
Eigen::MatrixXd getDataCovariance() const { return data_covariance_;}
void setData(const Eigen::VectorXd& _data);
void setDataCovariance(const Eigen::MatrixXd& _data_covariance);
};
} //namespace wolf
\ No newline at end of file
......@@ -64,7 +64,7 @@ inline bool FactorPose2d::operator ()(const T* const _p, const T* const _o, T* _
{
// measurement
// Eigen::Matrix<T,3,1> meas = getMeasurement().cast<T>();
auto& meas = getMeasurement();
auto& meas = getMeasurement();
// error
Eigen::Matrix<T,3,1> er;
......
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
#pragma once
//Wolf includes
#include "core/factor/factor_autodiff.h"
#include "core/frame/frame_base.h"
#include "core/math/rotations.h"
//#include "ceres/jet.h"
namespace wolf {
WOLF_PTR_TYPEDEFS(FactorPose2dWithExtrinsics);
//class
class FactorPose2dWithExtrinsics : public FactorAutodiff<FactorPose2dWithExtrinsics, 3, 2, 1, 2, 1>
{
public:
FactorPose2dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top,
FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorPose2dWithExtrinsics, 3, 2, 1, 2, 1>("FactorPose2dWithExtrinsics",
_top,
_ftr_ptr,
nullptr, nullptr, nullptr, nullptr,
_processor_ptr,
_apply_loss_function,
_status,
_ftr_ptr->getFrame()->getP(),
_ftr_ptr->getFrame()->getO(),
_ftr_ptr->getCapture()->getSensorP(),
_ftr_ptr->getCapture()->getSensorO())
{
assert(_ftr_ptr->getCapture()->getSensorP() != nullptr && "No extrinsics found!");
assert(_ftr_ptr->getCapture()->getSensorO() != nullptr && "No extrinsics found!");
}
~FactorPose2dWithExtrinsics() override = default;
template<typename T>
bool operator ()(const T* const _p,
const T* const _o,
const T* const _sp,
const T* const _so,
T* _residuals) const;
};
template<typename T>
inline bool FactorPose2dWithExtrinsics::operator ()(const T* const _p,
const T* const _o,
const T* const _sp,
const T* const _so,
T* _residuals) const
{
// MAPS
Eigen::Map<const Eigen::Matrix<T,2,1> > p(_p);
Eigen::Map<const Eigen::Matrix<T,2,1> > sp(_sp);
Eigen::Matrix<T, 3, 1> err; // error
err.head(2) = getMeasurement().head(2) - (p.head(2) + Eigen::Rotation2D<T>(_o[0]) * sp.head(2));
err(2) = pi2pi(getMeasurement()(2) - (_o[0] + _so[0]));
// Residuals
Eigen::Map<Eigen::Matrix<T,3,1> > res(_residuals);
res = getMeasurementSquareRootInformationUpper().cast<T>() * err;
return true;
}
} // namespace wolf
\ No newline at end of file
......@@ -40,13 +40,13 @@ struct ParamsProcessorPose : public ParamsProcessorBase
return ParamsProcessorBase::print();
}
};
WOLF_PTR_TYPEDEFS(ProcessorPose);
//class
class ProcessorPose : public ProcessorBase{
template <unsigned int DIM>
class ProcessorPose : public ProcessorBase
{
public:
ProcessorPose(ParamsProcessorPosePtr _params_pfnomove);
ProcessorPose(ParamsProcessorPosePtr _params_pose);
~ProcessorPose() override = default;
WOLF_PROCESSOR_CREATE(ProcessorPose, ParamsProcessorPose);
......@@ -62,8 +62,138 @@ class ProcessorPose : public ProcessorBase{
void createFactorIfNecessary();
protected:
ParamsProcessorPosePtr params_pfnomove_;
bool is_2d_;
ParamsProcessorPosePtr params_pose_;
};
} /* namespace wolf */
\ No newline at end of file
typedef ProcessorPose<2> ProcessorPose2d;
typedef ProcessorPose<3> ProcessorPose3d;
WOLF_DECLARED_PTR_TYPEDEFS(ProcessorPose2d);
WOLF_DECLARED_PTR_TYPEDEFS(ProcessorPose3d);
}
////////////////////////////////////////////////////////////////////////////////////////////////
// IMPLEMENTATION //
////////////////////////////////////////////////////////////////////////////////////////////////
#include "core/sensor/sensor_pose.h"
#include "core/capture/capture_pose.h"
#include "core/factor/factor_pose_3d_with_extrinsics.h"
namespace wolf {
template<unsigned int DIM>
inline ProcessorPose<DIM>::ProcessorPose(ParamsProcessorPosePtr _params_pose) :
ProcessorBase("ProcessorPose", DIM, _params_pose),
params_pose_(_params_pose)
{
static_assert(DIM == 2 or DIM == 3);
}
template<unsigned int DIM>
void ProcessorPose<DIM>::configure(SensorBasePtr _sensor)
{
if (not std::dynamic_pointer_cast<SensorPose<DIM>>(_sensor))
{
throw std::runtime_error("Configuring a ProcessorPose with a sensor " + _sensor->getType() + ", should be SensorPose" + toString(DIM) + "d");
}
}
template<unsigned int DIM>
void ProcessorPose<DIM>::createFactorIfNecessary()
{
auto kf_it_last = buffer_frame_.getContainer().end();
auto kf_it = buffer_frame_.getContainer().begin();
while (kf_it != buffer_frame_.getContainer().end())
{
TimeStamp t = kf_it->first;
auto cap_it = buffer_capture_.selectIterator(t, getTimeTolerance());
// if capture with corresponding timestamp is not found, assume you will get it later
if (cap_it != buffer_capture_.getContainer().end())
{
// if a corresponding capture exists, link it to the KF and create a factor
auto cap = std::static_pointer_cast<CapturePose>(cap_it->second);
cap->link(kf_it->second);
FeatureBasePtr feat = FeatureBase::emplace<FeatureBase>(cap, "Pose", cap->getData(), cap->getDataCovariance());
if (DIM == 2)
{
// TODO
throw std::runtime_error("FactorPose2dWithExtrinsics not implemented");
}
else
{
FactorBase::emplace<FactorPose3dWithExtrinsics>(feat, feat, shared_from_this(), false, TOP_ABS);
}
// erase removes range [first, last): it does not removes last
// so increment the iterator so that it points to the next element in the container
buffer_capture_.getContainer().erase(buffer_capture_.getContainer().begin(), std::next(cap_it));
// we cannot erase on the kf buffer since we are looping over it so we store the iterator for later
kf_it_last = kf_it;
}
kf_it++;
}
// whatever happened, remove very old captures
buffer_capture_.removeUpTo(buffer_frame_.getContainer().begin()->first - 5);
// now we erase the kf buffer if there was a match
if (kf_it_last != buffer_frame_.getContainer().end()){
buffer_frame_.getContainer().erase(buffer_frame_.getContainer().begin(), std::next(kf_it_last));
}
}
template<unsigned int DIM>
inline void ProcessorPose<DIM>::processCapture(CaptureBasePtr _capture)
{
if (!_capture)
{
WOLF_ERROR("Received capture is nullptr.");
return;
}
// nothing to do if any of the two buffer is empty
if(buffer_frame_.empty()){
WOLF_DEBUG("processCapture: Frame buffer empty, time ", _capture->getTimeStamp());
return;
}
if(buffer_frame_.empty()){
WOLF_DEBUG("processCapture: Capture buffer empty, time ", _capture->getTimeStamp());
return;
}
createFactorIfNecessary();
}
template<unsigned int DIM>
inline void ProcessorPose<DIM>::processKeyFrame(FrameBasePtr _keyframe_ptr)
{
if (!_keyframe_ptr)
{
WOLF_ERROR("Received KF is nullptr.");
return;
}
// nothing to do if any of the two buffer is empty
if(buffer_frame_.empty()){
WOLF_DEBUG("ProcessorPose: Frame buffer empty, time ", _keyframe_ptr->getTimeStamp());
return;
}
if(buffer_frame_.empty()){
WOLF_DEBUG("ProcessorPose: Capture buffer empty, time ", _keyframe_ptr->getTimeStamp());
return;
}
createFactorIfNecessary();
}
} /* namespace wolf */
// Register in the FactoryProcessor
#include "core/processor/factory_processor.h"
namespace wolf {
WOLF_REGISTER_PROCESSOR(ProcessorPose2d);
WOLF_REGISTER_PROCESSOR(ProcessorPose3d);
} // namespace wolf
\ No newline at end of file
follow: ProcessorBase.schema
\ No newline at end of file
follow: ProcessorBase.schema
\ No newline at end of file
......@@ -28,7 +28,9 @@ CapturePose::CapturePose(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const
data_(_data),
data_covariance_(_data_covariance)
{
//
assert(_data.size() == 3 or _data.size() == 7);
assert(_data_covariance.rows() == _data_covariance.cols());
assert(_data_covariance.rows() == 3 or _data_covariance.rows() == 6);
}
CapturePose::~CapturePose()
......@@ -36,6 +38,21 @@ CapturePose::~CapturePose()
//
}
void CapturePose::setData(const Eigen::VectorXd& _data)
{
assert(_data.size() == 3 or _data.size() == 7);
data_ = _data;
}
void CapturePose::setDataCovariance(const Eigen::MatrixXd& _data_covariance)
{
assert(_data_covariance.rows() == _data_covariance.cols());
assert(_data_covariance.rows() == 3 or _data_covariance.rows() == 6);
data_covariance_ = _data_covariance;
}
void CapturePose::emplaceFeatureAndFactor()
{
// Emplace feature
......
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
/**
* \file processor_pose.cpp
*
* Created on: Feb 19, 2020
* \author: mfourmy
*/
#include "core/processor/processor_pose.h"
#include "core/sensor/sensor_pose.h"
#include "core/capture/capture_pose.h"
#include "core/factor/factor_pose_3d_with_extrinsics.h"
namespace wolf
{
ProcessorPose::ProcessorPose(ParamsProcessorPosePtr _params_pfnomove) :
ProcessorBase("ProcessorPose", 0, _params_pfnomove),
params_pfnomove_(std::make_shared<ParamsProcessorPose>(*_params_pfnomove))
{
}
void ProcessorPose::configure(SensorBasePtr _sensor)
{
auto sen_pose_2d = std::dynamic_pointer_cast<SensorPose2d>(_sensor);
auto sen_pose_3d = std::dynamic_pointer_cast<SensorPose3d>(_sensor);
if (not sen_pose_2d and not sen_pose_3d)
{
throw std::runtime_error("Configuring a ProcessorPose with a sensor " + _sensor->getType() + ", should be SensorPose2d or SensorPose3d");
}
is_2d_ = sen_pose_2d != nullptr;
}
void ProcessorPose::createFactorIfNecessary()
{
auto kf_it_last = buffer_frame_.getContainer().end();
auto kf_it = buffer_frame_.getContainer().begin();
while (kf_it != buffer_frame_.getContainer().end())
{
TimeStamp t = kf_it->first;
auto cap_it = buffer_capture_.selectIterator(t, getTimeTolerance());
// if capture with corresponding timestamp is not found, assume you will get it later
if (cap_it != buffer_capture_.getContainer().end())
{
// if a corresponding capture exists, link it to the KF and create a factor
auto cap = std::static_pointer_cast<CapturePose>(cap_it->second);
cap->link(kf_it->second);
FeatureBasePtr feat = FeatureBase::emplace<FeatureBase>(cap, "Pose", cap->getData(), cap->getDataCovariance());
if (is_2d_)
{
// TODO
throw std::runtime_error("FactorPose2dWithExtrinsics not implemented");
}
else
{
FactorBase::emplace<FactorPose3dWithExtrinsics>(feat, feat, shared_from_this(), false, TOP_ABS);
}
// erase removes range [first, last): it does not removes last
// so increment the iterator so that it points to the next element in the container
buffer_capture_.getContainer().erase(buffer_capture_.getContainer().begin(), std::next(cap_it));
// we cannot erase on the kf buffer since we are looping over it so we store the iterator for later
kf_it_last = kf_it;
}
kf_it++;
}
// whatever happened, remove very old captures
buffer_capture_.removeUpTo(buffer_frame_.getContainer().begin()->first - 5);
// now we erase the kf buffer if there was a match
if (kf_it_last != buffer_frame_.getContainer().end()){
buffer_frame_.getContainer().erase(buffer_frame_.getContainer().begin(), std::next(kf_it_last));
}
}
inline void ProcessorPose::processCapture(CaptureBasePtr _capture)
{
if (!_capture)
{
WOLF_ERROR("Received capture is nullptr.");
return;
}
// nothing to do if any of the two buffer is empty
if(buffer_frame_.empty()){
WOLF_DEBUG("processCapture: Frame buffer empty, time ", _capture->getTimeStamp());
return;
}
if(buffer_frame_.empty()){
WOLF_DEBUG("processCapture: Capture buffer empty, time ", _capture->getTimeStamp());
return;
}
createFactorIfNecessary();
}
inline void ProcessorPose::processKeyFrame(FrameBasePtr _keyframe_ptr)
{
if (!_keyframe_ptr)
{
WOLF_ERROR("Received KF is nullptr.");
return;
}
// nothing to do if any of the two buffer is empty
if(buffer_frame_.empty()){
WOLF_DEBUG("ProcessorPose: Frame buffer empty, time ", _keyframe_ptr->getTimeStamp());
return;
}
if(buffer_frame_.empty()){
WOLF_DEBUG("ProcessorPose: Capture buffer empty, time ", _keyframe_ptr->getTimeStamp());
return;
}
createFactorIfNecessary();
}
} /* namespace wolf */
// Register in the FactoryProcessor
#include "core/processor/factory_processor.h"
namespace wolf {
WOLF_REGISTER_PROCESSOR(ProcessorPose);
} // namespace wolf
......@@ -147,9 +147,15 @@ wolf_add_gtest(gtest_factor_odom_2d_autodiff gtest_factor_odom_2d_autodiff.cpp)
# FactorPose2d class test
wolf_add_gtest(gtest_factor_pose_2d gtest_factor_pose_2d.cpp)
# FactorPose2dWithExtrinsics class test
wolf_add_gtest(gtest_factor_pose_2d_with_extrinsics gtest_factor_pose_2d_with_extrinsics.cpp)
# FactorPose3d class test
wolf_add_gtest(gtest_factor_pose_3d gtest_factor_pose_3d.cpp)
# FactorPose3dWithExtrinsics class test
wolf_add_gtest(gtest_factor_pose_3d_with_extrinsics gtest_factor_pose_3d_with_extrinsics.cpp)
# FactorRelativePose2d class test
wolf_add_gtest(gtest_factor_relative_pose_2d gtest_factor_relative_pose_2d.cpp)
......
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
#include "core/utils/utils_gtest.h"
#include "core/ceres_wrapper/solver_ceres.h"
#include "core/factor/factor_pose_2d_with_extrinsics.h"
#include "core/capture/capture_pose.h"
#include "core/sensor/sensor_pose.h"
#include "core/processor/processor_pose.h"
#include "core/math/rotations.h"
using namespace Eigen;
using namespace wolf;
using std::cout;
using std::endl;
std::string wolf_root = _WOLF_ROOT_DIR;
// Input odometry data and covariance
Matrix3d data_cov = 0.1*Matrix3d::Identity();
// Problem and solver
ProblemPtr problem_ptr = Problem::create("PO", 2);
SolverCeres solver(problem_ptr);
// Sensor
auto sensor_pose2d = problem_ptr->installSensor("SensorPose2d", wolf_root + "/test/yaml/sensor_pose_2d.yaml", {wolf_root});
auto processor_pose2d = ProcessorBase::emplace<ProcessorPose2d>(sensor_pose2d, std::make_shared<ParamsProcessorPose>());
// Two frames
FrameBasePtr frm = problem_ptr->emplaceFrame(0, "PO", Vector3d::Zero() );
// Capture for frm
auto cap = CaptureBase::emplace<CapturePose>(frm, 0, sensor_pose2d, Vector3d::Zero(), data_cov);
TEST(FactorPose2dWithExtrinsics, check_tree)
{
ASSERT_TRUE(problem_ptr->check(0));
}
TEST(FactorPose2dWithExtrinsics, solve_f_fix_s)
{
for (int i = 0; i < 1e2; i++)
{
// Random pose
Vector3d x0 = Vector3d::Random() * 10;
pi2pi(x0(2));
// Random extrinsics
Vector3d xs = Vector3d::Random();
pi2pi(xs(2));
// measurement
Vector3d meas;
meas(2) = pi2pi(x0(2) + xs(2));
meas.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * xs.head<2>();
// Set states
frm->setState(x0,"PO");
sensor_pose2d->setState(xs, "PO");
// feature & factor with meas measurement
auto fea1 = FeatureBase::emplace<FeatureBase>(cap, "FeaturePose2d", meas, data_cov);
FactorBase::emplace<FactorPose2dWithExtrinsics>(fea1, fea1, processor_pose2d, false, TOP_ABS);
// Perturb frm, fix the extrinsics
frm->unfix();
sensor_pose2d->fix();
frm->perturb(10);
// solve for frm1
std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
ASSERT_POSE2d_APPROX(frm->getStateVector("PO"), x0, 1e-6);
// remove feature (and factor) for the next loop
fea1->remove();
}
}
TEST(FactorPose2dWithExtrinsics, fix_f_solve_s)
{
for (int i = 0; i < 1e2; i++)
{
// Random pose
Vector3d x0 = Vector3d::Random() * 10;
pi2pi(x0(2));
// Random extrinsics
Vector3d xs = Vector3d::Random();
pi2pi(xs(2));
// measurement
Vector3d meas;
meas(2) = pi2pi(x0(2) + xs(2));
meas.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * xs.head<2>();
// Set states
frm->setState(x0,"PO");
sensor_pose2d->setState(xs, "PO");
// feature & factor with meas measurement
auto fea1 = FeatureBase::emplace<FeatureBase>(cap, "FeaturePose2d", meas, data_cov);
FactorBase::emplace<FactorPose2dWithExtrinsics>(fea1, fea1, processor_pose2d, false, TOP_ABS);
// Perturb extrinsics, fix the frame
frm->fix();
sensor_pose2d->unfix();
sensor_pose2d->perturb(1);
// solve for frm1
std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
ASSERT_POSE2d_APPROX(sensor_pose2d->getStateVector("PO"), xs, 1e-6);
// remove feature (and factor) for the next loop
fea1->remove();
}
}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
#include "core/utils/utils_gtest.h"
#include "core/ceres_wrapper/solver_ceres.h"
#include "core/factor/factor_pose_3d_with_extrinsics.h"
#include "core/capture/capture_pose.h"
#include "core/sensor/sensor_pose.h"
#include "core/processor/processor_pose.h"
#include "core/math/rotations.h"
using namespace Eigen;
using namespace wolf;
using std::cout;
using std::endl;
std::string wolf_root = _WOLF_ROOT_DIR;
// Input odometry data and covariance
Matrix6d data_cov = 0.1*Matrix6d::Identity();
// Problem and solver
ProblemPtr problem_ptr = Problem::create("PO", 3);
SolverCeres solver(problem_ptr);
// Sensor
auto sensor_pose3d = problem_ptr->installSensor("SensorPose3d", wolf_root + "/test/yaml/sensor_pose_3d.yaml", {wolf_root});
auto processor_pose2d = ProcessorBase::emplace<ProcessorPose3d>(sensor_pose3d, std::make_shared<ParamsProcessorPose>());
// Two frames
FrameBasePtr frm = problem_ptr->emplaceFrame(0, "PO", (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished() );
// Capture for frm
auto cap = CaptureBase::emplace<CapturePose>(frm, 0, sensor_pose3d, Vector7d::Zero(), data_cov);
TEST(FactorPose2dWithExtrinsics, check_tree)
{
ASSERT_TRUE(problem_ptr->check(0));
}
TEST(FactorPose2dWithExtrinsics, solve_f_fix_s)
{
for (int i = 0; i < 1e2; i++)
{
// Random pose
Vector7d x0 = Vector7d::Random() * 10;
x0.tail(4).normalize();
// Random extrinsics
Vector7d xs = Vector7d::Random();
xs.tail(4).normalize();
// measurement
Vector7d meas;
meas.head<3>() = x0.head<3>() + Quaterniond(x0.tail<4>()) * xs.head<3>();
meas.tail<4>() = (Quaterniond(x0.tail<4>()) * Quaterniond(xs.tail<4>())).coeffs();
// Set states
frm->setState(x0,"PO");
sensor_pose3d->setState(xs, "PO");
// feature & factor with meas measurement
auto fea1 = FeatureBase::emplace<FeatureBase>(cap, "FeaturePose3d", meas, data_cov);
FactorBase::emplace<FactorPose3dWithExtrinsics>(fea1, fea1, processor_pose2d, false, TOP_ABS);
// Perturb frm, fix the extrinsics
frm->unfix();
sensor_pose3d->fix();
frm->perturb(10);
// solve for frm1
std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
ASSERT_POSE3d_APPROX(frm->getStateVector("PO"), x0, 1e-6);
// remove feature (and factor) for the next loop
fea1->remove();
}
}
TEST(FactorPose2dWithExtrinsics, fix_f_solve_s)
{
for (int i = 0; i < 1e2; i++)
{
// Random pose
Vector7d x0 = Vector7d::Random() * 10;
x0.tail(4).normalize();
// Random extrinsics
Vector7d xs = Vector7d::Random();
xs.tail(4).normalize();
// measurement
Vector7d meas;
meas.head<3>() = x0.head<3>() + Quaterniond(x0.tail<4>()) * xs.head<3>();
meas.tail<4>() = (Quaterniond(x0.tail<4>()) * Quaterniond(xs.tail<4>())).coeffs();
// Set states
frm->setState(x0,"PO");
sensor_pose3d->setState(xs, "PO");
// feature & factor with meas measurement
auto fea1 = FeatureBase::emplace<FeatureBase>(cap, "FeaturePose3d", meas, data_cov);
FactorBase::emplace<FactorPose3dWithExtrinsics>(fea1, fea1, processor_pose2d, false, TOP_ABS);
// Perturb extrinsics, fix the frame
frm->fix();
sensor_pose3d->unfix();
sensor_pose3d->perturb(1);
// solve for frm1
std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
ASSERT_POSE3d_APPROX(sensor_pose3d->getStateVector("PO"), xs, 1e-6);
// remove feature (and factor) for the next loop
fea1->remove();
}
}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
......@@ -129,7 +129,7 @@ class FactorPose3dWithExtrinsicsBase_Test : public testing::Test
auto params_proc = std::make_shared<ParamsProcessorPose>();
params_proc->name = "pose processor";
params_proc->time_tolerance = 0.5;
auto proc_pose = ProcessorBase::emplace<ProcessorPose>(sensor_pose_, params_proc);
auto proc_pose = ProcessorBase::emplace<ProcessorPose3d>(sensor_pose_, params_proc);
}
void TearDown() override{};
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment