├── isam ├── .svn │ ├── format │ ├── wc.db-journal │ ├── entries │ ├── pristine │ │ ├── 14 │ │ │ └── 1496b33cf0f04afbd9a107d81e397411db3aaab9.svn-base │ │ ├── 20 │ │ │ └── 2024da76299676140101b366738ee33892dc722e.svn-base │ │ ├── 21 │ │ │ └── 2162b5eaef182f15ecab2f42f91feaf3a1926f20.svn-base │ │ ├── 27 │ │ │ └── 27db4a9ced7765ce48fb87c6595335d457860c01.svn-base │ │ ├── 33 │ │ │ └── 337b8c33fa51e232924be45987dbcd530c1b86bc.svn-base │ │ ├── 42 │ │ │ └── 4206d9b17106f76510795ccab2a014200df5a307.svn-base │ │ ├── 52 │ │ │ └── 522b6a3e126ac6b78ed7d377e61230397a64303f.svn-base │ │ ├── 55 │ │ │ └── 55c15f8fd0ececcd88525d904826b206e07cc448.svn-base │ │ ├── 58 │ │ │ └── 584489f1bd61a1c3d6b88e8d5045d468d147dcc9.svn-base │ │ ├── 63 │ │ │ └── 6361e1ba3ae5a636073644f0cc164aa5c935c8bb.svn-base │ │ ├── 67 │ │ │ └── 670ac3aa092d71b71d8ad4b07570fa7cb97cc28f.svn-base │ │ ├── 75 │ │ │ └── 751b7489c307a86e0ede563d7b15be83e06b5d25.svn-base │ │ ├── 77 │ │ │ └── 772380df70e416953a26103633700011754f35ec.svn-base │ │ ├── 82 │ │ │ └── 82cdecd818c4200c28ac6ff55a870eadb4a006ec.svn-base │ │ ├── 87 │ │ │ └── 87aae3226196f2892ee7c535ab22a7464cd54d55.svn-base │ │ ├── 97 │ │ │ └── 975c4482a96ef0f3f06aac33fda56cd35c16c0ae.svn-base │ │ ├── fd │ │ │ └── fdf01693d26ba4d1a0f74fbc5d607cffd07548ff.svn-base │ │ ├── d3 │ │ │ └── d3ec06879799cb1bdeddb7086cc9da04524a5d26.svn-base │ │ ├── da │ │ │ └── da0a0ce77bad19946bf8b4a71d09acbfeb3fb79f.svn-base │ │ ├── a4 │ │ │ └── a4f9f54def980732732152de4f63bad7cf9f4797.svn-base │ │ ├── 2e │ │ │ └── 2e9b2ab52a1aec4365ddf762f9d67cb5533ae8bc.svn-base │ │ ├── f6 │ │ │ └── f615e071c02c0e8aafbf8514e07c6ddee75fd764.svn-base │ │ ├── cf │ │ │ └── cfe14019eeadd617902826871d3076a9616d15e0.svn-base │ │ ├── d0 │ │ │ └── d07cdc14346979fb7a161421b984b001474cf422.svn-base │ │ ├── 06 │ │ │ └── 066b3c8b7b2b31c30ef7823e42b6e45f1929159f.svn-base │ │ ├── 0c │ │ │ └── 0c7726bd76d729f03f30a5b1bcce1c0b7703aa14.svn-base │ │ ├── 2b │ │ │ └── 2b642da8f576046edf8255bb4a6ba47660353d67.svn-base │ │ ├── 8f │ │ │ └── 8ff71bfd56ab26597d965fcc29995e1aac48d8a4.svn-base │ │ ├── ca │ │ │ └── ca7bb9539ed025f1c4669c092c4eb252c2c8b130.svn-base │ │ ├── e8 │ │ │ └── e8b090883df11a75df6b84ccc0192f449844cfd8.svn-base │ │ ├── ea │ │ │ └── ea9bd33e506a39fd8ac1e86bb3a0eb3c88a455f2.svn-base │ │ ├── 1b │ │ │ └── 1bf98790eb2dc0e17839198b9e394e8f443c8f0b.svn-base │ │ ├── 8d │ │ │ └── 8d81800db36466dbe6888420fb6b886844dcf522.svn-base │ │ ├── 0b │ │ │ └── 0bf86855590fe80c4e8a8b0a562a849456fdc568.svn-base │ │ ├── 7e │ │ │ └── 7ec1c0aeeca81cba1985a51b967fe5c30d386fa7.svn-base │ │ ├── d4 │ │ │ └── d4529d03edb1b4b6ba2c5f7593b1591beaf360c7.svn-base │ │ ├── 3b │ │ │ └── 3b9a434f0b10c459c9ce5faa42a7a719c2d5896d.svn-base │ │ ├── a9 │ │ │ └── a90a366a2e0ae8d2b39a575c5a039623d615cf55.svn-base │ │ ├── cb │ │ │ └── cb8ad72f9a74e9f214f0d5dd58af78dec68664b5.svn-base │ │ ├── a1 │ │ │ └── a12e19a7835e4130688003fff879b0ecef9da200.svn-base │ │ ├── f3 │ │ │ └── f3ea1cbba50adb51238ef2b814250c83b860f6b2.svn-base │ │ ├── be │ │ │ └── bee75c08e6b2f1bf5ab41e299ce5d88275b63d25.svn-base │ │ ├── d1 │ │ │ └── d1e59216a429059c2c6ffd6ba4f539e30cb1b920.svn-base │ │ └── 3a │ │ │ └── 3a7337d9c6cdeb5264445e25fa6a432c387a8a1c.svn-base │ └── wc.db ├── VERSION ├── bin │ └── isam ├── isam │ ├── lcmtypes │ │ ├── mrlcm_reset_collections_t.lcm │ │ ├── mrlcm_link_t.lcm │ │ ├── mrlcm_obj_t.lcm │ │ ├── mrlcm_cov_t.lcm │ │ ├── mrlcm_link_collection_t.lcm │ │ ├── mrlcm_cov_collection_t.lcm │ │ ├── mrlcm_obj_collection_t.lcm │ │ └── CMakeLists.txt │ ├── CMakeLists.txt │ ├── Lcm.h │ ├── Collections.h │ └── Viewer.h ├── misc │ └── CMakeLists.txt ├── isamlib │ ├── CMakeLists.txt │ ├── Node.cpp │ ├── numericalDiff.cpp │ └── util.cpp ├── Makefile ├── include │ └── isam │ │ ├── numericalDiff.h │ │ ├── Element.h │ │ ├── Noise.h │ │ ├── OptimizationInterface.h │ │ ├── Cholesky.h │ │ ├── Point2d.h │ │ ├── Point3d.h │ │ ├── SparseSystem.h │ │ ├── GLCReparam.h │ │ ├── Anchor.h │ │ ├── Jacobian.h │ │ └── Properties.h ├── README ├── CMakeLists.txt ├── cmake │ ├── FindCholmod.cmake │ └── FindEigen3.cmake └── ChangeLog ├── data ├── simulation.mat └── stata01162016obj.mat ├── include ├── nms │ ├── nms_mex.mexa64 │ ├── nms_gpu_mex.mexa64 │ ├── nms_multiclass.m │ ├── nvmex.m │ ├── nms.m │ └── nms_mex.cpp ├── utils │ ├── mkdir_if_missing.m │ ├── xVOChash_lookup.m │ ├── RectLTRB2LTWH.m │ ├── RectLTWH2LTRB.m │ ├── xVOCap.m │ ├── xVOChash_init.m │ ├── vis_label.m │ ├── prep_im_for_blob_size.m │ ├── im_list_to_blob.m │ ├── symbolic_link.m │ ├── boxes_filter.m │ ├── parse_rst.m │ ├── procid.m │ ├── tic_toc_print.m │ ├── auto_select_gpu.m │ ├── seed_rand.m │ ├── active_caffe_mex.m │ ├── prep_im_for_blob.m │ ├── boxoverlap.m │ ├── subsample_images.m │ ├── showboxes.m │ └── subsample_images_per_class.m ├── fast_rcnn │ ├── fast_rcnn_map_im_rois_to_feat_rois.m │ ├── fast_rcnn_bbox_transform.m │ ├── fast_rcnn_bbox_transform_inv.m │ ├── fast_rcnn_generate_sliding_windows.m │ └── fast_rcnn_config.m └── rpn │ ├── proposal_calc_output_size.m │ ├── proposal_locate_anchors.m │ ├── proposal_generate_anchors.m │ ├── proposal_visual_debug.m │ ├── proposal_test.m │ └── proposal_config.m ├── .gitmodules ├── README.md └── generateSimData.m /isam/.svn/format: -------------------------------------------------------------------------------- 1 | 12 2 | -------------------------------------------------------------------------------- /isam/.svn/wc.db-journal: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /isam/VERSION: -------------------------------------------------------------------------------- 1 | 1.7 2 | -------------------------------------------------------------------------------- /isam/.svn/entries: -------------------------------------------------------------------------------- 1 | 12 2 | -------------------------------------------------------------------------------- /isam/.svn/pristine/fd/fdf01693d26ba4d1a0f74fbc5d607cffd07548ff.svn-base: -------------------------------------------------------------------------------- 1 | 1.7 2 | -------------------------------------------------------------------------------- /isam/bin/isam: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubeipeng/objectSLAM/HEAD/isam/bin/isam -------------------------------------------------------------------------------- /isam/.svn/wc.db: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubeipeng/objectSLAM/HEAD/isam/.svn/wc.db -------------------------------------------------------------------------------- /data/simulation.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubeipeng/objectSLAM/HEAD/data/simulation.mat -------------------------------------------------------------------------------- /data/stata01162016obj.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubeipeng/objectSLAM/HEAD/data/stata01162016obj.mat -------------------------------------------------------------------------------- /include/nms/nms_mex.mexa64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubeipeng/objectSLAM/HEAD/include/nms/nms_mex.mexa64 -------------------------------------------------------------------------------- /include/nms/nms_gpu_mex.mexa64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubeipeng/objectSLAM/HEAD/include/nms/nms_gpu_mex.mexa64 -------------------------------------------------------------------------------- /isam/isam/lcmtypes/mrlcm_reset_collections_t.lcm: -------------------------------------------------------------------------------- 1 | package mrlcm; 2 | 3 | struct reset_collections_t 4 | { 5 | } 6 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "caffe-fast-rcnn"] 2 | path = caffe-fast-rcnn 3 | url = https://github.com/rbgirshick/caffe-fast-rcnn.git 4 | -------------------------------------------------------------------------------- /isam/.svn/pristine/55/55c15f8fd0ececcd88525d904826b206e07cc448.svn-base: -------------------------------------------------------------------------------- 1 | package mrlcm; 2 | 3 | struct reset_collections_t 4 | { 5 | } 6 | -------------------------------------------------------------------------------- /include/utils/mkdir_if_missing.m: -------------------------------------------------------------------------------- 1 | function made = mkdir_if_missing(path) 2 | made = false; 3 | if exist(path, 'dir') == 0 4 | mkdir(path); 5 | made = true; 6 | end 7 | -------------------------------------------------------------------------------- /isam/.svn/pristine/d3/d3ec06879799cb1bdeddb7086cc9da04524a5d26.svn-base: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubeipeng/objectSLAM/HEAD/isam/.svn/pristine/d3/d3ec06879799cb1bdeddb7086cc9da04524a5d26.svn-base -------------------------------------------------------------------------------- /isam/.svn/pristine/da/da0a0ce77bad19946bf8b4a71d09acbfeb3fb79f.svn-base: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubeipeng/objectSLAM/HEAD/isam/.svn/pristine/da/da0a0ce77bad19946bf8b4a71d09acbfeb3fb79f.svn-base -------------------------------------------------------------------------------- /include/utils/xVOChash_lookup.m: -------------------------------------------------------------------------------- 1 | function ind = xVOChash_lookup(hash,s) 2 | % From the PASCAL VOC 2011 devkit 3 | 4 | hsize=numel(hash.key); 5 | h=mod(str2double(s([4 6:end])),hsize)+1; 6 | ind=hash.val{h}(strmatch(s,hash.key{h},'exact')); 7 | -------------------------------------------------------------------------------- /isam/isam/lcmtypes/mrlcm_link_t.lcm: -------------------------------------------------------------------------------- 1 | package mrlcm; 2 | 3 | struct link_t 4 | { 5 | int64_t id; // unique ID, can be timestamp 6 | int32_t collection1; 7 | int64_t id1; 8 | int32_t collection2; 9 | int64_t id2; 10 | } 11 | -------------------------------------------------------------------------------- /isam/isam/lcmtypes/mrlcm_obj_t.lcm: -------------------------------------------------------------------------------- 1 | package mrlcm; 2 | 3 | struct obj_t 4 | { 5 | int64_t id; // unique ID, can be timestamp 6 | double x; 7 | double y; 8 | double z; 9 | double yaw; 10 | double pitch; 11 | double roll; 12 | } 13 | -------------------------------------------------------------------------------- /include/utils/RectLTRB2LTWH.m: -------------------------------------------------------------------------------- 1 | function [ rectsLTWH ] = RectLTRB2LTWH( rectsLTRB ) 2 | %rects (l, t, r, b) to (l, t, w, h) 3 | 4 | rectsLTWH = [rectsLTRB(:, 1), rectsLTRB(:, 2), rectsLTRB(:, 3)-rectsLTRB(:,1)+1, rectsLTRB(:,4)-rectsLTRB(:,2)+1]; 5 | end 6 | 7 | -------------------------------------------------------------------------------- /include/utils/RectLTWH2LTRB.m: -------------------------------------------------------------------------------- 1 | function [ rectsLTRB ] = RectLTWH2LTRB(rectsLTWH) 2 | %rects (l, t, r, b) to (l, t, w, h) 3 | 4 | rectsLTRB = [rectsLTWH(:, 1), rectsLTWH(:, 2), rectsLTWH(:, 1)+rectsLTWH(:,3)-1, rectsLTWH(:,2)+rectsLTWH(:,4)-1]; 5 | end 6 | 7 | -------------------------------------------------------------------------------- /isam/.svn/pristine/a4/a4f9f54def980732732152de4f63bad7cf9f4797.svn-base: -------------------------------------------------------------------------------- 1 | package mrlcm; 2 | 3 | struct link_t 4 | { 5 | int64_t id; // unique ID, can be timestamp 6 | int32_t collection1; 7 | int64_t id1; 8 | int32_t collection2; 9 | int64_t id2; 10 | } 11 | -------------------------------------------------------------------------------- /isam/isam/lcmtypes/mrlcm_cov_t.lcm: -------------------------------------------------------------------------------- 1 | package mrlcm; 2 | 3 | struct cov_t 4 | { 5 | int64_t id; // unique ID, can be timestamp 6 | 7 | int32_t collection; 8 | int64_t element_id; 9 | 10 | int32_t n; 11 | double entries[n]; // typically xx,xy,xz,yy,yz,zz 12 | } 13 | -------------------------------------------------------------------------------- /isam/.svn/pristine/2e/2e9b2ab52a1aec4365ddf762f9d67cb5533ae8bc.svn-base: -------------------------------------------------------------------------------- 1 | package mrlcm; 2 | 3 | struct obj_t 4 | { 5 | int64_t id; // unique ID, can be timestamp 6 | double x; 7 | double y; 8 | double z; 9 | double yaw; 10 | double pitch; 11 | double roll; 12 | } 13 | -------------------------------------------------------------------------------- /include/utils/xVOCap.m: -------------------------------------------------------------------------------- 1 | function ap = xVOCap(rec,prec) 2 | % From the PASCAL VOC 2011 devkit 3 | 4 | mrec=[0 ; rec ; 1]; 5 | mpre=[0 ; prec ; 0]; 6 | for i=numel(mpre)-1:-1:1 7 | mpre(i)=max(mpre(i),mpre(i+1)); 8 | end 9 | i=find(mrec(2:end)~=mrec(1:end-1))+1; 10 | ap=sum((mrec(i)-mrec(i-1)).*mpre(i)); 11 | -------------------------------------------------------------------------------- /isam/.svn/pristine/27/27db4a9ced7765ce48fb87c6595335d457860c01.svn-base: -------------------------------------------------------------------------------- 1 | package mrlcm; 2 | 3 | struct cov_t 4 | { 5 | int64_t id; // unique ID, can be timestamp 6 | 7 | int32_t collection; 8 | int64_t element_id; 9 | 10 | int32_t n; 11 | double entries[n]; // typically xx,xy,xz,yy,yz,zz 12 | } 13 | -------------------------------------------------------------------------------- /include/utils/xVOChash_init.m: -------------------------------------------------------------------------------- 1 | function hash = xVOChash_init(strs) 2 | % From the PASCAL VOC 2011 devkit 3 | 4 | hsize=4999; 5 | hash.key=cell(hsize,1); 6 | hash.val=cell(hsize,1); 7 | 8 | for i=1:numel(strs) 9 | s=strs{i}; 10 | h=mod(str2double(s([4 6:end])),hsize)+1; 11 | j=numel(hash.key{h})+1; 12 | hash.key{h}{j}=strs{i}; 13 | hash.val{h}(j)=i; 14 | end 15 | 16 | -------------------------------------------------------------------------------- /isam/isam/lcmtypes/mrlcm_link_collection_t.lcm: -------------------------------------------------------------------------------- 1 | package mrlcm; 2 | 3 | struct link_collection_t 4 | { 5 | int32_t id; // ID of collection, needed for example for links 6 | string name; // name to display in viewer 7 | int16_t type; // currently unused; 8 | boolean reset; // start from scratch or keep previous data 9 | 10 | int32_t nlinks; 11 | link_t links[nlinks]; 12 | } 13 | -------------------------------------------------------------------------------- /include/utils/vis_label.m: -------------------------------------------------------------------------------- 1 | function vis_label(imdb, roidb) 2 | 3 | rois = roidb.rois; 4 | for iIM = 1:length(rois) 5 | im = imread(imdb.image_at(iIM)); 6 | boxes = arrayfun(@(x) rois(iIM).boxes(rois(iIM).class == x, :), 1:length(imdb.classes), 'UniformOutput', false); 7 | legends = imdb.classes; 8 | showboxes(im, boxes, legends); 9 | pause; 10 | end 11 | end 12 | -------------------------------------------------------------------------------- /isam/.svn/pristine/f6/f615e071c02c0e8aafbf8514e07c6ddee75fd764.svn-base: -------------------------------------------------------------------------------- 1 | package mrlcm; 2 | 3 | struct link_collection_t 4 | { 5 | int32_t id; // ID of collection, needed for example for links 6 | string name; // name to display in viewer 7 | int16_t type; // currently unused; 8 | boolean reset; // start from scratch or keep previous data 9 | 10 | int32_t nlinks; 11 | link_t links[nlinks]; 12 | } 13 | -------------------------------------------------------------------------------- /isam/.svn/pristine/cf/cfe14019eeadd617902826871d3076a9616d15e0.svn-base: -------------------------------------------------------------------------------- 1 | 2 | Type "make doc" in the main directory (one level above this directory) 3 | to generate the full source documentation including installation 4 | instructions and examples. Then point your browser to: 5 | doc/html/index.html 6 | 7 | The latest documentation is also available online at: 8 | http://people.csail.mit.edu/kaess/isam/doc 9 | 10 | Michael Kaess, 2010 11 | -------------------------------------------------------------------------------- /isam/isam/lcmtypes/mrlcm_cov_collection_t.lcm: -------------------------------------------------------------------------------- 1 | package mrlcm; 2 | 3 | struct cov_collection_t 4 | { 5 | int32_t id; // ID of collection, needed for example for links 6 | string name; // name to display in viewer 7 | int16_t type; // what to draw, see constants below 8 | boolean reset; // start from scratch or keep previous data 9 | 10 | int32_t ncovs; 11 | cov_t covs[ncovs]; 12 | 13 | const int16_t ELLIPSOID=1; 14 | } 15 | -------------------------------------------------------------------------------- /isam/misc/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required (VERSION 2.6) 2 | 3 | link_libraries(isamlib) 4 | 5 | # generate a target for each .cpp file in this directory 6 | file(GLOB misc RELATIVE "${PROJECT_SOURCE_DIR}/misc" "*.cpp") 7 | add_custom_target(misc) 8 | foreach(file ${misc}) 9 | string(REPLACE ".cpp" "" file ${file}) 10 | add_executable(${file} EXCLUDE_FROM_ALL ${file}.cpp) 11 | add_dependencies(misc ${file}) 12 | endforeach(file) 13 | -------------------------------------------------------------------------------- /include/utils/prep_im_for_blob_size.m: -------------------------------------------------------------------------------- 1 | function im_scale = prep_im_for_blob_size(im_size, target_size, max_size) 2 | 3 | im_size_min = min(im_size(1:2)); 4 | im_size_max = max(im_size(1:2)); 5 | im_scale = double(target_size) / im_size_min; 6 | 7 | % Prevent the biggest axis from being more than MAX_SIZE 8 | if round(im_scale * im_size_max) > max_size 9 | im_scale = double(max_size) / double(im_size_max); 10 | end 11 | end -------------------------------------------------------------------------------- /isam/.svn/pristine/75/751b7489c307a86e0ede563d7b15be83e06b5d25.svn-base: -------------------------------------------------------------------------------- 1 | package mrlcm; 2 | 3 | struct cov_collection_t 4 | { 5 | int32_t id; // ID of collection, needed for example for links 6 | string name; // name to display in viewer 7 | int16_t type; // what to draw, see constants below 8 | boolean reset; // start from scratch or keep previous data 9 | 10 | int32_t ncovs; 11 | cov_t covs[ncovs]; 12 | 13 | const int16_t ELLIPSOID=1; 14 | } 15 | -------------------------------------------------------------------------------- /isam/isam/lcmtypes/mrlcm_obj_collection_t.lcm: -------------------------------------------------------------------------------- 1 | package mrlcm; 2 | 3 | struct obj_collection_t 4 | { 5 | int32_t id; // ID of collection, needed for example for links 6 | string name; // name to display in viewer 7 | int16_t type; // what to draw, see constants below 8 | boolean reset; // start from scratch or keep previous data 9 | 10 | int32_t nobjs; 11 | obj_t objs[nobjs]; 12 | 13 | const int16_t POSE=1, TREE=2, SQUARE=3, POSE3D=4; 14 | } 15 | -------------------------------------------------------------------------------- /include/utils/im_list_to_blob.m: -------------------------------------------------------------------------------- 1 | function blob = im_list_to_blob(ims) 2 | max_shape = max(cell2mat(cellfun(@size, ims(:), 'UniformOutput', false)), [], 1); 3 | assert(all(cellfun(@(x) size(x, 3), ims, 'UniformOutput', true) == 3)); 4 | num_images = length(ims); 5 | blob = zeros(max_shape(1), max_shape(2), 3, num_images, 'single'); 6 | 7 | for i = 1:length(ims) 8 | im = ims{i}; 9 | blob(1:size(im, 1), 1:size(im, 2), :, i) = im; 10 | end 11 | end -------------------------------------------------------------------------------- /isam/.svn/pristine/d0/d07cdc14346979fb7a161421b984b001474cf422.svn-base: -------------------------------------------------------------------------------- 1 | cmake_minimum_required (VERSION 2.6) 2 | 3 | link_libraries(isamlib) 4 | 5 | # generate a target for each .cpp file in this directory 6 | file(GLOB misc RELATIVE "${PROJECT_SOURCE_DIR}/misc" "*.cpp") 7 | add_custom_target(misc) 8 | foreach(file ${misc}) 9 | string(REPLACE ".cpp" "" file ${file}) 10 | add_executable(${file} EXCLUDE_FROM_ALL ${file}.cpp) 11 | add_dependencies(misc ${file}) 12 | endforeach(file) 13 | -------------------------------------------------------------------------------- /isam/.svn/pristine/77/772380df70e416953a26103633700011754f35ec.svn-base: -------------------------------------------------------------------------------- 1 | package mrlcm; 2 | 3 | struct obj_collection_t 4 | { 5 | int32_t id; // ID of collection, needed for example for links 6 | string name; // name to display in viewer 7 | int16_t type; // what to draw, see constants below 8 | boolean reset; // start from scratch or keep previous data 9 | 10 | int32_t nobjs; 11 | obj_t objs[nobjs]; 12 | 13 | const int16_t POSE=1, TREE=2, SQUARE=3, POSE3D=4; 14 | } 15 | -------------------------------------------------------------------------------- /isam/.svn/pristine/06/066b3c8b7b2b31c30ef7823e42b6e45f1929159f.svn-base: -------------------------------------------------------------------------------- 1 | cmake_minimum_required (VERSION 2.6) 2 | 3 | link_libraries(isamlib) 4 | 5 | # generate a target for each .cpp file in this directory 6 | file(GLOB examples RELATIVE "${PROJECT_SOURCE_DIR}/examples" "*.cpp") 7 | add_custom_target(examples) 8 | foreach(example ${examples}) 9 | string(REPLACE ".cpp" "" example ${example}) 10 | add_executable(${example} EXCLUDE_FROM_ALL ${example}.cpp) 11 | add_dependencies(examples ${example}) 12 | endforeach(example) 13 | -------------------------------------------------------------------------------- /include/utils/symbolic_link.m: -------------------------------------------------------------------------------- 1 | function symbolic_link(link, target) 2 | % symbolic_link(link, target) 3 | % -------------------------------------------------------- 4 | % Faster R-CNN 5 | % Copyright (c) 2015, Shaoqing Ren 6 | % Licensed under The MIT License [see LICENSE for details] 7 | % -------------------------------------------------------- 8 | 9 | if ispc() 10 | system(sprintf('mklink /J %s %s', link, target)); 11 | else 12 | system(sprintf('ln -s %s %s', link, target)); 13 | end 14 | 15 | end 16 | -------------------------------------------------------------------------------- /include/utils/boxes_filter.m: -------------------------------------------------------------------------------- 1 | function aboxes = boxes_filter(aboxes, per_nms_topN, nms_overlap_thres, after_nms_topN, use_gpu) 2 | area = (aboxes(:,3)-aboxes(:,1)).*(aboxes(:,4)-aboxes(:,2)); 3 | aboxes(area>0.2*640*480,:)=[]; 4 | % to speed up nms 5 | if per_nms_topN > 0 6 | aboxes = aboxes(1:min(length(aboxes), per_nms_topN), :); 7 | end 8 | % do nms 9 | if nms_overlap_thres > 0 && nms_overlap_thres < 1 10 | aboxes = aboxes(nms(aboxes, nms_overlap_thres, use_gpu), :); 11 | end 12 | if after_nms_topN > 0 13 | aboxes = aboxes(1:min(length(aboxes), after_nms_topN), :); 14 | end 15 | end -------------------------------------------------------------------------------- /include/utils/parse_rst.m: -------------------------------------------------------------------------------- 1 | function results = parse_rst(results, rst) 2 | % results = parse_rst(results, rst) 3 | % -------------------------------------------------------- 4 | % Faster R-CNN 5 | % Copyright (c) 2015, Shaoqing Ren 6 | % Licensed under The MIT License [see LICENSE for details] 7 | % -------------------------------------------------------- 8 | 9 | if isempty(results) 10 | for i = 1:length(rst) 11 | results.(rst(i).blob_name).data = []; 12 | end 13 | end 14 | 15 | for i = 1:length(rst) 16 | results.(rst(i).blob_name).data = [results.(rst(i).blob_name).data; rst(i).data(:)]; 17 | end 18 | end -------------------------------------------------------------------------------- /include/utils/procid.m: -------------------------------------------------------------------------------- 1 | function s = procid() 2 | % Returns a string identifying the process. 3 | 4 | % AUTORIGHTS 5 | % ------------------------------------------------------- 6 | % Copyright (C) 2009-2012 Ross Girshick 7 | % 8 | % This file is part of the voc-releaseX code 9 | % (http://people.cs.uchicago.edu/~rbg/latent/) 10 | % and is available under the terms of an MIT-like license 11 | % provided in COPYING. Please retain this notice and 12 | % COPYING if you use this file (or a portion of it) in 13 | % your project. 14 | % ------------------------------------------------------- 15 | 16 | d = pwd(); 17 | i = strfind(d, filesep); 18 | d = d(i(end)+1:end); 19 | s = d; 20 | -------------------------------------------------------------------------------- /isam/isam/lcmtypes/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required (VERSION 2.6) 2 | 3 | find_library(LIB_LCM lcm) 4 | 5 | FILE(GLOB LCM_FILES "*.lcm") 6 | 7 | string(REPLACE ".lcm" ".c" LCM_SOURCES "${LCM_FILES}") 8 | string(REPLACE ".lcm" ".h" LCM_HEADERS "${LCM_FILES}") 9 | string(REPLACE ".lcm" "" LCM_TYPES "${LCM_FILES}") 10 | 11 | foreach (LCM_TYPE ${LCM_TYPES}) 12 | add_custom_command ( 13 | OUTPUT "${LCM_TYPE}.c" "${LCM_TYPE}.h" 14 | COMMAND lcm-gen "${LCM_TYPE}.lcm" --lazy -c 15 | DEPENDS "${LCM_TYPE}.lcm" 16 | WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} 17 | ) 18 | endforeach (LCM_TYPE) 19 | 20 | set_source_files_properties(${LCM_SOURCES} ${LCM_HEADERS} PROPERTIES GENERATED true) 21 | add_library (lcmtypes ${LCM_SOURCES} ${LCM_HEADERS}) 22 | target_link_libraries (lcmtypes ${LIB_LCM}) 23 | -------------------------------------------------------------------------------- /include/utils/tic_toc_print.m: -------------------------------------------------------------------------------- 1 | function tic_toc_print(fmt, varargin) 2 | % Print only after 1 second has passed since the last print. 3 | % Arguments are the same as for fprintf. 4 | 5 | % AUTORIGHTS 6 | % ------------------------------------------------------- 7 | % Copyright (C) 2009-2012 Ross Girshick 8 | % 9 | % This file is part of the voc-releaseX code 10 | % (http://people.cs.uchicago.edu/~rbg/latent/) 11 | % and is available under the terms of an MIT-like license 12 | % provided in COPYING. Please retain this notice and 13 | % COPYING if you use this file (or a portion of it) in 14 | % your project. 15 | % ------------------------------------------------------- 16 | 17 | persistent th; 18 | 19 | if isempty(th) 20 | th = tic(); 21 | end 22 | 23 | if toc(th) > 1 24 | fprintf(fmt, varargin{:}); 25 | drawnow; 26 | th = tic(); 27 | end 28 | -------------------------------------------------------------------------------- /isam/.svn/pristine/0c/0c7726bd76d729f03f30a5b1bcce1c0b7703aa14.svn-base: -------------------------------------------------------------------------------- 1 | cmake_minimum_required (VERSION 2.6) 2 | 3 | find_library(LIB_LCM lcm) 4 | 5 | FILE(GLOB LCM_FILES "*.lcm") 6 | 7 | string(REPLACE ".lcm" ".c" LCM_SOURCES "${LCM_FILES}") 8 | string(REPLACE ".lcm" ".h" LCM_HEADERS "${LCM_FILES}") 9 | string(REPLACE ".lcm" "" LCM_TYPES "${LCM_FILES}") 10 | 11 | foreach (LCM_TYPE ${LCM_TYPES}) 12 | add_custom_command ( 13 | OUTPUT "${LCM_TYPE}.c" "${LCM_TYPE}.h" 14 | COMMAND lcm-gen "${LCM_TYPE}.lcm" --lazy -c 15 | DEPENDS "${LCM_TYPE}.lcm" 16 | WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} 17 | ) 18 | endforeach (LCM_TYPE) 19 | 20 | set_source_files_properties(${LCM_SOURCES} ${LCM_HEADERS} PROPERTIES GENERATED true) 21 | add_library (lcmtypes ${LCM_SOURCES} ${LCM_HEADERS}) 22 | target_link_libraries (lcmtypes ${LIB_LCM}) 23 | -------------------------------------------------------------------------------- /include/fast_rcnn/fast_rcnn_map_im_rois_to_feat_rois.m: -------------------------------------------------------------------------------- 1 | function [feat_rois] = fast_rcnn_map_im_rois_to_feat_rois(conf, im_rois, im_scale_factor) 2 | % [feat_rois] = fast_rcnn_map_im_rois_to_feat_rois(conf, im_rois, im_scale_factor) 3 | % -------------------------------------------------------- 4 | % Fast R-CNN 5 | % Reimplementation based on Python Fast R-CNN (https://github.com/rbgirshick/fast-rcnn) 6 | % Copyright (c) 2015, Shaoqing Ren 7 | % Licensed under The MIT License [see LICENSE for details] 8 | % -------------------------------------------------------- 9 | 10 | %% Map a ROI in image-pixel coordinates to a ROI in feature coordinates. 11 | % in matlab's index (start from 1) 12 | 13 | feat_rois = round((im_rois-1) * im_scale_factor) + 1; 14 | 15 | %feat_rois = round((im_rois-1) * im_scale_factor / single(conf.feat_stride)) + 1; 16 | 17 | end -------------------------------------------------------------------------------- /isam/isamlib/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.6) 2 | 3 | # find all source and header files 4 | file(GLOB SRCS RELATIVE "${PROJECT_SOURCE_DIR}/isamlib" "*.cpp") 5 | 6 | # isamlib 7 | add_library(isamlib ${SRCS}) 8 | set_property(TARGET isamlib PROPERTY OUTPUT_NAME isam) 9 | 10 | # simple way of finding CHOLMOD 11 | find_package(Cholmod REQUIRED) 12 | include_directories(${CHOLMOD_INCLUDES}) 13 | target_link_libraries(isamlib ${CHOLMOD_LIBRARIES}) 14 | 15 | # install library 16 | install(TARGETS isamlib 17 | PERMISSIONS OWNER_READ OWNER_WRITE OWNER_EXECUTE GROUP_READ GROUP_EXECUTE WORLD_READ WORLD_EXECUTE 18 | LIBRARY DESTINATION lib 19 | ARCHIVE DESTINATION lib 20 | ) 21 | # install header files 22 | install(DIRECTORY ${PROJECT_SOURCE_DIR}/include/isam/ DESTINATION include/isam 23 | FILES_MATCHING PATTERN "*.h" PATTERN ".svn" EXCLUDE) 24 | 25 | -------------------------------------------------------------------------------- /isam/.svn/pristine/2b/2b642da8f576046edf8255bb4a6ba47660353d67.svn-base: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.6) 2 | 3 | # find all source and header files 4 | file(GLOB SRCS RELATIVE "${PROJECT_SOURCE_DIR}/isamlib" "*.cpp") 5 | 6 | # isamlib 7 | add_library(isamlib ${SRCS}) 8 | set_property(TARGET isamlib PROPERTY OUTPUT_NAME isam) 9 | 10 | # simple way of finding CHOLMOD 11 | find_package(Cholmod REQUIRED) 12 | include_directories(${CHOLMOD_INCLUDES}) 13 | target_link_libraries(isamlib ${CHOLMOD_LIBRARIES}) 14 | 15 | # install library 16 | install(TARGETS isamlib 17 | PERMISSIONS OWNER_READ OWNER_WRITE OWNER_EXECUTE GROUP_READ GROUP_EXECUTE WORLD_READ WORLD_EXECUTE 18 | LIBRARY DESTINATION lib 19 | ARCHIVE DESTINATION lib 20 | ) 21 | # install header files 22 | install(DIRECTORY ${PROJECT_SOURCE_DIR}/include/isam/ DESTINATION include/isam 23 | FILES_MATCHING PATTERN "*.h" PATTERN ".svn" EXCLUDE) 24 | 25 | -------------------------------------------------------------------------------- /include/utils/auto_select_gpu.m: -------------------------------------------------------------------------------- 1 | function gpu_id = auto_select_gpu() 2 | % gpu_id = auto_select_gpu() 3 | % Select the gpu which has the maximum free memory 4 | % -------------------------------------------------------- 5 | % Faster R-CNN 6 | % Copyright (c) 2015, Shaoqing Ren 7 | % Licensed under The MIT License [see LICENSE for details] 8 | % -------------------------------------------------------- 9 | 10 | % deselects all GPU devices 11 | gpuDevice([]); 12 | 13 | maxFreeMemory = 0; 14 | for i = 1:gpuDeviceCount 15 | g = gpuDevice(i); 16 | freeMemory = g.FreeMemory(); 17 | fprintf('GPU %d: free memory %d\n', i, freeMemory); 18 | if freeMemory > maxFreeMemory 19 | maxFreeMemory = freeMemory; 20 | gpu_id = i; 21 | end 22 | end 23 | fprintf('Use GPU %d\n', gpu_id); 24 | 25 | % deselects all GPU devices 26 | gpuDevice([]); 27 | end 28 | -------------------------------------------------------------------------------- /include/utils/seed_rand.m: -------------------------------------------------------------------------------- 1 | function prev_rng = seed_rand(seed) 2 | % seed_rand - Set random number generator to a fixed seed. 3 | % prev_rng = seed_rand(seed) 4 | % 5 | % Strategic use ensures that results are reproducible. 6 | % 7 | % To restore the previous rng after calling this do: 8 | % rng(prev_rng); 9 | 10 | % AUTORIGHTS 11 | % --------------------------------------------------------- 12 | % Copyright (c) 2014, Ross Girshick 13 | % 14 | % This file is part of the R-CNN code and is available 15 | % under the terms of the Simplified BSD License provided in 16 | % LICENSE. Please retain this notice and LICENSE if you use 17 | % this file (or any portion of it) in your project. 18 | % --------------------------------------------------------- 19 | 20 | if nargin < 1 21 | % This value works best for me. 22 | seed = 3; 23 | % Just kidding, of course ;-). 24 | end 25 | 26 | prev_rng = rng; 27 | rng(seed, 'twister') 28 | -------------------------------------------------------------------------------- /isam/isam/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required (VERSION 2.6) 2 | 3 | link_libraries(isamlib) 4 | 5 | # main iSAM executable (for processing files, timing, includes GUI, LCM interface) 6 | set (ISAM_SOURCES isam.cpp Loader.cpp) 7 | if(USE_LCM) 8 | set (ISAM_SOURCES ${ISAM_SOURCES} Lcm.cpp) 9 | add_subdirectory(lcmtypes) 10 | endif(USE_LCM) 11 | if(USE_GUI) 12 | set (ISAM_SOURCES ${ISAM_SOURCES} Collections.cpp Viewer.cpp) 13 | endif(USE_GUI) 14 | 15 | add_executable(isam ${ISAM_SOURCES}) 16 | if (PROFILE) 17 | set_target_properties(isam PROPERTIES LINK_FLAGS "-pg") 18 | endif (PROFILE) 19 | if(USE_LCM) 20 | target_link_libraries(isam lcm lcmtypes) 21 | endif(USE_LCM) 22 | if(USE_GUI) 23 | find_package(OpenGL REQUIRED) 24 | find_package(SDL REQUIRED) 25 | include_directories(${GL_INCLUDE} ${SDL_INCLUDE_DIR}) 26 | target_link_libraries(isam ${OPENGL_LIBRARY} ${SDL_LIBRARY}) 27 | endif(USE_GUI) 28 | 29 | install(TARGETS isam 30 | RUNTIME DESTINATION bin 31 | ) 32 | -------------------------------------------------------------------------------- /isam/.svn/pristine/58/584489f1bd61a1c3d6b88e8d5045d468d147dcc9.svn-base: -------------------------------------------------------------------------------- 1 | cmake_minimum_required (VERSION 2.6) 2 | 3 | link_libraries(isamlib) 4 | 5 | # main iSAM executable (for processing files, timing, includes GUI, LCM interface) 6 | set (ISAM_SOURCES isam.cpp Loader.cpp) 7 | if(USE_LCM) 8 | set (ISAM_SOURCES ${ISAM_SOURCES} Lcm.cpp) 9 | add_subdirectory(lcmtypes) 10 | endif(USE_LCM) 11 | if(USE_GUI) 12 | set (ISAM_SOURCES ${ISAM_SOURCES} Collections.cpp Viewer.cpp) 13 | endif(USE_GUI) 14 | 15 | add_executable(isam ${ISAM_SOURCES}) 16 | if (PROFILE) 17 | set_target_properties(isam PROPERTIES LINK_FLAGS "-pg") 18 | endif (PROFILE) 19 | if(USE_LCM) 20 | target_link_libraries(isam lcm lcmtypes) 21 | endif(USE_LCM) 22 | if(USE_GUI) 23 | find_package(OpenGL REQUIRED) 24 | find_package(SDL REQUIRED) 25 | include_directories(${GL_INCLUDE} ${SDL_INCLUDE_DIR}) 26 | target_link_libraries(isam ${OPENGL_LIBRARY} ${SDL_LIBRARY}) 27 | endif(USE_GUI) 28 | 29 | install(TARGETS isam 30 | RUNTIME DESTINATION bin 31 | ) 32 | -------------------------------------------------------------------------------- /include/utils/active_caffe_mex.m: -------------------------------------------------------------------------------- 1 | function active_caffe_mex(gpu_id, caffe_version) 2 | % active_caffe_mex(gpu_id, caffe_version) 3 | % -------------------------------------------------------- 4 | % Faster R-CNN 5 | % Copyright (c) 2015, Shaoqing Ren 6 | % Licensed under The MIT License [see LICENSE for details] 7 | % -------------------------------------------------------- 8 | 9 | % set gpu in matlab 10 | gpuDevice(gpu_id); 11 | 12 | if ~exist('caffe_version', 'var') || isempty(caffe_version) 13 | caffe_version = 'caffe'; 14 | end 15 | cur_dir = pwd; 16 | caffe_dir = fullfile(pwd, 'external', 'caffe', 'matlab', caffe_version); 17 | 18 | if ~exist(caffe_dir, 'dir') 19 | warning('Specified caffe folder (%s) is not exist, change to default one (%s)', ... 20 | caffe_dir, fullfile(pwd, 'external', 'caffe', 'matlab')); 21 | caffe_dir = fullfile(pwd, 'external', 'caffe', 'matlab'); 22 | end 23 | 24 | addpath(genpath(caffe_dir)); 25 | cd(caffe_dir); 26 | caffe.set_device(gpu_id-1); 27 | cd(cur_dir); 28 | end 29 | -------------------------------------------------------------------------------- /include/fast_rcnn/fast_rcnn_bbox_transform.m: -------------------------------------------------------------------------------- 1 | function [regression_label] = fast_rcnn_bbox_transform(ex_boxes, gt_boxes) 2 | % [regression_label] = fast_rcnn_bbox_transform(ex_boxes, gt_boxes) 3 | % -------------------------------------------------------- 4 | % Fast R-CNN 5 | % Reimplementation based on Python Fast R-CNN (https://github.com/rbgirshick/fast-rcnn) 6 | % Copyright (c) 2015, Shaoqing Ren 7 | % Licensed under The MIT License [see LICENSE for details] 8 | % -------------------------------------------------------- 9 | 10 | ex_widths = ex_boxes(:, 3) - ex_boxes(:, 1) + 1; 11 | ex_heights = ex_boxes(:, 4) - ex_boxes(:, 2) + 1; 12 | ex_ctr_x = ex_boxes(:, 1) + 0.5 * (ex_widths - 1); 13 | ex_ctr_y = ex_boxes(:, 2) + 0.5 * (ex_heights - 1); 14 | 15 | gt_widths = gt_boxes(:, 3) - gt_boxes(:, 1) + 1; 16 | gt_heights = gt_boxes(:, 4) - gt_boxes(:, 2) + 1; 17 | gt_ctr_x = gt_boxes(:, 1) + 0.5 * (gt_widths - 1); 18 | gt_ctr_y = gt_boxes(:, 2) + 0.5 * (gt_heights - 1); 19 | 20 | targets_dx = (gt_ctr_x - ex_ctr_x) ./ (ex_widths+eps); 21 | targets_dy = (gt_ctr_y - ex_ctr_y) ./ (ex_heights+eps); 22 | targets_dw = log(gt_widths ./ ex_widths); 23 | targets_dh = log(gt_heights ./ ex_heights); 24 | 25 | regression_label = [targets_dx, targets_dy, targets_dw, targets_dh]; 26 | end -------------------------------------------------------------------------------- /isam/Makefile: -------------------------------------------------------------------------------- 1 | # iSAM Makefile 2 | # providing shortcuts to cmake for building outside the source tree 3 | # Michael Kaess, 2010 4 | 5 | # default parameters, including multi-core building 6 | make = make -j 4 -C build --no-print-directory 7 | 8 | # creates isam libraries and isam binary 9 | all: build build/Makefile 10 | @$(make) 11 | 12 | # generate documentation (requires doxygen and graphviz) 13 | .PHONY: doc 14 | doc: 15 | @doxygen doc/doxygen/isam.dox 16 | 17 | # remove all generated files and directories 18 | .PHONY: distclean 19 | distclean: 20 | @rm -rf build doc/html lib bin release 21 | @find . -name CMakeFiles |xargs rm -rf # clean up in case "cmake ." was called 22 | @find . -name cmake_install.cmake -delete 23 | @find . -name CMakeCache.txt -delete 24 | 25 | # internal target: the actual build directory 26 | build: 27 | @mkdir -p build bin lib include 28 | 29 | # internal target: populate the build directory 30 | build/Makefile: 31 | cd build && cmake .. 32 | 33 | 34 | # create all executables in the examples/ directory 35 | .PHONY: examples 36 | examples: 37 | @$(make) examples 38 | 39 | # create all executables in the misc/ directory 40 | .PHONY: misc 41 | misc: 42 | @$(make) misc 43 | 44 | # default target: any target such as "clean", "example"... 45 | # is simply passed on to the cmake-generated Makefile 46 | %:: 47 | @$(make) $@ 48 | -------------------------------------------------------------------------------- /isam/.svn/pristine/8f/8ff71bfd56ab26597d965fcc29995e1aac48d8a4.svn-base: -------------------------------------------------------------------------------- 1 | # iSAM Makefile 2 | # providing shortcuts to cmake for building outside the source tree 3 | # Michael Kaess, 2010 4 | 5 | # default parameters, including multi-core building 6 | make = make -j 4 -C build --no-print-directory 7 | 8 | # creates isam libraries and isam binary 9 | all: build build/Makefile 10 | @$(make) 11 | 12 | # generate documentation (requires doxygen and graphviz) 13 | .PHONY: doc 14 | doc: 15 | @doxygen doc/doxygen/isam.dox 16 | 17 | # remove all generated files and directories 18 | .PHONY: distclean 19 | distclean: 20 | @rm -rf build doc/html lib bin release 21 | @find . -name CMakeFiles |xargs rm -rf # clean up in case "cmake ." was called 22 | @find . -name cmake_install.cmake -delete 23 | @find . -name CMakeCache.txt -delete 24 | 25 | # internal target: the actual build directory 26 | build: 27 | @mkdir -p build bin lib include 28 | 29 | # internal target: populate the build directory 30 | build/Makefile: 31 | cd build && cmake .. 32 | 33 | 34 | # create all executables in the examples/ directory 35 | .PHONY: examples 36 | examples: 37 | @$(make) examples 38 | 39 | # create all executables in the misc/ directory 40 | .PHONY: misc 41 | misc: 42 | @$(make) misc 43 | 44 | # default target: any target such as "clean", "example"... 45 | # is simply passed on to the cmake-generated Makefile 46 | %:: 47 | @$(make) $@ 48 | -------------------------------------------------------------------------------- /include/utils/prep_im_for_blob.m: -------------------------------------------------------------------------------- 1 | function [im, im_scale] = prep_im_for_blob(im, im_means, target_size, max_size) 2 | im = single(im); 3 | 4 | if ~isa(im, 'gpuArray') 5 | try 6 | im = bsxfun(@minus, im, im_means); 7 | catch 8 | im_means = imresize(im_means, [size(im, 1), size(im, 2)], 'bilinear', 'antialiasing', false); 9 | im = bsxfun(@minus, im, im_means); 10 | end 11 | im_scale = prep_im_for_blob_size(size(im), target_size, max_size); 12 | 13 | target_size = round([size(im, 1), size(im, 2)] * im_scale); 14 | im = imresize(im, target_size, 'bilinear', 'antialiasing', false); 15 | else 16 | % for im as gpuArray 17 | try 18 | im = bsxfun(@minus, im, im_means); 19 | catch 20 | im_means_scale = max(double(size(im, 1)) / size(im_means, 1), double(size(im, 2)) / size(im_means, 2)); 21 | im_means = imresize(im_means, im_means_scale); 22 | y_start = floor((size(im_means, 1) - size(im, 1)) / 2) + 1; 23 | x_start = floor((size(im_means, 2) - size(im, 2)) / 2) + 1; 24 | im_means = im_means(y_start:(y_start+size(im, 1)-1), x_start:(x_start+size(im, 2)-1)); 25 | im = bsxfun(@minus, im, im_means); 26 | end 27 | 28 | im_scale = prep_im_for_blob_size(size(im), target_size, max_size); 29 | im = imresize(im, im_scale); 30 | end 31 | end -------------------------------------------------------------------------------- /include/utils/boxoverlap.m: -------------------------------------------------------------------------------- 1 | function o = boxoverlap(a, b) 2 | % Compute the symmetric intersection over union overlap between a set of 3 | % bounding boxes in a and a single bounding box in b. 4 | % 5 | % a a matrix where each row specifies a bounding box 6 | % b a matrix where each row specifies a bounding box 7 | 8 | % AUTORIGHTS 9 | % ------------------------------------------------------- 10 | % Copyright (C) 2011-2012 Ross Girshick 11 | % Copyright (C) 2008, 2009, 2010 Pedro Felzenszwalb, Ross Girshick 12 | % 13 | % This file is part of the voc-releaseX code 14 | % (http://people.cs.uchicago.edu/~rbg/latent/) 15 | % and is available under the terms of an MIT-like license 16 | % provided in COPYING. Please retain this notice and 17 | % COPYING if you use this file (or a portion of it) in 18 | % your project. 19 | % ------------------------------------------------------- 20 | 21 | o = cell(1, size(b, 1)); 22 | for i = 1:size(b, 1) 23 | x1 = max(a(:,1), b(i,1)); 24 | y1 = max(a(:,2), b(i,2)); 25 | x2 = min(a(:,3), b(i,3)); 26 | y2 = min(a(:,4), b(i,4)); 27 | 28 | w = x2-x1+1; 29 | h = y2-y1+1; 30 | inter = w.*h; 31 | aarea = (a(:,3)-a(:,1)+1) .* (a(:,4)-a(:,2)+1); 32 | barea = (b(i,3)-b(i,1)+1) * (b(i,4)-b(i,2)+1); 33 | % intersection over union overlap 34 | o{i} = inter ./ (aarea+barea-inter); 35 | % set invalid entries to 0 overlap 36 | o{i}(w <= 0) = 0; 37 | o{i}(h <= 0) = 0; 38 | end 39 | 40 | o = cell2mat(o); 41 | -------------------------------------------------------------------------------- /include/rpn/proposal_calc_output_size.m: -------------------------------------------------------------------------------- 1 | function [output_width_map, output_height_map] = proposal_calc_output_size(conf, test_net_def_file) 2 | % [output_width_map, output_height_map] = proposal_calc_output_size(conf, test_net_def_file) 3 | % -------------------------------------------------------- 4 | % Faster R-CNN 5 | % Copyright (c) 2015, Shaoqing Ren 6 | % Licensed under The MIT License [see LICENSE for details] 7 | % -------------------------------------------------------- 8 | 9 | % caffe.init_log(fullfile(pwd, 'caffe_log')); 10 | caffe_net = caffe.Net(test_net_def_file, 'test'); 11 | 12 | % set gpu/cpu 13 | if conf.use_gpu 14 | caffe.set_mode_gpu(); 15 | else 16 | caffe.set_mode_cpu(); 17 | end 18 | 19 | input = 100:conf.max_size; 20 | output_w = nan(size(input)); 21 | output_h = nan(size(input)); 22 | for i = 1:length(input) 23 | s = input(i); 24 | im_blob = single(zeros(s, s, 3, 1)); 25 | net_inputs = {im_blob}; 26 | 27 | % Reshape net's input blobs 28 | caffe_net.reshape_as_input(net_inputs); 29 | caffe_net.forward(net_inputs); 30 | 31 | cls_score = caffe_net.blobs('proposal_cls_score').get_data(); 32 | output_w(i) = size(cls_score, 1); 33 | output_h(i) = size(cls_score, 2); 34 | end 35 | 36 | output_width_map = containers.Map(input, output_w); 37 | output_height_map = containers.Map(input, output_h); 38 | 39 | caffe.reset_all(); 40 | end -------------------------------------------------------------------------------- /include/fast_rcnn/fast_rcnn_bbox_transform_inv.m: -------------------------------------------------------------------------------- 1 | function [pred_boxes] = fast_rcnn_bbox_transform_inv(boxes, box_deltas) 2 | % [pred_boxes] = fast_rcnn_bbox_transform_inv(boxes, box_deltas) 3 | % -------------------------------------------------------- 4 | % Fast R-CNN 5 | % Reimplementation based on Python Fast R-CNN (https://github.com/rbgirshick/fast-rcnn) 6 | % Copyright (c) 2015, Shaoqing Ren 7 | % Licensed under The MIT License [see LICENSE for details] 8 | % -------------------------------------------------------- 9 | 10 | src_w = double(boxes(:, 3) - boxes(:, 1) + 1); 11 | src_h = double(boxes(:, 4) - boxes(:, 2) + 1); 12 | src_ctr_x = double(boxes(:, 1) + 0.5*(src_w-1)); 13 | src_ctr_y = double(boxes(:, 2) + 0.5*(src_h-1)); 14 | 15 | dst_ctr_x = double(box_deltas(:, 1:4:end)); 16 | dst_ctr_y = double(box_deltas(:, 2:4:end)); 17 | dst_scl_x = double(box_deltas(:, 3:4:end)); 18 | dst_scl_y = double(box_deltas(:, 4:4:end)); 19 | 20 | pred_ctr_x = bsxfun(@plus, bsxfun(@times, dst_ctr_x, src_w), src_ctr_x); 21 | pred_ctr_y = bsxfun(@plus, bsxfun(@times, dst_ctr_y, src_h), src_ctr_y); 22 | pred_w = bsxfun(@times, exp(dst_scl_x), src_w); 23 | pred_h = bsxfun(@times, exp(dst_scl_y), src_h); 24 | pred_boxes = zeros(size(box_deltas), 'single'); 25 | pred_boxes(:, 1:4:end) = pred_ctr_x - 0.5*(pred_w-1); 26 | pred_boxes(:, 2:4:end) = pred_ctr_y - 0.5*(pred_h-1); 27 | pred_boxes(:, 3:4:end) = pred_ctr_x + 0.5*(pred_w-1); 28 | pred_boxes(:, 4:4:end) = pred_ctr_y + 0.5*(pred_h-1); 29 | end -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # object_slam 2 | This is the code for "SLAM with objects using a nonparametric pose graph". See bib reference below: 3 | 4 | @INPROCEEDINGS{Mu_iros_2016, 5 | author={B. Mu and S. Y. Liu and L. Paull and J. Leonard and J. P. How}, 6 | booktitle={2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, 7 | title={SLAM with objects using a nonparametric pose graph}, 8 | year={2016}, 9 | pages={4602-4609}, 10 | keywords={SLAM (robots);graph theory;object detection;pose estimation;robot vision;sensor fusion;SLAM;data association;nonparametric pose graph;object detection;robotic application;simultaneous localization and mapping;Machine learning;Object detection;Proposals;Robustness;Simultaneous localization and mapping;Three-dimensional displays}, 11 | doi={10.1109/IROS.2016.7759677}, 12 | month={Oct},} 13 | 14 | ## iSAM library 15 | Folder isam contains the modified isam library to optimize pose graphs. There are pre-compiled executable file isam is under the bin folder 16 | To compile from source, following the commands on ubuntu: 17 | cd isam 18 | mkdir build && cd build && cmake .. 19 | make 20 | Fore more details about the library, refer to readme file under isam folder. 21 | 22 | ## Simulation 23 | To generate simulated dataset, run generateSimData.m. Ground truth objects are randomly generated. Click in the figure to generate the ground truth trajectory, make sure there are enough loop closures. When finished, press enter button on the keyboard. 24 | 25 | To run the algorithm and compared algorithms, run main_simulation.m 26 | -------------------------------------------------------------------------------- /isam/isamlib/Node.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Node.cpp 3 | * @brief A single variable node 4 | * @author Michael Kaess 5 | * @author Hordur Johannsson 6 | * @version $Id: $ 7 | * 8 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 9 | * Michael Kaess, Hordur Johannsson, David Rosen, 10 | * Nicholas Carlevaris-Bianco and John. J. Leonard 11 | * 12 | * This file is part of iSAM. 13 | * 14 | * iSAM is free software; you can redistribute it and/or modify it under 15 | * the terms of the GNU Lesser General Public License as published by the 16 | * Free Software Foundation; either version 2.1 of the License, or (at 17 | * your option) any later version. 18 | * 19 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 20 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 21 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 22 | * License for more details. 23 | * 24 | * You should have received a copy of the GNU Lesser General Public License 25 | * along with iSAM. If not, see . 26 | * 27 | */ 28 | 29 | #include "isam/Node.h" 30 | #include "isam/Factor.h" 31 | 32 | namespace isam 33 | { 34 | void Node::mark_deleted() { 35 | _deleted = true; 36 | for (std::list::iterator factor = _factors.begin(); factor != _factors.end(); ++factor) 37 | (*factor)->mark_deleted(); 38 | } 39 | void Node::erase_marked_factors() { 40 | for (std::list::iterator factor = _factors.begin(); factor != _factors.end();) 41 | if ((*factor)->deleted()) factor = _factors.erase(factor); 42 | else ++factor; 43 | } 44 | } // namespace - isam 45 | -------------------------------------------------------------------------------- /include/utils/subsample_images.m: -------------------------------------------------------------------------------- 1 | function [imdbs, roidbs] = subsample_images(imdbs, roidbs, max_num_neg_images, seed) 2 | 3 | if ~exist('seed', 'var') 4 | seed = 6; 5 | end 6 | 7 | % class_num = cellfun(@(x) length(x.class_ids), imdbs, 'UniformOutput', true); 8 | % assert(length(unique(class_num)) == 1); 9 | % class_num = unique(class_num); 10 | 11 | rois = cellfun(@(x) x.rois(:), roidbs, 'UniformOutput', false); 12 | rois_combine = cell2mat(rois(:)); 13 | 14 | % fix the random seed for repeatability 15 | prev_rng = seed_rand(seed); 16 | inds = randperm(length(rois_combine), max_num_neg_images); 17 | inds = sort(inds); 18 | 19 | img_idx_start = 1; 20 | for i = 1:length(imdbs) 21 | imdb_img_num = length(imdbs{i}.image_ids); 22 | img_idx_end = img_idx_start + imdb_img_num - 1; 23 | inds_start = find(inds >= img_idx_start, 1, 'first'); 24 | inds_end = find(inds <= img_idx_end, 1, 'last'); 25 | 26 | inds_sub = inds(inds_start:inds_end); 27 | inds_sub = inds_sub - img_idx_start + 1; 28 | 29 | imdbs{i}.image_ids = imdbs{i}.image_ids(inds_sub); 30 | imdbs{i}.sizes = imdbs{i}.sizes(inds_sub, :); 31 | if isfield(imdbs{i}, 'image_dir') 32 | imdbs{i}.image_at = @(x) ... 33 | sprintf('%s/%s.%s', imdbs{i}.image_dir, imdbs{i}.image_ids{x}, imdbs{i}.extension); 34 | else 35 | imdbs{i}.image_at = @(x) ... 36 | sprintf('%s/%s.%s', imdbs{i}.imagedir, imdbs{i}.image_ids{x}, imdbs{i}.extension); 37 | end 38 | roidbs{i}.rois = roidbs{i}.rois(inds_sub); 39 | 40 | img_idx_start = img_idx_start + imdb_img_num; 41 | end 42 | 43 | % restore previous rng 44 | rng(prev_rng); 45 | 46 | end -------------------------------------------------------------------------------- /include/utils/showboxes.m: -------------------------------------------------------------------------------- 1 | function im = showboxes(im, boxes, legends) 2 | % Draw bounding boxes on top of an image. 3 | % showboxes(im, boxes) 4 | % 5 | % ------------------------------------------------------- 6 | 7 | % fix_width = 800; 8 | if isa(im, 'gpuArray') 9 | im = gather(im); 10 | end 11 | % imsz = size(im); 12 | % scale = fix_width / imsz(2); 13 | % im = imresize(im, scale); 14 | 15 | % axis image; 16 | % axis off; 17 | % set(gcf, 'Color', 'white'); 18 | 19 | valid_boxes_num = size(boxes,1); 20 | if valid_boxes_num > 0 21 | % boxes(:,1:4) = boxes(:, 1:4) * scale; 22 | 23 | colors_candidate = colormap('hsv'); 24 | colors_candidate = colors_candidate(1:(floor(size(colors_candidate, 1)/20)):end, :); 25 | colors_candidate = mat2cell(colors_candidate, ones(size(colors_candidate, 1), 1))'; 26 | colors = colors_candidate; 27 | 28 | for i = 1:valid_boxes_num 29 | box = boxes(i, 1:4); 30 | im = insertShape(im, 'Rectangle',RectLTRB2LTWH(box), 'LineWidth', 4,'Color',colors{boxes(i,6)}); 31 | im = insertText(im,[double(box(1))+2, double(box(2))],[legends{boxes(i,6)} num2str(boxes(i,5))]); 32 | %rectangle('Position', RectLTRB2LTWH(box), 'LineWidth', linewidth, 'EdgeColor', colors{boxes(i,6)}); 33 | %label = sprintf('%s(%d)', legends{boxes(i,6)}, i); 34 | %text(double(box(1))+2, double(box(2)), label, 'BackgroundColor', 'w'); 35 | end 36 | end 37 | end 38 | 39 | function [ rectsLTWH ] = RectLTRB2LTWH( rectsLTRB ) 40 | %rects (l, t, r, b) to (l, t, w, h) 41 | 42 | rectsLTWH = [rectsLTRB(:, 1), rectsLTRB(:, 2), rectsLTRB(:, 3)-rectsLTRB(:,1)+1, rectsLTRB(:,4)-rectsLTRB(2)+1]; 43 | end 44 | 45 | -------------------------------------------------------------------------------- /isam/.svn/pristine/ca/ca7bb9539ed025f1c4669c092c4eb252c2c8b130.svn-base: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Node.cpp 3 | * @brief A single variable node 4 | * @author Michael Kaess 5 | * @author Hordur Johannsson 6 | * @version $Id: $ 7 | * 8 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 9 | * Michael Kaess, Hordur Johannsson, David Rosen, 10 | * Nicholas Carlevaris-Bianco and John. J. Leonard 11 | * 12 | * This file is part of iSAM. 13 | * 14 | * iSAM is free software; you can redistribute it and/or modify it under 15 | * the terms of the GNU Lesser General Public License as published by the 16 | * Free Software Foundation; either version 2.1 of the License, or (at 17 | * your option) any later version. 18 | * 19 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 20 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 21 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 22 | * License for more details. 23 | * 24 | * You should have received a copy of the GNU Lesser General Public License 25 | * along with iSAM. If not, see . 26 | * 27 | */ 28 | 29 | #include "isam/Node.h" 30 | #include "isam/Factor.h" 31 | 32 | namespace isam 33 | { 34 | void Node::mark_deleted() { 35 | _deleted = true; 36 | for (std::list::iterator factor = _factors.begin(); factor != _factors.end(); ++factor) 37 | (*factor)->mark_deleted(); 38 | } 39 | void Node::erase_marked_factors() { 40 | for (std::list::iterator factor = _factors.begin(); factor != _factors.end();) 41 | if ((*factor)->deleted()) factor = _factors.erase(factor); 42 | else ++factor; 43 | } 44 | } // namespace - isam 45 | -------------------------------------------------------------------------------- /include/nms/nms_multiclass.m: -------------------------------------------------------------------------------- 1 | function picks = nms_multiclass(boxes, overlap) 2 | % top = nms(boxes, overlap) 3 | % Non-maximum suppression. (FAST VERSION) 4 | % Greedily select high-scoring detections and skip detections 5 | % that are significantly covered by a previously selected 6 | % detection. 7 | % 8 | % NOTE: This is adapted from Pedro Felzenszwalb's version (nms.m), 9 | % but an inner loop has been eliminated to significantly speed it 10 | % up in the case of a large number of boxes 11 | 12 | % Copyright (C) 2011-12 by Tomasz Malisiewicz 13 | % All rights reserved. 14 | % 15 | % This file is part of the Exemplar-SVM library and is made 16 | % available under the terms of the MIT license (see COPYING file). 17 | % Project homepage: https://github.com/quantombone/exemplarsvm 18 | 19 | 20 | if isempty(boxes) 21 | picks = {}; 22 | return; 23 | end 24 | 25 | if size(boxes, 1) < 10000 26 | picks = nms_multiclass_mex(double(boxes), double(overlap)); 27 | return; 28 | end 29 | 30 | x1 = boxes(:,1); 31 | y1 = boxes(:,2); 32 | x2 = boxes(:,3); 33 | y2 = boxes(:,4); 34 | 35 | area = (x2-x1+1) .* (y2-y1+1); 36 | 37 | picks = cell(size(boxes, 2)-4, 1); 38 | for iS = 5:size(boxes, 2) 39 | s = boxes(:,iS); 40 | [~, I] = sort(s); 41 | 42 | pick = s*0; 43 | counter = 1; 44 | while ~isempty(I) 45 | last = length(I); 46 | i = I(last); 47 | pick(counter) = i; 48 | counter = counter + 1; 49 | 50 | xx1 = max(x1(i), x1(I(1:last-1))); 51 | yy1 = max(y1(i), y1(I(1:last-1))); 52 | xx2 = min(x2(i), x2(I(1:last-1))); 53 | yy2 = min(y2(i), y2(I(1:last-1))); 54 | 55 | w = max(0.0, xx2-xx1+1); 56 | h = max(0.0, yy2-yy1+1); 57 | 58 | inter = w.*h; 59 | o = inter ./ (area(i) + area(I(1:last-1)) - inter); 60 | 61 | I = I(o<=overlap); 62 | end 63 | 64 | pick = pick(1:(counter-1)); 65 | picks{iS-4} = pick; 66 | end 67 | -------------------------------------------------------------------------------- /include/fast_rcnn/fast_rcnn_generate_sliding_windows.m: -------------------------------------------------------------------------------- 1 | function roidb = fast_rcnn_generate_sliding_windows(conf, imdb, roidb, roipool_in_size) 2 | % [pred_boxes, scores] = fast_rcnn_conv_feat_detect(conf, im, conv_feat, boxes, max_rois_num_in_gpu, net_idx) 3 | % -------------------------------------------------------- 4 | % Fast R-CNN 5 | % Reimplementation based on Python Fast R-CNN (https://github.com/rbgirshick/fast-rcnn) 6 | % Copyright (c) 2015, Shaoqing Ren 7 | % Licensed under The MIT License [see LICENSE for details] 8 | % -------------------------------------------------------- 9 | 10 | regions.images = imdb.image_ids; 11 | 12 | im_sizes = imdb.sizes; 13 | regions.boxes = cellfun(@(x) generate_sliding_windows_one_image(conf, x, roipool_in_size), num2cell(im_sizes, 2), 'UniformOutput', false); 14 | 15 | roidb = roidb_from_proposal(imdb, roidb, regions); 16 | end 17 | 18 | function boxes = generate_sliding_windows_one_image(conf, im_size, roipool_in_size) 19 | im_scale = prep_im_for_blob_size(im_size, conf.scales, conf.max_size); 20 | im_size = round(im_size * im_scale); 21 | 22 | x1 = 1:conf.feat_stride:im_size(2); 23 | y1 = 1:conf.feat_stride:im_size(1); 24 | [x1, y1] = meshgrid(x1, y1); 25 | x1 = x1(:); 26 | y1 = y1(:); 27 | x2 = x1 + roipool_in_size * conf.feat_stride - 1; 28 | y2 = y1 + roipool_in_size * conf.feat_stride - 1; 29 | 30 | boxes = [x1, y1, x2, y2]; 31 | boxes = filter_boxes(im_size, boxes); 32 | 33 | boxes = bsxfun(@times, boxes-1, 1/im_scale) + 1; 34 | end 35 | 36 | function boxes = filter_boxes(im_size, boxes) 37 | valid_ind = boxes(:, 1) >= 1 & boxes(:, 1) <= im_size(2) & ... 38 | boxes(:, 2) >= 1 & boxes(:, 2) <= im_size(1) & ... 39 | boxes(:, 3) >= 1 & boxes(:, 3) <= im_size(2) & ... 40 | boxes(:, 4) >= 1 & boxes(:, 4) <= im_size(1); 41 | 42 | boxes = boxes(valid_ind, :); 43 | end -------------------------------------------------------------------------------- /isam/include/isam/numericalDiff.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file numericalDiff.h 3 | * @brief Numerical differentiation. 4 | * @author Michael Kaess 5 | * @version $Id: numericalDiff.h 4038 2011-02-26 04:31:00Z kaess $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | #include 32 | 33 | #include "isam/Node.h" 34 | 35 | namespace isam { 36 | 37 | // Abstract class to enforce interface for function object. 38 | class Function { 39 | public: 40 | virtual ~Function() {} 41 | virtual int num_measurements() const = 0; 42 | virtual Eigen::VectorXd evaluate() const = 0; 43 | virtual std::vector& nodes() = 0; 44 | }; 45 | 46 | /** 47 | * Takes a general vector valued function and returns the 48 | * Jacobian at the linearization point given by x0. 49 | * @param func Function object with evaluation function that takes and returns vectors. 50 | * @return Matrix containing the Jacobian of func, with 51 | * dim(y) rows and dim(x) columns, where y=func(x). 52 | */ 53 | Eigen::MatrixXd numericalDiff(Function& func); 54 | 55 | } 56 | -------------------------------------------------------------------------------- /isam/include/isam/Element.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Element.h 3 | * @brief Basic functionality for nodes and factors of a graph. 4 | * @author Michael Kaess 5 | * @version $Id: Element.h 5796 2011-12-07 00:39:30Z kaess $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | #include 32 | 33 | namespace isam { 34 | 35 | class Element { 36 | Element(const Element& rhs); // not allowed 37 | const Element& operator= (const Element& rhs); // not allowed 38 | 39 | const char* _name; 40 | 41 | int _start; // needed for Slam::jacobian 42 | 43 | protected: 44 | int _id; 45 | int _dim; 46 | 47 | public: 48 | Element(const char* name, int dim) : _name(name), _dim(dim) {} 49 | virtual ~Element() {}; 50 | 51 | virtual int unique_id() {return _id;} 52 | virtual const char* name() const {return _name;} 53 | inline int dim() const {return _dim;} 54 | inline int start() const {return _start;} 55 | 56 | virtual void write(std::ostream &out) const { 57 | out << name(); 58 | } 59 | 60 | friend class Slam; 61 | friend class Covariances; 62 | }; 63 | 64 | } 65 | -------------------------------------------------------------------------------- /isam/.svn/pristine/82/82cdecd818c4200c28ac6ff55a870eadb4a006ec.svn-base: -------------------------------------------------------------------------------- 1 | /** 2 | * @file numericalDiff.h 3 | * @brief Numerical differentiation. 4 | * @author Michael Kaess 5 | * @version $Id: numericalDiff.h 4038 2011-02-26 04:31:00Z kaess $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | #include 32 | 33 | #include "isam/Node.h" 34 | 35 | namespace isam { 36 | 37 | // Abstract class to enforce interface for function object. 38 | class Function { 39 | public: 40 | virtual ~Function() {} 41 | virtual int num_measurements() const = 0; 42 | virtual Eigen::VectorXd evaluate() const = 0; 43 | virtual std::vector& nodes() = 0; 44 | }; 45 | 46 | /** 47 | * Takes a general vector valued function and returns the 48 | * Jacobian at the linearization point given by x0. 49 | * @param func Function object with evaluation function that takes and returns vectors. 50 | * @return Matrix containing the Jacobian of func, with 51 | * dim(y) rows and dim(x) columns, where y=func(x). 52 | */ 53 | Eigen::MatrixXd numericalDiff(Function& func); 54 | 55 | } 56 | -------------------------------------------------------------------------------- /isam/include/isam/Noise.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Noise.h 3 | * @brief Various noise models. 4 | * @author Michael Kaess 5 | * @version $Id: Noise.h 5797 2011-12-07 03:50:41Z kaess $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | #include 32 | 33 | namespace isam { 34 | 35 | // general noise model class 36 | class Noise { 37 | public: 38 | Eigen::MatrixXd _sqrtinf; 39 | const Eigen::MatrixXd& sqrtinf() const {return _sqrtinf;} 40 | }; 41 | 42 | // noise model based on square root information matrix 43 | class SqrtInformation : public Noise { 44 | public: 45 | SqrtInformation(const Eigen::MatrixXd& sqrtinf) {_sqrtinf = sqrtinf;} 46 | }; 47 | 48 | // noise model based on information matrix 49 | class Information : public Noise { 50 | public: 51 | Information(const Eigen::MatrixXd& inf) { 52 | _sqrtinf = inf.llt().matrixU(); 53 | } 54 | }; 55 | 56 | // noise model based on covariance matrix 57 | class Covariance : public Noise { 58 | public: 59 | Covariance(const Eigen::MatrixXd& cov) { 60 | _sqrtinf = cov.inverse().llt().matrixU(); 61 | } 62 | }; 63 | 64 | } 65 | -------------------------------------------------------------------------------- /include/nms/nvmex.m: -------------------------------------------------------------------------------- 1 | function nvmex(cuFileName, outDir) 2 | %NVMEX Compiles and links a CUDA file for MATLAB usage 3 | % NVMEX(FILENAME) will create a MEX-File (also with the name FILENAME) by 4 | % invoking the CUDA compiler, nvcc, and then linking with the MEX 5 | % function in MATLAB. 6 | 7 | if ispc % Windows 8 | Host_Compiler_Location = '-ccbin "C:\Program Files (x86)\Microsoft Visual Studio 12.0\VC\bin\x86_amd64"'; 9 | CUDA_INC_Location = ['"' getenv('CUDA_PATH') '\include"']; 10 | CUDA_SAMPLES_Location =['"' getenv('NVCUDASAMPLES6_5_ROOT') '\common\inc"']; 11 | PIC_Option = ''; 12 | if ( strcmp(computer('arch'),'win32') ==1) 13 | machine_str = ' --machine 32 '; 14 | CUDA_LIB_Location = ['"' getenv('CUDA_PATH') '\lib\Win32"']; 15 | elseif ( strcmp(computer('arch'),'win64') ==1) 16 | machine_str = ' --machine 64 '; 17 | CUDA_LIB_Location = ['"' getenv('CUDA_PATH') '\lib\x64"']; 18 | end 19 | NVCC = 'nvcc'; 20 | else % Mac and Linux (assuming gcc is on the path) 21 | CUDA_INC_Location = '/usr/local/cuda/include'; 22 | CUDA_SAMPLES_Location = '/usr/local/cuda/samples/common/inc'; 23 | Host_Compiler_Location = ' '; 24 | PIC_Option = ' --compiler-options -fPIC '; 25 | machine_str = []; 26 | CUDA_LIB_Location = '/usr/local/cuda/lib64'; 27 | NVCC = '/usr/local/cuda/bin/nvcc'; 28 | end 29 | % !!! End of things to modify !!! 30 | [~, filename] = fileparts(cuFileName); 31 | nvccCommandLine = [ ... 32 | NVCC ' --compile ' Host_Compiler_Location ' ' ... 33 | '-o ' filename '.o ' ... 34 | machine_str PIC_Option ... 35 | ' -I' '"' matlabroot '/extern/include "' ... 36 | ' -I' CUDA_INC_Location ' -I' CUDA_SAMPLES_Location ... 37 | ' "' cuFileName '" ' 38 | ]; 39 | mexCommandLine = ['mex ' '-outdir ' outDir ' ' filename '.o' ' -L' CUDA_LIB_Location ' -lcudart']; 40 | disp(nvccCommandLine); 41 | warning off; 42 | status = system(nvccCommandLine); 43 | warning on; 44 | if status < 0 45 | error 'Error invoking nvcc'; 46 | end 47 | disp(mexCommandLine); 48 | eval(mexCommandLine); 49 | end 50 | -------------------------------------------------------------------------------- /isam/.svn/pristine/33/337b8c33fa51e232924be45987dbcd530c1b86bc.svn-base: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Element.h 3 | * @brief Basic functionality for nodes and factors of a graph. 4 | * @author Michael Kaess 5 | * @version $Id: Element.h 5796 2011-12-07 00:39:30Z kaess $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | #include 32 | 33 | namespace isam { 34 | 35 | class Element { 36 | Element(const Element& rhs); // not allowed 37 | const Element& operator= (const Element& rhs); // not allowed 38 | 39 | const char* _name; 40 | 41 | int _start; // needed for Slam::jacobian 42 | 43 | protected: 44 | int _id; 45 | int _dim; 46 | 47 | public: 48 | Element(const char* name, int dim) : _name(name), _dim(dim) {} 49 | virtual ~Element() {}; 50 | 51 | virtual int unique_id() {return _id;} 52 | virtual const char* name() const {return _name;} 53 | inline int dim() const {return _dim;} 54 | inline int start() const {return _start;} 55 | 56 | virtual void write(std::ostream &out) const { 57 | out << name(); 58 | } 59 | 60 | friend class Slam; 61 | friend class Covariances; 62 | }; 63 | 64 | } 65 | -------------------------------------------------------------------------------- /isam/.svn/pristine/67/670ac3aa092d71b71d8ad4b07570fa7cb97cc28f.svn-base: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Noise.h 3 | * @brief Various noise models. 4 | * @author Michael Kaess 5 | * @version $Id: Noise.h 5797 2011-12-07 03:50:41Z kaess $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | #include 32 | 33 | namespace isam { 34 | 35 | // general noise model class 36 | class Noise { 37 | public: 38 | Eigen::MatrixXd _sqrtinf; 39 | const Eigen::MatrixXd& sqrtinf() const {return _sqrtinf;} 40 | }; 41 | 42 | // noise model based on square root information matrix 43 | class SqrtInformation : public Noise { 44 | public: 45 | SqrtInformation(const Eigen::MatrixXd& sqrtinf) {_sqrtinf = sqrtinf;} 46 | }; 47 | 48 | // noise model based on information matrix 49 | class Information : public Noise { 50 | public: 51 | Information(const Eigen::MatrixXd& inf) { 52 | _sqrtinf = inf.llt().matrixU(); 53 | } 54 | }; 55 | 56 | // noise model based on covariance matrix 57 | class Covariance : public Noise { 58 | public: 59 | Covariance(const Eigen::MatrixXd& cov) { 60 | _sqrtinf = cov.inverse().llt().matrixU(); 61 | } 62 | }; 63 | 64 | } 65 | -------------------------------------------------------------------------------- /include/nms/nms.m: -------------------------------------------------------------------------------- 1 | function pick = nms(boxes, overlap, use_gpu) 2 | % top = nms(boxes, overlap) 3 | % Non-maximum suppression. (FAST VERSION) 4 | % Greedily select high-scoring detections and skip detections 5 | % that are significantly covered by a previously selected 6 | % detection. 7 | % 8 | % NOTE: This is adapted from Pedro Felzenszwalb's version (nms.m), 9 | % but an inner loop has been eliminated to significantly speed it 10 | % up in the case of a large number of boxes 11 | 12 | % Copyright (C) 2011-12 by Tomasz Malisiewicz 13 | % All rights reserved. 14 | % 15 | % This file is part of the Exemplar-SVM library and is made 16 | % available under the terms of the MIT license (see COPYING file). 17 | % Project homepage: https://github.com/quantombone/exemplarsvm 18 | 19 | 20 | if isempty(boxes) 21 | pick = []; 22 | return; 23 | end 24 | 25 | if ~exist('use_gpu', 'var') 26 | use_gpu = false; 27 | end 28 | 29 | if use_gpu 30 | s = boxes(:, end); 31 | if ~issorted(s(end:-1:1)) 32 | [~, I] = sort(s, 'descend'); 33 | boxes = boxes(I, :); 34 | pick = nms_gpu_mex(single(boxes)', double(overlap)); 35 | pick = I(pick); 36 | else 37 | pick = nms_gpu_mex(single(boxes)', double(overlap)); 38 | end 39 | return; 40 | end 41 | 42 | if size(boxes, 1) < 1000000 43 | pick = nms_mex(double(boxes), double(overlap)); 44 | return; 45 | end 46 | 47 | x1 = boxes(:,1); 48 | y1 = boxes(:,2); 49 | x2 = boxes(:,3); 50 | y2 = boxes(:,4); 51 | s = boxes(:,end); 52 | 53 | area = (x2-x1+1) .* (y2-y1+1); 54 | [vals, I] = sort(s); 55 | 56 | pick = s*0; 57 | counter = 1; 58 | while ~isempty(I) 59 | last = length(I); 60 | i = I(last); 61 | pick(counter) = i; 62 | counter = counter + 1; 63 | 64 | xx1 = max(x1(i), x1(I(1:last-1))); 65 | yy1 = max(y1(i), y1(I(1:last-1))); 66 | xx2 = min(x2(i), x2(I(1:last-1))); 67 | yy2 = min(y2(i), y2(I(1:last-1))); 68 | 69 | w = max(0.0, xx2-xx1+1); 70 | h = max(0.0, yy2-yy1+1); 71 | 72 | inter = w.*h; 73 | o = inter ./ (area(i) + area(I(1:last-1)) - inter); 74 | 75 | I = I(find(o<=overlap)); 76 | end 77 | 78 | pick = pick(1:(counter-1)); 79 | -------------------------------------------------------------------------------- /isam/README: -------------------------------------------------------------------------------- 1 | 2 | Incremental Smoothing and Mapping (iSAM) library 3 | Michael Kaess, Hordur Johannsson, David Rosen, John Leonard, 2012 4 | 5 | isamlib/ - Source code for the iSAM library 6 | include/ - Header files for the iSAM library 7 | isam/ - Source code for main iSAM executable 8 | examples/ - Example code for iSAM 9 | doc/ - Documentation (after calling "make doc") 10 | misc/ - Code referenced from publications 11 | data/ - Example data files for 2D and 3D 12 | lib/ - iSAM library (after calling "make") 13 | bin/ - Executables (after calling "make") 14 | 15 | Type "make doc" to generate the full source documentation including 16 | installation instructions and examples. Then point your browser to: 17 | doc/html/index.html 18 | 19 | The latest documentation is also available online at: 20 | http://people.csail.mit.edu/kaess/isam/ 21 | 22 | -------------- 23 | 24 | Overview: 25 | 26 | The iSAM library provides a range of existing functionality for 27 | least-squares optimization, focused on the SLAM problem (e.g. 2D/3D 28 | pose graph, landmarks, visual SLAM). In addition to standard batch 29 | optimization, efficient incremental optimization is provided for sets 30 | of variables (nodes) and constraints (factors) that grow over 31 | time. The library performs (incremental) QR matrix factorization to 32 | solve the normal equations. Nonlinear constraints are dealt with using 33 | Gauss-Newton, Powell's Dog leg or, for batch only, the 34 | Levenberg-Marquardt algorithm. The default quadratic cost function can 35 | be replaced by a general cost function, allowing the use of robust 36 | estimators. 37 | 38 | Beyond the provided functionality, the library can easily be extended 39 | by the user to other sparse least-squares problems. In particular, 40 | the user can define new nodes, containing variables to be estimated as 41 | well as an exponential map for dealing with over-parameterized 42 | representations. Similarly, the user can define new factors, 43 | containing the error function for a given constraint and optionally a 44 | symbolic Jacobian for cases in which the provided numerical 45 | differentiation is not sufficiently fast. 46 | -------------------------------------------------------------------------------- /isam/.svn/pristine/87/87aae3226196f2892ee7c535ab22a7464cd54d55.svn-base: -------------------------------------------------------------------------------- 1 | 2 | Incremental Smoothing and Mapping (iSAM) library 3 | Michael Kaess, Hordur Johannsson, David Rosen, John Leonard, 2012 4 | 5 | isamlib/ - Source code for the iSAM library 6 | include/ - Header files for the iSAM library 7 | isam/ - Source code for main iSAM executable 8 | examples/ - Example code for iSAM 9 | doc/ - Documentation (after calling "make doc") 10 | misc/ - Code referenced from publications 11 | data/ - Example data files for 2D and 3D 12 | lib/ - iSAM library (after calling "make") 13 | bin/ - Executables (after calling "make") 14 | 15 | Type "make doc" to generate the full source documentation including 16 | installation instructions and examples. Then point your browser to: 17 | doc/html/index.html 18 | 19 | The latest documentation is also available online at: 20 | http://people.csail.mit.edu/kaess/isam/ 21 | 22 | -------------- 23 | 24 | Overview: 25 | 26 | The iSAM library provides a range of existing functionality for 27 | least-squares optimization, focused on the SLAM problem (e.g. 2D/3D 28 | pose graph, landmarks, visual SLAM). In addition to standard batch 29 | optimization, efficient incremental optimization is provided for sets 30 | of variables (nodes) and constraints (factors) that grow over 31 | time. The library performs (incremental) QR matrix factorization to 32 | solve the normal equations. Nonlinear constraints are dealt with using 33 | Gauss-Newton, Powell's Dog leg or, for batch only, the 34 | Levenberg-Marquardt algorithm. The default quadratic cost function can 35 | be replaced by a general cost function, allowing the use of robust 36 | estimators. 37 | 38 | Beyond the provided functionality, the library can easily be extended 39 | by the user to other sparse least-squares problems. In particular, 40 | the user can define new nodes, containing variables to be estimated as 41 | well as an exponential map for dealing with over-parameterized 42 | representations. Similarly, the user can define new factors, 43 | containing the error function for a given constraint and optionally a 44 | symbolic Jacobian for cases in which the provided numerical 45 | differentiation is not sufficiently fast. 46 | -------------------------------------------------------------------------------- /include/rpn/proposal_locate_anchors.m: -------------------------------------------------------------------------------- 1 | function [anchors, im_scales] = proposal_locate_anchors(conf, im_size, target_scale, feature_map_size) 2 | % [anchors, im_scales] = proposal_locate_anchors(conf, im_size, target_scale, feature_map_size) 3 | % -------------------------------------------------------- 4 | % Faster R-CNN 5 | % Copyright (c) 2015, Shaoqing Ren 6 | % Licensed under The MIT License [see LICENSE for details] 7 | % -------------------------------------------------------- 8 | % generate anchors for each scale 9 | 10 | % only for fcn 11 | if ~exist('feature_map_size', 'var') 12 | feature_map_size = []; 13 | end 14 | 15 | func = @proposal_locate_anchors_single_scale; 16 | 17 | if exist('target_scale', 'var') 18 | [anchors, im_scales] = func(im_size, conf, target_scale, feature_map_size); 19 | else 20 | [anchors, im_scales] = arrayfun(@(x) func(im_size, conf, x, feature_map_size), ... 21 | conf.scales, 'UniformOutput', false); 22 | end 23 | 24 | end 25 | 26 | function [anchors, im_scale] = proposal_locate_anchors_single_scale(im_size, conf, target_scale, feature_map_size) 27 | if isempty(feature_map_size) 28 | im_scale = prep_im_for_blob_size(im_size, target_scale, conf.max_size); 29 | img_size = round(im_size * im_scale); 30 | output_size = cell2mat([conf.output_height_map.values({img_size(1)}), conf.output_width_map.values({img_size(2)})]); 31 | else 32 | im_scale = prep_im_for_blob_size(im_size, target_scale, conf.max_size); 33 | output_size = feature_map_size; 34 | end 35 | 36 | shift_x = [0:(output_size(2)-1)] * conf.feat_stride; 37 | shift_y = [0:(output_size(1)-1)] * conf.feat_stride; 38 | [shift_x, shift_y] = meshgrid(shift_x, shift_y); 39 | 40 | % concat anchors as [channel, height, width], where channel is the fastest dimension. 41 | anchors = reshape(bsxfun(@plus, permute(conf.anchors, [1, 3, 2]), ... 42 | permute([shift_x(:), shift_y(:), shift_x(:), shift_y(:)], [3, 1, 2])), [], 4); 43 | 44 | % equals to 45 | % anchors = arrayfun(@(x, y) single(bsxfun(@plus, conf.anchors, [x, y, x, y])), shift_x, shift_y, 'UniformOutput', false); 46 | % anchors = reshape(anchors, [], 1); 47 | % anchors = cat(1, anchors{:}); 48 | 49 | end -------------------------------------------------------------------------------- /isam/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # iSAM master cmake file 2 | # Michael Kaess, 2010 3 | 4 | project(isam) 5 | 6 | cmake_minimum_required (VERSION 2.6) 7 | 8 | if(CMAKE_BINARY_DIR STREQUAL PROJECT_SOURCE_DIR) 9 | message(FATAL_ERROR "\nPlease do not build in the source directory, but instead build in a separate directory. The iSAM Makefile provides useful shortcuts for using cmake. Type:\nmake distclean\nto remove any local cmake build files. Then simply type\nmake\nwhich will call cmake in the build/ directory.") 10 | endif() 11 | 12 | if(NOT DEFINED CMAKE_BUILD_TYPE OR CMAKE_BUILD_TYPE STREQUAL "") 13 | set(CMAKE_BUILD_TYPE Release CACHE STRING 14 | "Choose the type of build, options are: None(CMAKE_CXX_FLAGS or CMAKE_C_FLAGS used) Debug Release RelWithDebInfo MinSizeRel." FORCE) 15 | endif() 16 | message(STATUS "Build configuration: ${CMAKE_BUILD_TYPE}") 17 | 18 | set (LIBRARY_OUTPUT_PATH "${PROJECT_SOURCE_DIR}/lib" CACHE PATH 19 | "Target directory for all libraries.") 20 | set (EXECUTABLE_OUTPUT_PATH "${PROJECT_SOURCE_DIR}/bin" CACHE PATH 21 | "Target directory for all executables.") 22 | 23 | # do not edit - use ccmake to change 24 | option (PROFILE "Enable profiling" OFF) 25 | option (USE_LCM "Compile with LCM interface (lcm library needed)" OFF) 26 | if(NOT DEFINED USE_GUI) 27 | # SDL is optional 28 | find_package(SDL) 29 | if (NOT SDL_FOUND) 30 | message(WARNING "\nSDL not found, disabling GUI.") 31 | set(USE_GUI_TMP OFF) 32 | else(NOT SDL_FOUND) 33 | set(USE_GUI_TMP ON) 34 | endif() 35 | option (USE_GUI "Compile with GUI (SDL library needed)" ${USE_GUI_TMP}) 36 | endif() 37 | 38 | add_definitions(-Wall) 39 | # note: -g automatic for Debug mode, -O3 -DNDEBUG automatic for Release 40 | 41 | if(PROFILE) 42 | message(STATUS "Profiling active") 43 | add_definitions(-pg) 44 | endif(PROFILE) 45 | 46 | if(USE_LCM) 47 | add_definitions(-DUSE_LCM) 48 | endif(USE_LCM) 49 | if(USE_GUI) 50 | add_definitions(-DUSE_GUI) 51 | endif(USE_GUI) 52 | 53 | # Eigen3 is needed 54 | SET(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 55 | find_package(Eigen3 REQUIRED) 56 | include_directories(${EIGEN3_INCLUDE_DIR}) 57 | 58 | include_directories("${PROJECT_SOURCE_DIR}/include") 59 | 60 | add_subdirectory(isamlib) 61 | add_subdirectory(isam) 62 | # add_subdirectory(examples) 63 | add_subdirectory(misc) 64 | -------------------------------------------------------------------------------- /isam/include/isam/OptimizationInterface.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file OptimizationInterface.h 3 | * @brief Abstract base class for nonlinear optimizer. 4 | * @author Michael Kaess 5 | * @author David Rosen 6 | * @version $Id: OptimizationInterface.h 6371 2012-03-29 22:22:23Z kaess $ 7 | * 8 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 9 | * Michael Kaess, Hordur Johannsson, David Rosen, 10 | * Nicholas Carlevaris-Bianco and John. J. Leonard 11 | * 12 | * This file is part of iSAM. 13 | * 14 | * iSAM is free software; you can redistribute it and/or modify it under 15 | * the terms of the GNU Lesser General Public License as published by the 16 | * Free Software Foundation; either version 2.1 of the License, or (at 17 | * your option) any later version. 18 | * 19 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 20 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 21 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 22 | * License for more details. 23 | * 24 | * You should have received a copy of the GNU Lesser General Public License 25 | * along with iSAM. If not, see . 26 | * 27 | */ 28 | 29 | #pragma once 30 | 31 | #include 32 | 33 | #include "SparseSystem.h" 34 | #include "Node.h" 35 | 36 | 37 | namespace isam { 38 | 39 | /** 40 | * Abstract base class providing an interface between the nonlinear system 41 | * to be optimized (stored in the Nodes of the Graph constructed in the SLAM) 42 | * and the Optimization class that actually performs the optimizations. 43 | */ 44 | class OptimizationInterface { 45 | 46 | protected: 47 | 48 | /** Factored Jacobian about the current linearization point.*/ 49 | SparseSystem _R; 50 | 51 | public: 52 | virtual SparseSystem jacobian() = 0; 53 | virtual void apply_exmap(const Eigen::VectorXd& delta) = 0; 54 | virtual void self_exmap(const Eigen::VectorXd& delta) = 0; 55 | virtual void estimate_to_linpoint() = 0; 56 | virtual void linpoint_to_estimate() = 0; 57 | virtual void swap_estimates() = 0; 58 | virtual Eigen::VectorXd weighted_errors(Selector s = ESTIMATE) = 0; 59 | 60 | OptimizationInterface(): _R(1,1) {} 61 | 62 | virtual ~OptimizationInterface() {} 63 | 64 | friend class Optimizer; 65 | }; 66 | 67 | } 68 | -------------------------------------------------------------------------------- /include/utils/subsample_images_per_class.m: -------------------------------------------------------------------------------- 1 | function [imdbs, roidbs] = subsample_images_per_class(imdbs, roidbs, max_per_class_image_num, seed) 2 | 3 | if ~exist('seed', 'var') 4 | seed = 6; 5 | end 6 | 7 | class_num = cellfun(@(x) length(x.class_ids), imdbs, 'UniformOutput', true); 8 | assert(length(unique(class_num)) == 1); 9 | class_num = unique(class_num); 10 | 11 | rois = cellfun(@(x) x.rois, roidbs, 'UniformOutput', false); 12 | rois_combine = cell2mat(rois(:)); 13 | rois_combine_class = arrayfun(@(x) x.class, rois_combine, 'UniformOutput', false); 14 | 15 | %% select images with max_image_num 16 | 17 | % fix the random seed for repeatability 18 | prev_rng = seed_rand(seed); 19 | inds = cell(class_num, 1); 20 | rois_combine_length = length(rois_combine); 21 | valid_idxs = cell(class_num, 1); 22 | parfor i = 1:class_num 23 | valid_idxs{i} = cellfun(@(x) any(x == i), rois_combine_class, 'UniformOutput', false); 24 | valid_idxs{i} = cell2mat(valid_idxs{i}); 25 | end 26 | 27 | for i = 1:class_num 28 | valid_num = sum(valid_idxs{i}); 29 | 30 | num = min(valid_num, max_per_class_image_num); 31 | inds{i} = 1:rois_combine_length; 32 | inds{i} = inds{i}(valid_idxs{i}); 33 | inds{i} = inds{i}(randperm(length(inds{i}), num)); 34 | end 35 | 36 | inds = cell2mat(inds')'; 37 | inds = unique(inds); 38 | 39 | % restore previous rng 40 | rng(prev_rng); 41 | 42 | img_idx_start = 1; 43 | for i = 1:length(imdbs) 44 | imdb_img_num = length(imdbs{i}.image_ids); 45 | img_idx_end = img_idx_start + imdb_img_num - 1; 46 | inds_start = find(inds >= img_idx_start, 1, 'first'); 47 | inds_end = find(inds <= img_idx_end, 1, 'last'); 48 | 49 | inds_sub = inds(inds_start:inds_end); 50 | inds_sub = inds_sub - img_idx_start + 1; 51 | 52 | imdbs{i}.image_ids = imdbs{i}.image_ids(inds_sub); 53 | imdbs{i}.sizes = imdbs{i}.sizes(inds_sub, :); 54 | if isfield(imdbs{i}, 'image_dir') 55 | imdbs{i}.image_at = @(x) ... 56 | sprintf('%s/%s.%s', imdbs{i}.image_dir, imdbs{i}.image_ids{x}, imdbs{i}.extension); 57 | else 58 | imdbs{i}.image_at = @(x) ... 59 | sprintf('%s/%s.%s', imdbs{i}.imagedir, imdbs{i}.image_ids{x}, imdbs{i}.extension); 60 | end 61 | roidbs{i}.rois = roidbs{i}.rois(inds_sub); 62 | 63 | img_idx_start = img_idx_start + imdb_img_num; 64 | end 65 | 66 | 67 | -------------------------------------------------------------------------------- /isam/.svn/pristine/e8/e8b090883df11a75df6b84ccc0192f449844cfd8.svn-base: -------------------------------------------------------------------------------- 1 | /** 2 | * @file OptimizationInterface.h 3 | * @brief Abstract base class for nonlinear optimizer. 4 | * @author Michael Kaess 5 | * @author David Rosen 6 | * @version $Id: OptimizationInterface.h 6371 2012-03-29 22:22:23Z kaess $ 7 | * 8 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 9 | * Michael Kaess, Hordur Johannsson, David Rosen, 10 | * Nicholas Carlevaris-Bianco and John. J. Leonard 11 | * 12 | * This file is part of iSAM. 13 | * 14 | * iSAM is free software; you can redistribute it and/or modify it under 15 | * the terms of the GNU Lesser General Public License as published by the 16 | * Free Software Foundation; either version 2.1 of the License, or (at 17 | * your option) any later version. 18 | * 19 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 20 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 21 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 22 | * License for more details. 23 | * 24 | * You should have received a copy of the GNU Lesser General Public License 25 | * along with iSAM. If not, see . 26 | * 27 | */ 28 | 29 | #pragma once 30 | 31 | #include 32 | 33 | #include "SparseSystem.h" 34 | #include "Node.h" 35 | 36 | 37 | namespace isam { 38 | 39 | /** 40 | * Abstract base class providing an interface between the nonlinear system 41 | * to be optimized (stored in the Nodes of the Graph constructed in the SLAM) 42 | * and the Optimization class that actually performs the optimizations. 43 | */ 44 | class OptimizationInterface { 45 | 46 | protected: 47 | 48 | /** Factored Jacobian about the current linearization point.*/ 49 | SparseSystem _R; 50 | 51 | public: 52 | virtual SparseSystem jacobian() = 0; 53 | virtual void apply_exmap(const Eigen::VectorXd& delta) = 0; 54 | virtual void self_exmap(const Eigen::VectorXd& delta) = 0; 55 | virtual void estimate_to_linpoint() = 0; 56 | virtual void linpoint_to_estimate() = 0; 57 | virtual void swap_estimates() = 0; 58 | virtual Eigen::VectorXd weighted_errors(Selector s = ESTIMATE) = 0; 59 | 60 | OptimizationInterface(): _R(1,1) {} 61 | 62 | virtual ~OptimizationInterface() {} 63 | 64 | friend class Optimizer; 65 | }; 66 | 67 | } 68 | -------------------------------------------------------------------------------- /isam/.svn/pristine/ea/ea9bd33e506a39fd8ac1e86bb3a0eb3c88a455f2.svn-base: -------------------------------------------------------------------------------- 1 | # iSAM master cmake file 2 | # Michael Kaess, 2010 3 | 4 | project(isam) 5 | 6 | cmake_minimum_required (VERSION 2.6) 7 | 8 | if(CMAKE_BINARY_DIR STREQUAL PROJECT_SOURCE_DIR) 9 | message(FATAL_ERROR "\nPlease do not build in the source directory, but instead build in a separate directory. The iSAM Makefile provides useful shortcuts for using cmake. Type:\nmake distclean\nto remove any local cmake build files. Then simply type\nmake\nwhich will call cmake in the build/ directory.") 10 | endif() 11 | 12 | if(NOT DEFINED CMAKE_BUILD_TYPE OR CMAKE_BUILD_TYPE STREQUAL "") 13 | set(CMAKE_BUILD_TYPE Release CACHE STRING 14 | "Choose the type of build, options are: None(CMAKE_CXX_FLAGS or CMAKE_C_FLAGS used) Debug Release RelWithDebInfo MinSizeRel." FORCE) 15 | endif() 16 | message(STATUS "Build configuration: ${CMAKE_BUILD_TYPE}") 17 | 18 | set (LIBRARY_OUTPUT_PATH "${PROJECT_SOURCE_DIR}/lib" CACHE PATH 19 | "Target directory for all libraries.") 20 | set (EXECUTABLE_OUTPUT_PATH "${PROJECT_SOURCE_DIR}/bin" CACHE PATH 21 | "Target directory for all executables.") 22 | 23 | # do not edit - use ccmake to change 24 | option (PROFILE "Enable profiling" OFF) 25 | option (USE_LCM "Compile with LCM interface (lcm library needed)" OFF) 26 | if(NOT DEFINED USE_GUI) 27 | # SDL is optional 28 | find_package(SDL) 29 | if (NOT SDL_FOUND) 30 | message(WARNING "\nSDL not found, disabling GUI.") 31 | set(USE_GUI_TMP OFF) 32 | else(NOT SDL_FOUND) 33 | set(USE_GUI_TMP ON) 34 | endif() 35 | option (USE_GUI "Compile with GUI (SDL library needed)" ${USE_GUI_TMP}) 36 | endif() 37 | 38 | add_definitions(-Wall) 39 | # note: -g automatic for Debug mode, -O3 -DNDEBUG automatic for Release 40 | 41 | if(PROFILE) 42 | message(STATUS "Profiling active") 43 | add_definitions(-pg) 44 | endif(PROFILE) 45 | 46 | if(USE_LCM) 47 | add_definitions(-DUSE_LCM) 48 | endif(USE_LCM) 49 | if(USE_GUI) 50 | add_definitions(-DUSE_GUI) 51 | endif(USE_GUI) 52 | 53 | # Eigen3 is needed 54 | SET(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 55 | find_package(Eigen3 REQUIRED) 56 | include_directories(${EIGEN3_INCLUDE_DIR}) 57 | 58 | include_directories("${PROJECT_SOURCE_DIR}/include") 59 | 60 | add_subdirectory(isamlib) 61 | add_subdirectory(isam) 62 | add_subdirectory(examples) 63 | add_subdirectory(misc) 64 | -------------------------------------------------------------------------------- /generateSimData.m: -------------------------------------------------------------------------------- 1 | clear all; 2 | %% generate 2D objects 3 | truth_objects = rand(15,2)*10; 4 | truth_objects(:,3)=mod(1:15,5); 5 | 6 | %% plot 2D objects 7 | figure; hold on; set(0,'DefaultLineMarkerSize',10) 8 | for i=1:length(truth_objects) 9 | switch truth_objects(i,3) 10 | case 0 11 | plot(truth_objects(i,1),truth_objects(i,2),'bo','MarkerFaceColor','b'); 12 | case 1 13 | plot(truth_objects(i,1),truth_objects(i,2),'rd','MarkerFaceColor','r'); 14 | case 2 15 | plot(truth_objects(i,1),truth_objects(i,2),'ms','MarkerFaceColor','m'); 16 | case 3 17 | plot(truth_objects(i,1),truth_objects(i,2),'g^','MarkerFaceColor',[0.2 1 0.2]); 18 | case 4 19 | plot(truth_objects(i,1),truth_objects(i,2),'yp','MarkerFaceColor',[1 1 0.3],... 20 | 'MarkerSize',15); 21 | end 22 | end 23 | 24 | %% draw trajectory 25 | [x,y] = ginput(); 26 | 27 | %% generate odometry 28 | truth_traj=[]; 29 | for i=2:length(x) 30 | l = norm([x(i)-x(i-1) y(i)-y(i-1)]); 31 | t=0:0.1:l; 32 | xq=interp1([0 l],[x(i-1) x(i)],t,'spline'); 33 | yq=interp1([0 l],[y(i-1) y(i)],t,'spline'); 34 | truth_traj = [truth_traj; [xq' yq']]; 35 | end 36 | for i=1:length(truth_traj)-1 37 | odoms(i,3)=atan2(truth_traj(i+1,2)-truth_traj(i,2),truth_traj(i+1,1)-truth_traj(i,1)); 38 | end 39 | odoms(end,3)=odoms(end-1,3); 40 | 41 | %% 42 | for i=2:length(odoms) 43 | R = [cos(odoms(i,3)),sin(odoms(i,3)); -sin(odoms(i,3)) cos(odoms(i,3))]; 44 | node_edge.dpos(:,i)= R'*(odoms(i,1:2)-odoms(i-1,1:2))'; 45 | node_edge.dtheta(i) = odoms(i,3)-odoms(i-1,3); 46 | end 47 | 48 | %% generate object measurements 49 | FOV=4; AOV=60/180*pi; 50 | lm_edge.id1=[]; 51 | lm_edge.id2=[]; 52 | lm_edge.dpos=[]; 53 | lm_edge.label=[]; 54 | for i=2:length(odoms) 55 | dx=truth_objects(:,1)-odoms(i,1); 56 | dy=truth_objects(:,2)-odoms(i,2); 57 | dtheta = mod(atan2(dy,dx)-odoms(i,3)+pi,2*pi)-pi; 58 | R = [cos(odoms(i,3)),sin(odoms(i,3)); -sin(odoms(i,3)) cos(odoms(i,3))]; 59 | idx = find((dx.^2+dy.^2). 25 | * 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | 32 | #include "SparseSystem.h" 33 | 34 | namespace isam { 35 | 36 | class Cholesky { 37 | public: 38 | virtual ~Cholesky() {} 39 | 40 | /** 41 | * Factorize a given system Ax=b and optionally solve. 42 | * @param Ab SparseSystem with measurement Jacobian A and right hand side b. 43 | * @param delta Optional parameter to return solution of system. 44 | * @param lambda Adds elements to diagonal of information matrix A'A before 45 | * factorization, used for Levenberg-Marquardt algorithm. 46 | */ 47 | virtual void factorize(const SparseSystem& Ab, Eigen::VectorXd* delta = NULL, double lambda = 0.) = 0; 48 | 49 | /** 50 | * Copy R into a SparseSystem data structure (expensive, so can be 51 | * avoided during batch factorization). 52 | * @param R SparseSystem that upon return will contain the R factor. 53 | */ 54 | virtual void get_R(SparseSystem& R) = 0; 55 | 56 | /** 57 | * Access the variable ordering used for Cholesky factorization. 58 | * @return Pointer to variable ordering. 59 | */ 60 | virtual int* get_order() = 0; 61 | 62 | static Cholesky* Create(); 63 | 64 | protected: 65 | Cholesky() {} 66 | }; 67 | 68 | } 69 | -------------------------------------------------------------------------------- /isam/.svn/pristine/42/4206d9b17106f76510795ccab2a014200df5a307.svn-base: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Cholesky.h 3 | * @brief Cholesky batch factorization using SuiteSparse by Tim Davis. 4 | * @author Michael Kaess 5 | * @version $Id: Cholesky.h 6377 2012-03-30 20:06:44Z kaess $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | 32 | #include "SparseSystem.h" 33 | 34 | namespace isam { 35 | 36 | class Cholesky { 37 | public: 38 | virtual ~Cholesky() {} 39 | 40 | /** 41 | * Factorize a given system Ax=b and optionally solve. 42 | * @param Ab SparseSystem with measurement Jacobian A and right hand side b. 43 | * @param delta Optional parameter to return solution of system. 44 | * @param lambda Adds elements to diagonal of information matrix A'A before 45 | * factorization, used for Levenberg-Marquardt algorithm. 46 | */ 47 | virtual void factorize(const SparseSystem& Ab, Eigen::VectorXd* delta = NULL, double lambda = 0.) = 0; 48 | 49 | /** 50 | * Copy R into a SparseSystem data structure (expensive, so can be 51 | * avoided during batch factorization). 52 | * @param R SparseSystem that upon return will contain the R factor. 53 | */ 54 | virtual void get_R(SparseSystem& R) = 0; 55 | 56 | /** 57 | * Access the variable ordering used for Cholesky factorization. 58 | * @return Pointer to variable ordering. 59 | */ 60 | virtual int* get_order() = 0; 61 | 62 | static Cholesky* Create(); 63 | 64 | protected: 65 | Cholesky() {} 66 | }; 67 | 68 | } 69 | -------------------------------------------------------------------------------- /isam/cmake/FindCholmod.cmake: -------------------------------------------------------------------------------- 1 | # Cholmod lib usually requires linking to a blas and lapack library. 2 | # It is up to the user of this module to find a BLAS and link to it. 3 | 4 | if (CHOLMOD_INCLUDES AND CHOLMOD_LIBRARIES) 5 | set(CHOLMOD_FIND_QUIETLY TRUE) 6 | endif (CHOLMOD_INCLUDES AND CHOLMOD_LIBRARIES) 7 | 8 | find_path(CHOLMOD_INCLUDES 9 | NAMES 10 | cholmod.h 11 | PATHS 12 | $ENV{CHOLMODDIR} 13 | ${INCLUDE_INSTALL_DIR} 14 | PATH_SUFFIXES 15 | suitesparse 16 | ) 17 | 18 | find_library(CHOLMOD_LIBRARIES cholmod PATHS $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 19 | 20 | if(CHOLMOD_LIBRARIES) 21 | 22 | get_filename_component(CHOLMOD_LIBDIR ${CHOLMOD_LIBRARIES} PATH) 23 | 24 | find_library(AMD_LIBRARY amd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 25 | if (AMD_LIBRARY) 26 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${AMD_LIBRARY}) 27 | else () 28 | set(CHOLMOD_LIBRARIES FALSE) 29 | endif () 30 | 31 | endif(CHOLMOD_LIBRARIES) 32 | 33 | if(CHOLMOD_LIBRARIES) 34 | 35 | find_library(COLAMD_LIBRARY colamd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 36 | if (COLAMD_LIBRARY) 37 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${COLAMD_LIBRARY}) 38 | else () 39 | set(CHOLMOD_LIBRARIES FALSE) 40 | endif () 41 | 42 | endif(CHOLMOD_LIBRARIES) 43 | 44 | if(CHOLMOD_LIBRARIES) 45 | 46 | find_library(CAMD_LIBRARY camd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 47 | if (CAMD_LIBRARY) 48 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CAMD_LIBRARY}) 49 | else () 50 | set(CHOLMOD_LIBRARIES FALSE) 51 | endif () 52 | 53 | endif(CHOLMOD_LIBRARIES) 54 | 55 | if(CHOLMOD_LIBRARIES) 56 | 57 | find_library(CCOLAMD_LIBRARY ccolamd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 58 | if (CCOLAMD_LIBRARY) 59 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CCOLAMD_LIBRARY}) 60 | else () 61 | set(CHOLMOD_LIBRARIES FALSE) 62 | endif () 63 | 64 | endif(CHOLMOD_LIBRARIES) 65 | 66 | if(CHOLMOD_LIBRARIES) 67 | 68 | find_library(CHOLMOD_METIS_LIBRARY metis PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 69 | if (CHOLMOD_METIS_LIBRARY) 70 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CHOLMOD_METIS_LIBRARY}) 71 | endif () 72 | 73 | endif(CHOLMOD_LIBRARIES) 74 | 75 | include(FindPackageHandleStandardArgs) 76 | find_package_handle_standard_args(CHOLMOD DEFAULT_MSG 77 | CHOLMOD_INCLUDES CHOLMOD_LIBRARIES) 78 | 79 | mark_as_advanced(CHOLMOD_INCLUDES CHOLMOD_LIBRARIES AMD_LIBRARY COLAMD_LIBRARY) 80 | -------------------------------------------------------------------------------- /isam/.svn/pristine/1b/1bf98790eb2dc0e17839198b9e394e8f443c8f0b.svn-base: -------------------------------------------------------------------------------- 1 | # Cholmod lib usually requires linking to a blas and lapack library. 2 | # It is up to the user of this module to find a BLAS and link to it. 3 | 4 | if (CHOLMOD_INCLUDES AND CHOLMOD_LIBRARIES) 5 | set(CHOLMOD_FIND_QUIETLY TRUE) 6 | endif (CHOLMOD_INCLUDES AND CHOLMOD_LIBRARIES) 7 | 8 | find_path(CHOLMOD_INCLUDES 9 | NAMES 10 | cholmod.h 11 | PATHS 12 | $ENV{CHOLMODDIR} 13 | ${INCLUDE_INSTALL_DIR} 14 | PATH_SUFFIXES 15 | suitesparse 16 | ) 17 | 18 | find_library(CHOLMOD_LIBRARIES cholmod PATHS $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 19 | 20 | if(CHOLMOD_LIBRARIES) 21 | 22 | get_filename_component(CHOLMOD_LIBDIR ${CHOLMOD_LIBRARIES} PATH) 23 | 24 | find_library(AMD_LIBRARY amd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 25 | if (AMD_LIBRARY) 26 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${AMD_LIBRARY}) 27 | else () 28 | set(CHOLMOD_LIBRARIES FALSE) 29 | endif () 30 | 31 | endif(CHOLMOD_LIBRARIES) 32 | 33 | if(CHOLMOD_LIBRARIES) 34 | 35 | find_library(COLAMD_LIBRARY colamd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 36 | if (COLAMD_LIBRARY) 37 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${COLAMD_LIBRARY}) 38 | else () 39 | set(CHOLMOD_LIBRARIES FALSE) 40 | endif () 41 | 42 | endif(CHOLMOD_LIBRARIES) 43 | 44 | if(CHOLMOD_LIBRARIES) 45 | 46 | find_library(CAMD_LIBRARY camd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 47 | if (CAMD_LIBRARY) 48 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CAMD_LIBRARY}) 49 | else () 50 | set(CHOLMOD_LIBRARIES FALSE) 51 | endif () 52 | 53 | endif(CHOLMOD_LIBRARIES) 54 | 55 | if(CHOLMOD_LIBRARIES) 56 | 57 | find_library(CCOLAMD_LIBRARY ccolamd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 58 | if (CCOLAMD_LIBRARY) 59 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CCOLAMD_LIBRARY}) 60 | else () 61 | set(CHOLMOD_LIBRARIES FALSE) 62 | endif () 63 | 64 | endif(CHOLMOD_LIBRARIES) 65 | 66 | if(CHOLMOD_LIBRARIES) 67 | 68 | find_library(CHOLMOD_METIS_LIBRARY metis PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 69 | if (CHOLMOD_METIS_LIBRARY) 70 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CHOLMOD_METIS_LIBRARY}) 71 | endif () 72 | 73 | endif(CHOLMOD_LIBRARIES) 74 | 75 | include(FindPackageHandleStandardArgs) 76 | find_package_handle_standard_args(CHOLMOD DEFAULT_MSG 77 | CHOLMOD_INCLUDES CHOLMOD_LIBRARIES) 78 | 79 | mark_as_advanced(CHOLMOD_INCLUDES CHOLMOD_LIBRARIES AMD_LIBRARY COLAMD_LIBRARY) 80 | -------------------------------------------------------------------------------- /isam/include/isam/Point2d.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Point2d.h 3 | * @brief Simple 2D point class. 4 | * @author Michael Kaess 5 | * @version $Id: Point2d.h 8263 2013-04-10 14:02:19Z carlevar $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | 32 | namespace Eigen { 33 | typedef Matrix VectorXb; 34 | } 35 | 36 | namespace isam { 37 | 38 | class Point2d { 39 | friend std::ostream& operator<<(std::ostream& out, const Point2d& p) { 40 | p.write(out); 41 | return out; 42 | } 43 | 44 | double _x; 45 | double _y; 46 | 47 | public: 48 | static const int dim = 2; 49 | static const int size = 2; 50 | static const char* name() { 51 | return "Point2d"; 52 | } 53 | Point2d() : _x(0), _y(0) {} 54 | Point2d(double x, double y) : _x(x), _y(y) {} 55 | Point2d(const Eigen::Vector2d& vec) : _x(vec(0)), _y(vec(1)) {} 56 | 57 | double x() const {return _x;} 58 | double y() const {return _y;} 59 | 60 | void set_x(double x) {_x = x;} 61 | void set_y(double y) {_y = y;} 62 | 63 | Point2d exmap(const Eigen::Vector2d& delta) const { 64 | Point2d res = *this; 65 | res._x += delta(0); 66 | res._y += delta(1); 67 | return res; 68 | } 69 | 70 | Eigen::Vector2d vector() const { 71 | Eigen::Vector2d v(_x, _y); 72 | return v; 73 | } 74 | void set(double x, double y) { 75 | _x = x; 76 | _y = y; 77 | } 78 | void set(const Eigen::Vector2d& v) { 79 | _x = v(0); 80 | _y = v(1); 81 | } 82 | void write(std::ostream &out) const { 83 | out << "(" << _x << ", " << _y << ")"; 84 | } 85 | 86 | Eigen::VectorXb is_angle() const { 87 | return Eigen::VectorXb::Zero(size); 88 | } 89 | 90 | }; 91 | 92 | } 93 | -------------------------------------------------------------------------------- /isam/.svn/pristine/8d/8d81800db36466dbe6888420fb6b886844dcf522.svn-base: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Point2d.h 3 | * @brief Simple 2D point class. 4 | * @author Michael Kaess 5 | * @version $Id: Point2d.h 8263 2013-04-10 14:02:19Z carlevar $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | 32 | namespace Eigen { 33 | typedef Matrix VectorXb; 34 | } 35 | 36 | namespace isam { 37 | 38 | class Point2d { 39 | friend std::ostream& operator<<(std::ostream& out, const Point2d& p) { 40 | p.write(out); 41 | return out; 42 | } 43 | 44 | double _x; 45 | double _y; 46 | 47 | public: 48 | static const int dim = 2; 49 | static const int size = 2; 50 | static const char* name() { 51 | return "Point2d"; 52 | } 53 | Point2d() : _x(0), _y(0) {} 54 | Point2d(double x, double y) : _x(x), _y(y) {} 55 | Point2d(const Eigen::Vector2d& vec) : _x(vec(0)), _y(vec(1)) {} 56 | 57 | double x() const {return _x;} 58 | double y() const {return _y;} 59 | 60 | void set_x(double x) {_x = x;} 61 | void set_y(double y) {_y = y;} 62 | 63 | Point2d exmap(const Eigen::Vector2d& delta) const { 64 | Point2d res = *this; 65 | res._x += delta(0); 66 | res._y += delta(1); 67 | return res; 68 | } 69 | 70 | Eigen::Vector2d vector() const { 71 | Eigen::Vector2d v(_x, _y); 72 | return v; 73 | } 74 | void set(double x, double y) { 75 | _x = x; 76 | _y = y; 77 | } 78 | void set(const Eigen::Vector2d& v) { 79 | _x = v(0); 80 | _y = v(1); 81 | } 82 | void write(std::ostream &out) const { 83 | out << "(" << _x << ", " << _y << ")"; 84 | } 85 | 86 | Eigen::VectorXb is_angle() const { 87 | return Eigen::VectorXb::Zero(size); 88 | } 89 | 90 | }; 91 | 92 | } 93 | -------------------------------------------------------------------------------- /isam/.svn/pristine/0b/0bf86855590fe80c4e8a8b0a562a849456fdc568.svn-base: -------------------------------------------------------------------------------- 1 | Short summary of the datasets 2 | 3 | File formats: 4 | 5 | Various file formats are parsed by iSAM. Note that often encountered 6 | variable initialization entries (eg. VERTEX) are not supported, as in 7 | online settings only the measurements are available, and the 8 | initialization has to be obtained from the measurements. Also, the 9 | measurements must be in time order as actually obtained by the robot. 10 | All measurement entries follow the same convention: 11 | 12 | NAME id1 id2 measurement_1...measurement_n sqrtinf_1...sqrtinf_n 13 | 14 | where sqrtinf_i is the i-th entry of the upper triangular square root 15 | of the information matrix in row-first order (eg. sxx sxy sxt syy syt 16 | stt). Note that for 3D poses the angles and corresponding sqrtinf 17 | entries are in the order: roll-pitch-yaw. 18 | 19 | The groundtruth directory contains the original trajectory for some of 20 | the datasets, before noise has been added. 21 | 22 | 23 | 2D datasets: 24 | 25 | - victoriaPark.txt 26 | Creator: Eduard Nebot 27 | Original data processed using tree detector by Ananth Ranganathan 28 | and Michael Kaess, with duplicate poses removed while robot stationary. 29 | 6969 poses, 151 landmarks, 3640 landmark measurements, 6968 odometry constraints 30 | 31 | - manhattanOlson3500.txt 32 | Creator: Ed Olson 33 | See "Fast Iterative Alignment of Pose Graphs with Poor Initial Estimates", 34 | Edwin Olson, John Leonard and Seth Teller, ICRA 2006 35 | Simulated pose graph. 36 | 3500 poses and 5598 constraints (3499 odometry and 2099 loop closings) 37 | 38 | - city10000.txt 39 | Creator: Hordur Johannsson and Michael Kaess 40 | Simulated pose graph. 41 | 10000 poses, 20687 constraints (9999 odometry and 10688 loop closings) 42 | 43 | - cityTrees10000.txt 44 | Creator: Hordur Johannsson and Michael Kaess 45 | Simulated graph with landmarks. 46 | 10000 poses, 100 landmarks, 4443 landmark measurements, 9999 odometry constraints 47 | 48 | 3D datasets: 49 | 50 | - sphere400.txt 51 | Creator: Michael Kaess 52 | Simulated trajectory along the surface of a sphere with the robot 53 | poses perpendicular to the surface of the sphere. 54 | 400 poses, 779 constraints (399 odometry and 380 loop closings) 55 | 56 | - sphere2500.txt 57 | Creator: Michael Kaess 58 | Simulated trajectory along the surface of a sphere with the robot 59 | poses perpendicular to the surface of the sphere. 60 | 2500 poses, 4949 constraints (2499 odometry and 2450 loop closings) 61 | 62 | - torus10000.txt 63 | Creator: Michael Kaess 64 | Simulated trajectory along the surface of a torus, essentially a Manhattan world 65 | without opposite edges identified. 66 | 10000 poses, 22280 constraints (9999 odometry and 12281 loop closings) 67 | -------------------------------------------------------------------------------- /include/rpn/proposal_generate_anchors.m: -------------------------------------------------------------------------------- 1 | function anchors = proposal_generate_anchors(cache_name, varargin) 2 | % anchors = proposal_generate_anchors(cache_name, varargin) 3 | % -------------------------------------------------------- 4 | % Faster R-CNN 5 | % Copyright (c) 2015, Shaoqing Ren 6 | % Licensed under The MIT License [see LICENSE for details] 7 | % -------------------------------------------------------- 8 | 9 | %% inputs 10 | ip = inputParser; 11 | ip.addRequired('cache_name', @isstr); 12 | 13 | % the size of the base anchor 14 | ip.addParamValue('base_size', 16, @isscalar); 15 | % ratio list of anchors 16 | ip.addParamValue('ratios', [0.5, 1, 2], @ismatrix); 17 | % scale list of anchors 18 | ip.addParamValue('scales', 2.^[3:5], @ismatrix); 19 | ip.addParamValue('ignore_cache', false, @islogical); 20 | ip.parse(cache_name, varargin{:}); 21 | opts = ip.Results; 22 | 23 | %% 24 | if ~opts.ignore_cache 25 | anchor_cache_dir = fullfile(pwd, 'output', 'rpn_cachedir', cache_name); 26 | mkdir_if_missing(anchor_cache_dir); 27 | anchor_cache_file = fullfile(anchor_cache_dir, 'anchors'); 28 | end 29 | try 30 | ld = load(anchor_cache_file); 31 | anchors = ld.anchors; 32 | catch 33 | base_anchor = [1, 1, opts.base_size, opts.base_size]; 34 | ratio_anchors = ratio_jitter(base_anchor, opts.ratios); 35 | anchors = cellfun(@(x) scale_jitter(x, opts.scales), num2cell(ratio_anchors, 2), 'UniformOutput', false); 36 | anchors = cat(1, anchors{:}); 37 | if ~opts.ignore_cache 38 | save(anchor_cache_file, 'anchors'); 39 | end 40 | end 41 | 42 | end 43 | 44 | function anchors = ratio_jitter(anchor, ratios) 45 | ratios = ratios(:); 46 | 47 | w = anchor(3) - anchor(1) + 1; 48 | h = anchor(4) - anchor(2) + 1; 49 | x_ctr = anchor(1) + (w - 1) / 2; 50 | y_ctr = anchor(2) + (h - 1) / 2; 51 | size = w * h; 52 | 53 | size_ratios = size ./ ratios; 54 | ws = round(sqrt(size_ratios)); 55 | hs = round(ws .* ratios); 56 | 57 | anchors = [x_ctr - (ws - 1) / 2, y_ctr - (hs - 1) / 2, x_ctr + (ws - 1) / 2, y_ctr + (hs - 1) / 2]; 58 | end 59 | 60 | function anchors = scale_jitter(anchor, scales) 61 | scales = scales(:); 62 | 63 | w = anchor(3) - anchor(1) + 1; 64 | h = anchor(4) - anchor(2) + 1; 65 | x_ctr = anchor(1) + (w - 1) / 2; 66 | y_ctr = anchor(2) + (h - 1) / 2; 67 | 68 | ws = w * scales; 69 | hs = h * scales; 70 | 71 | anchors = [x_ctr - (ws - 1) / 2, y_ctr - (hs - 1) / 2, x_ctr + (ws - 1) / 2, y_ctr + (hs - 1) / 2]; 72 | end 73 | 74 | -------------------------------------------------------------------------------- /isam/isamlib/numericalDiff.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file numericalDiff.cpp 3 | * @brief Numerical differentiation. 4 | * @author Michael Kaess 5 | * @version $Id: numericalDiff.cpp 4038 2011-02-26 04:31:00Z kaess $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #include 29 | 30 | #include "isam/numericalDiff.h" 31 | 32 | #define SYMMETRIC 33 | 34 | const double epsilon = 0.0001; 35 | 36 | using namespace std; 37 | using namespace Eigen; 38 | 39 | namespace isam { 40 | 41 | MatrixXd numericalDiff(Function& func) { 42 | #ifndef SYMMETRIC 43 | VectorXd y0 = func.evaluate(); 44 | #endif 45 | // number of measurement rows 46 | int m = func.num_measurements(); 47 | // number of variables 48 | int n = 0; 49 | vector& nodes = func.nodes(); 50 | for (vector::iterator it = nodes.begin(); it!=nodes.end(); it++) { 51 | n += (*it)->dim(); 52 | } 53 | // result has one column per variable 54 | MatrixXd Jacobian(m,n); 55 | int col = 0; 56 | // for each node... 57 | for (vector::iterator it = nodes.begin(); it!=nodes.end(); it++) { 58 | Node* node = *it; 59 | int dim_n = node->dim(); 60 | // for each dimension of the node... 61 | for (int j=0; jvector0(); 66 | // evaluate positive delta 67 | delta(j) = epsilon; 68 | node->self_exmap(delta); 69 | VectorXd y_plus = func.evaluate(); 70 | node->update0(original); 71 | #ifdef SYMMETRIC 72 | // evaluate negative delta 73 | delta(j) = -epsilon; 74 | node->self_exmap(delta); 75 | VectorXd y_minus = func.evaluate(); 76 | node->update0(original); 77 | // store column 78 | VectorXd diff = (y_plus - y_minus) / (epsilon + epsilon); 79 | #else 80 | VectorXd diff = (y_plus - y0) / epsilon; 81 | #endif 82 | Jacobian.col(col) = diff; 83 | } 84 | } 85 | 86 | return Jacobian; 87 | } 88 | 89 | } 90 | -------------------------------------------------------------------------------- /include/fast_rcnn/fast_rcnn_config.m: -------------------------------------------------------------------------------- 1 | function conf = fast_rcnn_config(varargin) 2 | % conf = fast_rcnn_config(varargin) 3 | % Fast R-CNN configuration 4 | % -------------------------------------------------------- 5 | % Fast R-CNN 6 | % Reimplementation based on Python Fast R-CNN (https://github.com/rbgirshick/fast-rcnn) 7 | % Copyright (c) 2015, Shaoqing Ren 8 | % Licensed under The MIT License [see LICENSE for details] 9 | % -------------------------------------------------------- 10 | % 11 | ip = inputParser; 12 | 13 | %% training 14 | % whether use gpu 15 | ip.addParamValue('use_gpu', gpuDeviceCount > 0, ... 16 | @islogical); 17 | % Image scales -- the short edge of input image 18 | ip.addParamValue('scales', 600, @ismatrix); 19 | % Max pixel size of a scaled input image 20 | ip.addParamValue('max_size', 800, @isscalar); 21 | % Images per batch 22 | ip.addParamValue('ims_per_batch', 1, @isscalar); 23 | % Minibatch size 24 | ip.addParamValue('batch_size', 128, @isscalar); 25 | % Fraction of minibatch that is foreground labeled (class > 0) 26 | ip.addParamValue('fg_fraction', 0.25, @isscalar); 27 | % Overlap threshold for a ROI to be considered foreground (if >= fg_thresh) 28 | ip.addParamValue('fg_thresh', 0.5, @isscalar); 29 | % Overlap threshold for a ROI to be considered background (class = 0 if 30 | % overlap in [bg_thresh_lo, bg_thresh_hi)) 31 | ip.addParamValue('bg_thresh_hi', 0.5, @isscalar); 32 | ip.addParamValue('bg_thresh_lo', 0.1, @isscalar); 33 | % mean image, in RGB order 34 | ip.addParamValue('image_means', 128, @ismatrix); 35 | % Use horizontally-flipped images during training? 36 | ip.addParamValue('use_flipped', true, @islogical); 37 | % Vaild training sample (IoU > bbox_thresh) for bounding box regresion 38 | ip.addParamValue('bbox_thresh', 0.5, @isscalar); 39 | 40 | % random seed 41 | ip.addParamValue('rng_seed', 6, @isscalar); 42 | 43 | 44 | %% testing 45 | ip.addParamValue('test_scales', 600, @isscalar); 46 | ip.addParamValue('test_max_size', 1000, @isscalar); 47 | ip.addParamValue('test_nms', 0.3, @isscalar); 48 | ip.addParamValue('test_binary', false, @islogical); 49 | 50 | ip.parse(varargin{:}); 51 | conf = ip.Results; 52 | 53 | % if image_means is a file, load it 54 | if ischar(conf.image_means) 55 | s = load(conf.image_means); 56 | s_fieldnames = fieldnames(s); 57 | assert(length(s_fieldnames) == 1); 58 | conf.image_means = s.(s_fieldnames{1}); 59 | end 60 | end -------------------------------------------------------------------------------- /isam/isam/Lcm.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file lcm.h 3 | * @brief LCM interface. 4 | * @author Michael Kaess 5 | * @version $Id: Lcm.h 4232 2011-04-02 20:18:18Z hordurj $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | #include 32 | 33 | #include 34 | 35 | #include 36 | #include 37 | 38 | class Lcm { 39 | lcm_t* lcm; 40 | public: 41 | /** 42 | * Default constructor, sets up lcm connection. 43 | */ 44 | Lcm(); 45 | 46 | ~Lcm(); 47 | 48 | void send_reset() const; 49 | 50 | /** 51 | * Sends a set of nodes (poses, landmarks...). 52 | * @param nodes Vector of nodes. 53 | * @param id Collection id (also determines color). 54 | * @param name Collection name. 55 | * @param type Type of object (pose, tree etc.) 56 | */ 57 | void send_nodes(const std::vector >& nodes, 58 | int id, 59 | char* name, 60 | int type) const; 61 | 62 | /** 63 | * Sends a set of links (measurements, odometry constraints...). 64 | * @param links Vector of links. 65 | * @param id Collection id (also determines color). 66 | * @param name Collection name. 67 | * @param collection1 Links start from elements of this collection id. 68 | * @param collection2 Links end at elements of this collection id. 69 | */ 70 | void send_links(const std::vector >& links, 71 | int id, char* name, int collection1, int collection2) const; 72 | 73 | /** 74 | * Sends a set of covariances (2D or 3D) 75 | * @param covariances Covariances matrices in same order as elements they refer to. 76 | * @param id Collection id (also determines color). 77 | * @param name Collection name. 78 | * @param collection Collection number that the covariances refer to. 79 | * @param is_3d True if 3D covariances. 80 | */ 81 | void send_covariances(const std::list& covariances, 82 | int id, char* name, int collection, bool is_3d) const; 83 | 84 | }; 85 | -------------------------------------------------------------------------------- /isam/.svn/pristine/14/1496b33cf0f04afbd9a107d81e397411db3aaab9.svn-base: -------------------------------------------------------------------------------- 1 | /** 2 | * @file numericalDiff.cpp 3 | * @brief Numerical differentiation. 4 | * @author Michael Kaess 5 | * @version $Id: numericalDiff.cpp 4038 2011-02-26 04:31:00Z kaess $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #include 29 | 30 | #include "isam/numericalDiff.h" 31 | 32 | #define SYMMETRIC 33 | 34 | const double epsilon = 0.0001; 35 | 36 | using namespace std; 37 | using namespace Eigen; 38 | 39 | namespace isam { 40 | 41 | MatrixXd numericalDiff(Function& func) { 42 | #ifndef SYMMETRIC 43 | VectorXd y0 = func.evaluate(); 44 | #endif 45 | // number of measurement rows 46 | int m = func.num_measurements(); 47 | // number of variables 48 | int n = 0; 49 | vector& nodes = func.nodes(); 50 | for (vector::iterator it = nodes.begin(); it!=nodes.end(); it++) { 51 | n += (*it)->dim(); 52 | } 53 | // result has one column per variable 54 | MatrixXd Jacobian(m,n); 55 | int col = 0; 56 | // for each node... 57 | for (vector::iterator it = nodes.begin(); it!=nodes.end(); it++) { 58 | Node* node = *it; 59 | int dim_n = node->dim(); 60 | // for each dimension of the node... 61 | for (int j=0; jvector0(); 66 | // evaluate positive delta 67 | delta(j) = epsilon; 68 | node->self_exmap(delta); 69 | VectorXd y_plus = func.evaluate(); 70 | node->update0(original); 71 | #ifdef SYMMETRIC 72 | // evaluate negative delta 73 | delta(j) = -epsilon; 74 | node->self_exmap(delta); 75 | VectorXd y_minus = func.evaluate(); 76 | node->update0(original); 77 | // store column 78 | VectorXd diff = (y_plus - y_minus) / (epsilon + epsilon); 79 | #else 80 | VectorXd diff = (y_plus - y0) / epsilon; 81 | #endif 82 | Jacobian.col(col) = diff; 83 | } 84 | } 85 | 86 | return Jacobian; 87 | } 88 | 89 | } 90 | -------------------------------------------------------------------------------- /isam/.svn/pristine/7e/7ec1c0aeeca81cba1985a51b967fe5c30d386fa7.svn-base: -------------------------------------------------------------------------------- 1 | /** 2 | * @file lcm.h 3 | * @brief LCM interface. 4 | * @author Michael Kaess 5 | * @version $Id: Lcm.h 4232 2011-04-02 20:18:18Z hordurj $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | #include 32 | 33 | #include 34 | 35 | #include 36 | #include 37 | 38 | class Lcm { 39 | lcm_t* lcm; 40 | public: 41 | /** 42 | * Default constructor, sets up lcm connection. 43 | */ 44 | Lcm(); 45 | 46 | ~Lcm(); 47 | 48 | void send_reset() const; 49 | 50 | /** 51 | * Sends a set of nodes (poses, landmarks...). 52 | * @param nodes Vector of nodes. 53 | * @param id Collection id (also determines color). 54 | * @param name Collection name. 55 | * @param type Type of object (pose, tree etc.) 56 | */ 57 | void send_nodes(const std::vector >& nodes, 58 | int id, 59 | char* name, 60 | int type) const; 61 | 62 | /** 63 | * Sends a set of links (measurements, odometry constraints...). 64 | * @param links Vector of links. 65 | * @param id Collection id (also determines color). 66 | * @param name Collection name. 67 | * @param collection1 Links start from elements of this collection id. 68 | * @param collection2 Links end at elements of this collection id. 69 | */ 70 | void send_links(const std::vector >& links, 71 | int id, char* name, int collection1, int collection2) const; 72 | 73 | /** 74 | * Sends a set of covariances (2D or 3D) 75 | * @param covariances Covariances matrices in same order as elements they refer to. 76 | * @param id Collection id (also determines color). 77 | * @param name Collection name. 78 | * @param collection Collection number that the covariances refer to. 79 | * @param is_3d True if 3D covariances. 80 | */ 81 | void send_covariances(const std::list& covariances, 82 | int id, char* name, int collection, bool is_3d) const; 83 | 84 | }; 85 | -------------------------------------------------------------------------------- /isam/isam/Collections.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Collections.h 3 | * @brief 3D visualization. 4 | * @author Michael Kaess 5 | * @version $Id: Collections.h 4232 2011-04-02 20:18:18Z hordurj $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | #include 36 | 37 | enum ViewerObjects { 38 | VIEWER_OBJ_POSE3D, 39 | VIEWER_OBJ_TREE, 40 | VIEWER_OBJ_POINT3D 41 | }; 42 | 43 | class Collection { 44 | public: 45 | int id; 46 | std::string name; 47 | int type; 48 | 49 | Collection(int id, std::string name, int type) : id(id), name(name), type(type) {} 50 | 51 | virtual ~Collection() {} 52 | virtual void draw() = 0; 53 | }; 54 | 55 | class ObjCollection : public Collection { 56 | typedef std::map, 59 | Eigen::aligned_allocator > objs_t; 60 | int maxid; 61 | objs_t objs; 62 | 63 | public: 64 | ObjCollection(int id, 65 | std::string name, 66 | int type, 67 | const std::vector >& nodes); 68 | virtual void draw(); 69 | 70 | friend class LinkCollection; 71 | friend class CovCollection; 72 | }; 73 | 74 | class LinkCollection : public Collection { 75 | typedef std::vector > elements_t; 76 | elements_t elements; 77 | int col1; 78 | int col2; 79 | 80 | public: 81 | LinkCollection(int id, std::string name, const std::vector >& links, int col1, int col2); 82 | virtual void draw(); 83 | }; 84 | 85 | class CovCollection : public Collection { 86 | typedef std::list covs_t; 87 | covs_t covs; 88 | int collection; 89 | bool is_3d; 90 | 91 | public: 92 | CovCollection(int id, std::string name, const std::list& covs, int collection, bool is_3d); 93 | virtual void draw(); 94 | }; 95 | 96 | typedef std::map collections_t; 97 | extern collections_t collections; 98 | -------------------------------------------------------------------------------- /isam/.svn/pristine/63/6361e1ba3ae5a636073644f0cc164aa5c935c8bb.svn-base: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Collections.h 3 | * @brief 3D visualization. 4 | * @author Michael Kaess 5 | * @version $Id: Collections.h 4232 2011-04-02 20:18:18Z hordurj $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | #include 36 | 37 | enum ViewerObjects { 38 | VIEWER_OBJ_POSE3D, 39 | VIEWER_OBJ_TREE, 40 | VIEWER_OBJ_POINT3D 41 | }; 42 | 43 | class Collection { 44 | public: 45 | int id; 46 | std::string name; 47 | int type; 48 | 49 | Collection(int id, std::string name, int type) : id(id), name(name), type(type) {} 50 | 51 | virtual ~Collection() {} 52 | virtual void draw() = 0; 53 | }; 54 | 55 | class ObjCollection : public Collection { 56 | typedef std::map, 59 | Eigen::aligned_allocator > objs_t; 60 | int maxid; 61 | objs_t objs; 62 | 63 | public: 64 | ObjCollection(int id, 65 | std::string name, 66 | int type, 67 | const std::vector >& nodes); 68 | virtual void draw(); 69 | 70 | friend class LinkCollection; 71 | friend class CovCollection; 72 | }; 73 | 74 | class LinkCollection : public Collection { 75 | typedef std::vector > elements_t; 76 | elements_t elements; 77 | int col1; 78 | int col2; 79 | 80 | public: 81 | LinkCollection(int id, std::string name, const std::vector >& links, int col1, int col2); 82 | virtual void draw(); 83 | }; 84 | 85 | class CovCollection : public Collection { 86 | typedef std::list covs_t; 87 | covs_t covs; 88 | int collection; 89 | bool is_3d; 90 | 91 | public: 92 | CovCollection(int id, std::string name, const std::list& covs, int collection, bool is_3d); 93 | virtual void draw(); 94 | }; 95 | 96 | typedef std::map collections_t; 97 | extern collections_t collections; 98 | -------------------------------------------------------------------------------- /isam/include/isam/Point3d.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Point3d.h 3 | * @brief Simple 3D point class. 4 | * @author Michael Kaess 5 | * @version $Id: Point3d.h 8263 2013-04-10 14:02:19Z carlevar $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | 32 | #include "Point2d.h" 33 | 34 | namespace isam { 35 | 36 | class Point3d { 37 | friend std::ostream& operator<<(std::ostream& out, const Point3d& p) { 38 | p.write(out); 39 | return out; 40 | } 41 | 42 | double _x; 43 | double _y; 44 | double _z; 45 | public: 46 | static const int dim = 3; 47 | static const int size = 3; 48 | static const char* name() { 49 | return "Point3d"; 50 | } 51 | Point3d() : _x(0.), _y(0.), _z(0.) {} 52 | Point3d(double x, double y, double z) : _x(x), _y(y), _z(z) {} 53 | Point3d(const Eigen::Vector3d& vec) : _x(vec(0)), _y(vec(1)), _z(vec(2)) {} 54 | 55 | double x() const {return _x;} 56 | double y() const {return _y;} 57 | double z() const {return _z;} 58 | 59 | void set_x(double x) {_x = x;} 60 | void set_y(double y) {_y = y;} 61 | void set_z(double z) {_z = z;} 62 | 63 | Point3d exmap(const Eigen::Vector3d& delta) const { 64 | Point3d res = *this; 65 | res._x += delta(0); 66 | res._y += delta(1); 67 | res._z += delta(2); 68 | return res; 69 | } 70 | 71 | Eigen::Vector3d vector() const { 72 | Eigen::Vector3d tmp(_x, _y, _z); 73 | return tmp; 74 | } 75 | void set(double x, double y, double z) { 76 | _x = x; 77 | _y = y; 78 | _z = z; 79 | } 80 | void set(const Eigen::Vector3d& v) { 81 | _x = v(0); 82 | _y = v(1); 83 | _z = v(2); 84 | } 85 | 86 | Eigen::VectorXb is_angle() const { 87 | return Eigen::VectorXb::Zero(size); 88 | } 89 | 90 | Point3d to_point3d() { 91 | return *this; 92 | } 93 | 94 | void of_point2d(const Point2d& p) { 95 | _x = p.x(); 96 | _y = p.y(); 97 | _z = 0.; 98 | } 99 | void write(std::ostream &out) const { 100 | out << "(" << _x << ", " << _y << ", " << _z << ")"; 101 | } 102 | }; 103 | 104 | } 105 | -------------------------------------------------------------------------------- /isam/include/isam/SparseSystem.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file SparseSystem.h 3 | * @brief Adds rhs functionality to sparse matrix for iSAM. 4 | * @author Michael Kaess 5 | * @version $Id: SparseSystem.h 4133 2011-03-22 20:40:38Z kaess $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | 32 | #include "OrderedSparseMatrix.h" 33 | 34 | namespace isam { 35 | 36 | class SparseSystem : public OrderedSparseMatrix { 37 | Eigen::VectorXd _rhs; 38 | public: 39 | SparseSystem(int num_rows, int num_cols); 40 | SparseSystem(const SparseSystem& mat); 41 | SparseSystem(const SparseSystem& mat, int num_rows, int num_cols, int first_row = 0, int first_col = 0); 42 | SparseSystem(int num_rows, int num_cols, SparseVector_p* rows, const Eigen::VectorXd& rhs); 43 | virtual ~SparseSystem(); 44 | const SparseSystem& operator= (const SparseSystem& mat); 45 | 46 | const Eigen::VectorXd& rhs() const {return _rhs;} 47 | void set_rhs(const Eigen::VectorXd& rhs) {_rhs = rhs;} 48 | 49 | // overridden functions 50 | 51 | /** 52 | * Note: While rows are passed in, the rhs is required to already 53 | * contain the new entry - necessary because we cannot change the 54 | * signature of the function. 55 | */ 56 | void apply_givens(int row, int col, double* c_givens = NULL, double* s_givens = NULL); 57 | 58 | void append_new_rows(int num); 59 | 60 | // new functions 61 | 62 | /** 63 | * Insert a new row 64 | * @param new_row The new sparse measurement row to add. 65 | * @param new_r New right hand side entry. 66 | */ 67 | virtual void add_row(const SparseVector& new_row, double new_r); 68 | 69 | /** 70 | * Insert a new measurement row and triangulate using Givens rotations 71 | * @param new_row The new sparse measurement row to add. 72 | * @param new_r New right hand side entry. 73 | * @return Number of Givens rotations applied (for analysis). 74 | */ 75 | virtual int add_row_givens(const SparseVector& new_row, double new_r); 76 | 77 | /** 78 | * Solve equation system by backsubstitution. 79 | * @return Solution for x in Rx=b' 80 | */ 81 | virtual Eigen::VectorXd solve() const; 82 | 83 | }; 84 | 85 | } 86 | -------------------------------------------------------------------------------- /isam/.svn/pristine/d4/d4529d03edb1b4b6ba2c5f7593b1591beaf360c7.svn-base: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Point3d.h 3 | * @brief Simple 3D point class. 4 | * @author Michael Kaess 5 | * @version $Id: Point3d.h 8263 2013-04-10 14:02:19Z carlevar $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | 32 | #include "Point2d.h" 33 | 34 | namespace isam { 35 | 36 | class Point3d { 37 | friend std::ostream& operator<<(std::ostream& out, const Point3d& p) { 38 | p.write(out); 39 | return out; 40 | } 41 | 42 | double _x; 43 | double _y; 44 | double _z; 45 | public: 46 | static const int dim = 3; 47 | static const int size = 3; 48 | static const char* name() { 49 | return "Point3d"; 50 | } 51 | Point3d() : _x(0.), _y(0.), _z(0.) {} 52 | Point3d(double x, double y, double z) : _x(x), _y(y), _z(z) {} 53 | Point3d(const Eigen::Vector3d& vec) : _x(vec(0)), _y(vec(1)), _z(vec(2)) {} 54 | 55 | double x() const {return _x;} 56 | double y() const {return _y;} 57 | double z() const {return _z;} 58 | 59 | void set_x(double x) {_x = x;} 60 | void set_y(double y) {_y = y;} 61 | void set_z(double z) {_z = z;} 62 | 63 | Point3d exmap(const Eigen::Vector3d& delta) const { 64 | Point3d res = *this; 65 | res._x += delta(0); 66 | res._y += delta(1); 67 | res._z += delta(2); 68 | return res; 69 | } 70 | 71 | Eigen::Vector3d vector() const { 72 | Eigen::Vector3d tmp(_x, _y, _z); 73 | return tmp; 74 | } 75 | void set(double x, double y, double z) { 76 | _x = x; 77 | _y = y; 78 | _z = z; 79 | } 80 | void set(const Eigen::Vector3d& v) { 81 | _x = v(0); 82 | _y = v(1); 83 | _z = v(2); 84 | } 85 | 86 | Eigen::VectorXb is_angle() const { 87 | return Eigen::VectorXb::Zero(size); 88 | } 89 | 90 | Point3d to_point3d() { 91 | return *this; 92 | } 93 | 94 | void of_point2d(const Point2d& p) { 95 | _x = p.x(); 96 | _y = p.y(); 97 | _z = 0.; 98 | } 99 | void write(std::ostream &out) const { 100 | out << "(" << _x << ", " << _y << ", " << _z << ")"; 101 | } 102 | }; 103 | 104 | } 105 | -------------------------------------------------------------------------------- /isam/isam/Viewer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Viewer.h 3 | * @brief 3D visualization. 4 | * @author Michael Kaess 5 | * @version $Id: Viewer.h 4232 2011-04-02 20:18:18Z hordurj $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | #include 32 | 33 | #include "Collections.h" 34 | 35 | class Viewer { 36 | 37 | public: 38 | 39 | /** 40 | * GLUT initialization, open window. 41 | */ 42 | void init(int (*process)(void*)); 43 | 44 | /** 45 | * Adds or overwrites a collection of nodes (poses, landmarks...). 46 | * @param nodes Vector of nodes. 47 | * @param id Collection id (also determines color). 48 | * @param name Collection name. 49 | * @param type Type of object (pose, tree etc.) 50 | */ 51 | void set_nodes(const std::vector >& nodes, 52 | int id, 53 | const std::string& name, 54 | int type); 55 | 56 | /** 57 | * Adds or overwrites a collection of links 58 | * (measurements, odometry constraints...). 59 | * @param links Vector of links. 60 | * @param id Collection id (also determines color). 61 | * @param name Collection name. 62 | * @param collection1 Links start from elements of this collection id. 63 | * @param collection2 Links end at elements of this collection id. 64 | */ 65 | void set_links(const std::vector >& links, 66 | int id, const std::string& name, int collection1, int collection2); 67 | 68 | /** 69 | * Adds or overwrites a collection of covariances (2D or 3D) 70 | * @param covariances Covariances matrices in same order as elements they refer to. 71 | * @param id Collection id (also determines color). 72 | * @param name Collection name. 73 | * @param collection Collection number that the covariances refer to. 74 | * @param is_3d True if 3D covariances. 75 | */ 76 | void set_covariances(const std::list& covariances, 77 | int id, char* name, int collection, bool is_3d) const; 78 | 79 | /** 80 | * Signals processing thread to quit upon user request through GUI. 81 | */ 82 | bool exit_requested(); 83 | 84 | }; 85 | -------------------------------------------------------------------------------- /isam/.svn/pristine/97/975c4482a96ef0f3f06aac33fda56cd35c16c0ae.svn-base: -------------------------------------------------------------------------------- 1 | /** 2 | * @file SparseSystem.h 3 | * @brief Adds rhs functionality to sparse matrix for iSAM. 4 | * @author Michael Kaess 5 | * @version $Id: SparseSystem.h 4133 2011-03-22 20:40:38Z kaess $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | 32 | #include "OrderedSparseMatrix.h" 33 | 34 | namespace isam { 35 | 36 | class SparseSystem : public OrderedSparseMatrix { 37 | Eigen::VectorXd _rhs; 38 | public: 39 | SparseSystem(int num_rows, int num_cols); 40 | SparseSystem(const SparseSystem& mat); 41 | SparseSystem(const SparseSystem& mat, int num_rows, int num_cols, int first_row = 0, int first_col = 0); 42 | SparseSystem(int num_rows, int num_cols, SparseVector_p* rows, const Eigen::VectorXd& rhs); 43 | virtual ~SparseSystem(); 44 | const SparseSystem& operator= (const SparseSystem& mat); 45 | 46 | const Eigen::VectorXd& rhs() const {return _rhs;} 47 | void set_rhs(const Eigen::VectorXd& rhs) {_rhs = rhs;} 48 | 49 | // overridden functions 50 | 51 | /** 52 | * Note: While rows are passed in, the rhs is required to already 53 | * contain the new entry - necessary because we cannot change the 54 | * signature of the function. 55 | */ 56 | void apply_givens(int row, int col, double* c_givens = NULL, double* s_givens = NULL); 57 | 58 | void append_new_rows(int num); 59 | 60 | // new functions 61 | 62 | /** 63 | * Insert a new row 64 | * @param new_row The new sparse measurement row to add. 65 | * @param new_r New right hand side entry. 66 | */ 67 | virtual void add_row(const SparseVector& new_row, double new_r); 68 | 69 | /** 70 | * Insert a new measurement row and triangulate using Givens rotations 71 | * @param new_row The new sparse measurement row to add. 72 | * @param new_r New right hand side entry. 73 | * @return Number of Givens rotations applied (for analysis). 74 | */ 75 | virtual int add_row_givens(const SparseVector& new_row, double new_r); 76 | 77 | /** 78 | * Solve equation system by backsubstitution. 79 | * @return Solution for x in Rx=b' 80 | */ 81 | virtual Eigen::VectorXd solve() const; 82 | 83 | }; 84 | 85 | } 86 | -------------------------------------------------------------------------------- /isam/.svn/pristine/3b/3b9a434f0b10c459c9ce5faa42a7a719c2d5896d.svn-base: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Viewer.h 3 | * @brief 3D visualization. 4 | * @author Michael Kaess 5 | * @version $Id: Viewer.h 4232 2011-04-02 20:18:18Z hordurj $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | #include 32 | 33 | #include "Collections.h" 34 | 35 | class Viewer { 36 | 37 | public: 38 | 39 | /** 40 | * GLUT initialization, open window. 41 | */ 42 | void init(int (*process)(void*)); 43 | 44 | /** 45 | * Adds or overwrites a collection of nodes (poses, landmarks...). 46 | * @param nodes Vector of nodes. 47 | * @param id Collection id (also determines color). 48 | * @param name Collection name. 49 | * @param type Type of object (pose, tree etc.) 50 | */ 51 | void set_nodes(const std::vector >& nodes, 52 | int id, 53 | const std::string& name, 54 | int type); 55 | 56 | /** 57 | * Adds or overwrites a collection of links 58 | * (measurements, odometry constraints...). 59 | * @param links Vector of links. 60 | * @param id Collection id (also determines color). 61 | * @param name Collection name. 62 | * @param collection1 Links start from elements of this collection id. 63 | * @param collection2 Links end at elements of this collection id. 64 | */ 65 | void set_links(const std::vector >& links, 66 | int id, const std::string& name, int collection1, int collection2); 67 | 68 | /** 69 | * Adds or overwrites a collection of covariances (2D or 3D) 70 | * @param covariances Covariances matrices in same order as elements they refer to. 71 | * @param id Collection id (also determines color). 72 | * @param name Collection name. 73 | * @param collection Collection number that the covariances refer to. 74 | * @param is_3d True if 3D covariances. 75 | */ 76 | void set_covariances(const std::list& covariances, 77 | int id, char* name, int collection, bool is_3d) const; 78 | 79 | /** 80 | * Signals processing thread to quit upon user request through GUI. 81 | */ 82 | bool exit_requested(); 83 | 84 | }; 85 | -------------------------------------------------------------------------------- /isam/.svn/pristine/a9/a90a366a2e0ae8d2b39a575c5a039623d615cf55.svn-base: -------------------------------------------------------------------------------- 1 | /** 2 | * @file addRemove.cpp 3 | * @brief Editing of pose graphs. 4 | * @author Michael Kaess 5 | * @version $Id: addRemove.cpp 6335 2012-03-22 23:13:52Z kaess $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #include 29 | #include 30 | 31 | using namespace std; 32 | using namespace isam; 33 | using namespace Eigen; 34 | 35 | // note that as the nodes have no IDs to print, it's not easy to know 36 | // which node is which; the user really has to keep track of that, or 37 | // the user could extend the Pose2d_Node class to for example contain 38 | // a string ID. 39 | void print_all(Slam& slam) { 40 | list ids = slam.get_nodes(); 41 | for (list::iterator iter = ids.begin(); iter != ids.end(); iter++) { 42 | Pose2d_Node* pose = dynamic_cast(*iter); 43 | cout << pose->value() << endl; 44 | } 45 | } 46 | 47 | int main() { 48 | // locally defined, as nodes and factors below are allocated on the 49 | // stack, and therefore will go out of scope after the end of this 50 | // function; note that you can (and typically have to) dynamically 51 | // allocate, but I used local variables here to make the example 52 | // simpler - see example.cpp for dynamic allocation. 53 | Slam slam; 54 | 55 | Noise noise = Covariance(0.01 * eye(3)); 56 | 57 | Pose2d prior(0., 0., 0.); 58 | Pose2d_Node x0; 59 | Pose2d_Node x1; 60 | Pose2d_Node x2; 61 | slam.add_node(&x0); 62 | Pose2d_Factor p_x0(&x0, prior, noise); 63 | slam.add_factor(&p_x0); 64 | Pose2d odo(1., 0., 0.); 65 | slam.add_node(&x1); 66 | Pose2d_Pose2d_Factor o_01(&x0, &x1, odo, noise); 67 | slam.add_factor(&o_01); 68 | slam.add_node(&x2); 69 | Pose2d_Pose2d_Factor o_12(&x1, &x2, odo, noise); 70 | slam.add_factor(&o_12); 71 | 72 | slam.batch_optimization(); 73 | 74 | Pose2d_Node a1; 75 | slam.add_node(&a1); // new node for x1 76 | Pose2d_Pose2d_Factor o_01_new(&x0, &a1, odo, noise); 77 | slam.add_factor(&o_01_new); 78 | Pose2d_Pose2d_Factor o_12_new(&a1, &x2, odo, noise); 79 | slam.add_factor(&o_12_new); 80 | slam.remove_node(&x1); 81 | 82 | slam.batch_optimization(); 83 | 84 | slam.print_stats(); 85 | slam.print_graph(); 86 | 87 | print_all(slam); 88 | 89 | return 0; 90 | } 91 | -------------------------------------------------------------------------------- /include/rpn/proposal_visual_debug.m: -------------------------------------------------------------------------------- 1 | function proposal_visual_debug(conf, image_roidb, input_blobs, bbox_means, bbox_stds, classes, scale_inds) 2 | % proposal_visual_debug(conf, image_roidb, input_blobs, bbox_means, bbox_stds, classes, scale_inds) 3 | % -------------------------------------------------------- 4 | % Faster R-CNN 5 | % Copyright (c) 2015, Shaoqing Ren 6 | % Licensed under The MIT License [see LICENSE for details] 7 | % -------------------------------------------------------- 8 | 9 | im_blob = input_blobs{1}; 10 | labels_blob = input_blobs{2}; 11 | label_weights_blob = input_blobs{3}; 12 | bbox_targets_blob = input_blobs{4}; 13 | bbox_loss_weights_blob = input_blobs{5}; 14 | 15 | % concat anchors as [channel, height, width], where channel is the fastest dimension. 16 | rois = proposal_locate_anchors(conf, image_roidb.im_size, conf.scales(scale_inds)); 17 | 18 | bbox_targets = double(permute(bbox_targets_blob, [3, 2, 1])); 19 | bbox_targets = reshape(bbox_targets, 4, [])'; 20 | bbox_targets = bsxfun(@times, bbox_targets, bbox_stds); 21 | bbox_targets = bsxfun(@plus, bbox_targets, bbox_means); 22 | 23 | labels_blob = double(permute(labels_blob, [3, 2, 1])); 24 | labels_blob = labels_blob(:); 25 | label_weights_blob = double(permute(label_weights_blob, [3, 2, 1])); 26 | label_weights_blob = label_weights_blob(:); 27 | pred_boxes = fast_rcnn_bbox_transform_inv(rois, bbox_targets); 28 | 29 | num_anchors = size(conf.anchors, 1); 30 | for i = 1:size(im_blob, 4) 31 | for j = 1:num_anchors 32 | im = im_blob(:, :, [3, 2, 1], i); 33 | im = permute(im, [2, 1, 3]); 34 | imshow(mat2gray(im)); 35 | hold on; 36 | 37 | sub_rois = rois(j:num_anchors:end, :); 38 | sub_labels = labels_blob(j:num_anchors:end); 39 | sub_label_weights = label_weights_blob(j:num_anchors:end); 40 | sub_pred_boxes = pred_boxes(j:num_anchors:end, :); 41 | 42 | % bg 43 | bg_ind = find(sub_labels == 0 & sub_label_weights > 0); 44 | if ~isempty(bg_ind) 45 | cellfun(@(x) rectangle('Position', RectLTRB2LTWH(x), 'EdgeColor', 'k'), ... 46 | num2cell(sub_rois(bg_ind, :), 2)); 47 | cellfun(@(x) rectangle('Position', RectLTRB2LTWH(x), 'EdgeColor', 'b'), ... 48 | num2cell(sub_rois(bg_ind(round(length(bg_ind)/2)), :), 2)); 49 | end 50 | 51 | % fg 52 | fg_ind = sub_labels > 0; 53 | cellfun(@(x) rectangle('Position', RectLTRB2LTWH(x), 'EdgeColor', 'r'), ... 54 | num2cell(sub_rois(fg_ind, :), 2)); 55 | cellfun(@(x) rectangle('Position', RectLTRB2LTWH(x), 'EdgeColor', 'g'), ... 56 | num2cell(sub_pred_boxes(fg_ind, :), 2)); 57 | 58 | % % others 59 | % others_ind = find(sub_labels == 0 & sub_label_weights == 0); 60 | % cellfun(@(x) rectangle('Position', RectLTRB2LTWH(x), 'EdgeColor', [0.5, 0.5, 0.5]), ... 61 | % num2cell(sub_rois(others_ind, :), 2)); 62 | 63 | hold off; 64 | pause; 65 | end 66 | end 67 | end -------------------------------------------------------------------------------- /isam/cmake/FindEigen3.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Eigen3 lib 2 | # 3 | # This module supports requiring a minimum version, e.g. you can do 4 | # find_package(Eigen3 3.1.2) 5 | # to require version 3.1.2 or newer of Eigen3. 6 | # 7 | # Once done this will define 8 | # 9 | # EIGEN3_FOUND - system has eigen lib with correct version 10 | # EIGEN3_INCLUDE_DIR - the eigen include directory 11 | # EIGEN3_VERSION - eigen version 12 | 13 | # Copyright (c) 2006, 2007 Montel Laurent, 14 | # Copyright (c) 2008, 2009 Gael Guennebaud, 15 | # Copyright (c) 2009 Benoit Jacob 16 | # Redistribution and use is allowed according to the terms of the 2-clause BSD license. 17 | 18 | if(NOT Eigen3_FIND_VERSION) 19 | if(NOT Eigen3_FIND_VERSION_MAJOR) 20 | set(Eigen3_FIND_VERSION_MAJOR 2) 21 | endif(NOT Eigen3_FIND_VERSION_MAJOR) 22 | if(NOT Eigen3_FIND_VERSION_MINOR) 23 | set(Eigen3_FIND_VERSION_MINOR 91) 24 | endif(NOT Eigen3_FIND_VERSION_MINOR) 25 | if(NOT Eigen3_FIND_VERSION_PATCH) 26 | set(Eigen3_FIND_VERSION_PATCH 0) 27 | endif(NOT Eigen3_FIND_VERSION_PATCH) 28 | 29 | set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") 30 | endif(NOT Eigen3_FIND_VERSION) 31 | 32 | macro(_eigen3_check_version) 33 | file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) 34 | 35 | string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") 36 | set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") 37 | string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") 38 | set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") 39 | string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") 40 | set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") 41 | 42 | set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) 43 | if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 44 | set(EIGEN3_VERSION_OK FALSE) 45 | else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 46 | set(EIGEN3_VERSION_OK TRUE) 47 | endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 48 | 49 | if(NOT EIGEN3_VERSION_OK) 50 | 51 | message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " 52 | "but at least version ${Eigen3_FIND_VERSION} is required") 53 | endif(NOT EIGEN3_VERSION_OK) 54 | endmacro(_eigen3_check_version) 55 | 56 | if (EIGEN3_INCLUDE_DIR) 57 | 58 | # in cache already 59 | _eigen3_check_version() 60 | set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) 61 | 62 | else (EIGEN3_INCLUDE_DIR) 63 | 64 | find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library 65 | PATHS 66 | ${CMAKE_INSTALL_PREFIX}/include 67 | ${KDE4_INCLUDE_DIR} 68 | PATH_SUFFIXES eigen3 eigen 69 | ) 70 | 71 | if(EIGEN3_INCLUDE_DIR) 72 | _eigen3_check_version() 73 | endif(EIGEN3_INCLUDE_DIR) 74 | 75 | include(FindPackageHandleStandardArgs) 76 | find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) 77 | 78 | mark_as_advanced(EIGEN3_INCLUDE_DIR) 79 | 80 | endif(EIGEN3_INCLUDE_DIR) 81 | 82 | -------------------------------------------------------------------------------- /isam/.svn/pristine/cb/cb8ad72f9a74e9f214f0d5dd58af78dec68664b5.svn-base: -------------------------------------------------------------------------------- 1 | # - Try to find Eigen3 lib 2 | # 3 | # This module supports requiring a minimum version, e.g. you can do 4 | # find_package(Eigen3 3.1.2) 5 | # to require version 3.1.2 or newer of Eigen3. 6 | # 7 | # Once done this will define 8 | # 9 | # EIGEN3_FOUND - system has eigen lib with correct version 10 | # EIGEN3_INCLUDE_DIR - the eigen include directory 11 | # EIGEN3_VERSION - eigen version 12 | 13 | # Copyright (c) 2006, 2007 Montel Laurent, 14 | # Copyright (c) 2008, 2009 Gael Guennebaud, 15 | # Copyright (c) 2009 Benoit Jacob 16 | # Redistribution and use is allowed according to the terms of the 2-clause BSD license. 17 | 18 | if(NOT Eigen3_FIND_VERSION) 19 | if(NOT Eigen3_FIND_VERSION_MAJOR) 20 | set(Eigen3_FIND_VERSION_MAJOR 2) 21 | endif(NOT Eigen3_FIND_VERSION_MAJOR) 22 | if(NOT Eigen3_FIND_VERSION_MINOR) 23 | set(Eigen3_FIND_VERSION_MINOR 91) 24 | endif(NOT Eigen3_FIND_VERSION_MINOR) 25 | if(NOT Eigen3_FIND_VERSION_PATCH) 26 | set(Eigen3_FIND_VERSION_PATCH 0) 27 | endif(NOT Eigen3_FIND_VERSION_PATCH) 28 | 29 | set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") 30 | endif(NOT Eigen3_FIND_VERSION) 31 | 32 | macro(_eigen3_check_version) 33 | file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) 34 | 35 | string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") 36 | set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") 37 | string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") 38 | set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") 39 | string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") 40 | set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") 41 | 42 | set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) 43 | if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 44 | set(EIGEN3_VERSION_OK FALSE) 45 | else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 46 | set(EIGEN3_VERSION_OK TRUE) 47 | endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 48 | 49 | if(NOT EIGEN3_VERSION_OK) 50 | 51 | message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " 52 | "but at least version ${Eigen3_FIND_VERSION} is required") 53 | endif(NOT EIGEN3_VERSION_OK) 54 | endmacro(_eigen3_check_version) 55 | 56 | if (EIGEN3_INCLUDE_DIR) 57 | 58 | # in cache already 59 | _eigen3_check_version() 60 | set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) 61 | 62 | else (EIGEN3_INCLUDE_DIR) 63 | 64 | find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library 65 | PATHS 66 | ${CMAKE_INSTALL_PREFIX}/include 67 | ${KDE4_INCLUDE_DIR} 68 | PATH_SUFFIXES eigen3 eigen 69 | ) 70 | 71 | if(EIGEN3_INCLUDE_DIR) 72 | _eigen3_check_version() 73 | endif(EIGEN3_INCLUDE_DIR) 74 | 75 | include(FindPackageHandleStandardArgs) 76 | find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) 77 | 78 | mark_as_advanced(EIGEN3_INCLUDE_DIR) 79 | 80 | endif(EIGEN3_INCLUDE_DIR) 81 | 82 | -------------------------------------------------------------------------------- /include/nms/nms_mex.cpp: -------------------------------------------------------------------------------- 1 | #include "mex.h" 2 | #ifdef _MSC_VER 3 | #include 4 | #include 5 | #endif 6 | #include 7 | #include 8 | using namespace std; 9 | 10 | struct score { 11 | double s; 12 | int idx; 13 | bool operator() (score i, score j) { return (i.idx < j.idx);} 14 | } score; 15 | 16 | template 17 | void nms(const mxArray *input_boxes, double overlap, vector &vPick, int &nPick) 18 | { 19 | int nSample = (int)mxGetM(input_boxes); 20 | int nDim_boxes = (int)mxGetN(input_boxes); 21 | 22 | T *pBoxes = (T*)mxGetData(input_boxes); 23 | 24 | vector vArea(nSample); 25 | for (int i = 0; i < nSample; ++i) 26 | { 27 | vArea[i] = double(pBoxes[2*nSample + i] - pBoxes[0*nSample + i] + 1) 28 | * (pBoxes[3*nSample + i] - pBoxes[1*nSample + i] + 1); 29 | if (vArea[i] < 0) 30 | mexErrMsgTxt("Boxes area must >= 0"); 31 | } 32 | 33 | std::multimap scores; 34 | for (int i = 0; i < nSample; ++i) 35 | scores.insert(std::pair(pBoxes[4*nSample + i], i)); 36 | 37 | nPick = 0; 38 | 39 | do 40 | { 41 | int last = scores.rbegin()->second; 42 | vPick[nPick] = last; 43 | nPick += 1; 44 | 45 | for (typename std::multimap::iterator it = scores.begin(); it != scores.end();) 46 | { 47 | int it_idx = it->second; 48 | T xx1 = max(pBoxes[0*nSample + last], pBoxes[0*nSample + it_idx]); 49 | T yy1 = max(pBoxes[1*nSample + last], pBoxes[1*nSample + it_idx]); 50 | T xx2 = min(pBoxes[2*nSample + last], pBoxes[2*nSample + it_idx]); 51 | T yy2 = min(pBoxes[3*nSample + last], pBoxes[3*nSample + it_idx]); 52 | 53 | double w = max(T(0.0), xx2-xx1+1), h = max(T(0.0), yy2-yy1+1); 54 | 55 | double ov = w*h / (vArea[last] + vArea[it_idx] - w*h); 56 | 57 | if (ov > overlap) 58 | { 59 | it = scores.erase(it); 60 | } 61 | else 62 | { 63 | it++; 64 | } 65 | } 66 | 67 | } while (scores.size() != 0); 68 | } 69 | 70 | 71 | void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) 72 | { 73 | if (nrhs != 2) 74 | mexErrMsgTxt("Wrong number of inputs"); 75 | if (nlhs != 1) 76 | mexErrMsgTxt("One output"); 77 | 78 | const mxArray *input_boxes = prhs[0]; 79 | if (mxGetClassID(input_boxes) != mxDOUBLE_CLASS && mxGetClassID(input_boxes) != mxSINGLE_CLASS) 80 | mexErrMsgTxt("Input boxes must be Double or Single"); 81 | 82 | const mxArray *input_overlap = prhs[1]; 83 | if (mxGetClassID(input_overlap) != mxDOUBLE_CLASS ) 84 | mexErrMsgTxt("Input overlap must be Double"); 85 | 86 | double overlap = mxGetScalar(input_overlap); 87 | 88 | int nSample = (int)mxGetM(input_boxes); 89 | int nDim_boxes = (int)mxGetN(input_boxes); 90 | 91 | if (nSample * nDim_boxes == 0) 92 | { 93 | plhs[0] = mxCreateNumericMatrix(0, 0, mxDOUBLE_CLASS, mxREAL); 94 | return; 95 | } 96 | 97 | if (nDim_boxes != 5) 98 | mexErrMsgTxt("nms_mex boxes must has 5 columns"); 99 | 100 | 101 | int nPick = 0; 102 | vector vPick(nSample); 103 | if(mxGetClassID(input_boxes) == mxDOUBLE_CLASS) 104 | nms(input_boxes, overlap, vPick, nPick); 105 | else 106 | nms(input_boxes, overlap, vPick, nPick); 107 | 108 | plhs[0] = mxCreateNumericMatrix(nPick, 1, mxDOUBLE_CLASS, mxREAL); 109 | double *pRst = mxGetPr(plhs[0]); 110 | for (int i = 0; i < nPick; ++i) 111 | pRst[i] = vPick[i] + 1; 112 | } 113 | -------------------------------------------------------------------------------- /isam/ChangeLog: -------------------------------------------------------------------------------- 1 | 2 | iSAM 1.7 (Jul 2, 2013) 3 | - Generic Linear Constraint (GLC) added to support sparsification based 4 | on the paper "Generic Factor-Based Node Marginalization and Edge 5 | Sparsification for Pose-Graph SLAM" by Nicholas Carleavaris-Bianco and 6 | Ryan Eustice, ICRA 2013. See examples/glc.cpp 7 | - Relative parameterization for homogeneous points added (McDonald et al., 8 | ECMR 11 and RAS 13) 9 | - Fixed compile problem in slam_monocular.h 10 | - Bug fix in non-relative formulation of slam_stereo.h 11 | - Numerically more stable quaternion conversion 12 | - Changed stack allocations of SparseVector to heap allocations to allow 13 | for very large vectors 14 | - Added FindCholmod.cmake as a more portable way of finding CHOLMOD 15 | - Added option for marking nodes as deleted. The marked nodes and factors 16 | are then deleted during update. This is more efficient then individually 17 | removing nodes and factors 18 | - Added support for multiple parallel instances of covariance recovery 19 | 20 | iSAM 1.6 (Mar 30, 2012) 21 | - Factors now take a new Noise class instead of the square root 22 | information matrix; existing calls are converted by wrapping the 23 | matrix with the SqrtInf() constructor; additional options are 24 | Covariance() and Information(), which internally convert to 25 | square root information matrix 26 | - added Powell's dog leg algorithm for batch (superior to 27 | Levenberg-Marquardt), and our novel incremental dog leg 28 | algorithm (see D. Rosen et al., ICRA 2012) 29 | - better stopping criteria for batch Gauss-Newton (see 30 | epsilon_abs and epsilon_rel in Properties) 31 | - added Levenberg-Marquardt option for batch optimization 32 | - Covariance access now through Slam.covariances; cloning allows 33 | running covariances recovery in a separate thread 34 | - exponential map for homogeneous points in stereo, based on Quaternions 35 | - factors for stereo vision added (see J. McDonald et al., ECMR 2011) 36 | - exponential map made explicit, was only implicitly used in the context 37 | of Euler angles so far; removed access_vector construct 38 | - rotations internally replaced by Quaternions for improved optimization 39 | - require() bug fixed, which removed all tests for Release mode; 40 | now split into require() (always active) and requireDebug() (only 41 | active in Debug mode, for time critical applications in inner loops) 42 | - simplified usage of anchor nodes, see examples/achorNodes 43 | - header file dependencies fixed (now using CMake's include_directories) 44 | - bug fix in covariance recovery (some off-diagonal entries were incorrectly 45 | set to 0) 46 | - added support for accessing arbitrary covariance entries, not just 47 | marginal covariances (see Slam.access_covariance() and 48 | examples/covariance.cpp) 49 | - replaced Vector/Matrix classes by third party Eigen3 library (faster, and 50 | provides full functionality, while Vector/Matrix only provided the 51 | functions actively being used in iSAM) 52 | 53 | iSAM 1.5 (Nov 12, 2010) 54 | - code optimization, reducing execution time for the Manhattan sequence by half 55 | - added optional CSparse-based batch factorization for comparison with CHOLMOD 56 | - changed default properties: 57 | + solve in every step (instead of every 10) 58 | + max_iterations for batch_optimization 20 (instead of 0 = unlimited) 59 | - added toggle (key 'c') to flip background color of 3D viewer 60 | 61 | iSAM 1.4 (Aug 29, 2010) 62 | - initial public release 63 | -------------------------------------------------------------------------------- /isam/.svn/pristine/21/2162b5eaef182f15ecab2f42f91feaf3a1926f20.svn-base: -------------------------------------------------------------------------------- 1 | /** 2 | * @file monocular.cpp 3 | * @brief Example of using monocular factors. 4 | * @author Michael Kaess 5 | * @version $Id: monocular.cpp 9315 2013-11-18 20:23:34Z kaess $ 6 | */ 7 | 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | using namespace std; 16 | using namespace isam; 17 | using namespace Eigen; 18 | 19 | // monocular camera parameters 20 | const double f = 360; // focal length in pixels 21 | const double u0 = 240; // principal point in pixels 22 | const double v0 = 120; 23 | 24 | double robust_cost_function(double d) { 25 | return cost_pseudo_huber(d, .5); 26 | } 27 | 28 | void simple_monocular() { 29 | 30 | Vector2d pp(u0, v0); 31 | MonocularCamera camera(f, pp); 32 | 33 | Pose3d origin; 34 | Point3d p0(5.,1.,2.); 35 | 36 | Slam slam; 37 | 38 | // first monocular camera 39 | Pose3d_Node* pose0 = new Pose3d_Node(); 40 | slam.add_node(pose0); 41 | 42 | // create a prior on the camera position 43 | Noise noise6 = Information(100. * eye(6)); 44 | Pose3d_Factor* prior = new Pose3d_Factor(pose0, origin, noise6); 45 | slam.add_factor(prior); 46 | 47 | // second monocular camera 48 | Pose3d_Node* pose1 = new Pose3d_Node(); 49 | slam.add_node(pose1); 50 | 51 | // add odometry measurement 52 | Pose3d delta(1.,0,0, 0,0,0); // move one meter forward (noise-free measurement...) 53 | Pose3d_Pose3d_Factor* odo = new Pose3d_Pose3d_Factor(pose0, pose1, delta, noise6); 54 | slam.add_factor(odo); 55 | 56 | // add some monocular measurements 57 | Point3d_Node* point = new Point3d_Node(); 58 | slam.add_node(point); 59 | Noise noise3 = Information(eye(3)); 60 | // first monocular camera projection 61 | MonocularMeasurement measurement0 = camera.project(origin, p0); 62 | cout << "Projection in first camera:" << endl; 63 | cout << measurement0 << endl; 64 | measurement0.u += 0.5; // add some "noise" 65 | measurement0.v -= 0.2; 66 | Monocular_Factor* factor1 = new Monocular_Factor(pose0, point, &camera, measurement0, noise3); 67 | slam.add_factor(factor1); 68 | // second monocular camera projection 69 | MonocularMeasurement measurement1 = camera.project(delta, p0); 70 | measurement1.u -= 0.3; // add some "noise" 71 | measurement1.v += 0.7; 72 | Monocular_Factor* factor2 = new Monocular_Factor(pose1, point, &camera, measurement1, noise3); 73 | slam.add_factor(factor2); 74 | 75 | cout << "Before optimization:" << endl; 76 | cout << setprecision(3) << setiosflags(ios::fixed); 77 | cout << point->value() << endl; 78 | cout << pose0->value() << endl; 79 | cout << pose1->value() << endl; 80 | 81 | // not needed here, but for more complex examples use Powell's dog leg and 82 | // optionally a robust estimator (pseudo Huber) to deal with occasional outliers 83 | Properties prop = slam.properties(); 84 | prop.method = DOG_LEG; 85 | slam.set_properties(prop); 86 | // slam.set_cost_function(robust_cost_function); 87 | 88 | // optimize 89 | slam.batch_optimization(); 90 | 91 | cout << "After optimization:" << endl; 92 | cout << point->value() << endl; 93 | cout << pose0->value() << endl; 94 | cout << pose1->value() << endl; 95 | } 96 | 97 | int main() { 98 | 99 | simple_monocular(); 100 | 101 | return 0; 102 | } 103 | -------------------------------------------------------------------------------- /isam/.svn/pristine/a1/a12e19a7835e4130688003fff879b0ecef9da200.svn-base: -------------------------------------------------------------------------------- 1 | 2 | iSAM 1.7 (Jul 2, 2013) 3 | - Generic Linear Constraint (GLC) added to support sparsification based 4 | on the paper "Generic Factor-Based Node Marginalization and Edge 5 | Sparsification for Pose-Graph SLAM" by Nicholas Carleavaris-Bianco and 6 | Ryan Eustice, ICRA 2013. See examples/glc.cpp 7 | - Relative parameterization for homogeneous points added (McDonald et al., 8 | ECMR 11 and RAS 13) 9 | - Fixed compile problem in slam_monocular.h 10 | - Bug fix in non-relative formulation of slam_stereo.h 11 | - Numerically more stable quaternion conversion 12 | - Changed stack allocations of SparseVector to heap allocations to allow 13 | for very large vectors 14 | - Added FindCholmod.cmake as a more portable way of finding CHOLMOD 15 | - Added option for marking nodes as deleted. The marked nodes and factors 16 | are then deleted during update. This is more efficient then individually 17 | removing nodes and factors 18 | - Added support for multiple parallel instances of covariance recovery 19 | 20 | iSAM 1.6 (Mar 30, 2012) 21 | - Factors now take a new Noise class instead of the square root 22 | information matrix; existing calls are converted by wrapping the 23 | matrix with the SqrtInf() constructor; additional options are 24 | Covariance() and Information(), which internally convert to 25 | square root information matrix 26 | - added Powell's dog leg algorithm for batch (superior to 27 | Levenberg-Marquardt), and our novel incremental dog leg 28 | algorithm (see D. Rosen et al., ICRA 2012) 29 | - better stopping criteria for batch Gauss-Newton (see 30 | epsilon_abs and epsilon_rel in Properties) 31 | - added Levenberg-Marquardt option for batch optimization 32 | - Covariance access now through Slam.covariances; cloning allows 33 | running covariances recovery in a separate thread 34 | - exponential map for homogeneous points in stereo, based on Quaternions 35 | - factors for stereo vision added (see J. McDonald et al., ECMR 2011) 36 | - exponential map made explicit, was only implicitly used in the context 37 | of Euler angles so far; removed access_vector construct 38 | - rotations internally replaced by Quaternions for improved optimization 39 | - require() bug fixed, which removed all tests for Release mode; 40 | now split into require() (always active) and requireDebug() (only 41 | active in Debug mode, for time critical applications in inner loops) 42 | - simplified usage of anchor nodes, see examples/achorNodes 43 | - header file dependencies fixed (now using CMake's include_directories) 44 | - bug fix in covariance recovery (some off-diagonal entries were incorrectly 45 | set to 0) 46 | - added support for accessing arbitrary covariance entries, not just 47 | marginal covariances (see Slam.access_covariance() and 48 | examples/covariance.cpp) 49 | - replaced Vector/Matrix classes by third party Eigen3 library (faster, and 50 | provides full functionality, while Vector/Matrix only provided the 51 | functions actively being used in iSAM) 52 | 53 | iSAM 1.5 (Nov 12, 2010) 54 | - code optimization, reducing execution time for the Manhattan sequence by half 55 | - added optional CSparse-based batch factorization for comparison with CHOLMOD 56 | - changed default properties: 57 | + solve in every step (instead of every 10) 58 | + max_iterations for batch_optimization 20 (instead of 0 = unlimited) 59 | - added toggle (key 'c') to flip background color of 3D viewer 60 | 61 | iSAM 1.4 (Aug 29, 2010) 62 | - initial public release 63 | -------------------------------------------------------------------------------- /isam/.svn/pristine/52/522b6a3e126ac6b78ed7d377e61230397a64303f.svn-base: -------------------------------------------------------------------------------- 1 | /** 2 | * @file stereo.cpp 3 | * @brief Example of using stereo factors. 4 | * @author Michael Kaess 5 | * @version $Id: stereoTest.cpp 6335 2012-03-22 23:13:52Z kaess $ 6 | */ 7 | 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | using namespace std; 16 | using namespace isam; 17 | using namespace Eigen; 18 | 19 | // stereo camera parameters 20 | const double f = 360; // focal length in pixels 21 | const double u0 = 240; // principal point in pixels 22 | const double v0 = 120; 23 | const double b = 0.12; // baseline in meters 24 | 25 | double robust_cost_function(double d) { 26 | return cost_pseudo_huber(d, .5); 27 | } 28 | 29 | void simple_stereo() { 30 | 31 | Vector2d pp(u0, v0); 32 | StereoCamera camera(f, pp, b); 33 | 34 | Pose3d origin; 35 | Point3d p0(5.,1.,2.); 36 | 37 | Slam slam; 38 | 39 | // first stereo camera 40 | Pose3d_Node* pose0 = new Pose3d_Node(); 41 | slam.add_node(pose0); 42 | 43 | // create a prior on the camera position 44 | Noise noise6 = Information(100. * eye(6)); 45 | Pose3d_Factor* prior = new Pose3d_Factor(pose0, origin, noise6); 46 | slam.add_factor(prior); 47 | 48 | // second stereo camera 49 | Pose3d_Node* pose1 = new Pose3d_Node(); 50 | slam.add_node(pose1); 51 | 52 | // add odometry measurement 53 | Pose3d delta(1.,0,0, 0,0,0); // move one meter forward (noise-free measurement...) 54 | Pose3d_Pose3d_Factor* odo = new Pose3d_Pose3d_Factor(pose0, pose1, delta, noise6); 55 | slam.add_factor(odo); 56 | 57 | // add some stereo measurements 58 | Point3d_Node* point = new Point3d_Node(); 59 | slam.add_node(point); 60 | Noise noise3 = Information(eye(3)); 61 | // first stereo camera projection 62 | StereoMeasurement measurement0 = camera.project(origin, p0); 63 | cout << "Projection in first camera:" << endl; 64 | cout << measurement0.u << " " << measurement0.v << " " << measurement0.u2 << endl; 65 | measurement0.u += 0.5; // add some "noise" 66 | measurement0.v -= 0.2; 67 | Stereo_Factor* factor1 = new Stereo_Factor(pose0, point, &camera, measurement0, noise3); 68 | slam.add_factor(factor1); 69 | // second stereo camera projection 70 | StereoMeasurement measurement1 = camera.project(delta, p0); 71 | measurement1.u -= 0.3; // add some "noise" 72 | measurement1.v += 0.7; 73 | Stereo_Factor* factor2 = new Stereo_Factor(pose1, point, &camera, measurement1, noise3); 74 | slam.add_factor(factor2); 75 | 76 | cout << "Before optimization:" << endl; 77 | cout << setprecision(3) << setiosflags(ios::fixed); 78 | cout << point->value() << endl; 79 | cout << pose0->value() << endl; 80 | cout << pose1->value() << endl; 81 | 82 | // not needed here, but for more complex examples use Powell's dog leg and 83 | // optionally a robust estimator (pseudo Huber) to deal with occasional outliers 84 | Properties prop = slam.properties(); 85 | prop.method = DOG_LEG; 86 | slam.set_properties(prop); 87 | // slam.set_cost_function(robust_cost_function); 88 | 89 | // optimize 90 | slam.batch_optimization(); 91 | 92 | cout << "After optimization:" << endl; 93 | cout << point->value() << endl; 94 | cout << pose0->value() << endl; 95 | cout << pose1->value() << endl; 96 | } 97 | 98 | int main() { 99 | 100 | simple_stereo(); 101 | 102 | return 0; 103 | } 104 | -------------------------------------------------------------------------------- /isam/include/isam/GLCReparam.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file GLCReparam.h 3 | * @brief Generic Linear Constraint Reparametrization 4 | * @author Nicholas Carlevaris-Bianco 5 | * @author Michael Kaess 6 | * @version $Id: GLCReparam.h 8578 2013-07-01 00:28:49Z kaess $ 7 | * 8 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 9 | * Michael Kaess, Hordur Johannsson, David Rosen, 10 | * Nicholas Carlevaris-Bianco and John. J. Leonard 11 | * 12 | * This file is part of iSAM. 13 | * 14 | * iSAM is free software; you can redistribute it and/or modify it under 15 | * the terms of the GNU Lesser General Public License as published by the 16 | * Free Software Foundation; either version 2.1 of the License, or (at 17 | * your option) any later version. 18 | * 19 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 20 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 21 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 22 | * License for more details. 23 | * 24 | * You should have received a copy of the GNU Lesser General Public License 25 | * along with iSAM. If not, see . 26 | * 27 | */ 28 | 29 | #pragma once 30 | 31 | #include 32 | #include 33 | #include 34 | 35 | #include "Node.h" 36 | #include "Factor.h" 37 | #include "Anchor.h" 38 | 39 | namespace isam { 40 | 41 | /** 42 | * interface class to allow for reparametrization by the user 43 | * see GLC_RootShift implementation as an example 44 | */ 45 | class GLC_Reparam { 46 | 47 | public: 48 | 49 | /** 50 | * set_nodes() 51 | * @param nodes Vector of Node pointers which support the factor 52 | */ 53 | virtual void set_nodes (std::vector nodes) = 0; 54 | /** 55 | * clone() 56 | * @return a new instance of the derived class 57 | */ 58 | virtual GLC_Reparam* clone() = 0; 59 | /** 60 | * is_angle() 61 | * @return boolean vector indacating if elemetes reparameterized vector are angles 62 | */ 63 | virtual Eigen::VectorXb is_angle() = 0; 64 | /** 65 | * jacobian() 66 | * @return jacobian of reparameterized output wrt node input 67 | */ 68 | virtual Eigen::MatrixXd jacobian() = 0; 69 | /** 70 | * reparameterize() 71 | * @return reparameterized vector 72 | */ 73 | virtual Eigen::VectorXd reparameterize (Selector s) = 0; 74 | 75 | }; 76 | 77 | /** 78 | * GLC_RootShift 79 | * implementation of GLC_Reparam 80 | * performs root shift operation 81 | */ 82 | class GLC_RootShift : public GLC_Reparam { 83 | 84 | private: 85 | std::vector _nodes; 86 | Eigen::VectorXb _is_angle; 87 | int _dim; // input and output dim 88 | 89 | public: 90 | 91 | void set_nodes (std::vector nodes) { 92 | _nodes = nodes; 93 | _dim = 0; 94 | for (size_t i=0; i<_nodes.size(); i++) { 95 | _dim += _nodes[i]->dim(); 96 | } 97 | _is_angle.resize(_dim); 98 | int ioff = 0; 99 | for (size_t i=0; i<_nodes.size(); i++) { 100 | _is_angle.segment(ioff, _nodes[i]->dim()) = _nodes[i]->is_angle(); 101 | ioff += _nodes[i]->dim(); 102 | } 103 | } 104 | 105 | GLC_RootShift* clone() {return new GLC_RootShift(*this);} 106 | Eigen::VectorXb is_angle() {return _is_angle;} 107 | Eigen::MatrixXd jacobian(); 108 | Eigen::VectorXd reparameterize (Selector s); 109 | Eigen::VectorXd root_shift (Node* node_i, Node* node_j, Selector s); 110 | 111 | }; 112 | 113 | } // namespace isam 114 | -------------------------------------------------------------------------------- /isam/isamlib/util.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file util.cpp 3 | * @brief Basic utility functions that are independent of iSAM. 4 | * @author Michael Kaess 5 | * @version $Id: util.cpp 6335 2012-03-22 23:13:52Z kaess $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #include 29 | #include 30 | #include 31 | #include // abs 32 | 33 | #include "isam/util.h" 34 | 35 | using namespace std; 36 | 37 | namespace isam { 38 | 39 | // simple class for accumulating execution timing information by name 40 | class Timing { 41 | class Stats { 42 | public: 43 | double t0; 44 | double t; 45 | double t_max; 46 | double t_min; 47 | int n; 48 | }; 49 | map stats; 50 | public: 51 | void add_t0(string id, double t0) { 52 | stats[id].t0 = t0; 53 | } 54 | double get_t0(string id) { 55 | return stats[id].t0; 56 | } 57 | void add_dt(string id, double dt) { 58 | Stats& s = stats[id]; 59 | s.t += dt; 60 | s.n++; 61 | if (s.n==1 || s.t_max < dt) s.t_max = dt; 62 | if (s.n==1 || s.t_min > dt) s.t_min = dt; 63 | } 64 | void print() { 65 | map::iterator it; 66 | for(it = stats.begin(); it!=stats.end(); it++) { 67 | Stats& s = it->second; 68 | printf("%s: %g (%i times, min: %g, max: %g)\n", 69 | it->first.c_str(), s.t, s.n, s.t_min, s.t_max); 70 | } 71 | } 72 | double time(string id) { 73 | Stats& s = stats[id]; 74 | return s.t; 75 | } 76 | }; 77 | Timing timing; 78 | 79 | double tic() { 80 | struct timeval t; 81 | gettimeofday(&t, NULL); 82 | return ((double)t.tv_sec + ((double)t.tv_usec)/1000000.); 83 | } 84 | 85 | double tic(string id) { 86 | double t0 = tic(); 87 | timing.add_t0(id, t0); 88 | return t0; 89 | } 90 | 91 | double toc(double t) { 92 | double s = tic(); 93 | return (max(0., s-t)); 94 | } 95 | 96 | double toc(string id) { 97 | double dt = toc(timing.get_t0(id)); 98 | timing.add_dt(id, dt); 99 | return dt; 100 | } 101 | 102 | void tictoc_print() { 103 | timing.print(); 104 | } 105 | 106 | double tictoc(string id) { 107 | return (timing.time(id)); 108 | } 109 | 110 | Eigen::MatrixXd eye(int num) { 111 | return Eigen::MatrixXd::Identity(num, num); 112 | } 113 | 114 | void givens(const double a, const double b, double& c, double& s) { 115 | if (b==0) { 116 | c = 1.0; 117 | s = 0.0; 118 | } else if (fabs(b)>fabs(a)) { 119 | double t = -a/b; 120 | s = 1/sqrt(1+t*t); 121 | c = t*s; 122 | } else { 123 | double t = -b/a; 124 | c = 1/sqrt(1+t*t); 125 | s = t*c; 126 | } 127 | } 128 | 129 | } 130 | -------------------------------------------------------------------------------- /isam/.svn/pristine/f3/f3ea1cbba50adb51238ef2b814250c83b860f6b2.svn-base: -------------------------------------------------------------------------------- 1 | /** 2 | * @file GLCReparam.h 3 | * @brief Generic Linear Constraint Reparametrization 4 | * @author Nicholas Carlevaris-Bianco 5 | * @author Michael Kaess 6 | * @version $Id: GLCReparam.h 8578 2013-07-01 00:28:49Z kaess $ 7 | * 8 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 9 | * Michael Kaess, Hordur Johannsson, David Rosen, 10 | * Nicholas Carlevaris-Bianco and John. J. Leonard 11 | * 12 | * This file is part of iSAM. 13 | * 14 | * iSAM is free software; you can redistribute it and/or modify it under 15 | * the terms of the GNU Lesser General Public License as published by the 16 | * Free Software Foundation; either version 2.1 of the License, or (at 17 | * your option) any later version. 18 | * 19 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 20 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 21 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 22 | * License for more details. 23 | * 24 | * You should have received a copy of the GNU Lesser General Public License 25 | * along with iSAM. If not, see . 26 | * 27 | */ 28 | 29 | #pragma once 30 | 31 | #include 32 | #include 33 | #include 34 | 35 | #include "Node.h" 36 | #include "Factor.h" 37 | #include "Anchor.h" 38 | 39 | namespace isam { 40 | 41 | /** 42 | * interface class to allow for reparametrization by the user 43 | * see GLC_RootShift implementation as an example 44 | */ 45 | class GLC_Reparam { 46 | 47 | public: 48 | 49 | /** 50 | * set_nodes() 51 | * @param nodes Vector of Node pointers which support the factor 52 | */ 53 | virtual void set_nodes (std::vector nodes) = 0; 54 | /** 55 | * clone() 56 | * @return a new instance of the derived class 57 | */ 58 | virtual GLC_Reparam* clone() = 0; 59 | /** 60 | * is_angle() 61 | * @return boolean vector indacating if elemetes reparameterized vector are angles 62 | */ 63 | virtual Eigen::VectorXb is_angle() = 0; 64 | /** 65 | * jacobian() 66 | * @return jacobian of reparameterized output wrt node input 67 | */ 68 | virtual Eigen::MatrixXd jacobian() = 0; 69 | /** 70 | * reparameterize() 71 | * @return reparameterized vector 72 | */ 73 | virtual Eigen::VectorXd reparameterize (Selector s) = 0; 74 | 75 | }; 76 | 77 | /** 78 | * GLC_RootShift 79 | * implementation of GLC_Reparam 80 | * performs root shift operation 81 | */ 82 | class GLC_RootShift : public GLC_Reparam { 83 | 84 | private: 85 | std::vector _nodes; 86 | Eigen::VectorXb _is_angle; 87 | int _dim; // input and output dim 88 | 89 | public: 90 | 91 | void set_nodes (std::vector nodes) { 92 | _nodes = nodes; 93 | _dim = 0; 94 | for (size_t i=0; i<_nodes.size(); i++) { 95 | _dim += _nodes[i]->dim(); 96 | } 97 | _is_angle.resize(_dim); 98 | int ioff = 0; 99 | for (size_t i=0; i<_nodes.size(); i++) { 100 | _is_angle.segment(ioff, _nodes[i]->dim()) = _nodes[i]->is_angle(); 101 | ioff += _nodes[i]->dim(); 102 | } 103 | } 104 | 105 | GLC_RootShift* clone() {return new GLC_RootShift(*this);} 106 | Eigen::VectorXb is_angle() {return _is_angle;} 107 | Eigen::MatrixXd jacobian(); 108 | Eigen::VectorXd reparameterize (Selector s); 109 | Eigen::VectorXd root_shift (Node* node_i, Node* node_j, Selector s); 110 | 111 | }; 112 | 113 | } // namespace isam 114 | -------------------------------------------------------------------------------- /include/rpn/proposal_test.m: -------------------------------------------------------------------------------- 1 | function aboxes = proposal_test(conf, imdb, varargin) 2 | % aboxes = proposal_test(conf, imdb, varargin) 3 | % -------------------------------------------------------- 4 | % Faster R-CNN 5 | % Copyright (c) 2015, Shaoqing Ren 6 | % Licensed under The MIT License [see LICENSE for details] 7 | % -------------------------------------------------------- 8 | 9 | %% inputs 10 | ip = inputParser; 11 | ip.addRequired('conf', @isstruct); 12 | ip.addRequired('imdb', @isstruct); 13 | ip.addParamValue('net_def_file', fullfile(pwd, 'proposal_models', 'Zeiler_conv5', 'test.prototxt'), ... 14 | @isstr); 15 | ip.addParamValue('net_file', fullfile(pwd, 'proposal_models', 'Zeiler_conv5', 'Zeiler_conv5.caffemodel'), ... 16 | @isstr); 17 | ip.addParamValue('cache_name', 'Zeiler_conv5', ... 18 | @isstr); 19 | 20 | ip.addParamValue('suffix', '', @isstr); 21 | 22 | ip.parse(conf, imdb, varargin{:}); 23 | opts = ip.Results; 24 | 25 | 26 | cache_dir = fullfile(pwd, 'output', 'rpn_cachedir', opts.cache_name, imdb.name); 27 | % try 28 | % % try to load cache 29 | % ld = load(fullfile(cache_dir, ['proposal_boxes_' imdb.name opts.suffix])); 30 | % aboxes = ld.aboxes; 31 | % clear ld; 32 | % catch 33 | %% init net 34 | % init caffe net 35 | caffe_log_file_base = fullfile(cache_dir, 'caffe_log'); 36 | caffe.init_log(caffe_log_file_base); 37 | caffe_net = caffe.Net(opts.net_def_file, 'test'); 38 | caffe_net.copy_from(opts.net_file); 39 | 40 | % init log 41 | timestamp = datestr(datevec(now()), 'yyyymmdd_HHMMSS'); 42 | mkdir_if_missing(fullfile(cache_dir, 'log')); 43 | log_file = fullfile(cache_dir, 'log', ['test_', timestamp, '.txt']); 44 | diary(log_file); 45 | 46 | % set random seed 47 | prev_rng = seed_rand(conf.rng_seed); 48 | caffe.set_random_seed(conf.rng_seed); 49 | 50 | % set gpu/cpu 51 | if conf.use_gpu 52 | caffe.set_mode_gpu(); 53 | else 54 | caffe.set_mode_cpu(); 55 | end 56 | 57 | disp('opts:'); 58 | disp(opts); 59 | disp('conf:'); 60 | disp(conf); 61 | 62 | %% testing 63 | num_images = length(imdb.image_ids); 64 | % all detections are collected into: 65 | % all_boxes[image] = N x 5 array of detections in 66 | % (x1, y1, x2, y2, score) 67 | aboxes = cell(num_images, 1); 68 | abox_deltas = cell(num_images, 1); 69 | aanchors = cell(num_images, 1); 70 | ascores = cell(num_images, 1); 71 | 72 | count = 0; 73 | for i = 1:num_images 74 | count = count + 1; 75 | fprintf('%s: test (%s) %d/%d ', procid(), imdb.name, count, num_images); 76 | th = tic; 77 | im = imread(imdb.image_at(i)); 78 | 79 | [boxes, scores, abox_deltas{i}, aanchors{i}, ascores{i}] = proposal_im_detect(conf, caffe_net, im); 80 | 81 | fprintf(' time: %.3fs\n', toc(th)); 82 | 83 | aboxes{i} = [boxes, scores]; 84 | end 85 | 86 | diary off; 87 | caffe.reset_all(); 88 | rng(prev_rng); 89 | % end 90 | end 91 | -------------------------------------------------------------------------------- /isam/include/isam/Anchor.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Anchor.h 3 | * @brief Implementation of the anchor node. 4 | * @author Hordur Johannsson 5 | * @author Michael Kaess 6 | * @version $Id: Anchor.h 6334 2012-03-22 18:53:24Z hordurj $ 7 | * 8 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 9 | * Michael Kaess, Hordur Johannsson, David Rosen, 10 | * Nicholas Carlevaris-Bianco and John. J. Leonard 11 | * 12 | * This file is part of iSAM. 13 | * 14 | * iSAM is free software; you can redistribute it and/or modify it under 15 | * the terms of the GNU Lesser General Public License as published by the 16 | * Free Software Foundation; either version 2.1 of the License, or (at 17 | * your option) any later version. 18 | * 19 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 20 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 21 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 22 | * License for more details. 23 | * 24 | * You should have received a copy of the GNU Lesser General Public License 25 | * along with iSAM. If not, see . 26 | * 27 | */ 28 | 29 | #pragma once 30 | 31 | #include "Node.h" 32 | #include "Factor.h" 33 | #include "Pose2d.h" 34 | #include "Point2d.h" 35 | #include "Pose3d.h" 36 | #include "Slam.h" 37 | 38 | namespace isam { 39 | 40 | typedef NodeT Pose2d_Node; 41 | typedef NodeT Pose3d_Node; 42 | 43 | /** 44 | * A anchor node for 2d poses 45 | */ 46 | class Anchor2d_Node : public Pose2d_Node { 47 | public: 48 | Anchor2d_Node(Slam* slam); 49 | ~Anchor2d_Node(); 50 | 51 | /** 52 | * Add a prior to the anchor. The prior will be removed 53 | * if this anchor is merged with another anchor's frame. 54 | */ 55 | void set_prior(); 56 | 57 | /** 58 | * Add a new anchor to this frame. 59 | */ 60 | void add_anchor(Anchor2d_Node* a); 61 | 62 | /** 63 | * Merges the node with anchor a. 64 | * 65 | * @param a the node to merge with. 66 | * @param old_origin the pose of this frame in the new frame. 67 | * 68 | * Usage: b.merge(a); 69 | * 70 | * All anchors in a's frame will be merged with the b's frame. 71 | * 72 | */ 73 | void merge(Anchor2d_Node* a, Pose2d old_origin); 74 | 75 | Anchor2d_Node* parent() { return _parent; } 76 | 77 | private: 78 | Anchor2d_Node* _parent; 79 | std::vector _childs; 80 | Factor* _prior; 81 | Slam* _slam; 82 | }; 83 | 84 | /** 85 | * A anchor node for 3d poses 86 | */ 87 | class Anchor3d_Node : public Pose3d_Node { 88 | public: 89 | Anchor3d_Node(Slam* slam); 90 | ~Anchor3d_Node(); 91 | 92 | /** 93 | * Add a prior to the anchor. The prior will be removed 94 | * if this anchor is merged with another anchor's frame. 95 | */ 96 | void set_prior(); 97 | 98 | /** 99 | * Add a new anchor to this frame. 100 | */ 101 | void add_anchor(Anchor3d_Node* a); 102 | 103 | /** 104 | * Merges the node with anchor a. 105 | * 106 | * @param a the node to merge with. 107 | * @param old_origin the pose of this frame in the new frame. 108 | * 109 | * Usage: b.merge(a); 110 | * 111 | * All anchors in a's frame will be merged with the b's frame. 112 | * 113 | */ 114 | void merge(Anchor3d_Node* a, Pose3d old_origin); 115 | 116 | Anchor3d_Node* parent() { return _parent; } 117 | 118 | private: 119 | Anchor3d_Node* _parent; 120 | std::vector _childs; 121 | Factor* _prior; 122 | Slam* _slam; 123 | }; 124 | 125 | } 126 | -------------------------------------------------------------------------------- /isam/.svn/pristine/be/bee75c08e6b2f1bf5ab41e299ce5d88275b63d25.svn-base: -------------------------------------------------------------------------------- 1 | /** 2 | * @file util.cpp 3 | * @brief Basic utility functions that are independent of iSAM. 4 | * @author Michael Kaess 5 | * @version $Id: util.cpp 6335 2012-03-22 23:13:52Z kaess $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #include 29 | #include 30 | #include 31 | #include // abs 32 | 33 | #include "isam/util.h" 34 | 35 | using namespace std; 36 | 37 | namespace isam { 38 | 39 | // simple class for accumulating execution timing information by name 40 | class Timing { 41 | class Stats { 42 | public: 43 | double t0; 44 | double t; 45 | double t_max; 46 | double t_min; 47 | int n; 48 | }; 49 | map stats; 50 | public: 51 | void add_t0(string id, double t0) { 52 | stats[id].t0 = t0; 53 | } 54 | double get_t0(string id) { 55 | return stats[id].t0; 56 | } 57 | void add_dt(string id, double dt) { 58 | Stats& s = stats[id]; 59 | s.t += dt; 60 | s.n++; 61 | if (s.n==1 || s.t_max < dt) s.t_max = dt; 62 | if (s.n==1 || s.t_min > dt) s.t_min = dt; 63 | } 64 | void print() { 65 | map::iterator it; 66 | for(it = stats.begin(); it!=stats.end(); it++) { 67 | Stats& s = it->second; 68 | printf("%s: %g (%i times, min: %g, max: %g)\n", 69 | it->first.c_str(), s.t, s.n, s.t_min, s.t_max); 70 | } 71 | } 72 | double time(string id) { 73 | Stats& s = stats[id]; 74 | return s.t; 75 | } 76 | }; 77 | Timing timing; 78 | 79 | double tic() { 80 | struct timeval t; 81 | gettimeofday(&t, NULL); 82 | return ((double)t.tv_sec + ((double)t.tv_usec)/1000000.); 83 | } 84 | 85 | double tic(string id) { 86 | double t0 = tic(); 87 | timing.add_t0(id, t0); 88 | return t0; 89 | } 90 | 91 | double toc(double t) { 92 | double s = tic(); 93 | return (max(0., s-t)); 94 | } 95 | 96 | double toc(string id) { 97 | double dt = toc(timing.get_t0(id)); 98 | timing.add_dt(id, dt); 99 | return dt; 100 | } 101 | 102 | void tictoc_print() { 103 | timing.print(); 104 | } 105 | 106 | double tictoc(string id) { 107 | return (timing.time(id)); 108 | } 109 | 110 | Eigen::MatrixXd eye(int num) { 111 | return Eigen::MatrixXd::Identity(num, num); 112 | } 113 | 114 | void givens(const double a, const double b, double& c, double& s) { 115 | if (b==0) { 116 | c = 1.0; 117 | s = 0.0; 118 | } else if (fabs(b)>fabs(a)) { 119 | double t = -a/b; 120 | s = 1/sqrt(1+t*t); 121 | c = t*s; 122 | } else { 123 | double t = -b/a; 124 | c = 1/sqrt(1+t*t); 125 | s = t*c; 126 | } 127 | } 128 | 129 | } 130 | -------------------------------------------------------------------------------- /isam/.svn/pristine/d1/d1e59216a429059c2c6ffd6ba4f539e30cb1b920.svn-base: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Anchor.h 3 | * @brief Implementation of the anchor node. 4 | * @author Hordur Johannsson 5 | * @author Michael Kaess 6 | * @version $Id: Anchor.h 6334 2012-03-22 18:53:24Z hordurj $ 7 | * 8 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 9 | * Michael Kaess, Hordur Johannsson, David Rosen, 10 | * Nicholas Carlevaris-Bianco and John. J. Leonard 11 | * 12 | * This file is part of iSAM. 13 | * 14 | * iSAM is free software; you can redistribute it and/or modify it under 15 | * the terms of the GNU Lesser General Public License as published by the 16 | * Free Software Foundation; either version 2.1 of the License, or (at 17 | * your option) any later version. 18 | * 19 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 20 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 21 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 22 | * License for more details. 23 | * 24 | * You should have received a copy of the GNU Lesser General Public License 25 | * along with iSAM. If not, see . 26 | * 27 | */ 28 | 29 | #pragma once 30 | 31 | #include "Node.h" 32 | #include "Factor.h" 33 | #include "Pose2d.h" 34 | #include "Point2d.h" 35 | #include "Pose3d.h" 36 | #include "Slam.h" 37 | 38 | namespace isam { 39 | 40 | typedef NodeT Pose2d_Node; 41 | typedef NodeT Pose3d_Node; 42 | 43 | /** 44 | * A anchor node for 2d poses 45 | */ 46 | class Anchor2d_Node : public Pose2d_Node { 47 | public: 48 | Anchor2d_Node(Slam* slam); 49 | ~Anchor2d_Node(); 50 | 51 | /** 52 | * Add a prior to the anchor. The prior will be removed 53 | * if this anchor is merged with another anchor's frame. 54 | */ 55 | void set_prior(); 56 | 57 | /** 58 | * Add a new anchor to this frame. 59 | */ 60 | void add_anchor(Anchor2d_Node* a); 61 | 62 | /** 63 | * Merges the node with anchor a. 64 | * 65 | * @param a the node to merge with. 66 | * @param old_origin the pose of this frame in the new frame. 67 | * 68 | * Usage: b.merge(a); 69 | * 70 | * All anchors in a's frame will be merged with the b's frame. 71 | * 72 | */ 73 | void merge(Anchor2d_Node* a, Pose2d old_origin); 74 | 75 | Anchor2d_Node* parent() { return _parent; } 76 | 77 | private: 78 | Anchor2d_Node* _parent; 79 | std::vector _childs; 80 | Factor* _prior; 81 | Slam* _slam; 82 | }; 83 | 84 | /** 85 | * A anchor node for 3d poses 86 | */ 87 | class Anchor3d_Node : public Pose3d_Node { 88 | public: 89 | Anchor3d_Node(Slam* slam); 90 | ~Anchor3d_Node(); 91 | 92 | /** 93 | * Add a prior to the anchor. The prior will be removed 94 | * if this anchor is merged with another anchor's frame. 95 | */ 96 | void set_prior(); 97 | 98 | /** 99 | * Add a new anchor to this frame. 100 | */ 101 | void add_anchor(Anchor3d_Node* a); 102 | 103 | /** 104 | * Merges the node with anchor a. 105 | * 106 | * @param a the node to merge with. 107 | * @param old_origin the pose of this frame in the new frame. 108 | * 109 | * Usage: b.merge(a); 110 | * 111 | * All anchors in a's frame will be merged with the b's frame. 112 | * 113 | */ 114 | void merge(Anchor3d_Node* a, Pose3d old_origin); 115 | 116 | Anchor3d_Node* parent() { return _parent; } 117 | 118 | private: 119 | Anchor3d_Node* _parent; 120 | std::vector _childs; 121 | Factor* _prior; 122 | Slam* _slam; 123 | }; 124 | 125 | } 126 | -------------------------------------------------------------------------------- /isam/include/isam/Jacobian.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Jacobian.h 3 | * @brief Jacobian and terms. 4 | * @author Michael Kaess 5 | * @version $Id: Jacobian.h 6536 2012-04-22 16:30:15Z kaess $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | 32 | namespace isam { 33 | 34 | typedef double (*cost_func_t)(double); 35 | 36 | 37 | // Elementary Jacobian for one specific variable/node, potentially containing multiple measurement rows. 38 | class Term { 39 | friend std::ostream& operator<<(std::ostream& output, const Term& t) { 40 | t.write(output); 41 | return output; 42 | } 43 | 44 | Node* _node; 45 | 46 | Eigen::MatrixXd _term; 47 | 48 | public: 49 | 50 | const Node* node() const {return _node;} 51 | 52 | const Eigen::MatrixXd& term() const {return _term;} 53 | 54 | Term(Node* node, const Eigen::MatrixXd& term) : _node(node), _term(term) {} 55 | 56 | Term(Node* node, const double * term, int r, int c) : _node(node), _term(r,c) { 57 | int n = 0; 58 | for (int row=0; row Terms; 71 | 72 | // Jacobian consisting of multiple blocks. 73 | class Jacobian { 74 | friend std::ostream& operator<<(std::ostream& output, const Jacobian& t) { 75 | t.write(output); 76 | return output; 77 | } 78 | 79 | int _dimtotal; 80 | 81 | Terms _terms; 82 | 83 | Eigen::VectorXd _residual; 84 | 85 | public: 86 | 87 | Jacobian() : _dimtotal(0), _residual() {} 88 | 89 | Jacobian(Eigen::VectorXd& residual) : _dimtotal(0), _residual(residual) {} 90 | 91 | inline Jacobian(const double * residual, int size) : _dimtotal(0), _residual(size) { 92 | memcpy(_residual.data(), residual, size*sizeof(double)); 93 | } 94 | 95 | const Terms& terms() const {return _terms;} 96 | 97 | // note: rhs for linear system Ax=b is negative of residual! 98 | Eigen::VectorXd rhs() const {return - _residual;} 99 | 100 | void add_term(Node* node, const Eigen::MatrixXd& term) { 101 | _terms.push_back(Term(node, term)); 102 | _dimtotal += node->dim(); 103 | } 104 | 105 | inline void add_term(Node* node, const double* term, int r, int c) { 106 | _terms.push_back(Term(node, term, r, c)); 107 | _dimtotal += node->dim(); 108 | } 109 | 110 | int dimtotal() const { return _dimtotal; } 111 | 112 | void write(std::ostream &out) const { 113 | int i=1; 114 | for (Terms::const_iterator it = _terms.begin(); it != _terms.end(); it++, i++) { 115 | out << "Term " << i << ":" << std::endl; 116 | out << *it; 117 | } 118 | out << "rhs: " << std::endl << _residual << std::endl; 119 | } 120 | }; 121 | 122 | } 123 | -------------------------------------------------------------------------------- /include/rpn/proposal_config.m: -------------------------------------------------------------------------------- 1 | function conf = proposal_config(varargin) 2 | % conf = proposal_config(varargin) 3 | % -------------------------------------------------------- 4 | % Faster R-CNN 5 | % Copyright (c) 2015, Shaoqing Ren 6 | % Licensed under The MIT License [see LICENSE for details] 7 | % -------------------------------------------------------- 8 | 9 | ip = inputParser; 10 | 11 | %% training 12 | ip.addParamValue('use_gpu', gpuDeviceCount > 0, ... 13 | @islogical); 14 | 15 | % whether drop the anchors that has edges outside of the image boundary 16 | ip.addParamValue('drop_boxes_runoff_image', ... 17 | true, @islogical); 18 | 19 | % Image scales -- the short edge of input image 20 | ip.addParamValue('scales', 600, @ismatrix); 21 | % Max pixel size of a scaled input image 22 | ip.addParamValue('max_size', 1000, @isscalar); 23 | % Images per batch, only supports ims_per_batch = 1 currently 24 | ip.addParamValue('ims_per_batch', 1, @isscalar); 25 | % Minibatch size 26 | ip.addParamValue('batch_size', 256, @isscalar); 27 | % Fraction of minibatch that is foreground labeled (class > 0) 28 | ip.addParamValue('fg_fraction', 0.5, @isscalar); 29 | % weight of background samples, when weight of foreground samples is 30 | % 1.0 31 | ip.addParamValue('bg_weight', 1.0, @isscalar); 32 | % Overlap threshold for a ROI to be considered foreground (if >= fg_thresh) 33 | ip.addParamValue('fg_thresh', 0.7, @isscalar); 34 | % Overlap threshold for a ROI to be considered background (class = 0 if 35 | % overlap in [bg_thresh_lo, bg_thresh_hi)) 36 | ip.addParamValue('bg_thresh_hi', 0.3, @isscalar); 37 | ip.addParamValue('bg_thresh_lo', 0, @isscalar); 38 | % mean image, in RGB order 39 | ip.addParamValue('image_means', 128, @ismatrix); 40 | % Use horizontally-flipped images during training? 41 | ip.addParamValue('use_flipped', true, @islogical); 42 | % Stride in input image pixels at ROI pooling level (network specific) 43 | % 16 is true for {Alex,Caffe}Net, VGG_CNN_M_1024, and VGG16 44 | ip.addParamValue('feat_stride', 16, @isscalar); 45 | % train proposal target only to labled ground-truths or also include 46 | % other proposal results (selective search, etc.) 47 | ip.addParamValue('target_only_gt', true, @islogical); 48 | 49 | % random seed 50 | ip.addParamValue('rng_seed', 6, @isscalar); 51 | 52 | 53 | %% testing 54 | ip.addParamValue('test_scales', 600, @isscalar); 55 | ip.addParamValue('test_max_size', 1000, @isscalar); 56 | ip.addParamValue('test_nms', 0.3, @isscalar); 57 | ip.addParamValue('test_binary', false, @islogical); 58 | ip.addParamValue('test_min_box_size',16, @isscalar); 59 | ip.addParamValue('test_drop_boxes_runoff_image', ... 60 | false, @islogical); 61 | 62 | ip.parse(varargin{:}); 63 | conf = ip.Results; 64 | 65 | assert(conf.ims_per_batch == 1, 'currently rpn only supports ims_per_batch == 1'); 66 | 67 | % if image_means is a file, load it 68 | if ischar(conf.image_means) 69 | s = load(conf.image_means); 70 | s_fieldnames = fieldnames(s); 71 | assert(length(s_fieldnames) == 1); 72 | conf.image_means = s.(s_fieldnames{1}); 73 | end 74 | end -------------------------------------------------------------------------------- /isam/include/isam/Properties.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Properties.h 3 | * @brief Properties class for easy access to internal parameters. 4 | * @author Michael Kaess 5 | * @version $Id: Properties.h 6377 2012-03-30 20:06:44Z kaess $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #pragma once 29 | 30 | namespace isam { 31 | 32 | enum Method {GAUSS_NEWTON, LEVENBERG_MARQUARDT, DOG_LEG}; 33 | 34 | /** 35 | * User changeable default parameters. 36 | */ 37 | class Properties { 38 | // default copy constructor and assignment operator in use 39 | public: 40 | 41 | /** print additional information if true */ 42 | bool verbose; 43 | 44 | /** omit all textual output if true */ 45 | bool quiet; 46 | 47 | /** Ignore any symbolic derivatives provided in factors */ 48 | bool force_numerical_jacobian; 49 | 50 | /** which optimization method to use */ 51 | Method method; 52 | 53 | /** Powell's Dog-Leg termination criteria: stop whenever the infinity-norm 54 | * of the gradient direction vector falls below this threshold. */ 55 | double epsilon1; 56 | /** Powell's Dog-Leg termination criteria: stop whenever the 2-norm of 57 | * the correction step falls below this threshold. */ 58 | double epsilon2; 59 | /** Powell's Dog-Leg termination criteria: stop whenever the infinity-norm 60 | * of the error-residual vector falls below this threshold. */ 61 | double epsilon3; 62 | 63 | /** Termination criterion: stop whenever the absolute sum-of-squares error 64 | * falls below this threshold. */ 65 | double epsilon_abs; 66 | /** Termination criterion: stop whenever the /difference/ in absolute 67 | * sum-of-squares errors between two estimates falls below this fraction 68 | * of the /total/ absolute sum-of-squares errors. */ 69 | double epsilon_rel; 70 | 71 | /** Maximum number of iterations */ 72 | int max_iterations; 73 | /** Starting value for lambda in LM */ 74 | double lm_lambda0; 75 | /** Factor for multiplying (failure) or dividing (success) lambda */ 76 | double lm_lambda_factor; 77 | 78 | /** Only update R matrix/solution/batch every mod_update steps */ 79 | int mod_update; 80 | /** Batch solve with variable reordering and relinearization every mod_batch steps */ 81 | int mod_batch; 82 | /** For incremental steps, solve by backsubstitution every mod_solve steps */ 83 | int mod_solve; 84 | 85 | // default parameters 86 | Properties() : 87 | verbose(false), 88 | quiet(false), 89 | 90 | force_numerical_jacobian(false), 91 | 92 | method(GAUSS_NEWTON), 93 | 94 | epsilon1(1e-2), 95 | epsilon2(1e-2), 96 | epsilon3(1e-2), 97 | 98 | epsilon_abs(1e-3), 99 | epsilon_rel(1e-5), 100 | 101 | max_iterations(500), 102 | 103 | lm_lambda0(1e-6), 104 | lm_lambda_factor(10.), 105 | 106 | mod_update(1), 107 | mod_batch(100), 108 | mod_solve(1) 109 | {} 110 | }; 111 | 112 | } 113 | -------------------------------------------------------------------------------- /isam/.svn/pristine/3a/3a7337d9c6cdeb5264445e25fa6a432c387a8a1c.svn-base: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Jacobian.h 3 | * @brief Jacobian and terms. 4 | * @author Michael Kaess 5 | * @version $Id: Jacobian.h 6536 2012-04-22 16:30:15Z kaess $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | 32 | namespace isam { 33 | 34 | typedef double (*cost_func_t)(double); 35 | 36 | 37 | // Elementary Jacobian for one specific variable/node, potentially containing multiple measurement rows. 38 | class Term { 39 | friend std::ostream& operator<<(std::ostream& output, const Term& t) { 40 | t.write(output); 41 | return output; 42 | } 43 | 44 | Node* _node; 45 | 46 | Eigen::MatrixXd _term; 47 | 48 | public: 49 | 50 | const Node* node() const {return _node;} 51 | 52 | const Eigen::MatrixXd& term() const {return _term;} 53 | 54 | Term(Node* node, const Eigen::MatrixXd& term) : _node(node), _term(term) {} 55 | 56 | Term(Node* node, const double * term, int r, int c) : _node(node), _term(r,c) { 57 | int n = 0; 58 | for (int row=0; row Terms; 71 | 72 | // Jacobian consisting of multiple blocks. 73 | class Jacobian { 74 | friend std::ostream& operator<<(std::ostream& output, const Jacobian& t) { 75 | t.write(output); 76 | return output; 77 | } 78 | 79 | int _dimtotal; 80 | 81 | Terms _terms; 82 | 83 | Eigen::VectorXd _residual; 84 | 85 | public: 86 | 87 | Jacobian() : _dimtotal(0), _residual() {} 88 | 89 | Jacobian(Eigen::VectorXd& residual) : _dimtotal(0), _residual(residual) {} 90 | 91 | inline Jacobian(const double * residual, int size) : _dimtotal(0), _residual(size) { 92 | memcpy(_residual.data(), residual, size*sizeof(double)); 93 | } 94 | 95 | const Terms& terms() const {return _terms;} 96 | 97 | // note: rhs for linear system Ax=b is negative of residual! 98 | Eigen::VectorXd rhs() const {return - _residual;} 99 | 100 | void add_term(Node* node, const Eigen::MatrixXd& term) { 101 | _terms.push_back(Term(node, term)); 102 | _dimtotal += node->dim(); 103 | } 104 | 105 | inline void add_term(Node* node, const double* term, int r, int c) { 106 | _terms.push_back(Term(node, term, r, c)); 107 | _dimtotal += node->dim(); 108 | } 109 | 110 | int dimtotal() const { return _dimtotal; } 111 | 112 | void write(std::ostream &out) const { 113 | int i=1; 114 | for (Terms::const_iterator it = _terms.begin(); it != _terms.end(); it++, i++) { 115 | out << "Term " << i << ":" << std::endl; 116 | out << *it; 117 | } 118 | out << "rhs: " << std::endl << _residual << std::endl; 119 | } 120 | }; 121 | 122 | } 123 | -------------------------------------------------------------------------------- /isam/.svn/pristine/20/2024da76299676140101b366738ee33892dc722e.svn-base: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Properties.h 3 | * @brief Properties class for easy access to internal parameters. 4 | * @author Michael Kaess 5 | * @version $Id: Properties.h 6377 2012-03-30 20:06:44Z kaess $ 6 | * 7 | * Copyright (C) 2009-2013 Massachusetts Institute of Technology. 8 | * Michael Kaess, Hordur Johannsson, David Rosen, 9 | * Nicholas Carlevaris-Bianco and John. J. Leonard 10 | * 11 | * This file is part of iSAM. 12 | * 13 | * iSAM is free software; you can redistribute it and/or modify it under 14 | * the terms of the GNU Lesser General Public License as published by the 15 | * Free Software Foundation; either version 2.1 of the License, or (at 16 | * your option) any later version. 17 | * 18 | * iSAM is distributed in the hope that it will be useful, but WITHOUT 19 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 20 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 21 | * License for more details. 22 | * 23 | * You should have received a copy of the GNU Lesser General Public License 24 | * along with iSAM. If not, see . 25 | * 26 | */ 27 | 28 | #pragma once 29 | 30 | namespace isam { 31 | 32 | enum Method {GAUSS_NEWTON, LEVENBERG_MARQUARDT, DOG_LEG}; 33 | 34 | /** 35 | * User changeable default parameters. 36 | */ 37 | class Properties { 38 | // default copy constructor and assignment operator in use 39 | public: 40 | 41 | /** print additional information if true */ 42 | bool verbose; 43 | 44 | /** omit all textual output if true */ 45 | bool quiet; 46 | 47 | /** Ignore any symbolic derivatives provided in factors */ 48 | bool force_numerical_jacobian; 49 | 50 | /** which optimization method to use */ 51 | Method method; 52 | 53 | /** Powell's Dog-Leg termination criteria: stop whenever the infinity-norm 54 | * of the gradient direction vector falls below this threshold. */ 55 | double epsilon1; 56 | /** Powell's Dog-Leg termination criteria: stop whenever the 2-norm of 57 | * the correction step falls below this threshold. */ 58 | double epsilon2; 59 | /** Powell's Dog-Leg termination criteria: stop whenever the infinity-norm 60 | * of the error-residual vector falls below this threshold. */ 61 | double epsilon3; 62 | 63 | /** Termination criterion: stop whenever the absolute sum-of-squares error 64 | * falls below this threshold. */ 65 | double epsilon_abs; 66 | /** Termination criterion: stop whenever the /difference/ in absolute 67 | * sum-of-squares errors between two estimates falls below this fraction 68 | * of the /total/ absolute sum-of-squares errors. */ 69 | double epsilon_rel; 70 | 71 | /** Maximum number of iterations */ 72 | int max_iterations; 73 | /** Starting value for lambda in LM */ 74 | double lm_lambda0; 75 | /** Factor for multiplying (failure) or dividing (success) lambda */ 76 | double lm_lambda_factor; 77 | 78 | /** Only update R matrix/solution/batch every mod_update steps */ 79 | int mod_update; 80 | /** Batch solve with variable reordering and relinearization every mod_batch steps */ 81 | int mod_batch; 82 | /** For incremental steps, solve by backsubstitution every mod_solve steps */ 83 | int mod_solve; 84 | 85 | // default parameters 86 | Properties() : 87 | verbose(false), 88 | quiet(false), 89 | 90 | force_numerical_jacobian(false), 91 | 92 | method(GAUSS_NEWTON), 93 | 94 | epsilon1(1e-2), 95 | epsilon2(1e-2), 96 | epsilon3(1e-2), 97 | 98 | epsilon_abs(1e-3), 99 | epsilon_rel(1e-5), 100 | 101 | max_iterations(500), 102 | 103 | lm_lambda0(1e-6), 104 | lm_lambda_factor(10.), 105 | 106 | mod_update(1), 107 | mod_batch(100), 108 | mod_solve(1) 109 | {} 110 | }; 111 | 112 | } 113 | --------------------------------------------------------------------------------