Skip to content

Commit 9fe7bc2

Browse files
alexlin2alexlin2
committed
untested humble support
1 parent c59dbfd commit 9fe7bc2

8 files changed

Lines changed: 216 additions & 41 deletions

File tree

‎README.md‎

Lines changed: 8 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -15,17 +15,18 @@ For the ROS API (topics/services), see `AUTONOMY_STACK_README.md`.
1515

1616
## Supported Environment
1717

18-
- Ubuntu 24.04
19-
- ROS 2 Jazzy
18+
- Ubuntu 22.04 + ROS 2 Humble
19+
- Ubuntu 24.04 + ROS 2 Jazzy
2020

2121
## Quick Start
2222

2323
### 1) Install ROS + system dependencies
2424

25-
After installing ROS 2 Jazzy, source it in your shell:
25+
After installing ROS 2 Humble or Jazzy, source it in your shell:
26+
Replace `$ROS_DISTRO` with `humble` or `jazzy` in the commands below.
2627

2728
```bash
28-
echo "source /opt/ros/jazzy/setup.bash" >> ~/.bashrc
29+
echo "source /opt/ros/$ROS_DISTRO/setup.bash" >> ~/.bashrc
2930
source ~/.bashrc
3031
```
3132

@@ -34,8 +35,8 @@ Install core dependencies:
3435
```bash
3536
sudo apt update
3637
sudo apt install -y \
37-
ros-jazzy-desktop-full \
38-
ros-jazzy-pcl-ros \
38+
ros-$ROS_DISTRO-desktop-full \
39+
ros-$ROS_DISTRO-pcl-ros \
3940
libpcl-dev \
4041
git
4142
```
@@ -76,7 +77,7 @@ colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --package
7677

7778
#### Build SLAM dependencies (Sophus / Ceres / GTSAM)
7879

79-
These dependencies are vendored under `src/slam/dependency/`. If you don’t already have them installed system-wide, you can install them from source:
80+
These dependencies are vendored under `src/slam/dependency/`. If you don’t already have them installed system-wide, you can install them from source (or use your distro packages, e.g. `libceres-dev`).
8081

8182
```bash
8283
# Sophus

‎src/slam/arise_slam_mid360/include/arise_slam_mid360/LaserMapping/lidarOptimization.h‎

Lines changed: 19 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -5,7 +5,7 @@
55
#ifndef _LIDAR_OPTIMIZATION_ANALYTIC_H_
66
#define _LIDAR_OPTIMIZATION_ANALYTIC_H_
77

8-
#include <ceres/ceres.h>
8+
#include "arise_slam_mid360/utils/ceres_compat.h"
99
#include <ceres/rotation.h>
1010
#include <Eigen/Dense>
1111
#include <Eigen/Geometry>
@@ -37,18 +37,30 @@ class SurfNormAnalyticCostFunction : public ceres::SizedCostFunction<1, 7> {
3737
double negative_OA_dot_norm;
3838
};
3939

40-
class PoseSE3Parameterization : public ceres::Manifold {
40+
class PoseSE3Parameterization :
41+
#if ARISE_SLAM_HAS_CERES_MANIFOLD
42+
public ceres::Manifold {
43+
#else
44+
public ceres::LocalParameterization {
45+
#endif
4146
public:
4247

4348
PoseSE3Parameterization() {}
4449
virtual ~PoseSE3Parameterization() {}
45-
virtual bool Plus(const double* x, const double* delta, double* x_plus_delta) const;
46-
virtual bool ComputeJacobian(const double* x, double* jacobian) const;
47-
virtual int GlobalSize() const { return 7; }
48-
virtual int LocalSize() const { return 6; }
50+
virtual bool Plus(const double* x, const double* delta, double* x_plus_delta) const override;
51+
#if ARISE_SLAM_HAS_CERES_MANIFOLD
52+
virtual bool PlusJacobian(const double* x, double* jacobian) const override;
53+
virtual int AmbientSize() const override { return 7; }
54+
virtual int TangentSize() const override { return 6; }
55+
virtual bool Minus(const double* y, const double* x, double* y_minus_x) const override;
56+
virtual bool MinusJacobian(const double* x, double* jacobian) const override;
57+
#else
58+
virtual bool ComputeJacobian(const double* x, double* jacobian) const override;
59+
virtual int GlobalSize() const override { return 7; }
60+
virtual int LocalSize() const override { return 6; }
61+
#endif
4962
};
5063

5164

5265

5366
#endif // _LIDAR_OPTIMIZATION_ANALYTIC_H_
54-

‎src/slam/arise_slam_mid360/include/arise_slam_mid360/LidarProcess/factor/lidarOptimization.h‎

Lines changed: 19 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -5,7 +5,7 @@
55
#ifndef _LIDAR_OPTIMIZATION_ANALYTIC_H_
66
#define _LIDAR_OPTIMIZATION_ANALYTIC_H_
77

8-
#include <ceres/ceres.h>
8+
#include "arise_slam_mid360/utils/ceres_compat.h"
99
#include <ceres/rotation.h>
1010
#include <Eigen/Dense>
1111
#include <Eigen/Geometry>
@@ -37,18 +37,30 @@ class SurfNormAnalyticCostFunction : public ceres::SizedCostFunction<1, 7> {
3737
double negative_OA_dot_norm;
3838
};
3939

40-
class PoseSE3Parameterization : public ceres::Manifold {
40+
class PoseSE3Parameterization :
41+
#if ARISE_SLAM_HAS_CERES_MANIFOLD
42+
public ceres::Manifold {
43+
#else
44+
public ceres::LocalParameterization {
45+
#endif
4146
public:
4247

4348
PoseSE3Parameterization() {}
4449
virtual ~PoseSE3Parameterization() {}
45-
virtual bool Plus(const double* x, const double* delta, double* x_plus_delta) const;
46-
virtual bool ComputeJacobian(const double* x, double* jacobian) const;
47-
virtual int GlobalSize() const { return 7; }
48-
virtual int LocalSize() const { return 6; }
50+
virtual bool Plus(const double* x, const double* delta, double* x_plus_delta) const override;
51+
#if ARISE_SLAM_HAS_CERES_MANIFOLD
52+
virtual bool PlusJacobian(const double* x, double* jacobian) const override;
53+
virtual int AmbientSize() const override { return 7; }
54+
virtual int TangentSize() const override { return 6; }
55+
virtual bool Minus(const double* y, const double* x, double* y_minus_x) const override;
56+
virtual bool MinusJacobian(const double* x, double* jacobian) const override;
57+
#else
58+
virtual bool ComputeJacobian(const double* x, double* jacobian) const override;
59+
virtual int GlobalSize() const override { return 7; }
60+
virtual int LocalSize() const override { return 6; }
61+
#endif
4962
};
5063

5164

5265

5366
#endif // _LIDAR_OPTIMIZATION_ANALYTIC_H_
54-

‎src/slam/arise_slam_mid360/include/arise_slam_mid360/LidarProcess/factor/local_parameterization_se3.hpp‎

Lines changed: 45 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -1,13 +1,18 @@
11
#ifndef SOPHUS_TEST_LOCAL_PARAMETERIZATION_SE3_HPP
22
#define SOPHUS_TEST_LOCAL_PARAMETERIZATION_SE3_HPP
33

4-
#include <ceres/local_parameterization.h>
54
#include <sophus/se3.hpp>
5+
#include "arise_slam_mid360/utils/ceres_compat.h"
66

77
namespace Sophus {
88
namespace test {
99

10-
class LocalParameterizationSE3 : public ceres::Manifold {
10+
class LocalParameterizationSE3 :
11+
#if ARISE_SLAM_HAS_CERES_MANIFOLD
12+
public ceres::Manifold {
13+
#else
14+
public ceres::LocalParameterization {
15+
#endif
1116
public:
1217
virtual ~LocalParameterizationSE3() {}
1318

@@ -16,7 +21,7 @@ class LocalParameterizationSE3 : public ceres::Manifold {
1621
// T * exp(x)
1722
//
1823
virtual bool Plus(double const* T_raw, double const* delta_raw,
19-
double* T_plus_delta_raw) const {
24+
double* T_plus_delta_raw) const override {
2025
Eigen::Map<SE3d const> const T(T_raw);
2126
Eigen::Map<Vector6d const> const delta(delta_raw);
2227
Eigen::Map<SE3d> T_plus_delta(T_plus_delta_raw);
@@ -28,18 +33,52 @@ class LocalParameterizationSE3 : public ceres::Manifold {
2833
//
2934
// Dx T * exp(x) with x=0
3035
//
36+
#if ARISE_SLAM_HAS_CERES_MANIFOLD
37+
virtual bool PlusJacobian(double const* T_raw,
38+
double* jacobian_raw) const override {
39+
Eigen::Map<SE3d const> T(T_raw);
40+
Eigen::Map<Eigen::Matrix<double, 7, 6, Eigen::RowMajor>> jacobian(
41+
jacobian_raw);
42+
jacobian = T.Dx_this_mul_exp_x_at_0();
43+
return true;
44+
}
45+
46+
virtual bool Minus(double const* T_plus_delta_raw, double const* T_raw,
47+
double* delta_raw) const override {
48+
Eigen::Map<SE3d const> const T_plus_delta(T_plus_delta_raw);
49+
Eigen::Map<SE3d const> const T(T_raw);
50+
Eigen::Map<Vector6d> delta(delta_raw);
51+
delta = (T.inverse() * T_plus_delta).log();
52+
return true;
53+
}
54+
55+
virtual bool MinusJacobian(double const* T_raw,
56+
double* jacobian_raw) const override {
57+
Eigen::Map<Eigen::Matrix<double, 6, 7, Eigen::RowMajor>> jacobian(
58+
jacobian_raw);
59+
jacobian.setZero();
60+
jacobian.block<3, 3>(0, 4).setIdentity();
61+
jacobian.block<3, 3>(3, 0) = 2.0 * Eigen::Matrix3d::Identity();
62+
return true;
63+
}
64+
65+
virtual int AmbientSize() const override { return SE3d::num_parameters; }
66+
67+
virtual int TangentSize() const override { return SE3d::DoF; }
68+
#else
3169
virtual bool ComputeJacobian(double const* T_raw,
32-
double* jacobian_raw) const {
70+
double* jacobian_raw) const override {
3371
Eigen::Map<SE3d const> T(T_raw);
3472
Eigen::Map<Eigen::Matrix<double, 7, 6, Eigen::RowMajor>> jacobian(
3573
jacobian_raw);
3674
jacobian = T.Dx_this_mul_exp_x_at_0();
3775
return true;
3876
}
3977

40-
virtual int GlobalSize() const { return SE3d::num_parameters; }
78+
virtual int GlobalSize() const override { return SE3d::num_parameters; }
4179

42-
virtual int LocalSize() const { return SE3d::DoF; }
80+
virtual int LocalSize() const override { return SE3d::DoF; }
81+
#endif
4382
};
4483
} // namespace test
4584
} // namespace Sophus

‎src/slam/arise_slam_mid360/include/arise_slam_mid360/LidarProcess/factor/pose_local_parameterization.h‎

Lines changed: 19 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -7,17 +7,27 @@
77

88

99
#include <eigen3/Eigen/Dense>
10-
#include <ceres/ceres.h>
10+
#include "arise_slam_mid360/utils/ceres_compat.h"
1111
#include "../../utils/utility.h"
12-
class PoseLocalParameterization : public ceres::Manifold
12+
class PoseLocalParameterization :
13+
#if ARISE_SLAM_HAS_CERES_MANIFOLD
14+
public ceres::Manifold
15+
#else
16+
public ceres::LocalParameterization
17+
#endif
1318
{
14-
virtual bool Plus(const double *x, const double *delta, double *x_plus_delta) const;
15-
virtual bool PlusJacobian(const double *x, double *jacobian) const;
16-
virtual int AmbientSize() const;
17-
virtual int TangentSize() const;
18-
19-
virtual bool Minus(const double *x, const double *delta, double *x_minus_delta) const;
20-
virtual bool MinusJacobian(const double* x, double* jacobian) const;
19+
virtual bool Plus(const double *x, const double *delta, double *x_plus_delta) const override;
20+
#if ARISE_SLAM_HAS_CERES_MANIFOLD
21+
virtual bool PlusJacobian(const double *x, double *jacobian) const override;
22+
virtual int AmbientSize() const override;
23+
virtual int TangentSize() const override;
24+
virtual bool Minus(const double *y, const double *x, double *y_minus_x) const override;
25+
virtual bool MinusJacobian(const double* x, double* jacobian) const override;
26+
#else
27+
virtual bool ComputeJacobian(const double *x, double *jacobian) const override;
28+
virtual int GlobalSize() const override;
29+
virtual int LocalSize() const override;
30+
#endif
2131
};
2232

2333

‎src/slam/arise_slam_mid360/include/arise_slam_mid360/utils/ceres_compat.h‎

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,14 @@
1+
#ifndef ARISE_SLAM_MID360_CERES_COMPAT_H
2+
#define ARISE_SLAM_MID360_CERES_COMPAT_H
3+
4+
#include <ceres/ceres.h>
5+
6+
#if defined(CERES_VERSION_MAJOR) && \
7+
(CERES_VERSION_MAJOR > 2 || \
8+
(CERES_VERSION_MAJOR == 2 && CERES_VERSION_MINOR >= 1))
9+
#define ARISE_SLAM_HAS_CERES_MANIFOLD 1
10+
#else
11+
#define ARISE_SLAM_HAS_CERES_MANIFOLD 0
12+
#endif
13+
14+
#endif // ARISE_SLAM_MID360_CERES_COMPAT_H

‎src/slam/arise_slam_mid360/src/LaserMapping/lidarOptimization.cpp‎

Lines changed: 46 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -112,6 +112,51 @@ bool PoseSE3Parameterization::Plus(const double *x, const double *delta, double
112112
}
113113

114114

115+
#if ARISE_SLAM_HAS_CERES_MANIFOLD
116+
bool PoseSE3Parameterization::PlusJacobian(const double *x, double *jacobian) const
117+
{
118+
Eigen::Map<Eigen::Matrix<double, 7, 6, Eigen::RowMajor>> j(jacobian);
119+
(j.topRows(6)).setIdentity();
120+
(j.bottomRows(1)).setZero();
121+
122+
return true;
123+
}
124+
125+
bool PoseSE3Parameterization::Minus(const double *y, const double *x, double *y_minus_x) const
126+
{
127+
Eigen::Map<const Eigen::Vector3d> trans_x(x);
128+
Eigen::Quaterniond quat_x = Eigen::Map<const Eigen::Quaterniond>(x + 3);
129+
Eigen::Map<const Eigen::Vector3d> trans_y(y);
130+
Eigen::Quaterniond quat_y = Eigen::Map<const Eigen::Quaterniond>(y + 3);
131+
132+
quat_x.normalize();
133+
quat_y.normalize();
134+
135+
Eigen::Quaterniond delta_q = quat_y * quat_x.conjugate();
136+
if (delta_q.w() < 0.0) {
137+
delta_q.coeffs() *= -1.0;
138+
}
139+
delta_q.normalize();
140+
141+
Eigen::AngleAxisd delta_aa(delta_q);
142+
143+
Eigen::Map<Eigen::Matrix<double, 6, 1>> delta(y_minus_x);
144+
delta.head<3>() = trans_y - delta_q * trans_x;
145+
delta.tail<3>() = delta_aa.axis() * delta_aa.angle();
146+
147+
return true;
148+
}
149+
150+
bool PoseSE3Parameterization::MinusJacobian(const double *x, double *jacobian) const
151+
{
152+
Eigen::Map<Eigen::Matrix<double, 6, 7, Eigen::RowMajor>> j(jacobian);
153+
j.setZero();
154+
j.block<3, 3>(0, 0).setIdentity();
155+
j.block<3, 3>(3, 3) = 2.0 * Eigen::Matrix3d::Identity();
156+
157+
return true;
158+
}
159+
#else
115160
bool PoseSE3Parameterization::ComputeJacobian(const double *x, double *jacobian) const
116161
{
117162
Eigen::Map<Eigen::Matrix<double, 7, 6, Eigen::RowMajor>> j(jacobian);
@@ -120,6 +165,7 @@ bool PoseSE3Parameterization::ComputeJacobian(const double *x, double *jacobian)
120165

121166
return true;
122167
}
168+
#endif
123169

124170

125171
void getTransformFromSe3(const Eigen::Matrix<double,6,1>& se3, Eigen::Quaterniond& q, Eigen::Vector3d& t){

0 commit comments

Comments
 (0)