void translate_bb(graph_t * g, int rankdir)
{
int c;
boxf bb, new_bb;
bb = GD_bb(g);
if (rankdir == RANKDIR_LR || rankdir == RANKDIR_BT) {
new_bb.LL = map_point(pointfof(bb.LL.x, bb.UR.y));
new_bb.UR = map_point(pointfof(bb.UR.x, bb.LL.y));
} else {
new_bb.LL = map_point(pointfof(bb.LL.x, bb.LL.y));
new_bb.UR = map_point(pointfof(bb.UR.x, bb.UR.y));
}
GD_bb(g) = new_bb;
if (GD_label(g)) {
GD_label(g)->pos = map_point(GD_label(g)->pos);
}
for (c = 1; c <= GD_n_cluster(g); c++)
translate_bb(GD_clust(g)[c], rankdir);
}
void map_edge(edge_t * e)
{
int j, k;
bezier bz;
if (ED_spl(e) == NULL) {
if ((Concentrate == FALSE) || (ED_edge_type(e) != IGNORED))
agerr(AGERR, "lost %s %s edge\n", e->tail->name,
e->head->name);
return;
}
for (j = 0; j < ED_spl(e)->size; j++) {
bz = ED_spl(e)->list[j];
for (k = 0; k < bz.size; k++)
bz.list[k] = map_point(bz.list[k]);
if (bz.sflag)
ED_spl(e)->list[j].sp = map_point(ED_spl(e)->list[j].sp);
if (bz.eflag)
ED_spl(e)->list[j].ep = map_point(ED_spl(e)->list[j].ep);
}
if (ED_label(e))
ED_label(e)->p = map_point(ED_label(e)->p);
/* vladimir */
if (ED_head_label(e))
ED_head_label(e)->p = map_point(ED_head_label(e)->p);
if (ED_tail_label(e))
ED_tail_label(e)->p = map_point(ED_tail_label(e)->p);
}
/* translate_drawing:
* Translate and/or rotate nodes, spline points, and bbox info if
* Offset is non-trivial.
* Also, if Rankdir, reset ND_lw, ND_rw, and ND_ht to correct value.
*/
static void translate_drawing(graph_t * g)
{
node_t *v;
edge_t *e;
int shift = (Offset.x || Offset.y);
if (!shift && !Rankdir) return;
for (v = agfstnode(g); v; v = agnxtnode(g, v)) {
if (Rankdir) dot_nodesize(v, FALSE);
if (shift) {
ND_coord_i(v) = map_point(ND_coord_i(v));
if (State == GVSPLINES)
for (e = agfstout(g, v); e; e = agnxtout(g, e))
map_edge(e);
}
}
if (shift)
translate_bb(g, GD_rankdir(g));
}
void Graph::showGraph(void)
{
cv::Size mapSize = map_.size();
for( int j = 0; j < mapSize.height; j++ )
{
for( int i = 0; i < mapSize.width; i++ )
{
MapPoint map_point(j,i);
if( isNodeInGraph(map_point.label) )
std::cout << std::setiosflags(std::ios::fixed) << std::setprecision(2)
<< std::setw(4) << nodes_list_[map_point.label]->total_cost_ << " ";
else if ((int)map_.at<uchar>(j, i) < MAP_FREE_THRESHOLD)
std::cout << " * ";
else
std::cout << " . ";
}
std::cout << "\n";
}
std::cout << std::endl;
}
QImage ImageStitching::getStitchedImages(QImage *target, QList<QPoint> target_points, QImage *source, QList<QPoint> source_points)
{
QImage new_image( Utility::blackImage(target->size()) );
_target = target;
_source = source;
Q_ASSERT(target_points.count() >= 4);
Q_ASSERT(source_points.count() >= 4);
// target_points.clear();
// target_points.append(QPoint(27, 231));
// target_points.append((QPoint(195, 229)));
// target_points.append(QPoint(170, 242));
// target_points.append(QPoint(53, 243));
// source_points.clear();
// source_points.append(QPoint(37, 29));
// source_points.append(QPoint(205, 29));
// source_points.append(QPoint(182,41));
// source_points.append(QPoint(62,43));
//Shift all points from a central coordinate system to a topleft corner system.
QPoint target_shift(target->width()/2, target->height()/2);
QPoint source_shift(source->width()/2, source->height()/2);
//Build our homographies
if ( target_points.count() == source_points.count() ) {
_A = Array2D<double>(8, 8, 0.0);
_B = Array2D<double>(8, 1, 0.0);
//Initialize our matrixes
for(int i=0; i < target_points.count(); i++) {
//First Row of A = i*2
int x_row = i*2;
int y_row = x_row + 1;
QPoint tp = target_points.at(i) + target_shift;
QPoint sp = source_points.at(i) + source_shift;
// QPoint tp = target_points.at(i);
// QPoint sp = source_points.at(i);
// qDebug() << "X: " << tp.x() << "->" << sp.x() << " Y: " << tp.y() << "->" << sp.y();
//X Row
_A[x_row][0] = tp.x();
_A[x_row][1] = tp.y();
_A[x_row][2] = 1;
_A[x_row][6] = -(sp.x() * tp.x());
_A[x_row][7] = -(sp.x() * tp.y());
_B[x_row][0] = sp.x();
qDebug() << "A: " << _A[x_row][0] << "," << _A[x_row][1] << "," << _A[x_row][2] << "," << _A[x_row][3] << "," << _A[x_row][4] << "," << _A[x_row][5] << "," << _A[x_row][6] << "," << _A[x_row][7];
qDebug() << "B: " << _B[x_row][0];
//Y Row
_A[y_row][3] = tp.x();
_A[y_row][4] = tp.y();
_A[y_row][5] = 1;
_A[y_row][6] = -(sp.y() * tp.x());
_A[y_row][7] = -(sp.y() * tp.y());
_B[y_row][0] = sp.y();
qDebug() << "A: " << _A[y_row][0] << "," << _A[y_row][1] << "," << _A[y_row][2] << "," << _A[y_row][3] << "," << _A[y_row][4] << "," << _A[y_row][5] << "," << _A[y_row][6] << "," << _A[y_row][7];
qDebug() << "B: " << _B[y_row][0];
}
//Solve for our homography
JAMA::LU<double> lu(_A);
Array2D<double> result = lu.solve(_B);
// JAMA::Cholesky<double> cho(_A);
// Array1D<double> result = cho.solve(_B);
_homography[0][0] = result[0][0];
_homography[0][1] = result[1][0];
_homography[0][2] = result[2][0];
_homography[1][0] = result[3][0];
_homography[1][1] = result[4][0];
_homography[1][2] = result[5][0];
_homography[2][0] = result[6][0];
_homography[2][1] = result[7][0];
_homography[2][2] = 1;
//Find the inverse of our homography
// JAMA::LU<double> h_solver(_homography);
// Array2D<double> id(_homography.dim1(), _homography.dim2(), 0.0);
// for (int i = 0; i < _homography.dim1(); i++)
// id[i][i] = 1;
// solves A * A_inv = Identity
// _h_prime = h_solver.solve(id);
} else {
//Is it over constrained?
}
//Now perform warping with the homography
Array2D<double> position(3, 1, 0.0);
position[2][0] = 1.0;
for(int y=0; y < target->height(); y++) {
for(int x=0; x < target->width(); x++) {
QPoint p(x,y);
position[0][0] = (double) x;
position[1][0] = (double) y;
double x_prime = ((_homography[0][0] * x) + (_homography[0][1] * y) + _homography[0][2]) / ((_homography[2][0] * x) + (_homography[2][1] * y) + 1);
double y_prime = ((_homography[1][0] * x) + (_homography[1][1] * y) + _homography[1][2]) / ((_homography[2][0] * x) + (_homography[2][1] * y) + 1);
QPoint map_point(qRound(x_prime), qRound(y_prime));
if ( map_point.x() < source->width() && map_point.x() >= 0 && map_point.y() < source->height() && map_point.y() >= 0) {
// int x_max = qRound(x_prime + 0.5);
// int x_min = qRound(x_prime - 0.5);
// int y_max = qRound(y_prime + 0.5);
// int y_min = qRound(y_prime - 0.5);
// double right_diff = qAbs(x_prime - x_max);
// double left_diff = qAbs(x_prime - x_min);
// QRgb br = source->pixel(QPoint(x_max, y_max));
// QRgb bl = source->pixel(QPoint(x_min, y_max));
// int b_red_val = qRound((qRed(br) * left_diff) + (qRed(bl) * right_diff) );
// int b_green_val = qRound((qGreen(br) * left_diff) + (qGreen(bl) * right_diff) );
// int b_blue_val = qRound((qBlue(br) * left_diff) + (qBlue(bl) * right_diff) );
// QRgb tr = source->pixel(QPoint(x_max, y_min));
// QRgb tl = source->pixel(QPoint(x_min, y_min));
// int t_red_val = qRound(qRed(tr) * left_diff + qRed(tl) * right_diff );
// int t_green_val = qRound(qGreen(tr) * left_diff + qGreen(tl) * right_diff );
// int t_blue_val = qRound(qBlue(tr) * left_diff + qBlue(tl) * right_diff );
// //Now do the up and down calc
// double up_diff = qAbs(y_prime - y_min);
// double down_diff = qAbs(y_prime, y_max);
// int final_red = qRound((b_red_val * up_diff) + (t_red_val * down_diff));
// int final_green = qRound((b_green_val * up_diff) + (t_green_val * down_diff));
// int final_blue = qRound((b_blue_val * up_diff) + (t_blue_val * down_diff));
// new_image.setPixel(p, qRgb(final_red, final_green, final_blue));
new_image.setPixel(p, source->pixel(map_point));
} else {
new_image.setPixel(p, target->pixel(p));
}
}
}
return new_image;
}
static int dview_on_mouse(DView *dv, int mx, int my, int button)
{
int clicked = (button && !dview_is_state(dv, VIEW_STATE_BUTTON));
int handled = TRUE;
int x,y;
if (button && dview_is_state_all(dv, VIEW_STATE_BUTTON | VIEW_STATE_MOVING))
{
int dx = mx - dv->last_x;
int dy = my - dv->last_y;
dv->ofs_x += dx;
dv->ofs_y += dy;
dv->last_x = mx;
dv->last_y = my;
dview_set_state(dv, VIEW_STATE_NEEDS_UPDATE, TRUE);
return TRUE;
}
else if (button && dview_is_state_all(dv, VIEW_STATE_BUTTON | VIEW_STATE_SIZING))
{
int dx = mx - dv->last_x;
int dy = my - dv->last_y;
dv->bounds.max_x += dx;
dv->bounds.max_y += dy;
dv->last_x = mx;
dv->last_y = my;
dview_set_state(dv, VIEW_STATE_NEEDS_UPDATE, TRUE);
return TRUE;
}
else
dview_set_state(dv, VIEW_STATE_MOVING | VIEW_STATE_SIZING, FALSE);
if (!map_point(dv, mx, my, &x, &y))
return FALSE;
if (dview_xy_in_rect(dv, RECT_DVIEW_TITLE, x, y))
{
/* on title, do nothing */
if (clicked) {
dv->last_x = mx;
dv->last_y = my;
set_focus_view(dv);
dview_set_state(dv, VIEW_STATE_MOVING, TRUE);
}
}
else if (dview_xy_in_rect(dv, RECT_DVIEW_HSB, x, y))
{
/* on horizontal scrollbar */
debug_view_xy pos;
adjustment *sb = &dv->hsb;
if (clicked)
{
rectangle r;
int xt;
dview_get_rect(dv, RECT_DVIEW_HSB, &r);
x -= r.min_x;
xt = (x - VSB_WIDTH) * (sb->upper - sb->lower) / (rect_get_width(&r) - 2 * dv->vsb.visible * VSB_WIDTH) + sb->lower;
if (x < VSB_WIDTH)
sb->value -= sb->step_increment;
else if (x > rect_get_width(&r) - VSB_WIDTH)
sb->value += sb->step_increment;
else if (xt < sb->value)
sb->value -= sb->page_increment;
else if (xt > sb->value)
sb->value += sb->page_increment;
if (sb->value < sb->lower)
sb->value = sb->lower;
if (sb->value > sb->upper)
sb->value = sb->upper;
}
pos = dv->view->visible_position();
if (sb->value != pos.x)
{
pos.x = sb->value;
dv->view->set_visible_position(pos);
dview_set_state(dv, VIEW_STATE_NEEDS_UPDATE, TRUE);
}
}
else if (dview_xy_in_rect(dv, RECT_DVIEW_VSB, x, y) )
{
/* on vertical scrollbar */
debug_view_xy pos;
adjustment *sb = &dv->vsb;
if (clicked)
{
rectangle r;
int yt;
dview_get_rect(dv, RECT_DVIEW_VSB, &r);
y -= r.min_y;
yt = (y - HSB_HEIGHT) * (sb->upper - sb->lower) / (rect_get_height(&r) - 2 * HSB_HEIGHT) + sb->lower;
if (y < HSB_HEIGHT)
sb->value -= sb->step_increment;
else if (y > rect_get_height(&r) - HSB_HEIGHT)
sb->value += sb->step_increment;
else if (yt < sb->value)
sb->value -= sb->page_increment;
else if (yt > sb->value)
sb->value += sb->page_increment;
if (sb->value < sb->lower)
sb->value = sb->lower;
if (sb->value > sb->upper)
sb->value = sb->upper;
}
pos = dv->view->visible_position();
if (sb->value != pos.y)
{
pos.y = sb->value;
dv->view->set_visible_position(pos);
dview_set_state(dv, VIEW_STATE_NEEDS_UPDATE, TRUE);
}
}
else if (dview_xy_in_rect(dv, RECT_DVIEW_SIZE, x, y))
{
/* on sizing area */
if (clicked)
{
dv->last_x = mx;
dv->last_y = my;
set_focus_view(dv);
dview_set_state(dv, VIEW_STATE_SIZING, TRUE);
}
}
else if (dview_xy_in_rect(dv, RECT_DVIEW_CLIENT, x, y))
{
y -= TITLE_HEIGHT;
if (dv->view->cursor_supported() && clicked && y >= 0)
{
debug_view_xy topleft = dv->view->visible_position();
debug_view_xy newpos;
newpos.x = topleft.x + x / debug_font_width;
newpos.y = topleft.y + y / debug_font_height;
dv->view->set_cursor_position(newpos);
dv->view->set_cursor_visible(true);
}
if (clicked)
set_focus_view(dv);
}
else
{
handled = FALSE;
}
dview_set_state(dv, VIEW_STATE_BUTTON, button);
return handled;
}
void rspfLandSatModel::imagingRay(const rspfDpt& inImgPt,
rspfEcefRay& image_ray) const
{
#if 0
if (traceExec()) rspfNotify(rspfNotifyLevel_DEBUG) << "rspfLandSatModel::imagingRay: entering..." << std::endl;
bool debug_flag = false; // setable by interactive debugger
if (traceDebug() || debug_flag)
{
rspfNotify(rspfNotifyLevel_DEBUG) << "inImgPt = " << inImgPt << std::endl;
}
#endif
rspfDpt rot_img_pt(-inImgPt.line*theMapAzimSin+inImgPt.samp*theMapAzimCos,
inImgPt.line*theMapAzimCos+inImgPt.samp*theMapAzimSin);
rspfDpt map_point
(theMapOffset.x + rot_img_pt.samp*(theGSD.samp+theSampGsdCorr),
theMapOffset.y - rot_img_pt.line*(theGSD.line+theLineGsdCorr));
rspfGpt inGndPt (theMapProjection->inverse(map_point));
#if 0
if (traceDebug() || debug_flag)
{
rspfNotify(rspfNotifyLevel_DEBUG) << "\t theMapOffset="<<theMapOffset<<endl;
rspfNotify(rspfNotifyLevel_DEBUG) << "\t rot_img_pt="<<rot_img_pt<<endl;
rspfNotify(rspfNotifyLevel_DEBUG) << "\t image point map_point="<<map_point<<endl;
rspfNotify(rspfNotifyLevel_DEBUG) << "\t inGndPt="<<inGndPt<<endl;
}
#endif
rspfDpt offInMapPt (inImgPt - theRefImgPt);
rspfDpt icInPt
(offInMapPt.line*theMap2IcRotSin + offInMapPt.samp*theMap2IcRotCos,
offInMapPt.line*theMap2IcRotCos - offInMapPt.samp*theMap2IcRotSin);
#if 0
if (traceDebug() || debug_flag)
{
rspfNotify(rspfNotifyLevel_DEBUG) << "\t offInMapPt="<<offInMapPt<<endl;
rspfNotify(rspfNotifyLevel_DEBUG) << "\t icInPt="<<icInPt<<endl;
}
#endif
rspfDpt icNdrPt (0.0, icInPt.line);
rspfDpt offNdrMapPt
(-icNdrPt.line*theMap2IcRotSin + icNdrPt.samp*theMap2IcRotCos,
icNdrPt.line*theMap2IcRotCos + icNdrPt.samp*theMap2IcRotSin);
rspfDpt ndrMapPt(offNdrMapPt + theRefImgPt);
rspfDpt rotNdrMapPt
(-ndrMapPt.line*theMapAzimSin + ndrMapPt.samp*theMapAzimCos,
ndrMapPt.line*theMapAzimCos + ndrMapPt.samp*theMapAzimSin);
#if 0
if (traceDebug() || debug_flag)
{
rspfNotify(rspfNotifyLevel_DEBUG) << "\t icNdrPt="<<icNdrPt<<endl;
rspfNotify(rspfNotifyLevel_DEBUG) << "\t offNdrMapPt="<<offNdrMapPt<<endl;
rspfNotify(rspfNotifyLevel_DEBUG) << "\t ndrMapPt="<<ndrMapPt<<endl;
rspfNotify(rspfNotifyLevel_DEBUG) << "\t rotNdrMapPt="<<rotNdrMapPt<<endl;
}
#endif
map_point.y =theMapOffset.y-rotNdrMapPt.y*(theGSD.y+theLineGsdCorr);//theSampGsdCorr);
if ((theProjectionType == SOM_ORBIT) || (theProjectionType == SOM_MAP))
map_point.x = theMapOffset.x+rotNdrMapPt.y*(theGSD.y+theLineGsdCorr);
else
map_point.x = theMapOffset.x+rotNdrMapPt.x*(theGSD.x+theSampGsdCorr);
rspfGpt vehiclePlhPos(theMapProjection->inverse(map_point));
vehiclePlhPos.hgt = theOrbitAltitude;
#if 0
if (traceDebug() || debug_flag)
{
rspfNotify(rspfNotifyLevel_DEBUG) << "\t map_point="<<map_point<<endl;
rspfNotify(rspfNotifyLevel_DEBUG) << "\t vehiclePlhPos="<<vehiclePlhPos<<endl;
}
#endif
rspfLsrSpace icrSpace (vehiclePlhPos, theMeridianalAngle-90.0);
#if 0
if (traceDebug() || debug_flag)
{
rspfNotify(rspfNotifyLevel_DEBUG) << "\t icrSpace="<<icrSpace<<endl;
}
#endif
rspfLsrPoint lsrInPt (inGndPt, icrSpace);
rspfLsrPoint vehicleLsrPos (0.0, 0.0, 0.0, icrSpace);
rspfLsrRay initLsrImgRay (vehicleLsrPos, lsrInPt);
#if 0
if (traceDebug() || debug_flag)
{
rspfNotify(rspfNotifyLevel_DEBUG) << "\t initLsrImgRay="<<initLsrImgRay<<endl;
}
#endif
double cos, sin;
double norm_line = inImgPt.line/theImageSize.line;
double yaw = theYawOffset + theYawRate*norm_line;
cos = rspf::cosd(yaw);
sin = rspf::sind(yaw);
NEWMAT::Matrix T_yaw = rspfMatrix3x3::create( cos,-sin, 0.0,
sin, cos, 0.0,
0.0, 0.0, 1.0);
NEWMAT::Matrix attRotMat = T_yaw * theRollRotMat;
rspfLsrVector adjLsrImgRayDir (attRotMat*initLsrImgRay.direction().data(),
icrSpace);
rspfLsrPoint adjLsrImgRayOrg (theIntrackOffset,
theCrtrackOffset,
0.0, // no radial adjustment of position
icrSpace);
rspfLsrRay adjLsrImgRay (adjLsrImgRayOrg, adjLsrImgRayDir);
image_ray = rspfEcefRay(adjLsrImgRay);
#if 0
if (traceDebug() || debug_flag)
{
rspfNotify(rspfNotifyLevel_DEBUG) << "\t adjLsrImgRay="<<adjLsrImgRay<<endl;
rspfNotify(rspfNotifyLevel_DEBUG) << "\t image_ray="<<image_ray<<endl;
}
if (traceExec())
{
rspfNotify(rspfNotifyLevel_DEBUG) << "DEBUG rspfLandSatModel::imagingRay: Returning..." << std::endl;
}
#endif
}
