← Back to branch summary

~ubuntu-branches/ubuntu/raring/ceres-solver/raring

« back to all changes in this revision

Viewing changes to internal/ceres/parameter_block_test.cc

  • Committer: Package Import Robot
  • Author(s): Koichi Akabe
  • Date: 2012-06-04 07:15:43 UTC
  • Revision ID: package-import@ubuntu.com-20120604071543-zx6uthupvmtqn3k2
Tags: upstream-1.1.1
Import upstream version 1.1.1

Show diffs side-by-side

added added

removed removed

Lines of Context:
internal/ceres/parameter_block_test.cc
 
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
 
 
31
#include "ceres/parameter_block.h"
 
32
 
 
33
#include "gtest/gtest.h"
 
34
#include "ceres/internal/eigen.h"
 
35
 
 
36
namespace ceres {
 
37
namespace internal {
 
38
 
 
39
TEST(ParameterBlock, SetLocalParameterization) {
 
40
  double x[3] = { 1.0, 2.0, 3.0 };
 
41
  ParameterBlock parameter_block(x, 3);
 
42
 
 
43
  // The indices to set constant within the parameter block (used later).
 
44
  vector<int> indices;
 
45
  indices.push_back(1);
 
46
 
 
47
  // Can't set the parameterization if the sizes don't match.
 
48
  SubsetParameterization subset_wrong_size(4, indices);
 
49
  ASSERT_DEATH(parameter_block.SetParameterization(&subset_wrong_size),
 
50
               "global");
 
51
 
 
52
  // Can't set parameterization to NULL from NULL.
 
53
  ASSERT_DEATH(parameter_block.SetParameterization(NULL), "NULL");
 
54
 
 
55
  // Now set the parameterization.
 
56
  SubsetParameterization subset(3, indices);
 
57
  parameter_block.SetParameterization(&subset);
 
58
 
 
59
  // Re-setting the parameterization to the same value is supported.
 
60
  parameter_block.SetParameterization(&subset);
 
61
 
 
62
  // Can't set parameterization to NULL from another parameterization.
 
63
  ASSERT_DEATH(parameter_block.SetParameterization(NULL), "NULL");
 
64
 
 
65
  // Can't set the parameterization more than once.
 
66
  SubsetParameterization subset_different(3, indices);
 
67
  ASSERT_DEATH(parameter_block.SetParameterization(&subset_different),
 
68
               "re-set");
 
69
 
 
70
  // Ensure the local parameterization jacobian result is correctly computed.
 
71
  ConstMatrixRef local_parameterization_jacobian(
 
72
      parameter_block.LocalParameterizationJacobian(),
 
73
      3,
 
74
      2);
 
75
  ASSERT_EQ(1.0, local_parameterization_jacobian(0, 0));
 
76
  ASSERT_EQ(0.0, local_parameterization_jacobian(0, 1));
 
77
  ASSERT_EQ(0.0, local_parameterization_jacobian(1, 0));
 
78
  ASSERT_EQ(0.0, local_parameterization_jacobian(1, 1));
 
79
  ASSERT_EQ(0.0, local_parameterization_jacobian(2, 0));
 
80
  ASSERT_EQ(1.0, local_parameterization_jacobian(2, 1));
 
81
 
 
82
  // Check that updating works as expected.
 
83
  double x_plus_delta[3];
 
84
  double delta[2] = { 0.5, 0.3 };
 
85
  parameter_block.Plus(x, delta, x_plus_delta);
 
86
  ASSERT_EQ(1.5, x_plus_delta[0]);
 
87
  ASSERT_EQ(2.0, x_plus_delta[1]);
 
88
  ASSERT_EQ(3.3, x_plus_delta[2]);
 
89
}
 
90
 
 
91
struct TestParameterization : public LocalParameterization {
 
92
 public:
 
93
  virtual ~TestParameterization() {}
 
94
  virtual bool Plus(const double* x,
 
95
                    const double* delta,
 
96
                    double* x_plus_delta) const {
 
97
    LOG(FATAL) << "Shouldn't get called.";
 
98
    return true;
 
99
  }
 
100
  virtual bool ComputeJacobian(const double* x,
 
101
                               double* jacobian) const {
 
102
    jacobian[0] = *x * 2;
 
103
    return true;
 
104
  }
 
105
 
 
106
  virtual int GlobalSize() const { return 1; }
 
107
  virtual int LocalSize() const { return 1; }
 
108
};
 
109
 
 
110
TEST(ParameterBlock, SetStateUpdatesLocalParameterizationJacobian) {
 
111
  TestParameterization test_parameterization;
 
112
  double x[1] = { 1.0 };
 
113
  ParameterBlock parameter_block(x, 1, &test_parameterization);
 
114
 
 
115
  EXPECT_EQ(2.0, *parameter_block.LocalParameterizationJacobian());
 
116
 
 
117
  x[0] = 5.5;
 
118
  parameter_block.SetState(x);
 
119
  EXPECT_EQ(11.0, *parameter_block.LocalParameterizationJacobian());
 
120
}
 
121
 
 
122
TEST(ParameterBlock, PlusWithNoLocalParameterization) {
 
123
  double x[2] = { 1.0, 2.0 };
 
124
  ParameterBlock parameter_block(x, 2);
 
125
 
 
126
  double delta[2] = { 0.2, 0.3 };
 
127
  double x_plus_delta[2];
 
128
  parameter_block.Plus(x, delta, x_plus_delta);
 
129
  EXPECT_EQ(1.2, x_plus_delta[0]);
 
130
  EXPECT_EQ(2.3, x_plus_delta[1]);
 
131
}
 
132
 
 
133
// Stops computing the jacobian after the first time.
 
134
class BadLocalParameterization : public LocalParameterization {
 
135
 public:
 
136
  BadLocalParameterization()
 
137
      : calls_(0) {
 
138
  }
 
139
 
 
140
  virtual ~BadLocalParameterization() {}
 
141
  virtual bool Plus(const double* x,
 
142
                    const double* delta,
 
143
                    double* x_plus_delta) const {
 
144
    *x_plus_delta = *x + *delta;
 
145
    return true;
 
146
  }
 
147
 
 
148
  virtual bool ComputeJacobian(const double* x, double* jacobian) const {
 
149
    if (calls_ == 0) {
 
150
      jacobian[0] = 0;
 
151
    }
 
152
    ++calls_;
 
153
    return true;
 
154
  }
 
155
 
 
156
  virtual int GlobalSize() const { return 1;}
 
157
  virtual int LocalSize()  const { return 1;}
 
158
 
 
159
 private:
 
160
  mutable int calls_;
 
161
};
 
162
 
 
163
TEST(ParameterBlock, DetectBadLocalParameterization) {
 
164
  double x = 1;
 
165
  BadLocalParameterization bad_parameterization;
 
166
  ParameterBlock parameter_block(&x, 1, &bad_parameterization);
 
167
  double y = 2;
 
168
  EXPECT_FALSE(parameter_block.SetState(&y));
 
169
}
 
170
 
 
171
}  // namespace internal
 
172
}  // namespace ceres