Go to the documentation of this file.00001 #include <stdio.h>
00002 #include <stdlib.h>
00003 #include <unistd.h>
00004 #include <string.h>
00005 #include <signal.h>
00006 #include <math.h>
00007 #include <time.h>
00008 #include <pthread.h>
00009 #include <setjmp.h>
00010 #include <errno.h>
00011
00012 #include <list>
00013 #include <vector>
00014 #include <set>
00015 #include <string>
00016 #include <map>
00017 #include <sstream>
00018
00019 #include <cstdio>
00020 #include <boost/thread/mutex.hpp>
00021 #include <boost/thread/condition.hpp>
00022 #include <boost/shared_ptr.hpp>
00023
00024 #include <ros/init.h>
00025 #include <ros/time.h>
00026 #include <ros/rate.h>
00027 #include <ros/master.h>
00028 #include <ros/this_node.h>
00029 #include <ros/node_handle.h>
00030 #include <ros/serialization.h>
00031
00032
00033 #define class eus_class
00034 #define throw eus_throw
00035 #define export eus_export
00036 #define vector eus_vector
00037 #define string eus_string
00038
00039 #include "eus.h"
00040 extern "C" {
00041 pointer ___eusimage_geometry(register context *ctx, int n, pointer *argv, pointer env);
00042 void register_eusimage_geometry(){
00043 char modname[] = "___eusimage_geometry";
00044 return add_module_initializer(modname, (pointer (*)())___eusimage_geometry);}
00045 }
00046
00047 #undef class
00048 #undef throw
00049 #undef export
00050 #undef vector
00051 #undef string
00052
00053 #include <sensor_msgs/CameraInfo.h>
00054 #include <image_geometry/pinhole_camera_model.h>
00055
00056 pointer EUSPINHOLE_CAMERA_MODEL(register context *ctx,int n,pointer *argv)
00057 {
00058 return (makeint((eusinteger_t)(new image_geometry::PinholeCameraModel())));
00059 }
00060
00061 pointer EUSPINHOLE_CAMERA_MODEL_DISPOSE(register context *ctx,int n,pointer *argv)
00062 {
00063 ckarg(1);
00064 image_geometry::PinholeCameraModel *pcm = (image_geometry::PinholeCameraModel *)(intval(argv[0]));
00065 delete(pcm);
00066 return(T);
00067 }
00068
00069 pointer EUSPINHOLE_CAMERA_MODEL_FROM_CAMERA_INFO(register context *ctx,int n,pointer *argv)
00070 {
00071 ckarg(3);
00072
00073 uint8_t* serialized_camera_info = (uint8_t *)(argv[1]->c.str.chars);
00074 uint32_t serialization_length = intval(argv[2]);
00075 image_geometry::PinholeCameraModel *pcm = (image_geometry::PinholeCameraModel *)(intval(argv[0]));
00076
00077 sensor_msgs::CameraInfo camera_info;
00078 boost::shared_array<uint8_t> buffer(new uint8_t[serialization_length]);
00079
00080 for(int i=0; i<serialization_length; ++i){
00081 buffer[i] = serialized_camera_info[i];
00082 }
00083
00084 ros::serialization::IStream stream(buffer.get(), serialization_length);
00085 ros::serialization::Serializer<sensor_msgs::CameraInfo>::read(stream, camera_info);
00086
00087 pcm->fromCameraInfo(camera_info);
00088 return (T);
00089 }
00090
00091 pointer EUSPINHOLE_CAMERA_MODEL_PROJECT_PIXEL_TO_3DRAY(register context *ctx,int n,pointer *argv)
00092 {
00093 ckarg(2);
00094 image_geometry::PinholeCameraModel *pcm = (image_geometry::PinholeCameraModel *)(intval(argv[0]));
00095 if(!isvector(argv[1])) error(E_NOVECTOR);
00096 eusfloat_t *pixel = argv[1]->c.fvec.fv;
00097
00098 cv::Point2d uv = cv::Point2d(pixel[0], pixel[1]);
00099 cv::Point3d xyz = pcm->projectPixelTo3dRay(uv);
00100
00101 pointer vs = makefvector(3);
00102 vpush(vs);
00103 vs->c.fvec.fv[0] = xyz.x;
00104 vs->c.fvec.fv[1] = xyz.y;
00105 vs->c.fvec.fv[2] = xyz.z;
00106 vpop();
00107 return(vs);
00108 }
00109
00110 pointer EUSPINHOLE_CAMERA_MODEL_PROJECT_3D_TO_PIXEL(register context *ctx,int n,pointer *argv)
00111 {
00112 ckarg(2);
00113 image_geometry::PinholeCameraModel *pcm = (image_geometry::PinholeCameraModel *)(intval(argv[0]));
00114 if(!isvector(argv[1])) error(E_NOVECTOR);
00115 eusfloat_t *pos = argv[1]->c.fvec.fv;
00116
00117 cv::Point3d xyz = cv::Point3d(pos[0]/1000.0, pos[1]/1000.0, pos[2]/1000.0);
00118 cv::Point2d uv = pcm->project3dToPixel(xyz);
00119
00120 pointer vs = makefvector(2);
00121 vpush(vs);
00122 vs->c.fvec.fv[0] = uv.x;
00123 vs->c.fvec.fv[1] = uv.y;
00124 vpop();
00125 return(vs);
00126 }
00127
00128 pointer ___eusimage_geometry(register context *ctx, int n, pointer *argv, pointer env)
00129 {
00130 defun(ctx,"EUSPINHOLE-CAMERA-MODEL",argv[0],(pointer (*)())EUSPINHOLE_CAMERA_MODEL,
00131 "Instantiate euspinhole-camera-model cobject");
00132 defun(ctx,"EUSPINHOLE-CAMERA-MODEL-DISPOSE",argv[0],(pointer (*)())EUSPINHOLE_CAMERA_MODEL_DISPOSE,
00133 "Dispose euspinhole-camera-model cobject");
00134 defun(ctx,"EUSPINHOLE-CAMERA-MODEL-FROM-CAMERA-INFO",argv[0],(pointer (*)())EUSPINHOLE_CAMERA_MODEL_FROM_CAMERA_INFO,
00135 "set camera info to euspinhoke-camera-model cobject");
00136 defun(ctx,"EUSPINHOLE-CAMERA-MODEL-PROJECT-PIXEL-TO-3DRAY",argv[0],(pointer (*)())EUSPINHOLE_CAMERA_MODEL_PROJECT_PIXEL_TO_3DRAY,
00137 "Compute 3D Ray from 2D pixel on the projected camera screen");
00138 defun(ctx,"EUSPINHOLE-CAMERA-MODEL-PROJECT-3D-TO-PIXEL",argv[0],(pointer (*)())EUSPINHOLE_CAMERA_MODEL_PROJECT_3D_TO_PIXEL,
00139 "Compute 2D pixel on the projected camera screen from 3D pose");
00140 return 0;
00141 }