/*
* This routine has the enemy ship turn its strongest shield towards
* the enemy and then accelerate to 2/3 maximum speed. (Always
* returns 1)
*/
int
e_runaway(struct ship *sp, struct ship *fed)
{
double bear;
int strong;
double strength;
double temp;
int sign = 1;
float course = 0.0;
int i;
bear = bearing(sp->x, fed->x, sp->y, fed->y);
/*
* Find the strongest shield
*/
strong = 0;
strength = 0.;
for (i=0; i< SHIELDS; i++) {
temp = sp->shields[i].eff * sp->shields[i].drain *
(i == 0 ? SHIELD1 : 1.);
if (temp > strength) {
strong = i;
strength = temp;
}
}
switch (strong) {
case 0: course = bear;
sign = -1;
break;
case 1: course = rectify(bear - 90);
sign = 1;
break;
case 2: course = rectify(bear + 180);
sign = 1;
break;
case 3: course = rectify(bear + 90);
sign = 1;
break;
}
sp->target = NULL;
sp->newcourse = course;
sp->newwarp = 2 / 3 * sp->max_speed * sign;
if (sp->newwarp > 1.0 && is_dead(sp, S_WARP))
sp->newwarp = 0.99;
if (cansee(sp) && syswork(fed, S_SENSOR))
printf("%s: The %s is retreating.\n", helmsman, sp->name);
#ifdef TRACE
if (trace) {
printf("*** Runaway: Newcourse = %.2f\n", sp->newcourse);
printf("*** Runaway: Newwarp = %.2f\n", sp->newwarp);
}
#endif
return 1;
}
// Split a given rectangle R into two subsets R1 and R2 s.t. R=union(R1,R2)
void split(const Rset& R, Rset& R1, Rset& R2) {
R1 = R;
R2 = R;
int ms = maxside(R);
short sloc = (short)floor((float)(R1.lo[ms]+R1.hi[ms])/2); // split location
R1.hi[ms] = sloc;
R2.lo[ms] = sloc + 1;
rectify(R1);
rectify(R2);
}
/*
* This routine will turn the ship, slowing down if necessary to
* facilitate the turn. (Always returns 1)
*/
int
e_pursue(struct ship *sp, struct ship *fed, float speed)
{
float bear;
float coursediff;
bear = bearing(sp->x, fed->x, sp->y, fed->y);
/*
* do a quick turn if our speed is > max_warp - 2 and
* (thus) we are never going to bear on the fed ship
* speed = max_warp / 2 is a magic cookie. Feel free to change.
*/
coursediff = rectify(sp->course - bear);
if (coursediff > 180.0)
coursediff -= 360.0;
if (speed >= sp->max_speed - 2 && fabs(coursediff) > 10)
speed = (int)(sp->max_speed / 2);
sp->target = fed;
sp->newcourse = bear;
sp->newwarp = speed;
if (speed > 1 && is_dead(sp, S_WARP))
sp->newwarp = 0.99;
#ifdef TRACE
if (trace) {
printf("*** Pursue: Newcourse = %.2f\n", sp->newcourse);
printf("*** Pursue: Newwarp = %.2f\n", sp->newwarp);
}
#endif
return 1;
}
/* Debugging only. Check the sanity of rectify and distortion methods
*/
void Intrinsic::distorsionChecker()
{
// print
print();
// check clipping algorithm
Edge2D edge = Edge2D(Vec2d(-200,-100),Vec2d(10,20));
Edge2D edge_clipped = clip(edge);
LOG(LEVEL_DEBUG,"(%f,%f) => (%f,%f)",edge._a[0],edge._a[1],edge_clipped._a[0],edge_clipped._a[1]);
LOG(LEVEL_DEBUG,"(%f,%f) => (%f,%f)",edge._b[0],edge._b[1],edge_clipped._b[0],edge_clipped._b[1]);
//pick a bunch of 2D points and check their rectified value
Vec2d point = Vec2d(2.500000000000000e+001 , 6.950000000000000e+001);
Vec2d point_rect = rectify(normalize(point));
LOG(LEVEL_DEBUG,"(%f,%f) ==> (%f,%f) should be (%f,%f)", point[0],point[1],point_rect[0],point_rect[1],
-1.271644101537981e+000 , -6.149558565018068e-001);
point = Vec2d(2.560000000000000e+002 , 1.950000000000000e+001);
point_rect = rectify(normalize(point));
LOG(LEVEL_DEBUG,"(%f,%f) ==> (%f,%f) should be (%f,%f)", point[0],point[1],point_rect[0],point_rect[1],
-1.559022165084900e-002 , -5.935958630008766e-001);
point = Vec2d(2.560000000000000e+002 , 6.650000000000000e+001);
point_rect = rectify(normalize(point));
LOG(LEVEL_DEBUG,"(%f,%f) ==> (%f,%f) should be (%f,%f)", point[0],point[1],point_rect[0],point_rect[1],
-1.308190696800208e-002 , -3.973240271620430e-001);
//pick a bunch of points
// perform successive distortion and rectification and check the discrepancy
int n_points = 10;
for (int i=0;i<n_points;i++) {
Vec2d point = Vec2d ((double)rand()/RAND_MAX,(double)rand()/RAND_MAX);
Vec2d point_dist = distort(point);
Vec2d point_dist_2 = distort(rectify(point_dist));
LOG(LEVEL_DEBUG,"(%f,%f) => (%f,%f) d = %f\n",point_dist[0],point_dist[1],point_dist_2[0],point_dist_2[1],len(point_dist-point_dist_2));
}
}
int lab2rgb(double res)
{
double p = (res + 16) / 116;
double ppp = p * p * p;
double x = 0.9513 * ppp;
double y = ppp;
double z = 1.0886 * ppp;
double r = 3.240479f*x + -1.537150f*y + -0.498535f*z;
return rectify(r * 255);
}
/* Distort a 2D image point
*/
Vec2d Intrinsic::distort(Vec2d xn)
{
double x = xn[0], y = xn[1];
Vec2d xd;
if (dist_model == POLYNOMIAL_DISTORTION) {
double r2 = x*x + y*y;
double c = 1+getKC0()*r2 + getKC1() *r2*r2 + getKC4()*r2*r2*r2;
Vec2d dx = Vec2d(2*getKC2()*x*y+getKC3()*(r2+2*x*x),getKC2()*(r2+2*y*y)+2*getKC3()*x*y);
xd = c*xn + dx;
} else if (dist_model == SPHERICAL_DISTORTION) {
Vec2d xp = xn - cc; ///
double rnew2 = sqrlen(xp);
double rnew = len(xp);
double r_1 = rnew / (value*value*sqrt(fabs(1 + rnew2)));
double r_2 = rnew / (value*value*sqrt(fabs(rnew2 - 1.0)));
Vec2d xu_1 = xp * r_1/rnew + cc; //
Vec2d xu_2 = xp * r_2/rnew + cc; //
Vec2d xc_1 = rectify(xu_1);
if (len(xc_1-xn) < 0.0001) {
xd = xu_1;
}
else {
Vec2d xc_2 = rectify(xu_2);
if (len (xc_2 - xn) < 0.00001)
xd = xu_2;
else {
xd = xu_1;
}
}
}
return xd;
}
CVStereo::CVStereo(
const CImg<float>& left,
const CImg<float>& right,
bool prerectivied) {
convertCImgToMat(left, original[0]);
convertCImgToMat(right, original[1]);
if (prerectivied) {
processPrerectified();
} else {
rectify();
}
}
/*
* Advance to the rear! (Always returns 1)
*/
int
e_evade(struct ship *sp, int x, int y, int type)
{
float newcourse = 0.0;
float bear;
bear = bearing(sp->x, x, sp->y, y);
if (cansee(sp) && syswork(shiplist[0], S_SENSOR))
printf("%s taking evasive action!\n", sp->name);
switch (randm(3)) {
case 1:
newcourse = rectify(bear - 90.0);
break;
case 2:
newcourse = rectify(bear + 90.0);
break;
case 3:
newcourse = rectify(bear + 180.0);
break;
default:
printf("error in evade()\n");
break;
}
sp->target = NULL;
sp->newcourse = newcourse;
sp->newwarp = 2 + randm((int)(sp->max_speed - 3));
if (is_dead(sp, S_WARP))
sp->newwarp = 1.0;
#ifdef TRACE
if (trace) {
printf("*** Evade: Newcourse = %3.0f\n", newcourse);
printf("*** Evade: Newwarp = %.2f\n", sp->newwarp);
}
#endif
type = type; /* LINT */
return 1;
}
int main(int argc, char **argv)
{
const char *exe = argv[0];
double t0;
double t1;
const char *p = NULL;
const char *m = NULL;
const char *o = NULL;
const char *t = NULL;
int n = 512;
int d = 0;
int b = -1;
int g = -1;
int A = 0;
int h = 0;
int l = 0;
int T = 0;
double E[4] = { 0.f, 0.f, 0.f , 0.f};
double L[3] = { 0.f, 0.f, 0.f };
double P[3] = { 0.f, 0.f, 0.f };
float N[2] = { 0.f, 0.f };
float R[2] = { 0.f, 1.f };
int c;
int r = 0;
t0 = now();
setexe(exe);
opterr = 0;
while ((c = getopt(argc, argv, "Ab:d:E:g:hL:l:m:n:N:o:p:P:Tt:R:w:")) != -1)
switch (c)
{
case 'A': A = 1; break;
case 'h': h = 1; break;
case 'T': T = 1; break;
case 'p': p = optarg; break;
case 'm': m = optarg; break;
case 'o': o = optarg; break;
case 't': t = optarg; break;
case 'n': sscanf(optarg, "%d", &n); break;
case 'd': sscanf(optarg, "%d", &d); break;
case 'b': sscanf(optarg, "%d", &b); break;
case 'g': sscanf(optarg, "%d", &g); break;
case 'l': sscanf(optarg, "%d", &l); break;
case 'E':
sscanf(optarg, "%lf,%lf,%lf,%lf", E + 0, E + 1, E + 2, E + 3);
break;
case 'L':
sscanf(optarg, "%lf,%lf,%lf", L + 0, L + 1, L + 2);
break;
case 'P':
sscanf(optarg, "%lf,%lf,%lf", P + 0, P + 1, P + 2);
break;
case 'N':
sscanf(optarg, "%f,%f", N + 0, N + 1);
break;
case 'R':
sscanf(optarg, "%f,%f", R + 0, R + 1);
break;
case '?': apperr("Bad option -%c", optopt); break;
}
argc -= optind;
argv += optind;
if (p == NULL || h)
apperr("\nUsage: %s [options] input [...]\n"
"\t\t-p process . . Select process\n"
"\t\t-o output . . Output file\n"
"\t\t-T . . . . . . Emit timing information\n\n"
"\t%s -p extrema\n\n"
"\t%s -p convert [options]\n"
"\t\t-n n . . . . . Page size\n"
"\t\t-d d . . . . . Tree depth\n"
"\t\t-b b . . . . . Channel depth override\n"
"\t\t-g g . . . . . Channel sign override\n"
"\t\t-E w,e,s,n . . Equirectangular range\n"
"\t\t-L c,d0,d1 . . Longitude blend range\n"
"\t\t-P c,d0,d1 . . Latitude blend range\n"
"\t\t-N n0,n1 . . . Normalization range\n"
"\t\t-A . . . . . . Coverage alpha\n\n"
"\t%s -p combine [-m mode]\n"
"\t\t-m sum . . . . Combine by sum\n"
"\t\t-m max . . . . Combine by maximum\n"
"\t\t-m avg . . . . Combine by average\n"
"\t\t-m blend . . . Combine by alpha blending\n\n"
"\t%s -p mipmap [-m mode]\n\n"
"\t\t-m sum . . . . Combine by sum\n"
"\t\t-m max . . . . Combine by maximum\n"
"\t\t-m avg . . . . Combine by average\n\n"
"\t%s -p border\n\n"
"\t%s -p finish [options]\n"
"\t\t-t text . . . Image description text file\n"
"\t\t-l l . . . . . Bounding volume oversample level\n\n"
"\t%s -p normal [options]\n"
"\t\t-R r0,r1 . . . Radius range\n",
exe, exe, exe, exe, exe, exe, exe, exe);
else if (strcmp(p, "extrema") == 0)
r = extrema(argc, argv);
else if (strcmp(p, "convert") == 0)
r = convert(argc, argv, o, n, d, b, g, A, N, E, L, P);
else if (strcmp(p, "rectify") == 0)
r = rectify(argc, argv, o, n, N, E, L, P);
else if (strcmp(p, "combine") == 0)
r = combine(argc, argv, o, m);
else if (strcmp(p, "mipmap") == 0)
r = mipmap (argc, argv, o, m, A);
else if (strcmp(p, "border") == 0)
r = border (argc, argv, o);
else if (strcmp(p, "finish") == 0)
r = finish (argc, argv, t, l);
else if (strcmp(p, "polish") == 0)
r = polish (argc, argv);
else if (strcmp(p, "normal") == 0)
r = normal (argc, argv, o, R);
else if (strcmp(p, "sample") == 0)
r = sample (argc, argv, R, d);
else apperr("Unknown process '%s'", p);
t1 = now();
if (T) printhms(t1 - t0);
return r;
}
/* Rectify a 2D image edge
*/
Edge2D Intrinsic::rectify ( Edge2D &edge )
{
return Edge2D( rectify( edge._a), rectify( edge._b ) );
}
void RectificationThread::run(){
/*
* rectify the two images
*/
unsigned char pixel_value;
FILE *fp_out; // encoder values log file for testing ... this code is not executed during normal operation
if (debug) {
printf("rectificationThread: parameters are\n%4.1f\n%4.1f\n%4.1f\n%4.1f\n%4.1f\n%4.1f\n%4.1f\n%4.1f\n\n",
*fxLeft,*fyLeft,*cxLeft,*cyLeft,*fxRight,*fyRight,*cxRight,*cyRight);
}
/* log encoder values during tests ... this code is not executed during normal operation */
if (log) {
if ((fp_out = fopen("rectification.log","w")) == 0) {
printf("rectificationThread: can't open output rectification.log\n");
}
}
while (isStopping() != true) { // the thread continues to run until isStopping() returns true
/*
* Step 1: determine the the camera angles which cause the epipolar distortion to be rectified
* ===========================================================================================
*
* version is the average camera azimuth angle: vs ~ (L+R)/2
* vergence is the relative camera azimuth angle: vg = L-R
*
* hence:
*
* L = vs + vg/2
* R = vs - vg/2
*
* where L and R are the angles specifying the rotation of the camera about the camera Y axis
* i.e. the absolute camera azimuth angle.
*
* See http://wiki.icub.org/wiki/Vergence%2C_Version_and_Disparity
*
* However, we wish to rectify relative to, not the absolute camera azimuth angle,
* but relative to the gaze angle given by the version angle.
* Thus, the angles we use are L'and R', viz
*
* L' = L - vs = +vg/2
* R' = R - vs = -vg/2
*
*/
do {
encoderPositions = robotPort->read(true);
} while ((encoderPositions == NULL) && (isStopping() != true)); // exit loop if shutting down
if (isStopping()) break; // abort this loop to avoid make sure we don't continue and possibly use NULL images
vergence = (float) encoderPositions->data()[5]; // get the vergence angle
if (debug) {
cout << "rectificationThread: vergence angle is " << vergence << endl;
}
if (log) {
if (fp_out != NULL) fprintf(fp_out,"Vergence angle is %f\n",vergence);
}
leftCameraAngle = vergence / 2;
rightCameraAngle = -vergence / 2;
/*
* Step 2: grab left and right images and copy images to local format
* ==================================================================
*/
if (debug) cout << "rectificationThread: grabbing images " << endl;
do {
leftImage = leftImagePortIn->read(true);
} while ((leftImage == NULL) && (isStopping() != true)); // exit loop if shutting down
do {
rightImage = rightImagePortIn->read(true);
} while ((rightImage == NULL) && (isStopping() != true)); // exit loop if shutting down
if (isStopping()) break; // abort this loop to avoid make sure we don't continue and possibly use NULL images
for (x=0; x<width; x++) {
for (y=0; y<height; y++) {
rgbPixel = leftImage->safePixel(x,y);
leftInput->put_pixel(x, y, rgbPixel.r, 0);
leftInput->put_pixel(x, y, rgbPixel.g, 1);
leftInput->put_pixel(x, y, rgbPixel.b, 2);
}
}
for (x=0; x<width; x++) {
for (y=0; y<height; y++) {
rgbPixel = rightImage->safePixel(x,y);
rightInput->put_pixel(x, y, rgbPixel.r, 0);
rightInput->put_pixel(x, y, rgbPixel.g, 1);
rightInput->put_pixel(x, y, rgbPixel.b, 2);
}
}
/*
* Step 3: rectify left and right images
* =====================================
*/
if (debug) cout << "rectificationThread: performing rectification " << endl;
rectify(leftInput, rightInput,
*fxLeft, *fyLeft, *cxLeft, *cyLeft, leftCameraAngle,
*fxRight,*fyRight,*cxRight,*cyRight,rightCameraAngle,
leftRectified, rightRectified);
/*
* Step 4: copy images back to YARP format and write them out
* ==========================================================
*/
if (debug) cout << "rectificationThread: sending images " << endl;
ImageOf<PixelRgb> &rectifiedLeftImage = leftImagePortOut->prepare();
ImageOf<PixelRgb> &rectifiedRightImage = rightImagePortOut->prepare();
rectifiedLeftImage.resize(width,height);
rectifiedRightImage.resize(width,height);
for (x=0; x < width; x++) {
for (y=0; y < height; y++) {
leftRectified->get_pixel(x, y, &pixel_value, 0); rgbPixel.r=pixel_value;
leftRectified->get_pixel(x, y, &pixel_value, 1); rgbPixel.g=pixel_value;
leftRectified->get_pixel(x, y, &pixel_value, 2); rgbPixel.b=pixel_value;
rectifiedLeftImage(x,y) = rgbPixel;
rightRectified->get_pixel(x, y, &pixel_value, 0); rgbPixel.r=pixel_value;
rightRectified->get_pixel(x, y, &pixel_value, 1); rgbPixel.g=pixel_value;
rightRectified->get_pixel(x, y, &pixel_value, 2); rgbPixel.b=pixel_value;
rectifiedRightImage(x,y) = rgbPixel;
}
}
leftImagePortOut->write();
rightImagePortOut->write();
}
if (log) {
if (fp_out != NULL) fclose(fp_out);
}
}
// convert bumblebee images to point cloud
int main( int ac, char** av )
{
if( ac != 3 )
{
std::cerr << "usage: " << av[0] << " <left> <right> " << std::endl;
return 1;
}
cv::Mat left = cv::imread( av[1], 0 );
cv::Mat right = cv::imread( av[2], 0 );
Eigen::Matrix3d leftCamera;
// leftCamera << 1632, 0, 631,
// 0, 1630.9, 474.2,
// 0, 0, 1;
leftCamera << 5.3471311032432391e+02, 0., 3.3513838135674735e+02, 0.,
5.3471311032432391e+02, 2.4020578137651341e+02, 0., 0., 1;
Eigen::Matrix< double, 5, 1 > leftDistortion;
// leftDistortion << -0.43938, 0.21826, -0.00001, 0.00076, 0;
leftDistortion << -2.7456815913629645e-01, -1.8329019064962277e-02, 0., 0., 0.;
Eigen::Matrix3d rightCamera;
// rightCamera << 1635.7, 0, 651.9,
// 0, 1633.9, 463.4,
// 0, 0, 1;
rightCamera << 5.3471311032432391e+02, 0., 3.3401518911545526e+02, 0.,
5.3471311032432391e+02, 2.4159041667844363e+02, 0., 0., 1.;
Eigen::Matrix< double, 5, 1 > rightDistortion;
rightDistortion << -2.8073450162365271e-01, 9.3000165783151290e-02, 0., 0., 0.;
// rightDistortion << -0.44416, 0.23526, 0.00127, -0.00017, 0;
// Eigen::Vector3d leftPosition( 0.2229,-0.1283,-0.772 );
// Eigen::Vector3d rightPosition( 0.2421,0.1018,-0.8288 );
// Eigen::Vector3d leftAngle( 1.4621,0.0074,1.5435 );
// Eigen::Vector3d rightAngle( 1.4262,0.0131,1.5456 );
// Eigen::Vector3d angleDiff = rightAngle - leftAngle;
// snark::rotation_matrix leftRotation( leftAngle );
// snark::rotation_matrix rot( angleDiff );
// Eigen::Matrix3d rotation = rot.rotation();
// Eigen::Vector3d translation = leftRotation.rotation().transpose() * ( rightPosition - leftPosition );
Eigen::Matrix3d rotation = Eigen::Matrix3d::Identity();
rotation << 9.9975845371004723e-01, 5.2938494283307751e-03,
-2.1330949194199030e-02, -4.9128856780201336e-03,
9.9982820089904900e-01, 1.7872667436219597e-02,
2.1421899766595000e-02, -1.7763553844914078e-02,
9.9961270418356973e-01 ;
Eigen::Vector3d translation( 0.24005, 0, 0 );
translation << -3.3385325916025859e+00, 4.8752483611573305e-02,
-1.0621381929002180e-01;
snark::imaging::rectify_map rectify( leftCamera, leftDistortion, rightCamera, rightDistortion, left.cols, left.rows, rotation, translation );
cv::Mat leftRectified = rectify.remap_left( left );
cv::Mat rightRectified = rectify.remap_right( right );
cv::imshow( "left", leftRectified );
cv::imshow( "right", rightRectified );
cv::imwrite( "left-rectified.png", leftRectified );
cv::imwrite( "right-rectified.png", rightRectified );
snark::imaging::point_cloud cloud( left.channels() );
cv::Mat points = cloud.get( rectify.Q(), leftRectified, rightRectified );
for( int i = 0; i < points.rows; i++ )
{
for( int j = 0; j < points.cols; j++ )
{
cv::Point3f point = points.at< cv::Point3f >( i, j );
if( point.z < 100 )
{
cv::Vec3b color = leftRectified.at< cv::Vec3b >( i, j );
std::cout << point.x << "," << point.y << "," << point.z << "," << (unsigned int)color[0] << "," << (unsigned int)color[1] << "," << (unsigned int)color[2] << std::endl;
}
}
}
cv::Mat disparity = cloud.disparity();
std::cerr << " Q " << std::endl << rectify.Q() << std::endl;
cv::Mat disparity8;
unsigned int numberOfDisparities = 80;
numberOfDisparities = ((left.cols/8) + 15) & -16;
disparity.convertTo( disparity8, CV_8U, 255 / ( numberOfDisparities *16.0 ) );
cv::imshow( "disparity", disparity8 );
cv::imwrite( "disparity.png", disparity8 );
cv::waitKey();
return 0;
}
void StereoCapturer::computeDisparity(cv::Mat &output)
{
rectify(this->imgLeftr,this->imgRightr);
bm(this->imgLeftr, this->imgRightr, output,CV_32F);
}
int main(int argc, char **argv)
{
char title[80];
char buf[80], *p;
struct Ortho_Image_Group group;
struct GModule *module;
struct Option *group_opt;
/* must run in a term window */
G_putenv("GRASS_UI_TERM", "1");
/* initialize grass */
G_gisinit(argv[0]);
module = G_define_module();
module->keywords = _("imagery, orthorectify");
module->description = _("Menu driver for the photo imagery programs.");
group_opt = G_define_standard_option(G_OPT_I_GROUP);
group_opt->description =
_("Name of imagery group for ortho-rectification");
if (G_parser(argc, argv))
exit(EXIT_FAILURE);
strncpy(group.name, group_opt->answer, 99);
group.name[99] = '\0';
/* strip off mapset if it's there: I_() fns only work with current mapset */
if ((p = strchr(group.name, '@')))
*p = 0;
/* get and check the group reference files */
if (!I_get_group_ref(group.name, &group.group_ref)) {
G_warning(_("Pre-selected group <%s> not found"), group.name);
/* clean the wrong name in GROUPFILE */
I_put_group("");
/* ask for new group name */
if (!I_ask_group_old(
_("Enter imagery group for ortho-rectification"), group.name))
exit(EXIT_SUCCESS);
I_get_group_ref(group.name, &group.group_ref);
}
if (group.group_ref.nfiles <= 0)
G_fatal_error(_("Group [%s] contains no files"), group.name);
I_put_group(group.name);
while (1) {
if (!I_get_group(group.name)) {
exit(EXIT_SUCCESS);
}
/* print the screen full of options */
sprintf(title, "i.ortho.photo -- \tImagery Group = %s ", group.name);
G_clear_screen();
fprintf(stderr, "%s\n\n", title);
fprintf(stderr, "Initialization Options:\n");
fprintf(stderr, "\n");
fprintf(stderr, " 1. Select/Modify imagery group\n");
fprintf(stderr, " 2. Select/Modify imagery group target\n");
fprintf(stderr, " 3. Select/Modify target elevation model\n");
fprintf(stderr, " 4. Select/Modify imagery group camera\n");
fprintf(stderr, "\n");
fprintf(stderr, "Transformation Parameter Computations:\n");
fprintf(stderr, "\n");
fprintf(stderr, " 5. Compute image-to-photo transformation\n");
fprintf(stderr, " 6. Initialize exposure station parameters\n");
fprintf(stderr, " 7. Compute ortho-rectification parameters\n");
fprintf(stderr, "\n");
fprintf(stderr, "Ortho-rectification Option:\n");
fprintf(stderr, "\n");
fprintf(stderr, " 8. Ortho-rectify imagery files\n");
fprintf(stderr, "\n");
fprintf(stderr, "RETURN exit\n");
fprintf(stderr, "\n> ");
/* Get the option */
if (!G_gets(buf))
continue;
if (*buf == 0) /* exit */
exit(EXIT_SUCCESS);
/* run the program chosen */
G_strip(buf);
fprintf(stderr, "<%s>\n", buf);
if (strcmp(buf, "1") == 0)
run_system("i.group");
if (strcmp(buf, "2") == 0)
run_etc_imagery("i.photo.target", group.name);
if (strcmp(buf, "3") == 0)
run_etc_imagery("i.photo.elev", group.name);
if (strcmp(buf, "4") == 0)
run_etc_imagery("i.photo.camera", group.name);
if (strcmp(buf, "5") == 0)
run_etc_imagery("i.photo.2image", group.name);
if (strcmp(buf, "6") == 0)
run_etc_imagery("i.photo.init", group.name);
if (strcmp(buf, "7") == 0)
run_etc_imagery("i.photo.2target", group.name);
if (strcmp(buf, "8") == 0) {
rectify(group.name);
/*
sprintf(buf, "i.photo.rectify group=%s", group.name);
run_system(buf);
*/
}
}
}
void FloatArray::rectify(){//in place
/// @note When built for ARM Cortex-M processor series, this method uses the optimized <a href="http://www.keil.com/pack/doc/CMSIS/General/html/index.html">CMSIS library</a>
rectify(*this);
}
void ImageRectificator::rectifyPointOfInterest()
{
rectify(QSizeF(81.9f, 61.3f), 640, true); //brahma
//rectify(QSizeF(18.f, 25.5f), 50, true); //book.
}
void ImageRectificator::rectifyAll()
{
rectify(QSizeF(81.9f, 61.3f), 640, false); //brahma
//rectify(QSizeF(18.f, 25.5f), 50, false); //book.
}
int exec_rectify(char *extension, char *interp_method, char *angle_map)
{
char *name;
char *mapset;
char *result;
char *type = "raster";
int n;
struct Colors colr;
struct Categories cats;
struct History hist;
int colr_ok, cats_ok;
long start_time, rectify_time;
double aver_z;
int elevfd;
struct cache *ebuffer;
G_debug(1, "Open elevation raster: ");
/* open elevation raster */
select_target_env();
G_set_window(&target_window);
G_debug(1, "target window: rs=%d cs=%d n=%f s=%f w=%f e=%f\n",
target_window.rows, target_window.cols, target_window.north,
target_window.south, target_window.west, target_window.east);
elevfd = Rast_open_old(elev_name, elev_mapset);
if (elevfd < 0) {
G_fatal_error(_("Could not open elevation raster"));
return 1;
}
ebuffer = readcell(elevfd, seg_mb_elev, 1);
select_target_env();
Rast_close(elevfd);
/* get an average elevation of the control points */
/* this is used only if target cells have no elevation */
get_aver_elev(&group.control_points, &aver_z);
G_message("-----------------------------------------------");
/* rectify each file */
for (n = 0; n < group.group_ref.nfiles; n++) {
if (!ref_list[n])
continue;
name = group.group_ref.file[n].name;
mapset = group.group_ref.file[n].mapset;
result =
G_malloc(strlen(group.group_ref.file[n].name) + strlen(extension) + 1);
strcpy(result, group.group_ref.file[n].name);
strcat(result, extension);
G_debug(2, "ORTHO RECTIFYING:");
G_debug(2, "NAME %s", name);
G_debug(2, "MAPSET %s", mapset);
G_debug(2, "RESULT %s", result);
G_debug(2, "select_current_env...");
select_current_env();
cats_ok = Rast_read_cats(name, mapset, &cats) >= 0;
colr_ok = Rast_read_colors(name, mapset, &colr) > 0;
/* Initialze History */
if (Rast_read_history(name, mapset, &hist) < 0)
Rast_short_history(result, type, &hist);
G_debug(2, "reading was fine...");
time(&start_time);
G_debug(2, "Starting the rectification...");
if (rectify(name, mapset, ebuffer, aver_z, result, interp_method)) {
G_debug(2, "Done. Writing results...");
select_target_env();
if (cats_ok) {
Rast_write_cats(result, &cats);
Rast_free_cats(&cats);
}
if (colr_ok) {
Rast_write_colors(result, G_mapset(), &colr);
Rast_free_colors(&colr);
}
/* Write out History */
Rast_command_history(&hist);
Rast_write_history(result, &hist);
select_current_env();
time(&rectify_time);
report(rectify_time - start_time, 1);
}
else
report((long)0, 0);
G_free(result);
}
close(ebuffer->fd);
release_cache(ebuffer);
if (angle_map) {
camera_angle(angle_map);
}
return 0;
}