Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

3D fuse models #354

Open
wants to merge 123 commits into
base: rolling
Choose a base branch
from
Open
Changes from 1 commit
Commits
Show all changes
123 commits
Select commit Hold shift + click to select a range
6878097
humble branch
andreaostuni Jul 21, 2023
edb7d60
make composable optimizer nodes (dev)
giafranchini Aug 4, 2023
504a1c6
batch and fixed lag optimizer as components
giafranchini Aug 7, 2023
699b9d1
optimizer components refactor
giafranchini Aug 23, 2023
ce960ad
undo optimizer.cpp changes
giafranchini Aug 23, 2023
dce95d2
set precision 1e-4 to validatePartialMeasurement
giafranchini Sep 25, 2023
f33bc5d
new branch iron_unicycle3d
giafranchini Oct 5, 2023
ddd06c2
odometry 3d development
giafranchini Nov 3, 2023
9927ad0
add normal_prior_pose_3d
giafranchini Nov 3, 2023
5a3df7b
odometry 3d with partial measurements development
giafranchini Nov 6, 2023
c065595
clean up: removed euler constraints
giafranchini Nov 8, 2023
38cae76
3D pose absolute and relative constraints with partial measure support
giafranchini Nov 8, 2023
5f037ce
clean up: removed euler constraints plugin from cmake
giafranchini Nov 8, 2023
d846fff
Add support partial measurements for odometry 3D
giafranchini Nov 8, 2023
052be96
imu 3D plugin dev
giafranchini Nov 8, 2023
ac1a0f8
fix missing type() template specialization for vel 3d
giafranchini Nov 14, 2023
c516176
add support for partial measurements in absolute_orientation_3d_stamp…
giafranchini Nov 15, 2023
16f4c53
fix: missing tf_timeout param
giafranchini Nov 15, 2023
5d4dd92
removed euler constraint headers
giafranchini Nov 15, 2023
0c63c45
fix: wrong array mapping in unicycle 3d motion model + tests
giafranchini Nov 15, 2023
3fb62bd
Update README.md
giafranchini Nov 20, 2023
10c0065
Update typo in fuse_constraints/fuse_plugins.xml
giafranchini Nov 20, 2023
df856ca
Update typo in fuse_constraints/fuse_plugins.xml
giafranchini Nov 20, 2023
92a3a69
Update typo in fuse_constraints/fuse_plugins.xml
giafranchini Nov 20, 2023
3fac148
Update typo in fuse_constraints/fuse_plugins.xml
giafranchini Nov 20, 2023
211e0eb
Update fuse_constraints/fuse_plugins.xml
giafranchini Nov 20, 2023
89656fb
Update copyright fuse_constraints/include/fuse_constraints/normal_pri…
giafranchini Nov 20, 2023
486336b
Fix: removed double new line
giafranchini Nov 20, 2023
a09db78
Update copyright in fuse_constraints/src/normal_prior_pose_3d.cpp
giafranchini Nov 20, 2023
f6e78fb
Cleaning is_angular_3d method in sensor config
giafranchini Nov 20, 2023
c6a0daf
Revert "Cleaning is_angular_3d method in sensor config"
giafranchini Nov 21, 2023
c1b1ede
Fix: delta orientation cost function residuals static map
giafranchini Nov 28, 2023
ae84092
Add independent param for pose delta
giafranchini Nov 28, 2023
1072fe4
Add test absolute pose 3d stamped constraint
giafranchini Nov 28, 2023
55280dc
unicycle 3d predict refactor + test
giafranchini Nov 28, 2023
d3e3c36
unicycle 3d cost functor refactor + test
giafranchini Nov 28, 2023
c44aa4e
different handling for orient/vel variables in sensor config
giafranchini Nov 28, 2023
569d8ff
remove TODOs
giafranchini Nov 28, 2023
bd30458
abs and diff pose handling
giafranchini Nov 28, 2023
04a984d
unicycle 3d tests + minor fixes
giafranchini Dec 5, 2023
e85eaa2
feature: quat2eul + jacobian
giafranchini Dec 14, 2023
ba5217d
feature: unicycle 3d analytical derivatives
giafranchini Dec 14, 2023
4f7dff8
refactor: jacobian computation in predict function with eigen
giafranchini Dec 14, 2023
3959d76
feature: odometry_3d_publisher predict to current time
giafranchini Dec 14, 2023
49d12a9
feature: quat2rpy and quat product jacobian
giafranchini Dec 20, 2023
5aa23b0
feature: add normal_prior_pose_3d
giafranchini Dec 20, 2023
0ae0f4c
process only orientation for imu3d
giafranchini Dec 20, 2023
7a8cde8
feature: add normal_prior_orientation_3d
giafranchini Dec 20, 2023
00ecb7e
fix: normal prior pose jacobian maps
giafranchini Dec 20, 2023
e9f1aa4
dev: sensor_proc refactor
giafranchini Dec 28, 2023
017242a
fix: typo in quat2rpy + tests
giafranchini Jan 2, 2024
436652d
chore: cost_function_gtest support for quaternions
giafranchini Jan 2, 2024
336b974
Merge branch 'iron_unicycle3d' into refactor/sensor_proc
giafranchini Jan 3, 2024
186a6e1
Merge pull request #13 from giafranchini/refactor/sensor_proc
giafranchini Jan 3, 2024
7f9bb01
refactor: relative_pose_3d fixed size
giafranchini Jan 4, 2024
60a8363
feature: relative_pose_3d_euler constraint
giafranchini Jan 4, 2024
b2058a8
refactor: sensor_proc + models relative pose update
giafranchini Jan 4, 2024
7191974
Fix: wrong fixed size sqrt_info in relatie euler cost functor + test
giafranchini Jan 5, 2024
3d78b1a
sensor_proc_3d tests
giafranchini Jan 8, 2024
d710b35
fix: wrong expected value in test absolutePose3DStampedEulerConstraint
giafranchini Jan 10, 2024
22190e8
Fix: imu3D getPositiveParam account for params namespace
giafranchini Jan 10, 2024
ff3ecff
fix tf2 conversion
giafranchini Jan 10, 2024
5a33b9f
Removed completed TODOs
giafranchini Jan 10, 2024
e7c74ce
Merge pull request #15 from giafranchini/refactor/relative_pose_proce…
giafranchini Jan 10, 2024
a7abf32
removed covariance_geometry dep from fuse_constraints
giafranchini Jan 11, 2024
3a8a14c
add missing absolute constraints and cleanup
giafranchini Jan 11, 2024
9014320
absolute orientation 3d cleanup
giafranchini Jan 11, 2024
c50296a
absolute pose 3d & 3d euler cleanup
giafranchini Jan 11, 2024
727afd1
delta pose/orientation cost functor cleanup
giafranchini Jan 11, 2024
656d306
Update normal_delta_pose_3d_cost_functor.hpp
giafranchini Jan 11, 2024
339952f
normal prior orientation 3d cleanup
giafranchini Jan 11, 2024
34ecebb
normal prior orientation 3d cost functor cleanup
giafranchini Jan 11, 2024
99c1055
normal prior pose 3d cost functor cleanup
giafranchini Jan 11, 2024
ac54c93
normal prior pose 3d euler cost functor/function cleanup
giafranchini Jan 11, 2024
0e930cd
relative pose 3d constraint cleanup
giafranchini Jan 11, 2024
7073ae7
normal prior pose 3d cost function cleanup
giafranchini Jan 11, 2024
4b554da
fuse_constraints tests cleanup
giafranchini Jan 11, 2024
666d6bc
removed fuse_core::Quaternion alias
giafranchini Jan 11, 2024
73d9178
sensor_proc cleanup
giafranchini Jan 11, 2024
23d5773
unicycle2D cleanup
giafranchini Jan 11, 2024
1353917
unicycle2d state kinematic constraint cleanup
giafranchini Jan 11, 2024
e66dadb
3d models cleanup
giafranchini Jan 11, 2024
b104d77
Update optimizer.hpp
giafranchini Jan 11, 2024
7a2a338
Update test_sensor_proc.cpp
giafranchini Jan 11, 2024
4366749
removed utils.py
giafranchini Jan 11, 2024
34a3ab3
fuse optimizers cleanup
giafranchini Jan 11, 2024
235bd94
range_sensor_tutorial launch file cleanup
giafranchini Jan 11, 2024
238b7d4
Update range_sensor_tutorial.launch.py
giafranchini Jan 11, 2024
0515c90
orientation_3d variable cleanup
giafranchini Jan 11, 2024
72ff6a9
rename unicycle3d to omnidirectional3d
giafranchini Jan 11, 2024
40f7645
Merge pull request #16 from giafranchini/iron-omni3d
giafranchini Jan 11, 2024
ae23ab8
Merge pull request #17 from giafranchini/iron_models3d
giafranchini Jan 16, 2024
4ee3981
update fuse.repos
giafranchini Jan 16, 2024
ca7e435
feature: covariance variation check in processDifferentialPose3DWithC…
giafranchini Jan 29, 2024
3b36fe4
add some comments in sensor_proc
giafranchini Feb 1, 2024
228dbd6
Merge pull request #19 from giafranchini/feature-covariance-check
giafranchini Feb 1, 2024
0b44b7a
revert composable optimizer
giafranchini Mar 11, 2024
88204cd
Update fuse_core/include/fuse_core/util.hpp
giafranchini Mar 11, 2024
0878114
Update fuse_core/include/fuse_core/util.hpp
giafranchini Mar 11, 2024
ee059b6
Tests for quat2rpy - quat product - quat2angleaxis jacobians
giafranchini Mar 11, 2024
69344a1
Remove comments in absolute_pose_3d_stamped_constraint.cpp
giafranchini Mar 11, 2024
8a5b97c
Add TODOs normal_prior_pose_3d.cpp
giafranchini Mar 11, 2024
a8c4d7b
Add TODO to normal_prior_orientation_3d.cpp
giafranchini Mar 11, 2024
cba0a03
Add single jacobian check in normal_prior_orientation_3d
giafranchini Mar 11, 2024
90ded4b
Formalize comparisons in quaternion2rpy
giafranchini Mar 11, 2024
0376050
Update fuse_models/include/fuse_models/omnidirectional_3d.hpp
giafranchini Sep 25, 2024
05bc6a3
Update fuse_models/include/fuse_models/omnidirectional_3d.hpp
giafranchini Sep 25, 2024
e729204
Update fuse_models/include/fuse_models/omnidirectional_3d.hpp
giafranchini Sep 25, 2024
80aec70
Update fuse_models/include/fuse_models/omnidirectional_3d.hpp
giafranchini Sep 25, 2024
d26d91f
fix typos
giafranchini Sep 25, 2024
4689930
fix typo: quat instead of rpy
giafranchini Sep 25, 2024
05a35c2
Fix wrong stride in linear acceleration covariance eigen map
giafranchini Sep 26, 2024
0e64676
fix omnidirectional_3d_ignition orientation init + tests
giafranchini Oct 3, 2024
7475196
add 3D tutorial (#27)
henrygerardmoore Oct 3, 2024
ec62d42
Fix wrong quaternion2angleaxis jacobian + test
giafranchini Oct 3, 2024
491fda4
remove summary print
giafranchini Oct 17, 2024
e27f6f8
Port support for Ceres 2.1.0 Manifold class into ROS 2 Rolling (#366)
svwilliams Apr 20, 2024
b21c169
uncrustify
giafranchini Oct 22, 2024
43c5833
test quaternionToAngleAxis for very small rotations
giafranchini Oct 22, 2024
1dbd922
Merge branch 'rolling' into models_3d
giafranchini Oct 22, 2024
1383744
fix typo omnidirectional_3d
giafranchini Oct 29, 2024
568decf
Missing support for Ceres 2.1.0 Manifold in pose_3d constraint tests
giafranchini Nov 2, 2024
720eaf0
add optional tolerance in ExpectCostFunctionsAreEqual
giafranchini Nov 2, 2024
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
Missing support for Ceres 2.1.0 Manifold in pose_3d constraint tests
giafranchini committed Nov 2, 2024
commit 568decf5e3a4375593badf6a2637989ab9cc30e2
Original file line number Diff line number Diff line change
@@ -40,6 +40,7 @@
#include <vector>

#include <fuse_constraints/absolute_pose_3d_stamped_euler_constraint.hpp>
#include <fuse_core/ceres_macros.hpp>
#include <fuse_core/eigen.hpp>
#include <fuse_core/eigen_gtest.hpp>
#include <fuse_core/serialization.hpp>
@@ -246,12 +247,19 @@ TEST(AbsolutePose3DStampedEulerConstraint, Optimization)
problem.AddParameterBlock(
position_variable->data(),
position_variable->size(),
#if !CERES_SUPPORTS_MANIFOLDS
position_variable->localParameterization());
#else
position_variable->manifold());
#endif
problem.AddParameterBlock(
orientation_variable->data(),
orientation_variable->size(),
#if !CERES_SUPPORTS_MANIFOLDS
orientation_variable->localParameterization());

#else
orientation_variable->manifold());
#endif
std::vector<double *> parameter_blocks;
parameter_blocks.push_back(position_variable->data());
parameter_blocks.push_back(orientation_variable->data());
@@ -365,12 +373,19 @@ TEST(AbsolutePose3DStampedEulerConstraint, OptimizationPartial)
problem.AddParameterBlock(
position_variable->data(),
position_variable->size(),
#if !CERES_SUPPORTS_MANIFOLDS
position_variable->localParameterization());
#else
position_variable->manifold());
#endif
problem.AddParameterBlock(
orientation_variable->data(),
orientation_variable->size(),
#if !CERES_SUPPORTS_MANIFOLDS
orientation_variable->localParameterization());

#else
orientation_variable->manifold());
#endif
std::vector<double *> parameter_blocks;
parameter_blocks.push_back(position_variable->data());
parameter_blocks.push_back(orientation_variable->data());
Original file line number Diff line number Diff line change
@@ -282,19 +282,35 @@ TEST(RelativePose3DStampedEulerConstraint, Optimization)
problem.AddParameterBlock(
orientation1->data(),
orientation1->size(),
#if !CERES_SUPPORTS_MANIFOLDS
orientation1->localParameterization());
#else
orientation1->manifold());
#endif
problem.AddParameterBlock(
position1->data(),
position1->size(),
#if !CERES_SUPPORTS_MANIFOLDS
position1->localParameterization());
#else
position1->manifold());
#endif
problem.AddParameterBlock(
orientation2->data(),
orientation2->size(),
#if !CERES_SUPPORTS_MANIFOLDS
orientation2->localParameterization());
#else
orientation2->manifold());
#endif
problem.AddParameterBlock(
position2->data(),
position2->size(),
#if !CERES_SUPPORTS_MANIFOLDS
position2->localParameterization());
#else
position2->manifold());
#endif
std::vector<double *> prior_parameter_blocks;
prior_parameter_blocks.push_back(position1->data());
prior_parameter_blocks.push_back(orientation1->data());
@@ -501,19 +517,35 @@ TEST(RelativePose3DStampedEulerConstraint, OptimizationPartial)
problem.AddParameterBlock(
orientation1->data(),
orientation1->size(),
#if !CERES_SUPPORTS_MANIFOLDS
orientation1->localParameterization());
#else
orientation1->manifold());
#endif
problem.AddParameterBlock(
position1->data(),
position1->size(),
#if !CERES_SUPPORTS_MANIFOLDS
position1->localParameterization());
#else
position1->manifold());
#endif
problem.AddParameterBlock(
orientation2->data(),
orientation2->size(),
#if !CERES_SUPPORTS_MANIFOLDS
orientation2->localParameterization());
#else
orientation2->manifold());
#endif
problem.AddParameterBlock(
position2->data(),
position2->size(),
#if !CERES_SUPPORTS_MANIFOLDS
position2->localParameterization());
#else
position2->manifold());
#endif
std::vector<double *> prior_parameter_blocks;
prior_parameter_blocks.push_back(position1->data());
prior_parameter_blocks.push_back(orientation1->data());