Skip to content

Commit a251cab

Browse files
author
Fabien Servant
committed
add Equirectangular camera
1 parent e3044bf commit a251cab

File tree

8 files changed

+349
-1
lines changed

8 files changed

+349
-1
lines changed

‎src/aliceVision/aliceVision.i‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -25,6 +25,7 @@
2525
#include <aliceVision/camera/IntrinsicBase.hpp>
2626
#include <aliceVision/camera/Pinhole.hpp>
2727
#include <aliceVision/camera/Equidistant.hpp>
28+
#include <aliceVision/camera/Equirectangular.hpp>
2829

2930
using namespace aliceVision;
3031

‎src/aliceVision/camera/CMakeLists.txt‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -14,6 +14,7 @@ set(camera_files_headers
1414
Undistortion3DEClassicLD.hpp
1515
UndistortionRadial.hpp
1616
Equidistant.hpp
17+
Equirectangular.hpp
1718
IntrinsicBase.hpp
1819
IntrinsicInitMode.hpp
1920
IntrinsicScaleOffset.hpp
@@ -28,6 +29,7 @@ set(camera_files_sources
2829
DistortionRadial.cpp
2930
UndistortionRadial.cpp
3031
Equidistant.cpp
32+
Equirectangular.cpp
3133
IntrinsicBase.cpp
3234
IntrinsicScaleOffset.cpp
3335
IntrinsicScaleOffsetDisto.cpp

‎src/aliceVision/camera/Camera.i‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -29,4 +29,5 @@ using namespace aliceVision::camera;
2929

3030
%include <aliceVision/camera/IntrinsicInitMode.i>
3131
%include <aliceVision/camera/Equidistant.i>
32+
%include <aliceVision/camera/Equirectangular.i>
3233
%include <aliceVision/camera/Pinhole.i>

‎src/aliceVision/camera/Equirectangular.cpp‎

Lines changed: 194 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,194 @@
1+
// This file is part of the AliceVision project.
2+
// Copyright (c) 2024 AliceVision contributors.
3+
// This Source Code Form is subject to the terms of the Mozilla Public License,
4+
// v. 2.0. If a copy of the MPL was not distributed with this file,
5+
// You can obtain one at https://mozilla.org/MPL/2.0/.
6+
7+
#include "Equirectangular.hpp"
8+
9+
#include <algorithm>
10+
#include <cmath>
11+
12+
namespace aliceVision {
13+
namespace camera {
14+
15+
std::shared_ptr<Equirectangular> Equirectangular::cast(std::shared_ptr<IntrinsicBase> sptr)
16+
{
17+
return std::dynamic_pointer_cast<Equirectangular>(sptr);
18+
}
19+
20+
Vec2 Equirectangular::transformProject(const Eigen::Matrix4d& pose, const Vec4& pt, bool applyDistortion) const
21+
{
22+
Vec4 X = pose * pt;
23+
Vec3 spherical = X.head(3).normalized();
24+
25+
Vec2 angles;
26+
angles.x() = atan2(spherical(0), spherical(2));
27+
angles.y() = asin(spherical(1));
28+
29+
Vec2 imapt = cam2ima(angles);
30+
31+
return imapt;
32+
}
33+
34+
Vec2 Equirectangular::project(const Vec4& pt, bool applyDistortion) const
35+
{
36+
Vec3 spherical = pt.head(3).normalized();
37+
38+
Vec2 angles;
39+
angles.x() = atan2(spherical(0), spherical(2));
40+
angles.y() = asin(spherical(1));
41+
42+
Vec2 imapt = cam2ima(angles);
43+
44+
return imapt;
45+
}
46+
47+
Eigen::Matrix<double, 2, 3> Equirectangular::getDerivativeTransformProjectWrtPoint3(const Eigen::Matrix4d& T, const Vec4& pt) const
48+
{
49+
Vec3 spherical = pt.head(3).normalized();
50+
51+
double sx = spherical(0);
52+
double sy = spherical(1);
53+
double sz = spherical(2);
54+
double len = sx*sx + sy*sy;
55+
56+
double norm = pt.norm();
57+
double normsq = norm * norm;
58+
double invnormsq = 1.0 / normsq;
59+
60+
double d_norm_d_x = pt.x() / norm;
61+
double d_norm_d_y = pt.y() / norm;
62+
double d_norm_d_z = pt.z() / norm;
63+
64+
// x / norm; y / norm; z / norm
65+
Eigen::Matrix<double, 3, 3> d_spherical_d_pt3;
66+
d_spherical_d_pt3(0, 0) = (norm * 1.0 - pt.x() * d_norm_d_x) * invnormsq;
67+
d_spherical_d_pt3(0, 1) = (norm * 0.0 - pt.x() * d_norm_d_y) * invnormsq;
68+
d_spherical_d_pt3(0, 2) = (norm * 0.0 - pt.x() * d_norm_d_z) * invnormsq;
69+
d_spherical_d_pt3(1, 0) = (norm * 0.0 - pt.y() * d_norm_d_x) * invnormsq;
70+
d_spherical_d_pt3(1, 1) = (norm * 1.0 - pt.y() * d_norm_d_y) * invnormsq;
71+
d_spherical_d_pt3(1, 2) = (norm * 0.0 - pt.y() * d_norm_d_z) * invnormsq;
72+
d_spherical_d_pt3(2, 0) = (norm * 0.0 - pt.z() * d_norm_d_x) * invnormsq;
73+
d_spherical_d_pt3(2, 1) = (norm * 0.0 - pt.z() * d_norm_d_y) * invnormsq;
74+
d_spherical_d_pt3(2, 2) = (norm * 1.0 - pt.z() * d_norm_d_z) * invnormsq;
75+
76+
77+
//atan2(spherical(0), spherical(2)) ; asin(spherical(1));
78+
Eigen::Matrix<double, 2, 3> d_coords_d_spherical = Eigen::Matrix<double, 2, 3>::Zero();
79+
d_coords_d_spherical(0, 0) = sz / len;
80+
d_coords_d_spherical(0, 2) = - sx / len;
81+
d_coords_d_spherical(1, 1) = 1.0 / sqrt(1.0 - (sy*sy));
82+
83+
84+
return getDerivativeCam2ImaWrtPoint() * d_coords_d_spherical * d_spherical_d_pt3;
85+
}
86+
87+
Eigen::Matrix<double, 2, 3> Equirectangular::getDerivativeTransformProjectWrtDisto(const Eigen::Matrix4d& pose, const Vec4& pt) const
88+
{
89+
return Eigen::Matrix<double, 2, 3>::Zero();
90+
}
91+
92+
Eigen::Matrix<double, 2, 2> Equirectangular::getDerivativeTransformProjectWrtScale(const Eigen::Matrix4d& pose, const Vec4& pt) const
93+
{
94+
Vec3 spherical = pt.head(3).normalized();
95+
96+
Vec2 angles;
97+
angles.x() = atan2(spherical(0), spherical(2));
98+
angles.y() = asin(spherical(1));
99+
100+
return getDerivativeCam2ImaWrtScale(angles);
101+
}
102+
103+
Eigen::Matrix<double, 2, 2> Equirectangular::getDerivativeTransformProjectWrtPrincipalPoint(const Eigen::Matrix4d& pose, const Vec4& pt) const
104+
{
105+
return getDerivativeCam2ImaWrtPrincipalPoint();
106+
}
107+
108+
Eigen::Matrix<double, 2, Eigen::Dynamic> Equirectangular::getDerivativeTransformProjectWrtParams(const Eigen::Matrix4d& pose, const Vec4& pt3D) const
109+
{
110+
Eigen::Matrix<double, 2, Eigen::Dynamic> ret(2, getParams().size());
111+
112+
ret.block<2, 2>(0, 0) = getDerivativeTransformProjectWrtScale(pose, pt3D);
113+
ret.block<2, 2>(0, 2) = getDerivativeTransformProjectWrtPrincipalPoint(pose, pt3D);
114+
115+
return ret;
116+
}
117+
118+
Vec3 Equirectangular::toUnitSphere(const Vec2& pt) const
119+
{
120+
const double latitude = pt(1);
121+
const double longitude = pt(0);
122+
123+
Vec3 spherical;
124+
spherical.x() = cos(latitude) * sin(longitude);
125+
spherical.y() = sin(latitude);
126+
spherical.z() = cos(latitude) * cos(longitude);
127+
128+
return spherical;
129+
}
130+
131+
Eigen::Matrix<double, 3, 2> Equirectangular::getDerivativetoUnitSphereWrtPoint(const Vec2& pt) const
132+
{
133+
const double latitude = pt(1);
134+
const double longitude = pt(0);
135+
136+
Vec3 spherical;
137+
spherical.x() = cos(latitude) * sin(longitude);
138+
spherical.y() = sin(latitude);
139+
spherical.z() = cos(latitude) * cos(longitude);
140+
141+
Eigen::Matrix<double, 3, 2> d_spherical_d_pt;
142+
d_spherical_d_pt(0, 0) = cos(latitude) * cos(longitude);
143+
d_spherical_d_pt(0, 1) = -sin(latitude) * sin(longitude);
144+
d_spherical_d_pt(1, 0) = 0;
145+
d_spherical_d_pt(1, 1) = cos(latitude);
146+
d_spherical_d_pt(0, 0) = cos(latitude) * -sin(longitude);
147+
d_spherical_d_pt(0, 1) = -sin(latitude) * cos(longitude);
148+
149+
return d_spherical_d_pt;
150+
}
151+
152+
Eigen::Matrix<double, 3, Eigen::Dynamic> Equirectangular::getDerivativeBackProjectUnitWrtParams(const Vec2& pt2D) const
153+
{
154+
const Vec2 ptMeters = ima2cam(pt2D);
155+
const Vec3 ptSphere = toUnitSphere(ptMeters);
156+
157+
Eigen::Matrix<double, 3, Eigen::Dynamic> ret(3, 4);
158+
159+
Eigen::Matrix<double, 3, 2> J = getDerivativetoUnitSphereWrtPoint(ptMeters);
160+
161+
ret.block<3, 2>(0, 0) = J * getDerivativeIma2CamWrtScale(pt2D);
162+
ret.block<3, 2>(0, 2) = J * getDerivativeIma2CamWrtPrincipalPoint();
163+
164+
return ret;
165+
}
166+
167+
double Equirectangular::imagePlaneToCameraPlaneError(double value) const { return 0.0; }
168+
169+
bool Equirectangular::isVisibleRay(const Vec3& ray) const
170+
{
171+
return true;
172+
}
173+
174+
EINTRINSIC Equirectangular::getType() const
175+
{
176+
return EINTRINSIC::EQUIRECTANGULAR_CAMERA;
177+
}
178+
179+
double Equirectangular::getHorizontalFov() const
180+
{
181+
return 2.0 * M_PI;
182+
}
183+
184+
double Equirectangular::getVerticalFov() const { return M_PI; }
185+
186+
double Equirectangular::pixelProbability() const
187+
{
188+
return 1.0 / double(w());
189+
}
190+
191+
192+
193+
} // namespace camera
194+
} // namespace aliceVision

‎src/aliceVision/camera/Equirectangular.hpp‎

Lines changed: 117 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,117 @@
1+
// This file is part of the AliceVision project.
2+
// Copyright (c) 2024 AliceVision contributors.
3+
// This Source Code Form is subject to the terms of the Mozilla Public License,
4+
// v. 2.0. If a copy of the MPL was not distributed with this file,
5+
// You can obtain one at https://mozilla.org/MPL/2.0/.
6+
7+
#pragma once
8+
9+
#include <aliceVision/numeric/numeric.hpp>
10+
#include <aliceVision/numeric/projection.hpp>
11+
#include <aliceVision/camera/cameraCommon.hpp>
12+
#include <aliceVision/camera/IntrinsicScaleOffsetDisto.hpp>
13+
#include <aliceVision/geometry/Pose3.hpp>
14+
15+
#include "DistortionFisheye1.hpp"
16+
17+
#include <memory>
18+
#include <algorithm>
19+
20+
namespace aliceVision {
21+
namespace camera {
22+
23+
/**
24+
* @brief Equirectangular is a camera model used for panoramas.
25+
* See https://en.wikipedia.org/wiki/Equirectangular_projection
26+
*
27+
*/
28+
class Equirectangular : public IntrinsicScaleOffsetDisto
29+
{
30+
public:
31+
Equirectangular()
32+
: Equirectangular(1, 1, 1.0, 1.0, 0.0, 0.0)
33+
{}
34+
35+
Equirectangular(unsigned int w,
36+
unsigned int h,
37+
double focalLengthPixX,
38+
double focalLengthPixY,
39+
double offsetX,
40+
double offsetY)
41+
: IntrinsicScaleOffsetDisto(w, h, focalLengthPixX, focalLengthPixY, offsetX, offsetY)
42+
{}
43+
44+
45+
~Equirectangular() override = default;
46+
47+
Equirectangular* clone() const override { return new Equirectangular(*this); }
48+
49+
static std::shared_ptr<Equirectangular> cast(std::shared_ptr<IntrinsicBase> sptr);
50+
51+
void assign(const IntrinsicBase& other) override { *this = dynamic_cast<const Equirectangular&>(other); }
52+
53+
bool isValid() const override { return _scale(0) > 0 && IntrinsicBase::isValid(); }
54+
55+
EINTRINSIC getType() const override;
56+
57+
Vec2 transformProject(const Eigen::Matrix4d& pose, const Vec4& pt, bool applyDistortion = true) const override;
58+
59+
Vec2 transformProject(const geometry::Pose3& pose, const Vec4& pt3D, bool applyDistortion = true) const
60+
{
61+
return transformProject(pose.getHomogeneous(), pt3D, applyDistortion);
62+
}
63+
64+
Vec2 project(const Vec4& pt, bool applyDistortion = true) const override;
65+
66+
Eigen::Matrix<double, 2, 3> getDerivativeTransformProjectWrtPoint3(const Eigen::Matrix4d& pose, const Vec4& pt) const override;
67+
68+
Eigen::Matrix<double, 2, 3> getDerivativeTransformProjectWrtDisto(const Eigen::Matrix4d& pose, const Vec4& pt) const;
69+
70+
Eigen::Matrix<double, 2, 2> getDerivativeTransformProjectWrtScale(const Eigen::Matrix4d& pose, const Vec4& pt) const;
71+
72+
Eigen::Matrix<double, 2, 2> getDerivativeTransformProjectWrtPrincipalPoint(const Eigen::Matrix4d& pose, const Vec4& pt) const;
73+
74+
Eigen::Matrix<double, 2, Eigen::Dynamic> getDerivativeTransformProjectWrtParams(const Eigen::Matrix4d& pose, const Vec4& pt3D) const override;
75+
76+
Vec3 toUnitSphere(const Vec2& pt) const override;
77+
78+
Eigen::Matrix<double, 3, 2> getDerivativetoUnitSphereWrtPoint(const Vec2& pt) const;
79+
80+
/**
81+
* @brief Get the derivative of the unit sphere backprojection
82+
* @param[in] pt2D The 2D point
83+
* @return The backproject jacobian with respect to the pose
84+
*/
85+
Eigen::Matrix<double, 3, Eigen::Dynamic> getDerivativeBackProjectUnitWrtParams(const Vec2& pt2D) const override;
86+
87+
double imagePlaneToCameraPlaneError(double value) const override;
88+
89+
/**
90+
* @brief Return true if this ray should be visible in the image
91+
* @param[in] ray the ray that may or may not be visible in the image
92+
* @return True if this ray is visible theoretically, false otherwise
93+
*/
94+
bool isVisibleRay(const Vec3& ray) const override;
95+
96+
/**
97+
* @brief Get the horizontal FOV in radians
98+
* @return Horizontal FOV in radians
99+
*/
100+
double getHorizontalFov() const override;
101+
102+
/**
103+
* @brief Get the vertical FOV in radians
104+
* @return Vertical FOV in radians
105+
*/
106+
double getVerticalFov() const override;
107+
108+
109+
/**
110+
* @brief how a one pixel change relates to an angular change
111+
* @return a value in radians
112+
*/
113+
double pixelProbability() const override;
114+
};
115+
116+
} // namespace camera
117+
} // namespace aliceVision

‎src/aliceVision/camera/Equirectangular.i‎

Lines changed: 17 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,17 @@
1+
// This file is part of the AliceVision project.
2+
// Copyright (c) 2024 AliceVision contributors.
3+
// This Source Code Form is subject to the terms of the Mozilla Public License,
4+
// v. 2.0. If a copy of the MPL was not distributed with this file,
5+
// You can obtain one at https://mozilla.org/MPL/2.0/.
6+
7+
%include <std_shared_ptr.i>
8+
%shared_ptr(aliceVision::camera::Equirectangular)
9+
10+
%include <aliceVision/camera/IntrinsicScaleOffsetDisto.i>
11+
%include <aliceVision/camera/Equirectangular.hpp>
12+
13+
%{
14+
#include <aliceVision/camera/Equirectangular.hpp>
15+
using namespace aliceVision;
16+
using namespace aliceVision::camera;
17+
%}

‎src/aliceVision/camera/camera.hpp‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,7 @@
2121
#include <aliceVision/camera/IntrinsicBase.hpp>
2222
#include <aliceVision/camera/Pinhole.hpp>
2323
#include <aliceVision/camera/Equidistant.hpp>
24+
#include <aliceVision/camera/Equirectangular.hpp>
2425
#include <aliceVision/camera/cameraUndistortImage.hpp>
2526

2627
#include <memory>
@@ -156,6 +157,11 @@ inline std::shared_ptr<IntrinsicBase> createIntrinsic(EINTRINSIC intrinsicType,
156157
return std::make_shared<Equidistant>(w, h, focalLengthPixX, offsetX, offsetY, distortion);
157158
}
158159

160+
if (isEquirectangular(intrinsicType))
161+
{
162+
return std::make_shared<Equirectangular>(w, h, focalLengthPixX, focalLengthPixY, offsetX, offsetY);
163+
}
164+
159165
return nullptr;
160166
}
161167

0 commit comments

Comments
 (0)