Skip to content

Commit aa71945

Browse files
Maurizio Mongemeta-codesync[bot]
Maurizio Monge
authored and
meta-codesync[bot]
committed
{Feature} Calib jacobians (#304)
Summary: Pull Request resolved: #304 Add Jacobians wrt the calibration parameters. This is required to make VI-BA work. This diff also changes some return types to const references, as it is better practice to avoid creating temporary objects with dynamic allocation, and allows Mut fields so that the CameraCalibration type can be used in optimization. Pull Request resolved: #293 Reviewed By: SeaOtocinclus Differential Revision: D85353427 Pulled By: maurimo fbshipit-source-id: 36b76b2d92869260d0f9ba8d60207ab2d3d9e4d1
1 parent ad88eab commit aa71945

File tree

13 files changed

+498
-85
lines changed

13 files changed

+498
-85
lines changed

‎core/calibration/CameraCalibration.cpp‎

Lines changed: 36 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -30,7 +30,7 @@ CameraCalibration::CameraCalibration(
3030
const double maxSolidAngle,
3131
const std::string& serialNumber,
3232
const double timeOffsetSecDeviceCamera,
33-
const std::optional<double> maybeReadOutTimeSec)
33+
const std::optional<double>& maybeReadOutTimeSec)
3434
: label_(label),
3535
projectionModel_(projectionModelType, projectionParams),
3636
T_Device_Camera_(T_Device_Camera),
@@ -70,10 +70,18 @@ double CameraCalibration::getTimeOffsetSecDeviceCamera() const {
7070
return timeOffsetSecDeviceCamera_;
7171
}
7272

73+
double& CameraCalibration::getTimeOffsetSecDeviceCameraMut() {
74+
return timeOffsetSecDeviceCamera_;
75+
}
76+
7377
std::optional<double> CameraCalibration::getReadOutTimeSec() const {
7478
return maybeReadOutTimeSec_;
7579
}
7680

81+
std::optional<double>& CameraCalibration::getReadOutTimeSecMut() {
82+
return maybeReadOutTimeSec_;
83+
}
84+
7785
namespace {
7886
// A helper function to determine if a pixel is within valid mask in a camera
7987
inline bool checkPixelValidInMask(
@@ -110,10 +118,26 @@ CameraProjection::ModelType CameraCalibration::modelName() const {
110118
return projectionModel_.modelName();
111119
}
112120

113-
Eigen::VectorXd CameraCalibration::projectionParams() const {
121+
const Eigen::VectorXd& CameraCalibration::projectionParams() const {
114122
return projectionModel_.projectionParams();
115123
}
116124

125+
Eigen::VectorXd& CameraCalibration::projectionParamsMut() {
126+
return projectionModel_.projectionParamsMut();
127+
}
128+
129+
int CameraCalibration::numParameters() const {
130+
return projectionModel_.numParameters();
131+
}
132+
133+
int CameraCalibration::numProjectionParameters() const {
134+
return projectionModel_.numProjectionParameters();
135+
}
136+
137+
int CameraCalibration::numDistortionParameters() const {
138+
return projectionModel_.numDistortionParameters();
139+
}
140+
117141
Eigen::Vector2d CameraCalibration::getPrincipalPoint() const {
118142
return projectionModel_.getPrincipalPoint();
119143
}
@@ -122,15 +146,21 @@ Eigen::Vector2d CameraCalibration::getFocalLengths() const {
122146
return projectionModel_.getFocalLengths();
123147
}
124148

125-
Eigen::Vector2d CameraCalibration::projectNoChecks(const Eigen::Vector3d& pointInCamera) const {
126-
return projectionModel_.project(pointInCamera);
149+
Eigen::Vector2d CameraCalibration::projectNoChecks(
150+
const Eigen::Vector3d& pointInCamera,
151+
Eigen::Ref<Eigen::Matrix<double, 2, 3>> jacobianWrtPoint,
152+
Eigen::Ref<Eigen::Matrix<double, 2, Eigen::Dynamic>> jacobianWrtParams) const {
153+
return projectionModel_.project(pointInCamera, jacobianWrtPoint, jacobianWrtParams);
127154
}
128155

129156
std::optional<Eigen::Vector2d> CameraCalibration::project(
130-
const Eigen::Vector3d& pointInCamera) const {
157+
const Eigen::Vector3d& pointInCamera,
158+
Eigen::Ref<Eigen::Matrix<double, 2, 3>> jacobianWrtPoint,
159+
Eigen::Ref<Eigen::Matrix<double, 2, Eigen::Dynamic>> jacobianWrtParams) const {
131160
// check point is in front of camera
132161
if (checkPointInVisibleCone(pointInCamera, maxSolidAngle_)) {
133-
const Eigen::Vector2d cameraPixel = projectNoChecks(pointInCamera);
162+
const Eigen::Vector2d cameraPixel =
163+
projectNoChecks(pointInCamera, jacobianWrtPoint, jacobianWrtParams);
134164
if (isVisible(cameraPixel)) {
135165
return cameraPixel;
136166
}

‎core/calibration/CameraCalibration.h‎

Lines changed: 46 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -37,6 +37,26 @@ class CameraCalibration {
3737
*/
3838
CameraCalibration() = default;
3939

40+
/**
41+
* @brief Default copy constructor.
42+
*/
43+
CameraCalibration(const CameraCalibration&) = default;
44+
45+
/**
46+
* @brief Default move constructor.
47+
*/
48+
CameraCalibration(CameraCalibration&&) = default;
49+
50+
/**
51+
* @brief Default copy assignment.
52+
*/
53+
CameraCalibration& operator=(const CameraCalibration&) = default;
54+
55+
/**
56+
* @brief Default move assignment.
57+
*/
58+
CameraCalibration& operator=(CameraCalibration&&) = default;
59+
4060
/**
4161
* @brief Constructor with a list of parameters for CameraCalibration.
4262
* @param label The label of the camera, e.g. "camera-slam-left".
@@ -67,7 +87,7 @@ class CameraCalibration {
6787
double maxSolidAngle,
6888
const std::string& serialNumber = "",
6989
double timeOffsetSecDeviceCamera = 0.0,
70-
std::optional<double> maybeReadOutTimeSec = {});
90+
const std::optional<double>& maybeReadOutTimeSec = {});
7191

7292
std::string getLabel() const;
7393
std::string getSerialNumber() const;
@@ -76,7 +96,9 @@ class CameraCalibration {
7696
double getMaxSolidAngle() const;
7797
std::optional<double> getValidRadius() const;
7898
double getTimeOffsetSecDeviceCamera() const;
99+
double& getTimeOffsetSecDeviceCameraMut();
79100
std::optional<double> getReadOutTimeSec() const;
101+
std::optional<double>& getReadOutTimeSecMut();
80102
/**
81103
* @brief Function to check whether a pixel is within the valid area of the sensor plane.
82104
*/
@@ -85,22 +107,41 @@ class CameraCalibration {
85107
CameraProjection::ModelType modelName() const; // return KB3 or Fisheye624
86108
Eigen::Vector2d getPrincipalPoint() const; // return optical center
87109
Eigen::Vector2d getFocalLengths() const; // return focal length in x and y
88-
Eigen::VectorXd projectionParams() const; // return full calibration parameters
110+
const Eigen::VectorXd& projectionParams() const; // return full calibration parameters
111+
Eigen::VectorXd& projectionParamsMut();
112+
int numParameters() const; // return number of parameters
113+
int numProjectionParameters() const; // return number of projection parameters
114+
int numDistortionParameters() const; // return number of distortion parameters
89115

90116
/**
91117
* @brief Function to project a 3d point (in camera frame) to a 2d camera pixel location. In this
92118
* function, no check is performed.
93119
* @param pointInCamera 3d point in camera frame.
120+
* @param jacobianWrtPoint Optional, if not null, will store the Jacobian of the projection
121+
* with respect to the point in camera space.
122+
* @param jacobianWrtParams Optional, if not null, will store the Jacobian of the projection
123+
* with respect to the projection parameters.
94124
* @return 2d pixel location in image plane.
95125
*/
96-
Eigen::Vector2d projectNoChecks(const Eigen::Vector3d& pointInCamera) const;
126+
Eigen::Vector2d projectNoChecks(
127+
const Eigen::Vector3d& pointInCamera,
128+
Eigen::Ref<Eigen::Matrix<double, 2, 3>> jacobianWrtPoint = NullRef(),
129+
Eigen::Ref<Eigen::Matrix<double, 2, Eigen::Dynamic>> jacobianWrtParams = NullRef()) const;
130+
97131
/**
98132
* @brief Function to project a 3d point (in camera frame) to a 2d camera pixel location, with a
99133
* number of validity checks to ensure the point is visible.
100134
* @param pointInCamera 3d point in camera frame.
135+
* @param jacobianWrtPoint Optional, if not null, will store the Jacobian of the projection
136+
* with respect to the point in camera space.
137+
* @param jacobianWrtParams Optional, if not null, will store the Jacobian of the projection
138+
* with respect to the projection parameters.
101139
* @return 2d pixel location in image plane. If any of the check failed, return std::nullopt.
102140
*/
103-
std::optional<Eigen::Vector2d> project(const Eigen::Vector3d& pointInCamera) const;
141+
std::optional<Eigen::Vector2d> project(
142+
const Eigen::Vector3d& pointInCamera,
143+
Eigen::Ref<Eigen::Matrix<double, 2, 3>> jacobianWrtPoint = NullRef(),
144+
Eigen::Ref<Eigen::Matrix<double, 2, Eigen::Dynamic>> jacobianWrtParams = NullRef()) const;
104145

105146
/**
106147
* @brief Function to unproject a 2d pixel location to a 3d ray in camera frame. In this function,
@@ -109,6 +150,7 @@ class CameraCalibration {
109150
* @return 3d ray, in camera frame.
110151
*/
111152
Eigen::Vector3d unprojectNoChecks(const Eigen::Vector2d& cameraPixel) const;
153+
112154
/**
113155
* @brief Function to unproject a 2d pixel location to a 3d ray, in camera frame, with a number of
114156
* validity checks to ensure the unprojection is valid.

‎core/calibration/camera_projections/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -27,5 +27,6 @@ target_link_libraries(camera_models INTERFACE common)
2727
target_include_directories(camera_models INTERFACE $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/../..>)
2828

2929
add_library(camera_projection CameraProjection.cpp CameraProjection.h CameraProjectionFormat.h)
30+
target_link_libraries(camera_projection INTERFACE calibration_nullref)
3031
target_include_directories(camera_projection PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/../..>)
3132
target_link_libraries(camera_projection PUBLIC camera_models format Eigen3::Eigen Sophus::Sophus)

‎core/calibration/camera_projections/CameraProjection.cpp‎

Lines changed: 53 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -39,7 +39,7 @@ typename CameraProjectionTemplated<Scalar>::ProjectionVariant getProjectionVaria
3939
template <typename Scalar>
4040
CameraProjectionTemplated<Scalar>::CameraProjectionTemplated(
4141
const ModelType& type,
42-
const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>& projectionParams)
42+
const Eigen::Vector<Scalar, Eigen::Dynamic>& projectionParams)
4343
: modelName_(type),
4444
projectionParams_(projectionParams),
4545
projectionVariant_(getProjectionVariant<Scalar>(type)) {}
@@ -51,15 +51,50 @@ typename CameraProjectionTemplated<Scalar>::ModelType CameraProjectionTemplated<
5151
}
5252

5353
template <typename Scalar>
54-
Eigen::Matrix<Scalar, Eigen::Dynamic, 1> CameraProjectionTemplated<Scalar>::projectionParams()
54+
const Eigen::Vector<Scalar, Eigen::Dynamic>& CameraProjectionTemplated<Scalar>::projectionParams()
5555
const {
5656
return projectionParams_;
5757
}
5858

5959
template <typename Scalar>
60-
Eigen::Matrix<Scalar, 2, 1> CameraProjectionTemplated<Scalar>::getFocalLengths() const {
60+
Eigen::Vector<Scalar, Eigen::Dynamic>& CameraProjectionTemplated<Scalar>::projectionParamsMut() {
61+
return projectionParams_;
62+
}
63+
64+
template <typename Scalar>
65+
int CameraProjectionTemplated<Scalar>::numParameters() const {
66+
return std::visit(
67+
[](auto&& projection) -> int {
68+
using T = std::decay_t<decltype(projection)>;
69+
return T::kNumParams;
70+
},
71+
projectionVariant_);
72+
}
73+
74+
template <typename Scalar>
75+
int CameraProjectionTemplated<Scalar>::numProjectionParameters() const {
76+
return std::visit(
77+
[](auto&& projection) -> int {
78+
using T = std::decay_t<decltype(projection)>;
79+
return T::kNumParams - T::kNumDistortionParams;
80+
},
81+
projectionVariant_);
82+
}
83+
84+
template <typename Scalar>
85+
int CameraProjectionTemplated<Scalar>::numDistortionParameters() const {
86+
return std::visit(
87+
[](auto&& projection) -> int {
88+
using T = std::decay_t<decltype(projection)>;
89+
return T::kNumDistortionParams;
90+
},
91+
projectionVariant_);
92+
}
93+
94+
template <typename Scalar>
95+
Eigen::Vector<Scalar, 2> CameraProjectionTemplated<Scalar>::getFocalLengths() const {
6196
return std::visit(
62-
[this](auto&& projection) -> Eigen::Matrix<Scalar, 2, 1> {
97+
[this](auto&& projection) -> Eigen::Vector<Scalar, 2> {
6398
using T = std::decay_t<decltype(projection)>;
6499
int focalXIdx = T::kFocalXIdx;
65100
int focalYIdx = T::kFocalYIdx;
@@ -69,9 +104,9 @@ Eigen::Matrix<Scalar, 2, 1> CameraProjectionTemplated<Scalar>::getFocalLengths()
69104
}
70105

71106
template <typename Scalar>
72-
Eigen::Matrix<Scalar, 2, 1> CameraProjectionTemplated<Scalar>::getPrincipalPoint() const {
107+
Eigen::Vector<Scalar, 2> CameraProjectionTemplated<Scalar>::getPrincipalPoint() const {
73108
return std::visit(
74-
[this](auto&& projection) -> Eigen::Matrix<Scalar, 2, 1> {
109+
[this](auto&& projection) -> Eigen::Vector<Scalar, 2> {
75110
using T = std::decay_t<decltype(projection)>;
76111
int principalPointColIdx = T::kPrincipalPointColIdx;
77112
int principalPointRowIdx = T::kPrincipalPointRowIdx;
@@ -81,20 +116,25 @@ Eigen::Matrix<Scalar, 2, 1> CameraProjectionTemplated<Scalar>::getPrincipalPoint
81116
}
82117

83118
template <typename Scalar>
84-
Eigen::Matrix<Scalar, 2, 1> CameraProjectionTemplated<Scalar>::project(
85-
const Eigen::Matrix<Scalar, 3, 1>& pointInCamera,
86-
Eigen::Matrix<Scalar, 2, 3>* jacobianWrtPoint) const {
119+
Eigen::Vector<Scalar, 2> CameraProjectionTemplated<Scalar>::project(
120+
const Eigen::Vector<Scalar, 3>& pointInCamera,
121+
Eigen::Ref<Eigen::Matrix<Scalar, 2, 3>> jacobianWrtPoint,
122+
Eigen::Ref<Eigen::Matrix<Scalar, 2, Eigen::Dynamic>> jacobianWrtParams) const {
87123
return std::visit(
88124
[&](auto&& projection) {
89125
using T = std::decay_t<decltype(projection)>;
90-
return T::project(pointInCamera, projectionParams_, jacobianWrtPoint);
126+
return T::project(
127+
pointInCamera,
128+
projectionParams_,
129+
isNull(jacobianWrtPoint) ? nullptr : &jacobianWrtPoint,
130+
isNull(jacobianWrtParams) ? nullptr : &jacobianWrtParams);
91131
},
92132
projectionVariant_);
93133
}
94134

95135
template <typename Scalar>
96-
Eigen::Matrix<Scalar, 3, 1> CameraProjectionTemplated<Scalar>::unproject(
97-
const Eigen::Matrix<Scalar, 2, 1>& cameraPixel) const {
136+
Eigen::Vector<Scalar, 3> CameraProjectionTemplated<Scalar>::unproject(
137+
const Eigen::Vector<Scalar, 2>& cameraPixel) const {
98138
return std::visit(
99139
[&](auto&& projection) {
100140
using T = std::decay_t<decltype(projection)>;
@@ -127,7 +167,7 @@ template <typename Scalar>
127167
template <typename OtherScalar>
128168
[[nodiscard]] CameraProjectionTemplated<OtherScalar> CameraProjectionTemplated<Scalar>::cast()
129169
const {
130-
Eigen::Matrix<OtherScalar, Eigen::Dynamic, 1> castedParams =
170+
Eigen::Vector<OtherScalar, Eigen::Dynamic> castedParams =
131171
projectionParams_.template cast<OtherScalar>();
132172
auto castedModelName =
133173
static_cast<typename CameraProjectionTemplated<OtherScalar>::ModelType>(modelName_);

0 commit comments

Comments
 (0)