From 1809d9b3cb9a63456c2a3b873a1e471742930b07 Mon Sep 17 00:00:00 2001 From: Gabe Sibley Date: Sun, 18 May 2014 02:00:03 -0400 Subject: [PATCH] Ports rectification to new camera model types. --- include/calibu/cam/ProjectionModel.h | 2 +- include/calibu/cam/Rectify.h | 1 + include/calibu/cam/camera_crtp.h | 14 +- include/calibu/cam/camera_models_crtp.h | 6 +- include/calibu/cam/rectify_crtp.h | 165 ++++++++++++++++++ src/CMakeLists.txt | 2 + src/cam/Rectify.cpp | 220 ++++++++++++------------ src/cam/StereoRectify.cpp | 4 +- src/cam/rectify_crtp.cpp | 133 ++++++++++++++ 9 files changed, 427 insertions(+), 120 deletions(-) create mode 100644 include/calibu/cam/rectify_crtp.h create mode 100644 src/cam/rectify_crtp.cpp diff --git a/include/calibu/cam/ProjectionModel.h b/include/calibu/cam/ProjectionModel.h index 6c99d27..7842ea2 100644 --- a/include/calibu/cam/ProjectionModel.h +++ b/include/calibu/cam/ProjectionModel.h @@ -75,7 +75,7 @@ struct ProjectionLinearId } template inline - static Eigen::Matrix dProject_dParams(const Eigen::Matrix& P, const Eigen::Matrix& params) + static Eigen::Matrix dProject_dParams(const Eigen::Matrix&, const Eigen::Matrix& ) { //TODO: implement this assert(false); diff --git a/include/calibu/cam/Rectify.h b/include/calibu/cam/Rectify.h index 2258cf4..47e7627 100644 --- a/include/calibu/cam/Rectify.h +++ b/include/calibu/cam/Rectify.h @@ -125,6 +125,7 @@ namespace calibu range.ExcludeLessThan(ln[0]); range.ExcludeGreaterThan(rn[0]); } + return range; } diff --git a/include/calibu/cam/camera_crtp.h b/include/calibu/cam/camera_crtp.h index 00ee5f5..3d48a13 100644 --- a/include/calibu/cam/camera_crtp.h +++ b/include/calibu/cam/camera_crtp.h @@ -117,7 +117,8 @@ class CameraInterface { return d_project_dparams + dtransfer3d_dray * dray_dparams; } - Scalar* GetParams() { + Scalar* GetParams() const + { return params_; } @@ -126,9 +127,14 @@ class CameraInterface { return n_params_; } - const Eigen::Vector2i& ImageSize() const + unsigned int Width() const + { + return image_size_[0]; + } + + unsigned int Height() const { - return image_size_; + return image_size_[1]; } protected: @@ -158,7 +164,7 @@ class CameraInterface { // Is the parameter list memory managed by us or externally? bool owns_memory_; - Eigen::Vector2i image_size_; + Eigen::Vector2i image_size_; // GTS: why imagesize? is width first element or height? why not just width and height? }; template diff --git a/include/calibu/cam/camera_models_crtp.h b/include/calibu/cam/camera_models_crtp.h index 17a9034..5289d66 100644 --- a/include/calibu/cam/camera_models_crtp.h +++ b/include/calibu/cam/camera_models_crtp.h @@ -73,7 +73,7 @@ struct CameraUtils { } template - static inline void dMultK_dparams(const T* params, const T* pix, T* j) { + static inline void dMultK_dparams(const T*, const T* pix, T* j) { j[0] = pix[0]; j[2] = 0; j[4] = 1; j[6] = 0; j[1] = 0; j[3] = pix[1]; j[5] = 0; j[7] = 1; } @@ -600,14 +600,14 @@ class Poly3Camera : public CameraInterface { } template - static void dProject_dparams(const T* ray, const T* params, T* j) { + static void dProject_dparams(const T*, const T*, T* ) { std::cerr << "dProjedt_dparams not defined for the poly3 model. " " Throwing exception." << std::endl; throw 0; } template - static void dUnproject_dparams(const T* pix, const T* params, T* j) { + static void dUnproject_dparams(const T*, const T*, T* ) { std::cerr << "dUnproject_dparams not defined for the poly3 model. " " Throwing exception." << std::endl; throw 0; diff --git a/include/calibu/cam/rectify_crtp.h b/include/calibu/cam/rectify_crtp.h new file mode 100644 index 0000000..12b0e1b --- /dev/null +++ b/include/calibu/cam/rectify_crtp.h @@ -0,0 +1,165 @@ +/* + This file is part of the Calibu Project. + https://github.com/gwu-robotics/Calibu + + Copyright (C) 2013 George Washington University, + Steven Lovegrove, + Gabe Sibley + + 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. + */ + +#pragma once + +#include +#include +#include + +#include +#include +#include + +#include // todo remove this file... + +#include + +namespace calibu +{ + /////////////////////////////////////////////////////////////////////////////// + /// This structure is used to build a LUT without requiring branching + // when rectifying images. The values out of the image to the top left + // pixels instead of having a test for out of bound access, the aim is + // to avoid a branch in the code. + // + // int xt = (int) x; /* top-left corner */ + // int yt = (int) y; + // float ax = x - xt; + // float ay = y - yt; + // float *ptr = image + (width*yt) + xt; + // + // return( (1-ax) * (1-ay) * *ptr + + // ( ax ) * (1-ay) * *(ptr+1) + + // (1-ax) * ( ay ) * *(ptr+(width)) + + // ( ax ) * ( ay ) * *(ptr+(width)+1) ); + struct BilinearLutPoint + { + int idx0; // index to pixel in src image + int idx1; // index to pixel + one row in src image + float w00; // top left weight for bilinear interpolation + float w01; // top right weight for bilinear interpolation + float w10; // bottom left weight ... + float w11; // bottom right weight ... + }; + + /////////////////////////////////////////////////////////////////////////////// + CALIBU_EXPORT + struct LookupTable + { + inline LookupTable(){}; + inline LookupTable( int nWidth, int nHeight ) + { + m_vLutPixels.resize( nWidth*nHeight ); + m_nWidth = nWidth; + } + + inline LookupTable( const LookupTable& rhs ) + { + m_vLutPixels.resize( rhs.m_vLutPixels.size() ); + m_nWidth = rhs.m_nWidth; + m_vLutPixels = rhs.m_vLutPixels; + } + + inline unsigned int Width() const + { + return m_nWidth; + } + + inline unsigned int Height() const + { + return m_vLutPixels.size() / m_nWidth; + } + + inline void SetPoint( unsigned int nRow, unsigned int nCol, const BilinearLutPoint& p ) + { + assert( m_vLutPixels.size() > 0 ); + m_vLutPixels[ nRow*m_nWidth + nCol ] = p; + } + + std::vector m_vLutPixels; + int m_nWidth; // so m_nHeight = m_vPixels.size()/m_nWidth + }; + + + /// Create lookup table which can be used to remap a general camera model + /// 'cam_from' to a linear and potentially rotated model, 'R_onK'. + /// R_onK is formed from the multiplication R_on (old form new) and the new + /// camera intrinsics K. + CALIBU_EXPORT void CreateLookupTable( + const calibu::CameraInterface& cam_from, + const Eigen::Matrix3d& R_onKinv, + LookupTable& lut + ); + + /// Create lookup table which can be used to remap a general camera model + /// 'cam_from' to a linear. + void CreateLookupTable( + const calibu::CameraInterface& cam_from, + LookupTable& lut + ); + + + /// Rectify image pInputImageData using lookup table generated by + /// 'CreateLookupTable' to output image pOutputRectImageData. + CALIBU_EXPORT void Rectify( + const LookupTable& lut, + const unsigned char* pInputImageData, + unsigned char* pOutputRectImageData, + int w, int h + ); + + /// + inline Range MinMaxRotatedCol( const calibu::CameraInterface& cam, const Eigen::Matrix3d& Rnl_l ) + { + using namespace Eigen; + Range range = Range::Open(); + + for(size_t row = 0; row < cam.Height(); ++row) { + const Vector3d lray = Rnl_l* cam.Unproject(Vector2d(0,row)); + const Vector3d rray = Rnl_l* cam.Unproject(Vector2d(cam.Width()-1,row)); +// std::cout << "lray: " << lray.transpose() << std::endl; +// std::cout << "rray: " << rray.transpose() << std::endl; +// double angle = acos( lray.dot(rray)/(lray.norm()*rray.norm()) ); +// printf( "row %zu, Angle: %f\n", row, angle*180.0/M_PI ); + const Vector2d ln = Project( lray ); + const Vector2d rn = Project( rray ); + range.ExcludeLessThan(ln[0]); + range.ExcludeGreaterThan(rn[0]); + } + + return range; + } + + /// + inline Range MinMaxRotatedRow( const calibu::CameraInterface& cam, const Eigen::Matrix3d& Rnl_l ) + { + Range range = Range::Open(); + for(size_t col = 0; col < cam.Width(); ++col) { + const Eigen::Vector2d tn = Project(Eigen::Vector3d(Rnl_l*cam.Unproject(Eigen::Vector2d(col,0)) )); + const Eigen::Vector2d bn = Project(Eigen::Vector3d(Rnl_l*cam.Unproject(Eigen::Vector2d(col,cam.Height()-1)) )); + range.ExcludeLessThan(tn[1]); + range.ExcludeGreaterThan(bn[1]); + } + return range; + } +} + diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index fdead97..177e373 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -23,6 +23,7 @@ set(HEADERS ${INC_DIR}/cam/ProjectionModel.h ${INC_DIR}/cam/ProjectionKannalaBrandt.h ${INC_DIR}/cam/Rectify.h + ${INC_DIR}/cam/rectify_crtp.h ${INC_DIR}/cam/StereoRectify.h ${INC_DIR}/conics/Conic.h ${INC_DIR}/conics/ConicFinder.h @@ -53,6 +54,7 @@ set(SRC_DIR ${Calibu_SOURCE_DIR}/src) SET(SOURCES ${SRC_DIR}/cam/CameraXml.cpp ${SRC_DIR}/cam/Rectify.cpp + ${SRC_DIR}/cam/rectify_crtp.cpp ${SRC_DIR}/cam/StereoRectify.cpp ${SRC_DIR}/conics/Conic.cpp ${SRC_DIR}/conics/ConicFinder.cpp diff --git a/src/cam/Rectify.cpp b/src/cam/Rectify.cpp index 27efe59..986b3ee 100644 --- a/src/cam/Rectify.cpp +++ b/src/cam/Rectify.cpp @@ -1,22 +1,22 @@ /* - This file is part of the Calibu Project. -https://github.com/gwu-robotics/Calibu + This file is part of the Calibu Project. + https://github.com/gwu-robotics/Calibu -Copyright (C) 2013 George Washington University, -Steven Lovegrove, -Gabe Sibley + Copyright (C) 2013 George Washington University, + Steven Lovegrove, + Gabe Sibley -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 + 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 + 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. + 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. */ #include @@ -25,105 +25,105 @@ limitations under the License. namespace calibu { - void CreateLookupTable( - const calibu::CameraModelInterface& cam_from, - const Eigen::Matrix3d& R_onKinv, - Eigen::Matrix& lookup_warp - ) - { - for(size_t r = 0; r < cam_from.Height(); ++r) { - for(size_t c = 0; c < cam_from.Width(); ++c) { - // Remap - const Eigen::Vector3d p_o = R_onKinv * Eigen::Vector3d(c,r,1); - Eigen::Vector2d p_warped = cam_from.Project(p_o); - - // Clamp to valid image coords - p_warped[0] = std::min(std::max(0.0, p_warped[0]), cam_from.Width() - 1.0 ); - p_warped[1] = std::min(std::max(0.0, p_warped[1]), cam_from.Height() - 1.0 ); - - lookup_warp(r,c) = p_warped.cast(); - } - } + void CreateLookupTable( + const calibu::CameraModelInterface& cam_from, + const Eigen::Matrix3d& R_onKinv, + Eigen::Matrix& lookup_warp + ) + { + for(size_t r = 0; r < cam_from.Height(); ++r) { + for(size_t c = 0; c < cam_from.Width(); ++c) { + // Remap + const Eigen::Vector3d p_o = R_onKinv * Eigen::Vector3d(c,r,1); + Eigen::Vector2d p_warped = cam_from.Project(p_o); + + // Clamp to valid image coords + p_warped[0] = std::min(std::max(0.0, p_warped[0]), cam_from.Width() - 1.0 ); + p_warped[1] = std::min(std::max(0.0, p_warped[1]), cam_from.Height() - 1.0 ); + + lookup_warp(r,c) = p_warped.cast(); + } } - - /////////////////////////////////////////////////////////////////////////////// - void CreateLookupTable( - const calibu::CameraModelInterface& cam_from, - const Eigen::Matrix3d& R_onKinv, - LookupTable& lut - ) - { - const int w = cam_from.Width(); - const int h = cam_from.Height(); - - for( int r = 0; r < h; ++r) { - for( int c = 0; c < w; ++c) { - // Remap - const Eigen::Vector3d p_o = R_onKinv * Eigen::Vector3d(c,r,1); - Eigen::Vector2d p_warped = cam_from.Project(p_o); - - // Clamp to valid image coords. This will cause out of image - // data to be stretched from nearest valid coords with - // no branching in rectify function. - p_warped[0] = std::min(std::max(0.0, p_warped[0]), w - 1.0 ); - p_warped[1] = std::min(std::max(0.0, p_warped[1]), h - 1.0 ); - - // Truncates the values for the left image - int u = (int) p_warped[0]; - int v = (int) p_warped[1]; - float su = p_warped[0] - (double)u; - float sv = p_warped[1] - (double)v; - - // Fix pixel access for last row/column to ensure all are in bounds - if(u == (w-1)) { - u -= 1; - su = 1.0; - } - if(v == (w-1)) { - v -= 1; - sv = 1.0; - } - - // Pre-compute the bilinear interpolation weights - BilinearLutPoint p; - p.idx0 = u + v*w; - p.idx1 = u + v*w + w; - p.w00 = (1-su)*(1-sv); - p.w01 = su *(1-sv); - p.w10 = (1-su)*sv; - p.w11 = su*sv; - lut.SetPoint( r, c, p ); - } + } + + /////////////////////////////////////////////////////////////////////////////// + void CreateLookupTable( + const calibu::CameraModelInterface& cam_from, + const Eigen::Matrix3d& R_onKinv, + LookupTable& lut + ) + { + const int w = cam_from.Width(); + const int h = cam_from.Height(); + + for( int r = 0; r < h; ++r) { + for( int c = 0; c < w; ++c) { + // Remap + const Eigen::Vector3d p_o = R_onKinv * Eigen::Vector3d(c,r,1); + Eigen::Vector2d p_warped = cam_from.Project(p_o); + + // Clamp to valid image coords. This will cause out of image + // data to be stretched from nearest valid coords with + // no branching in rectify function. + p_warped[0] = std::min(std::max(0.0, p_warped[0]), w - 1.0 ); + p_warped[1] = std::min(std::max(0.0, p_warped[1]), h - 1.0 ); + + // Truncates the values for the left image + int u = (int) p_warped[0]; + int v = (int) p_warped[1]; + float su = p_warped[0] - (double)u; + float sv = p_warped[1] - (double)v; + + // Fix pixel access for last row/column to ensure all are in bounds + if(u == (w-1)) { + u -= 1; + su = 1.0; } - } - - void Rectify( - const LookupTable& lut, - const unsigned char* pInputImageData, - unsigned char* pOutputRectImageData, - int w, - int h - ) - { - // Make the most of the continuous block of memory! - const BilinearLutPoint* ptr = &lut.m_vLutPixels[0]; - - const int nHeight = lut.Height(); - const int nWidth = lut.Width(); - - // Make sure we have been given a correct lookup table. - assert(w== nWidth && h == nHeight); - - for( int nRow = 0; nRow < nHeight; nRow++ ) { - for( int nCol = 0; nCol < nWidth; nCol++ ) { - *pOutputRectImageData++ = - (char) ( ptr->w00 * pInputImageData[ ptr->idx0 ] + - ptr->w01 * pInputImageData[ ptr->idx0 + 1 ] + - ptr->w10 * pInputImageData[ ptr->idx1 ] + - ptr->w11 * pInputImageData[ ptr->idx1 + 1 ] ); - ptr++; - } + if(v == (w-1)) { + v -= 1; + sv = 1.0; } + + // Pre-compute the bilinear interpolation weights + BilinearLutPoint p; + p.idx0 = u + v*w; + p.idx1 = u + v*w + w; + p.w00 = (1-su)*(1-sv); + p.w01 = su *(1-sv); + p.w10 = (1-su)*sv; + p.w11 = su*sv; + lut.SetPoint( r, c, p ); + } + } + } + + void Rectify( + const LookupTable& lut, + const unsigned char* pInputImageData, + unsigned char* pOutputRectImageData, + int w, + int h + ) + { + // Make the most of the continuous block of memory! + const BilinearLutPoint* ptr = &lut.m_vLutPixels[0]; + + const int nHeight = lut.Height(); + const int nWidth = lut.Width(); + + // Make sure we have been given a correct lookup table. + assert(w== nWidth && h == nHeight); + + for( int nRow = 0; nRow < nHeight; nRow++ ) { + for( int nCol = 0; nCol < nWidth; nCol++ ) { + *pOutputRectImageData++ = + (char) ( ptr->w00 * pInputImageData[ ptr->idx0 ] + + ptr->w01 * pInputImageData[ ptr->idx0 + 1 ] + + ptr->w10 * pInputImageData[ ptr->idx1 ] + + ptr->w11 * pInputImageData[ ptr->idx1 + 1 ] ); + ptr++; + } } + } } // end namespace diff --git a/src/cam/StereoRectify.cpp b/src/cam/StereoRectify.cpp index 82d1fc1..3097321 100644 --- a/src/cam/StereoRectify.cpp +++ b/src/cam/StereoRectify.cpp @@ -82,8 +82,8 @@ calibu::CameraModelT CreateScanlineRectifiedLookupAndCameras( // Setup new camera calibu::CameraModelT new_cam(cam_left.Width(),cam_left.Height()); - new_cam.Params() << fu, fv, u0, v0; - + new_cam.Params() << fu, fv, u0, v0; + // Homographies which should be applied to left and right images to scan-line rectify them const Eigen::Matrix3d Rl_nlKlinv = Rnl_l.transpose() * new_cam.Kinv(); const Eigen::Matrix3d Rr_nrKlinv = R_lr.inverse().matrix() * Rnl_l.transpose() * new_cam.Kinv(); diff --git a/src/cam/rectify_crtp.cpp b/src/cam/rectify_crtp.cpp new file mode 100644 index 0000000..0114e58 --- /dev/null +++ b/src/cam/rectify_crtp.cpp @@ -0,0 +1,133 @@ +/* + This file is part of the Calibu Project. + https://github.com/gwu-robotics/Calibu + + Copyright (C) 2013 George Washington University, + Steven Lovegrove, + Gabe Sibley + + 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. + */ + +#include +#include + +namespace calibu +{ + + /////////////////////////////////////////////////////////////////////////////// + void CreateLookupTable( + const calibu::CameraInterface& cam_from, + LookupTable& lut + ) + { + /* + TODO figure out what K should be for the "new" camera based on + what part of the original image we want to keep. + + // Work out parameters of "new" linear camera. We want to map range + // width/height to image via K. + const calibu::Range range_width + = calibu::MinMaxRotatedCol( cam_from, Eigen::Matrix3d::Identity() ); + const calibu::Range range_height + = calibu::MinMaxRotatedRow( cam_from, Eigen::Matrix3d::Identity() ); + + printf(" range_width.Size() = %f, range_width.maxr = %f, range_width.minr = %f\n", + range_width.Size(), range_width.maxr, range_width.minr ); + printf(" range_height.Size() = %f, range_height.maxr = %f, range_height.minr = %f\n", + range_height.Size(), range_height.maxr, range_height.minr ); + + double fu = (cam_from.Width()-1) / range_width.Size(); + double fv = (cam_from.Height()-1) / range_height.Size(); + double u0 = -fu * range_width.minr; + double v0 = -fv * range_height.minr; + */ + + // This is a hack as there is no guarntee the first 4 params are the K + // matrix. Really we should workout what a good linear model would be + // based on what portion of the original image is on the z=1 plane (e.g., + // cameras with FOV > 180 will need to ignore some pixels). + double fu = cam_from.GetParams()[0]; + double fv = cam_from.GetParams()[1]; + double u0 = cam_from.GetParams()[2]; + double v0 = cam_from.GetParams()[3]; + + // linear camera model inv(K) matrix + Eigen::Matrix3d R_onKinv; + R_onKinv << 1.0/fu, 0, -u0 / fu, + 0, 1.0/fv, -v0 / fv, + 0, 0, 1; + + CreateLookupTable( cam_from, R_onKinv, lut ); + } + + + + /////////////////////////////////////////////////////////////////////////////// + void CreateLookupTable( + const calibu::CameraInterface& cam_from, + const Eigen::Matrix3d& R_onKinv, + LookupTable& lut + ) + { + const int w = cam_from.Width(); + const int h = cam_from.Height(); + + // make sure we have mem in the look up table + lut.m_vLutPixels.resize( w*h ); + lut.m_nWidth = w; + + for( int r = 0; r < h; ++r) { + for( int c = 0; c < w; ++c) { + // Remap + const Eigen::Vector3d p_o = R_onKinv * Eigen::Vector3d(c,r,1); + Eigen::Vector2d p_warped = cam_from.Project(p_o); + + // Clamp to valid image coords. This will cause out of image + // data to be stretched from nearest valid coords with + // no branching in rectify function. + p_warped[0] = std::min(std::max(0.0, p_warped[0]), w - 1.0 ); + p_warped[1] = std::min(std::max(0.0, p_warped[1]), h - 1.0 ); + + // Truncates the values for the left image + int u = (int) p_warped[0]; + int v = (int) p_warped[1]; + float su = p_warped[0] - (double)u; + float sv = p_warped[1] - (double)v; + + // Fix pixel access for last row/column to ensure all are in bounds + if(u == (w-1)) { + u -= 1; + su = 1.0; + } + if(v == (w-1)) { + v -= 1; + sv = 1.0; + } + + // Pre-compute the bilinear interpolation weights + BilinearLutPoint p; + p.idx0 = u + v*w; + p.idx1 = u + v*w + w; + p.w00 = (1-su)*(1-sv); + p.w01 = su *(1-sv); + p.w10 = (1-su)*sv; + p.w11 = su*sv; + + lut.SetPoint( r, c, p ); + } + } + } + +} // end namespace +