public void MapUpdate() { int max_disparity_percent = 40; int map_x = 60; int map_y = 60; byte[] cartesian_map = new byte[map_x * map_y * 3]; int stereo_matches = 0; Hpolar map = new Hpolar(); map.imgWidth = 320; map.imgHeight = 240; map.footline = new ushort[(int)map.imgWidth / map.SVS_HORIZONTAL_SAMPLING]; map.footline_dist_mm = new ushort[(int)map.imgWidth / map.SVS_HORIZONTAL_SAMPLING]; // create a footline for (int x = 0; x < (int)map.imgWidth / map.SVS_HORIZONTAL_SAMPLING; x++) { map.footline[x] = (ushort)(map.imgHeight / 2); } map.FootlineUpdate(max_disparity_percent); /* create a map */ drawing.drawLine(cartesian_map, map_x, map_y, (map_x / 2) - 2, (map_y / 2) - 6, (map_x / 2) - 2, (map_y / 2) - 29, 255, 0, 0, 0, false); drawing.drawLine(cartesian_map, map_x, map_y, (map_x / 2) + 2, (map_y / 2) - 6, (map_x / 2) + 2, (map_y / 2) - 29, 255, 0, 0, 0, false); drawing.drawLine(cartesian_map, map_x, map_y, (map_x / 2) - 2, (map_y / 2) - 6, (map_x / 2) - 10, (map_y / 2) - 6, 255, 0, 0, 0, false); drawing.drawLine(cartesian_map, map_x, map_y, (map_x / 2) + 2, (map_y / 2) - 6, (map_x / 2) + 10, (map_y / 2) - 6, 255, 0, 0, 0, false); Bitmap bmp = new Bitmap(map_x, map_y, System.Drawing.Imaging.PixelFormat.Format24bppRgb); BitmapArrayConversions.updatebitmap_unsafe(cartesian_map, bmp); bmp.Save("HpolarTests_MapUpdate0.bmp", System.Drawing.Imaging.ImageFormat.Bmp); /* convert map to stereo view */ map.SimulateStereoView( max_disparity_percent, map_x, map_y, cartesian_map, ref stereo_matches); map.SaveDisparities( "HpolarTests_MapUpdate1.bmp", stereo_matches); map.MapUpdate(stereo_matches, 40); map.Show("HpolarTests_MapUpdate2.bmp"); Assert.IsTrue(stereo_matches > 0); }
public void Recenter() { int max_disparity_percent = 40; int map_x = 60; int map_y = 60; byte[] cartesian_map = new byte[map_x * map_y * 3]; int stereo_matches = 0; Hpolar map = new Hpolar(); map.imgWidth = 320; map.imgHeight = 240; map.footline = new ushort[(int)map.imgWidth / map.SVS_HORIZONTAL_SAMPLING]; map.footline_dist_mm = new ushort[(int)map.imgWidth / map.SVS_HORIZONTAL_SAMPLING]; // create a footline for (int x = 0; x < (int)map.imgWidth / map.SVS_HORIZONTAL_SAMPLING; x++) map.footline[x] = (ushort)(map.imgHeight/2); map.FootlineUpdate(max_disparity_percent); /* create a map */ drawing.drawLine(cartesian_map, map_x, map_y, (map_x/2) - 2, (map_y/2)-6, (map_x/2) - 2, (map_y/2)-29, 255,0,0,0,false); drawing.drawLine(cartesian_map, map_x, map_y, (map_x/2) + 2, (map_y/2)-6, (map_x/2) + 2, (map_y/2)-29, 255,0,0,0,false); drawing.drawLine(cartesian_map, map_x, map_y, (map_x/2) - 2, (map_y/2)-6, (map_x/2) - 10, (map_y/2)-6, 255,0,0,0,false); drawing.drawLine(cartesian_map, map_x, map_y, (map_x/2) + 2, (map_y/2)-6, (map_x/2) + 10, (map_y/2)-6, 255,0,0,0,false); Bitmap bmp = new Bitmap(map_x,map_y,System.Drawing.Imaging.PixelFormat.Format24bppRgb); BitmapArrayConversions.updatebitmap_unsafe(cartesian_map, bmp); bmp.Save("HpolarTests_Recenter0.bmp", System.Drawing.Imaging.ImageFormat.Bmp); /* convert map to stereo view */ map.SimulateStereoView( max_disparity_percent, map_x, map_y, cartesian_map, ref stereo_matches); map.SaveDisparities( "HpolarTests_Recenter1.bmp", stereo_matches); map.MapUpdate(stereo_matches, 40); map.Show("HpolarTests_Recenter2.bmp"); Assert.IsTrue(stereo_matches > 0); map.robot_y_mm -= 200; map.Recenter(); map.Show("HpolarTests_Recenter3.bmp"); map.robot_y_mm -= 200; map.Recenter(); map.Show("HpolarTests_Recenter4.bmp"); map.robot_y_mm += 200; map.Recenter(); map.Show("HpolarTests_Recenter5.bmp"); map.robot_y_mm += 200; map.Recenter(); map.Show("HpolarTests_Recenter6.bmp"); }