Added calibration and normal modes

CMakeLists.txt

@@ -102,61 +102,10 @@ file(GLOB SOURCE_FILES "src/apriltags/*.cc")
 
 ## Declare a cpp executable
 
-add_executable(april_tag_node src/april_tag_node.cpp ${SOURCE_FILES})
+add_executable(april_tag_node src/april_tag_node.cpp src/rigidtransform.cc ${SOURCE_FILES})
 
-## Add cmake target dependencies of the executable/library
-## as an example, message headers may need to be generated before nodes
-# add_dependencies(april_tag_node april_tag_generate_messages_cpp)
 
 ## Specify libraries to link a library or executable target against
 target_link_libraries(april_tag_node
-  ${catkin_LIBRARIES}
+  ${catkin_LIBRARIES} popt config++
 )
-
-#############
-## Install ##
-#############
-
-# all install targets should use catkin DESTINATION variables
-# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
-
-## Mark executable scripts (Python etc.) for installation
-## in contrast to setup.py, you can choose the destination
-# install(PROGRAMS
-#   scripts/my_python_script
-#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark executables and/or libraries for installation
-# install(TARGETS april_tag april_tag_node
-#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark cpp header files for installation
-# install(DIRECTORY include/${PROJECT_NAME}/
-#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-#   FILES_MATCHING PATTERN "*.h"
-#   PATTERN ".svn" EXCLUDE
-# )
-
-## Mark other files for installation (e.g. launch and bag files, etc.)
-# install(FILES
-#   # myfile1
-#   # myfile2
-#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-# )
-
-#############
-## Testing ##
-#############
-
-## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_april_tag.cpp)
-# if(TARGET ${PROJECT_NAME}-test)
-#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
-# endif()
-
-## Add folders to be run by python nosetests
-# catkin_add_nosetests(test)

include/popt_pp.h

@@ -0,0 +1,39 @@
+#ifndef _INCLUDED_POPT_PP_H_
+#define _INCLUDED_POPT_PP_H_
+
+#include <popt.h>
+
+class POpt{
+protected:
+  poptContext con;
+public:
+  // creation and deletion
+  POpt(const char *name, int argc, const char **argv,
+       const poptOption *options, int flags)
+    {con = poptGetContext(name,argc,argv,options,flags);}
+  POpt(const char *name, int argc, char **argv,
+       const poptOption *options, int flags)
+    {con = poptGetContext(name,argc,(const char **)argv,options,flags);}
+  ~POpt()
+    {poptFreeContext(con);}
+
+  // functions for processing options
+  int getNextOpt()
+    {return(poptGetNextOpt(con));}
+  void ignoreOptions()
+    {while(getNextOpt() >= 0);}
+  const char *getOptArg()
+    {return(poptGetOptArg(con));}
+  const char *strError(int error)
+    {return(poptStrerror(error));}
+  const char *badOption(int flags = POPT_BADOPTION_NOALIAS)
+    {return(poptBadOption(con,flags));}
+
+  // processing other arguments
+  const char *getArg()
+    {return(poptGetArg(con));}
+  void ignoreArgs()
+    {while(getArg());}
+};
+
+#endif

include/rigidtransform.h

@@ -0,0 +1,20 @@
+//using Eigen's SVD to fastly compute the rigid transformation between two point clouds.
+#ifndef RIGIDTRANSFORM
+#define RIGIDTRANSFORM
+
+#include <iostream>
+#include <ctime>
+
+#include <Eigen/SVD>
+#include <Eigen/Dense>
+#include <Eigen/Sparse>
+#include <Eigen/Geometry>
+
+using namespace Eigen;
+using namespace std;
+
+typedef std::pair<Eigen::Matrix3d, Eigen::Vector3d> TransformType;
+typedef std::vector<Eigen::Vector3d> PointsType;
+
+TransformType computeRigidTransform(const PointsType& src, const PointsType& dst);
+#endif

launch/april_tag_usb.launch

@@ -10,8 +10,8 @@
 	    <param name="io_method" value="mmap"/>
 	</node>
 
-	<node pkg="seer" name="april_tag_node" type="april_tag_node">
-<!--		<remap from="/camera/image_raw" to="/usb_cam/image_raw"/> -->
+	<node pkg="seer" name="april_tag_node" type="april_tag_node" output="screen">
+  <!--<remap from="/usb_cam/image_raw" to="/usb_cam2/image_raw"/>-->
 
 		<!-- focal_length_px: camera focal length in pixels.
 		700 works well forlate 2013 macbook pro retina 13in -->