/*M/////////////////////////////////////////////////////////////////////////////////////// // // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. // // By downloading, copying, installing or using the software you agree to this license. // If you do not agree to this license, do not download, install, // copy or use the software. // // // License Agreement // For Open Source Computer Vision Library // // Copyright (C) 2013, OpenCV Foundation, all rights reserved. // Third party copyrights are property of their respective owners. // // Redistribution and use in source and binary forms, with or without modification, // are permitted provided that the following conditions are met: // // * Redistribution's of source code must retain the above copyright notice, // this list of conditions and the following disclaimer. // // * Redistribution's in binary form must reproduce the above copyright notice, // this list of conditions and the following disclaimer in the documentation // and/or other materials provided with the distribution. // // * The name of the copyright holders may not be used to endorse or promote products // derived from this software without specific prior written permission. // // This software is provided by the copyright holders and contributors "as is" and // any express or implied warranties, including, but not limited to, the implied // warranties of merchantability and fitness for a particular purpose are disclaimed. // In no event shall the Intel Corporation or contributors be liable for any direct, // indirect, incidental, special, exemplary, or consequential damages // (including, but not limited to, procurement of substitute goods or services; // loss of use, data, or profits; or business interruption) however caused // and on any theory of liability, whether in contract, strict liability, // or tort (including negligence or otherwise) arising in any way out of // the use of this software, even if advised of the possibility of such damage. // // Authors: // * Ozan Tonkal, ozantonkal@gmail.com // * Anatoly Baksheev, Itseez Inc. myname.mysurname <> mycompany.com // //M*/ #include "precomp.hpp" cv::Affine3d cv::viz::makeTransformToGlobal(const Vec3d& axis_x, const Vec3d& axis_y, const Vec3d& axis_z, const Vec3d& origin) { Affine3d::Mat3 R(axis_x[0], axis_y[0], axis_z[0], axis_x[1], axis_y[1], axis_z[1], axis_x[2], axis_y[2], axis_z[2]); return Affine3d(R, origin); } cv::Affine3d cv::viz::makeCameraPose(const Vec3d& position, const Vec3d& focal_point, const Vec3d& y_dir) { // Compute the transformation matrix for drawing the camera frame in a scene Vec3d n = normalize(focal_point - position); Vec3d u = normalize(y_dir.cross(n)); Vec3d v = n.cross(u); return makeTransformToGlobal(u, v, n, position); } /////////////////////////////////////////////////////////////////////////////////////////////// /// VizStorage implementation #if defined(_WIN32) && !defined(__CYGWIN__) #include static BOOL WINAPI ConsoleHandlerRoutine(DWORD /*dwCtrlType*/) { vtkObject::GlobalWarningDisplayOff(); return FALSE; } static void register_console_handler() { HANDLE hOut = GetStdHandle(STD_OUTPUT_HANDLE); CONSOLE_SCREEN_BUFFER_INFO hOutInfo; if (GetConsoleScreenBufferInfo(hOut, &hOutInfo)) SetConsoleCtrlHandler(ConsoleHandlerRoutine, TRUE); } #else void register_console_handler() {} #endif cv::viz::VizStorage cv::viz::VizStorage::init; cv::viz::VizMap cv::viz::VizStorage::storage; void cv::viz::VizMap::replace_clear() { type().swap(m); } cv::viz::VizMap::~VizMap() { replace_clear(); } cv::viz::VizStorage::VizStorage() { register_console_handler(); } void cv::viz::VizStorage::unregisterAll() { storage.replace_clear(); } cv::viz::Viz3d& cv::viz::VizStorage::get(const String &window_name) { String name = generateWindowName(window_name); VizMap::iterator vm_itr = storage.m.find(name); CV_Assert(vm_itr != storage.m.end()); return vm_itr->second; } void cv::viz::VizStorage::add(const Viz3d& window) { String window_name = window.getWindowName(); VizMap::iterator vm_itr = storage.m.find(window_name); CV_Assert(vm_itr == storage.m.end()); storage.m.insert(std::make_pair(window_name, window)); } bool cv::viz::VizStorage::windowExists(const String &window_name) { String name = generateWindowName(window_name); return storage.m.find(name) != storage.m.end(); } void cv::viz::VizStorage::removeUnreferenced() { for(VizMap::iterator pos = storage.m.begin(); pos != storage.m.end();) if(pos->second.impl_->ref_counter == 1) storage.m.erase(pos++); else ++pos; } cv::String cv::viz::VizStorage::generateWindowName(const String &window_name) { String output = "Viz"; // Already is Viz if (window_name == output) return output; String prefixed = output + " - "; if (window_name.substr(0, prefixed.length()) == prefixed) output = window_name; // Already has "Viz - " else if (window_name.substr(0, output.length()) == output) output = prefixed + window_name; // Doesn't have prefix else output = (window_name == "" ? output : prefixed + window_name); return output; } cv::viz::Viz3d cv::viz::getWindowByName(const String &window_name) { return Viz3d (window_name); } void cv::viz::unregisterAllWindows() { VizStorage::unregisterAll(); } cv::viz::Viz3d cv::viz::imshow(const String& window_name, InputArray image, const Size& window_size) { Viz3d viz = getWindowByName(window_name); viz.showImage(image, window_size); return viz; } /////////////////////////////////////////////////////////////////////////////////////////////// /// Read/write clouds. Supported formats: ply, stl, xyz, obj void cv::viz::writeCloud(const String& file, InputArray cloud, InputArray colors, InputArray normals, bool binary) { CV_Assert(file.size() > 4 && "Extention is required"); String extention = file.substr(file.size()-4); vtkSmartPointer source = vtkSmartPointer::New(); source->SetColorCloudNormals(cloud, colors, normals); vtkSmartPointer writer; if (extention == ".xyz") { writer = vtkSmartPointer::New(); vtkXYZWriter::SafeDownCast(writer)->SetFileName(file.c_str()); } else if (extention == ".ply") { writer = vtkSmartPointer::New(); vtkPLYWriter::SafeDownCast(writer)->SetFileName(file.c_str()); vtkPLYWriter::SafeDownCast(writer)->SetFileType(binary ? VTK_BINARY : VTK_ASCII); vtkPLYWriter::SafeDownCast(writer)->SetArrayName("Colors"); } else if (extention == ".obj") { writer = vtkSmartPointer::New(); vtkOBJWriter::SafeDownCast(writer)->SetFileName(file.c_str()); } else CV_Assert(!"Unsupported format"); writer->SetInputConnection(source->GetOutputPort()); writer->Write(); } cv::Mat cv::viz::readCloud(const String& file, OutputArray colors, OutputArray normals) { CV_Assert(file.size() > 4 && "Extention is required"); String extention = file.substr(file.size()-4); vtkSmartPointer reader; if (extention == ".xyz") { reader = vtkSmartPointer::New(); vtkXYZReader::SafeDownCast(reader)->SetFileName(file.c_str()); } else if (extention == ".ply") { reader = vtkSmartPointer::New(); CV_Assert(vtkPLYReader::CanReadFile(file.c_str())); vtkPLYReader::SafeDownCast(reader)->SetFileName(file.c_str()); } else if (extention == ".obj") { reader = vtkSmartPointer::New(); vtkOBJReader::SafeDownCast(reader)->SetFileName(file.c_str()); } else if (extention == ".stl") { reader = vtkSmartPointer::New(); vtkSTLReader::SafeDownCast(reader)->SetFileName(file.c_str()); } else CV_Assert(!"Unsupported format"); cv::Mat cloud; vtkSmartPointer sink = vtkSmartPointer::New(); sink->SetInputConnection(reader->GetOutputPort()); sink->SetOutput(cloud, colors, normals); sink->Write(); return cloud; } cv::viz::Mesh cv::viz::readMesh(const String& file) { return Mesh::load(file); } /////////////////////////////////////////////////////////////////////////////////////////////// /// Read/write poses and trajectories bool cv::viz::readPose(const String& file, Affine3d& pose, const String& tag) { FileStorage fs(file, FileStorage::READ); if (!fs.isOpened()) return false; Mat hdr(pose.matrix, false); fs[tag] >> hdr; if (hdr.empty() || hdr.cols != pose.matrix.cols || hdr.rows != pose.matrix.rows) return false; hdr.convertTo(pose.matrix, CV_64F); return true; } void cv::viz::writePose(const String& file, const Affine3d& pose, const String& tag) { FileStorage fs(file, FileStorage::WRITE); fs << tag << Mat(pose.matrix, false); } void cv::viz::readTrajectory(OutputArray _traj, const String& files_format, int start, int end, const String& tag) { CV_Assert(_traj.kind() == _InputArray::STD_VECTOR || _traj.kind() == _InputArray::MAT); start = max(0, std::min(start, end)); end = std::max(start, end); std::vector traj; for(int i = start; i < end; ++i) { Affine3d affine; bool ok = readPose(cv::format(files_format.c_str(), i), affine, tag); if (!ok) break; traj.push_back(affine); } Mat(traj).convertTo(_traj, _traj.depth()); } void cv::viz::writeTrajectory(InputArray _traj, const String& files_format, int start, const String& tag) { if (_traj.kind() == _InputArray::STD_VECTOR_MAT) { std::vector& v = *(std::vector*)_traj.obj; for(size_t i = 0, index = max(0, start); i < v.size(); ++i, ++index) { Affine3d affine; Mat pose = v[i]; CV_Assert(pose.type() == CV_32FC(16) || pose.type() == CV_64FC(16)); pose.copyTo(affine.matrix); writePose(cv::format(files_format.c_str(), index), affine, tag); } return; } if (_traj.kind() == _InputArray::STD_VECTOR || _traj.kind() == _InputArray::MAT) { CV_Assert(_traj.type() == CV_32FC(16) || _traj.type() == CV_64FC(16)); Mat traj = _traj.getMat(); if (traj.depth() == CV_32F) for(size_t i = 0, index = max(0, start); i < traj.total(); ++i, ++index) writePose(cv::format(files_format.c_str(), index), traj.at((int)i), tag); if (traj.depth() == CV_64F) for(size_t i = 0, index = max(0, start); i < traj.total(); ++i, ++index) writePose(cv::format(files_format.c_str(), index), traj.at((int)i), tag); } CV_Assert(!"Unsupported array kind"); } /////////////////////////////////////////////////////////////////////////////////////////////// /// Computing normals for mesh void cv::viz::computeNormals(const Mesh& mesh, OutputArray _normals) { vtkSmartPointer polydata = getPolyData(WMesh(mesh)); vtkSmartPointer with_normals = VtkUtils::ComputeNormals(polydata); vtkSmartPointer generic_normals = with_normals->GetPointData()->GetNormals(); if(generic_normals) { Mat normals(1, generic_normals->GetNumberOfTuples(), CV_64FC3); Vec3d *optr = normals.ptr(); for(int i = 0; i < generic_normals->GetNumberOfTuples(); ++i, ++optr) generic_normals->GetTuple(i, optr->val); normals.convertTo(_normals, mesh.cloud.type()); } else _normals.release(); }