Commit c37afb9f authored by Timothy Stack's avatar Timothy Stack
Browse files

Merge in the COSINE_DEWARP branch.

parent 367d5ec0
......@@ -2,7 +2,7 @@
# Desc: Makefile for Mezzanine IPC library
# Author: Andrew Howard
# Date: 21 Mar 2001
# CVS: $Id: GNUmakefile.in,v 1.1 2004-12-21 23:25:01 stack Exp $
# CVS: $Id: GNUmakefile.in,v 1.2 2005-07-28 20:54:18 stack Exp $
###########################################################################
SRCDIR = @srcdir@
......@@ -21,13 +21,14 @@ VERSION=0.00-emulab
###########################################################################
LIB = libmezz.a
OBS = mezz.o
OBS = mezz.o geom.o blend_tris.o
INCLUDES = -I.
INCLUDES = .
CFLAGS = -g3 -Wall -I$(INCLUDES) -DVERSION=\"$(VERSION)\"
LFLAGS = -lm
CC = gcc
LINKER = gcc
MAKEDEP = makedepend
......@@ -35,7 +36,7 @@ MAKEDEP = makedepend
# Build section
###########################################################################
all: $(LIB)
all: $(LIB) mezzdump fauxmezz
include $(TESTBED_SRCDIR)/GNUmakerules
......@@ -49,6 +50,15 @@ dep:
clean:
rm -f $(LIB) *~ *.o
mezzdump: mezzdump.o
$(LINKER) $^ $(LFLAGS) -o $@
fauxmezz: fauxmezz.o
$(LINKER) $^ $(LFLAGS) -L. -lmezz -o $@
test_geom: test_geom.o geom.o
$(LINKER) $^ $(LFLAGS) -o $@
test_blend_tris: test_blend_tris.o blend_tris.o geom.o
$(LINKER) $^ $(LFLAGS) -o $@
###########################################################################
# Install
......@@ -57,6 +67,9 @@ clean:
install:
mkdir -p $(INSTALL_INCDIR)
mkdir -p $(INSTALL_LIBDIR)
mkdir -p $(INSTALL_BINDIR)
$(INSTALL) -m 755 mezzdump $(INSTALL_BINDIR)
$(INSTALL) -m 755 fauxmezz $(INSTALL_BINDIR)
$(INSTALL) -m 644 libmezz.a $(INSTALL_LIBDIR)
$(INSTALL) -m 644 $(SRCDIR)/mezz.h $(INSTALL_INCDIR)
......@@ -64,3 +77,6 @@ install:
# Dependancies (generated by 'make dep')
mezz.o: mezz.h
geom.o: geom.h
test_geom.o: geom.h
blend_tris.o: blend_tris.h
//
// EMULAB-COPYRIGHT
// Copyright (c) 2005 University of Utah and the Flux Group.
// All rights reserved.
//
#include <stdio.h>
#include "blend_tris.h"
// Initialize a BlendTri object. It's easier to deposit the verts, target, and error
// vecs into this space, than to pass and copy them, so we assume that has been done.
void initBlendTri(blendTri self)
{
int i;
for ( i = 0; i < 3; i++ )
{
// Construct edge lines from each vertex to its clockwise neighbor.
initGeomLine(self->verts[(i+1)%3], self->verts[(i+2)%3], &self->edges[i]);
// Perpendicular distances from each vertex to the opposing edge line.
self->dists[i] = signedDist(&self->edges[i], self->verts[i]);
}
}
// Debug print function.
void prBlendTri(blendTri self)
{
int i;
printf("BlendTri=(\n");
printf(" verts=(");
for ( i = 0; i < 3; i++ )
printf("(%g,%g)%s", self->verts[i][0], self->verts[i][1], i<2 ? ", " : "),\n");
printf(" target=(");
for ( i = 0; i < 3; i++ )
printf("(%g,%g)%s", self->target[i][0], self->target[i][1], i<2 ? ", " : "),\n");
printf(" error=(");
for ( i = 0; i < 3; i++ )
printf("(%g,%g)%s", self->error[i][0], self->error[i][1], i<2 ? ", " : "),\n");
printf(" edges=(\n");
for ( i = 0; i < 3; i++ )
{
printf(" ");
prGeomLine(&self->edges[i]);
}
printf(" ),\n");
printf(" dists=(%g,%g,%g)\n", self->dists[0], self->dists[1], self->dists[2]);
printf(")\n");
}
// baryCoords - Compute the three barycentric coordinates of a point
// relative to the triangle.
//
// They vary linearly from 1.0 at a vertex to 0.0 at the edge of the
// triangle opposite the vertex, and always sum to 1 everywhere.
//
// They're good both for finding out whether you are inside a triangle, and
// for linearly blending values across the triangle.
//
void baryCoords(blendTri self, geomPt pixPt, double bcs[3])
{
int i;
for ( i = 0; i < 3; i++ )
// Fraction of the distance all the way across.
bcs[i] = signedDist(&self->edges[i], pixPt) / self->dists[i];
}
// errorBlend - Blend the error vector components linearly over the triangle.
// bcs arg is baryCoords of an image point.
void errorBlend(blendTri self, double bcs[3], geomVec result)
{
int i;
result[0] = result[1] = 0.0; // Accumulate component vectors.
for ( i = 0; i < 3; i++ )
ptOffset(result, self->error[i], -bcs[i], result); // Negate error to cancel.
}
//
// EMULAB-COPYRIGHT
// Copyright (c) 2005 University of Utah and the Flux Group.
// All rights reserved.
//
#ifndef BLENDTRIS_H
#define BLENDTRIS_H
#include <stdio.h>
#include "geom.h"
// blendTri - Piecewise triangular linear blending using barycentric coordinates.
//
// Barycentric coordinate of vertex 0 / edge 0.
// v0 --------- 1.0 100% of v0 data at v0.
// /^ \ |
// / | \ |
// e1 e1 e2 | 0.5 50% of v0 data halfway down the triangle.
// / dist \ |
// / | \ |
// v2<--- e0 --- v1 --- 0.0 0% of v0 data on edge e0 from v1 to v2.
//
struct blendTriStruct // There are three of everything here.
{
// Image coordinates for the three vertices, in pixels, listed clockwise.
geomPt verts[3];
// Target world coordinates of vertices, in meters.
geomPt target[3];
// Error vectors in meters (difference from the target point to be corrected.)
geomVec error[3];
// Construct edge lines from each vertex to its clockwise neighbor.
geomLineObj edges[3];
// Perpendicular distances from each vertex to the opposing edge line.
double dists[3];
};
typedef struct blendTriStruct blendTriObj;
typedef blendTriObj *blendTri;
// Initialize a BlendTri object. It's easier to deposit the verts, target, and error
// vecs into this space, than to pass and copy them, so we assume that has been done.
void initBlendTri(blendTri self);
// Debug print function.
void prBlendTri(blendTri self);
// baryCoords - Compute the three barycentric coordinates of a point
// relative to the triangle.
//
// They vary linearly from 1.0 at a vertex to 0.0 at the edge of the
// triangle opposite the vertex, and always sum to 1 everywhere.
//
// They're good both for finding out whether you are inside a triangle, and
// for linearly blending values across the triangle.
//
void baryCoords(blendTri self, geomPt pixPt, double bcs[3]);
// errorBlend - Blend the error vector components linearly over the triangle.
// bcs arg is baryCoords of an image point.
void errorBlend(blendTri self, double bcs[3], geomVec result);
#endif
/*
* EMULAB-COPYRIGHT
* Copyright (c) 2005 University of Utah and the Flux Group.
* All rights reserved.
*/
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <fcntl.h>
#include <unistd.h>
#include "mezz.h"
static void usage(void)
{
fprintf(stderr,
"Usage: fauxmezz <ipcfile> <frame0> [frame1 frame2]\n"
"\n"
"Fake mezzanine that updates an IPC file with frame\n"
"data from a separate run of mezzanine. It is intended\n"
"for use with mezzcal, simply start this program with\n"
"the list of frames you wish to examine and then direct\n"
"mezzcal to this program's IPC file.\n");
}
int read_mmap(mezz_mmap_t *mm, char *file)
{
int fd, retval = 0;
if ((fd = open(file, O_RDONLY)) == -1) {
perror("open");
}
else if (read(fd, mm, sizeof(mezz_mmap_t)) != sizeof(mezz_mmap_t)) {
perror("short read");
}
if (fd != -1) {
close(fd);
}
memset(mm->pids, 0, sizeof(pid_t) * 32);
mm->calibrate = -1;
return retval;
}
int main(int argc, char *argv[])
{
int retval = EXIT_SUCCESS;
if (argc < 2) {
usage();
exit(0);
}
if (mezz_init(1, argv[1]) != 0) {
fprintf(stderr, "unable to initialize mezzanine");
}
else {
mezz_mmap_t *mm;
int lpc = 0;
argv += 2;
argc -= 2;
printf("Hit return to load the next frame\n");
mm = mezz_mmap();
read_mmap(mm, argv[lpc]);
while (!feof(stdin)) {
int c = getchar();
printf("reading %s\n", argv[lpc % argc]);
read_mmap(mm, argv[lpc % argc]);
printf("Time: %f Frame: %d\n", mm->time, mm->count);
mezz_raise_event();
lpc += 1;
}
mezz_term(1);
}
return retval;
}
/*
* EMULAB-COPYRIGHT
* Copyright (c) 2005 University of Utah and the Flux Group.
* All rights reserved.
*/
// geom.c - Some basic linear algebra of points, vectors, and lines.
#include <stdio.h>
#include "geom.h"
// vecFrom2Pts - Subtract two points for vec from the first to the second.
void vecFrom2Pts(geomPt p1, geomPt p2, geomVec result)
{
result[0] = p1[0] - p2[0];
result[1] = p1[1] - p2[1];
}
// vecLength - Length of a vector.
double vecLength(geomVec v)
{
return hypot(v[0], v[1]);
}
// vecNormalize - Returns a unit vector in the same direction.
// Result can be the same vector as the vector argument.
void vecNormalize(geomVec v, geomVec result)
{
double norm = vecLength(v);
result[0] = v[0]/norm;
result[1] = v[1]/norm;
}
// vecScale - Multiply the length.
// Result can be the same vector as the vector argument.
void vecScale(geomVec v, double scale, geomVec result)
{
result[0] = v[0] * scale;
result[1] = v[1] * scale;
}
// ptOffset - Offset a point by (a multiple of) a vector.
// Result can be the same point as the point argument.
void ptOffset(geomPt p, geomVec v, double scale, geomPt result)
{
result[0] = p[0] + v[0] * scale;
result[1] = p[1] + v[1] * scale;
}
// ptBlend - Blend between two points, 0.0 for first, 1.0 for the second.
// Result can be the same point as either of the two point arguments.b
void ptBlend(geomPt p1, geomPt p2, double t, geomPt result)
{
geomVec offset;
vecFrom2Pts(p1, p2, offset);
ptOffset(p1, offset, t, result);
}
// Construct a line through two points.
void initGeomLine(geomPt pt1, geomPt pt2, geomLine self)
{
geomVec perp;
geomCopy(pt1, self->pt1);
geomCopy(pt2, self->pt2);
// (A,B) is the vector perpendicular to right side of line, (deltaY, -deltaX).
perp[0] = pt2[1]-pt1[1];
perp[1] = pt1[0]-pt2[0];
vecNormalize(perp, perp);
self->A = perp[0];
self->B = perp[1];
// C is the negative of the distance of the line from the origin.
self->C = 0.0; // Small chicken-and-egg problem...
self->C = -signedDist(self,pt1);
}
// Debug print function.
void prGeomLine(geomLine self)
{
printf("Line=(pt1=(%g,%g), pt2=(%g,%g), A=%g, B=%g, C=%g)\n",
self->pt1[0], self->pt1[1], self->pt2[0], self->pt2[1],
self->A, self->B, self->C );
}
// signedDist - Signed distance, positive inside (right side) of the line.
double signedDist(geomLine self, geomPt pt)
{
return self->A*pt[0] + self->B*pt[1] + self->C; // [x y 1] . [A B C]
}
/*
* EMULAB-COPYRIGHT
* Copyright (c) 2005 University of Utah and the Flux Group.
* All rights reserved.
*/
// geom.h - Some basic linear algebra of points, vectors, and lines.
#ifndef GEOM_H
#define GEOM_H
#include <math.h>
#include <string.h>
// Restricted to 2D - Points and vectors are arrays of 2 doubles.
// No dimensionality hierarchy, no operand checking, no homogeneous coordinates.
// Arrays are passed by reference in C - turned to pointers to their first element.
typedef double geomVec[2];
typedef double geomPt[2];
#define geomCopy(src,dst) bcopy(src, dst, sizeof dst);
// vecFrom2Pts - Subtract two points for vec from the first to the second.
void vecFrom2Pts(geomPt p1, geomPt p2, geomVec result);
// vecLength - Length of a vector.
double vecLength(geomVec v);
// vecNormalize - Returns a unit vector in the same direction.
// Result can be the same vector as the vector argument.
void vecNormalize(geomVec v, geomVec result);
// vecScale - Multiply the length.
// Result can be the same vector as the vector argument.
void vecScale(geomVec v, double scale, geomVec result);
// ptOffset - Offset a point by (a multiple of) a vector.
// Result can be the same point as the point argument.
void ptOffset(geomPt p, geomVec v, double scale, geomPt result);
// ptBlend - Blend between two points, 0.0 for first, 1.0 for the second.
// Result can be the same point as either of the two point arguments.b
void ptBlend(geomPt p1, geomPt p2, double t, geomPt result);
// Line - Line equation is Ax+By+C, == 0 on the line, positive inside.
struct geomLineStruct
{
geomPt pt1, pt2;
double A, B, C;
};
typedef struct geomLineStruct geomLineObj;
typedef geomLineObj *geomLine;
// Construct a line through two points.
void initGeomLine(geomPt pt1, geomPt pt2, geomLine self);
// Debug print function.
void prGeomLine(geomLine self);
// signedDist - Signed distance, positive inside (right side) of the line.
double signedDist(geomLine self, geomPt pt);
#endif
/*
* EMULAB-COPYRIGHT
* Copyright (c) 2005 University of Utah and the Flux Group.
* All rights reserved.
*/
/*
* Mezzanine - an overhead visual object tracker.
* Copyright (C) Andrew Howard 2002
......@@ -27,7 +21,7 @@
* Desc: Mezzanine IPC wrapper
* Author: Andrew Howard
* Date: 28 Mar 2002
* CVS: $Id: mezz.c,v 1.4 2005-05-10 15:25:08 johnsond Exp $
* CVS: $Id: mezz.c,v 1.5 2005-07-28 20:54:18 stack Exp $
**************************************************************************/
#include <errno.h>
......
......@@ -21,7 +21,7 @@
* Desc: Mezzanine IPC interface
* Author: Andrew Howard
* Date: 28 Mar 2002
* CVS: $Id: mezz.h,v 1.2 2005-01-20 15:07:43 johnsond Exp $
* CVS: $Id: mezz.h,v 1.3 2005-07-28 20:54:18 stack Exp $
* Notes:
*
* This library sets up a shared, memory-mapped object for exchanging
......@@ -66,6 +66,16 @@ typedef struct
double wpos[MEZZ_MAX_DEWARP][2]; // The world coords of the calibration points.
double iwtrans[2][8]; // Parameters for the image-to-world transform.
double witrans[2][8]; // Parameters for the world-to-image transform.
double warpFactor;
double ocHeight; // Height of the optical center of the lens in meters
double scaleFactorX;
double scaleFactorY;
double ocX;
double ocY;
double gridX;
double gridY;
} mezz_dewarpdef_t;
......@@ -130,6 +140,9 @@ typedef struct
int max_missed; // Max frames we can miss before we look for a new match.
int missed; // Number of missed frames (object not seen)
double px, py, pa; // Object pose (world cs).
// additional info to make reporting blob pixel positions easier
mezz_blob_t ablob;
mezz_blob_t bblob;
} mezz_object_t;
/* // structure describing a "track" -- essentially, this just does a higher-level */
......
/*
* EMULAB-COPYRIGHT
* Copyright (c) 2005 University of Utah and the Flux Group.
* All rights reserved.
*/
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <fcntl.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/mman.h>
#include "mezz.h"
static void usage(void)
{
fprintf(stderr,
"Usage: mezzdump [-hbo] mezzfile\n"
"\n"
"Dump information about a mezzanine IPC file.\n"
"\n"
"Options:\n"
" -h\t\tPrint this message\n"
" -b\t\tPrint the blob list\n"
" -o\t\tPrint the object list\n");
}
int main(int argc, char *argv[])
{
int dump_blobs = 0, dump_objects = 0;
int c, fd, retval = EXIT_SUCCESS;
mezz_mmap_t *mm;
while ((c = getopt(argc, argv, "hbo")) != -1) {
switch (c) {
case 'b':
dump_blobs = 1;
break;
case 'o':
dump_objects = 1;
break;
case 'h':
case '?':
default:
usage();
exit(0);
break;
}
}
argv += optind;
argc -= optind;
if (argc == 0) {
fprintf(stderr, "error: missing mezzanine file argument\n");
usage();
exit(1);
}
if ((fd = open(argv[0], O_RDONLY)) == -1) {
perror("open");
}
else if ((mm = mmap(NULL,
sizeof(mezz_mmap_t),
PROT_READ,
MAP_SHARED,
fd,
0)) == NULL) {
perror("mmap");
}
else {
printf("Time: %f\n"
"Frame count: %d\n"
"Calibrate: %d\n"
"Save: %d\n"
"Dimensions: %dx%d\n"
"Pixel area: %d\n"
"Depth: %d\n",
mm->time,
mm->count,
mm->calibrate,
mm->save,
mm->width, mm->height,
mm->area,
mm->depth);
if (dump_blobs) {
int lpc;
for (lpc = 0; lpc < mm->bloblist.count; lpc++) {
mezz_blob_t *mb = &mm->bloblist.blobs[lpc];
printf("Blob[%d]:\n"
" Class: %d\n"
" Dimensions: %d,%d x %d,%d\n"
" Pixel area: %d\n"
" Image coords: %.2f %.2f\n"
" World coords: %.2f %.2f\n"
" Object: %d\n",
lpc,
mb->class,
mb->min_x, mb->min_y, mb->max_x, mb->max_y,
mb->area,