ellipse_approximation.cc revision 79397c21138f54fcff6ec067b44b847f1f7e0e98
179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// Ceres Solver - A fast non-linear least squares minimizer 279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// Copyright 2014 Google Inc. All rights reserved. 379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// http://code.google.com/p/ceres-solver/ 479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// 579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// Redistribution and use in source and binary forms, with or without 679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// modification, are permitted provided that the following conditions are met: 779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// 879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// * Redistributions of source code must retain the above copyright notice, 979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// this list of conditions and the following disclaimer. 1079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// * Redistributions in binary form must reproduce the above copyright notice, 1179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// this list of conditions and the following disclaimer in the documentation 1279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// and/or other materials provided with the distribution. 1379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// * Neither the name of Google Inc. nor the names of its contributors may be 1479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// used to endorse or promote products derived from this software without 1579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// specific prior written permission. 1679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// 1779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 1879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 1979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 2079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 2179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 2279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 2379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 2479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 2579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 2679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 2779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// POSSIBILITY OF SUCH DAMAGE. 2879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// 2979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// Author: richie.stebbing@gmail.com (Richard Stebbing) 3079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// 3179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// This fits points randomly distributed on an ellipse with an approximate 3279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// line segment contour. This is done by jointly optimizing the control points 3379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// of the line segment contour along with the preimage positions for the data 3479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// points. The purpose of this example is to show an example use case for 3579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// dynamic_sparsity, and how it can benefit problems which are numerically 3679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// dense but dynamically sparse. 3779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 3879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez#include <cmath> 3979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez#include <vector> 4079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez#include "ceres/ceres.h" 4179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez#include "glog/logging.h" 4279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 4379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// Data generated with the following Python code. 4479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// import numpy as np 4579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// np.random.seed(1337) 4679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// t = np.linspace(0.0, 2.0 * np.pi, 212, endpoint=False) 4779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// t += 2.0 * np.pi * 0.01 * np.random.randn(t.size) 4879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// theta = np.deg2rad(15) 4979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// a, b = np.cos(theta), np.sin(theta) 5079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// R = np.array([[a, -b], 5179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// [b, a]]) 5279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// Y = np.dot(np.c_[4.0 * np.cos(t), np.sin(t)], R.T) 5379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 5479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezconst int kYRows = 212; 5579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezconst int kYCols = 2; 5679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezconst double kYData[kYRows * kYCols] = { 5779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +3.871364e+00, +9.916027e-01, 5879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +3.864003e+00, +1.034148e+00, 5979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +3.850651e+00, +1.072202e+00, 6079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +3.868350e+00, +1.014408e+00, 6179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +3.796381e+00, +1.153021e+00, 6279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +3.857138e+00, +1.056102e+00, 6379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +3.787532e+00, +1.162215e+00, 6479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +3.704477e+00, +1.227272e+00, 6579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +3.564711e+00, +1.294959e+00, 6679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +3.754363e+00, +1.191948e+00, 6779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +3.482098e+00, +1.322725e+00, 6879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +3.602777e+00, +1.279658e+00, 6979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +3.585433e+00, +1.286858e+00, 7079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +3.347505e+00, +1.356415e+00, 7179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +3.220855e+00, +1.378914e+00, 7279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +3.558808e+00, +1.297174e+00, 7379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +3.403618e+00, +1.343809e+00, 7479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +3.179828e+00, +1.384721e+00, 7579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +3.054789e+00, +1.398759e+00, 7679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +3.294153e+00, +1.366808e+00, 7779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +3.247312e+00, +1.374813e+00, 7879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +2.988547e+00, +1.404247e+00, 7979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +3.114508e+00, +1.392698e+00, 8079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +2.899226e+00, +1.409802e+00, 8179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +2.533256e+00, +1.414778e+00, 8279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +2.654773e+00, +1.415909e+00, 8379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +2.565100e+00, +1.415313e+00, 8479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +2.976456e+00, +1.405118e+00, 8579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +2.484200e+00, +1.413640e+00, 8679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +2.324751e+00, +1.407476e+00, 8779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +1.930468e+00, +1.378221e+00, 8879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +2.329017e+00, +1.407688e+00, 8979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +1.760640e+00, +1.360319e+00, 9079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +2.147375e+00, +1.396603e+00, 9179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +1.741989e+00, +1.358178e+00, 9279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +1.743859e+00, +1.358394e+00, 9379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +1.557372e+00, +1.335208e+00, 9479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +1.280551e+00, +1.295087e+00, 9579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +1.429880e+00, +1.317546e+00, 9679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +1.213485e+00, +1.284400e+00, 9779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +9.168172e-01, +1.232870e+00, 9879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +1.311141e+00, +1.299839e+00, 9979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +1.231969e+00, +1.287382e+00, 10079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +7.453773e-01, +1.200049e+00, 10179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +6.151587e-01, +1.173683e+00, 10279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +5.935666e-01, +1.169193e+00, 10379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +2.538707e-01, +1.094227e+00, 10479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +6.806136e-01, +1.187089e+00, 10579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +2.805447e-01, +1.100405e+00, 10679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +6.184807e-01, +1.174371e+00, 10779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +1.170550e-01, +1.061762e+00, 10879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +2.890507e-01, +1.102365e+00, 10979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +3.834234e-01, +1.123772e+00, 11079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +3.980161e-04, +1.033061e+00, 11179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -3.651680e-01, +9.370367e-01, 11279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -8.386351e-01, +7.987201e-01, 11379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -8.105704e-01, +8.073702e-01, 11479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -8.735139e-01, +7.878886e-01, 11579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -9.913836e-01, +7.506100e-01, 11679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -8.784011e-01, +7.863636e-01, 11779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -1.181440e+00, +6.882566e-01, 11879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -1.229556e+00, +6.720191e-01, 11979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -1.035839e+00, +7.362765e-01, 12079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -8.031520e-01, +8.096470e-01, 12179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -1.539136e+00, +5.629549e-01, 12279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -1.755423e+00, +4.817306e-01, 12379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -1.337589e+00, +6.348763e-01, 12479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -1.836966e+00, +4.499485e-01, 12579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -1.913367e+00, +4.195617e-01, 12679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -2.126467e+00, +3.314900e-01, 12779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -1.927625e+00, +4.138238e-01, 12879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -2.339862e+00, +2.379074e-01, 12979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -1.881736e+00, +4.322152e-01, 13079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -2.116753e+00, +3.356163e-01, 13179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -2.255733e+00, +2.754930e-01, 13279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -2.555834e+00, +1.368473e-01, 13379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -2.770277e+00, +2.895711e-02, 13479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -2.563376e+00, +1.331890e-01, 13579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -2.826715e+00, -9.000818e-04, 13679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -2.978191e+00, -8.457804e-02, 13779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -3.115855e+00, -1.658786e-01, 13879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -2.982049e+00, -8.678322e-02, 13979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -3.307892e+00, -2.902083e-01, 14079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -3.038346e+00, -1.194222e-01, 14179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -3.190057e+00, -2.122060e-01, 14279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -3.279086e+00, -2.705777e-01, 14379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -3.322028e+00, -2.999889e-01, 14479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -3.122576e+00, -1.699965e-01, 14579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -3.551973e+00, -4.768674e-01, 14679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -3.581866e+00, -5.032175e-01, 14779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -3.497799e+00, -4.315203e-01, 14879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -3.565384e+00, -4.885602e-01, 14979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -3.699493e+00, -6.199815e-01, 15079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -3.585166e+00, -5.061925e-01, 15179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -3.758914e+00, -6.918275e-01, 15279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -3.741104e+00, -6.689131e-01, 15379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -3.688331e+00, -6.077239e-01, 15479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -3.810425e+00, -7.689015e-01, 15579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -3.791829e+00, -7.386911e-01, 15679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -3.789951e+00, -7.358189e-01, 15779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -3.823100e+00, -7.918398e-01, 15879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -3.857021e+00, -8.727074e-01, 15979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -3.858250e+00, -8.767645e-01, 16079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -3.872100e+00, -9.563174e-01, 16179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -3.864397e+00, -1.032630e+00, 16279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -3.846230e+00, -1.081669e+00, 16379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -3.834799e+00, -1.102536e+00, 16479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -3.866684e+00, -1.022901e+00, 16579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -3.808643e+00, -1.139084e+00, 16679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -3.868840e+00, -1.011569e+00, 16779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -3.791071e+00, -1.158615e+00, 16879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -3.797999e+00, -1.151267e+00, 16979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -3.696278e+00, -1.232314e+00, 17079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -3.779007e+00, -1.170504e+00, 17179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -3.622855e+00, -1.270793e+00, 17279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -3.647249e+00, -1.259166e+00, 17379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -3.655412e+00, -1.255042e+00, 17479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -3.573218e+00, -1.291696e+00, 17579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -3.638019e+00, -1.263684e+00, 17679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -3.498409e+00, -1.317750e+00, 17779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -3.304143e+00, -1.364970e+00, 17879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -3.183001e+00, -1.384295e+00, 17979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -3.202456e+00, -1.381599e+00, 18079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -3.244063e+00, -1.375332e+00, 18179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -3.233308e+00, -1.377019e+00, 18279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -3.060112e+00, -1.398264e+00, 18379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -3.078187e+00, -1.396517e+00, 18479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -2.689594e+00, -1.415761e+00, 18579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -2.947662e+00, -1.407039e+00, 18679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -2.854490e+00, -1.411860e+00, 18779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -2.660499e+00, -1.415900e+00, 18879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -2.875955e+00, -1.410930e+00, 18979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -2.675385e+00, -1.415848e+00, 19079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -2.813155e+00, -1.413363e+00, 19179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -2.417673e+00, -1.411512e+00, 19279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -2.725461e+00, -1.415373e+00, 19379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -2.148334e+00, -1.396672e+00, 19479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -2.108972e+00, -1.393738e+00, 19579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -2.029905e+00, -1.387302e+00, 19679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -2.046214e+00, -1.388687e+00, 19779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -2.057402e+00, -1.389621e+00, 19879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -1.650250e+00, -1.347160e+00, 19979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -1.806764e+00, -1.365469e+00, 20079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -1.206973e+00, -1.283343e+00, 20179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -8.029259e-01, -1.211308e+00, 20279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -1.229551e+00, -1.286993e+00, 20379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -1.101507e+00, -1.265754e+00, 20479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -9.110645e-01, -1.231804e+00, 20579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -1.110046e+00, -1.267211e+00, 20679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -8.465274e-01, -1.219677e+00, 20779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -7.594163e-01, -1.202818e+00, 20879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -8.023823e-01, -1.211203e+00, 20979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -3.732519e-01, -1.121494e+00, 21079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -1.918373e-01, -1.079668e+00, 21179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -4.671988e-01, -1.142253e+00, 21279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -4.033645e-01, -1.128215e+00, 21379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -1.920740e-01, -1.079724e+00, 21479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -3.022157e-01, -1.105389e+00, 21579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez -1.652831e-01, -1.073354e+00, 21679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +4.671625e-01, -9.085886e-01, 21779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +5.940178e-01, -8.721832e-01, 21879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +3.147557e-01, -9.508290e-01, 21979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +6.383631e-01, -8.591867e-01, 22079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +9.888923e-01, -7.514088e-01, 22179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +7.076339e-01, -8.386023e-01, 22279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +1.326682e+00, -6.386698e-01, 22379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +1.149834e+00, -6.988221e-01, 22479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +1.257742e+00, -6.624207e-01, 22579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +1.492352e+00, -5.799632e-01, 22679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +1.595574e+00, -5.421766e-01, 22779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +1.240173e+00, -6.684113e-01, 22879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +1.706612e+00, -5.004442e-01, 22979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +1.873984e+00, -4.353002e-01, 23079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +1.985633e+00, -3.902561e-01, 23179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +1.722880e+00, -4.942329e-01, 23279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +2.095182e+00, -3.447402e-01, 23379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +2.018118e+00, -3.768991e-01, 23479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +2.422702e+00, -1.999563e-01, 23579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +2.370611e+00, -2.239326e-01, 23679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +2.152154e+00, -3.205250e-01, 23779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +2.525121e+00, -1.516499e-01, 23879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +2.422116e+00, -2.002280e-01, 23979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +2.842806e+00, +9.536372e-03, 24079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +3.030128e+00, +1.146027e-01, 24179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +2.888424e+00, +3.433444e-02, 24279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +2.991609e+00, +9.226409e-02, 24379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +2.924807e+00, +5.445844e-02, 24479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +3.007772e+00, +1.015875e-01, 24579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +2.781973e+00, -2.282382e-02, 24679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +3.164737e+00, +1.961781e-01, 24779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +3.237671e+00, +2.430139e-01, 24879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +3.046123e+00, +1.240014e-01, 24979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +3.414834e+00, +3.669060e-01, 25079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +3.436591e+00, +3.833600e-01, 25179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +3.626207e+00, +5.444311e-01, 25279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +3.223325e+00, +2.336361e-01, 25379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +3.511963e+00, +4.431060e-01, 25479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +3.698380e+00, +6.187442e-01, 25579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +3.670244e+00, +5.884943e-01, 25679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +3.558833e+00, +4.828230e-01, 25779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +3.661807e+00, +5.797689e-01, 25879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +3.767261e+00, +7.030893e-01, 25979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +3.801065e+00, +7.532650e-01, 26079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +3.828523e+00, +8.024454e-01, 26179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +3.840719e+00, +8.287032e-01, 26279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +3.848748e+00, +8.485921e-01, 26379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +3.865801e+00, +9.066551e-01, 26479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +3.870983e+00, +9.404873e-01, 26579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +3.870263e+00, +1.001884e+00, 26679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +3.864462e+00, +1.032374e+00, 26779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +3.870542e+00, +9.996121e-01, 26879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez +3.865424e+00, +1.028474e+00 26979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez}; 27079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezceres::ConstMatrixRef kY(kYData, kYRows, kYCols); 27179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 27279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezclass PointToLineSegmentContourCostFunction : public ceres::CostFunction { 27379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez public: 27479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez PointToLineSegmentContourCostFunction(const int num_segments, 27579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const Eigen::Vector2d y) 27679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez : num_segments_(num_segments), y_(y) { 27779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // The first parameter is the preimage position. 27879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez mutable_parameter_block_sizes()->push_back(1); 27979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // The next parameters are the control points for the line segment contour. 28079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int i = 0; i < num_segments_; ++i) { 28179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez mutable_parameter_block_sizes()->push_back(2); 28279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 28379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez set_num_residuals(2); 28479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 28579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 28679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez virtual bool Evaluate(const double* const* x, 28779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez double* residuals, 28879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez double** jacobians) const { 28979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Convert the preimage position `t` into a segment index `i0` and the 29079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // line segment interpolation parameter `u`. `i1` is the index of the next 29179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // control point. 29279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const double t = ModuloNumSegments(*x[0]); 29379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CHECK_GE(t, 0.0); 29479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CHECK_LT(t, num_segments_); 29579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const int i0 = floor(t), i1 = (i0 + 1) % num_segments_; 29679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const double u = t - i0; 29779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 29879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Linearly interpolate between control points `i0` and `i1`. 29979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez residuals[0] = y_[0] - ((1.0 - u) * x[1 + i0][0] + u * x[1 + i1][0]); 30079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez residuals[1] = y_[1] - ((1.0 - u) * x[1 + i0][1] + u * x[1 + i1][1]); 30179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 30279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (jacobians == NULL) { 30379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return true; 30479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 30579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 30679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (jacobians[0] != NULL) { 30779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez jacobians[0][0] = x[1 + i0][0] - x[1 + i1][0]; 30879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez jacobians[0][1] = x[1 + i0][1] - x[1 + i1][1]; 30979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 31079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int i = 0; i < num_segments_; ++i) { 31179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (jacobians[i + 1] != NULL) { 31279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ceres::MatrixRef(jacobians[i + 1], 2, 2).setZero(); 31379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (i == i0) { 31479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez jacobians[i + 1][0] = -(1.0 - u); 31579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez jacobians[i + 1][3] = -(1.0 - u); 31679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } else if (i == i1) { 31779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez jacobians[i + 1][0] = -u; 31879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez jacobians[i + 1][3] = -u; 31979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 32079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 32179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 32279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return true; 32379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 32479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 32579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez static ceres::CostFunction* Create(const int num_segments, 32679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const Eigen::Vector2d y) { 32779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return new PointToLineSegmentContourCostFunction(num_segments, y); 32879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 32979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 33079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez private: 33179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez inline double ModuloNumSegments(const double& t) const { 33279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return t - num_segments_ * floor(t / num_segments_); 33379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 33479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 33579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const int num_segments_; 33679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const Eigen::Vector2d y_; 33779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez}; 33879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 33979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezstruct EuclideanDistanceFunctor { 34079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez EuclideanDistanceFunctor(const double& sqrt_weight) 34179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez : sqrt_weight_(sqrt_weight) {} 34279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 34379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez template <typename T> 34479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez bool operator()(const T* x0, const T* x1, T* residuals) const { 34579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez residuals[0] = T(sqrt_weight_) * (x0[0] - x1[0]); 34679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez residuals[1] = T(sqrt_weight_) * (x0[1] - x1[1]); 34779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return true; 34879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 34979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 35079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez static ceres::CostFunction* Create(const double& sqrt_weight) { 35179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return new ceres::AutoDiffCostFunction<EuclideanDistanceFunctor, 2, 2, 2>( 35279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez new EuclideanDistanceFunctor(sqrt_weight)); 35379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 35479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 35579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez private: 35679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const double sqrt_weight_; 35779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez}; 35879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 35979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezbool SolveWithFullReport(ceres::Solver::Options options, 36079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ceres::Problem* problem, 36179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez bool dynamic_sparsity) { 36279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez options.dynamic_sparsity = dynamic_sparsity; 36379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 36479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ceres::Solver::Summary summary; 36579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ceres::Solve(options, problem, &summary); 36679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 36779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez std::cout << "####################" << std::endl; 36879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez std::cout << "dynamic_sparsity = " << dynamic_sparsity << std::endl; 36979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez std::cout << "####################" << std::endl; 37079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez std::cout << summary.FullReport() << std::endl; 37179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 37279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return summary.termination_type == ceres::CONVERGENCE; 37379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez} 37479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 37579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezint main(int argc, char** argv) { 37679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez google::InitGoogleLogging(argv[0]); 37779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 37879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Problem configuration. 37979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const int num_segments = 151; 38079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const double regularization_weight = 1e-2; 38179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 38279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Eigen::MatrixXd is column major so we define our own MatrixXd which is 38379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // row major. Eigen::VectorXd can be used directly. 38479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez typedef Eigen::Matrix<double, 38579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez Eigen::Dynamic, Eigen::Dynamic, 38679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez Eigen::RowMajor> MatrixXd; 38779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez using Eigen::VectorXd; 38879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 38979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // `X` is the matrix of control points which make up the contour of line 39079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // segments. The number of control points is equal to the number of line 39179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // segments because the contour is closed. 39279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // 39379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Initialize `X` to points on the unit circle. 39479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez VectorXd w(num_segments + 1); 39579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez w.setLinSpaced(num_segments + 1, 0.0, 2.0 * M_PI); 39679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez w.conservativeResize(num_segments); 39779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez MatrixXd X(num_segments, 2); 39879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez X.col(0) = w.array().cos(); 39979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez X.col(1) = w.array().sin(); 40079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 40179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Each data point has an associated preimage position on the line segment 40279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // contour. For each data point we initialize the preimage positions to 40379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // the index of the closest control point. 40479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const int num_observations = kY.rows(); 40579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez VectorXd t(num_observations); 40679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int i = 0; i < num_observations; ++i) { 40779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez (X.rowwise() - kY.row(i)).rowwise().squaredNorm().minCoeff(&t[i]); 40879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 40979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 41079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ceres::Problem problem; 41179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 41279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // For each data point add a residual which measures its distance to its 41379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // corresponding position on the line segment contour. 41479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez std::vector<double*> parameter_blocks(1 + num_segments); 41579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez parameter_blocks[0] = NULL; 41679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int i = 0; i < num_segments; ++i) { 41779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez parameter_blocks[i + 1] = X.data() + 2 * i; 41879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 41979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int i = 0; i < num_observations; ++i) { 42079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez parameter_blocks[0] = &t[i]; 42179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez problem.AddResidualBlock( 42279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez PointToLineSegmentContourCostFunction::Create(num_segments, kY.row(i)), 42379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez NULL, 42479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez parameter_blocks); 42579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 42679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 42779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Add regularization to minimize the length of the line segment contour. 42879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez for (int i = 0; i < num_segments; ++i) { 42979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez problem.AddResidualBlock( 43079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez EuclideanDistanceFunctor::Create(sqrt(regularization_weight)), 43179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez NULL, 43279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez X.data() + 2 * i, 43379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez X.data() + 2 * ((i + 1) % num_segments)); 43479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 43579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 43679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ceres::Solver::Options options; 43779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez options.max_num_iterations = 100; 43879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY; 43979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 44079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // First, solve `X` and `t` jointly with dynamic_sparsity = true. 44179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez MatrixXd X0 = X; 44279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez VectorXd t0 = t; 44379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CHECK(SolveWithFullReport(options, &problem, true)); 44479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 44579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Second, solve with dynamic_sparsity = false. 44679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez X = X0; 44779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez t = t0; 44879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CHECK(SolveWithFullReport(options, &problem, false)); 44979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 45079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return 0; 45179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez} 452