jiluo
/
DIC
3
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
DIC源码
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
1 Commit
1 Branch
0 Tags
120 MiB
Branch: master
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from 'master'
${ noResults }
DIC/OpenCorr/oc_sift.cpp

1498 lines
53 KiB
Raw Permalink Normal View History

first commit
3 months ago
#include "oc_sift.h"
namespace opencorr
{
//SIFT 2D
SIFT2D::SIFT2D()
{
sift_config.n_features = 0;
sift_config.n_octave_layers = 3;
sift_config.contrast_threshold = 0.04f; //default configuration in OpenCV
sift_config.edge_threshold = 10.f;
sift_config.sigma = 1.6f;
matching_ratio = 0.8f;
}
SIFT2D::~SIFT2D() {}
Sift2dConfig SIFT2D::getSiftConfig() const
{
return sift_config;
}
float SIFT2D::getMatchingRatio() const
{
return matching_ratio;
}
void SIFT2D::setSiftConfig(Sift2dConfig sift_config)
{
this->sift_config = sift_config;
}
void SIFT2D::setMatching(float matching_ratio)
{
this->matching_ratio = matching_ratio;
}
void SIFT2D::prepare()
{
ref_mat = &ref_img->cv_mat;
tar_mat = &tar_img->cv_mat;
}
void SIFT2D::compute()
{
//initialization, refer to opencv document for details
std::vector<cv::KeyPoint> ref_kp;
cv::Mat ref_descriptor;
cv::Ptr<cv::Feature2D> ref_sift = cv::SIFT::create(
sift_config.n_features,
sift_config.n_octave_layers,
sift_config.contrast_threshold,
sift_config.edge_threshold,
sift_config.sigma);
std::vector<cv::KeyPoint> tar_kp;
cv::Mat tar_descriptor;
cv::Ptr<cv::Feature2D> tar_sift = cv::SIFT::create(
sift_config.n_features,
sift_config.n_octave_layers,
sift_config.contrast_threshold,
sift_config.edge_threshold,
sift_config.sigma);
//extract features and construct their descriptor
#pragma omp parallel sections
{
#pragma omp section
{
ref_sift->detect(*ref_mat, ref_kp);
ref_sift->compute(*ref_mat, ref_kp, ref_descriptor);
}
#pragma omp section
{
tar_sift->detect(*tar_mat, tar_kp);
tar_sift->compute(*tar_mat, tar_kp, tar_descriptor);
}
}
//match the keypoints in the reference image with those in the target image
cv::FlannBasedMatcher matcher;
std::vector<std::vector<cv::DMatch>> matches;
matcher.knnMatch(ref_descriptor, tar_descriptor, matches, 2);
//check if the matching ratio is satisfied, then assign the matched keypoints to the queues
int matches_size = (int)matches.size();
#pragma omp parallel sections
{
#pragma omp section
{
for (int i = 0; i < matches_size; ++i)
{
if (matches[i][0].distance < matching_ratio * matches[i][1].distance)
{
Point2D ref_candidate(ref_kp[i].pt.x, ref_kp[i].pt.y);
ref_matched_kp.push_back(ref_candidate);
}
}
}
#pragma omp section
{
for (int j = 0; j < matches_size; ++j)
{
if (matches[j][0].distance < matching_ratio * matches[j][1].distance)
{
Point2D tar_candidate(tar_kp[matches[j][0].trainIdx].pt.x, tar_kp[matches[j][0].trainIdx].pt.y);
tar_matched_kp.push_back(tar_candidate);
}
}
}
}
}
void SIFT2D::clear()
{
std::vector<Point2D>().swap(ref_matched_kp);
std::vector<Point2D>().swap(tar_matched_kp);
}
SIFT3D::SIFT3D()
{
sift_config.n_octave_layers = 3;
sift_config.min_dimension = 8;
sift_config.alpha = 0.1f;
sift_config.beta = 0.9f;
sift_config.gamma = 0.4f;
sift_config.sigma_source = 1.15f;
sift_config.sigma_base = 1.6f;
sift_config.gradient_threshold = 0.0000000001f;
sift_config.truncate_threshold = 0.2f * 128 / 768;
matching_ratio = 0.85f;
physical_unit[0] = 1.f;
physical_unit[1] = 1.f;
physical_unit[2] = 1.f;
}
SIFT3D::~SIFT3D() {}
Sift3dConfig SIFT3D::getSiftConfig() const
{
return sift_config;
}
float SIFT3D::getPhysicalUnit(int dim) const
{
float dim_unit = 0.f;
switch (dim)
{
case 0:
dim_unit = physical_unit[0];
break;
case 1:
dim_unit = physical_unit[1];
break;
case 2:
dim_unit = physical_unit[2];
break;
}
return dim_unit;
}
float SIFT3D::getMatchingRatio() const
{
return matching_ratio;
}
void SIFT3D::setSiftConfig(Sift3dConfig sift_config)
{
this->sift_config = sift_config;
}
void SIFT3D::setPhysicalUnit(float unit_x, float unit_y, float unit_z)
{
physical_unit[0] = unit_x;
physical_unit[1] = unit_y;
physical_unit[2] = unit_z;
}
void SIFT3D::setMatchingRatio(float matching_ratio)
{
this->matching_ratio = matching_ratio;
}
void SIFT3D::prepare()
{
//initialize icosahedron
icosahedron[0] = TriangleTile(Point3D(0.000000f, -0.525731f, 0.850651f), 1, Point3D(0.000000f, 0.525731f, 0.850651f), 0, Point3D(0.850651f, 0.000000f, 0.525731f), 8);
icosahedron[1] = TriangleTile(Point3D(0.850651f, 0.000000f, 0.525731f), 8, Point3D(0.000000f, 0.525731f, 0.850651f), 0, Point3D(0.525731f, 0.850651f, 0.000000f), 4);
icosahedron[2] = TriangleTile(Point3D(0.525731f, 0.850651f, 0.000000f), 4, Point3D(0.000000f, 0.525731f, 0.850651f), 0, Point3D(-0.525731f, 0.850651f, 0.000000f), 5);
icosahedron[3] = TriangleTile(Point3D(-0.525731f, 0.850651f, 0.000000f), 5, Point3D(0.000000f, 0.525731f, 0.850651f), 0, Point3D(-0.850651f, 0.000000f, 0.525731f), 9);
icosahedron[4] = TriangleTile(Point3D(-0.850651f, 0.000000f, 0.525731f), 9, Point3D(0.000000f, 0.525731f, 0.850651f), 0, Point3D(0.000000f, -0.525731f, 0.850651f), 1);
icosahedron[5] = TriangleTile(Point3D(0.525731f, -0.850651f, 0.000000f), 6, Point3D(0.000000f, -0.525731f, 0.850651f), 1, Point3D(0.850651f, 0.000000f, 0.525731f), 8);
icosahedron[6] = TriangleTile(Point3D(0.525731f, -0.850651f, 0.000000f), 6, Point3D(0.850651f, 0.000000f, 0.525731f), 8, Point3D(0.850651f, 0.000000f, -0.525731f), 10);
icosahedron[7] = TriangleTile(Point3D(0.850651f, 0.000000f, -0.525731f), 10, Point3D(0.850651f, 0.000000f, 0.525731f), 8, Point3D(0.525731f, 0.850651f, 0.000000f), 4);
icosahedron[8] = TriangleTile(Point3D(0.850651f, 0.000000f, -0.525731f), 10, Point3D(0.525731f, 0.850651f, 0.000000f), 4, Point3D(0.000000f, 0.525731f, -0.850651f), 2);
icosahedron[9] = TriangleTile(Point3D(0.000000f, 0.525731f, -0.850651f), 2, Point3D(0.525731f, 0.850651f, 0.000000f), 4, Point3D(-0.525731f, 0.850651f, 0.000000f), 5);
icosahedron[10] = TriangleTile(Point3D(0.000000f, 0.525731f, -0.850651f), 2, Point3D(-0.525731f, 0.850651f, 0.000000f), 5, Point3D(-0.850651f, 0.000000f, -0.525731f), 11);
icosahedron[11] = TriangleTile(Point3D(-0.850651f, 0.000000f, -0.525731f), 11, Point3D(-0.525731f, 0.850651f, 0.000000f), 5, Point3D(-0.850651f, 0.000000f, 0.525731f), 9);
icosahedron[12] = TriangleTile(Point3D(-0.850651f, 0.000000f, -0.525731f), 11, Point3D(-0.850651f, 0.000000f, 0.525731f), 9, Point3D(-0.525731f, -0.850651f, 0.000000f), 7);
icosahedron[13] = TriangleTile(Point3D(-0.525731f, -0.850651f, 0.000000f), 7, Point3D(-0.850651f, 0.000000f, 0.525731f), 9, Point3D(0.000000f, -0.525731f, 0.850651f), 1);
icosahedron[14] = TriangleTile(Point3D(-0.525731f, -0.850651f, 0.000000f), 7, Point3D(0.000000f, -0.525731f, 0.850651f), 1, Point3D(0.525731f, -0.850651f, 0.000000f), 6);
icosahedron[15] = TriangleTile(Point3D(0.525731f, -0.850651f, 0.000000f), 6, Point3D(0.000000f, -0.525731f, -0.850651f), 3, Point3D(-0.525731f, -0.850651f, 0.000000f), 7);
icosahedron[16] = TriangleTile(Point3D(-0.525731f, -0.850651f, 0.000000f), 7, Point3D(0.000000f, -0.525731f, -0.850651f), 3, Point3D(-0.850651f, 0.000000f, -0.525731f), 11);
icosahedron[17] = TriangleTile(Point3D(-0.850651f, 0.000000f, -0.525731f), 11, Point3D(0.000000f, -0.525731f, -0.850651f), 3, Point3D(0.000000f, 0.525731f, -0.850651f), 2);
icosahedron[18] = TriangleTile(Point3D(0.000000f, 0.525731f, -0.850651f), 2, Point3D(0.000000f, -0.525731f, -0.850651f), 3, Point3D(0.850651f, 0.000000f, -0.525731f), 10);
icosahedron[19] = TriangleTile(Point3D(0.850651f, 0.000000f, -0.525731f), 10, Point3D(0.000000f, -0.525731f, -0.850651f), 3, Point3D(0.525731f, -0.850651f, 0.000000f), 6);
}
void SIFT3D::compute()
{
//initialization
std::vector<Layer3D> gaussian_pyramid, dog_pyramid;
std::vector<Keypoint3D> ref_kp, tar_kp;
int ref_amount, tar_amount; //amount of extracted kp
//CAUTION: omp parallel for has been employed in the following functions,
//no need to run these functions in parallel using omp parallel sections
//extract features from the reference image
createGaussianPyramid(ref_img, gaussian_pyramid);
createDogPyramid(gaussian_pyramid, dog_pyramid);
detectExtrema(dog_pyramid, ref_kp);
assignOrientation(ref_kp, gaussian_pyramid);
//construct descriptors
ref_amount = (int)ref_kp.size();
float** ref_descriptor = new2D(ref_amount, 768);
constructDescriptor(ref_kp, gaussian_pyramid, ref_descriptor);
std::cout << ref_kp.size() << " features are extracted from the reference image." << std::endl;
//clear the data in pyramids
clearPyramid(gaussian_pyramid);
clearPyramid(dog_pyramid);
//extract features from the target image
createGaussianPyramid(tar_img, gaussian_pyramid);
createDogPyramid(gaussian_pyramid, dog_pyramid);
detectExtrema(dog_pyramid, tar_kp);
assignOrientation(tar_kp, gaussian_pyramid);
//construct descriptors
tar_amount = (int)tar_kp.size();
float** tar_descriptor = new2D(tar_amount, 768);
constructDescriptor(tar_kp, gaussian_pyramid, tar_descriptor);
std::cout << tar_kp.size() << " features are extracted from the target image." << std::endl;
//clear pyramids and release memory
clearPyramid(gaussian_pyramid);
clearPyramid(dog_pyramid);
//monodirectional matching, but many-to-one correspondences are eliminated through reverse matching
monodirectionalMatch(ref_kp, ref_descriptor, tar_kp, tar_descriptor, ref_matched_kp, tar_matched_kp);
//rigorous birectional matching, less matched keypoints, more computation time
//bidirectionalMatch(ref_kp, ref_descriptor, tar_kp, tar_descriptor, ref_matched_kp, tar_matched_kp);
//release memory
delete2D(ref_descriptor);
delete2D(tar_descriptor);
}
void SIFT3D::clear()
{
std::vector<Point3D>().swap(ref_matched_kp);
std::vector<Point3D>().swap(tar_matched_kp);
}
void SIFT3D::initializeIcosahedron(TriangleTile* icosahedron)
{
//vertex triplets forming the triangles of an icosahedron
int triangle_triplets[] =
{
1, 0, 8,
8, 0, 4,
4, 0, 5,
5, 0, 9,
9, 0, 1,
6, 1, 8,
6, 8, 10,
10, 8, 4,
10, 4, 2,
2, 4, 5,
2, 5, 11,
11, 5, 9,
11, 9, 7,
7, 9, 1,
7, 1, 6,
6, 3, 7,
7, 3, 11,
11, 3, 2,
2, 3, 10,
10, 3, 6
};
//vertices of a icosahedron
float triangle_vertices[] =
{
0.0000000f, -0.5257311f, 0.8506508f, 0.0000000f, 0.5257311f, 0.8506508f, 0.8506508f, 0.0000000f, 0.5257311f,
0.8506508f, 0.0000000f, 0.5257311f, 0.0000000f, 0.5257311f, 0.8506508f, 0.5257311f, 0.8506508f, 0.0000000f,
0.5257311f, 0.8506508f, 0.0000000f, 0.0000000f, 0.5257311f, 0.8506508f, -0.5257311f, 0.8506508f, 0.0000000f,
-0.5257311f, 0.8506508f, 0.0000000f, 0.0000000f, 0.5257311f, 0.8506508f, -0.8506508f, 0.0000000f, 0.5257311f,
-0.8506508f, 0.0000000f, 0.5257311f, 0.0000000f, 0.5257311f, 0.8506508f, 0.0000000f, -0.5257311f, 0.8506508f,
0.5257311f, -0.8506508f, 0.0000000f, 0.0000000f, -0.5257311f, 0.8506508f, 0.8506508f, 0.0000000f, 0.5257311f,
0.5257311f, -0.8506508f, 0.0000000f, 0.8506508f, 0.0000000f, 0.5257311f, 0.8506508f, 0.0000000f, -0.5257311f,
0.8506508f, 0.0000000f, -0.5257311f, 0.8506508f, 0.0000000f, 0.5257311f, 0.5257311f, 0.8506508f, 0.0000000f,
0.8506508f, 0.0000000f, -0.5257311f, 0.5257311f, 0.8506508f, 0.0000000f, 0.0000000f, 0.5257311f, -0.8506508f,
0.0000000f, 0.5257311f, -0.8506508f, 0.5257311f, 0.8506508f, 0.0000000f, -0.5257311f, 0.8506508f, 0.0000000f,
0.0000000f, 0.5257311f, -0.8506508f, -0.5257311f, 0.8506508f, 0.0000000f, -0.8506508f, 0.0000000f, -0.5257311f,
-0.8506508f, 0.0000000f, -0.5257311f, -0.5257311f, 0.8506508f, 0.0000000f, -0.8506508f, 0.0000000f, 0.5257311f,
-0.8506508f, 0.0000000f, -0.5257311f, -0.8506508f, 0.0000000f, 0.5257311f, -0.5257311f, -0.8506508f, 0.0000000f,
-0.5257311f, -0.8506508f, 0.0000000f, -0.8506508f, 0.0000000f, 0.5257311f, 0.0000000f, -0.5257311f, 0.8506508f,
-0.5257311f, -0.8506508f, 0.0000000f, 0.0000000f, -0.5257311f, 0.8506508f, 0.5257311f, -0.8506508f, 0.0000000f,
0.5257311f, -0.8506508f, 0.0000000f, 0.0000000f, -0.5257311f, -0.8506508f, -0.5257311f, -0.8506508f, 0.0000000f,
-0.5257311f, -0.8506508f, 0.0000000f, 0.0000000f, -0.5257311f, -0.8506508f, -0.8506508f, 0.0000000f, -0.5257311f,
-0.8506508f, 0.0000000f, -0.5257311f, 0.0000000f, -0.5257311f, -0.8506508f, 0.0000000f, 0.5257311f, -0.8506508f,
0.0000000f, 0.5257311f, -0.8506508f, 0.0000000f, -0.5257311f, -0.8506508f, 0.8506508f, 0.0000000f, -0.5257311f,
0.8506508f, 0.0000000f, -0.5257311f, 0.0000000f, -0.5257311f, -0.8506508f, 0.5257311f, -0.8506508f, 0.0000000f
};
for (int i = 0; i < 20; ++i)
{
for (int j = 0; j < 3; ++j)
{
icosahedron[i].vertex_idx[j] = triangle_triplets[i * 3 + j];
icosahedron[i].vertices[j].x = triangle_vertices[(i * 3 + j) * 3 + 0];
icosahedron[i].vertices[j].y = triangle_vertices[(i * 3 + j) * 3 + 1];
icosahedron[i].vertices[j].z = triangle_vertices[(i * 3 + j) * 3 + 2];
}
}
}
void SIFT3D::gaussianBlur(float*** src_img, float*** dst_img, int* dim_xyz, float* unit_xyz, float sigma)
{
//determine the dimension with maximum physical unit
float unit_max = unit_xyz[0] > unit_xyz[1] ? unit_xyz[0] : unit_xyz[1];
unit_max = unit_max > unit_xyz[2] ? unit_max : unit_xyz[2];
//size of kernel matrix = (6 * sigma + 1) * (6 * sigma + 1) * (6 * sigma + 1)
int kernel_radius;
if (sigma > 0)
{
kernel_radius = ceil(3.f * sigma) > 1 ? ceil(3.f * sigma) : 1;
}
else
{
sigma = 0.f;
kernel_radius = 1;
}
//create the convolution kernel for x-axis
int radius_x = kernel_radius * floor(unit_max / unit_xyz[0] + 0.5f);
float* kernel_x = new float[radius_x + 1];
kernel_x[0] = 1.f; //G(0,sigma) = exp(-0.5f * 0 * 0) = 1
for (int i = 1; i <= radius_x; i++)
{
float x = i / (sigma + FLT_EPSILON);
kernel_x[i] = exp(-0.5f * x * x);
kernel_x[0] += (kernel_x[i] * 2.f);
}
//normalize the kernel
kernel_x[0] = 1.f / kernel_x[0];
for (int i = 1; i <= radius_x; i++)
{
kernel_x[i] *= kernel_x[0];
}
//create an intermediate matrix for 3D convolution
float*** conv_buffer = new3D(dim_xyz[2], dim_xyz[1], dim_xyz[0]);
//convolution along x-axis
#pragma omp parallel for
for (int i = 0; i < dim_xyz[2]; i++)
{
for (int j = 0; j < dim_xyz[1]; j++)
{
for (int k = radius_x; k < dim_xyz[0] - radius_x; k++)
{
dst_img[i][j][k] = kernel_x[0] * src_img[i][j][k];
for (int r = 1; r <= radius_x; r++)
{
dst_img[i][j][k] += kernel_x[r] * (src_img[i][j][k - r] + src_img[i][j][k + r]);
}
}
//proess the points close to the boundary of image using mirror extension
for (int k = 0; k < radius_x; k++)
{
dst_img[i][j][k] = kernel_x[0] * src_img[i][j][k];
for (int r = 1; r <= radius_x; r++)
{
dst_img[i][j][k] += kernel_x[r] * (src_img[i][j][mirrorLow(k - r, 0)] + src_img[i][j][mirrorHigh(k + r, dim_xyz[0] - 1)]);
}
}
for (int k = dim_xyz[0] - radius_x; k < dim_xyz[0]; k++)
{
dst_img[i][j][k] = kernel_x[0] * src_img[i][j][k];
for (int r = 1; r <= radius_x; r++)
{
dst_img[i][j][k] += kernel_x[r] * (src_img[i][j][mirrorLow(k - r, 0)] + src_img[i][j][mirrorHigh(k + r, dim_xyz[0] - 1)]);
}
}
}
}
//create the convolution kernel for y-axis
int radius_y = kernel_radius * floor(unit_max / unit_xyz[1] + 0.5);
float* kernel_y = new float[radius_y + 1];
kernel_y[0] = 1.f; //G(0,sigma) = exp(-0.5f * 0 * 0) = 1
for (int i = 1; i <= radius_y; i++)
{
float y = i / (sigma + FLT_EPSILON);
kernel_y[i] = exp(-0.5f * y * y);
kernel_y[0] += (kernel_y[i] * 2.f);
}
//normalize the kernel
kernel_y[0] = 1.f / kernel_y[0];
for (int i = 1; i <= radius_y; i++)
{
kernel_y[i] *= kernel_y[0];
}
//convolution along y-axis
#pragma omp parallel for
for (int k = 0; k < dim_xyz[0]; k++)
{
for (int i = 0; i < dim_xyz[2]; i++)
{
for (int j = radius_y; j < dim_xyz[1] - radius_y; j++)
{
conv_buffer[i][j][k] = kernel_y[0] * dst_img[i][j][k];
for (int r = 1; r <= radius_y; r++)
{
conv_buffer[i][j][k] += kernel_y[r] * (dst_img[i][j - r][k] + dst_img[i][j + r][k]);
}
}
//proess the points close to the boundary of image using mirror extension
for (int j = 0; j < radius_y; j++)
{
conv_buffer[i][j][k] = kernel_y[0] * dst_img[i][j][k];
for (int r = 1; r <= radius_y; r++)
{
conv_buffer[i][j][k] += kernel_y[r] * (dst_img[i][mirrorLow(j - r, 0)][k] + dst_img[i][mirrorHigh(j + r, dim_xyz[1] - 1)][k]);
}
}
for (int j = dim_xyz[1] - radius_y; j < dim_xyz[1]; j++)
{
conv_buffer[i][j][k] = kernel_y[0] * dst_img[i][j][k];
for (int r = 1; r <= radius_y; r++)
{
conv_buffer[i][j][k] += kernel_y[r] * (dst_img[i][mirrorLow(j - r, 0)][k] + dst_img[i][mirrorHigh(j + r, dim_xyz[1] - 1)][k]);
}
}
}
}
//create the convolution kernel for z-axis
int radius_z = kernel_radius * floor(unit_max / unit_xyz[2] + 0.5f);
float* kernel_z = new float[radius_z + 1];
kernel_z[0] = 1.f; //G(0,sigma) = exp(-0.5f * 0 * 0) = 1
for (int i = 1; i <= radius_z; i++)
{
float z = i / (sigma + FLT_EPSILON);
kernel_z[i] = exp(-0.5f * z * z);
kernel_z[0] += (kernel_z[i] * 2.f);
}
//normalize the kernel
kernel_z[0] = 1.f / kernel_z[0];
for (int i = 1; i <= radius_z; i++)
{
kernel_z[i] *= kernel_z[0];
}
//convolution along z-axis
#pragma omp parallel for
for (int j = 0; j < dim_xyz[1]; j++)
{
for (int k = 0; k < dim_xyz[0]; k++)
{
for (int i = radius_z; i < dim_xyz[2] - radius_z; i++)
{
dst_img[i][j][k] = kernel_z[0] * conv_buffer[i][j][k];
for (int r = 1; r <= radius_z; r++)
{
dst_img[i][j][k] += kernel_z[r] * (conv_buffer[i - r][j][k] + conv_buffer[i + r][j][k]);
}
}
//proess the points close to the boundary of image using mirror extension
for (int i = 0; i < radius_z; i++)
{
dst_img[i][j][k] = kernel_z[0] * conv_buffer[i][j][k];
for (int r = 1; r <= radius_z; r++)
{
dst_img[i][j][k] += kernel_z[r] * (conv_buffer[mirrorLow(i - r, 0)][j][k] + conv_buffer[mirrorHigh(i + r, dim_xyz[2] - 1)][j][k]);
}
}
for (int i = dim_xyz[2] - radius_z; i < dim_xyz[2]; i++)
{
dst_img[i][j][k] = kernel_z[0] * conv_buffer[i][j][k];
for (int r = 1; r <= radius_z; r++)
{
dst_img[i][j][k] += kernel_z[r] * (conv_buffer[mirrorLow(i - r, 0)][j][k] + conv_buffer[mirrorHigh(i + r, dim_xyz[2] - 1)][j][k]);
}
}
}
}
//release memory
delete3D(conv_buffer);
delete[] kernel_x;
delete[] kernel_y;
delete[] kernel_z;
}
void SIFT3D::downSampling(float*** src_img, float*** dst_img, int* dst_dim_xyz)
{
#pragma omp parallel for
for (int i = 0; i < dst_dim_xyz[2]; i++)
{
for (int j = 0; j < dst_dim_xyz[1]; j++)
{
for (int k = 0; k < dst_dim_xyz[0]; k++)
{
dst_img[i][j][k] = src_img[i * 2][j * 2][k * 2];
}
}
}
}
void SIFT3D::clearPyramid(std::vector<Layer3D>& pyramid)
{
int layer_number = (int)pyramid.size();
for (int i = 0; i < layer_number; i++)
{
if (pyramid[i].vol_mat != nullptr)
{
delete3D(pyramid[i].vol_mat);
}
}
std::vector<Layer3D>().swap(pyramid);
}
int SIFT3D::cartisan2Barycentric(Point3D& cart_coor, Point3D& bary_coor, TriangleTile& triangle)
{
Point3D e1, e2, t, p, q;
float k;
e1 = triangle.vertices[1] - triangle.vertices[0]; //V2 - V1
e2 = triangle.vertices[2] - triangle.vertices[0]; //V3 - V1
t = -1 * triangle.vertices[0]; //O(0,0,0) - V1
//p = cart_coor x e2
p = cart_coor / e2; //cross porduct
//q = t x e1;
q = t / e1; //cross porduct
float det = e1 * p; //dot product
if (fabs(det) < FLT_EPSILON * 10.f)
{
return -1;
}
float det_inv = 1.f / det;
bary_coor.z = det_inv * (cart_coor * q);
bary_coor.y = det_inv * (p * t);
bary_coor.x = 1.f - bary_coor.y - bary_coor.z;
k = det_inv * (q * e2);
if (k < 0)
{
return -1;
}
if (bary_coor.x < -FLT_EPSILON * 10.f || bary_coor.y < -FLT_EPSILON * 10.f || bary_coor.z < -FLT_EPSILON * 10.f)
{
return -1;
}
//verify k * cart_coor = bary_coor.x * V1 + bary_coor.y * V2 + bary_coor.z * V3, this check can be omitted
Point3D residual = k * cart_coor - bary_coor.x * triangle.vertices[0] - bary_coor.y * triangle.vertices[1] - bary_coor.z * triangle.vertices[2];
if (residual.vectorNorm() > FLT_EPSILON * 10.f)
{
return -1;
}
return 1;
}
int SIFT3D::bruteforceMatch(std::vector<Keypoint3D>& kp1, float** descriptor1, std::vector<Keypoint3D>& kp2, float** descriptor2, int* matched_idx)
{
int kp1_amount = (int)kp1.size();
int kp2_amount = (int)kp2.size();
float matching_ratio_square = matching_ratio * matching_ratio;
int match_counter = 0;
//match each kp1 with all kp2
#pragma omp parallel for reduction (+:match_counter)
for (int i = 0; i < kp1_amount; i++)
{
//create two arrays for ratio check
int candidate_idx[2] = { -1,-1 };//[0]: the shortest, [1]: the second shortest,
float candidate_distance[2] = { FLT_MAX,FLT_MAX }; //[0]: the shortest, [1]: the second shortest
for (int j = 0; j < kp2_amount; j++)
{
//calculate squared Euclidean distance between kp1 and kp2
float squared_distance = 0;
for (int k = 0; k < 768; k++)
{
float component_difference = descriptor1[i][k] - descriptor2[j][k];
squared_distance += component_difference * component_difference;
}
//store the information of kp with the shortest distance or the second shortest distance
if (squared_distance < candidate_distance[0])
{
candidate_idx[1] = candidate_idx[0];
candidate_distance[1] = candidate_distance[0];
candidate_idx[0] = j;
candidate_distance[0] = squared_distance;
}
else if (squared_distance < candidate_distance[1])
{
candidate_idx[1] = j;
candidate_distance[1] = squared_distance;
}
}
//check if the matching ratio is satisfied
if (candidate_distance[0] < matching_ratio_square * candidate_distance[1])
{
matched_idx[i] = candidate_idx[0];
match_counter++;
}
}
return match_counter;
}
void SIFT3D::createGaussianPyramid(Image3D* vol_img, std::vector<Layer3D>& gaussian_pyramid)
{
//determine the minimum dimension of the input image
int dim_min = vol_img->dim_x < vol_img->dim_y ? vol_img->dim_x : vol_img->dim_y;
dim_min = dim_min < vol_img->dim_z ? dim_min : vol_img->dim_z;
//set the octave and height of pyramid
sift_config.n_octave = floor(log2((float)dim_min) - log2((float)sift_config.min_dimension)) + 1;
sift_config.n_octave = sift_config.n_octave > 0 ? sift_config.n_octave : 1;
int layer_per_octave = sift_config.n_octave_layers + 3; //no local extrema search in the bottom layer and the top layer in each octave of DoG pyramid
int layer_number = sift_config.n_octave * layer_per_octave; //overall number of layers in a pyramid
gaussian_pyramid.resize(layer_number);
//set the bottom layer
int x_length = vol_img->dim_x;
int y_length = vol_img->dim_y;
int z_length = vol_img->dim_z;
float x_unit = getPhysicalUnit(0);
float y_unit = getPhysicalUnit(1);
float z_unit = getPhysicalUnit(2);
float kappa = pow(2.f, 1.f / sift_config.n_octave_layers);
gaussian_pyramid[0].dim_xyz[0] = x_length;
gaussian_pyramid[0].dim_xyz[1] = y_length;
gaussian_pyramid[0].dim_xyz[2] = z_length;
gaussian_pyramid[0].unit_xyz[0] = x_unit;
gaussian_pyramid[0].unit_xyz[1] = y_unit;
gaussian_pyramid[0].unit_xyz[2] = z_unit;
gaussian_pyramid[0].octave = 0;
gaussian_pyramid[0].scale = 1.f / kappa * sift_config.sigma_base;
gaussian_pyramid[0].sigma = sqrt(gaussian_pyramid[0].scale * gaussian_pyramid[0].scale - sift_config.sigma_source * sift_config.sigma_source);
gaussian_pyramid[0].vol_mat = new3D(z_length, y_length, x_length);
//set the other layers
for (int i = 1; i < layer_number; i++)
{
gaussian_pyramid[i].octave = floor(1.f * i / layer_per_octave); //get current octave
int layer_in_octave = i % layer_per_octave; //get layer position in current octave
if (layer_in_octave == 0) //bottom layer of current octave
{
x_length /= 2;
y_length /= 2;
z_length /= 2;
x_unit *= 2;
y_unit *= 2;
z_unit *= 2;
gaussian_pyramid[i].scale = gaussian_pyramid[(gaussian_pyramid[i].octave - 1) * layer_per_octave + sift_config.n_octave_layers].scale;
}
else
{
gaussian_pyramid[i].scale = kappa * gaussian_pyramid[i - 1].scale;
gaussian_pyramid[i].sigma = sqrt(kappa * kappa - 1.f) * gaussian_pyramid[layer_in_octave - 1].scale;
}
gaussian_pyramid[i].dim_xyz[0] = x_length;
gaussian_pyramid[i].dim_xyz[1] = y_length;
gaussian_pyramid[i].dim_xyz[2] = z_length;
gaussian_pyramid[i].unit_xyz[0] = x_unit;
gaussian_pyramid[i].unit_xyz[1] = y_unit;
gaussian_pyramid[i].unit_xyz[2] = z_unit;
gaussian_pyramid[i].vol_mat = new3D(z_length, y_length, x_length);
}
//fill each layer with blurred image
gaussianBlur(vol_img->vol_mat, gaussian_pyramid[0].vol_mat, gaussian_pyramid[0].dim_xyz, gaussian_pyramid[0].unit_xyz, gaussian_pyramid[0].sigma); //bottom layer
for (int i = 1; i < layer_number; i++)
{
if (i % layer_per_octave == 0)
{
downSampling(gaussian_pyramid[i - 3].vol_mat, gaussian_pyramid[i].vol_mat, gaussian_pyramid[i].dim_xyz); //bottom layer in current octave
}
else
{
gaussianBlur(gaussian_pyramid[i - 1].vol_mat, gaussian_pyramid[i].vol_mat, gaussian_pyramid[i].dim_xyz, gaussian_pyramid[i].unit_xyz, gaussian_pyramid[i].sigma);
}
}
}
void SIFT3D::createDogPyramid(std::vector<Layer3D>& gaussian_pyramid, std::vector<Layer3D>& dog_pyramid)
{
int layer_per_octave = sift_config.n_octave_layers + 2; //number of layers in each octave
dog_pyramid.resize(sift_config.n_octave * layer_per_octave);
#pragma omp parallel for
for (int m = 0; m < sift_config.n_octave; m++)
{
for (int n = 0; n < layer_per_octave; n++)
{
int g_idx = m * (sift_config.n_octave_layers + 3) + n;
int d_idx = m * layer_per_octave + n;
dog_pyramid[d_idx].dim_xyz[0] = gaussian_pyramid[g_idx].dim_xyz[0];
dog_pyramid[d_idx].dim_xyz[1] = gaussian_pyramid[g_idx].dim_xyz[1];
dog_pyramid[d_idx].dim_xyz[2] = gaussian_pyramid[g_idx].dim_xyz[2];
dog_pyramid[d_idx].unit_xyz[0] = gaussian_pyramid[g_idx].unit_xyz[0];
dog_pyramid[d_idx].unit_xyz[1] = gaussian_pyramid[g_idx].unit_xyz[1];
dog_pyramid[d_idx].unit_xyz[2] = gaussian_pyramid[g_idx].unit_xyz[2];
dog_pyramid[d_idx].octave = m;
dog_pyramid[d_idx].scale = gaussian_pyramid[g_idx].scale;
dog_pyramid[d_idx].vol_mat = new3D(dog_pyramid[d_idx].dim_xyz[2], dog_pyramid[d_idx].dim_xyz[1], dog_pyramid[d_idx].dim_xyz[0]);
dog_pyramid[d_idx].max_abs = -1.f;
for (int i = 0; i < dog_pyramid[d_idx].dim_xyz[2]; i++)
{
for (int j = 0; j < dog_pyramid[d_idx].dim_xyz[1]; j++)
{
for (int k = 0; k < dog_pyramid[d_idx].dim_xyz[0]; k++)
{
dog_pyramid[d_idx].vol_mat[i][j][k] = gaussian_pyramid[g_idx + 1].vol_mat[i][j][k] - gaussian_pyramid[g_idx].vol_mat[i][j][k];
float dog_abs = fabs(dog_pyramid[d_idx].vol_mat[i][j][k]);
dog_pyramid[d_idx].max_abs = dog_pyramid[d_idx].max_abs < dog_abs ? dog_abs : dog_pyramid[d_idx].max_abs;
}
}
}
}
}
}
void SIFT3D::detectExtrema(std::vector<Layer3D>& dog_pyramid, std::vector<Keypoint3D>& kp_queue)
{
for (int m = 0; m < sift_config.n_octave; m++)
{
//skip the bottom layer and the top layer
for (int n = 1; n < (sift_config.n_octave_layers + 1); n++)
{
int layer_idx = m * (sift_config.n_octave_layers + 2) + n;
for (int i = IMG_BORDER; i < dog_pyramid[layer_idx].dim_xyz[2] - IMG_BORDER; i++)
{
for (int j = IMG_BORDER; j < dog_pyramid[layer_idx].dim_xyz[1] - IMG_BORDER; j++)
{
for (int k = IMG_BORDER; k < dog_pyramid[layer_idx].dim_xyz[0] - IMG_BORDER; k++)
{
float dog_value = dog_pyramid[layer_idx].vol_mat[i][j][k];
//check if the DoG value is large enough
if (fabs(dog_value) >= sift_config.alpha * dog_pyramid[layer_idx].max_abs)
{
//check if the DoG value at current position is greater or less than all the eight neighbors
if ((dog_value > dog_pyramid[layer_idx].vol_mat[i - 1][j][k]
&& dog_value > dog_pyramid[layer_idx].vol_mat[i + 1][j][k]
&& dog_value > dog_pyramid[layer_idx].vol_mat[i][j - 1][k]
&& dog_value > dog_pyramid[layer_idx].vol_mat[i][j + 1][k]
&& dog_value > dog_pyramid[layer_idx].vol_mat[i][j][k - 1]
&& dog_value > dog_pyramid[layer_idx].vol_mat[i][j][k + 1]
&& dog_value > dog_pyramid[layer_idx - 1].vol_mat[i][j][k]
&& dog_value > dog_pyramid[layer_idx + 1].vol_mat[i][j][k])
|| (dog_value < dog_pyramid[layer_idx].vol_mat[i - 1][j][k]
&& dog_value < dog_pyramid[layer_idx].vol_mat[i + 1][j][k]
&& dog_value < dog_pyramid[layer_idx].vol_mat[i][j - 1][k]
&& dog_value < dog_pyramid[layer_idx].vol_mat[i][j + 1][k]
&& dog_value < dog_pyramid[layer_idx].vol_mat[i][j][k - 1]
&& dog_value < dog_pyramid[layer_idx].vol_mat[i][j][k + 1]
&& dog_value < dog_pyramid[layer_idx - 1].vol_mat[i][j][k]
&& dog_value < dog_pyramid[layer_idx + 1].vol_mat[i][j][k]))
{
Keypoint3D keypoint_candidate;
keypoint_candidate.coor_layer.x = k;
keypoint_candidate.coor_layer.y = j;
keypoint_candidate.coor_layer.z = i;
keypoint_candidate.layer = n; //position of layer in its octave
keypoint_candidate.octave = m;
keypoint_candidate.scale = dog_pyramid[layer_idx].scale;
kp_queue.push_back(keypoint_candidate);
}
}
}
}
}
}
}
}
void SIFT3D::assignOrientation(std::vector<Keypoint3D>& kp_queue, std::vector<Layer3D>& gaussian_pyramid)
{
std::vector<Keypoint3D> kp_candidate(kp_queue);
int candidate_amount = (int)kp_candidate.size();
#pragma omp parallel for
for (int m = 0; m < candidate_amount; m++)
{
int g_idx = kp_candidate[m].layer + kp_candidate[m].octave * (sift_config.n_octave_layers + 3);
bool available_kp = true;
float sigma_w = 1.5f * kp_candidate[m].scale;
float window_radius = 3.f * sigma_w;
//estimate the boundary of spherical window in image
int x_min = floor(kp_candidate[m].coor_layer.x - window_radius / gaussian_pyramid[g_idx].unit_xyz[0]);
x_min = x_min > IMG_BORDER ? x_min : IMG_BORDER;
int x_max = ceil(kp_candidate[m].coor_layer.x + window_radius / gaussian_pyramid[g_idx].unit_xyz[0]);
x_max = x_max < gaussian_pyramid[g_idx].dim_xyz[0] - IMG_BORDER ? x_max : gaussian_pyramid[g_idx].dim_xyz[0] - IMG_BORDER;
int y_min = floor(kp_candidate[m].coor_layer.y - window_radius / gaussian_pyramid[g_idx].unit_xyz[1]);
y_min = y_min > IMG_BORDER ? y_min : IMG_BORDER;
int y_max = ceil(kp_candidate[m].coor_layer.y + window_radius * gaussian_pyramid[g_idx].unit_xyz[1]);
y_max = y_max < gaussian_pyramid[g_idx].dim_xyz[1] - IMG_BORDER ? y_max : gaussian_pyramid[g_idx].dim_xyz[1] - IMG_BORDER;
int z_min = floor(kp_candidate[m].coor_layer.z - window_radius / gaussian_pyramid[g_idx].unit_xyz[2]);
z_min = z_min > IMG_BORDER ? z_min : IMG_BORDER;
int z_max = ceil(kp_candidate[m].coor_layer.z + window_radius / gaussian_pyramid[g_idx].unit_xyz[2]);
z_max = z_max < gaussian_pyramid[g_idx].dim_xyz[2] - IMG_BORDER ? z_max : gaussian_pyramid[g_idx].dim_xyz[2] - IMG_BORDER;
//directional derivative d(x)
float d_mat_x = 0.f;
float d_mat_y = 0.f;
float d_mat_z = 0.f;
//initialize structure tensor
float structure_tensor[9] = { 0.f };
//construct the structure tensor
for (int i = z_min; i < z_max; i++)
{
for (int j = y_min; j < y_max; j++)
{
for (int k = x_min; k < x_max; k++)
{
Point3D img_coor(k, j, i); //location with voxel unit
Point3D phys_corr; //local coordinate using physical unit
phys_corr.x = (img_coor.x - kp_candidate[m].coor_layer.x) * gaussian_pyramid[g_idx].unit_xyz[0];
phys_corr.y = (img_coor.y - kp_candidate[m].coor_layer.y) * gaussian_pyramid[g_idx].unit_xyz[1];
phys_corr.z = (img_coor.z - kp_candidate[m].coor_layer.z) * gaussian_pyramid[g_idx].unit_xyz[2];
//check if the position is out of the sphere
if (phys_corr.vectorNorm() <= window_radius)
{
float cur_weight = exp(-0.5f * pow(phys_corr.vectorNorm() / sigma_w, 2.f));
//calculate the gradient components, with respect to physical coordinates
float grad_x = 0.5 * (gaussian_pyramid[g_idx].vol_mat[i][j][k + 1] - gaussian_pyramid[g_idx].vol_mat[i][j][k - 1]) / gaussian_pyramid[g_idx].unit_xyz[0];
float grad_y = 0.5 * (gaussian_pyramid[g_idx].vol_mat[i][j + 1][k] - gaussian_pyramid[g_idx].vol_mat[i][j - 1][k]) / gaussian_pyramid[g_idx].unit_xyz[1];
float grad_z = 0.5 * (gaussian_pyramid[g_idx].vol_mat[i + 1][j][k] - gaussian_pyramid[g_idx].vol_mat[i - 1][j][k]) / gaussian_pyramid[g_idx].unit_xyz[2];
//calculate the elements of structure tensor
structure_tensor[0] += grad_x * grad_x * cur_weight;
structure_tensor[1] += grad_x * grad_y * cur_weight;
structure_tensor[2] += grad_x * grad_z * cur_weight;
structure_tensor[4] += grad_y * grad_y * cur_weight;
structure_tensor[5] += grad_y * grad_z * cur_weight;
structure_tensor[8] += grad_z * grad_z * cur_weight;
//calculate the elements of directional derivative d(x)
d_mat_x += grad_x * cur_weight;
d_mat_y += grad_y * cur_weight;
d_mat_z += grad_z * cur_weight;
}
}
}
}
structure_tensor[3] = structure_tensor[1];
structure_tensor[6] = structure_tensor[2];
structure_tensor[7] = structure_tensor[5];
//check if the squared norm of directional derivative d is too small
if ((d_mat_x * d_mat_x + d_mat_y * d_mat_y + d_mat_z * d_mat_z) < sift_config.gradient_threshold)
{
available_kp = false;
}
else
{
//eigendecomposition of structure tensor, s_mat = Q * A * Q'
Eigen::Matrix3f s_mat;
s_mat(0, 0) = structure_tensor[0];
s_mat(0, 1) = structure_tensor[1];
s_mat(0, 2) = structure_tensor[2];
s_mat(1, 0) = structure_tensor[3];
s_mat(1, 1) = structure_tensor[4];
s_mat(1, 2) = structure_tensor[5];
s_mat(2, 0) = structure_tensor[6];
s_mat(2, 1) = structure_tensor[7];
s_mat(2, 2) = structure_tensor[8];
Eigen::EigenSolver<Eigen::Matrix3f> es(s_mat);
Eigen::Matrix3f s_eigenvalue = es.pseudoEigenvalueMatrix();
Eigen::Matrix3f s_eigenvector = es.pseudoEigenvectors();
//sort the eigenvalue and eigenvector in a descending order
EigenMatrix e_m[3];
e_m[0].eigen_value = s_eigenvalue(0, 0);
e_m[1].eigen_value = s_eigenvalue(1, 1);
e_m[2].eigen_value = s_eigenvalue(2, 2);
e_m[0].eigen_vector[0] = s_eigenvector(0, 0);
e_m[0].eigen_vector[1] = s_eigenvector(1, 0);
e_m[0].eigen_vector[2] = s_eigenvector(2, 0);
e_m[1].eigen_vector[0] = s_eigenvector(0, 1);
e_m[1].eigen_vector[1] = s_eigenvector(1, 1);
e_m[1].eigen_vector[2] = s_eigenvector(2, 1);
e_m[2].eigen_vector[0] = s_eigenvector(0, 2);
e_m[2].eigen_vector[1] = s_eigenvector(1, 2);
e_m[2].eigen_vector[2] = s_eigenvector(2, 2);
std::sort(std::begin(e_m), std::end(e_m), sortByEigenvalue);
//check if there are ambiguous eigenvalues or the eigenvalues are too small
if ((e_m[1].eigen_value / e_m[0].eigen_value) > sift_config.beta
|| (e_m[2].eigen_value / e_m[1].eigen_value) > sift_config.beta
|| fabs(e_m[0].eigen_value - e_m[1].eigen_value) < FLT_EPSILON
|| fabs(e_m[1].eigen_value - e_m[2].eigen_value) < FLT_EPSILON
|| fabs(e_m[2].eigen_value - e_m[0].eigen_value) < FLT_EPSILON)
{
available_kp = false;
}
else
{
Point3D d_vec(d_mat_x, d_mat_y, d_mat_z);
float cos_phi = FLT_MAX;
for (int i = 0; i < 2; i++)
{
Point3D q_vec(e_m[i].eigen_vector[0], e_m[i].eigen_vector[1], e_m[i].eigen_vector[2]);
//dot product of q and d
float q_d = q_vec * d_vec;
//keep the smallest cos_phi
float abs_cos_phi = fabs(q_d / (q_vec.vectorNorm() * d_vec.vectorNorm()));
cos_phi = cos_phi < abs_cos_phi ? cos_phi : abs_cos_phi;
//determine the sign of eigenvector
float sgn = q_d > 0 ? 1.f : -1.f;
//adjust the sign of eigenvector
e_m[i].eigen_vector[0] *= sgn;
e_m[i].eigen_vector[1] *= sgn;
e_m[i].eigen_vector[2] *= sgn;
}
//check if the angle between the two vectors q and d is too small
if (cos_phi < sift_config.gamma)
{
available_kp = false;
}
else
{
//build rotation matrix
Point3D r1(e_m[0].eigen_vector[0], e_m[0].eigen_vector[1], e_m[0].eigen_vector[2]);
Point3D r2(e_m[1].eigen_vector[0], e_m[1].eigen_vector[1], e_m[1].eigen_vector[2]);
//cross product of r1 and r2
Point3D rc = r1 / r2;
//inverse of rotation matrix = tansposition of rotation matrix
kp_candidate[m].rotation_matrix[0] = r1.x;
kp_candidate[m].rotation_matrix[3] = r2.x;
kp_candidate[m].rotation_matrix[6] = rc.x;
kp_candidate[m].rotation_matrix[1] = r1.y;
kp_candidate[m].rotation_matrix[4] = r2.y;
kp_candidate[m].rotation_matrix[7] = rc.y;
kp_candidate[m].rotation_matrix[2] = r1.z;
kp_candidate[m].rotation_matrix[5] = r2.z;
kp_candidate[m].rotation_matrix[8] = rc.z;
}
}
}
kp_candidate[m].coor_img.x = available_kp ? 1 : -1;
}
//fill kp_queue with validated keypoints
std::vector<Keypoint3D>().swap(kp_queue);
candidate_amount = (int)kp_candidate.size();
for (int i = 0; i < candidate_amount; i++)
{
if (kp_candidate[i].coor_img.x > 0)
{
//set the coordinates of keypoint in the original image
float scale_factor = pow(2.f, kp_candidate[i].octave);
kp_candidate[i].coor_img = kp_candidate[i].coor_layer * scale_factor;
kp_queue.push_back(kp_candidate[i]);
}
}
}
void SIFT3D::constructDescriptor(std::vector<Keypoint3D>& kp_queue, std::vector<Layer3D>& gaussian_pyramid, float** descriptor)
{
int kp_amount = (int)kp_queue.size();
float sqrt_2 = sqrt(2.f);
#pragma omp parallel for
for (int m = 0; m < kp_amount; m++)
{
int g_idx = kp_queue[m].layer + kp_queue[m].octave * (sift_config.n_octave_layers + 3);
float sigma = 5.f * sqrt_2 * kp_queue[m].scale;
float sphere_radius = 2.f * sigma;
float cube_radius = sphere_radius / sqrt_2; //subregion is an inscribed cube of the sphere
//estimate the circumscibed cubic boundary of spherical window in image
int x_min = floor(kp_queue[m].coor_layer.x - sphere_radius / gaussian_pyramid[g_idx].unit_xyz[0]);
x_min = x_min > IMG_BORDER ? x_min : IMG_BORDER;
int x_max = ceil(kp_queue[m].coor_layer.x + sphere_radius / gaussian_pyramid[g_idx].unit_xyz[0]);
x_max = x_max < gaussian_pyramid[g_idx].dim_xyz[0] - IMG_BORDER ? x_max : gaussian_pyramid[g_idx].dim_xyz[0] - IMG_BORDER;
int y_min = floor(kp_queue[m].coor_layer.y - sphere_radius / gaussian_pyramid[g_idx].unit_xyz[1]);
y_min = y_min > IMG_BORDER ? y_min : IMG_BORDER;
int y_max = ceil(kp_queue[m].coor_layer.y + sphere_radius / gaussian_pyramid[g_idx].unit_xyz[1]);
y_max = y_max < gaussian_pyramid[g_idx].dim_xyz[1] - IMG_BORDER ? y_max : gaussian_pyramid[g_idx].dim_xyz[1] - IMG_BORDER;
int z_min = floor(kp_queue[m].coor_layer.z - sphere_radius / gaussian_pyramid[g_idx].unit_xyz[2]);
z_min = z_min > IMG_BORDER ? z_min : IMG_BORDER;
int z_max = ceil(kp_queue[m].coor_layer.z + sphere_radius / gaussian_pyramid[g_idx].unit_xyz[2]);
z_max = z_max < gaussian_pyramid[g_idx].dim_xyz[2] - IMG_BORDER ? z_max : gaussian_pyramid[g_idx].dim_xyz[2] - IMG_BORDER;
//construct the descriptor
for (int i = z_min; i < z_max; i++)
{
for (int j = y_min; j < y_max; j++)
{
for (int k = x_min; k < x_max; k++)
{
Point3D img_coor(k, j, i); //location with voxel unit
//get local coordinates in a keypoint-centered coordinate system, using physical unit
Point3D phys_coor = img_coor - kp_queue[m].coor_layer;
phys_coor.x *= gaussian_pyramid[g_idx].unit_xyz[0];
phys_coor.y *= gaussian_pyramid[g_idx].unit_xyz[1];
phys_coor.z *= gaussian_pyramid[g_idx].unit_xyz[2];
float cur_distance = phys_coor.vectorNorm(); //distance to the kp
//check if the location is inside the spherical region
if (cur_distance > sphere_radius)
{
continue;
}
//rotate the local coordinate
Point3D rotated_coor;
rotated_coor.x = kp_queue[m].rotation_matrix[0] * phys_coor.x + kp_queue[m].rotation_matrix[1] * phys_coor.y + kp_queue[m].rotation_matrix[2] * phys_coor.z;
rotated_coor.y = kp_queue[m].rotation_matrix[3] * phys_coor.x + kp_queue[m].rotation_matrix[4] * phys_coor.y + kp_queue[m].rotation_matrix[5] * phys_coor.z;
rotated_coor.z = kp_queue[m].rotation_matrix[6] * phys_coor.x + kp_queue[m].rotation_matrix[7] * phys_coor.y + kp_queue[m].rotation_matrix[8] * phys_coor.z;
//get normalized coordinate in 4x4x4-cube subregion
Point3D subregion_coor;
subregion_coor.x = 2 * (rotated_coor.x + cube_radius) / cube_radius;
subregion_coor.y = 2 * (rotated_coor.y + cube_radius) / cube_radius;
subregion_coor.z = 2 * (rotated_coor.z + cube_radius) / cube_radius;
//let the keypoint is at [1.5, 1.5, 1.5], the diagonal corners of subregion are [-0.5, -0.5, -0.5] and [3.5, 3.5, 3.5]
subregion_coor.x -= 0.5f;
subregion_coor.y -= 0.5f;
subregion_coor.z -= 0.5f;
//the locations falling in the area between the sphere and its incribed cube are not involved in descriptor construction
if (subregion_coor.x <= -0.5f || subregion_coor.y <= -0.5f || subregion_coor.z <= -0.5f
|| subregion_coor.x >= 3.5f || subregion_coor.y >= 3.5f || subregion_coor.z >= 3.5f)
{
continue;
}
//calculate the gradient, using physical unit
float cur_weight = exp(-0.5 * pow(cur_distance / sigma, 2));
Point3D gradient;
gradient.x = 0.5 * (gaussian_pyramid[g_idx].vol_mat[i][j][k + 1] - gaussian_pyramid[g_idx].vol_mat[i][j][k - 1]) / gaussian_pyramid[g_idx].unit_xyz[0];
gradient.y = 0.5 * (gaussian_pyramid[g_idx].vol_mat[i][j + 1][k] - gaussian_pyramid[g_idx].vol_mat[i][j - 1][k]) / gaussian_pyramid[g_idx].unit_xyz[1];
gradient.z = 0.5 * (gaussian_pyramid[g_idx].vol_mat[i + 1][j][k] - gaussian_pyramid[g_idx].vol_mat[i - 1][j][k]) / gaussian_pyramid[g_idx].unit_xyz[2];
//modify the vector with Gaussian weight
gradient = gradient * cur_weight;
//rotate the gradient vector
Point3D rotated_gradient;
rotated_gradient.x = kp_queue[m].rotation_matrix[0] * gradient.x + kp_queue[m].rotation_matrix[1] * gradient.y + kp_queue[m].rotation_matrix[2] * gradient.z;
rotated_gradient.y = kp_queue[m].rotation_matrix[3] * gradient.x + kp_queue[m].rotation_matrix[4] * gradient.y + kp_queue[m].rotation_matrix[5] * gradient.z;
rotated_gradient.z = kp_queue[m].rotation_matrix[6] * gradient.x + kp_queue[m].rotation_matrix[7] * gradient.y + kp_queue[m].rotation_matrix[8] * gradient.z;
//check if the vector of rotated gradient is too small
float gradient_magnitude = rotated_gradient.vectorNorm();
if (gradient_magnitude * gradient_magnitude < FLT_EPSILON * 10.f)
{
continue;
}
//search the triangle tile of the icosahedron intersecting with the ray of gradient vector
int intersect_idx = -1;
Point3D bary_coor;
for (int n = 0; n < 20; n++)
{
intersect_idx = cartisan2Barycentric(rotated_gradient, bary_coor, icosahedron[n]);
//check if locate the triangle successfully
if (intersect_idx > 0)
{
intersect_idx = n;
break;
}
}
//in strange case that no triangle tile is located
if (intersect_idx < 0)
{
continue;
}
//get the decimal part of subregion_coor
Point3D decimal_coor;
decimal_coor.x = subregion_coor.x - floor(subregion_coor.x);
decimal_coor.y = subregion_coor.y - floor(subregion_coor.y);
decimal_coor.z = subregion_coor.z - floor(subregion_coor.z);
//distribute the gradient value at a point to the histograms of the 8 adjacent cubes in subregion
for (int dz = 0; dz < 2; dz++)
{
for (int dy = 0; dy < 2; dy++)
{
for (int dx = 0; dx < 2; dx++)
{
int local_x = (int)subregion_coor.x + dx;
int local_y = (int)subregion_coor.y + dy;
int local_z = (int)subregion_coor.z + dz;
//exlcude the voxels close to the boundaries of subregion
if (local_x < 0 || local_y < 0 || local_z < 0 || local_x >= 4 || local_y >= 4 || local_z >= 4)
{
continue;
}
//get the index of cube in a subregion
int cube_idx = local_x + local_y * 4 + local_z * 16;
//calculate the weight of trilinear interpolation
float interp_weight = ((dx == 0) ? (1.f - decimal_coor.x) : decimal_coor.x)
* ((dy == 0) ? (1.f - decimal_coor.y) : decimal_coor.y)
* ((dz == 0) ? (1.f - decimal_coor.z) : decimal_coor.z);
//locate the histogram bins
int offset_v1 = cube_idx * 12 + icosahedron[intersect_idx].vertex_idx[0];
int offset_v2 = cube_idx * 12 + icosahedron[intersect_idx].vertex_idx[1];
int offset_v3 = cube_idx * 12 + icosahedron[intersect_idx].vertex_idx[2];
//accumulate the gradients to the bins
descriptor[m][offset_v1] += gradient_magnitude * interp_weight * bary_coor.x;
descriptor[m][offset_v2] += gradient_magnitude * interp_weight * bary_coor.y;
descriptor[m][offset_v3] += gradient_magnitude * interp_weight * bary_coor.z;
}
}
}
}
}
}
//normalize the descriptor
float sq_sum = 0;
for (int i = 0; i < 768; i++)
{
sq_sum += descriptor[m][i] * descriptor[m][i];
}
float inv_norm = 1.f / (sqrt(sq_sum) + FLT_EPSILON);
for (int i = 0; i < 768; i++)
{
descriptor[m][i] *= inv_norm;
}
//truncate the descriptor with a threshold = 0.2 x 128 / 768
for (int i = 0; i < 768; i++)
{
descriptor[m][i] = (descriptor[m][i] < sift_config.truncate_threshold) ? descriptor[m][i] : sift_config.truncate_threshold;
}
//normalize the descriptor again
sq_sum = 0;
for (int i = 0; i < 768; i++)
{
sq_sum += descriptor[m][i] * descriptor[m][i];
}
inv_norm = 1.f / (sqrt(sq_sum) + FLT_EPSILON);
for (int i = 0; i < 768; i++)
{
descriptor[m][i] *= inv_norm;
}
}
}
void SIFT3D::monodirectionalMatch(std::vector<Keypoint3D>& kp1, float** descriptor1, std::vector<Keypoint3D>& kp2, float** descriptor2, std::vector<Point3D>& matched_kp1, std::vector<Point3D>& matched_kp2)
{
int kp1_amount = (int)kp1.size();
int kp2_amount = (int)kp2.size();
float matching_ratio_square = matching_ratio * matching_ratio;
//create a queue to check the pairing betwen ref_key and tar_kp
KeypointChecker kp_chk;
kp_chk.ref_idx = -1;
kp_chk.tar_idx = -1;
std::vector<KeypointChecker> kp_matches(kp1_amount, kp_chk);
//match each reference keypoint with target keypoints
#pragma omp parallel for
for (int i = 0; i < kp1_amount; i++)
{
//create two arrays for ratio check
int candidate_idx[2] = { -1,-1 };//[0]: the shortest, [1]: the second shortest,
float candidate_distance[2] = { FLT_MAX,FLT_MAX }; //[0]: the shortest, [1]: the second shortest
for (int j = 0; j < kp2_amount; j++)
{
//calculate squared Euclidean distance between kp1 and kp2
float squared_distance = 0;
for (int k = 0; k < 768; k++)
{
float component_difference = descriptor1[i][k] - descriptor2[j][k];
squared_distance += component_difference * component_difference;
}
//store the information of kp with the shortest distance or the second shortest distance
if (squared_distance < candidate_distance[0])
{
candidate_idx[1] = candidate_idx[0];
candidate_distance[1] = candidate_distance[0];
candidate_idx[0] = j;
candidate_distance[0] = squared_distance;
}
else if (squared_distance < candidate_distance[1])
{
candidate_idx[1] = j;
candidate_distance[1] = squared_distance;
}
}
//check if the matching ratio is satisfied
if (candidate_distance[0] < matching_ratio_square * candidate_distance[1])
{
kp_matches[i].ref_idx = i;
kp_matches[i].tar_idx = candidate_idx[0];
kp_matches[i].dist = candidate_distance[0];
}
}
//arrange the queue of matched keypoints in descending order of reference keypoint index, keep the ones greater than -1
std::sort(kp_matches.begin(), kp_matches.end(), sortByRefIdx);
int matched_amount = 0;
for (int i = 0; i < kp1_amount; i++)
{
if (kp_matches[i].ref_idx == -1)
{
matched_amount = i;
break;
}
}
if (matched_amount > 1)
{
//resize the queue of matched keypoints, exclude invalid elements
kp_matches.resize(matched_amount);
//rearrange the queue in descending order of target keypoint index
std::sort(kp_matches.begin(), kp_matches.end(), sortByTarIdx);
std::vector<int> mto_tar_idx; //index of target keypoint with many-to-one correspondence
std::vector<int> mto_tar_amount; //amount of target keypoints with many-to-one correspondence
//check if any tar_kp is matched with multiple ref_kp
int seg_start = 0;
int seg_len = 0;
for (int j = 0; j < matched_amount; j++)
{
if (kp_matches[j].tar_idx == kp_matches[j + 1].tar_idx)
{
if (j == 0)
{
seg_start = j;
mto_tar_idx.push_back(seg_start);
}
else if (kp_matches[j].tar_idx != kp_matches[j - 1].tar_idx)
{
seg_start = j;
mto_tar_idx.push_back(seg_start);
}
seg_len++;
}
else if (seg_len > 0) {
mto_tar_amount.push_back(seg_len + 1);
seg_len = 0;
}
}
int check_length = (int)mto_tar_idx.size();
if (check_length > 0)
{
#pragma omp parallel for
for (int i = 0; i < check_length; i++)
{
int candidate_idx[2] = { -1,-1 };//[0]: the shortest, [1]: the second shortest,
float candidate_distance[2] = { FLT_MAX,FLT_MAX }; //[0]: the shortest, [1]: the second shortest
for (int j = 0; j < mto_tar_amount[i]; j++)
{
//store the information of keypoint it has the shortest distance or the second shortest distance
int check_idx = mto_tar_idx[i] + j;
if (kp_matches[check_idx].dist < candidate_distance[0])
{
candidate_idx[1] = candidate_idx[0];
candidate_distance[1] = candidate_distance[0];
candidate_idx[0] = kp_matches[check_idx].ref_idx;
candidate_distance[0] = kp_matches[check_idx].dist;
}
else if (kp_matches[check_idx].dist < candidate_distance[1])
{
candidate_idx[1] = kp_matches[check_idx].ref_idx;
candidate_distance[1] = kp_matches[check_idx].dist;
}
kp_matches[check_idx].ref_idx = -1; //invalidate the inappropriate matched reference keypoints
}
//check if the match ratio is satisfied
if (candidate_distance[0] < matching_ratio_square * candidate_distance[1])
{
//validate the first element in the segment
kp_matches[mto_tar_idx[i]].ref_idx = candidate_idx[0];
}
}
}
}
//assign matched kypoints to the queues
#pragma omp parallel sections
{
#pragma omp section
{
for (int i = 0; i < matched_amount; i++)
{
if (kp_matches[i].ref_idx > -1)
{
Point3D cur_kp = kp1[kp_matches[i].ref_idx].coor_img;
matched_kp1.push_back(cur_kp);
}
}
}
#pragma omp section
{
for (int j = 0; j < matched_amount; j++)
{
if (kp_matches[j].ref_idx > -1)
{
Point3D cur_kp = kp2[kp_matches[j].tar_idx].coor_img;
matched_kp2.push_back(cur_kp);
}
}
}
}
}
void SIFT3D::bidirectionalMatch(std::vector<Keypoint3D>& kp1, float** descriptor1, std::vector<Keypoint3D>& kp2, float** descriptor2, std::vector<Point3D>& matched_kp1, std::vector<Point3D>& matched_kp2)
{
int kp1_amount = (int)kp1.size();
int kp2_amount = (int)kp2.size();
//match the keypoints in the two images, using brute force search, reject the keypoints with eta >= matching_ratio
int* r2t_idx = new int[kp1_amount]; //index of matched tar_kp
std::fill(&r2t_idx[0], &r2t_idx[0] + kp1_amount - 1, -1); //initialze the array of index
int r2t_number = bruteforceMatch(kp1, descriptor1, kp2, descriptor2, r2t_idx); //ref->tar
int* t2r_idx = new int[kp2_amount]; //index of matched ref_kp
std::fill(&t2r_idx[0], &t2r_idx[0] + kp2_amount - 1, -1); //initialze the array of index
int t2r_number = bruteforceMatch(kp2, descriptor2, kp1, descriptor1, t2r_idx); //tar->ref
//bidirectional check
#pragma omp parallel for
for (int i = 0; i < kp1_amount; i++)
{
if (r2t_idx[i] > -1)
{
if (t2r_idx[r2t_idx[i]] != i)
{
r2t_idx[i] = -1;
}
}
}
#pragma omp parallel for
for (int j = 0; j < kp2_amount; j++)
{
if (t2r_idx[j] > -1)
{
if (r2t_idx[t2r_idx[j]] != j)
{
t2r_idx[j] = -1;
}
}
}
//assign the matched kypoints to the queues
#pragma omp parallel sections
{
#pragma omp section
{
for (int i = 0; i < kp1_amount; i++)
{
if (r2t_idx[i] != -1)
{
matched_kp1.push_back(kp1[i].coor_img);
}
}
}
#pragma omp section
{
for (int j = 0; j < kp1_amount; j++)
{
if (r2t_idx[j] != -1)
{
matched_kp2.push_back(kp2[r2t_idx[j]].coor_img);
}
}
}
}
//release memory
delete[] r2t_idx;
delete[] t2r_idx;
}
bool sortByEigenvalue(const EigenMatrix& em1, const EigenMatrix& em2)
{
return em1.eigen_value > em2.eigen_value;
}
bool sortByRefIdx(const KeypointChecker& kc1, const KeypointChecker& kc2)
{
return kc1.ref_idx > kc2.ref_idx;
}
bool sortByTarIdx(const KeypointChecker& kc1, const KeypointChecker& kc2)
{
return kc1.tar_idx > kc2.tar_idx;
}
int mirrorLow(int x, int y)
{
int output_index = x < y ? (-1 * x) : x;
return output_index;
}
int mirrorHigh(int x, int y)
{
int output_index = x > y ? (2 * y - x) : x;
return output_index;
}
} //namespace opencorr