eusimage_geometry.cpp
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 // for eus.h
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 }


jsk_smart_gui
Author(s):
autogenerated on Thu Jun 6 2019 18:04:26