viewpoint_sampler.cpp
Go to the documentation of this file.
00001 // -*- mode: c++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2015, JSK Lab
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/o2r other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the JSK Lab nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 #include "jsk_pcl_ros/viewpoint_sampler.h"
00037 #include <angles/angles.h>
00038 #include <opencv2/opencv.hpp>
00039 
00040 namespace jsk_pcl_ros
00041 {
00042   ViewpointSampler::ViewpointSampler(
00043     double angle_step, double angle_min, double angle_max,
00044     double radius_step, double radius_min, double radius_max, int n_points):
00045     angle_step_(angle_step), angle_min_(angle_min), angle_max_(angle_max),
00046     radius_step_(radius_step), radius_min_(radius_min), radius_max_(radius_max),
00047     index_(0),
00048     angle_(angle_min),
00049     radius_(radius_min),
00050     n_points_(n_points)
00051   {
00052     
00053   }
00054 
00055   void ViewpointSampler::reset()
00056   {
00057     index_ = 0;
00058     angle_ = angle_min_;
00059     radius_ = radius_min_;
00060   }
00061 
00062   void ViewpointSampler::next()
00063   {
00064     angle_ += angle_step_;
00065     if (angle_ > angle_max_ * 1.001) { // 1.001 for prevent rounding error
00066       angle_ = angle_min_;
00067       radius_ += radius_step_;
00068       if (radius_ > radius_max_ * 1.001) { // 1.001 for prevent rounding error
00069         radius_ = radius_min_;
00070         ++index_;
00071       }
00072     }
00073   }
00074 
00075   size_t ViewpointSampler::sampleNum()
00076   {
00077     return ((angle_max_ - angle_min_) / angle_step_ + 1) * n_points_ * ((radius_max_ - radius_min_) / radius_step_ + 1);
00078   }
00079 
00080   void ViewpointSampler::normalizeVector(
00081     double& x, double& y, double& z)
00082   {
00083     double norm = sqrt(x*x + y*y + z*z);
00084     x /= norm;
00085     y /= norm;
00086     z /= norm;
00087   }
00088   
00089   void ViewpointSampler::get(Eigen::Affine3f& transform)
00090   {
00091     // compute the Point(x, y ,z) on the sphere based on index_ and radius_ using Golden Spiral technique
00092     double angle_rad = angles::from_degrees(angle_);
00093     const double inc = CV_PI * (3 - sqrt(5));
00094     const double off = 2.0f / double(n_points_);
00095     double y = index_ * off - 1.0f + (off / 2.0f);
00096     double r = sqrt(1.0f - y * y);
00097     double phi = index_ * inc;
00098     double x = std::cos(phi) * r;
00099     double z = std::sin(phi) * r;
00100     double lat = std::acos(z), lon;
00101     if ((fabs(std::sin(lat)) < 1e-5) || (fabs(y / std::sin(lat)) > 1))
00102       lon = 0;
00103     else
00104       lon = std::asin(y / std::sin(lat));
00105     x *= radius_; // * cos(lon) * sin(lat);
00106     y *= radius_; //double y = radius * sin(lon) * sin(lat);
00107     z *= radius_; //double z = radius * cos(lat);
00108 
00109     cv::Vec3d T(x, y, z);
00110 
00111     // Figure out the up vector
00112     double x_up = radius_ * std::cos(lon) * std::sin(lat - 1e-5) - x;
00113     double y_up = radius_ * std::sin(lon) * std::sin(lat - 1e-5) - y;
00114     double z_up = radius_ * std::cos(lat - 1e-5) - z;
00115     normalizeVector(x_up, y_up, z_up);
00116     
00117     // Figure out the third vector of the basis
00118     double x_right = -y_up * z + z_up * y;
00119     double y_right = x_up * z - z_up * x;
00120     double z_right = -x_up * y + y_up * x;
00121     normalizeVector(x_right, y_right, z_right);
00122 
00123     // Rotate the up vector in that basis
00124     double x_new_up = x_up * std::cos(angle_rad) + x_right * std::sin(angle_rad);
00125     double y_new_up = y_up * std::cos(angle_rad) + y_right * std::sin(angle_rad);
00126     double z_new_up = z_up * std::cos(angle_rad) + z_right * std::sin(angle_rad);
00127     cv::Vec3d up(x_new_up, y_new_up, z_new_up);
00128 
00129     // compute the left vector
00130     cv::Vec3d l;
00131     l = up.cross(T);  // cross product
00132     normalizeVector(l(0),l(1),l(2));
00133     
00134     up = T.cross(l);  // cross product
00135     normalizeVector(up(0), up(1), up(2));
00136 
00137     // compute transformation
00138     Eigen::Vector3f ez = Eigen::Vector3f(-T[0], -T[1], -T[2]).normalized();
00139     Eigen::Vector3f ey = Eigen::Vector3f(up[0], up[1], up[2]).normalized();
00140     Eigen::Vector3f ex = ey.cross(ez).normalized();
00141     Eigen::Vector3f translation = Eigen::Vector3f(T[0], T[1], T[2]);
00142     Eigen::Matrix3f mat;
00143     mat.col(0) = ex;
00144     mat.col(1) = ey;
00145     mat.col(2) = ez;
00146     
00147     Eigen::Quaternionf q(mat);
00148     transform = Eigen::Translation3f(translation) * Eigen::AngleAxisf(q);
00149   }
00150 }


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Tue Jul 2 2019 19:41:45