Isolated Snavely example
							parent
							
								
									439f51ec7f
								
							
						
					
					
						commit
						516bb4b0b1
					
				|  | @ -0,0 +1,78 @@ | |||
| // Ceres Solver - A fast non-linear least squares minimizer
 | ||||
| // Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
 | ||||
| // http://code.google.com/p/ceres-solver/
 | ||||
| //
 | ||||
| // Redistribution and use in source and binary forms, with or without
 | ||||
| // modification, are permitted provided that the following conditions are met:
 | ||||
| //
 | ||||
| // * Redistributions of source code must retain the above copyright notice,
 | ||||
| //   this list of conditions and the following disclaimer.
 | ||||
| // * Redistributions in binary form must reproduce the above copyright notice,
 | ||||
| //   this list of conditions and the following disclaimer in the documentation
 | ||||
| //   and/or other materials provided with the distribution.
 | ||||
| // * Neither the name of Google Inc. nor the names of its contributors may be
 | ||||
| //   used to endorse or promote products derived from this software without
 | ||||
| //   specific prior written permission.
 | ||||
| //
 | ||||
| // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 | ||||
| // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 | ||||
| // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 | ||||
| // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 | ||||
| // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 | ||||
| // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 | ||||
| // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 | ||||
| // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 | ||||
| // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 | ||||
| // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 | ||||
| // POSSIBILITY OF SUCH DAMAGE.
 | ||||
| //
 | ||||
| // Author: keir@google.com (Keir Mierle)
 | ||||
| //         sameeragarwal@google.com (Sameer Agarwal)
 | ||||
| //
 | ||||
| // Some Ceres Snippets copied for testing
 | ||||
| 
 | ||||
| #pragma once | ||||
| 
 | ||||
| #include <gtsam_unstable/nonlinear/ceres_rotation.h> | ||||
| 
 | ||||
| // Templated pinhole camera model for used with Ceres. The camera is
 | ||||
| // parameterized using 9 parameters: 3 for rotation, 3 for translation, 1 for
 | ||||
| // focal length and 2 for radial distortion. The principal point is not modeled
 | ||||
| // (i.e. it is assumed be located at the image center).
 | ||||
| struct SnavelyProjection { | ||||
| 
 | ||||
|   template<typename T> | ||||
|   bool operator()(const T* const camera, const T* const point, | ||||
|       T* predicted) const { | ||||
|     // camera[0,1,2] are the angle-axis rotation.
 | ||||
|     T p[3]; | ||||
|     ceres::AngleAxisRotatePoint(camera, point, p); | ||||
| 
 | ||||
|     // camera[3,4,5] are the translation.
 | ||||
|     p[0] += camera[3]; | ||||
|     p[1] += camera[4]; | ||||
|     p[2] += camera[5]; | ||||
| 
 | ||||
|     // Compute the center of distortion. The sign change comes from
 | ||||
|     // the camera model that Noah Snavely's Bundler assumes, whereby
 | ||||
|     // the camera coordinate system has a negative z axis.
 | ||||
|     T xp = -p[0] / p[2]; | ||||
|     T yp = -p[1] / p[2]; | ||||
| 
 | ||||
|     // Apply second and fourth order radial distortion.
 | ||||
|     const T& l1 = camera[7]; | ||||
|     const T& l2 = camera[8]; | ||||
|     T r2 = xp * xp + yp * yp; | ||||
|     T distortion = T(1.0) + r2 * (l1 + l2 * r2); | ||||
| 
 | ||||
|     // Compute final projected point position.
 | ||||
|     const T& focal = camera[6]; | ||||
|     predicted[0] = focal * distortion * xp; | ||||
|     predicted[1] = focal * distortion * yp; | ||||
| 
 | ||||
|     return true; | ||||
|   } | ||||
| 
 | ||||
| }; | ||||
| 
 | ||||
| 
 | ||||
|  | @ -7,7 +7,7 @@ | |||
| 
 | ||||
|  * See LICENSE for the license information | ||||
| 
 | ||||
|  * -------------------------------1------------------------------------------- */ | ||||
|  * -------------------------------------------------------------------------- */ | ||||
| 
 | ||||
| /**
 | ||||
|  * @file testExpression.cpp | ||||
|  | @ -27,7 +27,7 @@ | |||
| #include <gtsam/base/LieScalar.h> | ||||
| 
 | ||||
| #include <gtsam_unstable/nonlinear/ceres_autodiff.h> | ||||
| #include <gtsam_unstable/nonlinear/ceres_rotation.h> | ||||
| #include <gtsam_unstable/nonlinear/ceres_example.h> | ||||
| 
 | ||||
| #undef CHECK | ||||
| #include <CppUnitLite/TestHarness.h> | ||||
|  | @ -87,55 +87,6 @@ struct Projective { | |||
|   } | ||||
| }; | ||||
| 
 | ||||
| // Templated pinhole camera model for used with Ceres. The camera is
 | ||||
| // parameterized using 9 parameters: 3 for rotation, 3 for translation, 1 for
 | ||||
| // focal length and 2 for radial distortion. The principal point is not modeled
 | ||||
| // (i.e. it is assumed be located at the image center).
 | ||||
| struct SnavelyProjection { | ||||
| 
 | ||||
|   template<typename T> | ||||
|   bool operator()(const T* const camera, const T* const point, | ||||
|       T* predicted) const { | ||||
|     // camera[0,1,2] are the angle-axis rotation.
 | ||||
|     T p[3]; | ||||
|     ceres::AngleAxisRotatePoint(camera, point, p); | ||||
| 
 | ||||
|     // camera[3,4,5] are the translation.
 | ||||
|     p[0] += camera[3]; | ||||
|     p[1] += camera[4]; | ||||
|     p[2] += camera[5]; | ||||
| 
 | ||||
|     // Compute the center of distortion. The sign change comes from
 | ||||
|     // the camera model that Noah Snavely's Bundler assumes, whereby
 | ||||
|     // the camera coordinate system has a negative z axis.
 | ||||
|     T xp = -p[0] / p[2]; | ||||
|     T yp = -p[1] / p[2]; | ||||
| 
 | ||||
|     // Apply second and fourth order radial distortion.
 | ||||
|     const T& l1 = camera[7]; | ||||
|     const T& l2 = camera[8]; | ||||
|     T r2 = xp * xp + yp * yp; | ||||
|     T distortion = T(1.0) + r2 * (l1 + l2 * r2); | ||||
| 
 | ||||
|     // Compute final projected point position.
 | ||||
|     const T& focal = camera[6]; | ||||
|     predicted[0] = focal * distortion * xp; | ||||
|     predicted[1] = focal * distortion * yp; | ||||
| 
 | ||||
|     return true; | ||||
|   } | ||||
| 
 | ||||
|   // Adapt to GTSAM types
 | ||||
|   Vector2 operator()(const Vector9& P, const Vector3& X) const { | ||||
|     Vector2 x; | ||||
|     if (operator()(P.data(), X.data(), x.data())) | ||||
|       return x; | ||||
|     else | ||||
|       throw std::runtime_error("Snavely fail"); | ||||
|   } | ||||
| 
 | ||||
| }; | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| // Test Ceres AutoDiff
 | ||||
| TEST(Expression, AutoDiff) { | ||||
|  | @ -171,7 +122,17 @@ TEST(Expression, AutoDiff) { | |||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| // Test Ceres AutoDiff on Snavely
 | ||||
| // Test Ceres AutoDiff on Snavely, defined in ceres_example.h
 | ||||
| // Adapt to GTSAM types
 | ||||
| Vector2 adapted(const Vector9& P, const Vector3& X) { | ||||
|   SnavelyProjection snavely; | ||||
|   Vector2 x; | ||||
|   if (snavely(P.data(), X.data(), x.data())) | ||||
|     return x; | ||||
|   else | ||||
|     throw std::runtime_error("Snavely fail"); | ||||
| } | ||||
| 
 | ||||
| TEST(Expression, AutoDiff2) { | ||||
|   using ceres::internal::AutoDiff; | ||||
| 
 | ||||
|  | @ -185,14 +146,12 @@ TEST(Expression, AutoDiff2) { | |||
| 
 | ||||
|   // Apply the mapping, to get image point b_x.
 | ||||
|   Vector expected = Vector2(2, 1); | ||||
|   Vector2 actual = snavely(P, X); | ||||
|   Vector2 actual = adapted(P, X); | ||||
|   EXPECT(assert_equal(expected,actual,1e-9)); | ||||
| 
 | ||||
|   // Get expected derivatives
 | ||||
|   Matrix E1 = numericalDerivative21<Vector2, Vector9, Vector3>( | ||||
|       SnavelyProjection(), P, X); | ||||
|   Matrix E2 = numericalDerivative22<Vector2, Vector9, Vector3>( | ||||
|       SnavelyProjection(), P, X); | ||||
|   Matrix E1 = numericalDerivative21<Vector2, Vector9, Vector3>(adapted, P, X); | ||||
|   Matrix E2 = numericalDerivative22<Vector2, Vector9, Vector3>(adapted, P, X); | ||||
| 
 | ||||
|   // Get derivatives with AutoDiff
 | ||||
|   Vector2 actual2; | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue