gtsam
3rdparty
ceres
example.h
Go to the documentation of this file.
1
// Ceres Solver - A fast non-linear least squares minimizer
2
// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
3
// http://code.google.com/p/ceres-solver/
4
//
5
// Redistribution and use in source and binary forms, with or without
6
// modification, are permitted provided that the following conditions are met:
7
//
8
// * Redistributions of source code must retain the above copyright notice,
9
// this list of conditions and the following disclaimer.
10
// * Redistributions in binary form must reproduce the above copyright notice,
11
// this list of conditions and the following disclaimer in the documentation
12
// and/or other materials provided with the distribution.
13
// * Neither the name of Google Inc. nor the names of its contributors may be
14
// used to endorse or promote products derived from this software without
15
// specific prior written permission.
16
//
17
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27
// POSSIBILITY OF SUCH DAMAGE.
28
//
29
// Author: keir@google.com (Keir Mierle)
30
// sameeragarwal@google.com (Sameer Agarwal)
31
//
32
// Some Ceres Snippets copied for testing
33
34
#pragma once
35
36
#include <
gtsam/3rdparty/ceres/rotation.h
>
37
38
// Templated pinhole camera model for used with Ceres. The camera is
39
// parameterized using 9 parameters: 3 for rotation, 3 for translation, 1 for
40
// focal length and 2 for radial distortion. The principal point is not modeled
41
// (i.e. it is assumed be located at the image center).
42
struct
SnavelyProjection
{
43
44
template
<
typename
T>
45
bool
operator()
(
const
T
*
const
camera
,
const
T
*
const
point
,
46
T
* predicted)
const
{
47
// camera[0,1,2] are the angle-axis rotation.
48
T
p
[3];
49
ceres::AngleAxisRotatePoint
(
camera
,
point
,
p
);
50
51
// camera[3,4,5] are the translation.
52
p
[0] +=
camera
[3];
53
p
[1] +=
camera
[4];
54
p
[2] +=
camera
[5];
55
56
// Compute the center of distortion. The sign change comes from
57
// the camera model that Noah Snavely's Bundler assumes, whereby
58
// the camera coordinate system has a negative z axis.
59
T
xp = -
p
[0] /
p
[2];
60
T
yp = -
p
[1] /
p
[2];
61
62
// Apply second and fourth order radial distortion.
63
const
T
&
l1
=
camera
[7];
64
const
T
&
l2
=
camera
[8];
65
T
r2
= xp * xp + yp * yp;
66
T
distortion =
T
(1.0) +
r2
* (
l1
+
l2
*
r2
);
67
68
// Compute final projected point position.
69
const
T
& focal =
camera
[6];
70
predicted[0] = focal * distortion * xp;
71
predicted[1] = focal * distortion * yp;
72
73
return
true
;
74
}
75
76
};
77
78
l2
gtsam::Key l2
Definition:
testLinearContainerFactor.cpp:24
r2
static const double r2
Definition:
testSmartRangeFactor.cpp:32
rotation.h
SnavelyProjection
Definition:
example.h:42
T
Eigen::Triplet< double > T
Definition:
Tutorial_sparse_example.cpp:6
SnavelyProjection::operator()
bool operator()(const T *const camera, const T *const point, T *predicted) const
Definition:
example.h:45
gtsam_unstable.tests.test_ProjectionFactorRollingShutter.point
point
Definition:
test_ProjectionFactorRollingShutter.py:25
ceres::AngleAxisRotatePoint
void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3])
Definition:
rotation.h:584
Eigen::Triplet< double >
l1
gtsam::Key l1
Definition:
testLinearContainerFactor.cpp:24
p
float * p
Definition:
Tutorial_Map_using.cpp:9
camera
static const CalibratedCamera camera(kDefaultPose)
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:30