<commit_before>// MS WARNINGS MACRO
#define _SCL_SECURE_NO_WARNINGS

// Macro: Program Settings
#define ENABLE_NETWORK_IO 0

#include <iostream>
#include <deque>
#include <boost/noncopyable.hpp>
#include "data_type.hpp"
#include "pixel_sorter.hpp"
#include "ppm_reader.hpp"
#include "splitter.hpp"
#include "algorithm.hpp"
#include "algorithm_2.hpp"
#include "gui.hpp"
#include "network.hpp"

#include <sort_algorithm/yrange2.hpp>
#include <sort_algorithm/yrange5.hpp>
#include <sort_algorithm/genetic.hpp>
#include <sort_algorithm/Murakami.hpp>

class position_manager : boost::noncopyable
{
public:
    typedef question_data position_type;

    position_manager() = default;
    virtual ~position_manager() = default;

    template<class T>
    void add(T && pos)
    {
        std::lock_guard<std::mutex> lock(mutex_);
        items_.push_back(std::forward<T>(pos));
    }

    position_type get()
    {
        std::lock_guard<std::mutex> lock(mutex_);
        auto res = items_.front();
        items_.pop_front();
        return res;
    }

    bool empty()
    {
        return items_.empty();
    }

private:
    std::mutex mutex_;
    std::deque<position_type> items_;
};

class analyzer : boost::noncopyable
{
public:
    explicit analyzer(int const problem_id, std::string const& player_id)
        : client_(), problem_id_(problem_id), player_id_(player_id)
    {
    }
    virtual ~analyzer() = default;

    void operator() (position_manager& manager)
    {
        // 問題文の入手
        raw_data_ = get_raw_question();
        data_     = get_skelton_question(raw_data_);

        // 2次元画像に分割
        split_image_ = splitter().split_image(raw_data_);

        // 原画像推測部
        // TODO: yrangeなどの実行
        auto sorter_resolve = sorter_(raw_data_, split_image_);
        //data_.block = std::move(sorter_resolve);
        data_.block = sorter_resolve;
        manager.add(convert_block(data_)); // 解答

        // TODO: yrange5の実行(並列化)

        // 画面表示をいくつか(yrange/Murakmi/yrange5/algo2 etc.)
        std::vector<boost::shared_ptr<gui::impl::MoveWindow>> windows;

        windows.push_back(
            gui::make_mansort_window(split_image_, sorter_resolve, "Murakami")
            );
        windows.push_back(
            gui::make_mansort_window(split_image_, sorter_resolve, "Murakami")
            );

        boost::thread th(
            [&]()
            {
                // futureリストでvalidを巡回し，閉じられたWindowから解とする
                while(!windows.empty())
                {
                    for(auto it = windows.begin(); it != windows.end();)
                    {
                        auto res = gui::get_result(*it);
                        if(res)
                        {
                            data_.block = res.get();
                            manager.add(convert_block(data_)); // 解答

                            it = windows.erase(it);
                        }
                        else ++it;
                    }
                }
            });

        gui::wait_all_window();

        th.join();

    }

    std::string submit(answer_type const& ans) const
    {
        auto submit_result = client_.submit(problem_id_, player_id_, ans);
        return submit_result.get();
    }

private:
    question_raw_data get_raw_question() const
    {
#if ENABLE_NETWORK_IO
        // ネットワーク通信から
        std::string const data = client_.get_problem(01).get();
        return ppm_reader().from_data(data);
#else
        // ファイルから
        std::string const path("prob01.ppm");
        return ppm_reader().from_file(path);
#endif
    }

    question_data get_skelton_question(question_raw_data const& raw) const
    {
        question_data formed = {
            problem_id_,
            player_id_,
            raw.split_num,
            raw.selectable_num,
            raw.cost.first,
            raw.cost.second,
            std::vector<std::vector<point_type>>()
        };

        return formed;
    }

    question_data convert_block(question_data const& data) const
    {
        auto res = data.clone();

        for(int i = 0; i < data.size.second; ++i)
        {
            for(int j = 0; j < data.size.first; ++j)
            {
                auto const& target = data.block[i][j];
                res.block[target.y][target.x] = point_type{j, i};
            }
        }

        return res;
    }

    int const problem_id_;
    std::string const player_id_;

    question_raw_data raw_data_;
    question_data         data_;
    split_image_type  split_image_;

    mutable network::client client_;
    pixel_sorter<Murakami> sorter_;
};

question_data convert_block(question_data const& data)
{
    auto res = data.clone();

    for(int i = 0; i < data.size.second; ++i)
    {
        for(int j = 0; j < data.size.first; ++j)
        {
            auto const& target = data.block[i][j];
            res.block[target.y][target.x] = point_type{j, i};
        }
    }

    return res;
}

int main()
{
    auto const ploblemid = 1;
    auto const token     = "3935105806";

    analyzer         analyze(ploblemid, token);
    algorithm_2      algo;
    position_manager manager;

    boost::thread thread(boost::bind(&analyzer::operator(), &analyze, std::ref(manager)));

    while(true)
    {
        if(!manager.empty())
        {
            // 手順探索部
            algo.reset(manager.get());
            auto const answer = algo.get();

            if(answer)
            {
                // 解が見つかった
                // TODO: 前より良くなったら提出など
//                auto result = analyze.submit(answer.get());
//                std::cout << "Submit Result: " << result << std::endl;
                std::cout << "Test";
            }
        }
    }

    thread.join();

    return 0;
}

<commit_msg>提出(もしくはエミュレート)操作の挿入．77行目付近のTODOは，yrange5の仕様変更が必要だ．<commit_after>// MS WARNINGS MACRO
#define _SCL_SECURE_NO_WARNINGS

// Macro: Program Settings
#define ENABLE_NETWORK_IO 0

#include <iostream>
#include <deque>
#include <boost/noncopyable.hpp>
#include "data_type.hpp"
#include "pixel_sorter.hpp"
#include "ppm_reader.hpp"
#include "splitter.hpp"
#include "algorithm.hpp"
#include "algorithm_2.hpp"
#include "gui.hpp"
#include "network.hpp"
#include "test_tool.hpp"

#include <sort_algorithm/yrange2.hpp>
#include <sort_algorithm/yrange5.hpp>
#include <sort_algorithm/genetic.hpp>
#include <sort_algorithm/Murakami.hpp>

class position_manager : boost::noncopyable
{
public:
    typedef question_data position_type;

    position_manager() = default;
    virtual ~position_manager() = default;

    template<class T>
    void add(T && pos)
    {
        std::lock_guard<std::mutex> lock(mutex_);
        items_.push_back(std::forward<T>(pos));
    }

    position_type get()
    {
        std::lock_guard<std::mutex> lock(mutex_);
        auto res = items_.front();
        items_.pop_front();
        return res;
    }

    bool empty()
    {
        return items_.empty();
    }

private:
    std::mutex mutex_;
    std::deque<position_type> items_;
};

class analyzer : boost::noncopyable
{
public:
    explicit analyzer(int const problem_id, std::string const& player_id)
        : client_(), problem_id_(problem_id), player_id_(player_id)
    {
    }
    virtual ~analyzer() = default;

    void operator() (position_manager& manager)
    {
        // 問題文の入手
        raw_data_ = get_raw_question();
        data_     = get_skelton_question(raw_data_);

        // 2次元画像に分割
        split_image_ = splitter().split_image(raw_data_);

        // 原画像推測部
        // TODO: yrangeなどの実行
        auto sorter_resolve = sorter_(raw_data_, split_image_);
        //data_.block = std::move(sorter_resolve);
        data_.block = sorter_resolve;
        manager.add(convert_block(data_)); // 解答

        // TODO: yrange5の実行(並列化)

        // 画面表示をいくつか(yrange/Murakmi/yrange5/algo2 etc.)
        std::vector<boost::shared_ptr<gui::impl::MoveWindow>> windows;

        windows.push_back(
            gui::make_mansort_window(split_image_, sorter_resolve, "Murakami")
            );
        windows.push_back(
            gui::make_mansort_window(split_image_, sorter_resolve, "Murakami")
            );

        boost::thread th(
            [&]()
            {
                // futureリストでvalidを巡回し，閉じられたWindowから解とする
                while(!windows.empty())
                {
                    for(auto it = windows.begin(); it != windows.end();)
                    {
                        auto res = gui::get_result(*it);
                        if(res)
                        {
                            data_.block = res.get();
                            manager.add(convert_block(data_)); // 解答

                            it = windows.erase(it);
                        }
                        else ++it;
                    }
                }
            });

        gui::wait_all_window();

        th.join();

    }

    std::string submit(answer_type const& ans) const
    {
        auto submit_result = client_.submit(problem_id_, player_id_, ans);
        return submit_result.get();
    }

private:
    question_raw_data get_raw_question() const
    {
#if ENABLE_NETWORK_IO
        // ネットワーク通信から
        std::string const data = client_.get_problem(01).get();
        return ppm_reader().from_data(data);
#else
        // ファイルから
        std::string const path("prob01.ppm");
        return ppm_reader().from_file(path);
#endif
    }

    question_data get_skelton_question(question_raw_data const& raw) const
    {
        question_data formed = {
            problem_id_,
            player_id_,
            raw.split_num,
            raw.selectable_num,
            raw.cost.first,
            raw.cost.second,
            std::vector<std::vector<point_type>>()
        };

        return formed;
    }

    question_data convert_block(question_data const& data) const
    {
        auto res = data.clone();

        for(int i = 0; i < data.size.second; ++i)
        {
            for(int j = 0; j < data.size.first; ++j)
            {
                auto const& target = data.block[i][j];
                res.block[target.y][target.x] = point_type{j, i};
            }
        }

        return res;
    }

    int const problem_id_;
    std::string const player_id_;

    question_raw_data raw_data_;
    question_data         data_;
    split_image_type  split_image_;

    mutable network::client client_;
    pixel_sorter<Murakami> sorter_;
};

question_data convert_block(question_data const& data)
{
    auto res = data.clone();

    for(int i = 0; i < data.size.second; ++i)
    {
        for(int j = 0; j < data.size.first; ++j)
        {
            auto const& target = data.block[i][j];
            res.block[target.y][target.x] = point_type{j, i};
        }
    }

    return res;
}

int main()
{
    auto const ploblemid = 1;
    auto const token     = "3935105806";

    analyzer         analyze(ploblemid, token);
    algorithm_2      algo;
    position_manager manager;

    boost::thread thread(boost::bind(&analyzer::operator(), &analyze, std::ref(manager)));

    while(true)
    {
        if(!manager.empty())
        {
            // 手順探索部
            auto question = manager.get();
            algo.reset(question);
            auto const answer = algo.get();

            if(answer) // 解が見つかった
            {
#if ENABLE_NETWORK_IO
                // TODO: 前より良くなったら提出など(普通にいらないかも．提出前に目grepしてるわけだし)
                auto result = analyze.submit(answer.get());
                std::cout << "Submit Result: " << result << std::endl;
#else
                test_tool::emulator emu(question);
                auto result = emu.start(answer.get());
                std::cout << "Wrong: " << result.wrong << std::endl;
                std::cout << "Cost : " << result.cost  << std::endl;
                std::cout << "---" << std::endl;
#endif
            }
        }
    }

    thread.join();

    return 0;
}

<|endoftext|>


<commit_before>/*
Copyright 2016 Fixstars Corporation

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

http ://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/

#include <iostream>
#include <iomanip>
#include <string>
#include <chrono>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <cuda_runtime.h>

#include <libsgm.h>

#define ASSERT_MSG(expr, msg) \
	if (!(expr)) { \
		std::cerr << msg << std::endl; \
		std::exit(EXIT_FAILURE); \
	} \

struct device_buffer
{
	device_buffer() : data(nullptr) {}
	device_buffer(size_t count) { allocate(count); }
	void allocate(size_t count) { cudaMalloc(&data, count); }
	~device_buffer() { cudaFree(data); }
	void* data;
};

// Camera Parameters
struct CameraParameters
{
	float fu;                 //!< focal length x (pixel)
	float fv;                 //!< focal length y (pixel)
	float u0;                 //!< principal point x (pixel)
	float v0;                 //!< principal point y (pixel)
	float baseline;           //!< baseline (meter)
	float height;             //!< height position (meter), ignored when ROAD_ESTIMATION_AUTO
	float tilt;               //!< tilt angle (radian), ignored when ROAD_ESTIMATION_AUTO
};

// Transformation between pixel coordinate and world coordinate
struct CoordinateTransform
{
	CoordinateTransform(const CameraParameters& camera) : camera(camera)
	{
		sinTilt = (sinf(camera.tilt));
		cosTilt = (cosf(camera.tilt));
		bf = camera.baseline * camera.fu;
		invfu = 1.f / camera.fu;
		invfv = 1.f / camera.fv;
	}

	inline cv::Point3f imageToWorld(const cv::Point2f& pt, float d) const
	{
		const float u = pt.x;
		const float v = pt.y;

		const float Zc = bf / d;
		const float Xc = invfu * (u - camera.u0) * Zc;
		const float Yc = invfv * (v - camera.v0) * Zc;

		const float Xw = Xc;
		const float Yw = Yc * cosTilt + Zc * sinTilt;
		const float Zw = Zc * cosTilt - Yc * sinTilt;

		return cv::Point3f(Xw, Yw, Zw);
	}

	CameraParameters camera;
	float sinTilt, cosTilt, bf, invfu, invfv;
};

template <class... Args>
static std::string format_string(const char* fmt, Args... args)
{
	const int BUF_SIZE = 1024;
	char buf[BUF_SIZE];
	std::snprintf(buf, BUF_SIZE, fmt, args...);
	return std::string(buf);
}

static cv::Scalar computeColor(float val)
{
	const float hscale = 6.f;
	float h = 0.6f * (1.f - val), s = 1.f, v = 1.f;
	float r, g, b;

	static const int sector_data[][3] =
	{ { 1,3,0 },{ 1,0,2 },{ 3,0,1 },{ 0,2,1 },{ 0,1,3 },{ 2,1,0 } };
	float tab[4];
	int sector;
	h *= hscale;
	if (h < 0)
		do h += 6; while (h < 0);
	else if (h >= 6)
		do h -= 6; while (h >= 6);
	sector = cvFloor(h);
	h -= sector;
	if ((unsigned)sector >= 6u)
	{
		sector = 0;
		h = 0.f;
	}

	tab[0] = v;
	tab[1] = v*(1.f - s);
	tab[2] = v*(1.f - s*h);
	tab[3] = v*(1.f - s*(1.f - h));

	b = tab[sector_data[sector][0]];
	g = tab[sector_data[sector][1]];
	r = tab[sector_data[sector][2]];
	return 255 * cv::Scalar(b, g, r);
}

void reprojectPointsTo3D(const cv::Mat& disparity, const CameraParameters& camera, std::vector<cv::Point3f>& points, bool subpixeled)
{
	CV_Assert(disparity.type() == CV_8U || disparity.type() == CV_16U);

	CoordinateTransform tf(camera);

	points.clear();
	points.reserve(disparity.rows * disparity.cols);

	for (int y = 0; y < disparity.rows; y++)
	{
		for (int x = 0; x < disparity.cols; x++)
		{
			short raw;
			switch (disparity.elemSize1()) {
			case 1:
				raw = disparity.at<uchar>(y, x);
				break;
			case 2:
				raw = disparity.at<ushort>(y, x);
				break;
			default:
				CV_Assert(false);
			}
			float d = raw;
			if (subpixeled) {
				d /= sgm::StereoSGM::SUBPIXEL_SCALE;
			}
			if (d > 0)
				points.push_back(tf.imageToWorld(cv::Point(x, y), d));
		}
	}
}

void drawPoints3D(const std::vector<cv::Point3f>& points, cv::Mat& draw)
{
	const int SIZE_X = 512;
	const int SIZE_Z = 1024;
	const int maxz = 20; // [meter]
	const double pixelsPerMeter = 1. * SIZE_Z / maxz;

	draw = cv::Mat::zeros(SIZE_Z, SIZE_X, CV_8UC3);

	for (const cv::Point3f& pt : points)
	{
		const float X = pt.x;
		const float Z = pt.z;

		const int u = cvRound(pixelsPerMeter * X) + SIZE_X / 2;
		const int v = SIZE_Z - cvRound(pixelsPerMeter * Z);

		const cv::Scalar color = computeColor(std::min(Z, 1.f * maxz) / maxz);
		cv::circle(draw, cv::Point(u, v), 1, color);
	}
}

int main(int argc, char* argv[])
{
	if (argc < 4) {
		std::cout << "usage: " << argv[0] << " left-image-format right-image-format camera.xml [dizp_size] [ouput_depth] [subpixel]" << std::endl;
		return 0;
	}

	const int first_frame = 1;

	cv::Mat I1 = cv::imread(format_string(argv[1], first_frame), -1);
	cv::Mat I2 = cv::imread(format_string(argv[2], first_frame), -1);
	const cv::FileStorage cvfs(argv[3], cv::FileStorage::READ);
	const int disp_size = argc >= 5 ? std::stoi(argv[4]) : 128;
	const int output_depth = argc >= 6 ? std::stoi(argv[5]) : 16;
	const bool subpixel = argc >= 7 ? std::stoi(argv[6]) != 0 : true;

	ASSERT_MSG(!I1.empty() && !I2.empty(), "imread failed.");
	ASSERT_MSG(cvfs.isOpened(), "camera.xml read failed.");
	ASSERT_MSG(I1.size() == I2.size() && I1.type() == I2.type(), "input images must be same size and type.");
	ASSERT_MSG(I1.type() == CV_8U || I1.type() == CV_16U, "input image format must be CV_8U or CV_16U.");
	ASSERT_MSG(disp_size == 64 || disp_size == 128, "disparity size must be 64 or 128.");
	ASSERT_MSG(output_depth == 8 || output_depth == 16, "output depth must be 8 or 16");

	// read camera parameters
	const cv::FileNode node(cvfs.fs, NULL);
	CameraParameters camera;
	camera.fu = node["FocalLengthX"];
	camera.fv = node["FocalLengthY"];
	camera.u0 = node["CenterX"];
	camera.v0 = node["CenterY"];
	camera.baseline = node["BaseLine"];
	camera.tilt = node["Tilt"];

	const int width = I1.cols;
	const int height = I1.rows;

	const int input_depth = I1.type() == CV_8U ? 8 : 16;
	const int input_bytes = input_depth * width * height / 8;
	const int output_bytes = output_depth * width * height / 8;

	const sgm::StereoSGM::Parameters params{10, 120, 0.95f, subpixel};

	sgm::StereoSGM sgm(width, height, disp_size, input_depth, output_depth, sgm::EXECUTE_INOUT_CUDA2CUDA, params);

	cv::Mat disparity(height, width, output_depth == 8 ? CV_8U : CV_16U);
	cv::Mat disparity_8u, disparity_color, draw;
	std::vector<cv::Point3f> points;

	device_buffer d_I1(input_bytes), d_I2(input_bytes), d_disparity(output_bytes);

	for (int frame_no = first_frame;; frame_no++) {

		I1 = cv::imread(format_string(argv[1], frame_no), -1);
		I2 = cv::imread(format_string(argv[2], frame_no), -1);
		if (I1.empty() || I2.empty()) {
			frame_no = first_frame;
			continue;
		}

		cudaMemcpy(d_I1.data, I1.data, input_bytes, cudaMemcpyHostToDevice);
		cudaMemcpy(d_I2.data, I2.data, input_bytes, cudaMemcpyHostToDevice);

		const auto t1 = std::chrono::system_clock::now();

		sgm.execute(d_I1.data, d_I2.data, d_disparity.data);
		cudaDeviceSynchronize();

		const auto t2 = std::chrono::system_clock::now();
		const auto duration = std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1).count();
		const double fps = 1e6 / duration;

		cudaMemcpy(disparity.data, d_disparity.data, output_bytes, cudaMemcpyDeviceToHost);

		// draw results
		if (I1.type() != CV_8U) {
			cv::normalize(I1, I1, 0, 255, cv::NORM_MINMAX);
			I1.convertTo(I1, CV_8U);
		}

		if (subpixel) {
			disparity.convertTo(disparity_8u, CV_8U, 255. / disp_size / sgm::StereoSGM::SUBPIXEL_SCALE);
		} else {
			disparity.convertTo(disparity_8u, CV_8U, 255. / disp_size);
		}
		reprojectPointsTo3D(disparity, camera, points, subpixel);
		drawPoints3D(points, draw);

		cv::applyColorMap(disparity_8u, disparity_color, cv::COLORMAP_JET);
		cv::putText(disparity_color, format_string("sgm execution time: %4.1f[msec] %4.1f[FPS]", 1e-3 * duration, fps),
			cv::Point(50, 50), 2, 0.75, cv::Scalar(255, 255, 255));

		cv::imshow("left image", I1);
		cv::imshow("disparity", disparity_color);
		cv::imshow("points", draw);

		const char c = cv::waitKey(1);
		if (c == 27) // ESC
			break;
	}

	return 0;
}
<commit_msg>Use CV_32F<commit_after>/*
Copyright 2016 Fixstars Corporation

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

http ://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/

#include <iostream>
#include <iomanip>
#include <string>
#include <chrono>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <cuda_runtime.h>

#include <libsgm.h>

#define ASSERT_MSG(expr, msg) \
	if (!(expr)) { \
		std::cerr << msg << std::endl; \
		std::exit(EXIT_FAILURE); \
	} \

struct device_buffer
{
	device_buffer() : data(nullptr) {}
	device_buffer(size_t count) { allocate(count); }
	void allocate(size_t count) { cudaMalloc(&data, count); }
	~device_buffer() { cudaFree(data); }
	void* data;
};

// Camera Parameters
struct CameraParameters
{
	float fu;                 //!< focal length x (pixel)
	float fv;                 //!< focal length y (pixel)
	float u0;                 //!< principal point x (pixel)
	float v0;                 //!< principal point y (pixel)
	float baseline;           //!< baseline (meter)
	float height;             //!< height position (meter), ignored when ROAD_ESTIMATION_AUTO
	float tilt;               //!< tilt angle (radian), ignored when ROAD_ESTIMATION_AUTO
};

// Transformation between pixel coordinate and world coordinate
struct CoordinateTransform
{
	CoordinateTransform(const CameraParameters& camera) : camera(camera)
	{
		sinTilt = (sinf(camera.tilt));
		cosTilt = (cosf(camera.tilt));
		bf = camera.baseline * camera.fu;
		invfu = 1.f / camera.fu;
		invfv = 1.f / camera.fv;
	}

	inline cv::Point3f imageToWorld(const cv::Point2f& pt, float d) const
	{
		const float u = pt.x;
		const float v = pt.y;

		const float Zc = bf / d;
		const float Xc = invfu * (u - camera.u0) * Zc;
		const float Yc = invfv * (v - camera.v0) * Zc;

		const float Xw = Xc;
		const float Yw = Yc * cosTilt + Zc * sinTilt;
		const float Zw = Zc * cosTilt - Yc * sinTilt;

		return cv::Point3f(Xw, Yw, Zw);
	}

	CameraParameters camera;
	float sinTilt, cosTilt, bf, invfu, invfv;
};

template <class... Args>
static std::string format_string(const char* fmt, Args... args)
{
	const int BUF_SIZE = 1024;
	char buf[BUF_SIZE];
	std::snprintf(buf, BUF_SIZE, fmt, args...);
	return std::string(buf);
}

static cv::Scalar computeColor(float val)
{
	const float hscale = 6.f;
	float h = 0.6f * (1.f - val), s = 1.f, v = 1.f;
	float r, g, b;

	static const int sector_data[][3] =
	{ { 1,3,0 },{ 1,0,2 },{ 3,0,1 },{ 0,2,1 },{ 0,1,3 },{ 2,1,0 } };
	float tab[4];
	int sector;
	h *= hscale;
	if (h < 0)
		do h += 6; while (h < 0);
	else if (h >= 6)
		do h -= 6; while (h >= 6);
	sector = cvFloor(h);
	h -= sector;
	if ((unsigned)sector >= 6u)
	{
		sector = 0;
		h = 0.f;
	}

	tab[0] = v;
	tab[1] = v*(1.f - s);
	tab[2] = v*(1.f - s*h);
	tab[3] = v*(1.f - s*(1.f - h));

	b = tab[sector_data[sector][0]];
	g = tab[sector_data[sector][1]];
	r = tab[sector_data[sector][2]];
	return 255 * cv::Scalar(b, g, r);
}

void reprojectPointsTo3D(const cv::Mat& disparity, const CameraParameters& camera, std::vector<cv::Point3f>& points, bool subpixeled)
{
	CV_Assert(disparity.type() == CV_32F);

	CoordinateTransform tf(camera);

	points.clear();
	points.reserve(disparity.rows * disparity.cols);

	for (int y = 0; y < disparity.rows; y++)
	{
		for (int x = 0; x < disparity.cols; x++)
		{
			const float d = disparity.at<float>(y, x);
			if (d > 0)
				points.push_back(tf.imageToWorld(cv::Point(x, y), d));
		}
	}
}

void drawPoints3D(const std::vector<cv::Point3f>& points, cv::Mat& draw)
{
	const int SIZE_X = 512;
	const int SIZE_Z = 1024;
	const int maxz = 20; // [meter]
	const double pixelsPerMeter = 1. * SIZE_Z / maxz;

	draw = cv::Mat::zeros(SIZE_Z, SIZE_X, CV_8UC3);

	for (const cv::Point3f& pt : points)
	{
		const float X = pt.x;
		const float Z = pt.z;

		const int u = cvRound(pixelsPerMeter * X) + SIZE_X / 2;
		const int v = SIZE_Z - cvRound(pixelsPerMeter * Z);

		const cv::Scalar color = computeColor(std::min(Z, 1.f * maxz) / maxz);
		cv::circle(draw, cv::Point(u, v), 1, color);
	}
}

int main(int argc, char* argv[])
{
	if (argc < 4) {
		std::cout << "usage: " << argv[0] << " left-image-format right-image-format camera.xml [dizp_size] [ouput_depth] [subpixel]" << std::endl;
		return 0;
	}

	const int first_frame = 1;

	cv::Mat I1 = cv::imread(format_string(argv[1], first_frame), -1);
	cv::Mat I2 = cv::imread(format_string(argv[2], first_frame), -1);
	const cv::FileStorage cvfs(argv[3], cv::FileStorage::READ);
	const int disp_size = argc >= 5 ? std::stoi(argv[4]) : 128;
	const int output_depth = argc >= 6 ? std::stoi(argv[5]) : 16;
	const bool subpixel = argc >= 7 ? std::stoi(argv[6]) != 0 : true;

	ASSERT_MSG(!I1.empty() && !I2.empty(), "imread failed.");
	ASSERT_MSG(cvfs.isOpened(), "camera.xml read failed.");
	ASSERT_MSG(I1.size() == I2.size() && I1.type() == I2.type(), "input images must be same size and type.");
	ASSERT_MSG(I1.type() == CV_8U || I1.type() == CV_16U, "input image format must be CV_8U or CV_16U.");
	ASSERT_MSG(disp_size == 64 || disp_size == 128, "disparity size must be 64 or 128.");
	ASSERT_MSG(output_depth == 8 || output_depth == 16, "output depth must be 8 or 16");

	// read camera parameters
	const cv::FileNode node(cvfs.fs, NULL);
	CameraParameters camera;
	camera.fu = node["FocalLengthX"];
	camera.fv = node["FocalLengthY"];
	camera.u0 = node["CenterX"];
	camera.v0 = node["CenterY"];
	camera.baseline = node["BaseLine"];
	camera.tilt = node["Tilt"];

	const int width = I1.cols;
	const int height = I1.rows;

	const int input_depth = I1.type() == CV_8U ? 8 : 16;
	const int input_bytes = input_depth * width * height / 8;
	const int output_bytes = output_depth * width * height / 8;

	const sgm::StereoSGM::Parameters params{10, 120, 0.95f, subpixel};

	sgm::StereoSGM sgm(width, height, disp_size, input_depth, output_depth, sgm::EXECUTE_INOUT_CUDA2CUDA, params);

	cv::Mat disparity(height, width, output_depth == 8 ? CV_8U : CV_16U);
	cv::Mat disparity_8u, disparity_32f, disparity_color, draw;
	std::vector<cv::Point3f> points;

	device_buffer d_I1(input_bytes), d_I2(input_bytes), d_disparity(output_bytes);

	for (int frame_no = first_frame;; frame_no++) {

		I1 = cv::imread(format_string(argv[1], frame_no), -1);
		I2 = cv::imread(format_string(argv[2], frame_no), -1);
		if (I1.empty() || I2.empty()) {
			frame_no = first_frame;
			continue;
		}

		cudaMemcpy(d_I1.data, I1.data, input_bytes, cudaMemcpyHostToDevice);
		cudaMemcpy(d_I2.data, I2.data, input_bytes, cudaMemcpyHostToDevice);

		const auto t1 = std::chrono::system_clock::now();

		sgm.execute(d_I1.data, d_I2.data, d_disparity.data);
		cudaDeviceSynchronize();

		const auto t2 = std::chrono::system_clock::now();
		const auto duration = std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1).count();
		const double fps = 1e6 / duration;

		cudaMemcpy(disparity.data, d_disparity.data, output_bytes, cudaMemcpyDeviceToHost);

		// draw results
		if (I1.type() != CV_8U) {
			cv::normalize(I1, I1, 0, 255, cv::NORM_MINMAX);
			I1.convertTo(I1, CV_8U);
		}

		disparity.convertTo(disparity_32f, CV_32F, subpixel ? 1. / sgm::StereoSGM::SUBPIXEL_SCALE : 1);
		reprojectPointsTo3D(disparity_32f, camera, points, subpixel);
		drawPoints3D(points, draw);

		disparity_32f.convertTo(disparity_8u, CV_8U, 255. / disp_size);
		cv::applyColorMap(disparity_8u, disparity_color, cv::COLORMAP_JET);
		cv::putText(disparity_color, format_string("sgm execution time: %4.1f[msec] %4.1f[FPS]", 1e-3 * duration, fps),
			cv::Point(50, 50), 2, 0.75, cv::Scalar(255, 255, 255));

		cv::imshow("left image", I1);
		cv::imshow("disparity", disparity_color);
		cv::imshow("points", draw);

		const char c = cv::waitKey(1);
		if (c == 27) // ESC
			break;
	}

	return 0;
}
<|endoftext|>


<commit_before>//===- Dominators.cpp - Dominator Calculation -----------------------------===//
//
// This file implements simple dominator construction algorithms for finding
// forward dominators.  Postdominators are available in libanalysis, but are not
// included in libvmcore, because it's not needed.  Forward dominators are
// needed to support the Verifier pass.
//
//===----------------------------------------------------------------------===//

#include "llvm/Analysis/Dominators.h"
#include "llvm/Support/CFG.h"
#include "llvm/Assembly/Writer.h"
#include "Support/DepthFirstIterator.h"
#include "Support/SetOperations.h"
using std::set;

//===----------------------------------------------------------------------===//
//  DominatorSet Implementation
//===----------------------------------------------------------------------===//

static RegisterAnalysis<DominatorSet>
A("domset", "Dominator Set Construction", true);

// dominates - Return true if A dominates B.  This performs the special checks
// neccesary if A and B are in the same basic block.
//
bool DominatorSetBase::dominates(Instruction *A, Instruction *B) const {
  BasicBlock *BBA = A->getParent(), *BBB = B->getParent();
  if (BBA != BBB) return dominates(BBA, BBB);

  // Loop through the basic block until we find A or B.
  BasicBlock::iterator I = BBA->begin();
  for (; &*I != A && &*I != B; ++I) /*empty*/;

  // A dominates B if it is found first in the basic block...
  return &*I == A;
}

void DominatorSet::calculateDominatorsFromBlock(BasicBlock *RootBB) {
  bool Changed;
  Doms[RootBB].insert(RootBB);  // Root always dominates itself...
  do {
    Changed = false;

    DomSetType WorkingSet;
    df_iterator<BasicBlock*> It = df_begin(RootBB), End = df_end(RootBB);
    for ( ; It != End; ++It) {
      BasicBlock *BB = *It;
      pred_iterator PI = pred_begin(BB), PEnd = pred_end(BB);
      if (PI != PEnd) {                // Is there SOME predecessor?
	// Loop until we get to a predecessor that has had it's dom set filled
	// in at least once.  We are guaranteed to have this because we are
	// traversing the graph in DFO and have handled start nodes specially.
	//
	while (Doms[*PI].empty()) ++PI;
	WorkingSet = Doms[*PI];

	for (++PI; PI != PEnd; ++PI) { // Intersect all of the predecessor sets
	  DomSetType &PredSet = Doms[*PI];
	  if (PredSet.size())
	    set_intersect(WorkingSet, PredSet);
	}
      }

      WorkingSet.insert(BB);           // A block always dominates itself
      DomSetType &BBSet = Doms[BB];
      if (BBSet != WorkingSet) {
	BBSet.swap(WorkingSet);        // Constant time operation!
	Changed = true;                // The sets changed.
      }
      WorkingSet.clear();              // Clear out the set for next iteration
    }
  } while (Changed);
}

// runOnFunction - This method calculates the forward dominator sets for the
// specified function.
//
bool DominatorSet::runOnFunction(Function &F) {
  Doms.clear();   // Reset from the last time we were run...
  Root = &F.getEntryNode();
  assert(pred_begin(Root) == pred_end(Root) &&
	 "Root node has predecessors in function!");

  // Calculate dominator sets for the reachable basic blocks...
  calculateDominatorsFromBlock(Root);

  // Every basic block in the function should at least dominate themselves, and
  // thus every basic block should have an entry in Doms.  The one case where we
  // miss this is when a basic block is unreachable.  To get these we now do an
  // extra pass over the function, calculating dominator information for
  // unreachable blocks.
  //
  for (Function::iterator I = F.begin(), E = F.end(); I != E; ++I)
    if (Doms[I].empty()) {
      calculateDominatorsFromBlock(I);
    }

  return false;
}

static std::ostream &operator<<(std::ostream &o, const set<BasicBlock*> &BBs) {
  for (set<BasicBlock*>::const_iterator I = BBs.begin(), E = BBs.end();
       I != E; ++I) {
    o << "  ";
    WriteAsOperand(o, *I, false);
    o << "\n";
   }
  return o;
}

void DominatorSetBase::print(std::ostream &o) const {
  for (const_iterator I = begin(), E = end(); I != E; ++I)
    o << "=============================--------------------------------\n"
      << "\nDominator Set For Basic Block\n" << I->first
      << "-------------------------------\n" << I->second << "\n";
}

//===----------------------------------------------------------------------===//
//  ImmediateDominators Implementation
//===----------------------------------------------------------------------===//

static RegisterAnalysis<ImmediateDominators>
C("idom", "Immediate Dominators Construction", true);

// calcIDoms - Calculate the immediate dominator mapping, given a set of
// dominators for every basic block.
void ImmediateDominatorsBase::calcIDoms(const DominatorSetBase &DS) {
  // Loop over all of the nodes that have dominators... figuring out the IDOM
  // for each node...
  //
  for (DominatorSet::const_iterator DI = DS.begin(), DEnd = DS.end();
       DI != DEnd; ++DI) {
    BasicBlock *BB = DI->first;
    const DominatorSet::DomSetType &Dominators = DI->second;
    unsigned DomSetSize = Dominators.size();
    if (DomSetSize == 1) continue;  // Root node... IDom = null

    // Loop over all dominators of this node.  This corresponds to looping over
    // nodes in the dominator chain, looking for a node whose dominator set is
    // equal to the current nodes, except that the current node does not exist
    // in it.  This means that it is one level higher in the dom chain than the
    // current node, and it is our idom!
    //
    DominatorSet::DomSetType::const_iterator I = Dominators.begin();
    DominatorSet::DomSetType::const_iterator End = Dominators.end();
    for (; I != End; ++I) {   // Iterate over dominators...
      // All of our dominators should form a chain, where the number of elements
      // in the dominator set indicates what level the node is at in the chain.
      // We want the node immediately above us, so it will have an identical
      // dominator set, except that BB will not dominate it... therefore it's
      // dominator set size will be one less than BB's...
      //
      if (DS.getDominators(*I).size() == DomSetSize - 1) {
	IDoms[BB] = *I;
	break;
      }
    }
  }
}

void ImmediateDominatorsBase::print(std::ostream &o) const {
  for (const_iterator I = begin(), E = end(); I != E; ++I)
    o << "=============================--------------------------------\n"
      << "\nImmediate Dominator For Basic Block\n" << *I->first
      << "is: \n" << *I->second << "\n";
}

//===----------------------------------------------------------------------===//
//  DominatorTree Implementation
//===----------------------------------------------------------------------===//

static RegisterAnalysis<DominatorTree>
E("domtree", "Dominator Tree Construction", true);

// DominatorTreeBase::reset - Free all of the tree node memory.
//
void DominatorTreeBase::reset() {
  for (NodeMapType::iterator I = Nodes.begin(), E = Nodes.end(); I != E; ++I)
    delete I->second;
  Nodes.clear();
}

void DominatorTree::calculate(const DominatorSet &DS) {
  Nodes[Root] = new Node(Root, 0);   // Add a node for the root...

  // Iterate over all nodes in depth first order...
  for (df_iterator<BasicBlock*> I = df_begin(Root), E = df_end(Root);
       I != E; ++I) {
    BasicBlock *BB = *I;
    const DominatorSet::DomSetType &Dominators = DS.getDominators(BB);
    unsigned DomSetSize = Dominators.size();
    if (DomSetSize == 1) continue;  // Root node... IDom = null

    // Loop over all dominators of this node. This corresponds to looping over
    // nodes in the dominator chain, looking for a node whose dominator set is
    // equal to the current nodes, except that the current node does not exist
    // in it. This means that it is one level higher in the dom chain than the
    // current node, and it is our idom!  We know that we have already added
    // a DominatorTree node for our idom, because the idom must be a
    // predecessor in the depth first order that we are iterating through the
    // function.
    //
    DominatorSet::DomSetType::const_iterator I = Dominators.begin();
    DominatorSet::DomSetType::const_iterator End = Dominators.end();
    for (; I != End; ++I) {   // Iterate over dominators...
      // All of our dominators should form a chain, where the number of
      // elements in the dominator set indicates what level the node is at in
      // the chain.  We want the node immediately above us, so it will have
      // an identical dominator set, except that BB will not dominate it...
      // therefore it's dominator set size will be one less than BB's...
      //
      if (DS.getDominators(*I).size() == DomSetSize - 1) {
        // We know that the immediate dominator should already have a node,
        // because we are traversing the CFG in depth first order!
        //
        Node *IDomNode = Nodes[*I];
        assert(IDomNode && "No node for IDOM?");

        // Add a new tree node for this BasicBlock, and link it as a child of
        // IDomNode
        Nodes[BB] = IDomNode->addChild(new Node(BB, IDomNode));
        break;
      }
    }
  }
}

static std::ostream &operator<<(std::ostream &o,
                                const DominatorTreeBase::Node *Node) {
  return o << Node->getNode()
           << "\n------------------------------------------\n";
}

static void PrintDomTree(const DominatorTreeBase::Node *N, std::ostream &o,
                         unsigned Lev) {
  o << "Level #" << Lev << ":  " << N;
  for (DominatorTreeBase::Node::const_iterator I = N->begin(), E = N->end();
       I != E; ++I) {
    PrintDomTree(*I, o, Lev+1);
  }
}

void DominatorTreeBase::print(std::ostream &o) const {
  o << "=============================--------------------------------\n"
    << "Inorder Dominator Tree:\n";
  PrintDomTree(Nodes.find(getRoot())->second, o, 1);
}

//===----------------------------------------------------------------------===//
//  DominanceFrontier Implementation
//===----------------------------------------------------------------------===//

static RegisterAnalysis<DominanceFrontier>
G("domfrontier", "Dominance Frontier Construction", true);

const DominanceFrontier::DomSetType &
DominanceFrontier::calculate(const DominatorTree &DT,
                             const DominatorTree::Node *Node) {
  // Loop over CFG successors to calculate DFlocal[Node]
  BasicBlock *BB = Node->getNode();
  DomSetType &S = Frontiers[BB];       // The new set to fill in...

  for (succ_iterator SI = succ_begin(BB), SE = succ_end(BB);
       SI != SE; ++SI) {
    // Does Node immediately dominate this successor?
    if (DT[*SI]->getIDom() != Node)
      S.insert(*SI);
  }

  // At this point, S is DFlocal.  Now we union in DFup's of our children...
  // Loop through and visit the nodes that Node immediately dominates (Node's
  // children in the IDomTree)
  //
  for (DominatorTree::Node::const_iterator NI = Node->begin(), NE = Node->end();
       NI != NE; ++NI) {
    DominatorTree::Node *IDominee = *NI;
    const DomSetType &ChildDF = calculate(DT, IDominee);

    DomSetType::const_iterator CDFI = ChildDF.begin(), CDFE = ChildDF.end();
    for (; CDFI != CDFE; ++CDFI) {
      if (!Node->dominates(DT[*CDFI]))
	S.insert(*CDFI);
    }
  }

  return S;
}

void DominanceFrontierBase::print(std::ostream &o) const {
  for (const_iterator I = begin(), E = end(); I != E; ++I) {
    o << "=============================--------------------------------\n"
      << "\nDominance Frontier For Basic Block\n";
    WriteAsOperand(o, I->first, false);
    o << " is: \n" << I->second << "\n";
  }
}
<commit_msg> - Add methods to ImmediateDominators & DominatorTree to allow updates  - Make DominatorTree::Node not inherit from std::vector<commit_after>//===- Dominators.cpp - Dominator Calculation -----------------------------===//
//
// This file implements simple dominator construction algorithms for finding
// forward dominators.  Postdominators are available in libanalysis, but are not
// included in libvmcore, because it's not needed.  Forward dominators are
// needed to support the Verifier pass.
//
//===----------------------------------------------------------------------===//

#include "llvm/Analysis/Dominators.h"
#include "llvm/Support/CFG.h"
#include "llvm/Assembly/Writer.h"
#include "Support/DepthFirstIterator.h"
#include "Support/SetOperations.h"
using std::set;

//===----------------------------------------------------------------------===//
//  DominatorSet Implementation
//===----------------------------------------------------------------------===//

static RegisterAnalysis<DominatorSet>
A("domset", "Dominator Set Construction", true);

// dominates - Return true if A dominates B.  This performs the special checks
// neccesary if A and B are in the same basic block.
//
bool DominatorSetBase::dominates(Instruction *A, Instruction *B) const {
  BasicBlock *BBA = A->getParent(), *BBB = B->getParent();
  if (BBA != BBB) return dominates(BBA, BBB);

  // Loop through the basic block until we find A or B.
  BasicBlock::iterator I = BBA->begin();
  for (; &*I != A && &*I != B; ++I) /*empty*/;

  // A dominates B if it is found first in the basic block...
  return &*I == A;
}

void DominatorSet::calculateDominatorsFromBlock(BasicBlock *RootBB) {
  bool Changed;
  Doms[RootBB].insert(RootBB);  // Root always dominates itself...
  do {
    Changed = false;

    DomSetType WorkingSet;
    df_iterator<BasicBlock*> It = df_begin(RootBB), End = df_end(RootBB);
    for ( ; It != End; ++It) {
      BasicBlock *BB = *It;
      pred_iterator PI = pred_begin(BB), PEnd = pred_end(BB);
      if (PI != PEnd) {                // Is there SOME predecessor?
	// Loop until we get to a predecessor that has had it's dom set filled
	// in at least once.  We are guaranteed to have this because we are
	// traversing the graph in DFO and have handled start nodes specially.
	//
	while (Doms[*PI].empty()) ++PI;
	WorkingSet = Doms[*PI];

	for (++PI; PI != PEnd; ++PI) { // Intersect all of the predecessor sets
	  DomSetType &PredSet = Doms[*PI];
	  if (PredSet.size())
	    set_intersect(WorkingSet, PredSet);
	}
      }

      WorkingSet.insert(BB);           // A block always dominates itself
      DomSetType &BBSet = Doms[BB];
      if (BBSet != WorkingSet) {
	BBSet.swap(WorkingSet);        // Constant time operation!
	Changed = true;                // The sets changed.
      }
      WorkingSet.clear();              // Clear out the set for next iteration
    }
  } while (Changed);
}

// runOnFunction - This method calculates the forward dominator sets for the
// specified function.
//
bool DominatorSet::runOnFunction(Function &F) {
  Doms.clear();   // Reset from the last time we were run...
  Root = &F.getEntryNode();
  assert(pred_begin(Root) == pred_end(Root) &&
	 "Root node has predecessors in function!");

  // Calculate dominator sets for the reachable basic blocks...
  calculateDominatorsFromBlock(Root);

  // Every basic block in the function should at least dominate themselves, and
  // thus every basic block should have an entry in Doms.  The one case where we
  // miss this is when a basic block is unreachable.  To get these we now do an
  // extra pass over the function, calculating dominator information for
  // unreachable blocks.
  //
  for (Function::iterator I = F.begin(), E = F.end(); I != E; ++I)
    if (Doms[I].empty()) {
      calculateDominatorsFromBlock(I);
    }

  return false;
}

static std::ostream &operator<<(std::ostream &o, const set<BasicBlock*> &BBs) {
  for (set<BasicBlock*>::const_iterator I = BBs.begin(), E = BBs.end();
       I != E; ++I) {
    o << "  ";
    WriteAsOperand(o, *I, false);
    o << "\n";
   }
  return o;
}

void DominatorSetBase::print(std::ostream &o) const {
  for (const_iterator I = begin(), E = end(); I != E; ++I)
    o << "=============================--------------------------------\n"
      << "\nDominator Set For Basic Block\n" << I->first
      << "-------------------------------\n" << I->second << "\n";
}

//===----------------------------------------------------------------------===//
//  ImmediateDominators Implementation
//===----------------------------------------------------------------------===//

static RegisterAnalysis<ImmediateDominators>
C("idom", "Immediate Dominators Construction", true);

// calcIDoms - Calculate the immediate dominator mapping, given a set of
// dominators for every basic block.
void ImmediateDominatorsBase::calcIDoms(const DominatorSetBase &DS) {
  // Loop over all of the nodes that have dominators... figuring out the IDOM
  // for each node...
  //
  for (DominatorSet::const_iterator DI = DS.begin(), DEnd = DS.end();
       DI != DEnd; ++DI) {
    BasicBlock *BB = DI->first;
    const DominatorSet::DomSetType &Dominators = DI->second;
    unsigned DomSetSize = Dominators.size();
    if (DomSetSize == 1) continue;  // Root node... IDom = null

    // Loop over all dominators of this node.  This corresponds to looping over
    // nodes in the dominator chain, looking for a node whose dominator set is
    // equal to the current nodes, except that the current node does not exist
    // in it.  This means that it is one level higher in the dom chain than the
    // current node, and it is our idom!
    //
    DominatorSet::DomSetType::const_iterator I = Dominators.begin();
    DominatorSet::DomSetType::const_iterator End = Dominators.end();
    for (; I != End; ++I) {   // Iterate over dominators...
      // All of our dominators should form a chain, where the number of elements
      // in the dominator set indicates what level the node is at in the chain.
      // We want the node immediately above us, so it will have an identical
      // dominator set, except that BB will not dominate it... therefore it's
      // dominator set size will be one less than BB's...
      //
      if (DS.getDominators(*I).size() == DomSetSize - 1) {
	IDoms[BB] = *I;
	break;
      }
    }
  }
}

void ImmediateDominatorsBase::print(std::ostream &o) const {
  for (const_iterator I = begin(), E = end(); I != E; ++I)
    o << "=============================--------------------------------\n"
      << "\nImmediate Dominator For Basic Block\n" << *I->first
      << "is: \n" << *I->second << "\n";
}

//===----------------------------------------------------------------------===//
//  DominatorTree Implementation
//===----------------------------------------------------------------------===//

static RegisterAnalysis<DominatorTree>
E("domtree", "Dominator Tree Construction", true);

// DominatorTreeBase::reset - Free all of the tree node memory.
//
void DominatorTreeBase::reset() {
  for (NodeMapType::iterator I = Nodes.begin(), E = Nodes.end(); I != E; ++I)
    delete I->second;
  Nodes.clear();
}

void DominatorTreeBase::Node2::setIDom(Node2 *NewIDom) {
  assert(IDom && "No immediate dominator?");
  if (IDom != NewIDom) {
    std::vector<Node*>::iterator I =
      std::find(IDom->Children.begin(), IDom->Children.end(), this);
    assert(I != IDom->Children.end() &&
           "Not in immediate dominator children set!");
    // I am no longer your child...
    IDom->Children.erase(I);

    // Switch to new dominator
    IDom = NewIDom;
    IDom->Children.push_back(this);
  }
}

void DominatorTree::calculate(const DominatorSet &DS) {
  Nodes[Root] = new Node(Root, 0);   // Add a node for the root...

  // Iterate over all nodes in depth first order...
  for (df_iterator<BasicBlock*> I = df_begin(Root), E = df_end(Root);
       I != E; ++I) {
    BasicBlock *BB = *I;
    const DominatorSet::DomSetType &Dominators = DS.getDominators(BB);
    unsigned DomSetSize = Dominators.size();
    if (DomSetSize == 1) continue;  // Root node... IDom = null

    // Loop over all dominators of this node. This corresponds to looping over
    // nodes in the dominator chain, looking for a node whose dominator set is
    // equal to the current nodes, except that the current node does not exist
    // in it. This means that it is one level higher in the dom chain than the
    // current node, and it is our idom!  We know that we have already added
    // a DominatorTree node for our idom, because the idom must be a
    // predecessor in the depth first order that we are iterating through the
    // function.
    //
    DominatorSet::DomSetType::const_iterator I = Dominators.begin();
    DominatorSet::DomSetType::const_iterator End = Dominators.end();
    for (; I != End; ++I) {   // Iterate over dominators...
      // All of our dominators should form a chain, where the number of
      // elements in the dominator set indicates what level the node is at in
      // the chain.  We want the node immediately above us, so it will have
      // an identical dominator set, except that BB will not dominate it...
      // therefore it's dominator set size will be one less than BB's...
      //
      if (DS.getDominators(*I).size() == DomSetSize - 1) {
        // We know that the immediate dominator should already have a node,
        // because we are traversing the CFG in depth first order!
        //
        Node *IDomNode = Nodes[*I];
        assert(IDomNode && "No node for IDOM?");

        // Add a new tree node for this BasicBlock, and link it as a child of
        // IDomNode
        Nodes[BB] = IDomNode->addChild(new Node(BB, IDomNode));
        break;
      }
    }
  }
}

static std::ostream &operator<<(std::ostream &o,
                                const DominatorTreeBase::Node *Node) {
  return o << Node->getNode()
           << "\n------------------------------------------\n";
}

static void PrintDomTree(const DominatorTreeBase::Node *N, std::ostream &o,
                         unsigned Lev) {
  o << "Level #" << Lev << ":  " << N;
  for (DominatorTreeBase::Node::const_iterator I = N->begin(), E = N->end();
       I != E; ++I) {
    PrintDomTree(*I, o, Lev+1);
  }
}

void DominatorTreeBase::print(std::ostream &o) const {
  o << "=============================--------------------------------\n"
    << "Inorder Dominator Tree:\n";
  PrintDomTree(Nodes.find(getRoot())->second, o, 1);
}

//===----------------------------------------------------------------------===//
//  DominanceFrontier Implementation
//===----------------------------------------------------------------------===//

static RegisterAnalysis<DominanceFrontier>
G("domfrontier", "Dominance Frontier Construction", true);

const DominanceFrontier::DomSetType &
DominanceFrontier::calculate(const DominatorTree &DT,
                             const DominatorTree::Node *Node) {
  // Loop over CFG successors to calculate DFlocal[Node]
  BasicBlock *BB = Node->getNode();
  DomSetType &S = Frontiers[BB];       // The new set to fill in...

  for (succ_iterator SI = succ_begin(BB), SE = succ_end(BB);
       SI != SE; ++SI) {
    // Does Node immediately dominate this successor?
    if (DT[*SI]->getIDom() != Node)
      S.insert(*SI);
  }

  // At this point, S is DFlocal.  Now we union in DFup's of our children...
  // Loop through and visit the nodes that Node immediately dominates (Node's
  // children in the IDomTree)
  //
  for (DominatorTree::Node::const_iterator NI = Node->begin(), NE = Node->end();
       NI != NE; ++NI) {
    DominatorTree::Node *IDominee = *NI;
    const DomSetType &ChildDF = calculate(DT, IDominee);

    DomSetType::const_iterator CDFI = ChildDF.begin(), CDFE = ChildDF.end();
    for (; CDFI != CDFE; ++CDFI) {
      if (!Node->dominates(DT[*CDFI]))
	S.insert(*CDFI);
    }
  }

  return S;
}

void DominanceFrontierBase::print(std::ostream &o) const {
  for (const_iterator I = begin(), E = end(); I != E; ++I) {
    o << "=============================--------------------------------\n"
      << "\nDominance Frontier For Basic Block\n";
    WriteAsOperand(o, I->first, false);
    o << " is: \n" << I->second << "\n";
  }
}
<|endoftext|>


<commit_before>/******************************************************************************
 *                       ____    _    _____                                   *
 *                      / ___|  / \  |  ___|    C++                           *
 *                     | |     / _ \ | |_       Actor                         *
 *                     | |___ / ___ \|  _|      Framework                     *
 *                      \____/_/   \_|_|                                      *
 *                                                                            *
 * Copyright 2011-2018 Dominik Charousset                                     *
 *                                                                            *
 * Distributed under the terms and conditions of the BSD 3-Clause License or  *
 * (at your option) under the terms and conditions of the Boost Software      *
 * License 1.0. See accompanying files LICENSE and LICENSE_ALTERNATIVE.       *
 *                                                                            *
 * If you did not receive a copy of the license files, see                    *
 * http://opensource.org/licenses/BSD-3-Clause and                            *
 * http://www.boost.org/LICENSE_1_0.txt.                                      *
 ******************************************************************************/

#include "caf/config.hpp"

#define CAF_SUITE actor_clock
#include "caf/test/dsl.hpp"

#include <chrono>
#include <memory>

#include "caf/all.hpp"
#include "caf/detail/test_actor_clock.hpp"
#include "caf/raw_event_based_actor.hpp"

using namespace caf;

namespace {

using std::chrono::seconds;

struct testee_state {
  uint32_t timeout_id = 41;
};

behavior testee(stateful_actor<testee_state, raw_event_based_actor>* self,
                detail::test_actor_clock* t) {
  return {
    [=](ok_atom) {
      auto n = t->now() + seconds(10);
      self->state.timeout_id += 1;
      t->set_receive_timeout(n, self, self->state.timeout_id);
    },
    [=](add_atom) {
      auto n = t->now() + seconds(10);
      self->state.timeout_id += 1;
      auto mid = make_message_id(self->state.timeout_id).response_id();
      t->set_request_timeout(n, self, mid);
    },
    [](const timeout_msg&) {
      // nop
    },
    [](const error&) {
      // nop
    },
    [](const std::string&) {
      // nop
    },
    [=](group& grp) {
      self->join(grp);
    },
    [=](exit_msg& x) {
      self->quit(x.reason);
    }
  };
}

struct fixture : test_coordinator_fixture<> {
  detail::test_actor_clock t;
  actor aut;

  fixture() : aut(sys.spawn<lazy_init>(testee, &t)) {
    // nop
  }
};

struct tid {
  uint32_t value;
};

inline bool operator==(const timeout_msg& x, const tid& y) {
  return x.timeout_id == y.value;
}

} // namespace <anonymous>

CAF_TEST_FIXTURE_SCOPE(timer_tests, fixture)

CAF_TEST(single_receive_timeout) {
  // Have AUT call t.set_receive_timeout().
  self->send(aut, ok_atom::value);
  expect((ok_atom), from(self).to(aut).with(_));
  CAF_CHECK_EQUAL(t.schedule().size(), 1u);
  CAF_CHECK_EQUAL(t.actor_lookup().size(), 1u);
  // Advance time to send timeout message.
  t.advance_time(seconds(10));
  CAF_CHECK_EQUAL(t.schedule().size(), 0u);
  CAF_CHECK_EQUAL(t.actor_lookup().size(), 0u);
  // Have AUT receive the timeout.
  expect((timeout_msg), from(aut).to(aut).with(tid{42}));
}

CAF_TEST(override_receive_timeout) {
  // Have AUT call t.set_receive_timeout().
  self->send(aut, ok_atom::value);
  expect((ok_atom), from(self).to(aut).with(_));
  CAF_CHECK_EQUAL(t.schedule().size(), 1u);
  CAF_CHECK_EQUAL(t.actor_lookup().size(), 1u);
  // Have AUT call t.set_timeout() again.
  self->send(aut, ok_atom::value);
  expect((ok_atom), from(self).to(aut).with(_));
  CAF_CHECK_EQUAL(t.schedule().size(), 1u);
  CAF_CHECK_EQUAL(t.actor_lookup().size(), 1u);
  // Advance time to send timeout message.
  t.advance_time(seconds(10));
  CAF_CHECK_EQUAL(t.schedule().size(), 0u);
  CAF_CHECK_EQUAL(t.actor_lookup().size(), 0u);
  // Have AUT receive the timeout.
  expect((timeout_msg), from(aut).to(aut).with(tid{43}));
}

CAF_TEST(single_request_timeout) {
  // Have AUT call t.set_request_timeout().
  self->send(aut, add_atom::value);
  expect((add_atom), from(self).to(aut).with(_));
  CAF_CHECK_EQUAL(t.schedule().size(), 1u);
  CAF_CHECK_EQUAL(t.actor_lookup().size(), 1u);
  // Advance time to send timeout message.
  t.advance_time(seconds(10));
  CAF_CHECK_EQUAL(t.schedule().size(), 0u);
  CAF_CHECK_EQUAL(t.actor_lookup().size(), 0u);
  // Have AUT receive the timeout.
  expect((error), from(aut).to(aut).with(sec::request_timeout));
}

CAF_TEST(mixed_receive_and_request_timeouts) {
  // Have AUT call t.set_receive_timeout().
  self->send(aut, ok_atom::value);
  expect((ok_atom), from(self).to(aut).with(_));
  CAF_CHECK_EQUAL(t.schedule().size(), 1u);
  CAF_CHECK_EQUAL(t.actor_lookup().size(), 1u);
  // Cause the request timeout to arrive later.
  t.advance_time(seconds(5));
  // Have AUT call t.set_request_timeout().
  self->send(aut, add_atom::value);
  expect((add_atom), from(self).to(aut).with(_));
  CAF_CHECK_EQUAL(t.schedule().size(), 2u);
  CAF_CHECK_EQUAL(t.actor_lookup().size(), 2u);
  // Advance time to send receive timeout message.
  t.advance_time(seconds(5));
  CAF_CHECK_EQUAL(t.schedule().size(), 1u);
  CAF_CHECK_EQUAL(t.actor_lookup().size(), 1u);
  // Have AUT receive the timeout.
  expect((timeout_msg), from(aut).to(aut).with(tid{42}));
  // Advance time to send request timeout message.
  t.advance_time(seconds(10));
  CAF_CHECK_EQUAL(t.schedule().size(), 0u);
  CAF_CHECK_EQUAL(t.actor_lookup().size(), 0u);
  // Have AUT receive the timeout.
  expect((error), from(aut).to(aut).with(sec::request_timeout));
}

CAF_TEST(delay_actor_message) {
  // Schedule a message for now + 10s.
  auto n = t.now() + seconds(10);
  auto autptr = actor_cast<strong_actor_ptr>(aut);
  t.schedule_message(n, autptr,
                     make_mailbox_element(autptr, make_message_id(),
                                          no_stages, "foo"));
  CAF_CHECK_EQUAL(t.schedule().size(), 1u);
  CAF_CHECK_EQUAL(t.actor_lookup().size(), 0u);
  // Advance time to send the message.
  t.advance_time(seconds(10));
  CAF_CHECK_EQUAL(t.schedule().size(), 0u);
  CAF_CHECK_EQUAL(t.actor_lookup().size(), 0u);
  // Have AUT receive the message.
  expect((std::string), from(aut).to(aut).with("foo"));
}

CAF_TEST(delay_group_message) {
  // Have AUT join the group.
  auto grp = sys.groups().anonymous();
  self->send(aut, grp);
  expect((group), from(self).to(aut).with(_));
  // Schedule a message for now + 10s.
  auto n = t.now() + seconds(10);
  auto autptr = actor_cast<strong_actor_ptr>(aut);
  t.schedule_message(n, std::move(grp), autptr, make_message("foo"));
  CAF_CHECK_EQUAL(t.schedule().size(), 1u);
  CAF_CHECK_EQUAL(t.actor_lookup().size(), 0u);
  // Advance time to send the message.
  t.advance_time(seconds(10));
  CAF_CHECK_EQUAL(t.schedule().size(), 0u);
  CAF_CHECK_EQUAL(t.actor_lookup().size(), 0u);
  // Have AUT receive the message.
  expect((std::string), from(aut).to(aut).with("foo"));
  // Kill AUT (necessary because the group keeps a reference around).
  self->send_exit(aut, exit_reason::kill);
  expect((exit_msg), from(self).to(aut).with(_));
}

CAF_TEST_FIXTURE_SCOPE_END()
<commit_msg>Fix actor clock unit test<commit_after>/******************************************************************************
 *                       ____    _    _____                                   *
 *                      / ___|  / \  |  ___|    C++                           *
 *                     | |     / _ \ | |_       Actor                         *
 *                     | |___ / ___ \|  _|      Framework                     *
 *                      \____/_/   \_|_|                                      *
 *                                                                            *
 * Copyright 2011-2018 Dominik Charousset                                     *
 *                                                                            *
 * Distributed under the terms and conditions of the BSD 3-Clause License or  *
 * (at your option) under the terms and conditions of the Boost Software      *
 * License 1.0. See accompanying files LICENSE and LICENSE_ALTERNATIVE.       *
 *                                                                            *
 * If you did not receive a copy of the license files, see                    *
 * http://opensource.org/licenses/BSD-3-Clause and                            *
 * http://www.boost.org/LICENSE_1_0.txt.                                      *
 ******************************************************************************/

#include "caf/config.hpp"

#define CAF_SUITE actor_clock
#include "caf/test/dsl.hpp"

#include <chrono>
#include <memory>

#include "caf/all.hpp"
#include "caf/detail/test_actor_clock.hpp"
#include "caf/raw_event_based_actor.hpp"

using namespace caf;

namespace {

using std::chrono::seconds;

struct testee_state {
  uint64_t timeout_id = 41;
};

behavior testee(stateful_actor<testee_state, raw_event_based_actor>* self,
                detail::test_actor_clock* t) {
  return {
    [=](ok_atom) {
      auto n = t->now() + seconds(10);
      self->state.timeout_id += 1;
      t->set_ordinary_timeout(n, self, atom(""), self->state.timeout_id);
    },
    [=](add_atom) {
      auto n = t->now() + seconds(10);
      self->state.timeout_id += 1;
      auto mid = make_message_id(self->state.timeout_id).response_id();
      t->set_request_timeout(n, self, mid);
    },
    [](const timeout_msg&) {
      // nop
    },
    [](const error&) {
      // nop
    },
    [](const std::string&) {
      // nop
    },
    [=](group& grp) {
      self->join(grp);
    },
    [=](exit_msg& x) {
      self->quit(x.reason);
    }
  };
}

struct fixture : test_coordinator_fixture<> {
  detail::test_actor_clock t;
  actor aut;

  fixture() : aut(sys.spawn<lazy_init>(testee, &t)) {
    // nop
  }
};

struct tid {
  uint32_t value;
};

inline bool operator==(const timeout_msg& x, const tid& y) {
  return x.timeout_id == y.value;
}

} // namespace <anonymous>

CAF_TEST_FIXTURE_SCOPE(timer_tests, fixture)

CAF_TEST(single_receive_timeout) {
  // Have AUT call t.set_receive_timeout().
  self->send(aut, ok_atom::value);
  expect((ok_atom), from(self).to(aut).with(_));
  CAF_CHECK_EQUAL(t.schedule().size(), 1u);
  CAF_CHECK_EQUAL(t.actor_lookup().size(), 1u);
  // Advance time to send timeout message.
  t.advance_time(seconds(10));
  CAF_CHECK_EQUAL(t.schedule().size(), 0u);
  CAF_CHECK_EQUAL(t.actor_lookup().size(), 0u);
  // Have AUT receive the timeout.
  expect((timeout_msg), from(aut).to(aut).with(tid{42}));
}

CAF_TEST(override_receive_timeout) {
  // Have AUT call t.set_receive_timeout().
  self->send(aut, ok_atom::value);
  expect((ok_atom), from(self).to(aut).with(_));
  CAF_CHECK_EQUAL(t.schedule().size(), 1u);
  CAF_CHECK_EQUAL(t.actor_lookup().size(), 1u);
  // Have AUT call t.set_timeout() again.
  self->send(aut, ok_atom::value);
  expect((ok_atom), from(self).to(aut).with(_));
  CAF_CHECK_EQUAL(t.schedule().size(), 1u);
  CAF_CHECK_EQUAL(t.actor_lookup().size(), 1u);
  // Advance time to send timeout message.
  t.advance_time(seconds(10));
  CAF_CHECK_EQUAL(t.schedule().size(), 0u);
  CAF_CHECK_EQUAL(t.actor_lookup().size(), 0u);
  // Have AUT receive the timeout.
  expect((timeout_msg), from(aut).to(aut).with(tid{43}));
}

CAF_TEST(single_request_timeout) {
  // Have AUT call t.set_request_timeout().
  self->send(aut, add_atom::value);
  expect((add_atom), from(self).to(aut).with(_));
  CAF_CHECK_EQUAL(t.schedule().size(), 1u);
  CAF_CHECK_EQUAL(t.actor_lookup().size(), 1u);
  // Advance time to send timeout message.
  t.advance_time(seconds(10));
  CAF_CHECK_EQUAL(t.schedule().size(), 0u);
  CAF_CHECK_EQUAL(t.actor_lookup().size(), 0u);
  // Have AUT receive the timeout.
  expect((error), from(aut).to(aut).with(sec::request_timeout));
}

CAF_TEST(mixed_receive_and_request_timeouts) {
  // Have AUT call t.set_receive_timeout().
  self->send(aut, ok_atom::value);
  expect((ok_atom), from(self).to(aut).with(_));
  CAF_CHECK_EQUAL(t.schedule().size(), 1u);
  CAF_CHECK_EQUAL(t.actor_lookup().size(), 1u);
  // Cause the request timeout to arrive later.
  t.advance_time(seconds(5));
  // Have AUT call t.set_request_timeout().
  self->send(aut, add_atom::value);
  expect((add_atom), from(self).to(aut).with(_));
  CAF_CHECK_EQUAL(t.schedule().size(), 2u);
  CAF_CHECK_EQUAL(t.actor_lookup().size(), 2u);
  // Advance time to send receive timeout message.
  t.advance_time(seconds(5));
  CAF_CHECK_EQUAL(t.schedule().size(), 1u);
  CAF_CHECK_EQUAL(t.actor_lookup().size(), 1u);
  // Have AUT receive the timeout.
  expect((timeout_msg), from(aut).to(aut).with(tid{42}));
  // Advance time to send request timeout message.
  t.advance_time(seconds(10));
  CAF_CHECK_EQUAL(t.schedule().size(), 0u);
  CAF_CHECK_EQUAL(t.actor_lookup().size(), 0u);
  // Have AUT receive the timeout.
  expect((error), from(aut).to(aut).with(sec::request_timeout));
}

CAF_TEST(delay_actor_message) {
  // Schedule a message for now + 10s.
  auto n = t.now() + seconds(10);
  auto autptr = actor_cast<strong_actor_ptr>(aut);
  t.schedule_message(n, autptr,
                     make_mailbox_element(autptr, make_message_id(),
                                          no_stages, "foo"));
  CAF_CHECK_EQUAL(t.schedule().size(), 1u);
  CAF_CHECK_EQUAL(t.actor_lookup().size(), 0u);
  // Advance time to send the message.
  t.advance_time(seconds(10));
  CAF_CHECK_EQUAL(t.schedule().size(), 0u);
  CAF_CHECK_EQUAL(t.actor_lookup().size(), 0u);
  // Have AUT receive the message.
  expect((std::string), from(aut).to(aut).with("foo"));
}

CAF_TEST(delay_group_message) {
  // Have AUT join the group.
  auto grp = sys.groups().anonymous();
  self->send(aut, grp);
  expect((group), from(self).to(aut).with(_));
  // Schedule a message for now + 10s.
  auto n = t.now() + seconds(10);
  auto autptr = actor_cast<strong_actor_ptr>(aut);
  t.schedule_message(n, std::move(grp), autptr, make_message("foo"));
  CAF_CHECK_EQUAL(t.schedule().size(), 1u);
  CAF_CHECK_EQUAL(t.actor_lookup().size(), 0u);
  // Advance time to send the message.
  t.advance_time(seconds(10));
  CAF_CHECK_EQUAL(t.schedule().size(), 0u);
  CAF_CHECK_EQUAL(t.actor_lookup().size(), 0u);
  // Have AUT receive the message.
  expect((std::string), from(aut).to(aut).with("foo"));
  // Kill AUT (necessary because the group keeps a reference around).
  self->send_exit(aut, exit_reason::kill);
  expect((exit_msg), from(self).to(aut).with(_));
}

CAF_TEST_FIXTURE_SCOPE_END()
<|endoftext|>


<commit_before>#include <thread>
#include <condition_variable>
#include <mutex>
#include <sstream>

#include <lcm/lcm-cpp.hpp>
#include <ConciseArgs>

#include <drc_utils/LcmWrapper.hpp>
#include <drc_utils/BotWrapper.hpp>

#include <bot_lcmgl_client/lcmgl.h>

#include <lcmtypes/drc/map_scans_t.hpp>
#include <lcmtypes/drc/affordance_collection_t.hpp>

#include <maps/ScanBundleView.hpp>
#include <maps/LcmTranslator.hpp>

#include <pcl/common/io.h>
#include <pcl/filters/passthrough.h>

#include "BlockFitter.hpp"

struct State {
  drc::BotWrapper::Ptr mBotWrapper;
  drc::LcmWrapper::Ptr mLcmWrapper;
  bool mRunContinuously;
  bool mDoFilter;
  bool mRemoveGround;
  bool mDebug;
  Eigen::Vector3f mBlockSize;
  int mAlgorithm;  // TODO: use algo

  drc::map_scans_t mData;
  int64_t mLastDataTime;
  Eigen::Isometry3f mSensorPose;
  Eigen::Isometry3f mGroundPose;

  std::thread mWorkerThread;
  std::condition_variable mCondition;
  std::mutex mProcessMutex;
  std::mutex mDataMutex;

  State() {
    mRunContinuously = false;
    mDoFilter = true;
    mRemoveGround = true;
    mDebug = false;
    mAlgorithm = 0;  // TODO
    mBlockSize << 0, 0, 0;
  }

  void start() {
    mLastDataTime = 0;
    mData.utime = 0;
    mLcmWrapper->get()->subscribe("MAP_SCANS", &State::onScans, this);
    mWorkerThread = std::thread(std::ref(*this));
    mLcmWrapper->startHandleThread(true);
  }

  void stop() {
    mLcmWrapper->stopHandleThread();
    if (mWorkerThread.joinable()) mWorkerThread.join();
  }

  void operator()() {
    while (true) {
      // wait for data
      std::unique_lock<std::mutex> lock(mProcessMutex);
      mCondition.wait_for(lock, std::chrono::milliseconds(100));

      // grab data
      drc::map_scans_t data;
      Eigen::Isometry3f sensorPose;
      Eigen::Isometry3f groundPose;
      {
        std::unique_lock<std::mutex> dataLock(mDataMutex);
        if (mData.utime <= mLastDataTime) continue;
        data = mData;
        sensorPose = mSensorPose;
        groundPose = mGroundPose;
        mLastDataTime = mData.utime;
      }

      // convert scans to point cloud
      // TODO: could do this scan by scan and save away high deltas
      maps::ScanBundleView view;
      maps::LcmTranslator::fromLcm(data, view);
      auto rawCloud = view.getAsPointCloud();
      planeseg::LabeledCloud::Ptr cloud(new planeseg::LabeledCloud());
      pcl::copyPointCloud(*rawCloud, *cloud);

      // remove points outside bbox
      planeseg::LabeledCloud::Ptr tempCloud(new planeseg::LabeledCloud());
      pcl::PassThrough<pcl::PointXYZL> pass;
      pass.setInputCloud(cloud);
      pass.setFilterFieldName("y");
      pass.setFilterLimits (-3.0, 3.0);
      pass.filter(*tempCloud);
      std::swap(cloud, tempCloud);

      // process
      planeseg::BlockFitter fitter;
      fitter.setSensorPose(sensorPose.translation(),
                           sensorPose.rotation().col(2));
      fitter.setGroundBand(groundPose.translation()[2]-1.0,
                           groundPose.translation()[2]+0.5);
      if (mDoFilter) fitter.setAreaThresholds(0.8, 1.2);
      else fitter.setAreaThresholds(0, 1000);
      if (mBlockSize.norm() > 1e-5) fitter.setBlockDimensions(mBlockSize);
      fitter.setRemoveGround(mRemoveGround);
      fitter.setDebug(mDebug);
      fitter.setCloud(cloud);
      auto result = fitter.go();
      if (!result.mSuccess) {
        std::cout << "error: could not detect blocks" << std::endl;
        continue;
      }

      // construct json string
      std::string json;
      json += "{\n";
      json += "  \"command\": \"echo_response\",\n";
      json += "  \"descriptions\": {\n";
      std::string timeString = std::to_string(mBotWrapper->getCurrentTime());
      for (int i = 0; i < (int)result.mBlocks.size(); ++i) {
        const auto& block = result.mBlocks[i];
        std::string dimensionsString, positionString, quaternionString;
        {
          std::ostringstream oss;
          Eigen::Vector3f size = block.mSize;
          oss << size[0] << ", " << size[1] << ", " << size[2];
          dimensionsString = oss.str();
        }
        {
          std::ostringstream oss;
          Eigen::Vector3f p = block.mPose.translation();
          oss << p[0] << ", " << p[1] << ", " << p[2];
          positionString = oss.str();
        }
        {
          std::ostringstream oss;
          Eigen::Quaternionf q(block.mPose.rotation());
          oss << q.w() << ", " << q.x() << ", " << q.y() << ", " << q.z();
          quaternionString = oss.str();
        }
        std::string uuid = timeString + "_" + std::to_string(i+1);

        json += "    \"" + uuid + "\": {\n";
        json += "      \"classname\": \"BoxAffordanceItem\",\n";
        json += "      \"pose\": [[" + positionString + "], [" +
          quaternionString + "]],\n";
        json += "      \"uuid\": \"" + uuid + "\",\n";
        json += "      \"Dimensions\": [" + dimensionsString + "],\n";
        json += "      \"Name\": \"cinderblock " + std::to_string(i) + "\"\n";
        if (i == (int)result.mBlocks.size()-1) json += "    }\n";
        else json += "    },\n";
      }
      json += "  },\n";
      json += "  \"commandId\": \"" + timeString + "\",\n";
      json += "  \"collectionId\": \"block-fitter\"\n";
      json += "}\n";

      // publish result
      drc::affordance_collection_t msg;
      msg.utime = data.utime;
      msg.name = json;
      msg.naffs = 0;
      mLcmWrapper->get()->publish("AFFORDANCE_COLLECTION_COMMAND", &msg);
      std::cout << "Published affordance collection" << std::endl;

      // publish lcmgl
      if (mDebug) {
        bot_lcmgl_t* lcmgl;
        lcmgl = bot_lcmgl_init(mLcmWrapper->get()->getUnderlyingLCM(),
                              "block-fitter");
        for (const auto& block : result.mBlocks) {
          bot_lcmgl_color3f(lcmgl, 1, 0, 0);
          bot_lcmgl_line_width(lcmgl, 4);
          bot_lcmgl_begin(lcmgl, LCMGL_LINE_LOOP);
          for (const auto& pt : block.mHull) {
            bot_lcmgl_vertex3f(lcmgl, pt[0], pt[1], pt[2]);
          }
          bot_lcmgl_end(lcmgl);
        }
        bot_lcmgl_switch_buffer(lcmgl);
        bot_lcmgl_destroy(lcmgl);
      }

      if (!mRunContinuously) break;
    }
    mLcmWrapper->stopHandleThread();
  }

  void onScans(const lcm::ReceiveBuffer* iBuf,
               const std::string& iChannel,
               const drc::map_scans_t* iMessage) {
    std::unique_lock<std::mutex> lock(mDataMutex);
    mData = *iMessage;
    mBotWrapper->getTransform("head", "local", mSensorPose, iMessage->utime);
    mBotWrapper->getTransform("ground", "local", mGroundPose, iMessage->utime);
    mCondition.notify_one();
  }
};

int main(const int iArgc, const char** iArgv) {

  std::string sizeString("");
  State state;

  ConciseArgs opt(iArgc, (char**)iArgv);
  opt.add(state.mRunContinuously, "c", "continuous", "run continuously");
  opt.add(state.mDoFilter, "f", "filter", "filter blocks based on size");
  opt.add(sizeString, "s", "blocksize", "prior size for blocks \"x y z\"");
  opt.add(state.mRemoveGround, "g", "remove-ground",
          "whether to remove ground before processing");
  opt.add(state.mAlgorithm, "a", "algorithm",
          "0=min_area, 1=closest_size, 2=closest_hull");
  opt.add(state.mDebug, "d", "debug", "debug flag");
  opt.parse();

  if (sizeString.length() > 0) {
    std::istringstream iss(sizeString);
    float x, y, z;
    if (iss >> x) {
      if (iss >> y) {
        if (iss >> z) {
          state.mBlockSize << x,y,z;
          std::cout << "using block size " << state.mBlockSize.transpose() <<
            std::endl;
        }
      }
    }
  }

  state.mBotWrapper.reset(new drc::BotWrapper());
  state.mLcmWrapper.reset(new drc::LcmWrapper(state.mBotWrapper->getLcm()));

  state.start();
  state.stop();

  return 1;
}
<commit_msg>make sure valid ground and head poses are available before processing; properly filter points in region around robot<commit_after>#include <thread>
#include <condition_variable>
#include <mutex>
#include <sstream>

#include <lcm/lcm-cpp.hpp>
#include <ConciseArgs>

#include <drc_utils/LcmWrapper.hpp>
#include <drc_utils/BotWrapper.hpp>

#include <bot_lcmgl_client/lcmgl.h>

#include <lcmtypes/drc/map_scans_t.hpp>
#include <lcmtypes/drc/affordance_collection_t.hpp>

#include <maps/ScanBundleView.hpp>
#include <maps/LcmTranslator.hpp>

#include <pcl/common/io.h>
#include <pcl/filters/passthrough.h>

#include "BlockFitter.hpp"

struct State {
  drc::BotWrapper::Ptr mBotWrapper;
  drc::LcmWrapper::Ptr mLcmWrapper;
  bool mRunContinuously;
  bool mDoFilter;
  bool mRemoveGround;
  bool mDebug;
  Eigen::Vector3f mBlockSize;
  int mAlgorithm;  // TODO: use algo

  drc::map_scans_t mData;
  int64_t mLastDataTime;
  Eigen::Isometry3f mSensorPose;
  Eigen::Isometry3f mGroundPose;

  std::thread mWorkerThread;
  std::condition_variable mCondition;
  std::mutex mProcessMutex;
  std::mutex mDataMutex;

  State() {
    mRunContinuously = false;
    mDoFilter = true;
    mRemoveGround = true;
    mDebug = false;
    mAlgorithm = 0;  // TODO
    mBlockSize << 0, 0, 0;
  }

  void start() {
    mLastDataTime = 0;
    mData.utime = 0;
    mLcmWrapper->get()->subscribe("MAP_SCANS", &State::onScans, this);
    mWorkerThread = std::thread(std::ref(*this));
    mLcmWrapper->startHandleThread(true);
  }

  void stop() {
    mLcmWrapper->stopHandleThread();
    if (mWorkerThread.joinable()) mWorkerThread.join();
  }

  void operator()() {
    while (true) {
      // wait for data
      std::unique_lock<std::mutex> lock(mProcessMutex);
      mCondition.wait_for(lock, std::chrono::milliseconds(100));

      // grab data
      drc::map_scans_t data;
      Eigen::Isometry3f sensorPose;
      Eigen::Isometry3f groundPose;
      {
        std::unique_lock<std::mutex> dataLock(mDataMutex);
        if (mData.utime <= mLastDataTime) continue;
        data = mData;
        sensorPose = mSensorPose;
        groundPose = mGroundPose;
        mLastDataTime = mData.utime;
      }

      // convert scans to point cloud
      // TODO: could do this scan by scan and save away high deltas
      maps::ScanBundleView view;
      maps::LcmTranslator::fromLcm(data, view);
      auto rawCloud = view.getAsPointCloud();
      planeseg::LabeledCloud::Ptr cloud(new planeseg::LabeledCloud());
      pcl::copyPointCloud(*rawCloud, *cloud);

      // remove points outside max radius
      const float kValidRadius = 5;  // meters; TODO: could make this a param
      const float kValidRadius2 = kValidRadius*kValidRadius;
      planeseg::LabeledCloud::Ptr tempCloud(new planeseg::LabeledCloud());
      for (int i = 0; i < (int)cloud->size(); ++i) {
        Eigen::Vector3f p = cloud->points[i].getVector3fMap();
        float dist2 = (p-sensorPose.translation()).squaredNorm();
        if (dist2 > kValidRadius2) continue;
        tempCloud->push_back(cloud->points[i]);
      }
      std::swap(cloud, tempCloud);

      // process
      planeseg::BlockFitter fitter;
      fitter.setSensorPose(sensorPose.translation(),
                           sensorPose.rotation().col(2));
      fitter.setGroundBand(groundPose.translation()[2]-1.0,
                           groundPose.translation()[2]+0.5);
      if (mDoFilter) fitter.setAreaThresholds(0.8, 1.2);
      else fitter.setAreaThresholds(0, 1000);
      if (mBlockSize.norm() > 1e-5) fitter.setBlockDimensions(mBlockSize);
      fitter.setRemoveGround(mRemoveGround);
      fitter.setDebug(mDebug);
      fitter.setCloud(cloud);
      auto result = fitter.go();
      if (!result.mSuccess) {
        std::cout << "error: could not detect blocks" << std::endl;
        continue;
      }

      // construct json string
      std::string json;
      json += "{\n";
      json += "  \"command\": \"echo_response\",\n";
      json += "  \"descriptions\": {\n";
      std::string timeString = std::to_string(mBotWrapper->getCurrentTime());
      for (int i = 0; i < (int)result.mBlocks.size(); ++i) {
        const auto& block = result.mBlocks[i];
        std::string dimensionsString, positionString, quaternionString;
        {
          std::ostringstream oss;
          Eigen::Vector3f size = block.mSize;
          oss << size[0] << ", " << size[1] << ", " << size[2];
          dimensionsString = oss.str();
        }
        {
          std::ostringstream oss;
          Eigen::Vector3f p = block.mPose.translation();
          oss << p[0] << ", " << p[1] << ", " << p[2];
          positionString = oss.str();
        }
        {
          std::ostringstream oss;
          Eigen::Quaternionf q(block.mPose.rotation());
          oss << q.w() << ", " << q.x() << ", " << q.y() << ", " << q.z();
          quaternionString = oss.str();
        }
        std::string uuid = timeString + "_" + std::to_string(i+1);

        json += "    \"" + uuid + "\": {\n";
        json += "      \"classname\": \"BoxAffordanceItem\",\n";
        json += "      \"pose\": [[" + positionString + "], [" +
          quaternionString + "]],\n";
        json += "      \"uuid\": \"" + uuid + "\",\n";
        json += "      \"Dimensions\": [" + dimensionsString + "],\n";
        json += "      \"Name\": \"cinderblock " + std::to_string(i) + "\"\n";
        if (i == (int)result.mBlocks.size()-1) json += "    }\n";
        else json += "    },\n";
      }
      json += "  },\n";
      json += "  \"commandId\": \"" + timeString + "\",\n";
      json += "  \"collectionId\": \"block-fitter\"\n";
      json += "}\n";

      // publish result
      drc::affordance_collection_t msg;
      msg.utime = data.utime;
      msg.name = json;
      msg.naffs = 0;
      mLcmWrapper->get()->publish("AFFORDANCE_COLLECTION_COMMAND", &msg);
      std::cout << "Published affordance collection" << std::endl;

      // publish lcmgl
      if (mDebug) {
        bot_lcmgl_t* lcmgl;
        lcmgl = bot_lcmgl_init(mLcmWrapper->get()->getUnderlyingLCM(),
                              "block-fitter");
        for (const auto& block : result.mBlocks) {
          bot_lcmgl_color3f(lcmgl, 1, 0, 0);
          bot_lcmgl_line_width(lcmgl, 4);
          bot_lcmgl_begin(lcmgl, LCMGL_LINE_LOOP);
          for (const auto& pt : block.mHull) {
            bot_lcmgl_vertex3f(lcmgl, pt[0], pt[1], pt[2]);
          }
          bot_lcmgl_end(lcmgl);
        }
        bot_lcmgl_switch_buffer(lcmgl);
        bot_lcmgl_destroy(lcmgl);
      }

      if (!mRunContinuously) break;
    }
    mLcmWrapper->stopHandleThread();
  }

  void onScans(const lcm::ReceiveBuffer* iBuf,
               const std::string& iChannel,
               const drc::map_scans_t* iMessage) {
    std::unique_lock<std::mutex> lock(mDataMutex);
    mData = *iMessage;
    int64_t scanTime = iMessage->utime;
    int64_t headPoseTime = mBotWrapper->getLatestTime("head", "local");
    int64_t groundPoseTime = mBotWrapper->getLatestTime("ground", "local");
    if ((std::abs(headPoseTime-scanTime) > 1e6) ||
        (std::abs(groundPoseTime-scanTime) > 1e6)) {
      std::cout << "warning: got scans but no valid pose found" << std::endl;
      return;
    }
    mBotWrapper->getTransform("head", "local", mSensorPose, iMessage->utime);
    mBotWrapper->getTransform("ground", "local", mGroundPose, iMessage->utime);
    mCondition.notify_one();
  }
};

int main(const int iArgc, const char** iArgv) {

  std::string sizeString("");
  State state;

  ConciseArgs opt(iArgc, (char**)iArgv);
  opt.add(state.mRunContinuously, "c", "continuous", "run continuously");
  opt.add(state.mDoFilter, "f", "filter", "filter blocks based on size");
  opt.add(sizeString, "s", "blocksize", "prior size for blocks \"x y z\"");
  opt.add(state.mRemoveGround, "g", "remove-ground",
          "whether to remove ground before processing");
  opt.add(state.mAlgorithm, "a", "algorithm",
          "0=min_area, 1=closest_size, 2=closest_hull");
  opt.add(state.mDebug, "d", "debug", "debug flag");
  opt.parse();

  if (sizeString.length() > 0) {
    std::istringstream iss(sizeString);
    float x, y, z;
    if (iss >> x) {
      if (iss >> y) {
        if (iss >> z) {
          state.mBlockSize << x,y,z;
          std::cout << "using block size " << state.mBlockSize.transpose() <<
            std::endl;
        }
      }
    }
  }

  state.mBotWrapper.reset(new drc::BotWrapper());
  state.mLcmWrapper.reset(new drc::LcmWrapper(state.mBotWrapper->getLcm()));

  state.start();
  state.stop();

  return 1;
}
<|endoftext|>