383 lines
11 KiB
C++
383 lines
11 KiB
C++
/**
|
|
* This file is part of DSO.
|
|
*
|
|
* Copyright 2016 Technical University of Munich and Intel.
|
|
* Developed by Jakob Engel <engelj at in dot tum dot de>,
|
|
* for more information see <http://vision.in.tum.de/dso>.
|
|
* If you use this code, please cite the respective publications as
|
|
* listed on the above website.
|
|
*
|
|
* DSO is free software: you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation, either version 3 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* DSO is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License
|
|
* along with DSO. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
|
|
|
|
#pragma once
|
|
#include "util/settings.h"
|
|
#include "util/NumType.h"
|
|
#include "IOWrapper/ImageDisplay.h"
|
|
#include "fstream"
|
|
|
|
namespace dso
|
|
{
|
|
|
|
|
|
|
|
// reads interpolated element from a uchar* array
|
|
// SSE2 optimization possible
|
|
EIGEN_ALWAYS_INLINE float getInterpolatedElement(const float* const mat, const float x, const float y, const int width)
|
|
{
|
|
//stats.num_pixelInterpolations++;
|
|
|
|
int ix = (int)x;
|
|
int iy = (int)y;
|
|
float dx = x - ix;
|
|
float dy = y - iy;
|
|
float dxdy = dx*dy;
|
|
const float* bp = mat +ix+iy*width;
|
|
|
|
|
|
float res = dxdy * bp[1+width]
|
|
+ (dy-dxdy) * bp[width]
|
|
+ (dx-dxdy) * bp[1]
|
|
+ (1-dx-dy+dxdy) * bp[0];
|
|
|
|
return res;
|
|
}
|
|
|
|
EIGEN_ALWAYS_INLINE Eigen::Vector3f getInterpolatedElement43(const Eigen::Vector4f* const mat, const float x, const float y, const int width)
|
|
{
|
|
int ix = (int)x;
|
|
int iy = (int)y;
|
|
float dx = x - ix;
|
|
float dy = y - iy;
|
|
float dxdy = dx*dy;
|
|
const Eigen::Vector4f* bp = mat +ix+iy*width;
|
|
|
|
|
|
return dxdy * *(const Eigen::Vector3f*)(bp+1+width)
|
|
+ (dy-dxdy) * *(const Eigen::Vector3f*)(bp+width)
|
|
+ (dx-dxdy) * *(const Eigen::Vector3f*)(bp+1)
|
|
+ (1-dx-dy+dxdy) * *(const Eigen::Vector3f*)(bp);
|
|
}
|
|
|
|
EIGEN_ALWAYS_INLINE Eigen::Vector3f getInterpolatedElement33(const Eigen::Vector3f* const mat, const float x, const float y, const int width)
|
|
{
|
|
int ix = (int)x;
|
|
int iy = (int)y;
|
|
float dx = x - ix;
|
|
float dy = y - iy;
|
|
float dxdy = dx*dy;
|
|
const Eigen::Vector3f* bp = mat +ix+iy*width;
|
|
|
|
|
|
return dxdy * *(const Eigen::Vector3f*)(bp+1+width)
|
|
+ (dy-dxdy) * *(const Eigen::Vector3f*)(bp+width)
|
|
+ (dx-dxdy) * *(const Eigen::Vector3f*)(bp+1)
|
|
+ (1-dx-dy+dxdy) * *(const Eigen::Vector3f*)(bp);
|
|
}
|
|
|
|
EIGEN_ALWAYS_INLINE Eigen::Vector3f getInterpolatedElement33OverAnd(const Eigen::Vector3f* const mat, const bool* overMat, const float x, const float y, const int width, bool& over_out)
|
|
{
|
|
int ix = (int)x;
|
|
int iy = (int)y;
|
|
float dx = x - ix;
|
|
float dy = y - iy;
|
|
float dxdy = dx*dy;
|
|
const Eigen::Vector3f* bp = mat +ix+iy*width;
|
|
|
|
const bool* bbp = overMat +ix+iy*width;
|
|
over_out = bbp[1+width] && bbp[1] && bbp[width] && bbp[0];
|
|
|
|
return dxdy * *(const Eigen::Vector3f*)(bp+1+width)
|
|
+ (dy-dxdy) * *(const Eigen::Vector3f*)(bp+width)
|
|
+ (dx-dxdy) * *(const Eigen::Vector3f*)(bp+1)
|
|
+ (1-dx-dy+dxdy) * *(const Eigen::Vector3f*)(bp);
|
|
}
|
|
EIGEN_ALWAYS_INLINE Eigen::Vector3f getInterpolatedElement33OverOr(const Eigen::Vector3f* const mat, const bool* overMat, const float x, const float y, const int width, bool& over_out)
|
|
{
|
|
int ix = (int)x;
|
|
int iy = (int)y;
|
|
float dx = x - ix;
|
|
float dy = y - iy;
|
|
float dxdy = dx*dy;
|
|
const Eigen::Vector3f* bp = mat +ix+iy*width;
|
|
|
|
const bool* bbp = overMat +ix+iy*width;
|
|
over_out = bbp[1+width] || bbp[1] || bbp[width] || bbp[0];
|
|
|
|
return dxdy * *(const Eigen::Vector3f*)(bp+1+width)
|
|
+ (dy-dxdy) * *(const Eigen::Vector3f*)(bp+width)
|
|
+ (dx-dxdy) * *(const Eigen::Vector3f*)(bp+1)
|
|
+ (1-dx-dy+dxdy) * *(const Eigen::Vector3f*)(bp);
|
|
}
|
|
|
|
EIGEN_ALWAYS_INLINE float getInterpolatedElement31(const Eigen::Vector3f* const mat, const float x, const float y, const int width)
|
|
{
|
|
int ix = (int)x;
|
|
int iy = (int)y;
|
|
float dx = x - ix;
|
|
float dy = y - iy;
|
|
float dxdy = dx*dy;
|
|
const Eigen::Vector3f* bp = mat +ix+iy*width;
|
|
|
|
|
|
return dxdy * (*(const Eigen::Vector3f*)(bp+1+width))[0]
|
|
+ (dy-dxdy) * (*(const Eigen::Vector3f*)(bp+width))[0]
|
|
+ (dx-dxdy) * (*(const Eigen::Vector3f*)(bp+1))[0]
|
|
+ (1-dx-dy+dxdy) * (*(const Eigen::Vector3f*)(bp))[0];
|
|
}
|
|
|
|
EIGEN_ALWAYS_INLINE Eigen::Vector3f getInterpolatedElement13BiLin(const float* const mat, const float x, const float y, const int width)
|
|
{
|
|
int ix = (int)x;
|
|
int iy = (int)y;
|
|
const float* bp = mat +ix+iy*width;
|
|
|
|
float tl = *(bp);
|
|
float tr = *(bp+1);
|
|
float bl = *(bp+width);
|
|
float br = *(bp+width+1);
|
|
|
|
float dx = x - ix;
|
|
float dy = y - iy;
|
|
float topInt = dx * tr + (1-dx) * tl;
|
|
float botInt = dx * br + (1-dx) * bl;
|
|
float leftInt = dy * bl + (1-dy) * tl;
|
|
float rightInt = dy * br + (1-dy) * tr;
|
|
|
|
return Eigen::Vector3f(
|
|
dx * rightInt + (1-dx) * leftInt,
|
|
rightInt-leftInt,
|
|
botInt-topInt);
|
|
}
|
|
|
|
EIGEN_ALWAYS_INLINE Eigen::Vector3f getInterpolatedElement33BiLin(const Eigen::Vector3f* const mat, const float x, const float y, const int width)
|
|
{
|
|
int ix = (int)x;
|
|
int iy = (int)y;
|
|
const Eigen::Vector3f* bp = mat +ix+iy*width;
|
|
|
|
float tl = (*(bp))[0];
|
|
float tr = (*(bp+1))[0];
|
|
float bl = (*(bp+width))[0];
|
|
float br = (*(bp+width+1))[0];
|
|
|
|
float dx = x - ix;
|
|
float dy = y - iy;
|
|
float topInt = dx * tr + (1-dx) * tl;
|
|
float botInt = dx * br + (1-dx) * bl;
|
|
float leftInt = dy * bl + (1-dy) * tl;
|
|
float rightInt = dy * br + (1-dy) * tr;
|
|
|
|
return Eigen::Vector3f(
|
|
dx * rightInt + (1-dx) * leftInt,
|
|
rightInt-leftInt,
|
|
botInt-topInt);
|
|
}
|
|
EIGEN_ALWAYS_INLINE float getInterpolatedElement11Cub(const float* const p, const float x) // for x=0, this returns p[1].
|
|
{
|
|
return p[1] + 0.5f * x*(p[2] - p[0] + x*(2.0f*p[0] - 5.0f*p[1] + 4.0f*p[2] - p[3] + x*(3.0f*(p[1] - p[2]) + p[3] - p[0])));
|
|
}
|
|
|
|
EIGEN_ALWAYS_INLINE Eigen::Vector2f getInterpolatedElement12Cub(const float* const p, const float x) // for x=0, this returns p[1].
|
|
{
|
|
float c1 = 0.5f * (p[2] - p[0]);
|
|
float c2 = p[0]-2.5f*p[1]+2*p[2]-0.5f*p[3];
|
|
float c3 = 0.5f*(3.0f*(p[1]-p[2])+p[3]-p[0]);
|
|
float xx = x*x;
|
|
float xxx = xx*x;
|
|
return Eigen::Vector2f(p[1] + x*c1 + xx*c2 + xxx*c3, c1 + x*2.0f*c2 + xx*3.0f*c3);
|
|
}
|
|
EIGEN_ALWAYS_INLINE Eigen::Vector2f getInterpolatedElement32Cub(const Eigen::Vector3f* const p, const float x) // for x=0, this returns p[1].
|
|
{
|
|
float c1 = 0.5f * (p[2][0] - p[0][0]);
|
|
float c2 = p[0][0]-2.5f*p[1][0]+2*p[2][0]-0.5f*p[3][0];
|
|
float c3 = 0.5f*(3.0f*(p[1][0]-p[2][0])+p[3][0]-p[0][0]);
|
|
float xx = x*x;
|
|
float xxx = xx*x;
|
|
return Eigen::Vector2f(p[1][0] + x*c1 + xx*c2 + xxx*c3, c1 + x*2.0f*c2 + xx*3.0f*c3);
|
|
}
|
|
|
|
EIGEN_ALWAYS_INLINE float getInterpolatedElement11BiCub(const float* const mat, const float x, const float y, const int width)
|
|
{
|
|
int ix = (int)x;
|
|
int iy = (int)y;
|
|
float dx = x - ix;
|
|
const float* bp = mat +ix+iy*width;
|
|
|
|
float val[4];
|
|
val[0] =getInterpolatedElement11Cub(bp-width-1, dx);
|
|
val[1] =getInterpolatedElement11Cub(bp-1, dx);
|
|
val[2] =getInterpolatedElement11Cub(bp+width-1, dx);
|
|
val[3] =getInterpolatedElement11Cub(bp+2*width-1, dx);
|
|
|
|
float dy = y - iy;
|
|
return getInterpolatedElement11Cub(val, dy);
|
|
}
|
|
EIGEN_ALWAYS_INLINE Eigen::Vector3f getInterpolatedElement13BiCub(const float* const mat, const float x, const float y, const int width)
|
|
{
|
|
int ix = (int)x;
|
|
int iy = (int)y;
|
|
float dx = x - ix;
|
|
const float* bp = mat +ix+iy*width;
|
|
|
|
float val[4];
|
|
float grad[4];
|
|
Eigen::Vector2f v = getInterpolatedElement12Cub(bp-width-1, dx);
|
|
val[0] = v[0]; grad[0] = v[1];
|
|
|
|
v = getInterpolatedElement12Cub(bp-1, dx);
|
|
val[1] = v[0]; grad[1] = v[1];
|
|
|
|
v = getInterpolatedElement12Cub(bp+width-1, dx);
|
|
val[2] = v[0]; grad[2] = v[1];
|
|
|
|
v = getInterpolatedElement12Cub(bp+2*width-1, dx);
|
|
val[3] = v[0]; grad[3] = v[1];
|
|
|
|
float dy = y - iy;
|
|
v = getInterpolatedElement12Cub(val, dy);
|
|
|
|
return Eigen::Vector3f(v[0], getInterpolatedElement11Cub(grad, dy), v[1]);
|
|
}
|
|
|
|
EIGEN_ALWAYS_INLINE Eigen::Vector3f getInterpolatedElement33BiCub(const Eigen::Vector3f* const mat, const float x, const float y, const int width)
|
|
{
|
|
int ix = (int)x;
|
|
int iy = (int)y;
|
|
float dx = x - ix;
|
|
const Eigen::Vector3f* bp = mat +ix+iy*width;
|
|
|
|
float val[4];
|
|
float grad[4];
|
|
Eigen::Vector2f v = getInterpolatedElement32Cub(bp-width-1, dx);
|
|
val[0] = v[0]; grad[0] = v[1];
|
|
|
|
v = getInterpolatedElement32Cub(bp-1, dx);
|
|
val[1] = v[0]; grad[1] = v[1];
|
|
|
|
v = getInterpolatedElement32Cub(bp+width-1, dx);
|
|
val[2] = v[0]; grad[2] = v[1];
|
|
|
|
v = getInterpolatedElement32Cub(bp+2*width-1, dx);
|
|
val[3] = v[0]; grad[3] = v[1];
|
|
|
|
float dy = y - iy;
|
|
v = getInterpolatedElement12Cub(val, dy);
|
|
|
|
return Eigen::Vector3f(v[0], getInterpolatedElement11Cub(grad, dy), v[1]);
|
|
}
|
|
|
|
EIGEN_ALWAYS_INLINE Eigen::Vector4f getInterpolatedElement44(const Eigen::Vector4f* const mat, const float x, const float y, const int width)
|
|
{
|
|
int ix = (int)x;
|
|
int iy = (int)y;
|
|
float dx = x - ix;
|
|
float dy = y - iy;
|
|
float dxdy = dx*dy;
|
|
const Eigen::Vector4f* bp = mat +ix+iy*width;
|
|
|
|
|
|
return dxdy * *(bp+1+width)
|
|
+ (dy-dxdy) * *(bp+width)
|
|
+ (dx-dxdy) * *(bp+1)
|
|
+ (1-dx-dy+dxdy) * *(bp);
|
|
}
|
|
|
|
EIGEN_ALWAYS_INLINE Eigen::Vector2f getInterpolatedElement42(const Eigen::Vector4f* const mat, const float x, const float y, const int width)
|
|
{
|
|
int ix = (int)x;
|
|
int iy = (int)y;
|
|
float dx = x - ix;
|
|
float dy = y - iy;
|
|
float dxdy = dx*dy;
|
|
const Eigen::Vector4f* bp = mat +ix+iy*width;
|
|
|
|
|
|
return dxdy * *(const Eigen::Vector2f*)(bp+1+width)
|
|
+ (dy-dxdy) * *(const Eigen::Vector2f*)(bp+width)
|
|
+ (dx-dxdy) * *(const Eigen::Vector2f*)(bp+1)
|
|
+ (1-dx-dy+dxdy) * *(const Eigen::Vector2f*)(bp);
|
|
}
|
|
|
|
|
|
|
|
inline Vec3f makeRainbowf3F(float id)
|
|
{
|
|
id *= freeDebugParam3;
|
|
if(id < 0)
|
|
return Vec3f(1,1,1);
|
|
|
|
int icP = id;
|
|
float ifP = id-icP;
|
|
icP = icP%3;
|
|
|
|
if(icP == 0) return Vec3f((1-ifP), ifP, 0);
|
|
if(icP == 1) return Vec3f(0, (1-ifP), ifP);
|
|
if(icP == 2) return Vec3f(ifP, 0, (1-ifP));
|
|
assert(false);
|
|
return Vec3f(1,1,1);
|
|
}
|
|
|
|
inline Vec3b makeRainbow3B(float id)
|
|
{
|
|
id *= freeDebugParam3;
|
|
if(!(id > 0))
|
|
return Vec3b(255,255,255);
|
|
|
|
int icP = id;
|
|
float ifP = id-icP;
|
|
icP = icP%3;
|
|
|
|
if(icP == 0) return Vec3b(255*(1-ifP), 255*ifP, 0);
|
|
if(icP == 1) return Vec3b(0, 255*(1-ifP), 255*ifP);
|
|
if(icP == 2) return Vec3b(255*ifP, 0, 255*(1-ifP));
|
|
return Vec3b(255,255,255);
|
|
}
|
|
|
|
inline Vec3b makeJet3B(float id)
|
|
{
|
|
if(id <= 0) return Vec3b(128,0,0);
|
|
if(id >= 1) return Vec3b(0,0,128);
|
|
|
|
int icP = (id*8);
|
|
float ifP = (id*8)-icP;
|
|
|
|
if(icP == 0) return Vec3b(255*(0.5+0.5*ifP), 0, 0);
|
|
if(icP == 1) return Vec3b(255, 255*(0.5*ifP), 0);
|
|
if(icP == 2) return Vec3b(255, 255*(0.5+0.5*ifP), 0);
|
|
if(icP == 3) return Vec3b(255*(1-0.5*ifP), 255, 255*(0.5*ifP));
|
|
if(icP == 4) return Vec3b(255*(0.5-0.5*ifP), 255, 255*(0.5+0.5*ifP));
|
|
if(icP == 5) return Vec3b(0, 255*(1-0.5*ifP), 255);
|
|
if(icP == 6) return Vec3b(0, 255*(0.5-0.5*ifP), 255);
|
|
if(icP == 7) return Vec3b(0, 0, 255*(1-0.5*ifP));
|
|
return Vec3b(255,255,255);
|
|
}
|
|
|
|
inline Vec3b makeRedGreen3B(float val) // 0 = red, 1=green, 0.5=yellow.
|
|
{
|
|
if(val < 0) return Vec3b(0,0,255);
|
|
else if(val < 0.5) return Vec3b(0,255*2*val,255);
|
|
else if(val < 1) return Vec3b(0,255,255-255*2*(val-0.5));
|
|
else return Vec3b(0,255,0);
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
}
|