├── lsd_slam_to_pcl_nodelet.xml
├── include
    └── lsd_slam_to_pcl
    │   ├── lsd_slam_to_pcl_nodelet.hpp
    │   ├── lsd_slam_to_pcl.hpp
    │   └── third_party
    │       └── sophus
    │           ├── sophus.hpp
    │           ├── rxso3.hpp
    │           ├── so3.hpp
    │           └── sim3.hpp
├── msg
    └── keyframeMsg.msg
├── cfg
    └── Params.cfg
├── src
    ├── lsd_slam_to_pcl_nodelet.cpp
    └── lsd_slam_to_pcl.cpp
├── README.md
├── package.xml
└── CMakeLists.txt


/lsd_slam_to_pcl_nodelet.xml:
--------------------------------------------------------------------------------
1 | <library path="lib/liblsd_slam_to_pcl_nodelet">
2 |   <class name="LSDSLAMToPCLNodelet" type="LSDSLAMToPCLNodelet" base_class_type="nodelet::Nodelet">
3 |     <description> 
4 |       A node to convert lsd_slam depth keyframes to pcl PointClouds
5 |     </description>
6 |   </class>
7 | </library>


--------------------------------------------------------------------------------
/include/lsd_slam_to_pcl/lsd_slam_to_pcl_nodelet.hpp:
--------------------------------------------------------------------------------
 1 | #ifndef LSD_SLAM_TO_PCL_NODELET_HPP
 2 | #define LSD_SLAM_TO_PCL_NODELET_HPP
 3 | 
 4 | #include <nodelet/nodelet.h>
 5 | #include <pluginlib/class_list_macros.h>
 6 | 
 7 | #include "lsd_slam_to_pcl/lsd_slam_to_pcl.hpp"
 8 | 
 9 | class LSDSLAMToPCLNodelet : public nodelet::Nodelet
10 | {
11 | public:
12 |     LSDSLAMToPCLNodelet();
13 |     ~LSDSLAMToPCLNodelet();
14 | 
15 |     virtual void onInit();
16 | 
17 | private:
18 |     boost::shared_ptr<LSDSLAMToPCL> controller_;
19 |     boost::shared_ptr<ros::NodeHandle> nh_ptr_;
20 | 
21 | };
22 | 
23 | #endif


--------------------------------------------------------------------------------
/msg/keyframeMsg.msg:
--------------------------------------------------------------------------------
 1 | int32 id
 2 | float64 time
 3 | bool isKeyframe
 4 | 
 5 | # camToWorld as serialization of sophus sim(3).
 6 | # may change with keyframeGraph-updates.
 7 | float32[7] camToWorld 
 8 | 
 9 | 
10 | # camera parameter (fx fy cx cy), width, height
11 | # will never change, but required for display.
12 | float32 fx
13 | float32 fy
14 | float32 cx
15 | float32 cy
16 | uint32 height
17 | uint32 width
18 | 
19 | 
20 | # data as InputPointDense (float idepth, float idepth_var, uchar color[4]), width x height
21 | # may be empty, in that case no associated pointcloud is ever shown.
22 | uint8[] pointcloud
23 | 


--------------------------------------------------------------------------------
/cfg/Params.cfg:
--------------------------------------------------------------------------------
 1 | #!/usr/bin/env python
 2 | PACKAGE = "lsd_slam_to_pcl"
 3 | 
 4 | from dynamic_reconfigure.parameter_generator_catkin import *
 5 | 
 6 | gen = ParameterGenerator()
 7 | 
 8 | gen.add("min_near_support", int_t, 0, "only plot points that have #min_near_support similar neighbours (higher values remove outliers)", 7, 0, 9)
 9 | gen.add("sparsify_factor", int_t, 0, "only plot one out of #sparsify_factor points, selected at random. Use this to significantly speed up rendering for large maps.", 1, 1, 100)
10 | gen.add("depth_var_thresh_abs", double_t, 0, "log10 of threshold on point's variance, in absolute scale.", -1, -10, 1)
11 | gen.add("depth_var_thresh_scaled", double_t, 0, "log10 of threshold on point's variance, in the respective keyframe's scale. ", -3, -10, 1)
12 | 
13 | exit(gen.generate(PACKAGE, "lsd_slam_to_pcl", "Params"))


--------------------------------------------------------------------------------
/src/lsd_slam_to_pcl_nodelet.cpp:
--------------------------------------------------------------------------------
 1 | #include "lsd_slam_to_pcl/lsd_slam_to_pcl_nodelet.hpp"
 2 | 
 3 | LSDSLAMToPCLNodelet::LSDSLAMToPCLNodelet() {}
 4 | LSDSLAMToPCLNodelet::~LSDSLAMToPCLNodelet() {}
 5 | 
 6 | void LSDSLAMToPCLNodelet::onInit()
 7 | {
 8 |     nh_ptr_ = boost::make_shared<ros::NodeHandle>(this->getPrivateNodeHandle());
 9 | 
10 |     std::string name = nh_ptr_->getUnresolvedNamespace();
11 |     int pos = name.find_last_of('/');
12 |     name = name.substr(pos + 1);
13 | 
14 |     NODELET_INFO_STREAM("Initialising nodelet... [" << name << "]");
15 |     controller_.reset(new LSDSLAMToPCL(*nh_ptr_, name));
16 | 
17 |     if (controller_->Init())
18 |     {
19 |         NODELET_INFO_STREAM("Nodelet initialised. [" << name << "]");
20 |     }
21 |     else
22 |     {
23 |         NODELET_ERROR_STREAM("Couldn't initialise nodelet! Please restart. [" << name << "]");
24 |     }
25 | }
26 | 
27 | PLUGINLIB_EXPORT_CLASS(LSDSLAMToPCLNodelet, nodelet::Nodelet);
28 | 


--------------------------------------------------------------------------------
/include/lsd_slam_to_pcl/lsd_slam_to_pcl.hpp:
--------------------------------------------------------------------------------
 1 | #ifndef LSD_SLAM_TO_PCL_HPP
 2 | #define LSD_SLAM_TO_PCL_HPP
 3 | 
 4 | #include <ros/ros.h>
 5 | #include <pcl_ros/point_cloud.h>
 6 | #include "third_party/sophus/sim3.hpp"
 7 | 
 8 | #include "lsd_slam_to_pcl/keyframeMsg.h"
 9 | 
10 | struct InputPointDense
11 | {
12 |     float idepth;
13 |     float idepth_var;
14 |     unsigned char color[4];
15 | };
16 | 
17 | class LSDSLAMToPCL
18 | {
19 | public:
20 |     LSDSLAMToPCL(ros::NodeHandle& nh, std::string& name);
21 |     ~LSDSLAMToPCL();
22 | 
23 |     bool Init();
24 | 
25 | private:
26 |     ros::NodeHandle nh_;
27 |     ros::Subscriber depth_subscriber_;
28 |     ros::Publisher cloud_publisher_;
29 |     ros::Publisher indices_publisher_;
30 | 
31 |     pcl::PointXYZRGB point_invalid_;
32 |     std::string node_name_;
33 |     int sparsify_factor_;
34 |     int min_near_support_;
35 |     float scaled_depth_var_thresh_;
36 |     float abs_depth_var_thresh_;
37 | 
38 |     void depthCB(const lsd_slam_to_pcl::keyframeMsgConstPtr msg);
39 | };
40 | 
41 | #endif


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # LSD-SLAM to PCL
 2 | 
 3 | This package is a nodelet that converts [LSD_SLAM](https://github.com/tum-vision/lsd_slam) keyframes into PCL pointclouds (`pcl::PointCloud<pcl::PointXYZRGB>`).
 4 | 
 5 | To build:
 6 | 
 7 | ```bash
 8 | cd ~/catkin_ws/src
 9 | git clone https://github.com/alexanderkoumis/lsd_slam_to_pcl
10 | cd ..
11 | catkin_make
12 | ```
13 | 
14 | Using it in a launch file:
15 | ```xml
16 | <launch>
17 | 
18 |     <!-- Nodelet manager -->
19 |     <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager" output="screen"/>
20 | 
21 |     <!-- LSD SLAM -> PCL pointcloud converstion -->
22 |     <node pkg="nodelet" type="nodelet" name="lsd_slam_to_pcl" args="load LSDSLAMToPCLNodelet nodelet_manager" output="screen">
23 |          <remap from="~input" to="/lsd_slam/keyframes"/>
24 |     </node>
25 | 
26 | </launch>
27 | ```
28 | 
29 | Topics `lsd_slam_to_pcl/output_points` and `lsd_slam_to_pcl/output_indices` can now be subscribed to as `pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr` and `pcl::PointIndicesConstPtr`, respectively.
30 | 
31 | 


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package>
 3 |   <name>lsd_slam_to_pcl</name>
 4 |   <version>0.0.0</version>
 5 |   <description>The lsd_slam_to_pcl package</description>
 6 | 
 7 |   <maintainer email="alexander@visidyn.com">Alexander Koumis</maintainer>
 8 |   <license>TODO</license>
 9 | 
10 |   <buildtool_depend>catkin</buildtool_depend>
11 | 
12 |   <build_depend>message_generation</build_depend>
13 |   <build_depend>message_runtime</build_depend>
14 |   <build_depend>pcl_ros</build_depend>
15 |   <build_depend>pluginlib</build_depend>
16 |   <build_depend>roscpp</build_depend>
17 |   <build_depend>std_msgs</build_depend>
18 |   <build_depend>nodelet</build_depend>
19 | 
20 |   <run_depend>message_generation</run_depend>
21 |   <run_depend>message_runtime</run_depend>
22 |   <run_depend>pcl_ros</run_depend>
23 |   <run_depend>pluginlib</run_depend>
24 |   <run_depend>roscpp</run_depend>
25 |   <run_depend>std_msgs</run_depend>
26 |   <run_depend>nodelet</run_depend>
27 | 
28 |   <export>
29 |     <nodelet plugin="${prefix}/lsd_slam_to_pcl_nodelet.xml"/>
30 |   </export>
31 | </package>


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | project(lsd_slam_to_pcl)
 3 | 
 4 | set(CMAKE_BUILD_TYPE Release)
 5 | set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -ggdb -gdwarf-3")
 6 | 
 7 | find_package(catkin REQUIRED COMPONENTS
 8 |   message_generation
 9 |   message_runtime
10 |   pcl_ros
11 |   pluginlib
12 |   roscpp
13 |   std_msgs
14 | )
15 | 
16 | add_message_files(
17 |   FILES
18 |   keyframeMsg.msg
19 | )
20 | 
21 | generate_messages(
22 |   DEPENDENCIES
23 |   std_msgs
24 |   sensor_msgs
25 | )
26 | 
27 | catkin_package(
28 |    INCLUDE_DIRS include
29 |    LIBRARIES lsd_slam_to_pcl_nodelet
30 |    CATKIN_DEPENDS message_generation message_runtime pcl_ros pluginlib roscpp std_msgs
31 | )
32 | 
33 | include_directories(
34 |   ${catkin_INCLUDE_DIRS}
35 |   include
36 | )
37 | 
38 | add_library(lsd_slam_to_pcl_nodelet
39 |   src/lsd_slam_to_pcl.cpp
40 |   src/lsd_slam_to_pcl_nodelet.cpp
41 | )
42 | 
43 | add_dependencies(lsd_slam_to_pcl_nodelet lsd_slam_to_pcl_gencpp)
44 | 
45 | target_link_libraries(lsd_slam_to_pcl_nodelet
46 |   ${catkin_LIBRARIES}
47 | )
48 | 
49 | install(TARGETS lsd_slam_to_pcl_nodelet
50 |   DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
51 | )
52 | 
53 | install(DIRECTORY plugins
54 |   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
55 | )
56 | 
57 | install(DIRECTORY launch
58 |   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
59 | )


--------------------------------------------------------------------------------
/include/lsd_slam_to_pcl/third_party/sophus/sophus.hpp:
--------------------------------------------------------------------------------
 1 | // This file is part of Sophus.
 2 | //
 3 | // Copyright 2013 Hauke Strasdat
 4 | //
 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy
 6 | // of this software and associated documentation files (the "Software"), to
 7 | // deal in the Software without restriction, including without limitation the
 8 | // rights  to use, copy, modify, merge, publish, distribute, sublicense, and/or
 9 | // sell copies of the Software, and to permit persons to whom the Software is
10 | // furnished to do so, subject to the following conditions:
11 | //
12 | // The above copyright notice and this permission notice shall be included in
13 | // all copies or substantial portions of the Software.
14 | //
15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
20 | // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
21 | // IN THE SOFTWARE.
22 | 
23 | #ifndef SOPHUS_HPP
24 | #define SOPHUS_HPP
25 | 
26 | #include <stdexcept>
27 | 
28 | #include <Eigen/Eigen>
29 | #include <Eigen/Geometry>
30 | 
31 | namespace Sophus {
32 | using namespace Eigen;
33 | 
34 | template<typename Scalar>
35 | struct SophusConstants {
36 |   EIGEN_ALWAYS_INLINE static
37 |   const Scalar epsilon() {
38 |     return static_cast<Scalar>(1e-10);
39 |   }
40 | 
41 |   EIGEN_ALWAYS_INLINE static
42 |   const Scalar pi() {
43 |     return static_cast<Scalar>(M_PI);
44 |   }
45 | };
46 | 
47 | template<>
48 | struct SophusConstants<float> {
49 |   EIGEN_ALWAYS_INLINE static
50 |   const float epsilon() {
51 |     return static_cast<float>(1e-5);
52 |   }
53 | 
54 |   EIGEN_ALWAYS_INLINE static
55 |   const float pi() {
56 |     return static_cast<float>(M_PI);
57 |   }
58 | };
59 | 
60 | class SophusException : public std::runtime_error {
61 | public:
62 |   SophusException (const std::string& str)
63 |     : runtime_error("Sophus exception: " + str) {
64 |   }
65 | };
66 | 
67 | }
68 | 
69 | #endif
70 | 


--------------------------------------------------------------------------------
/src/lsd_slam_to_pcl.cpp:
--------------------------------------------------------------------------------
  1 | #include "lsd_slam_to_pcl/lsd_slam_to_pcl.hpp"
  2 | 
  3 | #include <limits> // quiet_NaN
  4 | 
  5 | #include <pcl_conversions/pcl_conversions.h>
  6 | #include <pcl_msgs/PointIndices.h>
  7 | #include <pcl_ros/pcl_nodelet.h>
  8 | 
  9 | 
 10 | LSDSLAMToPCL::LSDSLAMToPCL(ros::NodeHandle& nh, std::string& name) :
 11 |     nh_(nh),
 12 |     node_name_(name),
 13 |     sparsify_factor_(0),
 14 |     min_near_support_(5),
 15 |     scaled_depth_var_thresh_(1),
 16 |     abs_depth_var_thresh_(1)
 17 | {}
 18 | 
 19 | LSDSLAMToPCL::~LSDSLAMToPCL() {}
 20 | 
 21 | bool LSDSLAMToPCL::Init()
 22 | {
 23 |     depth_subscriber_ = nh_.subscribe("input", 10, &LSDSLAMToPCL::depthCB, this);
 24 |     cloud_publisher_ = nh_.advertise<pcl::PointCloud<pcl::PointXYZRGB> >("output_points", 10);
 25 |     indices_publisher_ = nh_.advertise<pcl_ros::PCLNodelet::PointIndices>("output_indices", 10);
 26 | 
 27 |     point_invalid_.x = std::numeric_limits<float>::quiet_NaN();
 28 |     point_invalid_.y = std::numeric_limits<float>::quiet_NaN();
 29 |     point_invalid_.z = std::numeric_limits<float>::quiet_NaN();
 30 |     point_invalid_.data[3] = 1.0f;
 31 |     return true;
 32 | }
 33 | 
 34 | void LSDSLAMToPCL::depthCB(const lsd_slam_to_pcl::keyframeMsgConstPtr msg)
 35 | {
 36 |     if (msg->isKeyframe)
 37 |     {
 38 | 
 39 |         pcl::PointCloud<pcl::PointXYZRGB> cloud_pcl;
 40 |         pcl_ros::PCLNodelet::PointIndices indices_ros;
 41 | 
 42 |         const float fxi = 1 / msg->fx;
 43 |         const float fyi = 1 / msg->fy;
 44 |         const float cxi = -msg->cx / msg->fx;
 45 |         const float cyi = -msg->cy / msg->fy;
 46 | 
 47 |         const int width = msg->width;
 48 |         const int height = msg->height;
 49 | 
 50 |         int num_pts_total = 0;
 51 | 
 52 |         boost::shared_ptr<InputPointDense> input_points(new InputPointDense[width * height]);
 53 | 
 54 |         Sophus::Sim3f cam_to_world = Sophus::Sim3f();
 55 | 
 56 |         memcpy(cam_to_world.data(), msg->camToWorld.data(), 7 * sizeof(float));
 57 |         memcpy(input_points.get(), msg->pointcloud.data(), width * height * sizeof(InputPointDense));
 58 | 
 59 |         const float cam_to_world_scale = cam_to_world.scale();
 60 | 
 61 |         for (int y = 0; y < height; ++y)
 62 |         {
 63 |             for (int x = 0; x < width; ++x)
 64 |             {
 65 | 
 66 |                 // Catch boundary points
 67 |                 if (x == 0 || y == 0 || x == width - 1 || y == height - 1)
 68 |                 {
 69 |                     cloud_pcl.push_back(point_invalid_);
 70 |                     continue;
 71 |                 }
 72 | 
 73 |                 int point_indice = x + y * width;
 74 | 
 75 |                 const InputPointDense point_in_curr = input_points.get()[point_indice];
 76 | 
 77 |                 if (point_in_curr.idepth <= 0)
 78 |                 {
 79 |                     cloud_pcl.push_back(point_invalid_);
 80 |                     continue;
 81 |                 }
 82 | 
 83 |                 num_pts_total++;
 84 | 
 85 |                 if (sparsify_factor_ > 1 && rand() % sparsify_factor_ != 0)
 86 |                 {
 87 |                     cloud_pcl.push_back(point_invalid_);
 88 |                     continue;
 89 |                 }
 90 | 
 91 |                 const float depth = 1 / point_in_curr.idepth;
 92 |                 const float depth4 = depth * depth * depth * depth;
 93 | 
 94 |                 if (point_in_curr.idepth_var * depth4 > scaled_depth_var_thresh_)
 95 |                 {
 96 |                     cloud_pcl.push_back(point_invalid_);
 97 |                     continue;
 98 |                 }
 99 | 
100 |                 if (point_in_curr.idepth_var * depth4 * cam_to_world_scale * cam_to_world_scale > abs_depth_var_thresh_)
101 |                 {
102 |                     cloud_pcl.push_back(point_invalid_);
103 |                     continue;
104 |                 }
105 | 
106 |                 if (min_near_support_ > 1)
107 |                 {
108 |                     int near_support = 0;
109 | 
110 |                     for (int dx = -1; dx < 2; ++dx)
111 |                     {
112 |                         for (int dy = -1; dy < 2; ++dy)
113 |                         {
114 | 
115 |                             const InputPointDense point_in_near = input_points.get()[x + dx + (y + dy) * width];
116 | 
117 |                             if (point_in_near.idepth > 0)
118 |                             {
119 |                                 const float diff = point_in_near.idepth - point_in_curr.idepth;
120 |                                 if (diff * diff < 2 * point_in_curr.idepth_var) { near_support++; }
121 |                             }
122 |                         }
123 |                     }
124 | 
125 |                     if (near_support < min_near_support_)
126 |                     {
127 |                         cloud_pcl.push_back(point_invalid_);
128 |                         continue;
129 |                     }
130 |                 }
131 | 
132 |                 Eigen::Vector3f point_eigen = cam_to_world * (Eigen::Vector3f((x * fxi + cxi), (y * fyi + cyi), 1) * depth);
133 | 
134 |                 pcl::PointXYZRGB point_pcl;
135 |                 point_pcl.x = point_eigen[0];
136 |                 point_pcl.y = point_eigen[1];
137 |                 point_pcl.z = point_eigen[2];
138 |                 point_pcl.r = point_in_curr.color[2];
139 |                 point_pcl.g = point_in_curr.color[1];
140 |                 point_pcl.b = point_in_curr.color[0];
141 |                 cloud_pcl.push_back(point_pcl);
142 |                 indices_ros.indices.push_back(point_indice);
143 | 
144 |             }
145 |         }
146 | 
147 |         cloud_pcl.width = width;
148 |         cloud_pcl.height = height;
149 |         cloud_pcl.is_dense = false;
150 |         cloud_pcl.header.frame_id = "frame";
151 |         pcl_conversions::toPCL(ros::Time::now(), cloud_pcl.header.stamp);
152 |         indices_ros.header.stamp = ros::Time::now();
153 | 
154 |         cloud_publisher_.publish(boost::make_shared<const pcl::PointCloud<pcl::PointXYZRGB> >(cloud_pcl));
155 |         indices_publisher_.publish(boost::make_shared<const pcl_ros::PCLNodelet::PointIndices>(indices_ros));
156 | 
157 |         ROS_DEBUG_STREAM("Published " << num_pts_total << " points to pointcloud, dimensions [" << cloud_pcl.width << " " << cloud_pcl.height << "]");
158 | 
159 |     }
160 | 
161 |     else
162 |     {
163 |         ROS_DEBUG_STREAM("Error, must subscribe to keyframe");
164 |     }
165 | 
166 | }
167 | 


--------------------------------------------------------------------------------
/include/lsd_slam_to_pcl/third_party/sophus/rxso3.hpp:
--------------------------------------------------------------------------------
  1 | // This file is part of Sophus.
  2 | //
  3 | // Copyright 2012-2013 Hauke Strasdat
  4 | //
  5 | // Permission is hereby granted, free of charge, to any person obtaining a copy
  6 | // of this software and associated documentation files (the "Software"), to
  7 | // deal in the Software without restriction, including without limitation the
  8 | // rights  to use, copy, modify, merge, publish, distribute, sublicense, and/or
  9 | // sell copies of the Software, and to permit persons to whom the Software is
 10 | // furnished to do so, subject to the following conditions:
 11 | //
 12 | // The above copyright notice and this permission notice shall be included in
 13 | // all copies or substantial portions of the Software.
 14 | //
 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 20 | // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
 21 | // IN THE SOFTWARE.
 22 | 
 23 | #ifndef SOPHUS_RXSO3_HPP
 24 | #define RXSO3_HPP
 25 | 
 26 | #include "sophus.hpp"
 27 | #include "so3.hpp"
 28 | 
 29 | ////////////////////////////////////////////////////////////////////////////
 30 | // Forward Declarations / typedefs
 31 | ////////////////////////////////////////////////////////////////////////////
 32 | 
 33 | namespace Sophus {
 34 | template<typename _Scalar, int _Options=0> class RxSO3Group;
 35 | typedef RxSO3Group<double> ScSO3 EIGEN_DEPRECATED;
 36 | typedef RxSO3Group<double> RxSO3d; /**< double precision RxSO3 */
 37 | typedef RxSO3Group<float> RxSO3f;  /**< single precision RxSO3 */
 38 | }
 39 | 
 40 | ////////////////////////////////////////////////////////////////////////////
 41 | // Eigen Traits (For querying derived types in CRTP hierarchy)
 42 | ////////////////////////////////////////////////////////////////////////////
 43 | 
 44 | namespace Eigen {
 45 | namespace internal {
 46 | 
 47 | template<typename _Scalar, int _Options>
 48 | struct traits<Sophus::RxSO3Group<_Scalar,_Options> > {
 49 |   typedef _Scalar Scalar;
 50 |   typedef Quaternion<Scalar> QuaternionType;
 51 | };
 52 | 
 53 | template<typename _Scalar, int _Options>
 54 | struct traits<Map<Sophus::RxSO3Group<_Scalar>, _Options> >
 55 |     : traits<Sophus::RxSO3Group<_Scalar, _Options> > {
 56 |   typedef _Scalar Scalar;
 57 |   typedef Map<Quaternion<Scalar>,_Options> QuaternionType;
 58 | };
 59 | 
 60 | template<typename _Scalar, int _Options>
 61 | struct traits<Map<const Sophus::RxSO3Group<_Scalar>, _Options> >
 62 |     : traits<const Sophus::RxSO3Group<_Scalar, _Options> > {
 63 |   typedef _Scalar Scalar;
 64 |   typedef Map<const Quaternion<Scalar>,_Options> QuaternionType;
 65 | };
 66 | 
 67 | }
 68 | }
 69 | 
 70 | namespace Sophus {
 71 | using namespace Eigen;
 72 | 
 73 | class ScaleNotPositive : public SophusException {
 74 | public:
 75 |   ScaleNotPositive ()
 76 |     : SophusException("Scale factor is not positive") {
 77 |   }
 78 | };
 79 | 
 80 | /**
 81 |  * \brief RxSO3 base type - implements RxSO3 class but is storage agnostic
 82 |  *
 83 |  * This class implements the group \f$ R^{+} \times SO3 \f$ (RxSO3), the direct
 84 |  * product of the group of positive scalar matrices (=isomorph to the positive
 85 |  * real numbers) and the three-dimensional special orthogonal group SO3.
 86 |  * Geometrically, it is the group of rotation and scaling in three dimensions.
 87 |  * As a matrix groups, RxSO3 consists of matrices of the form \f$ s\cdot R \f$
 88 |  * where \f$ R \f$ is an orthognal matrix with \f$ det(R)=1 \f$ and \f$ s>0 \f$
 89 |  * be a positive real number.
 90 |  *
 91 |  * Internally, RxSO3 is represented by the group of non-zero quaternions. This
 92 |  * is a most compact representation since the degrees of freedom (DoF) of RxSO3
 93 |  * (=4) equals the number of internal parameters (=4).
 94 |  *
 95 |  * [add more detailed description/tutorial]
 96 |  */
 97 | template<typename Derived>
 98 | class RxSO3GroupBase {
 99 | public:
100 |   /** \brief scalar type, use with care since this might be a Map type  */
101 |   typedef typename internal::traits<Derived>::Scalar Scalar;
102 |   /** \brief quaternion reference type */
103 |   typedef typename internal::traits<Derived>::QuaternionType &
104 |   QuaternionReference;
105 |   /** \brief quaternion const reference type */
106 |   typedef const typename internal::traits<Derived>::QuaternionType &
107 |   ConstQuaternionReference;
108 | 
109 | 
110 |   /** \brief degree of freedom of group
111 |    *         (three for rotation and one for scaling) */
112 |   static const int DoF = 4;
113 |   /** \brief number of internal parameters used
114 |    *         (quaternion for rotation and scaling) */
115 |   static const int num_parameters = 4;
116 |   /** \brief group transformations are NxN matrices */
117 |   static const int N = 3;
118 |   /** \brief group transfomation type */
119 |   typedef Matrix<Scalar,N,N> Transformation;
120 |   /** \brief point type */
121 |   typedef Matrix<Scalar,3,1> Point;
122 |   /** \brief tangent vector type */
123 |   typedef Matrix<Scalar,DoF,1> Tangent;
124 |   /** \brief adjoint transformation type */
125 |   typedef Matrix<Scalar,DoF,DoF> Adjoint;
126 | 
127 | 
128 |   /**
129 |    * \brief Adjoint transformation
130 |    *
131 |    * This function return the adjoint transformation \f$ Ad \f$ of the
132 |    * group instance \f$ A \f$  such that for all \f$ x \f$
133 |    * it holds that \f$ \widehat{Ad_A\cdot x} = A\widehat{x}A^{-1} \f$
134 |    * with \f$\ \widehat{\cdot} \f$ being the hat()-operator.
135 |    *
136 |    * For RxSO3, it simply returns the rotation matrix corresponding to
137 |    * \f$ A \f$.
138 |    */
139 |   inline
140 |   const Adjoint Adj() const {
141 |     Adjoint res;
142 |     res.setIdentity();
143 |     res.template topLeftCorner<3,3>() = rotationMatrix();
144 |     return res;
145 |   }
146 | 
147 |   /**
148 |    * \returns copy of instance casted to NewScalarType
149 |    */
150 |   template<typename NewScalarType>
151 |   inline RxSO3Group<NewScalarType> cast() const {
152 |     return RxSO3Group<NewScalarType>(quaternion()
153 |                                      .template cast<NewScalarType>() );
154 |   }
155 | 
156 |   /**
157 |    * \returns pointer to internal data
158 |    *
159 |    * This provides direct read/write access to internal data. RxSO3 is
160 |    * represented by an Eigen::Quaternion (four parameters).
161 |    *
162 |    * Note: The first three Scalars represent the imaginary parts, while the
163 |    * forth Scalar represent the real part.
164 |    */
165 |   inline Scalar* data() {
166 |     return quaternion().coeffs().data();
167 |   }
168 | 
169 |   /**
170 |    * \returns const pointer to internal data
171 |    *
172 |    * Const version of data().
173 |    */
174 |   inline const Scalar* data() const {
175 |     return quaternion().coeffs().data();
176 |   }
177 | 
178 |   /**
179 |    * \brief In-place group multiplication
180 |    *
181 |    *  Same as operator*=() for RxSO3.
182 |    *
183 |    * \see operator*=()
184 |    */
185 |   inline
186 |   void fastMultiply(const RxSO3Group<Scalar>& other) {
187 |     quaternion() *= other.quaternion();
188 |   }
189 | 
190 | 
191 |   /**
192 |    * \returns group inverse of instance
193 |    */
194 |   inline
195 |   const RxSO3Group<Scalar> inverse() const {
196 |     if(quaternion().squaredNorm() <= static_cast<Scalar>(0)) {
197 |       throw ScaleNotPositive();
198 |     }
199 |     return RxSO3Group<Scalar>(quaternion().inverse());
200 |   }
201 | 
202 |   /**
203 |    * \brief Logarithmic map
204 |    *
205 |    * \returns tangent space representation (=rotation vector) of instance
206 |    *
207 |    * \see  log().
208 |    */
209 |   inline
210 |   const Tangent log() const {
211 |     return RxSO3Group<Scalar>::log(*this);
212 |   }
213 | 
214 |   /**
215 |    * \returns 3x3 matrix representation of instance
216 |    *
217 |    * For RxSO3, the matrix representation is a scaled orthogonal
218 |    * matrix \f$ sR \f$ with \f$ det(sR)=s^3 \f$, thus a scaled rotation
219 |    * matrix \f$ R \f$  with scale s.
220 |    */
221 |   inline
222 |   const Transformation matrix() const {
223 |     //ToDO: implement this directly!
224 |     Scalar scale = quaternion().norm();
225 |     Quaternion<Scalar> norm_quad = quaternion();
226 |     norm_quad.coeffs() /= scale;
227 |     return scale*norm_quad.toRotationMatrix();
228 |   }
229 | 
230 |   /**
231 |    * \brief Assignment operator
232 |    */
233 |   template<typename OtherDerived> inline
234 |   RxSO3GroupBase<Derived>& operator=
235 |   (const RxSO3GroupBase<OtherDerived> & other) {
236 |     quaternion() = other.quaternion();
237 |     return *this;
238 |   }
239 | 
240 |   /**
241 |    * \brief Group multiplication
242 |    * \see operator*=()
243 |    */
244 |   inline
245 |   const RxSO3Group<Scalar> operator*(const RxSO3Group<Scalar>& other) const {
246 |     RxSO3Group<Scalar> result(*this);
247 |     result *= other;
248 |     return result;
249 |   }
250 | 
251 |   /**
252 |    * \brief Group action on \f$ \mathbf{R}^3 \f$
253 |    *
254 |    * \param p point \f$p \in \mathbf{R}^3 \f$
255 |    * \returns point \f$p' \in \mathbf{R}^3 \f$,
256 |    *          rotated and scaled version of \f$p\f$
257 |    *
258 |    * This function rotates and scales a point \f$ p \f$ in  \f$ \mathbf{R}^3 \f$
259 |    * by the RxSO3 transformation \f$sR\f$ (=rotation matrix)
260 |    * : \f$ p' = sR\cdot p \f$.
261 |    */
262 |   inline
263 |   const Point operator*(const Point & p) const {
264 |     //ToDO: implement this directly!
265 |     Scalar scale = quaternion().norm();
266 |     Quaternion<Scalar> norm_quad = quaternion();
267 |     norm_quad.coeffs() /= scale;
268 |     return scale*norm_quad._transformVector(p);
269 |   }
270 | 
271 |   /**
272 |    * \brief In-place group multiplication
273 |    * \see operator*=()
274 |    */
275 |   inline
276 |   void operator*=(const RxSO3Group<Scalar>& other) {
277 |     quaternion() *= other.quaternion();
278 |   }
279 | 
280 |   /**
281 |    * \brief Mutator of quaternion
282 |    */
283 |   EIGEN_STRONG_INLINE
284 |   QuaternionReference quaternion() {
285 |     return static_cast<Derived*>(this)->quaternion();
286 |   }
287 | 
288 |   /**
289 |    * \brief Accessor of quaternion
290 |    */
291 |   EIGEN_STRONG_INLINE
292 |   ConstQuaternionReference quaternion() const {
293 |     return static_cast<const Derived*>(this)->quaternion();
294 |   }
295 | 
296 |   /**
297 |    * \returns rotation matrix
298 |    */
299 |   inline
300 |   Transformation rotationMatrix() const {
301 |     Scalar scale = quaternion().norm();
302 |     Quaternion<Scalar> norm_quad = quaternion();
303 |     norm_quad.coeffs() /= scale;
304 |     return norm_quad.toRotationMatrix();
305 |   }
306 | 
307 |   /**
308 |    * \returns scale
309 |    */
310 |   EIGEN_STRONG_INLINE
311 |   const Scalar scale() const {
312 |     return quaternion().norm();
313 |   }
314 | 
315 |   /**
316 |    * \brief Setter of quaternion using rotation matrix, leaves scale untouched
317 |    *
318 |    * \param R a 3x3 rotation matrix
319 |    * \pre       the 3x3 matrix should be orthogonal and have a determinant of 1
320 |    */
321 |   inline
322 |   void setRotationMatrix(const Transformation & R) {
323 |     Scalar saved_scale = scale();
324 |     quaternion() = R;
325 |     quaternion() *= saved_scale;
326 |   }
327 | 
328 |   /**
329 |    * \brief Scale setter
330 |    */
331 |   EIGEN_STRONG_INLINE
332 |   void setScale(const Scalar & scale) {
333 |     quaternion().normalize();
334 |     quaternion() *= scale;
335 |   }
336 | 
337 |   /**
338 |    * \brief Setter of quaternion using scaled rotation matrix
339 |    *
340 |    * \param sR a 3x3 scaled rotation matrix
341 |    * \pre        the 3x3 matrix should be "scaled orthogonal"
342 |    *             and have a positive determinant
343 |    */
344 |   inline
345 |   void setScaledRotationMatrix
346 |   (const Transformation & sR) {
347 |     Transformation squared_sR = sR*sR.transpose();
348 |     Scalar squared_scale
349 |         = static_cast<Scalar>(1./3.)
350 |         *(squared_sR(0,0)+squared_sR(1,1)+squared_sR(2,2));
351 |     if (squared_scale <= static_cast<Scalar>(0)) {
352 |       throw ScaleNotPositive();
353 |     }
354 |     Scalar scale = std::sqrt(squared_scale);
355 |     if (scale <= static_cast<Scalar>(0)) {
356 |       throw ScaleNotPositive();
357 |     }
358 |     quaternion() = sR/scale;
359 |     quaternion().coeffs() *= scale;
360 |   }
361 | 
362 |   ////////////////////////////////////////////////////////////////////////////
363 |   // public static functions
364 |   ////////////////////////////////////////////////////////////////////////////
365 | 
366 |   /**
367 |    * \param   b 4-vector representation of Lie algebra element
368 |    * \returns   derivative of Lie bracket
369 |    *
370 |    * This function returns \f$ \frac{\partial}{\partial a} [a, b]_{rxso3} \f$
371 |    * with \f$ [a, b]_{rxso3} \f$ being the lieBracket() of the Lie
372 |    * algebra rxso3.
373 |    *
374 |    * \see lieBracket()
375 |    */
376 |   inline static
377 |   const Adjoint d_lieBracketab_by_d_a(const Tangent & b) {
378 |     Adjoint res;
379 |     res.setZero();
380 |     res.template topLeftCorner<3,3>() = -SO3::hat(b.template head<3>());
381 |     return res;
382 |   }
383 | 
384 |   /**
385 |    * \brief Group exponential
386 |    *
387 |    * \param a tangent space element
388 |    *          (rotation vector \f$ \omega \f$ and logarithm of scale)
389 |    * \returns corresponding element of the group RxSO3
390 |    *
391 |    * To be more specific, this function computes \f$ \exp(\widehat{a}) \f$
392 |    * with \f$ \exp(\cdot) \f$ being the matrix exponential
393 |    * and \f$ \widehat{\cdot} \f$ the hat()-operator of RxSO3.
394 |    *
395 |    * \see expAndTheta()
396 |    * \see hat()
397 |    * \see log()
398 |    */
399 |   inline static
400 |   const RxSO3Group<Scalar> exp(const Tangent & a) {
401 |     Scalar theta;
402 |     return expAndTheta(a, &theta);
403 |   }
404 | 
405 |   /**
406 |    * \brief Group exponential and theta
407 |    *
408 |    * \param      a     tangent space element
409 |    *                   (rotation vector \f$ \omega \f$ and logarithm of scale )
410 |    * \param[out] theta angle of rotation \f$ \theta = |\omega| \f$
411 |    * \returns          corresponding element of the group RxSO3
412 |    *
413 |    * \see exp() for details
414 |    */
415 |   inline static
416 |   const RxSO3Group<Scalar> expAndTheta(const Tangent & a,
417 |                                        Scalar * theta) {
418 |     const Matrix<Scalar,3,1> & omega = a.template head<3>();
419 |     Scalar sigma = a[3];
420 |     Scalar scale = std::exp(sigma);
421 |     Quaternion<Scalar> quat
422 |         = SO3Group<Scalar>::expAndTheta(omega, theta).unit_quaternion();
423 |     quat.coeffs() *= scale;
424 |     return RxSO3Group<Scalar>(quat);
425 |   }
426 | 
427 |   /**
428 |    * \brief Generators
429 |    *
430 |    * \pre \f$ i \in \{0,1,2,3\} \f$
431 |    * \returns \f$ i \f$th generator \f$ G_i \f$ of RxSO3
432 |    *
433 |    * The infinitesimal generators of RxSO3
434 |    * are \f$
435 |    *        G_0 = \left( \begin{array}{ccc}
436 |    *                          0&  0&  0& \\
437 |    *                          0&  0& -1& \\
438 |    *                          0&  1&  0&
439 |    *                     \end{array} \right),
440 |    *        G_1 = \left( \begin{array}{ccc}
441 |    *                          0&  0&  1& \\
442 |    *                          0&  0&  0& \\
443 |    *                         -1&  0&  0&
444 |    *                     \end{array} \right),
445 |    *        G_2 = \left( \begin{array}{ccc}
446 |    *                          0& -1&  0& \\
447 |    *                          1&  0&  0& \\
448 |    *                          0&  0&  0&
449 |    *                     \end{array} \right),
450 |    *        G_3 = \left( \begin{array}{ccc}
451 |    *                          1&  0&  0& \\
452 |    *                          0&  1&  0& \\
453 |    *                          0&  0&  1&
454 |    *                     \end{array} \right).
455 |    * \f$
456 |    * \see hat()
457 |    */
458 |   inline static
459 |   const Transformation generator(int i) {
460 |     if (i<0 || i>3) {
461 |       throw SophusException("i is not in range [0,3].");
462 |     }
463 |     Tangent e;
464 |     e.setZero();
465 |     e[i] = static_cast<Scalar>(1);
466 |     return hat(e);
467 |   }
468 | 
469 |   /**
470 |    * \brief hat-operator
471 |    *
472 |    * \param a 4-vector representation of Lie algebra element
473 |    * \returns 3x3-matrix representatin of Lie algebra element
474 |    *
475 |    * Formally, the hat-operator of RxSO3 is defined
476 |    * as \f$ \widehat{\cdot}: \mathbf{R}^4 \rightarrow \mathbf{R}^{3\times 3},
477 |    * \quad \widehat{a} = \sum_{i=0}^3 G_i a_i \f$
478 |    * with \f$ G_i \f$ being the ith infinitesial generator().
479 |    *
480 |    * \see generator()
481 |    * \see vee()
482 |    */
483 |   inline static
484 |   const Transformation hat(const Tangent & a) {
485 |     Transformation A;
486 |     A <<  a(3), -a(2),  a(1)
487 |         , a(2),  a(3), -a(0)
488 |         ,-a(1),  a(0),  a(3);
489 |     return A;
490 |   }
491 | 
492 |   /**
493 |    * \brief Lie bracket
494 |    *
495 |    * \param a 4-vector representation of Lie algebra element
496 |    * \param b 4-vector representation of Lie algebra element
497 |    * \returns 4-vector representation of Lie algebra element
498 |    *
499 |    * It computes the bracket of RxSO3. To be more specific, it
500 |    * computes \f$ [a, 2]_{rxso3}
501 |    * := [\widehat{a}, \widehat{b}]^\vee \f$
502 |    * with \f$ [A,B] = AB-BA \f$ being the matrix
503 |    * commutator, \f$ \widehat{\cdot} \f$ the
504 |    * hat()-operator and \f$ (\cdot)^\vee \f$ the vee()-operator of RxSO3.
505 |    *
506 |    * \see hat()
507 |    * \see vee()
508 |    */
509 |   inline static
510 |   const Tangent lieBracket(const Tangent & a,
511 |                            const Tangent & b) {
512 |     const Matrix<Scalar,3,1> & omega1 = a.template head<3>();
513 |     const Matrix<Scalar,3,1> & omega2 = b.template head<3>();
514 |     Matrix<Scalar,4,1> res;
515 |     res.template head<3>() = omega1.cross(omega2);
516 |     res[3] = static_cast<Scalar>(0);
517 |     return res;
518 |   }
519 | 
520 |   /**
521 |    * \brief Logarithmic map
522 |    *
523 |    * \param other element of the group RxSO3
524 |    * \returns     corresponding tangent space element
525 |    *              (rotation vector \f$ \omega \f$ and logarithm of scale)
526 |    *
527 |    * Computes the logarithmic, the inverse of the group exponential.
528 |    * To be specific, this function computes \f$ \log({\cdot})^\vee \f$
529 |    * with \f$ \vee(\cdot) \f$ being the matrix logarithm
530 |    * and \f$ \vee{\cdot} \f$ the vee()-operator of RxSO3.
531 |    *
532 |    * \see exp()
533 |    * \see logAndTheta()
534 |    * \see vee()
535 |    */
536 |   inline static
537 |   const Tangent log(const RxSO3Group<Scalar> & other) {
538 |     Scalar theta;
539 |     return logAndTheta(other, &theta);
540 |   }
541 | 
542 |   /**
543 |    * \brief Logarithmic map and theta
544 |    *
545 |    * \param      other element of the group RxSO3
546 |    * \param[out] theta angle of rotation \f$ \theta = |\omega| \f$
547 |    * \returns          corresponding tangent space element
548 |    *                   (rotation vector \f$ \omega \f$ and logarithm of scale)
549 |    *
550 |    * \see log() for details
551 |    */
552 |   inline static
553 |   const Tangent logAndTheta(const RxSO3Group<Scalar> & other,
554 |                             Scalar * theta) {
555 |     const Scalar & scale = other.quaternion().norm();
556 |     Tangent omega_sigma;
557 |     omega_sigma[3] = std::log(scale);
558 |     omega_sigma.template head<3>()
559 |         = SO3Group<Scalar>::logAndTheta(SO3Group<Scalar>(other.quaternion()),
560 |                                         theta);
561 |     return omega_sigma;
562 |   }
563 | 
564 |   /**
565 |    * \brief vee-operator
566 |    *
567 |    * \param Omega 3x3-matrix representation of Lie algebra element
568 |    * \returns     4-vector representatin of Lie algebra element
569 |    *
570 |    * This is the inverse of the hat()-operator.
571 |    *
572 |    * \see hat()
573 |    */
574 |   inline static
575 |   const Tangent vee(const Transformation & Omega) {
576 |     return Tangent( static_cast<Scalar>(0.5) * (Omega(2,1) - Omega(1,2)),
577 |                     static_cast<Scalar>(0.5) * (Omega(0,2) - Omega(2,0)),
578 |                     static_cast<Scalar>(0.5) * (Omega(1,0) - Omega(0,1)),
579 |                     static_cast<Scalar>(1./3.)
580 |                     * (Omega(0,0) + Omega(1,1) + Omega(2,2)) );
581 |   }
582 | };
583 | 
584 | /**
585 |  * \brief RxSO3 default type - Constructors and default storage for RxSO3 Type
586 |  */
587 | template<typename _Scalar, int _Options>
588 | class RxSO3Group : public RxSO3GroupBase<RxSO3Group<_Scalar,_Options> > {
589 |   typedef RxSO3GroupBase<RxSO3Group<_Scalar,_Options> > Base;
590 | public:
591 |   /** \brief scalar type */
592 |   typedef typename internal::traits<SO3Group<_Scalar,_Options> >
593 |   ::Scalar Scalar;
594 |   /** \brief quaternion reference type */
595 |   typedef typename internal::traits<SO3Group<_Scalar,_Options> >
596 |   ::QuaternionType & QuaternionReference;
597 |   /** \brief quaternion const reference type */
598 |   typedef const typename internal::traits<SO3Group<_Scalar,_Options> >
599 |   ::QuaternionType & ConstQuaternionReference;
600 | 
601 |   /** \brief degree of freedom of group */
602 |   static const int DoF = Base::DoF;
603 |   /** \brief number of internal parameters used */
604 |   static const int num_parameters = Base::num_parameters;
605 |   /** \brief group transformations are NxN matrices */
606 |   static const int N = Base::N;
607 |   /** \brief group transfomation type */
608 |   typedef typename Base::Transformation Transformation;
609 |   /** \brief point type */
610 |   typedef typename Base::Point Point;
611 |   /** \brief tangent vector type */
612 |   typedef typename Base::Tangent Tangent;
613 |   /** \brief adjoint transformation type */
614 |   typedef typename Base::Adjoint Adjoint;
615 | 
616 |   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
617 | 
618 |   /**
619 |    * \brief Default constructor
620 |    *
621 |    * Initialize Quaternion to identity rotation and scale.
622 |    */
623 |   inline RxSO3Group()
624 |     : quaternion_(static_cast<Scalar>(1), static_cast<Scalar>(0),
625 |                   static_cast<Scalar>(0), static_cast<Scalar>(0)) {
626 |   }
627 | 
628 |   /**
629 |    * \brief Copy constructor
630 |    */
631 |   template<typename OtherDerived> inline
632 |   RxSO3Group(const RxSO3GroupBase<OtherDerived> & other)
633 |     : quaternion_(other.quaternion()) {
634 |   }
635 | 
636 |   /**
637 |    * \brief Constructor from scaled rotation matrix
638 |    *
639 |    * \pre matrix need to be "scaled orthogonal" with positive determinant
640 |    */
641 |   inline explicit
642 |   RxSO3Group(const Transformation & sR) {
643 |     setScaledRotationMatrix(sR);
644 |   }
645 | 
646 |   /**
647 |    * \brief Constructor from scale factor and rotation matrix
648 |    *
649 |    * \pre rotation matrix need to be orthogonal with determinant of 1
650 |    * \pre scale need to be not zero
651 |    */
652 |   inline
653 |   RxSO3Group(const Scalar & scale, const Transformation & R)
654 |     : quaternion_(R) {
655 |     if(scale <= static_cast<Scalar>(0)) {
656 |       throw ScaleNotPositive();
657 |     }
658 |     quaternion_.normalize();
659 |     quaternion_.coeffs() *= scale;
660 |   }
661 | 
662 |   /**
663 |    * \brief Constructor from scale factor and SO3
664 |    *
665 |    * \pre scale need to be not zero
666 |    */
667 |   inline
668 |   RxSO3Group(const Scalar & scale, const SO3Group<Scalar> & so3)
669 |     : quaternion_(so3.unit_quaternion()) {
670 |     if (scale <= static_cast<Scalar>(0)) {
671 |       throw ScaleNotPositive();
672 |     }
673 |     quaternion_.normalize();
674 |     quaternion_.coeffs() *= scale;
675 |   }
676 | 
677 |   /**
678 |    * \brief Constructor from quaternion
679 |    *
680 |    * \pre quaternion must not be zero
681 |    */
682 |   inline explicit
683 |   RxSO3Group(const Quaternion<Scalar> & quat) : quaternion_(quat) {
684 |     if(quaternion_.squaredNorm() <= SophusConstants<Scalar>::epsilon()) {
685 |       throw ScaleNotPositive();
686 |     }
687 |   }
688 | 
689 |   /**
690 |    * \brief Mutator of quaternion
691 |    */
692 |   EIGEN_STRONG_INLINE
693 |   QuaternionReference quaternion() {
694 |     return quaternion_;
695 |   }
696 | 
697 |   /**
698 |    * \brief Accessor of quaternion
699 |    */
700 |   EIGEN_STRONG_INLINE
701 |   ConstQuaternionReference quaternion() const {
702 |     return quaternion_;
703 |   }
704 | 
705 | protected:
706 |   Quaternion<Scalar> quaternion_;
707 | };
708 | 
709 | } // end namespace
710 | 
711 | 
712 | namespace Eigen {
713 | /**
714 |  * \brief Specialisation of Eigen::Map for RxSO3GroupBase
715 |  *
716 |  * Allows us to wrap RxSO3 Objects around POD array
717 |  * (e.g. external c style quaternion)
718 |  */
719 | template<typename _Scalar, int _Options>
720 | class Map<Sophus::RxSO3Group<_Scalar>, _Options>
721 |     : public Sophus::RxSO3GroupBase<
722 |     Map<Sophus::RxSO3Group<_Scalar>,_Options> > {
723 |   typedef Sophus::RxSO3GroupBase<Map<Sophus::RxSO3Group<_Scalar>, _Options> >
724 |   Base;
725 | 
726 | public:
727 |   /** \brief scalar type */
728 |   typedef typename internal::traits<Map>::Scalar Scalar;
729 |   /** \brief quaternion reference type */
730 |   typedef typename internal::traits<Map>::QuaternionType &
731 |   QuaternionReference;
732 |   /** \brief quaternion const reference type */
733 |   typedef const typename internal::traits<Map>::QuaternionType &
734 |   ConstQuaternionReference;
735 | 
736 |   /** \brief degree of freedom of group */
737 |   static const int DoF = Base::DoF;
738 |   /** \brief number of internal parameters used */
739 |   static const int num_parameters = Base::num_parameters;
740 |   /** \brief group transformations are NxN matrices */
741 |   static const int N = Base::N;
742 |   /** \brief group transfomation type */
743 |   typedef typename Base::Transformation Transformation;
744 |   /** \brief point type */
745 |   typedef typename Base::Point Point;
746 |   /** \brief tangent vector type */
747 |   typedef typename Base::Tangent Tangent;
748 |   /** \brief adjoint transformation type */
749 |   typedef typename Base::Adjoint Adjoint;
750 | 
751 |   EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
752 |   using Base::operator*=;
753 |   using Base::operator*;
754 | 
755 |   EIGEN_STRONG_INLINE
756 |   Map(Scalar* coeffs) : quaternion_(coeffs) {
757 |   }
758 | 
759 |   /**
760 |    * \brief Mutator of quaternion
761 |    */
762 |   EIGEN_STRONG_INLINE
763 |   QuaternionReference quaternion() {
764 |     return quaternion_;
765 |   }
766 | 
767 |   /**
768 |    * \brief Accessor of quaternion
769 |    */
770 |   EIGEN_STRONG_INLINE
771 |   ConstQuaternionReference quaternion() const {
772 |     return quaternion_;
773 |   }
774 | 
775 | protected:
776 |   Map<Quaternion<Scalar>,_Options> quaternion_;
777 | };
778 | 
779 | /**
780 |  * \brief Specialisation of Eigen::Map for const RxSO3GroupBase
781 |  *
782 |  * Allows us to wrap RxSO3 Objects around POD array
783 |  * (e.g. external c style quaternion)
784 |  */
785 | template<typename _Scalar, int _Options>
786 | class Map<const Sophus::RxSO3Group<_Scalar>, _Options>
787 |     : public Sophus::RxSO3GroupBase<
788 |     Map<const Sophus::RxSO3Group<_Scalar>, _Options> > {
789 |   typedef Sophus::RxSO3GroupBase<
790 |   Map<const Sophus::RxSO3Group<_Scalar>, _Options> > Base;
791 | 
792 | public:
793 |   /** \brief scalar type */
794 |   typedef typename internal::traits<Map>::Scalar Scalar;
795 |   /** \brief quaternion const reference type */
796 |   typedef const typename internal::traits<Map>::QuaternionType &
797 |   ConstQuaternionReference;
798 | 
799 |   /** \brief degree of freedom of group */
800 |   static const int DoF = Base::DoF;
801 |   /** \brief number of internal parameters used */
802 |   static const int num_parameters = Base::num_parameters;
803 |   /** \brief group transformations are NxN matrices */
804 |   static const int N = Base::N;
805 |   /** \brief group transfomation type */
806 |   typedef typename Base::Transformation Transformation;
807 |   /** \brief point type */
808 |   typedef typename Base::Point Point;
809 |   /** \brief tangent vector type */
810 |   typedef typename Base::Tangent Tangent;
811 |   /** \brief adjoint transformation type */
812 |   typedef typename Base::Adjoint Adjoint;
813 | 
814 |   EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
815 |   using Base::operator*=;
816 |   using Base::operator*;
817 | 
818 |   EIGEN_STRONG_INLINE
819 |   Map(const Scalar* coeffs) : quaternion_(coeffs) {
820 |   }
821 | 
822 |   /**
823 |    * \brief Accessor of unit quaternion
824 |    *
825 |    * No direct write access is given to ensure the quaternion stays normalized.
826 |    */
827 |   EIGEN_STRONG_INLINE
828 |   ConstQuaternionReference quaternion() const {
829 |     return quaternion_;
830 |   }
831 | 
832 | protected:
833 |   const Map<const Quaternion<Scalar>,_Options> quaternion_;
834 | };
835 | 
836 | }
837 | 
838 | #endif // SOPHUS_RXSO3_HPP
839 | 


--------------------------------------------------------------------------------
/include/lsd_slam_to_pcl/third_party/sophus/so3.hpp:
--------------------------------------------------------------------------------
  1 | // This file is part of Sophus.
  2 | //
  3 | // Copyright 2011-2013 Hauke Strasdat
  4 | // Copyrifht 2012-2013 Steven Lovegrove
  5 | //
  6 | // Permission is hereby granted, free of charge, to any person obtaining a copy
  7 | // of this software and associated documentation files (the "Software"), to
  8 | // deal in the Software without restriction, including without limitation the
  9 | // rights  to use, copy, modify, merge, publish, distribute, sublicense, and/or
 10 | // sell copies of the Software, and to permit persons to whom the Software is
 11 | // furnished to do so, subject to the following conditions:
 12 | //
 13 | // The above copyright notice and this permission notice shall be included in
 14 | // all copies or substantial portions of the Software.
 15 | //
 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 21 | // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
 22 | // IN THE SOFTWARE.
 23 | 
 24 | #ifndef SOPHUS_SO3_HPP
 25 | #define SOPHUS_SO3_HPP
 26 | 
 27 | #include "sophus.hpp"
 28 | 
 29 | ////////////////////////////////////////////////////////////////////////////
 30 | // Forward Declarations / typedefs
 31 | ////////////////////////////////////////////////////////////////////////////
 32 | 
 33 | namespace Sophus {
 34 | template<typename _Scalar, int _Options=0> class SO3Group;
 35 | typedef EIGEN_DEPRECATED SO3Group<double> SO3;
 36 | typedef SO3Group<double> SO3d; /**< double precision SO3 */
 37 | typedef SO3Group<float> SO3f;  /**< single precision SO3 */
 38 | }
 39 | 
 40 | ////////////////////////////////////////////////////////////////////////////
 41 | // Eigen Traits (For querying derived types in CRTP hierarchy)
 42 | ////////////////////////////////////////////////////////////////////////////
 43 | 
 44 | namespace Eigen {
 45 | namespace internal {
 46 | 
 47 | template<typename _Scalar, int _Options>
 48 | struct traits<Sophus::SO3Group<_Scalar,_Options> > {
 49 |   typedef _Scalar Scalar;
 50 |   typedef Quaternion<Scalar> QuaternionType;
 51 | };
 52 | 
 53 | template<typename _Scalar, int _Options>
 54 | struct traits<Map<Sophus::SO3Group<_Scalar>, _Options> >
 55 |     : traits<Sophus::SO3Group<_Scalar, _Options> > {
 56 |   typedef _Scalar Scalar;
 57 |   typedef Map<Quaternion<Scalar>,_Options> QuaternionType;
 58 | };
 59 | 
 60 | template<typename _Scalar, int _Options>
 61 | struct traits<Map<const Sophus::SO3Group<_Scalar>, _Options> >
 62 |     : traits<const Sophus::SO3Group<_Scalar, _Options> > {
 63 |   typedef _Scalar Scalar;
 64 |   typedef Map<const Quaternion<Scalar>,_Options> QuaternionType;
 65 | };
 66 | 
 67 | }
 68 | }
 69 | 
 70 | namespace Sophus {
 71 | using namespace Eigen;
 72 | 
 73 | /**
 74 |  * \brief SO3 base type - implements SO3 class but is storage agnostic
 75 |  *
 76 |  * [add more detailed description/tutorial]
 77 |  */
 78 | template<typename Derived>
 79 | class SO3GroupBase {
 80 | public:
 81 |   /** \brief scalar type */
 82 |   typedef typename internal::traits<Derived>::Scalar Scalar;
 83 |   /** \brief quaternion reference type  */
 84 |   typedef typename internal::traits<Derived>::QuaternionType &
 85 |   QuaternionReference;
 86 |   /** \brief quaternion const reference type  */
 87 |   typedef const typename internal::traits<Derived>::QuaternionType &
 88 |   ConstQuaternionReference;
 89 | 
 90 |   /** \brief degree of freedom of group
 91 |    *         (three for rotation) */
 92 |   static const int DoF = 3;
 93 |   /** \brief number of internal parameters used
 94 |    *         (unit quaternion for rotation) */
 95 |   static const int num_parameters = 4;
 96 |   /** \brief group transformations are NxN matrices */
 97 |   static const int N = 3;
 98 |   /** \brief group transfomation type */
 99 |   typedef Matrix<Scalar,N,N> Transformation;
100 |   /** \brief point type */
101 |   typedef Matrix<Scalar,3,1> Point;
102 |   /** \brief tangent vector type */
103 |   typedef Matrix<Scalar,DoF,1> Tangent;
104 |   /** \brief adjoint transformation type */
105 |   typedef Matrix<Scalar,DoF,DoF> Adjoint;
106 | 
107 |   /**
108 |    * \brief Adjoint transformation
109 |    *
110 |    * This function return the adjoint transformation \f$ Ad \f$ of the
111 |    * group instance \f$ A \f$  such that for all \f$ x \f$
112 |    * it holds that \f$ \widehat{Ad_A\cdot x} = A\widehat{x}A^{-1} \f$
113 |    * with \f$\ \widehat{\cdot} \f$ being the hat()-Vector4Scalaror.
114 |    *
115 |    * For SO3, it simply returns the rotation matrix corresponding to \f$ A \f$.
116 |    */
117 |   inline
118 |   const Adjoint Adj() const {
119 |     return matrix();
120 |   }
121 | 
122 |   /**
123 |    * \returns copy of instance casted to NewScalarType
124 |    */
125 |   template<typename NewScalarType>
126 |   inline SO3Group<NewScalarType> cast() const {
127 |     return SO3Group<NewScalarType>(unit_quaternion()
128 |                                    .template cast<NewScalarType>() );
129 |   }
130 | 
131 |   /**
132 |    * \returns pointer to internal data
133 |    *
134 |    * This provides unsafe read/write access to internal data. SO3 is represented
135 |    * by an Eigen::Quaternion (four parameters). When using direct write access,
136 |    * the user needs to take care of that the quaternion stays normalized.
137 |    *
138 |    * Note: The first three Scalars represent the imaginary parts, while the
139 |    * forth Scalar represent the real part.
140 |    *
141 |    * \see normalize()
142 |    */
143 |   inline Scalar* data() {
144 |     return unit_quaternion_nonconst().coeffs().data();
145 |   }
146 | 
147 |   /**
148 |    * \returns const pointer to internal data
149 |    *
150 |    * Const version of data().
151 |    */
152 |   inline const Scalar* data() const {
153 |     return unit_quaternion().coeffs().data();
154 |   }
155 | 
156 |   /**
157 |    * \brief Fast group multiplication
158 |    *
159 |    * This method is a fast version of operator*=(), since it does not perform
160 |    * normalization. It is up to the user to call normalize() once in a while.
161 |    *
162 |    * \see operator*=()
163 |    */
164 |   inline
165 |   void fastMultiply(const SO3Group<Scalar>& other) {
166 |     unit_quaternion_nonconst() *= other.unit_quaternion();
167 |   }
168 | 
169 |   /**
170 |    * \returns group inverse of instance
171 |    */
172 |   inline
173 |   const SO3Group<Scalar> inverse() const {
174 |     return SO3Group<Scalar>(unit_quaternion().conjugate());
175 |   }
176 | 
177 |   /**
178 |    * \brief Logarithmic map
179 |    *
180 |    * \returns tangent space representation (=rotation vector) of instance
181 |    *
182 |    * \see  log().
183 |    */
184 |   inline
185 |   const Tangent log() const {
186 |     return SO3Group<Scalar>::log(*this);
187 |   }
188 | 
189 |   /**
190 |    * \brief Normalize quaternion
191 |    *
192 |    * It re-normalizes unit_quaternion to unit length. This method only needs to
193 |    * be called in conjunction with fastMultiply() or data() write access.
194 |    */
195 |   inline
196 |   void normalize() {
197 |     Scalar length = unit_quaternion_nonconst().norm();
198 |     if (length < SophusConstants<Scalar>::epsilon()) {
199 |       throw SophusException("Quaternion is (near) zero!");
200 |     }
201 |     unit_quaternion_nonconst().coeffs() /= length;
202 |   }
203 | 
204 |   /**
205 |    * \returns 3x3 matrix representation of instance
206 |    *
207 |    * For SO3, the matrix representation is an orthogonal matrix R with det(R)=1,
208 |    * thus the so-called rotation matrix.
209 |    */
210 |   inline
211 |   const Transformation matrix() const {
212 |     return unit_quaternion().toRotationMatrix();
213 |   }
214 | 
215 |   /**
216 |    * \brief Assignment operator
217 |    */
218 |   template<typename OtherDerived> inline
219 |   SO3GroupBase<Derived>& operator=(const SO3GroupBase<OtherDerived> & other) {
220 |     unit_quaternion_nonconst() = other.unit_quaternion();
221 |     return *this;
222 |   }
223 | 
224 |   /**
225 |    * \brief Group multiplication
226 |    * \see operator*=()
227 |    */
228 |   inline
229 |   const SO3Group<Scalar> operator*(const SO3Group<Scalar>& other) const {
230 |     SO3Group<Scalar> result(*this);
231 |     result *= other;
232 |     return result;
233 |   }
234 | 
235 |   /**
236 |    * \brief Group action on \f$ \mathbf{R}^3 \f$
237 |    *
238 |    * \param p point \f$p \in \mathbf{R}^3 \f$
239 |    * \returns point \f$p' \in \mathbf{R}^3 \f$, rotated version of \f$p\f$
240 |    *
241 |    * This function rotates a point \f$ p \f$ in  \f$ \mathbf{R}^3 \f$ by the
242 |    * SO3 transformation \f$R\f$ (=rotation matrix): \f$ p' = R\cdot p \f$.
243 |    *
244 |    *
245 |    * Since SO3 is intenally represented by a unit quaternion \f$q\f$, it is
246 |    * implemented as \f$ p' = q\cdot p \cdot q^{*} \f$
247 |    * with \f$ q^{*} \f$ being the quaternion conjugate of \f$ q \f$.
248 |    *
249 |    * Geometrically, \f$ p \f$  is rotated by angle \f$|\omega|\f$ around the
250 |    * axis \f$\frac{\omega}{|\omega|}\f$ with \f$ \omega = \log(R)^\vee \f$.
251 |    *
252 |    * \see log()
253 |    */
254 |   inline
255 |   const Point operator*(const Point & p) const {
256 |     return unit_quaternion()._transformVector(p);
257 |   }
258 | 
259 |   /**
260 |    * \brief In-place group multiplication
261 |    *
262 |    * \see fastMultiply()
263 |    * \see operator*()
264 |    */
265 |   inline
266 |   void operator*=(const SO3Group<Scalar>& other) {
267 |     fastMultiply(other);
268 |     normalize();
269 |   }
270 | 
271 |   /**
272 |    * \brief Setter of internal unit quaternion representation
273 |    *
274 |    * \param quaternion
275 |    * \pre   the quaternion must not be zero
276 |    *
277 |    * The quaternion is normalized to unit length.
278 |    */
279 |   inline
280 |   void setQuaternion(const Quaternion<Scalar>& quaternion) {
281 |     unit_quaternion_nonconst() = quaternion;
282 |     normalize();
283 |   }
284 | 
285 |   /**
286 |    * \brief Accessor of unit quaternion
287 |    *
288 |    * No direct write access is given to ensure the quaternion stays normalized.
289 |    */
290 |   EIGEN_STRONG_INLINE
291 |   ConstQuaternionReference unit_quaternion() const {
292 |     return static_cast<const Derived*>(this)->unit_quaternion();
293 |   }
294 | 
295 |   ////////////////////////////////////////////////////////////////////////////
296 |   // public static functions
297 |   ////////////////////////////////////////////////////////////////////////////
298 | 
299 |   /**
300 |    * \param   b 3-vector representation of Lie algebra element
301 |    * \returns   derivative of Lie bracket
302 |    *
303 |    * This function returns \f$ \frac{\partial}{\partial a} [a, b]_{so3} \f$
304 |    * with \f$ [a, b]_{so3} \f$ being the lieBracket() of the Lie algebra so3.
305 |    *
306 |    * \see lieBracket()
307 |    */
308 |   inline static
309 |   const Adjoint d_lieBracketab_by_d_a(const Tangent & b) {
310 |     return -hat(b);
311 |   }
312 | 
313 |   /**
314 |    * \brief Group exponential
315 |    *
316 |    * \param omega tangent space element (=rotation vector \f$ \omega \f$)
317 |    * \returns     corresponding element of the group SO3
318 |    *
319 |    * To be more specific, this function computes \f$ \exp(\widehat{\omega}) \f$
320 |    * with \f$ \exp(\cdot) \f$ being the matrix exponential
321 |    * and \f$ \widehat{\cdot} \f$ the hat()-operator of SO3.
322 |    *
323 |    * \see expAndTheta()
324 |    * \see hat()
325 |    * \see log()
326 |    */
327 |   inline static
328 |   const SO3Group<Scalar> exp(const Tangent & omega) {
329 |     Scalar theta;
330 |     return expAndTheta(omega, &theta);
331 |   }
332 | 
333 |   /**
334 |    * \brief Group exponential and theta
335 |    *
336 |    * \param      omega tangent space element (=rotation vector \f$ \omega \f$)
337 |    * \param[out] theta angle of rotation \f$ \theta = |\omega| \f$
338 |    * \returns          corresponding element of the group SO3
339 |    *
340 |    * \see exp() for details
341 |    */
342 |   inline static
343 |   const SO3Group<Scalar> expAndTheta(const Tangent & omega,
344 |                                      Scalar * theta) {
345 |     const Scalar theta_sq = omega.squaredNorm();
346 |     *theta = std::sqrt(theta_sq);
347 |     const Scalar half_theta = static_cast<Scalar>(0.5)*(*theta);
348 | 
349 |     Scalar imag_factor;
350 |     Scalar real_factor;;
351 |     if((*theta)<SophusConstants<Scalar>::epsilon()) {
352 |       const Scalar theta_po4 = theta_sq*theta_sq;
353 |       imag_factor = static_cast<Scalar>(0.5)
354 |                     - static_cast<Scalar>(1.0/48.0)*theta_sq
355 |                     + static_cast<Scalar>(1.0/3840.0)*theta_po4;
356 |       real_factor = static_cast<Scalar>(1)
357 |                     - static_cast<Scalar>(0.5)*theta_sq +
358 |                     static_cast<Scalar>(1.0/384.0)*theta_po4;
359 |     } else {
360 |       const Scalar sin_half_theta = std::sin(half_theta);
361 |       imag_factor = sin_half_theta/(*theta);
362 |       real_factor = std::cos(half_theta);
363 |     }
364 | 
365 |     return SO3Group<Scalar>(Quaternion<Scalar>(real_factor,
366 |                                                imag_factor*omega.x(),
367 |                                                imag_factor*omega.y(),
368 |                                                imag_factor*omega.z()));
369 |   }
370 | 
371 |   /**
372 |    * \brief Generators
373 |    *
374 |    * \pre \f$ i \in \{0,1,2\} \f$
375 |    * \returns \f$ i \f$th generator \f$ G_i \f$ of SO3
376 |    *
377 |    * The infinitesimal generators of SO3
378 |    * are \f$
379 |    *        G_0 = \left( \begin{array}{ccc}
380 |    *                          0&  0&  0& \\
381 |    *                          0&  0& -1& \\
382 |    *                          0&  1&  0&
383 |    *                     \end{array} \right),
384 |    *        G_1 = \left( \begin{array}{ccc}
385 |    *                          0&  0&  1& \\
386 |    *                          0&  0&  0& \\
387 |    *                         -1&  0&  0&
388 |    *                     \end{array} \right),
389 |    *        G_2 = \left( \begin{array}{ccc}
390 |    *                          0& -1&  0& \\
391 |    *                          1&  0&  0& \\
392 |    *                          0&  0&  0&
393 |    *                     \end{array} \right).
394 |    * \f$
395 |    * \see hat()
396 |    */
397 |   inline static
398 |   const Transformation generator(int i) {
399 |     if (i<0 || i>2) {
400 |       throw SophusException("i is not in range [0,2].");
401 |     }
402 |     Tangent e;
403 |     e.setZero();
404 |     e[i] = static_cast<Scalar>(1);
405 |     return hat(e);
406 |   }
407 | 
408 |   /**
409 |    * \brief hat-operator
410 |    *
411 |    * \param omega 3-vector representation of Lie algebra element
412 |    * \returns     3x3-matrix representatin of Lie algebra element
413 |    *
414 |    * Formally, the hat-operator of SO3 is defined
415 |    * as \f$ \widehat{\cdot}: \mathbf{R}^3 \rightarrow \mathbf{R}^{3\times 3},
416 |    * \quad \widehat{\omega} = \sum_{i=0}^2 G_i \omega_i \f$
417 |    * with \f$ G_i \f$ being the ith infinitesial generator().
418 |    *
419 |    * \see generator()
420 |    * \see vee()
421 |    */
422 |   inline static
423 |   const Transformation hat(const Tangent & omega) {
424 |     Transformation Omega;
425 |     Omega <<  static_cast<Scalar>(0), -omega(2),  omega(1)
426 |         ,  omega(2),     static_cast<Scalar>(0), -omega(0)
427 |         , -omega(1),  omega(0),     static_cast<Scalar>(0);
428 |     return Omega;
429 |   }
430 | 
431 |   /**
432 |    * \brief Lie bracket
433 |    *
434 |    * \param omega1 3-vector representation of Lie algebra element
435 |    * \param omega2 3-vector representation of Lie algebra element
436 |    * \returns      3-vector representation of Lie algebra element
437 |    *
438 |    * It computes the bracket of SO3. To be more specific, it
439 |    * computes \f$ [\omega_1, \omega_2]_{so3}
440 |    * := [\widehat{\omega_1}, \widehat{\omega_2}]^\vee \f$
441 |    * with \f$ [A,B] = AB-BA \f$ being the matrix
442 |    * commutator, \f$ \widehat{\cdot} \f$ the
443 |    * hat()-operator and \f$ (\cdot)^\vee \f$ the vee()-operator of SO3.
444 |    *
445 |    * For the Lie algebra so3, the Lie bracket is simply the
446 |    * cross product: \f$ [\omega_1, \omega_2]_{so3}
447 |    *                    = \omega_1 \times \omega_2 \f$.
448 |    *
449 |    * \see hat()
450 |    * \see vee()
451 |    */
452 |   inline static
453 |   const Tangent lieBracket(const Tangent & omega1,
454 |                            const Tangent & omega2) {
455 |     return omega1.cross(omega2);
456 |   }
457 | 
458 |   /**
459 |    * \brief Logarithmic map
460 |    *
461 |    * \param other element of the group SO3
462 |    * \returns     corresponding tangent space element
463 |    *              (=rotation vector \f$ \omega \f$)
464 |    *
465 |    * Computes the logarithmic, the inverse of the group exponential.
466 |    * To be specific, this function computes \f$ \log({\cdot})^\vee \f$
467 |    * with \f$ \vee(\cdot) \f$ being the matrix logarithm
468 |    * and \f$ \vee{\cdot} \f$ the vee()-operator of SO3.
469 |    *
470 |    * \see exp()
471 |    * \see logAndTheta()
472 |    * \see vee()
473 |    */
474 |   inline static
475 |   const Tangent log(const SO3Group<Scalar> & other) {
476 |     Scalar theta;
477 |     return logAndTheta(other, &theta);
478 |   }
479 | 
480 |   /**
481 |    * \brief Logarithmic map and theta
482 |    *
483 |    * \param      other element of the group SO3
484 |    * \param[out] theta angle of rotation \f$ \theta = |\omega| \f$
485 |    * \returns          corresponding tangent space element
486 |    *                   (=rotation vector \f$ \omega \f$)
487 |    *
488 |    * \see log() for details
489 |    */
490 |   inline static
491 |   const Tangent logAndTheta(const SO3Group<Scalar> & other,
492 |                             Scalar * theta) {
493 |     const Scalar squared_n
494 |         = other.unit_quaternion().vec().squaredNorm();
495 |     const Scalar n = std::sqrt(squared_n);
496 |     const Scalar w = other.unit_quaternion().w();
497 | 
498 |     Scalar two_atan_nbyw_by_n;
499 | 
500 |     // Atan-based log thanks to
501 |     //
502 |     // C. Hertzberg et al.:
503 |     // "Integrating Generic Sensor Fusion Algorithms with Sound State
504 |     // Representation through Encapsulation of Manifolds"
505 |     // Information Fusion, 2011
506 | 
507 |     if (n < SophusConstants<Scalar>::epsilon()) {
508 |       // If quaternion is normalized and n=0, then w should be 1;
509 |       // w=0 should never happen here!
510 |       if (std::abs(w) < SophusConstants<Scalar>::epsilon()) {
511 |         throw SophusException("Quaternion is not normalized!");
512 |       }
513 |       const Scalar squared_w = w*w;
514 |       two_atan_nbyw_by_n = static_cast<Scalar>(2) / w
515 |                            - static_cast<Scalar>(2)*(squared_n)/(w*squared_w);
516 |     } else {
517 |       if (std::abs(w)<SophusConstants<Scalar>::epsilon()) {
518 |         if (w > static_cast<Scalar>(0)) {
519 |           two_atan_nbyw_by_n = M_PI/n;
520 |         } else {
521 |           two_atan_nbyw_by_n = -M_PI/n;
522 |         }
523 |       }else{
524 |         two_atan_nbyw_by_n = static_cast<Scalar>(2) * atan(n/w) / n;
525 |       }
526 |     }
527 | 
528 |     *theta = two_atan_nbyw_by_n*n;
529 | 
530 |     return two_atan_nbyw_by_n * other.unit_quaternion().vec();
531 |   }
532 | 
533 |   /**
534 |    * \brief vee-operator
535 |    *
536 |    * \param Omega 3x3-matrix representation of Lie algebra element
537 |    * \pr          Omega must be a skew-symmetric matrix
538 |    * \returns     3-vector representatin of Lie algebra element
539 |    *
540 |    * This is the inverse of the hat()-operator.
541 |    *
542 |    * \see hat()
543 |    */
544 |   inline static
545 |   const Tangent vee(const Transformation & Omega) {
546 |     return static_cast<Scalar>(0.5) * Tangent(Omega(2,1) - Omega(1,2),
547 |                                               Omega(0,2) - Omega(2,0),
548 |                                               Omega(1,0) - Omega(0,1));
549 |   }
550 | 
551 | private:
552 |   // Mutator of unit_quaternion is private so users are hampered
553 |   // from setting non-unit quaternions.
554 |   EIGEN_STRONG_INLINE
555 |   QuaternionReference unit_quaternion_nonconst() {
556 |     return static_cast<Derived*>(this)->unit_quaternion_nonconst();
557 |   }
558 | 
559 | };
560 | 
561 | /**
562 |  * \brief SO3 default type - Constructors and default storage for SO3 Type
563 |  */
564 | template<typename _Scalar, int _Options>
565 | class SO3Group : public SO3GroupBase<SO3Group<_Scalar,_Options> > {
566 |   typedef SO3GroupBase<SO3Group<_Scalar,_Options> > Base;
567 | public:
568 |   /** \brief scalar type */
569 |   typedef typename internal::traits<SO3Group<_Scalar,_Options> >
570 |   ::Scalar Scalar;
571 |   /** \brief quaternion type */
572 |   typedef typename internal::traits<SO3Group<_Scalar,_Options> >
573 |   ::QuaternionType & QuaternionReference;
574 |   typedef const typename internal::traits<SO3Group<_Scalar,_Options> >
575 |   ::QuaternionType & ConstQuaternionReference;
576 | 
577 |   /** \brief degree of freedom of group */
578 |   static const int DoF = Base::DoF;
579 |   /** \brief number of internal parameters used */
580 |   static const int num_parameters = Base::num_parameters;
581 |   /** \brief group transformations are NxN matrices */
582 |   static const int N = Base::N;
583 |   /** \brief group transfomation type */
584 |   typedef typename Base::Transformation Transformation;
585 |   /** \brief point type */
586 |   typedef typename Base::Point Point;
587 |   /** \brief tangent vector type */
588 |   typedef typename Base::Tangent Tangent;
589 |   /** \brief adjoint transformation type */
590 |   typedef typename Base::Adjoint Adjoint;
591 | 
592 |   // base is friend so unit_quaternion_nonconst can be accessed from base
593 |   friend class SO3GroupBase<SO3Group<_Scalar,_Options> >;
594 | 
595 |   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
596 | 
597 |   /**
598 |    * \brief Default constructor
599 |    *
600 |    * Initialize Quaternion to identity rotation.
601 |    */
602 |   inline
603 |   SO3Group()
604 |     : unit_quaternion_(static_cast<Scalar>(1), static_cast<Scalar>(0),
605 |                        static_cast<Scalar>(0), static_cast<Scalar>(0)) {
606 |   }
607 | 
608 |   /**
609 |    * \brief Copy constructor
610 |    */
611 |   template<typename OtherDerived> inline
612 |   SO3Group(const SO3GroupBase<OtherDerived> & other)
613 |     : unit_quaternion_(other.unit_quaternion()) {
614 |   }
615 | 
616 |   /**
617 |    * \brief Constructor from rotation matrix
618 |    *
619 |    * \pre rotation matrix need to be orthogonal with determinant of 1
620 |    */
621 |   inline SO3Group(const Transformation & R)
622 |     : unit_quaternion_(R) {
623 |   }
624 | 
625 |   /**
626 |    * \brief Constructor from quaternion
627 |    *
628 |    * \pre quaternion must not be zero
629 |    */
630 |   inline explicit
631 |   SO3Group(const Quaternion<Scalar> & quat) : unit_quaternion_(quat) {
632 |     Base::normalize();
633 |   }
634 | 
635 |   /**
636 |    * \brief Constructor from Euler angles
637 |    *
638 |    * \param alpha1 rotation around x-axis
639 |    * \param alpha2 rotation around y-axis
640 |    * \param alpha3 rotation around z-axis
641 |    *
642 |    * Since rotations in 3D do not commute, the order of the individual rotations
643 |    * matter. Here, the following convention is used. We calculate a SO3 member
644 |    * corresponding to the rotation matrix \f$ R \f$ such
645 |    * that \f$ R=\exp\left(\begin{array}{c}\alpha_1\\ 0\\ 0\end{array}\right)
646 |    *    \cdot   \exp\left(\begin{array}{c}0\\ \alpha_2\\ 0\end{array}\right)
647 |    *    \cdot   \exp\left(\begin{array}{c}0\\ 0\\ \alpha_3\end{array}\right)\f$.
648 |    */
649 |   inline
650 |   SO3Group(Scalar alpha1, Scalar alpha2, Scalar alpha3) {
651 |     const static Scalar zero = static_cast<Scalar>(0);
652 |     unit_quaternion_
653 |         = ( SO3Group::exp(Tangent(alpha1,   zero,   zero))
654 |             *SO3Group::exp(Tangent(  zero, alpha2,   zero))
655 |             *SO3Group::exp(Tangent(  zero,   zero, alpha3)) )
656 |           .unit_quaternion_;
657 |   }
658 | 
659 |   /**
660 |    * \brief Accessor of unit quaternion
661 |    *
662 |    * No direct write access is given to ensure the quaternion stays normalized.
663 |    */
664 |   EIGEN_STRONG_INLINE
665 |   ConstQuaternionReference unit_quaternion() const {
666 |     return unit_quaternion_;
667 |   }
668 | 
669 | protected:
670 |   // Mutator of unit_quaternion is protected so users are hampered
671 |   // from setting non-unit quaternions.
672 |   EIGEN_STRONG_INLINE
673 |   QuaternionReference unit_quaternion_nonconst() {
674 |     return unit_quaternion_;
675 |   }
676 | 
677 |   Quaternion<Scalar> unit_quaternion_;
678 | };
679 | 
680 | } // end namespace
681 | 
682 | 
683 | namespace Eigen {
684 | /**
685 |  * \brief Specialisation of Eigen::Map for SO3GroupBase
686 |  *
687 |  * Allows us to wrap SO3 Objects around POD array
688 |  * (e.g. external c style quaternion)
689 |  */
690 | template<typename _Scalar, int _Options>
691 | class Map<Sophus::SO3Group<_Scalar>, _Options>
692 |     : public Sophus::SO3GroupBase<Map<Sophus::SO3Group<_Scalar>, _Options> > {
693 |   typedef Sophus::SO3GroupBase<Map<Sophus::SO3Group<_Scalar>, _Options> > Base;
694 | 
695 | public:
696 |   /** \brief scalar type */
697 |   typedef typename internal::traits<Map>::Scalar Scalar;
698 |   /** \brief quaternion reference type */
699 |   typedef typename internal::traits<Map>::QuaternionType &
700 |   QuaternionReference;
701 |   /** \brief quaternion const reference type */
702 |   typedef const typename internal::traits<Map>::QuaternionType &
703 |   ConstQuaternionReference;
704 | 
705 |   /** \brief degree of freedom of group */
706 |   static const int DoF = Base::DoF;
707 |   /** \brief number of internal parameters used */
708 |   static const int num_parameters = Base::num_parameters;
709 |   /** \brief group transformations are NxN matrices */
710 |   static const int N = Base::N;
711 |   /** \brief group transfomation type */
712 |   typedef typename Base::Transformation Transformation;
713 |   /** \brief point type */
714 |   typedef typename Base::Point Point;
715 |   /** \brief tangent vector type */
716 |   typedef typename Base::Tangent Tangent;
717 |   /** \brief adjoint transformation type */
718 |   typedef typename Base::Adjoint Adjoint;
719 | 
720 |   // base is friend so unit_quaternion_nonconst can be accessed from base
721 |   friend class Sophus::SO3GroupBase<Map<Sophus::SO3Group<_Scalar>, _Options> >;
722 | 
723 |   EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
724 |   using Base::operator*=;
725 |   using Base::operator*;
726 | 
727 |   EIGEN_STRONG_INLINE
728 |   Map(Scalar* coeffs) : unit_quaternion_(coeffs) {
729 |   }
730 | 
731 |   /**
732 |    * \brief Accessor of unit quaternion
733 |    *
734 |    * No direct write access is given to ensure the quaternion stays normalized.
735 |    */
736 |   EIGEN_STRONG_INLINE
737 |   ConstQuaternionReference unit_quaternion() const {
738 |     return unit_quaternion_;
739 |   }
740 | 
741 | protected:
742 |   // Mutator of unit_quaternion is protected so users are hampered
743 |   // from setting non-unit quaternions.
744 |   EIGEN_STRONG_INLINE
745 |   QuaternionReference unit_quaternion_nonconst() {
746 |     return unit_quaternion_;
747 |   }
748 | 
749 |   Map<Quaternion<Scalar>,_Options> unit_quaternion_;
750 | };
751 | 
752 | /**
753 |  * \brief Specialisation of Eigen::Map for const SO3GroupBase
754 |  *
755 |  * Allows us to wrap SO3 Objects around POD array
756 |  * (e.g. external c style quaternion)
757 |  */
758 | template<typename _Scalar, int _Options>
759 | class Map<const Sophus::SO3Group<_Scalar>, _Options>
760 |     : public Sophus::SO3GroupBase<
761 |     Map<const Sophus::SO3Group<_Scalar>, _Options> > {
762 |   typedef Sophus::SO3GroupBase<Map<const Sophus::SO3Group<_Scalar>, _Options> >
763 |   Base;
764 | 
765 | public:
766 |   /** \brief scalar type */
767 |   typedef typename internal::traits<Map>::Scalar Scalar;
768 |   /** \brief quaternion const reference type */
769 |   typedef const typename internal::traits<Map>::QuaternionType &
770 |   ConstQuaternionReference;
771 | 
772 |   /** \brief degree of freedom of group */
773 |   static const int DoF = Base::DoF;
774 |   /** \brief number of internal parameters used */
775 |   static const int num_parameters = Base::num_parameters;
776 |   /** \brief group transformations are NxN matrices */
777 |   static const int N = Base::N;
778 |   /** \brief group transfomation type */
779 |   typedef typename Base::Transformation Transformation;
780 |   /** \brief point type */
781 |   typedef typename Base::Point Point;
782 |   /** \brief tangent vector type */
783 |   typedef typename Base::Tangent Tangent;
784 |   /** \brief adjoint transformation type */
785 |   typedef typename Base::Adjoint Adjoint;
786 | 
787 |   EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
788 |   using Base::operator*=;
789 |   using Base::operator*;
790 | 
791 |   EIGEN_STRONG_INLINE
792 |   Map(const Scalar* coeffs) : unit_quaternion_(coeffs) {
793 |   }
794 | 
795 |   /**
796 |    * \brief Accessor of unit quaternion
797 |    *
798 |    * No direct write access is given to ensure the quaternion stays normalized.
799 |    */
800 |   EIGEN_STRONG_INLINE
801 |   const ConstQuaternionReference unit_quaternion() const {
802 |     return unit_quaternion_;
803 |   }
804 | 
805 | protected:
806 |   const Map<const Quaternion<Scalar>,_Options> unit_quaternion_;
807 | };
808 | 
809 | }
810 | 
811 | #endif
812 | 


--------------------------------------------------------------------------------
/include/lsd_slam_to_pcl/third_party/sophus/sim3.hpp:
--------------------------------------------------------------------------------
  1 | // This file is part of Sophus.
  2 | //
  3 | // Copyright 2012-2013 Hauke Strasdat
  4 | //
  5 | // Permission is hereby granted, free of charge, to any person obtaining a copy
  6 | // of this software and associated documentation files (the "Software"), to
  7 | // deal in the Software without restriction, including without limitation the
  8 | // rights  to use, copy, modify, merge, publish, distribute, sublicense, and/or
  9 | // sell copies of the Software, and to permit persons to whom the Software is
 10 | // furnished to do so, subject to the following conditions:
 11 | //
 12 | // The above copyright notice and this permission notice shall be included in
 13 | // all copies or substantial portions of the Software.
 14 | //
 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 20 | // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
 21 | // IN THE SOFTWARE.
 22 | 
 23 | #ifndef SOPHUS_SIM3_HPP
 24 | #define SOPHUS_SIM3_HPP
 25 | 
 26 | #include "rxso3.hpp"
 27 | 
 28 | ////////////////////////////////////////////////////////////////////////////
 29 | // Forward Declarations / typedefs
 30 | ////////////////////////////////////////////////////////////////////////////
 31 | 
 32 | namespace Sophus {
 33 | template<typename _Scalar, int _Options=0> class Sim3Group;
 34 | typedef Sim3Group<double> Sim3 EIGEN_DEPRECATED;
 35 | typedef Sim3Group<double> Sim3d; /**< double precision Sim3 */
 36 | typedef Sim3Group<float> Sim3f;  /**< single precision Sim3 */
 37 | typedef Matrix<double,7,1> Vector7d;
 38 | typedef Matrix<double,7,7> Matrix7d;
 39 | typedef Matrix<float,7,1> Vector7f;
 40 | typedef Matrix<float,7,7> Matrix7f;
 41 | }
 42 | 
 43 | ////////////////////////////////////////////////////////////////////////////
 44 | // Eigen Traits (For querying derived types in CRTP hierarchy)
 45 | ////////////////////////////////////////////////////////////////////////////
 46 | 
 47 | namespace Eigen {
 48 | namespace internal {
 49 | 
 50 | template<typename _Scalar, int _Options>
 51 | struct traits<Sophus::Sim3Group<_Scalar,_Options> > {
 52 |   typedef _Scalar Scalar;
 53 |   typedef Matrix<Scalar,3,1> TranslationType;
 54 |   typedef Sophus::RxSO3Group<Scalar> RxSO3Type;
 55 | };
 56 | 
 57 | template<typename _Scalar, int _Options>
 58 | struct traits<Map<Sophus::Sim3Group<_Scalar>, _Options> >
 59 |     : traits<Sophus::Sim3Group<_Scalar, _Options> > {
 60 |   typedef _Scalar Scalar;
 61 |   typedef Map<Matrix<Scalar,3,1>,_Options> TranslationType;
 62 |   typedef Map<Sophus::RxSO3Group<Scalar>,_Options> RxSO3Type;
 63 | };
 64 | 
 65 | template<typename _Scalar, int _Options>
 66 | struct traits<Map<const Sophus::Sim3Group<_Scalar>, _Options> >
 67 |     : traits<const Sophus::Sim3Group<_Scalar, _Options> > {
 68 |   typedef _Scalar Scalar;
 69 |   typedef Map<const Matrix<Scalar,3,1>,_Options> TranslationType;
 70 |   typedef Map<const Sophus::RxSO3Group<Scalar>,_Options> RxSO3Type;
 71 | };
 72 | 
 73 | }
 74 | }
 75 | 
 76 | namespace Sophus {
 77 | using namespace Eigen;
 78 | using namespace std;
 79 | 
 80 | /**
 81 |  * \brief Sim3 base type - implements Sim3 class but is storage agnostic
 82 |  *
 83 |  * [add more detailed description/tutorial]
 84 |  */
 85 | template<typename Derived>
 86 | class Sim3GroupBase {
 87 | public:
 88 |   /** \brief scalar type */
 89 |   typedef typename internal::traits<Derived>::Scalar Scalar;
 90 |   /** \brief translation reference type */
 91 |   typedef typename internal::traits<Derived>::TranslationType &
 92 |   TranslationReference;
 93 |   /** \brief translation const reference type */
 94 |   typedef const typename internal::traits<Derived>::TranslationType &
 95 |   ConstTranslationReference;
 96 |   /** \brief RxSO3 reference type */
 97 |   typedef typename internal::traits<Derived>::RxSO3Type &
 98 |   RxSO3Reference;
 99 |   /** \brief RxSO3 const reference type */
100 |   typedef const typename internal::traits<Derived>::RxSO3Type &
101 |   ConstRxSO3Reference;
102 | 
103 | 
104 |   /** \brief degree of freedom of group
105 |     *        (three for translation, three for rotation, one for scale) */
106 |   static const int DoF = 7;
107 |   /** \brief number of internal parameters used
108 |    *         (quaternion for rotation and scale + translation 3-vector) */
109 |   static const int num_parameters = 7;
110 |   /** \brief group transformations are NxN matrices */
111 |   static const int N = 4;
112 |   /** \brief group transfomation type */
113 |   typedef Matrix<Scalar,N,N> Transformation;
114 |   /** \brief point type */
115 |   typedef Matrix<Scalar,3,1> Point;
116 |   /** \brief tangent vector type */
117 |   typedef Matrix<Scalar,DoF,1> Tangent;
118 |   /** \brief adjoint transformation type */
119 |   typedef Matrix<Scalar,DoF,DoF> Adjoint;
120 | 
121 |   /**
122 |    * \brief Adjoint transformation
123 |    *
124 |    * This function return the adjoint transformation \f$ Ad \f$ of the
125 |    * group instance \f$ A \f$  such that for all \f$ x \f$
126 |    * it holds that \f$ \widehat{Ad_A\cdot x} = A\widehat{x}A^{-1} \f$
127 |    * with \f$\ \widehat{\cdot} \f$ being the hat()-operator.
128 |    */
129 |   inline
130 |   const Adjoint Adj() const {
131 |     const Matrix<Scalar,3,3> & R = rxso3().rotationMatrix();
132 |     Adjoint res;
133 |     res.setZero();
134 |     res.block(0,0,3,3) = scale()*R;
135 |     res.block(0,3,3,3) = SO3Group<Scalar>::hat(translation())*R;
136 |     res.block(0,6,3,1) = -translation();
137 |     res.block(3,3,3,3) = R;
138 |     res(6,6) = 1;
139 |     return res;
140 |   }
141 | 
142 |   /**
143 |    * \returns copy of instance casted to NewScalarType
144 |    */
145 |   template<typename NewScalarType>
146 |   inline Sim3Group<NewScalarType> cast() const {
147 |     return
148 |         Sim3Group<NewScalarType>(rxso3().template cast<NewScalarType>(),
149 |                                  translation().template cast<NewScalarType>() );
150 |   }
151 | 
152 |   /**
153 |    * \brief In-place group multiplication
154 |    *
155 |    * Same as operator*=() for Sim3.
156 |    *
157 |    * \see operator*()
158 |    */
159 |   inline
160 |   void fastMultiply(const Sim3Group<Scalar>& other) {
161 |     translation() += (rxso3() * other.translation());
162 |     rxso3() *= other.rxso3();
163 |   }
164 | 
165 |   /**
166 |    * \returns Group inverse of instance
167 |    */
168 |   inline
169 |   const Sim3Group<Scalar> inverse() const {
170 |     const RxSO3Group<Scalar> invR = rxso3().inverse();
171 |     return Sim3Group<Scalar>(invR, invR*(translation()
172 |                                          *static_cast<Scalar>(-1) ) );
173 |   }
174 | 
175 |   /**
176 |    * \brief Logarithmic map
177 |    *
178 |    * \returns tangent space representation
179 |    *          (translational part and rotation vector) of instance
180 |    *
181 |    * \see  log().
182 |    */
183 |   inline
184 |   const Tangent log() const {
185 |     return log(*this);
186 |   }
187 | 
188 |   /**
189 |    * \returns 4x4 matrix representation of instance
190 |    */
191 |   inline
192 |   const Transformation matrix() const {
193 |     Transformation homogenious_matrix;
194 |     homogenious_matrix.setIdentity();
195 |     homogenious_matrix.block(0,0,3,3) = rxso3().matrix();
196 |     homogenious_matrix.col(3).head(3) = translation();
197 |     return homogenious_matrix;
198 |   }
199 | 
200 |   /**
201 |    * \returns 3x4 matrix representation of instance
202 |    *
203 |    * It returns the three first row of matrix().
204 |    */
205 |   inline
206 |   const Matrix<Scalar,3,4> matrix3x4() const {
207 |     Matrix<Scalar,3,4> matrix;
208 |     matrix.block(0,0,3,3) = rxso3().matrix();
209 |     matrix.col(3) = translation();
210 |     return matrix;
211 |   }
212 | 
213 |   /**
214 |    * \brief Assignment operator
215 |    */
216 |   template<typename OtherDerived> inline
217 |   Sim3GroupBase<Derived>& operator=
218 |   (const Sim3GroupBase<OtherDerived> & other) {
219 |     rxso3() = other.rxso3();
220 |     translation() = other.translation();
221 |     return *this;
222 |   }
223 | 
224 |   /**
225 |    * \brief Group multiplication
226 |    * \see operator*=()
227 |    */
228 |   inline
229 |   const Sim3Group<Scalar> operator*(const Sim3Group<Scalar>& other) const {
230 |     Sim3Group<Scalar> result(*this);
231 |     result *= other;
232 |     return result;
233 |   }
234 | 
235 |   /**
236 |    * \brief Group action on \f$ \mathbf{R}^3 \f$
237 |    *
238 |    * \param p point \f$p \in \mathbf{R}^3 \f$
239 |    * \returns point \f$p' \in \mathbf{R}^3 \f$,
240 |    *          rotated, scaled and translated version of \f$p\f$
241 |    *
242 |    * This function scales, rotates and translates point \f$ p \f$
243 |    * in \f$ \mathbf{R}^3 \f$ by the Sim3 transformation \f$sR,t\f$
244 |    * (=scaled rotation matrix, translation vector): \f$ p' = sR\cdot p + t \f$.
245 |    */
246 |   inline
247 |   const Point operator*(const Point & p) const {
248 |     return rxso3()*p + translation();
249 |   }
250 | 
251 |   /**
252 |    * \brief In-place group multiplication
253 |    *
254 |    * \see operator*()
255 |    */
256 |   inline
257 |   void operator*=(const Sim3Group<Scalar>& other) {
258 |     translation() += (rxso3() * other.translation());
259 |     rxso3() *= other.rxso3();
260 |   }
261 | 
262 |   /**
263 |    * \brief Mutator of quaternion
264 |    */
265 |   inline
266 |   typename internal::traits<Derived>::RxSO3Type::QuaternionReference
267 |   quaternion() {
268 |     return rxso3().quaternion();
269 |   }
270 | 
271 |   /**
272 |    * \brief Accessor of quaternion
273 |    */
274 |   inline
275 |   typename internal::traits<Derived>::RxSO3Type::ConstQuaternionReference
276 |   quaternion() const {
277 |     return rxso3().quaternion();
278 |   }
279 | 
280 |   /**
281 |    * \returns Rotation matrix
282 |    *
283 |    * deprecated: use rotationMatrix() instead.
284 |    */
285 |   inline
286 |   EIGEN_DEPRECATED const Transformation rotation_matrix() const {
287 |     return rxso3().rotationMatrix();
288 |   }
289 | 
290 |   /**
291 |    * \returns Rotation matrix
292 |    */
293 |   inline
294 |   const Matrix<Scalar,3,3> rotationMatrix() const {
295 |     return rxso3().rotationMatrix();
296 |   }
297 | 
298 |   /**
299 |    * \brief Mutator of RxSO3 group
300 |    */
301 |   EIGEN_STRONG_INLINE
302 |   RxSO3Reference rxso3() {
303 |     return static_cast<Derived*>(this)->rxso3();
304 |   }
305 | 
306 |   /**
307 |    * \brief Accessor of RxSO3 group
308 |    */
309 |   EIGEN_STRONG_INLINE
310 |   ConstRxSO3Reference rxso3() const {
311 |     return static_cast<const Derived*>(this)->rxso3();
312 |   }
313 | 
314 |   /**
315 |    * \returns scale
316 |    */
317 |   EIGEN_STRONG_INLINE
318 |   const Scalar scale() const {
319 |     return rxso3().scale();
320 |   }
321 | 
322 |   /**
323 |    * \brief Setter of quaternion using rotation matrix, leaves scale untouched
324 |    *
325 |    * \param R a 3x3 rotation matrix
326 |    * \pre       the 3x3 matrix should be orthogonal and have a determinant of 1
327 |    */
328 |   inline
329 |   void setRotationMatrix
330 |   (const Matrix<Scalar,3,3> & R) {
331 |     rxso3().setRotationMatrix(R);
332 |   }
333 | 
334 |   /**
335 |    * \brief Scale setter
336 |    */
337 |   EIGEN_STRONG_INLINE
338 |   void setScale(const Scalar & scale) const {
339 |     rxso3().setScale(scale);
340 |   }
341 | 
342 |   /**
343 |    * \brief Setter of quaternion using scaled rotation matrix
344 |    *
345 |    * \param sR a 3x3 scaled rotation matrix
346 |    * \pre        the 3x3 matrix should be "scaled orthogonal"
347 |    *             and have a positive determinant
348 |    */
349 |   inline
350 |   void setScaledRotationMatrix
351 |   (const Matrix<Scalar,3,3> & sR) {
352 |     rxso3().setScaledRotationMatrix(sR);
353 |   }
354 | 
355 |   /**
356 |    * \brief Mutator of translation vector
357 |    */
358 |   EIGEN_STRONG_INLINE
359 |   TranslationReference translation() {
360 |     return static_cast<Derived*>(this)->translation();
361 |   }
362 | 
363 |   /**
364 |    * \brief Accessor of translation vector
365 |    */
366 |   EIGEN_STRONG_INLINE
367 |   ConstTranslationReference translation() const {
368 |     return static_cast<const Derived*>(this)->translation();
369 |   }
370 | 
371 |   ////////////////////////////////////////////////////////////////////////////
372 |   // public static functions
373 |   ////////////////////////////////////////////////////////////////////////////
374 | 
375 |   /**
376 |    * \param   b 7-vector representation of Lie algebra element
377 |    * \returns   derivative of Lie bracket
378 |    *
379 |    * This function returns \f$ \frac{\partial}{\partial a} [a, b]_{sim3} \f$
380 |    * with \f$ [a, b]_{sim3} \f$ being the lieBracket() of the Lie algebra sim3.
381 |    *
382 |    * \see lieBracket()
383 |    */
384 |   inline static
385 |   const Adjoint d_lieBracketab_by_d_a(const Tangent & b) {
386 |     const Matrix<Scalar,3,1> & upsilon2 = b.template head<3>();
387 |     const Matrix<Scalar,3,1> & omega2 = b.template segment<3>(3);
388 |     Scalar sigma2 = b[6];
389 | 
390 |     Adjoint res;
391 |     res.setZero();
392 |     res.template topLeftCorner<3,3>()
393 |         = -SO3::hat(omega2)-sigma2*Matrix3d::Identity();
394 |     res.template block<3,3>(0,3) = -SO3::hat(upsilon2);
395 |     res.template topRightCorner<3,1>() = upsilon2;
396 |     res.template block<3,3>(3,3) = -SO3::hat(omega2);
397 |     return res;
398 |   }
399 | 
400 |   /**
401 |    * \brief Group exponential
402 |    *
403 |    * \param a tangent space element (7-vector)
404 |    * \returns corresponding element of the group Sim3
405 |    *
406 |    * The first three components of \f$ a \f$ represent the translational
407 |    * part \f$ \upsilon \f$ in the tangent space of Sim3, while the last three
408 |    * components of \f$ a \f$ represents the rotation vector \f$ \omega \f$.
409 |    *
410 |    * To be more specific, this function computes \f$ \exp(\widehat{a}) \f$
411 |    * with \f$ \exp(\cdot) \f$ being the matrix exponential
412 |    * and \f$ \widehat{\cdot} \f$ the hat()-operator of Sim3.
413 |    *
414 |    * \see hat()
415 |    * \see log()
416 |    */
417 |   inline static
418 |   const Sim3Group<Scalar> exp(const Tangent & a) {
419 |     const Matrix<Scalar,3,1> & upsilon = a.segment(0,3);
420 |     const Matrix<Scalar,3,1> & omega = a.segment(3,3);
421 |     Scalar sigma = a[6];
422 |     Scalar theta;
423 |     RxSO3Group<Scalar> rxso3
424 |         = RxSO3Group<Scalar>::expAndTheta(a.template tail<4>(), &theta);
425 |     const Matrix<Scalar,3,3> & Omega = SO3Group<Scalar>::hat(omega);
426 |     const Matrix<Scalar,3,3> & W = calcW(theta, sigma, rxso3.scale(), Omega);
427 |     return Sim3Group<Scalar>(rxso3, W*upsilon);
428 |   }
429 | 
430 |   /**
431 |    * \brief Generators
432 |    *
433 |    * \pre \f$ i \in \{0,1,2,3,4,5,6\} \f$
434 |    * \returns \f$ i \f$th generator \f$ G_i \f$ of Sim3
435 |    *
436 |    * The infinitesimal generators of Sim3 are: \f[
437 |    *        G_0 = \left( \begin{array}{cccc}
438 |    *                          0&  0&  0&  1\\
439 |    *                          0&  0&  0&  0\\
440 |    *                          0&  0&  0&  0\\
441 |    *                          0&  0&  0&  0\\
442 |    *                     \end{array} \right),
443 |    *        G_1 = \left( \begin{array}{cccc}
444 |    *                          0&  0&  0&  0\\
445 |    *                          0&  0&  0&  1\\
446 |    *                          0&  0&  0&  0\\
447 |    *                          0&  0&  0&  0\\
448 |    *                     \end{array} \right),
449 |    *        G_2 = \left( \begin{array}{cccc}
450 |    *                          0&  0&  0&  0\\
451 |    *                          0&  0&  0&  0\\
452 |    *                          0&  0&  0&  1\\
453 |    *                          0&  0&  0&  0\\
454 |    *                     \end{array} \right).
455 |    *        G_3 = \left( \begin{array}{cccc}
456 |    *                          0&  0&  0&  0\\
457 |    *                          0&  0& -1&  0\\
458 |    *                          0&  1&  0&  0\\
459 |    *                          0&  0&  0&  0\\
460 |    *                     \end{array} \right),
461 |    *        G_4 = \left( \begin{array}{cccc}
462 |    *                          0&  0&  1&  0\\
463 |    *                          0&  0&  0&  0\\
464 |    *                         -1&  0&  0&  0\\
465 |    *                          0&  0&  0&  0\\
466 |    *                     \end{array} \right),
467 |    *        G_5 = \left( \begin{array}{cccc}
468 |    *                          0& -1&  0&  0\\
469 |    *                          1&  0&  0&  0\\
470 |    *                          0&  0&  0&  0\\
471 |    *                          0&  0&  0&  0\\
472 |    *                     \end{array} \right),
473 |    *        G_6 = \left( \begin{array}{cccc}
474 |    *                          1&  0&  0&  0\\
475 |    *                          0&  1&  0&  0\\
476 |    *                          0&  0&  1&  0\\
477 |    *                          0&  0&  0&  0\\
478 |    *                     \end{array} \right).
479 |    * \f]
480 |    * \see hat()
481 |    */
482 |   inline static
483 |   const Transformation generator(int i) {
484 |     if (i<0 || i>6) {
485 |       throw SophusException("i is not in range [0,6].");
486 |     }
487 |     Tangent e;
488 |     e.setZero();
489 |     e[i] = static_cast<Scalar>(1);
490 |     return hat(e);
491 |   }
492 | 
493 |   /**
494 |    * \brief hat-operator
495 |    *
496 |    * \param omega 7-vector representation of Lie algebra element
497 |    * \returns     4x4-matrix representatin of Lie algebra element
498 |    *
499 |    * Formally, the hat-operator of Sim3 is defined
500 |    * as \f$ \widehat{\cdot}: \mathbf{R}^7 \rightarrow \mathbf{R}^{4\times 4},
501 |    * \quad \widehat{\omega} = \sum_{i=0}^5 G_i \omega_i \f$
502 |    * with \f$ G_i \f$ being the ith infinitesial generator().
503 |    *
504 |    * \see generator()
505 |    * \see vee()
506 |    */
507 |   inline static
508 |   const Transformation hat(const Tangent & v) {
509 |     Transformation Omega;
510 |     Omega.template topLeftCorner<3,3>()
511 |         = RxSO3Group<Scalar>::hat(v.template tail<4>());
512 |     Omega.col(3).template head<3>() = v.template head<3>();
513 |     Omega.row(3).setZero();
514 |     return Omega;
515 |   }
516 | 
517 |   /**
518 |    * \brief Lie bracket
519 |    *
520 |    * \param a 7-vector representation of Lie algebra element
521 |    * \param b 7-vector representation of Lie algebra element
522 |    * \returns 7-vector representation of Lie algebra element
523 |    *
524 |    * It computes the bracket of Sim3. To be more specific, it
525 |    * computes \f$ [a, b]_{sim3}
526 |    * := [\widehat{a}, \widehat{b}]^\vee \f$
527 |    * with \f$ [A,B] = AB-BA \f$ being the matrix
528 |    * commutator, \f$ \widehat{\cdot} \f$ the
529 |    * hat()-operator and \f$ (\cdot)^\vee \f$ the vee()-operator of Sim3.
530 |    *
531 |    * \see hat()
532 |    * \see vee()
533 |    */
534 |   inline static
535 |   const Tangent lieBracket(const Tangent & a,
536 |                            const Tangent & b) {
537 |     const Matrix<Scalar,3,1> & upsilon1 = a.template head<3>();
538 |     const Matrix<Scalar,3,1> & upsilon2 = b.template head<3>();
539 |     const Matrix<Scalar,3,1> & omega1 = a.template segment<3>(3);
540 |     const Matrix<Scalar,3,1> & omega2 = b.template segment<3>(3);
541 |     Scalar sigma1 = a[6];
542 |     Scalar sigma2 = b[6];
543 | 
544 |     Tangent res;
545 |     res.template head<3>() =
546 |         SO3Group<Scalar>::hat(omega1)*upsilon2
547 |         + SO3Group<Scalar>::hat(upsilon1)*omega2
548 |         + sigma1*upsilon2 - sigma2*upsilon1;
549 |     res.template segment<3>(3) = omega1.cross(omega2);
550 |     res[6] = static_cast<Scalar>(0);
551 | 
552 |     return res;
553 |   }
554 | 
555 |   /**
556 |    * \brief Logarithmic map
557 |    *
558 |    * \param other element of the group Sim3
559 |    * \returns     corresponding tangent space element
560 |    *              (translational part \f$ \upsilon \f$
561 |    *               and rotation vector \f$ \omega \f$)
562 |    *
563 |    * Computes the logarithmic, the inverse of the group exponential.
564 |    * To be specific, this function computes \f$ \log({\cdot})^\vee \f$
565 |    * with \f$ \vee(\cdot) \f$ being the matrix logarithm
566 |    * and \f$ \vee{\cdot} \f$ the vee()-operator of Sim3.
567 |    *
568 |    * \see exp()
569 |    * \see vee()
570 |    */
571 |   inline static
572 |   const Tangent log(const Sim3Group<Scalar> & other) {
573 |     Tangent res;
574 |     Scalar theta;
575 |     const Matrix<Scalar,4,1> & omega_sigma
576 |         = RxSO3Group<Scalar>::logAndTheta(other.rxso3(), &theta);
577 |     const Matrix<Scalar,3,1> & omega = omega_sigma.template head<3>();
578 |     Scalar sigma = omega_sigma[3];
579 |     const Matrix<Scalar,3,3> & W
580 |         = calcW(theta, sigma, other.scale(), SO3Group<Scalar>::hat(omega));
581 |     res.segment(0,3) = W.partialPivLu().solve(other.translation());
582 |     res.segment(3,3) = omega;
583 |     res[6] = sigma;
584 |     return res;
585 |   }
586 | 
587 |   /**
588 |    * \brief vee-operator
589 |    *
590 |    * \param Omega 4x4-matrix representation of Lie algebra element
591 |    * \returns     7-vector representatin of Lie algebra element
592 |    *
593 |    * This is the inverse of the hat()-operator.
594 |    *
595 |    * \see hat()
596 |    */
597 |   inline static
598 |   const Tangent vee(const Transformation & Omega) {
599 |     Tangent upsilon_omega_sigma;
600 |     upsilon_omega_sigma.template head<3>()
601 |         = Omega.col(3).template head<3>();
602 |     upsilon_omega_sigma.template tail<4>()
603 |         = RxSO3Group<Scalar>::vee(Omega.template topLeftCorner<3,3>());
604 |     return upsilon_omega_sigma;
605 |   }
606 | 
607 | private:
608 |   static
609 |   Matrix<Scalar,3,3> calcW(const Scalar & theta,
610 |                            const Scalar & sigma,
611 |                            const Scalar & scale,
612 |                            const Matrix<Scalar,3,3> & Omega){
613 |     static const Matrix<Scalar,3,3> I
614 |         = Matrix<Scalar,3,3>::Identity();
615 |     static const Scalar one = static_cast<Scalar>(1.);
616 |     static const Scalar half = static_cast<Scalar>(1./2.);
617 |     Matrix<Scalar,3,3> Omega2 = Omega*Omega;
618 | 
619 |     Scalar A,B,C;
620 |     if (std::abs(sigma)<SophusConstants<Scalar>::epsilon()) {
621 |       C = one;
622 |       if (std::abs(theta)<SophusConstants<Scalar>::epsilon()) {
623 |         A = half;
624 |         B = static_cast<Scalar>(1./6.);
625 |       } else {
626 |         Scalar theta_sq = theta*theta;
627 |         A = (one-std::cos(theta))/theta_sq;
628 |         B = (theta-std::sin(theta))/(theta_sq*theta);
629 |       }
630 |     } else {
631 |       C = (scale-one)/sigma;
632 |       if (std::abs(theta)<SophusConstants<Scalar>::epsilon()) {
633 |         Scalar sigma_sq = sigma*sigma;
634 |         A = ((sigma-one)*scale+one)/sigma_sq;
635 |         B = ((half*sigma*sigma-sigma+one)*scale)/(sigma_sq*sigma);
636 |       } else {
637 |         Scalar theta_sq = theta*theta;
638 |         Scalar a = scale*std::sin(theta);
639 |         Scalar b = scale*std::cos(theta);
640 |         Scalar c = theta_sq+sigma*sigma;
641 |         A = (a*sigma+ (one-b)*theta)/(theta*c);
642 |         B = (C-((b-one)*sigma+a*theta)/(c))*one/(theta_sq);
643 |       }
644 |     }
645 |     return A*Omega + B*Omega2 + C*I;
646 |   }
647 | };
648 | 
649 | /**
650 |  * \brief Sim3 default type - Constructors and default storage for Sim3 Type
651 |  */
652 | template<typename _Scalar, int _Options>
653 | class Sim3Group : public Sim3GroupBase<Sim3Group<_Scalar,_Options> > {
654 |   typedef Sim3GroupBase<Sim3Group<_Scalar,_Options> > Base;
655 | public:
656 |   /** \brief scalar type */
657 |   typedef typename internal::traits<Sim3Group<_Scalar,_Options> >
658 |   ::Scalar Scalar;
659 |   /** \brief RxSO3 reference type */
660 |   typedef typename internal::traits<Sim3Group<_Scalar,_Options> >
661 |   ::RxSO3Type & RxSO3Reference;
662 |   /** \brief RxSO3 const reference type */
663 |   typedef const typename internal::traits<Sim3Group<_Scalar,_Options> >
664 |   ::RxSO3Type & ConstRxSO3Reference;
665 |   /** \brief translation reference type */
666 |   typedef typename internal::traits<Sim3Group<_Scalar,_Options> >
667 |   ::TranslationType & TranslationReference;
668 |   /** \brief translation const reference type */
669 |   typedef const typename internal::traits<Sim3Group<_Scalar,_Options> >
670 |   ::TranslationType & ConstTranslationReference;
671 | 
672 |   /** \brief degree of freedom of group */
673 |   static const int DoF = Base::DoF;
674 |   /** \brief number of internal parameters used */
675 |   static const int num_parameters = Base::num_parameters;
676 |   /** \brief group transformations are NxN matrices */
677 |   static const int N = Base::N;
678 |   /** \brief group transfomation type */
679 |   typedef typename Base::Transformation Transformation;
680 |   /** \brief point type */
681 |   typedef typename Base::Point Point;
682 |   /** \brief tangent vector type */
683 |   typedef typename Base::Tangent Tangent;
684 |   /** \brief adjoint transformation type */
685 |   typedef typename Base::Adjoint Adjoint;
686 | 
687 | 
688 |   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
689 | 
690 |   /**
691 |    * \brief Default constructor
692 |    *
693 |    * Initialize Quaternion to identity rotation and translation to zero.
694 |    */
695 |   inline
696 |   Sim3Group()
697 |     : translation_( Matrix<Scalar,3,1>::Zero() )
698 |   {
699 |   }
700 | 
701 |   /**
702 |    * \brief Copy constructor
703 |    */
704 |   template<typename OtherDerived> inline
705 |   Sim3Group(const Sim3GroupBase<OtherDerived> & other)
706 |     : rxso3_(other.rxso3()), translation_(other.translation()) {
707 |   }
708 | 
709 |   /**
710 |    * \brief Constructor from RxSO3 and translation vector
711 |    */
712 |   template<typename OtherDerived> inline
713 |   Sim3Group(const RxSO3GroupBase<OtherDerived> & rxso3,
714 |             const Point & translation)
715 |     : rxso3_(rxso3), translation_(translation) {
716 |   }
717 | 
718 |   /**
719 |    * \brief Constructor from quaternion and translation vector
720 |    *
721 |    * \pre quaternion must not be zero
722 |    */
723 |   inline
724 |   Sim3Group(const Quaternion<Scalar> & quaternion,
725 |             const Point & translation)
726 |     : rxso3_(quaternion), translation_(translation) {
727 |   }
728 | 
729 |   /**
730 |    * \brief Constructor from 4x4 matrix
731 |    *
732 |    * \pre top-left 3x3 sub-matrix need to be "scaled orthogonal"
733 |    *      with positive determinant of
734 |    */
735 |   inline explicit
736 |   Sim3Group(const Eigen::Matrix<Scalar,4,4>& T)
737 |     : rxso3_(T.template topLeftCorner<3,3>()),
738 |       translation_(T.template block<3,1>(0,3)) {
739 |   }
740 | 
741 |   /**
742 |    * \returns pointer to internal data
743 |    *
744 |    * This provides unsafe read/write access to internal data. Sim3 is
745 |    * represented by a pair of an RxSO3 element (4 parameters) and translation
746 |    * vector (three parameters).
747 |    *
748 |    * Note: The first three Scalars represent the imaginary parts, while the
749 |    */
750 |   EIGEN_STRONG_INLINE
751 |   Scalar* data() {
752 |     // rxso3_ and translation_ are layed out sequentially with no padding
753 |     return rxso3_.data();
754 |   }
755 | 
756 |   /**
757 |    * \returns const pointer to internal data
758 |    *
759 |    * Const version of data().
760 |    */
761 |   EIGEN_STRONG_INLINE
762 |   const Scalar* data() const {
763 |     // rxso3_ and translation_ are layed out sequentially with no padding
764 |     return rxso3_.data();
765 |   }
766 | 
767 |   /**
768 |    * \brief Accessor of RxSO3
769 |    */
770 |   EIGEN_STRONG_INLINE
771 |   RxSO3Reference rxso3() {
772 |     return rxso3_;
773 |   }
774 | 
775 |   /**
776 |    * \brief Mutator of RxSO3
777 |    */
778 |   EIGEN_STRONG_INLINE
779 |   ConstRxSO3Reference rxso3() const {
780 |     return rxso3_;
781 |   }
782 | 
783 |   /**
784 |    * \brief Mutator of translation vector
785 |    */
786 |   EIGEN_STRONG_INLINE
787 |   TranslationReference translation() {
788 |     return translation_;
789 |   }
790 | 
791 |   /**
792 |    * \brief Accessor of translation vector
793 |    */
794 |   EIGEN_STRONG_INLINE
795 |   ConstTranslationReference translation() const {
796 |     return translation_;
797 |   }
798 | 
799 | protected:
800 |   Sophus::RxSO3Group<Scalar> rxso3_;
801 |   Matrix<Scalar,3,1> translation_;
802 | };
803 | 
804 | 
805 | } // end namespace
806 | 
807 | 
808 | namespace Eigen {
809 | /**
810 |  * \brief Specialisation of Eigen::Map for Sim3GroupBase
811 |  *
812 |  * Allows us to wrap Sim3 Objects around POD array
813 |  * (e.g. external c style quaternion)
814 |  */
815 | template<typename _Scalar, int _Options>
816 | class Map<Sophus::Sim3Group<_Scalar>, _Options>
817 |     : public Sophus::Sim3GroupBase<Map<Sophus::Sim3Group<_Scalar>, _Options> > {
818 |   typedef Sophus::Sim3GroupBase<Map<Sophus::Sim3Group<_Scalar>, _Options> >
819 |   Base;
820 | 
821 | public:
822 |   /** \brief scalar type */
823 |   typedef typename internal::traits<Map>::Scalar Scalar;
824 |   /** \brief translation reference type */
825 |   typedef typename internal::traits<Map>::TranslationType &
826 |   TranslationReference;
827 |   /** \brief translation const reference type */
828 |   typedef const typename internal::traits<Map>::TranslationType &
829 |   ConstTranslationReference;
830 |   /** \brief RxSO3 reference type */
831 |   typedef typename internal::traits<Map>::RxSO3Type &
832 |   RxSO3Reference;
833 |   /** \brief RxSO3 const reference type */
834 |   typedef const typename internal::traits<Map>::RxSO3Type &
835 |   ConstRxSO3Reference;
836 | 
837 | 
838 |   /** \brief degree of freedom of group */
839 |   static const int DoF = Base::DoF;
840 |   /** \brief number of internal parameters used */
841 |   static const int num_parameters = Base::num_parameters;
842 |   /** \brief group transformations are NxN matrices */
843 |   static const int N = Base::N;
844 |   /** \brief group transfomation type */
845 |   typedef typename Base::Transformation Transformation;
846 |   /** \brief point type */
847 |   typedef typename Base::Point Point;
848 |   /** \brief tangent vector type */
849 |   typedef typename Base::Tangent Tangent;
850 |   /** \brief adjoint transformation type */
851 |   typedef typename Base::Adjoint Adjoint;
852 | 
853 |   EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
854 |   using Base::operator*=;
855 |   using Base::operator*;
856 | 
857 |   EIGEN_STRONG_INLINE
858 |   Map(Scalar* coeffs)
859 |     : rxso3_(coeffs),
860 |       translation_(coeffs+Sophus::RxSO3Group<Scalar>::num_parameters) {
861 |   }
862 | 
863 |   /**
864 |    * \brief Mutator of RxSO3
865 |    */
866 |   EIGEN_STRONG_INLINE
867 |   RxSO3Reference rxso3() {
868 |     return rxso3_;
869 |   }
870 | 
871 |   /**
872 |    * \brief Accessor of RxSO3
873 |    */
874 |   EIGEN_STRONG_INLINE
875 |   ConstRxSO3Reference rxso3() const {
876 |     return rxso3_;
877 |   }
878 | 
879 |   /**
880 |    * \brief Mutator of translation vector
881 |    */
882 |   EIGEN_STRONG_INLINE
883 |   TranslationReference translation() {
884 |     return translation_;
885 |   }
886 | 
887 |   /**
888 |    * \brief Accessor of translation vector
889 |    */
890 |   EIGEN_STRONG_INLINE
891 |   ConstTranslationReference translation() const {
892 |     return translation_;
893 |   }
894 | 
895 | protected:
896 |   Map<Sophus::RxSO3Group<Scalar>,_Options> rxso3_;
897 |   Map<Matrix<Scalar,3,1>,_Options> translation_;
898 | };
899 | 
900 | /**
901 |  * \brief Specialisation of Eigen::Map for const Sim3GroupBase
902 |  *
903 |  * Allows us to wrap Sim3 Objects around POD array
904 |  * (e.g. external c style quaternion)
905 |  */
906 | template<typename _Scalar, int _Options>
907 | class Map<const Sophus::Sim3Group<_Scalar>, _Options>
908 |     : public Sophus::Sim3GroupBase<
909 |     Map<const Sophus::Sim3Group<_Scalar>, _Options> > {
910 |   typedef Sophus::Sim3GroupBase<
911 |   Map<const Sophus::Sim3Group<_Scalar>, _Options> > Base;
912 | 
913 | public:
914 |   /** \brief scalar type */
915 |   typedef typename internal::traits<Map>::Scalar Scalar;
916 |   /** \brief translation type */
917 |   typedef const typename internal::traits<Map>::TranslationType &
918 |   ConstTranslationReference;
919 |   /** \brief RxSO3 const reference type */
920 |   typedef const typename internal::traits<Map>::RxSO3Type &
921 |   ConstRxSO3Reference;
922 | 
923 |   /** \brief degree of freedom of group */
924 |   static const int DoF = Base::DoF;
925 |   /** \brief number of internal parameters used */
926 |   static const int num_parameters = Base::num_parameters;
927 |   /** \brief group transformations are NxN matrices */
928 |   static const int N = Base::N;
929 |   /** \brief group transfomation type */
930 |   typedef typename Base::Transformation Transformation;
931 |   /** \brief point type */
932 |   typedef typename Base::Point Point;
933 |   /** \brief tangent vector type */
934 |   typedef typename Base::Tangent Tangent;
935 |   /** \brief adjoint transformation type */
936 |   typedef typename Base::Adjoint Adjoint;
937 | 
938 |   EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
939 |   using Base::operator*=;
940 |   using Base::operator*;
941 | 
942 |   EIGEN_STRONG_INLINE
943 |   Map(const Scalar* coeffs)
944 |     : rxso3_(coeffs),
945 |       translation_(coeffs+Sophus::RxSO3Group<Scalar>::num_parameters) {
946 |   }
947 | 
948 |   EIGEN_STRONG_INLINE
949 |   Map(const Scalar* trans_coeffs, const Scalar* rot_coeffs)
950 |     : translation_(trans_coeffs), rxso3_(rot_coeffs){
951 |   }
952 | 
953 |   /**
954 |    * \brief Accessor of RxSO3
955 |    */
956 |   EIGEN_STRONG_INLINE
957 |   ConstRxSO3Reference rxso3() const {
958 |     return rxso3_;
959 |   }
960 | 
961 |   /**
962 |    * \brief Accessor of translation vector
963 |    */
964 |   EIGEN_STRONG_INLINE
965 |   ConstTranslationReference translation() const {
966 |     return translation_;
967 |   }
968 | 
969 | protected:
970 |   const Map<const Sophus::RxSO3Group<Scalar>,_Options> rxso3_;
971 |   const Map<const Matrix<Scalar,3,1>,_Options> translation_;
972 | };
973 | 
974 | }
975 | 
976 | #endif
977 | 


--------------------------------------------------------------------------------