Isolated Snavely example
dellaert
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