[ { "path": ".gitignore", "content": "devel/\nlogs/\nbuild/\nbin/\nlib/\nmsg_gen/\nsrv_gen/\nmsg/*Action.msg\nmsg/*ActionFeedback.msg\nmsg/*ActionGoal.msg\nmsg/*ActionResult.msg\nmsg/*Feedback.msg\nmsg/*Goal.msg\nmsg/*Result.msg\nmsg/_*.py\nbuild_isolated/\ndevel_isolated/\n.catkin_workspace\n\n# Generated by dynamic reconfigure\n*.cfgc\n/cfg/cpp/\n/cfg/*.py\n\n# Ignore generated docs\n*.dox\n*.wikidoc\n\n# eclipse stuff\n.project\n.cproject\n\n# qcreator stuff\nCMakeLists.txt.user\n\nsrv/_*.py\n*.pcd\n*.pyc\nqtcreator-*\n*.user\n\n/planning/cfg\n/planning/docs\n/planning/src\n\n*~\n\n# Emacs\n.#*\n\n# Catkin custom files\nCATKIN_IGNORE\n\n# VS Code\n.vscode/*\n!.vscode/settings.json\n!.vscode/tasks.json\n!.vscode/launch.json\n!.vscode/extensions.json\n*.code-workspace\n# Local History for Visual Studio Code\n.history/\n\n# Commitizen\nnode_modules/\npackage-lock.json\npackage.json\n\n# Data\ndata/\nsrc/vloam_main/bags\nKITTI_odometry_evaluation_tool" }, { "path": "LICENSE", "content": "MIT License\n\nCopyright (c) 2021 Yukun Xia\n\nPermission is hereby granted, free of charge, to any person obtaining a copy\nof this software and associated documentation files (the \"Software\"), to deal\nin the Software without restriction, including without limitation the rights\nto use, copy, modify, merge, publish, distribute, sublicense, and/or sell\ncopies of the Software, and to permit persons to whom the Software is\nfurnished to do so, subject to the following conditions:\n\nThe above copyright notice and this permission notice shall be included in all\ncopies or substantial portions of the Software.\n\nTHE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR\nIMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,\nFITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE\nAUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER\nLIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,\nOUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE\nSOFTWARE.\n" }, { "path": "README.md", "content": "# Introduction\n\nThis repository is a reimplementation of the VLOAM algorithm [1]. The LOAM/Lidar Odometry part is adapted and refactored from ALOAM [2], and the Visual Odometry part is written according to the DEMO paper [3].\n\nThe following figure [1] illustrates the pipeline of the VLOAM algorithm. \n\n![demo](figures/VLOAM-figure1.png)\n\n# Results\n\n![demo](figures/results.png)\n\nVideo: https://youtu.be/NnoxB3r_cDM\n\n![demo](figures/evaluation.png)\n\n# Detailed Usage\n\nCheck README.md under `src/vloam_main`\n## Prerequisites\n\nOpenCV 4.5.1\nEigen3 3.3\nCeres 2.0\nPCL 1.2\n\n## Evaluation tool\n\n![demo](figures/kitti_car.png)\n\nhttps://github.com/LeoQLi/KITTI_odometry_evaluation_tool\n\n## Data format\n\nPlace bag files under \"src/vloam_main/bags/\"\n\nNote: current dataloader only support \"synced\" type dataset. \n\n# Reference:\n\n[1] J. Zhang and S. Singh. Laser-visual-inertial Odometry and\nMapping with High Robustness and Low Drift. Journal of\nField Robotics. vol. 35, no. 8, pp. 1242–1264, 2018.\n\n[2] T. Qin and S. Cao. A-LOAM. https://github.com/HKUST-Aerial-Robotics/A-LOAM\n\n[3] Zhang, Ji, Michael Kaess, and Sanjiv Singh. \"Real-time depth enhanced monocular odometry.\" 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, 2014." }, { "path": "src/lidar_odometry_mapping/CMakeLists.txt", "content": "cmake_minimum_required(VERSION 2.8.3)\nproject(lidar_odometry_mapping)\n\nset(CMAKE_BUILD_TYPE \"Release\")\nset(CMAKE_CXX_FLAGS \"-std=c++14\")\nset(CMAKE_CXX_FLAGS_RELEASE \"-O3 -Wall -g\")\n\nfind_package(catkin REQUIRED COMPONENTS\n geometry_msgs\n nav_msgs\n sensor_msgs\n roscpp\n rospy\n rosbag\n std_msgs\n image_transport\n cv_bridge\n tf\n vloam_tf\n)\n\n#find_package(Eigen3 REQUIRED)\nfind_package(PCL REQUIRED)\nfind_package(OpenCV 4.5.1 REQUIRED)\nfind_package(Ceres REQUIRED)\n\ninclude_directories(\n include\n\t${catkin_INCLUDE_DIRS} \n\t${PCL_INCLUDE_DIRS}\n ${CERES_INCLUDE_DIRS}\n ${OpenCV_INCLUDE_DIRS})\n\ncatkin_package(\n CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs\n DEPENDS EIGEN3 PCL \n INCLUDE_DIRS include\n)\n\nadd_library(scan_registration SHARED\n src/scan_registration.cpp\n)\ntarget_link_libraries(scan_registration ${catkin_LIBRARIES} ${PCL_LIBRARIES} vloam_tf)\nset_target_properties(scan_registration PROPERTIES PUBLIC_HEADER include/${PROJECT_NAME}/scan_registration.h)\n\nadd_library(laser_odometry SHARED\n src/laser_odometry.cpp\n)\ntarget_link_libraries(laser_odometry ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES} vloam_tf)\nset_target_properties(laser_odometry PROPERTIES PUBLIC_HEADER include/${PROJECT_NAME}/laser_odometry.h)\n\nadd_library(laser_mapping SHARED\n src/laser_mapping.cpp\n)\ntarget_link_libraries(laser_mapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES} vloam_tf)\nset_target_properties(laser_mapping PROPERTIES PUBLIC_HEADER include/${PROJECT_NAME}/laser_mapping.h)\n\nadd_library(lidar_odometry_mapping SHARED\n src/lidar_odometry_mapping.cpp\n)\ntarget_link_libraries(lidar_odometry_mapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES} scan_registration laser_odometry laser_mapping vloam_tf)\nset_target_properties(lidar_odometry_mapping PROPERTIES PUBLIC_HEADER include/${PROJECT_NAME}/lidar_odometry_mapping.h)\n\ninstall(\n DIRECTORY include/${PROJECT_NAME}/\n DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}\n FILES_MATCHING \n PATTERN \"*.hpp\"\n PATTERN \"*.h\"\n# PATTERN \".svn\" EXCLUDE\n)\n\n\n" }, { "path": "src/lidar_odometry_mapping/LICENSE", "content": "This is an advanced implementation of the algorithm described in the following paper:\n J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time.\n Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. \n\nModifier: Tong Qin qintonguav@gmail.com\n\t Shaozu Cao \t\t saozu.cao@connect.ust.hk\n\nCopyright 2013, Ji Zhang, Carnegie Mellon University\nFurther contributions copyright (c) 2016, Southwest Research Institute\nAll rights reserved.\n\nRedistribution and use in source and binary forms, with or without\nmodification, are permitted provided that the following conditions are met:\n\n1. Redistributions of source code must retain the above copyright notice, this\n list of conditions and the following disclaimer.\n2. Redistributions in binary form must reproduce the above copyright notice,\n this list of conditions and the following disclaimer in the documentation\n and/or other materials provided with the distribution.\n3. Neither the name of the copyright holder nor the names of its contributors\n may be used to endorse or promote products derived from this software without\n specific prior written permission.\n\nTHIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\" AND\nANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED\nWARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE\nDISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE\nFOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL\nDAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR\nSERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\nCAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,OR\nTORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF\nTHIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n\n\n" }, { "path": "src/lidar_odometry_mapping/include/lidar_odometry_mapping/common.h", "content": "// This is an advanced implementation of the algorithm described in the following paper:\n// J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time.\n// Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014.\n\n// Modifier: Tong Qin qintonguav@gmail.com\n// \t Shaozu Cao \t\t saozu.cao@connect.ust.hk\n\n// Copyright 2013, Ji Zhang, Carnegie Mellon University\n// Further contributions copyright (c) 2016, Southwest Research Institute\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are met:\n//\n// 1. Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// 2. Redistributions in binary form must reproduce the above copyright notice,\n// this list of conditions and the following disclaimer in the documentation\n// and/or other materials provided with the distribution.\n// 3. Neither the name of the copyright holder nor the names of its\n// contributors may be used to endorse or promote products derived from this\n// software without specific prior written permission.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\"\n// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE\n// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE\n// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE\n// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR\n// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF\n// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS\n// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN\n// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)\n// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n// POSSIBILITY OF SUCH DAMAGE.\n\n#pragma once\n\n#include \n\n#include \n\ntypedef pcl::PointXYZI PointType;\n\ninline double rad2deg(double radians)\n{\n return radians * 180.0 / M_PI;\n}\n\ninline double deg2rad(double degrees)\n{\n return degrees * M_PI / 180.0;\n}\n" }, { "path": "src/lidar_odometry_mapping/include/lidar_odometry_mapping/laser_mapping.h", "content": "// This is an advanced implementation of the algorithm described in the following paper:\n// J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time.\n// Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014.\n\n// Modifier: Tong Qin qintonguav@gmail.com\n// \t Shaozu Cao \t\t saozu.cao@connect.ust.hk\n\n// Copyright 2013, Ji Zhang, Carnegie Mellon University\n// Further contributions copyright (c) 2016, Southwest Research Institute\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are met:\n//\n// 1. Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// 2. Redistributions in binary form must reproduce the above copyright notice,\n// this list of conditions and the following disclaimer in the documentation\n// and/or other materials provided with the distribution.\n// 3. Neither the name of the copyright holder nor the names of its\n// contributors may be used to endorse or promote products derived from this\n// software without specific prior written permission.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\"\n// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE\n// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE\n// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE\n// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR\n// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF\n// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS\n// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN\n// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)\n// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n// POSSIBILITY OF SUCH DAMAGE.\n\n#pragma once\n\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\n// #include \"lidarFactor.hpp\"\n#include \n\n#include \"lidar_odometry_mapping/common.h\"\n#include \"lidar_odometry_mapping/lidarFactor.hpp\"\n#include \"lidar_odometry_mapping/tic_toc.h\"\n\nnamespace vloam\n{\nclass LaserMapping\n{\npublic:\n LaserMapping()\n : laserCloudCenWidth(10)\n , laserCloudCenHeight(10)\n , laserCloudCenDepth(5)\n , q_w_curr(parameters)\n , t_w_curr(parameters + 4)\n , nh(\"laser_mapping_node\")\n {\n }\n\n void init(std::shared_ptr& vloam_tf_);\n\n void reset();\n void input(const pcl::PointCloud::Ptr& laserCloudCornerLast_,\n const pcl::PointCloud::Ptr& laserCloudSurfLast_,\n const pcl::PointCloud::Ptr& laserCloudFullRes_, const Eigen::Quaterniond& q_wodom_curr_,\n const Eigen::Vector3d& t_wodom_curr_, const bool& skip_frame_);\n void publish();\n void solveMapping();\n void output();\n\n // set initial guess\n void transformAssociateToMap();\n void transformUpdate();\n void pointAssociateToMap(PointType const* const pi, PointType* const po);\n void pointAssociateTobeMapped(PointType const* const pi, PointType* const po);\n\nprivate:\n std::shared_ptr vloam_tf;\n\n int frameCount = 0;\n\n int laserCloudCenWidth;\n int laserCloudCenHeight;\n int laserCloudCenDepth;\n static const int laserCloudWidth = 21;\n static const int laserCloudHeight = 21;\n static const int laserCloudDepth = 11;\n\n static const int laserCloudNum = laserCloudWidth * laserCloudHeight * laserCloudDepth; // 4851\n\n int laserCloudValidInd[125];\n int laserCloudSurroundInd[125];\n\n // input: from odom\n pcl::PointCloud::Ptr laserCloudCornerLast;\n pcl::PointCloud::Ptr laserCloudSurfLast;\n\n // ouput: all visualble cube points\n pcl::PointCloud::Ptr laserCloudSurround;\n\n // surround points in map to build tree\n pcl::PointCloud::Ptr laserCloudCornerFromMap;\n pcl::PointCloud::Ptr laserCloudSurfFromMap;\n\n // input & output: points in one frame. local --> global\n pcl::PointCloud::Ptr laserCloudFullRes;\n\n // points in every cube\n pcl::PointCloud::Ptr laserCloudCornerArray[laserCloudNum];\n pcl::PointCloud::Ptr laserCloudSurfArray[laserCloudNum];\n\n // kd-tree\n pcl::KdTreeFLANN::Ptr kdtreeCornerFromMap;\n pcl::KdTreeFLANN::Ptr kdtreeSurfFromMap;\n\n double parameters[7];\n Eigen::Map q_w_curr;\n Eigen::Map t_w_curr;\n Eigen::Quaterniond q_w_curr_highfreq;\n Eigen::Vector3d t_w_curr_highfreq;\n\n // wmap_T_odom * odom_T_curr = wmap_T_curr;\n // transformation between odom's world and map's world frame\n Eigen::Quaterniond q_wmap_wodom;\n Eigen::Vector3d t_wmap_wodom;\n Eigen::Quaterniond q_wodom_curr;\n Eigen::Vector3d t_wodom_curr;\n\n sensor_msgs::PointCloud2ConstPtr cornerLastBuf;\n sensor_msgs::PointCloud2ConstPtr surfLastBuf;\n sensor_msgs::PointCloud2ConstPtr fullResBuf;\n nav_msgs::Odometry::ConstPtr odometryBuf;\n\n pcl::VoxelGrid downSizeFilterCorner;\n pcl::VoxelGrid downSizeFilterSurf;\n\n std::vector pointSearchInd;\n std::vector pointSearchSqDis;\n\n PointType pointOri, pointSel;\n\n ros::Publisher pubLaserCloudSurround, pubLaserCloudMap, pubLaserCloudFullRes, pubOdomAftMapped,\n pubOdomAftMappedHighFrec, pubLaserAfterMappedPath;\n\n nav_msgs::Path laserAfterMappedPath;\n\n ros::NodeHandle nh;\n int verbose_level;\n\n float lineRes;\n float planeRes;\n\n int laserCloudValidNum;\n int laserCloudSurroundNum;\n\n TicToc t_whole;\n\n bool skip_frame;\n int mapping_skip_frame;\n int map_pub_number;\n};\n\n} // namespace vloam" }, { "path": "src/lidar_odometry_mapping/include/lidar_odometry_mapping/laser_odometry.h", "content": "// This is an advanced implementation of the algorithm described in the following paper:\n// J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time.\n// Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014.\n\n// Modifier: Tong Qin qintonguav@gmail.com\n// \t Shaozu Cao \t\t saozu.cao@connect.ust.hk\n\n// Copyright 2013, Ji Zhang, Carnegie Mellon University\n// Further contributions copyright (c) 2016, Southwest Research Institute\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are met:\n//\n// 1. Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// 2. Redistributions in binary form must reproduce the above copyright notice,\n// this list of conditions and the following disclaimer in the documentation\n// and/or other materials provided with the distribution.\n// 3. Neither the name of the copyright holder nor the names of its\n// contributors may be used to endorse or promote products derived from this\n// software without specific prior written permission.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\"\n// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE\n// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE\n// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE\n// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR\n// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF\n// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS\n// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN\n// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)\n// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n// POSSIBILITY OF SUCH DAMAGE.\n\n#pragma once\n\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\n#include \n#include \n#include \n#include \n#include \n\nnamespace vloam\n{\nclass LaserOdometry\n{\npublic:\n LaserOdometry() : q_last_curr(para_q), t_last_curr(para_t), nh(\"laser_odometry_node\")\n {\n }\n\n void init(std::shared_ptr& vloam_tf_);\n\n // void reset ();\n\n void input(const pcl::PointCloud::Ptr& laserCloud_,\n const pcl::PointCloud::Ptr& cornerPointsSharp_,\n const pcl::PointCloud::Ptr& cornerPointsLessSharp_,\n const pcl::PointCloud::Ptr& surfPointsFlat_,\n const pcl::PointCloud::Ptr& surfPointsLessFlat_);\n void solveLO();\n void publish();\n void output(Eigen::Quaterniond& q_w_curr_, Eigen::Vector3d& t_w_curr,\n pcl::PointCloud::Ptr& laserCloudCornerLast_,\n pcl::PointCloud::Ptr& laserCloudSurfLast_, pcl::PointCloud::Ptr& laserCloudFullRes_,\n bool& skip_frame);\n\n void TransformToStart(PointType const* const pi, PointType* const po);\n void TransformToEnd(PointType const* const pi, PointType* const po);\n\nprivate:\n const bool DISTORTION = false;\n\n int corner_correspondence, plane_correspondence;\n const double SCAN_PERIOD = 0.1;\n const double DISTANCE_SQ_THRESHOLD = 25;\n const double NEARBY_SCAN = 2.5;\n\n int mapping_skip_frame;\n bool systemInited;\n\n double timeCornerPointsSharp;\n double timeCornerPointsLessSharp;\n double timeSurfPointsFlat;\n double timeSurfPointsLessFlat;\n double timeLaserCloudFullRes;\n\n pcl::KdTreeFLANN::Ptr kdtreeCornerLast;\n pcl::KdTreeFLANN::Ptr kdtreeSurfLast;\n\n pcl::PointCloud::Ptr cornerPointsSharp;\n pcl::PointCloud::Ptr cornerPointsLessSharp;\n pcl::PointCloud::Ptr surfPointsFlat;\n pcl::PointCloud::Ptr surfPointsLessFlat;\n\n pcl::PointCloud::Ptr laserCloudCornerLast;\n pcl::PointCloud::Ptr laserCloudSurfLast;\n pcl::PointCloud::Ptr laserCloudFullRes;\n\n int laserCloudCornerLastNum;\n int laserCloudSurfLastNum;\n\n // Transformation from current frame to world frame\n Eigen::Quaterniond q_w_curr;\n Eigen::Vector3d t_w_curr;\n\n // q_curr_last(x, y, z, w), t_curr_last\n double para_q[4];\n double para_t[3];\n\n Eigen::Map q_last_curr;\n Eigen::Map t_last_curr;\n\n std::shared_ptr vloam_tf;\n\n ros::NodeHandle nh;\n int verbose_level;\n bool detach_VO_LO;\n\n ros::Publisher pubLaserCloudCornerLast;\n ros::Publisher pubLaserCloudSurfLast;\n ros::Publisher pubLaserCloudFullRes;\n ros::Publisher pubLaserOdometry;\n ros::Publisher pubLaserPath;\n\n nav_msgs::Path laserPath;\n\n int frameCount;\n};\n\n} // namespace vloam" }, { "path": "src/lidar_odometry_mapping/include/lidar_odometry_mapping/lidarFactor.hpp", "content": "// Author: Tong Qin qintonguav@gmail.com\n// \t Shaozu Cao \t\t saozu.cao@connect.ust.hk\n#pragma once\n\n#include \n#include \n#include \n#include \n#include \n#include \n\n#include \n\nstruct LidarEdgeFactor\n{\n LidarEdgeFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_a_, Eigen::Vector3d last_point_b_, double s_)\n\t: curr_point(curr_point_), last_point_a(last_point_a_), last_point_b(last_point_b_), s(s_)\n {\n }\n\n template \n bool operator()(const T *q, const T *t, T *residual) const\n {\n\tEigen::Matrix cp{ T(curr_point.x()), T(curr_point.y()), T(curr_point.z()) };\n\tEigen::Matrix lpa{ T(last_point_a.x()), T(last_point_a.y()), T(last_point_a.z()) };\n\tEigen::Matrix lpb{ T(last_point_b.x()), T(last_point_b.y()), T(last_point_b.z()) };\n\n\t// Eigen::Quaternion q_last_curr{q[3], T(s) * q[0], T(s) * q[1], T(s) * q[2]};\n\tEigen::Quaternion q_last_curr{ q[3], q[0], q[1], q[2] };\n\tEigen::Quaternion q_identity{ T(1), T(0), T(0), T(0) };\n\tq_last_curr = q_identity.slerp(T(s), q_last_curr);\n\tEigen::Matrix t_last_curr{ T(s) * t[0], T(s) * t[1], T(s) * t[2] };\n\n\tEigen::Matrix lp;\n\tlp = q_last_curr * cp + t_last_curr;\n\n\tEigen::Matrix nu = (lp - lpa).cross(lp - lpb);\n\tEigen::Matrix de = lpa - lpb;\n\n\tresidual[0] = nu.x() / de.norm();\n\tresidual[1] = nu.y() / de.norm();\n\tresidual[2] = nu.z() / de.norm();\n\n\treturn true;\n }\n\n static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_a_,\n\t\t\t\t\t\t\t\t\t const Eigen::Vector3d last_point_b_, const double s_)\n {\n\treturn (new ceres::AutoDiffCostFunction(\n\t\tnew LidarEdgeFactor(curr_point_, last_point_a_, last_point_b_, s_)));\n }\n\n Eigen::Vector3d curr_point, last_point_a, last_point_b;\n double s;\n};\n\nstruct LidarPlaneFactor\n{\n LidarPlaneFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_j_, Eigen::Vector3d last_point_l_,\n\t\t\t\t Eigen::Vector3d last_point_m_, double s_)\n\t: curr_point(curr_point_)\n\t, last_point_j(last_point_j_)\n\t, last_point_l(last_point_l_)\n\t, last_point_m(last_point_m_)\n\t, s(s_)\n {\n\tljm_norm = (last_point_j - last_point_l).cross(last_point_j - last_point_m);\n\tljm_norm.normalize();\n }\n\n template \n bool operator()(const T *q, const T *t, T *residual) const\n {\n\tEigen::Matrix cp{ T(curr_point.x()), T(curr_point.y()), T(curr_point.z()) };\n\tEigen::Matrix lpj{ T(last_point_j.x()), T(last_point_j.y()), T(last_point_j.z()) };\n\t// Eigen::Matrix lpl{T(last_point_l.x()), T(last_point_l.y()), T(last_point_l.z())};\n\t// Eigen::Matrix lpm{T(last_point_m.x()), T(last_point_m.y()), T(last_point_m.z())};\n\tEigen::Matrix ljm{ T(ljm_norm.x()), T(ljm_norm.y()), T(ljm_norm.z()) };\n\n\t// Eigen::Quaternion q_last_curr{q[3], T(s) * q[0], T(s) * q[1], T(s) * q[2]};\n\tEigen::Quaternion q_last_curr{ q[3], q[0], q[1], q[2] };\n\tEigen::Quaternion q_identity{ T(1), T(0), T(0), T(0) };\n\tq_last_curr = q_identity.slerp(T(s), q_last_curr);\n\tEigen::Matrix t_last_curr{ T(s) * t[0], T(s) * t[1], T(s) * t[2] };\n\n\tEigen::Matrix lp;\n\tlp = q_last_curr * cp + t_last_curr;\n\n\tresidual[0] = (lp - lpj).dot(ljm);\n\n\treturn true;\n }\n\n static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_j_,\n\t\t\t\t\t\t\t\t\t const Eigen::Vector3d last_point_l_, const Eigen::Vector3d last_point_m_,\n\t\t\t\t\t\t\t\t\t const double s_)\n {\n\treturn (new ceres::AutoDiffCostFunction(\n\t\tnew LidarPlaneFactor(curr_point_, last_point_j_, last_point_l_, last_point_m_, s_)));\n }\n\n Eigen::Vector3d curr_point, last_point_j, last_point_l, last_point_m;\n Eigen::Vector3d ljm_norm;\n double s;\n};\n\nstruct LidarPlaneNormFactor\n{\n LidarPlaneNormFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d plane_unit_norm_, double negative_OA_dot_norm_)\n\t: curr_point(curr_point_), plane_unit_norm(plane_unit_norm_), negative_OA_dot_norm(negative_OA_dot_norm_)\n {\n }\n\n template \n bool operator()(const T *q, const T *t, T *residual) const\n {\n\tEigen::Quaternion q_w_curr{ q[3], q[0], q[1], q[2] };\n\tEigen::Matrix t_w_curr{ t[0], t[1], t[2] };\n\tEigen::Matrix cp{ T(curr_point.x()), T(curr_point.y()), T(curr_point.z()) };\n\tEigen::Matrix point_w;\n\tpoint_w = q_w_curr * cp + t_w_curr;\n\n\tEigen::Matrix norm(T(plane_unit_norm.x()), T(plane_unit_norm.y()), T(plane_unit_norm.z()));\n\tresidual[0] = norm.dot(point_w) + T(negative_OA_dot_norm);\n\treturn true;\n }\n\n static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d plane_unit_norm_,\n\t\t\t\t\t\t\t\t\t const double negative_OA_dot_norm_)\n {\n\treturn (new ceres::AutoDiffCostFunction(\n\t\tnew LidarPlaneNormFactor(curr_point_, plane_unit_norm_, negative_OA_dot_norm_)));\n }\n\n Eigen::Vector3d curr_point;\n Eigen::Vector3d plane_unit_norm;\n double negative_OA_dot_norm;\n};\n\nstruct LidarDistanceFactor\n{\n LidarDistanceFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d closed_point_)\n\t: curr_point(curr_point_), closed_point(closed_point_)\n {\n }\n\n template \n bool operator()(const T *q, const T *t, T *residual) const\n {\n\tEigen::Quaternion q_w_curr{ q[3], q[0], q[1], q[2] };\n\tEigen::Matrix t_w_curr{ t[0], t[1], t[2] };\n\tEigen::Matrix cp{ T(curr_point.x()), T(curr_point.y()), T(curr_point.z()) };\n\tEigen::Matrix point_w;\n\tpoint_w = q_w_curr * cp + t_w_curr;\n\n\tresidual[0] = point_w.x() - T(closed_point.x());\n\tresidual[1] = point_w.y() - T(closed_point.y());\n\tresidual[2] = point_w.z() - T(closed_point.z());\n\treturn true;\n }\n\n static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d closed_point_)\n {\n\treturn (new ceres::AutoDiffCostFunction(\n\t\tnew LidarDistanceFactor(curr_point_, closed_point_)));\n }\n\n Eigen::Vector3d curr_point;\n Eigen::Vector3d closed_point;\n};" }, { "path": "src/lidar_odometry_mapping/include/lidar_odometry_mapping/lidar_odometry_mapping.h", "content": "// This is an advanced implementation of the algorithm described in the following paper:\n// J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time.\n// Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014.\n\n// Modifier: Tong Qin qintonguav@gmail.com\n// \t Shaozu Cao \t\t saozu.cao@connect.ust.hk\n\n// Copyright 2013, Ji Zhang, Carnegie Mellon University\n// Further contributions copyright (c) 2016, Southwest Research Institute\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are met:\n//\n// 1. Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// 2. Redistributions in binary form must reproduce the above copyright notice,\n// this list of conditions and the following disclaimer in the documentation\n// and/or other materials provided with the distribution.\n// 3. Neither the name of the copyright holder nor the names of its\n// contributors may be used to endorse or promote products derived from this\n// software without specific prior written permission.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\"\n// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE\n// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE\n// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE\n// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR\n// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF\n// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS\n// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN\n// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)\n// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n// POSSIBILITY OF SUCH DAMAGE.\n\n#pragma once\n\n#include \n#include \n#include \n#include \n\nnamespace vloam\n{\nclass LidarOdometryMapping\n{\npublic:\n LidarOdometryMapping() : nh(\"lidar_odometry_mapping_node\")\n {\n }\n\n void init(std::shared_ptr& vloam_tf_);\n\n void reset();\n\n void scanRegistrationIO(const pcl::PointCloud& laserCloudIn);\n void laserOdometryIO();\n // void laserOdometryIO(const tf2::Transform& cam0_curr_T_cam0_last);\n void laserMappingIO();\n\nprivate:\n std::shared_ptr vloam_tf;\n\n ros::NodeHandle nh;\n int verbose_level;\n\n TicToc loam_timer;\n double frame_time;\n\n ScanRegistration scan_registration;\n pcl::PointCloud::Ptr laserCloud;\n pcl::PointCloud::Ptr cornerPointsSharp;\n pcl::PointCloud::Ptr cornerPointsLessSharp;\n pcl::PointCloud::Ptr surfPointsFlat;\n pcl::PointCloud::Ptr surfPointsLessFlat;\n\n LaserOdometry laser_odometry;\n Eigen::Quaterniond q_wodom_curr, q_w_curr;\n Eigen::Vector3d t_wodom_curr, t_w_curr;\n pcl::PointCloud::Ptr laserCloudCornerLast;\n pcl::PointCloud::Ptr laserCloudSurfLast;\n pcl::PointCloud::Ptr laserCloudFullRes;\n bool skip_frame;\n\n LaserMapping laser_mapping;\n};\n\n} // namespace vloam" }, { "path": "src/lidar_odometry_mapping/include/lidar_odometry_mapping/scan_registration.h", "content": "// This is an advanced implementation of the algorithm described in the following paper:\n// J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time.\n// Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014.\n\n// Modifier: Tong Qin qintonguav@gmail.com\n// \t Shaozu Cao \t\t saozu.cao@connect.ust.hk\n\n// Copyright 2013, Ji Zhang, Carnegie Mellon University\n// Further contributions copyright (c) 2016, Southwest Research Institute\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are met:\n//\n// 1. Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// 2. Redistributions in binary form must reproduce the above copyright notice,\n// this list of conditions and the following disclaimer in the documentation\n// and/or other materials provided with the distribution.\n// 3. Neither the name of the copyright holder nor the names of its\n// contributors may be used to endorse or promote products derived from this\n// software without specific prior written permission.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\"\n// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE\n// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE\n// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE\n// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR\n// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF\n// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS\n// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN\n// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)\n// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n// POSSIBILITY OF SUCH DAMAGE.\n\n#pragma once\n\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\n#include \n#include \n#include \n#include \n\n#include \"lidar_odometry_mapping/common.h\"\n#include \"lidar_odometry_mapping/tic_toc.h\"\n\nusing std::atan2;\nusing std::cos;\nusing std::sin;\n\nnamespace vloam\n{\nclass ScanRegistration\n{\npublic:\n ScanRegistration() : nh(\"scan_registration_node\")\n {\n }\n\n void init();\n void reset();\n void input(const pcl::PointCloud& laserCloudIn_);\n void publish();\n void output(pcl::PointCloud::Ptr& laserCloud_, pcl::PointCloud::Ptr& cornerPointsSharp_,\n pcl::PointCloud::Ptr& cornerPointsLessSharp_, pcl::PointCloud::Ptr& surfPointsFlat_,\n pcl::PointCloud::Ptr& surfPointsLessFlat_);\n\n // bool comp (int i,int j) { return (cloudCurvature[i]\n void removeClosedPointCloud(const pcl::PointCloud& cloud_in, pcl::PointCloud& cloud_out, float thres);\n\nprivate:\n const double scanPeriod = 0.1;\n\n const int systemDelay = 0;\n int systemInitCount;\n bool systemInited;\n int N_SCANS;\n float cloudCurvature[400000];\n int cloudSortInd[400000];\n int cloudNeighborPicked[400000];\n int cloudLabel[400000];\n\n ros::NodeHandle nh;\n int verbose_level;\n\n // ros::Subscriber subLaserCloud;\n ros::Publisher pubLaserCloud;\n ros::Publisher pubCornerPointsSharp;\n ros::Publisher pubCornerPointsLessSharp;\n ros::Publisher pubSurfPointsFlat;\n ros::Publisher pubSurfPointsLessFlat;\n // ros::Publisher pubRemovePoints;\n std::vector pubEachScan;\n\n bool PUB_EACH_LINE;\n\n double MINIMUM_RANGE = 0.1;\n\n pcl::PointCloud::Ptr laserCloud;\n pcl::PointCloud::Ptr cornerPointsSharp;\n pcl::PointCloud::Ptr cornerPointsLessSharp;\n pcl::PointCloud::Ptr surfPointsFlat;\n pcl::PointCloud::Ptr surfPointsLessFlat;\n\n std::vector> laserCloudScans;\n\n TicToc t_whole;\n TicToc t_prepare;\n};\n\n} // namespace vloam" }, { "path": "src/lidar_odometry_mapping/include/lidar_odometry_mapping/tic_toc.h", "content": "// Author: Tong Qin qintonguav@gmail.com\n// \t Shaozu Cao \t\t saozu.cao@connect.ust.hk\n\n#pragma once\n\n#include \n#include \n#include \n\nclass TicToc\n{\npublic:\n TicToc()\n {\n tic();\n }\n\n void tic()\n {\n start = std::chrono::system_clock::now();\n }\n\n double toc()\n {\n end = std::chrono::system_clock::now();\n std::chrono::duration elapsed_seconds = end - start;\n return elapsed_seconds.count() * 1000;\n }\n\nprivate:\n std::chrono::time_point start, end;\n};\n" }, { "path": "src/lidar_odometry_mapping/launch/loam_velodyne_HDL_32.launch", "content": "\n \n \n\n \n \n\n \n \n\n\n \n \n\n \n\n \n\n \n\n \n \n \n \n\n\n" }, { "path": "src/lidar_odometry_mapping/launch/loam_velodyne_HDL_64.launch", "content": "\n \n \n\n \n \n\n \n \n\n\n \n \n\n \n\n \n\n \n\n \n \n \n \n\n\n" }, { "path": "src/lidar_odometry_mapping/launch/loam_velodyne_HDL_64_kitti.launch", "content": "\n \n \n \n \n \n\n \n \n \n\n \n \n\n \n \n\n \n \n \n \n\n\n" }, { "path": "src/lidar_odometry_mapping/launch/loam_velodyne_VLP_16.launch", "content": "\n \n \n\n \n \n\n \n \n\n\n \n \n\n \n\n \n\n \n\n \n \n \n \n\n\n" }, { "path": "src/lidar_odometry_mapping/package.xml", "content": "\n\n lidar_odometry_mapping\n 0.0.0\n\n \n This is an advanced implentation of LOAM.\n LOAM is described in the following paper:\n J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time.\n Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014.\n \n\n qintong\n\n BSD\n\n Ji Zhang\n \n catkin\n geometry_msgs\n nav_msgs\n roscpp\n rospy\n std_msgs \n rosbag\n sensor_msgs\n tf\n image_transport\n vloam_tf\n \n geometry_msgs\n nav_msgs\n sensor_msgs\n roscpp\n rospy\n std_msgs\n rosbag\n tf\n image_transport\n vloam_tf\n\n \n \n\n" }, { "path": "src/lidar_odometry_mapping/rviz_cfg/lidar_odometry_mapping.rviz", "content": "Panels:\n - Class: rviz/Displays\n Help Height: 78\n Name: Displays\n Property Tree Widget:\n Expanded:\n - /Global Options1\n - /Status1\n - /odometry1\n - /odometry1/odomPath1\n - /odometry1/PointCloud21\n - /mapping1\n - /scan1\n - /TF1\n - /TF1/Status1\n Splitter Ratio: 0.5\n Tree Height: 531\n - Class: rviz/Selection\n Name: Selection\n - Class: rviz/Tool Properties\n Expanded:\n - /2D Pose Estimate1\n - /2D Nav Goal1\n - /Publish Point1\n Name: Tool Properties\n Splitter Ratio: 0.5886790156364441\n - Class: rviz/Views\n Expanded:\n - /Current View1\n Name: Views\n Splitter Ratio: 0.5\n - Class: rviz/Time\n Experimental: false\n Name: Time\n SyncMode: 0\n SyncSource: surround\nPreferences:\n PromptSaveOnExit: true\nToolbars:\n toolButtonStyle: 2\nVisualization Manager:\n Class: \"\"\n Displays:\n - Alpha: 0.5\n Cell Size: 1\n Class: rviz/Grid\n Color: 160; 160; 164\n Enabled: false\n Line Style:\n Line Width: 0.029999999329447746\n Value: Lines\n Name: Grid\n Normal Cell Count: 0\n Offset:\n X: 0\n Y: 0\n Z: 0\n Plane: XY\n Plane Cell Count: 160\n Reference Frame: \n Value: false\n - Class: rviz/Axes\n Enabled: true\n Length: 1\n Name: Axes\n Radius: 0.10000000149011612\n Reference Frame: \n Value: true\n - Class: rviz/Group\n Displays:\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 255; 170; 0\n Enabled: false\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Lines\n Line Width: 0.029999999329447746\n Name: gtPathlc\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /path_gt\n Unreliable: false\n Value: false\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 255; 170; 0\n Enabled: true\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Lines\n Line Width: 0.029999999329447746\n Name: gtPathLidar\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /path_gt_lidar\n Unreliable: false\n Value: true\n Enabled: false\n Name: GT\n - Class: rviz/Group\n Displays:\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 255; 85; 0\n Enabled: true\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Lines\n Line Width: 0.10000000149011612\n Name: odomPath\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /laser_odom_path\n Unreliable: false\n Value: true\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud2\n Color: 255; 255; 255\n Color Transformer: FlatColor\n Decay Time: 0\n Enabled: false\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Max Intensity: 63.10047912597656\n Min Color: 0; 0; 0\n Min Intensity: -0.0067862942814826965\n Name: PointCloud2\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 3\n Size (m): 0.05000000074505806\n Style: Flat Squares\n Topic: /velodyne_cloud_2\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: false\n Enabled: true\n Name: odometry\n - Class: rviz/Group\n Displays:\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 25; 255; 0\n Enabled: true\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Lines\n Line Width: 0.029999999329447746\n Name: mappingPath\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /aft_mapped_path\n Unreliable: false\n Value: true\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud2\n Color: 255; 255; 255\n Color Transformer: FlatColor\n Decay Time: 0\n Enabled: true\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Max Intensity: -999999\n Min Color: 0; 0; 0\n Min Intensity: 999999\n Name: allMapCloud\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 3\n Size (m): 0.05000000074505806\n Style: Flat Squares\n Topic: /laser_cloud_map\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: true\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud2\n Color: 255; 255; 255\n Color Transformer: FlatColor\n Decay Time: 0\n Enabled: true\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Max Intensity: 15\n Min Color: 0; 0; 0\n Min Intensity: 0\n Name: surround\n Position Transformer: XYZ\n Queue Size: 1\n Selectable: true\n Size (Pixels): 1\n Size (m): 0.05000000074505806\n Style: Squares\n Topic: /laser_cloud_surround\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: true\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud2\n Color: 255; 255; 255\n Color Transformer: Intensity\n Decay Time: 0\n Enabled: true\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Max Intensity: 47.11855697631836\n Min Color: 0; 0; 0\n Min Intensity: -0.014337587170302868\n Name: currPoints\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 2\n Size (m): 0.009999999776482582\n Style: Points\n Topic: /velodyne_cloud_registered\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: true\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud2\n Color: 255; 0; 0\n Color Transformer: FlatColor\n Decay Time: 0\n Enabled: false\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Max Intensity: 63.03843307495117\n Min Color: 255; 0; 0\n Min Intensity: -0.006431261543184519\n Name: corner\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 3\n Size (m): 0.20000000298023224\n Style: Flat Squares\n Topic: /mapping_corner\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: false\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud2\n Color: 255; 0; 0\n Color Transformer: FlatColor\n Decay Time: 0\n Enabled: false\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Max Intensity: 63.0818977355957\n Min Color: 0; 0; 0\n Min Intensity: -0.005241604056209326\n Name: surf\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 3\n Size (m): 0.20000000298023224\n Style: Flat Squares\n Topic: /mapping_surf\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: false\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud2\n Color: 0; 255; 0\n Color Transformer: FlatColor\n Decay Time: 0\n Enabled: false\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Max Intensity: 17.08800506591797\n Min Color: 0; 0; 0\n Min Intensity: -0.006790489889681339\n Name: used_corner\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 3\n Size (m): 0.5\n Style: Flat Squares\n Topic: /mapping_used_corner\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: false\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud2\n Color: 0; 255; 0\n Color Transformer: FlatColor\n Decay Time: 0\n Enabled: false\n Invert Rainbow: false\n Max Color: 0; 255; 0\n Max Intensity: 63.0818977355957\n Min Color: 0; 0; 0\n Min Intensity: -0.005241604056209326\n Name: used_surf\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 3\n Size (m): 0.5\n Style: Flat Squares\n Topic: /mapping_used_surf\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: false\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud2\n Color: 255; 255; 255\n Color Transformer: FlatColor\n Decay Time: 0\n Enabled: false\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Max Intensity: 21.062847137451172\n Min Color: 0; 0; 0\n Min Intensity: -0.006625724025070667\n Name: map_corner\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 3\n Size (m): 0.10000000149011612\n Style: Flat Squares\n Topic: /mapping_map_corner\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: false\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud2\n Color: 255; 255; 255\n Color Transformer: FlatColor\n Decay Time: 0\n Enabled: false\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Max Intensity: 63.08392333984375\n Min Color: 0; 0; 0\n Min Intensity: -0.00876065157353878\n Name: map_surf\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 3\n Size (m): 0.10000000149011612\n Style: Flat Squares\n Topic: /mapping_map_surf\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: false\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 25; 255; 0\n Enabled: false\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Lines\n Line Width: 0.029999999329447746\n Name: leftcameraPath\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /lc_path\n Unreliable: false\n Value: false\n Enabled: true\n Name: mapping\n - Class: rviz/Group\n Displays:\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud2\n Color: 255; 255; 255\n Color Transformer: FlatColor\n Decay Time: 0\n Enabled: false\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Max Intensity: 15\n Min Color: 0; 0; 0\n Min Intensity: 0\n Name: sharp\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 3\n Size (m): 0.009999999776482582\n Style: Points\n Topic: /laser_cloud_sharp\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: false\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud2\n Color: 255; 255; 255\n Color Transformer: Intensity\n Decay Time: 0\n Enabled: false\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Max Intensity: 63.03987121582031\n Min Color: 0; 0; 0\n Min Intensity: 0.0263746976852417\n Name: flat\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 3\n Size (m): 0.05000000074505806\n Style: Flat Squares\n Topic: /laser_cloud_flat\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: false\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud2\n Color: 255; 255; 255\n Color Transformer: FlatColor\n Decay Time: 0\n Enabled: true\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Max Intensity: 0.9900000095367432\n Min Color: 0; 0; 0\n Min Intensity: 0\n Name: raw_data\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 3\n Size (m): 0.05000000074505806\n Style: Flat Squares\n Topic: /velodyne_points\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: true\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud2\n Color: 255; 0; 0\n Color Transformer: FlatColor\n Decay Time: 0\n Enabled: false\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Max Intensity: 0.12361729145050049\n Min Color: 0; 0; 0\n Min Intensity: -0.00711920578032732\n Name: scan\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 3\n Size (m): 0.20000000298023224\n Style: Flat Squares\n Topic: /laser_scanid_63\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: false\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud2\n Color: 255; 85; 0\n Color Transformer: FlatColor\n Decay Time: 0\n Enabled: false\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Max Intensity: 62.07567596435547\n Min Color: 0; 0; 0\n Min Intensity: -0.007054195739328861\n Name: removPoints\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 3\n Size (m): 0.20000000298023224\n Style: Flat Squares\n Topic: /laser_remove_points\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: false\n Enabled: false\n Name: scan\n - Class: rviz/Image\n Enabled: true\n Image Topic: /kitti/image\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: Image\n Normalize Range: true\n Queue Size: 2\n Transport Hint: raw\n Unreliable: false\n Value: true\n - Class: rviz/TF\n Enabled: true\n Frame Timeout: 15\n Frames:\n All Enabled: true\n aft_mapped:\n Value: true\n base_link:\n Value: true\n camera_color_left:\n Value: true\n camera_color_right:\n Value: true\n camera_gray_left:\n Value: true\n camera_gray_right:\n Value: true\n camera_init:\n Value: true\n imu_link:\n Value: true\n velo_link:\n Value: true\n world:\n Value: true\n Marker Scale: 1\n Name: TF\n Show Arrows: true\n Show Axes: true\n Show Names: true\n Tree:\n camera_init:\n aft_mapped:\n {}\n Update Interval: 0\n Value: true\n Enabled: true\n Global Options:\n Background Color: 0; 0; 0\n Default Light: true\n Fixed Frame: camera_init\n Frame Rate: 30\n Name: root\n Tools:\n - Class: rviz/Interact\n Hide Inactive Objects: true\n - Class: rviz/MoveCamera\n - Class: rviz/Select\n - Class: rviz/FocusCamera\n - Class: rviz/Measure\n - Class: rviz/SetInitialPose\n Theta std deviation: 0.2617993950843811\n Topic: /initialpose\n X std deviation: 0.5\n Y std deviation: 0.5\n - Class: rviz/SetGoal\n Topic: /move_base_simple/goal\n - Class: rviz/PublishPoint\n Single click: true\n Topic: /clicked_point\n Value: true\n Views:\n Current:\n Class: rviz/XYOrbit\n Distance: 29.97575569152832\n Enable Stereo Rendering:\n Stereo Eye Separation: 0.05999999865889549\n Stereo Focal Distance: 1\n Swap Stereo Eyes: false\n Value: false\n Focal Point:\n X: 102.75230407714844\n Y: -5.920174598693848\n Z: -2.309799909591675\n Focal Shape Fixed Size: true\n Focal Shape Size: 0.05000000074505806\n Invert Z Axis: false\n Name: Current View\n Near Clip Distance: 0.009999999776482582\n Pitch: 0.30479696393013\n Target Frame: body\n Value: XYOrbit (rviz)\n Yaw: 3.1050009727478027\n Saved: ~\nWindow Geometry:\n Displays:\n collapsed: false\n Height: 1025\n Hide Left Dock: false\n Hide Right Dock: true\n Image:\n collapsed: false\n QMainWindow State: 000000ff00000000fd00000004000000000000021b00000363fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000029e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002e1000000bf0000001600ffffff000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000051c0000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000\n Selection:\n collapsed: false\n Time:\n collapsed: false\n Tool Properties:\n collapsed: false\n Views:\n collapsed: true\n Width: 1853\n X: 67\n Y: 27\n" }, { "path": "src/lidar_odometry_mapping/src/laser_mapping.cpp", "content": "// This is an advanced implementation of the algorithm described in the following paper:\n// J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time.\n// Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014.\n\n// Modifier: Tong Qin qintonguav@gmail.com\n// \t Shaozu Cao \t\t saozu.cao@connect.ust.hk\n\n// Copyright 2013, Ji Zhang, Carnegie Mellon University\n// Further contributions copyright (c) 2016, Southwest Research Institute\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are met:\n//\n// 1. Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// 2. Redistributions in binary form must reproduce the above copyright notice,\n// this list of conditions and the following disclaimer in the documentation\n// and/or other materials provided with the distribution.\n// 3. Neither the name of the copyright holder nor the names of its\n// contributors may be used to endorse or promote products derived from this\n// software without specific prior written permission.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\"\n// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE\n// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE\n// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE\n// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR\n// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF\n// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS\n// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN\n// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)\n// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n// POSSIBILITY OF SUCH DAMAGE.\n\n#include \n\nnamespace vloam\n{\nvoid LaserMapping::init(std::shared_ptr &vloam_tf_)\n{\n vloam_tf = vloam_tf_;\n\n if (!ros::param::get(\"loam_verbose_level\", verbose_level))\n ROS_BREAK();\n\n frameCount = 0;\n\n // input: from odom\n laserCloudCornerLast = boost::make_shared>();\n laserCloudSurfLast = boost::make_shared>();\n\n // ouput: all visualble cube points\n laserCloudSurround = boost::make_shared>();\n\n // surround points in map to build tree\n laserCloudCornerFromMap = boost::make_shared>();\n laserCloudSurfFromMap = boost::make_shared>();\n\n // input & output: points in one frame. local --> global\n laserCloudFullRes = boost::make_shared>();\n\n // points in every cube\n for (int j = 0; j < laserCloudNum; ++j)\n {\n laserCloudCornerArray[j] = boost::make_shared>();\n laserCloudSurfArray[j] = boost::make_shared>();\n }\n\n // kd-tree\n kdtreeCornerFromMap = boost::make_shared>();\n kdtreeSurfFromMap = boost::make_shared>();\n\n parameters[0] = 0.0;\n parameters[1] = 0.0;\n parameters[2] = 0.0;\n parameters[3] = 1.0;\n parameters[4] = 0.0;\n parameters[5] = 0.0;\n parameters[6] = 0.0;\n\n new (&q_w_curr) Eigen::Map(parameters);\n new (&t_w_curr) Eigen::Map(parameters + 4);\n\n // wmap_T_odom * odom_T_curr = wmap_T_curr;\n // transformation between odom's world and map's world frame\n q_wmap_wodom = Eigen::Quaterniond(1, 0, 0, 0);\n t_wmap_wodom = Eigen::Vector3d(0, 0, 0);\n\n q_wodom_curr = Eigen::Quaterniond(1, 0, 0, 0);\n t_wodom_curr = Eigen::Vector3d(0, 0, 0);\n\n lineRes = 0;\n planeRes = 0;\n if (!ros::param::get(\"mapping_line_resolution\", lineRes))\n ROS_BREAK();\n if (!ros::param::get(\"mapping_plane_resolution\", planeRes))\n ROS_BREAK();\n ROS_INFO(\"line resolution %f plane resolution %f \\n\", lineRes, planeRes);\n downSizeFilterCorner.setLeafSize(lineRes, lineRes, lineRes);\n downSizeFilterSurf.setLeafSize(planeRes, planeRes, planeRes);\n\n pubLaserCloudSurround = nh.advertise(\"/laser_cloud_surround\", 100);\n pubLaserCloudMap = nh.advertise(\"/laser_cloud_map\", 100);\n pubLaserCloudFullRes = nh.advertise(\"/velodyne_cloud_registered\", 100);\n pubOdomAftMapped = nh.advertise(\"/aft_mapped_to_init\", 100);\n // pubOdomAftMappedHighFrec = nh->advertise(\"/aft_mapped_to_init_high_frec\", 100);\n pubLaserAfterMappedPath = nh.advertise(\"/aft_mapped_path\", 100);\n\n laserAfterMappedPath.poses.clear();\n\n // for (int i = 0; i < laserCloudNum; i++)\n // {\n // \tlaserCloudCornerArray[i].reset(new pcl::PointCloud());\n // \tlaserCloudSurfArray[i].reset(new pcl::PointCloud());\n // } // TOOD: check when this is needed\n\n laserCloudValidNum = 0;\n laserCloudSurroundNum = 0;\n\n if (!ros::param::get(\"mapping_skip_frame\", mapping_skip_frame))\n ROS_BREAK();\n if (!ros::param::get(\"map_pub_number\", map_pub_number))\n ROS_BREAK();\n}\n\nvoid LaserMapping::reset()\n{\n laserCloudValidNum = 0;\n laserCloudSurroundNum = 0;\n}\n\n// // set initial guess\n// void LaserMapping::transformAssociateToMap()\n// {\n// q_w_curr = q_wmap_wodom * q_wodom_curr;\n// t_w_curr = q_wmap_wodom * t_wodom_curr + t_wmap_wodom;\n// }\n\nvoid LaserMapping::transformUpdate()\n{\n q_wmap_wodom = q_w_curr * q_wodom_curr.inverse();\n t_wmap_wodom = t_w_curr - q_wmap_wodom * t_wodom_curr;\n}\n\nvoid LaserMapping::pointAssociateToMap(PointType const *const pi, PointType *const po)\n{\n Eigen::Vector3d point_curr(pi->x, pi->y, pi->z);\n Eigen::Vector3d point_w = q_w_curr * point_curr + t_w_curr;\n po->x = point_w.x();\n po->y = point_w.y();\n po->z = point_w.z();\n po->intensity = pi->intensity;\n // po->intensity = 1.0;\n}\n\nvoid LaserMapping::pointAssociateTobeMapped(PointType const *const pi, PointType *const po)\n{\n Eigen::Vector3d point_w(pi->x, pi->y, pi->z);\n Eigen::Vector3d point_curr = q_w_curr.inverse() * (point_w - t_w_curr);\n po->x = point_curr.x();\n po->y = point_curr.y();\n po->z = point_curr.z();\n po->intensity = pi->intensity;\n}\n\nvoid LaserMapping::input(const pcl::PointCloud::Ptr &laserCloudCornerLast_,\n const pcl::PointCloud::Ptr &laserCloudSurfLast_,\n const pcl::PointCloud::Ptr &laserCloudFullRes_,\n const Eigen::Quaterniond &q_wodom_curr_, const Eigen::Vector3d &t_wodom_curr_,\n const bool &skip_frame_)\n{\n skip_frame = skip_frame_;\n\n if (!skip_frame)\n {\n laserCloudCornerLast = boost::make_shared>(*laserCloudCornerLast_);\n laserCloudSurfLast = boost::make_shared>(*laserCloudSurfLast_);\n laserCloudFullRes = boost::make_shared>(*laserCloudFullRes_);\n }\n\n q_wodom_curr = q_wodom_curr_;\n t_wodom_curr = t_wodom_curr_;\n\n // transformAssociateToMap\n if (skip_frame)\n {\n q_w_curr_highfreq = q_wmap_wodom * q_wodom_curr;\n t_w_curr_highfreq = q_wmap_wodom * t_wodom_curr + t_wmap_wodom;\n }\n else\n {\n q_w_curr = q_wmap_wodom * q_wodom_curr;\n t_w_curr = q_wmap_wodom * t_wodom_curr + t_wmap_wodom;\n }\n}\n\nvoid LaserMapping::solveMapping()\n{\n // this->reset();\n\n t_whole.tic();\n\n // transformAssociateToMap();\n\n TicToc t_shift;\n int centerCubeI = int((t_w_curr.x() + 25.0) / 50.0) + laserCloudCenWidth;\n int centerCubeJ = int((t_w_curr.y() + 25.0) / 50.0) + laserCloudCenHeight;\n int centerCubeK = int((t_w_curr.z() + 25.0) / 50.0) + laserCloudCenDepth;\n\n if (t_w_curr.x() + 25.0 < 0)\n centerCubeI--;\n if (t_w_curr.y() + 25.0 < 0)\n centerCubeJ--;\n if (t_w_curr.z() + 25.0 < 0)\n centerCubeK--;\n\n while (centerCubeI < 3)\n {\n for (int j = 0; j < laserCloudHeight; j++)\n {\n for (int k = 0; k < laserCloudDepth; k++)\n {\n int i = laserCloudWidth - 1;\n pcl::PointCloud::Ptr laserCloudCubeCornerPointer =\n laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];\n pcl::PointCloud::Ptr laserCloudCubeSurfPointer =\n laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];\n for (; i >= 1; i--)\n {\n laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =\n laserCloudCornerArray[i - 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];\n laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =\n laserCloudSurfArray[i - 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];\n }\n laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =\n laserCloudCubeCornerPointer;\n laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =\n laserCloudCubeSurfPointer;\n laserCloudCubeCornerPointer->clear();\n laserCloudCubeSurfPointer->clear();\n }\n }\n\n centerCubeI++;\n laserCloudCenWidth++;\n }\n\n while (centerCubeI >= laserCloudWidth - 3)\n {\n for (int j = 0; j < laserCloudHeight; j++)\n {\n for (int k = 0; k < laserCloudDepth; k++)\n {\n int i = 0;\n pcl::PointCloud::Ptr laserCloudCubeCornerPointer =\n laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];\n pcl::PointCloud::Ptr laserCloudCubeSurfPointer =\n laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];\n for (; i < laserCloudWidth - 1; i++)\n {\n laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =\n laserCloudCornerArray[i + 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];\n laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =\n laserCloudSurfArray[i + 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];\n }\n laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =\n laserCloudCubeCornerPointer;\n laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =\n laserCloudCubeSurfPointer;\n laserCloudCubeCornerPointer->clear();\n laserCloudCubeSurfPointer->clear();\n }\n }\n\n centerCubeI--;\n laserCloudCenWidth--;\n }\n\n while (centerCubeJ < 3)\n {\n for (int i = 0; i < laserCloudWidth; i++)\n {\n for (int k = 0; k < laserCloudDepth; k++)\n {\n int j = laserCloudHeight - 1;\n pcl::PointCloud::Ptr laserCloudCubeCornerPointer =\n laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];\n pcl::PointCloud::Ptr laserCloudCubeSurfPointer =\n laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];\n for (; j >= 1; j--)\n {\n laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =\n laserCloudCornerArray[i + laserCloudWidth * (j - 1) + laserCloudWidth * laserCloudHeight * k];\n laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =\n laserCloudSurfArray[i + laserCloudWidth * (j - 1) + laserCloudWidth * laserCloudHeight * k];\n }\n laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =\n laserCloudCubeCornerPointer;\n laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =\n laserCloudCubeSurfPointer;\n laserCloudCubeCornerPointer->clear();\n laserCloudCubeSurfPointer->clear();\n }\n }\n\n centerCubeJ++;\n laserCloudCenHeight++;\n }\n\n while (centerCubeJ >= laserCloudHeight - 3)\n {\n for (int i = 0; i < laserCloudWidth; i++)\n {\n for (int k = 0; k < laserCloudDepth; k++)\n {\n int j = 0;\n pcl::PointCloud::Ptr laserCloudCubeCornerPointer =\n laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];\n pcl::PointCloud::Ptr laserCloudCubeSurfPointer =\n laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];\n for (; j < laserCloudHeight - 1; j++)\n {\n laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =\n laserCloudCornerArray[i + laserCloudWidth * (j + 1) + laserCloudWidth * laserCloudHeight * k];\n laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =\n laserCloudSurfArray[i + laserCloudWidth * (j + 1) + laserCloudWidth * laserCloudHeight * k];\n }\n laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =\n laserCloudCubeCornerPointer;\n laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =\n laserCloudCubeSurfPointer;\n laserCloudCubeCornerPointer->clear();\n laserCloudCubeSurfPointer->clear();\n }\n }\n\n centerCubeJ--;\n laserCloudCenHeight--;\n }\n\n while (centerCubeK < 3)\n {\n for (int i = 0; i < laserCloudWidth; i++)\n {\n for (int j = 0; j < laserCloudHeight; j++)\n {\n int k = laserCloudDepth - 1;\n pcl::PointCloud::Ptr laserCloudCubeCornerPointer =\n laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];\n pcl::PointCloud::Ptr laserCloudCubeSurfPointer =\n laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];\n for (; k >= 1; k--)\n {\n laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =\n laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * (k - 1)];\n laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =\n laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * (k - 1)];\n }\n laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =\n laserCloudCubeCornerPointer;\n laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =\n laserCloudCubeSurfPointer;\n laserCloudCubeCornerPointer->clear();\n laserCloudCubeSurfPointer->clear();\n }\n }\n\n centerCubeK++;\n laserCloudCenDepth++;\n }\n\n while (centerCubeK >= laserCloudDepth - 3)\n {\n for (int i = 0; i < laserCloudWidth; i++)\n {\n for (int j = 0; j < laserCloudHeight; j++)\n {\n int k = 0;\n pcl::PointCloud::Ptr laserCloudCubeCornerPointer =\n laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];\n pcl::PointCloud::Ptr laserCloudCubeSurfPointer =\n laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];\n for (; k < laserCloudDepth - 1; k++)\n {\n laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =\n laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * (k + 1)];\n laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =\n laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * (k + 1)];\n }\n laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =\n laserCloudCubeCornerPointer;\n laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =\n laserCloudCubeSurfPointer;\n laserCloudCubeCornerPointer->clear();\n laserCloudCubeSurfPointer->clear();\n }\n }\n\n centerCubeK--;\n laserCloudCenDepth--;\n }\n\n for (int i = centerCubeI - 2; i <= centerCubeI + 2; i++)\n {\n for (int j = centerCubeJ - 2; j <= centerCubeJ + 2; j++)\n {\n for (int k = centerCubeK - 1; k <= centerCubeK + 1; k++)\n {\n if (i >= 0 && i < laserCloudWidth && j >= 0 && j < laserCloudHeight && k >= 0 && k < laserCloudDepth)\n {\n laserCloudValidInd[laserCloudValidNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k;\n laserCloudValidNum++;\n laserCloudSurroundInd[laserCloudSurroundNum] =\n i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k;\n laserCloudSurroundNum++;\n }\n }\n }\n }\n\n laserCloudCornerFromMap->clear();\n laserCloudSurfFromMap->clear();\n for (int i = 0; i < laserCloudValidNum; i++)\n {\n *laserCloudCornerFromMap += *laserCloudCornerArray[laserCloudValidInd[i]];\n *laserCloudSurfFromMap += *laserCloudSurfArray[laserCloudValidInd[i]];\n }\n int laserCloudCornerFromMapNum = laserCloudCornerFromMap->points.size();\n int laserCloudSurfFromMapNum = laserCloudSurfFromMap->points.size();\n\n pcl::PointCloud::Ptr laserCloudCornerStack(new pcl::PointCloud());\n downSizeFilterCorner.setInputCloud(laserCloudCornerLast);\n downSizeFilterCorner.filter(*laserCloudCornerStack);\n int laserCloudCornerStackNum = laserCloudCornerStack->points.size();\n\n pcl::PointCloud::Ptr laserCloudSurfStack(new pcl::PointCloud());\n downSizeFilterSurf.setInputCloud(laserCloudSurfLast);\n downSizeFilterSurf.filter(*laserCloudSurfStack);\n int laserCloudSurfStackNum = laserCloudSurfStack->points.size();\n\n if (verbose_level > 1)\n {\n ROS_INFO(\"map prepare time %f ms\\n\", t_shift.toc());\n ROS_INFO(\"map corner num %d surf num %d \\n\", laserCloudCornerFromMapNum, laserCloudSurfFromMapNum);\n }\n\n if (laserCloudCornerFromMapNum > 10 && laserCloudSurfFromMapNum > 50)\n {\n TicToc t_opt;\n TicToc t_tree;\n kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMap);\n kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMap);\n\n if (verbose_level > 1)\n ROS_INFO(\"build tree time %f ms \\n\", t_tree.toc());\n\n for (int iterCount = 0; iterCount < 2; iterCount++)\n {\n // ceres::LossFunction *loss_function = NULL;\n ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);\n ceres::LocalParameterization *q_parameterization = new ceres::EigenQuaternionParameterization();\n ceres::Problem::Options problem_options;\n\n ceres::Problem problem(problem_options);\n problem.AddParameterBlock(parameters, 4, q_parameterization);\n problem.AddParameterBlock(parameters + 4, 3);\n\n TicToc t_data;\n int corner_num = 0;\n\n for (int i = 0; i < laserCloudCornerStackNum; i++)\n {\n pointOri = laserCloudCornerStack->points[i];\n // double sqrtDis = pointOri.x * pointOri.x + pointOri.y * pointOri.y + pointOri.z * pointOri.z;\n pointAssociateToMap(&pointOri, &pointSel);\n kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);\n\n if (pointSearchSqDis[4] < 1.0)\n {\n std::vector nearCorners;\n Eigen::Vector3d center(0, 0, 0);\n for (int j = 0; j < 5; j++)\n {\n Eigen::Vector3d tmp(laserCloudCornerFromMap->points[pointSearchInd[j]].x,\n laserCloudCornerFromMap->points[pointSearchInd[j]].y,\n laserCloudCornerFromMap->points[pointSearchInd[j]].z);\n center = center + tmp;\n nearCorners.push_back(tmp);\n }\n center = center / 5.0;\n\n Eigen::Matrix3d covMat = Eigen::Matrix3d::Zero();\n for (int j = 0; j < 5; j++)\n {\n Eigen::Matrix tmpZeroMean = nearCorners[j] - center;\n covMat = covMat + tmpZeroMean * tmpZeroMean.transpose();\n }\n\n Eigen::SelfAdjointEigenSolver saes(covMat);\n\n // if is indeed line feature\n // note Eigen library sort eigenvalues in increasing order\n Eigen::Vector3d unit_direction = saes.eigenvectors().col(2);\n Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);\n if (saes.eigenvalues()[2] > 3 * saes.eigenvalues()[1])\n {\n Eigen::Vector3d point_on_line = center;\n Eigen::Vector3d point_a, point_b;\n point_a = 0.1 * unit_direction + point_on_line;\n point_b = -0.1 * unit_direction + point_on_line;\n\n ceres::CostFunction *cost_function = LidarEdgeFactor::Create(curr_point, point_a, point_b, 1.0);\n problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);\n corner_num++;\n }\n }\n /*\n else if(pointSearchSqDis[4] < 0.01 * sqrtDis)\n {\n Eigen::Vector3d center(0, 0, 0);\n for (int j = 0; j < 5; j++)\n {\n Eigen::Vector3d tmp(laserCloudCornerFromMap->points[pointSearchInd[j]].x,\n laserCloudCornerFromMap->points[pointSearchInd[j]].y,\n laserCloudCornerFromMap->points[pointSearchInd[j]].z);\n center = center + tmp;\n }\n center = center / 5.0;\n Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);\n ceres::CostFunction *cost_function = LidarDistanceFactor::Create(curr_point, center);\n problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);\n }\n */\n }\n\n int surf_num = 0;\n for (int i = 0; i < laserCloudSurfStackNum; i++)\n {\n pointOri = laserCloudSurfStack->points[i];\n // double sqrtDis = pointOri.x * pointOri.x + pointOri.y * pointOri.y + pointOri.z * pointOri.z;\n pointAssociateToMap(&pointOri, &pointSel);\n kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);\n\n Eigen::Matrix matA0;\n Eigen::Matrix matB0 = -1 * Eigen::Matrix::Ones();\n if (pointSearchSqDis[4] < 1.0)\n {\n for (int j = 0; j < 5; j++)\n {\n matA0(j, 0) = laserCloudSurfFromMap->points[pointSearchInd[j]].x;\n matA0(j, 1) = laserCloudSurfFromMap->points[pointSearchInd[j]].y;\n matA0(j, 2) = laserCloudSurfFromMap->points[pointSearchInd[j]].z;\n // ROS_INFO(\" pts %f %f %f \", matA0(j, 0), matA0(j, 1), matA0(j, 2));\n }\n // find the norm of plane\n Eigen::Vector3d norm = matA0.colPivHouseholderQr().solve(matB0);\n double negative_OA_dot_norm = 1 / norm.norm();\n norm.normalize();\n\n // Here n(pa, pb, pc) is unit norm of plane\n bool planeValid = true;\n for (int j = 0; j < 5; j++)\n {\n // if OX * n > 0.2, then plane is not fit well\n if (fabs(norm(0) * laserCloudSurfFromMap->points[pointSearchInd[j]].x +\n norm(1) * laserCloudSurfFromMap->points[pointSearchInd[j]].y +\n norm(2) * laserCloudSurfFromMap->points[pointSearchInd[j]].z + negative_OA_dot_norm) > 0.2)\n {\n planeValid = false;\n break;\n }\n }\n Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);\n if (planeValid)\n {\n ceres::CostFunction *cost_function = LidarPlaneNormFactor::Create(curr_point, norm, negative_OA_dot_norm);\n problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);\n surf_num++;\n }\n }\n /*\n else if(pointSearchSqDis[4] < 0.01 * sqrtDis)\n {\n Eigen::Vector3d center(0, 0, 0);\n for (int j = 0; j < 5; j++)\n {\n Eigen::Vector3d tmp(laserCloudSurfFromMap->points[pointSearchInd[j]].x,\n laserCloudSurfFromMap->points[pointSearchInd[j]].y,\n laserCloudSurfFromMap->points[pointSearchInd[j]].z);\n center = center + tmp;\n }\n center = center / 5.0;\n Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);\n ceres::CostFunction *cost_function = LidarDistanceFactor::Create(curr_point, center);\n problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);\n }\n */\n }\n\n if (verbose_level > 1)\n {\n // ROS_INFO(\"corner num %d used corner num %d \\n\", laserCloudCornerStackNum, corner_num);\n // ROS_INFO(\"surf num %d used surf num %d \\n\", laserCloudSurfStackNum, surf_num);\n\n ROS_INFO(\"mapping data assosiation time %f ms \\n\", t_data.toc());\n }\n\n TicToc t_solver;\n ceres::Solver::Options options;\n options.linear_solver_type = ceres::DENSE_QR;\n options.max_num_iterations = 4;\n options.minimizer_progress_to_stdout = false;\n options.check_gradients = false;\n options.gradient_check_relative_precision = 1e-4;\n ceres::Solver::Summary summary;\n ceres::Solve(options, &problem, &summary);\n\n if (verbose_level > 1)\n ROS_INFO(\"mapping solver time %f ms \\n\", t_solver.toc());\n\n // ROS_INFO(\"time %f \\n\", timeLaserOdometry);\n // ROS_INFO(\"corner factor num %d surf factor num %d\\n\", corner_num, surf_num);\n // ROS_INFO(\"result q %f %f %f %f result t %f %f %f\\n\", parameters[3], parameters[0], parameters[1],\n // parameters[2], \t parameters[4], parameters[5], parameters[6]);\n }\n\n if (verbose_level > 1)\n ROS_INFO(\"mapping optimization time %f \\n\", t_opt.toc());\n }\n else\n {\n if (verbose_level > 1)\n ROS_WARN(\"time Map corner and surf num are not enough\");\n }\n transformUpdate();\n\n TicToc t_add;\n for (int i = 0; i < laserCloudCornerStackNum; i++)\n {\n pointAssociateToMap(&laserCloudCornerStack->points[i], &pointSel);\n\n int cubeI = int((pointSel.x + 25.0) / 50.0) + laserCloudCenWidth;\n int cubeJ = int((pointSel.y + 25.0) / 50.0) + laserCloudCenHeight;\n int cubeK = int((pointSel.z + 25.0) / 50.0) + laserCloudCenDepth;\n\n if (pointSel.x + 25.0 < 0)\n cubeI--;\n if (pointSel.y + 25.0 < 0)\n cubeJ--;\n if (pointSel.z + 25.0 < 0)\n cubeK--;\n\n if (cubeI >= 0 && cubeI < laserCloudWidth && cubeJ >= 0 && cubeJ < laserCloudHeight && cubeK >= 0 &&\n cubeK < laserCloudDepth)\n {\n int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;\n laserCloudCornerArray[cubeInd]->push_back(pointSel);\n }\n }\n\n for (int i = 0; i < laserCloudSurfStackNum; i++)\n {\n pointAssociateToMap(&laserCloudSurfStack->points[i], &pointSel);\n\n int cubeI = int((pointSel.x + 25.0) / 50.0) + laserCloudCenWidth;\n int cubeJ = int((pointSel.y + 25.0) / 50.0) + laserCloudCenHeight;\n int cubeK = int((pointSel.z + 25.0) / 50.0) + laserCloudCenDepth;\n\n if (pointSel.x + 25.0 < 0)\n cubeI--;\n if (pointSel.y + 25.0 < 0)\n cubeJ--;\n if (pointSel.z + 25.0 < 0)\n cubeK--;\n\n if (cubeI >= 0 && cubeI < laserCloudWidth && cubeJ >= 0 && cubeJ < laserCloudHeight && cubeK >= 0 &&\n cubeK < laserCloudDepth)\n {\n int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;\n laserCloudSurfArray[cubeInd]->push_back(pointSel);\n }\n }\n\n if (verbose_level > 1)\n ROS_INFO(\"add points time %f ms\\n\", t_add.toc());\n\n TicToc t_filter;\n for (int i = 0; i < laserCloudValidNum; i++)\n {\n int ind = laserCloudValidInd[i];\n\n pcl::PointCloud::Ptr tmpCorner(new pcl::PointCloud());\n downSizeFilterCorner.setInputCloud(laserCloudCornerArray[ind]);\n downSizeFilterCorner.filter(*tmpCorner);\n laserCloudCornerArray[ind] = tmpCorner;\n\n pcl::PointCloud::Ptr tmpSurf(new pcl::PointCloud());\n downSizeFilterSurf.setInputCloud(laserCloudSurfArray[ind]);\n downSizeFilterSurf.filter(*tmpSurf);\n laserCloudSurfArray[ind] = tmpSurf;\n }\n\n if (verbose_level > 1)\n ROS_INFO(\"filter time %f ms \\n\", t_filter.toc());\n\n frameCount++;\n}\n\nvoid LaserMapping::publish()\n{ // TODO: take in global skip frame\n TicToc t_pub;\n\n nav_msgs::Odometry odomAftMapped;\n odomAftMapped.header.frame_id = \"map\";\n odomAftMapped.child_frame_id = \"aft_mapped\";\n odomAftMapped.header.stamp = ros::Time::now(); // TODO: globally config time stamp\n if (!skip_frame)\n {\n odomAftMapped.pose.pose.orientation.x = q_w_curr.x();\n odomAftMapped.pose.pose.orientation.y = q_w_curr.y();\n odomAftMapped.pose.pose.orientation.z = q_w_curr.z();\n odomAftMapped.pose.pose.orientation.w = q_w_curr.w();\n odomAftMapped.pose.pose.position.x = t_w_curr.x();\n odomAftMapped.pose.pose.position.y = t_w_curr.y();\n odomAftMapped.pose.pose.position.z = t_w_curr.z();\n\n vloam_tf->world_MOT_base_last.setOrigin(tf2::Vector3(t_w_curr.x(), t_w_curr.y(), t_w_curr.z()));\n vloam_tf->world_MOT_base_last.setRotation(tf2::Quaternion(q_w_curr.x(), q_w_curr.y(), q_w_curr.z(), q_w_curr.w()));\n\n // ROS_INFO(\n // \"lidar_mapping: q = %.4f, %.4f, %.4f, %.4f; t = %.4f, %.4f, %.4f\",\n // q_w_curr.x(),\n // q_w_curr.y(),\n // q_w_curr.z(),\n // q_w_curr.w(),\n // t_w_curr.x(),\n // t_w_curr.y(),\n // t_w_curr.z()\n // );\n }\n else\n {\n odomAftMapped.pose.pose.orientation.x = q_w_curr_highfreq.x();\n odomAftMapped.pose.pose.orientation.y = q_w_curr_highfreq.y();\n odomAftMapped.pose.pose.orientation.z = q_w_curr_highfreq.z();\n odomAftMapped.pose.pose.orientation.w = q_w_curr_highfreq.w();\n odomAftMapped.pose.pose.position.x = t_w_curr_highfreq.x();\n odomAftMapped.pose.pose.position.y = t_w_curr_highfreq.y();\n odomAftMapped.pose.pose.position.z = t_w_curr_highfreq.z();\n\n vloam_tf->world_MOT_base_last.setOrigin(\n tf2::Vector3(t_w_curr_highfreq.x(), t_w_curr_highfreq.y(), t_w_curr_highfreq.z()));\n vloam_tf->world_MOT_base_last.setRotation(\n tf2::Quaternion(q_w_curr_highfreq.x(), q_w_curr_highfreq.y(), q_w_curr_highfreq.z(), q_w_curr_highfreq.w()));\n }\n pubOdomAftMapped.publish(odomAftMapped);\n\n geometry_msgs::PoseStamped laserAfterMappedPose;\n laserAfterMappedPose.header = odomAftMapped.header;\n laserAfterMappedPose.pose = odomAftMapped.pose.pose;\n laserAfterMappedPath.header.stamp = odomAftMapped.header.stamp;\n laserAfterMappedPath.header.frame_id = \"map\";\n laserAfterMappedPath.poses.push_back(laserAfterMappedPose);\n pubLaserAfterMappedPath.publish(laserAfterMappedPath);\n\n static tf::TransformBroadcaster br;\n tf::Transform transform;\n tf::Quaternion q;\n transform.setOrigin(tf::Vector3(t_w_curr(0), t_w_curr(1), t_w_curr(2)));\n q.setW(q_w_curr.w());\n q.setX(q_w_curr.x());\n q.setY(q_w_curr.y());\n q.setZ(q_w_curr.z());\n transform.setRotation(q);\n br.sendTransform(tf::StampedTransform(transform, odomAftMapped.header.stamp, \"map\", \"aft_mapped\"));\n\n if ((frameCount * mapping_skip_frame) % map_pub_number == 0) // 0.5 Hz?\n {\n pcl::PointCloud laserCloudMap;\n for (int i = 0; i < 4851; i++)\n {\n laserCloudMap += *laserCloudCornerArray[i];\n laserCloudMap += *laserCloudSurfArray[i];\n }\n sensor_msgs::PointCloud2 laserCloudMsg;\n pcl::toROSMsg(laserCloudMap, laserCloudMsg);\n laserCloudMsg.header.stamp = ros::Time::now(); // TODO: globally config time stamp\n laserCloudMsg.header.frame_id = \"velo_origin\";\n pubLaserCloudMap.publish(laserCloudMsg);\n if (verbose_level > 1)\n ROS_INFO(\"publishing the map, with %ld points\", laserCloudMap.size());\n }\n\n int laserCloudFullResNum = laserCloudFullRes->points.size();\n for (int i = 0; i < laserCloudFullResNum; i++)\n {\n pointAssociateToMap(&laserCloudFullRes->points[i], &laserCloudFullRes->points[i]);\n }\n\n sensor_msgs::PointCloud2 laserCloudFullRes3; // Q: what's the difference between laserCloudFullRes 1 2 and 3?\n pcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3);\n laserCloudFullRes3.header.stamp = ros::Time::now(); // TODO: globally config time stamp\n laserCloudFullRes3.header.frame_id = \"map\";\n pubLaserCloudFullRes.publish(laserCloudFullRes3);\n\n if (verbose_level > 1)\n {\n ROS_INFO(\"mapping pub time %f ms \\n\", t_pub.toc());\n\n ROS_INFO(\"whole mapping time %f ms +++++\\n\",\n t_whole.toc()); // TODO check if t_whole tictoc obj can be shared in class\n }\n}\n\n} // namespace vloam" }, { "path": "src/lidar_odometry_mapping/src/laser_odometry.cpp", "content": "// This is an advanced implementation of the algorithm described in the following paper:\n// J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time.\n// Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014.\n\n// Modifier: Tong Qin qintonguav@gmail.com\n// \t Shaozu Cao \t\t saozu.cao@connect.ust.hk\n\n// Copyright 2013, Ji Zhang, Carnegie Mellon University\n// Further contributions copyright (c) 2016, Southwest Research Institute\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are met:\n//\n// 1. Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// 2. Redistributions in binary form must reproduce the above copyright notice,\n// this list of conditions and the following disclaimer in the documentation\n// and/or other materials provided with the distribution.\n// 3. Neither the name of the copyright holder nor the names of its\n// contributors may be used to endorse or promote products derived from this\n// software without specific prior written permission.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\"\n// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE\n// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE\n// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE\n// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR\n// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF\n// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS\n// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN\n// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)\n// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n// POSSIBILITY OF SUCH DAMAGE.\n\n#include \n\nnamespace vloam\n{\n// init before a new rosbag comes\nvoid LaserOdometry::init(std::shared_ptr& vloam_tf_)\n{\n vloam_tf = vloam_tf_;\n\n if (!ros::param::get(\"loam_verbose_level\", verbose_level))\n ROS_BREAK();\n if (!ros::param::get(\"detach_VO_LO\", detach_VO_LO))\n ROS_BREAK();\n\n corner_correspondence = 0;\n plane_correspondence = 0;\n\n if (!ros::param::get(\"mapping_skip_frame\", mapping_skip_frame))\n ROS_BREAK();\n\n systemInited = false;\n\n timeCornerPointsSharp = 0;\n timeCornerPointsLessSharp = 0;\n timeSurfPointsFlat = 0;\n timeSurfPointsLessFlat = 0;\n timeLaserCloudFullRes = 0;\n\n kdtreeCornerLast = boost::make_shared>();\n kdtreeSurfLast = boost::make_shared>();\n\n cornerPointsSharp = boost::make_shared>();\n cornerPointsLessSharp = boost::make_shared>();\n surfPointsFlat = boost::make_shared>();\n surfPointsLessFlat = boost::make_shared>();\n\n laserCloudCornerLast = boost::make_shared>();\n laserCloudSurfLast = boost::make_shared>();\n laserCloudFullRes = boost::make_shared>();\n\n laserCloudCornerLastNum = 0;\n laserCloudSurfLastNum = 0;\n\n // Transformation from current frame to world frame\n q_w_curr = Eigen::Quaterniond(1, 0, 0, 0);\n t_w_curr = Eigen::Vector3d(0, 0, 0);\n\n // q_curr_last(x, y, z, w), t_curr_last\n para_q[0] = 0.0;\n para_q[1] = 0.0;\n para_q[2] = 0.0;\n para_q[3] = 1.0;\n para_t[0] = 0.0;\n para_t[1] = 0.0;\n para_t[2] = 0.0;\n\n // // q_last_curr = Eigen::Map(para_q);\n // // t_last_curr = Eigen::Map(para_t);\n new (&q_last_curr) Eigen::Map(para_q);\n new (&t_last_curr) Eigen::Map(para_t);\n\n // ROS_INFO(\"INIT raw: q_last_curr: w = %f x = %f, y = %f, z = %f\", para_q[3], para_q[0], para_q[1], para_q[2]);\n // ROS_INFO(\"INIT: q_last_curr: w = %f x = %f, y = %f, z = %f\", q_last_curr.w(), q_last_curr.x(), q_last_curr.y(),\n // q_last_curr.z()); ROS_INFO(\"INIT: t_last_curr: x = %f, y = %f, z = %f\", t_last_curr.x(), t_last_curr.y(),\n // t_last_curr.z());\n\n // Eigen::Map q_last_curr2(para_q);\n // Eigen::Map t_last_curr2(para_t);\n // ROS_INFO(\"INIT2: q_last_curr: w = %f x = %f, y = %f, z = %f\", q_last_curr2.w(), q_last_curr2.x(), q_last_curr2.y(),\n // q_last_curr2.z()); ROS_INFO(\"INIT2: t_last_curr: x = %f, y = %f, z = %f\", t_last_curr2.x(), t_last_curr2.y(),\n // t_last_curr2.z());\n\n pubLaserCloudCornerLast = nh.advertise(\"/laser_cloud_corner_last\", 100);\n pubLaserCloudSurfLast = nh.advertise(\"/laser_cloud_surf_last\", 100);\n pubLaserCloudFullRes = nh.advertise(\"/velodyne_cloud_3\", 100);\n pubLaserOdometry = nh.advertise(\"/laser_odom_to_init\", 100);\n pubLaserPath = nh.advertise(\"/laser_odom_path\", 100);\n\n laserPath.poses.clear();\n\n frameCount = 0;\n}\n\n// // reset after a new message comes\n// void LaserOdometry::reset() {\n\n// // LOAM must use previous f2f odom as a prior\n// // para_q[0] = 0.0;\n// // para_q[1] = 0.0;\n// // para_q[2] = 0.0;\n// // para_q[3] = 1.0;\n// // para_t[0] = 0.0;\n// // para_t[1] = 0.0;\n// // para_t[2] = 0.0;\n\n// // new (&q_last_curr) Eigen::Map(para_q);\n// // new (&t_last_curr) Eigen::Map(para_t);\n// }\n\nvoid LaserOdometry::input(const pcl::PointCloud::Ptr& laserCloud_,\n const pcl::PointCloud::Ptr& cornerPointsSharp_,\n const pcl::PointCloud::Ptr& cornerPointsLessSharp_,\n const pcl::PointCloud::Ptr& surfPointsFlat_,\n const pcl::PointCloud::Ptr& surfPointsLessFlat_)\n{\n laserCloudFullRes = boost::make_shared>(*laserCloud_);\n cornerPointsSharp = boost::make_shared>(*cornerPointsSharp_);\n cornerPointsLessSharp = boost::make_shared>(*cornerPointsLessSharp_);\n surfPointsFlat = boost::make_shared>(*surfPointsFlat_);\n surfPointsLessFlat = boost::make_shared>(*surfPointsLessFlat_);\n}\n\n// undistort lidar point\nvoid LaserOdometry::TransformToStart(PointType const* const pi, PointType* const po)\n{\n // interpolation ratio\n double s;\n if (DISTORTION)\n s = (pi->intensity - int(pi->intensity)) / SCAN_PERIOD;\n else\n s = 1.0;\n // s = 1;\n Eigen::Quaterniond q_point_last = Eigen::Quaterniond::Identity().slerp(s, q_last_curr);\n Eigen::Vector3d t_point_last = s * t_last_curr;\n Eigen::Vector3d point(pi->x, pi->y, pi->z);\n Eigen::Vector3d un_point = q_point_last * point + t_point_last;\n\n po->x = un_point.x();\n po->y = un_point.y();\n po->z = un_point.z();\n po->intensity = pi->intensity;\n}\n\n// transform all lidar points to the start of the next frame\nvoid LaserOdometry::TransformToEnd(PointType const* const pi, PointType* const po)\n{\n // undistort point first\n pcl::PointXYZI un_point_tmp;\n TransformToStart(pi, &un_point_tmp);\n\n Eigen::Vector3d un_point(un_point_tmp.x, un_point_tmp.y, un_point_tmp.z);\n Eigen::Vector3d point_end = q_last_curr.inverse() * (un_point - t_last_curr);\n\n po->x = point_end.x();\n po->y = point_end.y();\n po->z = point_end.z();\n\n // Remove distortion time info\n po->intensity = int(pi->intensity);\n}\n\nvoid LaserOdometry::solveLO()\n{\n // this->reset();\n\n // ROS_INFO(\"LO: Finished reset\");\n\n TicToc t_whole;\n // initializing\n\n if (!systemInited)\n {\n systemInited = true;\n\n if (verbose_level > 1)\n {\n ROS_INFO(\"Initialization finished \\n\");\n }\n }\n else\n {\n int cornerPointsSharpNum = cornerPointsSharp->points.size();\n int surfPointsFlatNum = surfPointsFlat->points.size();\n\n TicToc t_opt;\n for (size_t opti_counter = 0; opti_counter < 2; ++opti_counter)\n {\n corner_correspondence = 0;\n plane_correspondence = 0;\n\n // ceres::LossFunction *loss_function = NULL;\n ceres::LossFunction* loss_function = new ceres::HuberLoss(0.1);\n ceres::LocalParameterization* q_parameterization = new ceres::EigenQuaternionParameterization();\n ceres::Problem::Options problem_options;\n\n ceres::Problem problem(problem_options);\n\n if (!detach_VO_LO)\n {\n para_q[0] = vloam_tf->velo_last_VOT_velo_curr.getRotation().x();\n para_q[1] = vloam_tf->velo_last_VOT_velo_curr.getRotation().y();\n para_q[2] = vloam_tf->velo_last_VOT_velo_curr.getRotation().z();\n para_q[3] = vloam_tf->velo_last_VOT_velo_curr.getRotation().w();\n\n para_t[0] = vloam_tf->velo_last_VOT_velo_curr.getOrigin().x();\n para_t[1] = vloam_tf->velo_last_VOT_velo_curr.getOrigin().y();\n para_t[2] = vloam_tf->velo_last_VOT_velo_curr.getOrigin().z();\n\n // new (&q_last_curr) Eigen::Map(para_q);\n // new (&t_last_curr) Eigen::Map(para_t);\n }\n\n if (verbose_level > 1)\n {\n ROS_INFO(\"\\nq_last_curr.x = %f, velo_last_T_velo_curr.q.x = %f\", para_q[0],\n vloam_tf->velo_last_VOT_velo_curr.getRotation().x());\n ROS_INFO(\"q_last_curr.y = %f, velo_last_T_velo_curr.q.y = %f\", para_q[1],\n vloam_tf->velo_last_VOT_velo_curr.getRotation().y());\n ROS_INFO(\"q_last_curr.z = %f, velo_last_T_velo_curr.q.z = %f\", para_q[2],\n vloam_tf->velo_last_VOT_velo_curr.getRotation().z());\n ROS_INFO(\"q_last_curr.w = %f, velo_last_T_velo_curr.q.w = %f\", para_q[3],\n vloam_tf->velo_last_VOT_velo_curr.getRotation().w());\n\n ROS_INFO(\"t_last_curr.x = %f, velo_last_T_velo_curr.t.x = %f\", para_t[0],\n vloam_tf->velo_last_VOT_velo_curr.getOrigin().x());\n ROS_INFO(\"t_last_curr.y = %f, velo_last_T_velo_curr.t.y = %f\", para_t[1],\n vloam_tf->velo_last_VOT_velo_curr.getOrigin().y());\n ROS_INFO(\"t_last_curr.z = %f, velo_last_T_velo_curr.t.z = %f\", para_t[2],\n vloam_tf->velo_last_VOT_velo_curr.getOrigin().z());\n }\n\n problem.AddParameterBlock(para_q, 4, q_parameterization);\n problem.AddParameterBlock(para_t, 3);\n\n pcl::PointXYZI pointSel;\n std::vector pointSearchInd;\n std::vector pointSearchSqDis;\n\n TicToc t_data;\n // find correspondence for corner features\n for (int i = 0; i < cornerPointsSharpNum; ++i)\n {\n TransformToStart(&(cornerPointsSharp->points[i]), &pointSel);\n kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);\n\n int closestPointInd = -1, minPointInd2 = -1;\n if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD)\n {\n closestPointInd = pointSearchInd[0];\n int closestPointScanID = int(laserCloudCornerLast->points[closestPointInd].intensity);\n\n double minPointSqDis2 = DISTANCE_SQ_THRESHOLD;\n // search in the direction of increasing scan line\n for (int j = closestPointInd + 1; j < (int)laserCloudCornerLast->points.size(); ++j)\n {\n // if in the same scan line, continue\n if (int(laserCloudCornerLast->points[j].intensity) <= closestPointScanID)\n continue;\n\n // if not in nearby scans, end the loop\n if (int(laserCloudCornerLast->points[j].intensity) > (closestPointScanID + NEARBY_SCAN))\n break;\n\n double pointSqDis =\n (laserCloudCornerLast->points[j].x - pointSel.x) * (laserCloudCornerLast->points[j].x - pointSel.x) +\n (laserCloudCornerLast->points[j].y - pointSel.y) * (laserCloudCornerLast->points[j].y - pointSel.y) +\n (laserCloudCornerLast->points[j].z - pointSel.z) * (laserCloudCornerLast->points[j].z - pointSel.z);\n\n if (pointSqDis < minPointSqDis2)\n {\n // find nearer point\n minPointSqDis2 = pointSqDis;\n minPointInd2 = j;\n }\n }\n\n // search in the direction of decreasing scan line\n for (int j = closestPointInd - 1; j >= 0; --j)\n {\n // if in the same scan line, continue\n if (int(laserCloudCornerLast->points[j].intensity) >= closestPointScanID)\n continue;\n\n // if not in nearby scans, end the loop\n if (int(laserCloudCornerLast->points[j].intensity) < (closestPointScanID - NEARBY_SCAN))\n break;\n\n double pointSqDis =\n (laserCloudCornerLast->points[j].x - pointSel.x) * (laserCloudCornerLast->points[j].x - pointSel.x) +\n (laserCloudCornerLast->points[j].y - pointSel.y) * (laserCloudCornerLast->points[j].y - pointSel.y) +\n (laserCloudCornerLast->points[j].z - pointSel.z) * (laserCloudCornerLast->points[j].z - pointSel.z);\n\n if (pointSqDis < minPointSqDis2)\n {\n // find nearer point\n minPointSqDis2 = pointSqDis;\n minPointInd2 = j;\n }\n }\n }\n if (minPointInd2 >= 0) // both closestPointInd and minPointInd2 is valid\n {\n Eigen::Vector3d curr_point(cornerPointsSharp->points[i].x, cornerPointsSharp->points[i].y,\n cornerPointsSharp->points[i].z);\n Eigen::Vector3d last_point_a(laserCloudCornerLast->points[closestPointInd].x,\n laserCloudCornerLast->points[closestPointInd].y,\n laserCloudCornerLast->points[closestPointInd].z);\n Eigen::Vector3d last_point_b(laserCloudCornerLast->points[minPointInd2].x,\n laserCloudCornerLast->points[minPointInd2].y,\n laserCloudCornerLast->points[minPointInd2].z);\n\n double s;\n if (DISTORTION)\n s = (cornerPointsSharp->points[i].intensity - int(cornerPointsSharp->points[i].intensity)) / SCAN_PERIOD;\n else\n s = 1.0;\n ceres::CostFunction* cost_function = LidarEdgeFactor::Create(curr_point, last_point_a, last_point_b, s);\n // if (corner_correspondence < 20) {\n // std::cout << \"Corner feature No.\" << corner_correspondence << \", curr_point: \" << curr_point << \",\n // last_point_a: \" << last_point_a << \", last_point_b: \" << last_point_b;\n // } // TODO: remove this later (test only code)\n problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);\n corner_correspondence++;\n }\n }\n\n // find correspondence for plane features\n for (int i = 0; i < surfPointsFlatNum; ++i)\n {\n TransformToStart(&(surfPointsFlat->points[i]), &pointSel);\n kdtreeSurfLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);\n\n int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1;\n if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD)\n {\n closestPointInd = pointSearchInd[0];\n\n // get closest point's scan ID\n int closestPointScanID = int(laserCloudSurfLast->points[closestPointInd].intensity);\n double minPointSqDis2 = DISTANCE_SQ_THRESHOLD, minPointSqDis3 = DISTANCE_SQ_THRESHOLD;\n\n // search in the direction of increasing scan line\n for (int j = closestPointInd + 1; j < (int)laserCloudSurfLast->points.size(); ++j)\n {\n // if not in nearby scans, end the loop\n if (int(laserCloudSurfLast->points[j].intensity) > (closestPointScanID + NEARBY_SCAN))\n break;\n\n double pointSqDis =\n (laserCloudSurfLast->points[j].x - pointSel.x) * (laserCloudSurfLast->points[j].x - pointSel.x) +\n (laserCloudSurfLast->points[j].y - pointSel.y) * (laserCloudSurfLast->points[j].y - pointSel.y) +\n (laserCloudSurfLast->points[j].z - pointSel.z) * (laserCloudSurfLast->points[j].z - pointSel.z);\n\n // if in the same or lower scan line\n if (int(laserCloudSurfLast->points[j].intensity) <= closestPointScanID && pointSqDis < minPointSqDis2)\n {\n minPointSqDis2 = pointSqDis;\n minPointInd2 = j;\n }\n // if in the higher scan line\n else if (int(laserCloudSurfLast->points[j].intensity) > closestPointScanID && pointSqDis < minPointSqDis3)\n {\n minPointSqDis3 = pointSqDis;\n minPointInd3 = j;\n }\n }\n\n // search in the direction of decreasing scan line\n for (int j = closestPointInd - 1; j >= 0; --j)\n {\n // if not in nearby scans, end the loop\n if (int(laserCloudSurfLast->points[j].intensity) < (closestPointScanID - NEARBY_SCAN))\n break;\n\n double pointSqDis =\n (laserCloudSurfLast->points[j].x - pointSel.x) * (laserCloudSurfLast->points[j].x - pointSel.x) +\n (laserCloudSurfLast->points[j].y - pointSel.y) * (laserCloudSurfLast->points[j].y - pointSel.y) +\n (laserCloudSurfLast->points[j].z - pointSel.z) * (laserCloudSurfLast->points[j].z - pointSel.z);\n\n // if in the same or higher scan line\n if (int(laserCloudSurfLast->points[j].intensity) >= closestPointScanID && pointSqDis < minPointSqDis2)\n {\n minPointSqDis2 = pointSqDis;\n minPointInd2 = j;\n }\n else if (int(laserCloudSurfLast->points[j].intensity) < closestPointScanID && pointSqDis < minPointSqDis3)\n {\n // find nearer point\n minPointSqDis3 = pointSqDis;\n minPointInd3 = j;\n }\n }\n\n if (minPointInd2 >= 0 && minPointInd3 >= 0)\n {\n Eigen::Vector3d curr_point(surfPointsFlat->points[i].x, surfPointsFlat->points[i].y,\n surfPointsFlat->points[i].z);\n Eigen::Vector3d last_point_a(laserCloudSurfLast->points[closestPointInd].x,\n laserCloudSurfLast->points[closestPointInd].y,\n laserCloudSurfLast->points[closestPointInd].z);\n Eigen::Vector3d last_point_b(laserCloudSurfLast->points[minPointInd2].x,\n laserCloudSurfLast->points[minPointInd2].y,\n laserCloudSurfLast->points[minPointInd2].z);\n Eigen::Vector3d last_point_c(laserCloudSurfLast->points[minPointInd3].x,\n laserCloudSurfLast->points[minPointInd3].y,\n laserCloudSurfLast->points[minPointInd3].z);\n\n double s;\n if (DISTORTION)\n s = (surfPointsFlat->points[i].intensity - int(surfPointsFlat->points[i].intensity)) / SCAN_PERIOD;\n else\n s = 1.0;\n ceres::CostFunction* cost_function =\n LidarPlaneFactor::Create(curr_point, last_point_a, last_point_b, last_point_c, s);\n problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);\n plane_correspondence++;\n }\n }\n }\n\n if (verbose_level > 1)\n {\n ROS_INFO(\"coner_correspondance %d, plane_correspondence %d \\n\", corner_correspondence, plane_correspondence);\n ROS_INFO(\"data association time %f ms \\n\", t_data.toc());\n }\n\n if ((corner_correspondence + plane_correspondence) < 10)\n {\n ROS_INFO(\"less correspondence! *************************************************\\n\");\n }\n\n TicToc t_solver;\n ceres::Solver::Options options;\n options.linear_solver_type = ceres::DENSE_QR;\n options.max_num_iterations = 4;\n options.minimizer_progress_to_stdout = false;\n ceres::Solver::Summary summary;\n ceres::Solve(options, &problem, &summary);\n // ROS_INFO(\"full report: \\n%s\", summary.FullReport().c_str());\n // ROS_INFO(\"raw: q_last_curr: w = %f x = %f, y = %f, z = %f\", para_q[3], para_q[0], para_q[1], para_q[2]);\n if (verbose_level > 1)\n {\n ROS_INFO(\"solver time %f ms \\n\", t_solver.toc());\n }\n }\n\n if (verbose_level > 1)\n {\n ROS_INFO(\"optimization twice time %f \\n\", t_opt.toc());\n }\n\n t_w_curr = t_w_curr + q_w_curr * t_last_curr;\n q_w_curr = q_w_curr * q_last_curr;\n }\n\n // ROS_INFO(\"raw: q_last_curr: w = %f x = %f, y = %f, z = %f\", para_q[3], para_q[0], para_q[1], para_q[2]);\n // ROS_INFO(\"q_last_curr: w = %f x = %f, y = %f, z = %f\", q_last_curr.w(), q_last_curr.x(), q_last_curr.y(),\n // q_last_curr.z()); ROS_INFO(\"q_w_curr: w = %f x = %f, y = %f, z = %f\", q_w_curr.w(), q_w_curr.x(), q_w_curr.y(),\n // q_w_curr.z());\n\n // ROS_INFO(\"t_last_curr: x = %f, y = %f, z = %f\", t_last_curr.x(), t_last_curr.y(), t_last_curr.z());\n // ROS_INFO(\"t_w_curr: x = %f, y = %f, z = %f\", t_w_curr.x(), t_w_curr.y(), t_w_curr.z());\n\n // transform corner features and plane features to the scan end point\n if (0)\n {\n int cornerPointsLessSharpNum = cornerPointsLessSharp->points.size();\n for (int i = 0; i < cornerPointsLessSharpNum; i++)\n {\n TransformToEnd(&cornerPointsLessSharp->points[i], &cornerPointsLessSharp->points[i]);\n }\n\n int surfPointsLessFlatNum = surfPointsLessFlat->points.size();\n for (int i = 0; i < surfPointsLessFlatNum; i++)\n {\n TransformToEnd(&surfPointsLessFlat->points[i], &surfPointsLessFlat->points[i]);\n }\n\n int laserCloudFullResNum = laserCloudFullRes->points.size();\n for (int i = 0; i < laserCloudFullResNum; i++)\n {\n TransformToEnd(&laserCloudFullRes->points[i], &laserCloudFullRes->points[i]);\n }\n }\n\n pcl::PointCloud::Ptr laserCloudTemp = cornerPointsLessSharp;\n cornerPointsLessSharp = laserCloudCornerLast;\n laserCloudCornerLast = laserCloudTemp;\n\n laserCloudTemp = surfPointsLessFlat;\n surfPointsLessFlat = laserCloudSurfLast;\n laserCloudSurfLast = laserCloudTemp;\n\n laserCloudCornerLastNum = laserCloudCornerLast->points.size();\n laserCloudSurfLastNum = laserCloudSurfLast->points.size();\n\n // std::cout << \"the size of corner last is \" << laserCloudCornerLastNum << \", and the size of surf last is \" <<\n // laserCloudSurfLastNum << '\\n';\n\n kdtreeCornerLast->setInputCloud(laserCloudCornerLast);\n kdtreeSurfLast->setInputCloud(laserCloudSurfLast);\n\n if (verbose_level > 1)\n {\n ROS_INFO(\"whole laserOdometry time %f ms \\n \\n\", t_whole.toc());\n if (t_whole.toc() > 100)\n ROS_WARN(\"odometry process over 100ms\");\n }\n\n frameCount++;\n}\n\nvoid LaserOdometry::publish()\n{\n TicToc t_pub;\n\n // publish odometry\n nav_msgs::Odometry laserOdometry;\n laserOdometry.header.frame_id = \"map\";\n laserOdometry.child_frame_id = \"laser_odom\";\n laserOdometry.header.stamp = ros::Time::now();\n laserOdometry.pose.pose.orientation.x = q_w_curr.x();\n laserOdometry.pose.pose.orientation.y = q_w_curr.y();\n laserOdometry.pose.pose.orientation.z = q_w_curr.z();\n laserOdometry.pose.pose.orientation.w = q_w_curr.w();\n laserOdometry.pose.pose.position.x = t_w_curr.x();\n laserOdometry.pose.pose.position.y = t_w_curr.y();\n laserOdometry.pose.pose.position.z = t_w_curr.z();\n pubLaserOdometry.publish(laserOdometry);\n\n ROS_INFO(\"lidar_odometry: q = %.4f, %.4f, %.4f, %.4f; t = %.4f, %.4f, %.4f\", q_last_curr.x(), q_last_curr.y(),\n q_last_curr.z(), q_last_curr.w(), t_last_curr.x(), t_last_curr.y(), t_last_curr.z());\n\n // vloam_tf->cam0_init_eigen_LOT_cam0_last.translation() = t_w_curr;\n // vloam_tf->cam0_init_eigen_LOT_cam0_last.linear() = q_w_curr.normalized().toRotationMatrix();\n\n // frame2frame estimation recording\n vloam_tf->base_prev_LOT_base_curr.setOrigin(tf2::Vector3(t_last_curr.x(), t_last_curr.y(), t_last_curr.z()));\n vloam_tf->base_prev_LOT_base_curr.setRotation(\n tf2::Quaternion(q_last_curr.x(), q_last_curr.y(), q_last_curr.z(), q_last_curr.w()));\n vloam_tf->cam0_curr_LOT_cam0_prev =\n vloam_tf->base_T_cam0.inverse() * vloam_tf->base_prev_LOT_base_curr.inverse() * vloam_tf->base_T_cam0;\n\n // world2frame estimation\n vloam_tf->world_LOT_base_last.setOrigin(tf2::Vector3(t_w_curr.x(), t_w_curr.y(), t_w_curr.z()));\n vloam_tf->world_LOT_base_last.setRotation(tf2::Quaternion(q_w_curr.x(), q_w_curr.y(), q_w_curr.z(), q_w_curr.w()));\n\n geometry_msgs::PoseStamped laserPose;\n laserPose.header = laserOdometry.header;\n laserPose.pose = laserOdometry.pose.pose;\n laserPath.header.stamp = laserOdometry.header.stamp;\n laserPath.poses.push_back(laserPose);\n laserPath.header.frame_id = \"map\";\n pubLaserPath.publish(laserPath);\n\n // if (frameCount % mapping_skip_frame == 0) // no need to publish\n // {\n // frameCount = 0;\n\n // sensor_msgs::PointCloud2 laserCloudCornerLast2;\n // pcl::toROSMsg(*laserCloudCornerLast, laserCloudCornerLast2);\n // laserCloudCornerLast2.header.stamp = ros::Time::now();\n // laserCloudCornerLast2.header.frame_id = \"velo\";\n // pubLaserCloudCornerLast.publish(laserCloudCornerLast2);\n\n // sensor_msgs::PointCloud2 laserCloudSurfLast2;\n // pcl::toROSMsg(*laserCloudSurfLast, laserCloudSurfLast2);\n // laserCloudSurfLast2.header.stamp = ros::Time::now();\n // laserCloudSurfLast2.header.frame_id = \"velo\";\n // pubLaserCloudSurfLast.publish(laserCloudSurfLast2);\n\n // sensor_msgs::PointCloud2 laserCloudFullRes3;\n // pcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3);\n // laserCloudFullRes3.header.stamp = ros::Time::now();\n // laserCloudFullRes3.header.frame_id = \"velo\";\n // pubLaserCloudFullRes.publish(laserCloudFullRes3);\n // }\n\n if (verbose_level > 1)\n {\n ROS_INFO(\"publication time %f ms \\n\", t_pub.toc());\n }\n}\n\nvoid LaserOdometry::output(Eigen::Quaterniond& q_w_curr_, Eigen::Vector3d& t_w_curr_,\n pcl::PointCloud::Ptr& laserCloudCornerLast_,\n pcl::PointCloud::Ptr& laserCloudSurfLast_,\n pcl::PointCloud::Ptr& laserCloudFullRes_, bool& skip_frame)\n{\n q_w_curr_ = q_w_curr;\n t_w_curr_ = t_w_curr;\n\n if (frameCount % mapping_skip_frame == 0)\n {\n laserCloudCornerLast_ = laserCloudCornerLast;\n laserCloudSurfLast_ = laserCloudSurfLast;\n laserCloudFullRes_ = laserCloudFullRes;\n skip_frame = false;\n } // TODO: check the meaning of skip frame.\n else\n {\n skip_frame = true;\n }\n}\n} // namespace vloam" }, { "path": "src/lidar_odometry_mapping/src/lidar_odometry_mapping.cpp", "content": "// This is an advanced implementation of the algorithm described in the following paper:\n// J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time.\n// Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014.\n\n// Modifier: Tong Qin qintonguav@gmail.com\n// \t Shaozu Cao \t\t saozu.cao@connect.ust.hk\n\n// Copyright 2013, Ji Zhang, Carnegie Mellon University\n// Further contributions copyright (c) 2016, Southwest Research Institute\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are met:\n//\n// 1. Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// 2. Redistributions in binary form must reproduce the above copyright notice,\n// this list of conditions and the following disclaimer in the documentation\n// and/or other materials provided with the distribution.\n// 3. Neither the name of the copyright holder nor the names of its\n// contributors may be used to endorse or promote products derived from this\n// software without specific prior written permission.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\"\n// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE\n// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE\n// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE\n// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR\n// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF\n// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS\n// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN\n// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)\n// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n// POSSIBILITY OF SUCH DAMAGE.\n\n#include \n\nnamespace vloam\n{\nvoid LidarOdometryMapping::init(std::shared_ptr& vloam_tf_)\n{\n vloam_tf = vloam_tf_;\n\n if (!ros::param::get(\"loam_verbose_level\", verbose_level))\n ROS_BREAK();\n\n scan_registration.init();\n laser_odometry.init(vloam_tf);\n laser_mapping.init(vloam_tf);\n\n // subLaserCloud = nh->subscribe(\"/velodyne_points\", 100,\n // &LidarOdometryMapping::laserCloudHandler, this);\n\n laserCloud = boost::make_shared>();\n cornerPointsSharp = boost::make_shared>();\n cornerPointsLessSharp = boost::make_shared>();\n surfPointsFlat = boost::make_shared>();\n surfPointsLessFlat = boost::make_shared>();\n\n laserCloudCornerLast = boost::make_shared>();\n laserCloudSurfLast = boost::make_shared>();\n laserCloudFullRes = boost::make_shared>();\n}\n\nvoid LidarOdometryMapping::reset()\n{\n // prepare: laserCloud, cornerPointsSharp, cornerPointsLessSharp, surfPointsFlat, surfPointsLessFlat\n scan_registration.reset();\n // laser_odometry.reset();\n laser_mapping.reset();\n}\n\nvoid LidarOdometryMapping::scanRegistrationIO(const pcl::PointCloud& laserCloudIn)\n{\n loam_timer.tic();\n frame_time = 0.0;\n\n scan_registration.input(laserCloudIn);\n\n // scan_registration.publish(); // no need to publish the point cloud feature\n\n scan_registration.output(laserCloud, // 10Hz\n cornerPointsSharp, // 10Hz\n cornerPointsLessSharp, // 10Hz\n surfPointsFlat, // 10Hz\n surfPointsLessFlat // 10Hz\n );\n\n if (verbose_level > 0)\n {\n ROS_INFO(\"Scan Registration takes %f ms \\n\", loam_timer.toc());\n }\n frame_time += loam_timer.toc();\n}\n\nvoid LidarOdometryMapping::laserOdometryIO()\n{\n loam_timer.tic();\n\n laser_odometry.input(laserCloud, // 10Hz\n cornerPointsSharp, // 10Hz\n cornerPointsLessSharp, // 10Hz\n surfPointsFlat, // 10Hz\n surfPointsLessFlat // 10Hz\n );\n\n laser_odometry.solveLO(); // 10Hz\n\n laser_odometry.publish();\n\n laser_odometry.output(q_wodom_curr, // 10Hz\n t_wodom_curr, // 10Hz\n laserCloudCornerLast, // 2Hz // no change if skip_frame\n laserCloudSurfLast, // 2Hz // no change if skip_frame\n laserCloudFullRes, // 2Hz // no change if skip_frameee\n skip_frame);\n\n if (verbose_level > 0)\n {\n ROS_INFO(\"Laser Odometry takes %f ms \\n\", loam_timer.toc());\n }\n frame_time += loam_timer.toc();\n}\n\nvoid LidarOdometryMapping::laserMappingIO()\n{\n loam_timer.tic();\n\n laser_mapping.input(laserCloudCornerLast, // 2Hz\n laserCloudSurfLast, // 2Hz\n laserCloudFullRes, // 2Hz\n q_wodom_curr, // 10Hz\n t_wodom_curr, // 10Hz\n skip_frame);\n\n if (!skip_frame)\n laser_mapping.solveMapping(); // 2Hz\n\n laser_mapping.publish();\n\n // laser_odometry.output(\n\n // );\n\n if (verbose_level > 0)\n {\n ROS_INFO(\"Laser Mapping takes %f ms \\n\", loam_timer.toc());\n }\n frame_time += loam_timer.toc();\n if (frame_time > 100)\n {\n ROS_WARN(\"LOAM takes %f ms (>100 ms)\", frame_time);\n }\n}\n\n// void LidarOdometryMapping::solveLOAM() {\n// laser_odometry.solveLO();\n// laser_mapping.solveMapping();\n\n// // TODO: check the running time constraint: 0.1s\n// }\n\n} // namespace vloam" }, { "path": "src/lidar_odometry_mapping/src/scan_registration.cpp", "content": "// This is an advanced implementation of the algorithm described in the following paper:\n// J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time.\n// Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014.\n\n// Modifier: Tong Qin qintonguav@gmail.com\n// \t Shaozu Cao \t\t saozu.cao@connect.ust.hk\n\n// Copyright 2013, Ji Zhang, Carnegie Mellon University\n// Further contributions copyright (c) 2016, Southwest Research Institute\n// All rights reserved.\n//\n// Redistribution and use in source and binary forms, with or without\n// modification, are permitted provided that the following conditions are met:\n//\n// 1. Redistributions of source code must retain the above copyright notice,\n// this list of conditions and the following disclaimer.\n// 2. Redistributions in binary form must reproduce the above copyright notice,\n// this list of conditions and the following disclaimer in the documentation\n// and/or other materials provided with the distribution.\n// 3. Neither the name of the copyright holder nor the names of its\n// contributors may be used to endorse or promote products derived from this\n// software without specific prior written permission.\n//\n// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\"\n// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE\n// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE\n// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE\n// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR\n// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF\n// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS\n// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN\n// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)\n// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n// POSSIBILITY OF SUCH DAMAGE.\n\n#include \n\nnamespace vloam\n{\nvoid ScanRegistration::init()\n{\n ros::param::get(\"loam_verbose_level\", verbose_level);\n\n systemInitCount = 0;\n systemInited = false;\n N_SCANS = 0;\n\n if (!ros::param::get(\"scan_line\", N_SCANS))\n ROS_BREAK();\n\n if (!ros::param::get(\"minimum_range\", MINIMUM_RANGE))\n ROS_BREAK();\n\n ROS_INFO(\"scan line number %d \\n\", N_SCANS);\n\n if (N_SCANS != 16 && N_SCANS != 32 && N_SCANS != 64)\n {\n ROS_ERROR(\"only support velodyne with 16, 32 or 64 scan line!\");\n }\n\n // // NOTE: publishers and subscribers won't necessarily used, but are kept for forward compatibility\n // subLaserCloud = nh->subscribe(\"/velodyne_points\", 100,\n // &ScanRegistration::laserCloudHandler, this);\n pubLaserCloud = nh.advertise(\"/velodyne_cloud_2\", 100);\n pubCornerPointsSharp = nh.advertise(\"/laser_cloud_sharp\", 100);\n pubCornerPointsLessSharp = nh.advertise(\"/laser_cloud_less_sharp\", 100);\n pubSurfPointsFlat = nh.advertise(\"/laser_cloud_flat\", 100);\n pubSurfPointsLessFlat = nh.advertise(\"/laser_cloud_less_flat\", 100);\n // pubRemovePoints = nh->advertise(\"/laser_remove_points\", 100);\n\n PUB_EACH_LINE = false;\n // // std::vector pubEachScan;\n // // if(PUB_EACH_LINE)\n // // {\n // // for(int i = 0; i < N_SCANS; i++)\n // // {\n // // ros::Publisher tmp = nh.advertise(\"/laser_scanid_\" + std::to_string(i), 100);\n // // pubEachScan.push_back(tmp);\n // // }\n // // }\n\n laserCloud = boost::make_shared>();\n cornerPointsSharp = boost::make_shared>();\n cornerPointsLessSharp = boost::make_shared>();\n surfPointsFlat = boost::make_shared>();\n surfPointsLessFlat = boost::make_shared>();\n}\n\nvoid ScanRegistration::reset()\n{\n laserCloud->clear();\n cornerPointsSharp->clear();\n cornerPointsLessSharp->clear();\n surfPointsFlat->clear();\n surfPointsLessFlat->clear();\n\n laserCloudScans = std::vector>(N_SCANS); // N_SCANS = 64 for kitti\n}\n\ntemplate \nvoid ScanRegistration::removeClosedPointCloud(const pcl::PointCloud& cloud_in,\n pcl::PointCloud& cloud_out, float thres)\n{\n if (&cloud_in != &cloud_out)\n {\n cloud_out.header = cloud_in.header;\n cloud_out.points.resize(cloud_in.points.size());\n }\n\n size_t j = 0;\n\n for (size_t i = 0; i < cloud_in.points.size(); ++i)\n {\n if (cloud_in.points[i].x * cloud_in.points[i].x + cloud_in.points[i].y * cloud_in.points[i].y +\n cloud_in.points[i].z * cloud_in.points[i].z <\n thres * thres)\n continue;\n cloud_out.points[j] = cloud_in.points[i];\n j++;\n }\n if (j != cloud_in.points.size())\n {\n cloud_out.points.resize(j);\n }\n\n cloud_out.height = 1;\n cloud_out.width = static_cast(j);\n cloud_out.is_dense = true;\n}\n\nvoid ScanRegistration::input(const pcl::PointCloud& laserCloudIn_)\n{\n if (!systemInited)\n {\n systemInitCount++;\n if (systemInitCount >= systemDelay)\n {\n systemInited = true;\n }\n else\n return;\n }\n if (verbose_level > 1)\n {\n ROS_INFO(\"scan registration starts\");\n t_whole.tic();\n }\n\n std::vector scanStartInd(N_SCANS, 0);\n std::vector scanEndInd(N_SCANS, 0);\n\n pcl::PointCloud laserCloudIn = laserCloudIn_;\n // pcl::fromROSMsg(laserCloudMsg, laserCloudIn); // TODO: see if the input of the function should have another type\n // from vloam_main\n std::vector indices;\n\n pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);\n removeClosedPointCloud(laserCloudIn, laserCloudIn, MINIMUM_RANGE);\n\n if (verbose_level > 1)\n {\n ROS_INFO(\"end of closed point removal\");\n }\n\n int cloudSize = laserCloudIn.points.size();\n float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);\n float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y, laserCloudIn.points[cloudSize - 1].x) + 2 * M_PI;\n\n if (endOri - startOri > 3 * M_PI)\n {\n endOri -= 2 * M_PI;\n }\n else if (endOri - startOri < M_PI)\n {\n endOri += 2 * M_PI;\n }\n\n if (verbose_level > 1)\n {\n ROS_INFO(\"end Ori %f\\n\", endOri);\n }\n\n bool halfPassed = false;\n int count = cloudSize;\n PointType point;\n for (int i = 0; i < cloudSize; i++)\n {\n point.x = laserCloudIn.points[i].x;\n point.y = laserCloudIn.points[i].y;\n point.z = laserCloudIn.points[i].z;\n\n float angle = atan(point.z / sqrt(point.x * point.x + point.y * point.y)) * 180 / M_PI;\n int scanID = 0;\n\n if (N_SCANS == 16)\n {\n scanID = int((angle + 15) / 2 + 0.5);\n if (scanID > (N_SCANS - 1) || scanID < 0)\n {\n count--;\n continue;\n }\n }\n else if (N_SCANS == 32)\n {\n scanID = int((angle + 92.0 / 3.0) * 3.0 / 4.0);\n if (scanID > (N_SCANS - 1) || scanID < 0)\n {\n count--;\n continue;\n }\n }\n else if (N_SCANS == 64)\n {\n if (angle >= -8.83)\n scanID = int((2 - angle) * 3.0 + 0.5);\n else\n scanID = N_SCANS / 2 + int((-8.83 - angle) * 2.0 + 0.5);\n\n // use [0 50] > 50 remove outlies\n if (angle > 2 || angle < -24.33 || scanID > 50 || scanID < 0)\n {\n count--;\n continue;\n }\n }\n else\n {\n ROS_INFO(\"wrong scan number\\n\");\n ROS_BREAK();\n }\n // ROS_INFO(\"angle %f scanID %d \\n\", angle, scanID);\n\n float ori = -atan2(point.y, point.x);\n if (!halfPassed)\n {\n if (ori < startOri - M_PI / 2)\n {\n ori += 2 * M_PI;\n }\n else if (ori > startOri + M_PI * 3 / 2)\n {\n ori -= 2 * M_PI;\n }\n\n if (ori - startOri > M_PI)\n {\n halfPassed = true;\n }\n }\n else\n {\n ori += 2 * M_PI;\n if (ori < endOri - M_PI * 3 / 2)\n {\n ori += 2 * M_PI;\n }\n else if (ori > endOri + M_PI / 2)\n {\n ori -= 2 * M_PI;\n }\n }\n\n float relTime = (ori - startOri) / (endOri - startOri);\n point.intensity = scanID + scanPeriod * relTime;\n laserCloudScans[scanID].push_back(point);\n }\n\n cloudSize = count;\n\n if (verbose_level > 1)\n {\n ROS_INFO(\"points size %d \\n\", cloudSize);\n }\n\n for (int i = 0; i < N_SCANS; i++)\n {\n scanStartInd[i] = laserCloud->size() + 5;\n *laserCloud += laserCloudScans[i];\n scanEndInd[i] = laserCloud->size() - 6;\n }\n\n if (verbose_level > 1)\n {\n ROS_INFO(\"prepare time %f \\n\", t_prepare.toc());\n }\n\n for (int i = 5; i < cloudSize - 5; i++)\n {\n float diffX = laserCloud->points[i - 5].x + laserCloud->points[i - 4].x + laserCloud->points[i - 3].x +\n laserCloud->points[i - 2].x + laserCloud->points[i - 1].x - 10 * laserCloud->points[i].x +\n laserCloud->points[i + 1].x + laserCloud->points[i + 2].x + laserCloud->points[i + 3].x +\n laserCloud->points[i + 4].x + laserCloud->points[i + 5].x;\n float diffY = laserCloud->points[i - 5].y + laserCloud->points[i - 4].y + laserCloud->points[i - 3].y +\n laserCloud->points[i - 2].y + laserCloud->points[i - 1].y - 10 * laserCloud->points[i].y +\n laserCloud->points[i + 1].y + laserCloud->points[i + 2].y + laserCloud->points[i + 3].y +\n laserCloud->points[i + 4].y + laserCloud->points[i + 5].y;\n float diffZ = laserCloud->points[i - 5].z + laserCloud->points[i - 4].z + laserCloud->points[i - 3].z +\n laserCloud->points[i - 2].z + laserCloud->points[i - 1].z - 10 * laserCloud->points[i].z +\n laserCloud->points[i + 1].z + laserCloud->points[i + 2].z + laserCloud->points[i + 3].z +\n laserCloud->points[i + 4].z + laserCloud->points[i + 5].z;\n\n cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ;\n cloudSortInd[i] = i;\n cloudNeighborPicked[i] = 0;\n cloudLabel[i] = 0;\n }\n\n TicToc t_pts;\n\n float t_q_sort = 0;\n for (int i = 0; i < N_SCANS; i++)\n {\n if (scanEndInd[i] - scanStartInd[i] < 6)\n continue;\n pcl::PointCloud::Ptr surfPointsLessFlatScan(new pcl::PointCloud);\n for (int j = 0; j < 6; j++)\n {\n int sp = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * j / 6;\n int ep = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * (j + 1) / 6 - 1;\n\n TicToc t_tmp;\n std::sort(cloudSortInd + sp, cloudSortInd + ep + 1,\n [&](const int& i, const int& j) { return cloudCurvature[i] < cloudCurvature[j]; });\n t_q_sort += t_tmp.toc();\n\n int largestPickedNum = 0;\n for (int k = ep; k >= sp; k--)\n {\n int ind = cloudSortInd[k];\n\n if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] > 0.1)\n {\n largestPickedNum++;\n if (largestPickedNum <= 2)\n {\n cloudLabel[ind] = 2;\n cornerPointsSharp->push_back(laserCloud->points[ind]);\n cornerPointsLessSharp->push_back(laserCloud->points[ind]);\n }\n else if (largestPickedNum <= 20)\n {\n cloudLabel[ind] = 1;\n cornerPointsLessSharp->push_back(laserCloud->points[ind]);\n }\n else\n {\n break;\n }\n\n cloudNeighborPicked[ind] = 1;\n\n for (int l = 1; l <= 5; l++)\n {\n float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;\n float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;\n float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;\n if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)\n {\n break;\n }\n\n cloudNeighborPicked[ind + l] = 1;\n }\n for (int l = -1; l >= -5; l--)\n {\n float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;\n float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;\n float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;\n if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)\n {\n break;\n }\n\n cloudNeighborPicked[ind + l] = 1;\n }\n }\n }\n\n int smallestPickedNum = 0;\n for (int k = sp; k <= ep; k++)\n {\n int ind = cloudSortInd[k];\n\n if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] < 0.1)\n {\n cloudLabel[ind] = -1;\n surfPointsFlat->push_back(laserCloud->points[ind]);\n\n smallestPickedNum++;\n if (smallestPickedNum >= 4)\n {\n break;\n }\n\n cloudNeighborPicked[ind] = 1;\n for (int l = 1; l <= 5; l++)\n {\n float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;\n float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;\n float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;\n if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)\n {\n break;\n }\n\n cloudNeighborPicked[ind + l] = 1;\n }\n for (int l = -1; l >= -5; l--)\n {\n float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;\n float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;\n float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;\n if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)\n {\n break;\n }\n\n cloudNeighborPicked[ind + l] = 1;\n }\n }\n }\n\n for (int k = sp; k <= ep; k++)\n {\n if (cloudLabel[k] <= 0)\n {\n surfPointsLessFlatScan->push_back(laserCloud->points[k]);\n }\n }\n }\n\n pcl::PointCloud surfPointsLessFlatScanDS;\n pcl::VoxelGrid downSizeFilter;\n downSizeFilter.setInputCloud(surfPointsLessFlatScan);\n downSizeFilter.setLeafSize(0.2, 0.2, 0.2);\n downSizeFilter.filter(surfPointsLessFlatScanDS);\n\n *surfPointsLessFlat += surfPointsLessFlatScanDS;\n }\n\n if (verbose_level > 1)\n {\n ROS_INFO(\"sort q time %f \\n\", t_q_sort);\n ROS_INFO(\"seperate points time %f \\n\", t_pts.toc());\n\n ROS_INFO(\"scan registration time %f ms *************\\n\", t_whole.toc());\n }\n}\n\nvoid ScanRegistration::publish()\n{\n sensor_msgs::PointCloud2 laserCloudOutMsg;\n pcl::toROSMsg(*laserCloud, laserCloudOutMsg);\n laserCloudOutMsg.header.stamp = ros::Time::now();\n laserCloudOutMsg.header.frame_id = \"velo\";\n pubLaserCloud.publish(laserCloudOutMsg);\n\n sensor_msgs::PointCloud2 cornerPointsSharpMsg;\n pcl::toROSMsg(*cornerPointsSharp, cornerPointsSharpMsg);\n cornerPointsSharpMsg.header.stamp = ros::Time::now();\n cornerPointsSharpMsg.header.frame_id = \"velo\";\n pubCornerPointsSharp.publish(cornerPointsSharpMsg);\n\n sensor_msgs::PointCloud2 cornerPointsLessSharpMsg;\n pcl::toROSMsg(*cornerPointsLessSharp, cornerPointsLessSharpMsg);\n cornerPointsLessSharpMsg.header.stamp = ros::Time::now();\n cornerPointsLessSharpMsg.header.frame_id = \"velo\";\n pubCornerPointsLessSharp.publish(cornerPointsLessSharpMsg);\n\n sensor_msgs::PointCloud2 surfPointsFlat2;\n pcl::toROSMsg(*surfPointsFlat, surfPointsFlat2);\n surfPointsFlat2.header.stamp = ros::Time::now();\n surfPointsFlat2.header.frame_id = \"velo\";\n pubSurfPointsFlat.publish(surfPointsFlat2);\n\n sensor_msgs::PointCloud2 surfPointsLessFlat2;\n pcl::toROSMsg(*surfPointsLessFlat, surfPointsLessFlat2);\n surfPointsLessFlat2.header.stamp = ros::Time::now();\n surfPointsLessFlat2.header.frame_id = \"velo\";\n pubSurfPointsLessFlat.publish(surfPointsLessFlat2);\n\n // pub each scam\n if (PUB_EACH_LINE)\n {\n for (int i = 0; i < N_SCANS; i++)\n {\n sensor_msgs::PointCloud2 scanMsg;\n pcl::toROSMsg(laserCloudScans[i], scanMsg);\n scanMsg.header.stamp = ros::Time::now();\n scanMsg.header.frame_id = \"map\";\n pubEachScan[i].publish(scanMsg);\n }\n }\n\n // ROS_INFO(\"scan registration time %f ms *************\\n\", t_whole.toc());\n // if(t_whole.toc() > 100)\n // ROS_WARN(\"scan registration process over 100ms\");\n}\n\nvoid ScanRegistration::output(pcl::PointCloud::Ptr& laserCloud_,\n pcl::PointCloud::Ptr& cornerPointsSharp_,\n pcl::PointCloud::Ptr& cornerPointsLessSharp_,\n pcl::PointCloud::Ptr& surfPointsFlat_,\n pcl::PointCloud::Ptr& surfPointsLessFlat_)\n{\n laserCloud_ = this->laserCloud;\n cornerPointsSharp_ = this->cornerPointsSharp;\n cornerPointsLessSharp_ = this->cornerPointsLessSharp;\n surfPointsFlat_ = this->surfPointsFlat;\n surfPointsLessFlat_ = this->surfPointsLessFlat;\n}\n\n} // namespace vloam" }, { "path": "src/visual_odometry/CMakeLists.txt", "content": "cmake_minimum_required(VERSION 3.0.2)\nproject(visual_odometry)\n\n## Compile as C++11, supported in ROS Kinetic and newer\nadd_compile_options(-std=c++14 -O3)\n# TODO: check if -O3 is needed -> Yes, Preprocessing 2 frames took 1000ms without -O3, but now takes 63ms with -O3 \n\n## Find catkin macros and libraries\n## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)\n## is used, also find other catkin packages\nfind_package(OpenCV 4.5.1)\n\nfind_package(PCL 1.2 REQUIRED)\ninclude_directories(${PCL_INCLUDE_DIRS})\nlink_directories(${PCL_LIBRARY_DIRS})\nadd_definitions(${PCL_DEFINITIONS})\n\n# find_package(Eigen3 3.3 REQUIRED NO_MODULE)\nfind_package(Ceres REQUIRED PATHS \"/usr/local/lib/cmake/Ceres/\")\n\nfind_package(catkin REQUIRED COMPONENTS\n cv_bridge\n roscpp\n sensor_msgs\n std_msgs\n roslib\n tf2\n vloam_tf\n lidar_odometry_mapping\n)\n\n## System dependencies are found with CMake's conventions\n# find_package(Boost REQUIRED COMPONENTS system)\n\n\n## Uncomment this if the package has a setup.py. This macro ensures\n## modules and global scripts declared therein get installed\n## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html\n# catkin_python_setup()\n\n################################################\n## Declare ROS messages, services and actions ##\n################################################\n\n## To declare and build messages, services or actions from within this\n## package, follow these steps:\n## * Let MSG_DEP_SET be the set of packages whose message types you use in\n## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).\n## * In the file package.xml:\n## * add a build_depend tag for \"message_generation\"\n## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET\n## * If MSG_DEP_SET isn't empty the following dependency has been pulled in\n## but can be declared for certainty nonetheless:\n## * add a exec_depend tag for \"message_runtime\"\n## * In this file (CMakeLists.txt):\n## * add \"message_generation\" and every package in MSG_DEP_SET to\n## find_package(catkin REQUIRED COMPONENTS ...)\n## * add \"message_runtime\" and every package in MSG_DEP_SET to\n## catkin_package(CATKIN_DEPENDS ...)\n## * uncomment the add_*_files sections below as needed\n## and list every .msg/.srv/.action file to be processed\n## * uncomment the generate_messages entry below\n## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)\n\n## Generate messages in the 'msg' folder\n# add_message_files(\n# FILES\n# Message1.msg\n# Message2.msg\n# )\n\n## Generate services in the 'srv' folder\n# add_service_files(\n# FILES\n# Service1.srv\n# Service2.srv\n# )\n\n## Generate actions in the 'action' folder\n# add_action_files(\n# FILES\n# Action1.action\n# Action2.action\n# )\n\n## Generate added messages and services with any dependencies listed here\n# generate_messages(\n# DEPENDENCIES\n# sensor_msgs# std_msgs\n# )\n\n################################################\n## Declare ROS dynamic reconfigure parameters ##\n################################################\n\n## To declare and build dynamic reconfigure parameters within this\n## package, follow these steps:\n## * In the file package.xml:\n## * add a build_depend and a exec_depend tag for \"dynamic_reconfigure\"\n## * In this file (CMakeLists.txt):\n## * add \"dynamic_reconfigure\" to\n## find_package(catkin REQUIRED COMPONENTS ...)\n## * uncomment the \"generate_dynamic_reconfigure_options\" section below\n## and list every .cfg file to be processed\n\n## Generate dynamic reconfigure parameters in the 'cfg' folder\n# generate_dynamic_reconfigure_options(\n# cfg/DynReconf1.cfg\n# cfg/DynReconf2.cfg\n# )\n\n###################################\n## catkin specific configuration ##\n###################################\n## The catkin_package macro generates cmake config files for your package\n## Declare things to be passed to dependent projects\n## INCLUDE_DIRS: uncomment this if your package contains header files\n## LIBRARIES: libraries you create in this project that dependent projects also need\n## CATKIN_DEPENDS: catkin_packages dependent projects also need\n## DEPENDS: system dependencies of this project that dependent projects also need\ncatkin_package(\n INCLUDE_DIRS include\n# LIBRARIES visual_odometry\n CATKIN_DEPENDS cv_bridge roscpp sensor_msgs std_msgs\n DEPENDS EIGEN3 PCL\n# DEPENDS system_lib\n)\n\n###########\n## Build ##\n###########\n\n## Specify additional locations of header files\n## Your package locations should be listed before other locations\ninclude_directories(\n include\n ${catkin_INCLUDE_DIRS}\n ${OpenCV_INCLUDE_DIRS}\n # ${EIGEN3_INCLUDE_DIR}\n)\n\n# Declare a C++ library\nadd_library(image_util SHARED\nsrc/image_util.cpp\n)\ntarget_link_libraries(image_util ${catkin_LIBRARIES})\nset_target_properties(image_util PROPERTIES PUBLIC_HEADER include/${PROJECT_NAME}/image_util.h)\n\n\nadd_library(point_cloud_util SHARED\n src/point_cloud_util.cpp\n)\ntarget_link_libraries(point_cloud_util ${catkin_LIBRARIES})\nset_target_properties(point_cloud_util PROPERTIES PUBLIC_HEADER include/${PROJECT_NAME}/point_cloud_util.h)\n\nadd_library(visual_odometry SHARED\n src/visual_odometry.cpp\n)\ntarget_link_libraries(visual_odometry \n ${catkin_LIBRARIES}\n ${OpenCV_LIBS}\n Eigen3::Eigen\n ${CERES_LIBRARIES}\n image_util\n point_cloud_util\n vloam_tf\n)\nset_target_properties(visual_odometry PROPERTIES PUBLIC_HEADER include/${PROJECT_NAME}/visual_odometry.h)\n\n## Add cmake target dependencies of the library\n## as an example, code may need to be generated before libraries\n## either from message generation or dynamic reconfigure\n# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})\n\n## Declare a C++ executable\n## With catkin_make all packages are built within a single CMake context\n## The recommended prefix ensures that target names across packages don't collide\n\n## Rename C++ executable without prefix\n## The above recommended prefix causes long target names, the following renames the\n## target back to the shorter version for ease of user use\n## e.g. \"rosrun someones_pkg node\" instead of \"rosrun someones_pkg someones_pkg_node\"\n# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX \"\")\n\n## Add cmake target dependencies of the executable\n## same as for the library above\n# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})\n\n## Specify libraries to link a library or executable target against\n\n#############\n## Install ##\n#############\n\n# all install targets should use catkin DESTINATION variables\n# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html\n\n## Mark executable scripts (Python etc.) for installation\n## in contrast to setup.py, you can choose the destination\n# catkin_install_python(PROGRAMS\n# scripts/my_python_script\n# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}\n# )\n\n## Mark executables for installation\n## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html\n# install(TARGETS ${PROJECT_NAME}_node\n# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}\n# )\n\n## Mark libraries for installation\n## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html\n# install(TARGETS ${PROJECT_NAME}\n# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}\n# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}\n# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}\n# )\n\n# Mark cpp header files for installation\ninstall(\n DIRECTORY include/${PROJECT_NAME}/\n DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}\n FILES_MATCHING \n PATTERN \"*.hpp\"\n PATTERN \"*.h\"\n# PATTERN \".svn\" EXCLUDE\n)\n\n## Mark other files for installation (e.g. launch and bag files, etc.)\n# install(FILES\n# # myfile1\n# # myfile2\n# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}\n# )\n\n#############\n## Testing ##\n#############\n\n## Add gtest based cpp test target and link libraries\n# catkin_add_gtest(${PROJECT_NAME}-test test/test_visual_odometry.cpp)\n# if(TARGET ${PROJECT_NAME}-test)\n# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})\n# endif()\n\n## Add folders to be run by python nosetests\n# catkin_add_nosetests(test)\n" }, { "path": "src/visual_odometry/README.md", "content": "# Prerequisites\n\nOpenCV 4.5.1\nEigen3 3.3\nCeres 2.0\n\n# Troubleshooting\n\n## 1. Catkin_Make fail with cv_bridge not found \n\nSolution: Modify `_include_dirs` in cv_bridgeConfig.cmake (at line 96)\n\nThe default `_include_dirs` is set by `set(_include_dirs \"include;/usr/include/opencv4\")`, while my opencv is installed under `/usr/local/include/opencv4`.\n\nReference: https://github.com/cipherdev/src/issues/13\n\n## 2. Installing Ceres\n\nI installed the [latest stable release](http://ceres-solver.org/ceres-solver-2.0.0.tar.gz) as mentioned in http://ceres-solver.org/installation.html. At step \"make test\", my computer got stuck at step 31 \"dynamic_sparsity_test\". This happened to both latest stable release and latest github repository. This bug was ignored, and I went to the next command \"make install\".\n\n## 3. Using catkin_make with Ceres\n\n- Bug 0: Cmake error, `Could not find a package configuration file provided by \"Ceres\" with any of the following names: CeresConfig.cmake ceres-config.cmake`\n - Reason: Ceres path is not recognized by Cmake\n - Fix: Add `set(Ceres_DIR \"/usr/local/lib/cmake/Ceres/\")` to CMakeLists.txt\n- Bug 1: fatal error: Eigen/Core: No such file or directory\n - Reason: the installation folder of Eigen is under .../eigen3/Eigen/...\n - Fix: `sudo ln -s /usr/local/include/eigen3/Eigen /usr/include/Eigen`\n - Reference: https://github.com/opencv/opencv/issues/14868\n- Bug 2: integer_sequence’ is not a member of ‘std’\n - Reason: Ceres 2.0 relies on c++ 14\n - Fix: change `add_compile_options(-std=c++11)` to `add_compile_options(-std=c++14)`\n- Bug 3: test_ceres.cpp:(.text+0xe4): undefined reference to `ceres::Problem::Problem()'\n - Reason: Ceres library was not linked\n - Fix: Add `${CERES_LIBRARIES}` to target_link_libraries. Eg. `target_link_libraries(YOUR_TARGET_EXEC ${catkin_LIBRARIES} ${CERES_LIBRARIES})`\n\nHello world file then returns me the following results\n>rosrun visual_odometry test_ceres \n>\n>iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time\n>\n> 0 4.512500e+01 0.00e+00 9.50e+00 0.00e+00 0.00e+00 1.00e+04 0 1.72e-05 1.09e-04\n>\n> 1 4.511598e-07 4.51e+01 9.50e-04 9.50e+00 1.00e+00 3.00e+04 1 4.20e-05 1.95e-04\n>\n> 2 5.012552e-16 4.51e-07 3.17e-08 9.50e-04 1.00e+00 9.00e+04 1 7.87e-06 2.11e-04\n>\n>Ceres Solver Report: Iterations: 3, Initial cost: 4.512500e+01, Final cost: 5.012552e-16, Termination: CONVERGENCE\n>\n>x : 0.5 -> 10\n\n# Kitti\n\n## Data in ROS BAG version\n\nData converted to ROS BAG can be downloaded from [google drive](https://drive.google.com/drive/folders/1Za3Csvq2ljI_88RoNnltZ1Dx4OWRlb8j?usp=sharing)\n\nRight now it contains `2011_09_26_drive_{0001, 0002, 0005}`. \n\n## raw data usage\n\nReference: http://www.cvlibs.net/datasets/kitti/raw_data.php\n\nThere're 4 image folders under each timestamp dataset: \"image_00\", \"image_01\", \"image_02\", \"image_03\". They correspond to grey left camera, grey right camera, RGB left camera, RGB right camera.\n\nNOTE: Currently, the running of the test code relies on that catkin_ws follows the following tree structure. And please run code at the root of catkin_ws.\n\n- catkin_ws\n - build\n - data\n - 2011_09_26\n - 2011_09_26_drive_0001_sync\n - calib_cam_to_cam.txt\n - calib_velo_to_cam.txt\n - devel\n - src\n\n## calibration file\n\nReference: https://github.com/yanii/kitti-pcl/blob/master/KITTI_README.TXT\n\n>calib_cam_to_cam.txt: Camera-to-camera calibration\n>--------------------------------------------------\n>\n> - S_xx: 1x2 size of image xx before rectification\n> - K_xx: 3x3 calibration matrix of camera xx before rectification\n> - D_xx: 1x5 distortion vector of camera xx before rectification\n> - R_xx: 3x3 rotation matrix of camera xx (extrinsic)\n> - T_xx: 3x1 translation vector of camera xx (extrinsic)\n> - S_rect_xx: 1x2 size of image xx after rectification\n> - R_rect_xx: 3x3 rectifying rotation to make image planes co-planar\n> - P_rect_xx: 3x4 projection matrix after rectification\n>\n>Note: When using this dataset you will most likely need to access only\n>P_rect_xx, as this matrix is valid for the rectified image sequences.\n>\n>calib_velo_to_cam.txt: Velodyne-to-camera registration\n>------------------------------------------------------\n>\n> - R: 3x3 rotation matrix\n> - T: 3x1 translation vector\n> - delta_f: deprecated\n> - delta_c: deprecated\n>\n>R|T takes a point in Velodyne coordinates and transforms it into the\n>coordinate system of the left video camera. Likewise it serves as a\n>representation of the Velodyne coordinate frame in camera coordinates.\n>\n>calib_imu_to_velo.txt: GPS/IMU-to-Velodyne registration\n>-------------------------------------------------------\n>\n> - R: 3x3 rotation matrix\n> - T: 3x1 translation vector\n>\n>R|T takes a point in GPS/IMU coordinates and transforms it into the\n>coordinate system of the Velodyne scanner. Likewise it serves as a\n>representation of the GPS/IMU coordinate frame in Velodyne coordinates.\n\nFrom the paper \"Vision meets robotics: The KITTI dataset\", the `P_rect` is defined with element(1,3) and element(2,3) to be 0, but in real calibration file, these two entries are not necessarily exactly zero, especailly element(1,3). Anyway, the value would be 2 orders smaller than element(0,3).\n\nEq.7 in the origin paper: \n\n$$y = P_{rect}^{(i)} R_{rect}^{(0)} T_{velo}^{cam} x$$\n\n$P_{rect}^{(i)}$ is 3x4, named as project, but is actually the intrinsics K.\n\n$R_{rect}^{(0)}$ is 4x4, named as rotation, but is actually appended with an unit diagonal element. This transforms from unrectified camera i to rectified camera i.\n\n\n\n\n$T_{velo}^{cam}$ is 4x4, consistent with the name. It transforms points from velodyne coordinate to (unrectified) camera 0 coordinate.\n\n## odometry data\n\nFrom http://www.cvlibs.net/datasets/kitti/eval_odometry.php: \n- Download odometry data set (grayscale, 22 GB)\n- Download odometry data set (color, 65 GB)\n- Download odometry data set (velodyne laser data, 80 GB)\n- Download odometry data set (calibration files, 1 MB)\n- Download odometry ground truth poses (4 MB)\n- Download odometry development kit (1 MB)\n\nOpenCV load dataset: https://docs.opencv.org/3.4/dc/dfb/group__datasets__slam.html\n\n## Processing KITTI data\n\n### Image with Keypoints\n\n![Image0](figures/gray_image_with_keypoints0.png)\n\n![Image0](figures/gray_image_with_keypoints1.png)\n\nPerformance:\n\n- Image0: Shi-Tomasi detection with n=2767 keypoints in 15.5039 ms\n- Image1: Shi-Tomasi detection with n=3062 keypoints in 12.8016 ms\n- Image0: ORB descriptor extraction in 6.35671 ms\n- Image1: ORB descriptor extraction in 5.10074 ms\n\n### Image with Point Cloud\n\n![Image with Point Cloud](figures/gray_image_with_depth.png)\n\n### Image with Downsampled Point Cloud (grid size = 5)\n\n![Image with Point Cloud](figures/gray_image_with_depth_dnsp.png)\n\n### Depth Image Estimated by averaging the 3 Nearest Neighbors' Depth in the Downsampled Point Cloud\n\n![Image with Point Cloud](figures/gray_image_with_depth_3nn_avg.png)\n\n### Depth Image Estimated by Fitting a Plane from the 3 Nearest Neighbors in the Downsampled Point Cloud\n\n![Image with Point Cloud](figures/gray_image_with_depth_3nn_plane.png)\n\nThis method is recommended in the original DEMO paper, but in the current downsampling method and hyperparameter setting, it's estimation quality is visually lower than naive averaing estimation.\n" }, { "path": "src/visual_odometry/include/visual_odometry/ceres_cost_function.h", "content": "#include \n#include \n#include \n\n#ifndef CERES_COST_FUNCTION_H\n#define CERES_COST_FUNCTION_H\n\nnamespace vloam\n{\nstruct CostFunctor33\n{ // 33 means 3d - 3d observation pair\n CostFunctor33(double observed_x0, double observed_y0, double observed_z0, double observed_x1, double observed_y1,\n double observed_z1)\n : // TODO: check if const & is necessary\n observed_x0(observed_x0)\n , observed_y0(observed_y0)\n , observed_z0(observed_z0)\n , // 3\n observed_x1(observed_x1)\n , observed_y1(observed_y1)\n , observed_z1(observed_z1)\n {\n } // 3\n\n template \n bool operator()(const T* const angles, const T* const t, T* residuals) const\n {\n T point3d_0to1[3];\n T point3d_0[3] = { T(observed_x0), T(observed_y0), T(observed_z0) };\n ceres::AngleAxisRotatePoint(angles, point3d_0, point3d_0to1);\n point3d_0to1[0] += t[0];\n point3d_0to1[1] += t[1];\n point3d_0to1[2] += t[2];\n\n residuals[0] = point3d_0to1[0] - T(observed_x1);\n residuals[1] = point3d_0to1[1] - T(observed_y1);\n residuals[2] = point3d_0to1[2] - T(observed_z1);\n\n return true;\n }\n\n static ceres::CostFunction* Create(const double observed_x0, const double observed_y0, const double observed_z0,\n const double observed_x1, const double observed_y1, const double observed_z1)\n {\n return (new ceres::AutoDiffCostFunction(\n new CostFunctor33(observed_x0, observed_y0, observed_z0, observed_x1, observed_y1, observed_z1)));\n }\n\n double observed_x0, observed_y0, observed_z0, observed_x1, observed_y1,\n observed_z1; // in rectified camera 0 coordinate\n // TODO: check if the repeated creations of cost functions will decreases the performance?\n};\n\nstruct CostFunctor32\n{ // 32 means 3d - 2d observation pair\n CostFunctor32(double observed_x0, double observed_y0, double observed_z0, double observed_x1_bar,\n double observed_y1_bar)\n : // TODO: check if const & is necessary\n observed_x0(observed_x0)\n , observed_y0(observed_y0)\n , observed_z0(observed_z0)\n , // 3\n observed_x1_bar(observed_x1_bar)\n , observed_y1_bar(observed_y1_bar)\n {\n } // 2\n\n template \n bool operator()(const T* const angles, const T* const t, T* residuals) const\n {\n T X0[3] = { T(observed_x0), T(observed_y0), T(observed_z0) };\n T observed_x1_bar_T = T(observed_x1_bar);\n T observed_y1_bar_T = T(observed_y1_bar);\n\n T R_dot_X0[3];\n ceres::AngleAxisRotatePoint(angles, X0, R_dot_X0);\n R_dot_X0[0] += t[0];\n R_dot_X0[1] += t[1];\n R_dot_X0[2] += t[2];\n\n residuals[0] = R_dot_X0[0] - R_dot_X0[2] * observed_x1_bar_T;\n residuals[1] = R_dot_X0[1] - R_dot_X0[2] * observed_y1_bar_T;\n\n return true;\n }\n\n static ceres::CostFunction* Create(const double observed_x0, const double observed_y0, const double observed_z0,\n const double observed_x1_bar, const double observed_y1_bar)\n {\n return (new ceres::AutoDiffCostFunction(\n new CostFunctor32(observed_x0, observed_y0, observed_z0, observed_x1_bar, observed_y1_bar)));\n }\n\n double observed_x0, observed_y0, observed_z0, observed_x1_bar, observed_y1_bar;\n // TODO: check if the repeated creations of cost functions will decreases the performance?\n};\n\nstruct CostFunctor23\n{ // 23 means 2d - 3d observation pair\n CostFunctor23(double observed_x0_bar, double observed_y0_bar, double observed_x1, double observed_y1,\n double observed_z1)\n : // TODO: check if const & is necessary\n observed_x0_bar(observed_x0_bar)\n , observed_y0_bar(observed_y0_bar)\n , // 2\n observed_x1(observed_x1)\n , observed_y1(observed_y1)\n , observed_z1(observed_z1)\n {\n } // 3\n\n template \n bool operator()(const T* const angles, const T* const t, T* residuals) const\n {\n T observed_x0_bar_T = T(observed_x0_bar);\n T observed_y0_bar_T = T(observed_y0_bar);\n\n T angles_inv[3] = { -angles[0], -angles[1], -angles[2] };\n T X1[3] = { T(observed_x1), T(observed_y1), T(observed_z1) };\n\n T RT_dot_X1[3];\n ceres::AngleAxisRotatePoint(angles_inv, X1, RT_dot_X1);\n\n T RT_dot_t[3];\n ceres::AngleAxisRotatePoint(angles_inv, t, RT_dot_t);\n\n T RT_dot_X1_minus_RT_dot_t[3] = { RT_dot_X1[0] - RT_dot_t[0], RT_dot_X1[1] - RT_dot_t[1],\n RT_dot_X1[2] - RT_dot_t[2] };\n\n residuals[0] = RT_dot_X1_minus_RT_dot_t[0] - RT_dot_X1_minus_RT_dot_t[2] * observed_x0_bar_T;\n residuals[1] = RT_dot_X1_minus_RT_dot_t[1] - RT_dot_X1_minus_RT_dot_t[2] * observed_y0_bar_T;\n\n return true;\n }\n\n static ceres::CostFunction* Create(const double observed_x0_bar, const double observed_y0_bar,\n const double observed_x1, const double observed_y1, const double observed_z1)\n {\n return (new ceres::AutoDiffCostFunction(\n new CostFunctor23(observed_x0_bar, observed_y0_bar, observed_x1, observed_y1, observed_z1)));\n }\n\n double observed_x0_bar, observed_y0_bar, observed_x1, observed_y1, observed_z1;\n // TODO: check if the repeated creations of cost functions will decreases the performance?\n};\n\nstruct CostFunctor22\n{ // 22 means 2d - 2d observation pair\n CostFunctor22(double observed_x0_bar, double observed_y0_bar, double observed_x1_bar, double observed_y1_bar)\n : // TODO: check if const & is necessary\n observed_x0_bar(observed_x0_bar)\n , observed_y0_bar(observed_y0_bar)\n , // 2\n observed_x1_bar(observed_x1_bar)\n , observed_y1_bar(observed_y1_bar)\n {\n } // 2\n\n template \n bool operator()(const T* const angles, const T* const t, T* residuals) const\n {\n T observed_X0_bar_T[3] = { T(observed_x0_bar), T(observed_y0_bar), T(1.0) };\n T observed_X1_bar_T[3] = { T(observed_x1_bar), T(observed_y1_bar), T(1.0) };\n\n T observed_X0_bar_T_to1[3];\n ceres::AngleAxisRotatePoint(angles, observed_X0_bar_T, observed_X0_bar_T_to1);\n\n T t_cross_observed_X0_bar_T_to1[3];\n ceres::CrossProduct(t, observed_X0_bar_T_to1, t_cross_observed_X0_bar_T_to1);\n\n residuals[0] = ceres::DotProduct(observed_X1_bar_T, t_cross_observed_X0_bar_T_to1);\n\n return true;\n }\n\n static ceres::CostFunction* Create(const double observed_x0_bar, const double observed_y0_bar,\n const double observed_x1_bar, const double observed_y1_bar)\n {\n return (new ceres::AutoDiffCostFunction(\n new CostFunctor22(observed_x0_bar, observed_y0_bar, observed_x1_bar, observed_y1_bar)));\n }\n\n double observed_x0_bar, observed_y0_bar, observed_x1_bar, observed_y1_bar;\n // TODO: check if the repeated creations of cost functions will decreases the performance?\n};\n} // namespace vloam\n\n#endif" }, { "path": "src/visual_odometry/include/visual_odometry/image_util.h", "content": "#include \n#include \n\n#include \n#include \n#include \n#include \n#include \n#include \n\n#ifndef IMAGE_UTIL_H\n#define IMAGE_UTIL_H\n\nnamespace vloam\n{\nenum class DetectorType\n{\n ShiTomasi,\n BRISK,\n FAST,\n ORB,\n AKAZE,\n SIFT\n};\nstatic const std::string DetectorType_str[] = { \"ShiTomasi\", \"BRISK\", \"FAST\", \"ORB\", \"AKAZE\", \"SIFT\" };\nenum class DescriptorType\n{\n BRISK,\n ORB,\n BRIEF,\n AKAZE,\n FREAK,\n SIFT\n};\nstatic const std::string DescriptorType_str[] = { \"BRISK\", \"ORB\", \"BRIEF\", \"AKAZE\", \"FREAK\", \"SIFT\" };\nenum class MatcherType\n{\n BF,\n FLANN\n};\nenum class SelectType\n{\n NN,\n KNN\n};\n\nclass ImageUtil\n{\npublic:\n ImageUtil()\n {\n print_result = false;\n visualize_result = false;\n detector_type = DetectorType::ShiTomasi;\n descriptor_type = DescriptorType::ORB;\n matcher_type = MatcherType::BF;\n selector_type = SelectType::NN;\n }\n\n std::vector detKeypoints(cv::Mat &img);\n std::vector keyPointsNMS(std::vector &&keypoints,\n const int bucket_width = 100, // width for horizontal direction in image plane\n // => x, col\n const int bucket_height = 100, // height for vertical direction in image plane\n // => y, row\n const int max_total_keypoints = 400);\n void saveKeypointsImage(const std::string file_name);\n cv::Mat descKeypoints(std::vector &keypoints, cv::Mat &img);\n std::vector matchDescriptors(cv::Mat &desc_source, cv::Mat &desc_ref);\n void visualizeMatchesCallBack(int event, int x, int y);\n cv::Mat visualizeMatches(const cv::Mat &image0, const cv::Mat &image1, const std::vector &keypoints0,\n const std::vector &keypoints1, const std::vector &matches);\n std::tuple, std::vector, std::vector>\n calculateOpticalFlow(const cv::Mat &image0, const cv::Mat &image1, const std::vector &keypoints0);\n cv::Mat visualizeOpticalFlow(const cv::Mat &image1, const std::vector &keypoints0_2f,\n const std::vector &keypoints1_2f,\n const std::vector &optical_flow_status);\n cv::Mat visualizeOpticalFlow(const cv::Mat &image1, const std::vector &keypoints0,\n const std::vector &keypoints1, const std::vector &matches);\n\n // std::string path_prefix;\n bool print_result;\n bool visualize_result;\n DetectorType detector_type;\n DescriptorType descriptor_type;\n MatcherType matcher_type;\n SelectType selector_type;\n\n int remove_VO_outlier;\n bool optical_flow_match;\n\nprivate:\n double time;\n cv::Mat img_keypoints;\n const int IMG_HEIGHT = 375;\n const int IMG_WIDTH = 1242;\n};\n} // namespace vloam\n\n#endif" }, { "path": "src/visual_odometry/include/visual_odometry/point_cloud_util.h", "content": "#include \n#include \n\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\n#ifndef POINT_CLOUD_UTIL_H\n#define POINT_CLOUD_UTIL_H\n\nnamespace vloam\n{\nclass PointCloudUtil\n{\npublic:\n PointCloudUtil()\n {\n cam_T_velo = Eigen::Matrix4f::Zero();\n rect0_T_cam = Eigen::Matrix4f::Zero();\n P_rect0 = Eigen::MatrixXf::Zero(3, 4);\n print_result = false;\n downsample_grid_size = 5;\n }\n void loadTransformations(const std::string& calib_cam_to_cam_file_path,\n const std::string& calib_velo_to_cam_file_path);\n void loadPointCloud(const std::string& bin_file_path);\n void projectPointCloud();\n void printPointCloud2dStats() const;\n void downsamplePointCloud();\n void visualizePointCloud(const std::string image_file_path, const bool select_downsampled = true);\n float queryDepth(const float x, const float y, const int searching_radius = 2) const;\n void visualizeDepthCallBack(int event, int x, int y);\n void visualizeDepth(const std::string image_file_path);\n cv::Mat visualizeDepth(const cv::Mat& gray_image);\n\n // private:\n const int IMG_HEIGHT = 375;\n const int IMG_WIDTH = 1242;\n Eigen::Matrix4f cam_T_velo; // T_velo^cam or cam_T_velo // TODO: rewrite related functions using Eigen::isometry or\n // Eigen::Transforma; https://eigen.tuxfamily.org/dox/classEigen_1_1Transform.html\n Eigen::Matrix4f rect0_T_cam; // T_cam0^rect0 or rect0_T_cam0 // NOTE: typedef Transform Isometry3f\n Eigen::MatrixXf P_rect0; // TODO: combine the 3 transformations into 1 3x4 matrix\n\n Eigen::MatrixXf point_cloud_3d_tilde; // row size is dynamic, and will be decided when load the point cloud; column\n // size is fixed as 4\n Eigen::MatrixXf point_cloud_2d;\n Eigen::MatrixXf point_cloud_2d_dnsp;\n\n int downsample_grid_size;\n\n Eigen::MatrixXf bucket_x;\n Eigen::MatrixXf bucket_y;\n Eigen::MatrixXf bucket_depth;\n Eigen::MatrixXi bucket_count;\n\n cv::Mat image_with_point_cloud;\n cv::Mat image_with_depth;\n\n bool print_result;\n};\n\n} // namespace vloam\n\n#endif" }, { "path": "src/visual_odometry/include/visual_odometry/visual_odometry.h", "content": "#pragma once\n\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\n#include \n#include \n#include \n\n#ifndef VISUAL_ODOMETRY_H\n#define VISUAL_ODOMETRY_H\n\nnamespace vloam\n{\n// typedef message_filters::sync_policies::ApproximateTime VO_policy;\n\nclass VisualOdometry\n{\npublic:\n VisualOdometry();\n\n void init(std::shared_ptr& vloam_tf_);\n\n void reset();\n\n void processImage(const cv::Mat& img00);\n\n // void setUpPointCloud(const Eigen::Isometry3f& imu_eigen_T_cam0, const Eigen::Isometry3f& imu_eigen_T_velo, const\n // sensor_msgs::CameraInfoConstPtr& camera_info_msg);\n void setUpPointCloud(const sensor_msgs::CameraInfoConstPtr& camera_info_msg);\n\n void processPointCloud(const sensor_msgs::PointCloud2ConstPtr& point_cloud_msg,\n const pcl::PointCloud& point_cloud_pcl, const bool& visualize_depth,\n const bool& publish_point_cloud);\n\n void solveRANSAC();\n void solveNlsAll();\n void solveNls2dOnly();\n\n void publish();\n\n // private:\n std::shared_ptr vloam_tf;\n\n int i, j, count;\n\n vloam::ImageUtil image_util;\n std::vector images;\n std::vector> keypoints;\n std::vector descriptors;\n std::vector matches;\n std::vector> keypoints_2f;\n std::vector optical_flow_status;\n\n std::vector point_cloud_utils;\n Eigen::MatrixXf point_cloud_3d_tilde;\n pcl::PointCloud point_cloud_pcl;\n\n float depth0, depth1;\n double angles_0to1[3];\n double t_0to1[3];\n Eigen::Vector3f point_3d_image0_0;\n Eigen::Vector3f point_3d_image0_1;\n Eigen::Vector3f point_3d_rect0_0;\n Eigen::Vector3f point_3d_rect0_1;\n ros::Publisher pub_point_cloud;\n\n // ceres::LossFunction *loss_function = new ceres::CauchyLoss(0.5);\n ceres::Solver::Options options;\n ceres::Solver::Summary summary;\n\n float angle;\n tf2::Transform cam0_curr_T_cam0_last;\n tf2::Quaternion cam0_curr_q_cam0_last;\n\nprivate:\n ros::NodeHandle nh;\n\n int verbose_level;\n bool reset_VO_to_identity;\n int remove_VO_outlier;\n bool keypoint_NMS;\n bool CLAHE;\n cv::Ptr clahe;\n bool visualize_optical_flow;\n bool optical_flow_match;\n\n nav_msgs::Odometry visualOdometry;\n ros::Publisher pubvisualOdometry;\n nav_msgs::Path visualPath;\n ros::Publisher pubvisualPath;\n image_transport::Publisher pub_matches_viz;\n cv_bridge::CvImage matches_viz_cvbridge;\n image_transport::Publisher pub_depth_viz;\n cv_bridge::CvImage depth_viz_cvbridge;\n image_transport::Publisher pub_optical_flow_viz;\n cv_bridge::CvImage optical_flow_viz_cvbridge;\n};\n} // namespace vloam\n\n#endif" }, { "path": "src/visual_odometry/package.xml", "content": "\n\n visual_odometry\n 0.0.0\n The visual_odometry package\n\n \n \n \n yukun\n\n\n \n \n \n MIT\n\n\n \n \n \n \n\n\n \n \n \n \n\n\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n catkin\n cv_bridge\n roscpp\n sensor_msgs\n std_msgs\n roslib\n Ceres\n vloam_tf\n cv_bridge\n roscpp\n sensor_msgs\n std_msgs\n cv_bridge\n roscpp\n sensor_msgs\n std_msgs\n roslib\n Ceres\n vloam_tf\n\n\n \n \n \n\n \n\n" }, { "path": "src/visual_odometry/src/image_util.cpp", "content": "#include \n\nnamespace vloam\n{\nstd::vector ImageUtil::detKeypoints(cv::Mat& img)\n{\n std::vector keypoints;\n\n if (print_result)\n time = (double)cv::getTickCount();\n\n if (detector_type == DetectorType::ShiTomasi)\n {\n int block_size =\n 5; // size of an average block for computing a derivative covariation matrix over each pixel neighborhood\n // double max_overlap = 0.0; // max. permissible overlap between two features in %\n // double min_distance = (1.0 - max_overlap) * block_size;\n double min_distance = block_size * 1.5;\n // int maxCorners = img.rows * img.cols / std::max(1.0, min_distance); // max. num. of keypoints\n int maxCorners = 1024;\n\n double quality_level = 0.03; // minimal accepted quality of image corners\n double k = 0.04;\n\n std::vector corners;\n cv::goodFeaturesToTrack(img, corners, maxCorners, quality_level, min_distance, cv::Mat(), block_size, false, k);\n\n // add corners to result vector\n for (auto it = corners.begin(); it != corners.end(); ++it)\n {\n cv::KeyPoint new_keypoint;\n new_keypoint.pt = cv::Point2f((*it).x, (*it).y);\n new_keypoint.size = block_size;\n keypoints.push_back(new_keypoint);\n }\n }\n else if (detector_type == DetectorType::FAST)\n {\n int threshold = 100;\n cv::FAST(img, keypoints, threshold, true);\n }\n else\n {\n cv::Ptr detector;\n if (detector_type == DetectorType::BRISK)\n detector = cv::BRISK::create();\n else if (detector_type == DetectorType::ORB)\n {\n int num_features = 2000;\n float scaleFactor = 1.2f;\n int nlevels = 8;\n int edgeThreshold = 31;\n int firstLevel = 0;\n int WTA_K = 2;\n cv::ORB::ScoreType scoreType = cv::ORB::FAST_SCORE;\n int patchSize = 31;\n int fastThreshold = 20;\n detector = cv::ORB::create(num_features, scaleFactor, nlevels, edgeThreshold, firstLevel, WTA_K, scoreType,\n patchSize, fastThreshold);\n }\n else if (detector_type == DetectorType::AKAZE)\n detector = cv::AKAZE::create();\n else if (detector_type == DetectorType::SIFT)\n detector = cv::SIFT::create();\n else\n {\n std::cerr << \"Detector is not implemented\" << std::endl;\n exit(EXIT_FAILURE);\n }\n\n detector->detect(img, keypoints);\n }\n\n if (print_result)\n {\n time = ((double)cv::getTickCount() - time) / cv::getTickFrequency();\n std::cout << DetectorType_str[static_cast(detector_type)] + \"detection with n=\" << keypoints.size()\n << \" keypoints in \" << 1000 * time / 1.0 << \" ms\" << std::endl;\n }\n\n if (visualize_result)\n {\n // std::vector fake_keypoints;\n // fake_keypoints.push_back(keypoints[0]);\n // std::cout << \"fake keypoints 0: \" << keypoints[0].pt.x << \", \" << keypoints[0].pt.y << std::endl;\n\n img_keypoints = img.clone();\n // cv::drawKeypoints(img, fake_keypoints, img_keypoints, cv::Scalar::all(-1),\n // cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS);\n cv::drawKeypoints(img, keypoints, img_keypoints, cv::Scalar::all(-1), cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS);\n std::string window_name = DetectorType_str[static_cast(detector_type)] + \" Detector Results\";\n cv::namedWindow(window_name, 6);\n cv::imshow(window_name, img_keypoints);\n cv::waitKey(0);\n }\n\n return keypoints;\n}\n\nstd::vector ImageUtil::keyPointsNMS( // TODO: check if opencv detector minDistance helps here\n std::vector&& keypoints,\n const int bucket_width, // width for horizontal direction in image plane => x, col\n const int bucket_height, // height for vertical direction in image plane => y, row\n const int max_total_keypoints)\n{\n const int bucket_shape_x = std::ceil(static_cast(IMG_WIDTH) / static_cast(bucket_width)); // 13\n const int bucket_shape_y = std::ceil(static_cast(IMG_HEIGHT) / static_cast(bucket_height)); // 4\n\n const int max_bucket_keypoints = max_total_keypoints / (bucket_shape_x * bucket_shape_y); // 7\n\n std::vector>> bucket(\n bucket_shape_x, std::vector>(bucket_shape_y, std::vector()));\n\n // put all keypoints into buckets\n for (const auto& keypoint : keypoints)\n {\n bucket[static_cast(keypoint.pt.x / static_cast(bucket_width))]\n [static_cast(keypoint.pt.y / static_cast(bucket_height))]\n .push_back(keypoint);\n }\n\n std::vector keypoints_after_NMS;\n keypoints_after_NMS.reserve(max_total_keypoints);\n\n auto keypoints_sort = [](const cv::KeyPoint& kp0, const cv::KeyPoint& kp1) { return kp0.response > kp1.response; };\n\n // iterate all bucket, sort and put keypoints with top response to the return\n int col, row;\n for (col = 0; col < bucket_shape_x; ++col)\n {\n for (row = 0; row < bucket_shape_y; ++row)\n {\n if (bucket[col][row].empty())\n continue;\n\n if (bucket[col][row].size() <= max_bucket_keypoints)\n {\n std::copy(bucket[col][row].begin(), bucket[col][row].end(), std::back_inserter(keypoints_after_NMS));\n continue;\n }\n\n std::sort(bucket[col][row].begin(), bucket[col][row].end(),\n keypoints_sort); // ascending order of keypoint response\n std::copy(bucket[col][row].begin(), bucket[col][row].begin() + max_bucket_keypoints,\n std::back_inserter(keypoints_after_NMS));\n }\n }\n\n return keypoints_after_NMS;\n}\n\nvoid ImageUtil::saveKeypointsImage(const std::string file_name)\n{\n if (!img_keypoints.data)\n {\n printf(\"No keypoints data \\n\");\n return;\n }\n cv::imwrite(ros::package::getPath(\"visual_odometry\") + \"/figures/\" + file_name + \".png\", img_keypoints);\n}\n\ncv::Mat ImageUtil::descKeypoints(std::vector& keypoints, cv::Mat& img)\n{\n cv::Mat descriptors;\n\n cv::Ptr extractor;\n if (descriptor_type == DescriptorType::BRISK)\n {\n int threshold = 30; // FAST/AGAST detection threshold score.\n int octaves = 3; // detection octaves (use 0 to do single scale)\n float pattern_scale = 1.0f; // apply this scale to the pattern used for sampling the neighbourhood of a keypoint.\n\n extractor = cv::BRISK::create(threshold, octaves, pattern_scale);\n }\n // else if (descriptor_type == DescriptorType::BRIEF) {\n // extractor = cv::xfeatures2d::BriefDescriptorExtractor::create();\n // }\n else if (descriptor_type == DescriptorType::ORB)\n {\n extractor = cv::ORB::create();\n }\n // else if (descriptor_type == DescriptorType::FREAK) {\n // extractor = cv::xfeatures2d::FREAK::create();\n // }\n else if (descriptor_type == DescriptorType::AKAZE)\n {\n extractor = cv::AKAZE::create();\n }\n else if (descriptor_type == DescriptorType::SIFT)\n {\n extractor = cv::SIFT::create();\n }\n else\n {\n std::cerr << \"Decscriptor is not implemented\" << std::endl;\n exit(EXIT_FAILURE);\n }\n\n if (print_result)\n time = (double)cv::getTickCount();\n\n extractor->compute(img, keypoints, descriptors);\n\n if (print_result)\n {\n time = ((double)cv::getTickCount() - time) / cv::getTickFrequency();\n std::cout << DescriptorType_str[static_cast(descriptor_type)] << \" descriptor extraction in \"\n << 1000 * time / 1.0 << \" ms\" << std::endl;\n }\n\n return descriptors;\n}\n\nstd::vector ImageUtil::matchDescriptors(cv::Mat& descriptors0, cv::Mat& descriptors1)\n{\n std::vector matches;\n bool crossCheck = (selector_type == SelectType::NN);\n cv::Ptr matcher;\n\n // Reference: https://docs.opencv.org/master/dc/dc3/tutorial_py_matcher.html\n if (matcher_type == MatcherType::BF)\n {\n int normType;\n if (descriptor_type == DescriptorType::AKAZE or descriptor_type == DescriptorType::BRISK or\n descriptor_type == DescriptorType::ORB)\n {\n normType = cv::NORM_HAMMING;\n }\n else if (descriptor_type == DescriptorType::SIFT)\n {\n normType = cv::NORM_L2;\n }\n else\n {\n std::cerr << \"Decscriptor is not implemented\" << std::endl;\n }\n matcher = cv::BFMatcher::create(normType, crossCheck);\n }\n else if (matcher_type == MatcherType::FLANN)\n {\n if (descriptors0.type() != CV_32F)\n { // OpenCV bug workaround : convert binary descriptors to floating point due to a bug in current OpenCV\n // implementation\n descriptors0.convertTo(descriptors0, CV_32F);\n }\n if (descriptors1.type() != CV_32F)\n { // OpenCV bug workaround : convert binary descriptors to floating point due to a bug in current OpenCV\n // implementation\n descriptors1.convertTo(descriptors1, CV_32F);\n }\n\n matcher = cv::DescriptorMatcher::create(cv::DescriptorMatcher::FLANNBASED);\n }\n\n if (print_result)\n time = (double)cv::getTickCount();\n\n if (selector_type == SelectType::NN)\n {\n matcher->match(descriptors0, descriptors1, matches);\n\n if (print_result)\n {\n time = ((double)cv::getTickCount() - time) / cv::getTickFrequency();\n std::cout << \" (NN) with n=\" << matches.size() << \" matches in \" << 1000 * time / 1.0 << \" ms\" << std::endl;\n }\n }\n else if (selector_type == SelectType::KNN)\n { // k nearest neighbors (k=2)\n // double t = (double)cv::getTickCount();\n\n std::vector> knn_matches;\n matcher->knnMatch(descriptors0, descriptors1, knn_matches, 2);\n for (const auto& knn_match : knn_matches)\n {\n if (knn_match[0].distance < 0.8 * knn_match[1].distance)\n {\n matches.push_back(knn_match[0]);\n }\n }\n\n if (print_result)\n {\n time = ((double)cv::getTickCount() - time) / cv::getTickFrequency();\n std::cout << \" (KNN) with n=\" << matches.size() << \" matches in \" << 1000 * time / 1.0 << \" ms\" << std::endl;\n std::cout << \"KNN matches = \" << knn_matches.size() << \", qualified matches = \" << matches.size()\n << \", discard ratio = \" << (float)(knn_matches.size() - matches.size()) / (float)knn_matches.size()\n << std::endl;\n }\n }\n\n if (print_result)\n std::cout << \"MATCH KEYPOINT DESCRIPTORS done, and the number of matches is \" << matches.size() << std::endl;\n\n return matches;\n}\n\nvoid ImageUtil::visualizeMatchesCallBack(int event, int x, int y)\n{\n if (event == cv::EVENT_LBUTTONDOWN)\n {\n std::cout << \"Left button of the mouse is clicked - position (\" << x << \", \" << y << \")\" << std::endl;\n }\n}\n\nvoid visualizeMatchesOnMouse(int ev, int x, int y, int, void* obj)\n{\n ImageUtil* iu = static_cast(obj);\n if (iu)\n iu->visualizeMatchesCallBack(ev, x, y);\n}\n\ncv::Mat ImageUtil::visualizeMatches(const cv::Mat& image0, const cv::Mat& image1,\n const std::vector& keypoints0,\n const std::vector& keypoints1, const std::vector& matches)\n{\n std::vector matches_dnsp;\n const int stride = std::ceil(static_cast(matches.size()) / 100.0f); // at most 100 points\n\n int prev_pt_x, prev_pt_y, curr_pt_x, curr_pt_y;\n for (int i = 0; i < matches.size(); i += stride)\n {\n prev_pt_x = keypoints0[matches[i].queryIdx].pt.x;\n prev_pt_y = keypoints0[matches[i].queryIdx].pt.y;\n curr_pt_x = keypoints1[matches[i].trainIdx].pt.x;\n curr_pt_y = keypoints1[matches[i].trainIdx].pt.y;\n if (remove_VO_outlier > 0)\n {\n if (std::pow(prev_pt_x - curr_pt_x, 2) + std::pow(prev_pt_y - curr_pt_y, 2) >\n remove_VO_outlier * remove_VO_outlier)\n continue;\n }\n matches_dnsp.push_back(matches[i]);\n }\n\n cv::Mat matchImg = image1.clone();\n cv::drawMatches(image0, keypoints0, image1, keypoints1, matches_dnsp, matchImg, cv::Scalar::all(-1),\n cv::Scalar::all(-1), std::vector(), cv::DrawMatchesFlags::DEFAULT);\n\n // std::string windowName = \"Matching keypoints between two camera images\";\n // cv::namedWindow(windowName, 7);\n // cv::setMouseCallback(windowName, visualizeMatchesOnMouse, this);\n // cv::imshow(windowName, matchImg);\n // std::cout << \"Press key to continue to next image\" << std::endl;\n // cv::waitKey(0); // wait for key to be pressed\n // ROS_INFO(\"image showed\");\n\n return matchImg;\n}\n\nstd::tuple, std::vector, std::vector> ImageUtil::calculateOpticalFlow(\n const cv::Mat& image0, const cv::Mat& image1, const std::vector& keypoints0)\n{\n std::vector keypoints0_2f;\n std::vector keypoints1_2f;\n std::vector optical_flow_status;\n\n // transform keypoints to points_2f\n std::transform(keypoints0.cbegin(), keypoints0.cend(), std::back_inserter(keypoints0_2f),\n [](const cv::KeyPoint& keypoint) { return keypoint.pt; });\n\n // calculate optical flow\n std::vector err;\n cv::TermCriteria criteria = cv::TermCriteria((cv::TermCriteria::COUNT) + (cv::TermCriteria::EPS), 10, 0.03);\n cv::calcOpticalFlowPyrLK(image0, image1, keypoints0_2f, keypoints1_2f, optical_flow_status, err, cv::Size(15, 15), 2,\n criteria);\n\n ROS_INFO(\"Optical Flow: total candidates = %ld\",\n std::count(optical_flow_status.cbegin(), optical_flow_status.cend(), 1));\n\n return std::make_tuple(std::cref(keypoints0_2f), std::cref(keypoints1_2f), std::cref(optical_flow_status));\n}\n\ncv::Mat ImageUtil::visualizeOpticalFlow(const cv::Mat& image1, const std::vector& keypoints0_2f,\n const std::vector& keypoints1_2f,\n const std::vector& optical_flow_status)\n{\n // Create some random colors\n cv::Mat image1_color = image1.clone();\n cv::cvtColor(image1_color, image1_color, cv::COLOR_GRAY2BGR);\n cv::RNG rng;\n cv::Scalar color;\n int r, g, b, j;\n for (j = 0; j < keypoints0_2f.size(); ++j)\n {\n // Select good points\n if (optical_flow_status[j] == 1)\n {\n // draw the tracks\n r = rng.uniform(0, 256);\n g = rng.uniform(0, 256);\n b = rng.uniform(0, 256);\n color = cv::Scalar(r, g, b);\n cv::line(image1_color, keypoints1_2f[j], keypoints0_2f[j], color, 2);\n cv::circle(image1_color, keypoints1_2f[j], 3, color, -1);\n }\n }\n\n return image1_color;\n}\ncv::Mat ImageUtil::visualizeOpticalFlow(const cv::Mat& image1, const std::vector& keypoints0,\n const std::vector& keypoints1,\n const std::vector& matches)\n{\n // Create some random colors\n cv::Mat image1_color = image1.clone();\n cv::cvtColor(image1_color, image1_color, cv::COLOR_GRAY2BGR);\n cv::RNG rng;\n cv::Scalar color;\n int r, g, b, j;\n for (const auto match : matches)\n {\n // draw the tracks\n r = rng.uniform(0, 256);\n g = rng.uniform(0, 256);\n b = rng.uniform(0, 256);\n color = cv::Scalar(r, g, b);\n cv::line(image1_color, keypoints1[match.trainIdx].pt, keypoints0[match.queryIdx].pt, color, 2);\n cv::circle(image1_color, keypoints1[match.trainIdx].pt, 3, color, -1);\n }\n\n return image1_color;\n}\n} // namespace vloam" }, { "path": "src/visual_odometry/src/point_cloud_util.cpp", "content": "#include \n\nnamespace vloam\n{\nvoid PointCloudUtil::loadTransformations(const std::string& calib_cam_to_cam_file_path,\n const std::string& calib_velo_to_cam_file_path)\n{\n std::string line;\n std::string delim = \" \";\n float value;\n std::string curr;\n\n std::fstream input1(calib_velo_to_cam_file_path.c_str(), std::ios::in);\n if (!input1.good())\n {\n std::cerr << \"Could not read file: \" << calib_velo_to_cam_file_path << std::endl;\n exit(EXIT_FAILURE);\n }\n\n while (getline(input1, line))\n { // read data from file object and put it into string.\n // std::cout << line << \"\\n\"; //print the data of the string\n int start = 0;\n int end = line.find(delim);\n curr = line.substr(start, end - start);\n // std::cout << curr << \", \";\n if (curr == \"R:\")\n {\n int index = 0;\n while (end != std::string::npos)\n {\n start = end + delim.length();\n end = line.find(delim, start);\n curr = line.substr(start, end - start);\n value = boost::lexical_cast(line.substr(start, end - start));\n // std::cout << value << \", \";\n cam_T_velo(index / 3, index % 3) = value;\n ++index;\n }\n }\n if (curr == \"T:\")\n {\n int index = 0;\n while (end != std::string::npos)\n {\n start = end + delim.length();\n end = line.find(delim, start);\n curr = line.substr(start, end - start);\n value = boost::lexical_cast(line.substr(start, end - start));\n // std::cout << value << \", \";\n cam_T_velo(index, 3) = value;\n ++index;\n }\n }\n }\n cam_T_velo(3, 3) = 1;\n\n if (print_result)\n std::cout << \"cam_T_velo = \\n\" << cam_T_velo << \"\\n\" << std::endl;\n\n std::fstream input2(calib_cam_to_cam_file_path.c_str(), std::ios::in);\n if (!input2.good())\n {\n std::cerr << \"Could not read file: \" << calib_cam_to_cam_file_path << std::endl;\n exit(EXIT_FAILURE);\n }\n\n while (getline(input2, line))\n { // read data from file object and put it into string.\n // std::cout << line << \"\\n\"; //print the data of the string\n int start = 0;\n int end = line.find(delim);\n curr = line.substr(start, end - start);\n // std::cout << curr << \", \";\n // TODO: if not just choosing camera 0, check R_0x T_0x for an extra transformation,\n // TODO: and R_00 is an identity matrix, T_00 is (almost) zero matrix\n if (curr == \"R_rect_00:\")\n {\n int index = 0;\n while (end != std::string::npos)\n {\n start = end + delim.length();\n end = line.find(delim, start);\n curr = line.substr(start, end - start);\n value = boost::lexical_cast(line.substr(start, end - start));\n // std::cout << value << \", \";\n rect0_T_cam(index / 3, index % 3) = value;\n ++index;\n }\n rect0_T_cam(3, 3) = 1;\n }\n if (curr == \"P_rect_00:\")\n {\n int index = 0;\n while (end != std::string::npos)\n {\n start = end + delim.length();\n end = line.find(delim, start);\n curr = line.substr(start, end - start);\n value = boost::lexical_cast(line.substr(start, end - start));\n // std::cout << value << \", \";\n P_rect0(index / 4, index % 4) = value;\n ++index;\n }\n }\n }\n\n if (print_result)\n {\n std::cout << \"rect0_T_cam = \\n\" << rect0_T_cam << \"\\n\" << std::endl;\n std::cout << \"P_rect0 = \\n\" << P_rect0 << \"\\n\" << std::endl;\n }\n\n input1.close();\n input2.close();\n}\n\nvoid PointCloudUtil::loadPointCloud(const std::string& bin_file_path)\n{\n int32_t num = 1000000; // about 10 times larger than the real point cloud size\n float* data = (float*)malloc(num * sizeof(float));\n\n // pointers\n float* px = data + 0;\n float* py = data + 1;\n float* pz = data + 2;\n\n // load point cloud\n FILE* stream;\n stream = fopen(bin_file_path.c_str(), \"rb\");\n num = fread(data, sizeof(float), num, stream) / 4;\n point_cloud_3d_tilde = Eigen::MatrixXf::Ones(num, 4); // TODO: check if this affect performance\n // std::cout << num << std::endl;\n for (int32_t i = 0; i < num; i++)\n {\n point_cloud_3d_tilde(i, 0) = *px;\n point_cloud_3d_tilde(i, 1) = *py;\n point_cloud_3d_tilde(i, 2) = *pz;\n px += 4;\n py += 4;\n pz += 4;\n }\n\n fclose(stream);\n free(data);\n}\n\nvoid PointCloudUtil::projectPointCloud()\n{\n // Projection\n Eigen::MatrixXf point_cloud_3d =\n point_cloud_3d_tilde * cam_T_velo.transpose() * rect0_T_cam.transpose() *\n P_rect0.transpose(); // shape = point_cloud_3d_tilde.rows(), 3; in rect cam0 coordinate; last column is the depth\n\n // Screen out back points\n Eigen::VectorXi select_front =\n (point_cloud_3d.col(2).array() > 0.1).cast(); // depth should not just be positive, but greater than a\n // threshold\n Eigen::MatrixXf point_cloud_3d_front(select_front.sum(), 3);\n int i, j = 0;\n for (i = 0; i < point_cloud_3d.rows(); ++i)\n {\n if (select_front(i))\n {\n point_cloud_3d_front.row(j) = point_cloud_3d.row(i);\n ++j;\n }\n }\n\n // From homogeneous to normal coordiante, last column is still depth\n point_cloud_2d = point_cloud_3d_front;\n point_cloud_2d.leftCols(2) =\n point_cloud_2d.leftCols(2).array().colwise() * Eigen::inverse(point_cloud_2d.col(2).array());\n}\n\nvoid PointCloudUtil::printPointCloud2dStats() const\n{\n std::cout << \"min x = \" << point_cloud_2d.col(0).minCoeff() << std::endl;\n std::cout << \"max x = \" << point_cloud_2d.col(0).maxCoeff() << std::endl;\n std::cout << \"mean x = \" << point_cloud_2d.col(0).mean() << std::endl;\n std::cout << \"std x = \"\n << std::sqrt((point_cloud_2d.col(0).array() - point_cloud_2d.col(0).mean()).square().sum() /\n (point_cloud_2d.rows() - 1))\n << std::endl;\n\n std::cout << \"\\nmin y = \" << point_cloud_2d.col(1).minCoeff() << std::endl;\n std::cout << \"max y = \" << point_cloud_2d.col(1).maxCoeff() << std::endl;\n std::cout << \"mean y = \" << point_cloud_2d.col(1).mean() << std::endl;\n std::cout << \"std y = \"\n << std::sqrt((point_cloud_2d.col(1).array() - point_cloud_2d.col(1).mean()).square().sum() /\n (point_cloud_2d.rows() - 1))\n << std::endl;\n\n std::cout << \"\\nmin depth = \" << point_cloud_2d.col(2).minCoeff() << std::endl;\n std::cout << \"max depth = \" << point_cloud_2d.col(2).maxCoeff() << std::endl;\n std::cout << \"mean depth = \" << point_cloud_2d.col(2).mean() << std::endl;\n std::cout << \"std depth = \"\n << std::sqrt((point_cloud_2d.col(2).array() - point_cloud_2d.col(2).mean()).square().sum() /\n (point_cloud_2d.rows() - 1))\n << \"\\n\"\n << std::endl;\n // min depth = 0.100293, max depth = 78.3935\n}\n\nvoid PointCloudUtil::downsamplePointCloud()\n{\n const int new_width = std::ceil(static_cast(IMG_WIDTH) / static_cast(downsample_grid_size));\n const int new_height = std::ceil(static_cast(IMG_HEIGHT) / static_cast(downsample_grid_size));\n bucket_x = Eigen::MatrixXf::Zero(new_width, new_height); // TODO: check if these lines are costly\n bucket_y = Eigen::MatrixXf::Zero(new_width, new_height);\n bucket_depth = Eigen::MatrixXf::Zero(new_width, new_height);\n bucket_count = Eigen::MatrixXi::Zero(new_width, new_height);\n\n int index_x, index_y, i, j, global_count = 0;\n for (i = 0; i < point_cloud_2d.rows(); ++i)\n {\n index_x = static_cast(point_cloud_2d(i, 0) / downsample_grid_size);\n index_y = static_cast(point_cloud_2d(i, 1) / downsample_grid_size);\n if (index_x >= 0 and index_x < new_width and index_y >= 0 and index_y < new_height)\n {\n if (bucket_count(index_x, index_y) == 0)\n {\n bucket_x(index_x, index_y) = point_cloud_2d(i, 0);\n bucket_y(index_x, index_y) = point_cloud_2d(i, 1);\n bucket_depth(index_x, index_y) = point_cloud_2d(i, 2);\n ++global_count;\n }\n else\n { // incremental averaging -> TODO: check better averaging method\n bucket_x(index_x, index_y) +=\n (point_cloud_2d(i, 0) - bucket_x(index_x, index_y)) / bucket_count(index_x, index_y);\n bucket_y(index_x, index_y) +=\n (point_cloud_2d(i, 1) - bucket_y(index_x, index_y)) / bucket_count(index_x, index_y);\n bucket_depth(index_x, index_y) +=\n (point_cloud_2d(i, 2) - bucket_depth(index_x, index_y)) / bucket_count(index_x, index_y);\n }\n ++bucket_count(index_x, index_y);\n }\n }\n\n if (print_result)\n std::cout << \"Point number in FOV after downsample = \" << global_count << \"\\n\" << std::endl;\n // grid size = 5 => print \\approx 9.5k => ~10 times less points\n\n point_cloud_2d_dnsp = Eigen::MatrixXf(global_count, 3); // TODO: check if this affect performance\n for (i = 0; i < new_width; ++i)\n {\n for (j = 0; j < new_height; ++j)\n {\n if (bucket_count(i, j) > 0)\n {\n --global_count;\n point_cloud_2d_dnsp(global_count, 0) = bucket_x(i, j);\n point_cloud_2d_dnsp(global_count, 1) = bucket_y(i, j);\n point_cloud_2d_dnsp(global_count, 2) = bucket_depth(i, j);\n }\n }\n }\n assert(global_count == 0);\n}\n\nvoid PointCloudUtil::visualizePointCloud(const std::string image_file_path, const bool select_downsampled)\n{\n image_with_point_cloud = cv::imread(image_file_path, cv::IMREAD_GRAYSCALE);\n // cv::Mat point_cloud_2d_image(IMG_HEIGHT, IMG_WIDTH, CV_8UC3, cv::Scalar(0, 0, 0));\n // TODO: remove points outside of Cam0 FOV -> will be done in downsampling\n assert(image_with_point_cloud.size().height == IMG_HEIGHT);\n assert(image_with_point_cloud.size().width == IMG_WIDTH);\n\n cv::cvtColor(image_with_point_cloud, image_with_point_cloud, cv::COLOR_GRAY2BGR);\n\n Eigen::MatrixXf& point_cloud_2d_ = (select_downsampled) ? point_cloud_2d_dnsp : point_cloud_2d;\n\n float depth, depth_min = 0.1f, depth_max = 50.0f, ratio;\n int i = 0;\n for (i = 0; i < point_cloud_2d_.rows(); ++i)\n {\n depth = point_cloud_2d_(i, 2);\n ratio = std::max(std::min((depth - depth_min) / (depth_max - depth_min), 1.0f), 0.0f);\n if (ratio < 0.5)\n {\n cv::circle(image_with_point_cloud, cv::Point(point_cloud_2d_(i, 0), point_cloud_2d_(i, 1)), // x, y\n 1, cv::Scalar(0, 255 * ratio * 2, 255 * (1 - ratio * 2)), cv::FILLED, cv::LINE_8);\n }\n else\n {\n cv::circle(image_with_point_cloud, cv::Point(point_cloud_2d_(i, 0), point_cloud_2d_(i, 1)), 1,\n cv::Scalar(255 * (ratio - 0.5) * 2, 255 * (1 - (ratio - 0.5) * 2), 0), cv::FILLED, cv::LINE_8);\n }\n }\n\n cv::namedWindow(\"Display Kitti Image With Depth\", cv::WINDOW_AUTOSIZE);\n cv::imshow(\"Display Kitti Image With Depth\", image_with_point_cloud);\n\n cv::waitKey(0); // wait for key to be pressed\n}\n\n// float triangleArea(const Eigen::Vector2f& A, const Eigen::Vector2f& B, const Eigen::Vector2f& C) {\n\n// }\n\nfloat PointCloudUtil::queryDepth(const float x, const float y, const int searching_radius) const\n{\n // grid size and searching radius are respectively recommended to be 5 and 2\n assert(std::ceil(static_cast(IMG_WIDTH) / static_cast(downsample_grid_size)) == bucket_x.rows());\n assert(std::ceil(static_cast(IMG_HEIGHT) / static_cast(downsample_grid_size)) == bucket_x.cols());\n\n // float x = static_cast(c);\n // float y = static_cast(IMG_HEIGHT - r);\n int index_x = static_cast(x / downsample_grid_size);\n int index_y = static_cast(y / downsample_grid_size);\n const int new_width = bucket_x.rows(); // cautious, bucket axis0 is x, axis1 is y => different from image array\n const int new_height = bucket_x.cols();\n\n // select all neighbors in a certain local block\n int index_x_, index_y_;\n std::vector neighbors;\n Eigen::Vector4f neighbor;\n for (index_x_ = index_x - searching_radius; index_x_ <= index_x + searching_radius; ++index_x_)\n {\n for (index_y_ = index_y - searching_radius; index_y_ <= index_y + searching_radius; ++index_y_)\n {\n if (index_x_ >= 0 and index_x_ < new_width and index_y_ >= 0 and index_y_ < new_height and\n bucket_count(index_x_, index_y_) > 0)\n {\n neighbor(0) = bucket_x(index_x_, index_y_);\n neighbor(1) = bucket_y(index_x_, index_y_);\n neighbor(2) = bucket_depth(index_x_, index_y_);\n neighbor(3) = std::sqrt(std::pow(x - neighbor(0), 2) + std::pow(y - neighbor(1), 2));\n neighbors.push_back(neighbor);\n // std::cout << neighbor.transpose() << std::endl;\n }\n }\n }\n\n // edge case, no enough neighbors\n if (neighbors.size() < 10)\n return -1.0f; // a fixed unrealistic value representing query failure\n\n // sort the vector; better ways can be quick select and heapify\n std::sort(neighbors.begin(), neighbors.end(),\n [&](const Eigen::Vector4f& n1, const Eigen::Vector4f& n2) -> bool { return n1(3) < n2(3); });\n\n // // Condition to be satisfied: point x,y should be inside of the n0, n1 and n2 triangle\n // Eigen::Vector2f n0to1 = neighbors[1].head(2) - neighbors[0].head(2);\n // Eigen::Vector2f n0to2 = neighbors[2].head(2) - neighbors[0].head(2);\n // Eigen::Vector2f n0toP;\n // n0toP << x - neighbors[0](0), y - neighbors[0](1);\n // Eigen::Vector2f n1to0 = neighbors[0].head(2) - neighbors[1].head(2);\n // Eigen::Vector2f n1to2 = neighbors[2].head(2) - neighbors[1].head(2);\n // Eigen::Vector2f n1toP;\n // n1toP << x - neighbors[1](0), y - neighbors[1](1);\n // Eigen::Vector2f n2to0 = neighbors[0].head(2) - neighbors[2].head(2);\n // Eigen::Vector2f n2to1 = neighbors[1].head(2) - neighbors[2].head(2);\n // Eigen::Vector2f n2toP;\n // n2toP << x - neighbors[2](0), y - neighbors[2](1);\n\n // if ((n0to1*n0to2) * (n0to1*n0toP) < 0 or )\n\n // float area_012 =\n\n // float z = (neighbors[0](2) + neighbors[1](2) + neighbors[2](2))/3.0f; // TODO: weighted distance -> Done? need to\n // test\n\n // std::cout << neighbors[0].head(3).transpose() << std::endl;\n // std::cout << neighbors[1].head(3).transpose() << std::endl;\n // std::cout << neighbors[2].head(3).transpose() << \"\\n\" << std::endl;\n\n // float depth_max = std::max({\n // neighbors[0](2),\n // neighbors[1](2),\n // neighbors[2](2)\n // });\n // float depth_min = std::min({\n // neighbors[0](2),\n // neighbors[1](2),\n // neighbors[2](2)\n // });\n // if (depth_max - depth_min > 1.0)\n // return -2.0f;\n\n float z = (neighbors[0](2) * neighbors[1](3) * neighbors[2](3) + neighbors[1](2) * neighbors[0](3) * neighbors[2](3) +\n neighbors[2](2) * neighbors[0](3) * neighbors[1](3)) /\n (0.0001f + neighbors[1](3) * neighbors[2](3) + neighbors[0](3) * neighbors[2](3) +\n neighbors[0](3) * neighbors[1](3)); // TODO: weighted distance -> Done? need to test\n assert(z > 0);\n return z;\n // // naive averaging is already providing good estimation => maybe just check the current bucket_depth is also a fast\n // and good estimation\n\n // // select 3 nearest neighbor\n // Eigen::Vector3f n1 = neighbors[0]; // n1 here is the \\hat{X}_i^{k−1} in eq.10 in DEMO paper\n // n1(0) *= n1(2); // go back to 3d coordinate\n // n1(1) *= n1(2); // go back to 3d coordinate\n // Eigen::Vector3f n2 = neighbors[1];\n // n2(0) *= n2(2);\n // n2(1) *= n2(2);\n // Eigen::Vector3f n3 = neighbors[2];\n // n3(0) *= n3(2);\n // n3(1) *= n3(2);\n\n // // calculate depth\n // Eigen::Vector3f cp = (n1 - n3).cross(n1 - n2);\n // float z = n1.dot(cp) / (0.0001f + x*cp(0) + y*cp(1) + cp(2));\n // // assert(z > 0); // can't guarantee\n // return z;\n}\n\nvoid PointCloudUtil::visualizeDepthCallBack(int event, int x, int y)\n{\n if (event == cv::EVENT_LBUTTONDOWN)\n {\n std::cout << \"Left button of the mouse is clicked - position (\" << x << \", \" << y << \"), and the depth is \"\n << PointCloudUtil::queryDepth(x, y) << std::endl;\n }\n}\n\nvoid visualizeDepthOnMouse(int ev, int x, int y, int, void* obj)\n{\n PointCloudUtil* pcu = static_cast(obj);\n if (pcu)\n pcu->visualizeDepthCallBack(ev, x, y);\n}\n\nvoid PointCloudUtil::visualizeDepth(const std::string image_file_path)\n{\n image_with_depth = cv::imread(image_file_path, cv::IMREAD_GRAYSCALE);\n cv::cvtColor(image_with_depth, image_with_depth, cv::COLOR_GRAY2BGR);\n\n int x, y;\n float depth, depth_min = 0.1f, depth_max = 50.0f, ratio;\n for (x = 0; x < IMG_WIDTH; x += 3)\n { // += 3 to make the visualization sparse\n for (y = 0; y < IMG_HEIGHT; y += 3)\n {\n depth = PointCloudUtil::queryDepth(static_cast(x), static_cast(y));\n if (depth > 0)\n {\n ratio = std::max(std::min((depth - depth_min) / (depth_max - depth_min), 1.0f), 0.0f);\n if (ratio < 0.5)\n {\n cv::circle(image_with_depth, cv::Point(x, y), // x, y\n 1, cv::Scalar(0, 255 * ratio * 2, 255 * (1 - ratio * 2)), cv::FILLED, cv::LINE_8);\n }\n else\n {\n cv::circle(image_with_depth, cv::Point(x, y), 1,\n cv::Scalar(255 * (ratio - 0.5) * 2, 255 * (1 - (ratio - 0.5) * 2), 0), cv::FILLED, cv::LINE_8);\n }\n }\n // else {\n // cv::circle(\n // image_with_depth,\n // cv::Point(x, y), // x, y\n // 1,\n // cv::Scalar(255, 0, 255),\n // cv::FILLED,\n // cv::LINE_8\n // );\n // }\n }\n }\n\n cv::namedWindow(\"Display Kitti Sample Image With Depth Estimation\", cv::WINDOW_AUTOSIZE);\n cv::setMouseCallback(\"Display Kitti Sample Image With Depth Estimation\", visualizeDepthOnMouse, this);\n cv::imshow(\"Display Kitti Sample Image With Depth Estimation\", image_with_depth);\n // cv::imwrite(ros::package::getPath(\"visual_odometry\") + \"/figures/gray_image_with_depth_3nn_plane.png\", image2);\n cv::waitKey(0); // wait for key to be pressed\n}\n\ncv::Mat PointCloudUtil::visualizeDepth(const cv::Mat& gray_image)\n{\n image_with_depth = gray_image.clone();\n cv::cvtColor(image_with_depth, image_with_depth, cv::COLOR_GRAY2BGR);\n int x, y;\n float depth, depth_min = 0.1f, depth_max = 50.0f, ratio;\n for (x = 0; x < IMG_WIDTH; x += 3)\n { // += 3 to make the visualization sparse\n for (y = 0; y < IMG_HEIGHT; y += 3)\n {\n depth = PointCloudUtil::queryDepth(static_cast(x), static_cast(y));\n if (depth > 0)\n {\n ratio = std::max(std::min((depth - depth_min) / (depth_max - depth_min), 1.0f), 0.0f);\n if (ratio < 0.5)\n {\n cv::circle(image_with_depth, cv::Point(x, y), // x, y\n 1, cv::Scalar(0, 255 * ratio * 2, 255 * (1 - ratio * 2)), cv::FILLED, cv::LINE_8);\n }\n else\n {\n cv::circle(image_with_depth, cv::Point(x, y), 1,\n cv::Scalar(255 * (ratio - 0.5) * 2, 255 * (1 - (ratio - 0.5) * 2), 0), cv::FILLED, cv::LINE_8);\n }\n }\n }\n }\n\n // cv::namedWindow(\"Display Kitti Sample Image With Depth Estimation\", cv::WINDOW_AUTOSIZE);\n // cv::imshow(\"Display Kitti Sample Image With Depth Estimation\", image_with_depth);\n // cv::waitKey(1); // wait for 1ms\n\n return image_with_depth;\n}\n} // namespace vloam" }, { "path": "src/visual_odometry/src/visual_odometry.cpp", "content": "#include \n\nnamespace vloam\n{\nVisualOdometry::VisualOdometry()\n{\n pub_point_cloud = nh.advertise(\"/point_cloud_follow_VO\", 5);\n}\n\nvoid VisualOdometry::init(std::shared_ptr& vloam_tf_)\n{\n vloam_tf = vloam_tf_;\n\n if (!ros::param::get(\"loam_verbose_level\", verbose_level))\n ROS_BREAK();\n if (!ros::param::get(\"reset_VO_to_identity\", reset_VO_to_identity))\n ROS_BREAK();\n if (!ros::param::get(\"remove_VO_outlier\", remove_VO_outlier))\n ROS_BREAK();\n if (!ros::param::get(\"keypoint_NMS\", keypoint_NMS))\n ROS_BREAK();\n if (!ros::param::get(\"CLAHE\", CLAHE))\n ROS_BREAK();\n if (!ros::param::get(\"visualize_optical_flow\", visualize_optical_flow))\n ROS_BREAK();\n if (!ros::param::get(\"optical_flow_match\", optical_flow_match))\n ROS_BREAK();\n\n count = -1;\n\n clahe = cv::createCLAHE(2.0);\n image_util.print_result = false;\n image_util.visualize_result = false;\n image_util.detector_type = DetectorType::ShiTomasi;\n image_util.descriptor_type = DescriptorType::ORB;\n image_util.matcher_type = MatcherType::BF;\n image_util.selector_type = SelectType::KNN;\n image_util.remove_VO_outlier = remove_VO_outlier;\n image_util.optical_flow_match = optical_flow_match;\n\n images.clear();\n images.resize(2);\n keypoints.clear();\n keypoints.resize(2);\n descriptors.clear();\n descriptors.resize(2);\n matches.clear();\n if (optical_flow_match)\n {\n keypoints_2f.clear();\n keypoints_2f.resize(2);\n }\n\n point_cloud_utils.clear();\n point_cloud_utils.resize(2);\n point_cloud_utils[0].print_result = false;\n point_cloud_utils[0].downsample_grid_size = 5;\n point_cloud_utils[1].print_result = false;\n point_cloud_utils[1].downsample_grid_size = 5;\n\n for (j = 0; j < 3; ++j)\n {\n angles_0to1[j] = 0.0;\n t_0to1[j] = 0.0;\n }\n\n options.max_num_iterations = 100;\n options.linear_solver_type = ceres::DENSE_QR; // TODO: check the best solver\n // Reference: http://ceres-solver.org/nnls_solving.html#linearsolver. For small problems (a couple of hundred\n // parameters and a few thousand residuals) with relatively dense Jacobians, DENSE_QR is the method of choice In our\n // case, residual num is 1000~2000, but num of param is only 6 options.minimizer_progress_to_stdout = true;\n\n cam0_curr_T_cam0_last.setOrigin(tf2::Vector3(0.0, 0.0, 0.0));\n cam0_curr_T_cam0_last.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0));\n\n pubvisualOdometry = nh.advertise(\"/visual_odom_to_init\", 100);\n pubvisualPath = nh.advertise(\"/visual_odom_path\", 100);\n visualPath.poses.clear();\n\n image_transport::ImageTransport it(nh);\n pub_matches_viz = it.advertise(\"/visual_odometry/matches_visualization\", 10);\n pub_depth_viz = it.advertise(\"/visual_odometry/depth_visualization\", 10);\n pub_optical_flow_viz = it.advertise(\"/visual_odometry/optical_flow_visualization\", 10);\n}\n\nvoid VisualOdometry::reset()\n{\n ++count;\n i = count % 2;\n}\n\nvoid VisualOdometry::processImage(const cv::Mat& img00)\n{\n TicToc t_process_image;\n\n // ROS_INFO(\"image type = %d\", img00.type());\n if (CLAHE)\n clahe->apply(img00, images[i]);\n else\n images[i] = img00;\n\n // if (keypoint_NMS)\n // keypoints[i] = image_util.keyPointsNMS(image_util.detKeypoints(images[i]));\n // else\n keypoints[i] = image_util.detKeypoints(images[i]); // TODO: might be optimizd\n\n if (!optical_flow_match)\n descriptors[i] = image_util.descKeypoints(keypoints[i], images[i]);\n\n if (verbose_level > 0)\n {\n ROS_INFO(\"keypoint number = %ld \\n\", keypoints[i].size());\n }\n\n if (count > 0)\n {\n if (!optical_flow_match)\n matches = image_util.matchDescriptors(descriptors[1 - i],\n descriptors[i]); // first one is prev image, second one is curr image\n else\n std::tie(keypoints_2f[1 - i], keypoints_2f[i], optical_flow_status) =\n image_util.calculateOpticalFlow(images[1 - i], images[i], keypoints[i]);\n if (verbose_level > 0)\n {\n ROS_INFO(\"match number = %ld \\n\", matches.size());\n }\n }\n\n ROS_INFO(\"Processing image took %f ms +++++\\n\", t_process_image.toc());\n}\n\nvoid VisualOdometry::setUpPointCloud(const sensor_msgs::CameraInfoConstPtr& camera_info_msg)\n{\n TicToc t_set_up_point_cloud;\n\n point_cloud_utils[0].cam_T_velo = vloam_tf->imu_eigen_T_cam0.matrix().inverse() * vloam_tf->imu_eigen_T_velo.matrix();\n point_cloud_utils[1].cam_T_velo = vloam_tf->imu_eigen_T_cam0.matrix().inverse() * vloam_tf->imu_eigen_T_velo.matrix();\n\n // point from unrectified camera 00 to rectified camera 00\n for (j = 0; j < 9; ++j)\n {\n point_cloud_utils[0].rect0_T_cam(j / 3, j % 3) = camera_info_msg->R[j]; // TODO: optimize this code later\n point_cloud_utils[1].rect0_T_cam(j / 3, j % 3) = camera_info_msg->R[j]; // assume P doesn't change\n }\n\n // point from rectified camera 00 to image coordinate\n for (j = 0; j < 12; ++j)\n {\n point_cloud_utils[0].P_rect0(j / 4, j % 4) = camera_info_msg->P[j]; // TODO: optimize this code later\n point_cloud_utils[1].P_rect0(j / 4, j % 4) = camera_info_msg->P[j]; // assume P doesn't change\n }\n // std::cout << \"\\nP_rect0 = \\n\" << point_cloud_utils[0].P_rect0 << std::endl;\n\n ROS_INFO(\"Setting up point cloud took %f ms +++++\\n\", t_set_up_point_cloud.toc());\n}\n\nvoid VisualOdometry::processPointCloud(const sensor_msgs::PointCloud2ConstPtr& point_cloud_msg,\n const pcl::PointCloud& point_cloud_pcl,\n const bool& visualize_depth, const bool& publish_point_cloud)\n{\n TicToc t_process_point_cloud;\n\n point_cloud_3d_tilde = Eigen::MatrixXf::Ones(point_cloud_pcl.size(), 4);\n for (j = 0; j < point_cloud_pcl.size(); ++j)\n {\n point_cloud_3d_tilde(j, 0) = point_cloud_pcl.points[j].x;\n point_cloud_3d_tilde(j, 1) = point_cloud_pcl.points[j].y;\n point_cloud_3d_tilde(j, 2) = point_cloud_pcl.points[j].z;\n }\n point_cloud_utils[i].point_cloud_3d_tilde = point_cloud_3d_tilde;\n point_cloud_utils[i].projectPointCloud();\n point_cloud_utils[i].downsamplePointCloud();\n if (visualize_depth)\n point_cloud_utils[i].visualizeDepth(images[i]); // uncomment this for depth visualization, but remember to reduce\n // the bag playing speed too\n if (publish_point_cloud)\n {\n sensor_msgs::PointCloud2 point_cloud_in_VO_msg = *point_cloud_msg;\n // ROS_INFO(\"point cloud frame id was %s\", point_cloud_msg->header.frame_id.c_str());\n point_cloud_in_VO_msg.header.frame_id = \"velo\";\n point_cloud_in_VO_msg.header.stamp = ros::Time::now();\n pub_point_cloud.publish(point_cloud_in_VO_msg);\n }\n\n ROS_INFO(\"Processing point cloud took %f ms +++++\\n\", t_process_point_cloud.toc());\n}\n\nvoid VisualOdometry::solveRANSAC()\n{ // TODO: consider KLT case => it's not working with KLT\n TicToc t_ransac;\n\n std::vector prev_pts, curr_pts;\n prev_pts.resize(matches.size());\n curr_pts.resize(matches.size());\n int j = 0;\n for (const auto& match : matches)\n { // ~ n=1400 matches\n prev_pts[j] = keypoints[1 - i][match.queryIdx].pt;\n curr_pts[j] = keypoints[i][match.trainIdx].pt;\n ++j;\n }\n\n cv::Mat camera_matrix;\n cv::eigen2cv(point_cloud_utils[0].P_rect0, camera_matrix);\n // std::cout << camera_matrix << std::endl;\n camera_matrix = camera_matrix.colRange(0, 3);\n cv::Mat E = cv::findEssentialMat(prev_pts, curr_pts, camera_matrix, cv::RANSAC, 0.999, 1.0);\n // cv::Mat E = cv::findEssentialMat(prev_pts, curr_pts, camera_matrix, cv::LMEDS);\n cv::Mat R, t;\n cv::Mat mask, triangulated_points;\n int recover_result = cv::recoverPose(E, prev_pts, curr_pts, camera_matrix, R, t, 200.0f);\n // ROS_INFO(\"recover result is = %d\", recover_result);\n // // ROS_INFO(\"det of R is %.4f\", cv::determinant(R));\n // ROS_INFO(\"R is \\n %.4f, %.4f, %.4f\\n %.4f, %.4f, %.4f \\n %.4f, %.4f, %.4f \\n\",\n // R.at(0, 0), R.at(0, 1), R.at(0, 2),\n // R.at(1, 0), R.at(1, 1), R.at(1, 2),\n // R.at(2, 0), R.at(2, 1), R.at(2, 2));\n\n tf2::Transform T;\n tf2::Matrix3x3 R_tf2(R.at(0, 0), R.at(0, 1), R.at(0, 2), R.at(1, 0),\n R.at(1, 1), R.at(1, 2), R.at(2, 0), R.at(2, 1),\n R.at(2, 2));\n T.setBasis(R_tf2);\n T.setOrigin(tf2::Vector3(t.at(0, 0), t.at(1, 0), t.at(2, 0)));\n tf2::Quaternion q_tf2 = T.getRotation();\n\n // ROS_INFO(\"R quaternion is %.4f, %.4f, %.4f, %.4f\", q_tf2.getX(), q_tf2.getY(), q_tf2.getZ(), q_tf2.getW());\n\n // ROS_INFO(\"From RANSAC, axis angle = %.4f, %.4f, %.4f\",\n // q_tf2.getAxis().getX() * q_tf2.getAngle(),\n // q_tf2.getAxis().getY() * q_tf2.getAngle(),\n // q_tf2.getAxis().getZ() * q_tf2.getAngle()\n // );\n ROS_INFO(\"From RANSAC axis = %.4f, %.4f, %.4f, and angle = %.4f\", q_tf2.getAxis().getX(), q_tf2.getAxis().getY(),\n q_tf2.getAxis().getZ(), q_tf2.getAngle());\n ROS_INFO(\"Froma RANSAC, t = %.4f, %.4f, %.4f\", t.at(0, 0), t.at(1, 0), t.at(2, 0));\n // std::cout << \"t = \" << t << std::endl;\n\n // cam0_curr_T_cam0_last.setRotation(q_tf2);\n // float f2f_distance = std::sqrt(\n // std::pow(cam0_curr_T_cam0_last.getOrigin().getX(), 2) +\n // std::pow(cam0_curr_T_cam0_last.getOrigin().getY(), 2) +\n // std::pow(cam0_curr_T_cam0_last.getOrigin().getZ(), 2)\n // );\n // cam0_curr_T_cam0_last.setOrigin(tf2::Vector3(\n // f2f_distance * t.at(0,0),\n // f2f_distance * t.at(1,0),\n // f2f_distance * t.at(2,0)\n // ));\n\n ROS_INFO(\"RANSAC VO took %f ms +++++\\n\", t_ransac.toc());\n}\n\nvoid VisualOdometry::solveNlsAll()\n{\n TicToc t_nls_all;\n\n ceres::LossFunction* loss_function = new ceres::HuberLoss(0.1);\n ceres::Problem problem;\n\n if (reset_VO_to_identity)\n {\n for (j = 0; j < 3; ++j)\n {\n angles_0to1[j] = 0.0;\n t_0to1[j] = 0.0;\n }\n }\n else\n {\n // init from LO\n t_0to1[0] = vloam_tf->cam0_curr_LOT_cam0_prev.getOrigin().getX();\n t_0to1[1] = vloam_tf->cam0_curr_LOT_cam0_prev.getOrigin().getY();\n t_0to1[2] = vloam_tf->cam0_curr_LOT_cam0_prev.getOrigin().getZ();\n angles_0to1[0] = vloam_tf->cam0_curr_LOT_cam0_prev.getRotation().getAxis().getX() *\n vloam_tf->cam0_curr_LOT_cam0_prev.getRotation().getAngle();\n angles_0to1[1] = vloam_tf->cam0_curr_LOT_cam0_prev.getRotation().getAxis().getY() *\n vloam_tf->cam0_curr_LOT_cam0_prev.getRotation().getAngle();\n angles_0to1[2] = vloam_tf->cam0_curr_LOT_cam0_prev.getRotation().getAxis().getZ() *\n vloam_tf->cam0_curr_LOT_cam0_prev.getRotation().getAngle();\n }\n\n int prev_pt_x, prev_pt_y, curr_pt_x, curr_pt_y;\n int counter33 = 0, counter32 = 0, counter23 = 0, counter22 = 0;\n int match_num = (optical_flow_match) ? keypoints_2f[i].size() : matches.size();\n for (int j = 0; j < match_num; ++j)\n { // ~ n=1400 matches\n\n if (!optical_flow_match)\n {\n prev_pt_x = keypoints[1 - i][matches[j].queryIdx].pt.x;\n prev_pt_y = keypoints[1 - i][matches[j].queryIdx].pt.y;\n curr_pt_x = keypoints[i][matches[j].trainIdx].pt.x;\n curr_pt_y = keypoints[i][matches[j].trainIdx].pt.y;\n }\n else\n {\n if (optical_flow_status[j] == 1)\n {\n prev_pt_x = keypoints_2f[1 - i][j].x;\n prev_pt_y = keypoints_2f[1 - i][j].y;\n curr_pt_x = keypoints_2f[i][j].x;\n curr_pt_y = keypoints_2f[i][j].y;\n }\n else\n continue;\n }\n\n if (remove_VO_outlier > 0)\n {\n if (std::pow(prev_pt_x - curr_pt_x, 2) + std::pow(prev_pt_y - curr_pt_y, 2) >\n remove_VO_outlier * remove_VO_outlier)\n continue;\n }\n\n depth0 = point_cloud_utils[1 - i].queryDepth(prev_pt_x, prev_pt_y);\n depth1 = point_cloud_utils[i].queryDepth(curr_pt_x, curr_pt_y);\n\n // if (std::abs(depth0 - depth1) > 3.0)\n // continue;\n\n // if (depth0 > 0.5 and depth1 > 0.5) {\n // point_3d_image0_0 << prev_pt_x*depth0, prev_pt_y*depth0, depth0;\n // point_3d_image0_1 << curr_pt_x*depth1, curr_pt_y*depth1, depth1;\n\n // point_3d_rect0_0 =\n // (point_cloud_utils[1-i].P_rect0.leftCols(3)).colPivHouseholderQr().solve(point_3d_image0_0); point_3d_rect0_1\n // = (point_cloud_utils[i].P_rect0.leftCols(3)).colPivHouseholderQr().solve(point_3d_image0_1);\n\n // // assert(std::abs(point_3d_rect0_0(2) - depth0) < 0.0001);\n // // assert(std::abs(point_3d_rect0_1(2) - depth1) < 0.0001);\n\n // ceres::CostFunction* cost_function = vloam::CostFunctor33::Create(\n // static_cast(point_3d_rect0_0(0)),\n // static_cast(point_3d_rect0_0(1)),\n // static_cast(point_3d_rect0_0(2)),\n // static_cast(point_3d_rect0_1(0)),\n // static_cast(point_3d_rect0_1(1)),\n // static_cast(point_3d_rect0_1(2))\n // );\n // problem.AddResidualBlock(cost_function, loss_function, angles_0to1, t_0to1);\n // ++counter33;\n // }\n // else if (depth0 > 0.5 and depth1 <= 0.5) {\n if (depth0 > 0)\n {\n point_3d_image0_0 << prev_pt_x * depth0, prev_pt_y * depth0, depth0;\n point_3d_image0_1 << curr_pt_x, curr_pt_y, 1.0f;\n\n point_3d_rect0_0 =\n (point_cloud_utils[1 - i].P_rect0.leftCols(3)).colPivHouseholderQr().solve(point_3d_image0_0); // assume\n // point_cloud_utils[i].P_rect0.col(3)\n // is zero\n point_3d_rect0_1 =\n (point_cloud_utils[i].P_rect0.leftCols(3)).colPivHouseholderQr().solve(point_3d_image0_1); // assume\n // point_cloud_utils[i].P_rect0.col(3)\n // is zero\n\n // assert(std::abs(point_3d_rect0_0(2) - depth0) < 0.0001);\n\n ceres::CostFunction* cost_function = vloam::CostFunctor32::Create(\n static_cast(point_3d_rect0_0(0)), static_cast(point_3d_rect0_0(1)),\n static_cast(point_3d_rect0_0(2)),\n static_cast(point_3d_rect0_1(0)) / static_cast(point_3d_rect0_1(2)),\n static_cast(point_3d_rect0_1(1)) / static_cast(point_3d_rect0_1(2)));\n problem.AddResidualBlock(cost_function, loss_function, angles_0to1, t_0to1);\n ++counter32;\n }\n\n // else if (depth0 <= 0.5 and depth1 > 0.5) {\n // // if (depth1 > 0) {\n // point_3d_image0_0 << prev_pt_x, prev_pt_y, 1.0f;\n // point_3d_image0_1 << curr_pt_x*depth1, curr_pt_y*depth1, depth1;\n\n // point_3d_rect0_0 =\n // (point_cloud_utils[1-i].P_rect0.leftCols(3)).colPivHouseholderQr().solve(point_3d_image0_0); // assume\n // point_cloud_utils[i].P_rect0.col(3) is zero point_3d_rect0_1 =\n // (point_cloud_utils[i].P_rect0.leftCols(3)).colPivHouseholderQr().solve(point_3d_image0_1); // assume\n // point_cloud_utils[i].P_rect0.col(3) is zero\n\n // // // assert(std::abs(point_3d_rect0_1(2) - depth1) < 0.0001);\n\n // ceres::CostFunction* cost_function = vloam::CostFunctor23::Create(\n // static_cast(point_3d_rect0_0(0)) / static_cast(point_3d_rect0_0(2)),\n // static_cast(point_3d_rect0_0(1)) / static_cast(point_3d_rect0_0(2)),\n // static_cast(point_3d_rect0_1(0)),\n // static_cast(point_3d_rect0_1(1)),\n // static_cast(point_3d_rect0_1(2))\n // );\n // problem.AddResidualBlock(cost_function, loss_function, angles_0to1, t_0to1);\n // ++counter23;\n // }\n else\n {\n // if (depth0 < 0 and depth1 < 0) {\n point_3d_image0_0 << prev_pt_x, prev_pt_y, 1.0f;\n point_3d_image0_1 << curr_pt_x, curr_pt_y, 1.0f;\n\n point_3d_rect0_0 =\n (point_cloud_utils[1 - i].P_rect0.leftCols(3)).colPivHouseholderQr().solve(point_3d_image0_0); // assume\n // point_cloud_utils[i].P_rect0.col(3)\n // is zero\n point_3d_rect0_1 =\n (point_cloud_utils[i].P_rect0.leftCols(3)).colPivHouseholderQr().solve(point_3d_image0_1); // assume\n // point_cloud_utils[i].P_rect0.col(3)\n // is zero\n\n ceres::CostFunction* cost_function = vloam::CostFunctor22::Create(\n static_cast(point_3d_rect0_0(0)) / static_cast(point_3d_rect0_0(2)),\n static_cast(point_3d_rect0_0(1)) / static_cast(point_3d_rect0_0(2)),\n static_cast(point_3d_rect0_1(0)) / static_cast(point_3d_rect0_1(2)),\n static_cast(point_3d_rect0_1(1)) / static_cast(point_3d_rect0_1(2)));\n problem.AddResidualBlock(cost_function, loss_function, angles_0to1, t_0to1);\n ++counter22;\n }\n }\n\n // ROS_INFO(\"counter33 = %d\", counter33);\n ROS_INFO(\"counter32 = %d\", counter32);\n // ROS_INFO(\"counter23 = %d\", counter33);\n ROS_INFO(\"counter22 = %d\", counter22);\n\n ceres::Solve(options, &problem, &summary);\n // std::cout << summary.FullReport() << \"\\n\";\n\n cam0_curr_T_cam0_last.setOrigin(tf2::Vector3(t_0to1[0], t_0to1[1], t_0to1[2]));\n angle = std::sqrt(std::pow(angles_0to1[0], 2) + std::pow(angles_0to1[1], 2) + std::pow(angles_0to1[2], 2));\n cam0_curr_q_cam0_last.setRotation(\n tf2::Vector3(angles_0to1[0] / angle, angles_0to1[1] / angle, angles_0to1[2] / angle), angle);\n cam0_curr_T_cam0_last.setRotation(cam0_curr_q_cam0_last);\n\n ROS_INFO(\"angles_0to1 = (%.4f, %.4f, %.4f)\", angles_0to1[0], angles_0to1[1], angles_0to1[2]);\n // ROS_INFO(\"q_0to1 = (%.4f, %.4f, %.4f, %.4f)\", cam0_curr_q_cam0_last.getX(), cam0_curr_q_cam0_last.getY(),\n // cam0_curr_q_cam0_last.getZ(), cam0_curr_q_cam0_last.getW());\n ROS_INFO(\"From nllsq axis = %.4f, %.4f, %.4f, and angle = %.4f\", cam0_curr_q_cam0_last.getAxis().getX(),\n cam0_curr_q_cam0_last.getAxis().getY(), cam0_curr_q_cam0_last.getAxis().getZ(),\n cam0_curr_q_cam0_last.getAngle());\n ROS_INFO(\"t_0to1 = (%.4f, %.4f, %.4f)\", t_0to1[0], t_0to1[1], t_0to1[2]);\n\n // ROS_INFO(\"From LM axis = %.4f, %.4f, %.4f, and angle = %.4f\",\n // cam0_curr_q_cam0_last.getAxis().getX(),\n // cam0_curr_q_cam0_last.getAxis().getY(),\n // cam0_curr_q_cam0_last.getAxis().getZ(),\n // cam0_curr_q_cam0_last.getAngle()\n // );\n\n // return cam0_curr_T_cam0_last;\n\n ROS_INFO(\"Nonlinear Least square (ALL) VO took %f ms +++++\\n\", t_nls_all.toc());\n}\n\nvoid VisualOdometry::publish()\n{\n visualOdometry.header.frame_id = \"map\";\n visualOdometry.child_frame_id = \"visual_odom\";\n visualOdometry.header.stamp = ros::Time::now(); // image_msg->header.stamp;\n Eigen::Quaterniond q_wodom_curr(vloam_tf->world_VOT_base_last.getRotation()); // wodom to cam\n Eigen::Vector3d t_wodom_curr(vloam_tf->world_VOT_base_last.getOrigin()); // wodom to cam\n visualOdometry.pose.pose.orientation.x = q_wodom_curr.x();\n visualOdometry.pose.pose.orientation.y = q_wodom_curr.y();\n visualOdometry.pose.pose.orientation.z = q_wodom_curr.z();\n visualOdometry.pose.pose.orientation.w = q_wodom_curr.w();\n visualOdometry.pose.pose.position.x = t_wodom_curr.x();\n visualOdometry.pose.pose.position.y = t_wodom_curr.y();\n visualOdometry.pose.pose.position.z = t_wodom_curr.z();\n pubvisualOdometry.publish(visualOdometry);\n // ROS_INFO(\"publish visualOdometry x = %f and %f\", vloam_tf->world_VOT_base_last.getOrigin().x(), t_wodom_curr.x());\n\n // vloam_tf->world_VOT_base_last.setOrigin(tf2::Vector3(\n // t_wodom_curr.x(),\n // t_wodom_curr.y(),\n // t_wodom_curr.z()\n // ));\n // vloam_tf->world_VOT_base_last.setRotation(tf2::Quaternion(\n // q_wodom_curr.x(),\n // q_wodom_curr.y(),\n // q_wodom_curr.z(),\n // q_wodom_curr.w()\n // ));\n\n geometry_msgs::PoseStamped visualPose;\n visualPose.header = visualOdometry.header;\n visualPose.pose = visualOdometry.pose.pose;\n visualPath.header.stamp = visualOdometry.header.stamp;\n visualPath.header.frame_id = \"map\";\n visualPath.poses.push_back(visualPose);\n pubvisualPath.publish(visualPath);\n\n if (verbose_level > 0 and count > 1)\n {\n std_msgs::Header header;\n header.stamp = ros::Time::now();\n cv::Mat match_image =\n image_util.visualizeMatches(images[1 - i], images[i], keypoints[1 - i], keypoints[i], matches);\n matches_viz_cvbridge = cv_bridge::CvImage(header, sensor_msgs::image_encodings::RGB8, match_image);\n pub_matches_viz.publish(matches_viz_cvbridge.toImageMsg());\n }\n\n if (verbose_level > 0 and count > 0)\n {\n std_msgs::Header header;\n header.stamp = ros::Time::now();\n cv::Mat depth_image = point_cloud_utils[i].visualizeDepth(images[i]);\n depth_viz_cvbridge = cv_bridge::CvImage(header, sensor_msgs::image_encodings::RGB8, depth_image);\n pub_depth_viz.publish(depth_viz_cvbridge.toImageMsg());\n }\n\n if (verbose_level > 0 and count > 0 and visualize_optical_flow)\n {\n std_msgs::Header header;\n header.stamp = ros::Time::now();\n\n cv::Mat optical_flow_image;\n if (optical_flow_match)\n optical_flow_image =\n image_util.visualizeOpticalFlow(images[i], keypoints_2f[1 - i], keypoints_2f[i], optical_flow_status);\n else\n optical_flow_image = image_util.visualizeOpticalFlow(images[i], keypoints[1 - i], keypoints[i], matches);\n\n optical_flow_viz_cvbridge = cv_bridge::CvImage(header, sensor_msgs::image_encodings::RGB8, optical_flow_image);\n pub_optical_flow_viz.publish(optical_flow_viz_cvbridge.toImageMsg());\n }\n}\n\n} // namespace vloam" }, { "path": "src/vloam_main/CMakeLists.txt", "content": "cmake_minimum_required(VERSION 3.0.2)\nproject(vloam_main)\n\n## Compile as C++11, supported in ROS Kinetic and newer\nadd_compile_options(-std=c++14 -O3)\n\n## Find catkin macros and libraries\n## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)\n## is used, also find other catkin packages\n\nfind_package(OpenCV 4.5.1 REQUIRED)\n\n# find_package(Eigen3 3.3 REQUIRED NO_MODULE)\n\nfind_package(PCL 1.2 REQUIRED)\ninclude_directories(${PCL_INCLUDE_DIRS})\nlink_directories(${PCL_LIBRARY_DIRS})\nadd_definitions(${PCL_DEFINITIONS})\n\nfind_package(Ceres REQUIRED PATHS \"/usr/local/lib/cmake/Ceres/\")\n\n\nfind_package(catkin REQUIRED COMPONENTS\n roscpp\n std_msgs\n cv_bridge\n sensor_msgs\n geometry_msgs\n # tf2\n tf2_ros\n tf2_geometry_msgs\n # tf2_msgs\n message_filters\n # pcl_ros\n pcl_conversions\n nav_msgs\n actionlib\n genmsg\n actionlib_msgs\n visual_odometry\n lidar_odometry_mapping\n vloam_tf\n)\n\n## System dependencies are found with CMake's conventions\n# find_package(Boost REQUIRED COMPONENTS system)\n\n\n## Uncomment this if the package has a setup.py. This macro ensures\n## modules and global scripts declared therein get installed\n## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html\n# catkin_python_setup()\n\n################################################\n## Declare ROS messages, services and actions ##\n################################################\n\n## To declare and build messages, services or actions from within this\n## package, follow these steps:\n## * Let MSG_DEP_SET be the set of packages whose message types you use in\n## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).\n## * In the file package.xml:\n## * add a build_depend tag for \"message_generation\"\n## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET\n## * If MSG_DEP_SET isn't empty the following dependency has been pulled in\n## but can be declared for certainty nonetheless:\n## * add a exec_depend tag for \"message_runtime\"\n## * In this file (CMakeLists.txt):\n## * add \"message_generation\" and every package in MSG_DEP_SET to\n## find_package(catkin REQUIRED COMPONENTS ...)\n## * add \"message_runtime\" and every package in MSG_DEP_SET to\n## catkin_package(CATKIN_DEPENDS ...)\n## * uncomment the add_*_files sections below as needed\n## and list every .msg/.srv/.action file to be processed\n## * uncomment the generate_messages entry below\n## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)\n\n## Generate messages in the 'msg' folder\n# add_message_files(\n# FILES\n# Message1.msg\n# Message2.msg\n# )\n\n## Generate services in the 'srv' folder\n# add_service_files(\n# FILES\n# Service1.srv\n# Service2.srv\n# )\n\n## Generate actions in the 'action' folder\nadd_action_files(\n FILES\n vloam_main.action\n)\n\n## Generate added messages and services with any dependencies listed here\ngenerate_messages(\n DEPENDENCIES\n std_msgs actionlib_msgs\n)\n\n################################################\n## Declare ROS dynamic reconfigure parameters ##\n################################################\n\n## To declare and build dynamic reconfigure parameters within this\n## package, follow these steps:\n## * In the file package.xml:\n## * add a build_depend and a exec_depend tag for \"dynamic_reconfigure\"\n## * In this file (CMakeLists.txt):\n## * add \"dynamic_reconfigure\" to\n## find_package(catkin REQUIRED COMPONENTS ...)\n## * uncomment the \"generate_dynamic_reconfigure_options\" section below\n## and list every .cfg file to be processed\n\n## Generate dynamic reconfigure parameters in the 'cfg' folder\n# generate_dynamic_reconfigure_options(\n# cfg/DynReconf1.cfg\n# cfg/DynReconf2.cfg\n# )\n\n###################################\n## catkin specific configuration ##\n###################################\n## The catkin_package macro generates cmake config files for your package\n## Declare things to be passed to dependent projects\n## INCLUDE_DIRS: uncomment this if your package contains header files\n## LIBRARIES: libraries you create in this project that dependent projects also need\n## CATKIN_DEPENDS: catkin_packages dependent projects also need\n## DEPENDS: system dependencies of this project that dependent projects also need\ncatkin_package(\n# INCLUDE_DIRS include\n# LIBRARIES vloam_main\n# CATKIN_DEPENDS cv_bridge roscpp sensor_msgs std_msgs\n# DEPENDS system_lib\nDEPENDS EIGEN3 PCL\n)\n\n###########\n## Build ##\n###########\n\n## Specify additional locations of header files\n## Your package locations should be listed before other locations\ninclude_directories(\n# include\n ${catkin_INCLUDE_DIRS}\n ${OpenCV_INCLUDE_DIRS}\n)\n\n## Declare a C++ library\n# add_library(${PROJECT_NAME}\n# src/${PROJECT_NAME}/vloam_main.cpp\n# )\n\n## Add cmake target dependencies of the library\n## as an example, code may need to be generated before libraries\n## either from message generation or dynamic reconfigure\n# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})\n\n## Declare a C++ executable\n## With catkin_make all packages are built within a single CMake context\n## The recommended prefix ensures that target names across packages don't collide\nadd_executable(vloam_main_node src/vloam_main_node.cpp)\n\n\n## Rename C++ executable without prefix\n## The above recommended prefix causes long target names, the following renames the\n## target back to the shorter version for ease of user use\n## e.g. \"rosrun someones_pkg node\" instead of \"rosrun someones_pkg someones_pkg_node\"\n# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX \"\")\n\n## Add cmake target dependencies of the executable\n## same as for the library above\nadd_dependencies(vloam_main_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})\n\n## Specify libraries to link a library or executable target against\ntarget_link_libraries(vloam_main_node\n ${catkin_LIBRARIES} ${OpenCV_LIBS} ${PCL_LIBRARIES} Eigen3::Eigen ${CERES_LIBRARIES} image_util point_cloud_util visual_odometry lidar_odometry_mapping vloam_tf\n)\n\n#############\n## Install ##\n#############\n\n# all install targets should use catkin DESTINATION variables\n# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html\n\n## Mark executable scripts (Python etc.) for installation\n## in contrast to setup.py, you can choose the destination\n# catkin_install_python(PROGRAMS\n# scripts/my_python_script\n# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}\n# )\n\n## Mark executables for installation\n## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html\n# install(TARGETS ${PROJECT_NAME}_node\n# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}\n# )\n\n## Mark libraries for installation\n## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html\n# install(TARGETS ${PROJECT_NAME}\n# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}\n# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}\n# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}\n# )\n\n## Mark cpp header files for installation\n# install(DIRECTORY include/${PROJECT_NAME}/\n# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}\n# FILES_MATCHING PATTERN \"*.h\"\n# PATTERN \".svn\" EXCLUDE\n# )\n\n## Mark other files for installation (e.g. launch and bag files, etc.)\n# install(FILES\n# # myfile1\n# # myfile2\n# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}\n# )\n\n#############\n## Testing ##\n#############\n\n## Add gtest based cpp test target and link libraries\n# catkin_add_gtest(${PROJECT_NAME}-test test/test_vloam_main.cpp)\n# if(TARGET ${PROJECT_NAME}-test)\n# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})\n# endif()\n\n## Add folders to be run by python nosetests\n# catkin_add_nosetests(test)\n" }, { "path": "src/vloam_main/README.md", "content": "# Prerequisites\n\nOpenCV 4.5.1\nPCL 1.2\n\n# Usage 1\n\n- Step 1, create and move bag files to \"REPO_ROOT/src/vloam_main/bags\"\n - For example: https://github.com/tomas789/kitti2bag\n\n- Step 2, `catkin_make`, and then `roslaunch vloam_main vloam_main.launch` (Toggle param ```save_traj``` to save odometry path into txt)\n\n- Step 3, check the sequence number and date for the bag file, and in another terminal, run\n\n - Test seq 04\n\n ```\n rostopic pub /load_small_dataset_action_server/goal vloam_main/vloam_mainActionGoal \"header:\n seq: 0\n stamp:\n secs: 0\n nsecs: 0\n frame_id: ''\n goal_id:\n stamp:\n secs: 0\n nsecs: 0\n id: ''\n goal:\n date: '2011_09_30'\n seq: '16' \n start_frame: 0\n end_frame: 270\"\n ```\n\n - Test seq 06\n\n ```\n rostopic pub /load_small_dataset_action_server/goal vloam_main/vloam_mainActionGoal \"header:\n seq: 0\n stamp:\n secs: 0\n nsecs: 0\n frame_id: ''\n goal_id:\n stamp:\n secs: 0\n nsecs: 0\n id: ''\n goal:\n date: '2011_09_30'\n seq: '20' \n start_frame: 0\n end_frame: 1100\"\n ```\n\n - After a bag is finished, the predicted poses will be autonmatically stored into VOx.txt, LOx.txt and MOx.txt, and vloam_main will be ready to take in another goal. Just remember to back up the results before sending another goal msg, because by default, \"save_traj\" is true in vloam_main.launch.\n\n (Note: changing the detach_VO_LO needs modification and relaunching of vloam_main.launch)\n\n- Step 4: repeat step 3 with different seq number\n\nThen, you should see a similar UI like this:\n\n![demo](figures/combine_pose.png)\n\n- Step 5: Move VOx.txt, LOx.txt and MOx.txt from `vloam_main/results/${date}_drive_${raw data seq number}` to evaluation_tool/data/xOx.txt, and rename them to be ${test seq number}_pred.txt\n\n (Note: VO0 is equal to VO1, so only VO0 folder exists uder evaluation_tool/data)\n\n\n# Naming convention for transformations\n\n## C++ type to name\n\n- `tf2::transform` => T\n- `geometry_msgs::transform` => tf\n- `geometry_msgs::Stampedtransform` => stamped_tf\n- `Eigen::Isometry3f` => eigen_T\n\n## Math of transformations\n\n(Take T as the example)\n\n`frameA_T_frameB` means $T_{frameB}^{frameA}$\n\nBesides, $T = \\begin{bmatrix}R & t \\\\ 0 & 1 \\end{bmatrix}$\n\n## Multiple odometry sources\n\nExcept from static transformations, VLOAM has three odometry sources: VO, LO, MO. To distinguish them, these sources will also be in the names. Here are some examples:\n\n- `tf2::transform` in VO => VOT\n- `geometry_msgs::transform` in MO => MOtf\n- `geometry_msgs::Stampedtransform` in LO => stamped_LOtf\n- `Eigen::Isometry3f` in VO => eigen_VOT\n\n# `vloam_main.cpp` code explanation\n\n## `main`\n\n- Read parameters from roslaunch\n- Generate publishers and subscribers after nh is created\n- Setup the action server, with `execute` as the callback\n\n## `execute`\n\n- Keep finishing initialization tasks, i.e. clean cache and call `init()`\n - Instantiate the message filter reading image, point cloud and camera info at the same time\n - message filter will start its `callback`\n- Read goal from the goal topic and load rosbag\n\n## `callback`\n\n- Section 1: Process Image // takes ~34ms\n- Section 2: Process Static Transformations and Camera Intrinsics\n- Section 3: Process Point Cloud // takes ~2.6ms\n - `point_cloud_pcl` is a pcl point cloud obj, which might be further reused\n- Section 4: Solve VO // takes ~11ms\n- Section 5: Publish VO\n - `dynamic_broadcaster.sendTransform(world_stamped_tf_base);` is the final result from VO. Base_link of the vehicle on the ground serves at the bridge between other frames on vehicle and world coordinate `map`. `world` frame is occupied by rosbag, so `map` is the world here.\n - Before that, `velo_last_T_velo_curr` updates the VO result in velodyne frame.\n" }, { "path": "src/vloam_main/action/vloam_main.action", "content": "# Define the goal\nstring date\nstring seq\nint32 start_frame\nint32 end_frame\n---\n# Define the result\nbool loading_finished\n---\n# Define a feedback message\nfloat32 loading_completion_percent" }, { "path": "src/vloam_main/launch/vloam_main.launch", "content": "\n \n\n \n \n \n \n \n \n \n\n \n \n \n \n \n \n\n \n\n" }, { "path": "src/vloam_main/package.xml", "content": "\n\n vloam_main\n 0.0.0\n The vloam_main package\n\n \n \n \n yukun\n\n\n \n \n \n MIT\n\n\n \n \n \n \n\n\n \n \n \n \n\n\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n catkin\n cv_bridge\n roscpp\n sensor_msgs\n geometry_msgs\n tf2_ros\n tf2_geometry_msgs\n std_msgs\n message_filters\n \n pcl\n pcl_conversions\n nav_msgs\n actionlib\n actionlib_msgs\n visual_odometry\n lidar_odometry_mappingy\n vloam_tf\n cv_bridge\n roscpp\n \n std_msgs\n cv_bridge\n roscpp\n sensor_msgs\n geometry_msgs\n tf2_ros\n tf2_geometry_msgs\n std_msgs\n message_filters\n \n pcl\n pcl_conversions\n nav_msgs\n actionlib\n actionlib_msgs\n visual_odometry\n lidar_odometry_mapping\n vloam_tf\n\n\n \n \n \n\n \n\n" }, { "path": "src/vloam_main/results/2011_09_26_drive_0001/LO0.txt", "content": "1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000\n0.999997 -0.000668 0.002551 -0.003070 0.000671 0.999999 -0.000841 -0.002262 -0.002550 0.000843 0.999996 1.308353\n0.999986 -0.000509 0.005253 -0.005230 0.000519 0.999998 -0.001977 -0.012189 -0.005252 0.001979 0.999984 2.562540\n0.999966 0.000405 0.008196 -0.007504 -0.000380 0.999995 -0.003038 -0.020037 -0.008197 0.003035 0.999962 3.838999\n0.999927 0.002015 0.011895 -0.013994 -0.001984 0.999995 -0.002565 -0.032832 -0.011900 0.002541 0.999926 5.137100\n0.999878 0.001737 0.015528 -0.005669 -0.001720 0.999998 -0.001093 -0.038748 -0.015530 0.001066 0.999879 6.412582\n0.999819 0.001827 0.018936 0.011232 -0.001818 0.999998 -0.000538 -0.048698 -0.018937 0.000503 0.999821 7.679665\n0.999753 0.002407 0.022088 0.029622 -0.002371 0.999996 -0.001662 -0.052095 -0.022092 0.001609 0.999755 8.907693\n0.999678 0.003708 0.025114 0.052642 -0.003638 0.999989 -0.002809 -0.061051 -0.025124 0.002717 0.999681 10.142304\n" }, { "path": "src/vloam_main/results/2011_09_26_drive_0001/MO0.txt", "content": "1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000\n1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000\n1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000\n1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000\n0.999966 0.000405 0.008196 -0.007504 -0.000380 0.999995 -0.003038 -0.020037 -0.008197 0.003035 0.999962 3.838999\n0.999878 0.001737 0.015528 -0.005669 -0.001720 0.999998 -0.001093 -0.038748 -0.015530 0.001066 0.999879 6.412582\n0.999819 0.001827 0.018936 0.011232 -0.001818 0.999998 -0.000538 -0.048698 -0.018937 0.000503 0.999821 7.679665\n0.999753 0.002407 0.022088 0.029622 -0.002371 0.999996 -0.001662 -0.052095 -0.022092 0.001609 0.999755 8.907693\n0.999678 0.003708 0.025114 0.052642 -0.003638 0.999989 -0.002809 -0.061051 -0.025124 0.002717 0.999681 10.142304\n" }, { "path": "src/vloam_main/results/2011_09_26_drive_0001/VO0.txt", "content": "1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000\n0.999993 0.003368 0.001741 -0.002553 -0.003367 0.999994 -0.000415 0.023634 -0.001743 0.000410 0.999998 1.274260\n0.999980 0.005956 0.002032 0.025424 -0.005951 0.999979 -0.002435 0.039961 -0.002046 0.002423 0.999995 2.464004\n0.999937 0.010438 0.004203 0.018584 -0.010425 0.999941 -0.003124 0.057060 -0.004235 0.003080 0.999986 3.698038\n0.999824 0.016683 0.008620 -0.029224 -0.016681 0.999861 -0.000229 0.036103 -0.008623 0.000085 0.999963 4.968617\n0.999742 0.020112 0.010524 -0.002731 -0.020135 0.999795 0.002061 0.053930 -0.010480 -0.002272 0.999942 6.197413\n0.999667 0.022465 0.012711 0.021684 -0.022493 0.999745 0.002058 0.068287 -0.012661 -0.002343 0.999917 7.428633\n0.999640 0.023871 0.012288 0.099534 -0.023862 0.999715 -0.000865 0.100522 -0.012305 0.000572 0.999924 8.594154\n0.999628 0.024776 0.011398 0.193885 -0.024735 0.999687 -0.003697 0.128718 -0.011486 0.003414 0.999928 9.752656\n" }, { "path": "src/vloam_main/results/2011_09_26_drive_0002/LO0.txt", "content": "1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000\n0.999990 -0.000821 0.004473 0.026271 0.000845 0.999985 -0.005333 0.002179 -0.004468 0.005337 0.999976 0.840170\n0.999965 0.001329 0.008244 0.026348 -0.001312 0.999997 -0.002027 0.006073 -0.008247 0.002016 0.999964 1.728960\n0.999921 0.005632 0.011209 0.011999 -0.005674 0.999977 0.003722 -0.004304 -0.011188 -0.003785 0.999930 2.633847\n0.999890 0.005358 0.013818 0.028257 -0.005422 0.999975 0.004563 0.015268 -0.013793 -0.004638 0.999894 3.528098\n0.999883 0.002807 0.015053 0.063692 -0.002829 0.999995 0.001439 0.014189 -0.015049 -0.001481 0.999886 4.400116\n0.999861 0.008431 0.014393 0.078821 -0.008392 0.999961 -0.002731 0.030179 -0.014415 0.002610 0.999893 5.246884\n0.999887 0.008353 0.012501 0.101812 -0.008289 0.999953 -0.005104 0.046102 -0.012543 0.005000 0.999909 6.124978\n0.999923 0.005258 0.011250 0.132022 -0.005189 0.999968 -0.006162 0.034618 -0.011282 0.006104 0.999918 7.005879\n0.999932 0.007908 0.008563 0.139168 -0.007846 0.999944 -0.007155 0.042086 -0.008620 0.007087 0.999938 7.891475\n0.999955 0.008462 0.004356 0.155559 -0.008430 0.999938 -0.007273 0.042374 -0.004417 0.007236 0.999964 8.781188\n" }, { "path": "src/vloam_main/results/2011_09_26_drive_0002/MO0.txt", "content": "1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000\n1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000\n1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000\n1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000\n0.999921 0.005632 0.011209 0.011999 -0.005674 0.999977 0.003722 -0.004304 -0.011188 -0.003785 0.999930 2.633847\n0.999883 0.002807 0.015053 0.063692 -0.002829 0.999995 0.001439 0.014189 -0.015049 -0.001481 0.999886 4.400116\n0.999861 0.008431 0.014393 0.078821 -0.008392 0.999961 -0.002731 0.030179 -0.014415 0.002610 0.999893 5.246884\n0.999887 0.008353 0.012501 0.101812 -0.008289 0.999953 -0.005104 0.046102 -0.012543 0.005000 0.999909 6.124978\n0.999923 0.005258 0.011250 0.132022 -0.005189 0.999968 -0.006162 0.034618 -0.011282 0.006104 0.999918 7.005879\n0.999923 0.005258 0.011250 0.132022 -0.005189 0.999968 -0.006162 0.034618 -0.011282 0.006104 0.999918 7.005879\n0.999946 0.009743 0.003591 0.090437 -0.009718 0.999928 -0.007042 0.011725 -0.003659 0.007007 0.999969 8.898920\n" }, { "path": "src/vloam_main/results/2011_09_26_drive_0002/VO0.txt", "content": "1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000\n1.000000 -0.000011 0.000873 0.068395 0.000017 0.999977 -0.006810 0.035978 -0.000872 0.006810 0.999976 0.816011\n0.999985 0.002821 0.004771 0.060287 -0.002811 0.999994 -0.001954 0.038179 -0.004776 0.001940 0.999987 1.689403\n0.999921 0.008324 0.009412 0.023507 -0.008370 0.999954 0.004783 0.015995 -0.009372 -0.004862 0.999944 2.580382\n0.999874 0.010310 0.012039 0.035071 -0.010375 0.999932 0.005362 0.052757 -0.011983 -0.005486 0.999913 3.459077\n0.999872 0.010952 0.011684 0.062757 -0.010932 0.999939 -0.001774 0.097662 -0.011703 0.001646 0.999930 4.327456\n0.999791 0.018794 0.008000 0.103781 -0.018740 0.999801 -0.006800 0.131739 -0.008127 0.006649 0.999945 5.160370\n0.999763 0.021128 0.005333 0.107413 -0.021075 0.999729 -0.009934 0.167616 -0.005542 0.009820 0.999936 6.042705\n0.999699 0.024326 0.003072 0.119525 -0.024283 0.999615 -0.013433 0.217107 -0.003398 0.013354 0.999905 6.939801\n0.999525 0.030823 -0.000304 0.119898 -0.030824 0.999404 -0.015548 0.248572 -0.000176 0.015550 0.999879 7.823939\n0.999367 0.035181 -0.005367 0.115826 -0.035262 0.999253 -0.015792 0.271865 0.004807 0.015972 0.999861 8.713019\n" }, { "path": "src/vloam_main/results/2011_09_26_drive_0005/LO0.txt", "content": "1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000\n0.999884 -0.000187 -0.015235 -0.003954 0.000238 0.999994 0.003361 0.001119 0.015234 -0.003365 0.999878 0.346943\n0.999619 0.000093 -0.027591 -0.015949 0.000241 0.999927 0.012112 0.001968 0.027590 -0.012114 0.999546 0.703812\n0.999179 0.003415 -0.040375 -0.047963 -0.002618 0.999801 0.019760 -0.010658 0.040435 -0.019638 0.998989 1.066572\n0.998582 0.003944 -0.053088 -0.064638 -0.002850 0.999782 0.020671 -0.009256 0.053158 -0.020490 0.998376 1.392752\n0.997852 0.004493 -0.065348 -0.084038 -0.003279 0.999821 0.018660 -0.008588 0.065420 -0.018405 0.997688 1.707508\n0.996987 0.002451 -0.077524 -0.084204 -0.001160 0.999860 0.016687 0.006141 0.077554 -0.016547 0.996851 2.029778\n0.996058 0.002277 -0.088677 -0.055701 -0.001113 0.999913 0.013173 0.020240 0.088700 -0.013022 0.995973 2.343308\n0.994975 0.004212 -0.100037 -0.092453 -0.003212 0.999943 0.010152 0.028883 0.100074 -0.009780 0.994932 2.647048\n0.993787 0.007151 -0.111073 -0.137573 -0.006225 0.999943 0.008677 0.036544 0.111129 -0.007932 0.993774 2.957877\n0.992489 0.006600 -0.122153 -0.164744 -0.006170 0.999973 0.003899 0.064460 0.122175 -0.003116 0.992504 3.257140\n" }, { "path": "src/vloam_main/results/2011_09_26_drive_0005/LO1.txt", "content": "1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000\n0.999908 0.000081 -0.013566 -0.005334 -0.000036 0.999994 0.003326 -0.000345 0.013566 -0.003325 0.999902 0.336810\n0.999663 0.000328 -0.025950 -0.014924 -0.000023 0.999931 0.011723 0.005039 0.025953 -0.011719 0.999595 0.701932\n0.999251 0.003212 -0.038559 -0.041047 -0.002474 0.999813 0.019178 -0.001845 0.038613 -0.019068 0.999072 1.068995\n0.998674 0.003735 -0.051342 -0.062151 -0.002702 0.999793 0.020171 -0.001485 0.051407 -0.020006 0.998477 1.398827\n0.997953 0.004277 -0.063808 -0.088067 -0.003112 0.999827 0.018343 -0.002438 0.063875 -0.018107 0.997794 1.719202\n0.997087 0.002431 -0.076236 -0.104220 -0.001133 0.999854 0.017067 0.003354 0.076266 -0.016930 0.996944 2.051829\n0.996104 0.002500 -0.088146 -0.130051 -0.001175 0.999886 0.015080 -0.004258 0.088174 -0.014918 0.995993 2.374606\n0.995006 0.004420 -0.099721 -0.170624 -0.003215 0.999920 0.012243 0.002596 0.099767 -0.011862 0.994940 2.685148\n0.993803 0.007297 -0.110915 -0.219584 -0.006133 0.999923 0.010829 0.009504 0.110985 -0.010081 0.993771 2.999832\n0.992509 0.006815 -0.121982 -0.252236 -0.006014 0.999958 0.006938 0.027805 0.122024 -0.006152 0.992508 3.303378\n" }, { "path": "src/vloam_main/results/2011_09_26_drive_0005/MO0.txt", "content": "1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000\n1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000\n1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000\n1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000\n0.999179 0.003415 -0.040375 -0.047963 -0.002618 0.999801 0.019760 -0.010658 0.040435 -0.019638 0.998989 1.066572\n0.997852 0.004493 -0.065348 -0.084038 -0.003279 0.999821 0.018660 -0.008588 0.065420 -0.018405 0.997688 1.707508\n0.996987 0.002451 -0.077524 -0.084204 -0.001160 0.999860 0.016687 0.006141 0.077554 -0.016547 0.996851 2.029778\n0.996058 0.002277 -0.088677 -0.055701 -0.001113 0.999913 0.013173 0.020240 0.088700 -0.013022 0.995973 2.343308\n0.994975 0.004212 -0.100037 -0.092453 -0.003212 0.999943 0.010152 0.028883 0.100074 -0.009780 0.994932 2.647048\n0.994975 0.004212 -0.100037 -0.092453 -0.003212 0.999943 0.010152 0.028883 0.100074 -0.009780 0.994932 2.647048\n0.992090 0.008377 -0.125245 -0.268379 -0.007797 0.999956 0.005121 0.024951 0.125283 -0.004104 0.992113 3.261387\n" }, { "path": "src/vloam_main/results/2011_09_26_drive_0005/MO1.txt", "content": "1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000\n0.999891 0.000156 -0.014769 -0.003547 -0.000096 0.999992 0.004105 -0.006930 0.014770 -0.004103 0.999883 0.350144\n0.999618 0.000469 -0.027638 -0.013382 -0.000107 0.999914 0.013118 -0.001610 0.027642 -0.013110 0.999532 0.712088\n0.999175 0.003246 -0.040489 -0.038174 -0.002363 0.999758 0.021848 -0.020330 0.040550 -0.021735 0.998941 1.083165\n0.998550 0.003982 -0.053694 -0.064380 -0.002719 0.999718 0.023574 -0.025081 0.053773 -0.023394 0.998279 1.417581\n0.997767 0.004824 -0.066609 -0.090675 -0.003389 0.999760 0.021641 -0.028895 0.066698 -0.021367 0.997544 1.739219\n0.996847 0.002788 -0.079295 -0.113691 -0.001208 0.999800 0.019974 -0.016675 0.079334 -0.019815 0.996651 2.062642\n0.995783 0.003267 -0.091687 -0.138889 -0.001613 0.999835 0.018109 -0.036886 0.091731 -0.017885 0.995623 2.382261\n0.994618 0.005206 -0.103479 -0.186819 -0.003705 0.999885 0.014691 -0.028676 0.103544 -0.014228 0.994523 2.683244\n0.993303 0.008915 -0.115191 -0.239959 -0.007433 0.999884 0.013288 -0.023902 0.115297 -0.012343 0.993254 2.998556\n0.991910 0.008116 -0.126684 -0.288753 -0.007089 0.999938 0.008562 0.005958 0.126745 -0.007595 0.991906 3.303084\n" }, { "path": "src/vloam_main/results/2011_09_26_drive_0005/VO0.txt", "content": "1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000\n0.999684 0.000385 -0.025123 -0.018777 -0.000305 0.999995 0.003190 -0.009443 0.025124 -0.003181 0.999679 0.318103\n0.999296 0.000138 -0.037528 -0.048930 0.000343 0.999918 0.012831 -0.043884 0.037527 -0.012835 0.999213 0.627971\n0.998678 0.004497 -0.051209 -0.106599 -0.003375 0.999753 0.021976 -0.101616 0.051295 -0.021774 0.998446 0.950089\n0.997939 0.006967 -0.063795 -0.104271 -0.005528 0.999727 0.022702 -0.084229 0.063936 -0.022302 0.997705 1.239813\n0.997190 0.007930 -0.074487 -0.090163 -0.006380 0.999759 0.021028 -0.049857 0.074636 -0.020494 0.997000 1.528507\n0.996189 0.006847 -0.086948 -0.043417 -0.005276 0.999819 0.018285 0.046987 0.087057 -0.017757 0.996045 1.791472\n0.995196 0.006577 -0.097682 0.085550 -0.005063 0.999863 0.015733 0.371814 0.097772 -0.015163 0.995093 2.032746\n0.994078 0.008070 -0.108370 0.062079 -0.006750 0.999899 0.012534 0.418257 0.108460 -0.011728 0.994032 2.300469\n0.992814 0.012379 -0.119027 0.023479 -0.011259 0.999886 0.010077 0.452569 0.119138 -0.008664 0.992840 2.583899\n0.991334 0.011392 -0.130872 0.021179 -0.010881 0.999930 0.004615 0.562244 0.130916 -0.003151 0.991388 2.861943\n" }, { "path": "src/vloam_main/results/2011_09_26_drive_0005/VO1.txt", "content": "1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000\n0.999871 0.000491 -0.016074 0.055240 -0.000432 0.999993 0.003659 0.024968 0.016076 -0.003652 0.999864 0.402408\n0.999553 0.000945 -0.029889 0.099591 -0.000555 0.999915 0.013048 0.051336 0.029899 -0.013026 0.999468 0.798319\n0.999007 0.006074 -0.044146 0.137850 -0.005200 0.999788 0.019899 0.090564 0.044257 -0.019649 0.998827 1.177750\n0.998240 0.007018 -0.058893 0.177757 -0.005863 0.999787 0.019770 0.120192 0.059019 -0.019390 0.998069 1.532997\n0.997252 0.008946 -0.073544 0.218119 -0.007737 0.999830 0.016706 0.146066 0.073681 -0.016091 0.997152 1.895436\n0.996107 0.007079 -0.087866 0.249597 -0.005862 0.999883 0.014094 0.184091 0.087955 -0.013524 0.996033 2.260151\n0.994780 0.008299 -0.101707 0.273193 -0.007257 0.999917 0.010610 0.201630 0.101787 -0.009816 0.994758 2.612748\n0.993310 0.010536 -0.114994 0.293411 -0.009791 0.999927 0.007036 0.226124 0.115059 -0.005863 0.993341 2.948963\n0.991551 0.015320 -0.128810 0.315179 -0.014861 0.999879 0.004523 0.262607 0.128864 -0.002570 0.991659 3.329251\n0.989644 0.014771 -0.142782 0.369806 -0.015126 0.999885 -0.001400 0.299126 0.142744 0.003545 0.989753 3.700685\n" }, { "path": "src/vloam_main/results/2011_09_30_drive_0016/LO0.txt", "content": "1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000\n1.000000 -0.000439 -0.000116 0.007042 0.000439 0.999999 0.001514 -0.025549 0.000115 -0.001514 0.999999 1.318176\n0.999998 -0.001760 -0.000555 0.015208 0.001761 0.999997 0.001888 -0.048715 0.000551 -0.001889 0.999998 2.631446\n0.999995 -0.002815 -0.001126 0.019296 0.002816 0.999996 0.000930 -0.077090 0.001124 -0.000933 0.999999 3.971109\n0.999996 -0.002766 -0.001129 0.018687 0.002767 0.999996 0.001054 -0.104292 0.001126 -0.001057 0.999999 5.279566\n0.999995 -0.002873 -0.001165 0.010484 0.002874 0.999995 0.001497 -0.123610 0.001160 -0.001501 0.999998 6.609272\n0.999991 -0.004039 -0.001130 0.010149 0.004042 0.999989 0.002517 -0.150379 0.001120 -0.002522 0.999996 7.942711\n0.999992 -0.003639 -0.001511 0.007592 0.003644 0.999989 0.002927 -0.174849 0.001500 -0.002933 0.999995 9.263927\n0.999994 -0.003012 -0.001883 -0.006635 0.003015 0.999994 0.001685 -0.203756 0.001878 -0.001691 0.999997 10.584520\n0.999998 -0.000312 -0.001833 -0.006706 0.000312 1.000000 0.000044 -0.231969 0.001833 -0.000044 0.999998 11.907107\n0.999995 0.002598 -0.001742 -0.029006 -0.002599 0.999996 -0.000624 -0.264682 0.001740 0.000628 0.999998 13.245830\n0.999986 0.004980 -0.001716 -0.052788 -0.004981 0.999987 -0.000666 -0.289893 0.001713 0.000675 0.999998 14.580807\n0.999981 0.005931 -0.001703 -0.068296 -0.005931 0.999982 0.000099 -0.316794 0.001703 -0.000089 0.999999 15.931218\n0.999991 0.003933 -0.001429 -0.069489 -0.003930 0.999990 0.002359 -0.339474 0.001439 -0.002353 0.999996 17.294188\n0.999999 0.000013 -0.001452 -0.070462 -0.000007 0.999990 0.004466 -0.364939 0.001452 -0.004466 0.999989 18.662273\n0.999996 -0.002060 -0.001928 -0.071510 0.002066 0.999993 0.003004 -0.395371 0.001922 -0.003008 0.999994 20.020752\n0.999992 -0.003213 -0.002318 -0.076288 0.003216 0.999994 0.001192 -0.418545 0.002314 -0.001200 0.999997 21.381350\n0.999998 -0.001218 -0.001670 -0.094695 0.001219 0.999999 0.000734 -0.466292 0.001669 -0.000736 0.999998 22.743546\n0.999998 0.001696 -0.001250 -0.110306 -0.001694 0.999997 0.002018 -0.484861 0.001254 -0.002016 0.999997 24.121166\n0.999996 0.002607 -0.000932 -0.117165 -0.002601 0.999981 0.005640 -0.509555 0.000947 -0.005638 0.999984 25.522898\n0.999999 0.000325 -0.001127 -0.112716 -0.000317 0.999976 0.006955 -0.519598 0.001129 -0.006955 0.999975 26.914228\n0.999994 -0.003037 -0.001618 -0.098348 0.003047 0.999976 0.006145 -0.547659 0.001599 -0.006150 0.999980 28.291470\n0.999991 -0.003916 -0.001650 -0.108016 0.003923 0.999985 0.003831 -0.562880 0.001635 -0.003838 0.999991 29.684906\n0.999991 -0.004227 -0.001024 -0.120711 0.004229 0.999988 0.002333 -0.591288 0.001014 -0.002338 0.999997 31.061016\n0.999998 -0.001839 -0.000483 -0.143921 0.001840 0.999997 0.001260 -0.613488 0.000481 -0.001261 0.999999 32.442169\n1.000000 -0.000688 0.000158 -0.154291 0.000688 1.000000 -0.000348 -0.639759 -0.000157 0.000348 1.000000 33.832027\n1.000000 -0.000552 -0.000077 -0.163945 0.000552 1.000000 0.000015 -0.661339 0.000077 -0.000015 1.000000 35.234177\n1.000000 -0.000566 -0.000093 -0.167934 0.000566 0.999999 0.000976 -0.689754 0.000093 -0.000976 1.000000 36.609585\n1.000000 0.000573 -0.000400 -0.180595 -0.000572 0.999997 0.002405 -0.715346 0.000401 -0.002405 0.999997 38.010311\n0.999997 0.002209 -0.000792 -0.194400 -0.002207 0.999995 0.002261 -0.737702 0.000797 -0.002259 0.999997 39.418495\n0.999991 0.004255 -0.000645 -0.224126 -0.004254 0.999989 0.001963 -0.763109 0.000654 -0.001960 0.999998 40.821335\n0.999982 0.006011 -0.000793 -0.247576 -0.006010 0.999980 0.001919 -0.783294 0.000805 -0.001915 0.999998 42.229221\n0.999979 0.006368 -0.001154 -0.262960 -0.006366 0.999978 0.001730 -0.804834 0.001165 -0.001723 0.999998 43.641018\n0.999984 0.005333 -0.001817 -0.270834 -0.005330 0.999984 0.001783 -0.827333 0.001827 -0.001773 0.999997 45.044804\n0.999987 0.004627 -0.002350 -0.284047 -0.004624 0.999989 0.001175 -0.851141 0.002355 -0.001164 0.999997 46.448044\n0.999985 0.004434 -0.003215 -0.300676 -0.004430 0.999989 0.001421 -0.880298 0.003221 -0.001407 0.999994 47.855663\n0.999984 0.004181 -0.003745 -0.325299 -0.004173 0.999989 0.002204 -0.901435 0.003754 -0.002189 0.999991 49.260094\n0.999981 0.004252 -0.004421 -0.352566 -0.004243 0.999989 0.002038 -0.927516 0.004429 -0.002019 0.999988 50.656990\n0.999973 0.004222 -0.005983 -0.366968 -0.004210 0.999989 0.001972 -0.952506 0.005991 -0.001947 0.999980 52.094193\n0.999967 0.003946 -0.007160 -0.385167 -0.003927 0.999989 0.002669 -0.974289 0.007170 -0.002641 0.999971 53.505672\n0.999965 0.002480 -0.007939 -0.403911 -0.002450 0.999990 0.003679 -0.993659 0.007948 -0.003660 0.999962 54.930965\n0.999962 0.000130 -0.008758 -0.421180 -0.000095 0.999992 0.004002 -1.013516 0.008758 -0.004001 0.999954 56.357616\n0.999950 -0.002506 -0.009730 -0.433739 0.002539 0.999991 0.003426 -1.032333 0.009721 -0.003451 0.999947 57.803097\n0.999949 -0.003118 -0.009633 -0.456632 0.003152 0.999989 0.003593 -1.057752 0.009621 -0.003623 0.999947 59.240345\n0.999948 -0.004464 -0.009137 -0.472057 0.004502 0.999981 0.004139 -1.082068 0.009118 -0.004180 0.999950 60.676159\n0.999943 -0.005920 -0.008904 -0.490594 0.005961 0.999972 0.004632 -1.103320 0.008876 -0.004685 0.999950 62.112480\n0.999940 -0.007194 -0.008210 -0.505936 0.007230 0.999965 0.004321 -1.128303 0.008179 -0.004380 0.999957 63.532616\n0.999935 -0.008353 -0.007753 -0.524602 0.008379 0.999960 0.003279 -1.150680 0.007726 -0.003344 0.999965 64.971207\n0.999934 -0.008815 -0.007406 -0.540113 0.008837 0.999957 0.002929 -1.178944 0.007380 -0.002995 0.999968 66.402542\n0.999942 -0.008358 -0.006795 -0.567091 0.008378 0.999961 0.002861 -1.200186 0.006771 -0.002918 0.999973 67.825142\n0.999959 -0.006453 -0.006406 -0.589620 0.006476 0.999973 0.003588 -1.222101 0.006382 -0.003630 0.999973 69.269211\n" }, { "path": "src/vloam_main/results/2011_09_30_drive_0016/MO0.txt", "content": "1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000\n1.000000 -0.000393 -0.000397 -0.009016 0.000394 0.999999 0.001507 -0.030358 0.000396 -0.001507 0.999999 1.309415\n0.999998 -0.001716 -0.001001 -0.011333 0.001718 0.999997 0.001800 -0.054419 0.000997 -0.001802 0.999998 2.623000\n0.999995 -0.002888 -0.001266 -0.009596 0.002890 0.999995 0.001120 -0.085893 0.001263 -0.001123 0.999999 3.947004\n0.999995 -0.002870 -0.001102 -0.020233 0.002872 0.999995 0.001214 -0.113958 0.001098 -0.001217 0.999999 5.256166\n0.999994 -0.003281 -0.001141 -0.026799 0.003283 0.999993 0.001998 -0.136460 0.001134 -0.002002 0.999997 6.583170\n0.999990 -0.004408 -0.001125 -0.034390 0.004411 0.999985 0.003253 -0.167355 0.001110 -0.003258 0.999994 7.920679\n0.999989 -0.004522 -0.001485 -0.046281 0.004527 0.999982 0.003896 -0.195891 0.001467 -0.003903 0.999991 9.255984\n0.999991 -0.003853 -0.001811 -0.058469 0.003858 0.999989 0.002699 -0.225128 0.001801 -0.002706 0.999995 10.580759\n0.999997 -0.001462 -0.001905 -0.077044 0.001464 0.999999 0.000926 -0.253876 0.001903 -0.000928 0.999998 11.910395\n0.999996 0.002159 -0.001840 -0.100909 -0.002160 0.999998 -0.000105 -0.285343 0.001840 0.000109 0.999998 13.250333\n0.999986 0.005061 -0.001760 -0.119650 -0.005061 0.999987 -0.000212 -0.310067 0.001759 0.000221 0.999998 14.589271\n0.999980 0.006062 -0.001874 -0.132364 -0.006061 0.999981 0.000554 -0.335812 0.001878 -0.000542 0.999998 15.943193\n0.999989 0.004362 -0.001526 -0.135311 -0.004358 0.999987 0.002773 -0.355107 0.001538 -0.002767 0.999995 17.290777\n0.999999 0.000247 -0.001499 -0.131580 -0.000240 0.999989 0.004628 -0.378114 0.001500 -0.004628 0.999988 18.649006\n0.999994 -0.002822 -0.001848 -0.135328 0.002828 0.999990 0.003354 -0.411857 0.001838 -0.003359 0.999993 20.006094\n0.999992 -0.003459 -0.002013 -0.133655 0.003461 0.999993 0.001089 -0.433381 0.002009 -0.001096 0.999997 21.354240\n0.999998 -0.001072 -0.001419 -0.151471 0.001073 0.999999 0.000499 -0.489667 0.001418 -0.000501 0.999999 22.705315\n0.999998 0.001738 -0.000946 -0.168505 -0.001737 0.999997 0.001663 -0.501483 0.000949 -0.001662 0.999998 24.075645\n0.999996 0.002987 -0.000245 -0.181569 -0.002986 0.999981 0.005406 -0.525822 0.000261 -0.005405 0.999985 25.466688\n1.000000 -0.000133 -0.000234 -0.177593 0.000134 0.999979 0.006487 -0.529836 0.000233 -0.006487 0.999979 26.839790\n0.999994 -0.003450 -0.000561 -0.177287 0.003453 0.999977 0.005752 -0.562544 0.000541 -0.005754 0.999983 28.210247\n0.999990 -0.004482 -0.000512 -0.182534 0.004483 0.999984 0.003362 -0.579012 0.000497 -0.003364 0.999994 29.576389\n0.999991 -0.004306 0.000096 -0.191285 0.004306 0.999989 0.001767 -0.622825 -0.000104 -0.001767 0.999998 30.951414\n0.999999 -0.001571 0.000656 -0.217871 0.001570 0.999999 0.000657 -0.643479 -0.000657 -0.000656 1.000000 32.317867\n0.999999 -0.000048 0.001078 -0.230663 0.000049 1.000000 -0.000916 -0.676759 -0.001078 0.000916 0.999999 33.706409\n0.999999 0.001383 0.001018 -0.241171 -0.001382 0.999999 -0.000981 -0.706948 -0.001020 0.000980 0.999999 35.087410\n0.999999 0.001326 0.000767 -0.245153 -0.001326 0.999999 0.000277 -0.743208 -0.000766 -0.000278 1.000000 36.478642\n0.999997 0.002298 0.000320 -0.249927 -0.002299 0.999995 0.001945 -0.768325 -0.000316 -0.001946 0.999998 37.880325\n0.999991 0.004122 0.000139 -0.261410 -0.004123 0.999988 0.002575 -0.795567 -0.000128 -0.002575 0.999997 39.268166\n0.999979 0.006527 0.000472 -0.275051 -0.006529 0.999976 0.002467 -0.822444 -0.000456 -0.002470 0.999997 40.668358\n0.999960 0.008909 0.000424 -0.289486 -0.008910 0.999956 0.002813 -0.847997 -0.000399 -0.002817 0.999996 42.061241\n0.999950 0.010007 0.000271 -0.306876 -0.010008 0.999946 0.002915 -0.869439 -0.000242 -0.002918 0.999996 43.458340\n0.999957 0.009258 -0.000143 -0.314132 -0.009257 0.999953 0.002824 -0.897361 0.000169 -0.002823 0.999996 44.863163\n0.999962 0.008666 -0.000881 -0.318120 -0.008663 0.999959 0.002480 -0.927445 0.000903 -0.002472 0.999997 46.267525\n0.999963 0.008386 -0.001810 -0.329572 -0.008381 0.999961 0.002736 -0.958007 0.001833 -0.002721 0.999995 47.683647\n0.999960 0.008491 -0.002852 -0.337728 -0.008481 0.999958 0.003552 -0.986552 0.002882 -0.003528 0.999990 49.095173\n0.999953 0.008948 -0.003852 -0.350353 -0.008933 0.999952 0.003899 -1.014783 0.003887 -0.003865 0.999985 50.507565\n0.999941 0.009480 -0.005262 -0.360588 -0.009459 0.999947 0.004043 -1.044297 0.005300 -0.003993 0.999978 51.930763\n0.999934 0.009533 -0.006349 -0.375721 -0.009501 0.999942 0.005118 -1.070849 0.006398 -0.005058 0.999967 53.349747\n0.999936 0.008689 -0.007208 -0.389969 -0.008646 0.999945 0.005999 -1.091557 0.007259 -0.005936 0.999956 54.768929\n0.999951 0.006302 -0.007677 -0.400105 -0.006254 0.999961 0.006281 -1.114183 0.007716 -0.006233 0.999951 56.188389\n0.999956 0.004555 -0.008254 -0.412864 -0.004506 0.999972 0.006014 -1.140607 0.008281 -0.005976 0.999948 57.614227\n0.999958 0.003625 -0.008418 -0.421534 -0.003572 0.999974 0.006309 -1.167578 0.008441 -0.006279 0.999945 59.040840\n0.999963 0.002630 -0.008235 -0.437415 -0.002572 0.999972 0.007033 -1.192769 0.008253 -0.007011 0.999941 60.460209\n0.999967 0.000572 -0.008115 -0.449525 -0.000509 0.999970 0.007756 -1.213166 0.008119 -0.007751 0.999937 61.880676\n0.999970 -0.001602 -0.007576 -0.459433 0.001659 0.999970 0.007517 -1.236473 0.007563 -0.007529 0.999943 63.306801\n0.999972 -0.002388 -0.007071 -0.475629 0.002437 0.999973 0.006924 -1.263479 0.007054 -0.006941 0.999951 64.717323\n0.999974 -0.002719 -0.006606 -0.491591 0.002764 0.999973 0.006805 -1.297482 0.006587 -0.006823 0.999955 66.125351\n0.999981 -0.000910 -0.006050 -0.512979 0.000952 0.999975 0.006964 -1.322684 0.006044 -0.006969 0.999957 67.535110\n0.999982 0.002245 -0.005537 -0.538636 -0.002200 0.999965 0.008072 -1.350384 0.005555 -0.008060 0.999952 68.975250\n" }, { "path": "src/vloam_main/results/2011_09_30_drive_0016/VO0.txt", "content": "1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000\n1.000000 -0.000160 -0.000255 0.002456 0.000160 0.999999 0.001021 0.002287 0.000255 -0.001021 0.999999 1.368196\n0.999999 -0.000863 -0.000632 0.005586 0.000863 1.000000 0.000466 0.006015 0.000632 -0.000467 1.000000 2.702788\n0.999997 -0.001903 -0.001725 0.040754 0.001902 0.999998 -0.001096 0.001573 0.001727 0.001093 0.999998 4.082907\n0.999996 -0.002329 -0.001429 0.041708 0.002327 0.999997 -0.001048 -0.013542 0.001432 0.001045 0.999998 5.401179\n0.999997 -0.002549 -0.000439 0.015661 0.002548 0.999997 -0.000618 -0.019354 0.000441 0.000617 1.000000 6.698064\n0.999993 -0.003721 -0.000077 0.009565 0.003721 0.999993 -0.000022 -0.021377 0.000077 0.000021 1.000000 8.042659\n0.999994 -0.003503 0.000212 -0.003017 0.003503 0.999994 0.000416 -0.033232 -0.000213 -0.000415 1.000000 9.356685\n0.999996 -0.002802 0.000786 -0.027915 0.002803 0.999995 -0.001592 -0.043650 -0.000782 0.001595 0.999998 10.669424\n1.000000 0.000150 0.000701 -0.024141 -0.000147 0.999990 -0.004404 -0.047252 -0.000701 0.004404 0.999990 12.027948\n0.999993 0.003227 0.001857 -0.050304 -0.003217 0.999981 -0.005185 -0.069579 -0.001874 0.005179 0.999985 13.383010\n0.999979 0.005623 0.003287 -0.089068 -0.005605 0.999969 -0.005522 -0.087309 -0.003318 0.005503 0.999979 14.709432\n0.999973 0.006527 0.003459 -0.087500 -0.006510 0.999967 -0.004891 -0.104973 -0.003491 0.004868 0.999982 16.090816\n0.999983 0.003886 0.004451 -0.097972 -0.003875 0.999990 -0.002308 -0.118752 -0.004460 0.002291 0.999987 17.467346\n0.999987 -0.000435 0.005173 -0.101892 0.000437 1.000000 -0.000469 -0.142227 -0.005173 0.000472 0.999987 18.846302\n0.999982 -0.002279 0.005492 -0.104597 0.002298 0.999991 -0.003546 -0.145939 -0.005484 0.003558 0.999979 20.235800\n0.999978 -0.003359 0.005699 -0.102319 0.003392 0.999978 -0.005680 -0.165514 -0.005680 0.005700 0.999968 21.636454\n0.999977 -0.001351 0.006590 -0.105484 0.001399 0.999972 -0.007301 -0.189368 -0.006580 0.007310 0.999952 23.022253\n0.999972 0.000802 0.007470 -0.095079 -0.000765 0.999987 -0.005035 -0.215296 -0.007474 0.005029 0.999959 24.430538\n0.999970 0.001665 0.007574 -0.071388 -0.001652 0.999997 -0.001660 -0.236782 -0.007577 0.001647 0.999970 25.866600\n0.999972 -0.000640 0.007449 -0.041496 0.000638 1.000000 0.000297 -0.255056 -0.007449 -0.000293 0.999972 27.285583\n0.999964 -0.005401 0.006599 -0.010254 0.005413 0.999984 -0.001737 -0.269443 -0.006589 0.001773 0.999977 28.687954\n0.999963 -0.005561 0.006576 0.000177 0.005590 0.999975 -0.004315 -0.283255 -0.006552 0.004352 0.999969 30.087927\n0.999955 -0.005920 0.007362 0.000380 0.005974 0.999955 -0.007318 -0.291513 -0.007319 0.007362 0.999946 31.462605\n0.999964 -0.003440 0.007749 0.010401 0.003502 0.999962 -0.007995 -0.315719 -0.007721 0.008021 0.999938 32.845577\n0.999962 -0.002935 0.008259 0.013371 0.003021 0.999940 -0.010486 -0.329249 -0.008228 0.010510 0.999911 34.230495\n0.999965 -0.002752 0.007951 0.027558 0.002835 0.999941 -0.010461 -0.337426 -0.007921 0.010483 0.999914 35.619114\n0.999962 -0.003489 0.008041 0.036315 0.003566 0.999949 -0.009487 -0.354473 -0.008008 0.009515 0.999923 36.997032\n0.999966 -0.002767 0.007738 0.044696 0.002828 0.999965 -0.007897 -0.381491 -0.007716 0.007919 0.999939 38.404388\n0.999972 -0.000710 0.007483 0.060817 0.000769 0.999968 -0.007927 -0.392100 -0.007478 0.007933 0.999941 39.824074\n0.999964 0.001061 0.008451 0.050609 -0.000991 0.999965 -0.008309 -0.411934 -0.008459 0.008300 0.999930 41.231934\n0.999963 0.002299 0.008345 0.055567 -0.002237 0.999970 -0.007470 -0.440837 -0.008361 0.007451 0.999937 42.666290\n0.999962 0.002721 0.008298 0.059171 -0.002653 0.999964 -0.008119 -0.450332 -0.008320 0.008097 0.999933 44.085209\n0.999971 0.001879 0.007335 0.077664 -0.001815 0.999961 -0.008634 -0.463478 -0.007351 0.008620 0.999936 45.507977\n0.999974 0.002113 0.006849 0.074600 -0.002043 0.999945 -0.010312 -0.462705 -0.006870 0.010298 0.999923 46.926128\n0.999980 0.000928 0.006309 0.069351 -0.000863 0.999947 -0.010301 -0.480700 -0.006318 0.010295 0.999927 48.339199\n0.999981 0.002766 0.005563 0.059387 -0.002707 0.999941 -0.010555 -0.473029 -0.005592 0.010540 0.999929 49.742485\n0.999981 0.002934 0.005455 0.029075 -0.002873 0.999933 -0.011222 -0.487250 -0.005488 0.011207 0.999922 51.127731\n0.999987 0.002409 0.004559 0.017603 -0.002359 0.999938 -0.010883 -0.506515 -0.004585 0.010872 0.999930 52.580482\n0.999990 0.002012 0.003896 0.000025 -0.001971 0.999944 -0.010389 -0.519211 -0.003917 0.010381 0.999938 54.001404\n0.999993 -0.000114 0.003652 -0.018246 0.000151 0.999949 -0.010075 -0.524085 -0.003651 0.010075 0.999943 55.432194\n0.999992 -0.002157 0.003254 -0.030086 0.002192 0.999940 -0.010703 -0.528355 -0.003230 0.010710 0.999937 56.886162\n0.999982 -0.005433 0.002465 -0.023513 0.005460 0.999924 -0.011055 -0.554582 -0.002405 0.011068 0.999936 58.359890\n0.999974 -0.006672 0.002711 -0.036501 0.006702 0.999915 -0.011193 -0.574049 -0.002636 0.011211 0.999934 59.822872\n0.999958 -0.008549 0.003162 -0.040348 0.008583 0.999904 -0.010879 -0.595188 -0.003069 0.010906 0.999936 61.266003\n0.999934 -0.010891 0.003739 -0.041879 0.010929 0.999888 -0.010273 -0.611882 -0.003626 0.010313 0.999940 62.702763\n0.999894 -0.014001 0.004080 -0.035823 0.014045 0.999841 -0.010984 -0.625259 -0.003925 0.011040 0.999931 64.138336\n0.999855 -0.016233 0.005216 -0.048703 0.016296 0.999792 -0.012270 -0.644768 -0.005016 0.012353 0.999911 65.584686\n0.999837 -0.017096 0.005859 -0.053425 0.017171 0.999768 -0.012983 -0.665360 -0.005635 0.013082 0.999899 67.024506\n0.999854 -0.015662 0.006802 -0.064048 0.015756 0.999780 -0.013869 -0.667658 -0.006584 0.013975 0.999881 68.465111\n0.999893 -0.012626 0.007421 -0.074386 0.012722 0.999833 -0.013116 -0.675868 -0.007254 0.013209 0.999886 69.933250\n" }, { "path": "src/vloam_main/results/2011_09_30_drive_0027/LO1.txt", "content": "1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000\n0.999981 0.000593 -0.006090 -0.004236 -0.000593 1.000000 0.000068 -0.001823 0.006090 -0.000064 0.999981 0.089769\n0.999916 0.001562 -0.012889 -0.009565 -0.001562 0.999999 0.000014 -0.005359 0.012889 0.000007 0.999917 0.183975\n0.999792 0.002108 -0.020267 -0.015411 -0.002117 0.999998 -0.000420 -0.009657 0.020266 0.000463 0.999795 0.272974\n0.999593 0.002687 -0.028406 -0.019991 -0.002733 0.999995 -0.001566 -0.012892 0.028402 0.001643 0.999595 0.373798\n0.999240 0.002946 -0.038869 -0.025277 -0.003049 0.999992 -0.002592 -0.017839 0.038861 0.002708 0.999241 0.482623\n0.998639 0.003202 -0.052066 -0.033512 -0.003381 0.999989 -0.003358 -0.021474 0.052055 0.003530 0.998638 0.606957\n0.997683 0.003053 -0.067967 -0.044918 -0.003314 0.999988 -0.003725 -0.023891 0.067954 0.003941 0.997681 0.747440\n0.996256 0.001856 -0.086432 -0.054205 -0.002201 0.999990 -0.003895 -0.025110 0.086424 0.004071 0.996250 0.892924\n0.994352 0.001192 -0.106125 -0.066242 -0.001637 0.999990 -0.004102 -0.030834 0.106119 0.004252 0.994344 1.050203\n0.991757 0.001626 -0.128123 -0.090664 -0.002264 0.999986 -0.004839 -0.038585 0.128113 0.005089 0.991746 1.217223\n0.988393 0.004867 -0.151843 -0.131323 -0.005804 0.999967 -0.005725 -0.052324 0.151811 0.006540 0.988388 1.386708\n0.984185 0.007977 -0.176964 -0.179620 -0.009398 0.999930 -0.007190 -0.062844 0.176894 0.008739 0.984191 1.557983\n0.978964 0.010542 -0.203759 -0.234218 -0.012493 0.999888 -0.008295 -0.071789 0.203648 0.010667 0.978986 1.740896\n0.972354 0.011586 -0.233226 -0.292734 -0.013919 0.999868 -0.008359 -0.081010 0.233099 0.011374 0.972387 1.925774\n0.964519 0.013213 -0.263684 -0.361060 -0.015953 0.999839 -0.008255 -0.089784 0.263532 0.012169 0.964574 2.113684\n0.955277 0.014693 -0.295348 -0.438136 -0.017830 0.999810 -0.007932 -0.096742 0.295175 0.012843 0.955357 2.308174\n0.944194 0.016417 -0.328980 -0.527622 -0.019907 0.999776 -0.007244 -0.107118 0.328788 0.013389 0.944309 2.505281\n0.931791 0.018465 -0.362525 -0.626267 -0.022582 0.999720 -0.007123 -0.114883 0.362292 0.014823 0.931947 2.699727\n0.918187 0.020410 -0.395622 -0.736093 -0.025288 0.999655 -0.007119 -0.120680 0.395340 0.016541 0.918386 2.900273\n0.902597 0.020682 -0.429989 -0.845504 -0.026201 0.999633 -0.006917 -0.120859 0.429688 0.017510 0.902808 3.101664\n0.884543 0.016993 -0.466149 -0.953923 -0.023371 0.999696 -0.007906 -0.119493 0.465873 0.017888 0.884671 3.306189\n0.863815 0.013816 -0.503620 -1.074103 -0.021064 0.999740 -0.008703 -0.127838 0.503369 0.018126 0.863882 3.518143\n0.839567 0.014982 -0.543050 -1.225684 -0.022276 0.999728 -0.006858 -0.141222 0.542800 0.017855 0.839672 3.726084\n0.811214 0.019668 -0.584419 -1.407819 -0.026901 0.999631 -0.003698 -0.154548 0.584131 0.018721 0.811444 3.922420\n0.779197 0.022109 -0.626389 -1.597761 -0.030081 0.999545 -0.002140 -0.164761 0.626057 0.020510 0.779508 4.108988\n0.743621 0.021244 -0.668264 -1.790137 -0.030505 0.999532 -0.002170 -0.164800 0.667906 0.021999 0.743921 4.295566\n0.704371 0.017453 -0.709617 -1.992707 -0.026837 0.999638 -0.002053 -0.167056 0.709324 0.020490 0.704584 4.492702\n0.662180 0.015206 -0.749191 -2.210372 -0.024273 0.999705 -0.001164 -0.173942 0.748952 0.018956 0.662353 4.679632\n0.617945 0.014595 -0.786086 -2.454298 -0.023292 0.999729 0.000252 -0.180870 0.785876 0.018154 0.618118 4.861948\n0.569888 0.014943 -0.821586 -2.721867 -0.022510 0.999743 0.002570 -0.185570 0.821414 0.017029 0.570078 5.035274\n0.518894 0.015114 -0.854705 -3.006581 -0.022314 0.999743 0.004132 -0.191688 0.854547 0.016928 0.519098 5.188268\n0.466026 0.015597 -0.884633 -3.310321 -0.022490 0.999730 0.005779 -0.193593 0.884485 0.017202 0.466251 5.334186\n0.411617 0.016667 -0.911204 -3.632512 -0.022665 0.999711 0.008048 -0.197094 0.911075 0.017340 0.411876 5.465319\n0.355852 0.017582 -0.934377 -3.976187 -0.023060 0.999684 0.010028 -0.202275 0.934258 0.017978 0.356145 5.573239\n0.300293 0.019462 -0.953648 -4.340522 -0.022623 0.999656 0.013277 -0.203870 0.953579 0.017587 0.300630 5.682573\n0.245991 0.020384 -0.969058 -4.721312 -0.019826 0.999676 0.015995 -0.206307 0.969069 0.015278 0.246316 5.774537\n0.194299 0.020330 -0.980732 -5.106851 -0.017920 0.999692 0.017172 -0.215300 0.980779 0.014238 0.194603 5.845954\n" }, { "path": "src/vloam_main/results/2011_09_30_drive_0027/MO1.txt", "content": "1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000\n0.999981 0.000503 -0.006166 0.000845 -0.000503 1.000000 0.000097 0.001968 0.006166 -0.000094 0.999981 0.086065\n0.999913 0.001192 -0.013139 -0.008107 -0.001191 0.999999 0.000028 0.000052 0.013139 -0.000012 0.999914 0.179844\n0.999781 0.001354 -0.020883 -0.014802 -0.001362 0.999999 -0.000397 -0.004318 0.020882 0.000425 0.999782 0.277173\n0.999549 0.001987 -0.029976 -0.019902 -0.002040 0.999996 -0.001741 -0.006640 0.029972 0.001802 0.999549 0.391437\n0.999136 0.001775 -0.041522 -0.034133 -0.001914 0.999993 -0.003312 -0.004168 0.041515 0.003388 0.999132 0.509875\n0.998465 0.001863 -0.055352 -0.046794 -0.002111 0.999988 -0.004420 -0.007312 0.055343 0.004530 0.998457 0.641517\n0.997442 0.001737 -0.071462 -0.062039 -0.002083 0.999986 -0.004762 -0.006246 0.071453 0.004898 0.997432 0.786669\n0.995932 0.000846 -0.090102 -0.079776 -0.001306 0.999986 -0.005049 -0.005997 0.090097 0.005146 0.995920 0.947394\n0.993917 -0.000330 -0.110132 -0.098814 -0.000277 0.999985 -0.005494 -0.011232 0.110132 0.005491 0.993902 1.116607\n0.991182 0.000012 -0.132510 -0.133501 -0.000842 0.999980 -0.006209 -0.018429 0.132507 0.006266 0.991162 1.289866\n0.987674 0.002891 -0.156496 -0.179711 -0.004083 0.999965 -0.007299 -0.035527 0.156469 0.007848 0.987652 1.469974\n0.983287 0.006518 -0.181945 -0.241227 -0.008330 0.999923 -0.009201 -0.043599 0.181871 0.010563 0.983266 1.655757\n0.977861 0.009490 -0.209039 -0.308386 -0.011973 0.999872 -0.010615 -0.046713 0.208912 0.012883 0.977850 1.847433\n0.971073 0.009863 -0.238579 -0.372233 -0.012952 0.999851 -0.011385 -0.047995 0.238431 0.014146 0.971056 2.046165\n0.962988 0.011698 -0.269290 -0.455137 -0.015348 0.999817 -0.011455 -0.057028 0.269107 0.015165 0.962991 2.244061\n0.953494 0.013000 -0.301131 -0.548250 -0.017177 0.999789 -0.011229 -0.057756 0.300921 0.015879 0.953517 2.449520\n0.942197 0.015446 -0.334705 -0.648725 -0.020130 0.999742 -0.010531 -0.068915 0.334455 0.016660 0.942264 2.662501\n0.929641 0.017578 -0.368047 -0.752825 -0.023068 0.999678 -0.010523 -0.076574 0.367743 0.018272 0.929748 2.858434\n0.915755 0.019961 -0.401241 -0.876818 -0.026199 0.999606 -0.010066 -0.078627 0.400882 0.019731 0.915917 3.064391\n0.899663 0.020114 -0.436121 -0.997729 -0.027388 0.999571 -0.010398 -0.074800 0.435725 0.021299 0.899828 3.268138\n0.881223 0.015743 -0.472438 -1.120768 -0.024129 0.999640 -0.011695 -0.066532 0.472084 0.021705 0.881286 3.479819\n0.859991 0.011854 -0.510171 -1.257183 -0.021177 0.999698 -0.012470 -0.077642 0.509869 0.021529 0.859982 3.693240\n0.835156 0.013962 -0.549835 -1.423963 -0.023227 0.999681 -0.009894 -0.097716 0.549522 0.021034 0.835214 3.904885\n0.806174 0.019175 -0.591367 -1.617333 -0.028485 0.999574 -0.006420 -0.109585 0.590992 0.022021 0.806377 4.097221\n0.773634 0.021108 -0.633281 -1.812435 -0.031493 0.999491 -0.005158 -0.119427 0.632849 0.023934 0.773905 4.293446\n0.737597 0.019537 -0.674959 -2.010469 -0.031580 0.999486 -0.005580 -0.112796 0.674503 0.025431 0.737834 4.483983\n0.697862 0.015644 -0.716061 -2.223068 -0.028083 0.999590 -0.005532 -0.110267 0.715682 0.023970 0.698015 4.684443\n0.655125 0.012197 -0.755422 -2.442326 -0.024353 0.999691 -0.004978 -0.120122 0.755128 0.021658 0.655219 4.870140\n0.610134 0.011478 -0.792215 -2.684898 -0.023253 0.999724 -0.003423 -0.129258 0.791957 0.020510 0.610233 5.048868\n0.561728 0.011733 -0.827239 -2.961311 -0.022387 0.999749 -0.001022 -0.135848 0.827019 0.019094 0.561850 5.229341\n0.510440 0.011787 -0.859832 -3.245569 -0.021779 0.999763 0.000776 -0.148378 0.859637 0.018330 0.510576 5.374135\n0.457118 0.012502 -0.889318 -3.554899 -0.022174 0.999751 0.002656 -0.150600 0.889129 0.018506 0.457281 5.519981\n0.402083 0.013816 -0.915499 -3.890706 -0.022095 0.999741 0.005383 -0.156220 0.915337 0.018064 0.402284 5.647282\n0.346226 0.015209 -0.938028 -4.228828 -0.023033 0.999705 0.007708 -0.163943 0.937868 0.018937 0.346474 5.750358\n0.290593 0.017289 -0.956691 -4.593495 -0.022750 0.999679 0.011156 -0.164985 0.956576 0.018523 0.290893 5.849444\n0.236322 0.018315 -0.971502 -4.960089 -0.019386 0.999712 0.014131 -0.165450 0.971482 0.015494 0.236609 5.916595\n0.184984 0.018459 -0.982568 -5.338185 -0.017653 0.999725 0.015457 -0.178134 0.982583 0.014486 0.185259 5.978823\n" }, { "path": "src/vloam_main/results/2011_09_30_drive_0027/VO1.txt", "content": "1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000\n0.999977 0.000335 -0.006831 0.000075 -0.000331 1.000000 0.000535 -0.004494 0.006831 -0.000533 0.999977 0.097957\n0.999905 0.000828 -0.013780 -0.012004 -0.000821 1.000000 0.000459 -0.006137 0.013781 -0.000448 0.999905 0.197556\n0.999769 0.000987 -0.021480 -0.017978 -0.000998 0.999999 -0.000494 -0.003870 0.021480 0.000515 0.999769 0.302305\n0.999515 0.001664 -0.031092 -0.027319 -0.001728 0.999996 -0.002040 -0.001080 0.031089 0.002093 0.999514 0.411621\n0.999097 0.002119 -0.042425 -0.045148 -0.002266 0.999992 -0.003428 -0.005316 0.042418 0.003521 0.999094 0.534416\n0.998438 0.002412 -0.055813 -0.070002 -0.002637 0.999989 -0.003953 -0.010590 0.055802 0.004094 0.998433 0.663438\n0.997409 0.002595 -0.071899 -0.099164 -0.002894 0.999988 -0.004049 -0.009927 0.071888 0.004246 0.997404 0.807333\n0.995916 0.000405 -0.090283 -0.135661 -0.000746 0.999993 -0.003747 -0.015402 0.090281 0.003799 0.995909 0.968452\n0.993903 -0.000579 -0.110254 -0.168425 0.000131 0.999992 -0.004071 -0.013981 0.110255 0.004031 0.993895 1.134779\n0.991202 -0.000248 -0.132360 -0.218817 -0.000333 0.999990 -0.004369 -0.019074 0.132359 0.004375 0.991192 1.308372\n0.987725 0.002943 -0.156177 -0.287222 -0.003887 0.999976 -0.005735 -0.024051 0.156156 0.006272 0.987712 1.499169\n0.983459 0.007140 -0.180989 -0.367641 -0.008577 0.999938 -0.007156 -0.028939 0.180927 0.008589 0.983459 1.684594\n0.978099 0.009794 -0.207911 -0.444241 -0.011695 0.999900 -0.007914 -0.037189 0.207812 0.010172 0.978116 1.880309\n0.971369 0.010702 -0.237333 -0.519203 -0.013024 0.999881 -0.008217 -0.037830 0.237217 0.011073 0.971394 2.070105\n0.963344 0.012440 -0.267982 -0.604893 -0.015124 0.999854 -0.007954 -0.037194 0.267844 0.011716 0.963391 2.260879\n0.953883 0.014276 -0.299840 -0.695751 -0.017110 0.999830 -0.006830 -0.040658 0.299691 0.011646 0.953965 2.448441\n0.942670 0.015773 -0.333354 -0.799622 -0.018835 0.999805 -0.005957 -0.037482 0.333195 0.011894 0.942783 2.635985\n0.930178 0.018097 -0.366662 -0.910531 -0.021553 0.999753 -0.005334 -0.043839 0.366475 0.012864 0.930339 2.821133\n0.916546 0.020621 -0.399398 -1.049098 -0.024525 0.999688 -0.004669 -0.043202 0.399177 0.014074 0.916766 3.026388\n0.900502 0.020284 -0.434378 -1.180035 -0.024671 0.999686 -0.004464 -0.036261 0.434151 0.014736 0.900720 3.222924\n0.882257 0.015093 -0.470527 -1.323978 -0.020160 0.999780 -0.005732 -0.027758 0.470337 0.014543 0.882367 3.427115\n0.861042 0.010201 -0.508431 -1.465348 -0.015475 0.999861 -0.006147 -0.031931 0.508298 0.013160 0.861081 3.612115\n0.836100 0.011759 -0.548450 -1.624945 -0.015963 0.999868 -0.002898 -0.040228 0.548344 0.011178 0.836178 3.786405\n0.807083 0.017264 -0.590186 -1.811359 -0.020874 0.999782 0.000700 -0.043922 0.590069 0.011754 0.807267 3.960381\n0.774471 0.020007 -0.632293 -2.006480 -0.023893 0.999712 0.002367 -0.047272 0.632158 0.013274 0.774726 4.127898\n0.738362 0.018943 -0.674139 -2.222323 -0.023342 0.999724 0.002526 -0.043018 0.674001 0.013870 0.738600 4.303214\n0.698243 0.014489 -0.715715 -2.445933 -0.017423 0.999843 0.003244 -0.040817 0.715649 0.010205 0.698385 4.476686\n0.654994 0.011120 -0.755552 -2.680421 -0.012432 0.999915 0.003939 -0.043093 0.755532 0.006813 0.655077 4.639613\n0.609395 0.010623 -0.792795 -2.939290 -0.010181 0.999933 0.005573 -0.040306 0.792801 0.004675 0.609462 4.797564\n0.560290 0.011373 -0.828219 -3.211698 -0.007458 0.999934 0.008686 -0.043704 0.828263 0.001310 0.560338 4.945599\n0.508717 0.011866 -0.860852 -3.499644 -0.006356 0.999930 0.010027 -0.036006 0.860910 0.000371 0.508757 5.069410\n0.455081 0.012977 -0.890355 -3.801037 -0.005923 0.999916 0.011546 -0.027258 0.890430 0.000019 0.455120 5.183798\n0.400012 0.015060 -0.916386 -4.148390 -0.004774 0.999886 0.014348 -0.022234 0.916497 -0.001365 0.400038 5.289347\n0.344040 0.017375 -0.938794 -4.514171 -0.004706 0.999848 0.016780 -0.021315 0.938943 -0.001355 0.344070 5.384810\n0.288578 0.020319 -0.957241 -4.903814 -0.002899 0.999789 0.020349 -0.015851 0.957452 -0.003097 0.288576 5.462905\n0.234831 0.021876 -0.971790 -5.323771 0.001162 0.999740 0.022786 -0.009815 0.972035 -0.006480 0.234745 5.526153\n0.183531 0.022642 -0.982753 -5.760025 0.004170 0.999708 0.023812 -0.010521 0.983005 -0.008468 0.183383 5.578844\n" }, { "path": "src/vloam_main/results/2011_10_03_drive_0027/LO0.txt", "content": "1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000\n0.999993 -0.001659 -0.003273 0.008716 0.001654 0.999997 -0.001490 -0.013225 0.003275 0.001485 0.999994 0.684043\n0.999971 -0.000793 -0.007632 0.003274 0.000773 0.999996 -0.002674 -0.019588 0.007634 0.002668 0.999967 1.386483\n0.999926 -0.000251 -0.012171 -0.004835 0.000202 0.999992 -0.003974 -0.026179 0.012172 0.003972 0.999918 2.123660\n0.999845 0.001572 -0.017561 -0.015903 -0.001657 0.999987 -0.004853 -0.033604 0.017553 0.004881 0.999834 2.857347\n0.999757 0.001001 -0.022024 -0.026450 -0.001113 0.999987 -0.005057 -0.046934 0.022019 0.005080 0.999745 3.611373\n0.999634 0.002093 -0.026968 -0.049151 -0.002250 0.999981 -0.005775 -0.061621 0.026956 0.005834 0.999620 4.380025\n0.999490 0.004717 -0.031596 -0.085670 -0.004913 0.999969 -0.006127 -0.073851 0.031566 0.006279 0.999482 5.170325\n0.999336 0.006087 -0.035919 -0.121844 -0.006276 0.999967 -0.005157 -0.086370 0.035887 0.005379 0.999341 5.987267\n0.999167 0.006164 -0.040345 -0.155639 -0.006274 0.999977 -0.002604 -0.103308 0.040328 0.002855 0.999182 6.811518\n0.999006 0.005814 -0.044196 -0.187779 -0.005855 0.999983 -0.000787 -0.122185 0.044191 0.001045 0.999023 7.640314\n0.998880 0.006672 -0.046845 -0.235581 -0.006718 0.999977 -0.000826 -0.134454 0.046839 0.001140 0.998902 8.466795\n0.998790 0.006555 -0.048734 -0.281808 -0.006637 0.999977 -0.001524 -0.144005 0.048723 0.001846 0.998811 9.298368\n0.998722 0.004673 -0.050324 -0.313495 -0.004771 0.999987 -0.001828 -0.159121 0.050314 0.002066 0.998731 10.142959\n0.998592 0.004303 -0.052868 -0.356936 -0.004371 0.999990 -0.001170 -0.169732 0.052863 0.001399 0.998601 11.000464\n0.998487 0.003213 -0.054898 -0.394817 -0.003296 0.999994 -0.001417 -0.187345 0.054894 0.001596 0.998491 11.868357\n0.998329 0.004097 -0.057648 -0.451548 -0.004273 0.999987 -0.002929 -0.204846 0.057636 0.003170 0.998333 12.743898\n0.998199 0.005164 -0.059763 -0.514080 -0.005473 0.999972 -0.005007 -0.222873 0.059736 0.005325 0.998200 13.639276\n0.998118 0.008330 -0.060747 -0.584606 -0.008733 0.999942 -0.006364 -0.240669 0.060691 0.006882 0.998133 14.522412\n0.998025 0.009157 -0.062152 -0.647032 -0.009574 0.999934 -0.006416 -0.254212 0.062089 0.006999 0.998046 15.434151\n0.997897 0.009061 -0.064185 -0.704036 -0.009409 0.999943 -0.005135 -0.277303 0.064135 0.005728 0.997925 16.371244\n0.997753 0.009491 -0.066330 -0.766717 -0.009769 0.999945 -0.003868 -0.299435 0.066290 0.004508 0.997790 17.299471\n0.997660 0.009618 -0.067687 -0.829194 -0.009876 0.999945 -0.003476 -0.322065 0.067650 0.004137 0.997701 18.225716\n0.997526 0.009806 -0.069604 -0.896171 -0.010163 0.999937 -0.004771 -0.337918 0.069553 0.005467 0.997563 19.137800\n0.997466 0.008792 -0.070601 -0.960881 -0.009210 0.999942 -0.005588 -0.357513 0.070548 0.006224 0.997489 20.064907\n0.997414 0.009076 -0.071296 -1.024922 -0.009398 0.999947 -0.004178 -0.382706 0.071254 0.004837 0.997446 21.005907\n0.997421 0.010682 -0.070976 -1.103215 -0.010813 0.999940 -0.001466 -0.403438 0.070956 0.002230 0.997477 21.951910\n0.997473 0.010988 -0.070198 -1.177156 -0.010932 0.999940 0.001180 -0.424566 0.070207 -0.000410 0.997532 22.903915\n0.997596 0.010094 -0.068555 -1.236652 -0.009871 0.999945 0.003583 -0.438517 0.068587 -0.002897 0.997641 23.860506\n0.997705 0.009647 -0.067026 -1.301346 -0.009400 0.999948 0.004003 -0.453401 0.067061 -0.003364 0.997743 24.817236\n0.997795 0.010197 -0.065579 -1.368882 -0.010127 0.999948 0.001387 -0.470144 0.065590 -0.000720 0.997846 25.777435\n0.997885 0.009984 -0.064239 -1.439307 -0.010220 0.999942 -0.003353 -0.484432 0.064201 0.004003 0.997929 26.725578\n0.998001 0.008953 -0.062558 -1.502228 -0.009377 0.999935 -0.006488 -0.498020 0.062496 0.007062 0.998020 27.682653\n0.998047 0.007797 -0.061974 -1.559233 -0.008184 0.999949 -0.006004 -0.522091 0.061924 0.006500 0.998060 28.656778\n0.998051 0.005999 -0.062107 -1.615665 -0.006140 0.999979 -0.002081 -0.544517 0.062093 0.002458 0.998067 29.647970\n0.998022 0.003613 -0.062759 -1.667733 -0.003569 0.999993 0.000808 -0.567463 0.062762 -0.000583 0.998028 30.651552\n0.997965 -0.004095 -0.063628 -1.699348 0.004096 0.999992 -0.000116 -0.584920 0.063627 -0.000145 0.997974 31.633680\n0.997847 -0.006467 -0.065261 -1.752559 0.006247 0.999974 -0.003579 -0.602367 0.065282 0.003164 0.997862 32.618046\n0.997725 -0.006086 -0.067133 -1.819706 0.005626 0.999959 -0.007044 -0.620201 0.067173 0.006651 0.997719 33.609489\n0.997555 -0.005278 -0.069693 -1.889548 0.004729 0.999957 -0.008036 -0.635722 0.069732 0.007687 0.997536 34.610912\n0.997504 -0.004796 -0.070449 -1.963730 0.004325 0.999967 -0.006828 -0.666200 0.070479 0.006506 0.997492 35.633209\n0.997401 -0.002875 -0.071989 -2.046202 0.002534 0.999985 -0.004821 -0.684569 0.072002 0.004626 0.997394 36.659763\n0.997331 -0.002362 -0.072976 -2.124398 0.002093 0.999991 -0.003763 -0.708027 0.072984 0.003601 0.997327 37.662518\n" }, { "path": "src/vloam_main/results/2011_10_03_drive_0027/MO0.txt", "content": "1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000\n0.999991 -0.002532 -0.003325 0.007601 0.002527 0.999996 -0.001473 -0.016596 0.003329 0.001464 0.999993 0.689208\n0.999973 -0.001377 -0.007255 -0.002937 0.001355 0.999995 -0.002963 -0.027776 0.007259 0.002953 0.999969 1.385449\n0.999935 -0.000960 -0.011327 -0.007938 0.000910 0.999990 -0.004385 -0.036778 0.011331 0.004374 0.999926 2.105824\n0.999872 0.000626 -0.015964 -0.032815 -0.000707 0.999987 -0.005079 -0.047140 0.015961 0.005090 0.999860 2.843953\n0.999791 -0.000283 -0.020431 -0.046530 0.000177 0.999987 -0.005193 -0.061772 0.020433 0.005188 0.999778 3.585939\n0.999686 0.000758 -0.025065 -0.076200 -0.000910 0.999981 -0.006069 -0.080687 0.025060 0.006090 0.999667 4.340120\n0.999547 0.003997 -0.029816 -0.111509 -0.004196 0.999969 -0.006600 -0.099609 0.029788 0.006722 0.999534 5.121953\n0.999393 0.006212 -0.034290 -0.146212 -0.006410 0.999963 -0.005683 -0.110601 0.034254 0.005900 0.999396 5.920063\n0.999234 0.006146 -0.038639 -0.181769 -0.006269 0.999976 -0.003064 -0.129515 0.038619 0.003304 0.999249 6.725366\n0.999088 0.004799 -0.042432 -0.208794 -0.004840 0.999988 -0.000853 -0.146308 0.042427 0.001058 0.999099 7.544958\n0.998974 0.004973 -0.045017 -0.252138 -0.005015 0.999987 -0.000811 -0.162500 0.045012 0.001035 0.998986 8.357373\n0.998875 0.004766 -0.047181 -0.294226 -0.004839 0.999987 -0.001429 -0.170390 0.047174 0.001656 0.998885 9.184386\n0.998797 0.002777 -0.048965 -0.327845 -0.002860 0.999995 -0.001622 -0.191625 0.048960 0.001760 0.998799 10.014240\n0.998691 0.002915 -0.051067 -0.374237 -0.002968 0.999995 -0.000966 -0.202171 0.051064 0.001116 0.998695 10.857125\n0.998631 0.000369 -0.052310 -0.409844 -0.000425 0.999999 -0.001061 -0.217502 0.052310 0.001082 0.998630 11.719256\n0.998537 0.001064 -0.054057 -0.466338 -0.001221 0.999995 -0.002859 -0.231677 0.054053 0.002920 0.998534 12.580934\n0.998463 0.002190 -0.055385 -0.526552 -0.002500 0.999982 -0.005515 -0.249095 0.055372 0.005645 0.998450 13.448643\n0.998343 0.006388 -0.057183 -0.590663 -0.006788 0.999954 -0.006800 -0.267237 0.057137 0.007177 0.998341 14.324721\n0.998267 0.008216 -0.058279 -0.648618 -0.008620 0.999941 -0.006686 -0.279569 0.058221 0.007177 0.998278 15.206080\n0.998189 0.008000 -0.059628 -0.702347 -0.008326 0.999952 -0.005214 -0.296854 0.059583 0.005701 0.998207 16.126270\n0.998107 0.008180 -0.060950 -0.761131 -0.008437 0.999957 -0.003956 -0.319603 0.060915 0.004462 0.998133 17.021088\n0.998039 0.007869 -0.062090 -0.817295 -0.008097 0.999961 -0.003410 -0.342441 0.062061 0.003906 0.998065 17.930737\n0.997951 0.006829 -0.063624 -0.883642 -0.007113 0.999966 -0.004239 -0.356117 0.063593 0.004683 0.997965 18.840097\n0.997888 0.006443 -0.064633 -0.941382 -0.006759 0.999966 -0.004669 -0.374705 0.064601 0.005096 0.997898 19.756872\n0.997834 0.007415 -0.065361 -0.999833 -0.007635 0.999966 -0.003111 -0.394854 0.065336 0.003603 0.997857 20.683264\n0.997807 0.009464 -0.065518 -1.072065 -0.009466 0.999955 0.000280 -0.412711 0.065517 0.000341 0.997851 21.616070\n0.997847 0.009842 -0.064838 -1.135284 -0.009607 0.999946 0.003937 -0.425375 0.064874 -0.003305 0.997888 22.560795\n0.997954 0.009285 -0.063261 -1.196808 -0.008860 0.999936 0.006994 -0.434931 0.063322 -0.006419 0.997972 23.498438\n0.998075 0.006042 -0.061722 -1.254876 -0.005568 0.999954 0.007849 -0.443514 0.061767 -0.007491 0.998062 24.452238\n0.998186 0.004607 -0.060025 -1.314149 -0.004289 0.999976 0.005425 -0.451563 0.060049 -0.005158 0.998182 25.388998\n0.998253 0.004104 -0.058934 -1.383431 -0.004034 0.999991 0.001299 -0.465997 0.058939 -0.001059 0.998261 26.347254\n0.998322 0.005583 -0.057628 -1.443418 -0.005691 0.999982 -0.001701 -0.479484 0.057618 0.002026 0.998337 27.293030\n0.998353 0.007298 -0.056909 -1.500229 -0.007375 0.999972 -0.001142 -0.502057 0.056899 0.001560 0.998379 28.267855\n0.998366 0.007506 -0.056655 -1.560371 -0.007334 0.999968 0.003240 -0.516336 0.056677 -0.002819 0.998389 29.247440\n0.998336 0.002551 -0.057608 -1.606208 -0.002160 0.999974 0.006846 -0.526232 0.057624 -0.006710 0.998316 30.240128\n0.998234 -0.008296 -0.058818 -1.631684 0.008710 0.999939 0.006782 -0.535418 0.058759 -0.007283 0.998246 31.214983\n0.998118 -0.011278 -0.060282 -1.682139 0.011515 0.999927 0.003584 -0.549919 0.060238 -0.004271 0.998175 32.188995\n0.998027 -0.010590 -0.061887 -1.741999 0.010617 0.999944 0.000094 -0.569401 0.061883 -0.000750 0.998083 33.166988\n0.997956 -0.006761 -0.063552 -1.818134 0.006708 0.999977 -0.001053 -0.578677 0.063558 0.000625 0.997978 34.153900\n0.997907 -0.005435 -0.064435 -1.882568 0.005443 0.999985 -0.000062 -0.603953 0.064434 -0.000289 0.997922 35.156147\n0.997832 -0.003330 -0.065729 -1.959979 0.003509 0.999990 0.002605 -0.618018 0.065720 -0.002830 0.997834 36.167004\n0.997769 -0.001090 -0.066747 -2.041527 0.001374 0.999990 0.004212 -0.635423 0.066742 -0.004295 0.997761 37.157394\n" }, { "path": "src/vloam_main/results/2011_10_03_drive_0027/VO0.txt", "content": "1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000\n0.999991 -0.002867 -0.003137 -0.008677 0.002861 0.999994 -0.001794 -0.005395 0.003142 0.001785 0.999993 0.689198\n0.999974 -0.001851 -0.006950 -0.025518 0.001826 0.999992 -0.003525 -0.003597 0.006956 0.003512 0.999970 1.401380\n0.999940 -0.002288 -0.010760 -0.054202 0.002234 0.999985 -0.005031 -0.000915 0.010772 0.005007 0.999929 2.157266\n0.999880 -0.000627 -0.015494 -0.074810 0.000541 0.999984 -0.005559 -0.005534 0.015498 0.005550 0.999865 2.880791\n0.999804 -0.002303 -0.019685 -0.101013 0.002189 0.999981 -0.005802 -0.008524 0.019698 0.005758 0.999789 3.644862\n0.999715 -0.000527 -0.023875 -0.137287 0.000362 0.999976 -0.006891 -0.016687 0.023878 0.006881 0.999691 4.423509\n0.999595 0.002205 -0.028359 -0.182794 -0.002422 0.999968 -0.007625 -0.019662 0.028342 0.007691 0.999569 5.212629\n0.999481 0.002270 -0.032123 -0.232011 -0.002459 0.999980 -0.005834 -0.025669 0.032109 0.005910 0.999467 6.035453\n0.999329 0.001430 -0.036605 -0.269019 -0.001559 0.999993 -0.003496 -0.024104 0.036599 0.003551 0.999324 6.859456\n0.999208 0.000668 -0.039788 -0.313713 -0.000707 0.999999 -0.000952 -0.025388 0.039788 0.000979 0.999208 7.692318\n0.999121 0.002400 -0.041859 -0.373539 -0.002471 0.999996 -0.001647 -0.033235 0.041855 0.001749 0.999122 8.535277\n0.999019 0.001428 -0.044258 -0.408697 -0.001525 0.999997 -0.002147 -0.032008 0.044255 0.002212 0.999018 9.378335\n0.998918 -0.001740 -0.046472 -0.440484 0.001616 0.999995 -0.002692 -0.036176 0.046476 0.002614 0.998916 10.237741\n0.998836 -0.002251 -0.048185 -0.487357 0.002159 0.999996 -0.001962 -0.039190 0.048189 0.001855 0.998837 11.096232\n0.998753 -0.004125 -0.049744 -0.525963 0.004018 0.999989 -0.002249 -0.043930 0.049753 0.002046 0.998759 11.959485\n0.998671 -0.002559 -0.051472 -0.577891 0.002341 0.999988 -0.004291 -0.056633 0.051482 0.004165 0.998665 12.826673\n0.998627 -0.002014 -0.052354 -0.643591 0.001663 0.999976 -0.006746 -0.067709 0.052367 0.006649 0.998606 13.719974\n0.998545 0.001476 -0.053904 -0.704790 -0.001923 0.999964 -0.008242 -0.078519 0.053890 0.008333 0.998512 14.615861\n0.998489 0.001458 -0.054938 -0.767282 -0.001902 0.999966 -0.008032 -0.078835 0.054924 0.008124 0.998457 15.545435\n0.998425 0.000119 -0.056110 -0.824220 -0.000475 0.999980 -0.006336 -0.093049 0.056108 0.006353 0.998404 16.477526\n0.998367 0.000408 -0.057122 -0.890117 -0.000683 0.999988 -0.004793 -0.104108 0.057119 0.004824 0.998356 17.404747\n0.998314 -0.000073 -0.058045 -0.950200 -0.000188 0.999990 -0.004485 -0.118554 0.058044 0.004489 0.998304 18.331909\n0.998237 0.000209 -0.059347 -1.013856 -0.000526 0.999986 -0.005325 -0.133091 0.059345 0.005347 0.998223 19.266102\n0.998175 -0.002006 -0.060353 -1.071375 0.001655 0.999981 -0.005874 -0.141091 0.060364 0.005763 0.998160 20.223894\n0.998132 -0.001994 -0.061058 -1.124848 0.001744 0.999990 -0.004137 -0.149156 0.061065 0.004023 0.998126 21.182606\n0.998145 -0.000810 -0.060869 -1.188426 0.000776 1.000000 -0.000587 -0.162216 0.060869 0.000539 0.998146 22.144611\n0.998199 0.000333 -0.059997 -1.251848 -0.000142 0.999995 0.003196 -0.171171 0.059998 -0.003182 0.998193 23.105305\n0.998293 -0.001066 -0.058394 -1.307305 0.001429 0.999980 0.006176 -0.178778 0.058386 -0.006248 0.998275 24.081455\n0.998371 -0.001029 -0.057053 -1.359591 0.001441 0.999973 0.007196 -0.184911 0.057044 -0.007267 0.998345 25.052111\n0.998450 -0.001157 -0.055639 -1.416490 0.001399 0.999990 0.004302 -0.184370 0.055633 -0.004373 0.998442 26.037388\n0.998499 -0.001175 -0.054752 -1.467745 0.001147 0.999999 -0.000544 -0.184285 0.054753 0.000481 0.998500 27.004524\n0.998560 -0.002964 -0.053563 -1.518991 0.002773 0.999990 -0.003634 -0.181360 0.053573 0.003480 0.998558 27.980488\n0.998592 -0.004665 -0.052850 -1.568298 0.004539 0.999987 -0.002497 -0.197010 0.052860 0.002254 0.998599 28.955654\n0.998573 -0.006734 -0.052984 -1.618079 0.006885 0.999973 0.002680 -0.209933 0.052965 -0.003041 0.998592 29.945097\n0.998474 -0.010665 -0.054193 -1.656987 0.011019 0.999920 0.006252 -0.222691 0.054122 -0.006840 0.998511 30.950569\n0.998294 -0.018785 -0.055279 -1.692170 0.019103 0.999804 0.005238 -0.216850 0.055170 -0.006285 0.998457 31.933308\n0.998162 -0.021260 -0.056753 -1.742199 0.021361 0.999771 0.001167 -0.216375 0.056715 -0.002378 0.998388 32.938709\n0.998084 -0.020845 -0.058252 -1.795509 0.020719 0.999781 -0.002775 -0.225727 0.058297 0.001562 0.998298 33.948219\n0.997958 -0.020198 -0.060601 -1.843116 0.020015 0.999793 -0.003618 -0.225298 0.060662 0.002398 0.998156 34.963974\n0.997903 -0.020146 -0.061508 -1.905117 0.020067 0.999797 -0.001902 -0.246244 0.061534 0.000664 0.998105 35.989594\n0.997919 -0.017080 -0.062181 -1.982621 0.017149 0.999853 0.000575 -0.252618 0.062162 -0.001640 0.998065 37.001781\n0.997830 -0.016921 -0.063637 -2.036683 0.017075 0.999852 0.001874 -0.266850 0.063596 -0.002956 0.997971 38.001797\n" }, { "path": "src/vloam_main/results/2011_10_03_drive_0042/LO0.txt", "content": "1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000\n0.999029 -0.001137 0.044044 0.002061 0.001214 0.999998 -0.001708 -0.021135 -0.044042 0.001760 0.999028 0.959330\n0.996009 -0.005297 0.089091 0.063082 0.005800 0.999969 -0.005384 -0.033378 -0.089060 0.005880 0.996009 1.939704\n0.990894 -0.005523 0.134528 0.150644 0.006636 0.999947 -0.007830 -0.073711 -0.134477 0.008652 0.990879 2.923102\n0.983635 -0.003934 0.180131 0.274686 0.005754 0.999938 -0.009584 -0.094159 -0.180082 0.010463 0.983596 3.901412\n0.974108 -0.006065 0.226001 0.452891 0.008343 0.999924 -0.009127 -0.119422 -0.225928 0.010776 0.974084 4.883620\n0.962409 -0.007499 0.271501 0.700310 0.010454 0.999901 -0.009441 -0.149805 -0.271403 0.011924 0.962392 5.917480\n0.948777 -0.010664 0.315766 0.978531 0.015011 0.999823 -0.011336 -0.170299 -0.315590 0.015495 0.948769 6.908070\n0.933660 -0.012510 0.357943 1.303064 0.017882 0.999772 -0.011700 -0.221228 -0.357715 0.017325 0.933670 7.843632\n0.916319 -0.013010 0.400238 1.669068 0.019142 0.999753 -0.011326 -0.244162 -0.399992 0.018040 0.916341 8.776813\n0.896984 -0.014126 0.441837 2.065593 0.020475 0.999744 -0.009602 -0.275840 -0.441589 0.017660 0.897044 9.670323\n0.875527 -0.014404 0.482954 2.487168 0.020710 0.999756 -0.007727 -0.313556 -0.482725 0.016767 0.875612 10.545819\n0.851768 -0.015022 0.523704 2.952663 0.021669 0.999744 -0.006568 -0.336206 -0.523471 0.016943 0.851875 11.387823\n0.826629 -0.015704 0.562529 3.444281 0.022807 0.999724 -0.005605 -0.359163 -0.562285 0.017463 0.826759 12.220148\n0.799730 -0.017830 0.600095 3.977656 0.025644 0.999661 -0.004474 -0.389255 -0.599811 0.018967 0.799917 13.013865\n0.771264 -0.018208 0.636255 4.566804 0.026836 0.999632 -0.003923 -0.409220 -0.635949 0.020100 0.771469 13.801761\n0.741191 -0.021489 0.670950 5.189893 0.031012 0.999516 -0.002246 -0.430465 -0.670577 0.022472 0.741499 14.545444\n0.709423 -0.021528 0.704454 5.832464 0.030982 0.999520 -0.000656 -0.460096 -0.704101 0.022291 0.709749 15.249025\n0.675609 -0.021649 0.736942 6.468256 0.032633 0.999467 -0.000555 -0.469607 -0.736537 0.024424 0.675956 15.911953\n0.640463 -0.023212 0.767638 7.152997 0.036540 0.999332 -0.000269 -0.488775 -0.767119 0.028222 0.640883 16.543024\n0.604319 -0.023197 0.796405 7.867389 0.036979 0.999316 0.001047 -0.513997 -0.795884 0.028817 0.604763 17.145657\n0.566872 -0.023214 0.823479 8.617120 0.039544 0.999217 0.000946 -0.532327 -0.822856 0.032027 0.567346 17.726057\n0.528511 -0.022698 0.848623 9.390218 0.043532 0.999052 -0.000390 -0.543977 -0.847810 0.037149 0.528998 18.243723\n0.490008 -0.022798 0.871420 10.178507 0.045097 0.998982 0.000777 -0.560735 -0.870551 0.038918 0.490537 18.750044\n0.452513 -0.025267 0.891400 11.005951 0.049130 0.998787 0.003370 -0.568389 -0.890403 0.042270 0.453206 19.211607\n0.415547 -0.026394 0.909189 11.843031 0.052240 0.998621 0.005114 -0.588617 -0.908070 0.045371 0.416353 19.646704\n0.380688 -0.025542 0.924351 12.669569 0.052955 0.998580 0.005784 -0.603740 -0.923186 0.046747 0.381500 20.030293\n0.347178 -0.026575 0.937423 13.529668 0.053264 0.998544 0.008581 -0.612506 -0.936286 0.046952 0.348087 20.408213\n0.314330 -0.028586 0.948883 14.440255 0.054968 0.998418 0.011870 -0.612476 -0.947721 0.048427 0.315404 20.731363\n0.281722 -0.029295 0.959049 15.359817 0.055607 0.998352 0.014161 -0.624260 -0.957883 0.049340 0.282887 21.039709\n0.249352 -0.027336 0.968027 16.318712 0.052375 0.998519 0.014706 -0.637180 -0.966995 0.047034 0.250415 21.344801\n0.218316 -0.026352 0.975522 17.275373 0.052154 0.998522 0.015301 -0.622816 -0.974483 0.047537 0.219368 21.610432\n0.187364 -0.026604 0.981930 18.254572 0.053286 0.998437 0.016883 -0.618733 -0.980844 0.049159 0.188489 21.830347\n0.156493 -0.027664 0.987292 19.245363 0.054316 0.998336 0.019364 -0.617770 -0.986184 0.050595 0.157735 22.020313\n0.125875 -0.028601 0.991634 20.257582 0.052883 0.998357 0.022082 -0.621096 -0.990636 0.049661 0.127181 22.194305\n0.096578 -0.028955 0.994904 21.301394 0.052985 0.998309 0.023911 -0.602413 -0.993914 0.050406 0.097948 22.335375\n0.069049 -0.029176 0.997187 22.373503 0.055146 0.998156 0.025386 -0.581567 -0.996088 0.053238 0.070531 22.434481\n0.043828 -0.029132 0.998614 23.458197 0.058767 0.997919 0.026533 -0.564733 -0.997309 0.057523 0.045448 22.503353\n0.021554 -0.029135 0.999343 24.521986 0.062715 0.997646 0.027732 -0.543203 -0.997799 0.062076 0.023331 22.535748\n0.001541 -0.029920 0.999551 25.628500 0.067593 0.997269 0.029747 -0.524931 -0.997712 0.067516 0.003559 22.539507\n-0.015931 -0.030832 0.999398 26.751551 0.071181 0.996953 0.031891 -0.502632 -0.997336 0.071646 -0.013688 22.520906\n-0.031436 -0.031129 0.999021 27.884748 0.073610 0.996729 0.033374 -0.481472 -0.996792 0.074587 -0.029041 22.493719\n-0.045712 -0.032024 0.998441 29.029823 0.075406 0.996524 0.035415 -0.461883 -0.996105 0.076908 -0.043138 22.450306\n-0.059058 -0.033178 0.997703 30.169725 0.075284 0.996453 0.037593 -0.439817 -0.995412 0.077331 -0.056351 22.402445\n-0.071233 -0.032997 0.996914 31.321182 0.074993 0.996447 0.038340 -0.414815 -0.994637 0.077492 -0.068506 22.333611\n-0.081957 -0.030330 0.996174 32.480049 0.075007 0.996514 0.036512 -0.381734 -0.993809 0.077712 -0.079396 22.246227\n-0.091765 -0.027119 0.995411 33.665653 0.075506 0.996562 0.034111 -0.351757 -0.992914 0.078290 -0.089402 22.146000\n-0.100832 -0.025124 0.994586 34.872837 0.075880 0.996575 0.032867 -0.320519 -0.992006 0.078784 -0.098580 22.043753\n-0.108350 -0.024317 0.993815 36.104008 0.075266 0.996631 0.032591 -0.290015 -0.991259 0.078332 -0.106155 21.919992\n-0.113877 -0.023885 0.993208 37.366032 0.074838 0.996664 0.032549 -0.253954 -0.990672 0.078036 -0.111709 21.786911\n-0.118963 -0.022937 0.992634 38.647568 0.075130 0.996659 0.032034 -0.218039 -0.990052 0.078388 -0.116842 21.637402\n-0.123091 -0.021488 0.992163 39.941994 0.074248 0.996764 0.030799 -0.184511 -0.989614 0.077457 -0.121097 21.486013\n-0.126755 -0.021025 0.991711 41.265621 0.074398 0.996758 0.030641 -0.155082 -0.989140 0.077665 -0.124780 21.323076\n-0.130096 -0.021831 0.991261 42.619091 0.073765 0.996774 0.031634 -0.123486 -0.988754 0.077235 -0.128066 21.160236\n-0.132072 -0.024073 0.990948 43.989983 0.074032 0.996673 0.034079 -0.084428 -0.988472 0.077862 -0.129850 20.973139\n-0.134194 -0.024742 0.990646 45.376198 0.074181 0.996633 0.034940 -0.044252 -0.988175 0.078175 -0.131907 20.785620\n-0.136217 -0.024422 0.990378 46.752171 0.075678 0.996518 0.034982 -0.005533 -0.987784 0.079715 -0.133894 20.595331\n-0.137319 -0.024082 0.990234 48.149204 0.075999 0.996501 0.034773 0.039154 -0.987607 0.080032 -0.135008 20.411806\n-0.138116 -0.023489 0.990137 49.592377 0.076802 0.996454 0.034352 0.078053 -0.987434 0.080789 -0.135822 20.211784\n-0.138230 -0.022868 0.990136 51.060905 0.076402 0.996508 0.033682 0.122698 -0.987449 0.080304 -0.136000 20.013060\n-0.137606 -0.022008 0.990242 52.532803 0.075801 0.996587 0.032682 0.157989 -0.987582 0.079559 -0.135469 19.814127\n" }, { "path": "src/vloam_main/results/2011_10_03_drive_0042/LO1.txt", "content": "1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000\n0.999756 0.000509 0.022065 -0.036462 -0.000488 0.999999 -0.000922 0.010289 -0.022065 0.000911 0.999756 0.236515\n0.998422 -0.002854 0.056087 -0.072147 0.003077 0.999988 -0.003894 0.013598 -0.056075 0.004060 0.998418 0.809941\n0.995202 -0.002760 0.097806 -0.086950 0.003393 0.999974 -0.006309 -0.020148 -0.097786 0.006611 0.995185 1.651002\n0.989721 -0.001187 0.143006 -0.018355 0.002377 0.999964 -0.008152 -0.037859 -0.142991 0.008408 0.989688 2.593370\n0.982093 -0.003241 0.188370 0.121008 0.004822 0.999957 -0.007933 -0.060805 -0.188336 0.008699 0.982066 3.555578\n0.972151 -0.004727 0.234306 0.295541 0.006826 0.999943 -0.008146 -0.089534 -0.234254 0.009519 0.972129 4.499622\n0.960357 -0.008013 0.278657 0.521069 0.011269 0.999886 -0.010085 -0.108208 -0.278544 0.012826 0.960338 5.471632\n0.946922 -0.009907 0.321310 0.783170 0.014022 0.999847 -0.010496 -0.158895 -0.321157 0.014445 0.946916 6.400439\n0.931404 -0.010411 0.363838 1.080749 0.015150 0.999834 -0.010172 -0.179952 -0.363672 0.014986 0.931407 7.311143\n0.913785 -0.011512 0.406035 1.429384 0.016413 0.999828 -0.008589 -0.211103 -0.405867 0.014513 0.913817 8.213135\n0.894089 -0.011867 0.447732 1.799911 0.016689 0.999837 -0.006826 -0.248725 -0.447579 0.013575 0.894142 9.102582\n0.872102 -0.012488 0.489165 2.217261 0.017601 0.999828 -0.005856 -0.271163 -0.489008 0.013717 0.872172 9.963015\n0.848657 -0.013195 0.528779 2.668339 0.018703 0.999812 -0.005069 -0.293909 -0.528613 0.014191 0.848745 10.804238\n0.823435 -0.015415 0.567202 3.161200 0.021574 0.999759 -0.004148 -0.324467 -0.567001 0.015652 0.823568 11.612742\n0.796406 -0.015931 0.604553 3.676780 0.022743 0.999735 -0.003615 -0.343854 -0.604335 0.016628 0.796557 12.391165\n0.768082 -0.019230 0.640063 4.242325 0.026803 0.999638 -0.002130 -0.366302 -0.639790 0.018791 0.768320 13.146576\n0.737872 -0.019295 0.674664 4.841731 0.026789 0.999641 -0.000711 -0.396421 -0.674408 0.018598 0.738124 13.867490\n0.705796 -0.019563 0.708145 5.443512 0.028463 0.999595 -0.000755 -0.406172 -0.707843 0.020689 0.706066 14.549724\n0.672175 -0.021347 0.740084 6.100236 0.032352 0.999476 -0.000554 -0.426567 -0.739685 0.024316 0.672514 15.213237\n0.637295 -0.021394 0.770323 6.774886 0.032917 0.999458 0.000525 -0.453245 -0.769917 0.025022 0.637654 15.843694\n0.601304 -0.021530 0.798730 7.479200 0.035485 0.999370 0.000224 -0.472352 -0.798232 0.028209 0.601690 16.440269\n0.564133 -0.021253 0.825410 8.208643 0.039509 0.999218 -0.001275 -0.484906 -0.824738 0.033331 0.564532 16.981503\n0.526809 -0.021404 0.849714 8.963189 0.041090 0.999155 -0.000307 -0.502150 -0.848990 0.035077 0.527243 17.511761\n0.490131 -0.024030 0.871318 9.771278 0.045170 0.998977 0.002141 -0.510693 -0.870478 0.038308 0.490715 18.003752\n0.454324 -0.025398 0.890475 10.602332 0.048273 0.998827 0.003859 -0.531555 -0.889528 0.041233 0.455017 18.467249\n0.419634 -0.024678 0.907358 11.443807 0.049059 0.998786 0.004476 -0.547521 -0.906367 0.042636 0.420335 18.891607\n0.386842 -0.025730 0.921787 12.293731 0.049422 0.998752 0.007138 -0.557940 -0.920821 0.042795 0.387631 19.304342\n0.354731 -0.027890 0.934552 13.178841 0.051215 0.998634 0.010363 -0.560829 -0.933565 0.044187 0.355675 19.668827\n0.322931 -0.028586 0.945991 14.085793 0.051887 0.998575 0.012463 -0.574472 -0.944999 0.045060 0.323954 20.014784\n0.291603 -0.026711 0.956166 15.009562 0.048763 0.998725 0.013028 -0.589111 -0.955296 0.042827 0.292534 20.350018\n0.261406 -0.025567 0.964890 15.928908 0.048551 0.998732 0.013310 -0.575675 -0.964007 0.043367 0.262316 20.646330\n0.230820 -0.026046 0.972648 16.883053 0.049788 0.998648 0.014927 -0.574483 -0.971722 0.044981 0.231805 20.906183\n0.200231 -0.027371 0.979366 17.849041 0.051000 0.998546 0.017480 -0.577029 -0.978420 0.046447 0.201336 21.137728\n0.170190 -0.028160 0.985009 18.837696 0.049560 0.998571 0.019985 -0.583185 -0.984164 0.045416 0.171342 21.359865\n0.141189 -0.028640 0.989568 19.862761 0.049771 0.998523 0.021798 -0.567868 -0.988731 0.046175 0.142406 21.551311\n0.114144 -0.029029 0.993040 20.908403 0.051980 0.998378 0.023211 -0.548559 -0.992103 0.048969 0.115467 21.693111\n0.089264 -0.029267 0.995578 21.973198 0.055745 0.998148 0.024345 -0.535172 -0.994447 0.053325 0.090730 21.808762\n0.066953 -0.029537 0.997319 23.042116 0.059767 0.997886 0.025542 -0.516358 -0.995964 0.057896 0.068577 21.887133\n0.046637 -0.030750 0.998438 24.150839 0.064667 0.997522 0.027702 -0.502086 -0.996817 0.063274 0.048510 21.942017\n0.028904 -0.031912 0.999073 25.265429 0.068318 0.997216 0.029876 -0.483267 -0.997245 0.067391 0.031003 21.980677\n0.013357 -0.032443 0.999384 26.375792 0.070907 0.996988 0.031418 -0.466273 -0.997393 0.070444 0.015617 22.011549\n-0.000633 -0.033408 0.999442 27.508982 0.072755 0.996792 0.033365 -0.450176 -0.997350 0.072736 0.001800 22.024658\n-0.014034 -0.034554 0.999304 28.655870 0.072751 0.996719 0.035486 -0.431531 -0.997251 0.073198 -0.011475 22.033968\n-0.026035 -0.034489 0.999066 29.793373 0.072616 0.996699 0.036299 -0.411188 -0.997020 0.073493 -0.023445 22.023664\n-0.036798 -0.031814 0.998816 30.938456 0.072701 0.996759 0.034427 -0.382267 -0.996675 0.073882 -0.034366 21.996384\n-0.046679 -0.028688 0.998498 32.105640 0.073258 0.996797 0.032063 -0.357465 -0.996220 0.074645 -0.044428 21.961039\n-0.055572 -0.026750 0.998096 33.295403 0.073714 0.996803 0.030819 -0.331195 -0.995730 0.075286 -0.053422 21.917257\n-0.063072 -0.026000 0.997670 34.513996 0.073238 0.996845 0.030608 -0.304868 -0.995318 0.074998 -0.060969 21.856207\n-0.068726 -0.025574 0.997308 35.766659 0.072879 0.996872 0.030585 -0.272446 -0.994970 0.074784 -0.066648 21.792593\n-0.073851 -0.024664 0.996964 37.035912 0.073347 0.996852 0.030094 -0.242215 -0.994568 0.075347 -0.071810 21.709007\n-0.078022 -0.023268 0.996680 38.317520 0.072689 0.996934 0.028964 -0.213340 -0.994298 0.074707 -0.076091 21.627468\n-0.081501 -0.022797 0.996413 39.625332 0.072772 0.996934 0.028761 -0.186345 -0.994013 0.074855 -0.079592 21.535948\n-0.084702 -0.023616 0.996126 40.963352 0.072223 0.996944 0.029776 -0.159471 -0.993785 0.074465 -0.082737 21.445786\n-0.086992 -0.025947 0.995871 42.325336 0.072597 0.996838 0.032314 -0.124589 -0.993560 0.075109 -0.084834 21.342009\n-0.089280 -0.026665 0.995650 43.702450 0.072903 0.996785 0.033233 -0.089098 -0.993335 0.075553 -0.087049 21.232573\n-0.091154 -0.026417 0.995486 45.081646 0.074397 0.996674 0.033261 -0.053249 -0.993054 0.077093 -0.088885 21.103821\n-0.092286 -0.026094 0.995391 46.486172 0.074891 0.996643 0.033070 -0.012587 -0.992912 0.077598 -0.090022 20.982178\n-0.093096 -0.025554 0.995329 47.936188 0.075844 0.996584 0.032680 0.021961 -0.992764 0.078532 -0.090840 20.853418\n-0.093247 -0.024950 0.995330 49.406193 0.075630 0.996620 0.032068 0.062777 -0.992766 0.078267 -0.091045 20.724943\n-0.092813 -0.024081 0.995392 50.885502 0.074967 0.996701 0.031103 0.094547 -0.992857 0.077508 -0.090702 20.598663\n-0.091381 -0.024078 0.995525 52.381191 0.074680 0.996727 0.030962 0.129410 -0.993012 0.077175 -0.089284 20.474686\n-0.089687 -0.024496 0.995669 53.911186 0.074021 0.996769 0.031190 0.164264 -0.993216 0.076498 -0.087584 20.351086\n-0.088309 -0.023881 0.995807 55.472939 0.074525 0.996752 0.030513 0.198845 -0.993301 0.076907 -0.086243 20.225580\n-0.086737 -0.023148 0.995962 57.041882 0.074503 0.996780 0.029656 0.233687 -0.993441 0.076775 -0.084733 20.088625\n-0.085296 -0.022478 0.996102 58.639210 0.074823 0.996778 0.028901 0.265827 -0.993542 0.076997 -0.083340 19.953283\n-0.083633 -0.022698 0.996238 60.270691 0.074308 0.996815 0.028949 0.300027 -0.993722 0.076450 -0.081680 19.820902\n-0.081835 -0.024225 0.996351 61.929928 0.074637 0.996748 0.030365 0.335598 -0.993847 0.076850 -0.079761 19.682945\n-0.079969 -0.025826 0.996463 63.605106 0.076169 0.996583 0.031942 0.377437 -0.993883 0.078454 -0.077729 19.550365\n-0.078274 -0.024976 0.996619 65.281868 0.077916 0.996475 0.031092 0.413182 -0.993882 0.080087 -0.076052 19.415775\n-0.075730 -0.024127 0.996836 66.962685 0.078690 0.996445 0.030095 0.442290 -0.994019 0.080720 -0.073562 19.284372\n-0.073912 -0.024209 0.996971 68.679337 0.077506 0.996542 0.029945 0.473008 -0.994248 0.079485 -0.071780 19.169977\n-0.072297 -0.024875 0.997073 70.414078 0.076744 0.996586 0.030428 0.502630 -0.994426 0.078719 -0.070141 19.042654\n-0.070947 -0.025376 0.997157 72.168510 0.076081 0.996627 0.030775 0.545774 -0.994574 0.078048 -0.068777 18.923725\n-0.069648 -0.025336 0.997250 73.952126 0.077271 0.996537 0.030714 0.585905 -0.994574 0.079198 -0.067449 18.795158\n-0.069173 -0.025323 0.997283 75.720764 0.078586 0.996433 0.030753 0.625355 -0.994505 0.080500 -0.066936 18.663628\n-0.068597 -0.024672 0.997339 77.495750 0.078789 0.996438 0.030069 0.657591 -0.994528 0.080642 -0.066409 18.545202\n-0.068485 -0.023886 0.997366 79.295815 0.078167 0.996512 0.029233 0.693187 -0.994585 0.079963 -0.066379 18.419369\n-0.069564 -0.023208 0.997307 81.092972 0.078226 0.996524 0.028646 0.722960 -0.994506 0.080008 -0.067507 18.305931\n-0.070234 -0.022830 0.997269 82.902679 0.077463 0.996594 0.028270 0.751575 -0.994518 0.079237 -0.068226 18.186979\n-0.070925 -0.023357 0.997208 84.726822 0.076780 0.996632 0.028804 0.783811 -0.994522 0.078609 -0.068893 18.079119\n-0.071428 -0.023814 0.997161 86.581932 0.076678 0.996625 0.029294 0.814137 -0.994494 0.078553 -0.069361 17.966213\n-0.071881 -0.024457 0.997113 88.442841 0.074681 0.996761 0.029833 0.851933 -0.994613 0.076610 -0.069822 17.855881\n-0.073123 -0.023759 0.997040 90.316582 0.073989 0.996832 0.029180 0.887186 -0.994575 0.075904 -0.071134 17.727997\n-0.074877 -0.022879 0.996930 92.193344 0.072940 0.996933 0.028357 0.925462 -0.994522 0.074839 -0.072979 17.598907\n-0.077855 -0.022332 0.996715 94.081627 0.073039 0.996935 0.028042 0.959985 -0.994286 0.074983 -0.075985 17.472523\n-0.081835 -0.022765 0.996386 95.998047 0.071169 0.997053 0.028625 0.987768 -0.994102 0.073255 -0.079974 17.334602\n-0.085915 -0.023560 0.996024 97.922813 0.068963 0.997182 0.029536 1.023593 -0.993913 0.071226 -0.084048 17.186455\n-0.090499 -0.022901 0.995633 99.851791 0.068127 0.997251 0.029130 1.052806 -0.993564 0.070466 -0.088690 17.037207\n-0.094924 -0.021762 0.995247 101.782066 0.068148 0.997274 0.028307 1.081493 -0.993149 0.070511 -0.093182 16.869471\n-0.099277 -0.020357 0.994852 103.716003 0.068074 0.997309 0.027200 1.106819 -0.992729 0.070424 -0.097624 16.691011\n-0.103448 -0.019893 0.994436 105.683563 0.067369 0.997364 0.026960 1.133395 -0.992351 0.069783 -0.101835 16.520256\n-0.107829 -0.020587 0.993956 107.689720 0.065533 0.997464 0.027769 1.154494 -0.992007 0.068131 -0.106207 16.343342\n-0.112421 -0.020963 0.993440 109.697037 0.063481 0.997584 0.028234 1.188220 -0.991631 0.066238 -0.110818 16.165457\n-0.117342 -0.021680 0.992855 111.697258 0.063424 0.997557 0.029279 1.215664 -0.991064 0.066406 -0.115680 15.955410\n-0.123173 -0.021900 0.992144 113.702446 0.064049 0.997497 0.029970 1.249258 -0.990316 0.067237 -0.121462 15.732436\n-0.129099 -0.020737 0.991415 115.706284 0.065149 0.997444 0.029346 1.276098 -0.989489 0.068378 -0.127418 15.490056\n-0.134477 -0.020036 0.990714 117.709122 0.064654 0.997488 0.028949 1.307992 -0.988805 0.067947 -0.132844 15.243717\n-0.140640 -0.020281 0.989853 119.739410 0.066004 0.997374 0.029813 1.333157 -0.987858 0.069527 -0.138932 14.992049\n-0.146156 -0.020550 0.989048 121.766747 0.066148 0.997344 0.030497 1.360937 -0.987048 0.069880 -0.144409 14.732016\n-0.151217 -0.019943 0.988299 123.809929 0.066038 0.997359 0.030230 1.396804 -0.986292 0.069836 -0.149501 14.442128\n-0.155638 -0.019925 0.987613 125.869957 0.064926 0.997428 0.030354 1.430678 -0.985678 0.068846 -0.153944 14.145097\n-0.160058 -0.020321 0.986898 127.928200 0.066328 0.997307 0.031293 1.471151 -0.984877 0.070467 -0.158280 13.825551\n-0.163865 -0.020949 0.986260 129.983643 0.068431 0.997125 0.032549 1.507634 -0.984106 0.072824 -0.161960 13.492085\n-0.167842 -0.020310 0.985605 132.046356 0.069759 0.997037 0.032425 1.545729 -0.983343 0.074197 -0.165928 13.155616\n-0.171824 -0.020025 0.984924 134.125183 0.069627 0.997046 0.032418 1.586270 -0.982664 0.074148 -0.169922 12.808827\n-0.174613 -0.020376 0.984426 136.202866 0.068780 0.997091 0.032838 1.625052 -0.982232 0.073443 -0.172703 12.460245\n-0.178103 -0.020135 0.983806 138.301910 0.067753 0.997167 0.032674 1.659717 -0.981677 0.072475 -0.176234 12.105251\n-0.181122 -0.020106 0.983255 140.388916 0.066056 0.997284 0.032561 1.698031 -0.981240 0.070848 -0.179302 11.736938\n-0.185070 -0.020138 0.982519 142.489944 0.065598 0.997307 0.032798 1.739746 -0.980534 0.070521 -0.183250 11.366482\n-0.189921 -0.019692 0.981602 144.571915 0.065783 0.997297 0.032734 1.774579 -0.979593 0.070790 -0.188112 10.997115\n-0.194499 -0.019143 0.980716 146.673447 0.065733 0.997308 0.032504 1.812268 -0.978698 0.070788 -0.192717 10.611251\n-0.198535 -0.018995 0.979910 148.783615 0.065616 0.997311 0.032626 1.847034 -0.977895 0.070775 -0.196755 10.209118\n-0.203143 -0.019287 0.978959 150.879822 0.065341 0.997310 0.033207 1.889302 -0.976967 0.070712 -0.201336 9.822110\n-0.207336 -0.019005 0.978085 152.989456 0.065856 0.997272 0.033338 1.931733 -0.976051 0.071325 -0.205518 9.402637\n-0.210695 -0.018724 0.977373 155.115387 0.066830 0.997201 0.033510 1.968503 -0.975265 0.072379 -0.208854 8.969947\n-0.215010 -0.018229 0.976442 157.246170 0.067237 0.997177 0.033421 2.011863 -0.974294 0.072839 -0.213178 8.520555\n-0.218633 -0.016759 0.975663 159.393341 0.067724 0.997181 0.032304 2.049922 -0.973454 0.073138 -0.216882 8.067238\n-0.221728 -0.016243 0.974973 161.559723 0.067608 0.997199 0.031989 2.089502 -0.972762 0.073009 -0.220009 7.607191\n-0.225100 -0.015719 0.974209 163.732651 0.067271 0.997233 0.031634 2.125287 -0.972010 0.072657 -0.223420 7.139631\n-0.228444 -0.015873 0.973428 165.893433 0.067265 0.997220 0.032047 2.164967 -0.971231 0.072799 -0.226741 6.662266\n-0.232256 -0.015744 0.972527 168.083939 0.067100 0.997227 0.032169 2.202081 -0.970337 0.072728 -0.230556 6.174058\n-0.236188 -0.015645 0.971582 170.277008 0.068467 0.997117 0.032700 2.243691 -0.969292 0.074244 -0.234436 5.676510\n-0.239619 -0.015445 0.970744 172.472168 0.070675 0.996943 0.033307 2.283976 -0.968291 0.076588 -0.237795 5.172512\n-0.242270 -0.015877 0.970079 174.680527 0.071071 0.996889 0.034065 2.324440 -0.967602 0.077197 -0.240388 4.656831\n-0.244828 -0.016672 0.969423 176.893784 0.070835 0.996873 0.035034 2.362010 -0.966976 0.077247 -0.242881 4.134908\n-0.247090 -0.016307 0.968855 179.103363 0.069716 0.996968 0.034560 2.403127 -0.966481 0.076084 -0.245204 3.618621\n-0.249928 -0.014712 0.968153 181.315491 0.069072 0.997066 0.032982 2.446918 -0.965798 0.075115 -0.248179 3.080128\n-0.252537 -0.013738 0.967490 183.540985 0.068613 0.997128 0.032068 2.479283 -0.965151 0.074481 -0.250869 2.540424\n-0.254859 -0.012960 0.966891 185.765472 0.067494 0.997233 0.031157 2.521097 -0.964620 0.073200 -0.253279 1.996487\n-0.258033 -0.012020 0.966061 188.007401 0.066582 0.997324 0.030193 2.557479 -0.963839 0.072113 -0.256542 1.445809\n-0.261285 -0.011728 0.965190 190.265106 0.067202 0.997279 0.030310 2.593612 -0.962919 0.072783 -0.259786 0.874740\n-0.264640 -0.012280 0.964269 192.525818 0.067303 0.997246 0.031171 2.630239 -0.961996 0.073147 -0.263085 0.297298\n-0.268147 -0.013035 0.963290 194.787262 0.067080 0.997229 0.032167 2.669102 -0.961040 0.073243 -0.266530 -0.296608\n-0.271460 -0.013456 0.962356 197.053253 0.067078 0.997206 0.032864 2.707716 -0.960109 0.073474 -0.269799 -0.909481\n-0.274803 -0.013242 0.961409 199.311432 0.066554 0.997245 0.032759 2.745358 -0.959194 0.072988 -0.273165 -1.523605\n-0.278221 -0.012551 0.960435 201.568466 0.066126 0.997292 0.032189 2.786202 -0.958238 0.072466 -0.276637 -2.136960\n-0.281117 -0.012313 0.959594 203.842667 0.066079 0.997296 0.032155 2.823550 -0.957396 0.072448 -0.279544 -2.766860\n-0.283399 -0.011713 0.958931 206.102051 0.067485 0.997203 0.032125 2.856730 -0.956625 0.073818 -0.281816 -3.413713\n-0.286102 -0.011998 0.958124 208.363235 0.069149 0.997056 0.033134 2.898169 -0.955701 0.075733 -0.284431 -4.068681\n-0.288294 -0.012558 0.957460 210.622711 0.070856 0.996893 0.034410 2.938800 -0.954917 0.077761 -0.286508 -4.719698\n-0.290384 -0.012141 0.956833 212.890076 0.072047 0.996804 0.034514 2.980613 -0.954194 0.078959 -0.288581 -5.383942\n-0.293136 -0.011801 0.955998 215.158066 0.071963 0.996815 0.034370 3.012804 -0.953359 0.078872 -0.291353 -6.053182\n-0.295455 -0.011436 0.955288 217.430511 0.072154 0.996805 0.034249 3.053942 -0.952628 0.079047 -0.293686 -6.748441\n-0.297828 -0.010887 0.954557 219.719177 0.071917 0.996837 0.033808 3.092393 -0.951907 0.078718 -0.296103 -7.437398\n-0.300638 -0.009948 0.953687 221.987686 0.071907 0.996863 0.033066 3.128067 -0.951024 0.078518 -0.298979 -8.124326\n-0.303108 -0.009714 0.952907 224.268372 0.070624 0.996969 0.032628 3.161444 -0.950336 0.077188 -0.301503 -8.804060\n-0.305085 -0.009270 0.952280 226.539276 0.071030 0.996946 0.032461 3.197967 -0.949673 0.077544 -0.303494 -9.513762\n-0.307993 -0.009849 0.951338 228.833725 0.072579 0.996789 0.033817 3.230881 -0.948616 0.079463 -0.306289 -10.234091\n-0.310479 -0.011190 0.950514 231.102371 0.072879 0.996707 0.035539 3.269298 -0.947782 0.080306 -0.308641 -10.948189\n-0.312934 -0.011830 0.949701 233.390121 0.072115 0.996740 0.036179 3.312506 -0.947033 0.079810 -0.311061 -11.666549\n-0.315452 -0.011820 0.948868 235.688217 0.071029 0.996823 0.036032 3.351547 -0.946280 0.078764 -0.313610 -12.398398\n-0.317472 -0.012348 0.948187 237.985687 0.069659 0.996910 0.036305 3.397362 -0.945706 0.077576 -0.315631 -13.140648\n-0.320385 -0.013144 0.947196 240.277267 0.068505 0.996964 0.037007 3.444320 -0.944807 0.076744 -0.318512 -13.872763\n-0.323951 -0.013195 0.945982 242.544418 0.067911 0.996999 0.037162 3.489276 -0.943633 0.076282 -0.322083 -14.622202\n-0.326743 -0.013902 0.945011 244.847992 0.066949 0.997039 0.037816 3.538589 -0.942739 0.075624 -0.324845 -15.372395\n-0.330559 -0.014357 0.943676 247.147858 0.066280 0.997062 0.038386 3.583525 -0.941455 0.075236 -0.328636 -16.120741\n-0.334055 -0.014583 0.942441 249.416107 0.065364 0.997115 0.038598 3.636127 -0.940285 0.074495 -0.332138 -16.881163\n-0.337650 -0.014338 0.941162 251.689804 0.065177 0.997128 0.038573 3.685555 -0.939012 0.074366 -0.335746 -17.656031\n-0.341008 -0.014341 0.939951 253.965805 0.065861 0.997062 0.039106 3.731454 -0.937750 0.075242 -0.339062 -18.451300\n-0.344626 -0.014543 0.938627 256.228577 0.065756 0.997050 0.039591 3.778969 -0.936434 0.075365 -0.342653 -19.234646\n-0.348098 -0.014402 0.937348 258.506042 0.064922 0.997111 0.039430 3.829770 -0.935207 0.074580 -0.346157 -20.042994\n-0.351746 -0.014387 0.935985 260.768982 0.065144 0.997082 0.039807 3.891871 -0.933826 0.074976 -0.349782 -20.851324\n-0.355310 -0.014070 0.934643 263.039764 0.065760 0.997033 0.040008 3.952028 -0.932433 0.075677 -0.353330 -21.667585\n-0.358891 -0.013326 0.933284 265.287170 0.066975 0.996953 0.039990 4.005448 -0.930973 0.076859 -0.356905 -22.506577\n-0.363152 -0.012172 0.931650 267.518188 0.068704 0.996843 0.039804 4.061740 -0.929193 0.078463 -0.361169 -23.345287\n-0.366775 -0.011774 0.930235 269.765381 0.069690 0.996763 0.040094 4.120373 -0.927696 0.079533 -0.364767 -24.205177\n-0.370382 -0.010551 0.928820 272.014130 0.071112 0.996679 0.039679 4.182892 -0.926153 0.080747 -0.368402 -25.084528\n-0.374099 -0.008952 0.927345 274.248566 0.072974 0.996569 0.039058 4.237538 -0.924513 0.082284 -0.372162 -25.968128\n-0.377942 -0.008694 0.925789 276.504608 0.073800 0.996491 0.039486 4.300721 -0.922883 0.083247 -0.375974 -26.863062\n-0.382118 -0.008583 0.924074 278.759918 0.073482 0.996508 0.039641 4.363300 -0.921187 0.083050 -0.380153 -27.756622\n-0.386051 -0.008809 0.922435 281.035492 0.072490 0.996573 0.039855 4.434304 -0.919625 0.082253 -0.384089 -28.662882\n-0.390375 -0.007859 0.920622 283.317078 0.072957 0.996555 0.039444 4.506722 -0.917761 0.082564 -0.388457 -29.596706\n-0.393856 -0.007974 0.919138 285.612549 0.071968 0.996625 0.039485 4.572362 -0.916350 0.081700 -0.391953 -30.556290\n-0.397310 -0.008321 0.917647 287.893890 0.070319 0.996743 0.039484 4.644245 -0.914986 0.080216 -0.395431 -31.510340\n-0.399967 -0.008038 0.916494 290.197906 0.071324 0.996656 0.039868 4.719253 -0.913750 0.081314 -0.398057 -32.503883\n-0.402619 -0.008235 0.915330 292.477325 0.072360 0.996544 0.040794 4.791990 -0.912503 0.082657 -0.400632 -33.505753\n-0.405098 -0.008427 0.914234 294.774078 0.073148 0.996453 0.041597 4.860313 -0.911343 0.083725 -0.403045 -34.493065\n-0.407480 -0.009045 0.913169 297.085480 0.072902 0.996437 0.042401 4.931063 -0.910299 0.083850 -0.405369 -35.490425\n-0.410525 -0.009750 0.911797 299.389496 0.072446 0.996433 0.043273 5.010506 -0.908967 0.083820 -0.408354 -36.502899\n-0.413292 -0.010088 0.910543 301.684875 0.072902 0.996362 0.044128 5.093233 -0.907676 0.084618 -0.411053 -37.530735\n-0.415404 -0.010875 0.909572 304.010956 0.073087 0.996297 0.045291 5.170431 -0.906696 0.085292 -0.413071 -38.569687\n-0.418006 -0.010457 0.908384 306.324432 0.072909 0.996322 0.045019 5.250002 -0.905514 0.085047 -0.415706 -39.608696\n-0.420474 -0.008978 0.907260 308.650482 0.071778 0.996488 0.043127 5.326487 -0.904461 0.083255 -0.418353 -40.669987\n-0.423586 -0.009828 0.905802 311.008759 0.069882 0.996607 0.043492 5.401456 -0.903156 0.081722 -0.421462 -41.725845\n-0.427793 -0.012187 0.903795 313.380890 0.067121 0.996720 0.045211 5.478120 -0.901381 0.080005 -0.425572 -42.774033\n-0.432721 -0.014451 0.901412 315.752838 0.064440 0.996818 0.046915 5.564133 -0.899222 0.078388 -0.430413 -43.852379\n-0.437347 -0.015588 0.899158 318.122925 0.063138 0.996850 0.047992 5.661900 -0.897074 0.077760 -0.434985 -44.955196\n-0.441332 -0.015931 0.897203 320.488861 0.062996 0.996825 0.048688 5.755820 -0.895130 0.078008 -0.438927 -46.062458\n-0.445410 -0.015051 0.895200 322.842438 0.064337 0.996736 0.048769 5.852245 -0.893012 0.079316 -0.442988 -47.208370\n-0.449326 -0.014534 0.893250 325.208038 0.065191 0.996669 0.049010 5.944547 -0.890986 0.080253 -0.446882 -48.367218\n-0.453099 -0.015366 0.891328 327.562012 0.065274 0.996596 0.050363 6.034824 -0.889067 0.081000 -0.450554 -49.545658\n-0.457436 -0.016806 0.889084 329.897369 0.064700 0.996542 0.052126 6.129167 -0.886886 0.081368 -0.454767 -50.722008\n-0.460966 -0.018765 0.887220 332.251495 0.063358 0.996529 0.053996 6.219017 -0.885153 0.081103 -0.458177 -51.912186\n-0.464663 -0.019478 0.885273 334.599518 0.062351 0.996557 0.054654 6.316256 -0.883290 0.080593 -0.461849 -53.118134\n-0.468580 -0.020225 0.883190 336.950775 0.061595 0.996557 0.055500 6.419256 -0.881271 0.080406 -0.465721 -54.322140\n-0.472618 -0.020508 0.881029 339.302734 0.062375 0.996443 0.056655 6.523853 -0.879057 0.081731 -0.469658 -55.542587\n-0.476123 -0.021429 0.879117 341.630371 0.062717 0.996330 0.058254 6.625786 -0.877139 0.082872 -0.473032 -56.779282\n-0.480167 -0.022751 0.876882 343.961639 0.061616 0.996319 0.059590 6.732362 -0.875010 0.082644 -0.476998 -57.990562\n-0.483513 -0.023576 0.875020 346.267456 0.060920 0.996307 0.060507 6.848070 -0.873215 0.082562 -0.480291 -59.228130\n-0.487066 -0.024658 0.873017 348.558929 0.060875 0.996212 0.062100 6.963513 -0.871241 0.083392 -0.483720 -60.480438\n-0.489916 -0.025660 0.871392 350.857452 0.058867 0.996311 0.062435 7.076973 -0.869780 0.081884 -0.486598 -61.737289\n-0.493298 -0.026172 0.869467 353.145752 0.055878 0.996529 0.061699 7.199569 -0.868064 0.079020 -0.490123 -62.995148\n-0.496905 -0.026673 0.867395 355.464081 0.052118 0.996806 0.060510 7.325261 -0.866239 0.075275 -0.493928 -64.265823\n-0.500299 -0.029563 0.865348 357.791626 0.048726 0.996872 0.062227 7.440177 -0.864481 0.073297 -0.497293 -65.564362\n-0.503698 -0.030773 0.863332 360.095886 0.048457 0.996785 0.063802 7.567661 -0.862520 0.073972 -0.500587 -66.873642\n-0.506622 -0.031231 0.861602 362.389282 0.047456 0.996819 0.064037 7.688374 -0.860861 0.073331 -0.503529 -68.196167\n-0.509854 -0.032921 0.859631 364.678802 0.044990 0.996880 0.064861 7.810418 -0.859083 0.071745 -0.506782 -69.514809\n-0.512763 -0.036994 0.857733 366.975372 0.041569 0.996830 0.067843 7.936289 -0.857523 0.070442 -0.509599 -70.839233\n-0.516168 -0.039545 0.855574 369.265869 0.038986 0.996813 0.069594 8.066970 -0.855600 0.069277 -0.512981 -72.181015\n-0.519057 -0.040422 0.853783 371.570404 0.037153 0.996870 0.069783 8.213427 -0.853932 0.067942 -0.515931 -73.535172\n-0.522147 -0.040705 0.851883 373.865234 0.036130 0.996908 0.069780 8.350028 -0.852090 0.067214 -0.519062 -74.904778\n-0.525698 -0.041673 0.849650 376.145203 0.035384 0.996864 0.070786 8.483373 -0.849935 0.067276 -0.522575 -76.265984\n-0.529375 -0.042880 0.847304 378.412231 0.034260 0.996827 0.071852 8.624732 -0.847696 0.067065 -0.526226 -77.643295\n-0.532422 -0.044445 0.845311 380.667511 0.031489 0.996890 0.072248 8.752752 -0.845893 0.065084 -0.529367 -79.021156\n-0.534906 -0.046764 0.843616 382.885468 0.023194 0.997278 0.069988 8.882895 -0.844593 0.057004 -0.532366 -80.386414\n-0.537382 -0.044305 0.842174 385.073761 0.014081 0.998008 0.061488 9.026137 -0.843221 0.044901 -0.535688 -81.716873\n-0.540935 -0.035073 0.840333 387.234833 0.021768 0.998212 0.055675 9.146310 -0.840783 0.048409 -0.539204 -83.097137\n-0.542148 -0.034342 0.839581 389.433563 0.029201 0.997791 0.059670 9.247426 -0.839775 0.056866 -0.539948 -84.534531\n-0.543840 -0.044206 0.838024 391.689514 0.028077 0.997094 0.070817 9.369326 -0.838719 0.062042 -0.541019 -85.983994\n-0.546174 -0.054831 0.835875 393.945312 0.022603 0.996527 0.080139 9.522486 -0.837367 0.062663 -0.543038 -87.421524\n-0.548924 -0.060865 0.833653 396.200073 0.016459 0.996365 0.083582 9.695453 -0.835710 0.059601 -0.545927 -88.853172\n-0.551762 -0.061664 0.831719 398.438446 0.014642 0.996393 0.083586 9.877352 -0.833873 0.058298 -0.548868 -90.301193\n-0.555079 -0.057216 0.829827 400.657379 0.019561 0.996458 0.081789 10.052266 -0.831567 0.061632 -0.551994 -91.767403\n-0.558256 -0.051536 0.828066 402.848053 0.024228 0.996631 0.078361 10.204973 -0.829315 0.063808 -0.555127 -93.223801\n-0.560694 -0.048426 0.826606 405.059845 0.025955 0.996770 0.076001 10.352926 -0.827616 0.064068 -0.557626 -94.692505\n-0.563692 -0.049128 0.824523 407.284912 0.025546 0.996715 0.076853 10.506447 -0.825590 0.064385 -0.560586 -96.171394\n-0.566890 -0.049901 0.822281 409.490448 0.026067 0.996577 0.078449 10.668918 -0.823381 0.065907 -0.563649 -97.655762\n-0.569385 -0.050634 0.820510 411.676941 0.026630 0.996441 0.079970 10.832727 -0.821640 0.067384 -0.566010 -99.143669\n-0.571267 -0.051089 0.819173 413.878235 0.026980 0.996353 0.080955 10.996205 -0.820321 0.068348 -0.567805 -100.646362\n-0.572815 -0.051455 0.818068 416.084656 0.026815 0.996317 0.081443 11.162690 -0.819246 0.068588 -0.569326 -102.161446\n-0.574072 -0.051758 0.817167 418.278168 0.027203 0.996244 0.082211 11.333157 -0.818353 0.069424 -0.570508 -103.682289\n-0.575660 -0.052576 0.815997 420.492584 0.028002 0.996078 0.083934 11.511806 -0.817210 0.071167 -0.571930 -105.212105\n-0.576931 -0.053801 0.815019 422.696960 0.029832 0.995775 0.086851 11.685916 -0.816248 0.074421 -0.572888 -106.748573\n-0.578602 -0.055126 0.813745 424.895477 0.030886 0.995517 0.089401 11.869704 -0.815025 0.076861 -0.574305 -108.285889\n-0.579875 -0.055284 0.812827 427.068176 0.029970 0.995572 0.089094 12.043920 -0.814154 0.076024 -0.575651 -109.797859\n-0.581312 -0.055292 0.811800 429.261078 0.026880 0.995839 0.087076 12.225813 -0.813237 0.072439 -0.577407 -111.331100\n-0.582758 -0.055534 0.810746 431.454742 0.024189 0.996035 0.085613 12.409235 -0.812286 0.069503 -0.579104 -112.857437\n-0.584085 -0.056255 0.809741 433.648529 0.023535 0.996002 0.086171 12.595183 -0.811351 0.069388 -0.580426 -114.405983\n-0.585084 -0.057357 0.808942 435.823914 0.021995 0.996006 0.086529 12.770849 -0.810674 0.068419 -0.581486 -115.941193\n-0.586017 -0.057931 0.808225 437.999023 0.019931 0.996109 0.085850 12.949258 -0.810053 0.066418 -0.582582 -117.471062\n-0.586646 -0.058707 0.807713 440.181885 0.019115 0.996087 0.086282 13.130798 -0.809618 0.066057 -0.583229 -119.022087\n-0.587080 -0.059966 0.807305 442.329987 0.019482 0.995917 0.088144 13.314708 -0.809294 0.067476 -0.583515 -120.569382\n-0.587238 -0.061514 0.807073 444.479218 0.021264 0.995591 0.091355 13.503747 -0.809135 0.070809 -0.583341 -122.118866\n-0.586991 -0.063393 0.807108 446.648529 0.025343 0.995002 0.096583 13.701235 -0.809197 0.077148 -0.582451 -123.688347\n-0.585730 -0.065448 0.807860 448.819458 0.028890 0.994415 0.101507 13.904055 -0.809991 0.082795 -0.580568 -125.266945\n-0.584113 -0.066917 0.808909 450.973724 0.026213 0.994521 0.101200 14.111997 -0.811249 0.080316 -0.579159 -126.795135\n-0.582412 -0.067050 0.810124 453.127838 0.020140 0.995096 0.096838 14.322731 -0.812644 0.072715 -0.578206 -128.311844\n-0.580622 -0.067989 0.811329 455.299530 0.015418 0.995410 0.094449 14.541880 -0.814027 0.067348 -0.576909 -129.826431\n-0.578051 -0.068617 0.813110 457.477875 0.013770 0.995496 0.093797 14.742216 -0.815884 0.065416 -0.574503 -131.354477\n-0.575107 -0.068796 0.815180 459.646484 0.014742 0.995424 0.094408 14.949389 -0.817945 0.066312 -0.571462 -132.880859\n-0.572115 -0.069107 0.817256 461.829102 0.015841 0.995327 0.095254 15.156524 -0.820020 0.067443 -0.568347 -134.405396\n-0.569522 -0.068892 0.819084 464.012177 0.013752 0.995544 0.093295 15.346860 -0.821861 0.064398 -0.566036 -135.902893\n-0.567426 -0.072830 0.820198 466.200256 0.007440 0.995587 0.093550 15.542682 -0.823391 0.059185 -0.564380 -137.390854\n-0.565776 -0.077891 0.820872 468.397827 0.001201 0.995449 0.095284 15.745875 -0.824558 0.054895 -0.563108 -138.855713\n-0.563974 -0.081227 0.821788 470.601593 -0.006030 0.995529 0.094262 15.947616 -0.825771 0.048206 -0.561942 -140.313110\n-0.563062 -0.081531 0.822383 472.804688 -0.007332 0.995575 0.093682 16.157331 -0.826382 0.046719 -0.561168 -141.789154\n-0.561827 -0.082096 0.823171 474.997070 -0.006167 0.995452 0.095069 16.361141 -0.827232 0.048336 -0.559778 -143.275314\n-0.560354 -0.084003 0.823982 477.196899 -0.004843 0.995159 0.098160 16.558285 -0.828239 0.051014 -0.558048 -144.770889\n-0.558556 -0.085784 0.825019 479.398193 -0.004946 0.994965 0.100106 16.771872 -0.829452 0.051834 -0.556167 -146.243881\n-0.557109 -0.086595 0.825912 481.585114 -0.006404 0.994967 0.100000 16.981918 -0.830415 0.050422 -0.554860 -147.694122\n-0.555164 -0.086438 0.827237 483.792358 -0.007824 0.995084 0.098726 17.197369 -0.831704 0.048337 -0.553111 -149.154434\n-0.553510 -0.085997 0.828391 486.008759 -0.007486 0.995128 0.098305 17.404997 -0.832809 0.048211 -0.551457 -150.616074\n-0.552246 -0.085975 0.829236 488.207611 -0.005637 0.995031 0.099411 17.613371 -0.833662 0.050225 -0.549986 -152.068787\n-0.550675 -0.086532 0.830223 490.423981 -0.003833 0.994864 0.101150 17.827141 -0.834711 0.052519 -0.548178 -153.530518\n-0.549132 -0.087386 0.831155 492.635742 -0.003561 0.994754 0.102234 18.042688 -0.835728 0.053180 -0.546562 -154.980942\n-0.548061 -0.088552 0.831738 494.857574 -0.004147 0.994656 0.103164 18.262770 -0.836428 0.053091 -0.545499 -156.415329\n-0.546680 -0.089835 0.832509 497.073273 -0.004277 0.994515 0.104509 18.488354 -0.837331 0.053572 -0.544065 -157.858841\n-0.545556 -0.091637 0.833049 499.294159 -0.004721 0.994324 0.106286 18.715555 -0.838061 0.054052 -0.542893 -159.287445\n-0.544629 -0.091679 0.833651 501.515747 -0.005528 0.994378 0.105743 18.943451 -0.838659 0.052982 -0.542074 -160.707336\n-0.543717 -0.091404 0.834276 503.721588 -0.005523 0.994420 0.105350 19.174475 -0.839251 0.052673 -0.541188 -162.124680\n-0.543120 -0.092269 0.834570 505.942932 -0.005163 0.994292 0.106568 19.406729 -0.839640 0.053570 -0.540496 -163.543411\n-0.542160 -0.093746 0.835030 508.163269 -0.005723 0.994146 0.107894 19.641050 -0.840256 0.053717 -0.539522 -164.960083\n-0.541916 -0.095095 0.835035 510.376892 -0.006575 0.994027 0.108934 19.880116 -0.840407 0.053543 -0.539304 -166.366165\n-0.542095 -0.096275 0.834784 512.587097 -0.007455 0.993927 0.109788 20.120552 -0.840284 0.053293 -0.539521 -167.766174\n-0.542381 -0.096685 0.834550 514.797180 -0.007695 0.993886 0.110143 20.363249 -0.840097 0.053318 -0.539809 -169.171234\n-0.542764 -0.097252 0.834236 517.016602 -0.007598 0.993801 0.110909 20.601133 -0.839851 0.053859 -0.540139 -170.587448\n-0.542807 -0.098771 0.834029 519.226440 -0.007637 0.993600 0.112698 20.848722 -0.839823 0.054804 -0.540087 -172.002197\n-0.543182 -0.100806 0.833541 521.437622 -0.008455 0.993373 0.114626 21.098757 -0.839572 0.055215 -0.540435 -173.417709\n-0.543178 -0.102104 0.833386 523.643250 -0.009287 0.993248 0.115637 21.346056 -0.839566 0.055072 -0.540459 -174.827560\n-0.543276 -0.102742 0.833244 525.860596 -0.010075 0.993210 0.115898 21.602589 -0.839494 0.054569 -0.540622 -176.245331\n-0.543564 -0.102841 0.833044 528.066528 -0.010510 0.993222 0.115758 21.859982 -0.839302 0.054167 -0.540960 -177.663528\n-0.543440 -0.103128 0.833089 530.267517 -0.010053 0.993153 0.116385 22.118299 -0.839388 0.054873 -0.540756 -179.080643\n-0.543213 -0.104080 0.833119 532.486938 -0.010185 0.993031 0.117416 22.376307 -0.839533 0.055296 -0.540487 -180.505508\n-0.542969 -0.104544 0.833220 534.703857 -0.010301 0.992975 0.117876 22.638628 -0.839690 0.055420 -0.540231 -181.912811\n-0.542968 -0.105057 0.833156 536.912109 -0.010746 0.992932 0.118201 22.899984 -0.839684 0.055226 -0.540259 -183.323059\n-0.543148 -0.106011 0.832918 539.124573 -0.011054 0.992814 0.119154 23.168898 -0.839564 0.055512 -0.540417 -184.737137\n-0.543269 -0.107910 0.832595 541.319275 -0.011571 0.992574 0.121094 23.430046 -0.839479 0.056152 -0.540483 -186.136414\n-0.543267 -0.109657 0.832368 543.525208 -0.012085 0.992352 0.122846 23.702026 -0.839473 0.056679 -0.540437 -187.550659\n-0.542910 -0.110011 0.832554 545.736511 -0.011950 0.992294 0.123326 23.974014 -0.839706 0.057006 -0.540041 -188.969147\n-0.542626 -0.109720 0.832778 547.957458 -0.011508 0.992310 0.123241 24.251869 -0.839896 0.057290 -0.539716 -190.389023\n-0.542537 -0.110530 0.832729 550.160400 -0.011636 0.992199 0.124116 24.528786 -0.839951 0.057648 -0.539591 -191.799316\n-0.542481 -0.111801 0.832595 552.364319 -0.012200 0.992049 0.125263 24.804436 -0.839980 0.057795 -0.539531 -193.215103\n-0.542193 -0.112437 0.832697 554.565674 -0.012883 0.992002 0.125560 25.087660 -0.840155 0.057350 -0.539305 -194.624542\n-0.542374 -0.112745 0.832538 556.776489 -0.013269 0.991980 0.125694 25.366846 -0.840032 0.057126 -0.539520 -196.034958\n-0.542894 -0.114139 0.832009 558.984314 -0.013785 0.991799 0.127065 25.653145 -0.839688 0.057513 -0.540015 -197.450302\n-0.542870 -0.116013 0.831765 561.186951 -0.014167 0.991537 0.129051 25.940798 -0.839698 0.058274 -0.539919 -198.859940\n-0.543284 -0.117371 0.831304 563.384949 -0.014783 0.991363 0.130309 26.232157 -0.839419 0.058506 -0.540327 -200.271194\n-0.543792 -0.116826 0.831049 565.580750 -0.015315 0.991480 0.129358 26.523111 -0.839080 0.057616 -0.540948 -201.678055\n-0.544253 -0.115352 0.830953 567.771301 -0.015254 0.991699 0.127676 26.814127 -0.838782 0.056813 -0.541494 -203.083313\n-0.544199 -0.115737 0.830935 569.976379 -0.015389 0.991649 0.128044 27.095510 -0.838815 0.056894 -0.541435 -204.498489\n-0.544321 -0.117367 0.830626 572.182434 -0.015194 0.991381 0.130124 27.394535 -0.838739 0.058208 -0.541413 -205.917511\n-0.544505 -0.118874 0.830291 574.411865 -0.014713 0.991107 0.132249 27.695572 -0.838628 0.059794 -0.541412 -207.349991\n-0.544797 -0.118997 0.830082 576.611328 -0.014063 0.991037 0.132842 27.992086 -0.838450 0.060699 -0.541587 -208.767181\n-0.544736 -0.118771 0.830154 578.819946 -0.014211 0.991085 0.132471 28.295456 -0.838487 0.060365 -0.541568 -210.198669\n-0.545057 -0.119040 0.829905 581.008362 -0.014303 0.991045 0.132760 28.594004 -0.838277 0.060492 -0.541879 -211.610413\n-0.545167 -0.119478 0.829770 583.213623 -0.013797 0.990937 0.133619 28.899984 -0.838214 0.061397 -0.541874 -213.039139\n-0.544993 -0.120520 0.829734 585.411499 -0.013317 0.990734 0.135159 29.204035 -0.838335 0.062611 -0.541548 -214.468552\n-0.544773 -0.121870 0.829680 587.628113 -0.013424 0.990524 0.136682 29.509624 -0.838476 0.063323 -0.541247 -215.895569\n-0.544786 -0.122340 0.829603 589.839844 -0.013314 0.990438 0.137315 29.824747 -0.838469 0.063762 -0.541206 -217.320557\n-0.544783 -0.122621 0.829564 592.058655 -0.012626 0.990339 0.138094 30.137577 -0.838482 0.064757 -0.541067 -218.748138\n-0.544652 -0.123961 0.829450 594.256042 -0.012701 0.990122 0.139633 30.450804 -0.838566 0.065517 -0.540847 -220.169174\n-0.544308 -0.125752 0.829407 596.452332 -0.013714 0.989902 0.141085 30.764732 -0.838773 0.065419 -0.540536 -221.578873\n-0.543943 -0.127752 0.829340 598.669250 -0.015321 0.989690 0.142404 31.088247 -0.838982 0.064754 -0.540292 -223.001724\n-0.543214 -0.127416 0.829870 600.884583 -0.015392 0.989763 0.141890 31.414894 -0.839453 0.064304 -0.539614 -224.425598\n-0.542463 -0.126662 0.830476 603.118835 -0.014393 0.989824 0.141564 31.744261 -0.839956 0.064841 -0.538766 -225.864014\n-0.541644 -0.126770 0.830994 605.327759 -0.013657 0.989760 0.142089 32.060635 -0.840497 0.065613 -0.537828 -227.285675\n-0.541120 -0.128172 0.831120 607.526001 -0.013786 0.989536 0.143627 32.380943 -0.840832 0.066261 -0.537225 -228.695267\n-0.540664 -0.129673 0.831184 609.706116 -0.013760 0.989279 0.145387 32.707985 -0.841126 0.067168 -0.536652 -230.087753\n-0.540378 -0.129889 0.831337 611.910217 -0.014061 0.989269 0.145425 33.033325 -0.841305 0.066895 -0.536405 -231.493149\n-0.539544 -0.130482 0.831786 614.108032 -0.014713 0.989229 0.145637 33.364674 -0.841829 0.066340 -0.535652 -232.895569\n-0.538867 -0.131292 0.832096 616.308716 -0.015193 0.989134 0.146231 33.699574 -0.842254 0.066157 -0.535007 -234.298798\n-0.538064 -0.132538 0.832418 618.518738 -0.015733 0.988968 0.147294 34.035320 -0.842757 0.066157 -0.534214 -235.699814\n-0.537367 -0.133310 0.832745 620.721558 -0.016466 0.988898 0.147683 34.373211 -0.843188 0.065648 -0.533596 -237.102478\n-0.536734 -0.134569 0.832951 622.920959 -0.017607 0.988771 0.148398 34.708866 -0.843568 0.064984 -0.533076 -238.491821\n-0.536472 -0.135467 0.832974 625.129150 -0.018921 0.988715 0.148609 35.050667 -0.843706 0.063964 -0.532981 -239.885361\n-0.536199 -0.135349 0.833170 627.330688 -0.019657 0.988795 0.147980 35.391907 -0.843863 0.062969 -0.532851 -241.269089\n-0.536470 -0.135851 0.832913 629.552307 -0.019572 0.988696 0.148653 35.737656 -0.843692 0.063446 -0.533064 -242.673477\n-0.536931 -0.137889 0.832281 631.752197 -0.020274 0.988376 0.150670 36.082096 -0.843383 0.064025 -0.533485 -244.068756\n-0.537243 -0.139537 0.831805 633.951599 -0.021502 0.988165 0.151879 36.427647 -0.843154 0.063711 -0.533885 -245.455582\n-0.537557 -0.139503 0.831607 636.163574 -0.021936 0.988200 0.151592 36.776718 -0.842942 0.063248 -0.534274 -246.858719\n-0.537849 -0.138285 0.831622 638.364868 -0.021896 0.988414 0.150196 37.128220 -0.842757 0.062573 -0.534645 -248.260025\n-0.537619 -0.137804 0.831851 640.569092 -0.021983 0.988510 0.149549 37.473366 -0.842901 0.062113 -0.534471 -249.661713\n-0.537423 -0.138541 0.831855 642.775696 -0.022813 0.988441 0.149881 37.813671 -0.843004 0.061572 -0.534372 -251.054138\n-0.537015 -0.139997 0.831875 644.962830 -0.022734 0.988177 0.151626 38.155781 -0.843266 0.062514 -0.533848 -252.442459\n-0.536767 -0.141006 0.831864 647.169922 -0.022475 0.987976 0.152966 38.509979 -0.843431 0.063411 -0.533482 -253.838470\n-0.536429 -0.142074 0.831901 649.374023 -0.022969 0.987821 0.153891 38.861023 -0.843633 0.063444 -0.533159 -255.234756\n-0.535993 -0.142477 0.832113 651.597717 -0.023372 0.987782 0.154077 39.212757 -0.843899 0.063136 -0.532774 -256.634979\n-0.535653 -0.142743 0.832286 653.811096 -0.023677 0.987761 0.154170 39.569889 -0.844106 0.062875 -0.532476 -258.026367\n-0.534836 -0.143692 0.832649 656.019958 -0.024061 0.987624 0.154981 39.927933 -0.844613 0.062855 -0.531674 -259.425598\n-0.534330 -0.144456 0.832841 658.210632 -0.024125 0.987494 0.155803 40.278175 -0.844932 0.063158 -0.531132 -260.801605\n-0.533807 -0.145068 0.833070 660.383728 -0.023977 0.987375 0.156575 40.633060 -0.845266 0.063606 -0.530546 -262.166931\n-0.532982 -0.145909 0.833451 662.565613 -0.024727 0.987285 0.157027 40.988400 -0.845765 0.063083 -0.529813 -263.547272\n-0.532230 -0.145781 0.833954 664.779663 -0.024756 0.987321 0.156791 41.345932 -0.846238 0.062803 -0.529091 -264.935364\n-0.531053 -0.146478 0.834582 666.981995 -0.025040 0.987228 0.157336 41.708076 -0.846969 0.062655 -0.527938 -266.317749\n-0.530144 -0.147626 0.834958 669.187439 -0.024417 0.986977 0.159000 42.067844 -0.847556 0.063906 -0.526844 -267.687012\n-0.529532 -0.148428 0.835203 671.398254 -0.024263 0.986821 0.159989 42.436260 -0.847943 0.064455 -0.526155 -269.067719\n-0.528773 -0.148393 0.835690 673.619934 -0.024207 0.986834 0.159915 42.804394 -0.848418 0.064329 -0.525404 -270.439331\n-0.528312 -0.147755 0.836095 675.827637 -0.023863 0.986936 0.159333 43.167175 -0.848715 0.064226 -0.524937 -271.805176\n-0.527427 -0.148987 0.836435 678.033447 -0.024194 0.986739 0.160504 43.529499 -0.849256 0.064417 -0.524037 -273.170105\n-0.526787 -0.150455 0.836576 680.254639 -0.024230 0.986468 0.162155 43.898544 -0.849652 0.065150 -0.523304 -274.543427\n-0.526134 -0.151728 0.836756 682.464111 -0.024329 0.986237 0.163536 44.270557 -0.850053 0.065684 -0.522585 -275.904755\n-0.525587 -0.152770 0.836911 684.669128 -0.024276 0.986037 0.164746 44.646786 -0.850393 0.066271 -0.521957 -277.255524\n-0.524746 -0.153662 0.837275 686.893677 -0.024438 0.985887 0.165620 45.028851 -0.850908 0.066447 -0.521095 -278.623688\n-0.524201 -0.153537 0.837639 689.080566 -0.024003 0.985886 0.165689 45.410458 -0.851256 0.066749 -0.520488 -279.960205\n-0.523467 -0.153515 0.838102 691.282471 -0.024413 0.985933 0.165345 45.788391 -0.851696 0.066092 -0.519851 -281.301453\n-0.523091 -0.153641 0.838314 693.476257 -0.024282 0.985904 0.165539 46.166973 -0.851931 0.066236 -0.519449 -282.638000\n-0.522566 -0.154346 0.838512 695.700684 -0.024794 0.985813 0.166009 46.553566 -0.852238 0.065961 -0.518979 -283.998993\n-0.522540 -0.155564 0.838303 697.916321 -0.025306 0.985611 0.167125 46.940342 -0.852239 0.066115 -0.518958 -285.341400\n-0.522505 -0.156675 0.838118 700.129822 -0.026478 0.985480 0.167715 47.325832 -0.852225 0.065441 -0.519066 -286.691895\n-0.522895 -0.157939 0.837637 702.351013 -0.028031 0.985339 0.168290 47.720436 -0.851936 0.064518 -0.519656 -288.035675\n-0.523354 -0.159088 0.837133 704.573669 -0.029608 0.985219 0.168720 48.113819 -0.851601 0.063514 -0.520329 -289.380920\n-0.523621 -0.159830 0.836825 706.780884 -0.030530 0.985134 0.169053 48.504124 -0.851404 0.062971 -0.520716 -290.733429\n-0.523547 -0.160814 0.836682 709.007385 -0.030979 0.984969 0.169931 48.905117 -0.851433 0.063047 -0.520659 -292.098633\n-0.523839 -0.161939 0.836282 711.223816 -0.030970 0.984734 0.171286 49.298889 -0.851254 0.063826 -0.520858 -293.457489\n-0.523944 -0.162242 0.836158 713.420349 -0.029982 0.984596 0.172256 49.695412 -0.851225 0.065183 -0.520738 -294.804779\n-0.523559 -0.163006 0.836250 715.630859 -0.030207 0.984461 0.172984 50.098133 -0.851454 0.065307 -0.520348 -296.163086\n-0.523068 -0.163034 0.836553 717.821106 -0.030497 0.984486 0.172795 50.498482 -0.851745 0.064872 -0.519925 -297.503662\n-0.522531 -0.162585 0.836976 720.014709 -0.030247 0.984568 0.172371 50.898582 -0.852084 0.064754 -0.519384 -298.852325\n-0.522444 -0.162285 0.837088 722.238281 -0.029088 0.984542 0.172717 51.306488 -0.852177 0.065885 -0.519088 -300.220520\n-0.521626 -0.162083 0.837637 724.449951 -0.028011 0.984513 0.173060 51.710968 -0.852714 0.066809 -0.518088 -301.574524\n-0.520642 -0.162897 0.838091 726.670349 -0.027640 0.984331 0.174151 52.114578 -0.853327 0.067506 -0.516987 -302.935150\n-0.519799 -0.163375 0.838521 728.902710 -0.027050 0.984199 0.174990 52.523739 -0.853860 0.068278 -0.516005 -304.307556\n-0.518988 -0.163296 0.839039 731.159790 -0.026979 0.984223 0.174864 52.942528 -0.854356 0.068116 -0.515205 -305.687225\n-0.518173 -0.163980 0.839409 733.399536 -0.028117 0.984184 0.174905 53.354279 -0.854814 0.067030 -0.514588 -307.033112\n-0.517991 -0.164323 0.839454 735.668396 -0.028518 0.984147 0.175050 53.773014 -0.854911 0.066735 -0.514465 -308.412170\n-0.517373 -0.165326 0.839638 737.953125 -0.028560 0.983950 0.176143 54.202190 -0.855283 0.067151 -0.513791 -309.787598\n-0.516807 -0.167040 0.839647 740.220703 -0.028314 0.983578 0.178247 54.626717 -0.855633 0.068345 -0.513050 -311.153961\n-0.516308 -0.168152 0.839733 742.471802 -0.027636 0.983295 0.179908 55.046207 -0.855957 0.069681 -0.512330 -312.517670\n-0.515837 -0.168100 0.840032 744.721924 -0.026888 0.983253 0.180249 55.472416 -0.856265 0.070392 -0.511719 -313.874847\n-0.514912 -0.167957 0.840629 746.966431 -0.026519 0.983270 0.180213 55.898670 -0.856833 0.070501 -0.510751 -315.226013\n-0.513788 -0.167291 0.841449 749.189453 -0.025746 0.983369 0.179786 56.325005 -0.857531 0.070708 -0.509550 -316.564270\n-0.512575 -0.167040 0.842238 751.419861 -0.025790 0.983447 0.179351 56.741905 -0.858255 0.070210 -0.508398 -317.899078\n-0.511530 -0.166924 0.842896 753.638123 -0.025408 0.983459 0.179341 57.164421 -0.858890 0.070322 -0.507310 -319.220337\n-0.510315 -0.166275 0.843760 755.877747 -0.024122 0.983512 0.179226 57.589954 -0.859649 0.071109 -0.505912 -320.554047\n-0.509361 -0.166769 0.844239 758.124390 -0.023996 0.983413 0.179784 58.010391 -0.860218 0.071316 -0.504914 -321.883606\n-0.508118 -0.168393 0.844665 760.364868 -0.025448 0.983208 0.180705 58.433304 -0.860911 0.070324 -0.503871 -323.206421\n-0.506629 -0.169415 0.845355 762.593079 -0.026689 0.983116 0.181029 58.859734 -0.861751 0.069153 -0.502597 -324.518768\n-0.505785 -0.169415 0.845860 764.825684 -0.026870 0.983145 0.180845 59.288216 -0.862241 0.068741 -0.501812 -325.830719\n-0.505280 -0.168459 0.846353 767.060669 -0.025609 0.983256 0.180420 59.717197 -0.862575 0.069488 -0.501134 -327.142639\n-0.504873 -0.167354 0.846815 769.293701 -0.024488 0.983408 0.179749 60.136482 -0.862846 0.070013 -0.500594 -328.453400\n-0.504045 -0.167988 0.847183 771.531982 -0.024436 0.983283 0.180436 60.560303 -0.863332 0.070246 -0.499724 -329.752716\n-0.503784 -0.170128 0.846911 773.798401 -0.025328 0.982902 0.182380 60.991207 -0.863458 0.070430 -0.499480 -331.069153\n-0.503838 -0.172161 0.846468 776.033386 -0.026708 0.982573 0.183946 61.425346 -0.863385 0.070072 -0.499656 -332.367340\n-0.504081 -0.172655 0.846223 778.262024 -0.028622 0.982615 0.183433 61.849960 -0.863182 0.068245 -0.500259 -333.657623\n-0.503793 -0.171354 0.846659 780.484863 -0.028860 0.982920 0.181759 62.282036 -0.863342 0.067134 -0.500133 -334.948975\n-0.503404 -0.171965 0.846766 782.705505 -0.027606 0.982696 0.183158 62.715374 -0.863610 0.068827 -0.499440 -336.253906\n-0.502927 -0.172202 0.847001 784.927673 -0.024943 0.982436 0.184926 63.146431 -0.863969 0.071878 -0.498389 -337.555939\n-0.502487 -0.172282 0.847246 787.153259 -0.022979 0.982261 0.186108 63.589577 -0.864279 0.074048 -0.497532 -338.867798\n-0.501791 -0.171031 0.847912 789.386475 -0.021963 0.982460 0.185173 64.030754 -0.864710 0.074296 -0.496746 -340.165955\n-0.501276 -0.169584 0.848507 791.642578 -0.021907 0.982780 0.183478 64.468300 -0.865010 0.073385 -0.496359 -341.470276\n-0.500801 -0.168253 0.849052 793.881714 -0.021759 0.983062 0.181975 64.903328 -0.865289 0.072659 -0.495980 -342.763641\n-0.499873 -0.168192 0.849611 796.131775 -0.021149 0.983041 0.182163 65.335609 -0.865840 0.073090 -0.494953 -344.072571\n-0.498805 -0.169276 0.850023 798.379944 -0.020624 0.982782 0.183612 65.767357 -0.866469 0.074056 -0.493708 -345.374512\n-0.497861 -0.170657 0.850301 800.605835 -0.020724 0.982509 0.185057 66.201271 -0.867009 0.074511 -0.492689 -346.652374\n-0.496717 -0.171034 0.850894 802.857300 -0.021182 0.982488 0.185120 66.637825 -0.867654 0.073928 -0.491641 -347.933777\n-0.495749 -0.171175 0.851429 805.108887 -0.021325 0.982487 0.185107 67.083405 -0.868204 0.073610 -0.490717 -349.223450\n-0.494858 -0.171635 0.851855 807.351257 -0.021452 0.982414 0.185478 67.524521 -0.868709 0.073511 -0.489838 -350.501678\n-0.493766 -0.171798 0.852455 809.604004 -0.021406 0.982395 0.185587 67.962334 -0.869331 0.073389 -0.488751 -351.780212\n-0.492807 -0.171791 0.853012 811.854614 -0.021623 0.982432 0.185363 68.404831 -0.869870 0.072904 -0.487864 -353.052002\n-0.492263 -0.171535 0.853378 814.091248 -0.020839 0.982432 0.185456 68.840111 -0.870197 0.073510 -0.487189 -354.316895\n-0.491601 -0.172392 0.853586 816.339111 -0.020307 0.982212 0.186674 69.278908 -0.870584 0.074435 -0.486358 -355.591675\n-0.491198 -0.174257 0.853439 818.608215 -0.020915 0.981862 0.188441 69.728249 -0.870797 0.074712 -0.485933 -356.856628\n-0.491408 -0.175869 0.852988 820.836243 -0.022252 0.981615 0.189570 70.167061 -0.870645 0.074176 -0.486287 -358.108643\n-0.492100 -0.176475 0.852464 823.055786 -0.023787 0.981597 0.189477 70.613609 -0.870214 0.072964 -0.487241 -359.338287\n-0.492693 -0.176370 0.852143 825.264648 -0.024788 0.981693 0.188851 71.052780 -0.869850 0.071922 -0.488046 -360.571625\n-0.493347 -0.176777 0.851680 827.490295 -0.025069 0.981614 0.189225 71.496208 -0.869471 0.072003 -0.488707 -361.825317\n-0.494000 -0.176833 0.851290 829.711853 -0.024620 0.981552 0.189605 71.940483 -0.869113 0.072706 -0.489240 -363.077057\n-0.494516 -0.176738 0.851009 831.941956 -0.023552 0.981473 0.190147 72.382904 -0.868849 0.073988 -0.489517 -364.351532\n-0.495093 -0.177227 0.850573 834.179504 -0.022938 0.981300 0.191114 72.827461 -0.868537 0.075109 -0.489900 -365.630493\n-0.495053 -0.178322 0.850367 836.415588 -0.022790 0.981041 0.192456 73.275047 -0.868564 0.075896 -0.489731 -366.907501\n-0.495471 -0.179076 0.849965 838.655762 -0.023429 0.980917 0.193009 73.727776 -0.868309 0.075717 -0.490211 -368.177216\n-0.495620 -0.178348 0.850031 840.890381 -0.024324 0.981156 0.191678 74.181297 -0.868199 0.074323 -0.490619 -369.447418\n-0.495495 -0.177140 0.850356 843.126892 -0.024869 0.981476 0.189963 74.628960 -0.868254 0.072978 -0.490722 -370.711578\n-0.494969 -0.176515 0.850793 845.372375 -0.024924 0.981630 0.189160 75.072037 -0.868553 0.072423 -0.490276 -371.993652\n-0.494518 -0.177279 0.850896 847.612976 -0.024852 0.981462 0.190039 75.516258 -0.868812 0.072831 -0.489756 -373.256744\n-0.493677 -0.178439 0.851142 849.866882 -0.024975 0.981228 0.191226 75.963875 -0.869286 0.073146 -0.488867 -374.539215\n-0.492539 -0.179319 0.851616 852.114136 -0.024379 0.981001 0.192462 76.410484 -0.869949 0.074034 -0.487553 -375.811890\n-0.491649 -0.179765 0.852036 854.363525 -0.023371 0.980831 0.193453 76.867760 -0.870480 0.075198 -0.486426 -377.094086\n-0.490740 -0.180002 0.852510 856.606812 -0.022330 0.980705 0.194215 77.323280 -0.871020 0.076273 -0.485291 -378.356659\n-0.489289 -0.180457 0.853248 858.826172 -0.021699 0.980575 0.194942 77.774567 -0.871852 0.076868 -0.483700 -379.611755\n-0.487998 -0.181316 0.853804 861.069946 -0.021347 0.980373 0.195994 78.232109 -0.872583 0.077418 -0.482291 -380.870239\n-0.486981 -0.181917 0.854258 863.312195 -0.021149 0.980238 0.196688 78.691711 -0.873156 0.077717 -0.481205 -382.124512\n-0.486099 -0.183059 0.854516 865.546936 -0.021005 0.979979 0.197988 79.150375 -0.873651 0.078293 -0.480212 -383.366760\n-0.484967 -0.184885 0.854766 867.793640 -0.021582 0.979630 0.199647 79.612526 -0.874266 0.078375 -0.479078 -384.617493\n-0.483308 -0.186336 0.855390 870.032227 -0.021872 0.979351 0.200982 80.076843 -0.875177 0.078427 -0.477404 -385.856567\n-0.481902 -0.186825 0.856077 872.285950 -0.022296 0.979303 0.201166 80.549530 -0.875942 0.077855 -0.476094 -387.079071\n-0.480981 -0.186783 0.856603 874.535339 -0.022050 0.979310 0.201159 81.026596 -0.876454 0.077866 -0.475148 -388.310944\n-0.480287 -0.186967 0.856953 876.833862 -0.021044 0.979192 0.201843 81.510094 -0.876859 0.078909 -0.474227 -389.569061\n-0.480181 -0.188033 0.856779 879.112915 -0.020941 0.978933 0.203106 81.995590 -0.876920 0.079585 -0.474002 -390.811066\n-0.480118 -0.189142 0.856570 881.395874 -0.021375 0.978710 0.204132 82.482773 -0.876943 0.079698 -0.473939 -392.036438\n-0.480185 -0.191617 0.855982 883.678467 -0.021886 0.978162 0.206690 82.974579 -0.876894 0.080516 -0.473892 -393.260834\n-0.480006 -0.193749 0.855603 885.945862 -0.023089 0.977759 0.208457 83.464333 -0.876961 0.080306 -0.473803 -394.481537\n-0.479324 -0.194018 0.855924 888.214294 -0.023952 0.977787 0.208229 83.965790 -0.877311 0.079308 -0.473324 -395.704834\n-0.478058 -0.193085 0.856842 890.481445 -0.023692 0.978017 0.207173 84.467529 -0.878009 0.078740 -0.472124 -396.918915\n-0.477125 -0.193584 0.857250 892.747986 -0.023349 0.977886 0.207831 84.967194 -0.878525 0.079145 -0.471093 -398.136200\n-0.476379 -0.195586 0.857210 895.030701 -0.024167 0.977489 0.209599 85.468277 -0.878908 0.079132 -0.470381 -399.370941\n-0.475786 -0.194861 0.857704 897.280823 -0.023621 0.977629 0.209004 85.970917 -0.879244 0.079181 -0.469745 -400.584534\n-0.475175 -0.194177 0.858198 899.553833 -0.022949 0.977749 0.208521 86.469826 -0.879592 0.079389 -0.469058 -401.802490\n-0.474626 -0.194573 0.858412 901.824219 -0.021623 0.977544 0.209620 86.968315 -0.879922 0.080930 -0.468175 -403.021576\n-0.474204 -0.195948 0.858333 904.103516 -0.020403 0.977102 0.211790 87.478020 -0.880178 0.082919 -0.467344 -404.254059\n-0.474047 -0.198155 0.857913 906.353333 -0.019685 0.976489 0.214666 87.978401 -0.880279 0.084873 -0.466802 -405.466248\n-0.473412 -0.199506 0.857950 908.629333 -0.019358 0.976134 0.216306 88.489143 -0.880628 0.085794 -0.465976 -406.680298\n-0.472661 -0.200567 0.858117 910.888489 -0.019426 0.975891 0.217394 89.004066 -0.881030 0.086085 -0.465162 -407.888611\n-0.471379 -0.200178 0.858912 913.123535 -0.018275 0.975908 0.217415 89.518272 -0.881741 0.086788 -0.463681 -409.088776\n-0.469526 -0.199283 0.860135 915.372986 -0.015871 0.975942 0.217451 90.038643 -0.882776 0.088448 -0.461393 -410.297760\n-0.468086 -0.199186 0.860942 917.641113 -0.013504 0.975764 0.218409 90.558540 -0.883580 0.090608 -0.459431 -411.504700\n-0.466565 -0.199803 0.861624 919.898254 -0.011999 0.975491 0.219711 91.077095 -0.884405 0.092171 -0.457528 -412.699097\n-0.464785 -0.200951 0.862319 922.163025 -0.011173 0.975159 0.221225 91.602425 -0.885353 0.093187 -0.455484 -413.880554\n-0.463396 -0.202004 0.862820 924.401306 -0.010331 0.974837 0.222681 92.119865 -0.886091 0.094276 -0.453822 -415.044800\n-0.462017 -0.202849 0.863361 926.657288 -0.009704 0.974589 0.223789 92.648148 -0.886818 0.095017 -0.452245 -416.219147\n-0.460918 -0.203612 0.863769 928.910645 -0.009794 0.974432 0.224471 93.176819 -0.887389 0.095003 -0.451127 -417.390472\n-0.459684 -0.204016 0.864331 931.158081 -0.010193 0.974403 0.224577 93.706635 -0.888024 0.094425 -0.449997 -418.542297\n-0.458255 -0.204633 0.864943 933.405212 -0.010315 0.974295 0.225040 94.240097 -0.888761 0.094204 -0.448587 -419.689301\n-0.456926 -0.204877 0.865589 935.649963 -0.009679 0.974201 0.225475 94.770523 -0.889452 0.094647 -0.447120 -420.835266\n-0.455512 -0.204903 0.866328 937.893127 -0.009032 0.974164 0.225659 95.300926 -0.890184 0.094966 -0.445594 -421.976654\n-0.454253 -0.204472 0.867090 940.159241 -0.007979 0.974199 0.225549 95.833832 -0.890837 0.095538 -0.444165 -423.128021\n-0.453217 -0.205267 0.867444 942.410828 -0.007016 0.973917 0.226796 96.369064 -0.891373 0.096702 -0.442836 -424.268066\n-0.452190 -0.206216 0.867755 944.653625 -0.006177 0.973606 0.228151 96.898033 -0.891900 0.097807 -0.441529 -425.407623\n-0.451148 -0.207413 0.868012 946.873230 -0.006064 0.973308 0.229423 97.428810 -0.892429 0.098240 -0.440363 -426.523529\n-0.450349 -0.207355 0.868441 949.076416 -0.005223 0.973254 0.229673 97.964035 -0.892837 0.098897 -0.439387 -427.632751\n-0.449968 -0.207335 0.868643 951.304932 -0.004988 0.973244 0.229719 98.499031 -0.893031 0.099033 -0.438963 -428.742188\n-0.449467 -0.208139 0.868710 953.507568 -0.005267 0.973077 0.230420 99.032654 -0.893282 0.098990 -0.438462 -429.845367\n-0.449217 -0.208318 0.868797 955.749207 -0.005223 0.973032 0.230610 99.573799 -0.893407 0.099056 -0.438191 -430.959991\n-0.448641 -0.208176 0.869128 957.995361 -0.004862 0.973047 0.230557 100.114281 -0.893699 0.099211 -0.437561 -432.071503\n-0.448176 -0.207573 0.869512 960.242004 -0.004619 0.973193 0.229943 100.651176 -0.893934 0.099039 -0.437120 -433.179565\n-0.448122 -0.208238 0.869381 962.486389 -0.004030 0.972953 0.230968 101.189827 -0.893963 0.099998 -0.436841 -434.295868\n-0.447698 -0.210795 0.868983 964.730225 -0.003385 0.972209 0.234091 101.734695 -0.894179 0.101861 -0.435969 -435.414764\n-0.447294 -0.211952 0.868910 966.953186 -0.004609 0.972048 0.234738 102.280945 -0.894375 0.100993 -0.435768 -436.515930\n-0.446903 -0.208636 0.869913 969.158081 -0.007789 0.973294 0.229429 102.810127 -0.894548 0.095757 -0.436594 -437.583679\n-0.446734 -0.205253 0.870804 971.384583 -0.009837 0.974396 0.224623 103.335609 -0.894613 0.091781 -0.437315 -438.680695\n-0.446965 -0.208677 0.869871 973.611023 -0.010350 0.973552 0.228231 103.870888 -0.894491 0.093008 -0.437304 -439.781372\n-0.446466 -0.213179 0.869035 975.854858 -0.009581 0.972289 0.233585 104.414009 -0.894749 0.095961 -0.436137 -440.894379\n-0.446043 -0.215459 0.868690 978.083130 -0.008857 0.971606 0.236438 104.971474 -0.894968 0.097768 -0.435287 -441.995544\n-0.446171 -0.215230 0.868682 980.319031 -0.007257 0.971489 0.236975 105.533600 -0.894918 0.099427 -0.435012 -443.098175\n-0.445867 -0.215061 0.868879 982.555847 -0.006313 0.971439 0.237207 106.090744 -0.895077 0.100277 -0.434490 -444.206512\n-0.444937 -0.216038 0.869114 984.804688 -0.005844 0.971147 0.238408 106.654236 -0.895543 0.100997 -0.433362 -445.322723\n-0.444109 -0.217932 0.869065 987.033020 -0.006640 0.970741 0.240036 107.211639 -0.895948 0.100832 -0.432562 -446.417114\n-0.443306 -0.218552 0.869318 989.253235 -0.006843 0.970618 0.240530 107.773186 -0.896344 0.100679 -0.431777 -447.500366\n-0.442398 -0.217196 0.870120 991.487976 -0.006789 0.971013 0.238929 108.337242 -0.896793 0.099794 -0.431049 -448.592682\n-0.441391 -0.217091 0.870658 993.722473 -0.006385 0.971028 0.238881 108.898537 -0.897292 0.099880 -0.429989 -449.681610\n-0.440860 -0.217778 0.870756 995.958435 -0.005842 0.970795 0.239841 109.460159 -0.897557 0.100649 -0.429257 -450.768707\n-0.440418 -0.218291 0.870851 998.172058 -0.005506 0.970629 0.240517 110.023262 -0.897776 0.101133 -0.428685 -451.839691\n-0.439730 -0.218261 0.871206 1000.405945 -0.006107 0.970726 0.240112 110.583534 -0.898109 0.100264 -0.428190 -452.922668\n-0.438827 -0.218992 0.871478 1002.660522 -0.006884 0.970639 0.240444 111.145042 -0.898545 0.099514 -0.427450 -454.001831\n-0.438476 -0.220393 0.871301 1004.894470 -0.007241 0.970302 0.241790 111.712555 -0.898714 0.099710 -0.427050 -455.080963\n-0.438105 -0.221831 0.871123 1007.126404 -0.007533 0.969945 0.243207 112.275963 -0.898892 0.099988 -0.426609 -456.157013\n-0.437814 -0.223760 0.870776 1009.330261 -0.007463 0.969405 0.245352 112.838936 -0.899034 0.100920 -0.426089 -457.216827\n-0.437992 -0.227905 0.869610 1011.547485 -0.008146 0.968298 0.249666 113.414314 -0.898942 0.102268 -0.425963 -458.280731\n-0.437653 -0.231023 0.868958 1013.738831 -0.008327 0.967428 0.253008 113.997025 -0.899105 0.103494 -0.425322 -459.340363\n-0.437692 -0.233076 0.868390 1015.915466 -0.010192 0.967041 0.254417 114.575462 -0.899067 0.102505 -0.425642 -460.387970\n-0.437881 -0.231411 0.868740 1018.118591 -0.009114 0.967398 0.253097 115.161629 -0.898987 0.102908 -0.425714 -461.450592\n-0.437956 -0.230525 0.868938 1020.316284 -0.006698 0.967374 0.253263 115.755630 -0.898972 0.105098 -0.425211 -462.515656\n-0.438380 -0.229667 0.868951 1022.535034 -0.004713 0.967376 0.253303 116.343529 -0.898777 0.106948 -0.425160 -463.597687\n-0.438525 -0.231123 0.868492 1024.739868 -0.005919 0.967088 0.254373 116.928909 -0.898700 0.106409 -0.425460 -464.652405\n-0.438131 -0.233024 0.868183 1026.948120 -0.009407 0.966951 0.254787 117.522377 -0.898862 0.103463 -0.425843 -465.710693\n-0.438692 -0.233659 0.867729 1029.160522 -0.011922 0.967033 0.254373 118.119797 -0.898558 0.101246 -0.427015 -466.771851\n-0.439147 -0.233449 0.867555 1031.384399 -0.011134 0.966990 0.254570 118.722237 -0.898346 0.102134 -0.427250 -467.847626\n-0.439266 -0.233961 0.867356 1033.605469 -0.009243 0.966618 0.256055 119.322823 -0.898309 0.104459 -0.426765 -468.922424\n-0.439241 -0.233817 0.867408 1035.836060 -0.007271 0.966430 0.256828 119.923653 -0.898340 0.106503 -0.426195 -470.005554\n-0.439436 -0.233248 0.867463 1038.070679 -0.006453 0.966494 0.256607 120.527908 -0.898251 0.107164 -0.426217 -471.089630\n-0.439682 -0.233907 0.867160 1040.294922 -0.006497 0.966295 0.257354 121.129005 -0.898130 0.107520 -0.426383 -472.160065\n-0.439412 -0.234988 0.867005 1042.548340 -0.006437 0.965976 0.258550 121.740677 -0.898263 0.108029 -0.425974 -473.244781\n-0.439009 -0.235534 0.867061 1044.792358 -0.006668 0.965856 0.258995 122.355492 -0.898458 0.107920 -0.425590 -474.339233\n-0.438220 -0.235001 0.867605 1047.045288 -0.006962 0.966078 0.258157 122.966263 -0.898841 0.107089 -0.424990 -475.428223\n-0.437406 -0.235227 0.867954 1049.304810 -0.007131 0.966059 0.258221 123.584229 -0.899236 0.106758 -0.424237 -476.506195\n-0.437266 -0.235965 0.867824 1051.552856 -0.006646 0.965787 0.259252 124.197830 -0.899308 0.107595 -0.423874 -477.588898\n-0.436665 -0.237761 0.867636 1053.816040 -0.006281 0.965226 0.261342 124.816811 -0.899602 0.108669 -0.422974 -478.671783\n-0.435880 -0.240017 0.867410 1056.063965 -0.006765 0.964630 0.263519 125.436440 -0.899980 0.108995 -0.422087 -479.742859\n-0.435184 -0.241664 0.867303 1058.305542 -0.007846 0.964285 0.264750 126.057259 -0.900308 0.108411 -0.421537 -480.798737\n-0.434293 -0.240922 0.867955 1060.546387 -0.008510 0.964623 0.263496 126.677994 -0.900731 0.107048 -0.420980 -481.858032\n-0.433642 -0.240397 0.868426 1062.786133 -0.009129 0.964878 0.262538 127.300774 -0.901039 0.105920 -0.420607 -482.917389\n-0.432950 -0.241113 0.868573 1065.024658 -0.008550 0.964618 0.263512 127.920708 -0.901377 0.106661 -0.419693 -483.980713\n-0.432631 -0.241718 0.868564 1067.257690 -0.007508 0.964322 0.264627 128.540451 -0.901540 0.107965 -0.419010 -485.039856\n-0.432320 -0.241515 0.868775 1069.509644 -0.006742 0.964303 0.264716 129.164047 -0.901695 0.108585 -0.418515 -486.106476\n-0.432401 -0.241716 0.868679 1071.764404 -0.005467 0.964084 0.265542 129.789886 -0.901665 0.110072 -0.418192 -487.173492\n-0.431590 -0.242212 0.868944 1074.008301 -0.003901 0.963770 0.266706 130.416611 -0.902061 0.111718 -0.416898 -488.238403\n-0.431339 -0.241365 0.869304 1076.263794 -0.001670 0.963761 0.266763 131.042801 -0.902189 0.113613 -0.416110 -489.308533\n-0.430723 -0.241255 0.869640 1078.498535 -0.000102 0.963620 0.267276 131.668182 -0.902484 0.115033 -0.415078 -490.363281\n-0.429504 -0.243015 0.869753 1080.751709 -0.000927 0.963230 0.268675 132.296829 -0.903064 0.114591 -0.413937 -491.422302\n-0.427736 -0.244645 0.870167 1083.007690 -0.003312 0.963094 0.269143 132.923584 -0.903898 0.112241 -0.412760 -492.468201\n-0.426104 -0.247358 0.870201 1085.271362 -0.007868 0.962871 0.269847 133.570587 -0.904640 0.108137 -0.412229 -493.519867\n-0.424573 -0.247969 0.870775 1087.538818 -0.010827 0.963086 0.268977 134.216995 -0.905329 0.104772 -0.411585 -494.569733\n-0.422965 -0.247311 0.871744 1089.764771 -0.012073 0.963488 0.267481 134.852127 -0.906065 0.102611 -0.410508 -495.601379\n-0.421540 -0.246460 0.872675 1092.007812 -0.010638 0.963635 0.267011 135.485580 -0.906748 0.103272 -0.408832 -496.640961\n-0.420176 -0.247093 0.873153 1094.243408 -0.008692 0.963265 0.268411 136.123932 -0.907401 0.105191 -0.406889 -497.681427\n-0.418419 -0.248095 0.873713 1096.472656 -0.006874 0.962807 0.270102 136.754547 -0.908228 0.107010 -0.404562 -498.717590\n-0.416919 -0.248278 0.874378 1098.702393 -0.005006 0.962584 0.270937 137.391769 -0.908930 0.108582 -0.402562 -499.740448\n-0.415435 -0.247531 0.875295 1100.928101 -0.004842 0.962850 0.269993 138.021957 -0.909610 0.107927 -0.401200 -500.740845\n" }, { "path": "src/vloam_main/results/2011_10_03_drive_0042/MO0.txt", "content": "1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000\n0.999040 -0.001213 0.043785 0.009204 0.001288 0.999998 -0.001663 -0.025504 -0.043783 0.001718 0.999040 0.968540\n0.996040 -0.005320 0.088745 0.081009 0.005836 0.999968 -0.005564 -0.032809 -0.088713 0.006060 0.996039 1.945831\n0.991006 -0.005410 0.133705 0.177676 0.006572 0.999944 -0.008244 -0.086052 -0.133653 0.009048 0.990987 2.912190\n0.983747 -0.004000 0.179517 0.321934 0.005898 0.999932 -0.010042 -0.102479 -0.179464 0.010937 0.983704 3.891226\n0.974302 -0.006656 0.225149 0.519738 0.008997 0.999916 -0.009373 -0.127200 -0.225068 0.011158 0.974279 4.851651\n0.962800 -0.008015 0.270096 0.759113 0.011127 0.999888 -0.009993 -0.156871 -0.269986 0.012626 0.962781 5.822557\n0.949298 -0.011463 0.314168 1.043840 0.016042 0.999799 -0.011994 -0.168389 -0.313968 0.016426 0.949292 6.773984\n0.934464 -0.013350 0.355807 1.377868 0.019000 0.999743 -0.012391 -0.236393 -0.355550 0.018340 0.934477 7.679627\n0.917212 -0.013910 0.398156 1.740469 0.020406 0.999719 -0.012083 -0.250290 -0.397876 0.019207 0.917238 8.589301\n0.897851 -0.014993 0.440044 2.140164 0.021719 0.999712 -0.010254 -0.285032 -0.439764 0.018764 0.897918 9.465033\n0.876276 -0.015352 0.481565 2.583436 0.021992 0.999725 -0.008148 -0.329514 -0.481308 0.017731 0.876372 10.333347\n0.852426 -0.016167 0.522598 3.057379 0.023080 0.999711 -0.006719 -0.348381 -0.522338 0.017789 0.852553 11.179647\n0.826892 -0.017145 0.562099 3.575622 0.024490 0.999685 -0.005534 -0.369102 -0.561827 0.018342 0.827051 11.991906\n0.799828 -0.019446 0.599915 4.133268 0.027545 0.999611 -0.004322 -0.405181 -0.599598 0.019981 0.800052 12.781466\n0.771076 -0.019729 0.636437 4.731267 0.028805 0.999577 -0.003912 -0.419866 -0.636091 0.021349 0.771319 13.541350\n0.740997 -0.022933 0.671116 5.359388 0.033046 0.999451 -0.002335 -0.439348 -0.670694 0.023908 0.741349 14.267226\n0.709246 -0.023031 0.704585 6.017320 0.033033 0.999454 -0.000582 -0.480566 -0.704187 0.023688 0.709619 14.958797\n0.675142 -0.023020 0.737329 6.682873 0.034497 0.999405 -0.000386 -0.484415 -0.736881 0.025696 0.675534 15.612626\n0.639759 -0.024349 0.768190 7.398717 0.038381 0.999263 -0.000290 -0.501133 -0.767617 0.029669 0.640222 16.251026\n0.603719 -0.023939 0.796838 8.132809 0.038769 0.999248 0.000646 -0.534306 -0.796254 0.030503 0.604193 16.835209\n0.566211 -0.023693 0.823919 8.893494 0.041309 0.999146 0.000344 -0.554349 -0.823224 0.033840 0.566707 17.391502\n0.527562 -0.022840 0.849210 9.695924 0.045326 0.998971 -0.001289 -0.562949 -0.848307 0.039171 0.528054 17.900707\n0.488899 -0.022599 0.872047 10.515223 0.047083 0.998891 -0.000510 -0.579372 -0.871069 0.041308 0.489421 18.393127\n0.450569 -0.024686 0.892400 11.370240 0.051248 0.998684 0.001751 -0.578655 -0.891269 0.044945 0.451242 18.833494\n0.413668 -0.026283 0.910048 12.235610 0.055009 0.998479 0.003833 -0.613178 -0.908764 0.048475 0.414485 19.259991\n0.378304 -0.025087 0.925342 13.121842 0.055969 0.998424 0.004187 -0.630784 -0.923988 0.050206 0.379111 19.633703\n0.344829 -0.025954 0.938307 14.038853 0.056199 0.998395 0.006962 -0.638393 -0.936981 0.050331 0.345735 20.002926\n0.311671 -0.027804 0.949783 14.975684 0.057829 0.998274 0.010247 -0.632813 -0.948429 0.051732 0.312741 20.311922\n0.278815 -0.028496 0.959922 15.927144 0.058637 0.998200 0.012601 -0.657986 -0.958553 0.052773 0.279984 20.614782\n0.246657 -0.026719 0.968735 16.895384 0.055851 0.998350 0.013315 -0.684867 -0.967492 0.050820 0.247742 20.902039\n0.215398 -0.025195 0.976201 17.864370 0.055198 0.998383 0.013588 -0.658271 -0.974965 0.050958 0.216441 21.135769\n0.184827 -0.025338 0.982444 18.861614 0.056276 0.998300 0.015160 -0.647992 -0.981158 0.052487 0.185939 21.331142\n0.154195 -0.026334 0.987689 19.865419 0.057378 0.998196 0.017657 -0.649015 -0.986373 0.053949 0.155428 21.505329\n0.123988 -0.027393 0.991906 20.903206 0.056078 0.998215 0.020558 -0.661852 -0.990698 0.053075 0.125303 21.667324\n0.094550 -0.027756 0.995133 21.957485 0.056084 0.998172 0.022512 -0.639981 -0.993939 0.053683 0.095934 21.791349\n0.067409 -0.027827 0.997337 23.030537 0.058314 0.998012 0.023904 -0.617451 -0.996020 0.056548 0.068898 21.878922\n0.042171 -0.027764 0.998725 24.121469 0.061937 0.997764 0.025122 -0.601488 -0.997189 0.060798 0.043796 21.924400\n0.019209 -0.027537 0.999436 25.208019 0.066007 0.997475 0.026214 -0.581156 -0.997634 0.065467 0.020978 21.951920\n-0.001259 -0.028158 0.999603 26.331835 0.071124 0.997069 0.028176 -0.567835 -0.997467 0.071131 0.000747 21.952934\n-0.019216 -0.029044 0.999393 27.462589 0.075217 0.996703 0.030412 -0.554688 -0.996982 0.075756 -0.016968 21.936741\n-0.035179 -0.029490 0.998946 28.602922 0.077953 0.996438 0.032161 -0.538116 -0.996336 0.079002 -0.032755 21.903400\n-0.049673 -0.030353 0.998304 29.753918 0.079876 0.996216 0.034264 -0.522380 -0.995566 0.081443 -0.047060 21.848150\n-0.063189 -0.031673 0.997499 30.900194 0.079921 0.996126 0.036692 -0.506763 -0.994796 0.082039 -0.060413 21.793982\n-0.075547 -0.031688 0.996639 32.046719 0.079976 0.996082 0.037733 -0.489370 -0.993930 0.082558 -0.072717 21.728493\n-0.086670 -0.029295 0.995806 33.207314 0.080340 0.996106 0.036296 -0.459511 -0.992992 0.083149 -0.083979 21.633080\n-0.096770 -0.025896 0.994970 34.386753 0.080674 0.996168 0.033773 -0.430056 -0.992032 0.083537 -0.094310 21.522898\n-0.106355 -0.023728 0.994045 35.589607 0.081098 0.996178 0.032456 -0.399824 -0.991015 0.084067 -0.104024 21.404305\n-0.113607 -0.022749 0.993265 36.811581 0.080488 0.996241 0.032023 -0.374079 -0.990260 0.083584 -0.111349 21.274212\n-0.119436 -0.022000 0.992598 38.069473 0.080387 0.996258 0.031754 -0.341875 -0.989582 0.083584 -0.117220 21.128698\n-0.124474 -0.020615 0.992009 39.342918 0.080978 0.996238 0.030864 -0.310138 -0.988913 0.084173 -0.122336 20.973839\n-0.128939 -0.019265 0.991465 40.627197 0.080321 0.996323 0.029805 -0.284095 -0.988394 0.083478 -0.126918 20.819830\n-0.132872 -0.018613 0.990958 41.944187 0.080245 0.996339 0.029474 -0.259605 -0.987879 0.083436 -0.130892 20.648544\n-0.135687 -0.019569 0.990559 43.286690 0.079667 0.996352 0.030596 -0.234364 -0.987544 0.083066 -0.133633 20.473463\n-0.137686 -0.021628 0.990240 44.651836 0.079828 0.996267 0.032859 -0.200647 -0.987254 0.083573 -0.135445 20.284866\n-0.139713 -0.022303 0.989941 46.034374 0.080619 0.996171 0.033822 -0.166047 -0.986905 0.084533 -0.137380 20.098186\n-0.141689 -0.022227 0.989662 47.423138 0.082167 0.996034 0.034134 -0.131035 -0.986495 0.086154 -0.139300 19.885185\n-0.142407 -0.021866 0.989567 48.840233 0.082973 0.995973 0.033948 -0.089024 -0.986324 0.086941 -0.140019 19.688587\n-0.142541 -0.021191 0.989562 50.288761 0.084169 0.995890 0.033451 -0.058545 -0.986204 0.088059 -0.140171 19.474182\n-0.142331 -0.020554 0.989606 51.754581 0.084194 0.995910 0.032794 -0.016762 -0.986232 0.087986 -0.140018 19.269396\n-0.141665 -0.019658 0.989719 53.218555 0.083866 0.995970 0.031787 0.006184 -0.986356 0.087507 -0.139446 19.062412\n" }, { "path": "src/vloam_main/results/2011_10_03_drive_0042/MO1.txt", "content": "1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000\n0.999572 -0.000230 0.029255 -0.016924 0.000256 1.000000 -0.000890 -0.000181 -0.029255 0.000897 0.999572 0.458197\n0.997450 -0.004727 0.071213 0.031305 0.005048 0.999978 -0.004326 -0.007234 -0.071191 0.004675 0.997452 1.287719\n0.993020 -0.004514 0.117862 0.110625 0.005391 0.999960 -0.007130 -0.055016 -0.117825 0.007715 0.993004 2.296315\n0.986430 -0.003301 0.164151 0.245317 0.004808 0.999950 -0.008784 -0.072726 -0.164113 0.009454 0.986396 3.288078\n0.977693 -0.005930 0.209954 0.426697 0.007859 0.999934 -0.008353 -0.095741 -0.209890 0.009817 0.977676 4.266836\n0.966821 -0.007347 0.255348 0.656683 0.009927 0.999912 -0.008817 -0.123994 -0.255261 0.011059 0.966809 5.227077\n0.954083 -0.010846 0.299347 0.923079 0.014866 0.999827 -0.011155 -0.135048 -0.299175 0.015093 0.954079 6.194756\n0.939781 -0.012678 0.341542 1.234736 0.017718 0.999775 -0.011642 -0.201739 -0.341317 0.016992 0.939795 7.123590\n0.923223 -0.013273 0.384035 1.583098 0.019109 0.999753 -0.011387 -0.216029 -0.383789 0.017851 0.923248 8.023061\n0.904473 -0.014452 0.426287 1.974419 0.020449 0.999746 -0.009494 -0.252007 -0.426041 0.017304 0.904538 8.910201\n0.883482 -0.014874 0.468229 2.394852 0.020748 0.999757 -0.007390 -0.296374 -0.468005 0.016243 0.883576 9.788986\n0.860257 -0.015607 0.509621 2.864675 0.021717 0.999746 -0.006042 -0.314095 -0.509397 0.016265 0.860378 10.645697\n0.835474 -0.016642 0.549278 3.367638 0.023157 0.999720 -0.004932 -0.334530 -0.549042 0.016840 0.835625 11.465844\n0.808954 -0.018856 0.587569 3.918877 0.026129 0.999651 -0.003894 -0.368586 -0.587291 0.018503 0.809165 12.263857\n0.780747 -0.019194 0.624552 4.491345 0.027341 0.999620 -0.003459 -0.382753 -0.624249 0.019776 0.780975 13.033236\n0.751103 -0.022449 0.659804 5.107123 0.031554 0.999500 -0.001914 -0.402758 -0.659431 0.022257 0.751436 13.770363\n0.719919 -0.022501 0.693693 5.749829 0.031631 0.999500 -0.000406 -0.445329 -0.693337 0.022234 0.720271 14.475348\n0.686291 -0.022420 0.726981 6.414773 0.033079 0.999453 -0.000405 -0.449343 -0.726574 0.024326 0.686657 15.152022\n0.651553 -0.023844 0.758228 7.118777 0.036923 0.999318 -0.000303 -0.464175 -0.757704 0.028193 0.651989 15.790865\n0.615726 -0.023412 0.787613 7.837874 0.037283 0.999305 0.000558 -0.498390 -0.787078 0.029021 0.616170 16.397993\n0.578716 -0.023270 0.815197 8.585885 0.039791 0.999208 0.000275 -0.518471 -0.814558 0.032278 0.579184 16.960114\n0.540612 -0.022412 0.840974 9.379487 0.043791 0.999040 -0.001527 -0.526410 -0.840132 0.037652 0.541074 17.475838\n0.502252 -0.022222 0.864436 10.188986 0.045576 0.998961 -0.000800 -0.544385 -0.863519 0.039800 0.502743 17.986839\n0.464101 -0.024437 0.885445 11.041178 0.049780 0.998759 0.001473 -0.543251 -0.884383 0.043394 0.464741 18.440174\n0.427659 -0.026004 0.903566 11.905699 0.053584 0.998558 0.003376 -0.577985 -0.902351 0.046972 0.428435 18.872763\n0.392221 -0.024908 0.919534 12.790526 0.054541 0.998504 0.003783 -0.596458 -0.918253 0.048669 0.392993 19.271761\n0.359013 -0.025664 0.932979 13.700810 0.054831 0.998475 0.006367 -0.603522 -0.931720 0.048870 0.359873 19.650539\n0.326030 -0.027636 0.944956 14.630372 0.056543 0.998353 0.009690 -0.601031 -0.943667 0.050272 0.327055 19.980888\n0.293356 -0.028341 0.955583 15.573013 0.057377 0.998281 0.011994 -0.626704 -0.954280 0.051310 0.294478 20.301102\n0.261492 -0.026560 0.964840 16.531034 0.054545 0.998430 0.012702 -0.653552 -0.963663 0.049306 0.262530 20.590851\n0.230356 -0.024913 0.972788 17.494057 0.053989 0.998460 0.012785 -0.628566 -0.971608 0.049575 0.231346 20.848854\n0.199812 -0.025155 0.979511 18.488155 0.055076 0.998378 0.014404 -0.619861 -0.978285 0.051069 0.200874 21.062954\n0.169321 -0.026155 0.985214 19.490349 0.056152 0.998280 0.016851 -0.620611 -0.983960 0.052469 0.170499 21.245344\n0.139171 -0.027258 0.989893 20.518883 0.054889 0.998297 0.019772 -0.634041 -0.988746 0.051583 0.140430 21.424995\n0.109752 -0.027614 0.993575 21.579203 0.054917 0.998256 0.021678 -0.613170 -0.992441 0.052185 0.111077 21.567287\n0.082328 -0.027807 0.996217 22.655390 0.057210 0.998094 0.023132 -0.592135 -0.994962 0.055089 0.083762 21.673550\n0.057035 -0.027659 0.997989 23.740000 0.060939 0.997849 0.024173 -0.578658 -0.996511 0.059437 0.058598 21.739151\n0.033874 -0.027538 0.999047 24.837912 0.065044 0.997562 0.025292 -0.558482 -0.997307 0.064126 0.035582 21.783041\n0.013436 -0.028375 0.999507 25.959894 0.070165 0.997160 0.027365 -0.548081 -0.997445 0.069763 0.015389 21.804148\n-0.004769 -0.029327 0.999559 27.086313 0.074247 0.996800 0.029600 -0.533665 -0.997229 0.074355 -0.002577 21.804296\n-0.020739 -0.029719 0.999343 28.216734 0.077023 0.996540 0.031234 -0.519161 -0.996814 0.077620 -0.018378 21.785435\n-0.035334 -0.030674 0.998905 29.372181 0.078979 0.996317 0.033388 -0.504926 -0.996250 0.080073 -0.032782 21.752159\n-0.048921 -0.031957 0.998291 30.526892 0.079072 0.996227 0.035766 -0.491184 -0.995668 0.080687 -0.046209 21.720524\n-0.061147 -0.032045 0.997614 31.676498 0.079123 0.996184 0.036849 -0.474157 -0.994988 0.081187 -0.058378 21.667957\n-0.072202 -0.029593 0.996951 32.833702 0.079416 0.996216 0.035323 -0.444468 -0.994223 0.081724 -0.069578 21.586061\n-0.082238 -0.026325 0.996265 34.008041 0.079828 0.996265 0.032915 -0.416288 -0.993411 0.082236 -0.079829 21.502777\n-0.091944 -0.024074 0.995473 35.217793 0.080250 0.996277 0.031505 -0.387644 -0.992525 0.082784 -0.089670 21.401590\n-0.099231 -0.023134 0.994796 36.438148 0.079649 0.996337 0.031114 -0.363329 -0.991872 0.082321 -0.097025 21.293848\n-0.104914 -0.022375 0.994229 37.706512 0.079587 0.996351 0.030821 -0.332353 -0.991292 0.082361 -0.102751 21.165930\n-0.110153 -0.020947 0.993694 38.981293 0.080235 0.996328 0.029897 -0.304313 -0.990671 0.083022 -0.108067 21.029051\n-0.114504 -0.019564 0.993230 40.272846 0.079633 0.996408 0.028807 -0.279378 -0.990226 0.082393 -0.112535 20.890678\n-0.118701 -0.019006 0.992748 41.590218 0.079469 0.996428 0.028579 -0.254377 -0.989745 0.082285 -0.116767 20.738014\n-0.121257 -0.019859 0.992423 42.928772 0.079025 0.996433 0.029594 -0.233938 -0.989470 0.082014 -0.119255 20.591402\n-0.123505 -0.022010 0.992100 44.296951 0.079051 0.996359 0.031946 -0.198981 -0.989190 0.082372 -0.121316 20.422365\n-0.125367 -0.022648 0.991852 45.673862 0.079850 0.996266 0.032842 -0.166143 -0.988892 0.083316 -0.123090 20.249565\n-0.127423 -0.022659 0.991590 47.058640 0.081395 0.996128 0.033222 -0.131898 -0.988503 0.084944 -0.125085 20.061178\n-0.128112 -0.022300 0.991509 48.480610 0.082182 0.996070 0.033021 -0.090877 -0.988349 0.085714 -0.125775 19.882339\n-0.128446 -0.021542 0.991482 49.937733 0.083373 0.995990 0.032441 -0.062759 -0.988206 0.086830 -0.126135 19.693361\n-0.128256 -0.020959 0.991520 51.416965 0.083511 0.995998 0.031856 -0.022654 -0.988219 0.086888 -0.125992 19.507542\n-0.127571 -0.020068 0.991626 52.887718 0.083270 0.996049 0.030870 -0.002737 -0.988328 0.086511 -0.125396 19.326820\n-0.126162 -0.020413 0.991800 54.404079 0.082733 0.996089 0.031025 0.034595 -0.988554 0.085969 -0.123980 19.138275\n-0.124567 -0.021073 0.991987 55.936729 0.082680 0.996077 0.031542 0.067266 -0.988760 0.085947 -0.122336 18.953939\n-0.123487 -0.020630 0.992132 57.481045 0.083850 0.995991 0.031147 0.098678 -0.988797 0.087037 -0.121263 18.768293\n-0.122212 -0.019875 0.992305 59.044712 0.083752 0.996027 0.030265 0.135793 -0.988964 0.086806 -0.120062 18.571949\n-0.121346 -0.019341 0.992422 60.652515 0.084403 0.995988 0.029731 0.170180 -0.989015 0.087371 -0.119227 18.365904\n-0.120213 -0.019719 0.992552 62.269352 0.083639 0.996047 0.029918 0.198720 -0.989218 0.086613 -0.118089 18.178583\n-0.119277 -0.021451 0.992629 63.910854 0.084313 0.995936 0.031654 0.233052 -0.989275 0.087467 -0.116984 17.987425\n-0.118125 -0.023022 0.992732 65.572006 0.086876 0.995658 0.033427 0.275811 -0.989191 0.090193 -0.115612 17.785471\n-0.117565 -0.021940 0.992823 67.226852 0.089858 0.995420 0.032638 0.313373 -0.988991 0.093050 -0.115055 17.580694\n-0.116671 -0.020877 0.992951 68.890373 0.090166 0.995428 0.031523 0.340162 -0.989069 0.093209 -0.114255 17.378981\n-0.115479 -0.021125 0.993085 70.585503 0.089946 0.995444 0.031634 0.372657 -0.989229 0.092977 -0.113053 17.185902\n-0.115635 -0.021955 0.993049 72.292686 0.089409 0.995467 0.032419 0.399216 -0.989260 0.092537 -0.113148 16.983309\n-0.115316 -0.022543 0.993073 74.013359 0.089803 0.995412 0.033024 0.443431 -0.989261 0.092989 -0.112762 16.791521\n-0.114796 -0.022627 0.993131 75.753937 0.090996 0.995298 0.033194 0.480006 -0.989213 0.094182 -0.112197 16.578140\n-0.114396 -0.022495 0.993181 77.511368 0.092179 0.995190 0.033158 0.522892 -0.989149 0.095344 -0.111772 16.373444\n-0.114361 -0.021990 0.993196 79.275337 0.093096 0.995118 0.032752 0.554980 -0.989068 0.096208 -0.111755 16.175434\n-0.114810 -0.021123 0.993163 81.056656 0.092360 0.995216 0.031843 0.593354 -0.989085 0.095384 -0.112310 15.978854\n-0.116183 -0.020723 0.993012 82.849091 0.091601 0.995298 0.031488 0.629840 -0.988995 0.094619 -0.113738 15.777287\n-0.117194 -0.020529 0.992897 84.651505 0.090632 0.995393 0.031278 0.656508 -0.988965 0.093653 -0.114793 15.578085\n-0.117981 -0.021098 0.992792 86.471588 0.089818 0.995449 0.031828 0.695502 -0.988945 0.092926 -0.115550 15.379650\n-0.118628 -0.021955 0.992696 88.327156 0.089259 0.995472 0.032683 0.726727 -0.988919 0.092484 -0.116132 15.166502\n-0.119220 -0.022568 0.992611 90.172699 0.087512 0.995612 0.033147 0.767100 -0.989004 0.090818 -0.116722 14.960321\n-0.120389 -0.021738 0.992489 92.024918 0.087623 0.995626 0.032435 0.804761 -0.988852 0.090869 -0.117958 14.747026\n-0.122253 -0.021019 0.992276 93.909225 0.086569 0.995740 0.031758 0.845688 -0.988716 0.089783 -0.119913 14.526943\n-0.126167 -0.020441 0.991798 95.799797 0.086863 0.995720 0.031571 0.880588 -0.988199 0.090134 -0.123851 14.300247\n-0.130410 -0.021103 0.991236 97.712074 0.084770 0.995875 0.032354 0.917634 -0.987830 0.088246 -0.128083 14.058298\n-0.135108 -0.021797 0.990591 99.627701 0.083410 0.995959 0.033292 0.960227 -0.987314 0.087123 -0.132744 13.819330\n-0.139875 -0.021303 0.989940 101.544624 0.082150 0.996072 0.033042 0.995429 -0.986755 0.085946 -0.137576 13.573524\n-0.144467 -0.020092 0.989306 103.469849 0.081977 0.996114 0.032201 1.032681 -0.986108 0.085752 -0.142258 13.310506\n-0.149072 -0.018502 0.988653 105.411507 0.082031 0.996147 0.031011 1.062756 -0.985418 0.085724 -0.146980 13.027239\n-0.153412 -0.018166 0.987995 107.363441 0.080264 0.996298 0.030782 1.096972 -0.984897 0.084023 -0.151386 12.750415\n-0.158299 -0.018677 0.987215 109.323631 0.078370 0.996429 0.031418 1.125195 -0.984276 0.082342 -0.156270 12.455636\n-0.163538 -0.019352 0.986347 111.302696 0.076286 0.996566 0.032201 1.162666 -0.983583 0.080511 -0.161500 12.160956\n-0.168797 -0.019932 0.985449 113.280235 0.075707 0.996580 0.033125 1.199968 -0.982739 0.080197 -0.166710 11.826548\n-0.174371 -0.020178 0.984473 115.264915 0.075830 0.996546 0.033857 1.240371 -0.981756 0.080557 -0.172239 11.496415\n-0.179928 -0.018999 0.983496 117.242470 0.076602 0.996507 0.033265 1.275648 -0.980693 0.081323 -0.177844 11.142294\n-0.185315 -0.018279 0.982509 119.233910 0.075576 0.996601 0.032796 1.314681 -0.979769 0.080332 -0.183303 10.790995\n-0.191099 -0.018463 0.981397 121.247246 0.076018 0.996542 0.033550 1.347788 -0.978623 0.081015 -0.189034 10.422847\n-0.196609 -0.018500 0.980307 123.268036 0.075795 0.996543 0.034008 1.387821 -0.977548 0.080989 -0.194527 10.042426\n-0.201891 -0.017922 0.979244 125.283585 0.075049 0.996610 0.033713 1.430337 -0.976528 0.080298 -0.199862 9.643343\n-0.206334 -0.017868 0.978319 127.308853 0.073839 0.996698 0.033776 1.466893 -0.975692 0.079208 -0.204333 9.244507\n-0.210789 -0.018548 0.977356 129.339432 0.074619 0.996597 0.035007 1.514077 -0.974679 0.080308 -0.208688 8.809826\n-0.214329 -0.019002 0.976577 131.382401 0.076629 0.996402 0.036205 1.560691 -0.973751 0.082594 -0.212102 8.368047\n-0.218044 -0.018507 0.975763 133.412704 0.078092 0.996283 0.036347 1.604907 -0.972810 0.084124 -0.215788 7.930841\n-0.220654 -0.018085 0.975185 135.463409 0.078263 0.996276 0.036184 1.650509 -0.972207 0.084305 -0.218417 7.467916\n-0.223864 -0.018397 0.974447 137.513901 0.077621 0.996309 0.036642 1.696043 -0.971525 0.083841 -0.221609 7.017274\n-0.227287 -0.018108 0.973659 139.587616 0.076427 0.996412 0.036371 1.741375 -0.970824 0.082681 -0.225088 6.557401\n-0.231680 -0.017770 0.972630 141.646484 0.074650 0.996560 0.035989 1.785655 -0.969924 0.080944 -0.229557 6.087780\n-0.236189 -0.017902 0.971542 143.736984 0.074240 0.996575 0.036411 1.838904 -0.968867 0.080727 -0.234051 5.592945\n-0.240618 -0.017701 0.970458 145.814911 0.074222 0.996571 0.036580 1.882116 -0.967778 0.080831 -0.238479 5.096268\n-0.244967 -0.017286 0.969377 147.898743 0.073577 0.996626 0.036365 1.931611 -0.966736 0.080232 -0.242869 4.592013\n-0.249525 -0.017504 0.968210 149.997650 0.072781 0.996670 0.036776 1.976008 -0.965629 0.079643 -0.247420 4.061396\n-0.253502 -0.018199 0.967164 152.097107 0.072501 0.996653 0.037757 2.025367 -0.964614 0.079692 -0.251334 3.531500\n-0.257512 -0.018236 0.966103 154.199768 0.073040 0.996594 0.038280 2.078086 -0.963511 0.080422 -0.255303 2.991678\n-0.261361 -0.017831 0.965076 156.331619 0.073941 0.996522 0.038437 2.126709 -0.962405 0.081405 -0.259134 2.425918\n-0.264722 -0.016919 0.964176 158.446213 0.074063 0.996536 0.037821 2.179207 -0.961476 0.081422 -0.262552 1.859446\n-0.267985 -0.015204 0.963303 160.580658 0.074583 0.996547 0.036477 2.225715 -0.960532 0.081621 -0.265926 1.286691\n-0.271319 -0.014513 0.962380 162.722717 0.074778 0.996546 0.036110 2.271655 -0.959580 0.081763 -0.269296 0.699768\n-0.274540 -0.014100 0.961472 164.866867 0.074566 0.996569 0.035907 2.316639 -0.958680 0.081551 -0.272547 0.107748\n-0.277878 -0.014402 0.960508 167.023865 0.074097 0.996587 0.036380 2.368402 -0.957754 0.081280 -0.275863 -0.492389\n-0.281449 -0.014227 0.959471 169.194458 0.074128 0.996580 0.036522 2.412670 -0.956709 0.081403 -0.279432 -1.100063\n-0.285631 -0.014244 0.958234 171.356262 0.075097 0.996482 0.037198 2.466034 -0.955393 0.082585 -0.283556 -1.733150\n-0.289083 -0.013966 0.957202 173.526337 0.077459 0.996274 0.037929 2.519294 -0.954165 0.085109 -0.286924 -2.377429\n-0.291401 -0.014420 0.956492 175.708206 0.078291 0.996172 0.038870 2.569364 -0.953392 0.086211 -0.289157 -3.019943\n-0.294253 -0.015104 0.955608 177.893051 0.078053 0.996155 0.039780 2.619031 -0.952535 0.086294 -0.291943 -3.672389\n-0.297002 -0.014845 0.954761 180.071930 0.077166 0.996236 0.039494 2.676534 -0.951754 0.085404 -0.294739 -4.340846\n-0.299556 -0.013070 0.953989 182.276215 0.076687 0.996341 0.037730 2.733878 -0.950992 0.084461 -0.297458 -5.016797\n-0.303048 -0.011858 0.952902 184.483322 0.075664 0.996466 0.036463 2.782660 -0.949967 0.083151 -0.301080 -5.696569\n-0.305934 -0.010924 0.951990 186.686478 0.074655 0.996580 0.035427 2.839807 -0.949121 0.081909 -0.304072 -6.394298\n-0.309340 -0.010181 0.950897 188.906525 0.073233 0.996718 0.034495 2.882941 -0.948127 0.080308 -0.307579 -7.084190\n-0.312679 -0.010017 0.949806 191.119415 0.073455 0.996695 0.034693 2.930905 -0.947014 0.080616 -0.310909 -7.799603\n-0.316284 -0.011052 0.948600 193.351212 0.072951 0.996688 0.035936 2.981245 -0.945856 0.080567 -0.314430 -8.523835\n-0.319346 -0.012050 0.947562 195.575958 0.072397 0.996687 0.037074 3.030843 -0.944869 0.080440 -0.317416 -9.252023\n-0.323156 -0.012289 0.946266 197.809509 0.072456 0.996659 0.037688 3.080871 -0.943568 0.080742 -0.321186 -10.009183\n-0.326766 -0.012159 0.945027 200.042801 0.071531 0.996731 0.037558 3.137273 -0.942394 0.079872 -0.324828 -10.765425\n-0.330422 -0.011675 0.943761 202.286194 0.070711 0.996807 0.037088 3.188387 -0.941181 0.078989 -0.328542 -11.514297\n-0.333314 -0.011556 0.942745 204.521683 0.070363 0.996831 0.037096 3.239065 -0.940187 0.078699 -0.331445 -12.283421\n-0.335717 -0.011049 0.941898 206.752289 0.071277 0.996766 0.037098 3.284925 -0.939262 0.079590 -0.333844 -13.075359\n-0.337979 -0.011369 0.941085 208.991272 0.072218 0.996666 0.037977 3.336594 -0.938379 0.080799 -0.336031 -13.865844\n-0.340495 -0.011786 0.940172 211.226242 0.073627 0.996517 0.039157 3.391472 -0.937359 0.082555 -0.338442 -14.663661\n-0.342378 -0.011526 0.939492 213.472290 0.074584 0.996436 0.039405 3.447562 -0.936597 0.083562 -0.340298 -15.467135\n-0.344699 -0.010971 0.938649 215.714813 0.074475 0.996460 0.038997 3.496588 -0.935754 0.083348 -0.342662 -16.283470\n-0.347523 -0.010789 0.937609 217.958740 0.074022 0.996498 0.038903 3.554284 -0.934745 0.082923 -0.345507 -17.106812\n-0.349707 -0.009840 0.936808 220.214020 0.073632 0.996563 0.037954 3.607943 -0.933961 0.082252 -0.347780 -17.935127\n-0.352527 -0.009045 0.935758 222.459564 0.073084 0.996633 0.037167 3.651909 -0.932943 0.081492 -0.350679 -18.754147\n-0.354883 -0.008888 0.934869 224.711288 0.071328 0.996783 0.036553 3.700705 -0.932186 0.079654 -0.353107 -19.581894\n-0.357539 -0.008238 0.933862 226.948486 0.071677 0.996769 0.036236 3.750399 -0.931144 0.079893 -0.355793 -20.421202\n-0.360450 -0.009096 0.932734 229.190201 0.072684 0.996638 0.037808 3.794791 -0.929943 0.081423 -0.358577 -21.274401\n-0.362391 -0.010176 0.931970 231.432922 0.072992 0.996559 0.039263 3.844992 -0.929163 0.082255 -0.360402 -22.128353\n-0.365266 -0.010745 0.930841 233.674469 0.072211 0.996593 0.039839 3.898557 -0.928098 0.081769 -0.363245 -22.985729\n-0.367892 -0.010781 0.929806 235.925415 0.071091 0.996680 0.039684 3.948084 -0.927147 0.080701 -0.365904 -23.858902\n-0.371065 -0.011380 0.928537 238.175308 0.069341 0.996794 0.039927 4.007004 -0.926014 0.079202 -0.369086 -24.736954\n-0.374476 -0.012262 0.927156 240.408676 0.068148 0.996844 0.040709 4.061803 -0.924729 0.078428 -0.372458 -25.608658\n-0.377920 -0.012390 0.925756 242.635345 0.067515 0.996879 0.040903 4.115996 -0.923373 0.077960 -0.375904 -26.497625\n-0.381294 -0.012840 0.924364 244.878372 0.066746 0.996912 0.041380 4.175131 -0.922041 0.077475 -0.379260 -27.397142\n-0.384577 -0.013409 0.922995 247.124100 0.065903 0.996944 0.041942 4.226794 -0.920737 0.076958 -0.382519 -28.293518\n-0.388426 -0.013686 0.921378 249.349854 0.064933 0.996998 0.042183 4.292213 -0.919189 0.076213 -0.386371 -29.220980\n-0.392107 -0.013387 0.919822 251.577072 0.064726 0.997015 0.042102 4.352464 -0.917640 0.076045 -0.390070 -30.140852\n-0.395361 -0.013130 0.918432 253.808319 0.065074 0.996985 0.042265 4.410416 -0.916218 0.076476 -0.393315 -31.082125\n-0.399190 -0.013402 0.916770 256.017700 0.064631 0.996994 0.042718 4.466665 -0.914588 0.076305 -0.397124 -32.015995\n-0.402596 -0.013394 0.915280 258.243652 0.063524 0.997074 0.042533 4.523783 -0.913171 0.075265 -0.400567 -32.966003\n-0.406284 -0.013164 0.913652 260.453064 0.063740 0.997052 0.042709 4.597222 -0.911521 0.075589 -0.404247 -33.928249\n-0.409869 -0.012455 0.912059 262.670227 0.064544 0.997004 0.042621 4.670192 -0.909858 0.076337 -0.407837 -34.900265\n-0.413161 -0.011542 0.910585 264.868866 0.065743 0.996933 0.042466 4.730047 -0.908282 0.077409 -0.411135 -35.886246\n-0.416377 -0.010073 0.909136 267.067047 0.067409 0.996844 0.041918 4.793633 -0.906690 0.078738 -0.414384 -36.887234\n-0.419412 -0.009440 0.907747 269.273224 0.068545 0.996762 0.042036 4.862449 -0.905204 0.079852 -0.417407 -37.903378\n-0.422310 -0.007544 0.906420 271.480743 0.070384 0.996673 0.041087 4.932333 -0.903715 0.081149 -0.420374 -38.923279\n-0.424987 -0.005485 0.905183 273.688568 0.072264 0.996584 0.039967 4.994406 -0.902311 0.082397 -0.423139 -39.946873\n-0.427676 -0.005037 0.903918 275.907196 0.073108 0.996516 0.040143 5.060288 -0.900971 0.083252 -0.425817 -40.982052\n-0.431012 -0.004594 0.902335 278.126160 0.073116 0.996521 0.039999 5.120998 -0.899379 0.083215 -0.429176 -42.020950\n-0.434118 -0.004476 0.900845 280.369019 0.072281 0.996591 0.039784 5.191811 -0.897952 0.082385 -0.432314 -43.076984\n-0.437532 -0.003412 0.899196 282.611694 0.072883 0.996568 0.039245 5.268353 -0.896244 0.082707 -0.435782 -44.162483\n-0.440842 -0.003459 0.897578 284.861877 0.071964 0.996637 0.039185 5.330413 -0.894695 0.081868 -0.439111 -45.255474\n-0.443306 -0.003493 0.896363 287.126160 0.070754 0.996736 0.038877 5.397781 -0.893573 0.080656 -0.441612 -46.338608\n-0.446179 -0.003283 0.894938 289.378906 0.071680 0.996650 0.039393 5.473852 -0.892068 0.081725 -0.444449 -47.456932\n-0.448376 -0.003506 0.893838 291.628967 0.073016 0.996507 0.040536 5.543000 -0.890858 0.083440 -0.446553 -48.576202\n-0.450155 -0.003919 0.892942 293.885803 0.073946 0.996392 0.041650 5.607831 -0.889883 0.084778 -0.448241 -49.703239\n-0.452414 -0.004281 0.891798 296.174469 0.073757 0.996383 0.042200 5.679173 -0.888753 0.084868 -0.450462 -50.838047\n-0.454561 -0.004941 0.890702 298.438751 0.073431 0.996373 0.043002 5.760910 -0.887684 0.084952 -0.452549 -51.982769\n-0.456960 -0.005336 0.889471 300.721710 0.074219 0.996266 0.044106 5.844817 -0.886385 0.086171 -0.454858 -53.144066\n-0.458945 -0.005686 0.888447 303.015228 0.074606 0.996201 0.044914 5.922556 -0.885327 0.086896 -0.456777 -54.310036\n-0.460905 -0.004846 0.887436 305.317596 0.074588 0.996235 0.044179 6.004673 -0.884309 0.086554 -0.458809 -55.485180\n-0.463097 -0.003324 0.886301 307.631348 0.073619 0.996393 0.042203 6.077140 -0.883245 0.084792 -0.461182 -56.678925\n-0.466286 -0.004175 0.884624 309.953674 0.071142 0.996573 0.042202 6.149312 -0.881769 0.082613 -0.464391 -57.869526\n-0.470322 -0.006413 0.882472 312.288696 0.068558 0.996686 0.043781 6.223060 -0.879828 0.081092 -0.468324 -59.075245\n-0.475154 -0.008615 0.879860 314.630341 0.066034 0.996783 0.045421 6.303677 -0.877421 0.079682 -0.473057 -60.302357\n-0.479595 -0.009685 0.877436 316.972412 0.065115 0.996789 0.046593 6.398011 -0.875070 0.079480 -0.477425 -61.549858\n-0.483638 -0.009794 0.875213 319.316437 0.065339 0.996743 0.047260 6.490807 -0.872826 0.080042 -0.481423 -62.810890\n-0.487564 -0.008531 0.873046 321.652130 0.067448 0.996596 0.047406 6.587177 -0.870478 0.081998 -0.485329 -64.104584\n-0.491386 -0.007804 0.870907 323.989960 0.068402 0.996525 0.047524 6.674463 -0.868252 0.082924 -0.489145 -65.397346\n-0.495155 -0.008581 0.868762 326.326111 0.068280 0.996474 0.048760 6.762047 -0.866117 0.083463 -0.492823 -66.712280\n-0.499161 -0.009980 0.866452 328.639435 0.068006 0.996398 0.050654 6.852180 -0.863837 0.084208 -0.496684 -68.028099\n-0.502961 -0.011491 0.864232 330.982727 0.067249 0.996360 0.052385 6.944685 -0.861689 0.084467 -0.500358 -69.359161\n-0.506695 -0.011966 0.862042 333.308838 0.066871 0.996346 0.053136 7.039320 -0.859528 0.084569 -0.504044 -70.687843\n-0.510732 -0.012597 0.859648 335.633789 0.065877 0.996380 0.053740 7.138666 -0.857212 0.084078 -0.508053 -72.029793\n-0.515141 -0.012700 0.857011 337.945343 0.066742 0.996260 0.054882 7.235377 -0.854503 0.085470 -0.512366 -73.396362\n-0.519081 -0.013525 0.854618 340.233795 0.067194 0.996135 0.056577 7.336938 -0.852080 0.086793 -0.516166 -74.751991\n-0.522748 -0.014721 0.852360 342.535309 0.066005 0.996150 0.057685 7.443983 -0.849928 0.086415 -0.519764 -76.126190\n-0.526098 -0.015607 0.850281 344.835907 0.065123 0.996156 0.058578 7.555956 -0.847927 0.086191 -0.523060 -77.512688\n-0.529511 -0.016683 0.848139 347.108124 0.065026 0.996067 0.060190 7.666697 -0.845807 0.087022 -0.526344 -78.898102\n-0.532245 -0.017560 0.846408 349.389343 0.063708 0.996119 0.060727 7.776175 -0.844190 0.086245 -0.529061 -80.295967\n-0.535076 -0.017807 0.844616 351.650574 0.061254 0.996329 0.059811 7.897658 -0.842580 0.083739 -0.532021 -81.695122\n-0.538256 -0.019273 0.842561 353.937805 0.056368 0.996677 0.058808 8.014718 -0.840894 0.079147 -0.535381 -83.105438\n-0.541481 -0.022461 0.840413 356.214355 0.052235 0.996813 0.060297 8.118649 -0.839088 0.076549 -0.538582 -84.532616\n-0.544771 -0.023044 0.838268 358.483398 0.053580 0.996623 0.062218 8.248891 -0.836872 0.078809 -0.541697 -85.974274\n-0.547554 -0.022400 0.836470 360.743317 0.054106 0.996602 0.062107 8.369031 -0.835019 0.079265 -0.544482 -87.430374\n-0.550324 -0.023472 0.834621 363.005524 0.052002 0.996701 0.062319 8.490545 -0.833330 0.077698 -0.547288 -88.881935\n-0.553644 -0.027213 0.832309 365.253693 0.048828 0.996686 0.065067 8.615973 -0.831321 0.076663 -0.550480 -90.338020\n-0.556642 -0.029282 0.830236 367.508392 0.047797 0.996594 0.067195 8.748408 -0.829377 0.077086 -0.553346 -91.818718\n-0.559421 -0.029945 0.828343 369.767273 0.046603 0.996630 0.067502 8.891455 -0.827573 0.076366 -0.556140 -93.302994\n-0.562268 -0.030417 0.826396 372.032227 0.045807 0.996643 0.067850 9.029961 -0.825685 0.076005 -0.558987 -94.809509\n-0.565058 -0.030746 0.824478 374.278412 0.046015 0.996576 0.068699 9.167123 -0.823767 0.076757 -0.561709 -96.317932\n-0.567562 -0.031466 0.822729 376.508667 0.045359 0.996557 0.069406 9.302192 -0.822080 0.076710 -0.564181 -97.827118\n-0.570161 -0.032726 0.820881 378.735840 0.043028 0.996645 0.069619 9.424552 -0.820406 0.075015 -0.566840 -99.341400\n-0.572418 -0.035676 0.819185 380.940704 0.034287 0.997138 0.067385 9.539768 -0.819245 0.066660 -0.569556 -100.823776\n-0.574849 -0.033109 0.817590 383.140137 0.026633 0.997895 0.059137 9.684357 -0.817826 0.055770 -0.572757 -102.301308\n-0.578089 -0.022816 0.815655 385.329712 0.035783 0.997939 0.053276 9.823243 -0.815189 0.059985 -0.576081 -103.846107\n-0.579391 -0.020784 0.814785 387.543060 0.042745 0.997524 0.055841 9.926766 -0.813928 0.067182 -0.577068 -105.426384\n-0.581054 -0.029789 0.813320 389.771576 0.041427 0.996952 0.066111 10.039250 -0.812810 0.072107 -0.578049 -107.001106\n-0.583470 -0.041177 0.811090 392.005127 0.035893 0.996430 0.076407 10.170867 -0.811341 0.073694 -0.579909 -108.568123\n-0.586070 -0.047963 0.808839 394.224121 0.030259 0.996255 0.081001 10.328445 -0.809695 0.071947 -0.582424 -110.110924\n-0.588731 -0.047701 0.806920 396.424744 0.029911 0.996288 0.080718 10.514842 -0.807776 0.071657 -0.585119 -111.675087\n-0.591719 -0.042994 0.804997 398.613342 0.035137 0.996252 0.079037 10.684156 -0.805378 0.075052 -0.587990 -113.261940\n-0.593732 -0.037519 0.803788 400.762787 0.039408 0.996358 0.075616 10.828049 -0.803697 0.076571 -0.590091 -114.844353\n-0.596258 -0.034440 0.802054 402.932495 0.040082 0.996556 0.072590 10.972507 -0.801792 0.075430 -0.592824 -116.431358\n-0.599123 -0.034790 0.799901 405.111420 0.039666 0.996539 0.073052 11.122506 -0.799674 0.075496 -0.595670 -118.032349\n-0.601781 -0.035147 0.797888 407.269104 0.040402 0.996412 0.074364 11.279646 -0.797639 0.076987 -0.598202 -119.629707\n-0.604204 -0.036247 0.796005 409.424652 0.040673 0.996260 0.076238 11.434736 -0.795791 0.078439 -0.600470 -121.235222\n-0.605960 -0.036504 0.794657 411.583374 0.040771 0.996208 0.076852 11.584021 -0.794449 0.078969 -0.602175 -122.852997\n-0.607450 -0.036150 0.793535 413.747833 0.041197 0.996186 0.076918 11.741623 -0.793289 0.079415 -0.603644 -124.481415\n-0.608404 -0.035880 0.792816 415.883240 0.041992 0.996123 0.077305 11.901690 -0.792516 0.080325 -0.604538 -126.102318\n-0.609765 -0.036430 0.791745 418.035095 0.043052 0.995946 0.078983 12.066042 -0.791412 0.082248 -0.605724 -127.741852\n-0.610727 -0.037167 0.790969 420.173553 0.044909 0.995664 0.081461 12.235188 -0.790567 0.085272 -0.606409 -129.380981\n-0.611550 -0.038040 0.790291 422.305695 0.046333 0.995408 0.083767 12.400093 -0.789848 0.087844 -0.606979 -131.015594\n-0.612482 -0.038399 0.789551 424.441254 0.044821 0.995526 0.083186 12.561329 -0.789213 0.086339 -0.608020 -132.637054\n-0.613713 -0.038355 0.788597 426.575195 0.041182 0.995905 0.080487 12.720086 -0.788455 0.081872 -0.609620 -134.257324\n-0.614924 -0.038441 0.787649 428.704956 0.038545 0.996152 0.078709 12.890203 -0.787644 0.078760 -0.611076 -135.875061\n-0.615924 -0.039140 0.786833 430.838409 0.037716 0.996155 0.079076 13.063081 -0.786902 0.078381 -0.612079 -137.517578\n-0.616656 -0.040696 0.786180 432.962982 0.035755 0.996184 0.079612 13.217072 -0.786420 0.077203 -0.612848 -139.148865\n-0.617356 -0.041297 0.785599 435.084534 0.033269 0.996357 0.078519 13.373137 -0.785980 0.074610 -0.613733 -140.786743\n-0.617971 -0.042139 0.785070 437.212494 0.032068 0.996381 0.078723 13.548465 -0.785546 0.073824 -0.614384 -142.427826\n-0.618318 -0.043458 0.784725 439.327637 0.032432 0.996209 0.080724 13.717458 -0.785258 0.075363 -0.614564 -144.076065\n-0.618215 -0.044629 0.784741 441.456299 0.034438 0.995890 0.083767 13.896633 -0.785254 0.078811 -0.614137 -145.731659\n-0.617112 -0.046398 0.785506 443.575165 0.038233 0.995313 0.088828 14.089123 -0.785946 0.084849 -0.612446 -147.394699\n-0.615726 -0.048202 0.786485 445.697784 0.041768 0.994727 0.093664 14.270130 -0.786853 0.090521 -0.610466 -149.059494\n-0.613751 -0.051060 0.787847 447.823975 0.037645 0.994879 0.093803 14.438115 -0.788602 0.087231 -0.608685 -150.681854\n-0.612177 -0.052071 0.789005 449.955780 0.030635 0.995518 0.089469 14.619838 -0.790127 0.078942 -0.607838 -152.288467\n-0.610248 -0.052817 0.790448 452.087372 0.025851 0.995916 0.086504 14.827656 -0.791789 0.073222 -0.606390 -153.897491\n-0.607392 -0.052591 0.792660 454.220795 0.024662 0.996077 0.084984 15.019326 -0.794019 0.071167 -0.603712 -155.517822\n-0.604654 -0.052290 0.794770 456.343262 0.026017 0.996013 0.085324 15.204606 -0.796063 0.072269 -0.600884 -157.125977\n-0.601459 -0.052258 0.797192 458.483398 0.027266 0.995934 0.085857 15.386867 -0.798438 0.073376 -0.597589 -158.731659\n-0.598863 -0.052523 0.799127 460.623322 0.024929 0.996141 0.084153 15.543918 -0.800463 0.070317 -0.595243 -160.310181\n-0.596730 -0.057119 0.800406 462.771759 0.017844 0.996272 0.084400 15.709531 -0.802243 0.064647 -0.593487 -161.876801\n-0.595080 -0.063088 0.801186 464.944550 0.009702 0.996278 0.085657 15.884868 -0.803608 0.058746 -0.592253 -163.427628\n-0.594292 -0.066000 0.801536 467.109467 0.003319 0.996417 0.084508 16.063339 -0.804242 0.052882 -0.591944 -164.984680\n-0.593657 -0.065818 0.802022 469.266968 0.001527 0.996556 0.082913 16.258425 -0.804717 0.050447 -0.591512 -166.543503\n-0.592507 -0.065781 0.802875 471.422150 0.002964 0.996476 0.083830 16.442329 -0.805560 0.052049 -0.590223 -168.122604\n-0.590967 -0.067484 0.803868 473.589355 0.004741 0.996187 0.087114 16.617441 -0.806682 0.055292 -0.588394 -169.703674\n-0.589380 -0.069733 0.804840 475.740509 0.003785 0.996018 0.089070 16.792639 -0.807847 0.055543 -0.586769 -171.262115\n-0.588139 -0.070598 0.805673 477.886719 0.001663 0.996075 0.088496 16.975929 -0.808758 0.053387 -0.585713 -172.823517\n-0.587344 -0.069384 0.806358 480.026642 0.000839 0.996266 0.086336 17.165195 -0.809337 0.051386 -0.585092 -174.361359\n-0.586116 -0.067904 0.807376 482.180786 0.001790 0.996371 0.085098 17.354656 -0.810225 0.051322 -0.583868 -175.907547\n-0.584834 -0.067537 0.808336 484.339264 0.003636 0.996299 0.085873 17.535749 -0.811145 0.053161 -0.582424 -177.477234\n-0.583556 -0.067891 0.809230 486.509857 0.005419 0.996151 0.087481 17.713263 -0.812055 0.055435 -0.580943 -179.030884\n-0.582093 -0.068561 0.810227 488.681458 0.005559 0.996080 0.088281 17.895725 -0.813103 0.055891 -0.579430 -180.581024\n-0.580846 -0.069534 0.811038 490.849762 0.004858 0.996031 0.088873 18.074169 -0.813999 0.055562 -0.578203 -182.118774\n-0.579846 -0.070303 0.811687 493.009003 0.005044 0.995941 0.089865 18.266399 -0.814710 0.056202 -0.577138 -183.655792\n-0.578706 -0.071467 0.812399 495.180573 0.004750 0.995841 0.090988 18.455231 -0.815522 0.056514 -0.575960 -185.193253\n-0.578048 -0.070890 0.812918 497.357941 0.004353 0.995937 0.089946 18.648779 -0.815991 0.055532 -0.575391 -186.722412\n-0.577391 -0.069873 0.813472 499.529968 0.004667 0.996033 0.088866 18.840261 -0.816454 0.055107 -0.574774 -188.245544\n-0.576631 -0.070597 0.813949 501.706207 0.004824 0.995948 0.089800 19.028215 -0.816991 0.055708 -0.573954 -189.780106\n-0.576124 -0.071707 0.814211 503.891052 0.004322 0.995863 0.090763 19.213081 -0.817351 0.055810 -0.573430 -191.297958\n-0.575630 -0.073269 0.814421 506.064026 0.003220 0.995767 0.091860 19.405655 -0.817704 0.055500 -0.572957 -192.818161\n-0.575601 -0.073934 0.814382 508.252228 0.002644 0.995731 0.092266 19.602535 -0.817727 0.055262 -0.572948 -194.329147\n-0.575706 -0.073428 0.814353 510.435150 0.003168 0.995752 0.092024 19.803431 -0.817651 0.055559 -0.573027 -195.851974\n-0.575991 -0.073550 0.814141 512.622009 0.003323 0.995725 0.092306 19.997833 -0.817449 0.055873 -0.573284 -197.387161\n-0.575915 -0.075012 0.814061 514.804688 0.002798 0.995595 0.093719 20.194199 -0.817505 0.056251 -0.573168 -198.906921\n-0.576005 -0.076558 0.813854 516.991638 0.002170 0.995458 0.095177 20.394386 -0.817444 0.056588 -0.573222 -200.426590\n-0.576071 -0.077572 0.813711 519.180176 0.001723 0.995369 0.096109 20.596319 -0.817398 0.056768 -0.573269 -201.952179\n-0.576126 -0.077461 0.813682 521.369812 0.001303 0.995411 0.095684 20.805456 -0.817360 0.056187 -0.573382 -203.475403\n-0.576013 -0.077194 0.813788 523.548767 0.001284 0.995445 0.095334 21.016562 -0.817440 0.055958 -0.573290 -204.997513\n-0.575587 -0.076820 0.814124 525.722168 0.001741 0.995460 0.095162 21.220766 -0.817738 0.056192 -0.572841 -206.515411\n-0.575245 -0.077145 0.814335 527.914673 0.001952 0.995411 0.095677 21.423321 -0.817978 0.056627 -0.572455 -208.046402\n-0.575082 -0.077543 0.814413 530.102234 0.001518 0.995395 0.095847 21.627300 -0.818095 0.056356 -0.572316 -209.571075\n-0.574881 -0.077556 0.814553 532.279175 0.000951 0.995434 0.095450 21.825882 -0.818237 0.055647 -0.572182 -211.081665\n-0.574562 -0.078321 0.814705 534.468811 0.000497 0.995377 0.096041 22.035696 -0.818461 0.055587 -0.571867 -212.594818\n-0.574241 -0.079514 0.814816 536.642578 0.000494 0.995238 0.097469 22.245516 -0.818686 0.056373 -0.571468 -214.120987\n-0.573711 -0.080730 0.815070 538.839600 0.000628 0.995087 0.099002 22.462349 -0.819058 0.057310 -0.570842 -215.643036\n-0.573312 -0.080580 0.815365 541.021118 0.000954 0.995086 0.099012 22.673706 -0.819337 0.057542 -0.570417 -217.147064\n-0.573013 -0.079705 0.815661 543.210388 0.002120 0.995112 0.098730 22.887224 -0.819544 0.058303 -0.570043 -218.674835\n-0.572849 -0.079989 0.815749 545.389709 0.002493 0.995052 0.099322 23.100296 -0.819657 0.058930 -0.569815 -220.189972\n-0.572472 -0.081138 0.815900 547.587646 0.002059 0.994946 0.100388 23.315153 -0.819922 0.059149 -0.569412 -221.704681\n-0.572191 -0.081490 0.816062 549.764221 0.001366 0.994955 0.100311 23.532366 -0.820119 0.058512 -0.569193 -223.207352\n-0.572302 -0.081675 0.815966 551.954834 0.001032 0.994955 0.100315 23.748608 -0.820042 0.058252 -0.569330 -224.722839\n-0.572390 -0.082925 0.815778 554.146484 0.000819 0.994815 0.101699 23.969782 -0.819981 0.058880 -0.569354 -226.236023\n-0.572197 -0.084576 0.815743 556.327026 0.000472 0.994634 0.103454 24.191833 -0.820116 0.059581 -0.569087 -227.745499\n-0.572315 -0.085688 0.815545 558.515076 -0.000332 0.994550 0.104263 24.416615 -0.820034 0.059400 -0.569224 -229.246078\n-0.572402 -0.084523 0.815605 560.705750 -0.000802 0.994730 0.102524 24.641283 -0.819973 0.058031 -0.569453 -230.750244\n-0.572475 -0.082450 0.815766 562.888367 -0.000923 0.994995 0.099917 24.866667 -0.819922 0.056447 -0.569686 -232.260635\n-0.572544 -0.082696 0.815693 565.068359 -0.001061 0.994974 0.100127 25.086069 -0.819873 0.056462 -0.569754 -233.775024\n-0.572395 -0.084102 0.815654 567.258301 -0.000527 0.994764 0.102200 25.313759 -0.819978 0.058069 -0.569442 -235.295197\n-0.572103 -0.085416 0.815722 569.463379 0.000468 0.994528 0.104467 25.539589 -0.820182 0.060147 -0.568933 -236.822174\n-0.572027 -0.085087 0.815809 571.649048 0.001313 0.994509 0.104646 25.764273 -0.820233 0.060932 -0.568774 -238.333191\n-0.571980 -0.084313 0.815923 573.840820 0.001617 0.994586 0.103909 25.996866 -0.820266 0.060753 -0.568746 -239.847260\n-0.571994 -0.084059 0.815939 576.032837 0.002144 0.994579 0.103966 26.229128 -0.820255 0.061217 -0.568713 -241.372391\n-0.571741 -0.084166 0.816105 578.237244 0.003074 0.994497 0.104717 26.463257 -0.820428 0.062379 -0.568337 -242.894165\n-0.571655 -0.085139 0.816065 580.434082 0.003712 0.994323 0.106336 26.691738 -0.820486 0.063817 -0.568094 -244.416611\n-0.571294 -0.086237 0.816202 582.624512 0.003406 0.994207 0.107429 26.920515 -0.820738 0.064153 -0.567691 -245.931427\n-0.571120 -0.086285 0.816319 584.820190 0.003602 0.994187 0.107605 27.162003 -0.820858 0.064395 -0.567490 -247.448318\n-0.570594 -0.086283 0.816687 587.017578 0.004565 0.994117 0.108218 27.399632 -0.821220 0.065477 -0.566843 -248.967178\n-0.570148 -0.087320 0.816889 589.202454 0.004843 0.993961 0.109628 27.631516 -0.821528 0.066460 -0.566282 -250.480286\n-0.569729 -0.088718 0.817030 591.393982 0.003653 0.993873 0.110468 27.874260 -0.821825 0.065921 -0.565914 -251.980743\n-0.569077 -0.090350 0.817306 593.595093 0.002072 0.993784 0.111303 28.119450 -0.822282 0.065034 -0.565352 -253.482224\n-0.568558 -0.089477 0.817762 595.784729 0.002443 0.993879 0.110445 28.369583 -0.822639 0.064792 -0.564860 -254.990952\n-0.567862 -0.088128 0.818392 597.987854 0.003867 0.993955 0.109716 28.621639 -0.823115 0.065468 -0.564089 -256.504761\n-0.567104 -0.087734 0.818960 600.188965 0.004896 0.993934 0.109869 28.857346 -0.823632 0.066316 -0.563234 -258.017761\n-0.566442 -0.088745 0.819309 602.394836 0.004967 0.993799 0.111079 29.096149 -0.824086 0.066989 -0.562489 -259.523254\n-0.565620 -0.089707 0.819772 604.580872 0.005320 0.993648 0.112405 29.346014 -0.824649 0.067940 -0.561550 -261.018433\n-0.564943 -0.089330 0.820280 606.774353 0.005476 0.993695 0.111986 29.585180 -0.825112 0.067758 -0.560891 -262.507446\n-0.563918 -0.089635 0.820952 608.973450 0.004877 0.993713 0.111847 29.830856 -0.825816 0.067076 -0.559936 -263.996368\n-0.563040 -0.089921 0.821523 611.168518 0.004545 0.993711 0.111883 30.079296 -0.826417 0.066729 -0.559090 -265.484039\n-0.562461 -0.090707 0.821833 613.367188 0.004090 0.993647 0.112470 30.328144 -0.826813 0.066621 -0.558517 -266.975647\n-0.561543 -0.091382 0.822386 615.562622 0.003571 0.993606 0.112846 30.578152 -0.827440 0.066305 -0.557626 -268.461243\n-0.560763 -0.092707 0.822770 617.767151 0.002151 0.993545 0.113415 30.822544 -0.827974 0.065369 -0.556944 -269.932159\n-0.560311 -0.093462 0.822992 619.972717 0.000655 0.993563 0.113278 31.077938 -0.828282 0.064010 -0.556643 -271.409973\n-0.560366 -0.092997 0.823008 622.171143 -0.000019 0.993678 0.112269 31.328382 -0.828245 0.062895 -0.556825 -272.881073\n-0.560623 -0.093343 0.822793 624.394470 0.000583 0.993582 0.113115 31.585995 -0.828071 0.063894 -0.556971 -274.372223\n-0.560510 -0.094744 0.822710 626.604065 0.000599 0.993388 0.114807 31.838762 -0.828147 0.064844 -0.556747 -275.852692\n-0.560351 -0.096114 0.822660 628.809814 -0.000190 0.993259 0.115916 32.098709 -0.828255 0.064797 -0.556592 -277.328979\n-0.560411 -0.095531 0.822687 631.018982 -0.000485 0.993363 0.115020 32.354599 -0.828215 0.064060 -0.556738 -278.804932\n-0.560348 -0.093820 0.822926 633.222839 -0.000089 0.993571 0.113214 32.615894 -0.828257 0.063366 -0.556754 -280.282959\n-0.560258 -0.092708 0.823114 635.422791 0.000227 0.993700 0.112076 32.866196 -0.828318 0.062978 -0.556707 -281.754730\n-0.560196 -0.092841 0.823141 637.631958 0.000555 0.993657 0.112450 33.121140 -0.828360 0.063451 -0.556591 -283.232300\n-0.560030 -0.093872 0.823137 639.848145 0.000613 0.993513 0.113718 33.372730 -0.828472 0.064190 -0.556339 -284.722656\n-0.559871 -0.094968 0.823119 642.062561 0.001026 0.993330 0.115304 33.622368 -0.828579 0.065400 -0.556039 -286.202118\n-0.559746 -0.095271 0.823169 644.293213 0.001350 0.993263 0.115876 33.885021 -0.828663 0.065972 -0.555846 -287.692383\n-0.559532 -0.095348 0.823306 646.514099 0.001252 0.993262 0.115881 34.140770 -0.828808 0.065870 -0.555642 -289.175842\n-0.558955 -0.095369 0.823696 648.716675 0.000960 0.993289 0.115656 34.399357 -0.829198 0.065437 -0.555112 -290.647339\n-0.558552 -0.095530 0.823950 650.935120 0.001195 0.993252 0.115969 34.663387 -0.829469 0.065759 -0.554669 -292.130676\n-0.557999 -0.096093 0.824259 653.159424 0.001276 0.993172 0.116649 34.920506 -0.829841 0.066142 -0.554066 -293.618011\n-0.557646 -0.096816 0.824414 655.377258 0.000842 0.993108 0.117196 35.182247 -0.830079 0.066048 -0.553721 -295.088776\n-0.557167 -0.097339 0.824676 657.595947 0.000514 0.993065 0.117562 35.447468 -0.830400 0.065925 -0.553254 -296.564972\n-0.556836 -0.096670 0.824978 659.825073 0.000638 0.993154 0.116807 35.709255 -0.830622 0.065569 -0.552963 -298.050873\n-0.556187 -0.097077 0.825368 662.033325 0.000776 0.993093 0.117327 35.971077 -0.831057 0.065896 -0.552270 -299.509827\n-0.556012 -0.097884 0.825390 664.262085 0.001313 0.992937 0.118638 36.233490 -0.831173 0.067048 -0.551956 -300.986481\n-0.555548 -0.098053 0.825683 666.474915 0.001616 0.992893 0.118997 36.499294 -0.831483 0.067443 -0.551441 -302.458588\n-0.555010 -0.097152 0.826151 668.700745 0.002041 0.992994 0.118143 36.767365 -0.831841 0.067257 -0.550923 -303.928680\n-0.554515 -0.096125 0.826604 670.920105 0.002368 0.993120 0.117078 37.028446 -0.832171 0.066879 -0.550472 -305.395294\n-0.553931 -0.096389 0.826964 673.145325 0.003103 0.993030 0.117823 37.290836 -0.832557 0.067831 -0.549770 -306.853455\n-0.553482 -0.097108 0.827181 675.371155 0.003704 0.992883 0.119039 37.552486 -0.832853 0.068950 -0.549183 -308.327667\n-0.553040 -0.098082 0.827361 677.596558 0.003904 0.992730 0.120295 37.817814 -0.833146 0.069758 -0.548637 -309.783752\n-0.552585 -0.098713 0.827591 679.815796 0.003796 0.992653 0.120936 38.085640 -0.833448 0.069969 -0.548150 -311.246094\n-0.552313 -0.099525 0.827674 682.041382 0.003283 0.992580 0.121545 38.358387 -0.833630 0.069849 -0.547889 -312.705872\n-0.552044 -0.098958 0.827922 684.275208 0.003438 0.992654 0.120940 38.632408 -0.833808 0.069611 -0.547649 -314.168762\n-0.551334 -0.098380 0.828464 686.486084 0.003607 0.992733 0.120287 38.904980 -0.834277 0.069306 -0.546973 -315.621918\n-0.551206 -0.098192 0.828571 688.720459 0.003445 0.992775 0.119943 39.175838 -0.834362 0.068968 -0.546885 -317.079407\n-0.550832 -0.098344 0.828802 690.941040 0.003029 0.992792 0.119815 39.442547 -0.834611 0.068508 -0.546563 -318.530945\n-0.550960 -0.098938 0.828646 693.176270 0.002760 0.992726 0.120364 39.717171 -0.834527 0.068603 -0.546679 -319.988129\n-0.550976 -0.099948 0.828515 695.396606 0.001971 0.992644 0.121058 39.981880 -0.834519 0.068332 -0.546725 -321.437897\n-0.551243 -0.100522 0.828267 697.624634 0.000969 0.992638 0.121116 40.262390 -0.834344 0.067567 -0.547087 -322.886871\n-0.551492 -0.100697 0.828080 699.856628 0.000316 0.992662 0.120921 40.534218 -0.834180 0.066948 -0.547413 -324.339752\n-0.551701 -0.100566 0.827957 702.076904 0.000140 0.992693 0.120668 40.807152 -0.834042 0.066689 -0.547656 -325.789978\n-0.552118 -0.101177 0.827605 704.309753 0.000375 0.992580 0.121595 41.083603 -0.833766 0.067445 -0.547983 -327.252075\n-0.552765 -0.101693 0.827109 706.543091 0.000965 0.992448 0.122666 41.359100 -0.833337 0.068603 -0.548492 -328.716644\n-0.553031 -0.101883 0.826908 708.762817 0.001912 0.992337 0.123544 41.636108 -0.833158 0.069904 -0.548599 -330.167511\n-0.552986 -0.102555 0.826855 710.976685 0.001733 0.992252 0.124228 41.913612 -0.833189 0.070129 -0.548524 -331.621368\n-0.553047 -0.102025 0.826880 713.200317 0.001623 0.992340 0.123526 42.195663 -0.833148 0.069658 -0.548645 -333.083313\n-0.553084 -0.101146 0.826963 715.428162 0.002467 0.992400 0.123031 42.481113 -0.833122 0.070087 -0.548631 -334.547791\n-0.552753 -0.100267 0.827291 717.647217 0.003843 0.992418 0.122848 42.761765 -0.833336 0.071084 -0.548177 -336.016205\n-0.552243 -0.099826 0.827685 719.861572 0.005055 0.992386 0.123063 43.035789 -0.833668 0.072144 -0.547533 -337.477814\n-0.551876 -0.099916 0.827919 722.091919 0.006303 0.992268 0.123951 43.318256 -0.833902 0.073624 -0.546979 -338.949493\n-0.551206 -0.099912 0.828365 724.308838 0.006532 0.992257 0.124026 43.588089 -0.834343 0.073774 -0.546286 -340.405060\n-0.550584 -0.099860 0.828786 726.532043 0.006349 0.992290 0.123778 43.866913 -0.834756 0.073413 -0.545704 -341.855225\n-0.549970 -0.100074 0.829167 728.760010 0.005451 0.992344 0.123384 44.148655 -0.835167 0.072378 -0.545214 -343.301605\n-0.549573 -0.100272 0.829406 731.002869 0.004881 0.992369 0.123208 44.424152 -0.835431 0.071760 -0.544890 -344.764038\n-0.549106 -0.101008 0.829626 733.252197 0.004798 0.992272 0.123986 44.710781 -0.835739 0.072062 -0.544378 -346.225739\n-0.548695 -0.102085 0.829767 735.484558 0.005765 0.992031 0.125861 44.998875 -0.836003 0.073843 -0.543734 -347.682037\n-0.548188 -0.102684 0.830028 737.719238 0.006780 0.991856 0.127181 45.282398 -0.836328 0.075346 -0.543028 -349.136383\n-0.547232 -0.102320 0.830703 739.925415 0.007760 0.991837 0.127279 45.565647 -0.836945 0.076098 -0.541971 -350.573090\n-0.546510 -0.101613 0.831265 742.157349 0.008528 0.991885 0.126854 45.852806 -0.837409 0.076416 -0.541208 -352.023041\n-0.545731 -0.100657 0.831893 744.380737 0.008943 0.992003 0.125898 46.141163 -0.837912 0.076146 -0.540467 -353.461426\n-0.544939 -0.099980 0.832493 746.615295 0.008886 0.992121 0.124967 46.424965 -0.838428 0.075497 -0.539757 -354.900970\n-0.544030 -0.099522 0.833143 748.831848 0.009392 0.992156 0.124650 46.707531 -0.839013 0.075638 -0.538828 -356.327576\n-0.543277 -0.098884 0.833710 751.054260 0.010283 0.992181 0.124380 46.989876 -0.839491 0.076146 -0.538012 -357.760406\n-0.542551 -0.099066 0.834161 753.296570 0.010164 0.992175 0.124443 47.266251 -0.839961 0.075995 -0.537299 -359.193176\n-0.541349 -0.100532 0.834766 755.541626 0.009080 0.992069 0.125365 47.541954 -0.840749 0.075446 -0.536143 -360.616516\n-0.540421 -0.101624 0.835235 757.778809 0.007420 0.992065 0.125507 47.829231 -0.841362 0.074024 -0.535379 -362.029358\n-0.539712 -0.101388 0.835722 759.999023 0.007354 0.992116 0.125111 48.121834 -0.841818 0.073670 -0.534711 -363.448151\n-0.538988 -0.100350 0.836314 762.237854 0.008500 0.992179 0.124531 48.409210 -0.842270 0.074229 -0.533920 -364.861664\n-0.538496 -0.099061 0.836785 764.474854 0.009910 0.992252 0.123843 48.683174 -0.842570 0.074982 -0.533342 -366.278381\n-0.537942 -0.099585 0.837079 766.714539 0.009869 0.992186 0.124380 48.960789 -0.842924 0.075170 -0.532755 -367.690491\n-0.537989 -0.101280 0.836845 768.966980 0.009325 0.991980 0.126051 49.238384 -0.842900 0.075617 -0.532730 -369.106995\n-0.537950 -0.103104 0.836648 771.193970 0.007828 0.991838 0.127262 49.527821 -0.842941 0.075010 -0.532752 -370.513641\n-0.537634 -0.103348 0.836821 773.427795 0.006108 0.991957 0.126431 49.811577 -0.843156 0.073085 -0.532679 -371.910675\n-0.537328 -0.102035 0.837178 775.653992 0.006061 0.992162 0.124814 50.103176 -0.843352 0.072141 -0.532498 -373.307770\n-0.536631 -0.102326 0.837590 777.895081 0.007465 0.992006 0.125973 50.395451 -0.843784 0.073854 -0.531577 -374.720795\n-0.536058 -0.102475 0.837939 780.137024 0.010001 0.991764 0.127685 50.682808 -0.844122 0.076827 -0.530618 -376.145569\n-0.535412 -0.101895 0.838422 782.376221 0.012381 0.991642 0.128422 50.981579 -0.844500 0.079139 -0.529676 -377.559418\n-0.534626 -0.100666 0.839072 784.607056 0.013558 0.991731 0.127619 51.265331 -0.844980 0.079605 -0.528840 -378.959167\n-0.534173 -0.098827 0.839579 786.859375 0.013734 0.991998 0.125506 51.549820 -0.845264 0.078573 -0.528541 -380.364014\n-0.533421 -0.097306 0.840234 789.101074 0.014019 0.992207 0.123806 51.836037 -0.845734 0.077820 -0.527900 -381.766052\n-0.532395 -0.096925 0.840929 791.347046 0.014913 0.992195 0.123801 52.119152 -0.846365 0.078451 -0.526794 -383.172729\n-0.531238 -0.098136 0.841520 793.603027 0.015265 0.991999 0.125321 52.395508 -0.847085 0.079421 -0.525490 -384.578125\n-0.530172 -0.099410 0.842043 795.853455 0.015023 0.991846 0.126555 52.681263 -0.847757 0.079745 -0.524355 -385.970367\n-0.528887 -0.099669 0.842820 798.110901 0.014391 0.991884 0.126327 52.967194 -0.848570 0.078941 -0.523160 -387.356689\n-0.527911 -0.100046 0.843386 800.360291 0.013830 0.991893 0.126319 53.252911 -0.849187 0.078349 -0.522248 -388.737671\n-0.526732 -0.100509 0.844069 802.610107 0.013548 0.991866 0.126562 53.544876 -0.849924 0.078100 -0.521086 -390.117004\n-0.525841 -0.100460 0.844630 804.852295 0.013476 0.991892 0.126366 53.827187 -0.850476 0.077831 -0.520223 -391.497833\n-0.524528 -0.100309 0.845463 807.100525 0.013000 0.991976 0.125757 54.115299 -0.851294 0.076954 -0.519015 -392.861908\n-0.523819 -0.099894 0.845952 809.363647 0.013658 0.991987 0.125596 54.404568 -0.851720 0.077344 -0.518257 -394.243500\n-0.523324 -0.100195 0.846223 811.610046 0.014156 0.991904 0.126199 54.680935 -0.852016 0.078022 -0.517669 -395.617828\n-0.523042 -0.101988 0.846183 813.871826 0.013293 0.991718 0.127745 54.963902 -0.852203 0.078064 -0.517355 -396.984314\n-0.522805 -0.103579 0.846136 816.123413 0.011920 0.991605 0.128752 55.246925 -0.852369 0.077398 -0.517182 -398.351898\n-0.522661 -0.103692 0.846211 818.369507 0.010741 0.991696 0.128153 55.543274 -0.852473 0.076069 -0.517207 -399.689117\n-0.522932 -0.103277 0.846095 820.612671 0.009830 0.991836 0.127142 55.830513 -0.852318 0.074804 -0.517647 -401.048645\n-0.523094 -0.103411 0.845978 822.859802 0.009692 0.991826 0.127232 56.121376 -0.852220 0.074753 -0.517816 -402.419464\n-0.523758 -0.102877 0.845632 825.102722 0.010540 0.991822 0.127190 56.417084 -0.851802 0.075530 -0.518390 -403.785797\n-0.524142 -0.102584 0.845430 827.352905 0.011670 0.991760 0.127575 56.702110 -0.851551 0.076734 -0.518626 -405.159088\n-0.524645 -0.102802 0.845091 829.612671 0.012437 0.991651 0.128351 56.985744 -0.851230 0.077849 -0.518986 -406.538544\n-0.524909 -0.103654 0.844823 831.847961 0.012631 0.991499 0.129498 57.270748 -0.851064 0.078646 -0.519138 -407.905609\n-0.524816 -0.104504 0.844776 834.085815 0.012027 0.991426 0.130117 57.557842 -0.851130 0.078448 -0.519059 -409.262146\n-0.524797 -0.103787 0.844876 836.331604 0.011081 0.991622 0.128697 57.852413 -0.851155 0.076902 -0.519251 -410.636566\n-0.524685 -0.101980 0.845166 838.548584 0.010714 0.991929 0.126341 58.137466 -0.851229 0.075344 -0.519358 -411.983185\n-0.524128 -0.101132 0.845614 840.771179 0.010813 0.992054 0.125348 58.420414 -0.851571 0.074842 -0.518869 -413.339935\n-0.523915 -0.101380 0.845716 843.006104 0.011097 0.991995 0.125790 58.700378 -0.851698 0.075288 -0.518596 -414.697540\n-0.523355 -0.102059 0.845981 845.251465 0.011377 0.991876 0.126698 58.985260 -0.852039 0.075933 -0.517943 -416.073822\n-0.522363 -0.102561 0.846533 847.508911 0.012065 0.991752 0.127599 59.274536 -0.852638 0.076867 -0.516818 -417.447540\n-0.521358 -0.102509 0.847159 849.751831 0.013137 0.991677 0.128081 59.564793 -0.853237 0.077905 -0.515672 -418.805969\n-0.520185 -0.102413 0.847891 852.004578 0.014291 0.991602 0.128538 59.857681 -0.853934 0.078981 -0.514353 -420.176147\n-0.518555 -0.102620 0.848864 854.237183 0.014959 0.991531 0.129005 60.144932 -0.854914 0.079594 -0.512628 -421.524994\n-0.517318 -0.103053 0.849566 856.489929 0.015223 0.991458 0.129535 60.435997 -0.855658 0.079944 -0.511330 -422.877472\n-0.515988 -0.103305 0.850344 858.754822 0.015688 0.991395 0.129960 60.728916 -0.856452 0.080398 -0.509927 -424.231201\n-0.514466 -0.104112 0.851167 861.010681 0.015982 0.991266 0.130909 61.024551 -0.857362 0.080951 -0.508309 -425.571869\n-0.513029 -0.105702 0.851838 863.276489 0.015320 0.991103 0.132210 61.321201 -0.858235 0.080878 -0.506845 -426.915344\n-0.511508 -0.107067 0.852582 865.548340 0.014736 0.990968 0.133286 61.622086 -0.859152 0.080740 -0.505310 -428.260010\n-0.510589 -0.107206 0.853115 867.840271 0.013756 0.991051 0.132772 61.929184 -0.859715 0.079528 -0.504545 -429.596283\n-0.509553 -0.106700 0.853798 870.096436 0.014151 0.991108 0.132305 62.241222 -0.860323 0.079499 -0.503512 -430.926270\n-0.508789 -0.106809 0.854240 872.395447 0.014924 0.991030 0.132801 62.547623 -0.860762 0.080316 -0.502632 -432.271484\n-0.508292 -0.107513 0.854447 874.675171 0.015094 0.990912 0.133663 62.852287 -0.861052 0.080836 -0.502050 -433.602234\n-0.508287 -0.108351 0.854344 876.952576 0.014632 0.990824 0.134365 63.159061 -0.861063 0.080797 -0.502038 -434.927673\n-0.508455 -0.110445 0.853976 879.241821 0.013678 0.990579 0.136255 63.471127 -0.860980 0.080960 -0.502155 -436.253906\n-0.508292 -0.112432 0.853814 881.525513 0.012272 0.990395 0.137723 63.782833 -0.861097 0.080481 -0.502030 -437.584747\n-0.508245 -0.112725 0.853803 883.808899 0.010928 0.990473 0.137275 64.094460 -0.861143 0.079099 -0.502171 -438.903992\n-0.508124 -0.111923 0.853981 886.094421 0.010677 0.990626 0.136185 64.419853 -0.861218 0.078317 -0.502166 -440.233337\n-0.507420 -0.112777 0.854287 888.389954 0.009952 0.990566 0.136678 64.736977 -0.861641 0.077855 -0.501511 -441.566559\n-0.506822 -0.114276 0.854443 890.689941 0.009368 0.990386 0.138014 65.053978 -0.862000 0.077953 -0.500878 -442.903564\n-0.506186 -0.113696 0.854897 892.977600 0.009572 0.990470 0.137394 65.379684 -0.862371 0.077730 -0.500274 -444.234924\n-0.505903 -0.112491 0.855224 895.264587 0.010322 0.990600 0.136403 65.694778 -0.862528 0.077834 -0.499987 -445.562958\n-0.505497 -0.112778 0.855426 897.562195 0.011469 0.990455 0.137358 66.013725 -0.862752 0.079245 -0.499379 -446.886536\n-0.505325 -0.114098 0.855353 899.866638 0.012305 0.990167 0.139351 66.332977 -0.862841 0.080943 -0.498952 -448.227844\n-0.505155 -0.115740 0.855233 902.173279 0.012865 0.989847 0.141556 66.656868 -0.862933 0.082510 -0.498537 -449.561768\n-0.504613 -0.117455 0.855318 904.469849 0.012421 0.989612 0.143225 66.980545 -0.863256 0.082897 -0.497913 -450.889221\n-0.504176 -0.118547 0.855426 906.776550 0.012025 0.989474 0.144211 67.314308 -0.863517 0.082994 -0.497444 -452.221741\n-0.503113 -0.118131 0.856109 909.063293 0.012673 0.989499 0.143985 67.644585 -0.864128 0.083290 -0.496333 -453.536621\n-0.501536 -0.116303 0.857284 911.349548 0.015006 0.989604 0.143034 67.979317 -0.865007 0.084601 -0.494577 -454.852875\n-0.500457 -0.115669 0.858000 913.660889 0.017193 0.989512 0.143427 68.307556 -0.865590 0.086531 -0.493220 -456.181274\n-0.499026 -0.116065 0.858780 915.966492 0.018103 0.989378 0.144235 68.633331 -0.866398 0.087524 -0.491624 -457.505646\n-0.497326 -0.117205 0.859610 918.252930 0.018719 0.989152 0.145697 68.965668 -0.867362 0.088550 -0.489737 -458.809967\n-0.496025 -0.118208 0.860225 920.537537 0.019270 0.988948 0.147008 69.292381 -0.868095 0.089496 -0.488264 -460.097961\n-0.494858 -0.119435 0.860727 922.835083 0.019658 0.988717 0.148497 69.629356 -0.868752 0.090405 -0.486926 -461.398956\n-0.493570 -0.120669 0.861294 925.133545 0.018893 0.988607 0.149332 69.963722 -0.869501 0.089978 -0.485667 -462.681976\n-0.492307 -0.120978 0.861974 927.426270 0.017833 0.988684 0.148946 70.297356 -0.870239 0.088699 -0.484579 -463.963654\n-0.490937 -0.121230 0.862719 929.717346 0.017610 0.988688 0.148952 70.641792 -0.871017 0.088318 -0.483249 -465.233002\n-0.489823 -0.121530 0.863310 932.011475 0.017912 0.988625 0.149334 70.980225 -0.871638 0.088610 -0.482074 -466.505676\n-0.488689 -0.121517 0.863954 934.296204 0.018662 0.988570 0.149601 71.321053 -0.872259 0.089232 -0.480835 -467.772614\n-0.487491 -0.121134 0.864685 936.584106 0.019294 0.988593 0.149370 71.658417 -0.872915 0.089500 -0.479593 -469.038971\n-0.486407 -0.122033 0.865168 938.855835 0.020080 0.988375 0.150701 72.000229 -0.873502 0.090675 -0.478302 -470.291443\n-0.485550 -0.123242 0.865478 941.145142 0.020409 0.988145 0.152159 72.338264 -0.873970 0.091544 -0.477279 -471.545929\n-0.484467 -0.124378 0.865923 943.414429 0.020315 0.987975 0.153275 72.681091 -0.874574 0.091847 -0.476114 -472.786774\n-0.483637 -0.124735 0.866335 945.676575 0.020037 0.987956 0.153432 73.028664 -0.875039 0.091564 -0.475313 -474.011566\n-0.482758 -0.124854 0.866808 947.956238 0.019806 0.987975 0.153337 73.378273 -0.875530 0.091193 -0.474480 -475.249481\n-0.482097 -0.125410 0.867096 950.228271 0.019121 0.987960 0.153521 73.723839 -0.875909 0.090592 -0.473895 -476.478546\n-0.481364 -0.125429 0.867500 952.497314 0.019160 0.987966 0.153478 74.073296 -0.876311 0.090500 -0.473169 -477.708832\n-0.480821 -0.124626 0.867917 954.759583 0.019733 0.988059 0.152809 74.421021 -0.876596 0.090600 -0.472621 -478.941559\n-0.480381 -0.123300 0.868350 957.013367 0.020755 0.988194 0.151798 74.764549 -0.876814 0.090944 -0.472150 -480.164459\n-0.480035 -0.123702 0.868484 959.284729 0.021264 0.988076 0.152489 75.104042 -0.876992 0.091668 -0.471681 -481.374298\n-0.479595 -0.126160 0.868373 961.546204 0.021615 0.987612 0.155421 75.447083 -0.877224 0.093309 -0.470926 -482.603516\n-0.479314 -0.126937 0.868415 963.809875 0.021405 0.987500 0.156158 75.806160 -0.877382 0.093437 -0.470606 -483.823944\n-0.478869 -0.123684 0.869130 966.063782 0.019370 0.988296 0.151315 76.150139 -0.877673 0.089296 -0.470868 -485.026520\n-0.478632 -0.120476 0.869711 968.315613 0.016773 0.989106 0.146247 76.479042 -0.877855 0.084586 -0.471397 -486.229279\n-0.478367 -0.123602 0.869418 970.582336 0.015309 0.988721 0.148987 76.821625 -0.878027 0.084580 -0.471079 -487.435425\n-0.478100 -0.128839 0.868804 972.858337 0.016479 0.987692 0.155538 77.168983 -0.878151 0.088679 -0.470093 -488.664581\n-0.477844 -0.131176 0.868595 975.094360 0.017445 0.987176 0.158681 77.524902 -0.878271 0.090978 -0.469428 -489.858154\n-0.477575 -0.130947 0.868778 977.360168 0.019067 0.987053 0.159255 77.889626 -0.878384 0.092621 -0.468895 -491.074158\n-0.477122 -0.130537 0.869088 979.622681 0.019907 0.987048 0.159183 78.246338 -0.878612 0.093250 -0.468344 -492.292328\n-0.476293 -0.131831 0.869348 981.904541 0.019602 0.986859 0.160390 78.609024 -0.879068 0.093434 -0.467450 -493.513702\n-0.475419 -0.133315 0.869600 984.146851 0.019316 0.986632 0.161817 78.973335 -0.879547 0.093728 -0.466489 -494.704834\n-0.474588 -0.133588 0.870012 986.399048 0.019575 0.986570 0.162163 79.344795 -0.879991 0.093991 -0.465599 -495.912750\n-0.473683 -0.132668 0.870646 988.665527 0.019871 0.986727 0.161167 79.714188 -0.880471 0.093643 -0.464759 -497.114532\n-0.472681 -0.132264 0.871252 990.920959 0.020036 0.986804 0.160677 80.078468 -0.881006 0.093405 -0.463793 -498.307648\n-0.471569 -0.132467 0.871823 993.206726 0.020955 0.986690 0.161255 80.446602 -0.881580 0.094311 -0.462517 -499.516449\n-0.470760 -0.132620 0.872237 995.453918 0.021787 0.986589 0.161766 80.816513 -0.881992 0.095156 -0.461557 -500.700134\n-0.469770 -0.132494 0.872789 997.708679 0.021556 0.986657 0.161382 81.177292 -0.882525 0.094626 -0.460646 -501.880615\n-0.468902 -0.132757 0.873216 999.977417 0.021333 0.986649 0.161458 81.540627 -0.882992 0.094336 -0.459810 -503.066162\n-0.468202 -0.133843 0.873426 1002.246826 0.021472 0.986447 0.162672 81.913109 -0.883361 0.094917 -0.458982 -504.249939\n-0.467510 -0.134970 0.873623 1004.493774 0.021490 0.986248 0.163870 82.274971 -0.883726 0.095385 -0.458181 -505.422760\n-0.467037 -0.136861 0.873582 1006.729675 0.021512 0.985898 0.165958 82.646736 -0.883976 0.096301 -0.457506 -506.590942\n-0.466307 -0.140511 0.873393 1008.999268 0.021032 0.985265 0.169738 83.031967 -0.884373 0.097519 -0.456481 -507.761383\n-0.465346 -0.143084 0.873487 1011.249023 0.021168 0.984766 0.172590 83.426506 -0.884876 0.098805 -0.455228 -508.927643\n-0.464734 -0.145224 0.873460 1013.506287 0.018863 0.984611 0.173741 83.811707 -0.885249 0.097220 -0.454843 -510.081970\n-0.464595 -0.142980 0.873904 1015.778992 0.020123 0.984919 0.171841 84.206223 -0.885295 0.097422 -0.454711 -511.249908\n-0.464534 -0.141266 0.874215 1018.029236 0.022035 0.985045 0.170884 84.598373 -0.885281 0.098645 -0.454474 -512.419312\n-0.464861 -0.139968 0.874250 1020.290710 0.024143 0.985054 0.170546 84.978363 -0.885055 0.100387 -0.454534 -513.595276\n-0.464734 -0.140702 0.874200 1022.578918 0.023217 0.985018 0.170881 85.369186 -0.885146 0.099711 -0.454505 -514.767578\n-0.464384 -0.142405 0.874110 1024.864014 0.020024 0.985047 0.171117 85.755569 -0.885407 0.096967 -0.454589 -515.938354\n-0.464528 -0.142448 0.874027 1027.150635 0.018129 0.985241 0.170209 86.152435 -0.885373 0.094912 -0.455089 -517.104431\n-0.464399 -0.141326 0.874277 1029.459595 0.019561 0.985308 0.169664 86.560234 -0.885410 0.095894 -0.454811 -518.291138\n-0.464443 -0.140943 0.874315 1031.747681 0.022418 0.985067 0.170705 86.959290 -0.885319 0.098883 -0.454348 -519.483521\n-0.464363 -0.140476 0.874433 1034.032349 0.024569 0.984917 0.171272 87.347061 -0.885304 0.101016 -0.453908 -520.674255\n-0.464320 -0.139425 0.874624 1036.330688 0.025563 0.985010 0.170593 87.738152 -0.885299 0.101568 -0.453795 -521.860779\n-0.464253 -0.139848 0.874593 1038.611328 0.025768 0.984905 0.171165 88.131546 -0.885328 0.102000 -0.453641 -523.032227\n-0.463759 -0.140389 0.874768 1040.921997 0.026074 0.984775 0.171867 88.526138 -0.885578 0.102514 -0.453038 -524.225220\n-0.463090 -0.140678 0.875076 1043.219360 0.025613 0.984787 0.171869 88.922546 -0.885941 0.102004 -0.452441 -525.408264\n-0.462406 -0.140036 0.875540 1045.521484 0.025126 0.984983 0.170810 89.310890 -0.886312 0.100983 -0.451944 -526.581665\n-0.461598 -0.140064 0.875962 1047.817383 0.024777 0.985036 0.170561 89.707565 -0.886743 0.100434 -0.451220 -527.751892\n-0.460942 -0.140658 0.876212 1050.114502 0.025296 0.984875 0.171409 90.099121 -0.887070 0.101174 -0.450412 -528.926147\n-0.460276 -0.142739 0.876226 1052.431396 0.025290 0.984481 0.173659 90.497826 -0.887415 0.102091 -0.449524 -530.100830\n-0.459538 -0.145098 0.876226 1054.716797 0.024480 0.984121 0.175804 90.889900 -0.887821 0.102238 -0.448689 -531.262939\n-0.459009 -0.146302 0.876302 1057.003906 0.022972 0.984064 0.176326 91.286812 -0.888134 0.101066 -0.448334 -532.416992\n-0.458461 -0.145411 0.876738 1059.284302 0.021782 0.984389 0.174655 91.682350 -0.888448 0.099169 -0.448136 -533.568542\n-0.458057 -0.144426 0.877112 1061.593506 0.021233 0.984654 0.173222 92.092758 -0.888669 0.097970 -0.447961 -534.735168\n-0.457732 -0.144122 0.877331 1063.883057 0.022257 0.984608 0.173357 92.497704 -0.888811 0.098877 -0.447479 -535.893555\n-0.457332 -0.144623 0.877458 1066.177856 0.023011 0.984433 0.174248 92.893234 -0.888998 0.099880 -0.446884 -537.056763\n-0.457076 -0.143797 0.877726 1068.458374 0.023885 0.984504 0.173729 93.288116 -0.889107 0.100371 -0.446559 -538.211609\n-0.457100 -0.143247 0.877804 1070.747559 0.025595 0.984418 0.173973 93.686134 -0.889047 0.101991 -0.446311 -539.366699\n-0.456657 -0.143171 0.878047 1073.024902 0.027502 0.984222 0.174787 94.081261 -0.889217 0.103965 -0.445515 -540.520264\n-0.456683 -0.142116 0.878205 1075.323975 0.029449 0.984202 0.174584 94.476898 -0.889142 0.105592 -0.445283 -541.686584\n-0.456188 -0.142001 0.878481 1077.587036 0.030542 0.984106 0.174935 94.868767 -0.889359 0.106633 -0.444600 -542.836792\n-0.455350 -0.143663 0.878645 1079.873657 0.029458 0.983924 0.176143 95.260330 -0.889825 0.106090 -0.443797 -543.986023\n-0.454263 -0.145394 0.878923 1082.167358 0.025751 0.984037 0.176091 95.649651 -0.890495 0.102625 -0.443268 -545.112671\n-0.453263 -0.147758 0.879045 1084.455566 0.020958 0.984126 0.176228 96.054443 -0.891131 0.098301 -0.442971 -546.237366\n-0.451850 -0.148395 0.879665 1086.729004 0.017804 0.984371 0.175204 96.449272 -0.891916 0.094828 -0.442146 -547.367859\n-0.450811 -0.147529 0.880344 1088.994385 0.016462 0.984706 0.173448 96.850174 -0.892468 0.092684 -0.441487 -548.478516\n-0.449942 -0.146535 0.880954 1091.268555 0.017882 0.984771 0.172937 97.249855 -0.892879 0.093564 -0.440470 -549.621521\n-0.448698 -0.147127 0.881489 1093.544556 0.019851 0.984471 0.174420 97.650238 -0.893463 0.095760 -0.438810 -550.755127\n-0.447391 -0.148100 0.881991 1095.823486 0.021614 0.984115 0.176211 98.044060 -0.894077 0.097898 -0.437083 -551.885376\n-0.446206 -0.148070 0.882596 1098.079590 0.022851 0.984011 0.176636 98.444229 -0.894638 0.098984 -0.435688 -553.003235\n-0.444886 -0.146849 0.883466 1100.354980 0.022781 0.984291 0.175080 98.838768 -0.895298 0.098017 -0.434551 -554.122742\n" }, { "path": "src/vloam_main/results/2011_10_03_drive_0042/VO0.txt", "content": "1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000\n0.999001 -0.000251 0.044690 0.023313 0.000358 0.999997 -0.002371 -0.020312 -0.044690 0.002385 0.998998 0.947016\n0.995907 -0.004633 0.090268 0.116319 0.005402 0.999951 -0.008271 -0.018594 -0.090225 0.008725 0.995883 1.965605\n0.990595 -0.003255 0.136786 0.233260 0.004988 0.999912 -0.012325 -0.032059 -0.136734 0.012891 0.990524 2.953857\n0.983125 -0.000899 0.182933 0.413556 0.003737 0.999878 -0.015171 -0.043698 -0.182897 0.015599 0.983008 3.984092\n0.973071 -0.002798 0.230489 0.631771 0.006476 0.999864 -0.015200 -0.065647 -0.230415 0.016284 0.972956 5.000090\n0.961295 -0.003330 0.275500 0.989974 0.008440 0.999814 -0.017366 -0.067785 -0.275391 0.019019 0.961144 6.180357\n0.947177 -0.005907 0.320657 1.320166 0.013115 0.999707 -0.020323 -0.083727 -0.320443 0.023455 0.946977 7.222177\n0.931859 -0.007076 0.362751 1.711062 0.016298 0.999617 -0.022369 -0.106343 -0.362454 0.026757 0.931618 8.184328\n0.914393 -0.007704 0.404754 2.158211 0.018668 0.999558 -0.023147 -0.130577 -0.404397 0.028722 0.914133 9.168549\n0.894725 -0.007965 0.446547 2.611246 0.020167 0.999542 -0.022579 -0.148425 -0.446162 0.029207 0.894475 10.063181\n0.872854 -0.006941 0.487931 3.096790 0.020335 0.999548 -0.022158 -0.171506 -0.487557 0.029262 0.872601 10.932746\n0.848543 -0.007060 0.529079 3.610270 0.021230 0.999560 -0.020710 -0.208593 -0.528700 0.028805 0.848320 11.763932\n0.822314 -0.007117 0.568990 4.146108 0.023474 0.999495 -0.021424 -0.222711 -0.568550 0.030974 0.822065 12.595169\n0.794493 -0.008270 0.607217 4.725168 0.027219 0.999387 -0.022003 -0.235066 -0.606663 0.034009 0.794231 13.386646\n0.766259 -0.006775 0.642496 5.413443 0.028408 0.999324 -0.023342 -0.244205 -0.641903 0.036138 0.765934 14.225825\n0.735436 -0.009584 0.677526 6.102504 0.033907 0.999168 -0.022672 -0.262317 -0.676745 0.039647 0.735149 14.987127\n0.703294 -0.008502 0.710848 6.795585 0.034434 0.999162 -0.022118 -0.292367 -0.710065 0.040033 0.702998 15.685899\n0.668132 -0.007199 0.744008 7.481177 0.036145 0.999087 -0.022792 -0.322933 -0.743164 0.042121 0.667782 16.342148\n0.631805 -0.008077 0.775085 8.180517 0.041741 0.998849 -0.023615 -0.342320 -0.774002 0.047273 0.631415 16.935192\n0.595388 -0.005804 0.803418 8.952331 0.042646 0.998793 -0.024388 -0.357569 -0.802306 0.048782 0.594916 17.504284\n0.556954 -0.005170 0.830527 9.757885 0.046788 0.998588 -0.025160 -0.389503 -0.829224 0.052872 0.556410 18.092897\n0.518492 -0.003203 0.855077 10.580888 0.052003 0.998260 -0.027793 -0.412507 -0.853500 0.058877 0.517756 18.604546\n0.479354 -0.002631 0.877618 11.414033 0.055427 0.998090 -0.027283 -0.433856 -0.875870 0.061722 0.478584 19.090649\n0.441576 -0.003635 0.897216 12.255479 0.061617 0.997754 -0.026283 -0.442004 -0.895105 0.066890 0.440808 19.529888\n0.403111 -0.004046 0.915142 13.089704 0.066331 0.997489 -0.024809 -0.466866 -0.912744 0.070703 0.402367 19.945587\n0.368170 -0.001314 0.929758 13.892914 0.068452 0.997323 -0.025697 -0.495755 -0.927235 0.073104 0.367274 20.274595\n0.333685 -0.001853 0.942683 14.761851 0.070718 0.997229 -0.023072 -0.513517 -0.940028 0.074364 0.332891 20.620235\n0.299798 -0.003046 0.953998 15.684539 0.073503 0.997096 -0.019915 -0.533086 -0.951167 0.076092 0.299152 20.916813\n0.265383 -0.002521 0.964140 16.588657 0.075558 0.996975 -0.018191 -0.551517 -0.961178 0.077676 0.264771 21.186584\n0.231020 0.000656 0.972949 17.587292 0.073253 0.997150 -0.018066 -0.569554 -0.970187 0.075445 0.230314 21.474377\n0.198276 0.002605 0.980143 18.608131 0.073788 0.997119 -0.017577 -0.581692 -0.977365 0.075807 0.197512 21.740719\n0.166697 0.003465 0.986002 19.604971 0.075641 0.997002 -0.016291 -0.599960 -0.983103 0.077298 0.165935 21.936131\n0.135430 0.003481 0.990781 20.618711 0.077311 0.996908 -0.014070 -0.617860 -0.987766 0.078504 0.134742 22.088131\n0.103886 0.003039 0.994585 21.660574 0.077511 0.996929 -0.011142 -0.624386 -0.991564 0.078249 0.103331 22.215313\n0.073824 0.003238 0.997266 22.726011 0.078345 0.996885 -0.009036 -0.629540 -0.994189 0.078798 0.073340 22.302004\n0.045148 0.003921 0.998973 23.823385 0.081280 0.996662 -0.007585 -0.641413 -0.995668 0.081539 0.044679 22.378317\n0.018925 0.004875 0.999809 24.919588 0.085755 0.996295 -0.006481 -0.651686 -0.996136 0.085861 0.018437 22.413698\n-0.004054 0.005708 0.999976 25.972244 0.089878 0.995939 -0.005320 -0.652604 -0.995944 0.089855 -0.004551 22.417892\n-0.023928 0.006735 0.999691 27.058256 0.096024 0.995369 -0.004407 -0.643053 -0.995091 0.095889 -0.024464 22.379496\n-0.041267 0.006339 0.999128 28.176407 0.099734 0.995012 -0.002194 -0.639253 -0.994158 0.099556 -0.041693 22.315996\n-0.057203 0.007037 0.998338 29.322542 0.102951 0.994686 -0.001112 -0.632706 -0.993040 0.102716 -0.057624 22.247791\n-0.072073 0.006703 0.997377 30.479465 0.105287 0.994441 0.000925 -0.633361 -0.991827 0.105077 -0.072378 22.168652\n-0.085120 0.005101 0.996358 31.603394 0.104902 0.994475 0.003870 -0.647848 -0.990833 0.104849 -0.085185 22.065596\n-0.097693 0.005522 0.995201 32.774963 0.104906 0.994471 0.004780 -0.649545 -0.989672 0.104870 -0.097733 21.945807\n-0.108929 0.008388 0.994014 33.951122 0.105088 0.994458 0.003124 -0.652332 -0.988479 0.104799 -0.109207 21.806461\n-0.118833 0.012212 0.992839 35.151070 0.105547 0.994414 0.000401 -0.641100 -0.987288 0.104839 -0.119458 21.641512\n-0.128977 0.015079 0.991533 36.374180 0.106157 0.994348 -0.001313 -0.630448 -0.985949 0.105089 -0.129849 21.493797\n-0.137047 0.016789 0.990422 37.615974 0.105679 0.994398 -0.002233 -0.619010 -0.984911 0.104361 -0.138054 21.321260\n-0.142793 0.017844 0.989592 38.889156 0.105329 0.994434 -0.002733 -0.607426 -0.984132 0.103842 -0.143878 21.120079\n-0.148231 0.019746 0.988756 40.187447 0.105657 0.994394 -0.004019 -0.599327 -0.983292 0.103874 -0.149487 20.913813\n-0.153021 0.021761 0.987983 41.500271 0.104185 0.994541 -0.005769 -0.588903 -0.982716 0.102050 -0.154453 20.702484\n-0.157366 0.022610 0.987281 42.828259 0.105416 0.994410 -0.005971 -0.604106 -0.981898 0.103136 -0.158870 20.480484\n-0.160954 0.021769 0.986722 44.185608 0.104261 0.994538 -0.004934 -0.594378 -0.981439 0.102082 -0.162345 20.252007\n-0.162760 0.019882 0.986465 45.553169 0.104603 0.994510 -0.002785 -0.591487 -0.981105 0.102734 -0.163946 19.989119\n-0.165030 0.018832 0.986109 46.947510 0.104026 0.994573 -0.001584 -0.589236 -0.980787 0.102319 -0.166093 19.735970\n-0.167114 0.020035 0.985734 48.319130 0.106260 0.994336 -0.002195 -0.583687 -0.980195 0.104377 -0.168296 19.505192\n-0.168500 0.019942 0.985500 49.702911 0.105301 0.994438 -0.002118 -0.578782 -0.980061 0.103417 -0.169663 19.279594\n-0.169213 0.021389 0.985347 51.122826 0.105952 0.994365 -0.003389 -0.570068 -0.979868 0.103826 -0.170525 19.027794\n-0.169700 0.021417 0.985263 52.581112 0.104455 0.994523 -0.003627 -0.563940 -0.979944 0.102301 -0.171007 18.775835\n-0.169173 0.023182 0.985314 54.041714 0.103988 0.994563 -0.005545 -0.563321 -0.980085 0.101523 -0.170664 18.519575\n" }, { "path": "src/vloam_main/results/2011_10_03_drive_0042/VO1.txt", "content": "1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000\n0.999001 -0.000251 0.044690 0.023313 0.000358 0.999997 -0.002371 -0.020312 -0.044690 0.002385 0.998998 0.947016\n0.995907 -0.004637 0.090261 0.116441 0.005406 0.999951 -0.008279 -0.018467 -0.090218 0.008733 0.995884 1.965661\n0.990597 -0.003258 0.136776 0.233342 0.004992 0.999911 -0.012333 -0.031950 -0.136723 0.012899 0.990525 2.953858\n0.983127 -0.000902 0.182921 0.413646 0.003741 0.999878 -0.015178 -0.043605 -0.182885 0.015607 0.983010 3.984057\n0.973074 -0.002801 0.230476 0.631852 0.006480 0.999863 -0.015207 -0.065562 -0.230402 0.016291 0.972959 5.000053\n0.961298 -0.003333 0.275490 0.989754 0.008444 0.999813 -0.017370 -0.067815 -0.275381 0.019024 0.961147 6.179461\n0.947176 -0.005911 0.320659 1.319894 0.013120 0.999707 -0.020327 -0.083736 -0.320445 0.023460 0.946977 7.221684\n0.931859 -0.007079 0.362753 1.710788 0.016303 0.999617 -0.022373 -0.106354 -0.362455 0.026762 0.931617 8.183846\n0.914391 -0.007703 0.404759 2.158105 0.018670 0.999558 -0.023156 -0.130516 -0.404402 0.028730 0.914130 9.168180\n0.894723 -0.007963 0.446551 2.611163 0.020169 0.999541 -0.022587 -0.148358 -0.446167 0.029216 0.894473 10.062816\n0.872852 -0.006939 0.487935 3.096716 0.020337 0.999547 -0.022167 -0.171445 -0.487561 0.029272 0.872598 10.932375\n0.848539 -0.007058 0.529085 3.610127 0.021233 0.999560 -0.020719 -0.208525 -0.528706 0.028815 0.848316 11.763622\n0.822310 -0.007113 0.568995 4.145977 0.023478 0.999495 -0.021435 -0.222613 -0.568555 0.030985 0.822061 12.594900\n0.794490 -0.008266 0.607221 4.725049 0.027223 0.999387 -0.022014 -0.234964 -0.606667 0.034021 0.794228 13.386350\n0.766256 -0.006771 0.642500 5.413331 0.028412 0.999323 -0.023354 -0.244115 -0.641907 0.036150 0.765930 14.225529\n0.735438 -0.009581 0.677525 6.102312 0.033913 0.999167 -0.022682 -0.262256 -0.676743 0.039658 0.735150 14.986596\n0.703295 -0.008499 0.710848 6.795318 0.034441 0.999162 -0.022128 -0.292325 -0.710064 0.040045 0.702998 15.685318\n0.668134 -0.007197 0.744006 7.480933 0.036152 0.999086 -0.022802 -0.322901 -0.743162 0.042132 0.667783 16.341564\n0.631808 -0.008075 0.775083 8.180257 0.041748 0.998849 -0.023625 -0.342296 -0.774000 0.047284 0.631418 16.934593\n0.595393 -0.005802 0.803414 8.952111 0.042653 0.998792 -0.024397 -0.357549 -0.802302 0.048794 0.594921 17.503677\n0.556962 -0.005167 0.830522 9.757703 0.046796 0.998587 -0.025169 -0.389484 -0.829219 0.052883 0.556417 18.092291\n0.518502 -0.003200 0.855070 10.580682 0.052011 0.998259 -0.027803 -0.412503 -0.853493 0.058889 0.517766 18.603867\n0.479369 -0.002629 0.877609 11.413684 0.055435 0.998089 -0.027290 -0.433892 -0.875861 0.061733 0.478599 19.089815\n0.441595 -0.003633 0.897207 12.255075 0.061626 0.997753 -0.026291 -0.442053 -0.895096 0.066901 0.440826 19.528996\n0.403126 -0.004044 0.915136 13.089048 0.066340 0.997488 -0.024815 -0.466947 -0.912737 0.070714 0.402382 19.944630\n0.368176 -0.001311 0.929755 13.892403 0.068461 0.997323 -0.025703 -0.495811 -0.927232 0.073116 0.367280 20.273857\n0.333690 -0.001850 0.942681 14.761309 0.070727 0.997229 -0.023079 -0.513586 -0.940026 0.074374 0.332896 20.619486\n0.299801 -0.003043 0.953997 15.683952 0.073512 0.997095 -0.019921 -0.533169 -0.951165 0.076102 0.299154 20.916088\n0.265380 -0.002519 0.964141 16.587984 0.075567 0.996975 -0.018195 -0.551627 -0.961178 0.077686 0.264768 21.185890\n0.231018 0.000658 0.972949 17.586628 0.073262 0.997149 -0.018070 -0.569666 -0.970187 0.075455 0.230312 21.473669\n0.198275 0.002606 0.980143 18.607405 0.073798 0.997118 -0.017580 -0.581816 -0.977364 0.075818 0.197511 21.739948\n0.166699 0.003465 0.986002 19.604191 0.075651 0.997001 -0.016294 -0.600106 -0.983101 0.077308 0.165937 21.935312\n0.135433 0.003481 0.990780 20.617903 0.077320 0.996907 -0.014072 -0.618010 -0.987765 0.078513 0.134745 22.087299\n0.103889 0.003039 0.994584 21.659698 0.077521 0.996928 -0.011144 -0.624525 -0.991563 0.078259 0.103335 22.214443\n0.073829 0.003238 0.997266 22.725100 0.078355 0.996885 -0.009038 -0.629679 -0.994188 0.078808 0.073345 22.301098\n0.045154 0.003921 0.998972 23.822437 0.081291 0.996662 -0.007586 -0.641555 -0.995667 0.081550 0.044685 22.377405\n0.018927 0.004872 0.999809 24.918734 0.085762 0.996295 -0.006479 -0.651857 -0.996136 0.085868 0.018439 22.412817\n-0.004052 0.005705 0.999976 25.971386 0.089886 0.995938 -0.005318 -0.652771 -0.995944 0.089862 -0.004548 22.417013\n-0.023926 0.006732 0.999691 27.057411 0.096031 0.995369 -0.004405 -0.643218 -0.995091 0.095896 -0.024462 22.378626\n-0.041265 0.006336 0.999128 28.175568 0.099740 0.995011 -0.002191 -0.639421 -0.994157 0.099563 -0.041691 22.315134\n-0.057202 0.007034 0.998338 29.321705 0.102958 0.994685 -0.001109 -0.632869 -0.993040 0.102723 -0.057622 22.246931\n-0.072072 0.006700 0.997377 30.478628 0.105294 0.994441 0.000928 -0.633521 -0.991826 0.105084 -0.072376 22.167795\n-0.085117 0.005098 0.996358 31.602499 0.104908 0.994474 0.003874 -0.648019 -0.990833 0.104856 -0.085181 22.064720\n-0.097690 0.005518 0.995202 32.774059 0.104913 0.994470 0.004784 -0.649714 -0.989672 0.104877 -0.097729 21.944929\n-0.108925 0.008384 0.994015 33.950199 0.105095 0.994457 0.003128 -0.652496 -0.988479 0.104806 -0.109203 21.805578\n-0.118829 0.012209 0.992840 35.150135 0.105554 0.994413 0.000405 -0.641260 -0.987288 0.104847 -0.119454 21.640635\n-0.128972 0.015075 0.991534 36.373211 0.106164 0.994348 -0.001309 -0.630607 -0.985949 0.105097 -0.129844 21.492929\n-0.137042 0.016785 0.990423 37.614994 0.105686 0.994397 -0.002229 -0.619166 -0.984911 0.104369 -0.138048 21.320389\n-0.142787 0.017840 0.989593 38.888153 0.105336 0.994433 -0.002729 -0.607580 -0.984132 0.103850 -0.143871 21.119204\n-0.148225 0.019742 0.988757 40.186443 0.105664 0.994394 -0.004014 -0.599476 -0.983293 0.103881 -0.149480 20.912947\n-0.153014 0.021757 0.987984 41.499241 0.104192 0.994540 -0.005764 -0.589051 -0.982716 0.102058 -0.154446 20.701645\n-0.157360 0.022606 0.987283 42.827206 0.105423 0.994410 -0.005966 -0.604253 -0.981898 0.103144 -0.158863 20.479666\n-0.160947 0.021765 0.986723 44.184551 0.104268 0.994537 -0.004929 -0.594515 -0.981440 0.102090 -0.162337 20.251188\n-0.162752 0.019878 0.986467 45.552086 0.104611 0.994509 -0.002780 -0.591629 -0.981106 0.102743 -0.163937 19.988316\n-0.165022 0.018827 0.986110 46.946426 0.104033 0.994573 -0.001579 -0.589370 -0.980788 0.102328 -0.166085 19.735178\n-0.167106 0.020030 0.985735 48.318050 0.106267 0.994335 -0.002190 -0.583814 -0.980195 0.104386 -0.168288 19.504412\n-0.168495 0.019935 0.985501 49.701839 0.105302 0.994438 -0.002112 -0.578874 -0.980062 0.103420 -0.169658 19.278849\n-0.169208 0.021382 0.985348 51.121765 0.105953 0.994365 -0.003383 -0.570150 -0.979869 0.103829 -0.170520 19.027058\n-0.169692 0.021408 0.985265 52.579967 0.104457 0.994523 -0.003619 -0.564050 -0.979946 0.102303 -0.170999 18.775095\n-0.169166 0.023173 0.985315 54.040569 0.103989 0.994563 -0.005537 -0.563421 -0.980086 0.101525 -0.170656 18.518843\n-0.168581 0.022897 0.985422 55.554340 0.102352 0.994732 -0.005603 -0.551405 -0.980359 0.099915 -0.170037 18.250801\n-0.167319 0.021836 0.985661 57.081303 0.100667 0.994908 -0.004952 -0.550678 -0.980750 0.098395 -0.168665 17.976700\n-0.166174 0.022661 0.985836 58.609646 0.101437 0.994825 -0.005769 -0.556206 -0.980865 0.099041 -0.167613 17.709110\n-0.165418 0.023777 0.985937 60.122379 0.100081 0.994953 -0.007203 -0.541184 -0.981132 0.097482 -0.166963 17.471003\n-0.165036 0.024425 0.985985 61.694233 0.099490 0.995006 -0.007996 -0.534880 -0.981257 0.096776 -0.166641 17.217754\n-0.164118 0.024466 0.986137 63.323444 0.098368 0.995115 -0.008318 -0.530820 -0.981524 0.095639 -0.165723 16.935692\n-0.163562 0.022754 0.986271 64.956726 0.099769 0.994990 -0.006409 -0.540443 -0.981475 0.097350 -0.165013 16.654207\n-0.163196 0.021409 0.986361 66.583916 0.100153 0.994959 -0.005025 -0.534643 -0.981497 0.097967 -0.164518 16.392822\n-0.162254 0.022434 0.986494 68.210228 0.101536 0.994814 -0.005923 -0.550132 -0.981511 0.099204 -0.163690 16.123188\n-0.161872 0.024127 0.986517 69.865158 0.102534 0.994701 -0.007503 -0.563117 -0.981470 0.099937 -0.163489 15.856177\n-0.161685 0.024252 0.986544 71.548347 0.103107 0.994642 -0.007553 -0.590932 -0.981441 0.100499 -0.163319 15.586890\n-0.161545 0.023962 0.986574 73.217850 0.102735 0.994682 -0.007336 -0.604933 -0.981503 0.100170 -0.163147 15.301408\n-0.160796 0.023116 0.986717 74.930504 0.101429 0.994820 -0.006776 -0.595385 -0.981762 0.098992 -0.162308 14.980659\n-0.160259 0.023893 0.986786 76.648659 0.102808 0.994674 -0.007387 -0.593633 -0.981706 0.100266 -0.161862 14.665009\n-0.159469 0.023865 0.986914 78.387650 0.103638 0.994588 -0.007305 -0.601569 -0.981748 0.101117 -0.161080 14.344709\n-0.158676 0.024867 0.987017 80.186707 0.103467 0.994597 -0.008425 -0.603915 -0.981894 0.100787 -0.160392 13.986380\n-0.158750 0.025775 0.986982 81.975899 0.102170 0.994721 -0.009543 -0.608000 -0.982018 0.099325 -0.160546 13.664238\n-0.159782 0.026363 0.986800 83.791718 0.101199 0.994814 -0.010191 -0.630157 -0.981951 0.098234 -0.161621 13.347337\n-0.159597 0.027266 0.986806 85.625526 0.100086 0.994915 -0.011303 -0.635923 -0.982095 0.096961 -0.161515 12.975254\n-0.160244 0.026166 0.986731 87.463165 0.098064 0.995125 -0.010463 -0.650152 -0.982194 0.095087 -0.162028 12.644705\n-0.160582 0.025892 0.986683 89.294777 0.097858 0.995148 -0.010188 -0.672251 -0.982159 0.094918 -0.162337 12.306527\n-0.160374 0.024999 0.986740 91.161293 0.095009 0.995428 -0.009777 -0.682660 -0.982473 0.092182 -0.162016 11.953529\n-0.161496 0.025760 0.986537 93.001068 0.095152 0.995408 -0.010416 -0.714108 -0.982276 0.092189 -0.163205 11.630342\n-0.163264 0.026777 0.986219 94.883507 0.093704 0.995533 -0.011518 -0.726292 -0.982122 0.090532 -0.165044 11.265370\n-0.166838 0.027958 0.985588 96.758774 0.094348 0.995464 -0.012267 -0.746929 -0.981460 0.090942 -0.168719 10.925136\n-0.171262 0.027165 0.984851 98.598198 0.091813 0.995710 -0.011499 -0.766731 -0.980938 0.088453 -0.173022 10.604902\n-0.176254 0.026349 0.983992 100.509720 0.089997 0.995886 -0.010547 -0.794864 -0.980222 0.086697 -0.177900 10.251446\n-0.180420 0.027180 0.983214 102.427841 0.089123 0.995958 -0.011178 -0.825161 -0.979544 0.085610 -0.182113 9.870381\n-0.186007 0.029205 0.982114 104.380463 0.089376 0.995917 -0.012688 -0.849726 -0.978475 0.085417 -0.187857 9.504425\n-0.191248 0.031668 0.981031 106.308746 0.088476 0.995967 -0.014902 -0.862447 -0.977546 0.083948 -0.193278 9.136972\n-0.196145 0.032054 0.980051 108.248962 0.087288 0.996069 -0.015108 -0.901591 -0.976682 0.082583 -0.198172 8.747081\n-0.202227 0.031358 0.978837 110.202744 0.085616 0.996227 -0.014227 -0.953704 -0.975589 0.080928 -0.204149 8.373149\n-0.208427 0.030988 0.977547 112.163124 0.082962 0.996456 -0.013899 -0.979425 -0.974513 0.078203 -0.210259 7.974920\n-0.214550 0.030271 0.976244 114.138191 0.082209 0.996532 -0.012833 -1.020340 -0.973247 0.077503 -0.216295 7.557283\n-0.220914 0.029967 0.974833 116.094849 0.081960 0.996563 -0.012061 -1.057502 -0.971843 0.077233 -0.222611 7.137310\n-0.226550 0.031818 0.973480 118.063499 0.082643 0.996490 -0.013337 -1.079390 -0.970487 0.077430 -0.228384 6.679911\n-0.233118 0.033118 0.971884 120.015213 0.082989 0.996451 -0.014049 -1.125283 -0.968901 0.077380 -0.235039 6.246099\n-0.240077 0.033259 0.970184 121.971977 0.083362 0.996427 -0.013530 -1.164039 -0.967168 0.077628 -0.241992 5.817731\n-0.247389 0.033458 0.968338 123.997536 0.082659 0.996489 -0.013313 -1.194384 -0.965384 0.076749 -0.249286 5.357292\n-0.253002 0.034492 0.966851 125.988747 0.081582 0.996565 -0.014204 -1.218741 -0.964020 0.075284 -0.254947 4.844392\n-0.259611 0.034707 0.965089 128.008881 0.080893 0.996623 -0.014080 -1.276645 -0.962319 0.074414 -0.261542 4.338080\n-0.264904 0.033799 0.963682 130.019150 0.082236 0.996536 -0.012346 -1.329713 -0.960762 0.075979 -0.266766 3.807419\n-0.270564 0.033899 0.962105 132.048828 0.084322 0.996373 -0.011394 -1.367996 -0.959002 0.078044 -0.272441 3.287223\n-0.275563 0.034565 0.960661 134.062988 0.086023 0.996231 -0.011169 -1.419691 -0.957426 0.079561 -0.277498 2.758814\n-0.279641 0.035274 0.959457 136.129517 0.085975 0.996230 -0.011568 -1.452236 -0.956248 0.079255 -0.281619 2.156014\n-0.284177 0.035145 0.958127 138.172668 0.085024 0.996315 -0.011328 -1.484873 -0.954994 0.078244 -0.286118 1.587301\n-0.289036 0.035554 0.956658 140.226730 0.083249 0.996458 -0.011881 -1.522515 -0.953692 0.076206 -0.290972 0.996600\n-0.293889 0.036371 0.955147 142.263199 0.082000 0.996551 -0.012717 -1.558199 -0.952316 0.074585 -0.295858 0.396246\n-0.299091 0.035527 0.953563 144.308731 0.080243 0.996704 -0.011965 -1.598517 -0.950845 0.072938 -0.300956 -0.222527\n-0.304995 0.035833 0.951680 146.301788 0.079831 0.996737 -0.011945 -1.639835 -0.949002 0.072331 -0.306860 -0.774481\n-0.309571 0.036279 0.950184 148.316940 0.079063 0.996794 -0.012300 -1.676846 -0.947584 0.071317 -0.311447 -1.432262\n-0.314638 0.035802 0.948536 150.319611 0.077842 0.996896 -0.011807 -1.717535 -0.946015 0.070121 -0.316448 -2.071382\n-0.319346 0.035055 0.946989 152.301697 0.077475 0.996936 -0.010778 -1.755561 -0.944466 0.069926 -0.321084 -2.703617\n-0.324915 0.034903 0.945099 154.290375 0.077141 0.996967 -0.010298 -1.785388 -0.942592 0.069560 -0.326622 -3.286336\n-0.328612 0.036099 0.943775 156.322495 0.078047 0.996889 -0.010955 -1.800438 -0.941235 0.070059 -0.330407 -3.997723\n-0.332476 0.037671 0.942359 158.331421 0.078478 0.996842 -0.012161 -1.824548 -0.939841 0.069911 -0.334383 -4.672129\n-0.336222 0.039098 0.940971 160.356552 0.077969 0.996864 -0.013561 -1.860736 -0.938550 0.068807 -0.338216 -5.373438\n-0.339422 0.040353 0.939768 162.367584 0.079045 0.996769 -0.014252 -1.904838 -0.937307 0.069447 -0.341515 -6.080944\n-0.343273 0.040537 0.938360 164.385498 0.077590 0.996877 -0.014680 -1.939391 -0.936025 0.067768 -0.345346 -6.757608\n-0.346784 0.040388 0.937075 166.451859 0.077166 0.996914 -0.014410 -1.978286 -0.934765 0.067313 -0.348831 -7.507367\n-0.350715 0.040184 0.935620 168.484070 0.076067 0.997000 -0.014306 -2.018817 -0.933388 0.066153 -0.352720 -8.229449\n-0.355146 0.041682 0.933881 170.570389 0.078243 0.996825 -0.014737 -2.042942 -0.931531 0.067836 -0.357280 -8.959569\n-0.358365 0.042011 0.932636 172.659088 0.080367 0.996667 -0.014014 -2.085652 -0.930116 0.069931 -0.360547 -9.748093\n-0.360349 0.040933 0.931919 174.717407 0.080331 0.996687 -0.012716 -2.133326 -0.929352 0.070280 -0.362444 -10.549022\n-0.362689 0.040490 0.931030 176.792023 0.079355 0.996769 -0.012436 -2.150929 -0.928526 0.069371 -0.364730 -11.365904\n-0.366254 0.041344 0.929596 178.957809 0.079016 0.996786 -0.013200 -2.199196 -0.927154 0.068619 -0.368343 -12.183985\n-0.368823 0.043341 0.928489 181.069031 0.078039 0.996829 -0.015531 -2.236404 -0.926218 0.066730 -0.371036 -13.026997\n-0.372219 0.045229 0.927042 183.227112 0.077389 0.996846 -0.017562 -2.280765 -0.924913 0.065206 -0.374545 -13.879342\n-0.375590 0.045970 0.925645 185.373993 0.075982 0.996934 -0.018679 -2.327979 -0.923666 0.063317 -0.377931 -14.743828\n-0.378448 0.046620 0.924448 187.559341 0.073813 0.997070 -0.020065 -2.367363 -0.922675 0.060643 -0.380781 -15.686193\n-0.383620 0.046917 0.922299 189.746826 0.073614 0.997084 -0.020102 -2.403254 -0.920552 0.060183 -0.385955 -16.544870\n-0.388088 0.045509 0.920498 191.949539 0.071168 0.997278 -0.019300 -2.427168 -0.918870 0.058020 -0.390270 -17.442738\n-0.392405 0.044454 0.918718 194.128143 0.069917 0.997383 -0.018397 -2.461792 -0.917131 0.057015 -0.394487 -18.349827\n-0.397203 0.043737 0.916688 196.285278 0.068530 0.997488 -0.017898 -2.498725 -0.915169 0.055712 -0.399202 -19.233173\n-0.401166 0.043923 0.914952 198.428070 0.066744 0.997596 -0.018626 -2.528385 -0.913570 0.053596 -0.403134 -20.159195\n-0.406097 0.044361 0.912753 200.615173 0.065614 0.997658 -0.019295 -2.579713 -0.911471 0.052054 -0.408057 -21.080242\n-0.409843 0.045306 0.911030 202.775055 0.065442 0.997652 -0.020174 -2.628216 -0.909806 0.051351 -0.411845 -22.038891\n-0.412598 0.046467 0.909727 204.916534 0.066339 0.997579 -0.020867 -2.679384 -0.908494 0.051741 -0.414682 -23.013020\n-0.415210 0.046997 0.908511 207.051849 0.067891 0.997481 -0.020572 -2.729910 -0.907189 0.053138 -0.417355 -23.997658\n-0.417672 0.046717 0.907396 209.203918 0.069303 0.997406 -0.019451 -2.786934 -0.905951 0.054761 -0.419827 -25.018835\n-0.420902 0.047524 0.905860 211.369797 0.069779 0.997364 -0.019902 -2.833035 -0.904418 0.054833 -0.423109 -26.000603\n-0.423886 0.048415 0.904421 213.586456 0.069228 0.997381 -0.020946 -2.882028 -0.903066 0.053732 -0.426127 -27.039865\n-0.427011 0.048918 0.902922 215.788376 0.068439 0.997420 -0.021671 -2.929463 -0.901653 0.052542 -0.429257 -28.080812\n-0.430907 0.050767 0.900967 218.024506 0.067633 0.997425 -0.023855 -2.960255 -0.899858 0.050656 -0.433231 -29.132162\n-0.435372 0.051956 0.898750 220.237228 0.066963 0.997437 -0.025223 -3.026275 -0.897757 0.049202 -0.437735 -30.162319\n-0.439946 0.052237 0.896504 222.454010 0.064584 0.997562 -0.026432 -3.087856 -0.895699 0.046272 -0.442247 -31.175909\n-0.444322 0.053467 0.894270 224.660217 0.065250 0.997498 -0.027219 -3.155208 -0.893488 0.046257 -0.446698 -32.235420\n-0.452361 0.053704 0.890217 226.955231 0.066569 0.997434 -0.026345 -3.220483 -0.889347 0.047344 -0.454775 -33.256870\n-0.461329 0.052727 0.885661 228.821060 0.067801 0.997409 -0.024064 -3.343231 -0.884635 0.048947 -0.463708 -34.034752\n-0.476509 0.049478 0.877776 230.203156 0.065335 0.997647 -0.020767 -3.529975 -0.876739 0.047454 -0.478620 -34.363632\n-0.482953 0.050013 0.874217 232.297607 0.064911 0.997665 -0.021216 -3.605247 -0.873237 0.046501 -0.485072 -35.440910\n-0.489911 0.049630 0.870359 234.225647 0.063715 0.997747 -0.021030 -3.671435 -0.869441 0.045152 -0.491969 -36.428692\n-0.494555 0.048947 0.867767 236.233353 0.062834 0.997814 -0.020472 -3.740995 -0.866872 0.044401 -0.496549 -37.559704\n-0.499265 0.049342 0.865043 238.315262 0.062831 0.997811 -0.020652 -3.805303 -0.864168 0.044040 -0.501272 -38.742859\n-0.504893 0.048470 0.861820 240.172272 0.062214 0.997869 -0.019674 -3.905938 -0.860937 0.043684 -0.506832 -39.733688\n-0.512813 0.046351 0.857248 241.984619 0.061417 0.997964 -0.017219 -4.037840 -0.856301 0.043819 -0.514615 -40.673721\n-0.516485 0.046399 0.855038 243.899002 0.060497 0.998013 -0.017614 -4.074812 -0.854156 0.042629 -0.518266 -41.895042\n-0.519827 0.047167 0.852969 245.853256 0.060633 0.997994 -0.018235 -4.126935 -0.852117 0.042239 -0.521643 -43.130371\n-0.522567 0.048083 0.851242 247.769943 0.061204 0.997948 -0.018798 -4.168614 -0.850399 0.042276 -0.524437 -44.341194\n-0.527365 0.047627 0.848303 249.774796 0.060495 0.997998 -0.018423 -4.214993 -0.847482 0.041602 -0.529191 -45.542217\n-0.530013 0.048514 0.846600 251.712097 0.059013 0.998052 -0.020248 -4.230621 -0.845933 0.039228 -0.531844 -46.789604\n-0.533402 0.050114 0.844376 253.670883 0.060381 0.997953 -0.021086 -4.247027 -0.843704 0.039737 -0.535336 -48.082832\n-0.534895 0.053356 0.843232 255.582947 0.061274 0.997826 -0.024269 -4.168272 -0.842694 0.038687 -0.537001 -49.501308\n-0.534360 0.056967 0.843335 257.367065 0.062195 0.997672 -0.027984 -4.063982 -0.842966 0.037498 -0.536658 -51.005501\n-0.537394 0.059836 0.841206 259.419159 0.064013 0.997496 -0.030059 -4.100092 -0.840899 0.037695 -0.539878 -52.428356\n-0.539321 0.062219 0.839798 261.462769 0.066443 0.997302 -0.031218 -4.131602 -0.839475 0.038962 -0.542000 -53.885048\n-0.539980 0.065479 0.839127 263.503876 0.068119 0.997099 -0.033971 -4.152133 -0.838917 0.038817 -0.542874 -55.408348\n-0.540878 0.067910 0.838355 265.450958 0.068821 0.996966 -0.036357 -4.206500 -0.838281 0.038032 -0.543911 -56.837940\n-0.541599 0.069116 0.837791 267.260864 0.069751 0.996872 -0.037148 -4.234990 -0.837738 0.038317 -0.544726 -58.167656\n-0.543428 0.069807 0.836548 269.249329 0.068078 0.996919 -0.038965 -4.266305 -0.836690 0.035776 -0.546506 -59.597717\n-0.545081 0.070112 0.835446 271.207825 0.067112 0.996948 -0.039878 -4.321754 -0.835693 0.034331 -0.548123 -61.003223\n-0.545908 0.071672 0.834774 273.092010 0.067280 0.996867 -0.041590 -4.372190 -0.835139 0.033459 -0.549020 -62.403709\n-0.547795 0.071394 0.833561 275.080170 0.065815 0.996942 -0.042135 -4.466098 -0.834020 0.031779 -0.550819 -63.831837\n-0.548932 0.070986 0.832847 277.048187 0.063505 0.997049 -0.043126 -4.544915 -0.833451 0.029217 -0.551821 -65.241096\n-0.548881 0.071128 0.832868 279.007599 0.064116 0.997020 -0.042893 -4.646405 -0.833438 0.029857 -0.551806 -66.727333\n-0.547770 0.071402 0.833577 280.941803 0.064094 0.997005 -0.043283 -4.674514 -0.834170 0.029719 -0.550706 -68.205437\n-0.548584 0.070265 0.833138 282.949585 0.062986 0.997104 -0.042620 -4.767658 -0.833720 0.029095 -0.551421 -69.605370\n-0.549300 0.070405 0.832654 284.996765 0.061425 0.997151 -0.043792 -4.806200 -0.833365 0.027091 -0.552059 -71.050217\n-0.549454 0.068383 0.832721 287.070251 0.057693 0.997371 -0.043837 -4.858534 -0.833530 0.023956 -0.551955 -72.650307\n-0.544468 0.066598 0.836133 289.114380 0.049814 0.997651 -0.047025 -4.737651 -0.837301 0.016047 -0.546507 -74.598557\n-0.541048 0.062749 0.838648 291.211578 0.038364 0.998016 -0.049923 -4.629896 -0.840116 0.005163 -0.542382 -76.457298\n-0.540937 0.060640 0.838874 293.366119 0.030842 0.998157 -0.052266 -4.654939 -0.840497 -0.002400 -0.541811 -78.094788\n-0.533391 0.056448 0.843983 295.234894 0.011393 0.998160 -0.059560 -4.446073 -0.845792 -0.022153 -0.533052 -79.948395\n-0.533224 0.052252 0.844359 297.133148 0.003498 0.998218 -0.059565 -4.517562 -0.845967 -0.028808 -0.532457 -81.316193\n-0.533944 0.044478 0.844349 299.156769 -0.011867 0.998123 -0.060083 -4.498016 -0.845437 -0.042101 -0.532413 -82.882118\n-0.535554 0.039350 0.843583 301.252808 -0.021356 0.997963 -0.060109 -4.542258 -0.844231 -0.050207 -0.533623 -84.426498\n-0.536321 0.033130 0.843364 303.227844 -0.033856 0.997581 -0.060718 -4.551115 -0.843335 -0.061118 -0.533902 -85.916809\n-0.536393 0.032876 0.843328 305.057770 -0.037172 0.997351 -0.062524 -4.606693 -0.843150 -0.064886 -0.533750 -87.275970\n-0.532351 0.033465 0.845862 306.717682 -0.036440 0.997386 -0.062394 -4.740662 -0.845739 -0.064038 -0.529740 -88.694946\n-0.533119 0.033543 0.845375 308.845490 -0.037861 0.997267 -0.063445 -4.876192 -0.845193 -0.065831 -0.530392 -90.190430\n-0.530971 0.030149 0.846854 311.042145 -0.045982 0.996869 -0.064320 -4.968877 -0.846142 -0.073092 -0.527922 -91.956406\n-0.530027 0.029219 0.847477 312.968414 -0.047051 0.996853 -0.063795 -5.101913 -0.846675 -0.073688 -0.526984 -93.411293\n-0.523238 0.027663 0.851738 314.668182 -0.052389 0.996538 -0.064550 -5.181444 -0.850575 -0.078397 -0.519977 -94.982613\n-0.517584 0.026739 0.855215 316.689392 -0.060448 0.995872 -0.067720 -5.247337 -0.853495 -0.086746 -0.513831 -96.745209\n-0.512802 0.028566 0.858031 318.136932 -0.060720 0.995737 -0.069440 -5.333635 -0.856357 -0.087708 -0.508881 -98.001762\n-0.512384 0.028658 0.858278 319.158020 -0.061334 0.995669 -0.069862 -5.396321 -0.856564 -0.088438 -0.508407 -98.694862\n-0.512904 0.024406 0.858099 320.084045 -0.068027 0.995297 -0.068970 -5.435822 -0.855746 -0.093749 -0.508831 -99.188011\n-0.516227 0.021248 0.856188 320.869720 -0.075639 0.994655 -0.070290 -5.457591 -0.853105 -0.101047 -0.511860 -99.511398\n-0.520287 0.019234 0.853775 321.690369 -0.079454 0.994320 -0.070820 -5.497258 -0.850287 -0.104683 -0.515804 -99.863281\n-0.517241 0.020132 0.855603 323.123718 -0.079505 0.994270 -0.071458 -5.604009 -0.852139 -0.104986 -0.512676 -101.020370\n-0.511615 0.022316 0.858925 324.527557 -0.081141 0.993940 -0.074155 -5.716979 -0.855375 -0.107632 -0.506704 -102.234985\n-0.507624 0.025331 0.861206 325.944885 -0.081521 0.993671 -0.077279 -5.838110 -0.857713 -0.109435 -0.502346 -103.404152\n-0.512203 0.027340 0.858429 327.033630 -0.080627 0.993549 -0.079752 -5.956963 -0.855071 -0.110062 -0.506695 -104.070381\n-0.515925 0.026467 0.856225 328.666260 -0.079051 0.993787 -0.078352 -6.145564 -0.852979 -0.108109 -0.510627 -105.067787\n-0.519021 0.025340 0.854386 330.753632 -0.076770 0.994139 -0.076121 -6.390900 -0.851307 -0.105099 -0.514034 -106.365005\n-0.525212 0.027052 0.850541 332.037872 -0.072270 0.994466 -0.076257 -6.567390 -0.847897 -0.101520 -0.520350 -107.134911\n-0.527033 0.026590 0.849429 333.040070 -0.069866 0.994771 -0.074489 -6.763045 -0.846968 -0.098604 -0.522420 -107.981827\n-0.525601 0.029454 0.850221 333.433533 -0.058793 0.995753 -0.070841 -6.924096 -0.848697 -0.087221 -0.521637 -108.559410\n-0.527550 0.030779 0.848966 333.626312 -0.055142 0.995995 -0.070375 -6.985267 -0.847733 -0.083940 -0.523740 -108.826523\n-0.526842 0.035954 0.849203 333.937927 -0.045527 0.996477 -0.070434 -7.085982 -0.848743 -0.075769 -0.523349 -109.357834\n-0.518900 0.043434 0.853731 334.353058 -0.034866 0.996802 -0.071904 -7.190951 -0.854124 -0.067077 -0.515726 -110.309227\n-0.514578 0.044095 0.856309 334.550659 -0.034311 0.996818 -0.071949 -7.218022 -0.856757 -0.066404 -0.511428 -110.819130\n-0.510222 0.045855 0.858820 334.661621 -0.032552 0.996832 -0.072562 -7.237518 -0.859427 -0.064979 -0.507113 -111.152168\n-0.512466 0.047018 0.857420 334.748718 -0.032622 0.996713 -0.074154 -7.253392 -0.858088 -0.065973 -0.509247 -111.245522\n-0.515174 0.046111 0.855844 334.850311 -0.040135 0.996158 -0.077830 -7.278508 -0.856145 -0.074445 -0.511345 -111.327705\n-0.521175 0.052333 0.851844 334.995331 -0.044642 0.995080 -0.088445 -7.303875 -0.852281 -0.084123 -0.516275 -111.374893\n-0.525863 0.063675 0.848182 335.122742 -0.032415 0.994969 -0.094792 -7.342525 -0.849951 -0.077341 -0.521154 -111.404793\n-0.525415 0.066199 0.848267 335.220306 -0.021479 0.995619 -0.091002 -7.400571 -0.850575 -0.066033 -0.521692 -111.528328\n-0.527047 0.058322 0.847833 335.310852 -0.021637 0.996398 -0.081992 -7.403426 -0.849561 -0.061558 -0.523887 -111.599411\n-0.529408 0.049376 0.846929 335.430908 -0.027114 0.996810 -0.075063 -7.388753 -0.847934 -0.062703 -0.526380 -111.691002\n-0.531556 0.045231 0.845814 335.548920 -0.032058 0.996783 -0.073452 -7.367796 -0.846416 -0.066159 -0.528397 -111.823166\n-0.530960 0.046706 0.846109 335.639191 -0.033060 0.996578 -0.075758 -7.339345 -0.846752 -0.068197 -0.527599 -111.992508\n-0.533759 0.053047 0.843971 335.775604 -0.025877 0.996539 -0.079002 -7.337236 -0.845241 -0.064007 -0.530539 -112.133369\n-0.534025 0.060401 0.843308 335.870453 -0.021248 0.996171 -0.084805 -7.325337 -0.845202 -0.063206 -0.530697 -112.257774\n-0.535077 0.064034 0.842373 336.001099 -0.019915 0.995890 -0.088354 -7.336224 -0.844569 -0.064052 -0.531602 -112.394516\n-0.538678 0.065367 0.839972 336.192444 -0.019408 0.995758 -0.089936 -7.338190 -0.842288 -0.064749 -0.535125 -112.513893\n-0.542482 0.065482 0.837511 336.331116 -0.018036 0.995820 -0.089543 -7.343232 -0.839874 -0.063681 -0.539034 -112.617142\n-0.548312 0.065642 0.833694 336.492279 -0.019224 0.995662 -0.091039 -7.310689 -0.836053 -0.065945 -0.544671 -112.663498\n-0.551082 0.066689 0.831782 336.596802 -0.018589 0.995573 -0.092137 -7.310359 -0.834244 -0.066237 -0.547402 -112.755470\n-0.550129 0.068809 0.832240 336.655884 -0.017149 0.995458 -0.093639 -7.310862 -0.834903 -0.065786 -0.546451 -112.922180\n-0.554643 0.070517 0.829095 336.769073 -0.015585 0.995347 -0.095084 -7.314146 -0.831942 -0.065659 -0.550964 -112.977516\n-0.558202 0.071726 0.826599 336.813446 -0.012790 0.995394 -0.095010 -7.305700 -0.829607 -0.063607 -0.554713 -113.049492\n-0.560097 0.071499 0.825336 336.927826 -0.011540 0.995499 -0.094072 -7.301271 -0.828347 -0.062213 -0.556751 -113.170143\n-0.562693 0.071404 0.823577 337.033936 -0.010388 0.995573 -0.093413 -7.301754 -0.826601 -0.061118 -0.559460 -113.274689\n-0.562578 0.073175 0.823499 337.109039 -0.011139 0.995314 -0.096053 -7.306846 -0.826669 -0.063210 -0.559127 -113.454720\n-0.567624 0.074574 0.819904 337.143524 -0.014821 0.994802 -0.100742 -7.308125 -0.823155 -0.069335 -0.563568 -113.503113\n-0.569088 0.075606 0.818794 337.189606 -0.016780 0.994489 -0.103492 -7.328860 -0.822106 -0.072635 -0.564682 -113.642426\n-0.571724 0.074227 0.817082 337.293427 -0.019156 0.994420 -0.103741 -7.351260 -0.820223 -0.074963 -0.567111 -113.728394\n-0.576272 0.073496 0.813946 337.440094 -0.022174 0.994175 -0.105470 -7.372817 -0.816957 -0.078828 -0.571286 -113.813637\n-0.577303 0.072253 0.813327 337.741669 -0.025567 0.993989 -0.106450 -7.462590 -0.816130 -0.082248 -0.571985 -114.031677\n-0.577238 0.073369 0.813274 337.789490 -0.024082 0.993993 -0.106765 -7.519067 -0.816221 -0.081214 -0.572003 -114.155922\n-0.580004 0.073141 0.811323 338.460114 -0.020127 0.994370 -0.104031 -7.643656 -0.814365 -0.076668 -0.575267 -114.642776\n-0.578827 0.070616 0.812387 338.585388 -0.018586 0.994842 -0.099719 -7.705938 -0.815239 -0.072819 -0.574529 -114.796440\n-0.578261 0.069737 0.812866 339.480042 -0.012220 0.995488 -0.094097 -7.840889 -0.815760 -0.064346 -0.574800 -115.602974\n-0.582618 0.070350 0.809696 340.712311 -0.006062 0.995843 -0.090885 -7.948938 -0.812724 -0.057860 -0.579769 -116.354431\n-0.583088 0.071681 0.809240 341.841583 -0.008917 0.995475 -0.094602 -8.055003 -0.812360 -0.062377 -0.579811 -117.309937\n-0.582020 0.072666 0.809921 343.794342 -0.014619 0.994903 -0.099768 -8.309961 -0.813043 -0.069907 -0.577992 -118.784416\n-0.580485 0.069969 0.811260 345.727997 -0.021142 0.994670 -0.100915 -8.589147 -0.813997 -0.075732 -0.575912 -120.218803\n-0.579094 0.072906 0.811994 345.994537 -0.017175 0.994681 -0.101557 -8.639430 -0.815080 -0.072757 -0.574762 -120.928734\n-0.562937 0.077144 0.822892 346.577057 -0.013026 0.994683 -0.102160 -8.721931 -0.826397 -0.068228 -0.558939 -122.339027\n-0.558664 0.078339 0.825686 347.813080 -0.010910 0.994749 -0.101762 -8.916726 -0.829322 -0.065859 -0.554876 -123.364815\n-0.555958 0.080656 0.827288 349.635559 -0.011873 0.994409 -0.104928 -9.175570 -0.831125 -0.068158 -0.551892 -124.676689\n-0.556963 0.075976 0.827055 351.838257 -0.020078 0.994284 -0.104859 -9.460476 -0.830295 -0.075008 -0.552254 -126.035706\n-0.561089 0.070176 0.824775 354.003418 -0.029207 0.994101 -0.104453 -9.736699 -0.827240 -0.082696 -0.555730 -127.261253\n-0.566272 0.067526 0.821448 356.162109 -0.033698 0.993908 -0.104933 -10.062329 -0.823529 -0.087102 -0.560547 -128.501114\n-0.558672 0.069897 0.826438 356.688385 -0.029162 0.994171 -0.103797 -10.222590 -0.828876 -0.082089 -0.553377 -129.332474\n-0.556897 0.069745 0.827648 357.156586 -0.026825 0.994438 -0.101850 -10.332072 -0.830148 -0.078922 -0.551929 -129.737457\n-0.563696 0.065224 0.823403 358.295685 -0.031887 0.994416 -0.100600 -10.453437 -0.825366 -0.082963 -0.558469 -129.989288\n-0.570008 0.061067 0.819366 360.289978 -0.034053 0.994622 -0.097818 -10.732450 -0.820933 -0.083659 -0.564863 -131.002792\n-0.580145 0.055189 0.812641 361.586121 -0.041198 0.994437 -0.096947 -10.937444 -0.813471 -0.089722 -0.574644 -131.203369\n-0.581462 0.055030 0.811711 362.567780 -0.044363 0.994081 -0.099173 -11.063014 -0.812364 -0.093675 -0.575578 -131.668533\n-0.585260 0.053351 0.809088 363.568390 -0.047331 0.993884 -0.099773 -11.196525 -0.809463 -0.096688 -0.579156 -131.990891\n-0.587381 0.054103 0.807500 364.184387 -0.044347 0.994112 -0.098864 -11.276801 -0.808095 -0.093881 -0.581524 -132.321960\n-0.598397 0.050425 0.799611 364.722717 -0.047188 0.994067 -0.098001 -11.253993 -0.799809 -0.096376 -0.592468 -131.873734\n-0.597679 0.049503 0.800205 365.074707 -0.047930 0.994101 -0.097298 -11.329407 -0.800301 -0.096507 -0.591781 -132.204651\n-0.595426 0.050735 0.801807 365.481812 -0.046680 0.994133 -0.097569 -11.387387 -0.802053 -0.095524 -0.589564 -132.657700\n-0.602289 0.051547 0.796612 366.003021 -0.048623 0.993691 -0.101061 -11.320363 -0.796796 -0.099601 -0.595983 -132.729843\n-0.601829 0.052028 0.796928 366.523346 -0.046705 0.993875 -0.100157 -11.387449 -0.797258 -0.097498 -0.595713 -133.111954\n-0.604359 0.053692 0.794901 367.350677 -0.046374 0.993664 -0.102376 -11.459931 -0.795361 -0.098735 -0.598040 -133.562683\n-0.627916 0.037467 0.777379 368.641724 -0.064482 0.992902 -0.099939 -11.577702 -0.775605 -0.112880 -0.621043 -132.766357\n-0.649620 0.027931 0.759746 369.481689 -0.073762 0.992295 -0.099550 -11.648340 -0.756672 -0.120710 -0.642554 -132.024506\n-0.643401 0.027737 0.765027 369.968597 -0.073782 0.992444 -0.098034 -11.745391 -0.761966 -0.119521 -0.636493 -132.752853\n-0.657852 0.022223 0.752819 371.460297 -0.080808 0.991712 -0.099889 -11.863400 -0.748800 -0.126546 -0.650604 -133.047684\n-0.654563 0.028090 0.755485 371.468933 -0.074280 0.992085 -0.101244 -11.893529 -0.752349 -0.122388 -0.647295 -133.468826\n-0.651291 0.032195 0.758145 371.764557 -0.070314 0.992241 -0.102539 -11.963263 -0.755563 -0.120091 -0.643974 -134.016663\n-0.651213 0.038507 0.757918 372.371277 -0.061729 0.992715 -0.103475 -12.069709 -0.756381 -0.114169 -0.644091 -134.698044\n-0.650488 0.041204 0.758398 372.395325 -0.059944 0.992627 -0.105345 -12.050139 -0.757147 -0.113987 -0.643223 -134.984634\n-0.650315 0.039266 0.758649 372.544678 -0.060994 0.992740 -0.103666 -12.118837 -0.757212 -0.113688 -0.643199 -135.211761\n-0.650328 0.040944 0.758549 372.747681 -0.059382 0.992751 -0.104496 -12.139651 -0.757329 -0.113001 -0.643182 -135.496277\n-0.646969 0.042521 0.761330 372.908630 -0.061116 0.992340 -0.107359 -12.133017 -0.760063 -0.115987 -0.639415 -135.953308\n-0.645319 0.045835 0.762537 373.246765 -0.058920 0.992239 -0.109504 -12.191011 -0.761637 -0.115594 -0.637610 -136.440155\n-0.644827 0.051999 0.762558 374.045959 -0.047969 0.992963 -0.108273 -12.401638 -0.762822 -0.106396 -0.637795 -137.336823\n-0.651435 0.057399 0.756530 374.626160 -0.047256 0.992128 -0.115966 -12.324030 -0.757231 -0.111295 -0.643595 -137.623718\n-0.653321 0.058364 0.754828 374.871246 -0.047009 0.991973 -0.117388 -12.402025 -0.755620 -0.112176 -0.645333 -137.842392\n-0.656727 0.058257 0.751874 375.123169 -0.047794 0.991792 -0.118592 -12.460649 -0.752612 -0.113818 -0.648553 -137.995438\n-0.656980 0.061555 0.751391 375.271637 -0.043668 0.991881 -0.119438 -12.504953 -0.752643 -0.111280 -0.648958 -138.288605\n-0.656609 0.063263 0.751573 375.613495 -0.039264 0.992258 -0.117825 -12.610271 -0.753208 -0.106875 -0.649042 -138.759872\n-0.656167 0.065823 0.751739 375.853088 -0.033941 0.992606 -0.116540 -12.702556 -0.753852 -0.101985 -0.649082 -139.105148\n-0.657286 0.067994 0.750568 376.261322 -0.031582 0.992562 -0.117573 -12.802392 -0.752979 -0.100984 -0.650250 -139.517120\n-0.662233 0.072653 0.745768 376.606995 -0.023730 0.992755 -0.117787 -12.878148 -0.748923 -0.095699 -0.655711 -139.787781\n-0.644644 0.084471 0.759802 376.692474 -0.013780 0.992431 -0.122026 -12.820226 -0.764359 -0.089133 -0.638601 -141.233749\n-0.650501 0.085984 0.754622 377.100555 -0.010062 0.992508 -0.121762 -12.908302 -0.759439 -0.086799 -0.644763 -141.526367\n-0.640001 0.091167 0.762946 376.879852 -0.005040 0.992417 -0.122815 -12.912478 -0.768357 -0.082447 -0.634689 -142.118973\n-0.639849 0.094144 0.762713 377.160187 -0.000240 0.992444 -0.122702 -13.030122 -0.768501 -0.078694 -0.634991 -142.597931\n-0.623998 0.101470 0.774810 376.883881 0.003228 0.991860 -0.127296 -12.893928 -0.781419 -0.076932 -0.619246 -143.542160\n-0.613867 0.107215 0.782095 377.778900 0.007992 0.991527 -0.129652 -12.959331 -0.789369 -0.073339 -0.609523 -145.033279\n-0.634466 0.100716 0.766361 379.389801 0.004934 0.991982 -0.126282 -13.266770 -0.772935 -0.076341 -0.629876 -145.214981\n-0.640264 0.104280 0.761043 379.715576 0.007044 0.991498 -0.129932 -13.308273 -0.768122 -0.077830 -0.635555 -145.458496\n-0.642161 0.108298 0.758881 380.189148 0.010565 0.991127 -0.132501 -13.418745 -0.766497 -0.077069 -0.637607 -145.862045\n-0.637480 0.114187 0.761958 380.621887 0.010672 0.990170 -0.139459 -13.364236 -0.770393 -0.080771 -0.632433 -146.677628\n-0.637237 0.114255 0.762151 381.238251 0.012898 0.990392 -0.137687 -13.477625 -0.770560 -0.077909 -0.632588 -147.229050\n-0.638645 0.115910 0.760722 382.864136 0.017005 0.990475 -0.136641 -13.755198 -0.769314 -0.074329 -0.634533 -148.480209\n-0.627873 0.121478 0.768777 382.502167 0.021337 0.990060 -0.139017 -13.713139 -0.778023 -0.070881 -0.624224 -148.995758\n-0.620503 0.124685 0.774228 383.158539 0.022918 0.989741 -0.141024 -13.872907 -0.783869 -0.069762 -0.616995 -149.969650\n-0.619650 0.125734 0.774742 383.226166 0.024167 0.989674 -0.141287 -13.958245 -0.784506 -0.068826 -0.616290 -150.157196\n-0.604653 0.130918 0.785656 383.543610 0.026018 0.989119 -0.144798 -13.994335 -0.796064 -0.067111 -0.601480 -151.140701\n-0.613103 0.126062 0.779880 384.763916 0.024036 0.989706 -0.141083 -14.296615 -0.789638 -0.067754 -0.609821 -151.431793\n-0.621089 0.121088 0.774330 386.022339 0.018120 0.989947 -0.140271 -14.523675 -0.783531 -0.073090 -0.617039 -151.781860\n-0.621107 0.116349 0.775042 387.141541 0.012692 0.990283 -0.138489 -14.801149 -0.783623 -0.076180 -0.616548 -152.435913\n-0.622842 0.116980 0.773553 387.597321 0.013054 0.990174 -0.139227 -14.903425 -0.782239 -0.076618 -0.618249 -152.779572\n-0.618260 0.118498 0.776990 388.094696 0.015139 0.990182 -0.138965 -15.031359 -0.785828 -0.074153 -0.613983 -153.442719\n-0.626072 0.115112 0.771222 389.093658 0.012675 0.990415 -0.137539 -15.218628 -0.779662 -0.076334 -0.621530 -153.769852\n-0.623679 0.115413 0.773114 389.174011 0.012537 0.990390 -0.137735 -15.265962 -0.781580 -0.076210 -0.619132 -154.121277\n-0.618474 0.119584 0.776652 389.076324 0.013699 0.989843 -0.141501 -15.239619 -0.785686 -0.076876 -0.613831 -154.515442\n-0.618833 0.120521 0.776222 389.909790 0.014016 0.989697 -0.142492 -15.404784 -0.785398 -0.077300 -0.614146 -155.000656\n-0.594259 0.133121 0.793180 389.300232 0.015276 0.987897 -0.154355 -15.144421 -0.804129 -0.079611 -0.589100 -155.781876\n-0.596400 0.135051 0.791245 389.767303 0.017769 0.987724 -0.155193 -15.214606 -0.802491 -0.078497 -0.591478 -156.116791\n-0.593062 0.137228 0.793377 390.369110 0.019899 0.987566 -0.155942 -15.347342 -0.804911 -0.076695 -0.588418 -156.757721\n-0.590617 0.140462 0.794633 390.419891 0.022297 0.987199 -0.157929 -15.417561 -0.806644 -0.075557 -0.586188 -157.059143\n-0.592850 0.141331 0.792815 391.037994 0.022688 0.987020 -0.158986 -15.561125 -0.804994 -0.076268 -0.588361 -157.447723\n-0.593186 0.141283 0.792571 391.556580 0.022183 0.986975 -0.159334 -15.715201 -0.804760 -0.076934 -0.588594 -157.872269\n-0.582589 0.151869 0.798452 391.007935 0.042687 0.986749 -0.156537 -15.749319 -0.811645 -0.057113 -0.581352 -158.177826\n-0.572808 0.164909 0.802929 390.446411 0.070512 0.985836 -0.152172 -15.893288 -0.816651 -0.030549 -0.576323 -158.381851\n-0.564988 0.167063 0.808009 390.360199 0.072724 0.985559 -0.152922 -15.953237 -0.821888 -0.027637 -0.568979 -158.884583\n-0.567496 0.165955 0.806478 390.712555 0.072982 0.985761 -0.151492 -16.095268 -0.820135 -0.027113 -0.571527 -159.040619\n-0.572053 0.165706 0.803304 391.268585 0.067819 0.985582 -0.155011 -16.178198 -0.817408 -0.034195 -0.575043 -159.319687\n-0.570674 0.163459 0.804744 391.348175 0.064944 0.985903 -0.154201 -16.298304 -0.818605 -0.035735 -0.573245 -159.651657\n-0.571168 0.161794 0.804730 391.914795 0.063878 0.986170 -0.152934 -16.429951 -0.818344 -0.035947 -0.573604 -160.067703\n-0.574546 0.160457 0.802590 392.360779 0.061361 0.986280 -0.153255 -16.507528 -0.816169 -0.038804 -0.576509 -160.345718\n-0.572059 0.165227 0.803398 392.565613 0.063282 0.985471 -0.157612 -16.478519 -0.817768 -0.039323 -0.574204 -160.704147\n-0.562927 0.173157 0.808164 392.203613 0.070126 0.984288 -0.162047 -16.389927 -0.823526 -0.034547 -0.566225 -161.044006\n-0.564075 0.173705 0.807246 392.622742 0.067911 0.984070 -0.164301 -16.513802 -0.822926 -0.037857 -0.566885 -161.395493\n-0.563577 0.175297 0.807249 392.944946 0.070485 0.983865 -0.164441 -16.612995 -0.823051 -0.035776 -0.566840 -161.787994\n-0.557157 0.176460 0.811442 393.269653 0.074097 0.983828 -0.163071 -16.740356 -0.827095 -0.030731 -0.561221 -162.360764\n-0.556205 0.176976 0.811982 393.810486 0.072912 0.983686 -0.164455 -16.824671 -0.827840 -0.032267 -0.560035 -162.785477\n-0.546561 0.179401 0.817977 393.761261 0.075127 0.983349 -0.165472 -16.818377 -0.834043 -0.028989 -0.550938 -163.284622\n-0.546891 0.180298 0.817559 394.343353 0.075265 0.983169 -0.166473 -16.957129 -0.833813 -0.029509 -0.551257 -163.711685\n-0.536216 0.186480 0.823224 394.062134 0.078533 0.982082 -0.171312 -16.826864 -0.840420 -0.027210 -0.541253 -164.130661\n-0.536912 0.187539 0.822530 394.442688 0.079052 0.981873 -0.172268 -16.902142 -0.839926 -0.027470 -0.542005 -164.445236\n-0.534913 0.187410 0.823860 394.757507 0.076815 0.981838 -0.173473 -16.974180 -0.841408 -0.029509 -0.539594 -164.814804\n-0.530303 0.188032 0.826694 395.121338 0.075817 0.981706 -0.174656 -17.052179 -0.844411 -0.029943 -0.534858 -165.264877\n-0.530172 0.187773 0.826837 395.547668 0.074724 0.981723 -0.175034 -17.157633 -0.844591 -0.031014 -0.534513 -165.605515\n-0.531002 0.189681 0.825868 395.909882 0.075251 0.981329 -0.177003 -17.231277 -0.844023 -0.031842 -0.535361 -165.826813\n-0.535734 0.189675 0.822808 396.735321 0.077514 0.981377 -0.175760 -17.461458 -0.840822 -0.030381 -0.540459 -166.202255\n-0.538429 0.189294 0.821134 398.388519 0.077421 0.981434 -0.175482 -17.783138 -0.839107 -0.030912 -0.543088 -166.974350\n-0.538259 0.188379 0.821456 399.043915 0.076903 0.981611 -0.174716 -17.948559 -0.839263 -0.030870 -0.542848 -167.443573\n-0.534649 0.189901 0.823461 399.445953 0.077610 0.981340 -0.175920 -18.076042 -0.841503 -0.030147 -0.539411 -167.959427\n-0.533382 0.189789 0.824309 399.988678 0.076333 0.981328 -0.176548 -18.249620 -0.842424 -0.031246 -0.537909 -168.399033\n-0.537744 0.188567 0.821750 400.985443 0.075488 0.981522 -0.175832 -18.475397 -0.839722 -0.032520 -0.542042 -168.733292\n-0.532819 0.187572 0.825179 401.282074 0.076247 0.981800 -0.173941 -18.590870 -0.842787 -0.029762 -0.537424 -169.164230\n-0.530929 0.184723 0.827038 401.838715 0.073470 0.982311 -0.172240 -18.772369 -0.844225 -0.030685 -0.535109 -169.529327\n-0.522605 0.187985 0.831593 401.965820 0.076652 0.981799 -0.173769 -18.764629 -0.849122 -0.027069 -0.527502 -170.007919\n-0.516642 0.190152 0.834819 402.125610 0.078450 0.981438 -0.174999 -18.783871 -0.852600 -0.024920 -0.521970 -170.432724\n-0.509360 0.193015 0.838629 402.291595 0.081012 0.980949 -0.176566 -18.809511 -0.856732 -0.021996 -0.515293 -170.834091\n-0.503034 0.194620 0.842069 402.682770 0.079993 0.980618 -0.178855 -18.892311 -0.860557 -0.022611 -0.508852 -171.255173\n-0.506746 0.194253 0.839925 403.414093 0.079101 0.980650 -0.179076 -19.081699 -0.858459 -0.024307 -0.512306 -171.528076\n-0.503064 0.194425 0.842096 403.984009 0.077895 0.980602 -0.179869 -19.218014 -0.860732 -0.024890 -0.508450 -172.017349\n-0.507201 0.194743 0.839537 404.929138 0.077472 0.980494 -0.180635 -19.386572 -0.858339 -0.026577 -0.512395 -172.343689\n-0.508029 0.194443 0.839106 405.425446 0.076394 0.980519 -0.180961 -19.517376 -0.857946 -0.027831 -0.512986 -172.685837\n-0.514680 0.194349 0.835065 406.401337 0.076526 0.980496 -0.181031 -19.706327 -0.853960 -0.029269 -0.519514 -172.906128\n-0.519951 0.193421 0.832009 407.270721 0.075926 0.980636 -0.180525 -19.943655 -0.850815 -0.030693 -0.524568 -173.300186\n-0.514700 0.195364 0.834815 408.424591 0.071708 0.980090 -0.185150 -20.106424 -0.854366 -0.035433 -0.518462 -174.141632\n-0.523733 0.194793 0.829312 409.804718 0.070988 0.980099 -0.185380 -20.349545 -0.848919 -0.038219 -0.527139 -174.420517\n-0.540435 0.196236 0.818182 411.432495 0.074035 0.979741 -0.186083 -20.567293 -0.838122 -0.039992 -0.544015 -174.520081\n-0.553126 0.196277 0.809646 412.811462 0.073890 0.979578 -0.186993 -20.794298 -0.829814 -0.043606 -0.556333 -174.698669\n-0.565323 0.195263 0.801425 414.087982 0.070607 0.979467 -0.188835 -21.011532 -0.821842 -0.050167 -0.567503 -174.959534\n-0.577361 0.194507 0.792983 415.348633 0.068376 0.979317 -0.190428 -21.267958 -0.813621 -0.055725 -0.578719 -175.238708\n-0.588808 0.195082 0.784378 416.912170 0.067347 0.978903 -0.192907 -21.543966 -0.805462 -0.060760 -0.589524 -175.621292\n-0.599088 0.198113 0.775787 418.679779 0.072514 0.978349 -0.193844 -21.865410 -0.797393 -0.059874 -0.600483 -176.116043\n-0.610320 0.199434 0.766639 420.612671 0.073443 0.977867 -0.195916 -22.243616 -0.788743 -0.063267 -0.611459 -176.985428\n-0.610839 0.200543 0.765936 421.332581 0.074638 0.977668 -0.196456 -22.430861 -0.788229 -0.062835 -0.612166 -177.587708\n-0.606862 0.203243 0.768382 421.538269 0.077749 0.977296 -0.197096 -22.530338 -0.790995 -0.059869 -0.608886 -178.148605\n-0.603336 0.204314 0.770871 422.101318 0.077202 0.977048 -0.198536 -22.723198 -0.793742 -0.060271 -0.605262 -178.764420\n-0.599297 0.204157 0.774056 422.536774 0.075397 0.977030 -0.199317 -22.893646 -0.796969 -0.061089 -0.600924 -179.340805\n-0.589718 0.210871 0.779594 422.595215 0.074865 0.975428 -0.207211 -22.797304 -0.804132 -0.063832 -0.591014 -179.928452\n-0.581551 0.217334 0.783941 422.295258 0.080489 0.974297 -0.210397 -22.664934 -0.809518 -0.059258 -0.584097 -180.309296\n-0.575053 0.224330 0.786759 421.965271 0.088894 0.973111 -0.212491 -22.518295 -0.813272 -0.052256 -0.579532 -180.598663\n-0.586234 0.224717 0.778352 423.120422 0.090895 0.972938 -0.212436 -22.754858 -0.805027 -0.053789 -0.590795 -180.870773\n-0.599854 0.225901 0.767557 423.842957 0.096263 0.972723 -0.211053 -22.935648 -0.794298 -0.052714 -0.605237 -180.972015\n-0.593065 0.233631 0.770513 423.603760 0.101082 0.971008 -0.216622 -22.810249 -0.798784 -0.050586 -0.599487 -181.353470\n-0.594030 0.235415 0.769225 423.930023 0.100243 0.970431 -0.219581 -22.874813 -0.798173 -0.053328 -0.600064 -181.708984\n-0.593018 0.241209 0.768211 424.109802 0.104656 0.969072 -0.223488 -22.865042 -0.798359 -0.052135 -0.599921 -181.984604\n-0.586819 0.248018 0.770799 423.884674 0.111005 0.967588 -0.226829 -22.759937 -0.802073 -0.047545 -0.595330 -182.297150\n-0.592813 0.245627 0.766968 424.968628 0.107607 0.967972 -0.226827 -23.143358 -0.798119 -0.051935 -0.600258 -182.848831\n-0.577062 0.254980 0.775877 424.719452 0.115019 0.965918 -0.231888 -23.025593 -0.808560 -0.044574 -0.586723 -183.470642\n-0.576228 0.252799 0.777209 425.319061 0.111714 0.966397 -0.231510 -23.261473 -0.809618 -0.046578 -0.585106 -184.019836\n-0.562014 0.258273 0.785771 424.995148 0.114022 0.965121 -0.235670 -23.138866 -0.819231 -0.042854 -0.571860 -184.548782\n-0.555953 0.263759 0.788256 424.674316 0.115947 0.963656 -0.240673 -22.963217 -0.823087 -0.042407 -0.566329 -184.712891\n-0.541547 0.271733 0.795542 424.224701 0.122263 0.961716 -0.245264 -22.795708 -0.831732 -0.035557 -0.554038 -185.195496\n-0.530357 0.280213 0.800126 423.815674 0.129466 0.959496 -0.250211 -22.612051 -0.837830 -0.029112 -0.545154 -185.554413\n-0.522868 0.280479 0.804948 423.894226 0.126690 0.959398 -0.252002 -22.683533 -0.842946 -0.029785 -0.537172 -186.076447\n-0.509487 0.286489 0.811386 423.546814 0.131463 0.957793 -0.255635 -22.482697 -0.850377 -0.023575 -0.525646 -186.577759\n-0.502958 0.278710 0.818141 423.431183 0.122134 0.960001 -0.251954 -22.665541 -0.855638 -0.026800 -0.516880 -187.076080\n-0.486777 0.289898 0.824019 423.243774 0.125338 0.956743 -0.262550 -22.448313 -0.864488 -0.024522 -0.502056 -187.775269\n-0.470690 0.298663 0.830212 422.944031 0.131310 0.954198 -0.268819 -22.282238 -0.872472 -0.017515 -0.488349 -188.327698\n-0.457122 0.305342 0.835348 422.380463 0.138156 0.952199 -0.272452 -22.038469 -0.878608 -0.009135 -0.477456 -188.552811\n-0.437356 0.313502 0.842874 422.033600 0.143459 0.949587 -0.278754 -21.777128 -0.887772 -0.000997 -0.460282 -189.159409\n-0.423389 0.320724 0.847277 421.874023 0.151796 0.947131 -0.282669 -21.644800 -0.893140 0.008934 -0.449690 -189.654510\n-0.410259 0.329991 0.850173 421.618774 0.155141 0.943904 -0.291508 -21.406490 -0.898676 0.012303 -0.438440 -190.121918\n-0.400205 0.335745 0.852708 421.295441 0.157236 0.941830 -0.297040 -21.236961 -0.902836 0.015200 -0.429717 -190.483597\n-0.386931 0.342822 0.856012 420.916046 0.159668 0.939207 -0.303968 -20.991957 -0.908180 0.019063 -0.418146 -190.923431\n-0.374170 0.349246 0.859084 420.572540 0.167206 0.936600 -0.307933 -20.806925 -0.912162 0.028425 -0.408843 -191.360718\n-0.361935 0.354958 0.861979 420.413605 0.174816 0.934108 -0.311257 -20.658886 -0.915665 0.038033 -0.400139 -191.815872\n-0.344259 0.360401 0.866947 420.037323 0.178960 0.931649 -0.316234 -20.403490 -0.921661 0.046282 -0.385226 -192.370224\n-0.335230 0.362895 0.869441 420.020447 0.177722 0.930630 -0.319911 -20.363441 -0.925222 0.047275 -0.376470 -192.842529\n-0.320393 0.368751 0.872566 419.782501 0.182526 0.927893 -0.325112 -20.153633 -0.929533 0.055103 -0.364597 -193.347076\n-0.302664 0.375427 0.876041 419.400146 0.187442 0.924648 -0.331498 -19.848198 -0.934484 0.063875 -0.350229 -193.899063\n-0.292478 0.377250 0.878715 419.295044 0.186968 0.923721 -0.334340 -19.804543 -0.937816 0.066505 -0.340701 -194.315140\n-0.278011 0.382432 0.881167 419.145172 0.189049 0.921173 -0.340149 -19.620333 -0.941791 0.072019 -0.328395 -194.882004\n-0.269137 0.387194 0.881843 419.134644 0.195059 0.918566 -0.343786 -19.531750 -0.943142 0.079486 -0.322745 -195.255768\n-0.277700 0.383695 0.880716 420.267975 0.189060 0.920674 -0.341490 -20.064350 -0.941880 0.071677 -0.328213 -195.306107\n-0.262673 0.388044 0.883416 420.205627 0.190350 0.918414 -0.346819 -19.869511 -0.945923 0.077059 -0.315107 -195.875748\n-0.252478 0.391497 0.884864 419.810852 0.193640 0.916435 -0.350214 -19.607170 -0.948029 0.082924 -0.307189 -196.176147\n-0.246970 0.394418 0.885121 420.014862 0.192810 0.915154 -0.354002 -19.635450 -0.949647 0.083232 -0.302063 -196.513046\n-0.243842 0.394806 0.885815 420.271088 0.189654 0.915163 -0.355680 -19.776827 -0.951090 0.081269 -0.298032 -196.850922\n-0.240544 0.395673 0.886330 420.707733 0.188983 0.914759 -0.357074 -19.945852 -0.952063 0.081610 -0.294815 -197.170135\n-0.235541 0.395654 0.887681 420.986938 0.187229 0.914743 -0.358036 -20.086699 -0.953659 0.081868 -0.289538 -197.512756\n-0.231325 0.393247 0.889857 421.401917 0.184944 0.915762 -0.356617 -20.363020 -0.955136 0.082079 -0.284567 -197.909683\n-0.226825 0.392321 0.891423 421.779388 0.184843 0.915985 -0.356097 -20.579672 -0.956234 0.084002 -0.280286 -198.259811\n-0.214477 0.395655 0.893004 421.319153 0.195623 0.913156 -0.357600 -20.407379 -0.956939 0.097995 -0.273250 -198.555588\n-0.211005 0.395721 0.893802 421.742462 0.194152 0.913138 -0.358447 -20.595100 -0.958009 0.097899 -0.269506 -198.875900\n-0.209550 0.395700 0.894153 422.405579 0.188834 0.913618 -0.360060 -20.922117 -0.959391 0.093396 -0.266171 -199.210510\n-0.193440 0.402753 0.894634 422.287170 0.194527 0.909499 -0.367384 -20.730839 -0.961634 0.102963 -0.254280 -199.774857\n-0.172369 0.410472 0.895434 422.401123 0.203480 0.904271 -0.375353 -20.608629 -0.963787 0.117503 -0.239391 -200.408615\n-0.166918 0.408254 0.897478 422.814697 0.200853 0.905241 -0.374429 -20.885580 -0.965296 0.117762 -0.233100 -200.823822\n-0.164566 0.408482 0.897809 423.645050 0.202603 0.904809 -0.374530 -21.249777 -0.965334 0.120264 -0.231660 -201.100052\n-0.159871 0.409421 0.898229 424.137726 0.202365 0.904201 -0.376125 -21.447575 -0.966173 0.121639 -0.227408 -201.375275\n-0.153326 0.409509 0.899330 424.526031 0.202909 0.903745 -0.376925 -21.590387 -0.967119 0.124690 -0.221661 -201.686813\n-0.134911 0.412206 0.901047 423.663361 0.198784 0.902132 -0.382939 -21.142475 -0.970713 0.127451 -0.203647 -201.873444\n-0.130665 0.410617 0.902397 424.388824 0.195635 0.902983 -0.382556 -21.530018 -0.971933 0.126554 -0.198320 -202.132874\n-0.115022 0.413849 0.903050 424.864319 0.195851 0.900687 -0.387821 -21.614782 -0.973865 0.132255 -0.184652 -202.648712\n-0.094775 0.419456 0.902815 425.035797 0.199649 0.896480 -0.395555 -21.479214 -0.975273 0.142757 -0.168708 -203.201614\n-0.097245 0.418400 0.903042 426.344330 0.202665 0.896656 -0.393617 -22.146709 -0.974408 0.144738 -0.171990 -203.327026\n-0.099547 0.417342 0.903281 427.147217 0.200136 0.897636 -0.392678 -22.562483 -0.974698 0.141689 -0.172882 -203.372574\n-0.099873 0.417115 0.903350 428.678802 0.200663 0.897678 -0.392311 -23.268154 -0.974556 0.142088 -0.173354 -203.603165\n-0.097744 0.417410 0.903446 429.696350 0.199836 0.897536 -0.393059 -23.765844 -0.974942 0.142122 -0.171142 -203.828064\n-0.087145 0.421308 0.902721 430.103821 0.199737 0.895157 -0.398496 -23.802874 -0.975967 0.145580 -0.162159 -204.193176\n-0.086466 0.420799 0.903024 430.867432 0.201005 0.895146 -0.397882 -24.195961 -0.975767 0.147109 -0.161983 -204.284653\n-0.089725 0.417747 0.904122 431.930450 0.197469 0.897224 -0.394963 -24.791512 -0.976194 0.143098 -0.162996 -204.324051\n-0.094163 0.418481 0.903331 432.737610 0.199174 0.896937 -0.394757 -25.156118 -0.975430 0.142749 -0.167809 -204.313095\n-0.098314 0.414203 0.904859 433.492706 0.199927 0.898947 -0.389774 -25.618448 -0.974866 0.142586 -0.171190 -204.335205\n-0.103884 0.413192 0.904699 434.515594 0.197484 0.900079 -0.388405 -26.121267 -0.974786 0.138315 -0.175102 -204.391724\n-0.103824 0.413499 0.904566 434.959106 0.196352 0.900103 -0.388922 -26.351862 -0.975021 0.137234 -0.174644 -204.550934\n-0.096077 0.415326 0.904585 435.361206 0.198619 0.898512 -0.391442 -26.526344 -0.975356 0.142059 -0.168818 -204.933319\n-0.091825 0.415363 0.905009 435.764130 0.199055 0.898162 -0.392024 -26.727631 -0.975677 0.144149 -0.165154 -205.157181\n-0.085738 0.415066 0.905742 435.262604 0.198254 0.898017 -0.392760 -26.503891 -0.976394 0.145893 -0.159283 -205.228073\n-0.074190 0.421112 0.903969 435.292358 0.204566 0.893617 -0.399501 -26.375481 -0.976037 0.155282 -0.152443 -205.500168\n-0.071668 0.419103 0.905106 434.782532 0.198192 0.895328 -0.398882 -26.282799 -0.977540 0.150798 -0.147229 -205.540588\n-0.054648 0.422919 0.904518 434.092377 0.202591 0.891727 -0.404698 -25.838240 -0.977737 0.161131 -0.134411 -205.846512\n-0.041905 0.424614 0.904404 433.392609 0.206191 0.889389 -0.408011 -25.419224 -0.977614 0.169382 -0.124822 -206.105362\n-0.030178 0.426534 0.903968 432.738953 0.208984 0.887084 -0.411591 -25.008196 -0.977453 0.176494 -0.115909 -206.354095\n-0.023322 0.426047 0.904401 432.856903 0.208579 0.886812 -0.412382 -25.040821 -0.977727 0.179021 -0.109546 -206.648926\n-0.022359 0.426158 0.904372 432.811432 0.207309 0.886913 -0.412806 -25.104401 -0.978020 0.178254 -0.108177 -206.550171\n-0.018613 0.425963 0.904549 433.291901 0.208952 0.886386 -0.413110 -25.454046 -0.977749 0.181318 -0.105504 -206.631042\n-0.027494 0.421297 0.906506 433.778442 0.211731 0.888727 -0.406613 -26.011053 -0.976941 0.180756 -0.113636 -206.401886\n-0.022987 0.420522 0.906991 434.222565 0.212158 0.888619 -0.406627 -26.274334 -0.976965 0.183078 -0.109644 -206.502747\n-0.020933 0.416072 0.909091 434.038513 0.205776 0.891615 -0.403336 -26.426203 -0.978375 0.178626 -0.104282 -206.553665\n-0.019873 0.416742 0.908808 434.649811 0.203803 0.891590 -0.404390 -26.694809 -0.978810 0.177181 -0.102652 -206.619141\n-0.012787 0.416291 0.909142 434.265686 0.198742 0.892134 -0.405708 -26.542753 -0.979968 0.175497 -0.094143 -206.724152\n-0.014848 0.416618 0.908961 434.211853 0.198018 0.892281 -0.405738 -26.534874 -0.980086 0.173967 -0.095746 -206.552444\n-0.010978 0.417481 0.908620 433.981628 0.193780 0.892336 -0.407658 -26.424465 -0.980983 0.171597 -0.090696 -206.533264\n-0.008807 0.419067 0.907913 433.809235 0.189129 0.892258 -0.410007 -26.363920 -0.981913 0.168102 -0.087116 -206.458267\n-0.007871 0.421036 0.907010 433.847351 0.183185 0.892295 -0.412616 -26.375000 -0.983047 0.162903 -0.084151 -206.255630\n-0.010677 0.421372 0.906825 433.748260 0.180747 0.892752 -0.412705 -26.357809 -0.983472 0.159499 -0.085693 -206.071854\n-0.011522 0.422136 0.906459 433.636566 0.177567 0.892975 -0.413600 -26.327648 -0.984041 0.156192 -0.085246 -205.919785\n-0.017669 0.423453 0.905746 433.720306 0.176899 0.892920 -0.414006 -26.403934 -0.984070 0.152910 -0.090685 -205.618271\n-0.021726 0.423039 0.905851 433.699982 0.177547 0.893295 -0.412917 -26.421171 -0.983872 0.151860 -0.094517 -205.382156\n-0.022332 0.422661 0.906012 433.615417 0.175652 0.893800 -0.412635 -26.401268 -0.984199 0.149928 -0.094202 -205.247879\n-0.024071 0.422923 0.905846 433.520447 0.174292 0.894007 -0.412764 -26.368279 -0.984400 0.147946 -0.095232 -205.096954\n-0.024857 0.422881 0.905844 433.419464 0.171530 0.894490 -0.412873 -26.340889 -0.984865 0.145117 -0.094771 -204.953049\n-0.026921 0.423900 0.905309 433.352966 0.168931 0.894541 -0.413835 -26.328949 -0.985260 0.141793 -0.095692 -204.780670\n-0.030057 0.424515 0.904922 433.283722 0.167274 0.894700 -0.414164 -26.313410 -0.985452 0.138921 -0.097902 -204.602631\n-0.032667 0.425453 0.904391 433.217987 0.166131 0.894597 -0.414845 -26.300013 -0.985562 0.136696 -0.099904 -204.435242\n-0.033134 0.426055 0.904091 433.145569 0.166561 0.894291 -0.415332 -26.301455 -0.985474 0.136825 -0.100596 -204.300751\n-0.033244 0.427581 0.903365 433.088989 0.165844 0.893694 -0.416901 -26.286964 -0.985592 0.135958 -0.100622 -204.193298\n-0.033542 0.427680 0.903308 433.098633 0.163858 0.893940 -0.417160 -26.296059 -0.985913 0.134022 -0.100063 -204.079239\n-0.035146 0.425190 0.904421 433.085724 0.162950 0.895308 -0.414574 -26.286871 -0.986008 0.132805 -0.100752 -203.961700\n-0.040443 0.426078 0.903782 433.133636 0.161857 0.895369 -0.414869 -26.314846 -0.985985 0.129505 -0.105175 -203.772949\n-0.044573 0.432901 0.900339 433.221985 0.158048 0.892940 -0.421519 -26.376303 -0.986425 0.123508 -0.108220 -203.613220\n-0.044616 0.436824 0.898440 433.177704 0.155930 0.891358 -0.425637 -26.392437 -0.986760 0.121104 -0.107882 -203.559860\n-0.046153 0.433934 0.899762 433.149872 0.155982 0.892804 -0.422577 -26.396437 -0.986681 0.120843 -0.108892 -203.460342\n-0.051165 0.430109 0.901326 433.287201 0.156869 0.894766 -0.418074 -26.470394 -0.986293 0.120000 -0.113251 -203.274048\n-0.052898 0.428298 0.902088 433.260498 0.156999 0.895685 -0.416052 -26.462194 -0.986181 0.119618 -0.114622 -203.181900\n-0.051338 0.429792 0.901468 433.260010 0.156377 0.894983 -0.417794 -26.435326 -0.986362 0.119520 -0.113156 -203.150543\n-0.050722 0.430860 0.900992 433.323273 0.155848 0.894516 -0.418989 -26.463804 -0.986478 0.119165 -0.112521 -203.092514\n-0.049170 0.430431 0.901283 433.299713 0.153508 0.894910 -0.419012 -26.437401 -0.986923 0.117751 -0.110077 -203.079575\n-0.049209 0.429607 0.901674 433.272583 0.150417 0.895660 -0.418532 -26.420517 -0.987397 0.115032 -0.108695 -203.028412\n-0.046199 0.430854 0.901238 433.292480 0.147271 0.895280 -0.420457 -26.387312 -0.988017 0.113302 -0.104814 -203.044540\n-0.043317 0.433359 0.900180 433.295593 0.144479 0.894271 -0.423562 -26.369549 -0.988559 0.111710 -0.101349 -203.041000\n-0.042478 0.435295 0.899285 433.314331 0.144211 0.893343 -0.425607 -26.359951 -0.988635 0.111607 -0.100722 -202.978561\n-0.041542 0.436923 0.898539 433.335754 0.143209 0.892633 -0.427431 -26.336769 -0.988820 0.110922 -0.099653 -202.927307\n-0.039611 0.438307 0.897952 433.381683 0.142475 0.891952 -0.429094 -26.348024 -0.989006 0.110939 -0.097778 -202.889420\n-0.037250 0.440556 0.896952 433.387970 0.140977 0.890915 -0.431736 -26.334404 -0.989312 0.110367 -0.095295 -202.862427\n-0.034550 0.441793 0.896451 433.401123 0.139476 0.890340 -0.433406 -26.346998 -0.989623 0.110059 -0.092381 -202.837326\n-0.033390 0.441210 0.896782 433.419189 0.136985 0.890835 -0.433184 -26.359951 -0.990010 0.108381 -0.090184 -202.797028\n-0.029236 0.442258 0.896411 433.428619 0.133736 0.890463 -0.434961 -26.351606 -0.990586 0.107166 -0.085179 -202.813660\n-0.027368 0.440849 0.897164 433.497589 0.130452 0.891400 -0.434037 -26.396553 -0.991077 0.105158 -0.081906 -202.768814\n-0.027197 0.437322 0.898894 433.639252 0.127463 0.893403 -0.430794 -26.483583 -0.991470 0.102859 -0.080040 -202.721222\n-0.026919 0.436246 0.899425 433.763184 0.121864 0.894472 -0.430197 -26.508787 -0.992182 0.098027 -0.077241 -202.703323\n-0.033458 0.437201 0.898741 434.494965 0.116212 0.894847 -0.430981 -26.880163 -0.992661 0.090025 -0.080748 -202.482040\n-0.034852 0.438336 0.898135 434.581909 0.111998 0.894733 -0.432330 -26.980120 -0.993097 0.085522 -0.080276 -202.391083\n-0.037641 0.438086 0.898144 435.239014 0.108260 0.895279 -0.432152 -27.367279 -0.993410 0.080967 -0.081127 -202.299286\n-0.048102 0.436759 0.898292 436.426788 0.107599 0.896365 -0.430060 -28.041471 -0.993030 0.075969 -0.090112 -201.908707\n-0.049257 0.434739 0.899208 437.229065 0.101171 0.897842 -0.428537 -28.485962 -0.993649 0.069866 -0.088208 -201.842682\n-0.051197 0.432669 0.900098 438.080566 0.093223 0.899415 -0.427039 -28.993114 -0.994328 0.062047 -0.086382 -201.827957\n-0.040570 0.436146 0.898961 438.612549 0.087084 0.897821 -0.431663 -29.159296 -0.995375 0.060772 -0.074406 -202.157043\n-0.031598 0.436965 0.898923 439.250366 0.081867 0.897482 -0.433387 -29.458918 -0.996142 0.059898 -0.064132 -202.368408\n-0.031129 0.433076 0.900820 440.230591 0.076651 0.899637 -0.429859 -30.057674 -0.996572 0.055668 -0.061201 -202.289520\n-0.031708 0.431572 0.901521 441.360596 0.076612 0.900370 -0.428327 -30.680910 -0.996557 0.055486 -0.061612 -202.181488\n-0.035641 0.429949 0.902150 442.574127 0.072856 0.901439 -0.426732 -31.364136 -0.996705 0.050518 -0.063453 -201.992722\n-0.041165 0.427910 0.902883 443.759338 0.071090 0.902613 -0.424541 -32.018887 -0.996620 0.046710 -0.067576 -201.719574\n-0.043008 0.428282 0.902621 445.346985 0.070560 0.902503 -0.424864 -32.742641 -0.996580 0.045416 -0.069035 -201.594193\n-0.038187 0.428381 0.902791 446.509064 0.067566 0.902489 -0.425380 -33.303402 -0.996984 0.044754 -0.063407 -201.694931\n-0.039205 0.428682 0.902605 447.696899 0.064555 0.902499 -0.425828 -33.893887 -0.997144 0.041573 -0.063056 -201.551422\n-0.038522 0.429607 0.902194 449.494751 0.063599 0.902088 -0.426841 -34.721737 -0.997232 0.040935 -0.062073 -201.606964\n-0.037885 0.428119 0.902928 450.885681 0.060055 0.902919 -0.425594 -35.420815 -0.997476 0.038102 -0.059918 -201.489792\n-0.040047 0.426214 0.903736 453.204254 0.061281 0.903806 -0.423531 -36.522408 -0.997317 0.038421 -0.062314 -201.514847\n-0.037495 0.426283 0.903813 455.336151 0.059811 0.903784 -0.423788 -37.469536 -0.997505 0.038168 -0.059384 -201.701462\n-0.037424 0.425418 0.904223 457.452637 0.056058 0.904326 -0.423146 -38.473980 -0.997726 0.034853 -0.057691 -201.775772\n-0.046770 0.427327 0.902887 459.749207 0.057347 0.903534 -0.424663 -39.565815 -0.997258 0.031916 -0.066764 -201.507263\n-0.054486 0.427957 0.902155 461.197174 0.058379 0.903317 -0.424982 -40.297573 -0.996806 0.029511 -0.074202 -201.160233\n-0.061413 0.426293 0.902498 462.671814 0.059643 0.904156 -0.423017 -41.049213 -0.996329 0.027849 -0.080953 -200.851440\n-0.068290 0.424435 0.902880 463.968201 0.061342 0.905066 -0.420823 -41.720131 -0.995778 0.026646 -0.087843 -200.514664\n-0.076497 0.424116 0.902371 465.371857 0.064180 0.905241 -0.420024 -42.425842 -0.995002 0.025784 -0.096468 -200.176285\n-0.072564 0.424269 0.902624 465.706268 0.062530 0.905165 -0.420436 -42.564228 -0.995402 0.025932 -0.092212 -200.295258\n-0.065080 0.425861 0.902445 466.134674 0.062288 0.904332 -0.422260 -42.680843 -0.995934 0.028731 -0.085380 -200.542099\n-0.057503 0.426390 0.902710 466.403198 0.061041 0.904016 -0.423119 -42.816917 -0.996477 0.030772 -0.078011 -200.788147\n-0.054742 0.426618 0.902774 467.119110 0.060958 0.903871 -0.423441 -43.153065 -0.996638 0.031851 -0.075485 -200.822113\n-0.054613 0.426298 0.902933 467.830200 0.060294 0.904040 -0.423174 -43.449818 -0.996686 0.031330 -0.075075 -200.766068\n-0.051848 0.427036 0.902747 468.226135 0.054205 0.903834 -0.424437 -43.614277 -0.997183 0.026927 -0.070010 -200.913330\n-0.055895 0.425785 0.903096 468.982178 0.048082 0.904609 -0.423522 -44.000694 -0.997278 0.019750 -0.071036 -200.795532\n-0.050994 0.428135 0.902275 469.399811 0.042557 0.903561 -0.426340 -44.166878 -0.997792 0.016658 -0.064296 -200.999939\n-0.051487 0.429366 0.901662 470.344299 0.039373 0.903030 -0.427770 -44.643509 -0.997897 0.013476 -0.063400 -200.926910\n-0.054323 0.428847 0.901742 471.441406 0.041032 0.903272 -0.427103 -45.223198 -0.997680 0.013799 -0.066665 -200.764221\n-0.050399 0.428182 0.902286 472.085754 0.041188 0.903556 -0.426485 -45.516552 -0.997880 0.015669 -0.063174 -200.821823\n-0.046291 0.427531 0.902815 472.756317 0.040645 0.903841 -0.425933 -45.823536 -0.998101 0.016978 -0.059217 -200.922577\n-0.042639 0.428680 0.902450 473.453644 0.039499 0.903288 -0.427212 -46.086269 -0.998309 0.017430 -0.055447 -200.993927\n-0.035166 0.432445 0.900974 474.090546 0.034664 0.901517 -0.431353 -46.308632 -0.998780 0.016062 -0.046693 -201.289627\n" }, { "path": "src/vloam_main/rviz/vloam.rviz", "content": "Panels:\n - Class: rviz/Displays\n Help Height: 138\n Name: Displays\n Property Tree Widget:\n Expanded:\n - /PointCloud21/Autocompute Value Bounds1\n - /Path1\n - /Path2\n - /PointCloud22\n - /PointCloud22/Autocompute Value Bounds1\n - /Path3\n - /Image1\n - /Image2\n - /Image3\n Splitter Ratio: 0.32778701186180115\n Tree Height: 1989\n - Class: rviz/Selection\n Name: Selection\n - Class: rviz/Tool Properties\n Expanded:\n - /2D Pose Estimate1\n - /2D Nav Goal1\n - /Publish Point1\n Name: Tool Properties\n Splitter Ratio: 0.5886790156364441\n - Class: rviz/Views\n Expanded:\n - /Current View1\n Name: Views\n Splitter Ratio: 0.5\n - Class: rviz/Time\n Experimental: false\n Name: Time\n SyncMode: 0\n SyncSource: PointCloud2\nPreferences:\n PromptSaveOnExit: true\nToolbars:\n toolButtonStyle: 2\nVisualization Manager:\n Class: \"\"\n Displays:\n - Alpha: 0.5\n Cell Size: 1\n Class: rviz/Grid\n Color: 160; 160; 164\n Enabled: true\n Line Style:\n Line Width: 0.029999999329447746\n Value: Lines\n Name: Grid\n Normal Cell Count: 0\n Offset:\n X: 0\n Y: 0\n Z: 0\n Plane: XY\n Plane Cell Count: 10\n Reference Frame: \n Value: true\n - Class: rviz/TF\n Enabled: true\n Frame Timeout: 15\n Frames:\n All Enabled: true\n aft_mapped:\n Value: true\n base:\n Value: true\n base_link:\n Value: true\n cam0:\n Value: true\n camera_color_left:\n Value: true\n camera_color_right:\n Value: true\n camera_gray_left:\n Value: true\n camera_gray_right:\n Value: true\n imu:\n Value: true\n imu_link:\n Value: true\n map:\n Value: true\n velo:\n Value: true\n velo_link:\n Value: true\n velo_origin:\n Value: true\n world:\n Value: true\n Marker Alpha: 1\n Marker Scale: 1\n Name: TF\n Show Arrows: true\n Show Axes: true\n Show Names: true\n Tree:\n map:\n aft_mapped:\n {}\n base:\n imu:\n cam0:\n {}\n velo:\n {}\n velo_origin:\n {}\n Update Interval: 0\n Value: true\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 5\n Min Value: -5\n Value: false\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud2\n Color: 255; 255; 255\n Color Transformer: AxisColor\n Decay Time: 0\n Enabled: false\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Min Color: 0; 0; 0\n Name: PointCloud2\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 3\n Size (m): 0.10000000149011612\n Style: Flat Squares\n Topic: /point_cloud_follow_VO\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: false\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 255; 0; 0\n Enabled: true\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Billboards\n Line Width: 0.4000000059604645\n Name: Path\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Queue Size: 10\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /visual_odom_path\n Unreliable: false\n Value: true\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 255; 245; 0\n Enabled: true\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Billboards\n Line Width: 0.4000000059604645\n Name: Path\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Queue Size: 10\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /laser_odom_path\n Unreliable: false\n Value: true\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 4\n Min Value: -4\n Value: false\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud2\n Color: 255; 255; 255\n Color Transformer: AxisColor\n Decay Time: 0\n Enabled: true\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Min Color: 0; 0; 0\n Name: PointCloud2\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 1\n Size (m): 0.10000000149011612\n Style: Flat Squares\n Topic: /laser_cloud_map\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: true\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 0; 255; 0\n Enabled: true\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Billboards\n Line Width: 0.4000000059604645\n Name: Path\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Queue Size: 10\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /aft_mapped_path\n Unreliable: false\n Value: true\n - Class: rviz/Image\n Enabled: true\n Image Topic: /visual_odometry/depth_visualization\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: Image\n Normalize Range: true\n Queue Size: 2\n Transport Hint: compressed\n Unreliable: false\n Value: true\n - Class: rviz/Image\n Enabled: false\n Image Topic: /visual_odometry/matches_visualization\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: Image\n Normalize Range: true\n Queue Size: 2\n Transport Hint: compressed\n Unreliable: false\n Value: false\n - Class: rviz/Image\n Enabled: true\n Image Topic: /visual_odometry/optical_flow_visualization\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: Image\n Normalize Range: true\n Queue Size: 2\n Transport Hint: compressed\n Unreliable: false\n Value: true\n Enabled: true\n Global Options:\n Background Color: 48; 48; 48\n Default Light: true\n Fixed Frame: map\n Frame Rate: 30\n Name: root\n Tools:\n - Class: rviz/Interact\n Hide Inactive Objects: true\n - Class: rviz/MoveCamera\n - Class: rviz/Select\n - Class: rviz/FocusCamera\n - Class: rviz/Measure\n - Class: rviz/SetInitialPose\n Theta std deviation: 0.2617993950843811\n Topic: /initialpose\n X std deviation: 0.5\n Y std deviation: 0.5\n - Class: rviz/SetGoal\n Topic: /move_base_simple/goal\n - Class: rviz/PublishPoint\n Single click: true\n Topic: /clicked_point\n Value: true\n Views:\n Current:\n Class: rviz/Orbit\n Distance: 674.3966674804688\n Enable Stereo Rendering:\n Stereo Eye Separation: 0.05999999865889549\n Stereo Focal Distance: 1\n Swap Stereo Eyes: false\n Value: false\n Field of View: 0.7853981852531433\n Focal Point:\n X: -92.00663757324219\n Y: -417.1578369140625\n Z: -11.893792152404785\n Focal Shape Fixed Size: true\n Focal Shape Size: 0.05000000074505806\n Invert Z Axis: false\n Name: Current View\n Near Clip Distance: 0.009999999776482582\n Pitch: 1.5697963237762451\n Target Frame: \n Yaw: 2.796668529510498\n Saved: ~\nWindow Geometry:\n Displays:\n collapsed: false\n Height: 3319\n Hide Left Dock: false\n Hide Right Dock: true\n Image:\n collapsed: false\n QMainWindow State: 000000ff00000000fd000000040000000000000508000008bdfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000006e000008bd0000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000002ee000002e8fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002e80000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000b06000002dbfc0100000003fb0000000a0049006d006100670065010000000000000b060000009100fffffffb0000000a0049006d006100670065000000000000000b060000009100fffffffb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000b0600000392fc0100000002fc0000000000000b060000009100fffffffa000000000200000003fb0000000a0049006d0061006700650100000000ffffffff0000002600fffffffb0000000a0049006d0061006700650000000000ffffffff0000000000000000fb0000000800540069006d0065000000080f000002340000006000fffffffb0000000800540069006d0065010000000000000450000000000000000000000b06000005d600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000\n Selection:\n collapsed: false\n Time:\n collapsed: false\n Tool Properties:\n collapsed: false\n Views:\n collapsed: true\n Width: 2822\n X: 210\n Y: 54\n" }, { "path": "src/vloam_main/rviz/vloam_combined.rviz", "content": "Panels:\n - Class: rviz/Displays\n Help Height: 138\n Name: Displays\n Property Tree Widget:\n Expanded:\n - /Global Options1\n - /Status1\n - /Sensor input1\n - /VO1\n - /LO1\n - /Mapping1\n Splitter Ratio: 0.5\n Tree Height: 433\n - Class: rviz/Selection\n Name: Selection\n - Class: rviz/Tool Properties\n Expanded:\n - /2D Pose Estimate1\n - /2D Nav Goal1\n - /Publish Point1\n Name: Tool Properties\n Splitter Ratio: 0.5886790156364441\n - Class: rviz/Views\n Expanded:\n - /Current View1\n Name: Views\n Splitter Ratio: 0.5\n - Class: rviz/Time\n Experimental: false\n Name: Time\n SyncMode: 0\n SyncSource: Registered Pointcloud\nPreferences:\n PromptSaveOnExit: true\nToolbars:\n toolButtonStyle: 2\nVisualization Manager:\n Class: \"\"\n Displays:\n - Alpha: 0.5\n Cell Size: 1\n Class: rviz/Grid\n Color: 160; 160; 164\n Enabled: true\n Line Style:\n Line Width: 0.029999999329447746\n Value: Lines\n Name: Grid\n Normal Cell Count: 0\n Offset:\n X: 0\n Y: 0\n Z: 0\n Plane: XY\n Plane Cell Count: 10\n Reference Frame: \n Value: true\n - Class: rviz/TF\n Enabled: true\n Frame Timeout: 15\n Frames:\n All Enabled: true\n aft_mapped:\n Value: true\n base:\n Value: true\n base_link:\n Value: true\n cam0:\n Value: true\n camera_color_left:\n Value: true\n camera_color_right:\n Value: true\n camera_gray_left:\n Value: true\n camera_gray_right:\n Value: true\n imu:\n Value: true\n imu_link:\n Value: true\n map:\n Value: true\n velo:\n Value: true\n velo_link:\n Value: true\n velo_origin:\n Value: true\n world:\n Value: true\n Marker Alpha: 1\n Marker Scale: 1\n Name: TF\n Show Arrows: true\n Show Axes: true\n Show Names: true\n Tree:\n map:\n aft_mapped:\n {}\n base:\n imu:\n cam0:\n {}\n velo:\n {}\n velo_origin:\n {}\n Update Interval: 0\n Value: true\n - Class: rviz/Group\n Displays:\n - Class: rviz/Image\n Enabled: true\n Image Topic: /kitti/camera_gray_left/image_raw\n Max Value: 1\n Median window: 5\n Min Value: 0\n Name: Kitti Image Input\n Normalize Range: true\n Queue Size: 2\n Transport Hint: raw\n Unreliable: false\n Value: true\n Enabled: true\n Name: Sensor input\n - Class: rviz/Group\n Displays:\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 5\n Min Value: -5\n Value: false\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud2\n Color: 255; 255; 255\n Color Transformer: AxisColor\n Decay Time: 0\n Enabled: false\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Max Intensity: 4096\n Min Color: 0; 0; 0\n Min Intensity: 0\n Name: Map from VO\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 3\n Size (m): 0.10000000149011612\n Style: Flat Squares\n Topic: /point_cloud_follow_VO\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: false\n - Angle Tolerance: 0.10000000149011612\n Class: rviz/Odometry\n Covariance:\n Orientation:\n Alpha: 0.5\n Color: 255; 255; 127\n Color Style: Unique\n Frame: Local\n Offset: 1\n Scale: 1\n Value: true\n Position:\n Alpha: 0.30000001192092896\n Color: 204; 51; 204\n Scale: 1\n Value: true\n Value: true\n Enabled: false\n Keep: 100\n Name: VO to lidar center\n Position Tolerance: 0.10000000149011612\n Shape:\n Alpha: 1\n Axes Length: 1\n Axes Radius: 0.10000000149011612\n Color: 52; 101; 164\n Head Length: 0.30000001192092896\n Head Radius: 0.10000000149011612\n Shaft Length: 1\n Shaft Radius: 0.05000000074505806\n Value: Axes\n Topic: /visual_odom_to_init\n Unreliable: false\n Value: false\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 25; 255; 0\n Enabled: true\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Billboards\n Line Width: 0.10000000149011612\n Name: VO Trajectory\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /visual_odom_path\n Unreliable: false\n Value: true\n Enabled: true\n Name: VO\n - Class: rviz/Group\n Displays:\n - Angle Tolerance: 0.10000000149011612\n Class: rviz/Odometry\n Covariance:\n Orientation:\n Alpha: 0.5\n Color: 255; 255; 127\n Color Style: Unique\n Frame: Local\n Offset: 1\n Scale: 1\n Value: true\n Position:\n Alpha: 0.30000001192092896\n Color: 204; 51; 204\n Scale: 1\n Value: true\n Value: true\n Enabled: false\n Keep: 100\n Name: LO to lidar center\n Position Tolerance: 0.10000000149011612\n Shape:\n Alpha: 1\n Axes Length: 1\n Axes Radius: 0.10000000149011612\n Color: 255; 25; 0\n Head Length: 0.30000001192092896\n Head Radius: 0.10000000149011612\n Shaft Length: 1\n Shaft Radius: 0.05000000074505806\n Value: Axes\n Topic: /laser_odom_to_init\n Unreliable: false\n Value: false\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 32; 74; 135\n Enabled: true\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Billboards\n Line Width: 0.10000000149011612\n Name: \"LO Trajectory \"\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /laser_odom_path\n Unreliable: false\n Value: true\n Enabled: true\n Name: LO\n - Class: rviz/Group\n Displays:\n - Alpha: 1\n Autocompute Intensity Bounds: true\n Autocompute Value Bounds:\n Max Value: 10\n Min Value: -10\n Value: true\n Axis: Z\n Channel Name: intensity\n Class: rviz/PointCloud2\n Color: 255; 255; 255\n Color Transformer: Intensity\n Decay Time: 0\n Enabled: true\n Invert Rainbow: false\n Max Color: 255; 255; 255\n Max Intensity: 50.12456130981445\n Min Color: 0; 0; 0\n Min Intensity: -0.023822348564863205\n Name: Registered Pointcloud\n Position Transformer: XYZ\n Queue Size: 10\n Selectable: true\n Size (Pixels): 3\n Size (m): 0.10000000149011612\n Style: Flat Squares\n Topic: /velodyne_cloud_registered\n Unreliable: false\n Use Fixed Frame: true\n Use rainbow: true\n Value: true\n - Alpha: 1\n Buffer Length: 1\n Class: rviz/Path\n Color: 239; 41; 41\n Enabled: true\n Head Diameter: 0.30000001192092896\n Head Length: 0.20000000298023224\n Length: 0.30000001192092896\n Line Style: Billboards\n Line Width: 0.10000000149011612\n Name: Aft Map Trajectory\n Offset:\n X: 0\n Y: 0\n Z: 0\n Pose Color: 255; 85; 255\n Pose Style: None\n Radius: 0.029999999329447746\n Shaft Diameter: 0.10000000149011612\n Shaft Length: 0.10000000149011612\n Topic: /aft_mapped_path\n Unreliable: false\n Value: true\n - Angle Tolerance: 0.10000000149011612\n Class: rviz/Odometry\n Covariance:\n Orientation:\n Alpha: 0.5\n Color: 255; 255; 127\n Color Style: Unique\n Frame: Local\n Offset: 1\n Scale: 1\n Value: true\n Position:\n Alpha: 0.30000001192092896\n Color: 204; 51; 204\n Scale: 1\n Value: true\n Value: true\n Enabled: false\n Keep: 100\n Name: Aft Map Odom to lidar center\n Position Tolerance: 0.10000000149011612\n Shape:\n Alpha: 1\n Axes Length: 1\n Axes Radius: 0.10000000149011612\n Color: 255; 25; 0\n Head Length: 0.30000001192092896\n Head Radius: 0.10000000149011612\n Shaft Length: 1\n Shaft Radius: 0.05000000074505806\n Value: Axes\n Topic: /aft_mapped_to_init\n Unreliable: false\n Value: false\n Enabled: true\n Name: Mapping\n Enabled: true\n Global Options:\n Background Color: 48; 48; 48\n Default Light: true\n Fixed Frame: map\n Frame Rate: 30\n Name: root\n Tools:\n - Class: rviz/Interact\n Hide Inactive Objects: true\n - Class: rviz/MoveCamera\n - Class: rviz/Select\n - Class: rviz/FocusCamera\n - Class: rviz/Measure\n - Class: rviz/SetInitialPose\n Theta std deviation: 0.2617993950843811\n Topic: /initialpose\n X std deviation: 0.5\n Y std deviation: 0.5\n - Class: rviz/SetGoal\n Topic: /move_base_simple/goal\n - Class: rviz/PublishPoint\n Single click: true\n Topic: /clicked_point\n Value: true\n Views:\n Current:\n Class: rviz/Orbit\n Distance: 124.70527648925781\n Enable Stereo Rendering:\n Stereo Eye Separation: 0.05999999865889549\n Stereo Focal Distance: 1\n Swap Stereo Eyes: false\n Value: false\n Field of View: 0.7853981852531433\n Focal Point:\n X: 8.445307731628418\n Y: 1.7248053550720215\n Z: -1.981568694114685\n Focal Shape Fixed Size: true\n Focal Shape Size: 0.05000000074505806\n Invert Z Axis: false\n Name: Current View\n Near Clip Distance: 0.009999999776482582\n Pitch: 0.11039930582046509\n Target Frame: \n Yaw: 0.22412317991256714\n Saved: ~\nWindow Geometry:\n Displays:\n collapsed: false\n Height: 1977\n Hide Left Dock: false\n Hide Right Dock: true\n Kitti Image Input:\n collapsed: false\n QMainWindow State: 000000ff00000000fd00000004000000000000036500000646fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006e0000046b0000018200fffffffb0000000a0049006d00610067006501000004e5000001cf0000002600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000002ee000002e8fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002e80000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000b64000000cbfc0100000002fb0000000800540069006d0065010000000000000b640000057100fffffffb0000000800540069006d00650100000000000004500000000000000000000007f30000064600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000\n Selection:\n collapsed: false\n Time:\n collapsed: false\n Tool Properties:\n collapsed: false\n Views:\n collapsed: true\n Width: 2916\n X: 3096\n Y: 54\n" }, { "path": "src/vloam_main/scripts/plotTrajectory.py", "content": "import numpy as np\nimport matplotlib.pyplot as plt\n\nlo = np.loadtxt('../results/seq1/lo.txt')\nvo = np.loadtxt('../results/seq1/vo.txt')\nmo = np.loadtxt('../results/seq1/mo.txt')\n\nfig = plt.figure(1)\nax = fig.add_subplot(111, projection='3d')\nax.scatter(lo[:,0], lo[:,1], lo[:,2], c='r')\nax.scatter(vo[:,0], vo[:,1], vo[:,2], c='g')\nax.scatter(mo[:,0], mo[:,1], mo[:,2], c='b')\nplt.show()\n" }, { "path": "src/vloam_main/src/vloam_main_node.cpp", "content": "#include \n#include \n// #include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\n#include \n#include \n#include \n#include \n\n// parameters from launch file\ndouble rosbag_rate;\nbool visualize_depth, publish_point_cloud, detach_VO_LO, save_traj;\nint start_frame, end_frame; // inclusive interval\n\ntypedef message_filters::sync_policies::ApproximateTime\n MySyncPolicy;\nstd::shared_ptr> sub_image00_ptr;\nstd::shared_ptr> sub_camera00_ptr;\nstd::shared_ptr> sub_velodyne_ptr;\n\ntypedef actionlib::SimpleActionServer Server;\nvloam_main::vloam_mainFeedback feedback;\nvloam_main::vloam_mainResult result;\n\nint count, i, j; // TODO: check if count will overflow\n\nstd::shared_ptr VO;\ncv_bridge::CvImagePtr cv_ptr;\nstd::shared_ptr LOAM;\npcl::PointCloud point_cloud_pcl;\nstd::shared_ptr vloam_tf;\n\nstd::string seq, cmd;\nstd::ostringstream ss;\nint sys_ret;\n\n// output for evaluation\nstd::string VOFileName;\nstd::string LOFileName;\nstd::string MOFileName;\nFILE* LOFilePtr = NULL;\nFILE* VOFilePtr = NULL;\nFILE* MOFilePtr = NULL;\n\nvoid init(const vloam_main::vloam_mainGoalConstPtr& goal)\n{\n // section 1, initialize file pointers\n if (save_traj)\n {\n ss.clear();\n ss.str(\"\");\n ss << std::setw(4) << std::setfill('0') << goal->seq;\n seq = std::string(ss.str());\n\n VOFileName = ros::package::getPath(\"vloam_main\") + \"/results/\" + goal->date + +\"_drive_\" + seq;\n LOFileName = ros::package::getPath(\"vloam_main\") + \"/results/\" + goal->date + +\"_drive_\" + seq;\n MOFileName = ros::package::getPath(\"vloam_main\") + \"/results/\" + goal->date + +\"_drive_\" + seq;\n\n cmd = \"mkdir -p \" + VOFileName;\n sys_ret = system(cmd.c_str());\n cmd = \"mkdir -p \" + LOFileName;\n sys_ret = system(cmd.c_str());\n cmd = \"mkdir -p \" + MOFileName;\n sys_ret = system(cmd.c_str());\n\n VOFileName += \"/VO\" + std::to_string(detach_VO_LO) + \".txt\";\n LOFileName += \"/LO\" + std::to_string(detach_VO_LO) + \".txt\";\n MOFileName += \"/MO\" + std::to_string(detach_VO_LO) + \".txt\";\n\n VOFilePtr = fopen(VOFileName.c_str(), \"w\");\n LOFilePtr = fopen(LOFileName.c_str(), \"w\");\n MOFilePtr = fopen(MOFileName.c_str(), \"w\");\n\n if (VOFilePtr == NULL or LOFilePtr == NULL or MOFilePtr == NULL)\n {\n ROS_INFO(\"FilePtr == NULL\");\n ROS_BREAK();\n }\n ROS_INFO(\"\\n LO file path; %s\\n\", LOFileName.c_str());\n }\n\n // section 2, prepare for a new set of estimation\n count = 0;\n\n vloam_tf = std::make_shared();\n VO = std::make_shared();\n LOAM = std::make_shared();\n\n vloam_tf->init();\n VO->init(vloam_tf);\n LOAM->init(vloam_tf);\n}\n\nvoid callback(const sensor_msgs::Image::ConstPtr& image_msg, const sensor_msgs::CameraInfo::ConstPtr& camera_info_msg,\n const sensor_msgs::PointCloud2::ConstPtr& point_cloud_msg)\n{\n static tf2_ros::StaticTransformBroadcaster static_broadcaster;\n static tf2_ros::TransformBroadcaster dynamic_broadcaster;\n\n i = count % 2;\n\n VO->reset();\n LOAM->reset();\n\n // Section 1: Process Image // takes ~34ms\n cv_ptr = cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::MONO8);\n VO->processImage(cv_ptr->image);\n\n // Section 2: Process Static Transformations and Camera Intrinsics\n if (count == 0)\n {\n vloam_tf->processStaticTransform();\n VO->setUpPointCloud(camera_info_msg);\n }\n\n // Section 3: Process Point Cloud // takes ~2.6ms\n pcl::fromROSMsg(*point_cloud_msg, point_cloud_pcl); // optimization can be applied if pcl library is not necessarily\n // ROS_INFO(\"point cloud width=%d, height=%d\", point_cloud_pcl.width, point_cloud_pcl.height); // typical output\n // \"point cloud width=122270, height=1053676\" // TODO: check why height is so large\n VO->processPointCloud(point_cloud_msg, point_cloud_pcl, visualize_depth, publish_point_cloud);\n\n // Section 4: Solve and Publish VO\n if (count > 0)\n {\n VO->solveNlsAll(); // result is cam0_curr_T_cam0_last, f2f odometry\n // VO->solveNls2dOnly();\n // VO->solveRANSAC();\n }\n vloam_tf->VO2VeloAndBase(VO->cam0_curr_T_cam0_last); // transform f2f VO to world VO\n vloam_tf->dynamic_broadcaster.sendTransform(vloam_tf->world_stamped_VOtf_base); // publish for visualization // can\n // be commented out\n VO->publish(); // publish nav_msgs::odometry\n\n // Section 5: Solve and Publish LO MO\n LOAM->scanRegistrationIO(point_cloud_pcl);\n LOAM->laserOdometryIO();\n LOAM->laserMappingIO();\n\n // Section 6, save odoms\n if (save_traj and count >= start_frame and count <= end_frame)\n {\n vloam_tf->VO2Cam0StartFrame(VOFilePtr, count - start_frame);\n vloam_tf->LO2Cam0StartFrame(LOFilePtr, count - start_frame);\n vloam_tf->MO2Cam0StartFrame(MOFilePtr, count - start_frame);\n }\n\n ++count;\n ROS_INFO(\"total count = %d\", count);\n}\n\nvoid execute(const vloam_main::vloam_mainGoalConstPtr& goal, Server* as)\n{\n result.loading_finished = false;\n\n // section1, generate new message filter and tf_buffer\n message_filters::Synchronizer sync(MySyncPolicy(20), *sub_image00_ptr, *sub_camera00_ptr,\n *sub_velodyne_ptr);\n sync.setInterMessageLowerBound(ros::Duration(0.09));\n sync.registerCallback(boost::bind(&callback, _1, _2, _3));\n\n // section 2, initialize all pointers\n init(goal);\n\n // section 3, play the bag\n start_frame = goal->start_frame;\n end_frame = goal->end_frame;\n\n ss.clear();\n ss.str(\"\");\n ss << std::setw(4) << std::setfill('0') << goal->seq;\n seq = std::string(ss.str());\n cmd = \"rosbag play \" + ros::package::getPath(\"vloam_main\") + \"/bags/kitti_\" + goal->date + \"_drive_\" + seq +\n \"_synced.bag -d 2 -r \" + std::to_string(rosbag_rate);\n // TODO: add one more entry of goal for different dataset type:\n // In kitti2bag, kitti_types = [\"raw_synced\", \"odom_color\", \"odom_gray\"]\n // https://github.com/tomas789/kitti2bag/blob/bf0d46c49a77f5d5500621934ccd617d18cf776b/kitti2bag/kitti2bag.py#L264\n ROS_INFO(\"The command is %s\", cmd.c_str());\n sys_ret = system(cmd.c_str());\n\n // section 4, save files\n if (save_traj)\n {\n fclose(LOFilePtr);\n fclose(VOFilePtr);\n fclose(MOFilePtr);\n ROS_INFO(\"\\n LO, VO, MO saved\\n\\n\");\n }\n\n result.loading_finished = true;\n as->setSucceeded(result);\n}\n\nint main(int argc, char** argv)\n{\n ros::init(argc, argv, \"vloam_main_node\");\n\n ros::NodeHandle nh_private = ros::NodeHandle(\"~\");\n nh_private.getParam(\"rosbag_rate\", rosbag_rate);\n nh_private.getParam(\"visualize_depth\", visualize_depth);\n nh_private.getParam(\"publish_point_cloud\", publish_point_cloud);\n nh_private.getParam(\"save_traj\", save_traj);\n if (!ros::param::get(\"detach_VO_LO\", detach_VO_LO))\n ROS_BREAK();\n\n ros::NodeHandle nh;\n\n sub_image00_ptr =\n std::make_shared>(nh, \"/kitti/camera_gray_left/image_raw\", 10);\n sub_camera00_ptr = std::make_shared>(\n nh, \"/kitti/camera_gray_left/camera_info\", 10);\n sub_velodyne_ptr = std::make_shared>(\n nh, \"/kitti/velo/pointcloud\", 10); // TODO: change the buffer size to potentially reduce the data lost\n\n Server server(nh, \"load_small_dataset_action_server\", boost::bind(&execute, _1, &server), false);\n server.start();\n\n ros::Rate loop_rate(100);\n while (ros::ok())\n {\n ros::spinOnce();\n loop_rate.sleep();\n }\n\n return 0;\n}" }, { "path": "src/vloam_tf/CMakeLists.txt", "content": "cmake_minimum_required(VERSION 3.0.2)\nproject(vloam_tf)\n\n## Compile as C++11, supported in ROS Kinetic and newer\n# add_compile_options(-std=c++11)\n\n## Find catkin macros and libraries\n## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)\n## is used, also find other catkin packages\nfind_package(catkin REQUIRED COMPONENTS\n geometry_msgs\n roscpp\n tf2\n tf2_eigen\n tf2_geometry_msgs\n tf2_ros\n)\n\n## System dependencies are found with CMake's conventions\n# find_package(Boost REQUIRED COMPONENTS system)\n\n\n## Uncomment this if the package has a setup.py. This macro ensures\n## modules and global scripts declared therein get installed\n## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html\n# catkin_python_setup()\n\n################################################\n## Declare ROS messages, services and actions ##\n################################################\n\n## To declare and build messages, services or actions from within this\n## package, follow these steps:\n## * Let MSG_DEP_SET be the set of packages whose message types you use in\n## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).\n## * In the file package.xml:\n## * add a build_depend tag for \"message_generation\"\n## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET\n## * If MSG_DEP_SET isn't empty the following dependency has been pulled in\n## but can be declared for certainty nonetheless:\n## * add a exec_depend tag for \"message_runtime\"\n## * In this file (CMakeLists.txt):\n## * add \"message_generation\" and every package in MSG_DEP_SET to\n## find_package(catkin REQUIRED COMPONENTS ...)\n## * add \"message_runtime\" and every package in MSG_DEP_SET to\n## catkin_package(CATKIN_DEPENDS ...)\n## * uncomment the add_*_files sections below as needed\n## and list every .msg/.srv/.action file to be processed\n## * uncomment the generate_messages entry below\n## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)\n\n## Generate messages in the 'msg' folder\n# add_message_files(\n# FILES\n# Message1.msg\n# Message2.msg\n# )\n\n## Generate services in the 'srv' folder\n# add_service_files(\n# FILES\n# Service1.srv\n# Service2.srv\n# )\n\n## Generate actions in the 'action' folder\n# add_action_files(\n# FILES\n# Action1.action\n# Action2.action\n# )\n\n## Generate added messages and services with any dependencies listed here\n# generate_messages(\n# DEPENDENCIES\n# geometry_msgs# tf2_geometry_msgs\n# )\n\n################################################\n## Declare ROS dynamic reconfigure parameters ##\n################################################\n\n## To declare and build dynamic reconfigure parameters within this\n## package, follow these steps:\n## * In the file package.xml:\n## * add a build_depend and a exec_depend tag for \"dynamic_reconfigure\"\n## * In this file (CMakeLists.txt):\n## * add \"dynamic_reconfigure\" to\n## find_package(catkin REQUIRED COMPONENTS ...)\n## * uncomment the \"generate_dynamic_reconfigure_options\" section below\n## and list every .cfg file to be processed\n\n## Generate dynamic reconfigure parameters in the 'cfg' folder\n# generate_dynamic_reconfigure_options(\n# cfg/DynReconf1.cfg\n# cfg/DynReconf2.cfg\n# )\n\n###################################\n## catkin specific configuration ##\n###################################\n## The catkin_package macro generates cmake config files for your package\n## Declare things to be passed to dependent projects\n## INCLUDE_DIRS: uncomment this if your package contains header files\n## LIBRARIES: libraries you create in this project that dependent projects also need\n## CATKIN_DEPENDS: catkin_packages dependent projects also need\n## DEPENDS: system dependencies of this project that dependent projects also need\ncatkin_package(\n INCLUDE_DIRS include\n# LIBRARIES vloam_tf\n# CATKIN_DEPENDS geometry_msgs roscpp tf2 tf2_eigen tf2_geometry_msgs tf2_ros\n# DEPENDS system_lib\n)\n\n###########\n## Build ##\n###########\n\n## Specify additional locations of header files\n## Your package locations should be listed before other locations\ninclude_directories(\n include\n ${catkin_INCLUDE_DIRS}\n)\n\n## Declare a C++ library\nadd_library(vloam_tf\n src/vloam_tf.cpp\n)\ntarget_link_libraries(vloam_tf ${catkin_LIBRARIES})\nset_target_properties(vloam_tf PROPERTIES PUBLIC_HEADER include/${PROJECT_NAME}/vloam_tf.h)\n\n## Add cmake target dependencies of the library\n## as an example, code may need to be generated before libraries\n## either from message generation or dynamic reconfigure\n# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})\n\n## Declare a C++ executable\n## With catkin_make all packages are built within a single CMake context\n## The recommended prefix ensures that target names across packages don't collide\n# add_executable(${PROJECT_NAME}_node src/vloam_tf_node.cpp)\n\n## Rename C++ executable without prefix\n## The above recommended prefix causes long target names, the following renames the\n## target back to the shorter version for ease of user use\n## e.g. \"rosrun someones_pkg node\" instead of \"rosrun someones_pkg someones_pkg_node\"\n# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX \"\")\n\n## Add cmake target dependencies of the executable\n## same as for the library above\n# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})\n\n## Specify libraries to link a library or executable target against\n# target_link_libraries(${PROJECT_NAME}_node\n# ${catkin_LIBRARIES}\n# )\n\n#############\n## Install ##\n#############\n\n# all install targets should use catkin DESTINATION variables\n# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html\n\n## Mark executable scripts (Python etc.) for installation\n## in contrast to setup.py, you can choose the destination\n# catkin_install_python(PROGRAMS\n# scripts/my_python_script\n# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}\n# )\n\n## Mark executables for installation\n## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html\n# install(TARGETS ${PROJECT_NAME}_node\n# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}\n# )\n\n## Mark libraries for installation\n## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html\n# install(TARGETS ${PROJECT_NAME}\n# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}\n# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}\n# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}\n# )\n\n## Mark cpp header files for installation\ninstall(\n DIRECTORY include/${PROJECT_NAME}/\n DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}\n FILES_MATCHING \n PATTERN \"*.hpp\"\n PATTERN \"*.h\"\n# PATTERN \".svn\" EXCLUDE\n)\n\n## Mark other files for installation (e.g. launch and bag files, etc.)\n# install(FILES\n# # myfile1\n# # myfile2\n# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}\n# )\n\n#############\n## Testing ##\n#############\n\n## Add gtest based cpp test target and link libraries\n# catkin_add_gtest(${PROJECT_NAME}-test test/test_vloam_tf.cpp)\n# if(TARGET ${PROJECT_NAME}-test)\n# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})\n# endif()\n\n## Add folders to be run by python nosetests\n# catkin_add_nosetests(test)\n" }, { "path": "src/vloam_tf/include/vloam_tf/vloam_tf.h", "content": "#pragma once\n\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n// #include \n\nnamespace vloam\n{\nclass VloamTF\n{\npublic:\n void init();\n void processStaticTransform();\n void VO2VeloAndBase(const tf2::Transform &cam0_curr_T_cam0_last);\n\n void VO2Cam0StartFrame(FILE *VOFilePtr, const int count);\n void LO2Cam0StartFrame(FILE *LOFilePtr, const int count);\n void MO2Cam0StartFrame(FILE *MOFilePtr, const int count);\n\n tf2_ros::StaticTransformBroadcaster static_broadcaster;\n tf2_ros::TransformBroadcaster dynamic_broadcaster;\n\n std::shared_ptr tf_buffer_ptr;\n std::shared_ptr tf_listener;\n\n // static transform\n geometry_msgs::TransformStamped imu_stamped_tf_velo, map_stamped_tf_velo_origin, imu_stamped_tf_cam0,\n base_stamped_tf_imu;\n tf2::Transform imu_T_velo, imu_T_cam0, base_T_imu, base_T_cam0, velo_T_cam0;\n Eigen::Isometry3f imu_eigen_T_velo, imu_eigen_T_cam0;\n\n // dynamic transform for VO\n geometry_msgs::TransformStamped world_stamped_VOtf_base;\n tf2::Transform world_VOT_base_last, base_last_VOT_base_curr, cam0_curr_VOT_cam0_last, velo_last_VOT_velo_curr;\n tf2::Transform cam0_init_VOT_cam0_last, cam0_init_VOT_cam0_start, cam0_start_VOT_cam0_last;\n Eigen::Isometry3f cam0_start_eigen_VOT_cam0_last;\n\n // dynamic transform for LO\n tf2::Transform world_LOT_base_last, cam0_init_LOT_cam0_last, cam0_init_LOT_cam0_start, cam0_start_LOT_cam0_last;\n Eigen::Isometry3f cam0_start_eigen_LOT_cam0_last;\n tf2::Transform base_prev_LOT_base_curr, cam0_curr_LOT_cam0_prev;\n\n // dynamic transform for MO\n tf2::Transform world_MOT_base_last, cam0_init_MOT_cam0_last, cam0_init_MOT_cam0_start, cam0_start_MOT_cam0_last;\n Eigen::Isometry3f cam0_start_eigen_MOT_cam0_last;\n};\n} // namespace vloam" }, { "path": "src/vloam_tf/package.xml", "content": "\n\n vloam_tf\n 0.0.0\n The vloam_tf package\n\n \n \n \n yukun\n\n\n \n \n \n TODO\n\n\n \n \n \n \n\n\n \n \n \n \n\n\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n catkin\n geometry_msgs\n roscpp\n tf2\n tf2_eigen\n tf2_geometry_msgs\n tf2_ros\n geometry_msgs\n roscpp\n tf2\n tf2_eigen\n tf2_geometry_msgs\n tf2_ros\n geometry_msgs\n roscpp\n tf2\n tf2_eigen\n tf2_geometry_msgs\n tf2_ros\n\n\n \n \n \n\n \n\n" }, { "path": "src/vloam_tf/src/vloam_tf.cpp", "content": "#include \n\nnamespace vloam\n{\nvoid VloamTF::init()\n{\n tf_buffer_ptr = std::make_shared();\n tf_listener = std::make_shared(*tf_buffer_ptr);\n\n world_VOT_base_last.setOrigin(tf2::Vector3(0.0, 0.0, 0.0));\n world_VOT_base_last.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0));\n world_stamped_VOtf_base.header.frame_id = \"map\";\n world_stamped_VOtf_base.child_frame_id = \"base\";\n world_stamped_VOtf_base.transform = tf2::toMsg(world_VOT_base_last);\n}\n\nvoid VloamTF::processStaticTransform()\n{\n // initial base to world is static as identity transform\n world_stamped_VOtf_base.header.stamp = ros::Time::now();\n dynamic_broadcaster.sendTransform(world_stamped_VOtf_base);\n\n imu_stamped_tf_velo = tf_buffer_ptr->lookupTransform(\"imu_link\", \"velo_link\", ros::Time(0), ros::Duration(3.0));\n // NOTE: lookupTransform() will actually block until the transform between the two turtles becomes available (this\n // will usually take a few milliseconds)\n tf2::fromMsg(imu_stamped_tf_velo.transform, imu_T_velo); // TODO: later use, if not, remove\n imu_eigen_T_velo = tf2::transformToEigen(imu_stamped_tf_velo).cast(); // for point cloud util internal use\n\n imu_stamped_tf_velo.header.frame_id = \"imu\";\n imu_stamped_tf_velo.child_frame_id = \"velo\";\n static_broadcaster.sendTransform(imu_stamped_tf_velo);\n\n map_stamped_tf_velo_origin =\n tf_buffer_ptr->lookupTransform(\"base_link\", \"velo_link\", ros::Time(0), ros::Duration(3.0));\n map_stamped_tf_velo_origin.header.frame_id = \"map\";\n map_stamped_tf_velo_origin.child_frame_id = \"velo_origin\";\n static_broadcaster.sendTransform(map_stamped_tf_velo_origin);\n\n imu_stamped_tf_cam0 =\n tf_buffer_ptr->lookupTransform(\"imu_link\", \"camera_gray_left\", ros::Time(0), ros::Duration(3.0));\n tf2::fromMsg(imu_stamped_tf_cam0.transform, imu_T_cam0); // TODO: later use, if not, remove\n imu_eigen_T_cam0 = tf2::transformToEigen(imu_stamped_tf_cam0).cast(); // for point cloud util internal use\n\n imu_stamped_tf_cam0.header.frame_id = \"imu\";\n imu_stamped_tf_cam0.child_frame_id = \"cam0\";\n static_broadcaster.sendTransform(imu_stamped_tf_cam0);\n\n base_stamped_tf_imu = tf_buffer_ptr->lookupTransform(\"base_link\", \"imu_link\", ros::Time(0), ros::Duration(3.0));\n tf2::fromMsg(base_stamped_tf_imu.transform, base_T_imu); // TODO: later use, if not, remove\n\n base_stamped_tf_imu.header.frame_id = \"base\";\n base_stamped_tf_imu.child_frame_id = \"imu\";\n static_broadcaster.sendTransform(base_stamped_tf_imu);\n\n base_T_cam0 = base_T_imu * imu_T_cam0;\n velo_T_cam0 = imu_T_velo.inverse() * imu_T_cam0;\n}\n\nvoid VloamTF::VO2VeloAndBase(const tf2::Transform &cam0_curr_VOT_cam0_last)\n{\n // get T_velo_last^velo_curr (from VO)\n velo_last_VOT_velo_curr =\n velo_T_cam0 * cam0_curr_VOT_cam0_last.inverse() * velo_T_cam0.inverse(); // odom for velodyne\n // get T_base_last^base_curr (from VO)\n base_last_VOT_base_curr = base_T_cam0 * cam0_curr_VOT_cam0_last.inverse() * base_T_cam0.inverse();\n\n // get T_world^curr = T_last^curr * T_world^last\n geometry_msgs::Transform temp = tf2::toMsg(base_last_VOT_base_curr); // TODO: check better solution\n if (!std::isnan(temp.translation.x) and !std::isnan(temp.translation.y) and !std::isnan(temp.translation.z) and\n !std::isnan(temp.rotation.x) and !std::isnan(temp.rotation.y) and !std::isnan(temp.rotation.z) and\n !std::isnan(temp.rotation.w)) // avoid nan at the first couple steps\n world_VOT_base_last *= base_last_VOT_base_curr; // after update, last becomes the curr\n world_stamped_VOtf_base.header.stamp = ros::Time::now();\n world_stamped_VOtf_base.transform = tf2::toMsg(world_VOT_base_last);\n}\n\nvoid VloamTF::VO2Cam0StartFrame(FILE *VOFilePtr, const int count)\n{\n if (count < 0)\n return;\n\n cam0_init_VOT_cam0_last = base_T_cam0.inverse() * world_VOT_base_last * base_T_cam0;\n\n if (count == 0)\n cam0_init_VOT_cam0_start = cam0_init_VOT_cam0_last;\n\n cam0_start_VOT_cam0_last = cam0_init_VOT_cam0_start.inverse() * cam0_init_VOT_cam0_last;\n\n cam0_start_eigen_VOT_cam0_last = tf2::transformToEigen(tf2::toMsg(cam0_start_VOT_cam0_last)).cast();\n\n if (VOFilePtr != NULL)\n {\n fprintf(VOFilePtr, \"%f %f %f %f %f %f %f %f %f %f %f %f\\n\", cam0_start_eigen_VOT_cam0_last(0, 0),\n cam0_start_eigen_VOT_cam0_last(0, 1), cam0_start_eigen_VOT_cam0_last(0, 2),\n cam0_start_eigen_VOT_cam0_last(0, 3), cam0_start_eigen_VOT_cam0_last(1, 0),\n cam0_start_eigen_VOT_cam0_last(1, 1), cam0_start_eigen_VOT_cam0_last(1, 2),\n cam0_start_eigen_VOT_cam0_last(1, 3), cam0_start_eigen_VOT_cam0_last(2, 0),\n cam0_start_eigen_VOT_cam0_last(2, 1), cam0_start_eigen_VOT_cam0_last(2, 2),\n cam0_start_eigen_VOT_cam0_last(2, 3));\n }\n}\n\nvoid VloamTF::LO2Cam0StartFrame(FILE *LOFilePtr, const int count)\n{\n if (count < 0)\n return;\n\n cam0_init_LOT_cam0_last = base_T_cam0.inverse() * world_LOT_base_last * base_T_cam0;\n\n if (count == 0)\n cam0_init_LOT_cam0_start = cam0_init_LOT_cam0_last;\n\n cam0_start_LOT_cam0_last = cam0_init_LOT_cam0_start.inverse() * cam0_init_LOT_cam0_last;\n\n cam0_start_eigen_LOT_cam0_last = tf2::transformToEigen(tf2::toMsg(cam0_start_LOT_cam0_last)).cast();\n\n if (LOFilePtr != NULL)\n {\n fprintf(LOFilePtr, \"%f %f %f %f %f %f %f %f %f %f %f %f\\n\", cam0_start_eigen_LOT_cam0_last(0, 0),\n cam0_start_eigen_LOT_cam0_last(0, 1), cam0_start_eigen_LOT_cam0_last(0, 2),\n cam0_start_eigen_LOT_cam0_last(0, 3), cam0_start_eigen_LOT_cam0_last(1, 0),\n cam0_start_eigen_LOT_cam0_last(1, 1), cam0_start_eigen_LOT_cam0_last(1, 2),\n cam0_start_eigen_LOT_cam0_last(1, 3), cam0_start_eigen_LOT_cam0_last(2, 0),\n cam0_start_eigen_LOT_cam0_last(2, 1), cam0_start_eigen_LOT_cam0_last(2, 2),\n cam0_start_eigen_LOT_cam0_last(2, 3));\n }\n}\n\nvoid VloamTF::MO2Cam0StartFrame(FILE *MOFilePtr, const int count)\n{\n if (count < 0)\n return;\n\n cam0_init_MOT_cam0_last = base_T_cam0.inverse() * world_MOT_base_last * base_T_cam0;\n\n if (count == 0)\n cam0_init_MOT_cam0_start = cam0_init_MOT_cam0_last;\n\n cam0_start_MOT_cam0_last = cam0_init_MOT_cam0_start.inverse() * cam0_init_MOT_cam0_last;\n\n cam0_start_eigen_MOT_cam0_last = tf2::transformToEigen(tf2::toMsg(cam0_start_MOT_cam0_last)).cast();\n\n if (MOFilePtr != NULL)\n {\n fprintf(MOFilePtr, \"%f %f %f %f %f %f %f %f %f %f %f %f\\n\", cam0_start_eigen_MOT_cam0_last(0, 0),\n cam0_start_eigen_MOT_cam0_last(0, 1), cam0_start_eigen_MOT_cam0_last(0, 2),\n cam0_start_eigen_MOT_cam0_last(0, 3), cam0_start_eigen_MOT_cam0_last(1, 0),\n cam0_start_eigen_MOT_cam0_last(1, 1), cam0_start_eigen_MOT_cam0_last(1, 2),\n cam0_start_eigen_MOT_cam0_last(1, 3), cam0_start_eigen_MOT_cam0_last(2, 0),\n cam0_start_eigen_MOT_cam0_last(2, 1), cam0_start_eigen_MOT_cam0_last(2, 2),\n cam0_start_eigen_MOT_cam0_last(2, 3));\n }\n}\n} // namespace vloam" } ]