├── .gitignore ├── CMakeLists.txt ├── README.md ├── analysis ├── ComputeLocalizationErrors.m ├── matchnearest.m ├── matchnearest2.m ├── qGetQ.m ├── qGetR.m ├── quat2rpy.m ├── quatinv.m ├── quatmultiply.m ├── quatrotate.m └── rpy2quat.m ├── bin └── map.stl ├── cmake └── Modules │ ├── FindGCOP.cmake │ ├── FindOGRE.cmake │ ├── FindOIS.cmake │ ├── FindObjectRenderer.cmake │ ├── FindPkgMacros.cmake │ ├── FindTinyXML.cmake │ ├── LibFindMacros.cmake │ └── PreprocessorUtils.cmake ├── data └── cheezit │ ├── depth000.xml │ ├── depth001.xml │ ├── depth002.xml │ ├── depth003.xml │ ├── depth004.xml │ ├── depth005.xml │ ├── depth006.xml │ ├── depth007.xml │ ├── depth008.xml │ ├── depth009.xml │ ├── depth010.xml │ ├── depth011.xml │ ├── descriptors000.xml │ ├── descriptors001.xml │ ├── descriptors002.xml │ ├── descriptors003.xml │ ├── descriptors004.xml │ ├── descriptors005.xml │ ├── descriptors006.xml │ ├── descriptors007.xml │ ├── descriptors008.xml │ ├── descriptors009.xml │ ├── descriptors010.xml │ ├── descriptors011.xml │ ├── intrinsics.xml │ ├── keyframe000.jpg │ ├── keyframe001.jpg │ ├── keyframe002.jpg │ ├── keyframe003.jpg │ ├── keyframe004.jpg │ ├── keyframe005.jpg │ ├── keyframe006.jpg │ ├── keyframe007.jpg │ ├── keyframe008.jpg │ ├── keyframe009.jpg │ ├── keyframe010.jpg │ ├── keyframe011.jpg │ ├── keypoints000.xml │ ├── keypoints001.xml │ ├── keypoints002.xml │ ├── keypoints003.xml │ ├── keypoints004.xml │ ├── keypoints005.xml │ ├── keypoints006.xml │ ├── keypoints007.xml │ ├── keypoints008.xml │ ├── keypoints009.xml │ ├── keypoints010.xml │ ├── keypoints011.xml │ ├── pose000.xml │ ├── pose001.xml │ ├── pose002.xml │ ├── pose003.xml │ ├── pose004.xml │ ├── pose005.xml │ ├── pose006.xml │ ├── pose007.xml │ ├── pose008.xml │ ├── pose009.xml │ ├── pose010.xml │ └── pose011.xml ├── include └── mesh_localize │ ├── ASiftDetector.h │ ├── CameraContainer.h │ ├── Common.h │ ├── DepthFeatureMatchLocalizer.h │ ├── EdgeTrackingUtil.h │ ├── FABMAPLocalizer.h │ ├── FeatureMatchLocalizer.h │ ├── FindCameraMatrices.h │ ├── IMUMotionModel.h │ ├── ImageDbUtil.h │ ├── KLTTracker.h │ ├── KeyframeContainer.h │ ├── KeyframeMatch.h │ ├── MapFeatures.h │ ├── MeshLocalizer.h │ ├── MonocularLocalizer.h │ ├── OgreImageGenerator.h │ ├── PnPUtil.h │ ├── PointCloudImageGenerator.h │ ├── Triangulation.h │ └── VirtualImageGenerator.h ├── launch ├── gazebo.launch ├── gazebo_campus.launch ├── gazebo_debug.launch ├── localize.launch ├── localize_box_ogre.launch ├── localize_box_ogre_debug.launch ├── localize_box_ogre_test.launch ├── localize_box_ogre_valgrind.launch ├── localize_campus_fabmap.launch ├── localize_campus_matching.launch ├── localize_car.launch ├── localize_cheezit_ogre.launch ├── localize_cheezit_ogre_imu.launch ├── localize_cheezit_ogre_quad.launch ├── localize_debug.launch ├── localize_lab_fabmap.launch ├── localize_lab_matching.launch ├── localize_lab_ogre.launch ├── localize_pringles_ogre.launch ├── mesh_localize.rviz ├── mesh_localize.rviz.vcg ├── mesh_localize_rosserial.launch └── rviz.launch ├── ogre_cfg ├── ogre.cfg ├── plugins.cfg └── resources.cfg ├── package.xml ├── src ├── ASiftDetector.cpp ├── CameraContainer.cpp ├── Common.cpp ├── DepthFeatureMatchLocalizer.cpp ├── EdgeTrackingUtil.cpp ├── FABMAPLocalizer.cpp ├── FeatureMatchLocalizer.cpp ├── FindCameraMatrices.cpp ├── IMUMotionModel.cpp ├── ImageDbUtil.cpp ├── KLTTracker.cpp ├── KeyframeContainer.cpp ├── KeyframeMatch.cpp ├── MapFeatures.cpp ├── MeshLocalizer.cpp ├── OgreImageGenerator.cpp ├── PnPUtil.cpp ├── PointCloudImageGenerator.cpp ├── Triangulation.cpp ├── mesh_localize_node.cpp └── render_node.cpp └── worlds ├── campus.world ├── car_terrain.world └── lab_cage.world /.gitignore: -------------------------------------------------------------------------------- 1 | data/ 2 | bin/ 3 | build/ 4 | lib/ 5 | models/ 6 | analysis/data 7 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # MeshLocalize: Model-based object tracking in 3D for ROS # 2 | 3 | Tracking Video: https://www.youtube.com/watch?v=UqzNlcw1U7s 4 | 5 | Grasping Demo: https://www.youtube.com/watch?v=T953WeLroqg 6 | 7 | # 1. Installation # 8 | 9 | ## 1.1 Dependencies ## 10 | 11 | This has only been tested with Ubuntu 14.04 + ROS Indigo. 12 | 13 | Install Eigen3. 14 | 15 | Install OpenCV 2.4.11. Source can be downloaded from https://github.com/Itseez/opencv/archive/2.4.11.zip 16 | 17 | Install OIS (needed for OGRE). 18 | 19 | sudo apt-get install libois-dev 20 | sudo apt-get install libois-1.3.0 21 | 22 | or source can be downloaded from http://sourceforge.net/projects/wgois/files/ 23 | 24 | Install Nvidia Cg Toolkit 25 | 26 | sudo apt-get install nvidia-cg-toolkit 27 | 28 | Install Ogre 1.8.1. You can download the source from http://sourceforge.net/projects/ogre/files/ogre/1.8/1.8.1/ogre_src_v1-8-1.tar.bz2/download 29 | 30 | Install TooN from http://www.edwardrosten.com/cvd/toon.html 31 | 32 | Install the object_renderer library. It is a small wrapper around OGRE. It is used to generate the virtual views of the object that the package uses to initialize the pose. 33 | 34 | git clone https://github.com/msheckells/object_renderer 35 | 36 | Build using cmake+make. 37 | 38 | ## 1.2 Package Install ## 39 | 40 | From your catkin source directory: 41 | 42 | git clone https://github.com/msheckells/mesh_localize.git 43 | 44 | Call catkin_make as usual 45 | 46 | # 2. Quick Start # 47 | Download a test rosbag which contains image and camera_info data from http://www.mattsheckells.com/wp-content/uploads/2015/cheezit_test.bag.zip and extract it. 48 | 49 | Launch the node: 50 | 51 | roslaunch mesh_localize localize_cheezit_ogre.launch 52 | 53 | Play the sequence: 54 | 55 | rosbag play cheezit_test.bag 56 | 57 | # 3. Setup Guide # 58 | ## 3.1 Model Creation ## 59 | Create a textured 3D model of the object you wish to track, preferably in DAE or OBJ format. This model can be converted into an OGRE .mesh file using blender and the blender2ogre exporter which can be downloaded from https://blender2ogre.googlecode.com/files/blender2ogre-0.6.0.zip. The path of the object must be added to ogre_cfg/resources.cfg so that the package can find the model. 60 | 61 | ## 3.2 Descriptor/Pose Database Creation ## 62 | Make sure the path of the object has been added to cfg/resources.cfg in the object_renderer parent folder. Use the render_views tool in the object_renderer package to extract descriptors from images of the object rendered from known poses. This is necessary to initialize the object tracker. This can be done with the command 63 | 64 | render_views 65 | 66 | This will render num_samples views of the object taken at random points lying on a sphere with the specified radius with the camera pointing at the origin. The output will be saved to the path given as output_dir. 67 | 68 | ## 3.3 Calibration ## 69 | Calibrate your camera using ROS camera calibration. 70 | 71 | ## 3.4 Tracking Modes ## 72 | There are three tracking modes: PNP, KLT, and EDGE. All initialize using the descriptor/pose database. 73 | 74 | PNP mode extracts features from an input image and from a virtual image of the object rendered from its last known pose. The features are matched to give a set of 2D-3D correspondences between the model and the input image. A PnP problem is solved to give the object pose in the frame of the camera. 75 | 76 | KLT mode first obtains a set of 2D-3D correspondences using PNP mode, then tracks the 2D keypoints using a KLT tracker. A PnP problem is solved to give the object pose. Correspondences are re-matched when the tracking diverges or when the re-projection error rises above a given threshold. 77 | 78 | EDGE mode performs edge-based object tracking and is suitable for objects with little texture. A Canny edge detecttor is used on both a virtual view of the model and an input image. Edge points are matched form the model to the input image by performing a 1D search in the gradient direction of the edge. The distance between matched edges is minimized to estimate the objects pose. 79 | 80 | # 4. Topics # 81 | ## 4.1 Published ## 82 | /mesh_localize/image [sensor_msgs::Image] Rectified version of the input image on which tracking is performed 83 | 84 | /mesh_localize/camera_info [sensor_msgs::CameraInfo] Camera info corresonding to resized and cropped input image. 85 | 86 | /mesh_localize/depth [sensor_msgs::Image] Depth map corresponding to /mesh_localize/image 87 | 88 | /mesh_localize/estimated_pose [geometry_msgs::PoseStamped] Pose of the object in the frame of the camera 89 | 90 | ## 4.2 Subscribed ## 91 | /image [sensor_msgs::Image] Unrectified input image on which tracking will be performed 92 | 93 | /camera_info [sensor_msgs::CameraInfo] Camera info of input image 94 | 95 | # 5. Parameters # 96 | TODO 97 | 98 | # Troubleshooting 99 | 100 | ## Using with ROS Kinetic 101 | 102 | Build vision_opencv from source using the indigo branch. Make sure it builds using OpenCV 2.4 instead of the OpenCV3 that comes packaged with Kinetic. 103 | 104 | ## Cuda Linking Error 105 | 106 | If you receive the error 107 | 108 | /usr/bin/ld: cannot find -lopencv_dep_cudart 109 | 110 | Then set the following cmake variable 111 | 112 | -DCUDA_USE_STATIC_CUDA_RUNTIME=OFF 113 | -------------------------------------------------------------------------------- /analysis/ComputeLocalizationErrors.m: -------------------------------------------------------------------------------- 1 | close all; clear all; 2 | 3 | % Found with known measurement 4 | map_scale = .8862; 5 | calibrated_Hmarker_cam = [0.0120 -0.9998 -0.0107 0.0010; 6 | 0.9791 0.0096 0.2030 0.2036; 7 | -0.2029 -0.0129 0.9791 -0.3100; 8 | 0 0 0 1.0000]; 9 | 10 | % Locations of optitrak markers in map 11 | map_opti_o = [-1.37162; 1.76992; 0.401571]; 12 | map_opti_x = [-1.058911; 1.78101; 0.302765]; 13 | map_opti_y = [-1.22047; 1.73605; 0.828085]; 14 | 15 | map_opti_x = map_opti_x - map_opti_o; 16 | map_opti_x = map_opti_x / norm(map_opti_x); 17 | map_opti_y = map_opti_y - map_opti_o; 18 | map_opti_y = map_opti_y / norm(map_opti_y); 19 | 20 | map_opti_z = cross(map_opti_x, map_opti_y); 21 | map_opti_z = map_opti_z / norm(map_opti_z); 22 | 23 | Rmap_opti = [map_opti_x, map_opti_y, map_opti_z]; 24 | 25 | 26 | % project to rotation space due to imperfect point measurements 27 | [U, sig, V] = svd(Rmap_opti); 28 | Rmap_opti = U*eye(3)*V'; 29 | 30 | % Qmap_opti = qGetQ(Rmap_opti); 31 | Tmap_opti = map_scale*map_opti_o; 32 | 33 | % transform from optitrak to map 34 | Hmap_opti = [Rmap_opti Tmap_opti; 0 0 0 1]; 35 | 36 | fileNum = 2; 37 | fileNumStr = num2str(fileNum); 38 | 39 | % Open and parse data 40 | est_poses = csvread(strcat('data/est_pose', fileNumStr, '.csv'), 1, 3); 41 | true_poses = csvread(strcat('data/true_pose', fileNumStr, '.csv'), 1, 5); 42 | 43 | % Open and parse data 44 | fid = fopen(strcat('data/true_pose', fileNumStr, '.csv')); %open file 45 | headers = fgetl(fid); %get first line 46 | headers = textscan(headers,'%s','delimiter',','); %read first line 47 | format = repmat('%s',1,size(headers{1,1},1)); %count columns and make format string 48 | true_pose_times = textscan(fid,format,'delimiter',','); %read rest of the file 49 | true_pose_times = [true_pose_times{:}]; 50 | true_pose_times = [cellfun(@str2num, true_pose_times(:,1:3))]; 51 | 52 | fid = fopen(strcat('data/est_pose', fileNumStr, '.csv')); %open file 53 | headers = fgetl(fid); %get first line 54 | headers = textscan(headers,'%s','delimiter',','); %read first line 55 | format = repmat('%s',1,size(headers{1,1},1)); %count columns and make format string 56 | est_pose_times = textscan(fid,format,'delimiter',','); %read rest of the file 57 | est_pose_times = [est_pose_times{:}]; 58 | est_pose_times = [cellfun(@str2num, est_pose_times(:,1:3))]; 59 | 60 | true_poses = [true_pose_times true_poses]; 61 | true_poses = true_poses(isfinite(true_poses(:,4)),:); 62 | est_poses = [est_pose_times est_poses]; 63 | 64 | % Shows one dimension of both data sets to see how they line up 65 | % figure, plot(1:size(true_poses),true_poses(:,4)) 66 | % figure, plot(1:size(est_poses),est_poses(:,5)) 67 | 68 | % Align times by inspection using plots from above 69 | if fileNum == 3 70 | est_poses(:,3) = est_poses(:,3) - est_poses(137,3); 71 | true_poses(:,3) = true_poses(:,3) - true_poses(2250,3); 72 | end 73 | if fileNum == 2 74 | est_poses(:,3) = est_poses(:,3) - est_poses(214,3); 75 | true_poses(:,3) = true_poses(:,3) - true_poses(2640,3); 76 | end 77 | 78 | true_match_idx = matchnearest(est_poses(:,3), true_poses(:,3)); 79 | 80 | %sample_idx = 60:120; 81 | %sample_idx = 220:300; 82 | sample_idx = 20:450; 83 | cam_est_sample = est_poses(sample_idx,:); 84 | 85 | marker_true_sample = true_poses(true_match_idx(sample_idx),:); 86 | 87 | % These plots should be time aligned 88 | % figure, plot(1:size(marker_true_sample),marker_true_sample(:,4)) 89 | % hold on 90 | % plot(1:size(cam_est_sample),cam_est_sample(:,5), '-g') 91 | % hold off 92 | 93 | 94 | Tmarker_cam = []; 95 | Qmarker_cam = []; 96 | RPYmarker_cam = []; 97 | 98 | est_Topti_cam = []; 99 | est_Zopti_cam = []; 100 | true_Hopti_markers = zeros(4,4,size(sample_idx,2)); 101 | true_rpy = []; 102 | for i=1:size(sample_idx,2) 103 | 104 | est_pos = map_scale*cam_est_sample(i, 5:7); 105 | est_rot = qGetR([cam_est_sample(i, 11) cam_est_sample(i, 8:10)]); 106 | est_Hmap_cam = [est_rot est_pos'; 0 0 0 1]; 107 | 108 | est_Hopti_cam = Hmap_opti \ est_Hmap_cam; 109 | est_Topti_cam = [est_Topti_cam est_Hopti_cam(1:3,4)]; 110 | est_Zopti_cam = [est_Zopti_cam est_Hopti_cam(1:3,1)]; 111 | 112 | %Hopti_cam_quat = qGetQ(est_Hcam_opti(1:3, 1:3)); 113 | %Hopti_cam_rpy = quat2rpy([Hopti_cam_quat(4), Hopti_cam_quat(1:3)'])*180/pi; 114 | 115 | true_pos = marker_true_sample(i, 4:6); 116 | true_rot = qGetR([marker_true_sample(i, 10) marker_true_sample(i, 7:9)]); 117 | true_rpy = [true_rpy quat2rpy([marker_true_sample(i, 10), marker_true_sample(i, 7:9)])']; 118 | %true_quat = marker_true_sample(i, 7:10); 119 | %true_rpy = quat2rpy([true_quat(4), true_quat(1:3)])*180/pi; 120 | 121 | true_Hopti_marker = [true_rot true_pos'; 0 0 0 1]; 122 | true_Hopti_markers(:,:,i) = true_Hopti_marker; 123 | 124 | Hmarker_cam = true_Hopti_marker \ est_Hopti_cam; 125 | 126 | Hmarker_cam_quat = qGetQ(Hmarker_cam(1:3, 1:3)); 127 | Hmarker_cam_rpy = quat2rpy(Hmarker_cam_quat(1:4)')'; 128 | 129 | RPYmarker_cam = [RPYmarker_cam Hmarker_cam_rpy]; 130 | Qmarker_cam = [Qmarker_cam qGetQ(Hmarker_cam(1:3, 1:3))]; 131 | Tmarker_cam = [Tmarker_cam Hmarker_cam(1:3, 4)]; 132 | end 133 | 134 | 135 | mean_Tmarker_cam = mean(Tmarker_cam'); 136 | mean_Qmarker_cam = mean(Qmarker_cam'); 137 | mean_Rmarker_cam = qGetR(mean_Qmarker_cam); 138 | mean_Hmarker_cam = [mean_Rmarker_cam mean_Tmarker_cam'; 0 0 0 1]; 139 | 140 | disp(['Hmarker_cam info:']); 141 | disp(['transalation mean: ', num2str(mean_Tmarker_cam)]); 142 | disp(['translation variance: ', num2str(var(Tmarker_cam'))]); 143 | 144 | disp(['quat mean: ', num2str(mean_Qmarker_cam)]); 145 | disp(['quat variance: ', num2str(var(Qmarker_cam'))]); 146 | 147 | disp(['rpy mean: ', num2str(mean(unwrap(RPYmarker_cam')*180/pi))]); 148 | disp(['rpy variance: ', num2str(var(unwrap(RPYmarker_cam')*180/pi))]); 149 | 150 | 151 | figure 152 | quiver3(est_Topti_cam(1,:), est_Topti_cam(2,:), est_Topti_cam(3,:), est_Zopti_cam(1,:), est_Zopti_cam(2,:), est_Zopti_cam(3,:),'AutoScale','off') 153 | axis([-.5 2 -.5 2 0 2]) 154 | xlabel('x') 155 | ylabel('y') 156 | 157 | 158 | true_Hopti_cams = zeros(4,4,size(sample_idx,2)); 159 | for i=1:size(sample_idx,2) 160 | true_Hopti_cams(:,:,i) = true_Hopti_markers(:,:,i)*calibrated_Hmarker_cam; 161 | end 162 | 163 | true_opti_cams_x = reshape(true_Hopti_cams(1,4,:),[size(true_Hopti_cams,3) 1]); 164 | true_opti_cams_y = reshape(true_Hopti_cams(2,4,:),[size(true_Hopti_cams,3) 1]); 165 | true_opti_cams_z = reshape(true_Hopti_cams(3,4,:),[size(true_Hopti_cams,3) 1]); 166 | 167 | figure, plot(est_Topti_cam(1,:), est_Topti_cam(2,:), '-g') 168 | hold on 169 | plot(true_opti_cams_x,true_opti_cams_y) 170 | 171 | errors = sqrt(sum((est_Topti_cam(1:3,:) - [true_opti_cams_x'; true_opti_cams_y'; true_opti_cams_z']).^2, 1)); 172 | 173 | mean_error = mean(errors); 174 | var_error = var(errors); 175 | 176 | disp(['--------------------------', char(13), 'Error Mean: ', num2str(mean_error)]); 177 | disp(['Error Variance: ', num2str(var_error)]); 178 | 179 | 180 | -------------------------------------------------------------------------------- /analysis/matchnearest.m: -------------------------------------------------------------------------------- 1 | function idx = matchnearest(tt1, tt2, maxdt) 2 | % MATCHNEAREST - Find matching events in two time series 3 | % idx = MATCHNEAREST(tt1, tt2) returns a vector in which the k-th 4 | % element indicates which event in time series TT2 occured most 5 | % closely to the k-th event in time series TT1. 6 | % idx = MATCHNEAREST(tt1, tt2, maxdt) specifies a maximum time interval 7 | % beyond which matches cannot be declared. 8 | % Events that do not have a match result in a zero entry in the IDX. 9 | % Note that this function does not guarantee that the matching is 10 | % one-to-one, i.e., it is possible for more than one event in TT1 to be 11 | % matched against a given event in TT2. See MATCHNEAREST2 if this is 12 | % undesirable. 13 | 14 | if nargin<3 15 | maxdt = inf; 16 | end 17 | 18 | N = length(tt1); 19 | idx = zeros(N,1); 20 | for n=1:N 21 | t0 = tt1(n); 22 | dt = tt2 - t0; 23 | [mindt, id] = min(abs(dt)); 24 | if mindt 0 ) 23 | w = 0; 24 | end 25 | 26 | x = sqrt( 1 + Rxx - Ryy - Rzz ) / 2; 27 | y = sqrt( 1 + Ryy - Rxx - Rzz ) / 2; 28 | z = sqrt( 1 + Rzz - Ryy - Rxx ) / 2; 29 | 30 | [element, i ] = max( [w,x,y,z] ); 31 | 32 | if( i == 1 ) 33 | x = ( Rzy - Ryz ) / (4*w); 34 | y = ( Rxz - Rzx ) / (4*w); 35 | z = ( Ryx - Rxy ) / (4*w); 36 | end 37 | 38 | if( i == 2 ) 39 | w = ( Rzy - Ryz ) / (4*x); 40 | y = ( Rxy + Ryx ) / (4*x); 41 | z = ( Rzx + Rxz ) / (4*x); 42 | end 43 | 44 | if( i == 3 ) 45 | w = ( Rxz - Rzx ) / (4*y); 46 | x = ( Rxy + Ryx ) / (4*y); 47 | z = ( Ryz + Rzy ) / (4*y); 48 | end 49 | 50 | if( i == 4 ) 51 | w = ( Ryx - Rxy ) / (4*z); 52 | x = ( Rzx + Rxz ) / (4*z); 53 | y = ( Ryz + Rzy ) / (4*z); 54 | end 55 | 56 | Q = [ w; x; y; z ]; -------------------------------------------------------------------------------- /analysis/qGetR.m: -------------------------------------------------------------------------------- 1 | function R = qGetR( Qrotation ) 2 | % qGetR: get a 3x3 rotation matrix 3 | % R = qGetR( Qrotation ) 4 | % IN: 5 | % Qrotation - quaternion describing rotation 6 | % 7 | % OUT: 8 | % R - rotation matrix 9 | % 10 | % VERSION: 03.03.2012 11 | 12 | 13 | w = Qrotation( 1 ); 14 | x = Qrotation( 2 ); 15 | y = Qrotation( 3 ); 16 | z = Qrotation( 4 ); 17 | 18 | Rxx = 1 - 2*(y^2 + z^2); 19 | Rxy = 2*(x*y - z*w); 20 | Rxz = 2*(x*z + y*w); 21 | 22 | Ryx = 2*(x*y + z*w); 23 | Ryy = 1 - 2*(x^2 + z^2); 24 | Ryz = 2*(y*z - x*w ); 25 | 26 | Rzx = 2*(x*z - y*w ); 27 | Rzy = 2*(y*z + x*w ); 28 | Rzz = 1 - 2 *(x^2 + y^2); 29 | 30 | R = [ 31 | Rxx, Rxy, Rxz; 32 | Ryx, Ryy, Ryz; 33 | Rzx, Rzy, Rzz]; -------------------------------------------------------------------------------- /analysis/quat2rpy.m: -------------------------------------------------------------------------------- 1 | function rpy = quat2rpy(q) 2 | %Converts the quaternion in the form(q0 + q1 i + q2 j + q3 k into the roll 3 | %pitch yaw (ZYX convention) other conventions can be supported in later 4 | %versions. q is nx4 matrix output in radians 5 | rpy(:,1) = atan2(2*(q(:,1).*q(:,2) +q(:,3).*q(:,4)), 1 - 2*(q(:,2).^2 + q(:,3).^2)); 6 | rpy(:,2) = asin(2*(q(:,1).*q(:,3) -q(:,4).*q(:,2))); 7 | rpy(:,3) = atan2(2*(q(:,1).*q(:,4) + q(:,2).*q(:,3)), 1 - 2*(q(:,3).^2 + q(:,4).^2)); 8 | end -------------------------------------------------------------------------------- /analysis/quatinv.m: -------------------------------------------------------------------------------- 1 | function qinv = quatinv(q) 2 | %Performs Quaternion q; q is a nx4 matrix 3 | %q is of the form: a1 + b1 i + c1 j + d1 k 4 | qinv = [q(:,1) -q(:,2:4)]; 5 | end 6 | -------------------------------------------------------------------------------- /analysis/quatmultiply.m: -------------------------------------------------------------------------------- 1 | function qres = quatmultiply(q1,q2) 2 | %Performs Quaternion multiplication of q1*q2. q1 and q2 are nx4 matrices 3 | %q1 is of the form: a1 + b1 i + c1 j + d1 k 4 | qres(:,1) = q1(:,1).*q2(:,1) - q1(:,2).*q2(:,2) - q1(:,3).*q2(:,3) - q1(:,4).*q2(:,4); 5 | qres(:,2) = q1(:,1).*q2(:,2) + q1(:,2).*q2(:,1) + q1(:,3).*q2(:,4) - q1(:,4).*q2(:,3); 6 | qres(:,3) = q1(:,1).*q2(:,3) - q1(:,2).*q2(:,4) + q1(:,3).*q2(:,1) + q1(:,4).*q2(:,2); 7 | qres(:,4) = q1(:,1).*q2(:,4) + q1(:,2).*q2(:,3) - q1(:,3).*q2(:,2) + q1(:,4).*q2(:,1); 8 | end 9 | -------------------------------------------------------------------------------- /analysis/quatrotate.m: -------------------------------------------------------------------------------- 1 | function vecres = quatrotate(q,vec) 2 | sizevec = size(vec,1);%Number of rows 3 | qvec = [zeros(sizevec,1) vec]; 4 | vecres = quatmultiply(q, quatmultiply(qvec, quatinv(q))); 5 | vecres(:,1) = []; 6 | end 7 | -------------------------------------------------------------------------------- /analysis/rpy2quat.m: -------------------------------------------------------------------------------- 1 | function q = rpy2quat(rpy) 2 | %Converts rpy body 321 sequence (yaw pitch roll) to quaternion 3 | %q = q0 + q1 i + q2 j + q3 k 4 | r = rpy(3); 5 | p = rpy(2); 6 | y = rpy(1); 7 | q = [cos(r/2)*cos(p/2)*cos(y/2) + sin(r/2)*sin(p/2)*sin(y/2); 8 | sin(r/2)*cos(p/2)*cos(y/2) - cos(r/2)*sin(p/2)*sin(y/2); 9 | cos(r/2)*sin(p/2)*cos(y/2) + sin(r/2)*cos(p/2)*sin(y/2); 10 | cos(r/2)*cos(p/2)*sin(y/2) - sin(r/2)*sin(p/2)*cos(y/2)]; 11 | end -------------------------------------------------------------------------------- /bin/map.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/msheckells/mesh_localize/64b1251794337cfff23ae1e81d36daf0ba77f812/bin/map.stl -------------------------------------------------------------------------------- /cmake/Modules/FindGCOP.cmake: -------------------------------------------------------------------------------- 1 | include(LibFindMacros) 2 | 3 | # Dependencies 4 | #libfind_package(gcop) 5 | unset(GCOP_LIBRARY CACHE) 6 | 7 | # Use pkg-config to get hints about paths 8 | libfind_pkg_check_modules(GCOP_PKGCONF gcop) 9 | 10 | # Include dir 11 | find_path(GCOP_INCLUDE_DIR 12 | NAMES gcop/system.h 13 | PATHS ${GCOP_PKGCONF_INCLUDE_DIRS} 14 | ) 15 | 16 | # Finally the library itself 17 | if( GCOP_FIND_COMPONENTS )# If no components specified manually specifiy all the components 18 | else() 19 | set(GCOP_FIND_COMPONENTS algos views systems est) 20 | endif() 21 | 22 | foreach(component ${GCOP_FIND_COMPONENTS}) 23 | find_library(GCOP_LIB_${component} 24 | NAMES gcop_${component} 25 | ) 26 | set(GCOP_LIBRARY "${GCOP_LIBRARY};${GCOP_LIB_${component}}") 27 | endforeach(component) 28 | # Utils is common to all 29 | find_library(GCOP_LIB_utils 30 | NAMES gcop_utils 31 | ) 32 | set(GCOP_LIBRARY "${GCOP_LIBRARY};${GCOP_LIB_utils}") 33 | # Set the include dir variables and the libraries and let libfind_process do the rest. 34 | # NOTE: Singular variables for this library, plural for libraries this this lib depends on. 35 | set(GCOP_PROCESS_INCLUDES GCOP_INCLUDE_DIR) 36 | set(GCOP_PROCESS_LIBS GCOP_LIBRARY) 37 | libfind_process(GCOP) 38 | 39 | -------------------------------------------------------------------------------- /cmake/Modules/FindOIS.cmake: -------------------------------------------------------------------------------- 1 | #------------------------------------------------------------------- 2 | # This file is part of the CMake build system for OGRE 3 | # (Object-oriented Graphics Rendering Engine) 4 | # For the latest info, see http://www.ogre3d.org/ 5 | # 6 | # The contents of this file are placed in the public domain. Feel 7 | # free to make use of it in any way you like. 8 | #------------------------------------------------------------------- 9 | 10 | # - Try to find OIS 11 | # Once done, this will define 12 | # 13 | # OIS_FOUND - system has OIS 14 | # OIS_INCLUDE_DIRS - the OIS include directories 15 | # OIS_LIBRARIES - link these to use OIS 16 | # OIS_BINARY_REL / OIS_BINARY_DBG - DLL names (windows only) 17 | 18 | include(FindPkgMacros) 19 | findpkg_begin(OIS) 20 | 21 | # Get path, convert backslashes as ${ENV_${var}} 22 | getenv_path(OIS_HOME) 23 | getenv_path(OGRE_SDK) 24 | getenv_path(OGRE_HOME) 25 | getenv_path(OGRE_SOURCE) 26 | getenv_path(OGRE_DEPENDENCIES_DIR) 27 | 28 | # construct search paths 29 | set(OIS_PREFIX_PATH ${OIS_HOME} ${ENV_OIS_HOME} 30 | ${OGRE_DEPENDENCIES_DIR} ${ENV_OGRE_DEPENDENCIES_DIR} 31 | ${OGRE_SOURCE}/iOSDependencies ${ENV_OGRE_SOURCE}/iOSDependencies 32 | ${OGRE_SOURCE}/Dependencies ${ENV_OGRE_SOURCE}/Dependencies 33 | ${OGRE_SDK} ${ENV_OGRE_SDK} 34 | ${OGRE_HOME} ${ENV_OGRE_HOME}) 35 | create_search_paths(OIS) 36 | # redo search if prefix path changed 37 | clear_if_changed(OIS_PREFIX_PATH 38 | OIS_LIBRARY_FWK 39 | OIS_LIBRARY_REL 40 | OIS_LIBRARY_DBG 41 | OIS_INCLUDE_DIR 42 | ) 43 | 44 | set(OIS_LIBRARY_NAMES OIS) 45 | get_debug_names(OIS_LIBRARY_NAMES) 46 | 47 | use_pkgconfig(OIS_PKGC OIS) 48 | 49 | # For OIS, prefer static library over framework (important when referencing OIS source build) 50 | set(CMAKE_FIND_FRAMEWORK "LAST") 51 | 52 | findpkg_framework(OIS) 53 | if (OIS_HOME) 54 | # OIS uses the 'includes' path for its headers in the source release, not 'include' 55 | set(OIS_INC_SEARCH_PATH ${OIS_INC_SEARCH_PATH} ${OIS_HOME}/includes) 56 | endif() 57 | if (APPLE AND OIS_HOME) 58 | # OIS source build on Mac stores libs in a different location 59 | # Also this is for static build 60 | set(OIS_LIB_SEARCH_PATH ${OIS_LIB_SEARCH_PATH} ${OIS_HOME}/Mac/XCode-2.2/build) 61 | endif() 62 | find_path(OIS_INCLUDE_DIR NAMES OIS.h HINTS ${OIS_INC_SEARCH_PATH} ${OIS_PKGC_INCLUDE_DIRS} PATH_SUFFIXES OIS) 63 | find_library(OIS_LIBRARY_REL NAMES ${OIS_LIBRARY_NAMES} HINTS ${OIS_LIB_SEARCH_PATH} ${OIS_PKGC_LIBRARY_DIRS} PATH_SUFFIXES "" release relwithdebinfo minsizerel) 64 | find_library(OIS_LIBRARY_DBG NAMES ${OIS_LIBRARY_NAMES_DBG} HINTS ${OIS_LIB_SEARCH_PATH} ${OIS_PKGC_LIBRARY_DIRS} PATH_SUFFIXES "" debug) 65 | 66 | make_library_set(OIS_LIBRARY) 67 | 68 | if (WIN32) 69 | set(OIS_BIN_SEARCH_PATH ${OIS_HOME}/dll ${ENV_OIS_HOME}/dll 70 | ${OGRE_DEPENDENCIES_DIR}/bin ${ENV_OGRE_DEPENDENCIES_DIR}/bin 71 | ${OGRE_SOURCE}/Dependencies/bin ${ENV_OGRE_SOURCE}/Dependencies/bin 72 | ${OGRE_SDK}/bin ${ENV_OGRE_SDK}/bin 73 | ${OGRE_HOME}/bin ${ENV_OGRE_HOME}/bin) 74 | find_file(OIS_BINARY_REL NAMES "OIS.dll" HINTS ${OIS_BIN_SEARCH_PATH} 75 | PATH_SUFFIXES "" release relwithdebinfo minsizerel) 76 | find_file(OIS_BINARY_DBG NAMES "OIS_d.dll" HINTS ${OIS_BIN_SEARCH_PATH} 77 | PATH_SUFFIXES "" debug ) 78 | endif() 79 | mark_as_advanced(OIS_BINARY_REL OIS_BINARY_DBG) 80 | 81 | 82 | findpkg_finish(OIS) 83 | 84 | # add parent of OIS folder to support OIS/OIS.h 85 | add_parent_dir(OIS_INCLUDE_DIRS OIS_INCLUDE_DIR) 86 | 87 | # Reset framework finding 88 | set(CMAKE_FIND_FRAMEWORK "FIRST") 89 | -------------------------------------------------------------------------------- /cmake/Modules/FindObjectRenderer.cmake: -------------------------------------------------------------------------------- 1 | include(LibFindMacros) 2 | unset(OBJECT_RENDERER_LIBS CACHE) 3 | 4 | # Dependencies 5 | #libfind_package(gcop) 6 | 7 | # Use pkg-config to get hints about paths 8 | libfind_pkg_check_modules(OBJECT_RENDERER_PKGCONF object_renderer) 9 | 10 | # Include dir 11 | find_path(OBJECT_RENDERER_INCLUDE_DIR 12 | NAMES object_renderer/ogre_application.h 13 | PATHS ${OBJECT_RENDERER_PKGCONF_INCLUDE_DIRS} 14 | ) 15 | 16 | find_library(OBJECT_RENDERER_LIB_ogre_render 17 | NAMES ogre_render 18 | PATHS ${OBJECT_RENDERER_PKGCONF_LIBRARY_DIRS} 19 | ) 20 | find_library(OBJECT_RENDERER_LIB_virtual_image_proc 21 | NAMES virtual_image_proc 22 | PATHS ${OBJECT_RENDERER_PKGCONF_LIBRARY_DIRS} 23 | ) 24 | set(OBJECT_RENDERER_LIBS "${OBJECT_RENDERER_LIB_ogre_render};${OBJECT_RENDERER_LIB_virtual_image_proc}") 25 | 26 | # Set the include dir variables and the libraries and let libfind_process do the rest. 27 | # NOTE: Singular variables for this library, plural for libraries this this lib depends on. 28 | set(OBJECT_RENDERER_PROCESS_INCLUDES OBJECT_RENDERER_INCLUDE_DIR) 29 | set(OBJECT_RENDERER_PROCESS_LIBS OBJECT_RENDERER_LIBS) 30 | libfind_process(OBJECT_RENDERER) 31 | 32 | -------------------------------------------------------------------------------- /cmake/Modules/FindPkgMacros.cmake: -------------------------------------------------------------------------------- 1 | #------------------------------------------------------------------- 2 | # This file is part of the CMake build system for OGRE 3 | # (Object-oriented Graphics Rendering Engine) 4 | # For the latest info, see http://www.ogre3d.org/ 5 | # 6 | # The contents of this file are placed in the public domain. Feel 7 | # free to make use of it in any way you like. 8 | #------------------------------------------------------------------- 9 | 10 | ################################################################## 11 | # Provides some common functionality for the FindPackage modules 12 | ################################################################## 13 | 14 | # Begin processing of package 15 | macro(findpkg_begin PREFIX) 16 | if (NOT ${PREFIX}_FIND_QUIETLY) 17 | message(STATUS "Looking for ${PREFIX}...") 18 | endif () 19 | endmacro(findpkg_begin) 20 | 21 | # Display a status message unless FIND_QUIETLY is set 22 | macro(pkg_message PREFIX) 23 | if (NOT ${PREFIX}_FIND_QUIETLY) 24 | message(STATUS ${ARGN}) 25 | endif () 26 | endmacro(pkg_message) 27 | 28 | # Get environment variable, define it as ENV_$var and make sure backslashes are converted to forward slashes 29 | macro(getenv_path VAR) 30 | set(ENV_${VAR} $ENV{${VAR}}) 31 | # replace won't work if var is blank 32 | if (ENV_${VAR}) 33 | string( REGEX REPLACE "\\\\" "/" ENV_${VAR} ${ENV_${VAR}} ) 34 | endif () 35 | endmacro(getenv_path) 36 | 37 | # Construct search paths for includes and libraries from a PREFIX_PATH 38 | macro(create_search_paths PREFIX) 39 | foreach(dir ${${PREFIX}_PREFIX_PATH}) 40 | set(${PREFIX}_INC_SEARCH_PATH ${${PREFIX}_INC_SEARCH_PATH} 41 | ${dir}/include ${dir}/Include ${dir}/include/${PREFIX} ${dir}/Headers) 42 | set(${PREFIX}_LIB_SEARCH_PATH ${${PREFIX}_LIB_SEARCH_PATH} 43 | ${dir}/lib ${dir}/Lib ${dir}/lib/${PREFIX} ${dir}/Libs) 44 | set(${PREFIX}_BIN_SEARCH_PATH ${${PREFIX}_BIN_SEARCH_PATH} 45 | ${dir}/bin) 46 | endforeach(dir) 47 | set(${PREFIX}_FRAMEWORK_SEARCH_PATH ${${PREFIX}_PREFIX_PATH}) 48 | endmacro(create_search_paths) 49 | 50 | # clear cache variables if a certain variable changed 51 | macro(clear_if_changed TESTVAR) 52 | # test against internal check variable 53 | # HACK: Apparently, adding a variable to the cache cleans up the list 54 | # a bit. We need to also remove any empty strings from the list, but 55 | # at the same time ensure that we are actually dealing with a list. 56 | list(APPEND ${TESTVAR} "") 57 | list(REMOVE_ITEM ${TESTVAR} "") 58 | if (NOT "${${TESTVAR}}" STREQUAL "${${TESTVAR}_INT_CHECK}") 59 | message(STATUS "${TESTVAR} changed.") 60 | foreach(var ${ARGN}) 61 | set(${var} "NOTFOUND" CACHE STRING "x" FORCE) 62 | endforeach(var) 63 | endif () 64 | set(${TESTVAR}_INT_CHECK ${${TESTVAR}} CACHE INTERNAL "x" FORCE) 65 | endmacro(clear_if_changed) 66 | 67 | # Try to get some hints from pkg-config, if available 68 | macro(use_pkgconfig PREFIX PKGNAME) 69 | find_package(PkgConfig) 70 | if (PKG_CONFIG_FOUND) 71 | pkg_check_modules(${PREFIX} ${PKGNAME}) 72 | endif () 73 | endmacro (use_pkgconfig) 74 | 75 | # Couple a set of release AND debug libraries (or frameworks) 76 | macro(make_library_set PREFIX) 77 | if (${PREFIX}_FWK) 78 | set(${PREFIX} ${${PREFIX}_FWK}) 79 | elseif (${PREFIX}_REL AND ${PREFIX}_DBG) 80 | set(${PREFIX} optimized ${${PREFIX}_REL} debug ${${PREFIX}_DBG}) 81 | elseif (${PREFIX}_REL) 82 | set(${PREFIX} ${${PREFIX}_REL}) 83 | elseif (${PREFIX}_DBG) 84 | set(${PREFIX} ${${PREFIX}_DBG}) 85 | endif () 86 | endmacro(make_library_set) 87 | 88 | # Generate debug names from given release names 89 | macro(get_debug_names PREFIX) 90 | foreach(i ${${PREFIX}}) 91 | set(${PREFIX}_DBG ${${PREFIX}_DBG} ${i}d ${i}D ${i}_d ${i}_D ${i}_debug ${i}) 92 | endforeach(i) 93 | endmacro(get_debug_names) 94 | 95 | # Add the parent dir from DIR to VAR 96 | macro(add_parent_dir VAR DIR) 97 | get_filename_component(${DIR}_TEMP "${${DIR}}/.." ABSOLUTE) 98 | set(${VAR} ${${VAR}} ${${DIR}_TEMP}) 99 | endmacro(add_parent_dir) 100 | 101 | # Do the final processing for the package find. 102 | macro(findpkg_finish PREFIX) 103 | # skip if already processed during this run 104 | if (NOT ${PREFIX}_FOUND) 105 | if (${PREFIX}_INCLUDE_DIR AND ${PREFIX}_LIBRARY) 106 | set(${PREFIX}_FOUND TRUE) 107 | set(${PREFIX}_INCLUDE_DIRS ${${PREFIX}_INCLUDE_DIR}) 108 | set(${PREFIX}_LIBRARIES ${${PREFIX}_LIBRARY}) 109 | if (NOT ${PREFIX}_FIND_QUIETLY) 110 | message(STATUS "Found ${PREFIX}: ${${PREFIX}_LIBRARIES}") 111 | endif () 112 | else () 113 | if (NOT ${PREFIX}_FIND_QUIETLY) 114 | message(STATUS "Could not locate ${PREFIX}") 115 | endif () 116 | if (${PREFIX}_FIND_REQUIRED) 117 | message(FATAL_ERROR "Required library ${PREFIX} not found! Install the library (including dev packages) and try again. If the library is already installed, set the missing variables manually in cmake.") 118 | endif () 119 | endif () 120 | 121 | mark_as_advanced(${PREFIX}_INCLUDE_DIR ${PREFIX}_LIBRARY ${PREFIX}_LIBRARY_REL ${PREFIX}_LIBRARY_DBG ${PREFIX}_LIBRARY_FWK) 122 | endif () 123 | endmacro(findpkg_finish) 124 | 125 | 126 | # Slightly customised framework finder 127 | macro(findpkg_framework fwk) 128 | if(APPLE) 129 | set(${fwk}_FRAMEWORK_PATH 130 | ${${fwk}_FRAMEWORK_SEARCH_PATH} 131 | ${CMAKE_FRAMEWORK_PATH} 132 | ~/Library/Frameworks 133 | /Library/Frameworks 134 | /System/Library/Frameworks 135 | /Network/Library/Frameworks 136 | ${CMAKE_CURRENT_SOURCE_DIR}/lib/Release 137 | ${CMAKE_CURRENT_SOURCE_DIR}/lib/Debug 138 | ) 139 | # These could be arrays of paths, add each individually to the search paths 140 | foreach(i ${OGRE_PREFIX_PATH}) 141 | set(${fwk}_FRAMEWORK_PATH ${${fwk}_FRAMEWORK_PATH} ${i}/lib/Release ${i}/lib/Debug) 142 | endforeach(i) 143 | 144 | foreach(i ${OGRE_PREFIX_BUILD}) 145 | set(${fwk}_FRAMEWORK_PATH ${${fwk}_FRAMEWORK_PATH} ${i}/lib/Release ${i}/lib/Debug) 146 | endforeach(i) 147 | 148 | foreach(dir ${${fwk}_FRAMEWORK_PATH}) 149 | set(fwkpath ${dir}/${fwk}.framework) 150 | if(EXISTS ${fwkpath}) 151 | set(${fwk}_FRAMEWORK_INCLUDES ${${fwk}_FRAMEWORK_INCLUDES} 152 | ${fwkpath}/Headers ${fwkpath}/PrivateHeaders) 153 | set(${fwk}_FRAMEWORK_PATH ${dir}) 154 | if (NOT ${fwk}_LIBRARY_FWK) 155 | set(${fwk}_LIBRARY_FWK "-framework ${fwk}") 156 | endif () 157 | endif(EXISTS ${fwkpath}) 158 | endforeach(dir) 159 | endif(APPLE) 160 | endmacro(findpkg_framework) 161 | -------------------------------------------------------------------------------- /cmake/Modules/FindTinyXML.cmake: -------------------------------------------------------------------------------- 1 | ################################################################################################## 2 | # 3 | # CMake script for finding TinyXML. 4 | # 5 | # Input variables: 6 | # 7 | # - TinyXML_ROOT_DIR (optional): When specified, header files and libraries will be searched for in 8 | # ${TinyXML_ROOT_DIR}/include 9 | # ${TinyXML_ROOT_DIR}/libs 10 | # respectively, and the default CMake search order will be ignored. When unspecified, the default 11 | # CMake search order is used. 12 | # This variable can be specified either as a CMake or environment variable. If both are set, 13 | # preference is given to the CMake variable. 14 | # Use this variable for finding packages installed in a nonstandard location, or for enforcing 15 | # that one of multiple package installations is picked up. 16 | # 17 | # 18 | # Cache variables (not intended to be used in CMakeLists.txt files) 19 | # 20 | # - TinyXML_INCLUDE_DIR: Absolute path to package headers. 21 | # - TinyXML_LIBRARY: Absolute path to library. 22 | # 23 | # 24 | # Output variables: 25 | # 26 | # - TinyXML_FOUND: Boolean that indicates if the package was found 27 | # - TinyXML_INCLUDE_DIRS: Paths to the necessary header files 28 | # - TinyXML_LIBRARIES: Package libraries 29 | # 30 | # 31 | # Example usage: 32 | # 33 | # find_package(TinyXML) 34 | # if(NOT TinyXML_FOUND) 35 | # # Error handling 36 | # endif() 37 | # ... 38 | # include_directories(${TinyXML_INCLUDE_DIRS} ...) 39 | # ... 40 | # target_link_libraries(my_target ${TinyXML_LIBRARIES}) 41 | # 42 | ################################################################################################## 43 | 44 | # Get package location hint from environment variable (if any) 45 | if(NOT TinyXML_ROOT_DIR AND DEFINED ENV{TinyXML_ROOT_DIR}) 46 | set(TinyXML_ROOT_DIR "$ENV{TinyXML_ROOT_DIR}" CACHE PATH 47 | "TinyXML base directory location (optional, used for nonstandard installation paths)") 48 | endif() 49 | 50 | # Search path for nonstandard package locations 51 | if(TinyXML_ROOT_DIR) 52 | set(TinyXML_INCLUDE_PATH PATHS "${TinyXML_ROOT_DIR}/include" NO_DEFAULT_PATH) 53 | set(TinyXML_LIBRARY_PATH PATHS "${TinyXML_ROOT_DIR}/lib" NO_DEFAULT_PATH) 54 | endif() 55 | 56 | # Find headers and libraries 57 | find_path(TinyXML_INCLUDE_DIR NAMES tinyxml.h PATH_SUFFIXES "tinyxml" ${TinyXML_INCLUDE_PATH}) 58 | find_library(TinyXML_LIBRARY NAMES tinyxml PATH_SUFFIXES "tinyxml" ${TinyXML_LIBRARY_PATH}) 59 | 60 | mark_as_advanced(TinyXML_INCLUDE_DIR 61 | TinyXML_LIBRARY) 62 | 63 | # Output variables generation 64 | include(FindPackageHandleStandardArgs) 65 | find_package_handle_standard_args(TinyXML DEFAULT_MSG TinyXML_LIBRARY 66 | TinyXML_INCLUDE_DIR) 67 | 68 | set(TinyXML_FOUND ${TINYXML_FOUND}) # Enforce case-correctness: Set appropriately cased variable... 69 | unset(TINYXML_FOUND) # ...and unset uppercase variable generated by find_package_handle_standard_args 70 | 71 | if(TinyXML_FOUND) 72 | set(TinyXML_INCLUDE_DIRS ${TinyXML_INCLUDE_DIR}) 73 | set(TinyXML_LIBRARIES ${TinyXML_LIBRARY}) 74 | endif() 75 | 76 | -------------------------------------------------------------------------------- /cmake/Modules/LibFindMacros.cmake: -------------------------------------------------------------------------------- 1 | # Works the same as find_package, but forwards the "REQUIRED" and "QUIET" arguments 2 | # used for the current package. For this to work, the first parameter must be the 3 | # prefix of the current package, then the prefix of the new package etc, which are 4 | # passed to find_package. 5 | macro (libfind_package PREFIX) 6 | set (LIBFIND_PACKAGE_ARGS ${ARGN}) 7 | if (${PREFIX}_FIND_QUIETLY) 8 | set (LIBFIND_PACKAGE_ARGS ${LIBFIND_PACKAGE_ARGS} QUIET) 9 | endif (${PREFIX}_FIND_QUIETLY) 10 | if (${PREFIX}_FIND_REQUIRED) 11 | set (LIBFIND_PACKAGE_ARGS ${LIBFIND_PACKAGE_ARGS} REQUIRED) 12 | endif (${PREFIX}_FIND_REQUIRED) 13 | find_package(${LIBFIND_PACKAGE_ARGS}) 14 | endmacro (libfind_package) 15 | 16 | # CMake developers made the UsePkgConfig system deprecated in the same release (2.6) 17 | # where they added pkg_check_modules. Consequently I need to support both in my scripts 18 | # to avoid those deprecated warnings. Here's a helper that does just that. 19 | # Works identically to pkg_check_modules, except that no checks are needed prior to use. 20 | macro (libfind_pkg_check_modules PREFIX PKGNAME) 21 | if (${CMAKE_MAJOR_VERSION} EQUAL 2 AND ${CMAKE_MINOR_VERSION} EQUAL 4) 22 | include(UsePkgConfig) 23 | pkgconfig(${PKGNAME} ${PREFIX}_INCLUDE_DIRS ${PREFIX}_LIBRARY_DIRS ${PREFIX}_LDFLAGS ${PREFIX}_CFLAGS) 24 | else (${CMAKE_MAJOR_VERSION} EQUAL 2 AND ${CMAKE_MINOR_VERSION} EQUAL 4) 25 | find_package(PkgConfig) 26 | if (PKG_CONFIG_FOUND) 27 | pkg_check_modules(${PREFIX} ${PKGNAME}) 28 | endif (PKG_CONFIG_FOUND) 29 | endif (${CMAKE_MAJOR_VERSION} EQUAL 2 AND ${CMAKE_MINOR_VERSION} EQUAL 4) 30 | endmacro (libfind_pkg_check_modules) 31 | 32 | # Do the final processing once the paths have been detected. 33 | # If include dirs are needed, ${PREFIX}_PROCESS_INCLUDES should be set to contain 34 | # all the variables, each of which contain one include directory. 35 | # Ditto for ${PREFIX}_PROCESS_LIBS and library files. 36 | # Will set ${PREFIX}_FOUND, ${PREFIX}_INCLUDE_DIRS and ${PREFIX}_LIBRARIES. 37 | # Also handles errors in case library detection was required, etc. 38 | macro (libfind_process PREFIX) 39 | # Skip processing if already processed during this run 40 | if (NOT ${PREFIX}_FOUND) 41 | # Start with the assumption that the library was found 42 | set (${PREFIX}_FOUND TRUE) 43 | 44 | # Process all includes and set _FOUND to false if any are missing 45 | foreach (i ${${PREFIX}_PROCESS_INCLUDES}) 46 | if (${i}) 47 | set (${PREFIX}_INCLUDE_DIRS ${${PREFIX}_INCLUDE_DIRS} ${${i}}) 48 | mark_as_advanced(${i}) 49 | else (${i}) 50 | set (${PREFIX}_FOUND FALSE) 51 | endif (${i}) 52 | endforeach (i) 53 | 54 | # Process all libraries and set _FOUND to false if any are missing 55 | foreach (i ${${PREFIX}_PROCESS_LIBS}) 56 | if (${i}) 57 | set (${PREFIX}_LIBRARIES ${${PREFIX}_LIBRARIES} ${${i}}) 58 | mark_as_advanced(${i}) 59 | else (${i}) 60 | set (${PREFIX}_FOUND FALSE) 61 | endif (${i}) 62 | endforeach (i) 63 | 64 | # Print message and/or exit on fatal error 65 | if (${PREFIX}_FOUND) 66 | if (NOT ${PREFIX}_FIND_QUIETLY) 67 | message (STATUS "Found ${PREFIX} ${${PREFIX}_VERSION}") 68 | endif (NOT ${PREFIX}_FIND_QUIETLY) 69 | else (${PREFIX}_FOUND) 70 | if (${PREFIX}_FIND_REQUIRED) 71 | foreach (i ${${PREFIX}_PROCESS_INCLUDES} ${${PREFIX}_PROCESS_LIBS}) 72 | message("${i}=${${i}}") 73 | endforeach (i) 74 | message (FATAL_ERROR "Required library ${PREFIX} NOT FOUND.\nInstall the library (dev version) and try again. If the library is already installed, use ccmake to set the missing variables manually.") 75 | endif (${PREFIX}_FIND_REQUIRED) 76 | endif (${PREFIX}_FOUND) 77 | endif (NOT ${PREFIX}_FOUND) 78 | endmacro (libfind_process) 79 | 80 | macro(libfind_library PREFIX basename) 81 | set(TMP "") 82 | if(MSVC80) 83 | set(TMP -vc80) 84 | endif(MSVC80) 85 | if(MSVC90) 86 | set(TMP -vc90) 87 | endif(MSVC90) 88 | set(${PREFIX}_LIBNAMES ${basename}${TMP}) 89 | if(${ARGC} GREATER 2) 90 | set(${PREFIX}_LIBNAMES ${basename}${TMP}-${ARGV2}) 91 | string(REGEX REPLACE "\\." "_" TMP ${${PREFIX}_LIBNAMES}) 92 | set(${PREFIX}_LIBNAMES ${${PREFIX}_LIBNAMES} ${TMP}) 93 | endif(${ARGC} GREATER 2) 94 | find_library(${PREFIX}_LIBRARY 95 | NAMES ${${PREFIX}_LIBNAMES} 96 | PATHS ${${PREFIX}_PKGCONF_LIBRARY_DIRS} 97 | ) 98 | endmacro(libfind_library) 99 | 100 | -------------------------------------------------------------------------------- /cmake/Modules/PreprocessorUtils.cmake: -------------------------------------------------------------------------------- 1 | #------------------------------------------------------------------- 2 | # This file is part of the CMake build system for OGRE 3 | # (Object-oriented Graphics Rendering Engine) 4 | # For the latest info, see http://www.ogre3d.org/ 5 | # 6 | # The contents of this file are placed in the public domain. Feel 7 | # free to make use of it in any way you like. 8 | #------------------------------------------------------------------- 9 | 10 | macro(get_preprocessor_entry CONTENTS KEYWORD VARIABLE) 11 | string(REGEX MATCH 12 | "# *define +${KEYWORD} +((\"([^\n]*)\")|([^ \n]*))" 13 | PREPROC_TEMP_VAR 14 | ${${CONTENTS}} 15 | ) 16 | if (CMAKE_MATCH_3) 17 | set(${VARIABLE} ${CMAKE_MATCH_3}) 18 | else () 19 | set(${VARIABLE} ${CMAKE_MATCH_4}) 20 | endif () 21 | endmacro() 22 | 23 | macro(has_preprocessor_entry CONTENTS KEYWORD VARIABLE) 24 | string(REGEX MATCH 25 | "\n *# *define +(${KEYWORD})" 26 | PREPROC_TEMP_VAR 27 | ${${CONTENTS}} 28 | ) 29 | if (CMAKE_MATCH_1) 30 | set(${VARIABLE} TRUE) 31 | else () 32 | set(${VARIABLE} FALSE) 33 | endif () 34 | endmacro() 35 | 36 | macro(replace_preprocessor_entry VARIABLE KEYWORD NEW_VALUE) 37 | string(REGEX REPLACE 38 | "(// *)?# *define +${KEYWORD} +[^ \n]*" 39 | "#define ${KEYWORD} ${NEW_VALUE}" 40 | ${VARIABLE}_TEMP 41 | ${${VARIABLE}} 42 | ) 43 | set(${VARIABLE} ${${VARIABLE}_TEMP}) 44 | endmacro() 45 | 46 | macro(set_preprocessor_entry VARIABLE KEYWORD ENABLE) 47 | if (${ENABLE}) 48 | set(TMP_REPLACE_STR "#define ${KEYWORD}") 49 | else () 50 | set(TMP_REPLACE_STR "// #define ${KEYWORD}") 51 | endif () 52 | string(REGEX REPLACE 53 | "(// *)?# *define +${KEYWORD} *\n" 54 | ${TMP_REPLACE_STR} 55 | ${VARIABLE}_TEMP 56 | ${${VARIABLE}} 57 | ) 58 | set(${VARIABLE} ${${VARIABLE}_TEMP}) 59 | endmacro() 60 | 61 | -------------------------------------------------------------------------------- /data/cheezit/descriptors002.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 7 5 | 64 6 |
f
7 | 8 | 0. 0. 0. 0. 0. 0. 0. 0. 4.45474088e-02 3.88100138e-03 6.88906163e-02 9 | 1.69065371e-02 -5.42266236e-04 -7.09195330e-04 7.35539477e-04 10 | 2.00738711e-03 0. 0. 0. 0. 0. 0. 0. 0. 3.24785620e-01 5.02791163e-03 11 | 6.42183185e-01 1.15643539e-01 -4.47771931e-03 -2.85543455e-03 12 | 5.03672706e-03 6.14211475e-03 0. 0. 0. 0. 0. 0. 0. 0. 3.35196465e-01 13 | 2.51896456e-02 5.76384962e-01 9.34740081e-02 -5.90676954e-03 14 | 1.57441525e-03 8.77326261e-03 4.49703401e-03 0. 0. 0. 0. 0. 0. 0. 0. 15 | 5.01921177e-02 -1.61464177e-02 6.44690841e-02 2.30466295e-02 16 | -4.08572145e-03 1.33136206e-03 4.23562061e-03 1.35740242e-03 0. 0. 17 | 0. 0. 0. 0. 0. 0. 4.47841622e-02 1.39758354e-02 6.43217787e-02 18 | 1.58292893e-02 -1.87283021e-03 -2.34348990e-05 2.09560874e-03 19 | 9.14619071e-04 0. 0. 0. 0. 0. 0. 0. 0. 3.30312431e-01 2.22856272e-03 20 | 6.22525036e-01 1.17538415e-01 -1.10007636e-02 -4.26421547e-03 21 | 1.14629632e-02 7.25619495e-03 0. 0. 0. 0. 0. 0. 0. 0. 3.61380368e-01 22 | 3.45313735e-02 5.68321645e-01 1.36298731e-01 -2.12052241e-02 23 | 2.57646968e-03 2.12250240e-02 3.54862306e-03 0. 0. 0. 0. 0. 0. 0. 0. 24 | 5.55683896e-02 2.61049950e-04 7.03615397e-02 1.55966813e-02 25 | -5.36366412e-03 4.71691194e-04 5.42678777e-03 6.25771412e-04 0. 0. 26 | 0. 0. 0. 0. 0. 0. 4.39357646e-02 1.77513901e-02 8.60911682e-02 27 | 2.51459051e-02 2.64703063e-04 -5.13419836e-05 2.64703063e-04 28 | 1.02556380e-03 0. 0. 0. 0. 0. 0. 0. 0. 3.02636713e-01 29 | -7.14957789e-02 4.97276694e-01 9.13641229e-02 0. 1.32089434e-03 0. 30 | 1.32089434e-03 0. 0. 0. 0. 0. 0. 0. 0. 3.78734022e-01 1.11200556e-01 31 | 6.65402114e-01 1.58399358e-01 -1.97514286e-03 5.57383231e-04 32 | 1.97514286e-03 1.19902547e-02 0. 0. 0. 0. 0. 0. 0. 0. 5.85669354e-02 33 | -9.02225263e-03 9.91630331e-02 1.47552751e-02 -3.63081170e-04 34 | 7.62959826e-04 3.63081170e-04 1.76824210e-03 0. 0. 0. 0. 0. 0. 0. 0. 35 | 5.13345934e-02 6.93057152e-03 1.19843490e-01 1.61249060e-02 36 | -4.52122651e-03 -5.30952588e-04 4.70027421e-03 6.86773157e-04 0. 0. 37 | 0. 0. 0. 0. 0. 0. 3.20768237e-01 -5.91619350e-02 6.93289936e-01 38 | 1.51346549e-01 -1.50820129e-02 -6.79606805e-04 1.55195938e-02 39 | 2.62728310e-03 0. 0. 0. 0. 0. 0. 0. 0. 2.89983362e-01 2.53086090e-02 40 | 5.19639313e-01 7.39694759e-02 -5.47606777e-03 9.68541892e-04 41 | 5.85106807e-03 2.90003186e-03 0. 0. 0. 0. 9.38982557e-05 42 | 5.00736460e-05 9.38982557e-05 5.00736460e-05 6.59721419e-02 43 | 2.84480546e-02 7.52850473e-02 3.07315718e-02 -6.18685829e-03 44 | 5.19622397e-03 6.23433385e-03 6.86944881e-03 0. 0. 0. 0. 0. 0. 0. 0. 45 | 4.44728136e-02 6.76795561e-03 8.12976360e-02 1.02559933e-02 46 | -1.84291464e-04 -7.72381900e-04 1.88301623e-04 1.51441770e-03 0. 0. 47 | 0. 0. 0. 0. 0. 0. 3.21482509e-01 5.76982228e-03 5.89866579e-01 48 | 1.06883660e-01 -4.69614146e-03 1.39875585e-04 5.80532197e-03 49 | 4.92106378e-03 0. 0. 0. 0. 0. 0. 0. 0. 3.40163171e-01 1.36627606e-03 50 | 6.28054559e-01 1.10138811e-01 -9.20754299e-03 2.17559352e-03 51 | 1.09594939e-02 4.50201938e-03 0. 0. 0. 0. 0. 0. 0. 0. 4.37383614e-02 52 | -2.74288431e-02 4.87260558e-02 2.82756351e-02 -5.37482789e-03 53 | 1.40324596e-03 5.47321839e-03 1.46708987e-03 0. 0. 0. 0. 0. 0. 0. 0. 54 | 3.40272598e-02 4.06533517e-02 4.30598632e-02 4.06533517e-02 55 | 9.60103993e-04 4.55879234e-03 1.30421098e-03 5.37784956e-03 0. 0. 0. 56 | 0. 0. 0. 0. 0. 3.48439515e-01 7.32563715e-03 6.09277725e-01 57 | 5.55390902e-02 -2.60778842e-03 -6.60274271e-03 3.32856807e-03 58 | 1.50486771e-02 0. 0. 0. 0. 0. 0. 0. 0. 3.52897763e-01 4.77533508e-03 59 | 5.90429187e-01 1.24875806e-01 2.08810042e-03 1.80204166e-03 60 | 2.69309990e-03 5.73312910e-03 0. 0. 0. 0. 0. 0. 0. 0. 4.58435118e-02 61 | -2.67060418e-02 7.09725097e-02 3.00215576e-02 -1.41762628e-03 62 | 1.90870650e-03 1.73097686e-03 2.19306536e-03 0. 0. 0. 0. 0. 0. 0. 0. 63 | 3.79957259e-02 -5.99068357e-03 6.62551150e-02 6.62527559e-03 64 | -4.02917882e-04 2.53727630e-04 5.04250696e-04 6.72506634e-04 0. 0. 65 | 0. 0. 0. 0. 0. 0. 4.24797505e-01 4.31047902e-02 5.70268393e-01 66 | 9.27028432e-02 -5.25623411e-02 3.44058149e-03 5.25623411e-02 67 | 1.53228957e-02 0. 0. 0. 0. 0. 0. 0. 0. 3.94643098e-01 68 | -7.20637068e-02 5.38302302e-01 1.29590780e-01 -3.76369357e-02 69 | -2.51732621e-04 3.76369357e-02 1.24710845e-02 0. 0. 0. 0. 0. 0. 0. 70 | 0. 1.17875524e-02 -2.57150829e-02 1.17875524e-02 2.57150829e-02 71 | 8.89357878e-04 -7.49212224e-03 8.89357878e-04 7.51690753e-03 72 | 73 | -------------------------------------------------------------------------------- /data/cheezit/descriptors003.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 5 | 64 6 |
f
7 | 8 | 0. 0. 0. 0. 0. 0. 0. 0. 3.27897631e-02 5.60788298e-03 5.33535480e-02 9 | 7.54610635e-03 -3.81144043e-03 -2.44240189e-04 4.78107808e-03 10 | 7.31789740e-04 0. 0. 0. 0. 0. 0. 0. 0. 2.15047821e-01 11 | -4.70308028e-02 6.82945549e-01 1.30725428e-01 -7.41194282e-03 12 | -1.19818200e-03 8.26681033e-03 2.52848887e-03 0. 0. 0. 0. 0. 0. 0. 13 | 0. 1.94169119e-01 2.18716860e-02 6.39733553e-01 7.95082077e-02 14 | 8.73277895e-03 1.48526067e-02 1.37495920e-02 1.84102766e-02 0. 0. 0. 15 | 0. 0. 0. 0. 0. 3.52177396e-02 2.20057787e-03 8.89076814e-02 16 | 1.00155817e-02 1.59891555e-03 9.61658719e-04 4.30224882e-03 17 | 1.45062106e-03 0. 0. 0. 0. 0. 0. 0. 0. 3.66988964e-02 1.91171914e-02 18 | 6.51396811e-02 2.07819659e-02 -4.71198116e-04 -3.67999979e-04 19 | 1.01159152e-03 7.64947676e-04 0. 0. 0. 0. 0. 0. 0. 0. 2.86710799e-01 20 | -4.70259152e-02 6.66652441e-01 1.44765764e-01 9.63667408e-04 21 | 1.84958262e-04 3.26170214e-03 3.07425787e-03 0. 0. 0. 0. 0. 0. 0. 0. 22 | 3.34549397e-01 4.57017198e-02 5.57799518e-01 9.43069234e-02 23 | -8.61555710e-03 3.38340662e-02 1.09527335e-02 3.84921767e-02 0. 0. 24 | 0. 0. 0. 0. 0. 0. 5.68129309e-02 -1.26913330e-02 5.72565123e-02 25 | 1.56514496e-02 -2.20708875e-03 1.11132773e-04 2.42439192e-03 26 | 1.73094368e-03 0. 0. 0. 0. 0. 0. 0. 0. 5.72022647e-02 1.79127511e-02 27 | 1.08401380e-01 2.52168886e-02 -1.27954129e-03 -3.57495650e-04 28 | 1.39112945e-03 1.03014777e-03 0. 0. 0. 0. 0. 0. 0. 0. 3.72664601e-01 29 | -2.17089932e-02 5.30525029e-01 1.39110073e-01 8.09375569e-03 30 | 4.41400521e-02 2.00834088e-02 4.72094864e-02 0. 0. 0. 0. 0. 0. 0. 0. 31 | 4.98222739e-01 -2.01914478e-02 5.14151216e-01 1.29931837e-01 32 | -2.09189560e-02 3.52579728e-03 2.12615822e-02 1.74627043e-02 0. 0. 33 | 0. 0. 0. 0. 0. 0. 6.20868988e-02 -8.96322262e-03 6.21605627e-02 34 | 1.74773205e-02 -2.23670650e-04 -7.74374348e-04 7.33119610e-04 35 | 5.56719303e-03 0. 0. 0. 0. 0. 0. 0. 0. 3.72334644e-02 1.16469134e-02 36 | 7.79115930e-02 1.67870261e-02 -1.46966209e-04 2.21358845e-04 37 | 3.23908054e-04 2.21358845e-04 0. 0. 0. 0. 0. 0. 0. 0. 3.14125955e-01 38 | -2.06545983e-02 6.56735003e-01 1.29024118e-01 -2.48703710e-03 39 | -3.00828891e-04 3.02505400e-03 3.56605928e-03 0. 0. 0. 0. 0. 0. 0. 40 | 0. 4.22478318e-01 1.63339842e-02 4.85703677e-01 1.38773277e-01 41 | -8.94434843e-03 4.45973985e-02 9.04736947e-03 4.60737944e-02 0. 0. 42 | 0. 0. 0. 0. 0. 0. 5.56890331e-02 -1.51925525e-02 5.57127446e-02 43 | 1.88769791e-02 1.47882063e-04 -5.96613507e-04 7.78073038e-04 44 | 7.74830300e-03 0. 0. 0. 0. 0. 0. 0. 0. 2.46600751e-02 3.07680736e-03 45 | 2.89065633e-02 1.16736842e-02 5.75718041e-05 2.44983064e-04 46 | 9.63765924e-05 7.62169715e-04 0. 0. 0. 0. 0. 0. 0. 0. 3.08575958e-01 47 | 4.43705134e-02 5.59673011e-01 2.02198118e-01 -8.20935424e-03 48 | 2.60697841e-03 9.53448936e-03 6.16872124e-03 0. 0. 0. 0. 0. 0. 0. 0. 49 | 3.47100019e-01 4.84921299e-02 6.16766810e-01 1.72677934e-01 50 | -1.62392817e-02 2.80561531e-03 2.42574438e-02 8.84174276e-03 0. 0. 51 | 0. 0. 0. 0. 0. 0. 6.02943115e-02 -1.80348270e-02 8.97829086e-02 52 | 2.51269713e-02 -1.04575818e-02 1.62500888e-03 1.20595153e-02 53 | 1.82483555e-03 0. 0. 0. 0. 0. 0. 0. 0. 4.40629721e-02 1.43597983e-02 54 | 8.59883502e-02 2.02767421e-02 3.91415000e-04 8.93295568e-04 55 | 4.26415500e-04 1.44688529e-03 0. 0. 0. 0. 0. 0. 0. 0. 3.52796853e-01 56 | -1.98085676e-03 5.61809659e-01 1.66331992e-01 -6.83148066e-03 57 | 4.83773462e-02 7.29032932e-03 5.11850268e-02 0. 0. 0. 0. 0. 0. 0. 0. 58 | 4.86495882e-01 -1.38634387e-02 5.10820210e-01 1.08535133e-01 59 | -1.00403810e-02 5.37956320e-03 1.06046554e-02 1.91559661e-02 0. 0. 60 | 0. 0. 0. 0. 0. 0. 5.11348844e-02 -2.86397375e-02 5.11348844e-02 61 | 2.86397375e-02 5.33571932e-04 -8.66096094e-03 6.93613430e-04 62 | 8.67013074e-03 63 | 64 | -------------------------------------------------------------------------------- /data/cheezit/intrinsics.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 3 5 | 3 6 |
d
7 | 8 | 400. 0. 320. 0. 400. 240. 0. 0. 1. 9 | 10 | -------------------------------------------------------------------------------- /data/cheezit/keyframe000.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/msheckells/mesh_localize/64b1251794337cfff23ae1e81d36daf0ba77f812/data/cheezit/keyframe000.jpg -------------------------------------------------------------------------------- /data/cheezit/keyframe001.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/msheckells/mesh_localize/64b1251794337cfff23ae1e81d36daf0ba77f812/data/cheezit/keyframe001.jpg -------------------------------------------------------------------------------- /data/cheezit/keyframe002.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/msheckells/mesh_localize/64b1251794337cfff23ae1e81d36daf0ba77f812/data/cheezit/keyframe002.jpg -------------------------------------------------------------------------------- /data/cheezit/keyframe003.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/msheckells/mesh_localize/64b1251794337cfff23ae1e81d36daf0ba77f812/data/cheezit/keyframe003.jpg -------------------------------------------------------------------------------- /data/cheezit/keyframe004.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/msheckells/mesh_localize/64b1251794337cfff23ae1e81d36daf0ba77f812/data/cheezit/keyframe004.jpg -------------------------------------------------------------------------------- /data/cheezit/keyframe005.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/msheckells/mesh_localize/64b1251794337cfff23ae1e81d36daf0ba77f812/data/cheezit/keyframe005.jpg -------------------------------------------------------------------------------- /data/cheezit/keyframe006.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/msheckells/mesh_localize/64b1251794337cfff23ae1e81d36daf0ba77f812/data/cheezit/keyframe006.jpg -------------------------------------------------------------------------------- /data/cheezit/keyframe007.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/msheckells/mesh_localize/64b1251794337cfff23ae1e81d36daf0ba77f812/data/cheezit/keyframe007.jpg -------------------------------------------------------------------------------- /data/cheezit/keyframe008.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/msheckells/mesh_localize/64b1251794337cfff23ae1e81d36daf0ba77f812/data/cheezit/keyframe008.jpg -------------------------------------------------------------------------------- /data/cheezit/keyframe009.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/msheckells/mesh_localize/64b1251794337cfff23ae1e81d36daf0ba77f812/data/cheezit/keyframe009.jpg -------------------------------------------------------------------------------- /data/cheezit/keyframe010.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/msheckells/mesh_localize/64b1251794337cfff23ae1e81d36daf0ba77f812/data/cheezit/keyframe010.jpg -------------------------------------------------------------------------------- /data/cheezit/keyframe011.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/msheckells/mesh_localize/64b1251794337cfff23ae1e81d36daf0ba77f812/data/cheezit/keyframe011.jpg -------------------------------------------------------------------------------- /data/cheezit/keypoints002.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 2.9027566528320312e+02 1.8028810501098633e+01 21. 5 | 2.7896963500976562e+02 2.7709030151367188e+02 0 1 6 | 2.8386898803710938e+02 1.5828254699707031e+01 14. 7 | 2.7758651733398438e+02 2.4707949829101562e+02 0 1 8 | 3.2010647583007812e+02 1.7957834243774414e+01 28. 9 | 2.6839776611328125e+02 2.2178961181640625e+02 0 1 10 | 3.1548980712890625e+02 1.6107307434082031e+01 15. 11 | 2.6170101928710938e+02 1.9120336914062500e+02 0 1 12 | 2.9570422363281250e+02 1.8836040496826172e+01 21. 13 | 2.7780749511718750e+02 1.8473075866699219e+02 0 1 14 | 2.8922406005859375e+02 1.9943708419799805e+01 28. 15 | 2.7945935058593750e+02 1.8384133911132812e+02 0 1 16 | 3.3925506591796875e+02 1.8734872817993164e+01 26. 17 | 2.6602828979492188e+02 1.3567492675781250e+02 0 1 18 | 19 | -------------------------------------------------------------------------------- /data/cheezit/keypoints003.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 3.0069848632812500e+02 4.6429074096679688e+02 15. 5 | 8.3071479797363281e+01 3.7170629882812500e+02 0 1 6 | 2.9958383178710938e+02 4.6231054687500000e+02 21. 7 | 8.3286132812500000e+01 1.9620092773437500e+02 0 1 8 | 2.9087631225585938e+02 4.6299801635742188e+02 20. 9 | 8.2252883911132812e+01 1.6207296752929688e+02 0 1 10 | 2.9859475708007812e+02 4.6017529296875000e+02 28. 11 | 8.6354415893554688e+01 1.4144044494628906e+02 0 1 12 | 3.3406530761718750e+02 4.6529736328125000e+02 14. 13 | 1.0194908142089844e+02 1.2311705017089844e+02 0 1 14 | 2.9314358520507812e+02 4.6076281738281250e+02 27. 15 | 8.4371238708496094e+01 1.0852398681640625e+02 0 1 16 | 17 | -------------------------------------------------------------------------------- /data/cheezit/keypoints005.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 3.4095150756835938e+02 1.9368946838378906e+02 15. 5 | 4.7999196052551270e+00 5.3486944580078125e+02 0 1 6 | 3.4406500244140625e+02 2.6649853515625000e+02 15. 7 | 2.5585054397583008e+01 5.1967266845703125e+02 0 1 8 | 3.3993609619140625e+02 2.0329945373535156e+02 15. 9 | 3.5394387817382812e+02 4.7632479858398438e+02 0 1 10 | 2.9509646606445312e+02 2.4010919189453125e+02 15. 11 | 1.9105726623535156e+02 4.1206750488281250e+02 0 1 12 | 2.9294277954101562e+02 2.7469638061523438e+02 23. 13 | 3.4998220825195312e+02 3.8777343750000000e+02 0 -1 14 | 2.9384451293945312e+02 2.5669262695312500e+02 21. 15 | 1.7758677673339844e+02 3.7346945190429688e+02 0 -1 16 | 2.9616894531250000e+02 2.6427380371093750e+02 15. 17 | 1.7532856750488281e+02 3.4017779541015625e+02 0 1 18 | 2.8577169799804688e+02 2.5496635437011719e+02 15. 19 | 8.2873648405075073e-01 3.3317355346679688e+02 0 1 20 | 2.8723687744140625e+02 2.6724795532226562e+02 16. 21 | 3.5800595092773438e+02 3.0116467285156250e+02 0 1 22 | 2.9709375000000000e+02 2.6080557250976562e+02 15. 23 | 1.7900846862792969e+02 2.5247003173828125e+02 0 1 24 | 3.5292123413085938e+02 2.6423471069335938e+02 15. 25 | 1.6036260986328125e+02 2.5070896911621094e+02 0 1 26 | 2.8932644653320312e+02 2.5427365112304688e+02 25. 27 | 3.5845788574218750e+02 2.4926535034179688e+02 0 -1 28 | 2.9604010009765625e+02 2.0776464843750000e+02 14. 29 | 1.7223519897460938e+02 2.3253399658203125e+02 0 1 30 | 3.4036010742187500e+02 1.9451716613769531e+02 34. 31 | 1.7209240722656250e+02 2.1707696533203125e+02 1 -1 32 | 3.5254586791992188e+02 2.8742114257812500e+02 15. 33 | 1.9305001831054688e+02 1.8549458312988281e+02 0 1 34 | 3.3414440917968750e+02 1.9439897155761719e+02 22. 35 | 5.6409811973571777e+00 1.8119157409667969e+02 0 -1 36 | 3.5404415893554688e+02 2.6226278686523438e+02 20. 37 | 1.7153649902343750e+02 1.7226982116699219e+02 0 1 38 | 2.8387158203125000e+02 2.0794348144531250e+02 13. 39 | 1.5349230766296387e+01 1.6069020080566406e+02 0 1 40 | 3.3589068603515625e+02 1.8721702575683594e+02 16. 41 | 7.8762288093566895e+00 1.5508824157714844e+02 0 -1 42 | 2.9874160766601562e+02 2.4255851745605469e+02 31. 43 | 1.8053080749511719e+02 1.4879617309570312e+02 1 1 44 | 2.8351568603515625e+02 2.5627465820312500e+02 21. 45 | 3.5721328735351562e+02 1.4389509582519531e+02 0 1 46 | 3.4836950683593750e+02 2.5562420654296875e+02 23. 47 | 1.7952622985839844e+02 1.4270695495605469e+02 0 -1 48 | 3.4967944335937500e+02 2.5587876892089844e+02 22. 49 | 1.7705273437500000e+02 1.3537512207031250e+02 0 -1 50 | 3.4213586425781250e+02 2.1768064880371094e+02 15. 51 | 1.7893200683593750e+02 1.3237158203125000e+02 0 1 52 | 2.9729690551757812e+02 2.8413607788085938e+02 14. 53 | 1.8103944396972656e+02 1.2961053466796875e+02 0 1 54 | 3.4611135864257812e+02 2.5563656616210938e+02 22. 55 | 1.6713426113128662e+00 1.2867121887207031e+02 0 -1 56 | 3.4217718505859375e+02 1.8677430725097656e+02 22. 57 | 1.7027713012695312e+02 1.2830206298828125e+02 0 -1 58 | 2.9818270874023438e+02 2.9672436523437500e+02 15. 59 | 1.7862954711914062e+02 1.2364035034179688e+02 0 1 60 | 3.4212200927734375e+02 1.8527462768554688e+02 21. 61 | 3.0388795852661133e+01 1.2108255004882812e+02 0 -1 62 | 3.5288354492187500e+02 2.5711468505859375e+02 15. 63 | 1.8715005493164062e+02 1.2090310668945312e+02 0 1 64 | 3.5590631103515625e+02 2.6024908447265625e+02 29. 65 | 1.7027134704589844e+02 1.1504272460937500e+02 0 1 66 | 3.4083013916015625e+02 2.7543588256835938e+02 15. 67 | 2.7868063449859619e+00 1.1048470306396484e+02 0 1 68 | 3.4206427001953125e+02 2.6087719726562500e+02 29. 69 | 8.3562841415405273e+00 1.0514929962158203e+02 1 1 70 | 71 | -------------------------------------------------------------------------------- /data/cheezit/keypoints011.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 2.9567941284179688e+02 2.6192169189453125e+02 15. 5 | 3.5426547241210938e+02 1.6176535644531250e+03 0 -1 6 | 3.3718035888671875e+02 1.9852772521972656e+02 23. 7 | 1.7673979187011719e+02 6.7718902587890625e+02 0 -1 8 | 3.3341345214843750e+02 1.9860456848144531e+02 17. 9 | 5.5443067550659180e+00 5.9247320556640625e+02 0 -1 10 | 3.3709347534179688e+02 1.9965406799316406e+02 27. 11 | 3.5759155273437500e+02 5.6927593994140625e+02 1 -1 12 | 3.3418203735351562e+02 2.0057987976074219e+02 22. 13 | 7.7057104110717773e+00 4.7231597900390625e+02 0 -1 14 | 2.9550292968750000e+02 2.4761009216308594e+02 16. 15 | 3.5052120685577393e+00 4.5119711303710938e+02 0 -1 16 | 3.3789556884765625e+02 2.7244232177734375e+02 15. 17 | 2.6231420040130615e+00 3.7437683105468750e+02 0 1 18 | 3.3739944458007812e+02 2.1803376770019531e+02 15. 19 | 1.0896605491638184e+01 3.5289590454101562e+02 0 1 20 | 3.0013162231445312e+02 2.5511354064941406e+02 16. 21 | 1.7628948974609375e+02 3.4811444091796875e+02 0 1 22 | 3.3921575927734375e+02 2.3056446838378906e+02 15. 23 | 1.9882267761230469e+02 3.2819271850585938e+02 0 1 24 | 3.3956723022460938e+02 2.6077322387695312e+02 15. 25 | 5.8030281066894531e+00 3.1031887817382812e+02 0 1 26 | 3.4632150268554688e+02 2.7426925659179688e+02 15. 27 | 1.9008782958984375e+02 2.8062701416015625e+02 0 1 28 | 3.3767425537109375e+02 2.2649490356445312e+02 30. 29 | 2.2099505615234375e+02 2.4508343505859375e+02 0 -1 30 | 2.9103085327148438e+02 2.2309422302246094e+02 15. 31 | 3.5794906616210938e+02 2.2378076171875000e+02 0 1 32 | 3.3796624755859375e+02 2.6169085693359375e+02 14. 33 | 1.4113758087158203e+01 1.8585438537597656e+02 0 1 34 | 3.0318676757812500e+02 2.8557986450195312e+02 16. 35 | 1.8267575073242188e+02 1.4668421936035156e+02 0 1 36 | 3.4300134277343750e+02 2.5379171752929688e+02 15. 37 | 1.7127806091308594e+02 1.4526815795898438e+02 0 -1 38 | 2.9451593017578125e+02 2.8547702026367188e+02 15. 39 | 3.5556503295898438e+02 1.4331686401367188e+02 0 1 40 | 3.0199002075195312e+02 2.6082723999023438e+02 14. 41 | 1.7706896972656250e+02 1.3663529968261719e+02 0 1 42 | 2.9975140380859375e+02 2.0552770996093750e+02 15. 43 | 1.7684169006347656e+02 1.3647088623046875e+02 0 1 44 | 3.3562628173828125e+02 2.1424710083007812e+02 21. 45 | 3.2493579101562500e+02 1.2793219757080078e+02 0 1 46 | 3.3472479248046875e+02 2.2486549377441406e+02 28. 47 | 2.0804449462890625e+02 1.1643300628662109e+02 0 -1 48 | 3.0121267700195312e+02 2.7224139404296875e+02 15. 49 | 1.7968119812011719e+02 1.1523215484619141e+02 0 1 50 | 3.3678393554687500e+02 2.1099948120117188e+02 14. 51 | 3.4072415161132812e+02 1.1463151550292969e+02 0 1 52 | 3.3448574829101562e+02 2.1398158264160156e+02 20. 53 | 3.4152282714843750e+02 1.1196097564697266e+02 0 1 54 | 3.4318930053710938e+02 2.3365335083007812e+02 20. 55 | 2.0319090270996094e+02 1.0477941894531250e+02 0 1 56 | 2.9701556396484375e+02 2.4298162841796875e+02 28. 57 | 1.7629229736328125e+02 1.0435573577880859e+02 0 -1 58 | 59 | -------------------------------------------------------------------------------- /data/cheezit/pose000.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 4 5 | 4 6 |
d
7 | 8 | 3.4228539891323351e-08 -8.7422776749110680e-08 9 | 9.9999996577145633e-01 -5.0000000000000000e-01 0. 10 | -9.9999993154291644e-01 -8.7422776749110680e-08 0. 11 | 9.9999996577146011e-01 0. 3.4228539868016955e-08 0. 0. 0. 0. 1. 12 | 13 | -------------------------------------------------------------------------------- /data/cheezit/pose001.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 4 5 | 4 6 |
d
7 | 8 | 3.4228539891323351e-08 8.7422776749110680e-08 9 | -9.9999996577145633e-01 5.0000000000000000e-01 0. 10 | -9.9999993154291644e-01 -8.7422776749110680e-08 0. 11 | -9.9999996577146011e-01 0. 3.4228539868016955e-08 0. 0. 0. 0. 1. 12 | 13 | -------------------------------------------------------------------------------- /data/cheezit/pose002.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 4 5 | 4 6 |
d
7 | 8 | 1. 0. 0. 0. 0. 5.0000000000000000e-01 4.9999997019767761e-01 9 | -5.0000000000000000e-01 0. -4.9999997019767761e-01 10 | 5.0000000000000000e-01 0. 0. 0. 0. 1. 11 | 12 | -------------------------------------------------------------------------------- /data/cheezit/pose003.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 4 5 | 4 6 |
d
7 | 8 | 1. 0. 0. 0. 0. 5.0000005960464300e-01 -4.9999997019767761e-01 9 | 5.0000000000000000e-01 0. 4.9999997019767761e-01 10 | 5.0000005960464300e-01 0. 0. 0. 0. 1. 11 | 12 | -------------------------------------------------------------------------------- /data/cheezit/pose004.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 4 5 | 4 6 |
d
7 | 8 | -1.0000000000000038e+00 0. 0. 0. 0. -1. -8.7422776573475858e-08 0. 9 | 0. -8.7422776573475858e-08 9.9999999999999623e-01 10 | -5.0000000000000000e-01 0. 0. 0. 1. 11 | 12 | -------------------------------------------------------------------------------- /data/cheezit/pose005.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 4 5 | 4 6 |
d
7 | 8 | 1. 0. 0. 0. 0. -1. -8.7422776573475858e-08 0. 0. 9 | 8.7422776573475858e-08 -1. 5.0000000000000000e-01 0. 0. 0. 1. 10 | 11 | -------------------------------------------------------------------------------- /data/cheezit/pose006.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 4 5 | 4 6 |
d
7 | 8 | 3.4228539891323351e-08 -8.7422776749110680e-08 9 | 9.9999996577145633e-01 -4.0000000000000002e-01 0. 10 | -9.9999993154291644e-01 -8.7422776749110680e-08 0. 11 | 9.9999996577146011e-01 0. 3.4228539868016955e-08 12 | 2.0000000000000001e-01 0. 0. 0. 1. 13 | 14 | -------------------------------------------------------------------------------- /data/cheezit/pose007.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 4 5 | 4 6 |
d
7 | 8 | 3.4228539891323351e-08 8.7422776749110680e-08 9 | -9.9999996577145633e-01 4.0000000000000002e-01 0. 10 | -9.9999993154291644e-01 -8.7422776749110680e-08 0. 11 | -9.9999996577146011e-01 0. 3.4228539868016955e-08 12 | 2.0000000000000001e-01 0. 0. 0. 1. 13 | 14 | -------------------------------------------------------------------------------- /data/cheezit/pose008.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 4 5 | 4 6 |
d
7 | 8 | 1. 0. 0. 0. 0. 5.0000000000000000e-01 4.9999997019767761e-01 9 | -4.0000000000000002e-01 0. -4.9999997019767761e-01 10 | 5.0000000000000000e-01 2.0000000000000001e-01 0. 0. 0. 1. 11 | 12 | -------------------------------------------------------------------------------- /data/cheezit/pose009.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 4 5 | 4 6 |
d
7 | 8 | 1. 0. 0. 0. 0. 5.0000005960464300e-01 -4.9999997019767761e-01 9 | 4.0000000000000002e-01 0. 4.9999997019767761e-01 10 | 5.0000005960464300e-01 2.0000000000000001e-01 0. 0. 0. 1. 11 | 12 | -------------------------------------------------------------------------------- /data/cheezit/pose010.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 4 5 | 4 6 |
d
7 | 8 | -1.0000000000000038e+00 0. 0. 0. 0. -1. -8.7422776573475858e-08 0. 9 | 0. -8.7422776573475858e-08 9.9999999999999623e-01 10 | -2.0000000000000001e-01 0. 0. 0. 1. 11 | 12 | -------------------------------------------------------------------------------- /data/cheezit/pose011.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 4 5 | 4 6 |
d
7 | 8 | 1. 0. 0. 0. 0. -1. -8.7422776573475858e-08 0. 0. 9 | 8.7422776573475858e-08 -1. 6.0000000000000009e-01 0. 0. 0. 1. 10 | 11 | -------------------------------------------------------------------------------- /include/mesh_localize/ASiftDetector.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | using namespace cv; 6 | 7 | class ASiftDetector 8 | { 9 | public: 10 | enum DescriptorType 11 | { 12 | SIFT, 13 | SURF 14 | }; 15 | 16 | ASiftDetector(); 17 | 18 | void detectAndCompute(const Mat& img, std::vector< KeyPoint >& keypoints, Mat& descriptors, const Mat& mask, DescriptorType desc_type = SURF); 19 | void detectAndCompute(const Mat& img, std::vector< KeyPoint >& keypoints, Mat& descriptors, DescriptorType desc_type = SURF); 20 | 21 | private: 22 | void affineSkew(double tilt, double phi, Mat& img, Mat& mask, Mat& Ai); 23 | }; 24 | -------------------------------------------------------------------------------- /include/mesh_localize/CameraContainer.h: -------------------------------------------------------------------------------- 1 | #ifndef _CAMERACONTAINER_H_ 2 | #define _CAMERACONTAINER_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | using namespace cv; 11 | 12 | class CameraContainer 13 | { 14 | public: 15 | CameraContainer(Mat image, Eigen::Matrix4f tf = Eigen::Matrix4f(), Eigen::Matrix3f K = Eigen::Matrix3f()); 16 | 17 | Mat GetImage(); 18 | Eigen::Matrix4f GetTf(); 19 | Eigen::Matrix3f GetK(); 20 | private: 21 | 22 | Eigen::Matrix4f tf; 23 | Eigen::Matrix3f K; 24 | Mat img; 25 | }; 26 | 27 | #endif 28 | -------------------------------------------------------------------------------- /include/mesh_localize/Common.h: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | * ExploringSfMWithOpenCV 3 | ****************************************************************************** 4 | * by Roy Shilkrot, 5th Dec 2012 5 | * http://www.morethantechnical.com/ 6 | ****************************************************************************** 7 | * Ch4 of the book "Mastering OpenCV with Practical Computer Vision Projects" 8 | * Copyright Packt Publishing 2012. 9 | * http://www.packtpub.com/cool-projects-with-opencv/book 10 | *****************************************************************************/ 11 | 12 | #pragma once 13 | 14 | //#pragma warning(disable: 4244 18 4996 4800) 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | struct CloudPoint { 24 | cv::Point3d pt; 25 | std::vector imgpt_for_img; 26 | double reprojection_error; 27 | }; 28 | 29 | std::vector FlipMatches(const std::vector& matches); 30 | void KeyPointsToPoints(const std::vector& kps, std::vector& ps); 31 | void PointsToKeyPoints(const std::vector& ps, std::vector& kps); 32 | 33 | std::vector CloudPointsToPoints(const std::vector cpts); 34 | 35 | void GetAlignedPointsFromMatch(const std::vector& imgpts1, 36 | const std::vector& imgpts2, 37 | const std::vector& matches, 38 | std::vector& pt_set1, 39 | std::vector& pt_set2); 40 | 41 | void drawArrows(cv::Mat& frame, const std::vector& prevPts, const std::vector& nextPts, const std::vector& status, const std::vector& verror, const cv::Scalar& line_color = cv::Scalar(0, 0, 255)); 42 | 43 | #ifdef USE_PROFILING 44 | #define CV_PROFILE(msg,code) {\ 45 | std::cout << msg << " ";\ 46 | double __time_in_ticks = (double)cv::getTickCount();\ 47 | { code }\ 48 | std::cout << "DONE " << ((double)cv::getTickCount() - __time_in_ticks)/cv::getTickFrequency() << "s" << std::endl;\ 49 | } 50 | #else 51 | #define CV_PROFILE(msg,code) code 52 | #endif 53 | 54 | void open_imgs_dir(char* dir_name, std::vector& images, std::vector& images_names, double downscale_factor); 55 | void imshow_250x250(const std::string& name_, const cv::Mat& patch); 56 | -------------------------------------------------------------------------------- /include/mesh_localize/DepthFeatureMatchLocalizer.h: -------------------------------------------------------------------------------- 1 | #ifndef _DEPTH_FEATURE_MATCH_LOCALIZER_H_ 2 | #define _DEPTH_FEATURE_MATCH_LOCALIZER_H_ 3 | 4 | #include "MonocularLocalizer.h" 5 | #include "KeyframeMatch.h" 6 | #include "KeyframeContainer.h" 7 | 8 | class DepthFeatureMatchLocalizer : public MonocularLocalizer 9 | { 10 | struct KeyframePositionSorter 11 | { 12 | Eigen::Matrix4f currentPose; 13 | KeyframePositionSorter(Eigen::Matrix4f pose): currentPose(pose) {}; 14 | 15 | bool operator()(KeyframeContainer* kfc1, KeyframeContainer* kfc2) 16 | { 17 | return (kfc1->GetTf().block<3,1>(0,3)-currentPose.block<3,1>(0,3)).norm() < (kfc2->GetTf().block<3,1>(0,3)-currentPose.block<3,1>(0,3)).norm(); 18 | } 19 | }; 20 | 21 | public: 22 | 23 | DepthFeatureMatchLocalizer(const std::vector& train, 24 | std::string desc_type = "surf", bool show_matches = false, int min_inliers = 10, 25 | double max_reproj_error = 3, double ratio_test_thresh = 0.8); 26 | virtual bool localize(const cv::Mat& img, const cv::Mat& K, Eigen::Matrix4f* pose, 27 | Eigen::Matrix4f* pose_guess = NULL); 28 | 29 | private: 30 | 31 | std::vector< KeyframeMatch > FindImageMatches(KeyframeContainer* img, int k, Eigen::Matrix4f* pose_guess = NULL, unsigned int search_bound = 0); 32 | 33 | std::vector keyframes; 34 | int min_inliers; 35 | double max_reproj_error; 36 | double ratio_test_thresh; 37 | bool show_matches; 38 | std::string desc_type; 39 | }; 40 | 41 | #endif 42 | -------------------------------------------------------------------------------- /include/mesh_localize/EdgeTrackingUtil.h: -------------------------------------------------------------------------------- 1 | #ifndef _EDGE_TRACKING_UTIL_H_ 2 | #define _EDGE_TRACKING_UTIL_H_ 3 | 4 | /** 5 | * This class is directly adapted from an implementation given by 6 | * Changhyun Choi and Henrik Christensen, the authors of "Real-time 7 | * 3D Model-based Tracking Using Edge and Keypoint Features for 8 | * Robotic Manipulation" (ICRA 2010) 9 | */ 10 | #include 11 | #include 12 | #include 13 | #include "TooN/se3.h" // for special Euclidean group 14 | 15 | class EdgeTrackingUtil 16 | { 17 | public: 18 | struct SamplePoint 19 | { 20 | CvPoint3D32f coord3; // Coordinate of a visible 3D sampling point 21 | CvPoint2D32f coord2; // Coordinate of a visible 2D sampling point 22 | CvPoint2D32f nuv; // Normal unit vector of a visible 2D sampling point 23 | unsigned char nidx; // Normal index of a visible 2D sampling point (0, 1, 2, 3) 24 | double dist; // Normal distance of a visible 3D sampling point 25 | double normal_ang; // Normal angle (4 ways) 26 | double normal_ang_deg; // Normal angle in degree (accurate) 27 | double dx; // Normal unit vector (x) 28 | double dy; // Normal unit vector (y) 29 | CvPoint2D32f edge_pt2; // corresponding edge coordinate 30 | int edge_mem; // Edge membership (0, 1, 2, ... until # of edges -1) 31 | }; 32 | static void getEstimatedPosePnP(Eigen::Matrix4f& pose_cur, const Eigen::Matrix4f& pose_pre, 33 | const std::vector& vSamplePt, const cv::Mat& intrinsics); 34 | static void getEstimatedPoseIRLS(Eigen::Matrix4f& pose_cur, const Eigen::Matrix4f& pose_pre, const std::vector& vSamplePt, const Eigen::Matrix3f& intrinsics); 35 | static TooN::Vector<6> calcJacobian(const CvPoint3D32f& pts3, const CvPoint2D32f& pts2, 36 | const CvPoint2D32f& ptsnv, double ptsd, const TooN::SE3 &E, 37 | const Eigen::Matrix3f& intrinsics); 38 | static std::vector getEdgeMatches(const cv::Mat& vimg, const cv::Mat& kf, 39 | const Eigen::Matrix3f vimgK, const Eigen::Matrix3f K, const cv::Mat& vdepth, 40 | const cv::Mat& kf_mask, const Eigen::Matrix4f& vimgTf); 41 | static std::vector getEdgeMatches(const std::vector& vimg_edge_pts, 42 | const std::vector& vimg_edge_dirs, const cv::Mat& kf_detected_edges, 43 | const cv::Mat& kf_edge_dir, const Eigen::Matrix3f vimgK, const Eigen::Matrix3f K, 44 | const cv::Mat& vdepth, const Eigen::Matrix4f& vimgTf); 45 | static std::vector getWindowedEdgeMatches( 46 | const cv::Mat& vimg, 47 | const std::vector& vimg_edge_pts, const std::vector& vimg_edge_dirs, 48 | const cv::Mat& kf, 49 | const cv::Mat& kf_detected_edges, const cv::Mat& kf_edge_dir, const Eigen::Matrix3f vimgK, 50 | const Eigen::Matrix3f K, const cv::Mat& vdepth, const Eigen::Matrix4f& vimgTf); 51 | static std::vector calcImageGradientDirection(const cv::Mat& src, const std::vector& pts); 52 | static void calcImageGradientDirection(cv::Mat& dst, const cv::Mat& src); 53 | static void calcImageGradientDirection(cv::Mat& dst, const cv::Mat& src, const std::vector& pts); 54 | 55 | static double ncc(const Eigen::VectorXd& d1, const Eigen::VectorXd& d2); 56 | static bool extractEdgeDescriptor(Eigen::VectorXd& desc, const cv::Mat& im, cv::Point pt, 57 | double edge_dir, unsigned int window_size); 58 | static void drawGradientLines(cv::Mat& dst, const cv::Mat& src, const cv::Mat& edges, 59 | const cv::Mat& gdir); 60 | static void drawGradientLines(cv::Mat& dst, const cv::Mat& src, const std::vector& edges, 61 | const std::vector& gdir); 62 | static void drawEdgeMatching(cv::Mat& dst, const cv::Mat& src, const std::vector& sps); 63 | static void drawLines(cv::Mat& dst, const cv::Mat& src, const std::vector& lines); 64 | static bool withinOri(float o1, float o2, float oth); 65 | static double getMedian(const cv::Mat& im, int start_bin=0); 66 | 67 | static bool show_debug; 68 | static bool autotune_canny; 69 | static double canny_high_thresh; 70 | static double canny_low_thresh; 71 | static double canny_sigma; 72 | static double dmax; 73 | }; 74 | 75 | #endif 76 | -------------------------------------------------------------------------------- /include/mesh_localize/FABMAPLocalizer.h: -------------------------------------------------------------------------------- 1 | #ifndef _FABMAP_LOCALIZER_H_ 2 | #define _FABMAP_LOCALIZER_H_ 3 | 4 | #include "MonocularLocalizer.h" 5 | #include "KeyframeMatch.h" 6 | #include "KeyframeContainer.h" 7 | 8 | #include 9 | #include 10 | 11 | class FABMAPLocalizer : public MonocularLocalizer 12 | { 13 | struct CameraPositionSorter 14 | { 15 | Eigen::Matrix4f currentPose; 16 | CameraPositionSorter(Eigen::Matrix4f pose): currentPose(pose) {}; 17 | 18 | bool operator()(CameraContainer* kfc1, CameraContainer* kfc2) 19 | { 20 | return (kfc1->GetTf().block<3,1>(0,3)-currentPose.block<3,1>(0,3)).norm() < (kfc2->GetTf().block<3,1>(0,3)-currentPose.block<3,1>(0,3)).norm(); 21 | } 22 | }; 23 | 24 | public: 25 | 26 | FABMAPLocalizer(const std::vector& train, std::string descriptor_type, bool show_matches = false, bool load = false, std::string filename = ""); 27 | virtual bool localize(const cv::Mat& img, const cv::Mat& K, Eigen::Matrix4f* pose, Eigen::Matrix4f* poseGuess = NULL); 28 | private: 29 | 30 | std::vector keyframes; 31 | std::string desc_type; 32 | bool show_matches; 33 | 34 | Ptr detector; 35 | Ptr extractor; 36 | Ptr matcher; 37 | Ptr fabmap; 38 | Ptr bide; 39 | }; 40 | 41 | #endif 42 | -------------------------------------------------------------------------------- /include/mesh_localize/FeatureMatchLocalizer.h: -------------------------------------------------------------------------------- 1 | #ifndef _FEATURE_MATCH_LOCALIZER_H_ 2 | #define _FEATURE_MATCH_LOCALIZER_H_ 3 | 4 | #include "MonocularLocalizer.h" 5 | #include "KeyframeMatch.h" 6 | #include "KeyframeContainer.h" 7 | 8 | class FeatureMatchLocalizer : public MonocularLocalizer 9 | { 10 | struct KeyframePositionSorter 11 | { 12 | Eigen::Matrix4f currentPose; 13 | KeyframePositionSorter(Eigen::Matrix4f pose): currentPose(pose) {}; 14 | 15 | bool operator()(KeyframeContainer* kfc1, KeyframeContainer* kfc2) 16 | { 17 | return (kfc1->GetTf().block<3,1>(0,3)-currentPose.block<3,1>(0,3)).norm() < (kfc2->GetTf().block<3,1>(0,3)-currentPose.block<3,1>(0,3)).norm(); 18 | } 19 | }; 20 | 21 | public: 22 | 23 | FeatureMatchLocalizer(const std::vector& train, std::string descriptor_type, bool show_matches = false, bool load_descriptors = false, std::string descriptor_filename = ""); 24 | virtual bool localize(const cv::Mat& img, const cv::Mat& K, Eigen::Matrix4f* pose, Eigen::Matrix4f* pose_guess = NULL); 25 | private: 26 | std::vector< KeyframeMatch > FindImageMatches(KeyframeContainer* img, int k, Eigen::Matrix4f* pose_guess = NULL, unsigned int search_bound = 0); 27 | bool WriteDescriptorsToFile(std::string filename); 28 | 29 | 30 | std::vector keyframes; 31 | std::string desc_type; 32 | bool show_matches; 33 | }; 34 | 35 | #endif 36 | -------------------------------------------------------------------------------- /include/mesh_localize/FindCameraMatrices.h: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | * ExploringSfMWithOpenCV 3 | ****************************************************************************** 4 | * by Roy Shilkrot, 5th Dec 2012 5 | * http://www.morethantechnical.com/ 6 | ****************************************************************************** 7 | * Ch4 of the book "Mastering OpenCV with Practical Computer Vision Projects" 8 | * Copyright Packt Publishing 2012. 9 | * http://www.packtpub.com/cool-projects-with-opencv/book 10 | *****************************************************************************/ 11 | 12 | #pragma once 13 | 14 | #include 15 | #include 16 | 17 | #include "Common.h" 18 | 19 | //#undef __SFM__DEBUG__ 20 | 21 | bool CheckCoherentRotation(cv::Mat_& R); 22 | bool TestTriangulation(const std::vector& pcloud, const cv::Matx34d& P, std::vector& status); 23 | bool TestCoplanarity(const std::vector& pcloud, std::vector& planeIdx, std::vector& nonplaneIdx); 24 | 25 | cv::Mat GetHomographyMat( const std::vector& imgpts1, 26 | const std::vector& imgpts2, 27 | std::vector& imgpts1_good, 28 | std::vector& imgpts2_good, 29 | std::vector& matches, 30 | std::vector& nonmatches); 31 | 32 | cv::Mat GetFundamentalMat( const std::vector& imgpts1, 33 | const std::vector& imgpts2, 34 | std::vector& imgpts1_good, 35 | std::vector& imgpts2_good, 36 | std::vector& matches 37 | #ifdef __SFM__DEBUG__ 38 | ,const cv::Mat& = cv::Mat(), const cv::Mat& = cv::Mat() 39 | #endif 40 | ); 41 | bool FindCameraMatricesWithH(const cv::Matx33d& K, 42 | const cv::Matx33d& Kinv, 43 | const cv::Mat& distcoeff, 44 | const std::vector& imgpts1, 45 | const std::vector& imgpts2, 46 | std::vector& imgpts1_good, 47 | std::vector& imgpts2_good, 48 | cv::Matx34d& P1, 49 | std::vector& matches); 50 | 51 | bool FindCameraMatrices(const cv::Matx33d& K, 52 | const cv::Matx33d& Kinv, 53 | const cv::Mat& distcoeff, 54 | const std::vector& imgpts1, 55 | const std::vector& imgpts2, 56 | std::vector& imgpts1_good, 57 | std::vector& imgpts2_good, 58 | cv::Matx34d& P, 59 | cv::Matx34d& P1, 60 | std::vector& matches, 61 | std::vector& outCloud, 62 | double& reproj_error 63 | #ifdef __SFM__DEBUG__ 64 | ,const cv::Mat& = cv::Mat(), const cv::Mat& = cv::Mat() 65 | #endif 66 | ); 67 | -------------------------------------------------------------------------------- /include/mesh_localize/IMUMotionModel.h: -------------------------------------------------------------------------------- 1 | #ifndef _IMU_MOTION_MODEL_H_ 2 | #define _IMU_MOTION_MODEL_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include "vi_estimator_msgs/ImuSimple.h" 11 | 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | using namespace gcop; 23 | 24 | class IMUMotionModel 25 | { 26 | typedef Eigen::Matrix Matrix6f; 27 | typedef KalmanPredictor InsKalmanPredictor; 28 | typedef KalmanCorrector InsPoseKalmanCorrector; 29 | typedef KalmanPredictor ImuKalmanPredictor; 30 | typedef KalmanCorrector, 3> ImuKalmanCorrector; 31 | 32 | public: 33 | IMUMotionModel(); 34 | Eigen::Matrix4f predict(); 35 | Eigen::Matrix4f correct(const Eigen::Matrix4f& tf, const Matrix6f& cov); 36 | void reset(); 37 | void init(const Eigen::Matrix4f& tf, const Matrix6f& cov); 38 | bool isCalibrated(); 39 | 40 | private: 41 | void handleImu(const vi_estimator_msgs::ImuSimpleConstPtr& msg_imu); 42 | void initSensors(); 43 | void storeMeasurements(Vector3d a, Vector3d w, double dt); 44 | void publishTf(); 45 | void monitorQueue(); 46 | 47 | ros::NodeHandle nh; 48 | 49 | ros::Subscriber imu_sub; 50 | //ros::Timer timer_send_tf; 51 | 52 | InsKalmanPredictor* kp_ins; 53 | InsPoseKalmanCorrector* kc_inspose; 54 | ImuKalmanPredictor* kp_imu; 55 | ImuKalmanCorrector* kc_imu; 56 | 57 | Imu imu; // Imu for prediction 58 | ImuSensor<3> imu_sensor; // Imu for correction 59 | Ins ins; // Prediction for pose tracker 60 | InsPose<> inspose; // Correction step coming from pose tracker 61 | InsState ins_state; // Tracks full state with respect to tracked object 62 | InsState ins_pre_correct_state; // Tracks full state with respect to tracked object, prediction applied before correction 63 | ImuState imu_state; // Tracks rotation with respect to world frame 64 | 65 | Vector3d filtered_acc; 66 | Vector3d gyro_bias, gyro_bias_var; 67 | Vector3d acc_bias, acc_bias_var; 68 | 69 | const double acc_gravity = 9.80665; 70 | const int num_bias_samples = 1500; 71 | bool estimated_bias; 72 | std::vector bias_samples, acc_bias_samples; 73 | Eigen::Matrix3d cam_transform; 74 | 75 | ros::Time t_epoch_0; 76 | ros::Time t_epoch_prev; 77 | 78 | std::vector a_store, w_store; 79 | std::vector dt_store; 80 | 81 | boost::thread callback_thread; 82 | boost::mutex data_store_mutex; 83 | ros::CallbackQueue callback_queue; 84 | }; 85 | 86 | #endif 87 | -------------------------------------------------------------------------------- /include/mesh_localize/ImageDbUtil.h: -------------------------------------------------------------------------------- 1 | #ifndef _IMAGE_DB_UTIL_H_ 2 | #define _IMAGE_DB_UTIL_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include "CameraContainer.h" 13 | #include "KeyframeContainer.h" 14 | 15 | class ImageDbUtil 16 | { 17 | public: 18 | 19 | static Eigen::Matrix4f StringToMatrix4f(std::string str); 20 | static bool LoadPhotoscanFile(std::string filename, std::vector& cameras, Mat map_Kcv, Mat map_distcoeffcv); 21 | static bool LoadOgreDataDir(std::string data_dir, std::vector& keyframes); 22 | }; 23 | #endif 24 | -------------------------------------------------------------------------------- /include/mesh_localize/KLTTracker.h: -------------------------------------------------------------------------------- 1 | #ifndef _KLTTracker_hpp 2 | #define _KLTTracker_hpp 3 | 4 | #include 5 | #include 6 | 7 | class KLTTracker 8 | { 9 | 10 | public: 11 | KLTTracker(); 12 | void init(const cv::Mat& inputFrame, const cv::Mat& depth, const Eigen::Matrix3f& inputK, 13 | const Eigen::Matrix3f& depthK, const Eigen::Matrix4f& inputTf, const cv::Mat& mask); 14 | virtual bool processFrame(const cv::Mat& inputFrame, cv::Mat& outputFrame, 15 | std::vector& pts2d, std::vector& pts3d, std::vector& ptIDs); 16 | std::vector filterMatchesEpipolarContraint(const std::vector& pts1, 17 | const std::vector& pts2); 18 | void filterPointsByIndex(const std::vector& idxs); 19 | 20 | private: 21 | int m_maxNumberOfPoints; 22 | 23 | cv::Mat m_prevImg; 24 | cv::Mat m_nextImg; 25 | cv::Mat m_mask; 26 | 27 | std::vector m_prevPts; 28 | std::vector m_nextPts; 29 | std::vector m_tracked3dPts; 30 | 31 | std::vector m_prevKeypoints; 32 | std::vector m_nextKeypoints; 33 | 34 | std::vector m_ptIDs; 35 | int m_nextID; 36 | 37 | std::vector m_status; 38 | std::vector m_error; 39 | 40 | cv::Ptr m_fastDetector; 41 | }; 42 | #endif 43 | -------------------------------------------------------------------------------- /include/mesh_localize/KeyframeContainer.h: -------------------------------------------------------------------------------- 1 | #ifndef _KEYFRAMECONTAINER_H_ 2 | #define _KEYFRAMECONTAINER_H_ 3 | 4 | #include "CameraContainer.h" 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #ifdef MESH_LOCALIZER_ENABLE_GPU 14 | #include 15 | #endif 16 | 17 | #include 18 | #include 19 | 20 | using namespace cv; 21 | 22 | class KeyframeContainer 23 | { 24 | public: 25 | KeyframeContainer(Mat img, std::string desc_type = "asurf", bool extract_now = true); 26 | KeyframeContainer(Mat img, std::vector& keypoints, Mat& descriptors); 27 | KeyframeContainer(Mat img, std::vector& keypoints, Mat& descriptors, Mat& depth); 28 | KeyframeContainer(CameraContainer* cc, std::string desc_type = "asurf"); 29 | KeyframeContainer(CameraContainer* cc, std::vector& keypoints, Mat& descriptors); 30 | KeyframeContainer(CameraContainer* cc, std::vector& keypoints, Mat& descriptors, Mat& depth); 31 | ~KeyframeContainer(); 32 | KeyframeContainer(const KeyframeContainer&); 33 | 34 | Mat GetImage(); 35 | Mat GetDescriptors(); 36 | Mat GetDepth(); 37 | #ifdef MESH_LOCALIZER_ENABLE_GPU 38 | gpu::GpuMat GetGPUDescriptors(); 39 | #endif 40 | std::vector GetKeypoints(); 41 | Eigen::Matrix4f GetTf(); 42 | Eigen::Matrix3f GetK(); 43 | 44 | void ExtractFeatures(); 45 | void SetMask(Mat new_mask); 46 | private: 47 | 48 | void ExtractFeatures(std::string desc_type); 49 | 50 | CameraContainer* cc; 51 | std::vector keypoints; 52 | Mat descriptors; 53 | #ifdef MESH_LOCALIZER_ENABLE_GPU 54 | gpu::GpuMat descriptors_gpu; 55 | #endif 56 | Mat depth; //May not be used 57 | Mat mask; 58 | string desc_type; 59 | 60 | bool delete_cc; 61 | bool has_depth; 62 | }; 63 | 64 | #endif 65 | -------------------------------------------------------------------------------- /include/mesh_localize/KeyframeMatch.h: -------------------------------------------------------------------------------- 1 | #ifndef _KEYFRAMEMATCH_H_ 2 | #define _KEYFRAMEMATCH_H_ 3 | 4 | #include "KeyframeContainer.h" 5 | 6 | class KeyframeMatch 7 | { 8 | public: 9 | KeyframeMatch(KeyframeContainer* kfc, std::vector< DMatch >& matches, std::vector< DMatch >& allMatches, std::vector < Point2f >& matchPts1, std::vector< Point2f >& matchPts2, std::vector< KeyPoint > matchKps1, std::vector< KeyPoint > matchKps2); 10 | 11 | KeyframeContainer* kfc; 12 | std::vector< DMatch > matches; 13 | std::vector< DMatch > allMatches; 14 | std::vector< Point2f > matchPts1; 15 | std::vector< Point2f > matchPts2; 16 | std::vector< KeyPoint > matchKps1; 17 | std::vector< KeyPoint > matchKps2; 18 | 19 | bool operator< (const KeyframeMatch& kfm) const; 20 | private: 21 | mutable int tot_dist; 22 | }; 23 | 24 | #endif 25 | -------------------------------------------------------------------------------- /include/mesh_localize/MapFeatures.h: -------------------------------------------------------------------------------- 1 | #ifndef _MAPFEATURES_H_ 2 | #define _MAPFEATURES_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include "KeyframeContainer.h" 12 | 13 | using namespace cv; 14 | 15 | class MapFeatures 16 | { 17 | public: 18 | MapFeatures(std::vector& kcv, pcl::PointCloud::Ptr cloud); 19 | MapFeatures(){}; 20 | 21 | Mat GetDescriptors() const; 22 | std::vector GetKeypoints() const; 23 | private: 24 | std::vector kcv; 25 | pcl::PointCloud::Ptr cloud; 26 | std::vector keypoints; 27 | Mat descriptors; 28 | }; 29 | 30 | #endif 31 | -------------------------------------------------------------------------------- /include/mesh_localize/MeshLocalizer.h: -------------------------------------------------------------------------------- 1 | #ifndef _MAPLOCALIZER_H_ 2 | #define _MAPLOCALIZER_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include "tf/transform_broadcaster.h" 8 | #include "sensor_msgs/Image.h" 9 | #include "sensor_msgs/CameraInfo.h" 10 | 11 | #include "MonocularLocalizer.h" 12 | #include "VirtualImageGenerator.h" 13 | #include "KeyframeContainer.h" 14 | #include "KeyframeMatch.h" 15 | #include "MapFeatures.h" 16 | #include "EdgeTrackingUtil.h" 17 | //#include "IMUMotionModel.h" 18 | #include "KLTTracker.h" 19 | 20 | #include "pcl_ros/point_cloud.h" 21 | #include 22 | #include 23 | #include 24 | class MeshLocalizer 25 | { 26 | 27 | enum LocalizeState 28 | { 29 | INIT, 30 | INIT_PNP, 31 | LOCAL_INIT, 32 | PNP, 33 | EDGES, 34 | KLT_INIT, 35 | KLT 36 | } localize_state; 37 | 38 | public: 39 | 40 | MeshLocalizer(ros::NodeHandle nh, ros::NodeHandle nh_private); 41 | ~MeshLocalizer(); 42 | 43 | private: 44 | Eigen::Matrix4f FindImageTfPnp(KeyframeContainer* kcv, const MapFeatures& mf); 45 | bool FindImageTfVirtualPnp(KeyframeContainer* kcv, Eigen::Matrix4f vimgTf, Eigen::Matrix4f& out, std::string vdesc_type, bool mask_kf, Eigen::Matrix& cov); 46 | bool FindImageTfVirtualEdges(KeyframeContainer* kcv, Eigen::Matrix4f vimgTf, Eigen::Matrix4f& out, bool mask_kf); 47 | std::vector GetPointCloudFromFrames(KeyframeContainer*, KeyframeContainer*); 48 | std::vector FindPlaneInPointCloud(const std::vector& pts); 49 | Mat GetVirtualImageFromTopic(Mat& depths, Mat& mask); 50 | Mat GenerateVirtualImage(Eigen::Matrix4f tf, Eigen::Matrix3f K, int height, int width, pcl::PointCloud::Ptr cloud, Mat& depth, Mat& mask); 51 | 52 | void UpdateMotionModel(const Eigen::Matrix4f& olfTf, const Eigen::Matrix4f& newTf, 53 | const Eigen::Matrix& cov, double dt); 54 | Eigen::Matrix4f ApplyMotionModel(double dt); 55 | void ResetMotionModel(); 56 | 57 | void PublishDepthMat(const Mat& depth, ros::Time stamp); 58 | void PublishPose(Eigen::Matrix4f tf); 59 | void PublishMap(); 60 | void PublishPointCloud(const std::vector&); 61 | void PublishPointCloud(pcl::PointCloud::Ptr pc); 62 | void PlotTf(Eigen::Matrix4f tf, std::string name); 63 | 64 | void spin(const ros::TimerEvent& e); 65 | void HandleImage(const sensor_msgs::ImageConstPtr& msg); 66 | void HandleVirtualImage(const sensor_msgs::ImageConstPtr& msg); 67 | void HandleVirtualDepth(const sensor_msgs::ImageConstPtr& msg); 68 | void UpdateVirtualSensorState(Eigen::Matrix4f tf); 69 | 70 | std::vector PCLToPoint3d(const std::vector& cpvec); 71 | void PublishProcessedImageAndDepth(const cv::Mat& image, const cv::Mat& depth, ros::Time stamp); 72 | void ReprojectMask(cv::Mat& dst, const cv::Mat& src, const Eigen::Matrix3f& dstK, 73 | const Eigen::Matrix3f& srcK, bool median_blur = true); 74 | void TransformDepthFrame(const Mat& d1, const Eigen::Matrix4f& tf1, const Eigen::Matrix3f K1, 75 | Mat& d2, const Eigen::Matrix4f& tf2, const Eigen::Matrix3f& K2); 76 | void CreateTfViz(Mat& src, Mat& dst, const Eigen::Matrix4f& tf, 77 | const Eigen::Matrix3f& K); 78 | 79 | std::vector cameras; 80 | 81 | ros::Time img_time_stamp; 82 | Mat current_image; 83 | Mat current_virtual_image; 84 | sensor_msgs::ImageConstPtr current_virtual_depth_msg; 85 | 86 | Mat virtual_depth; 87 | std::vector positionList; 88 | Eigen::Matrix4f currentPose; 89 | bool get_frame; 90 | bool get_virtual_image; 91 | bool get_virtual_depth; 92 | int numPnpRetrys; 93 | int numLocalizeRetrys; 94 | double pnpReprojError; 95 | double pixel_noise; 96 | double virtual_fx; 97 | double virtual_fy; 98 | 99 | MonocularLocalizer* localization_init; 100 | VirtualImageGenerator* vig; 101 | 102 | ros::Time spin_time; 103 | 104 | MapFeatures map_features; 105 | 106 | std::string ogre_data_dir; 107 | std::string ogre_cfg_dir; 108 | std::string ogre_model; 109 | std::string pc_filename; 110 | std::string mesh_filename; 111 | std::string photoscan_filename; 112 | std::string virtual_image_source; 113 | std::string pnp_descriptor_type; 114 | std::string img_match_descriptor_type; 115 | bool show_pnp_matches; 116 | bool show_global_matches; 117 | bool show_debug; 118 | std::string tracking_mode; 119 | std::string global_localization_alg; 120 | double image_scale; 121 | double canny_high_thresh; 122 | double canny_low_thresh; 123 | double canny_sigma; 124 | bool autotune_canny; 125 | double edge_tracking_dmax; 126 | int edge_tracking_iterations; 127 | double pnp_match_radius; 128 | int min_pnp_inliers; 129 | double max_pnp_reproj_error; 130 | double ratio_test_thresh; 131 | std::string motion_model; 132 | bool do_undistort; 133 | bool use_depth_shader; 134 | 135 | ros::NodeHandle nh; 136 | ros::NodeHandle nh_private; 137 | 138 | ros::ServiceClient gazebo_client; 139 | ros::Publisher estimated_pose_pub; 140 | ros::Publisher map_marker_pub; 141 | ros::Publisher pointcloud_pub; 142 | ros::Publisher image_pub; 143 | ros::Publisher image_cam_info_pub; 144 | ros::Publisher depth_pub; 145 | tf::TransformBroadcaster br; 146 | 147 | ros::Subscriber image_sub; 148 | ros::Subscriber virtual_image_sub; 149 | ros::Subscriber virtual_depth_sub; 150 | 151 | ros::Timer timer; 152 | 153 | Eigen::Matrix4f camera_velocity; 154 | ros::Time last_spin_time; 155 | 156 | Eigen::Matrix3f K; 157 | Eigen::Matrix3f K_scaled; 158 | Eigen::Matrix3f map_K; 159 | Eigen::Matrix3f virtual_K; 160 | Eigen::VectorXf distcoeff; 161 | Eigen::VectorXf map_distcoeff; 162 | Mat Kcv; 163 | Mat Kcv_undistort; 164 | Mat map_Kcv; 165 | Mat virtual_Kcv; 166 | Mat distcoeffcv; 167 | Mat map_distcoeffcv; 168 | int virtual_height; 169 | int virtual_width; 170 | bool init_undistort; 171 | Mat undistort_map1, undistort_map2; 172 | 173 | //IMUMotionModel * imu_mm; 174 | 175 | KLTTracker klt_tracker; 176 | Mat klt_init_img; 177 | }; 178 | 179 | #endif 180 | -------------------------------------------------------------------------------- /include/mesh_localize/MonocularLocalizer.h: -------------------------------------------------------------------------------- 1 | #ifndef _MONOCULAR_LOCALIZER_H_ 2 | #define _MONOCULAR_LOCALIZER_H_ 3 | 4 | #include 5 | #include 6 | 7 | class MonocularLocalizer 8 | { 9 | public: 10 | virtual bool localize(const cv::Mat& img, const cv::Mat& K, Eigen::Matrix4f* pose, Eigen::Matrix4f* pose_guess = NULL) = 0; 11 | }; 12 | 13 | #endif 14 | -------------------------------------------------------------------------------- /include/mesh_localize/OgreImageGenerator.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef _OGRE_IMAGE_GENERATOR_ 3 | #define _OGRE_IMAGE_GENERATOR_ 4 | 5 | #include "VirtualImageGenerator.h" 6 | #include "object_renderer/virtual_image_handler.h" 7 | 8 | class OgreImageGenerator : public VirtualImageGenerator 9 | { 10 | public: 11 | OgreImageGenerator(std::string resource_path, std::string model_name, double fx = 400, 12 | double fy = 400, bool use_depth_shader = true); 13 | virtual cv::Mat GenerateVirtualImage(const Eigen::Matrix4f& pose, cv::Mat& depth, cv::Mat& mask); 14 | virtual Eigen::Matrix3f GetK(); 15 | double GetWidth(); 16 | double GetHeight(); 17 | 18 | private: 19 | CameraRenderApplication* app; 20 | VirtualImageHandler* vih; 21 | bool use_depth_shader; 22 | }; 23 | #endif 24 | -------------------------------------------------------------------------------- /include/mesh_localize/PnPUtil.h: -------------------------------------------------------------------------------- 1 | #ifndef _PNP_UTIL_H_ 2 | #define _PNP_UTIL_H_ 3 | 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | class PnPUtil 12 | { 13 | public: 14 | static std::vector BackprojectPts(const std::vector& pts, 15 | const Eigen::Matrix4f& camTf, const Eigen::Matrix3f& K, const cv::Mat& depth); 16 | static bool RansacPnP(const std::vector& matchPts3d, 17 | const std::vector& matchPts, cv::Mat Kcv, Eigen::Matrix4f tfguess, 18 | Eigen::Matrix4f& tf, std::vector& inlierIdx, double* avgReprojError = NULL, 19 | Eigen::Matrix* cov = NULL); 20 | }; 21 | #endif 22 | -------------------------------------------------------------------------------- /include/mesh_localize/PointCloudImageGenerator.h: -------------------------------------------------------------------------------- 1 | #ifndef _POINTCLOUD_IMAGE_GENERATOR_ 2 | #define _POINTCLOUD_IMAGE_GENERATOR_ 3 | 4 | #include "VirtualImageGenerator.h" 5 | #include "pcl_ros/point_cloud.h" 6 | #include 7 | #include 8 | #include 9 | 10 | class PointCloudImageGenerator : public VirtualImageGenerator 11 | { 12 | public: 13 | PointCloudImageGenerator(pcl::PointCloud::Ptr pc, const Eigen::Matrix3f& K, int rows, int cols); 14 | virtual cv::Mat GenerateVirtualImage(const Eigen::Matrix4f& pose, cv::Mat& depth, cv::Mat& mask); 15 | virtual Eigen::Matrix3f GetK(); 16 | 17 | 18 | private: 19 | pcl::PointCloud::Ptr map_cloud; 20 | Eigen::Matrix3f K; 21 | int rows; 22 | int cols; 23 | }; 24 | #endif 25 | -------------------------------------------------------------------------------- /include/mesh_localize/Triangulation.h: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | * ExploringSfMWithOpenCV 3 | ****************************************************************************** 4 | * by Roy Shilkrot, 5th Dec 2012 5 | * http://www.morethantechnical.com/ 6 | ****************************************************************************** 7 | * Ch4 of the book "Mastering OpenCV with Practical Computer Vision Projects" 8 | * Copyright Packt Publishing 2012. 9 | * http://www.packtpub.com/cool-projects-with-opencv/book 10 | *****************************************************************************/ 11 | 12 | #pragma once 13 | 14 | #undef __SFM__DEBUG__ 15 | 16 | #include 17 | #include 18 | #include 19 | #ifdef __SFM__DEBUG__ 20 | #include 21 | #include 22 | #endif 23 | #include 24 | 25 | #include "Common.h" 26 | 27 | /** 28 | From "Triangulation", Hartley, R.I. and Sturm, P., Computer vision and image understanding, 1997 29 | */ 30 | cv::Mat_ LinearLSTriangulation(cv::Point3d u, //homogenous image point (u,v,1) 31 | cv::Matx34d P, //camera 1 matrix 32 | cv::Point3d u1, //homogenous image point in 2nd camera 33 | cv::Matx34d P1, //camera 2 matrix 34 | double* reprojError = NULL 35 | ); 36 | 37 | #define EPSILON 0.0001 38 | /** 39 | From "Triangulation", Hartley, R.I. and Sturm, P., Computer vision and image understanding, 1997 40 | */ 41 | cv::Mat_ IterativeLinearLSTriangulation(cv::Point3d u, //homogenous image point (u,v,1) 42 | cv::Matx34d P, //camera 1 matrix 43 | cv::Point3d u1, //homogenous image point in 2nd camera 44 | cv::Matx34d P1 //camera 2 matrix 45 | ); 46 | 47 | double TriangulatePoints(const std::vector& pt_set1, 48 | const std::vector& pt_set2, 49 | const cv::Matx33d& K, 50 | const cv::Matx33d& Kinv, 51 | const cv::Mat& distcoeff, 52 | const cv::Matx34d& P, 53 | const cv::Matx34d& P1, 54 | std::vector& pointcloud, 55 | std::vector& correspImg1Pt); 56 | -------------------------------------------------------------------------------- /include/mesh_localize/VirtualImageGenerator.h: -------------------------------------------------------------------------------- 1 | #ifndef _VIRTUAL_IMAGE_GENERATOR_ 2 | #define _VIRTUAL_IMAGE_GENERATOR_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | class VirtualImageGenerator 9 | { 10 | public: 11 | virtual cv::Mat GenerateVirtualImage(const Eigen::Matrix4f& pose, cv::Mat& depth, cv::Mat& mask) = 0; 12 | virtual Eigen::Matrix3f GetK() = 0; 13 | }; 14 | #endif 15 | -------------------------------------------------------------------------------- /launch/gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /launch/gazebo_campus.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /launch/gazebo_debug.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /launch/localize.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /launch/localize_box_ogre.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /launch/localize_box_ogre_debug.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /launch/localize_box_ogre_test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /launch/localize_box_ogre_valgrind.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /launch/localize_campus_fabmap.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /launch/localize_campus_matching.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /launch/localize_car.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /launch/localize_cheezit_ogre.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /launch/localize_cheezit_ogre_imu.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /launch/localize_cheezit_ogre_quad.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /launch/localize_debug.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /launch/localize_lab_fabmap.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /launch/localize_lab_matching.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /launch/localize_lab_ogre.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /launch/localize_pringles_ogre.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /launch/mesh_localize.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 108 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /TF1 10 | - /TF1/Status1 11 | - /TF1/Frames1 12 | - /Map1 13 | - /Estimated Location1 14 | - /Estimated Path1 15 | - /Matched Points1 16 | - /Camera1 17 | - /Camera1/Status1 18 | - /Camera1/Visibility1 19 | - /Image1 20 | Splitter Ratio: 0.5 21 | Tree Height: 525 22 | - Class: rviz/Selection 23 | Name: Selection 24 | - Class: rviz/Tool Properties 25 | Expanded: 26 | - /2D Pose Estimate1 27 | - /2D Nav Goal1 28 | - /Publish Point1 29 | Name: Tool Properties 30 | Splitter Ratio: 0.588679 31 | - Class: rviz/Views 32 | Expanded: 33 | - /Current View1 34 | Name: Views 35 | Splitter Ratio: 0.5 36 | - Class: rviz/Time 37 | Experimental: false 38 | Name: Time 39 | SyncMode: 0 40 | SyncSource: Image 41 | Visualization Manager: 42 | Class: "" 43 | Displays: 44 | - Alpha: 0.5 45 | Cell Size: 1 46 | Class: rviz/Grid 47 | Color: 160; 160; 164 48 | Enabled: true 49 | Line Style: 50 | Line Width: 0.03 51 | Value: Lines 52 | Name: Grid 53 | Normal Cell Count: 0 54 | Offset: 55 | X: 0 56 | Y: 0 57 | Z: 0 58 | Plane: XY 59 | Plane Cell Count: 10 60 | Reference Frame: 61 | Value: true 62 | - Class: rviz/TF 63 | Enabled: true 64 | Frame Timeout: 15 65 | Frames: 66 | All Enabled: false 67 | camera: 68 | Value: true 69 | camera_pose: 70 | Value: true 71 | map_localize: 72 | Value: true 73 | markers: 74 | Value: true 75 | object_pose: 76 | Value: true 77 | world: 78 | Value: true 79 | Marker Scale: 1 80 | Name: TF 81 | Show Arrows: true 82 | Show Axes: true 83 | Show Names: true 84 | Tree: 85 | camera: 86 | {} 87 | map_localize: 88 | world: 89 | camera_pose: 90 | {} 91 | markers: 92 | {} 93 | object_pose: 94 | {} 95 | Update Interval: 0 96 | Value: true 97 | - Class: rviz/Marker 98 | Enabled: false 99 | Marker Topic: /map_localize/map 100 | Name: Map 101 | Namespaces: 102 | {} 103 | Queue Size: 0 104 | Value: false 105 | - Class: rviz/Marker 106 | Enabled: false 107 | Marker Topic: /map_localize/estimated_position 108 | Name: Estimated Location 109 | Namespaces: 110 | {} 111 | Queue Size: 100 112 | Value: false 113 | - Class: rviz/Marker 114 | Enabled: false 115 | Marker Topic: /map_localize/estimated_path 116 | Name: Estimated Path 117 | Namespaces: 118 | {} 119 | Queue Size: 100 120 | Value: false 121 | - Class: rviz/Marker 122 | Enabled: false 123 | Marker Topic: /map_localize/match_points 124 | Name: Matched Points 125 | Namespaces: 126 | {} 127 | Queue Size: 100 128 | Value: false 129 | - Class: rviz/Camera 130 | Enabled: true 131 | Image Rendering: background and overlay 132 | Image Topic: /camera/image_color 133 | Name: Camera 134 | Overlay Alpha: 0.5 135 | Queue Size: 2 136 | Transport Hint: raw 137 | Value: true 138 | Visibility: 139 | Estimated Location: true 140 | Estimated Path: true 141 | Grid: true 142 | Image: true 143 | Map: true 144 | Matched Points: true 145 | TF: true 146 | Value: true 147 | Zoom Factor: 1 148 | - Class: rviz/Image 149 | Enabled: false 150 | Image Topic: /camera/image_color 151 | Max Value: 1 152 | Median window: 5 153 | Min Value: 0 154 | Name: Image 155 | Normalize Range: true 156 | Queue Size: 2 157 | Transport Hint: raw 158 | Value: false 159 | Enabled: true 160 | Global Options: 161 | Background Color: 0; 0; 0 162 | Fixed Frame: camera 163 | Frame Rate: 30 164 | Name: root 165 | Tools: 166 | - Class: rviz/Interact 167 | Hide Inactive Objects: true 168 | - Class: rviz/MoveCamera 169 | - Class: rviz/Select 170 | - Class: rviz/FocusCamera 171 | - Class: rviz/Measure 172 | - Class: rviz/SetInitialPose 173 | Topic: /initialpose 174 | - Class: rviz/SetGoal 175 | Topic: /move_base_simple/goal 176 | - Class: rviz/PublishPoint 177 | Single click: true 178 | Topic: /clicked_point 179 | Value: true 180 | Views: 181 | Current: 182 | Class: rviz/Orbit 183 | Distance: 5.95279 184 | Enable Stereo Rendering: 185 | Stereo Eye Separation: 0.06 186 | Stereo Focal Distance: 1 187 | Swap Stereo Eyes: false 188 | Value: false 189 | Focal Point: 190 | X: -1.14797 191 | Y: 0.651923 192 | Z: -0.577302 193 | Name: Current View 194 | Near Clip Distance: 0.01 195 | Pitch: 0.375203 196 | Target Frame: 197 | Value: Orbit (rviz) 198 | Yaw: 6.20032 199 | Saved: ~ 200 | Window Geometry: 201 | Camera: 202 | collapsed: false 203 | Displays: 204 | collapsed: false 205 | Height: 1056 206 | Hide Left Dock: false 207 | Hide Right Dock: true 208 | Image: 209 | collapsed: false 210 | QMainWindow State: 000000ff00000000fd000000040000000000000169000003dafc020000000bfb0000001200530065006c0065006300740069006f006e00000000280000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002ba000000dd00fffffffb0000000c00430061006d00650072006101000002e80000011a0000001600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000028c0000015a0000000000000000fb0000000a0049006d0061006700650000000335000000cd0000001600ffffff000000010000010f000003dafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000003da000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003ce0000003efc0100000002fb0000000800540069006d00650000000000000003ce000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000005d0000003da00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 211 | Selection: 212 | collapsed: false 213 | Time: 214 | collapsed: false 215 | Tool Properties: 216 | collapsed: false 217 | Views: 218 | collapsed: true 219 | Width: 1855 220 | X: 65 221 | Y: 24 222 | -------------------------------------------------------------------------------- /launch/mesh_localize.rviz.vcg: -------------------------------------------------------------------------------- 1 | Actual\ Position.Enabled=0 2 | Actual\ Position.Marker\ Topic=/map_localize/actual_position 3 | Actual\ Position.Queue\ Size=0 4 | Background\ ColorB=0 5 | Background\ ColorG=0 6 | Background\ ColorR=0 7 | Camera\ Config=1.5698 1.4604 33.8951 -6.48201 0.264907 -5.35071 8 | Camera\ Type=rviz::OrbitViewController 9 | Estimated\ Path.Enabled=1 10 | Estimated\ Path.Marker\ Topic=/map_localize/estimated_path 11 | Estimated\ Path.Queue\ Size=100 12 | Estimated\ Path.map_localize=1 13 | Estimated\ Position.Enabled=1 14 | Estimated\ Position.Marker\ Topic=/map_localize/estimated_position 15 | Estimated\ Position.Queue\ Size=0 16 | Estimated\ Position.map_localize=1 17 | Fixed\ Frame=/world 18 | Frames.All\ Enabled=0 19 | Frames.Enabled=0 20 | Frames.Frame\ Timeout=1e+07 21 | Frames.Marker\ Scale=7 22 | Frames.Show\ Arrows=0 23 | Frames.Show\ Axes=1 24 | Frames.Show\ Names=1 25 | Frames.Update\ Interval=0 26 | Grid.Alpha=0.5 27 | Grid.Cell\ Size=1 28 | Grid.ColorB=0.5 29 | Grid.ColorG=0.5 30 | Grid.ColorR=0.5 31 | Grid.Enabled=0 32 | Grid.Line\ Style=0 33 | Grid.Line\ Width=0.03 34 | Grid.Normal\ Cell\ Count=0 35 | Grid.OffsetX=0 36 | Grid.OffsetY=0 37 | Grid.OffsetZ=0 38 | Grid.Plane=0 39 | Grid.Plane\ Cell\ Count=100 40 | Grid.Reference\ Frame=/world 41 | Map.Enabled=1 42 | Map.Marker\ Topic=/map_localize/map 43 | Map.Queue\ Size=0 44 | Map.map_localize=1 45 | Match\ Positions.Enabled=1 46 | Match\ Positions.Marker\ Topic=/map_localize/match_points 47 | Match\ Positions.Queue\ Size=0 48 | Match\ Positions.map_localize=1 49 | Match\ T\ Vectors.Enabled=1 50 | Match\ T\ Vectors.Marker\ Array\ Topic=/map_localize/t_vectors 51 | Match\ T\ Vectors.Queue\ Size=0 52 | Match\ T\ Vectors.map_localize=1 53 | Point\ Cloud..AxisColorAutocompute\ Value\ Bounds=1 54 | Point\ Cloud..AxisColorAxis=2 55 | Point\ Cloud..AxisColorMax\ Value=10 56 | Point\ Cloud..AxisColorMin\ Value=-10 57 | Point\ Cloud..AxisColorUse\ Fixed\ Frame=1 58 | Point\ Cloud..FlatColorColorB=1 59 | Point\ Cloud..FlatColorColorG=1 60 | Point\ Cloud..FlatColorColorR=1 61 | Point\ Cloud..IntensityAutocompute\ Intensity\ Bounds=1 62 | Point\ Cloud..IntensityChannel\ Name=intensity 63 | Point\ Cloud..IntensityMax\ ColorB=1 64 | Point\ Cloud..IntensityMax\ ColorG=1 65 | Point\ Cloud..IntensityMax\ ColorR=1 66 | Point\ Cloud..IntensityMax\ Intensity=4096 67 | Point\ Cloud..IntensityMin\ ColorB=0 68 | Point\ Cloud..IntensityMin\ ColorG=0 69 | Point\ Cloud..IntensityMin\ ColorR=0 70 | Point\ Cloud..IntensityMin\ Intensity=0 71 | Point\ Cloud..IntensityUse\ full\ RGB\ spectrum=0 72 | Point\ Cloud.Alpha=1 73 | Point\ Cloud.Billboard\ Size=0.01 74 | Point\ Cloud.Color\ Transformer=Intensity 75 | Point\ Cloud.Decay\ Time=0 76 | Point\ Cloud.Enabled=0 77 | Point\ Cloud.Position\ Transformer=XYZ 78 | Point\ Cloud.Queue\ Size=10 79 | Point\ Cloud.Selectable=1 80 | Point\ Cloud.Style=1 81 | Point\ Cloud.Topic=/map_localize/pointcloud 82 | Property\ Grid\ Splitter=771,78 83 | Property\ Grid\ State=expanded=.Global Options,.Global Status,Map.Enabled,Map.Status,Match Positions.Enabled,Match Positions.Status,Match T Vectors.Enabled,Estimated Position.Enabled,Actual Position.Enabled,Frames.Enabled,Frames.Frames;splitterratio=0.486979 84 | QMainWindow=000000ff00000000fd000000030000000000000192000003a3fc0200000002fb000000100044006900730070006c006100790073000000001d000003a3000000ee00fffffffb0000001600410063007400750061006c00200050006100740068000000038f000000b90000004c00ffffff0000000100000131000003e3fc0200000003fb0000001e0054006f006f006c002000500072006f0070006500720074006900650073000000001d000000ff0000006700fffffffb0000000a00560069006500770073000000001d00000283000000bb00fffffffb0000001200530065006c0065006300740069006f006e000000001d000003e30000006700ffffff000000030000039f0000003efc0100000001fb0000000800540069006d006501000000000000039f000002bf00ffffff0000039f000003a300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 85 | Target\ Frame= 86 | Tool\ 2D\ Nav\ GoalTopic=/move_base_simple/goal 87 | Tool\ 2D\ Pose\ EstimateTopic=initialpose 88 | [Display0] 89 | ClassName=rviz::MarkerDisplay 90 | Name=Map 91 | [Display1] 92 | ClassName=rviz::MarkerDisplay 93 | Name=Match Positions 94 | [Display2] 95 | ClassName=rviz::MarkerArrayDisplay 96 | Name=Match T Vectors 97 | [Display3] 98 | ClassName=rviz::MarkerDisplay 99 | Name=Estimated Position 100 | [Display4] 101 | ClassName=rviz::MarkerDisplay 102 | Name=Actual Position 103 | [Display5] 104 | ClassName=rviz::MarkerDisplay 105 | Name=Estimated Path 106 | [Display6] 107 | ClassName=rviz::GridDisplay 108 | Name=Grid 109 | [Display7] 110 | ClassName=rviz::PointCloud2Display 111 | Name=Point Cloud 112 | [Display8] 113 | ClassName=rviz::TFDisplay 114 | Name=Frames 115 | [Panel0] 116 | ClassLookupName=rviz_plugin_tutorials/Teleop 117 | Name=Actual Path 118 | Topic= 119 | [Window] 120 | Height=1064 121 | Width=943 122 | X=1045 123 | Y=-31 124 | -------------------------------------------------------------------------------- /launch/mesh_localize_rosserial.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /launch/rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /ogre_cfg/ogre.cfg: -------------------------------------------------------------------------------- 1 | Render System=OpenGL Rendering Subsystem 2 | 3 | [OpenGL Rendering Subsystem] 4 | Display Frequency=50 Hz 5 | FSAA=0 6 | Fixed Pipeline Enabled=Yes 7 | Full Screen=No 8 | RTT Preferred Mode=FBO 9 | VSync=Yes 10 | Video Mode= 640 x 480 11 | sRGB Gamma Conversion=No 12 | -------------------------------------------------------------------------------- /ogre_cfg/plugins.cfg: -------------------------------------------------------------------------------- 1 | # Defines plugins to load 2 | 3 | # Define plugin folder 4 | PluginFolder=/usr/local/lib/OGRE 5 | 6 | # Define plugins 7 | # Plugin=RenderSystem_Direct3D9 8 | # Plugin=RenderSystem_Direct3D10 9 | # Plugin=RenderSystem_Direct3D11 10 | Plugin=RenderSystem_GL 11 | # Plugin=RenderSystem_GLES 12 | Plugin=Plugin_ParticleFX 13 | Plugin=Plugin_BSPSceneManager 14 | Plugin=Plugin_CgProgramManager 15 | Plugin=Plugin_PCZSceneManager 16 | Plugin=Plugin_OctreeZone 17 | Plugin=Plugin_OctreeSceneManager 18 | -------------------------------------------------------------------------------- /ogre_cfg/resources.cfg: -------------------------------------------------------------------------------- 1 | # Resources required by the sample browser and most samples. 2 | [Essential] 3 | #Zip=/home/matt/object_renderer/media/models/box/box.zip 4 | #Zip=/home/matt/object_renderer/media/models/lab_cage/lab_cage.zip 5 | #Zip=/home/matt/object_renderer/media/models/drill/drill_side1.zip 6 | Zip=/home/matt/object_renderer/media/models/cheezit/cheezit2.zip 7 | #Zip=/home/matt/object_renderer/media/models/multimeter/multimeter.zip 8 | #Zip=/home/matt/object_renderer/media/models/cord/cord.zip 9 | #Zip=/home/matt/object_renderer/media/models/makita_drill/makita_drill.zip 10 | #Zip=/home/matt/object_renderer/media/models/pringles/pringles.zip 11 | FileSystem=/home/matt/object_renderer/media/materials/scripts 12 | FileSystem=/home/matt/object_renderer/media/materials/programs 13 | 14 | #Zip=/usr/local/share/OGRE/Media/packs/SdkTrays.zip 15 | 16 | 17 | 18 | # Resource locations to be added to the default path 19 | 20 | #[General] 21 | 22 | #FileSystem=/usr/local/share/OGRE/Media 23 | 24 | #FileSystem=/usr/local/share/OGRE/Media/materials/scripts 25 | #FileSystem=/usr/local/share/OGRE/Media/materials/programs/Cg 26 | #FileSystem=/usr/local/share/OGRE/Media/materials/programs/GLSL 27 | 28 | #FileSystem=/usr/local/share/OGRE/Media/materials/textures 29 | 30 | #FileSystem=/usr/local/share/OGRE/Media/models 31 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mesh_localize 4 | 0.0.0 5 | The mesh_localize package 6 | 7 | 8 | 9 | 10 | matt 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | cv_bridge 44 | gazebo_msgs 45 | image_transport 46 | libpcl-all-dev 47 | roscpp 48 | rospy 49 | sensor_msgs 50 | std_msgs 51 | tf 52 | tinyxml 53 | cv_bridge 54 | gazebo_msgs 55 | image_transport 56 | libpcl-all 57 | roscpp 58 | rospy 59 | sensor_msgs 60 | std_msgs 61 | tf 62 | tinyxml 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | -------------------------------------------------------------------------------- /src/ASiftDetector.cpp: -------------------------------------------------------------------------------- 1 | #include "mesh_localize/ASiftDetector.h" 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | ASiftDetector::ASiftDetector() 10 | { 11 | 12 | } 13 | 14 | void ASiftDetector::detectAndCompute(const Mat& img, std::vector< KeyPoint >& keypoints, Mat& descriptors, ASiftDetector::DescriptorType desc_type) 15 | { 16 | int h = img.rows; 17 | int w = img.cols; 18 | Mat mask(h, w, CV_8UC1, Scalar(255)); 19 | 20 | detectAndCompute(img, keypoints, descriptors, mask, desc_type); 21 | } 22 | 23 | void ASiftDetector::detectAndCompute(const Mat& img, std::vector< KeyPoint >& keypoints, Mat& descriptors, const Mat& mask, ASiftDetector::DescriptorType desc_type) 24 | { 25 | keypoints.clear(); 26 | descriptors = Mat(0, 128, CV_32F); 27 | 28 | for(int tl = 1; tl < 4/*6*/; tl++) 29 | { 30 | double t = pow(2, 0.5*tl); 31 | int lim = 1.8*t; 32 | #pragma omp parallel for 33 | for(int phil = 0; phil < lim; phil ++) 34 | //for(int phi = 0; phi < 180; phi += 100.0/t /*72.0/t*/) 35 | { 36 | double phi = phil*100.0/t; 37 | std::vector kps; 38 | Mat desc; 39 | 40 | Mat timg, skew_mask, Ai; 41 | img.copyTo(timg); 42 | mask.copyTo(skew_mask); 43 | affineSkew(t, phi, timg, skew_mask, Ai); 44 | 45 | #if 0 46 | Mat img_disp; 47 | bitwise_and(mask, timg, img_disp); 48 | namedWindow( "Skew", WINDOW_AUTOSIZE );// Create a window for display. 49 | imshow( "Skew", img_disp ); 50 | waitKey(0); 51 | #endif 52 | if(desc_type == ASiftDetector::SIFT) 53 | { 54 | SiftFeatureDetector detector; 55 | detector.detect(timg, kps, skew_mask); 56 | 57 | SiftDescriptorExtractor extractor; 58 | extractor.compute(timg, kps, desc); 59 | } 60 | else if(desc_type == ASiftDetector::SURF) 61 | { 62 | SurfFeatureDetector detector(600); 63 | detector.detect(timg, kps, skew_mask); 64 | 65 | SurfDescriptorExtractor extractor; 66 | extractor.compute(timg, kps, desc); 67 | } 68 | 69 | for(unsigned int i = 0; i < kps.size(); i++) 70 | { 71 | Point3f kpt(kps[i].pt.x, kps[i].pt.y, 1); 72 | //std::cout << Ai << std::endl; 73 | //std::cout << Mat(kpt) << std::endl; 74 | Mat kpt_t = Ai*Mat(kpt); 75 | //std::cout << "Transformed kp" << std::endl; 76 | kps[i].pt.x = kpt_t.at(0,0); 77 | kps[i].pt.y = kpt_t.at(1,0); 78 | } 79 | #pragma omp critical 80 | { 81 | keypoints.insert(keypoints.end(), kps.begin(), kps.end()); 82 | descriptors.push_back(desc); 83 | } 84 | //std::cout << "Added desc " << descriptors.cols << " " << descriptors.rows << " " << desc.cols << " " << desc.rows << std::endl; 85 | } 86 | } 87 | } 88 | 89 | void ASiftDetector::affineSkew(double tilt, double phi, Mat& img, Mat& mask, Mat& Ai) 90 | { 91 | int h = img.rows; 92 | int w = img.cols; 93 | 94 | //mask = Mat(h, w, CV_8UC1, Scalar(255)); 95 | 96 | Mat A = Mat::eye(2,3, CV_32F); 97 | 98 | if(phi != 0.0) 99 | { 100 | phi *= M_PI/180.; 101 | double s = sin(phi); 102 | double c = cos(phi); 103 | 104 | A = (Mat_(2,2) << c, -s, s, c); 105 | 106 | Mat corners = (Mat_(4,2) << 0, 0, w, 0, w, h, 0, h); 107 | Mat tcorners = corners*A.t(); 108 | Mat tcorners_x, tcorners_y; 109 | tcorners.col(0).copyTo(tcorners_x); 110 | tcorners.col(1).copyTo(tcorners_y); 111 | std::vector channels; 112 | channels.push_back(tcorners_x); 113 | channels.push_back(tcorners_y); 114 | merge(channels, tcorners); 115 | 116 | Rect rect = boundingRect(tcorners); 117 | A = (Mat_(2,3) << c, -s, -rect.x, s, c, -rect.y); 118 | 119 | warpAffine(img, img, A, Size(rect.width, rect.height), INTER_LINEAR, BORDER_REPLICATE); 120 | } 121 | if(tilt != 1.0) 122 | { 123 | double s = 0.8*sqrt(tilt*tilt-1); 124 | GaussianBlur(img, img, Size(0,0), s, 0.01); 125 | resize(img, img, Size(0,0), 1.0/tilt, 1.0, INTER_NEAREST); 126 | A.row(0) = A.row(0)/tilt; 127 | } 128 | if(tilt != 1.0 || phi != 0.0) 129 | { 130 | h = img.rows; 131 | w = img.cols; 132 | warpAffine(mask, mask, A, Size(w, h), INTER_NEAREST); 133 | } 134 | invertAffineTransform(A, Ai); 135 | } 136 | 137 | -------------------------------------------------------------------------------- /src/CameraContainer.cpp: -------------------------------------------------------------------------------- 1 | #include "mesh_localize/CameraContainer.h" 2 | 3 | CameraContainer::CameraContainer(Mat image, Eigen::Matrix4f tf, Eigen::Matrix3f K) : 4 | tf(tf), 5 | K(K), 6 | img(image) 7 | { 8 | } 9 | 10 | Mat CameraContainer::GetImage() 11 | { 12 | return img; 13 | } 14 | 15 | Eigen::Matrix4f CameraContainer::GetTf() 16 | { 17 | return tf; 18 | } 19 | 20 | Eigen::Matrix3f CameraContainer::GetK() 21 | { 22 | return K; 23 | } 24 | 25 | -------------------------------------------------------------------------------- /src/Common.cpp: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | * ExploringSfMWithOpenCV 3 | ****************************************************************************** 4 | * by Roy Shilkrot, 5th Dec 2012 5 | * http://www.morethantechnical.com/ 6 | ****************************************************************************** 7 | * Ch4 of the book "Mastering OpenCV with Practical Computer Vision Projects" 8 | * Copyright Packt Publishing 2012. 9 | * http://www.packtpub.com/cool-projects-with-opencv/book 10 | *****************************************************************************/ 11 | 12 | #include "mesh_localize/Common.h" 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | #ifndef WIN32 19 | #include 20 | #endif 21 | 22 | using namespace std; 23 | using namespace cv; 24 | 25 | std::vector FlipMatches(const std::vector& matches) { 26 | std::vector flip; 27 | for(int i=0;i CloudPointsToPoints(const std::vector cpts) { 35 | std::vector out; 36 | for (unsigned int i=0; i& imgpts1, 43 | const std::vector& imgpts2, 44 | const std::vector& matches, 45 | std::vector& pt_set1, 46 | std::vector& pt_set2) 47 | { 48 | for (unsigned int i=0; i& kps, vector& ps) { 58 | ps.clear(); 59 | for (unsigned int i=0; i& ps, vector& kps) { 63 | kps.clear(); 64 | for (unsigned int i=0; i& prevPts, const vector& nextPts, const vector& status, const vector& verror, const Scalar& _line_color) 70 | { 71 | double minVal,maxVal; minMaxIdx(verror,&minVal,&maxVal,0,0,status); 72 | int line_thickness = 1; 73 | 74 | for (size_t i = 0; i < prevPts.size(); ++i) 75 | { 76 | if (status[i]) 77 | { 78 | double alpha = intrpmnmx(verror[i],minVal,maxVal); alpha = 1.0 - alpha; 79 | Scalar line_color(alpha*_line_color[0], 80 | alpha*_line_color[1], 81 | alpha*_line_color[2]); 82 | 83 | Point p = prevPts[i]; 84 | Point q = nextPts[i]; 85 | 86 | double angle = atan2((double) p.y - q.y, (double) p.x - q.x); 87 | 88 | double hypotenuse = sqrt( (double)(p.y - q.y)*(p.y - q.y) + (double)(p.x - q.x)*(p.x - q.x) ); 89 | 90 | if (hypotenuse < 1.0) 91 | continue; 92 | 93 | // Here we lengthen the arrow by a factor of three. 94 | q.x = (int) (p.x - 3 * hypotenuse * cos(angle)); 95 | q.y = (int) (p.y - 3 * hypotenuse * sin(angle)); 96 | 97 | // Now we draw the main line of the arrow. 98 | line(frame, p, q, line_color, line_thickness); 99 | 100 | // Now draw the tips of the arrow. I do some scaling so that the 101 | // tips look proportional to the main line of the arrow. 102 | 103 | p.x = (int) (q.x + 9 * cos(angle + CV_PI / 4)); 104 | p.y = (int) (q.y + 9 * sin(angle + CV_PI / 4)); 105 | line(frame, p, q, line_color, line_thickness); 106 | 107 | p.x = (int) (q.x + 9 * cos(angle - CV_PI / 4)); 108 | p.y = (int) (q.y + 9 * sin(angle - CV_PI / 4)); 109 | line(frame, p, q, line_color, line_thickness); 110 | } 111 | } 112 | } 113 | 114 | bool hasEnding (std::string const &fullString, std::string const &ending) 115 | { 116 | if (fullString.length() >= ending.length()) { 117 | return (0 == fullString.compare (fullString.length() - ending.length(), ending.length(), ending)); 118 | } else { 119 | return false; 120 | } 121 | } 122 | 123 | bool hasEndingLower (string const &fullString_, string const &_ending) 124 | { 125 | string fullstring = fullString_, ending = _ending; 126 | transform(fullString_.begin(),fullString_.end(),fullstring.begin(),::tolower); // to lower 127 | return hasEnding(fullstring,ending); 128 | } 129 | 130 | void imshow_250x250(const string& name_, const Mat& patch) { 131 | Mat bigpatch; cv::resize(patch,bigpatch,Size(250,250)); 132 | imshow(name_,bigpatch); 133 | } 134 | 135 | void open_imgs_dir(char* dir_name, std::vector& images, std::vector& images_names, double downscale_factor) { 136 | if (dir_name == NULL) { 137 | return; 138 | } 139 | 140 | string dir_name_ = string(dir_name); 141 | vector files_; 142 | 143 | #ifndef WIN32 144 | //open a directory the POSIX way 145 | 146 | DIR *dp; 147 | struct dirent *ep; 148 | dp = opendir (dir_name); 149 | 150 | if (dp != NULL) 151 | { 152 | while (ep = readdir (dp)) { 153 | if (ep->d_name[0] != '.') 154 | files_.push_back(ep->d_name); 155 | } 156 | 157 | (void) closedir (dp); 158 | } 159 | else { 160 | cerr << ("Couldn't open the directory"); 161 | return; 162 | } 163 | 164 | #else 165 | //open a directory the WIN32 way 166 | HANDLE hFind = INVALID_HANDLE_VALUE; 167 | WIN32_FIND_DATA fdata; 168 | 169 | if(dir_name_[dir_name_.size()-1] == '\\' || dir_name_[dir_name_.size()-1] == '/') { 170 | dir_name_ = dir_name_.substr(0,dir_name_.size()-1); 171 | } 172 | 173 | hFind = FindFirstFile(string(dir_name_).append("\\*").c_str(), &fdata); 174 | if (hFind != INVALID_HANDLE_VALUE) 175 | { 176 | do 177 | { 178 | if (strcmp(fdata.cFileName, ".") != 0 && 179 | strcmp(fdata.cFileName, "..") != 0) 180 | { 181 | if (fdata.dwFileAttributes & FILE_ATTRIBUTE_DIRECTORY) 182 | { 183 | continue; // a diretory 184 | } 185 | else 186 | { 187 | files_.push_back(fdata.cFileName); 188 | } 189 | } 190 | } 191 | while (FindNextFile(hFind, &fdata) != 0); 192 | } else { 193 | cerr << "can't open directory\n"; 194 | return; 195 | } 196 | 197 | if (GetLastError() != ERROR_NO_MORE_FILES) 198 | { 199 | FindClose(hFind); 200 | cerr << "some other error with opening directory: " << GetLastError() << endl; 201 | return; 202 | } 203 | 204 | FindClose(hFind); 205 | hFind = INVALID_HANDLE_VALUE; 206 | #endif 207 | 208 | for (unsigned int i=0; i 5 | #include 6 | #include 7 | 8 | using namespace cv; 9 | using namespace std; 10 | 11 | DepthFeatureMatchLocalizer::DepthFeatureMatchLocalizer(const std::vector& train, 12 | std::string desc_type, bool show_matches, int min_inliers, double max_reproj_error, 13 | double ratio_test_thresh) 14 | : keyframes(train), desc_type(desc_type), show_matches(show_matches), min_inliers(min_inliers), 15 | max_reproj_error(max_reproj_error), ratio_test_thresh(ratio_test_thresh) 16 | { 17 | namedWindow( "Match", WINDOW_NORMAL ); 18 | } 19 | 20 | bool DepthFeatureMatchLocalizer::localize(const Mat& img, const Mat& Kcv, Eigen::Matrix4f* pose, Eigen::Matrix4f* pose_guess) 21 | { 22 | KeyframeContainer* kf = new KeyframeContainer(img, desc_type); 23 | std::vector< KeyframeMatch > matches; 24 | 25 | if(pose_guess) 26 | { 27 | matches = FindImageMatches(kf, 10, pose_guess, keyframes.size()/4); 28 | } 29 | else 30 | { 31 | matches = FindImageMatches(kf, 10); 32 | } 33 | 34 | // Find most geometrically consistent match 35 | int bestMatch = -1; 36 | std::vector bestInliers; 37 | double bestReprojError; 38 | for(int i = 0; i < matches.size(); i++) 39 | { 40 | if(matches[i].matchKps1.size() >= 5) 41 | { 42 | std::vector inlierIdx; 43 | Eigen::Matrix4f vimgTf = matches[i].kfc->GetTf(); 44 | std::vector matchPts3d = PnPUtil::BackprojectPts(matches[i].matchPts2, vimgTf, matches[i].kfc->GetK(), matches[i].kfc->GetDepth()); 45 | 46 | /** Debugging Backproject ** 47 | Eigen::Matrix4f vimgTfI = vimgTf.inverse(); 48 | Mat distcoeffcvPnp = (Mat_(4,1) << 0, 0, 0, 0); 49 | Mat Rvec, t; 50 | Mat Rtest = (Mat_(3,3) << vimgTfI(0,0), vimgTfI(0,1), vimgTfI(0,2), 51 | vimgTfI(1,0), vimgTfI(1,1), vimgTfI(1,2), 52 | vimgTfI(2,0), vimgTfI(2,1), vimgTfI(2,2)); 53 | Rodrigues(Rtest, Rvec); 54 | t = (Mat_(3,1) << vimgTfI(0,3), vimgTfI(1,3), vimgTfI(2,3)); 55 | std::vector reprojPts; 56 | projectPoints(matchPts3d, Rvec, t, Kcv, distcoeffcvPnp, reprojPts); 57 | for(int j = 0; j < reprojPts.size(); j++) 58 | { 59 | std::cout << j << ": " << reprojPts[j] << " " << matches[i].matchPts2[j] << std::endl; 60 | } 61 | /******/ 62 | 63 | #if 0 64 | Mat depth_im; 65 | double min_depth, max_depth; 66 | minMaxLoc(matches[i].kfc->GetDepth(), &min_depth, &max_depth); 67 | //std::cout << "min_depth=" << min_depth << " max_depth=" << max_depth << std::endl; 68 | matches[i].kfc->GetDepth().convertTo(depth_im, CV_8U, 255.0/(max_depth-min_depth), 0);// -min_depth*255.0/(max_depth-min_depth)); 69 | namedWindow( "Global Depth", WINDOW_NORMAL );// Create a window for display. 70 | imshow( "Global Depth", depth_im ); 71 | #endif 72 | if(matchPts3d.size() == 0 || matches[i].matchPts1.size() == 0) 73 | continue; 74 | 75 | Eigen::Matrix4f tf_ransac; 76 | double reprojError; 77 | if(!PnPUtil::RansacPnP(matchPts3d, matches[i].matchPts1, Kcv, vimgTf.inverse(), tf_ransac, inlierIdx, &reprojError)) 78 | { 79 | continue; 80 | } 81 | std::cout << "KF " << i << " reproj error: " << reprojError << " #inliers: " << inlierIdx.size() 82 | << std::endl; 83 | if(inlierIdx.size() < min_inliers || reprojError >= max_reproj_error) 84 | { 85 | continue; 86 | } 87 | 88 | //std::cout << "Match K=" << std::endl << matches[i].kfc->GetK() << std::endl; 89 | //std::cout << "Image K=" << std::endl << Kcv << std::endl; 90 | if(inlierIdx.size() > bestInliers.size()); 91 | { 92 | bestMatch = i; 93 | bestInliers = inlierIdx; 94 | *pose = tf_ransac.inverse(); 95 | bestReprojError = reprojError; 96 | } 97 | } 98 | } 99 | if(bestMatch >= 0) 100 | { 101 | if(show_matches) 102 | { 103 | std::vector< DMatch > inlierMatches; 104 | for(int j = 0; j < bestInliers.size(); j++) 105 | { 106 | inlierMatches.push_back(matches[bestMatch].matches[bestInliers[j]]); 107 | } 108 | 109 | Mat img_matches; 110 | drawMatches(img, kf->GetKeypoints(), matches[bestMatch].kfc->GetImage(), matches[bestMatch].kfc->GetKeypoints(), inlierMatches, img_matches); 111 | imshow("Match", img_matches); 112 | waitKey(1); 113 | } 114 | std::cout << "DepthFeatureMatchLocalizer: Found consistent match (" << bestInliers.size() << " inliers). Avg reproj error: " << bestReprojError << std::endl; 115 | return true; 116 | } 117 | else 118 | { 119 | std::cout << "DepthFeatureMatchLocalizer: no matches are geometrically consistent" << std::endl; 120 | return false; 121 | } 122 | } 123 | 124 | std::vector< KeyframeMatch > DepthFeatureMatchLocalizer::FindImageMatches(KeyframeContainer* img, int k, Eigen::Matrix4f* pose_guess, unsigned int search_bound) 125 | { 126 | const double numMatchThresh = 0;//0.16; 127 | const double matchRatio = ratio_test_thresh;; 128 | std::vector< KeyframeMatch > kfMatches; 129 | 130 | if(pose_guess) 131 | { 132 | if(search_bound >= keyframes.size()) 133 | { 134 | search_bound = keyframes.size(); 135 | } 136 | else 137 | { 138 | KeyframePositionSorter kps(*pose_guess); 139 | std::sort(keyframes.begin(), keyframes.end(), kps); 140 | } 141 | } 142 | else 143 | { 144 | search_bound = keyframes.size(); 145 | } 146 | 147 | // Find potential frame matches 148 | #pragma omp parallel for 149 | for(unsigned int i = 0; i < search_bound; i++) 150 | { 151 | //std::cout << i/double(keyframes.size()) << std::endl; 152 | 153 | FlannBasedMatcher matcher; 154 | std::vector < std::vector< DMatch > > matches; 155 | if(keyframes[i]->GetDescriptors().rows == 0 || keyframes[i]->GetDescriptors().cols == 0) 156 | continue; 157 | matcher.knnMatch( img->GetDescriptors(), keyframes[i]->GetDescriptors(), matches, 2 ); 158 | 159 | std::vector< DMatch > goodMatches; 160 | std::vector< DMatch > allMatches; 161 | std::vector matchPts1; 162 | std::vector matchPts2; 163 | std::vector matchKps1; 164 | std::vector matchKps2; 165 | 166 | // Use ratio test to find good keypoint matches 167 | for(unsigned int j = 0; j < matches.size(); j++) 168 | { 169 | allMatches.push_back(matches[j][0]); 170 | if(matches[j][0].distance < matchRatio*matches[j][1].distance) 171 | { 172 | goodMatches.push_back(matches[j][0]); 173 | matchPts1.push_back(img->GetKeypoints()[matches[j][0].queryIdx].pt); 174 | matchPts2.push_back(keyframes[i]->GetKeypoints()[matches[j][0].trainIdx].pt); 175 | matchKps1.push_back(img->GetKeypoints()[matches[j][0].queryIdx]); 176 | matchKps2.push_back(keyframes[i]->GetKeypoints()[matches[j][0].trainIdx]); 177 | } 178 | } 179 | if(goodMatches.size() >= numMatchThresh*matches.size()) 180 | { 181 | //std:: cout << "Found Match!" << std::endl; 182 | #pragma omp critical 183 | { 184 | kfMatches.push_back(KeyframeMatch(keyframes[i], goodMatches, allMatches, matchPts1, matchPts2, matchKps1, matchKps2)); 185 | } 186 | } 187 | } 188 | 189 | k = (kfMatches.size() < k) ? kfMatches.size() : k; 190 | std::sort(kfMatches.begin(), kfMatches.end()); 191 | 192 | return std::vector< KeyframeMatch > (kfMatches.begin(), kfMatches.begin()+k); 193 | } 194 | -------------------------------------------------------------------------------- /src/FABMAPLocalizer.cpp: -------------------------------------------------------------------------------- 1 | #include "mesh_localize/FABMAPLocalizer.h" 2 | 3 | #include 4 | 5 | using namespace cv; 6 | 7 | FABMAPLocalizer::FABMAPLocalizer(const std::vector& train, std::string descriptor_type, bool show_matches, bool load, std::string filename) 8 | : desc_type(descriptor_type), show_matches(show_matches) 9 | { 10 | keyframes = train; 11 | 12 | detector = new DynamicAdaptedFeatureDetector( 13 | AdjusterAdapter::create("SURF"), 1000, 1100); 14 | extractor = new SurfDescriptorExtractor();//1000, 4, 2, false, true); 15 | matcher = DescriptorMatcher::create("FlannBased"); 16 | 17 | std::string v_filename = filename + std::string("vocab.yml"); 18 | std::string td_filename = filename + std::string("train_data.yml"); 19 | std::string tree_filename = filename + std::string("tree.yml"); 20 | 21 | clock_t start_time; 22 | Mat vocab; 23 | Mat train_data; 24 | Mat tree; 25 | 26 | if(load) 27 | { 28 | FileStorage fs; 29 | //load vocab 30 | std::cout << "Loading Vocabulary: " << v_filename << std::endl; 31 | fs.open(std::string(v_filename), FileStorage::READ); 32 | fs["Vocabulary"] >> vocab; 33 | if (vocab.empty()) { 34 | std::cout << "Vocabulary not found" << std::endl; 35 | return; 36 | } 37 | fs.release(); 38 | 39 | bide = new BOWImgDescriptorExtractor(extractor, matcher); 40 | bide->setVocabulary(vocab); 41 | 42 | std::cout << "Loading Training Data: " << td_filename << std::endl; 43 | fs.open(td_filename, FileStorage::READ); 44 | fs["BOWImageDescs"] >> train_data; 45 | if (train_data.empty()) { 46 | std::cout << "Training Data not found" << std::endl; 47 | return; 48 | } 49 | fs.release(); 50 | 51 | std::cout << "Loading tree: " << tree_filename << std::endl; 52 | fs.open(tree_filename, FileStorage::READ); 53 | fs["CLTree"] >> tree; 54 | if (tree.empty()) { 55 | std::cout << "Tree not found" << std::endl; 56 | return; 57 | } 58 | fs.release(); 59 | 60 | } 61 | else 62 | { 63 | std::cout << "Creating vocab..." << std::endl; 64 | Mat vocab_data; 65 | for(unsigned int i = 0; i < keyframes.size(); i ++) 66 | { 67 | Mat desc; 68 | vector kpts; 69 | detector->detect(keyframes.at(i)->GetImage(), kpts); 70 | extractor->compute(keyframes.at(i)->GetImage(), kpts, desc); 71 | vocab_data.push_back(desc); 72 | //vocab_data.push_back(keyframes.at(i)->GetDescriptors()); 73 | } 74 | 75 | start_time = clock(); 76 | of2::BOWMSCTrainer bowt; 77 | vocab = bowt.cluster(vocab_data); 78 | std::cout << "Vocab created" << std::endl; 79 | std::cout << double( clock() - start_time ) / (double)CLOCKS_PER_SEC<< " seconds" << std::endl; 80 | 81 | bide = new BOWImgDescriptorExtractor(extractor, matcher); 82 | bide->setVocabulary(vocab); 83 | 84 | std::cout << "Creating training data..." << std::endl; 85 | for(unsigned int i = 0; i < keyframes.size(); i++) 86 | { 87 | Mat bow; 88 | vector kpts; 89 | detector->detect(keyframes[i]->GetImage(), kpts); 90 | bide->compute(keyframes[i]->GetImage(), kpts, bow); 91 | train_data.push_back(bow); 92 | } 93 | std::cout << "Train data created" << std::endl; 94 | 95 | std::cout << "Building tree.." << std::endl; 96 | start_time = clock(); 97 | of2::ChowLiuTree tree_builder; 98 | tree_builder.add(train_data); 99 | tree = tree_builder.make(); 100 | std::cout << "Tree built" << std::endl; 101 | std::cout << double( clock() - start_time ) / (double)CLOCKS_PER_SEC<< " seconds" << std::endl; 102 | 103 | if(filename != std::string("")) 104 | { 105 | std::cout << "Saving vocab and data..." << std::endl; 106 | FileStorage fs_vocab(v_filename, FileStorage::WRITE); 107 | if(!fs_vocab.isOpened()) 108 | { 109 | std::cout << "Failed to save vocab to " << v_filename << std::endl; 110 | return; 111 | } 112 | write(fs_vocab, std::string("Vocabulary"), vocab); 113 | fs_vocab.release(); 114 | 115 | std::cout << "Successfully saved vocab to " << v_filename << std::endl; 116 | 117 | FileStorage fs_train(td_filename, FileStorage::WRITE); 118 | if(!fs_train.isOpened()) 119 | { 120 | std::cout << "Failed to save train data to " << td_filename << std::endl; 121 | return; 122 | } 123 | write(fs_train, std::string("BOWImageDescs"), train_data); 124 | fs_train.release(); 125 | 126 | std::cout << "Successfully saved train data to " << td_filename << std::endl; 127 | 128 | FileStorage fs_tree(tree_filename, FileStorage::WRITE); 129 | if(!fs_tree.isOpened()) 130 | { 131 | std::cout << "Failed to save tree to " << tree_filename << std::endl; 132 | return; 133 | } 134 | write(fs_tree, std::string("CLTree"), tree); 135 | fs_tree.release(); 136 | 137 | std::cout << "Successfully saved tree to " << tree_filename << std::endl; 138 | } 139 | } 140 | std::cout << "Vocab size = " << vocab.rows << std::endl; 141 | 142 | 143 | //fabmap = new of2::FabMap2(tree, 0.39, 0, of2::FabMap::SAMPLED | of2::FabMap::CHOW_LIU); 144 | //fabmap->addTraining(train_data); 145 | fabmap = new of2::FabMapLUT(tree, 0.39, 0.05, of2::FabMap::MEAN_FIELD | of2::FabMap::CHOW_LIU); 146 | 147 | fabmap->add(train_data); 148 | } 149 | 150 | bool FABMAPLocalizer::localize(const Mat& img, const Mat& K, Eigen::Matrix4f* pose, Eigen::Matrix4f* pose_guess) 151 | { 152 | Mat bow; 153 | vector kpts; 154 | detector->detect(img, kpts); 155 | bide->compute(img, kpts, bow); 156 | 157 | if(bow.empty()) 158 | { 159 | std::cout << "Warning: no keypoints detected" << std::endl; 160 | return false; 161 | } 162 | 163 | vector matches; 164 | fabmap->compare(bow, matches, false); 165 | 166 | if(show_matches) 167 | { 168 | //namedWindow( "Query", WINDOW_AUTOSIZE ); 169 | //imshow("Query", img); 170 | //waitKey(0); 171 | 172 | std::cout << "# comparisons = " << matches.size() << std::endl; 173 | for(int i = 0; i < matches.size(); i++) 174 | { 175 | std::cout << "match prob = " << matches.at(i).match << std::endl; 176 | std::cout << "match likelihood = " << matches.at(i).likelihood << std::endl; 177 | std::cout << "imgIdx = " << matches.at(i).imgIdx << " queryIdx = " << matches.at(i).queryIdx << std::endl; 178 | std::cout << "# keyframes = " << keyframes.size() << std::endl; 179 | if(matches.at(i).imgIdx > -1 && matches.at(i).match > 0.5) 180 | { 181 | namedWindow( "Match", WINDOW_AUTOSIZE ); 182 | imshow("Match", keyframes.at(matches.at(i).imgIdx)->GetImage()); 183 | waitKey(2000); 184 | } 185 | } 186 | } 187 | 188 | for(int i = 0; i < matches.size(); i++) 189 | { 190 | if(matches.at(i).imgIdx > -1 && matches.at(i).match > 0.5) 191 | { 192 | *pose = keyframes.at(matches.at(i).imgIdx)->GetTf(); 193 | return true; 194 | } 195 | } 196 | return false; 197 | } 198 | -------------------------------------------------------------------------------- /src/FeatureMatchLocalizer.cpp: -------------------------------------------------------------------------------- 1 | #include "mesh_localize/FeatureMatchLocalizer.h" 2 | 3 | #include 4 | 5 | using namespace cv; 6 | 7 | FeatureMatchLocalizer::FeatureMatchLocalizer(const std::vector& train, std::string descriptor_type, bool show_matches, bool load_descriptors, std::string desc_filename) 8 | : desc_type(descriptor_type), show_matches(show_matches) 9 | { 10 | std::ifstream desc_file; 11 | if(load_descriptors) 12 | { 13 | printf("Opening keypoints and descriptors..."); 14 | desc_file.open(desc_filename.c_str(), std::ifstream::binary); 15 | if(!desc_file.is_open()) 16 | { 17 | printf("Could not open descriptor file %s", desc_filename.c_str()); 18 | return; 19 | } 20 | std::cout << "Successfully opened keypoints and descriptors" << std::endl; 21 | } 22 | 23 | for(int i = 0; i < train.size(); i++) 24 | { 25 | KeyframeContainer* kfc; 26 | if(load_descriptors && desc_filename != "") 27 | { 28 | std::vector keypoints; 29 | int size; 30 | desc_file.read((char*)&size, sizeof(int)); 31 | for(int j = 0; j < size; j++) 32 | { 33 | KeyPoint kp; 34 | desc_file.read((char*)&kp, sizeof(KeyPoint)); 35 | keypoints.push_back(kp); 36 | } 37 | int rows; 38 | int cols; 39 | int elemSize; 40 | desc_file.read((char*)&rows, sizeof(int)); 41 | desc_file.read((char*)&cols, sizeof(int)); 42 | desc_file.read((char*)&elemSize, sizeof(int)); 43 | 44 | unsigned char* data = new unsigned char[rows*cols*elemSize]; 45 | desc_file.read((char*)data, sizeof(unsigned char)*rows*cols*elemSize); 46 | 47 | Mat descriptors(Size(cols,rows), CV_32F, data); 48 | 49 | kfc = new KeyframeContainer(train[i], keypoints, descriptors); 50 | } 51 | else 52 | { 53 | kfc = new KeyframeContainer(train[i], descriptor_type); 54 | } 55 | keyframes.push_back(kfc); 56 | } 57 | printf("Successfully loaded images"); 58 | 59 | if(load_descriptors) 60 | { 61 | desc_file.close(); 62 | } 63 | else if(desc_filename != "") 64 | { 65 | WriteDescriptorsToFile(desc_filename); 66 | } 67 | } 68 | 69 | bool FeatureMatchLocalizer::WriteDescriptorsToFile(std::string filename) 70 | { 71 | std::ofstream file; 72 | file.open(filename.c_str(), std::ios::out | std::ios::binary); 73 | 74 | if(!file.is_open()) 75 | { 76 | printf("Could not open descriptor file %s", filename.c_str()); 77 | return false; 78 | } 79 | 80 | printf("Saving keypoints and descriptors..."); 81 | for(unsigned int i = 0; i < keyframes.size(); i++) 82 | { 83 | int size = keyframes[i]->GetKeypoints().size(); 84 | file.write((char*)&size, sizeof(int)); 85 | for(int j = 0; j < size; j++) 86 | { 87 | KeyPoint kp = keyframes[i]->GetKeypoints()[j]; 88 | file.write((char*)&kp, sizeof(KeyPoint)); 89 | } 90 | int rows = keyframes[i]->GetDescriptors().rows; 91 | int cols = keyframes[i]->GetDescriptors().cols; 92 | int elemSize = keyframes[i]->GetDescriptors().elemSize1(); 93 | unsigned char* data = keyframes[i]->GetDescriptors().data; 94 | 95 | file.write((char*)&rows, sizeof(int)); 96 | file.write((char*)&cols, sizeof(int)); 97 | file.write((char*)&elemSize, sizeof(int)); 98 | file.write((char*)data, sizeof(unsigned char)*rows*cols*elemSize); 99 | } 100 | 101 | file.close(); 102 | printf("Successfully saved keypoints and descriptors to %s", filename.c_str()); 103 | return true; 104 | } 105 | 106 | bool FeatureMatchLocalizer::localize(const Mat& img, const Mat& K, Eigen::Matrix4f* pose, Eigen::Matrix4f* pose_guess) 107 | { 108 | KeyframeContainer* kf = new KeyframeContainer(img, desc_type); 109 | std::vector< KeyframeMatch > matches; 110 | 111 | if(pose_guess) 112 | { 113 | matches = FindImageMatches(kf, 5, pose_guess, keyframes.size()/4); 114 | } 115 | else 116 | { 117 | matches = FindImageMatches(kf, 5); 118 | } 119 | 120 | if(show_matches) 121 | { 122 | for(int i = 0; i < matches.size(); i++) 123 | { 124 | namedWindow( "Match", WINDOW_AUTOSIZE ); 125 | imshow("Match", matches[i].kfc->GetImage()); 126 | waitKey(0); 127 | } 128 | } 129 | 130 | if(matches[0].matchKps1.size() >= 40) 131 | { 132 | *pose = matches[0].kfc->GetTf(); 133 | return true; 134 | } 135 | else 136 | { 137 | printf("Match not good enough: only %d match points\n", int(matches[0].matchKps1.size())); 138 | return false; 139 | } 140 | } 141 | 142 | std::vector< KeyframeMatch > FeatureMatchLocalizer::FindImageMatches(KeyframeContainer* img, int k, Eigen::Matrix4f* pose_guess, unsigned int search_bound) 143 | { 144 | const double numMatchThresh = 0;//0.16; 145 | const double matchRatio = 0.7; 146 | std::vector< KeyframeMatch > kfMatches; 147 | 148 | if(pose_guess) 149 | { 150 | if(search_bound >= keyframes.size()) 151 | { 152 | search_bound = keyframes.size(); 153 | } 154 | else 155 | { 156 | KeyframePositionSorter kps(*pose_guess); 157 | std::sort(keyframes.begin(), keyframes.end(), kps); 158 | } 159 | } 160 | else 161 | { 162 | search_bound = keyframes.size(); 163 | } 164 | 165 | // Find potential frame matches 166 | #pragma omp parallel for 167 | for(unsigned int i = 0; i < search_bound; i++) 168 | { 169 | //std::cout << i/double(keyframes.size()) << std::endl; 170 | 171 | FlannBasedMatcher matcher; 172 | std::vector < std::vector< DMatch > > matches; 173 | matcher.knnMatch( img->GetDescriptors(), keyframes[i]->GetDescriptors(), matches, 2 ); 174 | 175 | std::vector< DMatch > goodMatches; 176 | std::vector< DMatch > allMatches; 177 | std::vector matchPts1; 178 | std::vector matchPts2; 179 | std::vector matchKps1; 180 | std::vector matchKps2; 181 | 182 | // Use ratio test to find good keypoint matches 183 | for(unsigned int j = 0; j < matches.size(); j++) 184 | { 185 | allMatches.push_back(matches[j][0]); 186 | if(matches[j][0].distance < matchRatio*matches[j][1].distance) 187 | { 188 | goodMatches.push_back(matches[j][0]); 189 | matchPts1.push_back(img->GetKeypoints()[matches[j][0].queryIdx].pt); 190 | matchPts2.push_back(keyframes[i]->GetKeypoints()[matches[j][0].trainIdx].pt); 191 | matchKps1.push_back(img->GetKeypoints()[matches[j][0].queryIdx]); 192 | matchKps2.push_back(keyframes[i]->GetKeypoints()[matches[j][0].trainIdx]); 193 | } 194 | } 195 | if(goodMatches.size() >= numMatchThresh*matches.size()) 196 | { 197 | //std:: cout << "Found Match!" << std::endl; 198 | #pragma omp critical 199 | { 200 | kfMatches.push_back(KeyframeMatch(keyframes[i], goodMatches, allMatches, matchPts1, matchPts2, matchKps1, matchKps2)); 201 | } 202 | } 203 | } 204 | 205 | k = (kfMatches.size() < k) ? kfMatches.size() : k; 206 | std::sort(kfMatches.begin(), kfMatches.end()); 207 | 208 | return std::vector< KeyframeMatch > (kfMatches.begin(), kfMatches.begin()+k); 209 | } 210 | -------------------------------------------------------------------------------- /src/ImageDbUtil.cpp: -------------------------------------------------------------------------------- 1 | #include "mesh_localize/ImageDbUtil.h" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | Eigen::Matrix4f ImageDbUtil::StringToMatrix4f(std::string str) 8 | { 9 | Eigen::Matrix4f mat; 10 | std::vector fields; 11 | size_t cur_pos=0; 12 | size_t found_pos=0; 13 | size_t last_found_pos=0; 14 | 15 | while((found_pos = str.find(' ', cur_pos)) != std::string::npos) 16 | { 17 | fields.push_back(atof(str.substr(cur_pos, found_pos-cur_pos).c_str())); 18 | cur_pos = found_pos+1; 19 | last_found_pos = found_pos; 20 | } 21 | fields.push_back(atof(str.substr(last_found_pos).c_str())); 22 | if(fields.size() != 16) 23 | { 24 | std::cout << "String is not correctly formatted" << std::endl; 25 | return mat; 26 | } 27 | 28 | for(int i = 0; i < 16; i++) 29 | { 30 | mat(i/4,i%4) = fields[i]; 31 | } 32 | return mat; 33 | } 34 | 35 | bool ImageDbUtil::LoadOgreDataDir(std::string data_dir, std::vector& keyframes) 36 | { 37 | printf("Opening keypoints and descriptors..."); 38 | std::fstream fsk; 39 | fsk.open((data_dir + "/" + "keyframe000.jpg").c_str()); 40 | int num_keyframes = 0; 41 | while(fsk.is_open()) 42 | { 43 | num_keyframes++; 44 | fsk.close(); 45 | std::stringstream ss; 46 | ss << data_dir << "/" << "keyframe" << std::setw(3) << std::setfill('0') << num_keyframes << ".jpg"; 47 | fsk.open(ss.str().c_str()); 48 | } 49 | 50 | if(num_keyframes == 0) 51 | { 52 | printf("No data in directory %s", data_dir.c_str()); 53 | return false; 54 | } 55 | 56 | std::cout << "Successfully opened " << num_keyframes << " keypoints and descriptors" << std::endl; 57 | 58 | std::stringstream ss; 59 | Mat Kcv; 60 | Eigen::MatrixXf K; 61 | ss.str(std::string()); // cleaning ss 62 | ss << data_dir << "/intrinsics.xml"; 63 | FileStorage fs_K(ss.str(), FileStorage::READ); 64 | fs_K["intrinsics"] >> Kcv; 65 | fs_K.release(); 66 | cv2eigen(Kcv, K); 67 | 68 | for(int i = 0; i < num_keyframes; i++) 69 | { 70 | ss.str(std::string()); // cleaning ss 71 | ss << data_dir << "/" << "keyframe" << std::setw(3) << std::setfill('0') << i << ".jpg"; 72 | Mat keyframe = imread(ss.str()); 73 | 74 | Mat pose; 75 | ss.str(std::string()); // cleaning ss 76 | ss << data_dir << "/" << "pose" << std::setw(3) << std::setfill('0') << i << ".xml"; 77 | FileStorage fs_pose(ss.str(), FileStorage::READ); 78 | fs_pose["pose"] >> pose; 79 | fs_pose.release(); 80 | 81 | Mat desc; 82 | ss.str(std::string()); // cleaning ss 83 | ss << data_dir << "/" << "descriptors" << std::setw(3) << std::setfill('0') << i << ".xml"; 84 | FileStorage fs_desc(ss.str(), FileStorage::READ); 85 | fs_desc["descriptors"] >> desc; 86 | fs_desc.release(); 87 | 88 | std::vector kps; 89 | ss.str(std::string()); // cleaning ss 90 | ss << data_dir << "/" << "keypoints" << std::setw(3) << std::setfill('0') << i << ".xml"; 91 | FileStorage fs_kp(ss.str(), FileStorage::READ); 92 | FileNode kn = fs_kp["keypoints"]; 93 | read(kn, kps); 94 | fs_kp.release(); 95 | 96 | Mat depth; 97 | ss.str(std::string()); // cleaning ss 98 | ss << data_dir << "/" << "depth" << std::setw(3) << std::setfill('0') << i << ".xml"; 99 | FileStorage fs_depth(ss.str(), FileStorage::READ); 100 | fs_depth["depth"] >> depth; 101 | fs_depth.release(); 102 | 103 | Eigen::MatrixXf pose_eig; 104 | cv2eigen(pose, pose_eig); 105 | //std::cout << "pose=" << std::endl << pose << std::endl; 106 | //std::cout << "pose_eig=" << std::endl << pose_eig << std::endl; 107 | //std::cout << "Kcv=" << std::endl << Kcv << std::endl; 108 | //std::cout << "K=" << std::endl << K << std::endl; 109 | CameraContainer* cc = new CameraContainer(keyframe, Eigen::Matrix4f(pose_eig), K); 110 | KeyframeContainer* kfc = new KeyframeContainer(cc, kps, desc, depth); 111 | keyframes.push_back(kfc); 112 | } 113 | std::cout << "Successfully loaded model keyframes" << std::endl; 114 | return true; 115 | } 116 | 117 | bool ImageDbUtil::LoadPhotoscanFile(std::string filename, vector& cameras, Mat map_Kcv, Mat map_distcoeffcv) 118 | { 119 | TiXmlDocument doc(filename); 120 | 121 | printf("Loading %s...\n", filename.c_str()); 122 | if(!doc.LoadFile()) 123 | { 124 | printf("Failed to load photoscan file\n"); 125 | return false; 126 | } 127 | printf("Successfully loaded photoscan file\n"); 128 | 129 | printf("Loading images...\n"); 130 | TiXmlHandle docHandle(&doc); 131 | for(TiXmlElement* chunk = docHandle.FirstChild( "document" ).FirstChild( "chunk" ).ToElement(); 132 | chunk != NULL; chunk = chunk->NextSiblingElement("chunk")) 133 | { 134 | if (std::string(chunk->Attribute("active")) == "true") 135 | { 136 | TiXmlHandle chunkHandle(chunk); 137 | for(TiXmlElement* camera = chunkHandle.FirstChild("cameras").FirstChild("camera").ToElement(); 138 | camera != NULL; camera = camera->NextSiblingElement("camera")) 139 | { 140 | std::string filename = camera->FirstChild("frames")->FirstChild("frame")->FirstChild("image")->ToElement()->Attribute("path"); 141 | TiXmlNode* tfNode = camera->FirstChild("transform"); 142 | if(!tfNode) 143 | continue; 144 | std::string tfStr = tfNode->ToElement()->GetText(); 145 | 146 | //std::cout << "Loading: " << filename << std::endl; 147 | //std::cout << tfStr << std::endl; 148 | 149 | Mat img_in = imread(filename, CV_LOAD_IMAGE_GRAYSCALE ); 150 | 151 | if(! img_in.data ) 152 | { 153 | printf("Could not open or find the image %s\n", filename.c_str()); 154 | return false; 155 | } 156 | 157 | Mat img_undistort; 158 | undistort(img_in, img_undistort, map_Kcv, map_distcoeffcv); 159 | img_in.release(); 160 | 161 | // downsample large images to save space 162 | if(img_undistort.rows > 480) 163 | { 164 | double scale = 1.2*480./img_undistort.rows; 165 | resize(img_undistort, img_undistort, Size(0,0), scale, scale); 166 | } 167 | 168 | Eigen::Matrix4f tf = StringToMatrix4f(tfStr); 169 | Eigen::MatrixXf map_K; 170 | cv2eigen(map_Kcv, map_K); 171 | cameras.push_back(new CameraContainer(img_undistort, tf, map_K)); 172 | } 173 | //std::cout << "Found chunk " << chunk->Attribute("label") << std::endl; 174 | } 175 | } 176 | return true; 177 | } 178 | -------------------------------------------------------------------------------- /src/KLTTracker.cpp: -------------------------------------------------------------------------------- 1 | #include "mesh_localize/KLTTracker.h" 2 | #include 3 | 4 | using namespace Eigen; 5 | using namespace cv; 6 | 7 | KLTTracker::KLTTracker() 8 | { 9 | m_nextID = 0; 10 | m_maxNumberOfPoints = 200; 11 | m_fastDetector = cv::FastFeatureDetector::create(std::string("FAST")); 12 | } 13 | 14 | std::vector KLTTracker::filterMatchesEpipolarContraint( 15 | const std::vector& pts1, 16 | const std::vector& pts2) 17 | { 18 | std::vector status; 19 | findFundamentalMat(pts1, pts2, CV_FM_RANSAC, 3, .99, status); 20 | return status; 21 | } 22 | 23 | void KLTTracker::init(const cv::Mat& inputFrame, const cv::Mat& depth, const Eigen::Matrix3f& inputK, 24 | const Eigen::Matrix3f& depthK, const Eigen::Matrix4f& inputTf, const cv::Mat& mask) 25 | { 26 | m_nextPts.clear(); 27 | m_prevPts.clear(); 28 | m_ptIDs.clear(); 29 | m_tracked3dPts.clear(); 30 | m_nextKeypoints.clear(); 31 | m_prevKeypoints.clear(); 32 | m_mask = mask; 33 | m_nextID = 0; 34 | 35 | m_fastDetector->detect(inputFrame, m_nextKeypoints, m_mask); 36 | std::random_shuffle(m_nextKeypoints.begin(), m_nextKeypoints.end()); 37 | m_nextKeypoints.resize(m_maxNumberOfPoints < m_nextKeypoints.size() ? 38 | m_maxNumberOfPoints : m_nextKeypoints.size()); 39 | 40 | for (size_t i=0; i(int(depth_kp(1)), int(depth_kp(0))); 46 | if(pt_depth == 0 || pt_depth == -1) 47 | continue; 48 | 49 | m_prevPts.push_back(m_nextKeypoints[i].pt); 50 | m_ptIDs.push_back(m_nextID++); 51 | 52 | Eigen::Vector3f backproj = inputK.inverse()*hkp; 53 | backproj /= backproj(2); 54 | backproj *= pt_depth; 55 | Eigen::Vector4f backproj_h(backproj(0), backproj(1), backproj(2), 1); 56 | backproj_h = inputTf*backproj_h; 57 | m_tracked3dPts.push_back(Point3f(backproj_h(0), backproj_h(1), backproj_h(2))); 58 | } 59 | inputFrame.copyTo(m_prevImg); 60 | } 61 | 62 | //! Processes a frame and returns output image 63 | bool KLTTracker::processFrame(const cv::Mat& inputFrame, cv::Mat& outputFrame, 64 | std::vector& pts2d, std::vector& pts3d, std::vector& ptIDs) 65 | { 66 | pts2d.clear(); 67 | pts3d.clear(); 68 | inputFrame.copyTo(m_nextImg); 69 | cv::cvtColor(inputFrame, outputFrame, CV_GRAY2BGR); 70 | 71 | if (m_mask.rows != inputFrame.rows || m_mask.cols != inputFrame.cols) 72 | m_mask.create(inputFrame.rows, inputFrame.cols, CV_8UC1); 73 | if (m_prevPts.size() > 0) 74 | { 75 | cv::calcOpticalFlowPyrLK(m_prevImg, m_nextImg, m_prevPts, m_nextPts, m_status, m_error); 76 | } 77 | m_mask = cv::Scalar(255); 78 | std::vector lkPrevPts, lkNextPts; 79 | std::vector lk3dPts; 80 | std::vector lkTrackedPtIDs; 81 | for (size_t i=0; i epStatus; 92 | if(lkPrevPts.size() > 0) 93 | epStatus = filterMatchesEpipolarContraint(lkPrevPts, lkNextPts); 94 | std::vector trackedPts; 95 | std::vector tracked3dPts; 96 | std::vector trackedPtIDs; 97 | for (size_t i=0; i& idxs) 122 | { 123 | std::vector trackedPts; 124 | std::vector tracked3dPts; 125 | std::vector trackedPtIDs; 126 | for (size_t i = 0; i < idxs.size(); i++) 127 | { 128 | trackedPts.push_back(m_prevPts.at(idxs.at(i))); 129 | tracked3dPts.push_back(m_tracked3dPts.at(idxs.at(i))); 130 | trackedPtIDs.push_back(m_ptIDs.at(idxs.at(i))); 131 | } 132 | m_tracked3dPts = tracked3dPts; 133 | m_prevPts = trackedPts; 134 | m_ptIDs = trackedPtIDs; 135 | } 136 | -------------------------------------------------------------------------------- /src/KeyframeContainer.cpp: -------------------------------------------------------------------------------- 1 | #include "mesh_localize/KeyframeContainer.h" 2 | #include "mesh_localize/ASiftDetector.h" 3 | #include "opencv2/features2d/features2d.hpp" 4 | 5 | #ifdef MESH_LOCALIZER_ENABLE_GPU 6 | #include 7 | #include 8 | #endif 9 | 10 | KeyframeContainer::KeyframeContainer(Mat img, std::string desc_type, bool extract_now) 11 | : desc_type(desc_type), has_depth(false), delete_cc(true) 12 | { 13 | mask = Mat(img.rows, img.cols, CV_8U, Scalar(255)); 14 | cc = new CameraContainer(img); 15 | if(extract_now) 16 | ExtractFeatures(desc_type); 17 | } 18 | 19 | KeyframeContainer::KeyframeContainer(Mat img, std::vector& keypoints, Mat& descriptors) : 20 | keypoints(keypoints), 21 | descriptors(descriptors) 22 | { 23 | cc = new CameraContainer(img); 24 | mask = Mat(img.rows, img.cols, CV_8U, Scalar(255)); 25 | delete_cc = true; 26 | has_depth = false; 27 | } 28 | 29 | KeyframeContainer::KeyframeContainer(Mat img, std::vector& keypoints, Mat& descriptors, Mat& depth) : 30 | keypoints(keypoints), 31 | descriptors(descriptors), 32 | depth(depth) 33 | { 34 | cc = new CameraContainer(img); 35 | mask = Mat(img.rows, img.cols, CV_8U, Scalar(255)); 36 | delete_cc = true; 37 | has_depth = true; 38 | } 39 | KeyframeContainer::KeyframeContainer(CameraContainer* cc, std::string desc_type) : 40 | cc(cc) 41 | { 42 | delete_cc = false; 43 | mask = Mat(cc->GetImage().rows, cc->GetImage().cols, CV_8U, Scalar(255)); 44 | ExtractFeatures(desc_type); 45 | has_depth = false; 46 | } 47 | 48 | KeyframeContainer::KeyframeContainer(CameraContainer* cc, std::vector& keypoints, Mat& descriptors) : 49 | cc(cc), 50 | keypoints(keypoints), 51 | descriptors(descriptors) 52 | { 53 | mask = Mat(cc->GetImage().rows, cc->GetImage().cols, CV_8U, Scalar(255)); 54 | delete_cc = false; 55 | has_depth = false; 56 | } 57 | 58 | KeyframeContainer::KeyframeContainer(CameraContainer* cc, std::vector& keypoints, Mat& descriptors, Mat& depth) : 59 | cc(cc), 60 | keypoints(keypoints), 61 | descriptors(descriptors), 62 | depth(depth) 63 | { 64 | mask = Mat(cc->GetImage().rows, cc->GetImage().cols, CV_8U, Scalar(255)); 65 | delete_cc = false; 66 | has_depth = true; 67 | } 68 | 69 | KeyframeContainer::KeyframeContainer(const KeyframeContainer& kfc) 70 | { 71 | this->keypoints = kfc.keypoints; 72 | this->descriptors = kfc.descriptors; 73 | this->cc = new CameraContainer(kfc.cc->GetImage(), kfc.cc->GetTf(), kfc.cc->GetK()); 74 | this->delete_cc = true; 75 | this->has_depth = kfc.has_depth; 76 | this->depth = kfc.depth; 77 | this->mask = kfc.mask; 78 | } 79 | 80 | KeyframeContainer::~KeyframeContainer() 81 | { 82 | if(delete_cc) 83 | { 84 | delete cc; 85 | } 86 | } 87 | 88 | void KeyframeContainer::SetMask(Mat new_mask) 89 | { 90 | assert(cc->GetImage().rows == mask.rows); 91 | assert(cc->GetImage().cols == mask.cols); 92 | mask = new_mask; 93 | } 94 | 95 | void KeyframeContainer::ExtractFeatures() 96 | { 97 | ExtractFeatures(desc_type); 98 | } 99 | 100 | void KeyframeContainer::ExtractFeatures(std::string desc_type) 101 | { 102 | Mat img = cc->GetImage(); 103 | if(desc_type == "asift") 104 | { 105 | ASiftDetector detector; 106 | detector.detectAndCompute(img, keypoints, descriptors, mask, ASiftDetector::SIFT); 107 | } 108 | else if(desc_type == "asurf") 109 | { 110 | ASiftDetector detector; 111 | detector.detectAndCompute(img, keypoints, descriptors, mask, ASiftDetector::SURF); 112 | } 113 | else if(desc_type == "orb") 114 | { 115 | ORB orb(1000, 1.2f, 4); 116 | orb(img, mask, keypoints, descriptors); 117 | } 118 | else if(desc_type == "surf") 119 | { 120 | SurfFeatureDetector detector; 121 | detector.detect(img, keypoints, mask); 122 | 123 | SurfDescriptorExtractor extractor; 124 | extractor.compute(img, keypoints, descriptors); 125 | //std::cout << "surf kps: " << keypoints.size() << std::endl; 126 | } 127 | #ifdef MESH_LOCALIZER_ENABLE_GPU 128 | else if(desc_type == "surf_gpu") 129 | { 130 | gpu::SURF_GPU surf_gpu; 131 | gpu::GpuMat kps_gpu, mask_gpu(mask), img_gpu(img); 132 | surf_gpu(img_gpu, mask_gpu, kps_gpu, descriptors_gpu); 133 | surf_gpu.downloadKeypoints(kps_gpu, keypoints); 134 | } 135 | #endif 136 | 137 | } 138 | 139 | Mat KeyframeContainer::GetImage() 140 | { 141 | return cc->GetImage(); 142 | } 143 | 144 | Mat KeyframeContainer::GetDepth() 145 | { 146 | if(!has_depth) 147 | { 148 | std::cout << "KeyframeContainer: WARNING, DEPTH NOT SET" << std::endl; 149 | } 150 | return depth; 151 | } 152 | 153 | #ifdef MESH_LOCALIZER_ENABLE_GPU 154 | gpu::GpuMat KeyframeContainer::GetGPUDescriptors() 155 | { 156 | return descriptors_gpu; 157 | } 158 | #endif 159 | 160 | Mat KeyframeContainer::GetDescriptors() 161 | { 162 | return descriptors; 163 | } 164 | 165 | std::vector KeyframeContainer::GetKeypoints() 166 | { 167 | return keypoints; 168 | } 169 | 170 | Eigen::Matrix4f KeyframeContainer::GetTf() 171 | { 172 | return cc->GetTf(); 173 | } 174 | 175 | Eigen::Matrix3f KeyframeContainer::GetK() 176 | { 177 | return cc->GetK(); 178 | } 179 | 180 | -------------------------------------------------------------------------------- /src/KeyframeMatch.cpp: -------------------------------------------------------------------------------- 1 | #include "mesh_localize/KeyframeMatch.h" 2 | 3 | KeyframeMatch::KeyframeMatch(KeyframeContainer* kfc, std::vector< DMatch >& matches, std::vector< DMatch >& allMatches, std::vector < Point2f >& matchPts1, std::vector< Point2f >& matchPts2, std::vector< KeyPoint > matchKps1, std::vector< KeyPoint > matchKps2) : 4 | kfc(kfc), 5 | matches(matches), 6 | allMatches(allMatches), 7 | matchPts1(matchPts1), 8 | matchPts2(matchPts2), 9 | matchKps1(matchKps1), 10 | matchKps2(matchKps2), 11 | tot_dist(-1) 12 | { 13 | } 14 | 15 | bool KeyframeMatch::operator< (const KeyframeMatch& kfm) const 16 | { 17 | /** Try sorting by avg match distance? ** 18 | if(tot_dist < 0) 19 | { 20 | tot_dist = 0; 21 | for(int i = 0; i < matches.size(); i++) 22 | { 23 | tot_dist += matches[i].distance; 24 | } 25 | } 26 | if(kfm.tot_dist < 0) 27 | { 28 | kfm.tot_dist = 0; 29 | for(int i = 0; i < kfm.matches.size(); i++) 30 | { 31 | kfm.tot_dist += kfm.matches[i].distance; 32 | } 33 | } 34 | return tot_dist/double(matches.size()) < kfm.tot_dist/double(kfm.matches.size()); 35 | **/ 36 | return (matches.size() > kfm.matches.size()); 37 | } 38 | 39 | -------------------------------------------------------------------------------- /src/MapFeatures.cpp: -------------------------------------------------------------------------------- 1 | #include "mesh_localize/MapFeatures.h" 2 | 3 | MapFeatures::MapFeatures(std::vector& kcv, pcl::PointCloud::Ptr cloud) : 4 | kcv(kcv), cloud(cloud) 5 | { 6 | const int match_neighborhood = 1; 7 | const int maxDepth = 99999999; 8 | descriptors = Mat(0, kcv[0]->GetDescriptors().cols, CV_32F); 9 | 10 | for(unsigned int i = 0; i < kcv.size(); i++) 11 | { 12 | int height = kcv[i]->GetImage().size().height; 13 | int width = kcv[i]->GetImage().size().width; 14 | std::vector< std::vector< int > > idx_3dpt(width); 15 | std::vector< std::vector< double > > depths(width); 16 | for(int j = 0; j < width; j++) 17 | { 18 | idx_3dpt[j].resize(height, -1); 19 | depths[j].resize(height, maxDepth); 20 | } 21 | 22 | //Create depth map for keyframe 23 | Eigen::MatrixXf P(3,4); 24 | P = kcv[i]->GetK()*kcv[i]->GetTf().inverse().block<3,4>(0,0); 25 | for(unsigned int j = 0; j < cloud->points.size(); j++) 26 | { 27 | Eigen::Vector4f hpt(cloud->points[j].x, cloud->points[j].y, cloud->points[j].z, 1); 28 | Eigen::Vector3f impt = P*hpt; 29 | impt /= impt(2); 30 | if(impt(0) < 0 || impt(0) >= width || impt(1) < 0 || impt(1) >= height) 31 | { 32 | continue; 33 | } 34 | double depth = (kcv[i]->GetTf().inverse()*hpt)(2); 35 | int dx_idx = floor(impt(0)); 36 | int dy_idx = floor(impt(1)); 37 | if(depth > 0 && depth < depths[dx_idx][dy_idx]) 38 | { 39 | depths[dx_idx][dy_idx] = depth; 40 | idx_3dpt[dx_idx][dy_idx] = j; 41 | } 42 | } 43 | 44 | // Find closest 3d pt to each image descriptor (only use if it's in some neighborhood) 45 | std::vector kps = kcv[i]->GetKeypoints(); 46 | for(unsigned int j = 0; j < kps.size(); j++) 47 | { 48 | int closestPt = -1; 49 | int pt_x = kps[i].pt.x; 50 | int pt_y = kps[i].pt.y; 51 | 52 | for(int mn = 0; mn <= match_neighborhood && closestPt == -1; mn++) 53 | { 54 | for(int k = pt_x-mn; k <= pt_x+mn && closestPt == -1; k++) 55 | { 56 | for(int l = pt_y-mn; l <= pt_y+mn && closestPt == -1; l++) 57 | { 58 | if(k < 0 || k >= width || l < 0 || l >= height) 59 | continue; 60 | if(depths[k][l] < maxDepth) 61 | { 62 | closestPt = idx_3dpt[k][l]; 63 | } 64 | } 65 | } 66 | } 67 | if(closestPt != -1) 68 | { 69 | keypoints.push_back(pcl::PointXYZ(cloud->points[closestPt].x, cloud->points[closestPt].y, cloud->points[closestPt].z)); 70 | descriptors.push_back(kcv[i]->GetDescriptors().row(j)); 71 | } 72 | } 73 | } 74 | std::cout << "Num Map Features: " << keypoints.size() << " " << descriptors.rows << std::endl; 75 | } 76 | 77 | Mat MapFeatures::GetDescriptors() const 78 | { 79 | return descriptors; 80 | } 81 | 82 | std::vector MapFeatures::GetKeypoints() const 83 | { 84 | return keypoints; 85 | } 86 | 87 | -------------------------------------------------------------------------------- /src/OgreImageGenerator.cpp: -------------------------------------------------------------------------------- 1 | #include "mesh_localize/OgreImageGenerator.h" 2 | 3 | using namespace cv; 4 | 5 | OgreImageGenerator::OgreImageGenerator(std::string resource_path, std::string model_name, double fx, 6 | double fy, bool use_depth_shader) 7 | : use_depth_shader(use_depth_shader) 8 | { 9 | app = new CameraRenderApplication(resource_path, fx, fy); 10 | std::cout << "Using OGRE resource path " << resource_path << std::endl; 11 | app->go(); 12 | app->loadModel("model", model_name); 13 | 14 | vih = new VirtualImageHandler(app); 15 | } 16 | 17 | double OgreImageGenerator::GetHeight() 18 | { 19 | return vih->getImageHeight(); 20 | } 21 | 22 | double OgreImageGenerator::GetWidth() 23 | { 24 | return vih->getImageWidth(); 25 | } 26 | 27 | Eigen::Matrix3f OgreImageGenerator::GetK() 28 | { 29 | Mat Kcv = vih->getCameraIntrinsics(); 30 | //std::cout << "Kcv=" << std::endl << Kcv << std::endl ; 31 | Eigen::Matrix3f K; 32 | K << Kcv.at(0,0), Kcv.at(0,1), Kcv.at(0,2), 33 | Kcv.at(1,0), Kcv.at(1,1), Kcv.at(1,2), 34 | Kcv.at(2,0), Kcv.at(2,1), Kcv.at(2,2); 35 | //std::cout << "K=" << std::endl << K << std::endl; 36 | return K; 37 | } 38 | 39 | Mat OgreImageGenerator::GenerateVirtualImage(const Eigen::Matrix4f& pose, cv::Mat& depth, cv::Mat& mask) 40 | { 41 | double x = pose(0,3); 42 | double y = pose(1,3); 43 | double z = pose(2,3); 44 | 45 | Eigen::Quaternionf q(pose.block<3,3>(0,0)); 46 | Mat im; 47 | vih->getVirtualImageAndDepthInternal(im, depth, x, y, z, q.w(), q.x(), q.y(), q.z()); 48 | if(!use_depth_shader) 49 | { 50 | vih->getVirtualDepthNoShader(depth, x, y, z, q.w(), q.x(), q.y(), q.z()); 51 | } 52 | // dilate the depth a bit so the outline edges aren't masked out 53 | int dilate_size = 1; 54 | Mat element = getStructuringElement(MORPH_RECT, Size(2*dilate_size+1,2*dilate_size+1), Point(dilate_size,dilate_size)); 55 | dilate(depth, depth, element); 56 | //medianBlur(depth, depth, 5); 57 | mask = Mat(app->getWindowHeight(), app->getWindowWidth(), CV_8U, Scalar(255)); 58 | 59 | for(int i = 0; i < app->getWindowHeight(); i++) 60 | { 61 | for(int j = 0; j < app->getWindowWidth(); j++) 62 | { 63 | if(depth.at(i, j) == 0 || depth.at(i, j) == -1) 64 | { 65 | mask.at(i, j) = 0; 66 | } 67 | } 68 | } 69 | return im; 70 | } 71 | -------------------------------------------------------------------------------- /src/PnPUtil.cpp: -------------------------------------------------------------------------------- 1 | #include "mesh_localize/PnPUtil.h" 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | using namespace cv; 8 | 9 | std::vector PnPUtil::BackprojectPts(const std::vector& pts, const Eigen::Matrix4f& camTf, const Eigen::Matrix3f& K, const Mat& depth) 10 | { 11 | std::vector pts3d; 12 | for(int i = 0; i < pts.size(); i++) 13 | { 14 | Point2f pt = pts[i]; 15 | Eigen::Vector3f hkp(pt.x, pt.y, 1); 16 | Eigen::Vector3f backproj = K.inverse()*hkp; 17 | backproj /= backproj(2); 18 | backproj *= depth.at(pt.y, pt.x); 19 | if(depth.at(pt.y, pt.x) == 0) 20 | std::cout << "Bad depth (0)" << std::endl; 21 | Eigen::Vector4f backproj_h(backproj(0), backproj(1), backproj(2), 1); 22 | backproj_h = camTf*backproj_h; 23 | pts3d.push_back(Point3f(backproj_h(0), backproj_h(1), backproj_h(2))); 24 | } 25 | return pts3d; 26 | } 27 | 28 | bool PnPUtil::RansacPnP(const std::vector& matchPts3d, const std::vector& matchPts, Mat Kcv, Eigen::Matrix4f tfguess, Eigen::Matrix4f& tf, std::vector& bestInliersIdx, double* avgReprojError, Eigen::Matrix* cov) 29 | { 30 | bestInliersIdx.clear(); 31 | Mat distcoeffcvPnp = (Mat_(4,1) << 0, 0, 0, 0); 32 | tf = Eigen::MatrixXf::Identity(4,4); 33 | Mat Rvec, t; 34 | Mat Rguess = (Mat_(3,3) << tfguess(0,0), tfguess(0,1), tfguess(0,2), 35 | tfguess(1,0), tfguess(1,1), tfguess(1,2), 36 | tfguess(2,0), tfguess(2,1), tfguess(2,2)); 37 | Rodrigues(Rguess, Rvec); 38 | t = (Mat_(3,1) << tfguess(0,3), tfguess(1,3), tfguess(2,3)); 39 | 40 | // RANSAC PnP 41 | const int niter = 50; // Assumes about 45% outliers 42 | const double reprojThresh = 5.0; // in pixels 43 | const int m = 4; // points per sample 44 | const int inlier_ratio_cutoff = 0.4; 45 | std::vector ind; 46 | for(unsigned int i = 0; i < matchPts.size(); i++) 47 | { 48 | ind.push_back(i); 49 | } 50 | 51 | bool abort = false; 52 | //ros::Time start = ros::Time::now(); 53 | //#pragma omp parallel for 54 | for(int i = 0; i < niter; i++) 55 | { 56 | //#pragma omp flush (abort) 57 | //if(abort) 58 | //{ 59 | // continue; 60 | //} 61 | 62 | Eigen::Matrix4f rand_tf; 63 | // Get m random points 64 | std::random_shuffle(ind.begin(), ind.end()); 65 | std::vector randInd(ind.begin(), ind.begin()+m); 66 | 67 | std::vector rand_matchPts3d; 68 | std::vector rand_matchPts; 69 | for(int j = 0; j < m; j++) 70 | { 71 | rand_matchPts3d.push_back(matchPts3d[randInd[j]]); 72 | rand_matchPts.push_back(matchPts[randInd[j]]); 73 | } 74 | 75 | Mat ran_Rvec, ran_t; 76 | Rvec.copyTo(ran_Rvec); 77 | t.copyTo(ran_t); 78 | 79 | solvePnP(rand_matchPts3d, rand_matchPts, Kcv, distcoeffcvPnp, ran_Rvec, ran_t, true, CV_P3P); 80 | 81 | // Test for inliers 82 | std::vector reprojPts; 83 | projectPoints(matchPts3d, ran_Rvec, ran_t, Kcv, distcoeffcvPnp, reprojPts); 84 | std::vector inliersIdx; 85 | for(unsigned int j = 0; j < reprojPts.size(); j++) 86 | { 87 | double reprojError = sqrt((reprojPts[j].x-matchPts[j].x)*(reprojPts[j].x-matchPts[j].x) + (reprojPts[j].y-matchPts[j].y)*(reprojPts[j].y-matchPts[j].y)); 88 | 89 | if(reprojError < reprojThresh) 90 | { 91 | inliersIdx.push_back(j); 92 | } 93 | } 94 | 95 | //#pragma omp critical 96 | { 97 | if(inliersIdx.size() > bestInliersIdx.size()) 98 | { 99 | bestInliersIdx = inliersIdx; 100 | } 101 | if(bestInliersIdx.size() > inlier_ratio_cutoff*matchPts.size()) 102 | { 103 | //std::cout << "Pnp abort n=" << i << std::endl; 104 | //abort = true; 105 | //#pragma omp flush (abort) 106 | } 107 | } 108 | 109 | } 110 | std::cout << "Num inliers: " << bestInliersIdx.size() << "/" << matchPts.size() << std::endl; 111 | if(bestInliersIdx.size() < 3) 112 | { 113 | std::cout << "RansacPnP: Could not find enough inliers (" << bestInliersIdx.size() << ")" 114 | << std::endl; 115 | return false; 116 | } 117 | //ROS_INFO("PnpRansac: Ransac time: %f", (ros::Time::now()-start).toSec()); 118 | 119 | std::vector inlierPts3d; 120 | std::vector inlierPts2d; 121 | for(unsigned int i = 0; i < bestInliersIdx.size(); i++) 122 | { 123 | inlierPts3d.push_back(matchPts3d[bestInliersIdx[i]]); 124 | inlierPts2d.push_back(matchPts[bestInliersIdx[i]]); 125 | } 126 | 127 | //start = ros::Time::now(); 128 | solvePnP(inlierPts3d, inlierPts2d, Kcv, distcoeffcvPnp, Rvec, t, true); 129 | //ROS_INFO("PnpRansac: Pnp Inliers time: %f", (ros::Time::now()-start).toSec()); 130 | if(avgReprojError) 131 | { 132 | *avgReprojError = 0; 133 | std::vector reprojPts; 134 | Mat J; 135 | projectPoints(inlierPts3d, Rvec, t, Kcv, distcoeffcvPnp, reprojPts, J); 136 | for(unsigned int j = 0; j < reprojPts.size(); j++) 137 | { 138 | double reprojError = sqrt((reprojPts[j].x-inlierPts2d[j].x)*(reprojPts[j].x-inlierPts2d[j].x) + (reprojPts[j].y-inlierPts2d[j].y)*(reprojPts[j].y-inlierPts2d[j].y)); 139 | *avgReprojError += reprojError/reprojPts.size(); 140 | } 141 | // Compute covariance matrix of rotation and translation 142 | Mat Sigma = Mat(J.t() * J, Rect(0,0,6,6)).inv(); 143 | 144 | // Compute standard deviation 145 | //Mat std_dev; 146 | //sqrt(Sigma.diag(), std_dev); 147 | 148 | if(cov) 149 | { 150 | cov->setZero(); 151 | for(int i = 0; i < Sigma.rows; i++) 152 | { 153 | cov->diagonal()[i] = Sigma.at(i,i); 154 | } 155 | //std::cout << "R, t covariance:" << std::endl << *cov << std::endl; 156 | } 157 | } 158 | 159 | Mat R; 160 | Rodrigues(Rvec, R); 161 | 162 | tf << R.at(0,0), R.at(0,1), R.at(0,2), t.at(0), 163 | R.at(1,0), R.at(1,1), R.at(1,2), t.at(1), 164 | R.at(2,0), R.at(2,1), R.at(2,2), t.at(2), 165 | 0, 0, 0, 1; 166 | 167 | 168 | return true; 169 | 170 | } 171 | -------------------------------------------------------------------------------- /src/PointCloudImageGenerator.cpp: -------------------------------------------------------------------------------- 1 | #include "mesh_localize/PointCloudImageGenerator.h" 2 | 3 | #include 4 | 5 | using namespace cv; 6 | 7 | PointCloudImageGenerator::PointCloudImageGenerator(pcl::PointCloud::Ptr pc, const Eigen::Matrix3f& K, int rows, int cols) : 8 | map_cloud(pc), 9 | K(K), 10 | rows(rows), 11 | cols(cols) 12 | { 13 | } 14 | 15 | Eigen::Matrix3f PointCloudImageGenerator::GetK() 16 | { 17 | return K; 18 | } 19 | 20 | cv::Mat PointCloudImageGenerator::GenerateVirtualImage(const Eigen::Matrix4f& tf, cv::Mat& depths, cv::Mat& mask) 21 | { 22 | double scale = 1.0; 23 | int height = rows * scale; 24 | int width = cols * scale; 25 | 26 | Mat img(height, width, CV_8U, Scalar(0)); 27 | depths = Mat(height, width, CV_32F, Scalar(-1)); 28 | mask = Mat(height, width, CV_8U, Scalar(0)); 29 | 30 | Eigen::MatrixXf P(3,4); 31 | P = K*tf.inverse().block<3,4>(0,0); 32 | for(unsigned int j = 0; j < map_cloud->points.size(); j++) 33 | { 34 | Eigen::Matrix3f Rinv = tf.inverse().block<3,3>(0,0); 35 | Eigen::Vector3f normal(map_cloud->points[j].normal_x, map_cloud->points[j].normal_y, map_cloud->points[j].normal_z); 36 | normal = Rinv*normal; 37 | 38 | Eigen::Vector4f hpt(map_cloud->points[j].x, map_cloud->points[j].y, map_cloud->points[j].z, 1); 39 | Eigen::Vector3f impt = P*hpt; 40 | impt /= impt(2); 41 | int dx_idx = floor(impt(0)*scale); 42 | int dy_idx = floor(impt(1)*scale); 43 | if(dx_idx < 0 || dx_idx >= width || dy_idx < 0 || dy_idx >= height) 44 | { 45 | continue; 46 | } 47 | double depth = (tf.inverse()*hpt)(2); 48 | 49 | if(depth > 0 /*&& normal(2) < 0*/ && (depths.at(dy_idx, dx_idx) == -1 || depth < depths.at(dy_idx, dx_idx))) 50 | { 51 | depths.at(dy_idx, dx_idx) = depth; 52 | mask.at(dy_idx, dx_idx) = 255; 53 | img.at(dy_idx, dx_idx) = (*reinterpret_cast(&(map_cloud->points[j].rgb)) & 0x0000ff); 54 | } 55 | } 56 | 57 | //pyrUp(img, img);//, Size(oldheight, oldwidth)); 58 | medianBlur(img, img, 3); 59 | medianBlur(depths, depths, 5); 60 | //medianBlur(mask, mask, 3); 61 | return img; 62 | } 63 | -------------------------------------------------------------------------------- /src/mesh_localize_node.cpp: -------------------------------------------------------------------------------- 1 | #include "mesh_localize/MeshLocalizer.h" 2 | 3 | int main (int argc, char **argv) 4 | { 5 | ros::init (argc, argv, "mesh_localize"); 6 | ros::NodeHandle nh; 7 | ros::NodeHandle nh_private("~"); 8 | MeshLocalizer ml(nh, nh_private); 9 | ros::spin (); 10 | return 0; 11 | } 12 | -------------------------------------------------------------------------------- /src/render_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include "mesh_localize/OgreImageGenerator.h" 10 | 11 | using namespace cv; 12 | 13 | int main (int argc, char **argv) 14 | { 15 | ros::init (argc, argv, "render"); 16 | ros::NodeHandle nh; 17 | ros::NodeHandle nh_private("~"); 18 | 19 | double step_size; 20 | 21 | if(!nh_private.getParam("step_size", step_size)) 22 | step_size = 0.1; 23 | 24 | std::string resource_path = ros::package::getPath("mesh_localize"); 25 | resource_path += "/ogre_cfg/"; 26 | OgreImageGenerator oig(resource_path,"box.mesh"); 27 | Eigen::Matrix3f K = oig.GetK(); 28 | 29 | ros::Publisher image_pub = nh.advertise("/virtual_camera/image",1000); 30 | ros::Publisher depth_pub = nh.advertise("/virtual_camera/depth",1000); 31 | ros::Publisher info_pub = nh.advertise("/virtual_camera/camera_info",1000); 32 | ros::Rate loop_rate(60); 33 | 34 | double x = 0; 35 | double y = 0; 36 | double z = 10; 37 | double xybound = 4; 38 | double zubound = 12; 39 | double zlbound = 8; 40 | while(ros::ok()) 41 | { 42 | Mat depth, mask; 43 | Eigen::Matrix4f pose; 44 | 45 | x += (double(rand())/RAND_MAX)*step_size - step_size/2; 46 | y += (double(rand())/RAND_MAX)*step_size - step_size/2; 47 | z += (double(rand())/RAND_MAX)*step_size - step_size/2; 48 | x = std::max(x, -xybound); 49 | y = std::max(y, -xybound); 50 | z = std::max(z, zlbound); 51 | x = std::min(x, xybound); 52 | y = std::min(y, xybound); 53 | z = std::max(z, zubound); 54 | pose << 1, 0, 0, x, 55 | 0, -1, 0, y, 56 | 0, 0, -1, z, 57 | 0, 0, 0, 1; 58 | //pose << 0.97152, -0.0578314, 0.229792, 0.29792, 59 | // -3.78345e-10, 0.96976, 0.244059, 2.6059, 60 | // -0.236957, -0.237108, 0.942142, -9.42142, 61 | // 0, 0, 0, 1; 62 | Mat img = oig.GenerateVirtualImage(pose, depth, mask); 63 | cv_bridge::CvImage cv_img; 64 | cv_img.image = img; 65 | cv_img.encoding = sensor_msgs::image_encodings::MONO8; 66 | 67 | cv_bridge::CvImage cv_depth; 68 | cv_depth.image = depth; 69 | cv_depth.encoding = sensor_msgs::image_encodings::TYPE_32FC1; 70 | 71 | sensor_msgs::CameraInfo ci_msg; 72 | ci_msg.header.stamp = ros::Time::now(); 73 | ci_msg.height = img.rows; 74 | ci_msg.width = img.cols; 75 | ci_msg.distortion_model = "plumb_bob"; 76 | for(int i = 0; i < 5; i++) 77 | { 78 | ci_msg.D.push_back(0); 79 | } 80 | ci_msg.K[0] = K(0,0); ci_msg.K[1] = K(0,1); ci_msg.K[2] = K(0,2); 81 | ci_msg.K[3] = K(1,0); ci_msg.K[4] = K(1,1); ci_msg.K[5] = K(1,2); 82 | ci_msg.K[6] = K(2,0); ci_msg.K[7] = K(2,1); ci_msg.K[8] = K(2,2); 83 | 84 | info_pub.publish(ci_msg); 85 | image_pub.publish(cv_img.toImageMsg()); 86 | depth_pub.publish(cv_depth.toImageMsg()); 87 | img.release(); 88 | ros::spinOnce(); 89 | loop_rate.sleep(); 90 | } 91 | 92 | return 0; 93 | } 94 | -------------------------------------------------------------------------------- /worlds/campus.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 0.8 0.8 0.8 1 6 | 0.3 0.3 0.3 1.0 7 | false 8 | 9 | 10 | 13 | 14 | model://campus_terrain 15 | 0 0 0 0 0 0 16 | 17 | 18 | model://kinect 19 | 0 0 0 0 0 0 20 | 21 | 22 | 23 | 35 | 61 | 62 | 63 | 0 0 0 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /worlds/car_terrain.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 0.8 0.8 0.8 1 6 | 0.3 0.3 0.3 1.0 7 | false 8 | 9 | 10 | 13 | 14 | model://car_terrain 15 | 0 0 0 0 0 0 16 | 17 | 18 | model://kinect 19 | 0 0 0 0 0 0 20 | 21 | 22 | 23 | 35 | 61 | 62 | 63 | 0 0 0 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /worlds/lab_cage.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 1.0 1.0 1.0 1 6 | 0.3 0.3 0.3 1.0 7 | false 8 | 9 | 10 | 13 | 14 | model://lab_cage 15 | 0 0 0 0 0 0 16 | 17 | 18 | model://kinect 19 | 0 0 0 0 0 0 20 | 21 | 22 | 23 | 24 | 10 -15 15 0 0 0 25 | 0.8 0.8 0.8 1 26 | 0.2 0.2 0.2 1 27 | 28 | 1000 29 | 0.9 30 | 0.01 31 | 0.001 32 | 33 | 1 2 1 34 | 35 | 61 | 62 | 63 | 0 0 0 64 | 65 | 66 | 67 | 68 | 69 | --------------------------------------------------------------------------------