Skip to content
Snippets Groups Projects
Commit 4b988af9 authored by Joaquim Casals Buñuel's avatar Joaquim Casals Buñuel
Browse files

Merge branch 'devel' into 289-problem-check-more-exhaustive

parents 6fee02af c8b354bd
No related branches found
No related tags found
1 merge request!340Resolve "Problem::check() more exhaustive"
Pipeline #5042 passed
Stage: build
    Hide whitespace changes
    Inline Side-by-side
    Showing
    with 160 additions and 113 deletions
    CMakeLists.txt
    + 27
    − 26
    View file @ 4b988af9
    ......@@ -216,15 +216,15 @@ SET(HDRS_STATE_BLOCK
    include/core/state_block/local_parametrization_quaternion.h
    include/core/state_block/state_angle.h
    include/core/state_block/state_block.h
    include/core/state_block/state_homogeneous_3D.h
    include/core/state_block/state_homogeneous_3d.h
    include/core/state_block/state_quaternion.h
    )
    SET(HDRS_CAPTURE
    include/core/capture/capture_base.h
    include/core/capture/capture_motion.h
    include/core/capture/capture_odom_2D.h
    include/core/capture/capture_odom_3D.h
    include/core/capture/capture_odom_2d.h
    include/core/capture/capture_odom_3d.h
    include/core/capture/capture_pose.h
    include/core/capture/capture_velocity.h
    include/core/capture/capture_void.h
    ......@@ -233,25 +233,25 @@ SET(HDRS_CAPTURE
    SET(HDRS_FACTOR
    include/core/factor/factor_analytic.h
    include/core/factor/factor_autodiff.h
    include/core/factor/factor_autodiff_distance_3D.h
    include/core/factor/factor_autodiff_distance_3d.h
    include/core/factor/factor_base.h
    include/core/factor/factor_block_absolute.h
    include/core/factor/factor_block_difference.h
    include/core/factor/factor_diff_drive.h
    include/core/factor/factor_odom_2D.h
    include/core/factor/factor_odom_2D_closeloop.h
    include/core/factor/factor_odom_2D_analytic.h
    include/core/factor/factor_odom_3D.h
    include/core/factor/factor_pose_2D.h
    include/core/factor/factor_pose_3D.h
    include/core/factor/factor_odom_2d.h
    include/core/factor/factor_odom_2d_closeloop.h
    include/core/factor/factor_odom_2d_analytic.h
    include/core/factor/factor_odom_3d.h
    include/core/factor/factor_pose_2d.h
    include/core/factor/factor_pose_3d.h
    include/core/factor/factor_quaternion_absolute.h
    include/core/factor/factor_relative_2D_analytic.h
    include/core/factor/factor_relative_2d_analytic.h
    )
    SET(HDRS_FEATURE
    include/core/feature/feature_base.h
    include/core/feature/feature_match.h
    include/core/feature/feature_motion.h
    include/core/feature/feature_odom_2D.h
    include/core/feature/feature_odom_2d.h
    include/core/feature/feature_pose.h
    )
    SET(HDRS_LANDMARK
    ......@@ -259,6 +259,7 @@ SET(HDRS_LANDMARK
    include/core/landmark/landmark_match.h
    )
    SET(HDRS_PROCESSOR
    include/core/processor/is_motion.h
    include/core/processor/motion_buffer.h
    include/core/processor/processor_base.h
    include/core/processor/processor_diff_drive.h
    ......@@ -266,8 +267,8 @@ SET(HDRS_PROCESSOR
    include/core/processor/processor_logging.h
    include/core/processor/processor_loopclosure.h
    include/core/processor/processor_motion.h
    include/core/processor/processor_odom_2D.h
    include/core/processor/processor_odom_3D.h
    include/core/processor/processor_odom_2d.h
    include/core/processor/processor_odom_3d.h
    include/core/processor/processor_tracker.h
    include/core/processor/processor_tracker_feature.h
    include/core/processor/processor_tracker_landmark.h
    ......@@ -277,8 +278,8 @@ SET(HDRS_SENSOR
    include/core/sensor/sensor_base.h
    include/core/sensor/sensor_diff_drive.h
    include/core/sensor/sensor_factory.h
    include/core/sensor/sensor_odom_2D.h
    include/core/sensor/sensor_odom_3D.h
    include/core/sensor/sensor_odom_2d.h
    include/core/sensor/sensor_odom_3d.h
    )
    SET(HDRS_SOLVER
    include/core/solver/solver_manager.h
    ......@@ -325,8 +326,8 @@ SET(SRCS_UTILS
    SET(SRCS_CAPTURE
    src/capture/capture_base.cpp
    src/capture/capture_motion.cpp
    src/capture/capture_odom_2D.cpp
    src/capture/capture_odom_3D.cpp
    src/capture/capture_odom_2d.cpp
    src/capture/capture_odom_3d.cpp
    src/capture/capture_pose.cpp
    src/capture/capture_velocity.cpp
    src/capture/capture_void.cpp
    ......@@ -339,7 +340,7 @@ SET(SRCS_FACTOR
    SET(SRCS_FEATURE
    src/feature/feature_base.cpp
    src/feature/feature_motion.cpp
    src/feature/feature_odom_2D.cpp
    src/feature/feature_odom_2d.cpp
    src/feature/feature_pose.cpp
    )
    SET(SRCS_LANDMARK
    ......@@ -351,8 +352,8 @@ SET(SRCS_PROCESSOR
    src/processor/processor_diff_drive.cpp
    src/processor/processor_loopclosure.cpp
    src/processor/processor_motion.cpp
    src/processor/processor_odom_2D.cpp
    src/processor/processor_odom_3D.cpp
    src/processor/processor_odom_2d.cpp
    src/processor/processor_odom_3d.cpp
    src/processor/processor_tracker.cpp
    src/processor/processor_tracker_feature.cpp
    src/processor/processor_tracker_landmark.cpp
    ......@@ -361,16 +362,16 @@ SET(SRCS_PROCESSOR
    SET(SRCS_SENSOR
    src/sensor/sensor_base.cpp
    src/sensor/sensor_diff_drive.cpp
    src/sensor/sensor_odom_2D.cpp
    src/sensor/sensor_odom_3D.cpp
    src/sensor/sensor_odom_2d.cpp
    src/sensor/sensor_odom_3d.cpp
    )
    SET(SRCS_SOLVER
    src/solver/solver_manager.cpp
    )
    SET(SRCS_YAML
    src/yaml/processor_odom_3D_yaml.cpp
    src/yaml/sensor_odom_2D_yaml.cpp
    src/yaml/sensor_odom_3D_yaml.cpp
    src/yaml/processor_odom_3d_yaml.cpp
    src/yaml/sensor_odom_2d_yaml.cpp
    src/yaml/sensor_odom_3d_yaml.cpp
    )
    #OPTIONALS
    #optional HDRS and SRCS
    ......
    demos/demo_analytic_autodiff_factor.cpp
    + 5
    − 5
    View file @ 4b988af9
    ......@@ -57,9 +57,9 @@ int main(int argc, char** argv)
    std::map<unsigned int, FrameBasePtr> index_2_frame_ptr_analytic;
    // Wolf problem
    ProblemPtr wolf_problem_autodiff = new Problem(FRM_PO_2D);
    ProblemPtr wolf_problem_analytic = new Problem(FRM_PO_2D);
    SensorBasePtr sensor = new SensorBase("ODOM 2D", std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)), std::make_shared<StateBlock>(Eigen::VectorXd::Zero(1)), std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)), 2);
    ProblemPtr wolf_problem_autodiff = new Problem(FRM_PO_2d);
    ProblemPtr wolf_problem_analytic = new Problem(FRM_PO_2d);
    SensorBasePtr sensor = new SensorBase("ODOM 2d", std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)), std::make_shared<StateBlock>(Eigen::VectorXd::Zero(1)), std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)), 2);
    // Ceres wrapper
    ceres::Solver::Options ceres_options;
    ......@@ -242,7 +242,7 @@ int main(int argc, char** argv)
    FrameBasePtr frame_new_ptr_autodiff = index_2_frame_ptr_autodiff[edge_new];
    frame_new_ptr_autodiff->addCapture(capture_ptr_autodiff);
    capture_ptr_autodiff->addFeature(feature_ptr_autodiff);
    FactorOdom2D* factor_ptr_autodiff = new FactorOdom2D(feature_ptr_autodiff, frame_old_ptr_autodiff);
    FactorOdom2d* factor_ptr_autodiff = new FactorOdom2d(feature_ptr_autodiff, frame_old_ptr_autodiff);
    feature_ptr_autodiff->addFactor(factor_ptr_autodiff);
    //std::cout << "Added autodiff edge! " << factor_ptr_autodiff->id() << " from vertex " << factor_ptr_autodiff->getCapture()->getFrame()->id() << " to " << factor_ptr_autodiff->getFrameOther()->id() << std::endl;
    ......@@ -255,7 +255,7 @@ int main(int argc, char** argv)
    FrameBasePtr frame_new_ptr_analytic = index_2_frame_ptr_analytic[edge_new];
    frame_new_ptr_analytic->addCapture(capture_ptr_analytic);
    capture_ptr_analytic->addFeature(feature_ptr_analytic);
    FactorOdom2DAnalytic* factor_ptr_analytic = new FactorOdom2DAnalytic(feature_ptr_analytic, frame_old_ptr_analytic);
    FactorOdom2dAnalytic* factor_ptr_analytic = new FactorOdom2dAnalytic(feature_ptr_analytic, frame_old_ptr_analytic);
    feature_ptr_analytic->addFactor(factor_ptr_analytic);
    //std::cout << "Added analytic edge! " << factor_ptr_analytic->id() << " from vertex " << factor_ptr_analytic->getCapture()->getFrame()->id() << " to " << factor_ptr_analytic->getFrameOther()->id() << std::endl;
    //std::cout << "vector " << factor_ptr_analytic->getMeasurement().transpose() << std::endl;
    ......
    demos/demo_map_yaml.cpp
    + 3
    − 3
    View file @ 4b988af9
    ......@@ -8,7 +8,7 @@
    #include "core/common/wolf.h"
    #include "core/problem/problem.h"
    #include "core/map/map_base.h"
    #include "core/landmark/landmark_polyline_2D.h"
    #include "core/landmark/landmark_polyline_2d.h"
    #include "core/landmark/landmark_AHP.h"
    #include "core/state_block/state_block.h"
    #include "core/yaml/yaml_conversion.h"
    ......@@ -22,9 +22,9 @@ void print(MapBase& _map)
    {
    std::cout << "Lmk ID: " << lmk_ptr->id();
    std::cout << "\nLmk type: " << lmk_ptr->getType();
    if (lmk_ptr->getType() == "POLYLINE 2D")
    if (lmk_ptr->getType() == "POLYLINE 2d")
    {
    LandmarkPolyline2DPtr poly_ptr = std::static_pointer_cast<LandmarkPolyline2D>(lmk_ptr);
    LandmarkPolyline2dPtr poly_ptr = std::static_pointer_cast<LandmarkPolyline2d>(lmk_ptr);
    std::cout << "\npos: " << poly_ptr->getP()->getState().transpose() << " -- fixed: " << poly_ptr->getP()->isFixed();
    std::cout << "\nori: " << poly_ptr->getO()->getState().transpose() << " -- fixed: " << poly_ptr->getO()->isFixed();
    std::cout << "\nn points: " << poly_ptr->getNPoints();
    ......
    demos/demo_wolf_imported_graph.cpp
    + 5
    − 5
    View file @ 4b988af9
    ......@@ -61,9 +61,9 @@ int main(int argc, char** argv)
    std::map<FrameBasePtr, unsigned int> frame_ptr_2_index_prun;
    // Wolf problem
    ProblemPtr wolf_problem_full = new Problem(FRM_PO_2D);
    ProblemPtr wolf_problem_prun = new Problem(FRM_PO_2D);
    SensorBasePtr sensor = new SensorBase("ODOM 2D", std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)), std::make_shared<StateBlock>(Eigen::VectorXd::Zero(1)), std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)), 2);
    ProblemPtr wolf_problem_full = new Problem(FRM_PO_2d);
    ProblemPtr wolf_problem_prun = new Problem(FRM_PO_2d);
    SensorBasePtr sensor = new SensorBase("ODOM 2d", std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)), std::make_shared<StateBlock>(Eigen::VectorXd::Zero(1)), std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)), 2);
    Eigen::SparseMatrix<double> Lambda(0,0);
    ......@@ -259,8 +259,8 @@ int main(int argc, char** argv)
    frame_new_ptr_prun->addCapture(capture_ptr_prun);
    capture_ptr_full->addFeature(feature_ptr_full);
    capture_ptr_prun->addFeature(feature_ptr_prun);
    FactorOdom2D* factor_ptr_full = new FactorOdom2D(feature_ptr_full, frame_old_ptr_full);
    FactorOdom2D* factor_ptr_prun = new FactorOdom2D(feature_ptr_prun, frame_old_ptr_prun);
    FactorOdom2d* factor_ptr_full = new FactorOdom2d(feature_ptr_full, frame_old_ptr_full);
    FactorOdom2d* factor_ptr_prun = new FactorOdom2d(feature_ptr_prun, frame_old_ptr_prun);
    feature_ptr_full->addFactor(factor_ptr_full);
    feature_ptr_prun->addFactor(factor_ptr_prun);
    //std::cout << "Added edge! " << factor_ptr_prun->id() << " from vertex " << factor_ptr_prun->getCapture()->getFrame()->id() << " to " << factor_ptr_prun->getFrameTo()->id() << std::endl;
    ......
    demos/processor_odom_3D.yaml → demos/processor_odom_3d.yaml
    + 1
    − 1
    View file @ 4b988af9
    type: "ODOM 3D" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
    type: "ProcessorOdom3d" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
    time_tolerance: 0.01 # seconds
    unmeasured_perturbation_std: 0.001
    ......
    test/yaml/sensor_odom_3D.yaml → demos/sensor_odom_3d.yaml
    + 1
    − 1
    View file @ 4b988af9
    type: "SensorOdom3D" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
    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
    ......
    demos/sensor_odom_3D_HQ.yaml → demos/sensor_odom_3d_HQ.yaml
    + 2
    − 2
    View file @ 4b988af9
    type: "ODOM 3D" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
    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
    \ No newline at end of file
    min_rot_var: 0.00000001 # rad^2
    doc/interpolate.rtf
    + 7
    − 7
    View file @ 4b988af9
    ......@@ -179,12 +179,12 @@
    \cf2 *\cf0 \
    \cf2 * For simple pose increments, we can use a local implementation:\cf0 \
    \cf2 *\cf0 \
    \cf2 * - for 2D\cf0 \
    \cf2 * - for 2d\cf0 \
    \cf2 *\cf0 \
    \cf2 * dp_S = dR_I.tr * (1-tau)*dp_F // dR is the rotation matrix of the angle delta '\ul da\ulnone '; '\ul tr\ulnone ' is transposed\cf0 \
    \cf2 * da_S = dR_I.tr * (1-tau)*da_F\cf0 \
    \cf2 *\cf0 \
    \cf2 * - for 3D\cf0 \
    \cf2 * - for 3d\cf0 \
    \cf2 *\cf0 \
    \cf2 * dp_S = dq_I.conj * (1-tau)*dp_F // \ul dq\ulnone is a \ul quaternion\ulnone ; '\ul conj\ulnone ' is the \ul conjugate\ulnone \ul quaternion\ulnone .\cf0 \
    \cf2 * dq_S = dq_I.conj * dq_F\cf0 \
    ......@@ -206,14 +206,14 @@
    \cf2 *\cf0 \
    \cf2 * ### Examples\cf0 \
    \cf2 *\cf0 \
    \cf2 * #### Example 1: For 2D poses\cf0 \
    \cf2 * #### Example 1: For 2d poses\cf0 \
    \cf2 *\cf0 \
    \cf2 *\cf0 \
    \cf2 * t_I = _ts // time stamp of the interpolated motion\cf0 \
    \cf2 * tau = (t_I - t_R) / (t_F - t_R) // interpolation factor\cf0 \
    \cf2 *\cf0 \
    \cf2 * dp_I = tau*dp_F // \ul dp\ulnone is a 2-vector\cf0 \
    \cf2 * da_I = tau*da_F // \ul da\ulnone is an angle, for 2D poses\cf0 \
    \cf2 * da_I = tau*da_F // \ul da\ulnone is an angle, for 2d poses\cf0 \
    \cf2 *\cf0 \
    \cf2 * D_I = deltaPlusDelta(D_R, d_I)\cf0 \
    \cf2 *\cf0 \
    ......@@ -222,7 +222,7 @@
    \cf2 *\cf0 \
    \cf2 * D_S = D_F\cf0 \
    \cf2 *\cf0 \
    \cf2 * #### Example 2: For 3D poses\cf0 \
    \cf2 * #### Example 2: For 3d poses\cf0 \
    \cf2 *\cf0 \
    \cf2 * Orientation is in \ul quaternion\ulnone form, which is the best for interpolation using `\ul slerp\ulnone ()` :\cf0 \
    \cf2 *\cf0 \
    ......@@ -230,7 +230,7 @@
    \cf2 * tau = (t_I - t_R) / (t_F - t_R) // interpolation factor\cf0 \
    \cf2 *\cf0 \
    \cf2 * dp_I = tau*dp_F // \ul dp\ulnone is a 3-vector\cf0 \
    \cf2 * dq_I = 1.\ul slerp\ulnone (tau, dq_F) // '1' is the identity \ul quaternion\ulnone ; \ul slerp\ulnone () interpolates 3D rotation.\cf0 \
    \cf2 * dq_I = 1.\ul slerp\ulnone (tau, dq_F) // '1' is the identity \ul quaternion\ulnone ; \ul slerp\ulnone () interpolates 3d rotation.\cf0 \
    \cf2 *\cf0 \
    \cf2 * D_I = deltaPlusDelta(D_R, d_I)\cf0 \
    \cf2 *\cf0 \
    ......@@ -260,4 +260,4 @@
    \cf2 * DC_S = DC_F\cf0 \
    \cf2 *\cf0 \
    \cf2 */\cf0 \
    }
    \ No newline at end of file
    }
    hello_wolf/CMakeLists.txt
    + 3
    − 3
    View file @ 4b988af9
    ......@@ -7,7 +7,7 @@ SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
    ${CMAKE_CURRENT_SOURCE_DIR}/factor_bearing.h
    ${CMAKE_CURRENT_SOURCE_DIR}/factor_range_bearing.h
    ${CMAKE_CURRENT_SOURCE_DIR}/feature_range_bearing.h
    ${CMAKE_CURRENT_SOURCE_DIR}/landmark_point_2D.h
    ${CMAKE_CURRENT_SOURCE_DIR}/landmark_point_2d.h
    ${CMAKE_CURRENT_SOURCE_DIR}/processor_range_bearing.h
    ${CMAKE_CURRENT_SOURCE_DIR}/sensor_range_bearing.h
    )
    ......@@ -15,7 +15,7 @@ SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
    SET(SRCS_HELLOWOLF
    ${CMAKE_CURRENT_SOURCE_DIR}/capture_range_bearing.cpp
    ${CMAKE_CURRENT_SOURCE_DIR}/feature_range_bearing.cpp
    ${CMAKE_CURRENT_SOURCE_DIR}/landmark_point_2D.cpp
    ${CMAKE_CURRENT_SOURCE_DIR}/landmark_point_2d.cpp
    ${CMAKE_CURRENT_SOURCE_DIR}/processor_range_bearing.cpp
    ${CMAKE_CURRENT_SOURCE_DIR}/sensor_range_bearing.cpp
    )
    ......@@ -31,7 +31,7 @@ ADD_EXECUTABLE(hello_wolf_autoconf hello_wolf_autoconf.cpp)
    #TARGET_LINK_LIBRARIES(hello_wolf_autoconf ${PROJECT_NAME})
    TARGET_LINK_LIBRARIES(hello_wolf_autoconf ${PROJECT_NAME} hellowolf)
    #add_library(sensor_odom SHARED ../src/sensor/sensor_odom_2D.cpp ../src/processor/processor_odom_2D.cpp)
    #add_library(sensor_odom SHARED ../src/sensor/sensor_odom_2d.cpp ../src/processor/processor_odom_2d.cpp)
    #TARGET_LINK_LIBRARIES(sensor_odom ${PROJECT_NAME} hellowolf)
    #
    #add_library(range_bearing SHARED sensor_range_bearing.cpp processor_range_bearing.cpp)
    ......
    hello_wolf/capture_range_bearing.cpp
    + 1
    − 1
    View file @ 4b988af9
    /*
    * CaptureRangeBearing2D.cpp
    * CaptureRangeBearing2d.cpp
    *
    * Created on: Nov 30, 2017
    * Author: jsola
    ......
    hello_wolf/feature_range_bearing.cpp
    + 1
    − 1
    View file @ 4b988af9
    /*
    * FeatureRangeBearing2D.cpp
    * FeatureRangeBearing2d.cpp
    *
    * Created on: Nov 30, 2017
    * Author: jsola
    ......
    hello_wolf/feature_range_bearing.h
    + 1
    − 1
    View file @ 4b988af9
    /*
    * FeatureRangeBearing2D.h
    * FeatureRangeBearing2d.h
    *
    * Created on: Nov 30, 2017
    * Author: jsola
    ......
    hello_wolf/hello_wolf.cpp
    + 24
    − 24
    View file @ 4b988af9
    ......@@ -15,9 +15,9 @@
    // wolf core includes
    #include "core/common/wolf.h"
    #include "core/sensor/sensor_odom_2D.h"
    #include "core/processor/processor_odom_2D.h"
    #include "core/capture/capture_odom_2D.h"
    #include "core/sensor/sensor_odom_2d.h"
    #include "core/processor/processor_odom_2d.h"
    #include "core/capture/capture_odom_2d.h"
    #include "core/ceres_wrapper/ceres_manager.h"
    // hello wolf local includes
    ......@@ -110,12 +110,12 @@ int main()
    options.max_num_iterations = 1000; // We depart far from solution, need a lot of iterations
    CeresManagerPtr ceres = std::make_shared<CeresManager>(problem, options);
    // sensor odometer 2D
    ParamsSensorOdom2DPtr intrinsics_odo = std::make_shared<ParamsSensorOdom2D>();
    SensorBasePtr sensor_odo = problem->installSensor("SensorOdom2D", "sensor odo", Vector3d(0,0,0), intrinsics_odo);
    // 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
    ProcessorParamsOdom2DPtr params_odo = std::make_shared<ProcessorParamsOdom2D>();
    // processor odometer 2d
    ProcessorParamsOdom2dPtr params_odo = std::make_shared<ProcessorParamsOdom2d>();
    params_odo->voting_active = true;
    params_odo->time_tolerance = 0.1;
    params_odo->max_time_span = 999;
    ......@@ -123,7 +123,7 @@ int main()
    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);
    ProcessorBasePtr processor = problem->installProcessor("ProcessorOdom2d", "processor odo", sensor_odo, params_odo);
    // sensor Range and Bearing
    ParamsSensorRangeBearingPtr intrinsics_rb = std::make_shared<ParamsSensorRangeBearing>();
    ......@@ -183,7 +183,7 @@ int main()
    t += 1.0; // t : 1.0
    // motion
    CaptureOdom2DPtr cap_motion = std::make_shared<CaptureOdom2D>(t, sensor_odo, motion_data, motion_cov);
    CaptureOdom2dPtr cap_motion = std::make_shared<CaptureOdom2d>(t, sensor_odo, motion_data, motion_cov);
    sensor_odo ->process(cap_motion); // KF2 : (1,0,0)
    // observe lmks
    ......@@ -267,7 +267,7 @@ int main()
    *
    * IF YOU SEE at the end of the printed problem the estimate for Lmk 3 as:
    *
    * L3 POINT 2D <-- c8
    * L3 POINT 2d <-- c8
    * Est, x = ( 3 2 )
    * sb: Est
    *
    ......@@ -289,10 +289,10 @@ int main()
    *
    * P: wolf tree status ---------------------
    Hardware
    S1 ODOM 2D // Sensor 1, type ODOMETRY 2D.
    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
    pm1 ODOM 2D // Processor 1, type ODOMETRY 2D
    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.
    ......@@ -307,7 +307,7 @@ int main()
    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)
    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
    ......@@ -315,10 +315,10 @@ int main()
    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 <--
    CM3 ODOM 2d -> S1 [Sta, Sta] <--
    f3 ODOM 2d <--
    m = ( 1 0 0 )
    c3 ODOM 2D --> F1 // Factor 3, type ODOM, against Frame 1
    c3 ODOM 2d --> F1 // Factor 3, type ODOM, against Frame 1
    C9 RANGE BEARING -> S2 [Sta, Sta] <--
    f4 RANGE BEARING <--
    m = ( 1.41 2.36 )
    ......@@ -329,10 +329,10 @@ int main()
    KF3 <--
    Est, ts=2, x = ( 2 4.1e-10 1.7e-10 )
    sb: Est Est
    CM7 ODOM 2D -> S1 [Sta, Sta] <--
    f6 ODOM 2D <--
    CM7 ODOM 2d -> S1 [Sta, Sta] <--
    f6 ODOM 2d <--
    m = ( 1 0 0 )
    c6 ODOM 2D --> F2
    c6 ODOM 2d --> F2
    C12 RANGE BEARING -> S2 [Sta, Sta] <--
    f7 RANGE BEARING <--
    m = ( 1.41 2.36 )
    ......@@ -343,15 +343,15 @@ int main()
    F4 <--
    Est, ts=2, x = ( 0.11 -0.045 0.26 )
    sb: Est Est
    CM10 ODOM 2D -> S1 [Sta, Sta] <--
    CM10 ODOM 2d -> S1 [Sta, Sta] <--
    Map
    L1 POINT 2D <-- c2 c4 // Landmark 1, constrained by Factors 2 and 4
    L1 POINT 2d <-- c2 c4 // Landmark 1, constrained by Factors 2 and 4
    Est, x = ( 1 2 ) // L4 state is estimated, state vector
    sb: Est // L4 has 1 state block estimated
    L2 POINT 2D <-- c5 c7
    L2 POINT 2d <-- c5 c7
    Est, x = ( 2 2 )
    sb: Est
    L3 POINT 2D <-- c8
    L3 POINT 2d <-- c8
    Est, x = ( 3 2 )
    sb: Est
    -----------------------------------------
    ......
    hello_wolf/hello_wolf_autoconf.cpp
    + 16
    − 16
    View file @ 4b988af9
    ......@@ -8,7 +8,7 @@
    // wolf core includes
    #include "core/common/wolf.h"
    #include "core/capture/capture_odom_2D.h"
    #include "core/capture/capture_odom_2d.h"
    #include "core/yaml/parser_yaml.hpp"
    #include "core/ceres_wrapper/ceres_manager.h"
    ......@@ -163,7 +163,7 @@ int main()
    t += 1.0; // t : 1.0
    // motion
    CaptureOdom2DPtr cap_motion = std::make_shared<CaptureOdom2D>(t, sensor_odo, motion_data, motion_cov);
    CaptureOdom2dPtr cap_motion = std::make_shared<CaptureOdom2d>(t, sensor_odo, motion_data, motion_cov);
    cap_motion ->process(); // KF2 : (1,0,0)
    // observe lmks
    ......@@ -250,7 +250,7 @@ int main()
    *
    * IF YOU SEE at the end of the printed problem the estimate for Lmk 3 as:
    *
    * L3 POINT 2D <-- c8
    * L3 POINT 2d <-- c8
    * Est, x = ( 3 2 )
    * sb: Est
    *
    ......@@ -272,10 +272,10 @@ int main()
    *
    * P: wolf tree status ---------------------
    Hardware
    S1 ODOM 2D // Sensor 1, type ODOMETRY 2D.
    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
    pm1 ODOM 2D // Processor 1, type ODOMETRY 2D
    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.
    ......@@ -290,7 +290,7 @@ int main()
    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)
    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
    ......@@ -298,10 +298,10 @@ int main()
    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 <--
    CM3 ODOM 2d -> S1 [Sta, Sta] <--
    f3 ODOM 2d <--
    m = ( 1 0 0 )
    c3 ODOM 2D --> F1 // Factor 3, type ODOM, against Frame 1
    c3 ODOM 2d --> F1 // Factor 3, type ODOM, against Frame 1
    C9 RANGE BEARING -> S2 [Sta, Sta] <--
    f4 RANGE BEARING <--
    m = ( 1.41 2.36 )
    ......@@ -312,10 +312,10 @@ int main()
    KF3 <--
    Est, ts=2, x = ( 2 4.1e-10 1.7e-10 )
    sb: Est Est
    CM7 ODOM 2D -> S1 [Sta, Sta] <--
    f6 ODOM 2D <--
    CM7 ODOM 2d -> S1 [Sta, Sta] <--
    f6 ODOM 2d <--
    m = ( 1 0 0 )
    c6 ODOM 2D --> F2
    c6 ODOM 2d --> F2
    C12 RANGE BEARING -> S2 [Sta, Sta] <--
    f7 RANGE BEARING <--
    m = ( 1.41 2.36 )
    ......@@ -326,15 +326,15 @@ int main()
    F4 <--
    Est, ts=2, x = ( 0.11 -0.045 0.26 )
    sb: Est Est
    CM10 ODOM 2D -> S1 [Sta, Sta] <--
    CM10 ODOM 2d -> S1 [Sta, Sta] <--
    Map
    L1 POINT 2D <-- c2 c4 // Landmark 1, constrained by Factors 2 and 4
    L1 POINT 2d <-- c2 c4 // Landmark 1, constrained by Factors 2 and 4
    Est, x = ( 1 2 ) // L4 state is estimated, state vector
    sb: Est // L4 has 1 state block estimated
    L2 POINT 2D <-- c5 c7
    L2 POINT 2d <-- c5 c7
    Est, x = ( 2 2 )
    sb: Est
    L3 POINT 2D <-- c8
    L3 POINT 2d <-- c8
    Est, x = ( 3 2 )
    sb: Est
    -----------------------------------------
    ......
    hello_wolf/landmark_point_2D.cpp → hello_wolf/landmark_point_2d.cpp
    + 24
    − 0
    View file @ 4b988af9
    /**
    * \file landmark_point_2D.cpp
    * \file landmark_point_2d.cpp
    *
    * Created on: Dec 4, 2017
    * \author: jsola
    */
    #include "landmark_point_2D.h"
    #include "landmark_point_2d.h"
    namespace wolf
    {
    LandmarkPoint2D::LandmarkPoint2D(int _id, const Eigen::Vector2d& _xy) :
    LandmarkBase("POINT 2D", std::make_shared<StateBlock>(_xy))
    LandmarkPoint2d::LandmarkPoint2d(int _id, const Eigen::Vector2d& _xy) :
    LandmarkBase("POINT 2d", std::make_shared<StateBlock>(_xy))
    {
    setId(_id);
    }
    LandmarkPoint2D::~LandmarkPoint2D()
    LandmarkPoint2d::~LandmarkPoint2d()
    {
    //
    }
    ......
    hello_wolf/landmark_point_2D.h → hello_wolf/landmark_point_2d.h
    + 27
    − 0
    View file @ 4b988af9
    /**
    * \file landmark_point_2D.h
    * \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_
    #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);
    WOLF_PTR_TYPEDEFS(LandmarkPoint2d);
    class LandmarkPoint2D : public LandmarkBase
    class LandmarkPoint2d : public LandmarkBase
    {
    public:
    LandmarkPoint2D(int _id, const Eigen::Vector2d& _xy);
    virtual ~LandmarkPoint2D();
    LandmarkPoint2d(int _id, const Eigen::Vector2d& _xy);
    virtual ~LandmarkPoint2d();
    };
    } /* namespace wolf */
    #endif /* HELLO_WOLF_LANDMARK_POINT_2D_H_ */
    #endif /* HELLO_WOLF_LANDMARK_POINT_2d_H_ */
    hello_wolf/processor_range_bearing.cpp
    + 5
    − 5
    View file @ 4b988af9
    ......@@ -7,7 +7,7 @@
    #include "processor_range_bearing.h"
    #include "capture_range_bearing.h"
    #include "landmark_point_2D.h"
    #include "landmark_point_2d.h"
    #include "feature_range_bearing.h"
    #include "factor_range_bearing.h"
    ......@@ -15,7 +15,7 @@ namespace wolf
    {
    ProcessorRangeBearing::ProcessorRangeBearing(ProcessorParamsBasePtr _params) :
    ProcessorBase("ProcessorRangeBearing", _params)
    ProcessorBase("ProcessorRangeBearing", 2, _params)
    {
    //
    }
    ......@@ -63,19 +63,19 @@ void ProcessorRangeBearing::processCapture(CaptureBasePtr _capture)
    double bearing = capture_rb->getBearing(i);
    // 4. create or recover landmark
    LandmarkPoint2DPtr lmk;
    LandmarkPoint2dPtr 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<LandmarkPoint2d>(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<LandmarkPoint2d>(getProblem()->getMap(), id, invObserve(range, bearing));
    WOLF_TRACE("new lmk(", id, "): ", lmk->getP()->getState().transpose());
    // - add to known landmarks
    known_lmks.emplace(id, lmk);
    ......
    hello_wolf/sensor_range_bearing.cpp
    + 1
    − 1
    View file @ 4b988af9
    ......@@ -20,7 +20,7 @@ SensorRangeBearing::SensorRangeBearing(const Eigen::VectorXd& _extrinsics, const
    nullptr,
    _noise_std)
    {
    assert(_extrinsics.size() == 3 && "Bad extrinsics vector size. Must be 3 for 2D");
    assert(_extrinsics.size() == 3 && "Bad extrinsics vector size. Must be 3 for 2d");
    }
    SensorRangeBearing::SensorRangeBearing(const Eigen::VectorXd& _extrinsics, const ParamsSensorRangeBearingPtr _intr) :
    ......
    hello_wolf/yaml/hello_wolf_config.yaml
    + 5
    − 10
    View file @ 4b988af9
    ......@@ -3,7 +3,7 @@ config:
    problem:
    frame_structure: "PO" # keyframes have position and orientation
    dimension: 2 # space is 2D
    dimension: 2 # space is 2d
    prior:
    timestamp: 0.0
    state: [0,0,0]
    ......@@ -12,12 +12,12 @@ config:
    sensors:
    - type: "SensorOdom2D"
    - type: "SensorOdom2d"
    plugin: "core"
    name: "sen odom"
    extrinsic:
    pose: [0,0, 0]
    follow: "hello_wolf/yaml/sensor_odom_2D.yaml" # config parameters in this file
    follow: "hello_wolf/yaml/sensor_odom_2d.yaml" # config parameters in this file
    - type: "SensorRangeBearing"
    plugin: "core"
    ......@@ -29,19 +29,14 @@ config:
    processors:
    - type: "ProcessorOdom2D"
    - type: "ProcessorOdom2d"
    plugin: "core"
    name: "prc odom"
    sensor_name: "sen odom" # attach processor to this sensor
    follow: hello_wolf/yaml/processor_odom_2D.yaml # config parameters in this file
    follow: 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: hello_wolf/yaml/processor_range_bearing.yaml # config parameters in this file
    \ No newline at end of file
    hello_wolf/yaml/processor_odom_2D.yaml → hello_wolf/yaml/processor_odom_2d.yaml
    + 1
    − 1
    View file @ 4b988af9
    type: "ProcessorOdom2D"
    type: "ProcessorOdom2d"
    time_tolerance: 0.1
    unmeasured_perturbation_std: 0.001
    ......
    0% or .
    You are about to add 0 people to the discussion. Proceed with caution.
    Finish editing this message first!
    Please register or to comment