void db_FrameToReferenceRegistration::Set_H_dref_to_ins(double H[9])
{
double H_ins_to_ref[9];
db_Identity3x3(H_ins_to_ref); // Ensure it has proper values
db_InvertAffineTransform(H_ins_to_ref,m_H_ref_to_ins); // Invert to get ins to ref
db_Multiply3x3_3x3(m_H_dref_to_ref,H,H_ins_to_ref); // Update dref to ref using the input H from dref to ins
}
// Save the reference image, detect features and update the dref-to-ref transformation
int db_FrameToReferenceRegistration::UpdateReference(const unsigned char * const * im, bool subsample, bool detect_corners)
{
double temp[9];
db_Multiply3x3_3x3(temp,m_H_dref_to_ref,m_H_ref_to_ins);
db_Copy9(m_H_dref_to_ref,temp);
const unsigned char * const * imptr = im;
if (m_quarter_resolution && subsample)
{
GenerateQuarterResImage(im);
imptr = m_quarter_res_image;
}
// save the reference image, detect features and quit
db_CopyImage_u(m_reference_image,imptr,m_im_width,m_im_height,m_over_allocation);
if(detect_corners)
{
#if MB
m_cd.DetectCorners(imptr, m_x_corners_ref,m_y_corners_ref,&m_nr_corners_ref);
int nr = 0;
for(int k=0; k<m_nr_corners_ref; k++)
{
if(m_x_corners_ref[k]>m_im_width/3)
{
m_x_corners_ref[nr] = m_x_corners_ref[k];
m_y_corners_ref[nr] = m_y_corners_ref[k];
nr++;
}
}
m_nr_corners_ref = nr;
#else
m_cd.DetectCorners(imptr, m_x_corners_ref,m_y_corners_ref,&m_nr_corners_ref);
#endif
}
else
{
m_nr_corners_ref = m_nr_corners_ins;
for(int k=0; k<m_nr_corners_ins; k++)
{
m_x_corners_ref[k] = m_x_corners_ins[k];
m_y_corners_ref[k] = m_y_corners_ins[k];
}
}
db_Identity3x3(m_H_ref_to_ins);
m_max_inlier_count = 0; // Reset to 0 as no inliers seen until now
m_sq_cost_computed = false;
m_reference_set = true;
m_current_is_reference = true;
return 1;
}
void AllocateTextureMemory(int widthHR, int heightHR, int widthLR, int heightLR)
{
gPreviewImageWidth[HR] = widthHR;
gPreviewImageHeight[HR] = heightHR;
gPreviewImageWidth[LR] = widthLR;
gPreviewImageHeight[LR] = heightLR;
sem_wait(&gPreviewImage_semaphore);
gPreviewImage[LR] = ImageUtils::allocateImage(gPreviewImageWidth[LR],
gPreviewImageHeight[LR], 4);
gPreviewImage[HR] = ImageUtils::allocateImage(gPreviewImageWidth[HR],
gPreviewImageHeight[HR], 4);
sem_post(&gPreviewImage_semaphore);
gPreviewFBOWidth = PREVIEW_FBO_WIDTH_SCALE * gPreviewImageWidth[HR];
gPreviewFBOHeight = PREVIEW_FBO_HEIGHT_SCALE * gPreviewImageHeight[HR];
// The origin is such that the current frame will sit with its center
// at the center of the previewFBO
gCenterOffsetX = (gPreviewFBOWidth / 2 - gPreviewImageWidth[HR] / 2);
gCenterOffsetY = (gPreviewFBOHeight / 2 - gPreviewImageHeight[HR] / 2);
gPanOffset = 0.0f;
db_Identity3x3(gThisH1t);
db_Identity3x3(gLastH1t);
gPanViewfinder = true;
int w = gPreviewImageWidth[HR];
int h = gPreviewImageHeight[HR];
int wm = gPreviewFBOWidth;
int hm = gPreviewFBOHeight;
// K is the transformation to map the canonical [-1,1] vertex coordinate
// system to the [0,w] image coordinate system before applying the given
// affine transformation trs.
gKm[0] = wm / 2.0 - 0.5;
gKm[1] = 0.0;
gKm[2] = wm / 2.0 - 0.5;
gKm[3] = 0.0;
gKm[4] = hm / 2.0 - 0.5;
gKm[5] = hm / 2.0 - 0.5;
gKm[6] = 0.0;
gKm[7] = 0.0;
gKm[8] = 1.0;
gK[0] = w / 2.0 - 0.5;
gK[1] = 0.0;
gK[2] = w / 2.0 - 0.5;
gK[3] = 0.0;
gK[4] = h / 2.0 - 0.5;
gK[5] = h / 2.0 - 0.5;
gK[6] = 0.0;
gK[7] = 0.0;
gK[8] = 1.0;
db_Identity3x3(gKinv);
db_InvertCalibrationMatrix(gKinv, gK);
db_Identity3x3(gKminv);
db_InvertCalibrationMatrix(gKminv, gKm);
//////////////////////////////////////////
////// Compute g_Translation now... //////
//////////////////////////////////////////
double T[9], Tp[9], Ttemp[9];
db_Identity3x3(T);
T[2] = gCenterOffsetX;
T[5] = gCenterOffsetY;
// Tp = inv(K) * T * K
db_Identity3x3(Ttemp);
db_Multiply3x3_3x3(Ttemp, T, gK);
db_Multiply3x3_3x3(Tp, gKinv, Ttemp);
ConvertAffine3x3toGL4x4(g_dTranslationToFBOCenter, Tp);
UpdateWarpTransformation(g_dIdent3x3);
}
// This function computes fills the 4x4 matrices g_dAffinetrans,
// and g_dAffinetransPan using the specified 3x3 affine
// transformation between the first captured frame and the current frame.
// The computed g_dAffinetrans is such that it warps the preview mosaic in
// the last frame's coordinate system into the coordinate system of the
// current frame. Thus, applying this transformation will create the current
// frame mosaic but with the current frame missing. This frame will then be
// pasted in by gWarper2 after translating it by g_dTranslationToFBOCenter.
// The computed g_dAffinetransPan is such that it offsets the computed preview
// mosaic horizontally to make the viewfinder pan within the UI layout.
void UpdateWarpTransformation(float *trs)
{
double H[9], Hp[9], Htemp1[9], Htemp2[9], T[9];
for(int i = 0; i < 9; i++)
{
gThisH1t[i] = trs[i];
}
// Alignment is done based on low-res data.
// To render the preview mosaic, the translation of the high-res mosaic is estimated to
// H2L_FACTOR x low-res-based tranlation.
gThisH1t[2] *= H2L_FACTOR;
gThisH1t[5] *= H2L_FACTOR;
db_Identity3x3(T);
T[2] = -gCenterOffsetX;
T[5] = -gCenterOffsetY;
// H = ( inv(gThisH1t) * gLastH1t ) * T
db_Identity3x3(Htemp1);
db_Identity3x3(Htemp2);
db_Identity3x3(H);
db_InvertAffineTransform(Htemp1, gThisH1t);
db_Multiply3x3_3x3(Htemp2, Htemp1, gLastH1t);
db_Multiply3x3_3x3(H, Htemp2, T);
for(int i = 0; i < 9; i++)
{
gLastH1t[i] = gThisH1t[i];
}
// Move the origin such that the frame is centered in the previewFBO
// i.e. H = inv(T) * H
H[2] += gCenterOffsetX;
H[5] += gCenterOffsetY;
// Hp = inv(Km) * H * Km
// Km moves the coordinate system from openGL to image pixels so
// that the alignment transform H can be applied to them.
// inv(Km) moves the coordinate system back to openGL normalized
// coordinates so that the shader can correctly render it.
db_Identity3x3(Htemp1);
db_Multiply3x3_3x3(Htemp1, H, gKm);
db_Multiply3x3_3x3(Hp, gKminv, Htemp1);
ConvertAffine3x3toGL4x4(g_dAffinetrans, Hp);
////////////////////////////////////////////////
////// Compute g_dAffinetransPan now... //////
////////////////////////////////////////////////
gThisTx = trs[2];
if(gPanViewfinder)
{
gPanOffset += (gThisTx - gLastTx) * VIEWFINDER_PAN_FACTOR_HORZ;
}
gLastTx = gThisTx;
gPanViewfinder = continuePanningFBO(gPanOffset);
db_Identity3x3(H);
H[2] = gPanOffset;
// Hp = inv(Km) * H * Km
db_Identity3x3(Htemp1);
db_Multiply3x3_3x3(Htemp1, H, gKm);
db_Multiply3x3_3x3(Hp, gKminv, Htemp1);
//if (gIsLandscapeOrientation) {
ConvertAffine3x3toGL4x4(g_dAffinetransPan, Hp);
//} else {
// // rotate Hp by 90 degress.
// db_Multiply3x3_3x3(Htemp1, gRotation90, Hp);
// ConvertAffine3x3toGL4x4(g_dAffinetransPan, Htemp1);
// }
}
void db_FrameToReferenceRegistration::Get_H_dref_to_ins(double H[9])
{
db_Multiply3x3_3x3(H,m_H_dref_to_ref,m_H_ref_to_ins);
}
