Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • mobile_robotics/wolf_projects/wolf_lib/wolf
1 result
Show changes
Showing
with 641 additions and 790 deletions
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/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.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// 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/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
/*
* FeatureRangeBearing2d.cpp
*
* Created on: Nov 30, 2017
* Author: jsola
*/
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include "feature_range_bearing.h"
namespace wolf
{
FeatureRangeBearing::FeatureRangeBearing(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance) :
FeatureBase("FeatureRangeBearing", _measurement, _meas_covariance)
FeatureRangeBearing::FeatureRangeBearing(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance)
: FeatureBase("FeatureRangeBearing", _measurement, _meas_covariance)
{
//
}
......@@ -42,5 +31,4 @@ FeatureRangeBearing::~FeatureRangeBearing()
//
}
} // namespace wolf
} // namespace wolf
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/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.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// 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/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
/*
* FeatureRangeBearing2d.h
*
* Created on: Nov 30, 2017
* Author: jsola
*/
#ifndef HELLO_WOLF_FEATURE_RANGE_BEARING_H_
#define HELLO_WOLF_FEATURE_RANGE_BEARING_H_
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
#include "core/feature/feature_base.h"
namespace wolf
{
WOLF_PTR_TYPEDEFS(FeatureRangeBearing);
class FeatureRangeBearing : public FeatureBase
{
public:
FeatureRangeBearing(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance);
~FeatureRangeBearing() override;
public:
FeatureRangeBearing(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance);
~FeatureRangeBearing() override;
};
} // namespace wolf
#endif /* HELLO_WOLF_FEATURE_RANGE_BEARING_H_ */
} // namespace wolf
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/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.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// 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/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
/**
* \file hello_wolf.cpp
*
* Created on: Dec 1, 2017 -- two months exactly after Oct-1st, we still have 4 political prisoners.
*
* ###
* ## ##
* ###
* ## ##
* ## ##
*
* \author: jsola
*/
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// wolf core includes
#include "../../include/core/ceres_wrapper/solver_ceres.h"
#include "core/common/wolf.h"
#include "core/sensor/sensor_odom_2d.h"
#include "core/ceres_wrapper/solver_ceres.h"
#include "core/sensor/sensor_odom.h"
#include "core/processor/processor_odom_2d.h"
#include "core/capture/capture_odom_2d.h"
// hello wolf local includes
#include "sensor_range_bearing.h"
#include "processor_range_bearing.h"
#include "capture_range_bearing.h"
int main()
{
{
/*
* ============= PROBLEM DEFINITION ==================
*
......@@ -114,7 +98,8 @@ int main()
* - Second, using random values
* Both solutions must produce the same exact values as in the sketches above.
*
* Optionally, the user can opt to self-calibrate the sensor's orientation (see NOTE within the code around Line 151)
* Optionally, the user can opt to self-calibrate the sensor's orientation (see NOTE within the code around Line
* 151)
*
* (c) 2017 Joan Sola @ IRI-CSIC
*/
......@@ -123,61 +108,123 @@ int main()
using namespace wolf;
// Wolf problem and solver
ProblemPtr problem = Problem::create("PO", 2);
SolverCeresPtr ceres = std::make_shared<SolverCeres>(problem);
ceres->getSolverOptions().max_num_iterations = 1000; // We depart far from solution, need a lot of iterations
// sensor odometer 2d
ParamsSensorOdom2dPtr intrinsics_odo = std::make_shared<ParamsSensorOdom2d>();
SensorBasePtr sensor_odo = problem->installSensor("SensorOdom2d", "sensor odo", Vector3d(0,0,0), intrinsics_odo);
// processor odometer 2d
ParamsProcessorOdom2dPtr params_odo = std::make_shared<ParamsProcessorOdom2d>();
params_odo->voting_active = true;
params_odo->time_tolerance = 0.1;
params_odo->max_time_span = 999;
params_odo->dist_traveled = 0.95; // Will make KFs automatically every 1m displacement
params_odo->angle_turned = 999;
params_odo->cov_det = 999;
params_odo->unmeasured_perturbation_std = 0.001;
ProcessorBasePtr processor = problem->installProcessor("ProcessorOdom2d", "processor odo", sensor_odo, params_odo);
// sensor Range and Bearing
ParamsSensorRangeBearingPtr intrinsics_rb = std::make_shared<ParamsSensorRangeBearing>();
intrinsics_rb->noise_range_metres_std = 0.1;
intrinsics_rb->noise_bearing_degrees_std = 1.0;
SensorBasePtr sensor_rb = problem->installSensor("SensorRangeBearing", "sensor RB", Vector3d(1,1,0), intrinsics_rb);
// processor Range and Bearing
ParamsProcessorRangeBearingPtr params_rb = std::make_shared<ParamsProcessorRangeBearing>();
params_rb->voting_active = false;
params_rb->time_tolerance = 0.01;
ProcessorBasePtr processor_rb = problem->installProcessor("ProcessorRangeBearing", "processor RB", sensor_rb, params_rb);
// initialize
TimeStamp t(0.0); // t : 0.0
// Vector3d x(0,0,0);
VectorComposite x(Vector3d(0,0,0), "PO", {2,1});
// Matrix3d P = Matrix3d::Identity() * 0.1;
VectorComposite P(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1});
FrameBasePtr KF1 = problem->setPriorFactor(x, P, t); // KF1 : (0,0,0)
// Wolf problem
ProblemPtr problem = Problem::create(2);
// Solver
YAML::Node params_solver;
params_solver["period"] = 1;
params_solver["verbose"] = 2;
params_solver["interrupt_on_problem_change"] = false;
params_solver["max_num_iterations"] = 1000;
params_solver["n_threads"] = 2;
params_solver["parameter_tolerance"] = 1e-8;
params_solver["function_tolerance"] = 1e-8;
params_solver["gradient_tolerance"] = 1e-9;
params_solver["minimizer"] = "levenberg_marquardt";
params_solver["use_nonmonotonic_steps"] = false;
params_solver["compute_cov"] = false;
SolverCeresPtr ceres = std::static_pointer_cast<SolverCeres>(SolverCeres::create(problem, params_solver));
// Sensor odometer 2d
YAML::Node params_sen_odo;
params_sen_odo["type"] = "SensorOdom2d";
params_sen_odo["name"] = "Sensor Odometry";
params_sen_odo["states"]["P"]["value"] = Vector2d::Zero();
params_sen_odo["states"]["P"]["prior"]["mode"] = "fix";
params_sen_odo["states"]["P"]["dynamic"] = false;
params_sen_odo["states"]["O"]["value"] = Vector1d::Zero();
params_sen_odo["states"]["O"]["prior"]["mode"] = "fix";
params_sen_odo["states"]["O"]["dynamic"] = false;
params_sen_odo["enabled"] = true;
params_sen_odo["k_disp_to_disp"] = 0.1;
params_sen_odo["k_disp_to_rot"] = 0.1;
params_sen_odo["k_rot_to_rot"] = 0.1;
params_sen_odo["min_disp_var"] = 1;
params_sen_odo["min_rot_var"] = 1;
SensorBasePtr sensor_odo = problem->installSensor(params_sen_odo);
std::cout << "sensor_odo created\n";
// Processor odometer 2d
YAML::Node params_prc_odo;
params_prc_odo["type"] = "ProcessorOdom2d";
params_prc_odo["name"] = "Processor Odometry";
params_prc_odo["sensor_name"] = "Sensor Odometry";
params_prc_odo["time_tolerance"] = 0.1;
params_prc_odo["keyframe_vote"]["voting_active"] = true;
params_prc_odo["keyframe_vote"]["max_time_span"] = 999;
params_prc_odo["keyframe_vote"]["max_dist_traveled"] = 0.95; // Will make KFs automatically every 1m displacement
params_prc_odo["keyframe_vote"]["max_angle_turned"] = 999;
params_prc_odo["keyframe_vote"]["max_cov_det"] = 999;
params_prc_odo["keyframe_vote"]["max_buff_length"] = 999;
params_prc_odo["state_provider"] = true;
params_prc_odo["state_provider_order"] = 1;
params_prc_odo["unmeasured_perturbation_std"] = 0.001;
params_prc_odo["apply_loss_function"] = false;
ProcessorBasePtr processor = problem->installProcessor(params_prc_odo);
std::cout << "processor created\n";
// Sensor Range and Bearing
YAML::Node params_sen_rb;
params_sen_rb["type"] = "SensorRangeBearing";
params_sen_rb["name"] = "Sensor Range Bearing";
params_sen_rb["states"]["P"]["value"] = Vector2d::Zero();
params_sen_rb["states"]["P"]["prior"]["mode"] = "fix";
params_sen_rb["states"]["P"]["dynamic"] = false;
params_sen_rb["states"]["O"]["value"] = Vector1d::Zero();
params_sen_rb["states"]["O"]["prior"]["mode"] = "fix";
params_sen_rb["states"]["O"]["dynamic"] = false;
params_sen_rb["enabled"] = true;
params_sen_rb["noise_range_metres_std"] = 0.1;
params_sen_rb["noise_bearing_degrees_std"] = 1.0;
SensorBasePtr sensor_rb = problem->installSensor(params_sen_rb);
std::cout << "sensor_rb created\n";
// Processor Range and Bearing
YAML::Node params_prc_rb;
params_prc_rb["type"] = "ProcessorRangeBearing";
params_prc_rb["name"] = "Processor Range Bearing";
params_prc_rb["sensor_name"] = "Sensor Range Bearing";
params_prc_rb["voting_active"] = false;
params_prc_rb["time_tolerance"] = 0.01;
params_prc_rb["apply_loss_function"] = false;
ProcessorBasePtr processor_rb = problem->installProcessor(params_prc_rb);
std::cout << "processor_rb created\n";
// Initialize
TimeStamp t(0.0); // t : 0.0
YAML::Node params_first_frame;
params_first_frame["P"]["value"] = Vector2d::Zero();
params_first_frame["P"]["prior"]["mode"] = "factor";
params_first_frame["P"]["prior"]["factor_std"] = Vector2d::Constant(sqrt(0.1));
params_first_frame["O"]["value"] = Vector1d::Zero();
params_first_frame["O"]["prior"]["mode"] = "factor";
params_first_frame["O"]["prior"]["factor_std"] = Vector1d::Constant(sqrt(0.1));
FrameBasePtr KF1 = problem->emplaceFirstFrame(t,
params_first_frame); // KF1 : (0,0,0)
std::static_pointer_cast<ProcessorMotion>(processor)->setOrigin(KF1);
// SELF CALIBRATION ===================================================
// SELF-CALIBRATION OF SENSOR ORIENTATION
// Uncomment this line below to achieve sensor self-calibration (of the orientation only, since the position is not observable)
sensor_rb->getO()->unfix();
// Uncomment this line below to achieve sensor self-calibration (of the orientation only, since the position is not
// observable):
// sensor_rb->getO()->unfix();
// SELF-CALIBRATION OF SENSOR POSITION
// The position is however not observable, and thus self-calibration would not work. You can try uncommenting the line below.
// The position is however not observable, and thus self-calibration would not work.
// You can try uncommenting the line below:
// sensor_rb->getP()->unfix();
// CONFIGURE ==========================================================
// Motion data
Vector2d motion_data(1.0, 0.0); // Will advance 1m at each keyframe, will not turn.
Vector2d motion_data(1.0, 0.0); // Will advance 1m at each keyframe, will not turn.
Matrix2d motion_cov = 0.1 * Matrix2d::Identity();
// landmark observations data
......@@ -193,82 +240,91 @@ int main()
// STEP 1 --------------------------------------------------------------
// observe lmks
ids.resize(1); ranges.resize(1); bearings.resize(1);
ids << 1; // will observe Lmk 1
ranges << 1.0; // see drawing
bearings << M_PI/2;
ids.resize(1);
ranges.resize(1);
bearings.resize(1);
ids << 1; // will observe Lmk 1
ranges << 1.0; // see drawing
bearings << M_PI / 2;
CaptureRangeBearingPtr cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings);
sensor_rb ->process(cap_rb); // L1 : (1,2)
cap_rb->process(); // L1 : (1,2)
// STEP 2 --------------------------------------------------------------
t += 1.0; // t : 1.0
t += 1.0; // t : 1.0
// motion
CaptureOdom2dPtr cap_motion = std::make_shared<CaptureOdom2d>(t, sensor_odo, motion_data, motion_cov);
sensor_odo ->process(cap_motion); // KF2 : (1,0,0)
cap_motion->process(); // KF2 : (1,0,0)
// observe lmks
ids.resize(2); ranges.resize(2); bearings.resize(2);
ids << 1, 2; // will observe Lmks 1 and 2
ranges << sqrt(2.0), 1.0; // see drawing
bearings << 3*M_PI/4, M_PI/2;
cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings);
sensor_rb ->process(cap_rb); // L1 : (1,2), L2 : (2,2)
ids.resize(2);
ranges.resize(2);
bearings.resize(2);
ids << 1, 2; // will observe Lmks 1 and 2
ranges << sqrt(2.0), 1.0; // see drawing
bearings << 3 * M_PI / 4, M_PI / 2;
cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings);
cap_rb->process(); // L1 : (1,2), L2 : (2,2)
// STEP 3 --------------------------------------------------------------
t += 1.0; // t : 2.0
t += 1.0; // t : 2.0
// motion
cap_motion ->setTimeStamp(t);
sensor_odo ->process(cap_motion); // KF3 : (2,0,0)
cap_motion->setTimeStamp(t);
cap_motion->process(); // KF3 : (2,0,0)
// observe lmks
ids.resize(2); ranges.resize(2); bearings.resize(2);
ids << 2, 3; // will observe Lmks 2 and 3
ranges << sqrt(2.0), 1.0; // see drawing
bearings << 3*M_PI/4, M_PI/2;
cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings);
sensor_rb ->process(cap_rb); // L1 : (1,2), L2 : (2,2), L3 : (3,2)
problem->print(1,0,1,0);
ids.resize(2);
ranges.resize(2);
bearings.resize(2);
ids << 2, 3; // will observe Lmks 2 and 3
ranges << sqrt(2.0), 1.0; // see drawing
bearings << 3 * M_PI / 4, M_PI / 2;
cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings);
cap_rb->process(); // L1 : (1,2), L2 : (2,2), L3 : (3,2)
problem->print(1, 0, 1, 0);
// SOLVE ================================================================
// SOLVE with exact initial guess
WOLF_TRACE("======== SOLVE PROBLEM WITH EXACT PRIORS =======")
std::string report = ceres->solve(wolf::SolverManager::ReportVerbosity::FULL);
WOLF_TRACE(report); // should show a very low iteration number (possibly 1)
problem->print(1,0,1,0);
WOLF_TRACE(report); // should show a very low iteration number (possibly 1)
problem->print(1, 0, 1, 0);
// PERTURB initial guess
WOLF_TRACE("======== PERTURB PROBLEM PRIORS =======")
problem->perturb(0.5); // Perturb all state blocks that are not fixed
problem->print(1,0,1,0);
problem->perturb(0.5); // Perturb all state blocks that are not fixed
problem->print(1, 0, 1, 0);
// SOLVE again
WOLF_TRACE("======== SOLVE PROBLEM WITH PERTURBED PRIORS =======")
report = ceres->solve(wolf::SolverManager::ReportVerbosity::FULL);
WOLF_TRACE(report); // should show a very high iteration number (more than 10, or than 100!)
problem->print(1,0,1,0);
WOLF_TRACE(report); // should show a very high iteration number (more than 10, or than 100!)
problem->print(1, 0, 1, 0);
// GET COVARIANCES of all states
WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======")
ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
bool succeed = ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
WOLF_ERROR_COND(not succeed, "Covariances could not be computed.");
for (auto& kf_pair : problem->getTrajectory()->getFrameMap())
{
Eigen::MatrixXd cov;
kf_pair.second->getCovariance(cov);
succeed = kf_pair.second->getCovariance("PO", cov);
WOLF_ERROR_COND(not succeed, "Covariance matrix for frame ", kf_pair.second->id(), " could not be recovered.");
WOLF_TRACE("KF", kf_pair.second->id(), "_cov = \n", cov);
}
for (auto& lmk : problem->getMap()->getLandmarkList())
{
Eigen::MatrixXd cov;
lmk->getCovariance(cov);
succeed = lmk->getCovariance("P", cov);
WOLF_ERROR_COND(not succeed, "Covariance matrix for landmark ", lmk->id(), " could not be recovered.");
WOLF_TRACE("L", lmk->id(), "_cov = \n", cov);
}
std::cout << std::endl;
WOLF_TRACE("======== FINAL PRINT FOR INTERPRETATION =======")
problem->print(4,0,1,0);
problem->print(4, 0, 1, 0);
/*
* ============= FIRST COMMENT ON THE RESULTS ==================
......@@ -298,33 +354,24 @@ int main()
* P: wolf tree status ---------------------
Hardware
S1 ODOM 2d // Sensor 1, type ODOMETRY 2d.
Extr [Sta] = [ Fix( 0 0 ) Fix( 0 ) ] // Static extrinsics, fixed position, fixed orientation (See notes 1 and 2 below)
Intr [Sta] = [ ] // Static intrinsics, but no intrinsics anyway
Extr [Sta] = [ Fix( 0 0 ) Fix( 0 ) ] // Static extrinsics, fixed position, fixed orientation (See
notes 1 and 2 below) Intr [Sta] = [ ] // Static intrinsics, but no intrinsics anyway
pm1 ODOM 2d // Processor 1, type ODOMETRY 2d
o: C7 - F3 // origin at Capture 7, Frame 3
l: C10 - F4 // last at Capture 10, frame 4
S2 RANGE BEARING // Sensor 2, type RANGE and BEARING.
Extr [Sta] = [ Fix( 1 1 ) Est( 0 ) ] // Static extrinsics, fixed position, estimated orientation (See notes 1 and 2 below)
Intr [Sta] = [ ] // Static intrinsics, but no intrinsics anyway
pt2 RANGE BEARING // Processor 2: type Range and Bearing
Trajectory
KF1 <-- c3 // KeyFrame 1, constrained by Factor 3
Est, ts=0, x = ( -1.6e-13 9.4e-11 1.4e-10 ) // State is estimated; time stamp and state vector
sb: Est Est // State's pos and orient are estimated
C1 FIX -> S- [ <-- // Capture 1, type FIX or Absolute
f1 FIX <-- // Feature 1, type Fix
m = ( 0 0 0 ) // The absolute measurement for this frame is (0,0,0) --> origin
c1 FIX --> A // Factor 1, type FIX, it is Absolute
CM2 ODOM 2d -> S1 [Sta, Sta] <-- // Capture 2, type ODOM, from Sensor 1 (static extr and intr)
C5 RANGE BEARING -> S2 [Sta, Sta] <-- // Capture 5, type R+B, from Sensor 2 (static extr and intr)
f2 RANGE BEARING <-- // Feature 2, type R+B
m = ( 1 1.57 ) // The feature's measurement is 1m, 1.57rad
c2 RANGE BEARING --> L1 // Factor 2 against Landmark 1
KF2 <-- c6
Est, ts=1, x = ( 1 2.5e-10 1.6e-10 )
sb: Est Est
CM3 ODOM 2d -> S1 [Sta, Sta] <--
f3 ODOM 2d <--
Extr [Sta] = [ Fix( 1 1 ) Est( 0 ) ] // Static extrinsics, fixed position, estimated orientation
(See notes 1 and 2 below) Intr [Sta] = [ ] // Static intrinsics, but no intrinsics
anyway pt2 RANGE BEARING // Processor 2: type Range and Bearing Trajectory KF1 <-- c3
// KeyFrame 1, constrained by Factor 3 Est, ts=0, x = ( -1.6e-13 9.4e-11 1.4e-10 ) // State is estimated; time
stamp and state vector sb: Est Est // State's pos and orient are estimated C1 FIX
-> S- [ <-- // Capture 1, type FIX or Absolute f1 FIX <-- // Feature 1, type Fix m = ( 0
0 0 ) // The absolute measurement for this frame is (0,0,0) --> origin c1 FIX --> A //
Factor 1, type FIX, it is Absolute CM2 ODOM 2d -> S1 [Sta, Sta] <-- // Capture 2, type ODOM, from
Sensor 1 (static extr and intr) C5 RANGE BEARING -> S2 [Sta, Sta] <-- // Capture 5, type R+B, from Sensor 2
(static extr and intr) f2 RANGE BEARING <-- // Feature 2, type R+B m = ( 1 1.57 ) // The
feature's measurement is 1m, 1.57rad c2 RANGE BEARING --> L1 // Factor 2 against Landmark 1 KF2
<-- c6 Est, ts=1, x = ( 1 2.5e-10 1.6e-10 ) sb: Est Est CM3 ODOM 2d -> S1 [Sta, Sta] <-- f3 ODOM 2d <--
m = ( 1 0 0 )
c3 ODOM 2d --> F1 // Factor 3, type ODOM, against Frame 1
C9 RANGE BEARING -> S2 [Sta, Sta] <--
......@@ -376,7 +423,8 @@ int main()
* Fixed: they are used as constant values, never estimated
* Estimated: they are estimated by the solver iteratively
*
* Therefore, in the case of Sensors, each block of parameters can be Static/Dynamic and Fixed/Estimated. This produces 4 combinations:
* Therefore, in the case of Sensors, each block of parameters can be Static/Dynamic and Fixed/Estimated. This
produces 4 combinations:
*
* 1 Fixed + Static : general case of calibrated sensor.
* Example: rigidly fixed sensor with calibrated parameters
......
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/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.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// 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/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
/**
* \file hello_wolf_autoconf.cpp
*
* Created on: Jul 16, 2019
* \author: jsola
*/
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// wolf core includes
#include "core/common/wolf.h"
#include "core/ceres_wrapper/solver_ceres.h"
#include "core/solver/factory_solver.h"
#include "core/common/wolf.h"
#include "core/capture/capture_odom_2d.h"
#include "core/processor/processor_motion.h"
#include "core/yaml/parser_yaml.h"
#include "capture_range_bearing.h"
// hello wolf local includes
#include "capture_range_bearing.h"
int main()
{
{
/*
* ============= PROBLEM DEFINITION ==================
*
......@@ -106,7 +96,8 @@ int main()
* - Second, using random values
* Both solutions must produce the same exact values as in the sketches above.
*
* Optionally, the user can opt to self-calibrate the sensor's orientation (see NOTE within the code around Line 136)
* Optionally, the user can opt to self-calibrate the sensor's orientation (see NOTE within the code around Line
* 136)
*
* (c) 2017 Joan Sola @ IRI-CSIC
*/
......@@ -115,52 +106,46 @@ int main()
using namespace wolf;
WOLF_INFO("======== CONFIGURE PROBLEM =======");
// Config file to parse. Here is where all the problem is defined:
std::string config_file = "demos/hello_wolf/yaml/hello_wolf_config.yaml";
std::string wolf_path = std::string(_WOLF_ROOT_DIR);
// parse file into params server: each param will be retrievable from this params server:
ParserYaml parser = ParserYaml(config_file, wolf_path);
ParamsServer server = ParamsServer(parser.getParams());
// Wolf problem: automatically build the left branch of the wolf tree from the contents of the params server:
ProblemPtr problem = Problem::autoSetup(server);
std::string wolf_path = _WOLF_CODE_DIR;
std::string config_file = wolf_path + "/demos/hello_wolf/yaml/hello_wolf_config.yaml";
// Print problem to see its status before processing any sensor data
problem->print(4,0,1,0);
// Wolf problem: Build the Hardware branch of the wolf tree from the config yaml file
ProblemPtr problem = Problem::autoSetup(config_file, {wolf_path});
// Solver. Configure a Ceres solver
// ceres::Solver::Options options;
// options.max_num_iterations = 1000; // We depart far from solution, need a lot of iterations
// SolverCeresPtr ceres = std::make_shared<SolverCeres>(problem, options);
SolverManagerPtr ceres = FactorySolver::create("SolverCeres", problem, server);
// Solver: Create a Solver Ceres from the config yaml file
SolverManagerPtr ceres = SolverManager::autoSetup(problem, config_file, {wolf_path});
// recover sensor pointers and other stuff for later use (access by sensor name)
SensorBasePtr sensor_odo = problem->findSensor("sen odom");
SensorBasePtr sensor_rb = problem->findSensor("sen rb");
SensorBasePtr sensor_odo = problem->findSensor("sen odom");
SensorBasePtr sensor_rb = problem->findSensor("sen rb");
// APPLY PRIOR and SET PROCESSOR ODOM ORIGIN ===================================================
TimeStamp t(0.0);
FrameBasePtr KF1 = problem->applyPriorOptions(t);
// std::static_pointer_cast<ProcessorMotion>(problem->getMotionProviderMap().begin()->second)->setOrigin(KF1);
// Apply first frame options
TimeStamp t(0.0);
FrameBasePtr KF1 = problem->applyFirstFrameOptions(t);
// Print problem to see its status before processing any sensor data
problem->print(4, 0, 1, 0);
// SELF CALIBRATION ===================================================
// These few lines control whether we calibrate some sensor parameters or not.
// SELF-CALIBRATION OF SENSOR ORIENTATION
// Uncomment this line below to achieve sensor self-calibration (of the orientation only, since the position is not observable)
// Uncomment this line below to achieve sensor self-calibration (of the orientation only, since the position is not
// observable)
// sensor_rb->getO()->unfix();
// SELF-CALIBRATION OF SENSOR POSITION
// The position is however not observable, and thus self-calibration would not work. You can try uncommenting the line below.
// The position is however not observable, and thus self-calibration would not work.
// You can try uncommenting the line below.
// sensor_rb->getP()->unfix();
// CONFIGURE input data (motion and measurements) ==============================================
// Motion data
Vector2d motion_data(1.0, 0.0); // Will advance 1m at each keyframe, will not turn.
Vector2d motion_data(1.0, 0.0); // Will advance 1m at each keyframe, will not turn.
Matrix2d motion_cov = 0.1 * Matrix2d::Identity();
// landmark observations data
......@@ -176,69 +161,75 @@ int main()
// STEP 1 --------------------------------------------------------------
// move zero motion to accept the first keyframe and initialize the processor
CaptureOdom2dPtr cap_motion = std::make_shared<CaptureOdom2d>(t, sensor_odo, 0*motion_data, 0*motion_cov);
cap_motion ->process(); // KF1 : (0,0,0)
CaptureOdom2dPtr cap_motion = std::make_shared<CaptureOdom2d>(t, sensor_odo, 0 * motion_data, 0 * motion_cov);
cap_motion->process(); // KF1 : (0,0,0)
// observe lmks
ids.resize(1); ranges.resize(1); bearings.resize(1);
ids << 1; // will observe Lmk 1
ranges << 1.0; // see drawing
bearings << M_PI/2;
ids.resize(1);
ranges.resize(1);
bearings.resize(1);
ids << 1; // will observe Lmk 1
ranges << 1.0; // see drawing
bearings << M_PI / 2;
CaptureRangeBearingPtr cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings);
cap_rb->process(); // L1 : (1,2)
cap_rb->process(); // L1 : (1,2)
// STEP 2 --------------------------------------------------------------
t += 1.0; // t : 1.0
t += 1.0; // t : 1.0
// motion
cap_motion = std::make_shared<CaptureOdom2d>(t, sensor_odo, motion_data, motion_cov);
cap_motion ->process(); // KF2 : (1,0,0)
cap_motion->process(); // KF2 : (1,0,0)
// observe lmks
ids.resize(2); ranges.resize(2); bearings.resize(2);
ids << 1, 2; // will observe Lmks 1 and 2
ranges << sqrt(2.0), 1.0; // see drawing
bearings << 3*M_PI/4, M_PI/2;
ids.resize(2);
ranges.resize(2);
bearings.resize(2);
ids << 1, 2; // will observe Lmks 1 and 2
ranges << sqrt(2.0), 1.0; // see drawing
bearings << 3 * M_PI / 4, M_PI / 2;
cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings);
cap_rb ->process(); // L1 : (1,2), L2 : (2,2)
cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings);
cap_rb->process(); // L1 : (1,2), L2 : (2,2)
// STEP 3 --------------------------------------------------------------
t += 1.0; // t : 2.0
t += 1.0; // t : 2.0
// motion
cap_motion ->setTimeStamp(t);
cap_motion ->process(); // KF3 : (2,0,0)
cap_motion->setTimeStamp(t);
cap_motion->process(); // KF3 : (2,0,0)
// observe lmks
ids.resize(2); ranges.resize(2); bearings.resize(2);
ids << 2, 3; // will observe Lmks 2 and 3
ranges << sqrt(2.0), 1.0; // see drawing
bearings << 3*M_PI/4, M_PI/2;
ids.resize(2);
ranges.resize(2);
bearings.resize(2);
ids << 2, 3; // will observe Lmks 2 and 3
ranges << sqrt(2.0), 1.0; // see drawing
bearings << 3 * M_PI / 4, M_PI / 2;
cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings);
cap_rb ->process(); // L1 : (1,2), L2 : (2,2), L3 : (3,2)
cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings);
cap_rb->process(); // L1 : (1,2), L2 : (2,2), L3 : (3,2)
problem->print(1,0,1,0);
problem->print(1, 0, 1, 0);
// SOLVE ================================================================
// SOLVE with exact initial guess
WOLF_INFO("======== SOLVE PROBLEM WITH EXACT PRIORS =======")
std::string report = ceres->solve(wolf::SolverManager::ReportVerbosity::FULL);
WOLF_INFO(report); // should show a very low iteration number (possibly 1)
problem->print(1,0,1,0);
std::string report = ceres->solve(SolverManager::ReportVerbosity::FULL);
WOLF_INFO(report); // should show a very low iteration number (possibly 1)
problem->print(1, 0, 1, 0);
// PERTURB initial guess
WOLF_INFO("======== PERTURB PROBLEM PRIORS =======")
problem->perturb(0.5); // Perturb all state blocks that are not fixed
problem->print(1,0,1,0);
problem->perturb(0.5); // Perturb all state blocks that are not fixed
problem->print(1, 0, 1, 0);
// SOLVE again
WOLF_INFO("======== SOLVE PROBLEM WITH PERTURBED PRIORS =======")
report = ceres->solve(wolf::SolverManager::ReportVerbosity::FULL);
WOLF_INFO(report); // should show a very high iteration number (more than 10, or than 100!)
problem->print(1,0,1,0);
report = ceres->solve(SolverManager::ReportVerbosity::FULL);
WOLF_INFO(report); // should show a very high iteration number (more than 10, or than 100!)
problem->print(1, 0, 1, 0);
// GET COVARIANCES of all states
WOLF_INFO("======== COVARIANCES OF SOLVED PROBLEM =======")
......@@ -246,26 +237,26 @@ int main()
for (auto& kf_pair : problem->getTrajectory()->getFrameMap())
{
Eigen::MatrixXd cov;
kf_pair.second->getCovariance(cov);
kf_pair.second->getCovariance("PO", cov);
WOLF_INFO("KF", kf_pair.second->id(), "_cov = \n", cov);
}
for (auto& lmk : problem->getMap()->getLandmarkList())
{
Eigen::MatrixXd cov;
lmk->getCovariance(cov);
lmk->getCovariance("P", cov);
WOLF_INFO("L", lmk->id(), "_cov = \n", cov);
}
std::cout << std::endl;
WOLF_INFO("======== FINAL PRINT FOR INTERPRETATION =======")
problem->print(4,1,1,1);
problem->print(4, 1, 1, 1);
/*
* ============= FIRST COMMENT ON THE RESULTS ==================
*
* IF YOU SEE at the end of the printed problem the estimate for Lmk 3 as:
*
* Lmk3 LandmarkPoint2d <-- Fac9
* Lmk3 Landmark2d <-- Fac9
* P[Est] = ( 1 2 )
*
* it means WOLF SOLVED SUCCESSFULLY (L3 is effectively at location (3,2) ) !
......@@ -285,81 +276,83 @@ int main()
* The full message is explained below.
*
* P: wolf tree status ---------------------
Hardware
Sen1 SensorOdom2d "sen odom" // Sensor 1, type odometry 2D
sb: P[Sta,Fix] = ( 0 0 ); O[Sta,Fix] = ( 0 ); // Static extrinsics, fixed position, fixed orientation (See notes 1 and 2 below)
PrcM1 ProcessorOdom2d "prc odom" // Processor 1, type odometry 2D
o: Cap6 - KFrm3 // origin at Capture 6, Frame 3
l: Cap8 - Frm4 // last at Capture 8, Frame 4
Sen2 SensorRangeBearing "sen rb" // Sensor 2, type range and bearing
sb: P[Sta,Fix] = ( 1 1 ); O[Sta,Fix] = ( 0 ); // Static extrinsics, fixed position, estimated orientation (See notes 1 and 2 below)
Prc2 ProcessorRangeBearing "prc rb" // Processor 2, type range and bearing
Trajectory
KFrm1 <-- Fac4 // KeyFrame 1, constrained by Factor 4
P[Est] = ( -4.4e-12 1.5e-09 ) // Position is estimated
O[Est] = ( 2.6e-09 ) // Orientation is estimated
Cap1 CaptureVoid -> Sen- <-- // This is an "artificial" capture used to hold the features relative to the prior
Ftr1 trk0 prior <-- // Position prior
m = ( 0 0 )
Fac1 FactorBlockAbsolute --> Abs
Ftr2 trk0 prior <-- // Orientation prior
m = ( 0 )
Fac2 FactorBlockAbsolute --> Abs
CapM2 CaptureOdom2d -> Sen1 <-- // Capture 2 of type motion odom 2d from sensor 1.
buffer size : 0
Cap4 CaptureRangeBearing -> Sen2 <--
Ftr3 trk0 FeatureRangeBearing <--
m = ( 1 1.6 )
Fac3 RANGE BEARING --> Lmk1
KFrm2 <-- Fac7
P[Est] = ( 1 5.7e-09 )
O[Est] = ( 5.7e-09 )
CapM3 CaptureOdom2d -> Sen1 -> OCap2 ; OFrm1 <--// Capture 3 of type motion odom2d from sensor 1.
// Its origin is at Capture 2; Frame 1
buffer size : 2
delta preint : (1 0 0)
Ftr4 trk0 ProcessorOdom2d <--
m = ( 1 0 0 )
Fac4 FactorOdom2d --> Frm1
Cap7 CaptureRangeBearing -> Sen2 <--
Ftr5 trk0 FeatureRangeBearing <--
m = ( 1.4 2.4 )
Fac5 RANGE BEARING --> Lmk1
Ftr6 trk0 FeatureRangeBearing <--
m = ( 1 1.6 )
Fac6 RANGE BEARING --> Lmk2
KFrm3 <--
P[Est] = ( 2 1.2e-08 )
O[Est] = ( 6.6e-09 )
CapM6 CaptureOdom2d -> Sen1 -> OCap3 ; OFrm2 <--
buffer size : 2
delta preint : (1 0 0)
Ftr7 trk0 ProcessorOdom2d <--
m = ( 1 0 0 )
Fac7 FactorOdom2d --> Frm2
Cap9 CaptureRangeBearing -> Sen2 <--
Ftr8 trk0 FeatureRangeBearing <--
m = ( 1.4 2.4 )
Fac8 RANGE BEARING --> Lmk2
Ftr9 trk0 FeatureRangeBearing <--
m = ( 1 1.6 )
Fac9 RANGE BEARING --> Lmk3
Frm4 <--
P[Est] = ( 2 0 )
O[Est] = ( 0 )
CapM8 CaptureOdom2d -> Sen1 -> OCap6 ; OFrm3 <--
buffer size : 1
delta preint : (0 0 0)
Map
Lmk1 LandmarkPoint2d <-- Fac3 Fac5 // Landmark 1 constrained by factors 3 & 5
P[Est] = ( 1 2 )
Lmk2 LandmarkPoint2d <-- Fac6 Fac8
P[Est] = ( 2 2 )
Lmk3 LandmarkPoint2d <-- Fac9
P[Est] = ( 3 2 )
-----------------------------------------
*
* Hardware
* Sen1 SensorOdom2d "sen odom" // Sensor 1, type odometry 2D
* sb: P[Sta,Fix] = ( 0 0 ); O[Sta,Fix] = ( 0 ); // Static extrinsics, fixed position, fixed orientation
* // (See notes 1 and 2 below)
* PrcM1 ProcessorOdom2d "prc odom" // Processor 1, type odometry 2D
* o: Cap6 - KFrm3 // origin at Capture 6, Frame 3
* l: Cap8 - Frm4 // last at Capture 8, Frame 4
* Sen2 SensorRangeBearing "sen rb" // Sensor 2, type range and bearing
* sb: P[Sta,Fix] = ( 1 1 ); O[Sta,Fix] = ( 0 ); // Static extrinsics, fixed position, estimated orientation
* // (See notes 1 and 2 below)
* Prc2 ProcessorRangeBearing "prc rb" // Processor 2, type range and bearing
* Trajectory
* KFrm1 <-- Fac4 // KeyFrame 1, constrained by Factor 4
* P[Est] = ( -4.4e-12 1.5e-09 ) // Position is estimated
* O[Est] = ( 2.6e-09 ) // Orientation is estimated
* Cap1 CaptureVoid -> Sen- <-- // This is an "artificial" capture used to hold the
* // features relative to the prior
* Ftr1 trk0 prior <-- // Position prior
* m = ( 0 0 )
* Fac1 FactorBlockAbsolute --> Abs
* Ftr2 trk0 prior <-- // Orientation prior
* m = ( 0 )
* Fac2 FactorBlockAbsolute --> Abs
* CapM2 CaptureOdom2d -> Sen1 <-- // Capture 2 of type motion odom 2d from sensor 1.
* buffer size : 0
* Cap4 CaptureRangeBearing -> Sen2 <--
* Ftr3 trk0 FeatureRangeBearing <--
* m = ( 1 1.6 )
* Fac3 RANGE BEARING --> Lmk1
* KFrm2 <-- Fac7
* P[Est] = ( 1 5.7e-09 )
* O[Est] = ( 5.7e-09 )
* CapM3 CaptureOdom2d -> Sen1 -> OCap2 ; OFrm1 <--// Capture 3 of type motion odom2d from sensor 1.
* // Its origin is at Capture 2; Frame 1
* buffer size : 2
* delta preint : (1 0 0)
* Ftr4 trk0 ProcessorOdom2d <--
* m = ( 1 0 0 )
* Fac4 FactorOdom2d --> Frm1
* Cap7 CaptureRangeBearing -> Sen2 <--
* Ftr5 trk0 FeatureRangeBearing <--
* m = ( 1.4 2.4 )
* Fac5 RANGE BEARING --> Lmk1
* Ftr6 trk0 FeatureRangeBearing <--
* m = ( 1 1.6 )
* Fac6 RANGE BEARING --> Lmk2
* KFrm3 <--
* P[Est] = ( 2 1.2e-08 )
* O[Est] = ( 6.6e-09 )
* CapM6 CaptureOdom2d -> Sen1 -> OCap3 ; OFrm2 <--
* buffer size : 2
* delta preint : (1 0 0)
* Ftr7 trk0 ProcessorOdom2d <--
* m = ( 1 0 0 )
* Fac7 FactorOdom2d --> Frm2
* Cap9 CaptureRangeBearing -> Sen2 <--
* Ftr8 trk0 FeatureRangeBearing <--
* m = ( 1.4 2.4 )
* Fac8 RANGE BEARING --> Lmk2
* Ftr9 trk0 FeatureRangeBearing <--
* m = ( 1 1.6 )
* Fac9 RANGE BEARING --> Lmk3
* Frm4 <--
* P[Est] = ( 2 0 )
* O[Est] = ( 0 )
* CapM8 CaptureOdom2d -> Sen1 -> OCap6 ; OFrm3 <--
* buffer size : 1
* delta preint : (0 0 0)
* Map
* Lmk1 Landmark2d <-- Fac3 Fac5 // Landmark 1 constrained by factors 3 & 5
* P[Est] = ( 1 2 )
* Lmk2 Landmark2d <-- Fac6 Fac8
* P[Est] = ( 2 2 )
* Lmk3 Landmark2d <-- Fac9
* P[Est] = ( 3 2 )
* -----------------------------------------
* *
* ============= GENERAL WOLF NOTES ==================
*
* Explanatory notes, general to the Wolf architecture design:
......@@ -372,7 +365,8 @@ int main()
* Fixed: they are used as constant values, never estimated
* Estimated: they are estimated by the solver iteratively
*
* Therefore, in the case of Sensors, each block of parameters can be Static/Dynamic and Fixed/Estimated. This produces 4 combinations:
* Therefore, in the case of Sensors, each block of parameters can be Static/Dynamic and Fixed/Estimated. This
* produces 4 combinations:
*
* 1 Fixed + Static : general case of calibrated sensor.
* Example: rigidly fixed sensor with calibrated parameters
......
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 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 landmark_point_2d.cpp
*
* Created on: Dec 4, 2017
* \author: jsola
*/
#include "landmark_point_2d.h"
#include "core/state_block/state_block_derived.h"
namespace wolf
{
LandmarkPoint2d::LandmarkPoint2d(int _id, const Eigen::Vector2d& _xy) :
LandmarkBase("LandmarkPoint2d", std::make_shared<StatePoint2d>(_xy))
{
setId(_id);
}
LandmarkPoint2d::~LandmarkPoint2d()
{
//
}
} /* namespace wolf */
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 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 landmark_point_2d.h
*
* Created on: Dec 4, 2017
* \author: jsola
*/
#ifndef HELLO_WOLF_LANDMARK_POINT_2d_H_
#define HELLO_WOLF_LANDMARK_POINT_2d_H_
#include "core/landmark/landmark_base.h"
namespace wolf
{
WOLF_PTR_TYPEDEFS(LandmarkPoint2d);
class LandmarkPoint2d : public LandmarkBase
{
public:
LandmarkPoint2d(int _id, const Eigen::Vector2d& _xy);
~LandmarkPoint2d() override;
};
} /* namespace wolf */
#endif /* HELLO_WOLF_LANDMARK_POINT_2d_H_ */
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/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.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// 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/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
/*
* processor_range_bearing.cpp
*
* Created on: Nov 30, 2017
* Author: jsola
*/
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include "processor_range_bearing.h"
#include "capture_range_bearing.h"
#include "landmark_point_2d.h"
#include "core/landmark/landmark.h"
#include "core/state_block/state_block_derived.h"
#include "feature_range_bearing.h"
#include "factor_range_bearing.h"
namespace wolf
{
ProcessorRangeBearing::ProcessorRangeBearing(ParamsProcessorBasePtr _params) :
ProcessorBase("ProcessorRangeBearing", 2, _params)
ProcessorRangeBearing::ProcessorRangeBearing(const YAML::Node& _params)
: ProcessorBase("ProcessorRangeBearing", {{'P', "StatePoint2d"}, {'O', "StateAngle"}}, _params)
{
//
}
......@@ -51,21 +41,22 @@ void ProcessorRangeBearing::processCapture(CaptureBasePtr _capture)
// 1. get KF
FrameBasePtr keyframe(nullptr);
if ( !buffer_frame_.empty() )
if (!buffer_frame_.empty())
{
// KeyFrame Callback received
keyframe = buffer_frame_.select( _capture->getTimeStamp(), params_->time_tolerance );
keyframe = buffer_frame_.select(_capture->getTimeStamp(), getTimeTolerance());
buffer_frame_.removeUpTo( _capture->getTimeStamp() );
buffer_frame_.removeUpTo(_capture->getTimeStamp());
assert( keyframe && "Callback KF is not close enough to _capture!");
assert(keyframe && "Callback KF is not close enough to _capture!");
}
if (!keyframe)
{
// No KeyFrame callback received -- we assume a KF is available to hold this _capture (checked in assert below)
keyframe = getProblem()->closestFrameToTimeStamp(_capture->getTimeStamp());
assert( (fabs(keyframe->getTimeStamp() - _capture->getTimeStamp()) < params_->time_tolerance) && "Could not find a KF close enough to _capture!");
assert((fabs(keyframe->getTimeStamp() - _capture->getTimeStamp()) < getTimeTolerance()) &&
"Could not find a KF close enough to _capture!");
}
// 2. cast incoming capture to the range-and-bearing type, add it to the keyframe
......@@ -76,49 +67,42 @@ void ProcessorRangeBearing::processCapture(CaptureBasePtr _capture)
for (SizeEigen i = 0; i < capture_rb->getIds().size(); i++)
{
// extract info
int id = capture_rb->getId (i);
double range = capture_rb->getRange (i);
double bearing = capture_rb->getBearing(i);
int id = capture_rb->getId(i);
double range = capture_rb->getRange(i);
double bearing = capture_rb->getBearing(i);
// 4. create or recover landmark
LandmarkPoint2dPtr lmk;
auto lmk_it = known_lmks.find(id);
if ( lmk_it != known_lmks.end() )
Landmark2dPtr lmk;
auto lmk_it = known_lmks.find(id);
if (lmk_it != known_lmks.end())
{
// known landmarks : recover landmark
lmk = std::static_pointer_cast<LandmarkPoint2d>(lmk_it->second);
lmk = std::static_pointer_cast<Landmark2d>(lmk_it->second);
WOLF_TRACE("known lmk(", id, "): ", lmk->getP()->getState().transpose());
}
else
{
// new landmark:
// - create landmark
lmk = LandmarkBase::emplace<LandmarkPoint2d>(getProblem()->getMap(), id, invObserve(range, bearing));
lmk = LandmarkBase::emplace<Landmark2d>(
getProblem()->getMap(), VectorComposite{{'P', invObserve(range, bearing)}}, PriorComposite());
lmk->setId(id);
WOLF_TRACE("new lmk(", id, "): ", lmk->getP()->getState().transpose());
// - add to known landmarks
known_lmks.emplace(id, lmk);
}
// 5. create feature
Vector2d measurement_rb(range,bearing);
auto ftr = FeatureBase::emplace<FeatureRangeBearing>(capture_rb,
measurement_rb,
getSensor()->getNoiseCov());
Vector2d measurement_rb(range, bearing);
auto ftr = FeatureBase::emplace<FeatureRangeBearing>(
capture_rb, measurement_rb, getSensor()->computeNoiseCov(measurement_rb));
// 6. create factor
auto prc = shared_from_this();
auto fac = FactorBase::emplace<FactorRangeBearing>(ftr,
capture_rb,
ftr,
lmk,
prc,
false,
FAC_ACTIVE);
auto fac = FactorBase::emplace<FactorRangeBearing>(ftr, capture_rb, ftr, lmk, prc, false, FAC_ACTIVE);
}
}
Eigen::Vector2d ProcessorRangeBearing::observe(const Eigen::Vector2d& lmk_w) const
{
return polar(toSensor(lmk_w));
......@@ -131,27 +115,26 @@ Eigen::Vector2d ProcessorRangeBearing::invObserve(double r, double b) const
ProcessorRangeBearing::Trf ProcessorRangeBearing::transform(const Eigen::Vector3d& _pose) const
{
Trf H = Eigen::Translation<double,2>(_pose(0), _pose(1)) * Eigen::Rotation2D<double>(_pose(2)) ;
Trf H = Eigen::Translation<double, 2>(_pose(0), _pose(1)) * Eigen::Rotation2D<double>(_pose(2));
return H;
}
ProcessorRangeBearing::Trf ProcessorRangeBearing::transform(const Eigen::Vector2d& _position,
const Eigen::Vector1d& _orientation) const
{
Trf H = Eigen::Translation<double,2>(_position(0), _position(1)) * Eigen::Rotation2D<double>(_orientation(0)) ;
Trf H = Eigen::Translation<double, 2>(_position(0), _position(1)) * Eigen::Rotation2D<double>(_orientation(0));
return H;
}
Eigen::Vector2d ProcessorRangeBearing::fromSensor(const Eigen::Vector2d& lmk_s) const
{
Trf H_w_r = transform(getProblem()->getState().vector("PO"));
Trf H_w_r = transform(getProblem()->getState("PO").vector("PO"));
return H_w_r * H_r_s * lmk_s;
}
Eigen::Vector2d ProcessorRangeBearing::toSensor(const Eigen::Vector2d& lmk_w) const
{
// Trf H_w_r = transform(getSensor()->getP()->getState(), getSensor()->getO()->getState());
Trf H_w_r = transform(getProblem()->getState().vector("PO"));
Trf H_w_r = transform(getProblem()->getState("PO").vector("PO"));
return (H_w_r * H_r_s).inverse() * lmk_w;
}
......@@ -170,19 +153,12 @@ Eigen::Vector2d ProcessorRangeBearing::rect(double range, double bearing) const
bool ProcessorRangeBearing::storeKeyFrame(FrameBasePtr _frame_ptr)
{
return true;
return true;
}
bool ProcessorRangeBearing::storeCapture(CaptureBasePtr _cap_ptr)
{
return false;
return false;
}
} /* namespace wolf */
// Register in the SensorFactory
#include "core/processor/factory_processor.h"
namespace wolf
{
WOLF_REGISTER_PROCESSOR(ProcessorRangeBearing)
WOLF_REGISTER_PROCESSOR_AUTO(ProcessorRangeBearing)
} // namespace wolf
} // namespace wolf
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/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.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// 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/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
/*
* ProcessorRangeBearing.h
*
* Created on: Nov 30, 2017
* Author: jsola
*/
#ifndef HELLO_WOLF_PROCESSOR_RANGE_BEARING_H_
#define HELLO_WOLF_PROCESSOR_RANGE_BEARING_H_
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
#include "core/processor/processor_base.h"
#include "sensor_range_bearing.h"
......@@ -37,79 +26,72 @@
namespace wolf
{
WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorRangeBearing);
struct ParamsProcessorRangeBearing : public ParamsProcessorBase
{
// We do not need special parameters, but in case you need they should be defined here.
ParamsProcessorRangeBearing()
{
//DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE.
}
ParamsProcessorRangeBearing(std::string _unique_name, const ParamsServer& _server) :
ParamsProcessorBase(_unique_name, _server)
{
//
}
std::string print() const
{
return ParamsProcessorBase::print();
}
};
using namespace Eigen;
WOLF_PTR_TYPEDEFS(ProcessorRangeBearing);
class ProcessorRangeBearing : public ProcessorBase
{
public:
typedef Eigen::Transform<double, 2, Eigen::Isometry> Trf;
ProcessorRangeBearing(ParamsProcessorBasePtr _params);
~ProcessorRangeBearing() override {/* empty */}
void configure(SensorBasePtr _sensor) override;
// Factory method for high level API
WOLF_PROCESSOR_CREATE(ProcessorRangeBearing, ParamsProcessorRangeBearing);
protected:
// Implementation of pure virtuals from ProcessorBase
void processCapture (CaptureBasePtr _capture) override;
void processKeyFrame (FrameBasePtr _keyframe_ptr) override {};
bool triggerInCapture (CaptureBasePtr) const override { return true;};
bool triggerInKeyFrame (FrameBasePtr _keyframe_ptr) const override {return false;}
bool voteForKeyFrame () const override {return false;}
/** \brief store key frame
*
* Returns true if the key frame should be stored
*/
bool storeKeyFrame(FrameBasePtr) override;
/** \brief store capture
*
* Returns true if the capture should be stored
*/
bool storeCapture(CaptureBasePtr) override;
private:
// control variables
Trf H_r_s; // transformation matrix, robot to sensor
std::map<int, LandmarkBasePtr> known_lmks; // all observed lmks so far
protected:
// helper methods -- to be used only here -- they would be better off in a separate library e.g. range_bearing_tools.h
Eigen::Vector2d observe (const Eigen::Vector2d& lmk_w) const;
Eigen::Vector2d invObserve (double r, double b) const;
private:
// helper methods -- to be used only here -- they would be better off in a separate library e.g. range_bearing_tools.h
Trf transform (const Eigen::Vector3d& _pose) const;
Trf transform (const Eigen::Vector2d& _position, const Eigen::Vector1d& _orientation) const;
Eigen::Vector2d fromSensor (const Eigen::Vector2d& lmk_s) const;
Eigen::Vector2d toSensor (const Eigen::Vector2d& lmk_w) const;
Eigen::Vector2d polar (const Eigen::Vector2d& rect) const;
Eigen::Vector2d rect (double range, double bearing) const;
public:
typedef Eigen::Transform<double, 2, Eigen::Isometry> Trf;
ProcessorRangeBearing(const YAML::Node& _params);
~ProcessorRangeBearing() override
{ /* empty */
}
void configure(SensorBasePtr _sensor) override;
// Factory method for high level API
WOLF_PROCESSOR_CREATE(ProcessorRangeBearing);
protected:
// Implementation of pure virtuals from ProcessorBase
void processCapture(CaptureBasePtr _capture) override;
void processKeyFrame(FrameBasePtr _keyframe_ptr) override{};
bool triggerInCapture(CaptureBasePtr) const override
{
return true;
};
bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const override
{
return false;
}
bool voteForKeyFrame() const override
{
return false;
}
/** \brief store key frame
*
* Returns true if the key frame should be stored
*/
bool storeKeyFrame(FrameBasePtr) override;
/** \brief store capture
*
* Returns true if the capture should be stored
*/
bool storeCapture(CaptureBasePtr) override;
private:
// control variables
Trf H_r_s; // transformation matrix, robot to sensor
std::map<int, LandmarkBasePtr> known_lmks; // all observed lmks so far
protected:
// helper methods -- to be used only here -- they would be better off in a separate library e.g.
// range_bearing_tools.h
Eigen::Vector2d observe(const Eigen::Vector2d& lmk_w) const;
Eigen::Vector2d invObserve(double r, double b) const;
private:
// helper methods -- to be used only here -- they would be better off in a separate library e.g.
// range_bearing_tools.h
Trf transform(const Eigen::Vector3d& _pose) const;
Trf transform(const Eigen::Vector2d& _position, const Eigen::Vector1d& _orientation) const;
Eigen::Vector2d fromSensor(const Eigen::Vector2d& lmk_s) const;
Eigen::Vector2d toSensor(const Eigen::Vector2d& lmk_w) const;
Eigen::Vector2d polar(const Eigen::Vector2d& rect) const;
Eigen::Vector2d rect(double range, double bearing) const;
};
inline void ProcessorRangeBearing::configure(SensorBasePtr _sensor)
......@@ -118,5 +100,3 @@ inline void ProcessorRangeBearing::configure(SensorBasePtr _sensor)
}
} /* namespace wolf */
#endif /* HELLO_WOLF_PROCESSOR_RANGE_BEARING_H_ */
follow: ProcessorBase.schema
\ No newline at end of file
follow: SensorBase.schema
noise_range_metres_std:
_mandatory: true
_type: double
_doc: Standard deviation of the noise of the range measurements (meters)
noise_bearing_degrees_std:
_mandatory: true
_type: double
_doc: Standard deviation of the noise of the bearing measurements (degrees)
states:
P:
follow: StateSensorP2d.schema
O:
follow: StateSensorO2d.schema
\ No newline at end of file
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/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.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// 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/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
/*
* SensorRangeBearing.cpp
*
* Created on: Nov 30, 2017
* Author: jsola
*/
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include "sensor_range_bearing.h"
#include "core/state_block/state_angle.h"
#include "core/state_block/state_block_derived.h"
namespace wolf
{
WOLF_PTR_TYPEDEFS(SensorRangeBearing);
SensorRangeBearing::SensorRangeBearing(const Eigen::VectorXd& _extrinsics, const Eigen::Vector2d& _noise_std) :
SensorBase("SensorRangeBearing",
std::make_shared<StatePoint2d>(_extrinsics.head<2>(), true),
std::make_shared<StateAngle>(_extrinsics(2), true),
nullptr,
_noise_std)
SensorRangeBearing::SensorRangeBearing(const YAML::Node& _params)
: SensorBase("SensorRangeBearing", {{'P', "StatePoint2d"}, {'O', "StateAngle"}}, _params),
noise_range_metres_std_(_params["noise_range_metres_std"].as<double>()),
noise_bearing_degrees_std_(_params["noise_bearing_degrees_std"].as<double>())
{
assert(_extrinsics.size() == 3 && "Bad extrinsics vector size. Must be 3 for 2d");
}
SensorRangeBearing::SensorRangeBearing(const Eigen::VectorXd& _extrinsics, const ParamsSensorRangeBearingPtr _intr) :
SensorRangeBearing(_extrinsics, Eigen::Vector2d(_intr->noise_range_metres_std, toRad(_intr->noise_bearing_degrees_std)))
SensorRangeBearing::~SensorRangeBearing()
{
//
}
SensorRangeBearing::~SensorRangeBearing()
Eigen::MatrixXd SensorRangeBearing::computeNoiseCov(const Eigen::VectorXd& _data) const
{
//
return (Eigen::Vector2d() << noise_range_metres_std_, noise_bearing_degrees_std_)
.finished()
.cwiseAbs2()
.asDiagonal();
}
} /* namespace wolf */
// Register in the SensorFactory
#include "core/sensor/factory_sensor.h"
namespace wolf
{
WOLF_REGISTER_SENSOR(SensorRangeBearing)
WOLF_REGISTER_SENSOR_AUTO(SensorRangeBearing)
} // namespace wolf
} // namespace wolf
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/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.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// 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/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
/*
* SensorRangeBearing.h
*
* Created on: Nov 30, 2017
* Author: jsola
*/
#ifndef HELLO_WOLF_SENSOR_RANGE_BEARING_H_
#define HELLO_WOLF_SENSOR_RANGE_BEARING_H_
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
#include "core/sensor/sensor_base.h"
#include "core/utils/params_server.h"
#include "yaml-cpp/yaml.h"
namespace wolf
{
WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorRangeBearing);
struct ParamsSensorRangeBearing : public ParamsSensorBase
{
double noise_range_metres_std = 0.05;
double noise_bearing_degrees_std = 0.5;
ParamsSensorRangeBearing()
{
//DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE.
}
ParamsSensorRangeBearing(std::string _unique_name, const ParamsServer& _server):
ParamsSensorBase(_unique_name, _server)
{
noise_range_metres_std = _server.getParam<double>(prefix + _unique_name + "/noise_range_metres_std");
noise_bearing_degrees_std = _server.getParam<double>(prefix + _unique_name + "/noise_bearing_degrees_std");
}
std::string print() const
{
return ParamsSensorBase::print() + "\n"
+ "noise_range_metres_std: " + std::to_string(noise_range_metres_std) + "\n"
+ "noise_bearing_degrees_std: " + std::to_string(noise_bearing_degrees_std) + "\n";
}
};
WOLF_PTR_TYPEDEFS(SensorRangeBearing)
class SensorRangeBearing : public SensorBase
{
public:
SensorRangeBearing(const Eigen::VectorXd& _extrinsics, const Eigen::Vector2d& _noise_std);
SensorRangeBearing(const Eigen::VectorXd& _extrinsics, const ParamsSensorRangeBearingPtr _intr);
WOLF_SENSOR_CREATE(SensorRangeBearing, ParamsSensorRangeBearing, 3);
~SensorRangeBearing() override;
private:
double noise_range_metres_std_ = 0.05;
double noise_bearing_degrees_std_ = 0.5;
SensorRangeBearing(const YAML::Node& _params);
public:
WOLF_SENSOR_CREATE(SensorRangeBearing);
~SensorRangeBearing() override;
Eigen::MatrixXd computeNoiseCov(const Eigen::VectorXd& _data) const override;
};
} /* namespace wolf */
#endif /* HELLO_WOLF_SENSOR_RANGE_BEARING_H_ */
config:
problem:
problem:
dimension: 2 # space is 2d
frame_structure: "PO" # keyframes have position and orientation
first_frame:
P:
value: [0,0]
prior:
mode: "factor"
factor_std: [0.31, 0.31]
O:
value: [0]
prior:
mode: "factor"
factor_std: [0.31]
tree_manager:
type: "none"
solver:
type: SolverCeres
plugin: core
max_num_iterations: 100
verbose: 0
period: 0.2
interrupt_on_problem_change: false
n_threads: 2
compute_cov: false
minimizer: levenberg_marquardt
use_nonmonotonic_steps: false # only for levenberg_marquardt and DOGLEG
parameter_tolerance: 1e-6
function_tolerance: 1e-6
gradient_tolerance: 1e-10
dimension: 2 # space is 2d
frame_structure: "PO" # keyframes have position and orientation
sensors:
prior:
mode: "factor"
$state:
P: [0,0]
O: [0]
$sigma:
P: [0.31, 0.31]
O: [0.31]
time_tolerance: 0.5
- type: "SensorOdom2d"
plugin: "core"
name: "sen odom"
follow: "sensor_odom_2d.yaml" # config parameters in this file
tree_manager:
type: "none"
plugin: "core"
- type: "SensorRangeBearing"
plugin: "core"
name: "sen rb"
noise_range_metres_std: 0.2 # parser only considers first appearence so the following file parsing will not overwrite this param
follow: sensor_range_bearing.yaml # config parameters in this file
processors:
solver:
max_num_iterations: 100
verbose: 0
period: 0.2
interrupt_on_problem_change: false
n_threads: 2
compute_cov: false
minimizer: LEVENBERG_MARQUARDT
use_nonmonotonic_steps: false # only for LEVENBERG_MARQUARDT and DOGLEG
function_tolerance: 0.000001
gradient_tolerance: 0.0000000001
sensors:
- type: "SensorOdom2d"
plugin: "core"
name: "sen odom"
extrinsic:
pose: [0,0, 0]
follow: "demos/hello_wolf/yaml/sensor_odom_2d.yaml" # config parameters in this file
- type: "SensorRangeBearing"
plugin: "core"
name: "sen rb"
extrinsic:
pose: [1,1, 0]
noise_range_metres_std: 0.2 # parser only considers first appearence so the following file parsing will not overwrite this param
follow: demos/hello_wolf/yaml/sensor_range_bearing.yaml # config parameters in this file
processors:
- type: "ProcessorOdom2d"
plugin: "core"
name: "prc odom"
sensor_name: "sen odom" # attach processor to this sensor
follow: demos/hello_wolf/yaml/processor_odom_2d.yaml # config parameters in this file
- type: "ProcessorRangeBearing"
plugin: "core"
name: "prc rb"
sensor_name: "sen rb" # attach processor to this sensor
follow: demos/hello_wolf/yaml/processor_range_bearing.yaml # config parameters in this file
- type: "ProcessorOdom2d"
plugin: "core"
name: "prc odom"
sensor_name: "sen odom" # attach processor to this sensor
follow: processor_odom_2d.yaml # config parameters in this file
- type: "ProcessorRangeBearing"
plugin: "core"
name: "prc rb"
sensor_name: "sen rb" # attach processor to this sensor
follow: processor_range_bearing.yaml # config parameters in this file
#type: "ProcessorOdom2d"
time_tolerance: 0.1
unmeasured_perturbation_std: 0.001
keyframe_vote:
voting_active: true
max_time_span: 999
dist_traveled: 0.95
angle_turned: 999
max_dist_traveled: 0.95
max_angle_turned: 999
max_buff_length: 999
cov_det: 999
max_cov_det: 999
apply_loss_function: true
state_getter: true
state_priority: 1
state_provider: true
state_provider_order: 1
#type: "ProcessorRangeBearing"
time_tolerance: 0.1
keyframe_vote:
......
#type: "SensorOdom2d" # This must match the KEY used in the FactorySensor. Otherwise it is an error.
k_disp_to_disp: 0.1 # m^2 / m
k_rot_to_rot: 0.1 # rad^2 / rad
k_disp_to_rot: 0.0 # m^2 / rad
min_disp_var: 0.0
min_rot_var: 0.0
apply_loss_function: true
states:
P:
prior:
mode: fix
value: [0,0]
dynamic: false
O:
prior:
mode: fix
value: [0]
dynamic: false
\ No newline at end of file
#type: "SensorRangeBearing"
noise_range_metres_std: 0.1
noise_bearing_degrees_std: 0.5
apply_loss_function: true
states:
P:
prior:
mode: fix
value: [1,1]
dynamic: false
O:
prior:
mode: fix
value: [0]
dynamic: false
\ No newline at end of file
type: "SensorOdom3d" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
k_disp_to_disp: 0.02 # m^2 / m
k_disp_to_rot: 0.02 # rad^2 / m
k_rot_to_rot: 0.01 # rad^2 / rad
min_disp_var: 0.01 # m^2
min_rot_var: 0.01 # rad^2
type: "SensorOdom3d" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
k_disp_to_disp: 0.000001 # m^2 / m
k_disp_to_rot: 0.000001 # rad^2 / m
k_rot_to_rot: 0.000001 # rad^2 / rad
min_disp_var: 0.00000001 # m^2
min_rot_var: 0.00000001 # rad^2
ADD_EXECUTABLE(demo_wolf_imported_graph demo_wolf_imported_graph.cpp)
TARGET_LINK_LIBRARIES(demo_wolf_imported_graph ${PLUGIN_NAME})
ADD_EXECUTABLE(demo_analytic_autodiff_comparison demo_analytic_autodiff_comparison.cpp)
TARGET_LINK_LIBRARIES(demo_analytic_autodiff_comparison ${PLUGIN_NAME})