pointer ICONVCLOSE(context *ctx, int n, pointer *argv)
{ int cd, ret;
ckarg(1);
cd=bigintval(argv[0]);
ret=iconv_close(cd);
return(makeint(ret));
}
pointer EUSPINHOLE_CAMERA_MODEL_DISPOSE(register context *ctx,int n,pointer *argv)
{
ckarg(1);
image_geometry::PinholeCameraModel *pcm = (image_geometry::PinholeCameraModel *)(intval(argv[0]));
delete(pcm);
return(T);
}
pointer ICONVOPEN(context *ctx, int n, pointer *argv)
{ int cd;
ckarg(2);
if (!isstring(argv[0])) error (E_NOSTRING, argv[0]);
if (!isstring(argv[1])) error (E_NOSTRING, argv[1]);
cd=iconv_open(argv[0]->c.str.chars, argv[1]->c.str.chars);
return(mkbigint(cd));}
pointer PNG_WRITE_IMAGE(register context *ctx, int n, register pointer *argv)
{
char *file_name, *image_ptr;
int width, height, channels;
ckarg(5);
if (isstring(argv[0])) file_name = argv[0]->c.str.chars;
else error(E_NOSTRING);
width = ckintval(argv[1]);
height = ckintval(argv[2]);
channels = ckintval(argv[3]);
image_ptr = argv[4]->c.str.chars;
fprintf(stderr, "%d %d %d %p\n", width, height, channels, image_ptr);
FILE *fp = fopen(file_name, "wb");
if (!fp) {
error(E_OPENFILE);
return(NIL);
}
png_structp png_ptr;
png_infop info_ptr;
png_ptr = png_create_write_struct(PNG_LIBPNG_VER_STRING,NULL,NULL,NULL);
info_ptr = png_create_info_struct(png_ptr);
if (setjmp(png_jmpbuf(png_ptr))) {
png_destroy_write_struct(&png_ptr, &info_ptr);
fclose(fp);
error(E_EOF);
return(NIL);
}
png_init_io(png_ptr, fp);
png_set_IHDR(png_ptr, info_ptr, width, height, 8, PNG_COLOR_TYPE_RGB, //GRAY
PNG_INTERLACE_NONE, PNG_COMPRESSION_TYPE_BASE, PNG_FILTER_TYPE_BASE);
png_bytep * row_pointers = (png_bytep*) malloc(sizeof(png_bytep) * height);
int y, byte_per_scanline = png_get_rowbytes(png_ptr, info_ptr);
fprintf(stderr, "%d\n", byte_per_scanline);
for(y=0;y<height;y++){
row_pointers[y] = &(image_ptr[y*byte_per_scanline]);
}
png_set_rows(png_ptr, info_ptr, row_pointers);
png_write_png(png_ptr, info_ptr, PNG_TRANSFORM_IDENTITY, (png_voidp)NULL);
png_write_end(png_ptr, info_ptr);
free(row_pointers);
png_destroy_write_struct(&png_ptr, &info_ptr);
fclose(fp);
return (T);
}
pointer EUSPINHOLE_CAMERA_MODEL_PROJECT_3D_TO_PIXEL(register context *ctx,int n,pointer *argv)
{
ckarg(2);
image_geometry::PinholeCameraModel *pcm = (image_geometry::PinholeCameraModel *)(intval(argv[0]));
if(!isvector(argv[1])) error(E_NOVECTOR);
eusfloat_t *pos = argv[1]->c.fvec.fv;
cv::Point3d xyz = cv::Point3d(pos[0]/1000.0, pos[1]/1000.0, pos[2]/1000.0);
cv::Point2d uv = pcm->project3dToPixel(xyz);
pointer vs = makefvector(2);
vpush(vs);
vs->c.fvec.fv[0] = uv.x;
vs->c.fvec.fv[1] = uv.y;
vpop();
return(vs);
}
pointer EUSPINHOLE_CAMERA_MODEL_PROJECT_PIXEL_TO_3DRAY(register context *ctx,int n,pointer *argv)
{
ckarg(2);
image_geometry::PinholeCameraModel *pcm = (image_geometry::PinholeCameraModel *)(intval(argv[0]));
if(!isvector(argv[1])) error(E_NOVECTOR);
eusfloat_t *pixel = argv[1]->c.fvec.fv;
cv::Point2d uv = cv::Point2d(pixel[0], pixel[1]);
cv::Point3d xyz = pcm->projectPixelTo3dRay(uv);
pointer vs = makefvector(3);
vpush(vs);
vs->c.fvec.fv[0] = xyz.x;
vs->c.fvec.fv[1] = xyz.y;
vs->c.fvec.fv[2] = xyz.z;
vpop();
return(vs);
}
pointer EUSPINHOLE_CAMERA_MODEL_FROM_CAMERA_INFO(register context *ctx,int n,pointer *argv)
{
ckarg(3);
uint8_t* serialized_camera_info = (uint8_t *)(argv[1]->c.str.chars);
uint32_t serialization_length = intval(argv[2]);
image_geometry::PinholeCameraModel *pcm = (image_geometry::PinholeCameraModel *)(intval(argv[0]));
sensor_msgs::CameraInfo camera_info;
boost::shared_array<uint8_t> buffer(new uint8_t[serialization_length]);
for(int i=0; i<serialization_length; ++i){
buffer[i] = serialized_camera_info[i];
}
ros::serialization::IStream stream(buffer.get(), serialization_length);
ros::serialization::Serializer<sensor_msgs::CameraInfo>::read(stream, camera_info);
pcm->fromCameraInfo(camera_info);
return (T);
}
pointer ICONV(context *ctx, int n, pointer *argv)
{ int cd, ret, malloced=NULL;
char *srcstrp, *deststrp, *deststrpv, deststr[1024];
size_t srcstrlen, deststrlen;
pointer dest;
ckarg(2);
cd=bigintval(argv[0]);
if (!isstring(argv[1])) error(E_NOSTRING);
srcstrp=argv[1]->c.str.chars;
srcstrlen=strlength(argv[1]);
deststrlen=2*srcstrlen;
if (deststrlen>=1024) {
deststrp=malloc(deststrlen);
malloced=1;}
else deststrp=deststr;
deststrpv=deststrp;
ret=iconv(cd, &srcstrp, &srcstrlen, &deststrpv, &deststrlen);
if (ret== -1) { dest=NIL; goto iconvend;}
dest=makestring(deststrp, 2*strlength(argv[1])-deststrlen);
iconvend:
if (malloced) cfree(deststrp);
return(dest);
}
pointer PNG_READ_IMAGE(register context *ctx, int n, register pointer *argv)
{
char *file_name;
pointer ret, image_ptr;
ckarg(1);
if (isstring(argv[0])) file_name = argv[0]->c.str.chars;
else error(E_NOSTRING);
FILE *fp = fopen(file_name, "rb");
if (!fp) {
error(E_OPENFILE);
return(NIL);
}
png_structp png_ptr;
png_infop info_ptr;
png_ptr = png_create_read_struct(PNG_LIBPNG_VER_STRING,NULL,NULL,NULL);
info_ptr = png_create_info_struct(png_ptr);
if (setjmp(png_jmpbuf(png_ptr))) {
png_destroy_read_struct(&png_ptr, &info_ptr, (png_infopp)NULL);
fclose(fp);
error(E_EOF);
return(NIL);
}
png_init_io(png_ptr, fp);
png_read_info(png_ptr, info_ptr);
int width = png_get_image_width(png_ptr, info_ptr);
int height = png_get_image_height(png_ptr, info_ptr);
int bit_depth = png_get_bit_depth(png_ptr, info_ptr);
int channels = png_get_channels(png_ptr, info_ptr);
int color_type = png_get_color_type(png_ptr, info_ptr);
//fprintf(stderr, "bit_depth = %d, channels %d, color_type =%d (pal:%d,gray:%d,rgb:%d,rgba:%d)\n", bit_depth, channels, color_type, PNG_COLOR_TYPE_PALETTE,PNG_COLOR_TYPE_GRAY,PNG_COLOR_TYPE_RGB,PNG_COLOR_TYPE_RGB_ALPHA);
switch (color_type) {
case PNG_COLOR_TYPE_PALETTE:
png_set_palette_to_rgb(png_ptr);
break;
case PNG_COLOR_TYPE_GRAY:
#ifdef PNG_READ_RGB_TO_GRAY_SUPPORTED
if ( bit_depth < 8) png_set_gray_to_rgb(png_ptr);
#else
if ( bit_depth < 8) png_set_gray_1_2_4_to_8(png_ptr);
#endif
break;
case PNG_COLOR_TYPE_RGB:
//png_set_bgr(png_ptr);
if (bit_depth == 16) png_set_strip_16(png_ptr); // 16bit -> 8bit
break;
case PNG_COLOR_TYPE_RGB_ALPHA:
if (bit_depth == 16) png_set_strip_16(png_ptr); // 16bit -> 8bit
png_set_invert_alpha(png_ptr);
//png_set_bgr(png_ptr);
//png_set_strip_alpha(png_ptr);
// RGBA -> rgb , GA -> g
png_color_16 my_background = {0xff, 0xff, 0xff, 0xff, 0xff};
png_set_background(png_ptr, &my_background,
PNG_BACKGROUND_GAMMA_SCREEN, 0, 1.0);
break;
}
png_read_update_info(png_ptr, info_ptr);
width = png_get_image_width(png_ptr, info_ptr); height = png_get_image_height(png_ptr, info_ptr);;
bit_depth = png_get_bit_depth(png_ptr, info_ptr); channels = png_get_channels(png_ptr, info_ptr);
color_type = png_get_color_type(png_ptr, info_ptr);
png_bytep * row_pointers = (png_bytep *)malloc(height*sizeof(png_bytep));
int y, byte_per_scanline = png_get_rowbytes(png_ptr, info_ptr);
image_ptr = makebuffer(height*byte_per_scanline);
for(y=0;y<height;y++){
row_pointers[y] = image_ptr->c.str.chars+y*byte_per_scanline;
}
png_read_image(png_ptr, row_pointers);
free(row_pointers);
png_read_end(png_ptr,info_ptr);
png_destroy_read_struct(&png_ptr, &info_ptr, (png_infopp)NULL);
fclose(fp);
ret=cons(ctx,image_ptr,NIL);
ret=cons(ctx,makeint(channels),ret);
ret=cons(ctx,makeint(height),ret);
ret=cons(ctx,makeint(width),ret);
return (ret);
}