ACloudViewer  3.9.4
A Modern Library for 3D Data Processing
cost_functions_test.cc
Go to the documentation of this file.
1 // Copyright (c) 2018, ETH Zurich and UNC Chapel Hill.
2 // All rights reserved.
3 //
4 // Redistribution and use in source and binary forms, with or without
5 // modification, are permitted provided that the following conditions are met:
6 //
7 // * Redistributions of source code must retain the above copyright
8 // notice, this list of conditions and the following disclaimer.
9 //
10 // * Redistributions in binary form must reproduce the above copyright
11 // notice, this list of conditions and the following disclaimer in the
12 // documentation and/or other materials provided with the distribution.
13 //
14 // * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
15 // its contributors may be used to endorse or promote products derived
16 // from this software without specific prior written permission.
17 //
18 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
22 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
23 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
24 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
25 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
26 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
27 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
28 // POSSIBILITY OF SUCH DAMAGE.
29 //
30 // Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
31 
32 #define TEST_NAME "base/cost_functions"
33 #include "util/testing.h"
34 
35 #include "base/camera_models.h"
36 #include "base/cost_functions.h"
37 #include "base/pose.h"
38 
39 using namespace colmap;
40 
41 BOOST_AUTO_TEST_CASE(TestBundleAdjustmentCostFunction) {
42  ceres::CostFunction* cost_function =
43  BundleAdjustmentCostFunction<SimplePinholeCameraModel>::Create(
44  Eigen::Vector2d::Zero());
45  double qvec[4] = {1, 0, 0, 0};
46  double tvec[3] = {0, 0, 0};
47  double point3D[3] = {0, 0, 1};
48  double camera_params[3] = {1, 0, 0};
49  double residuals[2];
50  const double* parameters[4] = {qvec, tvec, point3D, camera_params};
51  BOOST_CHECK(cost_function->Evaluate(parameters, residuals, nullptr));
52  BOOST_CHECK_EQUAL(residuals[0], 0);
53  BOOST_CHECK_EQUAL(residuals[1], 0);
54 
55  point3D[1] = 1;
56  BOOST_CHECK(cost_function->Evaluate(parameters, residuals, nullptr));
57  BOOST_CHECK_EQUAL(residuals[0], 0);
58  BOOST_CHECK_EQUAL(residuals[1], 1);
59 
60  camera_params[0] = 2;
61  BOOST_CHECK(cost_function->Evaluate(parameters, residuals, nullptr));
62  BOOST_CHECK_EQUAL(residuals[0], 0);
63  BOOST_CHECK_EQUAL(residuals[1], 2);
64 
65  point3D[0] = -1;
66  BOOST_CHECK(cost_function->Evaluate(parameters, residuals, nullptr));
67  BOOST_CHECK_EQUAL(residuals[0], -2);
68  BOOST_CHECK_EQUAL(residuals[1], 2);
69 }
70 
71 BOOST_AUTO_TEST_CASE(TestBundleAdjustmentConstantPoseCostFunction) {
72  ceres::CostFunction* cost_function = BundleAdjustmentConstantPoseCostFunction<
73  SimplePinholeCameraModel>::Create(ComposeIdentityQuaternion(),
74  Eigen::Vector3d::Zero(),
75  Eigen::Vector2d::Zero());
76  double point3D[3] = {0, 0, 1};
77  double camera_params[3] = {1, 0, 0};
78  double residuals[2];
79  const double* parameters[2] = {point3D, camera_params};
80  BOOST_CHECK(cost_function->Evaluate(parameters, residuals, nullptr));
81  BOOST_CHECK_EQUAL(residuals[0], 0);
82  BOOST_CHECK_EQUAL(residuals[1], 0);
83 
84  point3D[1] = 1;
85  BOOST_CHECK(cost_function->Evaluate(parameters, residuals, nullptr));
86  BOOST_CHECK_EQUAL(residuals[0], 0);
87  BOOST_CHECK_EQUAL(residuals[1], 1);
88 
89  camera_params[0] = 2;
90  BOOST_CHECK(cost_function->Evaluate(parameters, residuals, nullptr));
91  BOOST_CHECK_EQUAL(residuals[0], 0);
92  BOOST_CHECK_EQUAL(residuals[1], 2);
93 
94  point3D[0] = -1;
95  BOOST_CHECK(cost_function->Evaluate(parameters, residuals, nullptr));
96  BOOST_CHECK_EQUAL(residuals[0], -2);
97  BOOST_CHECK_EQUAL(residuals[1], 2);
98 }
99 
100 BOOST_AUTO_TEST_CASE(TestRigBundleAdjustmentCostFunction) {
101  ceres::CostFunction* cost_function =
102  RigBundleAdjustmentCostFunction<SimplePinholeCameraModel>::Create(
103  Eigen::Vector2d::Zero());
104  double rig_qvec[4] = {1, 0, 0, 0};
105  double rig_tvec[3] = {0, 0, -1};
106  double rel_qvec[4] = {1, 0, 0, 0};
107  double rel_tvec[3] = {0, 0, 1};
108  double point3D[3] = {0, 0, 1};
109  double camera_params[3] = {1, 0, 0};
110  double residuals[2];
111  const double* parameters[6] = {rig_qvec, rig_tvec, rel_qvec,
112  rel_tvec, point3D, camera_params};
113  BOOST_CHECK(cost_function->Evaluate(parameters, residuals, nullptr));
114  BOOST_CHECK_EQUAL(residuals[0], 0);
115  BOOST_CHECK_EQUAL(residuals[1], 0);
116 
117  point3D[1] = 1;
118  BOOST_CHECK(cost_function->Evaluate(parameters, residuals, nullptr));
119  BOOST_CHECK_EQUAL(residuals[0], 0);
120  BOOST_CHECK_EQUAL(residuals[1], 1);
121 
122  camera_params[0] = 2;
123  BOOST_CHECK(cost_function->Evaluate(parameters, residuals, nullptr));
124  BOOST_CHECK_EQUAL(residuals[0], 0);
125  BOOST_CHECK_EQUAL(residuals[1], 2);
126 
127  point3D[0] = -1;
128  BOOST_CHECK(cost_function->Evaluate(parameters, residuals, nullptr));
129  BOOST_CHECK_EQUAL(residuals[0], -2);
130  BOOST_CHECK_EQUAL(residuals[1], 2);
131 }
132 
133 BOOST_AUTO_TEST_CASE(TestRelativePoseCostFunction) {
134  ceres::CostFunction* cost_function = RelativePoseCostFunction::Create(
135  Eigen::Vector2d(0, 0), Eigen::Vector2d(0, 0));
136  double qvec[4] = {1, 0, 0, 0};
137  double tvec[3] = {0, 1, 0};
138  double residuals[1];
139  const double* parameters[2] = {qvec, tvec};
140  BOOST_CHECK(cost_function->Evaluate(parameters, residuals, nullptr));
141  BOOST_CHECK_EQUAL(residuals[0], 0);
142 
143  cost_function = RelativePoseCostFunction::Create(Eigen::Vector2d(0, 0),
144  Eigen::Vector2d(1, 0));
145  BOOST_CHECK(cost_function->Evaluate(parameters, residuals, nullptr));
146  BOOST_CHECK_EQUAL(residuals[0], 0.5);
147 
148  cost_function = RelativePoseCostFunction::Create(Eigen::Vector2d(0, 0),
149  Eigen::Vector2d(1, 1));
150  BOOST_CHECK(cost_function->Evaluate(parameters, residuals, nullptr));
151  BOOST_CHECK_EQUAL(residuals[0], 0.5);
152 }
camera_models.h
colmap::BundleAdjustmentCostFunction::Create
static ceres::CostFunction * Create(const Eigen::Vector2d &point2D)
Definition: cost_functions.h:25
colmap::RelativePoseCostFunction::Create
static ceres::CostFunction * Create(const Eigen::Vector2d &x1, const Eigen::Vector2d &x2)
Definition: cost_functions.h:211
colmap::RigBundleAdjustmentCostFunction::Create
static ceres::CostFunction * Create(const Eigen::Vector2d &point2D)
Definition: cost_functions.h:145
cost_functions.h
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(TestBundleAdjustmentCostFunction)
Definition: cost_functions_test.cc:41
colmap::ComposeIdentityQuaternion
Eigen::Vector4d ComposeIdentityQuaternion()
Definition: pose.h:209