10ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Ceres Solver - A fast non-linear least squares minimizer 20ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. 30ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// http://code.google.com/p/ceres-solver/ 40ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 50ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Redistribution and use in source and binary forms, with or without 60ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// modification, are permitted provided that the following conditions are met: 70ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 80ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Redistributions of source code must retain the above copyright notice, 90ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// this list of conditions and the following disclaimer. 100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Redistributions in binary form must reproduce the above copyright notice, 110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// this list of conditions and the following disclaimer in the documentation 120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// and/or other materials provided with the distribution. 130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Neither the name of Google Inc. nor the names of its contributors may be 140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// used to endorse or promote products derived from this software without 150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// specific prior written permission. 160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// POSSIBILITY OF SUCH DAMAGE. 280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Author: rennie@google.com (Jeffrey Rennie) 300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Author: sanjay@google.com (Sanjay Ghemawat) -- renamed to FixedArray 310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#ifndef CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_ 330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#define CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_ 340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <cstddef> 360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "Eigen/Core" 370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/internal/macros.h" 380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/internal/manual_constructor.h" 391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "glog/logging.h" 400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace ceres { 420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace internal { 430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// A FixedArray<T> represents a non-resizable array of T where the 450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// length of the array does not need to be a compile time constant. 460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// FixedArray allocates small arrays inline, and large arrays on 480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// the heap. It is a good replacement for non-standard and deprecated 490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// uses of alloca() and variable length arrays (a GCC extension). 500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// FixedArray keeps performance fast for small arrays, because it 520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// avoids heap operations. It also helps reduce the chances of 530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// accidentally overflowing your stack if large input is passed to 540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// your function. 550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Also, FixedArray is useful for writing portable code. Not all 570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// compilers support arrays of dynamic size. 580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Most users should not specify an inline_elements argument and let 600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// FixedArray<> automatically determine the number of elements 610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// to store inline based on sizeof(T). 620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// If inline_elements is specified, the FixedArray<> implementation 640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// will store arrays of length <= inline_elements inline. 650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Finally note that unlike vector<T> FixedArray<T> will not zero-initialize 670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// simple types like int, double, bool, etc. 680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Non-POD types will be default-initialized just like regular vectors or 700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// arrays. 710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#if defined(_WIN64) 730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong typedef __int64 ssize_t; 740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#elif defined(_WIN32) 750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong typedef __int32 ssize_t; 760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#endif 770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate <typename T, ssize_t inline_elements = -1> 790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongclass FixedArray { 800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong public: 810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // For playing nicely with stl: 820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong typedef T value_type; 830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong typedef T* iterator; 840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong typedef T const* const_iterator; 850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong typedef T& reference; 860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong typedef T const& const_reference; 870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong typedef T* pointer; 880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong typedef std::ptrdiff_t difference_type; 890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong typedef size_t size_type; 900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // REQUIRES: n >= 0 920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Creates an array object that can store "n" elements. 930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // FixedArray<T> will not zero-initialiaze POD (simple) types like int, 950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // double, bool, etc. 960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Non-POD types will be default-initialized just like regular vectors or 970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // arrays. 980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong explicit FixedArray(size_type n); 990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Releases any resources. 1010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ~FixedArray(); 1020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Returns the length of the array. 1040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong inline size_type size() const { return size_; } 1050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Returns the memory size of the array in bytes. 1070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong inline size_t memsize() const { return size_ * sizeof(T); } 1080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Returns a pointer to the underlying element array. 1100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong inline const T* get() const { return &array_[0].element; } 1110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong inline T* get() { return &array_[0].element; } 1120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // REQUIRES: 0 <= i < size() 1140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Returns a reference to the "i"th element. 1150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong inline T& operator[](size_type i) { 1160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong DCHECK_LT(i, size_); 1170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return array_[i].element; 1180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // REQUIRES: 0 <= i < size() 1210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Returns a reference to the "i"th element. 1220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong inline const T& operator[](size_type i) const { 1230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong DCHECK_LT(i, size_); 1240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return array_[i].element; 1250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong inline iterator begin() { return &array_[0].element; } 1280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong inline iterator end() { return &array_[size_].element; } 1290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong inline const_iterator begin() const { return &array_[0].element; } 1310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong inline const_iterator end() const { return &array_[size_].element; } 1320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong private: 1340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Container to hold elements of type T. This is necessary to handle 1350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // the case where T is a a (C-style) array. The size of InnerContainer 1360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // and T must be the same, otherwise callers' assumptions about use 1370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // of this code will be broken. 1380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong struct InnerContainer { 1390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong T element; 1400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong }; 1410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // How many elements should we store inline? 1430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // a. If not specified, use a default of 256 bytes (256 bytes 1440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // seems small enough to not cause stack overflow or unnecessary 1450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // stack pollution, while still allowing stack allocation for 1460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // reasonably long character arrays. 1470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // b. Never use 0 length arrays (not ISO C++) 1480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong static const size_type S1 = ((inline_elements < 0) 1490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ? (256/sizeof(T)) : inline_elements); 1500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong static const size_type S2 = (S1 <= 0) ? 1 : S1; 1510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong static const size_type kInlineElements = S2; 1520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong size_type const size_; 1540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong InnerContainer* const array_; 1550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Allocate some space, not an array of elements of type T, so that we can 1570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // skip calling the T constructors and destructors for space we never use. 1580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ManualConstructor<InnerContainer> inline_space_[kInlineElements]; 1590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong}; 1600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Implementation details follow 1620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate <class T, ssize_t S> 1640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Konginline FixedArray<T, S>::FixedArray(typename FixedArray<T, S>::size_type n) 1650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong : size_(n), 1660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong array_((n <= kInlineElements 1670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ? reinterpret_cast<InnerContainer*>(inline_space_) 1680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong : new InnerContainer[n])) { 1690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Construct only the elements actually used. 1700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (array_ == reinterpret_cast<InnerContainer*>(inline_space_)) { 1710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (size_t i = 0; i != size_; ++i) { 1720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong inline_space_[i].Init(); 1730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate <class T, ssize_t S> 1780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Konginline FixedArray<T, S>::~FixedArray() { 1790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (array_ != reinterpret_cast<InnerContainer*>(inline_space_)) { 1800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong delete[] array_; 1810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } else { 1820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (size_t i = 0; i != size_; ++i) { 1830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong inline_space_[i].Destroy(); 1840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace internal 1890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace ceres 1900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#endif // CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_ 192