Point Cloud Library (PCL)  1.12.1-dev
camera_pose.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2014-, Open Perception, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the copyright holder(s) nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * Author: Marco Paladini <marco.paladini@ocado.com>
37  * Date: March 2014
38  */
39 
40 #pragma once
41 
42 #include <Eigen/Core>
43 #include <Eigen/Geometry>
44 #include <fstream>
45 
46 #include <pcl/memory.h>
47 
48 /**
49  * @brief The CameraPoseProcessor class is an interface to extract
50  * camera pose data generated by the pcl_kinfu_app program.
51  * Use the CameraPoseWriter implementation if writing the camera
52  * pose to a file is all you need, or provide an alternative implementation.
53  */
55 {
56  public:
57  using Ptr = pcl::shared_ptr<CameraPoseProcessor>;
58  using ConstPtr = pcl::shared_ptr<const CameraPoseProcessor>;
59 
60  virtual ~CameraPoseProcessor () = default;
61 
62  /// process the camera pose, this method is called at every frame.
63  virtual void
64  processPose (const Eigen::Affine3f &pose)=0;
65 };
66 
67 /**
68  * @brief CameraPoseWriter writes all camera poses computed by
69  * the KinfuTracker to a file on disk.
70  *
71  */
73 {
74  std::string output_filename_;
75  std::ofstream out_stream_;
76  public:
77  /**
78  * @param output_filename name of file to write
79  */
80  CameraPoseWriter (const std::string &output_filename) :
81  output_filename_ (output_filename)
82  {
83  out_stream_.open (output_filename_.c_str () );
84  }
85 
87  {
88  if (out_stream_.is_open ())
89  {
90  out_stream_.close ();
91  std::cout << "wrote camera poses to file " << output_filename_ << std::endl;
92  }
93  }
94 
95  void
96  processPose (const Eigen::Affine3f &pose) override
97  {
98  if (out_stream_.good ())
99  {
100  // convert 3x4 affine transformation to quaternion and write to file
101  Eigen::Quaternionf q (pose.rotation ());
102  Eigen::Vector3f t (pose.translation ());
103  // write translation , quaternion in a row
104  out_stream_ << t[0] << "," << t[1] << "," << t[2]
105  << "," << q.w () << "," << q.x ()
106  << "," << q.y ()<< "," << q.z () << std::endl;
107  }
108  }
109 
110 };
The CameraPoseProcessor class is an interface to extract camera pose data generated by the pcl_kinfu_...
Definition: camera_pose.h:55
pcl::shared_ptr< const CameraPoseProcessor > ConstPtr
Definition: camera_pose.h:58
virtual void processPose(const Eigen::Affine3f &pose)=0
process the camera pose, this method is called at every frame.
pcl::shared_ptr< CameraPoseProcessor > Ptr
Definition: camera_pose.h:57
virtual ~CameraPoseProcessor()=default
CameraPoseWriter writes all camera poses computed by the KinfuTracker to a file on disk.
Definition: camera_pose.h:73
void processPose(const Eigen::Affine3f &pose) override
process the camera pose, this method is called at every frame.
Definition: camera_pose.h:96
CameraPoseWriter(const std::string &output_filename)
Definition: camera_pose.h:80
Defines functions, macros and traits for allocating and using memory.