Skip to content
Fetching contributors…
Cannot retrieve contributors at this time
454 lines (398 sloc) 19.7 KB
// __BEGIN_LICENSE__
// Copyright (c) 2006-2012, United States Government as represented by the
// Administrator of the National Aeronautics and Space Administration. All
// rights reserved.
//
// The NASA Vision Workbench is licensed under the Apache License,
// Version 2.0 (the "License"); you may not use this file except in
// compliance with the License. You may obtain a copy of the License at
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// __END_LICENSE__
/// \file PinholeModelCalibrate.h
///
/// This file contains the methods for calibrating a pinhole camera model based on a set of known 3D points a
/// and corresponding pixel coordinates.
///
#ifndef __VW_CAMERAMODEL_PINHOLE_CALIBRATE_H__
#define __VW_CAMERAMODEL_PINHOLE_CALIBRATE_H__
#include <vw/config.h>
#include <vw/Math/RANSAC.h>
#include <vw/Math/Vector.h>
#include <vw/Camera/PinholeModel.h>
#include <vw/Math/LevenbergMarquardt.h>
namespace vw {
namespace camera {
namespace {
/// \brief Intrinsic PinholeModel parameter extraction class
///
/// This class converts from/to a PinholeModel to/from a Vector of doubles
/// containing the focal lengths f_u, f_v and camera center coordinates c_u, c_v
///
/// Pass this class as template parameter to serialize_pinholemodel()
/// or deserialize_pinholemodel()
class PinholeModelSerializeIntrinsic {
public:
static const unsigned int size = 4; /// number of parameters this
/// class will extract
/// serialize the passed PinholeModel into a Vector of doubles
static inline const vw::Vector<double, size> get(const PinholeModel& m) {
Vector<double, size> data;
subvector(data,0,2) = m.focal_length();
subvector(data,2,2) = m.point_offset();
return data;
}
/// Set the parameters of the PinholeModel to the values contained
/// in the Vector
static inline void set(PinholeModel& m, const vw::Vector<double, size>& v) {
m.set_focal_length( subvector(v,0,2), false );
m.set_point_offset( subvector(v,2,2), true );
}
};
/// \brief Extrinsic Rotation parameter extraction class
///
/// This class converts from/to a PinholeModel to/from a Vector of doubles
/// containing the rotation vector components r_x, r_y and r_z
///
/// Pass this class as template parameter to serialize_pinholemodel()
/// or deserialize_pinholemodel()
class PinholeModelSerializeRotation {
public:
static const unsigned int size = 3; /// number of parameters this
/// class will extract
/// Serialize the passed PinholeModel into a Vector of doubles
static inline const vw::Vector<double, size> get(const PinholeModel& m) {
vw::Vector<double, size> data = m.camera_pose().axis_angle();
return data;
}
/// Set the parameters of the PinholeModel to the values contained
/// in the Vector
static inline void set(PinholeModel& m, const vw::Vector<double, size>& v) {
m.set_camera_pose( axis_angle_to_quaternion( v ));
}
};
/// \brief Extrinsic Translation parameter extraction class
///
/// This class converts from/to a PinholeModel to/from a Vector of doubles
/// containing the translations t_x, t_y and t_z
///
/// Pass this class as template parameter to serialize_pinholemodel()
/// or deserialize_pinholemodel()
class PinholeModelSerializeTranslation {
public:
static const unsigned int size = 3; /// number of parameters this
/// class will extract
/// Serialize the passed PinholeModel into a Vector of doubles
static inline const vw::Vector<double, size> get(const PinholeModel& m) {
vw::Vector<double, size> data = m.camera_center();
return data;
}
/// Set the parameters of the PinholeModel to the values contained
/// in the Vector
static inline void set(PinholeModel& m, const vw::Vector<double, size>& v) {
m.set_camera_center(v);
}
};
/// \brief Tsai lens distortion parameter extraction class
///
/// This class converts from/to a PinholeModel to/from a Vector of four doubles
/// containing the TSAI distortion parameters k_1, k_2, p_1 and p_2
///
/// Pass this class as template parameter to serialize_pinholemodel()
/// or deserialize_pinholemodel()
class PinholeModelSerializeTSAI {
public:
static const unsigned int size = 4; /// number of parameters this
/// class will extract
/// Serialize the passed PinholeModel into a Vector of doubles
static inline const vw::Vector<double, size> get(const PinholeModel& m) {
const vw::camera::TsaiLensDistortion* d = dynamic_cast<const vw::camera::TsaiLensDistortion*>(m.lens_distortion());
VW_ASSERT( d, ArgumentErr() << "Bad lens distortion model cast in PinholeModelSerializeTSAI::get()" );
return d->distortion_parameters();
}
/// Set the parameters of the PinholeModel to the values contained
/// in the Vector
static inline void set(PinholeModel& m, const vw::Vector<double, size>& v) {
vw::camera::TsaiLensDistortion t(v);
m.set_lens_distortion(t); // set_lens_distortion creates a copy of
// t on the heap, no memory problem here...
}
};
/// \brief PinholeModel parameter extraction class doing nothing
///
/// This class is for internal use only in order to avoid code quadruplication
class PinholeModelSerializeNull {
public:
static const unsigned int size = 0; /// number of parameters this
/// class will extract
/// serialize the passed PinholeModel into a Vector of doubles
static inline const Vector<double, size> get(const PinholeModel& /*m*/) {
return Vector<double, size>();
}
/// Set the parameters of the PinholeModel to the values contained
/// in the Vector
static inline void set(PinholeModel& /*m*/, const Vector<double, size>& /*v*/) {
}
};
} /* anonymous namespace */
/// \brief Serialize PinholeModel using the specified serialization classes
///
/// Specialize on one or several of PinholeModelSerializeIntrinsic,
/// PinholeModelSerializeRotation, PinholeModelSerializeTranslation,
/// PinholeModelSerializeTSAI or a custom serialization class
template<class Ex1T, class Ex2T, class Ex3T, class Ex4T>
inline const Vector<double, Ex1T::size + Ex2T::size + Ex3T::size + Ex4T::size> serialize_pinholemodel(const PinholeModel& m) {
Vector<double, Ex1T::size + Ex2T::size + Ex3T::size + Ex4T::size> res;
subvector(res, 0, Ex1T::size) = Ex1T::get(m);
subvector(res, Ex1T::size, Ex2T::size) = Ex2T::get(m);
subvector(res, Ex1T::size + Ex2T::size, Ex3T::size) = Ex3T::get(m);
subvector(res, Ex1T::size + Ex2T::size + Ex3T::size, Ex4T::size) = Ex4T::get(m);
return res;
}
/// \brief Serialize PinholeModel using the specified serialization classes
///
/// Specialize on one or several of PinholeModelSerializeIntrinsic,
/// PinholeModelSerializeRotation, PinholeModelSerializeTranslation,
/// PinholeModelSerializeTSAI or a custom serialization class
template<class Ex1T, class Ex2T, class Ex3T>
inline const Vector<double, Ex1T::size + Ex2T::size + Ex3T::size> serialize_pinholemodel(const PinholeModel& m) {
Vector<double, Ex1T::size + Ex2T::size + Ex3T::size> res;
subvector(res, 0, Ex1T::size) = Ex1T::get(m);
subvector(res, Ex1T::size, Ex2T::size) = Ex2T::get(m);
subvector(res, Ex1T::size + Ex2T::size, Ex3T::size) = Ex3T::get(m);
return res;
}
/// \brief Serialize PinholeModel using the specified serialization classes
///
/// Specialize on one or several of PinholeModelSerializeIntrinsic,
/// PinholeModelSerializeRotation, PinholeModelSerializeTranslation,
/// PinholeModelSerializeTSAI or a custom serialization class
template<class Ex1T, class Ex2T>
inline const Vector<double, Ex1T::size + Ex2T::size> serialize_pinholemodel(const PinholeModel& m) {
Vector<double, Ex1T::size + Ex2T::size> res;
subvector(res, 0, Ex1T::size) = Ex1T::get(m);
subvector(res, Ex1T::size, Ex2T::size) = Ex2T::get(m);
return res;
}
/// \brief Serialize PinholeModel using the specified serialization classes
///
/// Specialize on one or several of PinholeModelSerializeIntrinsic,
/// PinholeModelSerializeRotation, PinholeModelSerializeTranslation,
/// PinholeModelSerializeTSAI or a custom serialization class
template<class Ex1T>
inline const Vector<double, Ex1T::size> serialize_pinholemodel(const PinholeModel& m) {
return Ex1T::get(m);
}
/// \brief Deserialize PinholeModel using the specified serialization classes
///
/// Specialize on one or several of PinholeModelSerializeIntrinsic,
/// PinholeModelSerializeRotation, PinholeModelSerializeTranslation,
/// PinholeModelSerializeTSAI or a custom serialization class
template<class Ex1T, class Ex2T, class Ex3T, class Ex4T>
inline void deserialize_pinholemodel(PinholeModel& m, const Vector<double, Ex1T::size + Ex2T::size + Ex3T::size + Ex4T::size>& in) {
Ex1T::set(m, subvector(in, 0, Ex1T::size));
Ex2T::set(m, subvector(in, Ex1T::size, Ex2T::size));
Ex3T::set(m, subvector(in, Ex1T::size + Ex2T::size, Ex3T::size));
Ex4T::set(m, subvector(in, Ex1T::size + Ex2T::size + Ex3T::size, Ex4T::size));
}
/// \brief Deserialize PinholeModel using the specified serialization classes
///
/// Specialize on one or several of PinholeModelSerializeIntrinsic,
/// PinholeModelSerializeRotation, PinholeModelSerializeTranslation,
/// PinholeModelSerializeTSAI or a custom serialization class
template<class Ex1T, class Ex2T, class Ex3T>
inline void deserialize_pinholemodel(PinholeModel& m, const Vector<double, Ex1T::size + Ex2T::size + Ex3T::size>& in) {
Ex1T::set(m, subvector(in, 0, Ex1T::size));
Ex2T::set(m, subvector(in, Ex1T::size, Ex2T::size));
Ex3T::set(m, subvector(in, Ex1T::size + Ex2T::size, Ex3T::size));
}
/// \brief Deserialize PinholeModel using the specified serialization classes
///
/// Specialize on one or several of PinholeModelSerializeIntrinsic, PinholeModelSerializeRotation, PinholeModelSerializeTranslation,
/// PinholeModelSerializeTSAI or a custom serialization class
template<class Ex1T, class Ex2T>
inline void deserialize_pinholemodel(PinholeModel& m, const Vector<double, Ex1T::size + Ex2T::size>& in) {
Ex1T::set(m, subvector(in, 0, Ex1T::size));
Ex2T::set(m, subvector(in, Ex1T::size, Ex2T::size));
}
/// \brief Deserialize PinholeModel using the specified serialization classes
///
/// Specialize on one or several of PinholeModelSerializeIntrinsic,
/// PinholeModelSerializeRotation, PinholeModelSerializeTranslation,
/// PinholeModelSerializeTSAI or a custom serialization class
template<class Ex1T>
inline void deserialize_pinholemodel(PinholeModel& m, const Vector<double, Ex1T::size>& in) {
Ex1T::set(m, subvector(in, 0, Ex1T::size));
}
namespace {
/// The LeastSquaresModel for the LevenbergMarquardt method used for
/// calibrating the potentially nonlinear (tangential and radial
/// distortion-corrected) camera model
///
/// The template arguments allow customization of the camera
/// (de)serialization process in the optimization and thus
/// specification of which parameters to optimize and which parameters to
/// leave untouched
template<class Ex1T, class Ex2T, class Ex3T, class Ex4T>
class PinholeModelLeastSquares : public math::LeastSquaresModelBase<PinholeModelLeastSquares<Ex1T, Ex2T, Ex3T, Ex4T> >
{
PinholeModel& m_camera;
const std::vector<Vector2>& m_pixels;
const std::vector<Vector3>& m_points;
public:
typedef Vector<double> result_type;
typedef Vector<double> domain_type;
typedef Matrix<double> jacobian_type;
inline PinholeModelLeastSquares(PinholeModel& camera, const std::vector<Vector2>& pixels, const std::vector<Vector3>& points)
: math::LeastSquaresModelBase<PinholeModelLeastSquares<Ex1T, Ex2T, Ex3T, Ex4T> >(),
m_camera(camera), m_pixels(pixels), m_points(points)
{}
inline result_type operator()( domain_type const& x ) const {
deserialize_pinholemodel<Ex1T, Ex2T, Ex3T, Ex4T>(m_camera, x);
result_type diff(m_points.size()*2);
for (uint32 i = 0; i < m_points.size(); i++) {
Vector2 proj(m_camera.point_to_pixel(m_points[i]));
diff[2*i] = proj.x();
diff[2*i+1] = proj.y();
}
return diff;
}
};
} /* anonymous namespace */
/// Calibrate using the given camera model as starting point and using
/// the given (de)serialization functors - thus some camera parameters
/// can be hold fixed
///
/// \param Ex1T Serialization one of PinholeModelSerializeIntrinsic,
/// PinholeModelSerializeRotation, PinholeModelSerializeTranslation,
/// PinholeModelSerializeTSAI or a custom serialization class
/// \param m Initial camera model guess
/// \param pixels Pixel locations in image plane
/// \param points Points in 3D euclidian space
/// \param niter Maximum number of Levenberg Marquardt iterations
template<class Ex1T, class Ex2T, class Ex3T, class Ex4T>
void pinholemodel_calibrate(PinholeModel& m, const std::vector<Vector2>& pixels, const std::vector<Vector3>& points, unsigned int niter = 1000) {
typedef typename PinholeModelLeastSquares<Ex1T, Ex2T, Ex3T, Ex4T>::domain_type domain_type;
typedef typename PinholeModelLeastSquares<Ex1T, Ex2T, Ex3T, Ex4T>::result_type result_type;
PinholeModelLeastSquares<Ex1T, Ex2T, Ex3T, Ex4T> model(m, pixels, points);
// reference pixel coordinates - function optimization goal
result_type observations(pixels.size()*2);
for (uint32 i = 0; i < pixels.size(); i++) {
observations[2*i] = pixels[i].x();
observations[2*i+1] = pixels[i].y();
}
domain_type seed(serialize_pinholemodel<Ex1T, Ex2T, Ex3T, Ex4T>(m));
int status = 0;
domain_type result =
math::levenberg_marquardt(model,
serialize_pinholemodel<Ex1T, Ex2T, Ex3T, Ex4T>(m),
observations, status, 1e-16, 1e-16, niter);
vw_out(DebugMessage, "Camera") << "Nonlinear calibration found: camera_parameter_vector " << result << std::endl;
}
/// Convenience overload with less template parameters
template<class Ex1T, class Ex2T, class Ex3T>
inline void pinholemodel_calibrate(PinholeModel& m,
const std::vector<Vector2>& pixels,
const std::vector<Vector3>& points,
unsigned int niter = 1000) {
pinholemodel_calibrate<Ex1T, Ex2T, Ex3T, PinholeModelSerializeNull>(m, pixels, points, niter);
}
/// Convenience overload with less template parameters
template<class Ex1T, class Ex2T>
inline void pinholemodel_calibrate(PinholeModel& m,
const std::vector<Vector2>& pixels,
const std::vector<Vector3>& points,
unsigned int niter = 1000) {
pinholemodel_calibrate<Ex1T, Ex2T, PinholeModelSerializeNull>(m, pixels, points, niter);
}
/// Convenience overload with less template parameters
template<class Ex1T>
inline void pinholemodel_calibrate(PinholeModel& m,
const std::vector<Vector2>& pixels,
const std::vector<Vector3>& points,
unsigned int niter = 1000) {
pinholemodel_calibrate<Ex1T, PinholeModelSerializeNull>(m, pixels, points, niter);
}
namespace {
/// The RANSAC fitting functor
template<class Ex1T, class Ex2T, class Ex3T, class Ex4T>
class PinholeModelRansacFitting {
const PinholeModel &m_cam;
int m_levenberg_marquardt_iter;
public:
typedef PinholeModel result_type;
PinholeModelRansacFitting(const PinholeModel& cam, int levenberg_marquardt_iter) : m_cam(cam), m_levenberg_marquardt_iter(levenberg_marquardt_iter)
{}
PinholeModel operator()(const std::vector<Vector2>& pixels, const std::vector<Vector3>& points) const {
PinholeModel tmp_cam(m_cam);
pinholemodel_calibrate<Ex1T, Ex2T, Ex3T, Ex4T>(tmp_cam, pixels, points, m_levenberg_marquardt_iter);
return tmp_cam;
}
PinholeModel operator()(const std::vector<Vector2>& pixels, const std::vector<Vector3>& points, const PinholeModel& cam) const {
PinholeModel tmp_cam(cam);
pinholemodel_calibrate<Ex1T, Ex2T, Ex3T, Ex4T>(tmp_cam, pixels, points, m_levenberg_marquardt_iter);
return tmp_cam;
}
template <class DummyT>
unsigned int min_elements_needed_for_fit(const DummyT&) const {
return (Ex1T::size + Ex2T::size + Ex3T::size + Ex4T::size + 1)/2;
}
};
/// The RANSAC error functor
class PinholeModelRansacError {
public:
double operator()(const PinholeModel& cam, const Vector2& pixel, const Vector3& point) const {
return math::norm_2( pixel - cam.point_to_pixel(point));
}
};
} /* anonymous namespace */
/// Calibrate using the given camera model as starting point and using
/// the given (de)serialization functors - thus some camera parameters
/// can be held fixed
///
/// This method uses RANSAC and runs pinholemodel_calibrate at every
/// iteration to find the best camera model for the set of points
/// selected by RANSAC
///
/// \param Ex1T Serialization one of PinholeModelSerializeIntrinsic,
/// PinholeModelSerializeRotation, PinholeModelSerializeTranslation,
/// PinholeModelSerializeTSAI or a custom serialization class
/// \param m Initial camera model guess
/// \param pixels Pixel locations in image plane
/// \param points Points in 3D euclidian space
/// \parm inlier_threshold Distance in image space below which pixels are considered inliers
/// \parm ransac_iter number of RANSAC iterations, 0 uses the default value specified in the RANSAC implementation (currently twice the pixel/point vector size)
/// \parm lm_iter number of Levenberg Marquardt iterations to run at every RANSAC iteration
/// \return indices of points and pixels RANSAC considers to be inliers
template<class Ex1T, class Ex2T, class Ex3T, class Ex4T>
std::vector<size_t> pinholemodel_calibrate_ransac(PinholeModel& m, const std::vector<Vector2>& pixels, const std::vector<Vector3>& points, double inlier_threshold = 10, int ransac_iter = 0, int lm_iter = 100) {
PinholeModelRansacFitting<Ex1T, Ex2T, Ex3T, Ex4T> fitting(m, lm_iter);
PinholeModelRansacError error;
math::RandomSampleConsensus<PinholeModelRansacFitting<Ex1T, Ex2T, Ex3T, Ex4T>, PinholeModelRansacError> ransac(fitting, error, ransac_iter, inlier_threshold, pixels.size()/2, true);
m = ransac(pixels, points);
std::vector<size_t> inlier_indices(ransac.inlier_indices(m, pixels, points));
vw_out(DebugMessage, "Camera") << "RANSAC classified as inliers: " << inlier_indices.size() << '/' << pixels.size() << std::endl;
return inlier_indices;
}
/// Convenience overload with less template parameters
template<class Ex1T, class Ex2T, class Ex3T>
inline std::vector<size_t> pinholemodel_calibrate_ransac(PinholeModel& m, const std::vector<Vector2>& pixels, const std::vector<Vector3>& points, double inlier_threshold = 10, int ransac_iter = 0, int lm_iter = 100) {
return pinholemodel_calibrate_ransac<Ex1T, Ex2T, Ex3T, PinholeModelSerializeNull>(m, pixels, points, inlier_threshold, ransac_iter, lm_iter);
}
/// Convenience overload with less template parameters
template<class Ex1T, class Ex2T>
inline std::vector<size_t> pinholemodel_calibrate_ransac(PinholeModel& m, const std::vector<Vector2>& pixels, const std::vector<Vector3>& points, double inlier_threshold = 10, int ransac_iter = 0, int lm_iter = 100) {
return pinholemodel_calibrate_ransac<Ex1T, Ex2T, PinholeModelSerializeNull>(m, pixels, points, inlier_threshold, ransac_iter, lm_iter);
}
/// Convenience overload with less template parameters
template<class Ex1T>
inline std::vector<size_t> pinholemodel_calibrate_ransac(PinholeModel& m, const std::vector<Vector2>& pixels, const std::vector<Vector3>& points, double inlier_threshold = 10, int ransac_iter = 0, int lm_iter = 100) {
return pinholemodel_calibrate_ransac<Ex1T, PinholeModelSerializeNull>(m, pixels, points, inlier_threshold, ransac_iter, lm_iter);
}
}}
#endif /* __VW_CAMERAMODEL_PINHOLE_CALIBRATE_H__ */
Something went wrong with that request. Please try again.