/*!
* Evaluates curvature at (x,y,z) through discrete finite difference scheme.
*/
float Implicit::GetCurvature(float x, float y, float z) const
{
const float& d = mDelta;
float r2d = 1.0f/(2.0f*d);
Vector3<float> gradient = -GetGradient(x,y,z);
Vector4<float> ex_gradient(gradient[0],gradient[1],gradient[2],1.0f);
Vector3<float> gradientsP[3] = { -GetGradient(x + d, y, z) , -GetGradient(x, y + d, z) , -GetGradient(x, y, z + d) };
Vector3<float> gradientsN[3] = { -GetGradient(x - d, y, z) , -GetGradient(x, y - d, z) , -GetGradient(x, y, z - d) };
float hessian[4][4] = {
{ ( gradientsP[0][0] - gradientsN[0][0] )*r2d, ( gradientsP[1][0] - gradientsN[1][0] )*r2d, ( gradientsP[2][0] - gradientsN[2][0] )*r2d , 0.0f},
{ ( gradientsP[0][1] - gradientsN[0][1] )*r2d, ( gradientsP[1][1] - gradientsN[1][1] )*r2d, ( gradientsP[2][1] - gradientsN[2][1] )*r2d , 0.0f},
{ ( gradientsP[0][2] - gradientsN[0][2] )*r2d, ( gradientsP[1][2] - gradientsN[1][2] )*r2d, ( gradientsP[2][2] - gradientsN[2][2] )*r2d , 0.0f},
{ 0.0f, 0.0f, 0.0f, 1.0f}
};
float hTrace = hessian[0][0] + hessian[1][1] + hessian[2][2];
//the - 1 is just because we have a 4d vector so the dot product has a 1 at the end we need to get rid of
//3x3 matrix is all we need here really
float hMult = ex_gradient*(Matrix4x4<float>(hessian)*ex_gradient) - 1.0f;
float gradLength = gradient.Length();
float gradLengthSq = gradient*gradient;
float curv = (hMult - gradLengthSq*hTrace)/(2.0f* gradLengthSq*gradLength);
return curv;
}
//----------------------------------------------------------------------------
void CIsoSurface::OrientTriangles (std::vector<TVector3D>& rkVA,
std::vector<TriangleKey>& rkTA, bool bSameDir)
{
for (int i = 0; i < (int)rkTA.size(); i++)
{
TriangleKey& rkTri = rkTA[i];
// get triangle vertices
TVector3D kV0 = rkVA[rkTri.V[0]];
TVector3D kV1 = rkVA[rkTri.V[1]];
TVector3D kV2 = rkVA[rkTri.V[2]];
// construct triangle normal based on current orientation
TVector3D kEdge1 = kV1 - kV0;
TVector3D kEdge2 = kV2 - kV0;
TVector3D kNormal = cross(kEdge1,kEdge2);
// get the image gradient at the vertices
TVector3D kGrad0 = GetGradient(kV0);
TVector3D kGrad1 = GetGradient(kV1);
TVector3D kGrad2 = GetGradient(kV2);
// compute the average gradient
TVector3D kGradAvr = (kGrad0 + kGrad1 + kGrad2)/3.0f;
// compute the dot product of normal and average gradient
float fDot = dot(kGradAvr,kNormal);
// choose triangle orientation based on gradient direction
int iSave;
if ( bSameDir )
{
if ( fDot < 0.0f )
{
// wrong orientation, reorder it
iSave = rkTri.V[1];
rkTri.V[1] = rkTri.V[2];
rkTri.V[2] = iSave;
}
}
else
{
if ( fDot > 0.0f )
{
// wrong orientation, reorder it
iSave = rkTri.V[1];
rkTri.V[1] = rkTri.V[2];
rkTri.V[2] = iSave;
}
}
}
}
/////////////////////////////////////////////////////////////////////////////
// Steepest descent
/////////////////////////////////////////////////////////////////////////////
void CDiffFunction::SteepestDescent(std::vector<double> &vMax, bool fTrace)
{
const std::vector<double> &vG = GetGradient();
int n = GetDimensions();
//
// Loop of iterations
//
for (int Iteration = MaxSDIterations; --Iteration >= 0;)
{
GetOutput(&vMax[0]);
ComputeGradient();
//
// Maximize in gradient direction
//
vGCopy = vG;
double x = LineOpt(&vMax[0], &vGCopy[0], fTrace);
double Delta2 = 0.0;
for (int i = n; --i >= 0;)
{
double New = Normalize(vMax[i] + x * vGCopy[i]);
double Delta = New - vMax[i];
vMax[i] = New;
Delta2 += Delta * Delta;
}
if (Delta2 < CGEpsilon)
break;
}
}
void EddyPulse::GetValue (double * dAllVal, double const time){
//if (time < 0.0 || time > m_parent->GetDuration() + m_linger_time ) { return ; }
dAllVal[1+m_axis] += GetGradient(time);
return;
}
void ImplicitSurface<Real>::GetFrame (const Vector3<Real>& pos,
Vector3<Real>& tangent0, Vector3<Real>& tangent1,
Vector3<Real>& normal) const
{
normal = GetGradient(pos);
Vector3<Real>::GenerateOrthonormalBasis(tangent0, tangent1, normal);
}
///
/// \brief MapViewer::on_actionSet_Color_Scheme_triggered
/// Opens a dialog to select a new color scheme
void MapViewer::on_actionSet_Color_Scheme_triggered()
{
QString color_name = QInputDialog::getItem(this, "Select Color Scheme", "Choose Scheme", color_list_);
int color_index = color_list_.indexOf(color_name);
QCPColorGradient new_gradient = GetGradient(color_index);
parent_->setGradient(new_gradient);
}
double inpainting::ComputeData(int i, int j) {
gradient grad, temp, grad_T;
grad.grad_x = 0;
grad.grad_y = 0;
double result;
double magnitude;
double max = 0;
int x, y;
for (y = MAX(j - myWinSize, 0); y <= MIN(j + myWinSize, m_height - 1); ++y) {
for (x = MAX(i - myWinSize, 0); x <= MIN(i + myWinSize, m_width - 1); ++x) {
if (m_mark[y * m_width + x] >=0) {
if (m_mark[y * m_width + x + 1] < 0 ||
m_mark[y * m_width + x - 1] < 0 ||
m_mark[(y + 1) * m_width + x] < 0 ||
m_mark[(y - 1) * m_width + x] < 0) continue;
temp = GetGradient(x, y);
magnitude = temp.grad_x * temp.grad_x + temp.grad_y * temp.grad_y;
if (magnitude > max) {
grad.grad_x = temp.grad_x;
grad.grad_y = temp.grad_y;
max = magnitude;
}
}
}
}
grad_T.grad_x = grad.grad_y;
grad_T.grad_y = -grad.grad_x;
norm nn = GetNorm(i, j);
result = nn.norm_x * grad_T.grad_x + nn.norm_y * grad_T.grad_y;
result /= 255;
result = fabs(result);
return result;
}
void ImplicitSurface<Real>::GetFrame (const Vector3<Real>& rkP,
Vector3<Real>& rkTangent0, Vector3<Real>& rkTangent1,
Vector3<Real>& rkNormal) const
{
rkNormal = GetGradient(rkP);
Vector3<Real>::GenerateOrthonormalBasis(rkTangent0,rkTangent1,rkNormal,
false);
}
double EddyPulse::GetAreaNumeric (int steps){
double Sum = 0.0, DeltaT = GetDuration()/steps;
for (int i=0; i<steps; ++i ) Sum += GetGradient(i*DeltaT);
return (Sum*DeltaT) ;
}
Array<double> PolyNomial::GetGradient()
{
Array<double> var;
for(int i = 0; i < numVar_; i++) {
var.append(GetGradient(i).ComputePlValue());
}
return var;
}
/*!
* Evaluates curvature at (x,y,z) through discrete finite difference scheme.
*/
float Implicit::GetCurvature(float x, float y, float z) const
{
// Implement finite difference evaluation of curvature at world coordinates (x,y,z)
// Use mDelta variable as epsilon in eqn. 16 in lab text
float h = mDelta;
float D2x = (GetValue(x+h, y, z) - 2.0*GetValue(x, y, z) + GetValue(x-h, y, z))/(h*h);
float D2y = (GetValue(x, y+h, z) - 2.0*GetValue(x, y, z) + GetValue(x, y-h, z))/(h*h);
float D2z = (GetValue(x, y, z+h) - 2.0*GetValue(x, y, z) + GetValue(x, y, z-h))/(h*h);
return (D2x + D2y + D2z)/(2.0*GetGradient(x, y, z).Length());
}
inline std::vector<double> GetGradient(const Eigen::Vector3d& location, const bool enable_edge_gradients=false) const
{
const std::vector<int64_t> indices = LocationToGridIndex(location);
if (indices.size() == 3)
{
return GetGradient(indices[0], indices[1], indices[2], enable_edge_gradients);
}
else
{
return std::vector<double>();
}
}
int GradientCalculator::GetPixelGradient(int x, int y) const {
int gradient = 0;
const PixelPacket* p1 = GetPixel(x, y);
assert(p1 != NULL);
for (int i = -1; i <= 1; ++i) {
for (int j = -1; j <= 1; ++j) {
const PixelPacket* p2 = GetPixel(x + i, y + j);
if (p2 != NULL) {
gradient += GetGradient(*p1, *p2);
}
}
}
return gradient;
}
void DrawingUtils::DrawHorizontalButton(wxDC& dc,
const wxRect& rect,
const bool& focus,
const bool& upperTabs,
bool vertical,
bool hover)
{
wxColour lightGray = GetGradient();
wxColour topStartColor(wxT("WHITE"));
wxColour topEndColor(wxSystemSettings::GetColour(wxSYS_COLOUR_3DFACE));
// Define the middle points
if(focus) {
if(upperTabs) {
PaintStraightGradientBox(dc, rect, topStartColor, topEndColor, vertical);
} else {
PaintStraightGradientBox(dc, rect, topEndColor, topStartColor, vertical);
}
} else {
topStartColor = wxSystemSettings::GetColour(wxSYS_COLOUR_3DFACE);
topEndColor = lightGray;
wxRect r1;
wxRect r2;
if(upperTabs) {
r1 = wxRect(rect.x, rect.y, rect.width, rect.height / 4);
r2 = wxRect(rect.x, rect.y + rect.height / 4, rect.width, (rect.height * 3) / 4);
PaintStraightGradientBox(dc, r1, topEndColor, topStartColor, vertical);
PaintStraightGradientBox(dc, r2, topStartColor, topStartColor, vertical);
} else {
r1 = wxRect(rect.x, rect.y, rect.width, (rect.height * 3) / 4);
r2 = wxRect(rect.x, rect.y + (rect.height * 3) / 4, rect.width, rect.height / 4);
PaintStraightGradientBox(dc, r1, topStartColor, topStartColor, vertical);
PaintStraightGradientBox(dc, r2, topStartColor, topEndColor, vertical);
}
}
dc.SetBrush(*wxTRANSPARENT_BRUSH);
}
void DrawingUtils::DrawVerticalButton(wxDC& dc,
const wxRect& rect,
const bool &focus,
const bool &leftTabs,
bool vertical,
bool hover )
{
wxColour lightGray = GetGradient();
// Define the rounded rectangle base on the given rect
// we need an array of 9 points for it
wxColour topStartColor(wxT("WHITE"));
wxColour topEndColor(wxSystemSettings::GetColour(wxSYS_COLOUR_3DFACE));
// Define the middle points
if ( focus ) {
PaintStraightGradientBox(dc, rect, topStartColor, topEndColor, vertical);
} else {
wxRect r1;
wxRect r2;
topStartColor = wxSystemSettings::GetColour(wxSYS_COLOUR_3DFACE);
topEndColor = lightGray;
if (leftTabs) {
r1 = wxRect(rect.x, rect.y, rect.width, rect.height/4);
r2 = wxRect(rect.x, rect.y+rect.height/4, rect.width, (rect.height*3)/4);
PaintStraightGradientBox(dc, r1, topEndColor, topStartColor, vertical);
PaintStraightGradientBox(dc, r2, topStartColor, topStartColor, vertical);
} else {
r1 = wxRect(rect.x, rect.y, rect.width, (rect.height*3)/4);
r2 = wxRect(rect.x, rect.y+(rect.height*3)/4, rect.width, rect.height/4);
PaintStraightGradientBox(dc, r1, topStartColor, topStartColor, vertical);
PaintStraightGradientBox(dc, r2, topStartColor, topEndColor, vertical);
}
}
dc.SetBrush( *wxTRANSPARENT_BRUSH );
}
void Implicit::Update()
{
if (mVisualizationMode == Curvature) {
if(typeid(*mMesh) == typeid(SimpleMesh)) {
SimpleMesh * ptr = static_cast<SimpleMesh*>(mMesh);
std::vector<SimpleMesh::Vertex>& verts = ptr->GetVerts();
Matrix4x4<float> M = GetTransform().Transpose();
// Compute curvature of implicit geometry and assign to the vertex property
for(unsigned int i=0; i < verts.size(); i++){
const Vector3<float> vObject = verts.at(i).pos;
// Transform vertex position to world space
Vector4<float> vWorld = GetTransform() * Vector4<float>(vObject[0],vObject[1],vObject[2],1);
// Get curvature in world space
verts.at(i).curvature = GetCurvature(vWorld[0], vWorld[1], vWorld[2]);
// Get gradient in world space (used for lighting)
Vector3<float> nWorld = GetGradient(vWorld[0], vWorld[1], vWorld[2]);
// Transform gradient to object space
Vector4<float> nObject = M * Vector4<float>(nWorld[0],nWorld[1],nWorld[2],0);
verts.at(i).normal = Vector3<float>(nObject[0], nObject[1], nObject[2]).Normalize();
}
ptr->mAutoMinMax = mAutoMinMax;
ptr->mMinCMap = mMinCMap;
ptr->mMaxCMap = mMaxCMap;
ptr->SetVisualizationMode(Mesh::CurvatureVertex);
ptr->Update();
}
else {
std::cerr << "No Curvature visualization mode implemented for mesh type: " << typeid(*mMesh).name() << std::endl;
}
}
}
inline std::vector<double> GetGradient(const VoxelGrid::GRID_INDEX& index, const bool enable_edge_gradients=false) const
{
return GetGradient(index.x, index.y, index.z, enable_edge_gradients);
}
void inpainting::DrawEdge(ImageData *origin_canny_edge) {
int x, y;
bool isEdgeConnected_b;
bool isTargetRegion = false;
int start_x;
int start_y;
int end_x;
int end_y;
gradient grad, temp, grad_T;
grad.grad_x = 0;
grad.grad_y = 0;
double result;
double magnitude;
double max = 0;
for (y = 0; y < m_height; ++y) {
for (x = 0; x < m_width; ++x) {
isTargetRegion = false;
if (origin_canny_edge->GetPixel(y * m_width + x) == 0xffffff && m_mark[y * m_width + x] == SOURCE) {
isEdgeConnected_b = isEdgeConnected(x, y, origin_canny_edge);
start_x = std::max(0, x - 2);
start_y = std::max(0, y - 2);
end_x = std::min(x + 2 , m_width - 1);
end_y = std::min(y + 2 , m_height - 1);
for (int i = start_x; i <= end_x; ++i) {
for (int j = start_y; j <= end_y; ++j) {
if (m_mark[j * m_width + i] < 0) {
isTargetRegion = true;
}
}
}
if (isTargetRegion && isEdgeConnected_b) {
//if (isTargetRegion) {
m_edge_mark[y * m_width +x] = 1;
vec.push_back(y * m_width + x);
grad.grad_x = 0;
grad.grad_y = 0;
max = 0;
for (int j = MAX(y - 2, 0); j <= MIN(y + 2, m_height - 1); ++j) {
for (int i = MAX(x - 2, 0); i <= MIN(x + 2, m_width - 1); ++i) {
if (m_mark[j * m_width + i] >= 0) {
if (m_mark[j * m_width + i + 1] < 0 ||
m_mark[j * m_width + i - 1] < 0 ||
m_mark[(j + 1) * m_width + i] < 0 ||
m_mark[(j - 1) * m_width + i] < 0) continue;
temp = GetGradient(i, j);
magnitude = temp.grad_x * temp.grad_x + temp.grad_y * temp.grad_y;
if (magnitude > max) {
grad.grad_x = temp.grad_x;
grad.grad_y = temp.grad_y;
max = magnitude;
}
}
}
}
vec_gradient.push_back(grad);
}
/*if (x == 0 && y == 0) {
if (m_mark[(y + 1) * m_width + x] < 0 ||
m_mark[y * m_width + x + 1] <0 ||
m_mark[(y + 1) * m_width + x + 1] < 0) {
m_edge_mark[0] = 1;
vec.push_back(0);
}
} else if (x == 0 && y == (m_height - 1)) {
if (m_mark[(y - 1) * m_width + x] < 0 ||
m_mark[y * m_width + x + 1] < 0 ||
m_mark[(y - 1) * m_width + x + 1] < 0) {
m_edge_mark[y * m_width + x] = 1;
vec.push_back(y * m_width + x);
}
} else if (x == (m_width - 1) && y == 0) {
if (m_mark[(y + 1) * m_width + x] < 0 ||
m_mark[y * m_width + x - 1] < 0 ||
m_mark[(y + 1) * m_width + x - 1] < 0) {
m_edge_mark[y * m_width + x] = 1;
vec.push_back(y * m_width + x);
}
} else if (x == (m_width - 1) && y == (m_height - 1)) {
if (m_mark[(y - 1) * m_width + x] < 0 ||
m_mark[y * m_width + x - 1] < 0 ||
m_mark[(y - 1) * m_width + x - 1] < 0) {
m_edge_mark[y * m_width + x] = 1;
vec.push_back(y * m_width + x);
}
} else if (x == 0 && y > 0 && y < (m_height - 1)) {
if (m_mark[(y + 1) * m_width + x] < 0 ||
m_mark[y * m_width + x + 1] < 0 ||
m_mark[(y + 1) * m_width + x + 1] < 0 ||
m_mark[(y - 1) * m_width + x + 1] < 0 ||
m_mark[(y - 1) * m_width + x ] <0) {
m_edge_mark[y * m_width + x] = 1;
vec.push_back(y * m_width + x);
}
} else if (x == (m_width - 1) && y > 0 && y < (m_height - 1)) {
if (m_mark[(y + 1) * m_width + x] < 0 ||
m_mark[y * m_width + x - 1] < 0 ||
m_mark[(y + 1) * m_width + x - 1] < 0 ||
m_mark[(y - 1) * m_width + x - 1] < 0 ||
m_mark[(y - 1) * m_width + x ] < 0) {
m_edge_mark[y * m_width + x] = 1;
vec.push_back(y * m_width + x);
}
} else if (x < (m_width - 1) && x > 0 &&y == 0) {
if (m_mark[(y + 1) * m_width + x] < 0 ||
m_mark[y * m_width + x + 1] < 0 ||
m_mark[(y + 1) * m_width + x + 1] < 0 ||
m_mark[y * m_width + x - 1] < 0 ||
m_mark[(y + 1) * m_width + x - 1] < 0) {
m_edge_mark[y * m_width + x] = 1;
vec.push_back(y * m_width + x);
}
} else if (x < (m_width - 1) && x > 0 && y ==(m_height - 1)) {
if (m_mark[(y - 1) * m_width + x] < 0 ||
m_mark[y * m_width + x + 1] < 0 ||
m_mark[(y - 1) * m_width + x + 1] < 0 ||
m_mark[y * m_width + x - 1] < 0 ||
m_mark[(y - 1) * m_width + x - 1] < 0) {
m_edge_mark[y * m_width + x] = 1;
vec.push_back(y * m_width + x);
}
} else {
if (m_mark[(y - 1) * m_width + x] < 0 ||
m_mark[y * m_width + x + 1] < 0 ||
m_mark[(y - 1) * m_width + x + 1] < 0 ||
m_mark[y * m_width + x - 1] < 0 ||
m_mark[(y - 1) * m_width + x - 1] < 0 ||
m_mark[(y + 1) * m_width + x] < 0 ||
m_mark[(y + 1) * m_width + x + 1] < 0 ||
m_mark[(y + 1) * m_width + x - 1] < 0) {
m_edge_mark[y * m_width + x] = 1;
vec.push_back(y * m_width + x);
}
}*/
}
}
}
}
inline std::vector<double> GetGradient(const double x, const double y, const double z, const bool enable_edge_gradients=false) const
{
return GetGradient(Eigen::Vector3d(x, y, z), enable_edge_gradients);
}
inline std::pair<double, bool> EstimateDistance(const Eigen::Vector3d& location) const
{
const std::vector<int64_t> indices = LocationToGridIndex(location);
if (indices.size() == 3)
{
const Eigen::Vector3d gradient = EigenHelpers::StdVectorDoubleToEigenVector3d(GetGradient(indices[0], indices[1], indices[2], true));
const std::vector<double> cell_location = GridIndexToLocation(indices[0], indices[1], indices[2]);
const Eigen::Vector3d cell_location_to_our_location(location.x() - cell_location[0], location.y() - cell_location[1], location.z() - cell_location[2]);
const double nominal_distance = (double)distance_field_.GetImmutable(indices[0], indices[1], indices[2]).first;
const double corrected_nominal_distance = (nominal_distance >= 0.0) ? nominal_distance - (GetResolution() * 0.5) : nominal_distance + (GetResolution() * 0.5);
const double cell_location_to_our_location_dot_gradient = cell_location_to_our_location.dot(gradient);
//const double gradient_dot_gradient = gradient.dot(gradient); // == squared norm of gradient
//const Eigen::Vector3d cell_location_to_our_location_projected_on_gradient = (cell_location_to_our_location_dot_gradient / gradient.dot(gradient)) * gradient;
//const double distance_adjustment = cell_location_to_our_location_projected_on_gradient.norm();
const double distance_adjustment = cell_location_to_our_location_dot_gradient / gradient.norm();
const double distance_estimate = corrected_nominal_distance + distance_adjustment;
if ((corrected_nominal_distance >= 0.0) == (distance_estimate >= 0.0))
{
return std::make_pair(distance_estimate, true);
}
else if (corrected_nominal_distance >= 0.0)
{
const double fudge_distance = GetResolution() * 0.0625;
return std::make_pair(fudge_distance, true);
}
else
{
const double fudge_distance = GetResolution() * -0.0625;
return std::make_pair(fudge_distance, true);
}
// else
// {
// const double real_distance_adjustment = GetResolution() * 0.20710678118654757;
// const double revised_corrected_nominal_distance = (nominal_distance >= 0.0) ? nominal_distance - real_distance_adjustment : nominal_distance + real_distance_adjustment;
// const double revised_distance_estimate = revised_corrected_nominal_distance + distance_adjustment;
// return std::make_pair(revised_distance_estimate, true);
// }
}
else
{
return std::make_pair((double)distance_field_.GetOOBValue(), false);
}
}
/*
OPWEIGHTEDL2: Solves weighted L2 regularized inverse problems.
Minimizes the cost function
X* = argmin_X ||A(X)-b_FC||_2^2 + lambda ||W |D(X)| ||_2^2
where X* = recovered image
A = linear measurement operator
b_FC = (noisy) measurements
% W = diagonal weight matrix built from the edge mask
|D(X)| = gradient magnitude at each pixel
Inputs: A = function handle representing the forward
model/measurement operator
At = function handle representing the backwards model/
the transpose of the measurment operator.
(e.g. if A is a downsampling, At is a upsampling)
b_FC = a vector of measurements; should match the
dimensions of A(X)
lambda = regularization parameter that balances data fidelity
and smoothness. set lambda high for more smoothing.
siz = output image size, e.g. siz = [512,512]
Niter = is the number of iterations; should be ~100-500
Output: X = high-resolution output image
cost = array of cost function value vs. iteration
Define AtA fourier mask
PrecisionType lambda, uvec ind_samples, frowvec res, int Niter, double tol, PrecisionType gam, FloatImageType::Pointer& X, frowvec& cost, frowvec& resvec)
*/
FloatImageType::Pointer OpWeightedL2(FloatImageType::Pointer norm01_lowres, FloatImageType::Pointer edgemask)
{
const PrecisionType lambda = 1e-3F ;
constexpr int Niter = 100;
const PrecisionType tol = 1e-8F ;
const PrecisionType gam = 1.0F ;
//typedef itk::VectorMagnitudeImageFilter<CVImageType, FloatImageType> GMType;
//The optimal filter for modeling the measurement operator is low pass filter in this case
// NOTE: That the A operator is a projection operator, so A^{T}A = A, That is to say that applying
// the A^{T} to A results in A.
FloatImageType::Pointer p_image = GetDiracDeltaImage(edgemask);
// Precompute
//Make high-res coefficients
const HalfHermetianImageType::Pointer b_FC = GetAFP_of_b(norm01_lowres, edgemask);
//TODO: too many copies of Atb here.
FloatImageType::Pointer Atb = At_fhp(b_FC,
edgemask->GetLargestPossibleRegion().GetSize()[0]%2 == 1,
edgemask.GetPointer());
FloatImageType::Pointer TwoAtb = MakeTwoAtb(Atb);
FloatImageType::Pointer X = DeepImageCopy<FloatImageType>(Atb);
Atb = nullptr; //Save memory here
CVImageType::Pointer DX = GetGradient(X);
CVImageType::Pointer L = CreateEmptyImage<CVImageType>(DX);
CVImageType::Pointer Y = CreateEmptyImage<CVImageType>(DX);
//CVImageType::Pointer WDX = CreateEmptyImage<CVImageType>(DX);
CVImageType::Pointer residue = CreateEmptyImage<CVImageType>(DX);
CVImageType::Pointer YminusL = CreateEmptyImage<CVImageType>(DX);
FloatImageType::Pointer tempValue=CreateEmptyImage<FloatImageType>(DX);
std::vector<PrecisionType> resvec(Niter,0);
std::vector<PrecisionType> cost(Niter,0);
#ifdef USE_WRITE_DEGUBBING
itk::ComplexToModulusImageFilter<HalfHermetianImageType,FloatImageType>::Pointer cpx2abs =
itk::ComplexToModulusImageFilter<HalfHermetianImageType,FloatImageType>::New();
#endif
CVImageType::Pointer gradIm = GetGradient(p_image);
FloatImageType::Pointer divIm = GetDivergence(gradIm);
HalfHermetianImageType::Pointer DtDhat = GetForwardFFT(divIm);
// TODO: ALL SAME TO HERE!
typedef HalfHermetianImageType::PixelType FCType;
HalfHermetianImageType::Pointer TwoTimesAtAhatPlusLamGamDtDhat = CreateEmptyImage<HalfHermetianImageType>(DtDhat);
{
HalfHermetianImageType::Pointer TwoTimesAtAhat = GetLowpassOperator(norm01_lowres,p_image, 2.0F);
TwoTimesAtAhatPlusLamGamDtDhat = opIC(TwoTimesAtAhatPlusLamGamDtDhat,FCType(lambda*gam),'*',DtDhat);
//TODO: Make This an inverse!
TwoTimesAtAhatPlusLamGamDtDhat = opII(TwoTimesAtAhatPlusLamGamDtDhat,TwoTimesAtAhat,'+',TwoTimesAtAhatPlusLamGamDtDhat);
}
p_image = nullptr; //Save memory
const bool edgemask_ActualXDimensionIsOdd = edgemask->GetLargestPossibleRegion().GetSize()[0] % 2 == 1;
CVImageType::Pointer InvTwoMuPlusGamma = ComputeInvTwoMuPlusGamma(edgemask,gam);
//FloatImageType::Pointer SqrtMu = ComputeSqrtMu(edgemask);
#define USE_BLAS_WRAPPERS
#ifdef USE_BLAS_WRAPPERS
#else
typedef itk::AddImageFilter<CVImageType,CVImageType> CVImageAdder;
CVImageAdder::Pointer dxPlusL = CVImageAdder::New();
#endif
itk::TimeProbe tp;
tp.Start();
HalfHermetianImageType::Pointer tempRatioFC = CreateEmptyImage<HalfHermetianImageType>(DtDhat);
for (size_t i=0; i < Niter; ++i)
{
std::cout << "Iteration : " << i << std::endl;
#ifdef USE_BLAS_WRAPPERS
//Z = 1.0*L+DX
AddAllElements(DX,1.0F,L,DX,gam);//DX destroyed
CVImageType::Pointer & Z = DX;
#else
//Z = opII(Z,DX,'+',L);
dxPlusL->SetInput1(DX);
dxPlusL->SetInput2(L);
dxPlusL->SetInPlace(true);
dxPlusL->Update();
CVImageType::Pointer Z=dxPlusL->GetOutput();
MultiplyCVByScalar(Z,gam);
#endif
#ifdef USE_BLAS_WRAPPERS
//Y=InvTwoMuPlusGamm.*Z
MultiplyVectors(Y,InvTwoMuPlusGamma,Z);
#else
Y = opII(Y,Z,'*',InvTwoMuPlusGamma);
#endif
// X Subprob
// Numerator = 2*Atb+lambda*gam*SRdiv(Y-L))
#ifdef USE_BLAS_WRAPPERS
//YminusL = 1.0F* SRdiv( -1.0F*L + Y)
Duplicate(Y,YminusL);
AddAllElements(YminusL,-1.0F,L,YminusL,1.0F);
#else
YminusL=opII(YminusL,Y,'-',L);
#endif
FloatImageType::Pointer tempNumerator=GetDivergence(YminusL);
#ifdef USE_BLAS_WRAPPERS
//lambd*gam*tempNumerator+TwoAtb
Duplicate(TwoAtb,tempValue);
AddAllElements(tempValue,lambda*gam,tempNumerator,tempValue,1.0F);
HalfHermetianImageType::Pointer tempNumeratorFC = GetForwardFFT(tempValue);
#else
tempNumerator=opIC(tempNumerator,lambda*gam,'*',tempNumerator);
tempNumerator=opII(tempNumerator,TwoAtb,'+',tempNumerator);
HalfHermetianImageType::Pointer tempNumeratorFC = GetForwardFFT(tempNumerator);
#endif
//KEEP
tempRatioFC = opII_scalar(tempRatioFC,tempNumeratorFC,'/',TwoTimesAtAhatPlusLamGamDtDhat);
X = GetInverseFFT(tempRatioFC,edgemask_ActualXDimensionIsOdd,1.0); //TODO: Determine scale factor here. X
// should be on same dynamic range as b
DX = GetGradient(X);
residue = opII(residue, DX, '-', Y); //TODO: Implement math graph output here
L = opII(L,L,'+',residue);
//
resvec[i] = 0; //TODO: Figure out the math for here
if ( i > 900000 )
{
tp.Stop();
std::cout << " Only iterations " << tp.GetTotal() << tp.GetUnit() << std::endl;
return X;
}
if( i > 99 ) //HACK: Cutting this function short
{
return X;
}
//WDX = opII_CVmult(WDX,SqrtMu,'*',DX);
//diff = opII(diff,A_fhp(X,norm01_lowres.GetPointer()),'-',b_FC);
//
//cost[i] = 0; //TODO: Need to figure out math for here
}
return X;
}
/////////////////////////////////////////////////////////////////////////////
// Conjugate Gradient
/////////////////////////////////////////////////////////////////////////////
void CDiffFunction::CG(double vMax[], bool fTrace)
{
const std::vector<double> &vG = GetGradient();
std::vector<double> vPrevG;
std::vector<double> vD;
const int Cycles = GetDimensions();
//
// Loop of iterations
//
for (int Iteration = 0; Iteration < MaxCGIterations; Iteration++)
{
//
// Compute output and gradient at current point
//
GetOutput(&vMax[0]);
ComputeGradient();
//
// At the beginning of a cycle, go in the gradient direction
//
int Cycle = Iteration % Cycles;
if (Cycle == 0)
{
vPrevG = vG;
vD = vG;
}
//
// Otherwise, compute conjugate direction
//
else
{
double Num = 0;
double Den = 0;
for (int i = GetDimensions(); --i >= 0;)
{
Num += vG[i] * (vG[i] - vPrevG[i]);
Den += vPrevG[i] * vPrevG[i];
}
double Beta = Num / Den;
if (Den == 0 || Beta > MaxBeta)
Beta = MaxBeta;
for (int i = GetDimensions(); --i >= 0;)
{
vD[i] = vG[i] + Beta * vD[i];
vPrevG[i] = vG[i];
}
}
//
// Trick to avoid getting stuck at a minimum
//
{
int x = Iteration % GetDimensions();
if (vD[x] * vD[x] == 0.0)
{
if (vD[x] >= 0)
vD[x] = 1.0;
else
vD[x] = -1.0;
}
}
//
// Perform line optimization
//
double x = LineOpt(vMax, &vD[0], fTrace);
double Delta2 = 0.0;
for (int i = GetDimensions(); --i >= 0;)
{
double New = Normalize(vMax[i] + x * vD[i]);
double Delta = New - vMax[i];
vMax[i] = New;
Delta2 += Delta * Delta;
}
if (Cycle == 0 && Delta2 < CGEpsilon)
return;
}
// std::cerr << "warning: reached MaxCGIterations\n";
}
int main(int argc, char **argv)
{
int x, y;
char ch;
int ret;
unsigned char buff[5] = {0,};
unsigned int buf_addr;
unsigned short (*img_buf)[256];
if(argc <= 1) {
printf("Usage 1 : imgproc_test -rd <Read Image Data>\n");
printf("Usage 2 : imgproc_test -dp <Display to LCD>\n");
printf("Usage 3 : imgproc_test -dp2 <Display 2 Frame>\n");
exit(1);
}
eagle_camera_off(); // 커널에서 동작되는 Camera OFF : comment by yyb[110909]
//[[ molink_yyb_110909_BEGIN -- 로봇 몸체와의 통신을 위한 UART 초기화
ret = uart_open();
if (ret < 0) return EXIT_FAILURE;
uart_config(UART1, 115200, 8, UART_PARNONE, 1);
//]] molink_yyb_110909_END -- 로봇 몸체와의 통신을 위한 UART 초기화
ret = ImageProcess_Open(); // 이미지 처리를 위한 드라이버 Open : comment by yyb[110909]
if (ret < 0) return EXIT_FAILURE;
Delay(0xffffff);
if(strcmp("-rd", argv[1]) == 0) {
ret = ReadImageFromFPGA(&buf_addr); // FPGA로부터 1Frame(180x120)의 이미지 Read : comment by yyb[110909]
if (ret < 0) return EXIT_FAILURE;
img_buf = (unsigned short (*)[256])buf_addr;
//TEST
GetDistance(img_buf);
GetGradient(img_buf);
}
else if(strcmp("-dp", argv[1]) == 0) {
ret = ImgDisplayToLCD(); // FPGA로 부터 읽어온 이미지를 LCD에 실시간으로 Display : comment by yyb[110909]
if (ret < 0) return EXIT_FAILURE;
printf("\nPress Enter Key to STOP the test !!!");
ch = getchar();
ImgDisplayQuit(); // LCD Display 종료 : comment by yyb[110909]
printf("\nTest is Stopped\n");
}
else if(strcmp("-dp2", argv[1]) == 0) {
ClearScreen(255, 255, 255); // LCD 화면을 백색으로 Clear : comment by yyb[110909]
gFlip(); // 그래픽 처리된 내용을 LCD에 보여줌 : comment by yyb[110909]
ClearScreen(255, 255, 255);
gFlip();
ClearScreen(255, 255, 255);
gFlip();
ClearScreen(255, 255, 255);
gFlip();
printf("Clear Screen!\n");
ret = ReadImageFromFPGA(&buf_addr);
if (ret < 0) return EXIT_FAILURE;
printf("Image Load from FPGA!\n");
draw_value.imgbuf_en = 0; // 읽어 온 데이터를 처리하지 않고 그대로 다시 LCD에 보여줄 때 설정 : comment by yyb[110909]
ClearScreen(255, 255, 255);
draw_img_from_buffer((unsigned short *)buf_addr, 160, 130, 1.8, 0); // buf_addr에 들어 있는 내용을 1.8배 확대하여 0도 회전하고 중심을 (160, 130)로 하여 Display : comment by yyb[110909]
// gFlip();
printf("1. Draw Image to LCD!\n");
// }
printf("\nPress Enter Key to load image again !!!");
ch = getchar();
ret = ReadImageFromFPGA(&buf_addr);
if (ret < 0) return EXIT_FAILURE;
img_buf = (unsigned short (*)[256])buf_addr;
printf("Image Load from FPGA!\n");
//////////////////////////////////////////////////////////////////
// img_buf의 데이터에 대한 이미지 처리 과정이 이 위치에서 이루어짐 //
//////////////////////////////////////////////////////////////////
draw_value.imgbuf_en = 1; // 읽어 온 데이터를 처리하고, 처리된 데이터를 LCD에 보여줄 때 설정 : comment by yyb[110909]
draw_img_from_buffer((unsigned short *)buf_addr, 160, 360, 1.8, 0);
gFlip();
printf("2. Draw Image to LCD!\n");
printf("\nPress Enter Key to STOP the test !!!");
ch = getchar();
printf("\nTest is Stopped\n");
}
else {
printf("Usage 1 : imgproc_test -rd <Read Image Data>\n");
printf("Usage 2 : imgproc_test -dp <Display to LCD>\n");
printf("Usage 3 : imgproc_test -dp2 <Display 2 Frame>\n");
exit(-1);
}
uart_close();
close(devfb);
eagle_camera_on(); // 커널에서 동작되는 Camera ON : comment by yyb[110909]
return(0);
}
/////////////////////////////////////////////////////////////////////////////
// Newton-Raphson's method
/////////////////////////////////////////////////////////////////////////////
void CDiffFunction::Newton(std::vector<double> &vMax, bool fTrace)
{
const std::vector<double> &vG = GetGradient();
const std::vector<double> &vH = GetHessian();
int n = GetDimensions();
if (fTrace)
std::cout << '\n';
for (int Iterations = MaxNewtonIterations; --Iterations >= 0;)
{
double L = GetOutput(&vMax[0]);
ComputeGradient();
ComputeHessian();
if (fTrace)
{
std::cout << std::setw(5) << Iterations;
std::cout << std::setw(12) << vMax[0];
std::cout << std::setw(12) << L;
std::cout << std::setw(12) << vG[0];
std::cout << std::setw(12) << vH[0];
std::cout << '\n';
}
//
// Compute Gradient multiplied by inverse of opposite of Hessian
//
vStep = vG;
if (CMatrixOperations::Cholesky(&vH[0], &vCholesky[0], n))
CMatrixOperations::Solve(&vCholesky[0], &vStep[0], n);
//
// If the Hessian is not definite negative, CG
//
else
{
CG(&vMax[0], fTrace);
return;
}
//
// Apply change
//
for (int i = n; --i >= 0;)
vxTemp[i] = Normalize(vMax[i] + vStep[i]);
//
// If probability did not improve, use CG
//
double LNew = GetOutput(&vxTemp[0]);
double NewtonStep = 1.0;
if ((LNew != LNew || LNew < L) && NewtonStep > MinNewtonStep)
{
CG(&vMax[0], fTrace);
return;
}
//
// Stop looping if small improvement
//
vMax = vxTemp;
if (LNew - L < NewtonThreshold)
return;
}
//std::cerr << "warning: reached MaxNewtonIterations\n";
}
void Implicit::Render()
{
// Draw bounding box for debugging
Bbox b = GetBoundingBox();
Vector3<float> & v0 = b.pMin;
Vector3<float> & v1 = b.pMax;
if(mSelected) {
glLineWidth(2.0f);
glColor3f(0.8f,0.8f,0.8f);
}
else glColor3f(0.1f,0.1f,0.1f);
glBegin(GL_LINE_STRIP);
glVertex3f(v0[0], v0[1], v0[2]);
glVertex3f(v1[0], v0[1], v0[2]);
glVertex3f(v1[0], v1[1], v0[2]);
glVertex3f(v0[0], v1[1], v0[2]);
glVertex3f(v0[0], v0[1], v0[2]);
glEnd();
glBegin(GL_LINE_STRIP);
glVertex3f(v0[0], v0[1], v1[2]);
glVertex3f(v1[0], v0[1], v1[2]);
glVertex3f(v1[0], v1[1], v1[2]);
glVertex3f(v0[0], v1[1], v1[2]);
glVertex3f(v0[0], v0[1], v1[2]);
glEnd();
glBegin(GL_LINES);
glVertex3f(v0[0], v0[1], v0[2]);
glVertex3f(v0[0], v0[1], v1[2]);
glVertex3f(v1[0], v0[1], v0[2]);
glVertex3f(v1[0], v0[1], v1[2]);
glVertex3f(v0[0], v1[1], v0[2]);
glVertex3f(v0[0], v1[1], v1[2]);
glVertex3f(v1[0], v1[1], v0[2]);
glVertex3f(v1[0], v1[1], v1[2]);
glEnd();
glLineWidth(1.0f);
glPushMatrix();
glMultMatrixf(mTransform.ToGLMatrix().GetArrayPtr());
Geometry * mesh = dynamic_cast<Geometry *>(mMesh);
if (mesh == NULL)
std::cerr << "Error: implicit geometry not triangulated, add call to triangulate()" << std::endl;
else {
mesh->SetShowNormals(mShowNormals);
mesh->SetWireframe(mWireframe);
mesh->SetOpacity(mOpacity);
mesh->Render();
}
if (mVisualizationMode == Gradients) {
if(typeid(*mMesh) == typeid(SimpleMesh)){
SimpleMesh * ptr = static_cast<SimpleMesh*>(mMesh);
const std::vector<SimpleMesh::Vertex>& verts = ptr->GetVerts();
glDisable(GL_LIGHTING);
Matrix4x4<float> M = GetTransform().Transpose();
glColor3f(0, 0, 1);
glBegin(GL_LINES);
for(unsigned int i=0; i < verts.size(); i++){
const Vector3<float> vObject = verts.at(i).pos;
// Transform vertex position to world space
Vector4<float> vWorld = GetTransform() * Vector4<float>(vObject[0],vObject[1],vObject[2],1);
// Get gradient in world space
Vector3<float> nWorld = GetGradient(vWorld[0], vWorld[1], vWorld[2]);
// Transform gradient to object space
Vector4<float> nObject = M * Vector4<float>(nWorld[0],nWorld[1],nWorld[2],0);
Vector3<float> n = Vector3<float>(nObject[0], nObject[1], nObject[2]);
glVertex3fv(vObject.GetArrayPtr());
glVertex3fv((vObject + n*0.1).GetArrayPtr());
}
glEnd();
}
else {
std::cerr << "No Gradient visualization mode implemented for mesh type: " << typeid(*mMesh).name() << std::endl;
}
}
glPopMatrix();
GLObject::Render();
}
