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

Resolve "Loss function API"

parent c6e02a9d
No related branches found
No related tags found
2 merge requests!39release after RAL,!38After 2nd RAL submission
......@@ -19,8 +19,8 @@ class FactorIMU : public FactorAutodiff<FactorIMU, 15, 3, 4, 3, 6, 3, 4, 3, 6>
public:
FactorIMU(const FeatureIMUPtr& _ftr_ptr,
const CaptureIMUPtr& _capture_origin_ptr,
const ProcessorBasePtr& _processor_ptr = nullptr,
bool _apply_loss_function = false,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE);
virtual ~FactorIMU() = default;
......
......@@ -19,7 +19,7 @@ WOLF_PTR_TYPEDEFS(FactorFixBias);
class FactorFixBias: public FactorAutodiff<FactorFixBias,6,3,3>
{
public:
FactorFixBias(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
FactorFixBias(FeatureBasePtr _ftr_ptr, bool _apply_loss_function, FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorFixBias, 6, 3, 3>("FactorFixBias",
nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, std::static_pointer_cast<CaptureIMU>(_ftr_ptr->getCapture())->getAccBias(),
std::static_pointer_cast<CaptureIMU>(_ftr_ptr->getCapture())->getGyroBias())
......
#include "base/feature/feature_apriltag.h"
namespace wolf {
FeatureApriltag::FeatureApriltag(const Eigen::Vector7d & _measurement,
const Eigen::Matrix6d & _meas_uncertainty,
const int _tag_id,
const apriltag_detection_t & _det,
double _rep_error1,
double _rep_error2,
bool _use_rotation,
UncertaintyType _uncertainty_type) :
FeatureBase("FeatureApriltag", _measurement, _meas_uncertainty, _uncertainty_type),
tag_id_ (_tag_id),
tag_corners_(4),
detection_ (_det),
rep_error1_(_rep_error1),
rep_error2_(_rep_error2),
use_rotation_(_use_rotation)
{
assert(_det.id == _tag_id && "Tag ID and detection ID do not match!");
setTrackId(_tag_id);
for (int i = 0; i < 4; i++)
{
tag_corners_[i].x = detection_.p[i][0];
tag_corners_[i].y = detection_.p[i][1];
}
}
FeatureApriltag::~FeatureApriltag()
{
//
}
double FeatureApriltag::getTagId() const
{
return tag_id_;
}
const apriltag_detection_t& FeatureApriltag::getDetection() const
{
return detection_;
}
const std::vector<cv::Point2d>& FeatureApriltag::getTagCorners() const
{
return tag_corners_;
}
double FeatureApriltag::getRepError1() const
{
return rep_error1_;
}
double FeatureApriltag::getRepError2() const
{
return rep_error2_;
}
bool FeatureApriltag::getUserotation() const
{
return use_rotation_;
}
} // namespace wolf
#include "base/landmark/landmark_apriltag.h"
#include "base/common/factory.h"
#include "base/math/rotations.h"
#include "base/yaml/yaml_conversion.h"
namespace wolf {
LandmarkApriltag::LandmarkApriltag(Eigen::Vector7d& pose, const int& _tagid, const double& _tag_width) :
LandmarkBase("LandmarkApriltag", std::make_shared<StateBlock>(pose.head(3)), std::make_shared<StateQuaternion>(pose.tail(4))),
tag_id_(_tagid),
tag_width_(_tag_width)
{
setDescriptor(Eigen::VectorXd::Constant(1,_tagid)); //change tagid to int ? do not use descriptor vector ?
setId(_tagid); // overwrite lmk ID to match tag_id.
}
LandmarkApriltag::~LandmarkApriltag()
{
//
}
double LandmarkApriltag::getTagWidth() const
{
return tag_width_;
}
int LandmarkApriltag::getTagId() const
{
return tag_id_;
}
// LANDMARK SAVE AND LOAD FROM YAML
// static
LandmarkBasePtr LandmarkApriltag::create(const YAML::Node& _lmk_node)
{
// Parse YAML node with lmk info and data
unsigned int id = _lmk_node["id"] .as<unsigned int>();
unsigned int tag_id = _lmk_node["tag id"] .as<unsigned int>();
double tag_width = _lmk_node["tag width"] .as<double>();
Eigen::Vector3d pos = _lmk_node["position"] .as<Eigen::Vector3d>();
bool pos_fixed = _lmk_node["position fixed"] .as<bool>();
Eigen::Vector4d vquat;
if (_lmk_node["orientation"].size() == 3)
{
// we have been given 3 Euler angles in degrees
Eigen::Vector3d euler = M_TORAD * ( _lmk_node["orientation"] .as<Eigen::Vector3d>() );
Eigen::Matrix3d R = e2R(euler);
Eigen::Quaterniond quat = R2q(R);
vquat = quat.coeffs();
}
else if (_lmk_node["orientation"].size() == 4)
{
// we have been given a quaternion
vquat = _lmk_node["orientation"] .as<Eigen::Vector4d>();
}
bool ori_fixed = _lmk_node["orientation fixed"] .as<bool>();
Eigen::Vector7d pose; pose << pos, vquat;
// Create a new landmark
LandmarkApriltagPtr lmk_ptr = std::make_shared<LandmarkApriltag>(pose, tag_id, tag_width);
lmk_ptr->setId(id);
lmk_ptr->getP()->setFixed(pos_fixed);
lmk_ptr->getO()->setFixed(ori_fixed);
return lmk_ptr;
}
YAML::Node LandmarkApriltag::saveToYaml() const
{
// First base things
YAML::Node node = LandmarkBase::saveToYaml();
// Then Apriltag specific things
node["tag id"] = getTagId();
node["tag width"] = getTagWidth();
return node;
}
// Register landmark creator
namespace
{
const bool WOLF_UNUSED registered_lmk_apriltag = LandmarkFactory::get().registerCreator("LandmarkApriltag", LandmarkApriltag::create);
}
} // namespace wolf
......@@ -94,7 +94,7 @@ FactorBasePtr ProcessorIMU::emplaceFactor(FeatureBasePtr _feature_motion, Captur
CaptureIMUPtr cap_imu = std::static_pointer_cast<CaptureIMU>(_capture_origin);
FeatureIMUPtr ftr_imu = std::static_pointer_cast<FeatureIMU>(_feature_motion);
auto fac_imu = FactorBase::emplace<FactorIMU>(_feature_motion, ftr_imu, cap_imu, shared_from_this());
auto fac_imu = FactorBase::emplace<FactorIMU>(_feature_motion, ftr_imu, cap_imu, shared_from_this(), params_->apply_loss_function);
return fac_imu;
}
......
This diff is collapsed.
#include "core/ceres_wrapper/ceres_manager.h"
SolverManager::SolverManager()
{
}
SolverManager::~SolverManager()
{
removeAllStateUnits();
}
void SolverManager::solve()
{
}
//void SolverManager::computeCovariances(WolfProblemPtr _problem_ptr)
//{
//}
void SolverManager::update(const WolfProblemPtr _problem_ptr)
{
// IF REALLOCATION OF STATE, REMOVE EVERYTHING AND BUILD THE PROBLEM AGAIN
if (_problem_ptr->isReallocated())
{
// todo: reallocate x
}
else
{
// ADD/UPDATE STATE UNITS
for(auto state_unit_it = _problem_ptr->getStateList().begin(); state_unit_it!=_problem_ptr->getStateList().end(); state_unit_it++)
{
if ((*state_unit_it)->getPendingStatus() == ADD_PENDING)
addStateUnit(*state_unit_it);
else if((*state_unit_it)->getPendingStatus() == UPDATE_PENDING)
updateStateUnitStatus(*state_unit_it);
}
//std::cout << "state units updated!" << std::endl;
// REMOVE STATE UNITS
while (!_problem_ptr->getRemovedStateList().empty())
{
// TODO: remove state unit
//_problem_ptr->getRemovedStateList().pop_front();
}
//std::cout << "state units removed!" << std::endl;
// ADD CONSTRAINTS
FactorBasePtrList fac_list;
_problem_ptr->getTrajectory()->getFactorList(fac_list);
//std::cout << "fac_list.size() = " << fac_list.size() << std::endl;
for(auto fac_it = fac_list.begin(); fac_it!=fac_list.end(); fac_it++)
if ((*fac_it)->getPendingStatus() == ADD_PENDING)
addFactor(*fac_it);
//std::cout << "factors updated!" << std::endl;
}
}
void SolverManager::addFactor(FactorBasePtr _corr_ptr)
{
//TODO MatrixXd J; Vector e;
// getResidualsAndJacobian(_corr_ptr, J, e);
// solverQR->addFactor(_corr_ptr, J, e);
// factor_map_[_corr_ptr->id()] = blockIdx;
_corr_ptr->setPendingStatus(NOT_PENDING);
}
void SolverManager::removeFactor(const unsigned int& _corr_idx)
{
// TODO
}
void SolverManager::addStateUnit(StateBlockPtr _st_ptr)
{
//std::cout << "Adding State Unit " << _st_ptr->id() << std::endl;
//_st_ptr->print();
switch (_st_ptr->getStateType())
{
case ST_COMPLEX_ANGLE:
{
// TODO
//std::cout << "Adding Complex angle Local Parametrization to the List... " << std::endl;
//ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StateComplexAngle*)_st_ptr)->BLOCK_SIZE, new ComplexAngleParameterization);
break;
}
case ST_THETA:
{
//std::cout << "No Local Parametrization to be added" << std::endl;
ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StateBlockPtr)_st_ptr)->BLOCK_SIZE, nullptr);
break;
}
case ST_POINT_1D:
{
//std::cout << "No Local Parametrization to be added" << std::endl;
ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StatePoint1D*)_st_ptr)->BLOCK_SIZE, nullptr);
break;
}
case ST_VECTOR:
{
//std::cout << "No Local Parametrization to be added" << std::endl;
ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StateBlockPtr)_st_ptr)->BLOCK_SIZE, nullptr);
break;
}
case ST_POINT_3D:
{
//std::cout << "No Local Parametrization to be added" << std::endl;
ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StateBlockPtr)_st_ptr)->BLOCK_SIZE, nullptr);
break;
}
default:
std::cout << "Unknown Local Parametrization type!" << std::endl;
}
if (_st_ptr->isFixed())
updateStateUnitStatus(_st_ptr);
_st_ptr->setPendingStatus(NOT_PENDING);
}
void SolverManager::removeAllStateUnits()
{
std::vector<double*> parameter_blocks;
ceres_problem_->GetParameterBlocks(&parameter_blocks);
for (unsigned int i = 0; i< parameter_blocks.size(); i++)
ceres_problem_->RemoveParameterBlock(parameter_blocks[i]);
}
void SolverManager::updateStateUnitStatus(StateBlockPtr _st_ptr)
{
// TODO
// if (!_st_ptr->isFixed())
// ceres_problem_->SetParameterBlockVariable(_st_ptr->get());
// else if (_st_ptr->isFixed())
// ceres_problem_->SetParameterBlockConstant(_st_ptr->get());
// else
// printf("\nERROR: Update state unit status with unknown status");
//
// _st_ptr->setPendingStatus(NOT_PENDING);
}
ceres::CostFunction* SolverManager::createCostFunction(FactorBasePtr _corrPtr)
{
//std::cout << "adding ctr " << _corrPtr->id() << std::endl;
//_corrPtr->print();
switch (_corrPtr->getFactorType())
{
case FAC_GPS_FIX_2D:
{
FactorGPS2D* specific_ptr = (FactorGPS2D*)(_corrPtr);
return new ceres::AutoDiffCostFunction<FactorGPS2D,
specific_ptr->residualSize,
specific_ptr->block0Size,
specific_ptr->block1Size,
specific_ptr->block2Size,
specific_ptr->block3Size,
specific_ptr->block4Size,
specific_ptr->block5Size,
specific_ptr->block6Size,
specific_ptr->block7Size,
specific_ptr->block8Size,
specific_ptr->block9Size>(specific_ptr);
break;
}
case FAC_ODOM_2D_COMPLEX_ANGLE:
{
FactorOdom2DComplexAngle* specific_ptr = (FactorOdom2DComplexAngle*)(_corrPtr);
return new ceres::AutoDiffCostFunction<FactorOdom2DComplexAngle,
specific_ptr->residualSize,
specific_ptr->block0Size,
specific_ptr->block1Size,
specific_ptr->block2Size,
specific_ptr->block3Size,
specific_ptr->block4Size,
specific_ptr->block5Size,
specific_ptr->block6Size,
specific_ptr->block7Size,
specific_ptr->block8Size,
specific_ptr->block9Size>(specific_ptr);
break;
}
case FAC_ODOM_2D:
{
FactorOdom2D* specific_ptr = (FactorOdom2D*)(_corrPtr);
return new ceres::AutoDiffCostFunction<FactorOdom2D,
specific_ptr->residualSize,
specific_ptr->block0Size,
specific_ptr->block1Size,
specific_ptr->block2Size,
specific_ptr->block3Size,
specific_ptr->block4Size,
specific_ptr->block5Size,
specific_ptr->block6Size,
specific_ptr->block7Size,
specific_ptr->block8Size,
specific_ptr->block9Size>(specific_ptr);
break;
}
case FAC_CORNER_2D:
{
FactorCorner2D* specific_ptr = (FactorCorner2D*)(_corrPtr);
return new ceres::AutoDiffCostFunction<FactorCorner2D,
specific_ptr->residualSize,
specific_ptr->block0Size,
specific_ptr->block1Size,
specific_ptr->block2Size,
specific_ptr->block3Size,
specific_ptr->block4Size,
specific_ptr->block5Size,
specific_ptr->block6Size,
specific_ptr->block7Size,
specific_ptr->block8Size,
specific_ptr->block9Size>(specific_ptr);
break;
}
case FAC_IMU:
{
FactorIMU* specific_ptr = (FactorIMU*)(_corrPtr);
return new ceres::AutoDiffCostFunction<FactorIMU,
specific_ptr->residualSize,
specific_ptr->block0Size,
specific_ptr->block1Size,
specific_ptr->block2Size,
specific_ptr->block3Size,
specific_ptr->block4Size,
specific_ptr->block5Size,
specific_ptr->block6Size,
specific_ptr->block7Size,
specific_ptr->block8Size,
specific_ptr->block9Size>(specific_ptr);
break;
}
default:
std::cout << "Unknown factor type! Please add it in the CeresWrapper::createCostFunction()" << std::endl;
return nullptr;
}
}
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