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
38namespace ceres {
39namespace internal {
40
41TEST(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
51TEST(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
78TEST(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
106TEST(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
136TEST(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
162TEST(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
180TEST(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
198TEST(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