openFrameworksでOMPLを動かしたい（その3）
障害物を追加してみましょう。参考文献１を参考に、障害物を置きます。
変更点を抜粋して説明していきます。
１．障害物オブジェクトobstacleをofApp.hで定義します。
ofAppクラスに物体を定義します。
ofBoxPrimitive obstacle;
２．isStateValid関数に判別ルーチンを追加。姿勢は考慮せず、obstacle内には進入禁止の条件を追加
bool ofApp::isStateValid(const ob::State *state) { // cast the abstract state type to the type we expect const auto *se3state = state->as<ob::SE3StateSpace::StateType>(); // extract the first component of the state and cast it to what we expect const auto *pos = se3state->as<ob::RealVectorStateSpace::StateType>(0); // extract the second component of the state and cast it to what we expect const auto *rot = se3state->as<ob::SO3StateSpace::StateType>(1); // check validity of state defined by pos & rot ofVec3f low = -0.5f * obstacle.getSize() + obstacle.getPosition(); ofVec3f high = 0.5f * obstacle.getSize() + obstacle.getPosition(); if ((low.x <= pos->values[0] && pos->values[0] < high.x) && (low.y <= pos->values[1] && pos->values[1] < high.y) && (low.z <= pos->values[2] && pos->values[2] < high.z)) { return false; } // return a value that is always true but uses the two variables we define, so we avoid compiler warnings return (const void*)rot != (const void*)pos; }
３．obstacleの形状定義をofApp::setupに追加。
void ofApp::setup(){ app = this; std::cout << "OMPL version: " << OMPL_VERSION << std::endl; obstacle.setWidth(0.4f); obstacle.setHeight(0.5f); obstacle.setDepth(0.6f); obstacle.setPosition(0.2f, 0.2f, 0.2f); planWithSimpleSetup(); cam.setNearClip(0.001f); cam.setTarget(boundbox); cam.setDistance(boundbox.getSize().length() * 2.f); }
４．ofApp::drawにobstacleの描画ルーチンを追加
//-------------------------------------------------------------- void ofApp::draw(){ ofSetBackgroundColor(ofColor::black); cam.begin(); ofPushStyle(); ofSetColor(ofColor::blue); ofDrawGridPlane(0.01f); ofSetColor(ofColor::grey, 66); boundbox.drawWireframe(); ofSetColor(ofColor::darkBlue, 66); obstacle.draw(); ofPopStyle(); if (solved) { ofPushStyle(); ofSetColor(ofColor::yellow); polyline.draw(); ofPopStyle(); for (unsigned int i = 0; i < stateAxis.size(); i++) { ofPushMatrix(); ofMultMatrix(stateAxis[i]); ofDrawAxis(boundbox.getSize().length() / 10.f); ofPopMatrix(); } } cam.end(); }
実行結果はこんな感じ。障害物を避けてパスをつないでいますね。
[1] Robotics/OMPL - NAIST::OnlineText 幾何学的プラニングを参照
openFrameworksでOMPLを動かしたい（その2）
ビルドの準備が整いましたので、ompl.slnを開いてビルドをしていきます。
ハマリポイント４
omplのEigenのパスが一部おかしい。
そのままビルドにかけると、Eigenのインクルードパスがおかしいとエラーが出ます。Linuxとかだと多分大丈夫なんでしょうけど、< eigen3と頭に余計なパスの指定があります。
ex.
#include < eigen3/Eigen/Core > -> #include
これを強引に全置換でeigen3を消してしまうことにします。一部のファイルでは、eigen3の無いパスを指定していますので、Eigen3を置く場所を本当にeigen3ディレクトリの下においてもあまり意味はありません。いっぱいあってうんざりしますが、全部置き換えると無事にビルドが通るようになります。
リンク用の空のofxOMPL.h
以前、別のライブラリで自前でリンクするならaddonはほぼ空でも良いんじゃね？という悪い方法を学んでしまいましたので、今回もその手で行きます。
ofxOMPL/src/ofxOMPL.h
#pragma once // Plaease add these settings manually // #include path example: D:\workspace\ompl\src // D:\workspace\eigen-334 // D:\workspace\boost_1_58_0 // #library path example: D:\workspace\ompl\build\release\lib\Release // (D:\workspace\ompl\build\release\lib\Debug) // D:\workspace\boost_1_58_0\stage\x64\lib #include <ompl/base/SpaceInformation.h> #include <ompl/base/spaces/SE3StateSpace.h> #include <ompl/geometric/planners/rrt/RRTConnect.h> #include <ompl/geometric/SimpleSetup.h> #include <ompl/config.h> #if _DEBUG #pragma comment(lib, "ompl.lib") #pragma comment(lib, "boost_serialization-vc140-mt-1_58.lib") #else #pragma comment(lib, "ompl.lib") #pragma comment(lib, "boost_serialization-vc140-mt-1_58.lib") #endif
OMPLの最初のtutorialを一部可視化しただけのofxOMPLRigidBodyPlanning
すみません、初歩的なミスを。CallBack関数にClassのメンバー関数であるofApp::isStateValid関数を直接設定することができません。修正しました。
main.cpp 変更なし
#include "ofMain.h" #include "ofApp.h" //======================================================================== int main( ){ ofSetupOpenGL(1024,768,OF_WINDOW); // <-------- setup the GL context // this kicks off the running of my app // can be OF_WINDOW or OF_FULLSCREEN // pass in width and height too: ofRunApp(new ofApp()); }
ofApp.h
#pragma once #include "ofxOMPL.h" #include "ofMain.h" class ofApp : public ofBaseApp{ public: void setup(); void update(); void draw(); void keyPressed(int key); void keyReleased(int key); void mouseMoved(int x, int y ); void mouseDragged(int x, int y, int button); void mousePressed(int x, int y, int button); void mouseReleased(int x, int y, int button); void mouseEntered(int x, int y); void mouseExited(int x, int y); void windowResized(int w, int h); void dragEvent(ofDragInfo dragInfo); void gotMessage(ofMessage msg); void planWithSimpleSetup(); bool isStateValid(const ompl::base::State *state); std::shared_ptr<ompl::base::RealVectorBounds> bounds; std::shared_ptr<ompl::geometric::SimpleSetup> ss; ompl::base::PlannerStatus solved; protected: ofEasyCam cam; ofBoxPrimitive boundbox; ofPolyline polyline; std::vector<ofMatrix4x4> stateAxis; };
ofApp.cpp
/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2010, Rice University * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions 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. * * Neither the name of the Rice University nor the names of its * contributors may 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 * COPYRIGHT OWNER 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. *********************************************************************/ /* Author: Ioan Sucan */ #include "ofApp.h" #include <iostream> namespace ob = ompl::base; namespace og = ompl::geometric; ofApp *app; bool isStateValid(const ob::State *state) { return app->isStateValid(state); } bool ofApp::isStateValid(const ob::State *state) { // cast the abstract state type to the type we expect const auto *se3state = state->as<ob::SE3StateSpace::StateType>(); // extract the first component of the state and cast it to what we expect const auto *pos = se3state->as<ob::RealVectorStateSpace::StateType>(0); // extract the second component of the state and cast it to what we expect const auto *rot = se3state->as<ob::SO3StateSpace::StateType>(1); // check validity of state defined by pos & rot // return a value that is always true but uses the two variables we define, so we avoid compiler warnings return (const void*)rot != (const void*)pos; } void ofApp::planWithSimpleSetup() { // construct the state space we are planning in auto space(std::make_shared<ob::SE3StateSpace>()); // set the bounds for the R^3 part of SE(3) bounds = std::make_shared<ob::RealVectorBounds>(3); bounds->setLow(-1); bounds->setHigh(1); space->setBounds(*bounds); // define a simple setup class ss = std::make_shared<og::SimpleSetup>(space); // set state validity checking for this space ss->setStateValidityChecker([](const ob::State *state) { return ::isStateValid(state); }); // create a random start state ob::ScopedState<> start(space); start.random(); // create a random goal state ob::ScopedState<> goal(space); goal.random(); // set the start and goal states ss->setStartAndGoalStates(start, goal); // this call is optional, but we put it in to get more output information ss->setup(); ss->print(); // attempt to solve the problem within one second of planning time solved = ss->solve(1.0); if (solved) { std::cout << "Found solution:" << std::endl; // print the path to screen ss->simplifySolution(); ss->getSolutionPath().print(std::cout); } else std::cout << "No solution found" << std::endl; // setups for drawings // bound std::vector<double> h = bounds->high; std::vector<double> l = bounds->low; ofVec3f high, low, vol; for (int i = 0; i < h.size(); i++) { high[i] = h[i]; low[i] = l[i]; } vol = high - low; boundbox.set(vol[0], vol[1], vol[2], 1, 1, 1); boundbox.setPosition((high + low) / 2.f); // SolutionPath if (solved) { stateAxis.clear(); polyline.clear(); for (int i = 0; i < ss->getSolutionPath().getStateCount(); i++) { std::vector<double> reals; ss->getStateSpace()->copyToReals(reals, ss->getSolutionPath().getState(i)); ofMatrix4x4 mat; mat.setTranslation(reals[0], reals[1], reals[2]); mat.setRotate(ofQuaternion(reals[3], reals[4], reals[5], reals[6])); stateAxis.push_back(mat); polyline.addVertex(mat.getTranslation()); } } } //-------------------------------------------------------------- void ofApp::setup(){ app = this; std::cout << "OMPL version: " << OMPL_VERSION << std::endl; planWithSimpleSetup(); cam.setNearClip(0.001f); cam.setTarget(boundbox); cam.setDistance(boundbox.getSize().length() * 2.f); } //-------------------------------------------------------------- void ofApp::update(){ } //-------------------------------------------------------------- void ofApp::draw(){ ofSetBackgroundColor(ofColor::black); cam.begin(); ofPushStyle(); ofSetColor(ofColor::blue); ofDrawGridPlane(0.01f); ofSetColor(ofColor::grey, 66); boundbox.drawWireframe(); ofPopStyle(); if (solved) { ofPushStyle(); ofSetColor(ofColor::yellow); polyline.draw(); ofPopStyle(); for (unsigned int i = 0; i < stateAxis.size(); i++) { ofPushMatrix(); ofMultMatrix(stateAxis[i]); ofDrawAxis(boundbox.getSize().length() / 10.f); ofPopMatrix(); } } cam.end(); } //-------------------------------------------------------------- void ofApp::keyPressed(int key){ } //-------------------------------------------------------------- void ofApp::keyReleased(int key){ } //-------------------------------------------------------------- void ofApp::mouseMoved(int x, int y ){ } //-------------------------------------------------------------- void ofApp::mouseDragged(int x, int y, int button){ } //-------------------------------------------------------------- void ofApp::mousePressed(int x, int y, int button){ } //-------------------------------------------------------------- void ofApp::mouseReleased(int x, int y, int button){ } //-------------------------------------------------------------- void ofApp::mouseEntered(int x, int y){ } //-------------------------------------------------------------- void ofApp::mouseExited(int x, int y){ } //-------------------------------------------------------------- void ofApp::windowResized(int w, int h){ } //-------------------------------------------------------------- void ofApp::gotMessage(ofMessage msg){ } //-------------------------------------------------------------- void ofApp::dragEvent(ofDragInfo dragInfo){ }
実行結果はこんな感じ｡もともと何もしてないサンプルなので、今のところまだ何もしてくれません。
openFrameworksでOMPLを動かしたい（その1）
ロボットの経路計画や動作計画用ライブラリとして有名なOMPL（OpenMotionPlanningLibrary）をWindows10MSVC環境のopenFrameworksで動かせるようにしたいと思います。
The Open Motion Planning Library
当初は、C＋＋できれいに書かれているし単に.libをリンクするだけでいいんじゃない？と思っていましたし、結果的にはそうだったのですが、いろいろはまりポイントがあったので記録に残します。
インストールガイドによると、Windowsにインストールはできるけれどもサポート外でAppVeyorを参考にしてくれ、とあります。
Installation
ハマリポイント１
多分やり方が悪いのだと思うのですが、AppVeyorでビルドしたompl.libはうまくリンクできない。
appveyor版ビルドは使いませんが、ビルドの仕方だけ箇条書きで残します。またＧＵＩのompl.appはこの記事では扱いません。
１．appVeyorにgithubのアカウント連携を使ってアカウントを作る
２．OMPLを自分のGithubにフォークする
３．appVeyorにてProject->NewProject->Omplのフォークを指定
４．ビルドを開始。３０分ほどで完了、メールで教えてくれる
５．appVeyorにてompl->Artifacts->build/ompl-1.3.2-win64.zipをダウンロード
これを展開して生成物を普通にリンク指定して動かしてみようとしても、リンクの不具合が出てうまく解消できませんでした。
あきらめて、プロジェクトフォルダを眺めるとCMakeList.txtがあり、中を見るとMSVCの記述もあるので普通にCMakeでも良さそうです。
CMakeでビルドしようとみていくと、まずboostで引っかかることに気が付きました。
ハマリポイント２
boostが必要だけど、openFrameworksのboostじゃいろいろ足らない
omplは外部ライブラリとして、EigenとBoost(大きなライブラリにしては、だけ)を必要とします。
openFrameworksは、自身にboost1.58を内包しているのですが、全てではなく一部だけしか持っていません。なので、不足分を自前で用意する必要があります。
もっと新しいバージョンのboostでも多分大丈夫ですが、ここではopenFrameworks0.9.8に合わせてboost1.58を自前で用意したいと思います。
１．https://sourceforge.net/projects/boost/files/boost/からboost1.58.0.zipのソースコードをダウンロードする
２．VisualStudioの「開発者コマンドプロンプトforVS2015」を開き、bootstrap.batを実行します。
３．新しくできたb2.exeを使ってlibを作成[１].ちゃんとカレントディレクトリをbootstrap.batと同じ位置に移動してからじゃないと駄目ですよ。
b2 toolset=msvc threading=multi variant=debug,release link=static runtime-link=static address-model=32 --stagedir=stage/x86 -j 8
b2 toolset=msvc threading=multi variant=debug,release link=shared runtime-link=shared address-model=32 --stagedir=stage/x86 -j 8
b2 toolset=msvc threading=multi variant=debug,release link=static runtime-link=static address-model=64 --stagedir=stage/x64 -j 8
b2 toolset=msvc threading=multi variant=debug,release link=shared runtime-link=shared address-model=64 --stagedir=stage/x64 -j 8
ハマリポイント３
omplのCMakeオプションの見えるところにBoostの表示がない
結果BOOST_ROOTでパスを指定すればよかっただけなのですが、見えるところになかったので焦りました。
CMakeの指定の例：
Where is the source code: D:\workspace\ompl
Where to build the binaries: D:\workspace\ompl\build\release
EIGEN3_INCLUDE_DIR: D:/workspace/eigen-334
Add Entry
Name: BOOST_ROOT
Type: PATH
Value: D:\workspace\boost_1_58_0
Add Entry
Name: BOOST_LIBRARYDIR
Type: PATH
Value: D:/workspace/boost_1_58_0/stage/x64/lib
ようやくompl.libが出来上がりました。つづく。
自分用読むべき論文メモ 2018年04月版
github.com
MotionCaptureデータのビューアやアノテーションなどに使えるopenFrameworksアドオン。VRM対応誰かやってくれないかな。
nico-opendata.jp
openpose.pyの出力を元に３次元位置推定．
https://www.researchgate.net/publication/260691259_The_Open_Motion_Planning_Library
The Open Motion Planning Library
Robotics/Exercise/OMPLProgramming - NAIST::OnlineText
メモ： openFrameworksで2点を指定してofCylinderPrimitiveを描画する方法
忘れそうだったのでメモ。
端点を2点指定してその間にシリンダーを描画するサンプル。
ofCylinderPrimitiveは、デフォルトで(0,1,0)の方向に長軸が表示されるので、そのベクトルを2点を結ぶベクトルに沿うように回転させるだけ。
原理は簡単。忘れそうなのは、ofVec3fで外積を求める方法。
z.cross(v)だと、外積を返した上にzも同じ値になってしまう。外積を返すだけなら、z.getCrossed(v)｡
間に余計な処理を書いたせいで、無駄に時間を食ってしまった。
void ofApp::drawCylinder2Pt(ofVec3f p, ofVec3f q, float r) { ofCylinderPrimitive cylinder; ofVec3f v = q - p; cylinder.setHeight(v.length()); ofVec3f z(0.f, 1.f, 0.f); // default cylinder direction ofQuaternion quat; quat.makeRotate(z, v); //float l = v.length(); //l = (l == 0.f) ? FLT_EPSILON : l; //ofVec3f t = z.getCrossed(v); //float angle = 180.f / PI * acos(v.dot(z) / l); //quat.makeRotate(angle, t); cylinder.setRadius(r); cylinder.resetTransform(); cylinder.rotate(quat); cylinder.setPosition((p + q)*0.5f); cylinder.draw(); }
今日の本文
明治時代に実在したイザベラバードという英国人女性が日本を旅行した旅行記をもとにした漫画。とても面白い。
原作の方も、バードおばさんが遠慮なく口が悪くて気分屋なのが面白い。
- 作者: 佐々大河
- 出版社/メーカー: KADOKAWA/エンターブレイン
- 発売日: 2015/05/15
- メディア: コミック
- この商品を含むブログ (20件) を見る
- 作者: イザベラ・バード
- 出版社/メーカー: 講談社
- 発売日: 2014/09/26
- メディア: Kindle版
- この商品を含むブログを見る
Open3DをIntel RealSense D435で動かすためにTestRealSense.cppをlibrealsense2に対応するよう改造する（その２）色付きPointCloudに変換
RealSense D435で取得したRGBColor画像とDepthをDepth解像度でlibrealsense側でalignして、Open3DのRGBD画像を作り、RGBD画像をさらにPointCloudに変換するサンプルです。
目視で確認したい場合は、DrawGeometriesのコメントアウトを外してください。ただし、１フレーム毎にループ処理が止まります。
// ---------------------------------------------------------------------------- // - Open3D: www.open3d.org - // ---------------------------------------------------------------------------- // The MIT License (MIT) // // Copyright (c) 2018 www.open3d.org // // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to deal // in the Software without restriction, including without limitation the rights // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell // copies of the Software, and to permit persons to whom the Software is // furnished to do so, subject to the following conditions: // // The above copyright notice and this permission notice shall be included in // all copies or substantial portions of the Software. // // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS // IN THE SOFTWARE. // ---------------------------------------------------------------------------- #include <iostream> #include <thread> #include <librealsense2/rs.hpp> #include <Core/Core.h> #include <Visualization/Visualization.h> // Why IO.a, Core.a, Visualization.a don't have these objects? #include <Core/Utility/FileSystem.cpp> #include <Core/Camera/PinholeCameraTrajectory.cpp> #include <Core/Geometry/TriangleMesh.cpp> #include <Core/Geometry/TriangleMeshFactory.cpp> #include <Core/Geometry/PointCloud.cpp> #include <IO/ClassIO/PointCloudIO.cpp> #include <IO/ClassIO/IJsonConvertible.cpp> #include <IO/ClassIO/IJsonConvertibleIO.cpp> #include <Core/Geometry/LineSet.cpp> #include <Core/Geometry/DownSample.cpp> using namespace three; void print_option_range(const rs2::sensor& sensor, rs2_option option_type) { // from api_how_to.h std::cout << "Supported range fro option " << option_type << ":" << std::endl; rs2::option_range range = sensor.get_option_range(option_type); float default_value = range.def; float maximum_supported_value = range.max; float minimum_supported_value = range.min; float difference_to_next_value = range.step; std::cout << " Min Value : " << minimum_supported_value << std::endl; std::cout << " Max Value : " << maximum_supported_value << std::endl; std::cout << " Default Value : " << default_value << std::endl; std::cout << " Step : " << difference_to_next_value << std::endl; } void change_option(const rs2::sensor& sensor, rs2_option option_type, float requested_value) { try { sensor.set_option(option_type, requested_value); } catch(const rs2::error& e) { std::cerr << "Failed to set option " << option_type << ". (" << e.what() << ")" << std::endl; } } int main(int argc, char **args) try { rs2::context ctx; rs2::device_list devices = ctx.query_devices(); if(devices.size() == 1){ PrintInfo("There is a connected RealSense device.\n"); } else { PrintInfo("There are %d connected RealSense devices.\n", devices.size()); } if(devices.size() == 0){ return 0; } int dev_id = 0; rs2::device dev = devices[dev_id]; std::string name = "Unknown Device"; if(dev.supports(RS2_CAMERA_INFO_NAME)){ // include/h/rs_sensor.h name = dev.get_info(RS2_CAMERA_INFO_NAME); } PrintInfo("Using device %d, an %s\n", dev_id, name.c_str()); std::string sn = "########"; if(dev.supports(RS2_CAMERA_INFO_SERIAL_NUMBER)){ sn = std::string("#") + dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER); } PrintInfo(" Serial number: %s\n", sn.c_str()); std::string fn = "########"; if(dev.supports(RS2_CAMERA_INFO_FIRMWARE_VERSION)){ fn =dev.get_info(RS2_CAMERA_INFO_FIRMWARE_VERSION); } PrintInfo(" Firmware version: %s\n\n", fn.c_str()); int index = 0; int rgb_camera = -1; int stereo_module = -1; std::vector<rs2::sensor> sensors = dev.query_sensors(); // api_how_to.h for(rs2::sensor sensor : sensors){ if(sensor.supports(RS2_CAMERA_INFO_NAME)){ PrintInfo("%d: %s\n", index, sensor.get_info(RS2_CAMERA_INFO_NAME)); if(sensor.get_info(RS2_CAMERA_INFO_NAME) == std::string("Stereo Module")){ stereo_module = index; } if(sensor.get_info(RS2_CAMERA_INFO_NAME) == std::string("RGB Camera")){ rgb_camera = index; } index++; } } if(sensors[rgb_camera].supports(RS2_OPTION_ENABLE_AUTO_EXPOSURE)){ // rs_option.h print_option_range(sensors[rgb_camera], RS2_OPTION_ENABLE_AUTO_EXPOSURE); change_option(sensors[rgb_camera], RS2_OPTION_ENABLE_AUTO_EXPOSURE, 0.0); // api_how_to.h } if(sensors[rgb_camera].supports(RS2_OPTION_EXPOSURE)){ print_option_range(sensors[rgb_camera], RS2_OPTION_EXPOSURE); change_option(sensors[rgb_camera], RS2_OPTION_EXPOSURE, 625.0); } if(sensors[rgb_camera].supports(RS2_OPTION_GAIN)){ print_option_range(sensors[rgb_camera], RS2_OPTION_GAIN); change_option(sensors[rgb_camera], RS2_OPTION_GAIN, 128.0); } if(sensors[rgb_camera].supports(RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE)){ print_option_range(sensors[rgb_camera], RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE); change_option(sensors[rgb_camera], RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE, 0.0); } // Above, it seems doesn't work... // Decrare Realsense pipeline, encapsulating the actual device and sensors rs2::pipeline pipe; // rs_pipeline.h int depth_width = 1280; // depth_intr.width; int depth_height = 720; // depth_intr.height; int depth_fps = 30; // 30 or 60 due to resolution int color_width = 1280; // color_intr.width int color_height = 720; // color_intr.height int color_fps = 30; // 30 or 60 rs2::config cfg; // examples/measure/rs-measure.cpp cfg.enable_stream(RS2_STREAM_DEPTH, depth_width, depth_height, RS2_FORMAT_Z16, depth_fps); cfg.enable_stream(RS2_STREAM_COLOR, color_width, color_height, RS2_FORMAT_RGB8, color_fps); // Start streaming with recommended configuration auto profile = pipe.start(cfg); std::this_thread::sleep_for(std::chrono::milliseconds(50)); if(sensors[stereo_module].supports(RS2_OPTION_WHITE_BALANCE)){ change_option(sensors[stereo_module], RS2_OPTION_WHITE_BALANCE, 2100.0); } auto depth_stream = profile.get_stream(RS2_STREAM_DEPTH).as<rs2::video_stream_profile>(); auto depth_intr = depth_stream.get_intrinsics(); // Calibration data PrintInfo("%d %d %.6f %.6f %.6f %.6f\n", depth_intr.width, depth_intr.height, depth_intr.fx, depth_intr.fy, depth_intr.ppx, depth_intr.ppy); for(int i=0; i<5; i++){ PrintInfo("%.6f ", depth_intr.coeffs[i]); } PrintInfo("\n\n"); auto color_stream = profile.get_stream(RS2_STREAM_COLOR).as<rs2::video_stream_profile>(); auto color_intr = color_stream.get_intrinsics(); PrintInfo("%d %d %.6f %.6f %.6f %.6f\n", color_intr.width, color_intr.height, color_intr.fx, color_intr.fy, color_intr.ppx, color_intr.ppy); for(int i=0; i<5; i++){ PrintInfo("%.6f ", color_intr.coeffs[i]); } PrintInfo("\n\n"); // get_extrinsics from api_how_to.h try { auto extrinsics = depth_stream.get_extrinsics_to(color_stream); for(int i=0; i<9; i++){ PrintInfo("%.6f ", extrinsics.rotation[i]); } PrintInfo("\n"); for(int i=0; i<3; i++){ PrintInfo("%.6f ", extrinsics.translation[i]); } PrintInfo("\n"); } catch(const std::exception& e) { std::cerr << "Failed to get extrinsics for the given streams. " << e.what() << std::endl; } auto depth_image_ptr = std::make_shared<Image>(); depth_image_ptr->PrepareImage(depth_width, depth_height, 1, 2); auto color_image_ptr = std::make_shared<Image>(); color_image_ptr->PrepareImage(color_width, color_height, 3, 1); FPSTimer timer("Realsense stream"); Visualizer depth_vis, color_vis; if(depth_vis.CreateWindow("Depth", depth_width, depth_height, 15, 50) == false || depth_vis.AddGeometry(depth_image_ptr) == false || color_vis.CreateWindow("Color", color_width, color_height, 675, 50) == false || color_vis.AddGeometry(color_image_ptr) == false) { return 0; } while(depth_vis.PollEvents() && color_vis.PollEvents()){ timer.Signal(); rs2::frameset data = pipe.wait_for_frames(); // Wait for next set of frames from the camera // Retrieve Aligned Frame rs2::align align( rs2_stream::RS2_STREAM_COLOR ); rs2::frameset aligned_frameset = align.process( data ); if( !aligned_frameset.size() ){ return -1; } rs2::frame depth = aligned_frameset.get_depth_frame(); // Find the depth data rs2::frame color = aligned_frameset.get_color_frame(); // Find the color data memcpy(depth_image_ptr->data_.data(), depth.get_data(), depth_width * depth_height * 2); memcpy(color_image_ptr->data_.data(), color.get_data(), color_width * color_height * 3); depth_vis.UpdateGeometry(); color_vis.UpdateGeometry(); double depth_scale = 1000; double depth_trunc = 4.0; bool convert_rgb_to_intensity = false; auto rgbd = CreateRGBDImageFromColorAndDepth(*color_image_ptr, *depth_image_ptr, depth_scale, depth_trunc, convert_rgb_to_intensity); auto pcd = CreatePointCloudFromRGBDImage(*rgbd, PinholeCameraIntrinsic(color_intr.width, color_intr.height, color_intr.fx, color_intr.fy, color_intr.ppx, color_intr.ppy)); // DrawGeometries({pcd}); // uncomment, if you want to check the result if(sensors[rgb_camera].supports(RS2_OPTION_WHITE_BALANCE)){ PrintInfo("\r %.2f\t", sensors[rgb_camera].get_option(RS2_OPTION_WHITE_BALANCE)); } } return 1; } catch(const rs2::error & e) { std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):\n " << e.what() << std::endl; return EXIT_FAILURE; } catch(const std::exception& e) { std::cerr << e.what() << std::endl; // pipe.stop(); // how can I get the pointer of the pipe? return EXIT_FAILURE; }
updated 2018/03/19
auto pcd = CreatePointCloudFromRGBDImage(*rgbd,
PinholeCameraIntrinsic(color_intr.width, color_intr.height,
color_intr.fx, color_intr.fy,
color_intr.ppx, color_intr.ppy));
I'm sorry, I missed. not depth_intr.
Open3Dに寄稿する方法とマナーを調べる
（編集中）
Open3Dのソースコードは、githubで管理されています。普段、仕事ではほとんど必要ないので、githubはgit cloneして便利に人のソースコードを使わせてもらうか、メモ代わりにGistにコードを張り付けるだけで、あまりちゃんと使ったことがありません。c++のTutorialをCotributeしたら？と声をかけてもらったので、ちゃんとその方法とマナーを調べたいと思います。
Open3DのContributeの仕方マニュアル
Contributing to Open3D — Open3D 0.1 dev documentation
まずはこれにざっと目を通します。
issues and pull request system
masterブランチは安定板のOpen3Dのためだけに使われていて、全てのソースコードの変更はissuesとpull requestの仕組みを通して管理されていて、次の4つの手順に寄るのだそうです。よく分かってないので、殆ど全訳になりますが読んでいきましょう。
1. Issueを開いて、リクエストやバグの報告をする。
Issues · IntelVCL/Open3D · GitHub
2. 新しいブランチやリポジトリのフォークを用意して、変更を加えて、pull requestを送信します。
Pull Requests · IntelVCL/Open3D · GitHub
3. Pull Requestでコードの変更が検査され、議論されます。変更は、Issueで提起され、議論を通して育てられます。
4. 管理者の一人がpull requestをmasterブランチにマージします。
pull requestってよく聴くけど使わないのでよく分かってない。あとで良く調べることにして、とりあえず続けましょう。
ざっくりわかることは、プロジェクトに参加したい人はまず1と2をやれば良いということですね。ソースコードの管理は、Adminsがちゃんと面倒みているので、いきなり何か変なことしてしまって壊したりする心配とかは不要で、レベルの低いソースコードを書いても、ちゃんとブラッシュアップしてもらえる仕組みがあって安心だということですね。
Maintain sanity of the project
buildを止めてしまうことが無いように、Windows, Mac, Linuxの3つの環境でテストしなさい、とあります。困ったな、Macの開発環境が無いぞ？
Coding style
コードの可読性を保つために、コーディングスタイルを保つことは重要な要素です。
1. コード自体がドキュメントと思え
2. 既存のコードとの一貫性を保て
3. 常識に従え
基本的には、ちょっとアレンジしたGoogle Coding Styleに従う。
Google C++ Style Guide
- インデントにはtabを使う。空白4つ分。強制改行にはインデント2つ分。（一行80文字の制限)
- ヘッダーガードには#pragma onceを使う
- 全てのOpen3Dメンバーはthreeネームスペースの中に入れること。
- 裸のポインタを使わずに、std::shared_ptrやstd::unique_ptrを使うこと。
- c++11の機能を推奨
その他、現代的なC++コーディングスタイルは次の資料を参考にすること。
CppCoreGuidelines/CppCoreGuidelines.md at master · isocpp/CppCoreGuidelines · GitHub
で、分からないところの整理。
そんなに難しくないので、概ね分かったような気になっていますが、根本的にgitの使い方が分かりません。結局pull requestをするまでどう準備してどう操作すればいいものやら?また明日調べましょう。
How to use github
0. Sign up for GIthub and log-in.
1. Go to https://github.com/IntelVCL/Open3D, and push [fork] button.
2. git clone https://github.com/IntelVCL/Open3D
3. git config user.name "Your name"
4. git config user.email "your@email.com"
5. Edit your source code and put it on your cloned directory.
6. git add "your source code"
7. git commit
8. git push
Open3Dをライブラリとして使い、自分の独立したプロジェクトとしてプログラムを書きたいときのCMakeサンプル
CMakeの正しい書き方がわからない・・・
今、公式にmake installやfind cmakeを準備中だそうですので、急がないのであればそれを待つことをお勧めします。
とりあえず、Ubuntu環境でひとつのチュートリアルをビルドするためのCMakeファイルの例。わからないことだらけ。
- どうすれば、Open３Dのルートを見つけられる？
- libjsoncppの謎の挙動。nativeインストールしたせい？
- libpng12じゃなくてlibpng16を自動で見つける方法
- 様々な環境でのテストの方法
cmake_minimum_required(VERSION 3.0.0) # Please set a path to Open3D root set(Open3D_DIR "/home/cvl/workspace/Open3D") set(Open3D_SOURCE_DIR "${Open3D_DIR}/src") set(Open3D_LIBRARY_DIR "${Open3D_DIR}/build/lib/") # Please set a project name set(PROJECT_NAME "ColoredPointRegistration") set(EXECUTABLE_OUTPUT_PATH "${Open3D_DIR}/build/bin/Tutorial") project(${PROJECT_NAME}) add_executable(${PROJECT_NAME} "${PROJECT_NAME}.cpp") target_compile_features(${PROJECT_NAME} PRIVATE cxx_range_for) find_package(PkgConfig REQUIRED) # set include directories include_directories(${Open3D_SOURCE_DIR}) pkg_check_modules(EIGEN3 eigen3>=3.2.7) include_directories(${EIGEN3_INCLUDE_DIRS}) # link dynamic libraries pkg_check_modules(JSONCPP jsoncpp) find_package(OpenGL) find_package(GLEW REQUIRED) find_package(glfw3) include(FindJPEG) target_link_libraries(${PROJECT_NAME} ${JSONCPP_LIBRARIES}) target_link_libraries(${PROJECT_NAME} ${OPENGL_gl_LIBRARY}) target_link_libraries(${PROJECT_NAME} ${JSONCPP_LIBRARIES}) target_link_libraries(${PROJECT_NAME} ${GLEW_LIBRARIES}) target_link_libraries(${PROJECT_NAME} ${GLFW3_LIBRARY}) target_link_libraries(${PROJECT_NAME} ${JPEG_LIBRARY}) # link static libraries find_library(Open3D_CORE NAMES libCore.a PATHS ${Open3D_LIBRARY_DIR}) find_library(Open3D_IO NAMES libIO.a PATHS ${Open3D_LIBRARY_DIR}) find_library(Open3D_VISUALIZATION libVisualization.a PATHS ${Open3D_LIBRARY_DIR}) find_library(LIBPNG NAMES libpng.a PATHS ${Open3D_LIBRARY_DIR}) find_library(LIBZ NAMES libzlib.a PATHS ${Open3D_LIBRARY_DIR}) find_library(LIBTINYFILEDIALOGS NAMES libtinyfiledialogs.a PATHS ${Open3D_LIBRARY_DIR}) find_library(LIBJSONCPP NAMES libjsoncpp.a PATHS ${Open3D_LIBRARY_DIR}) target_link_libraries(${PROJECT_NAME} ${Open3D_CORE}) target_link_libraries(${PROJECT_NAME} ${Open3D_IO}) target_link_libraries(${PROJECT_NAME} ${Open3D_VISUALIZATION}) target_link_libraries(${PROJECT_NAME} "${Open3D_LIBRARY_DIR}/libpng.a") # Force libpng16 target_link_libraries(${PROJECT_NAME} ${LIBZ}) target_link_libraries(${PROJECT_NAME} ${LIBTINYFILEDIALOGS}) target_link_libraries(${PROJECT_NAME} ${LIBJSONCPP}) # OpenMP settings find_package(OpenMP) if (OPENMP_FOUND) set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") endif()
