Point Cloud Library (PCL) 1.13.1
Loading...
Searching...
No Matches
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.