/// computes the parameters for each pixel cost update during scanline optimization
void scanlineOptimizationParameters(int x1, int x2, int y1, int y2, int d,
float& P1, float& P2,
const PARAMETERS& p)
{
int a=p.activePicture, b=1-a;
if(a!=0) d=-d;
// compute color differences between pixels in the two pictures
int d1 = colorDiff(p.im[a].pixel(x1,y1), p.im[a].pixel(x2,y2), p.wh),
d2 = p.tauSO + 1;
if(0<=x1+d && x1+d<p.w && 0<=x2+d && x2+d<p.w)
d2 = colorDiff(p.im[b].pixel(x1+d,y1), p.im[b].pixel(x2+d,y2), p.wh);
// depending on the color differences computed previously, find the so parameters
if(d1 < p.tauSO){
if(d2 < p.tauSO){
P1 = p.pi1;
P2 = p.pi2;
}
else{
P1 = float(p.pi1/4.0);
P2 = float(p.pi2/4.0);
}
}
else {
if(d2 < p.tauSO){
P1 = float(p.pi1/4.0);
P2 = float(p.pi2/4.0);
}
else {
P1 = float(p.pi1/10.0);
P2 = float(p.pi2/10.0);
}
}
}
/*
* Note that this implementation of nearestColor works "geometrically", i.e.
* it just computes the best match for equal (index) distance color cells, not the
* absolute best value (regardless of cell distance). We therefore shouldn't use
* its result to set cells in m->pix while other empty cells are still waiting to
* be served.
*/
unsigned char
nearestColor ( Rgb2Pseudo* m, int pi, int pj, int pk, int dist )
{
int i, j, k, i0, i1, j0, j1, k0, k1;
int pix, idx = -1;
double drgb, d = 1e6;
if ( (i0 = pi-dist) < 0 ) i0 = 0;
if ( (i1 = pi+dist) > 7 ) i1 = 7;
if ( (j0 = pj-dist) < 0 ) j0 = 0;
if ( (j1 = pj+dist) > 7 ) j1 = 7;
if ( (k0 = pk-dist) < 0 ) k0 = 0;
if ( (k1 = pk+dist) > 7 ) k1 = 7;
for ( i=i0; i <= i1; i++ ) {
for ( j=j0; j <= j1; j++ ) {
for ( k=k0; k <= k1; k++ ) {
if ( (pix = m->pix[i][j][k]) ) {
drgb = colorDiff( m->rgb[pix].r, m->rgb[pix].g, m->rgb[pix].b,
rgb8[pi], rgb8[pj], rgb8[pk]);
if ( drgb < d ){
d = drgb;
idx = pix;
}
}
}
}
}
if ( idx >= 0 )
return idx;
else {
if ( (i0 == 0) && (i1 == 7) ) /* should never happen, backup */
return 0;
else
return nearestColor( m, pi, pj, pk, dist+1);
}
}
// ######################################################################
Image<PixRGB<byte> > SuperPixelRoadSegmenter::getSuperPixel(Image<PixRGB<byte> > img)
{
if(!img.initialized()) return Image<PixRGB<byte> >(320,240,ZEROS);
// default parameters for the Superpixel segmentation
float sigma = .5;
uint k = 400;
uint minSize = 100;
int num_ccs;
std::vector<std::vector<Point2D<int> > > groups;
Image<int> groupImage =
SuperPixelSegment(img,sigma, k, minSize, num_ccs, &groups);
Image<PixRGB<byte> > sp_img = SuperPixelDebugImage(groups,img);
Image<int> sp_size_img = SuperPixelRegionSizeImage(groups,groupImage);
itsRawSuperPixelImg = sp_img;
//debugWin(itsRawSuperPixelImg,"itsRawSuperPixelImg");
int w = sp_img.getWidth();
int h = sp_img.getHeight();
// Look for road color,
// let's assume it always show up in the middle bottom (h-5,w/2 +- 10)
std::vector<PixRGB<byte> > color_map;
std::vector<int> color_size_map;
std::vector<int> color_map_count;
// Pick all road pixel candidates
int windowL = -SEARCH_WINDOW_W/2; //-10
int windowR = SEARCH_WINDOW_W/2; // 10
int windowB = SEARCH_WINDOW_BOTTOM;
int windowT = SEARCH_WINDOW_H;
for(int i = windowL; i<= windowR; i++)
{
//We are search bottom area as most likely road pixel candidates
for(int k = windowB; k <=windowT; k++) {
//set our grow window float to the middle of road
int middlePoint;
if(itsMiddlePoint[k-1]!=0 && itsUseFloatWindow) {
middlePoint = itsMiddlePoint[k-1]/4;//1/4 size image
//LINFO("Float Window %d midpoint %d",middlePoint,k-1);
} else {
middlePoint = w/2;
//LINFO("Fixed Window %d midpoint",middlePoint);
}
if(sp_img.coordsOk(middlePoint+i,h-k)) {
PixRGB<byte> tmp_color = sp_img.getVal(middlePoint + i,h-k);
int regionSize = sp_size_img.getVal(middlePoint+i,h-k);
bool notfound = true;
// Search color
for(int j = 0; j < (int)color_map.size() && notfound ; j++)
{
if(color_map[j] == tmp_color)
{
notfound = false;
color_map_count[j]++;
}
}
if(notfound)
{
color_map.push_back(tmp_color);
color_map_count.push_back(0);
color_size_map.push_back(regionSize);
}
}
}
}
if(color_map.size() > 1)
{ //if we found more than one color
//Some Option Here:
//1.Choose max count color
//2.Pick min color difference from previous avg road color pixel
//if road color is not available, we pick max pixel color
if(itsRoadColor == PixRGB<byte>(0,0,0)) {
int max = color_map_count[0];
int max_index = 0;
for(int i = 1; i < (int)color_map_count.size() ; i++)
{
if(max < color_map_count[i])
{
max = color_map_count[i];
max_index = i;
}
}
itsRoadColor = color_map[max_index];
//LINFO("Max count color have count %d",max);
} else {
//Pick min color difference color
int min_index = 0;
float min = colorDiff(itsRoadColor,color_map[0]);//
for(int i = 1; i < (int)color_map_count.size() ; i++)
{
float cd = colorDiff(itsRoadColor,color_map[i]);
int rs = color_size_map[i];//region size
//LINFO("Road Region Size %d",rs);
if(cd < min && rs > 100)
{
min = cd;
min_index = i;
}
}
itsRoadColorDiff = colorDiff(itsRoadColor,color_map[min_index]);
//to prevent jump too much
if(itsRoadColorDiff < 50.0) {
itsRoadColor = color_map[min_index];
} else {
//keep avg color so it will help to solve kid-napping problem
PixRGB<byte> avgColor = colorAvg(itsRoadColor,color_map[0],0.8);//first color will have 80% weight
itsRoadColor = avgColor;
//LINFO("COLOR DIFF1 %f",itsRoadColorDiff);
}
}
}
else
{
//if only one region
itsRoadColorDiff = colorDiff(itsRoadColor,color_map[0]);
itsRoadColorDiffSub = colorDiffSub(itsRoadColor,color_map[0]);
if((itsRoadColorDiff < 50.0 && color_map[0].green() > 150)||itsRoadColor == PixRGB<byte>(0,0,0)) { //for outdoor concrete road
//if((itsRoadColorDiff < 90.0 && color_map[0].green()<150)||itsRoadColor == PixRGB<byte>(0,0,0)){//indoor
//if((itsRoadColorDiff < 90.0)||itsRoadColor == PixRGB<byte>(0,0,0)){//general, high fail rate
itsRoadColor = color_map[0];
//itsUseFloatWindow = false;//FIXXXXXX
} else {
PixRGB<byte> avgColor = colorAvg(itsRoadColor,color_map[0],0.8);//80% on first one
itsRoadColor = avgColor;
itsUseFloatWindow = true;
//LINFO("COLOR DIFF2 %f,USE float window",itsRoadColorDiff);
}
//LINFO("Only one color (%d,%d,%d)",itsRoadColor.red(),itsRoadColor.green(),itsRoadColor.blue());
}
// use iterator!!!!!!
Image<PixRGB<byte> > output(w,h,ZEROS); //image with full red road region
Image<PixRGB<byte> > hybrid(w,h,ZEROS); //image with red grid dot road region
itsRoadIndexMap = Image<int>(w, h, ZEROS);
// inplacePaste(output, sp_img, Point2D<int>(w, 0));
for(int y = 0 ; y < h ; y ++)
{
int dot = 0;
for(int x = 0 ; x < w ; x ++)
{
PixRGB<byte> c = sp_img.getVal(x,y);
//LINFO("Pixel color(%d,%d,%d)road color(%d,%d,%d)",c.red(),c.green(),c.blue(),
// itsRoadColor.red(),itsRoadColor.green(),itsRoadColor.blue());
if(c == itsRoadColor) {
//Set road color to red
byte red = 250+c.red();
if(red > 255) red = 255;
//output.setVal(x,y,PixRGB<byte>(red,c.green()/32,c.blue()/32));//this will mess up find red region
output.setVal(x,y,PixRGB<byte>(255,0,0));
itsRoadIndexMap.setVal(x,y,255);
if(dot % 3 == 0) {
hybrid.setVal(x,y,PixRGB<byte>(255,0,0));
} else {
hybrid.setVal(x,y,c);
}
dot ++;
}
else {
//set to it's color
output.setVal(x,y,c);
hybrid.setVal(x,y,c);
itsRoadIndexMap.setVal(x,y,0);
// output.setVal(x,y,c);
}
}
}
LINFO("hi11");
if(!output.initialized())
return img;
LINFO("hi12");
//LINFO("Finish Road Finding");
itsRawRoadSuperPixelImg = hybrid;
return output;
}
void
initColormap ( JNIEnv* env, Toolkit* Tlk, Colormap cm, Rgb2Pseudo* map )
{
jclass clazz;
jfieldID fid;
jarray rgbRequests = 0;
jboolean isCopy;
jint *jrgbs = 0;
int nReq = 0;
unsigned long pixels[MAX_REQUESTS];
jint req[MAX_REQUESTS];
unsigned long planeMasks[1];
int n, i, j, k, l, m, pix;
Visual *v = DefaultVisualOfScreen( DefaultScreenOfDisplay( Tlk->dsp));
XColor xclr;
int r, g, b;
char blackSeen = 0;
unsigned char (*mp)[8][8][8] = alloca( 8*8*8 * sizeof( char));
memset( *mp, 0, 8*8*8);
/* get the java.awt.DefaultsRGB.RgbRequests field */
if ( (clazz = (*env)->FindClass( env, "java/awt/Defaults")) ){
if ( (fid = (*env)->GetStaticFieldID( env, clazz, "RgbRequests", "[I")) ){
if ( (rgbRequests = (*env)->GetStaticObjectField( env, clazz, fid)) ){
jrgbs = (*env)->GetIntArrayElements( env, rgbRequests, &isCopy);
nReq = (*env)->GetArrayLength( env, rgbRequests);
if ( nReq > MAX_REQUESTS )
nReq = MAX_REQUESTS;
memcpy( req, jrgbs, nReq * sizeof( jint));
(*env)->ReleaseIntArrayElements( env, rgbRequests, jrgbs, JNI_ABORT);
}
}
}
/*
* Determine how many RW cells there are available. Don't try to grab
* too many cells, since this might disturb other apps and could end up
* in even worse results
*/
for ( n= 10; n; n-- ) {
if ( XAllocColorCells( Tlk->dsp, cm, False, planeMasks, 0, pixels, n) )
break;
}
xclr.red = 0; xclr.green = 0; xclr.blue = 0;
xclr.flags = DoRed | DoGreen | DoBlue;
/* mark all of our cells (so that we don't rely on their current values) */
for ( i=0; i<n; i++ ){
xclr.pixel = pixels[i];
XStoreColor( Tlk->dsp, cm, &xclr);
}
/* check which of our rgb requests are already in the colormap */
for ( l=0; l<v->map_entries; l++ ) {
xclr.pixel = l;
XQueryColor( Tlk->dsp, cm, &xclr);
r = xclr.red >> 8;
g = xclr.green >> 8;
b = xclr.blue >> 8;
i = JI8(r);
j = JI8(g);
k = JI8(b);
if ( r | g | b ) {
for ( m=0; m<nReq; m++ ) {
if ( req[m] &&
colorDiff( JRED(req[m]), JGREEN(req[m]), JBLUE(req[m]), r, g, b) < CLR_DIST ) {
req[m] = 0; /* mark color request as satisfied */
(*mp)[i][j][k] = 1; /* mark cube cell (i,j,k) as requested */
break;
}
}
}
/*
* we start to populate the color cube as we go (XQueryColor might be expensive),
* but we don't overwrite an already set cell with a worse match (if we don't
* have a standard system colormap, there is a good chance that several
* colormap cells will map to the same indices of our 3/3/3 color cube). We also
* shouldn't overwrite requested colors with better cube-cell matches (hence the
* 'mp' check)
*/
if ( !(i|j|k) && blackSeen++ ) /* don't overwrite real black with avail cell */
continue;
if ( ((*mp)[i][j][k] < 2) && (!(pix = map->pix [i][j][k]) ||
(colorDiff( r, g, b, rgb8[i], rgb8[j], rgb8[k]) <
colorDiff( map->rgb[pix].r, map->rgb[pix].g, map->rgb[pix].b,
rgb8[i], rgb8[j], rgb8[k]))) ) {
if ( (*mp)[i][j][k] ) /* prevent cube cell from being overwritten, again */
(*mp)[i][j][k] ++;
map->pix [i][j][k] = l;
map->rgb[l].r = r;
map->rgb[l].g = g;
map->rgb[l].b = b;
}
}
/* set cells of not-yet satisfied rgb requests */
for ( i=0, j=0; (i<nReq) && (i<n); i++ ) {
if ( req[i] ){
r = JRED( req[i]);
g = JGREEN( req[i]);
b = JBLUE( req[i]);
xclr.pixel = pixels[j++];
xclr.red = r << 8;
xclr.green = g << 8;
xclr.blue = b << 8;
XStoreColor( Tlk->dsp, cm, &xclr);
map->pix [JI8(r)] [JI8(g)] [JI8(b)] = xclr.pixel;
map->rgb[xclr.pixel].r = r;
map->rgb[xclr.pixel].g = g;
map->rgb[xclr.pixel].b = b;
}
}
/*
* initalize rest of cube cells by computing their nearest rgb value, optionally
* allocating new cells for the worst mismatches (if there still are colors avail)
*/
fillUpColorCube( map, cm, (n - j), pixels+j, mp );
}
void
fillUpColorCube ( Rgb2Pseudo* map, Colormap cm, int nAvail, unsigned long *pixels,
unsigned char (*mp) [8][8][8] )
{
int i, j, k, pix, l;
int nMis = 0, maxMis = nAvail;
int r, g, b;
Mismatch *mm = alloca( maxMis* sizeof( Mismatch));
unsigned char d;
XColor xclr;
memset( *mp, 0, 8*8*8);
/*
* Find the nearest values for not yet initialized cells. Note that we
* cannot set these values directly in map->pix because it would cause
* interference with other unset cells (the way nearestColor works), and
* pixel values should just be computed from direct match cells
*/
for ( i=0; i<8; i++ ){
for ( j=0; j<8; j++ ) {
for ( k=0; k<8; k++ ){
if ( (map->pix[i][j][k] == 0) && (i | j | k) ){
pix = nearestColor( map, i, j, k, 1);
(*mp)[i][j][k] = pix;
/*
* If we still have available color cells, build a sorted list of the
* worst mismatches (but skip dark values)
*/
if ( (nAvail > 0) && (i|j|k) > 2) {
if ( (d = (unsigned char) colorDiff( rgb8[i], rgb8[j], rgb8[k],
map->rgb[pix].r, map->rgb[pix].g, map->rgb[pix].b)) > 50 ){
for ( l=0; l<nMis && mm[l].d > d ; l++ );
if ( l < nMis )
memmove( mm + l+1, mm+l, (nMis - l)*sizeof( Mismatch));
mm[l].d = d; mm[l].i = i; mm[l].j = j; mm[l].k = k;
if ( nMis < maxMis )
nMis++;
}
}
}
}
}
}
/* if there is a mismatch list, resolve it */
for ( l=0; l< nMis-1; l++ ) {
r = rgb8[ mm[l].i ];
g = rgb8[ mm[l].j ];
b = rgb8[ mm[l].k ];
xclr.pixel = pixels[l];
xclr.flags = DoRed | DoGreen | DoBlue;
xclr.red = r << 8;
xclr.green = g << 8;
xclr.blue = b << 8;
XStoreColor( Tlk->dsp, cm, &xclr);
map->pix [mm[l].i] [mm[l].j] [mm[l].k] = xclr.pixel;
map->rgb[xclr.pixel].r = r;
map->rgb[xclr.pixel].g = g;
map->rgb[xclr.pixel].b = b;
/* mark this cell as satisifed */
(*mp) [mm[l].i] [mm[l].j] [mm[l].k] = 0;
}
/* store all still uninitialized cube cells from our temp cube of nearest values */
for ( i=0; i<8; i++ ){
for ( j=0; j<8; j++ ) {
for ( k=0; k<8; k++ ){
if ( (pix = (*mp) [i][j][k]) )
map->pix[i][j][k] = pix;
}
}
}
}