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++;
}
}
}
void CreateLookupTable(
const std::shared_ptr<calibu::CameraInterface<double>>& cam_from,
const Eigen::Matrix3d& R_onKinv,
LookupTable& lut,
int lookup_width,
int lookup_height
)
{
const int cam_width = cam_from->Width();
const int cam_height = cam_from->Height();
if(lookup_width < 1 || lookup_height < 1){
if(lut.Height() == 0){
lookup_width = cam_width;
lookup_height = cam_height;
// make sure we have mem in the look up table
lut.m_vLutPixels.resize( lookup_width*lookup_height );
lut.m_nWidth = lookup_width;
}else{
lookup_width = lut.Width();
lookup_height = lut.Height();
}
}
double x_offset = (lookup_width - cam_width) / 2.0;
double y_offset = (lookup_height - cam_height) / 2.0;
for( int r = 0; r < lookup_height; ++r) {
for( int c = 0; c < lookup_width; ++c) {
// Remap
const Eigen::Vector3d p_o = R_onKinv * Eigen::Vector3d(c - x_offset,r - y_offset,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]), cam_width - 1.0 );
p_warped[1] = std::min(std::max(0.0, p_warped[1]), cam_height - 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 accesses are in bounds
if(u == (cam_width-1)) {
u -= 1;
su = 1.0;
}
if(v == (cam_height-1)) {
v -= 1;
sv = 1.0;
}
// Pre-compute the bilinear interpolation weights
BilinearLutPoint p;
p.idx0 = u + v*cam_width;
p.idx1 = u + v*cam_width + cam_width;
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 );
}
}
}