1 // Ceres Solver - A fast non-linear least squares minimizer
2 // Copyright 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: sameeragarwal@google.com (Sameer Agarwal)
30
31 #include "ceres/ordered_groups.h"
32
33 #include <cstddef>
34 #include <vector>
35 #include "gtest/gtest.h"
36 #include "ceres/collections_port.h"
37
38 namespace ceres {
39 namespace internal {
40
TEST(OrderedGroups,EmptyOrderedGroupBehavesCorrectly)41 TEST(OrderedGroups, EmptyOrderedGroupBehavesCorrectly) {
42 ParameterBlockOrdering ordering;
43 EXPECT_EQ(ordering.NumGroups(), 0);
44 EXPECT_EQ(ordering.NumElements(), 0);
45 EXPECT_EQ(ordering.GroupSize(1), 0);
46 double x;
47 EXPECT_EQ(ordering.GroupId(&x), -1);
48 EXPECT_FALSE(ordering.Remove(&x));
49 }
50
TEST(OrderedGroups,EverythingInOneGroup)51 TEST(OrderedGroups, EverythingInOneGroup) {
52 ParameterBlockOrdering ordering;
53 double x[3];
54 ordering.AddElementToGroup(x, 1);
55 ordering.AddElementToGroup(x + 1, 1);
56 ordering.AddElementToGroup(x + 2, 1);
57 ordering.AddElementToGroup(x, 1);
58
59 EXPECT_EQ(ordering.NumGroups(), 1);
60 EXPECT_EQ(ordering.NumElements(), 3);
61 EXPECT_EQ(ordering.GroupSize(1), 3);
62 EXPECT_EQ(ordering.GroupSize(0), 0);
63 EXPECT_EQ(ordering.GroupId(x), 1);
64 EXPECT_EQ(ordering.GroupId(x + 1), 1);
65 EXPECT_EQ(ordering.GroupId(x + 2), 1);
66
67 ordering.Remove(x);
68 EXPECT_EQ(ordering.NumGroups(), 1);
69 EXPECT_EQ(ordering.NumElements(), 2);
70 EXPECT_EQ(ordering.GroupSize(1), 2);
71 EXPECT_EQ(ordering.GroupSize(0), 0);
72
73 EXPECT_EQ(ordering.GroupId(x), -1);
74 EXPECT_EQ(ordering.GroupId(x + 1), 1);
75 EXPECT_EQ(ordering.GroupId(x + 2), 1);
76 }
77
TEST(OrderedGroups,StartInOneGroupAndThenSplit)78 TEST(OrderedGroups, StartInOneGroupAndThenSplit) {
79 ParameterBlockOrdering ordering;
80 double x[3];
81 ordering.AddElementToGroup(x, 1);
82 ordering.AddElementToGroup(x + 1, 1);
83 ordering.AddElementToGroup(x + 2, 1);
84 ordering.AddElementToGroup(x, 1);
85
86 EXPECT_EQ(ordering.NumGroups(), 1);
87 EXPECT_EQ(ordering.NumElements(), 3);
88 EXPECT_EQ(ordering.GroupSize(1), 3);
89 EXPECT_EQ(ordering.GroupSize(0), 0);
90 EXPECT_EQ(ordering.GroupId(x), 1);
91 EXPECT_EQ(ordering.GroupId(x + 1), 1);
92 EXPECT_EQ(ordering.GroupId(x + 2), 1);
93
94 ordering.AddElementToGroup(x, 5);
95 EXPECT_EQ(ordering.NumGroups(), 2);
96 EXPECT_EQ(ordering.NumElements(), 3);
97 EXPECT_EQ(ordering.GroupSize(1), 2);
98 EXPECT_EQ(ordering.GroupSize(5), 1);
99 EXPECT_EQ(ordering.GroupSize(0), 0);
100
101 EXPECT_EQ(ordering.GroupId(x), 5);
102 EXPECT_EQ(ordering.GroupId(x + 1), 1);
103 EXPECT_EQ(ordering.GroupId(x + 2), 1);
104 }
105
TEST(OrderedGroups,AddAndRemoveEveryThingFromOneGroup)106 TEST(OrderedGroups, AddAndRemoveEveryThingFromOneGroup) {
107 ParameterBlockOrdering ordering;
108 double x[3];
109 ordering.AddElementToGroup(x, 1);
110 ordering.AddElementToGroup(x + 1, 1);
111 ordering.AddElementToGroup(x + 2, 1);
112 ordering.AddElementToGroup(x, 1);
113
114 EXPECT_EQ(ordering.NumGroups(), 1);
115 EXPECT_EQ(ordering.NumElements(), 3);
116 EXPECT_EQ(ordering.GroupSize(1), 3);
117 EXPECT_EQ(ordering.GroupSize(0), 0);
118 EXPECT_EQ(ordering.GroupId(x), 1);
119 EXPECT_EQ(ordering.GroupId(x + 1), 1);
120 EXPECT_EQ(ordering.GroupId(x + 2), 1);
121
122 ordering.AddElementToGroup(x, 5);
123 ordering.AddElementToGroup(x + 1, 5);
124 ordering.AddElementToGroup(x + 2, 5);
125 EXPECT_EQ(ordering.NumGroups(), 1);
126 EXPECT_EQ(ordering.NumElements(), 3);
127 EXPECT_EQ(ordering.GroupSize(1), 0);
128 EXPECT_EQ(ordering.GroupSize(5), 3);
129 EXPECT_EQ(ordering.GroupSize(0), 0);
130
131 EXPECT_EQ(ordering.GroupId(x), 5);
132 EXPECT_EQ(ordering.GroupId(x + 1), 5);
133 EXPECT_EQ(ordering.GroupId(x + 2), 5);
134 }
135
TEST(OrderedGroups,ReverseOrdering)136 TEST(OrderedGroups, ReverseOrdering) {
137 ParameterBlockOrdering ordering;
138 double x[3];
139 ordering.AddElementToGroup(x, 1);
140 ordering.AddElementToGroup(x + 1, 2);
141 ordering.AddElementToGroup(x + 2, 2);
142
143 EXPECT_EQ(ordering.NumGroups(), 2);
144 EXPECT_EQ(ordering.NumElements(), 3);
145 EXPECT_EQ(ordering.GroupSize(1), 1);
146 EXPECT_EQ(ordering.GroupSize(2), 2);
147 EXPECT_EQ(ordering.GroupId(x), 1);
148 EXPECT_EQ(ordering.GroupId(x + 1), 2);
149 EXPECT_EQ(ordering.GroupId(x + 2), 2);
150
151 ordering.Reverse();
152
153 EXPECT_EQ(ordering.NumGroups(), 2);
154 EXPECT_EQ(ordering.NumElements(), 3);
155 EXPECT_EQ(ordering.GroupSize(3), 1);
156 EXPECT_EQ(ordering.GroupSize(2), 2);
157 EXPECT_EQ(ordering.GroupId(x), 3);
158 EXPECT_EQ(ordering.GroupId(x + 1), 2);
159 EXPECT_EQ(ordering.GroupId(x + 2), 2);
160 }
161
TEST(OrderedGroups,BulkRemove)162 TEST(OrderedGroups, BulkRemove) {
163 ParameterBlockOrdering ordering;
164 double x[3];
165 ordering.AddElementToGroup(x, 1);
166 ordering.AddElementToGroup(x + 1, 2);
167 ordering.AddElementToGroup(x + 2, 2);
168
169 vector<double*> elements_to_remove;
170 elements_to_remove.push_back(x);
171 elements_to_remove.push_back(x + 2);
172
173 EXPECT_EQ(ordering.Remove(elements_to_remove), 2);
174 EXPECT_EQ(ordering.NumElements(), 1);
175 EXPECT_EQ(ordering.GroupId(x), -1);
176 EXPECT_EQ(ordering.GroupId(x + 1), 2);
177 EXPECT_EQ(ordering.GroupId(x + 2), -1);
178 }
179
TEST(OrderedGroups,BulkRemoveWithNoElements)180 TEST(OrderedGroups, BulkRemoveWithNoElements) {
181 ParameterBlockOrdering ordering;
182
183 double x[3];
184 vector<double*> elements_to_remove;
185 elements_to_remove.push_back(x);
186 elements_to_remove.push_back(x + 2);
187
188 EXPECT_EQ(ordering.Remove(elements_to_remove), 0);
189
190 ordering.AddElementToGroup(x, 1);
191 ordering.AddElementToGroup(x + 1, 2);
192 ordering.AddElementToGroup(x + 2, 2);
193
194 elements_to_remove.clear();
195 EXPECT_EQ(ordering.Remove(elements_to_remove), 0);
196 }
197
TEST(OrderedGroups,MinNonZeroGroup)198 TEST(OrderedGroups, MinNonZeroGroup) {
199 ParameterBlockOrdering ordering;
200 double x[3];
201
202 ordering.AddElementToGroup(x, 1);
203 ordering.AddElementToGroup(x + 1, 1);
204 ordering.AddElementToGroup(x + 2, 2);
205
206 EXPECT_EQ(ordering.MinNonZeroGroup(), 1);
207 ordering.Remove(x);
208
209 EXPECT_EQ(ordering.MinNonZeroGroup(), 1);
210 ordering.Remove(x + 1);
211
212 EXPECT_EQ(ordering.MinNonZeroGroup(), 2);
213 ordering.Remove(x + 2);
214
215 // No non-zero groups left.
216 EXPECT_DEATH_IF_SUPPORTED(ordering.MinNonZeroGroup(), "NumGroups()");
217 }
218 } // namespace internal
219 } // namespace ceres
220